1/******************************************************************************
2 * $Id: AKFS_AOC.c 580 2012-03-29 09:56:21Z yamada.rj $
3 ******************************************************************************
4 *
5 * Copyright (C) 2012 Asahi Kasei Microdevices Corporation, Japan
6 *
7 * Licensed under the Apache License, Version 2.0 (the "License");
8 * you may not use this file except in compliance with the License.
9 * You may obtain a copy of the License at
10 *
11 *      http://www.apache.org/licenses/LICENSE-2.0
12 *
13 * Unless required by applicable law or agreed to in writing, software
14 * distributed under the License is distributed on an "AS IS" BASIS,
15 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
16 * See the License for the specific language governing permissions and
17 * limitations under the License.
18 */
19#include "AKFS_AOC.h"
20#include "AKFS_Math.h"
21
22/*
23 * CalcR
24 */
25static AKFLOAT CalcR(
26	const	AKFVEC*	x,
27	const	AKFVEC*	y
28){
29	int16	i;
30	AKFLOAT	r;
31
32	r = 0.0;
33	for(i = 0; i < 3; i++){
34		r += (x->v[i]-y->v[i]) * (x->v[i]-y->v[i]);
35	}
36	r = sqrt(r);
37
38	return r;
39}
40
41/*
42 * From4Points2Sphere()
43 */
44static int16 From4Points2Sphere(
45	const	AKFVEC		points[],	/*! (i/o)	: input vectors  */
46			AKFVEC*		center,		/*! (o)	: center of sphere   */
47			AKFLOAT*	r			/*! (i)	: add/subtract value */
48){
49	AKFLOAT	dif[3][3];
50	AKFLOAT	r2[3];
51
52	AKFLOAT	A;
53	AKFLOAT	B;
54	AKFLOAT	C;
55	AKFLOAT	D;
56	AKFLOAT	E;
57	AKFLOAT	F;
58	AKFLOAT	G;
59
60	AKFLOAT	OU;
61	AKFLOAT	OD;
62
63	int16	i, j;
64
65	for(i = 0; i < 3; i++){
66		r2[i] = 0.0;
67		for(j = 0; j < 3; j++){
68			dif[i][j] = points[i].v[j] - points[3].v[j];
69			r2[i] += (points[i].v[j]*points[i].v[j]
70					- points[3].v[j]*points[3].v[j]);
71		}
72		r2[i] *= 0.5;
73	}
74
75	A = dif[0][0]*dif[2][2] - dif[0][2]*dif[2][0];
76	B = dif[0][1]*dif[2][0] - dif[0][0]*dif[2][1];
77	C = dif[0][0]*dif[2][1] - dif[0][1]*dif[2][0];
78	D = dif[0][0]*r2[2]		- dif[2][0]*r2[0];
79	E = dif[0][0]*dif[1][1] - dif[0][1]*dif[1][0];
80	F = dif[1][0]*dif[0][2] - dif[0][0]*dif[1][2];
81	G = dif[0][0]*r2[1]		- dif[1][0]*r2[0];
82
83	OU = D*E + B*G;
84	OD = C*F + A*E;
85
86	if(fabs(OD) < AKFS_EPSILON){
87		return -1;
88	}
89
90	center->v[2] = OU / OD;
91
92	OU = F*center->v[2] + G;
93	OD = E;
94
95	if(fabs(OD) < AKFS_EPSILON){
96		return -1;
97	}
98
99	center->v[1] = OU / OD;
100
101	OU = r2[0] - dif[0][1]*center->v[1] - dif[0][2]*center->v[2];
102	OD = dif[0][0];
103
104	if(fabs(OD) < AKFS_EPSILON){
105		return -1;
106	}
107
108	center->v[0] = OU / OD;
109
110	*r = CalcR(&points[0], center);
111
112	return 0;
113
114}
115
116/*
117 * MeanVar
118 */
119static void MeanVar(
120	const	AKFVEC	v[],			/*!< (i)   : input vectors */
121	const	int16	n,				/*!< (i)   : number of vectors */
122			AKFVEC*	mean,			/*!< (o)   : (max+min)/2 */
123			AKFVEC*	var				/*!< (o)   : variation in vectors */
124){
125	int16	i;
126	int16	j;
127	AKFVEC	max;
128	AKFVEC	min;
129
130	for(j = 0; j < 3; j++){
131		min.v[j] = v[0].v[j];
132		max.v[j] = v[0].v[j];
133		for(i = 1; i < n; i++){
134			if(v[i].v[j] < min.v[j]){
135				min.v[j] = v[i].v[j];
136			}
137			if(v[i].v[j] > max.v[j]){
138				max.v[j] = v[i].v[j];
139			}
140		}
141		mean->v[j] = (max.v[j] + min.v[j]) / 2.0;	/*mean */
142		var->v[j] = max.v[j] - min.v[j];			/*var  */
143	}
144}
145
146/*
147 * Get4points
148 */
149static void Get4points(
150	const	AKFVEC	v[],			/*!< (i)   : input vectors */
151	const	int16	n,				/*!< (i)   : number of vectors */
152			AKFVEC	out[]			/*!< (o)   : */
153){
154	int16	i, j;
155	AKFLOAT temp;
156	AKFLOAT d;
157
158	AKFVEC	dv[AKFS_HBUF_SIZE];
159	AKFVEC	cross;
160	AKFVEC	tempv;
161
162	/* out 0 */
163	out[0] = v[0];
164
165	/* out 1 */
166	d = 0.0;
167	for(i = 1; i < n; i++){
168		temp = CalcR(&v[i], &out[0]);
169		if(d < temp){
170			d = temp;
171			out[1] = v[i];
172		}
173	}
174
175	/* out 2 */
176	d = 0.0;
177	for(j = 0; j < 3; j++){
178		dv[0].v[j] = out[1].v[j] - out[0].v[j];
179	}
180	for(i = 1; i < n; i++){
181		for(j = 0; j < 3; j++){
182			dv[i].v[j] = v[i].v[j] - out[0].v[j];
183		}
184		tempv.v[0] = dv[0].v[1]*dv[i].v[2] - dv[0].v[2]*dv[i].v[1];
185		tempv.v[1] = dv[0].v[2]*dv[i].v[0] - dv[0].v[0]*dv[i].v[2];
186		tempv.v[2] = dv[0].v[0]*dv[i].v[1] - dv[0].v[1]*dv[i].v[0];
187		temp =	tempv.u.x * tempv.u.x
188			  +	tempv.u.y * tempv.u.y
189			  +	tempv.u.z * tempv.u.z;
190		if(d < temp){
191			d = temp;
192			out[2] = v[i];
193			cross = tempv;
194		}
195	}
196
197	/* out 3 */
198	d = 0.0;
199	for(i = 1; i < n; i++){
200		temp =	  dv[i].u.x * cross.u.x
201				+ dv[i].u.y * cross.u.y
202				+ dv[i].u.z * cross.u.z;
203		temp = fabs(temp);
204		if(d < temp){
205			d = temp;
206			out[3] = v[i];
207		}
208	}
209}
210
211/*
212 * CheckInitFvec
213 */
214static int16 CheckInitFvec(
215	const	AKFVEC	*v		/*!< [in]	vector */
216){
217	int16 i;
218
219	for(i = 0; i < 3; i++){
220		if(AKFS_FMAX <= v->v[i]){
221			return 1;       /* initvalue */
222		}
223	}
224
225	return 0;       /* not initvalue */
226}
227
228/*
229 * AKFS_AOC
230 */
231int16 AKFS_AOC(				/*!< (o)   : calibration success(1), failure(0) */
232			AKFS_AOC_VAR*	haocv,	/*!< (i/o)	: a set of variables */
233	const	AKFVEC*			hdata,	/*!< (i)	: vectors of data    */
234			AKFVEC*			ho		/*!< (i/o)	: offset             */
235){
236	int16	i, j;
237	int16	num;
238	AKFLOAT	tempf;
239	AKFVEC	tempho;
240
241	AKFVEC	fourpoints[4];
242
243	AKFVEC	var;
244	AKFVEC	mean;
245
246	/* buffer new data */
247	for(i = 1; i < AKFS_HBUF_SIZE; i++){
248		haocv->hbuf[AKFS_HBUF_SIZE-i] = haocv->hbuf[AKFS_HBUF_SIZE-i-1];
249	}
250	haocv->hbuf[0] = *hdata;
251
252	/* Check Init */
253	num = 0;
254	for(i = AKFS_HBUF_SIZE; 3 < i; i--){
255		if(CheckInitFvec(&haocv->hbuf[i-1]) == 0){
256			num = i;
257			break;
258		}
259	}
260	if(num < 4){
261		return AKFS_ERROR;
262	}
263
264	/* get 4 points */
265	Get4points(haocv->hbuf, num, fourpoints);
266
267	/* estimate offset */
268	if(0 != From4Points2Sphere(fourpoints, &tempho, &haocv->hraoc)){
269		return AKFS_ERROR;
270	}
271
272	/* check distance */
273	for(i = 0; i < 4; i++){
274		for(j = (i+1); j < 4; j++){
275			tempf = CalcR(&fourpoints[i], &fourpoints[j]);
276			if((tempf < haocv->hraoc)||(tempf < AKFS_HR_TH)){
277				return AKFS_ERROR;
278			}
279		}
280	}
281
282	/* update offset buffer */
283	for(i = 1; i < AKFS_HOBUF_SIZE; i++){
284		haocv->hobuf[AKFS_HOBUF_SIZE-i] = haocv->hobuf[AKFS_HOBUF_SIZE-i-1];
285	}
286	haocv->hobuf[0] = tempho;
287
288	/* clear hbuf */
289	for(i = (AKFS_HBUF_SIZE>>1); i < AKFS_HBUF_SIZE; i++) {
290		for(j = 0; j < 3; j++) {
291			haocv->hbuf[i].v[j]= AKFS_FMAX;
292		}
293	}
294
295	/* Check Init */
296	if(CheckInitFvec(&haocv->hobuf[AKFS_HOBUF_SIZE-1]) == 1){
297		return AKFS_ERROR;
298	}
299
300	/* Check ovar */
301	tempf = haocv->hraoc * AKFS_HO_TH;
302	MeanVar(haocv->hobuf, AKFS_HOBUF_SIZE, &mean, &var);
303	if ((var.u.x >= tempf) || (var.u.y >= tempf) || (var.u.z >= tempf)){
304		return AKFS_ERROR;
305	}
306
307	*ho = mean;
308
309	return AKFS_SUCCESS;
310}
311
312/*
313 * AKFS_InitAOC
314 */
315void AKFS_InitAOC(
316			AKFS_AOC_VAR*	haocv
317){
318	int16 i, j;
319
320	/* Initialize buffer */
321	for(i = 0; i < AKFS_HBUF_SIZE; i++) {
322		for(j = 0; j < 3; j++) {
323			haocv->hbuf[i].v[j]= AKFS_FMAX;
324		}
325	}
326	for(i = 0; i < AKFS_HOBUF_SIZE; i++) {
327		for(j = 0; j < 3; j++) {
328			haocv->hobuf[i].v[j]= AKFS_FMAX;
329		}
330	}
331
332	haocv->hraoc = 0.0;
333}
334
335