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 */
CalcR(const AKFVEC * x,const AKFVEC * y)25 static 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 */
From4Points2Sphere(const AKFVEC points[],AKFVEC * center,AKFLOAT * r)44 static 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 */
MeanVar(const AKFVEC v[],const int16 n,AKFVEC * mean,AKFVEC * var)119 static 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 */
Get4points(const AKFVEC v[],const int16 n,AKFVEC out[])149 static 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 */
CheckInitFvec(const AKFVEC * v)214 static 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 */
AKFS_AOC(AKFS_AOC_VAR * haocv,const AKFVEC * hdata,AKFVEC * ho)231 int16 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 */
AKFS_InitAOC(AKFS_AOC_VAR * haocv)315 void 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