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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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
