/******************************************************************************
* $Id: AKFS_AOC.c 580 2012-03-29 09:56:21Z yamada.rj $
******************************************************************************
*
* Copyright (C) 2012 Asahi Kasei Microdevices Corporation, Japan
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "AKFS_AOC.h"
#include "AKFS_Math.h"
/*
* CalcR
*/
static AKFLOAT CalcR(
const AKFVEC* x,
const AKFVEC* y
){
int16 i;
AKFLOAT r;
r = 0.0;
for(i = 0; i < 3; i++){
r += (x->v[i]-y->v[i]) * (x->v[i]-y->v[i]);
}
r = sqrt(r);
return r;
}
/*
* From4Points2Sphere()
*/
static int16 From4Points2Sphere(
const AKFVEC points[], /*! (i/o) : input vectors */
AKFVEC* center, /*! (o) : center of sphere */
AKFLOAT* r /*! (i) : add/subtract value */
){
AKFLOAT dif[3][3];
AKFLOAT r2[3];
AKFLOAT A;
AKFLOAT B;
AKFLOAT C;
AKFLOAT D;
AKFLOAT E;
AKFLOAT F;
AKFLOAT G;
AKFLOAT OU;
AKFLOAT OD;
int16 i, j;
for(i = 0; i < 3; i++){
r2[i] = 0.0;
for(j = 0; j < 3; j++){
dif[i][j] = points[i].v[j] - points[3].v[j];
r2[i] += (points[i].v[j]*points[i].v[j]
- points[3].v[j]*points[3].v[j]);
}
r2[i] *= 0.5;
}
A = dif[0][0]*dif[2][2] - dif[0][2]*dif[2][0];
B = dif[0][1]*dif[2][0] - dif[0][0]*dif[2][1];
C = dif[0][0]*dif[2][1] - dif[0][1]*dif[2][0];
D = dif[0][0]*r2[2] - dif[2][0]*r2[0];
E = dif[0][0]*dif[1][1] - dif[0][1]*dif[1][0];
F = dif[1][0]*dif[0][2] - dif[0][0]*dif[1][2];
G = dif[0][0]*r2[1] - dif[1][0]*r2[0];
OU = D*E + B*G;
OD = C*F + A*E;
if(fabs(OD) < AKFS_EPSILON){
return -1;
}
center->v[2] = OU / OD;
OU = F*center->v[2] + G;
OD = E;
if(fabs(OD) < AKFS_EPSILON){
return -1;
}
center->v[1] = OU / OD;
OU = r2[0] - dif[0][1]*center->v[1] - dif[0][2]*center->v[2];
OD = dif[0][0];
if(fabs(OD) < AKFS_EPSILON){
return -1;
}
center->v[0] = OU / OD;
*r = CalcR(&points[0], center);
return 0;
}
/*
* MeanVar
*/
static void MeanVar(
const AKFVEC v[], /*!< (i) : input vectors */
const int16 n, /*!< (i) : number of vectors */
AKFVEC* mean, /*!< (o) : (max+min)/2 */
AKFVEC* var /*!< (o) : variation in vectors */
){
int16 i;
int16 j;
AKFVEC max;
AKFVEC min;
for(j = 0; j < 3; j++){
min.v[j] = v[0].v[j];
max.v[j] = v[0].v[j];
for(i = 1; i < n; i++){
if(v[i].v[j] < min.v[j]){
min.v[j] = v[i].v[j];
}
if(v[i].v[j] > max.v[j]){
max.v[j] = v[i].v[j];
}
}
mean->v[j] = (max.v[j] + min.v[j]) / 2.0; /*mean */
var->v[j] = max.v[j] - min.v[j]; /*var */
}
}
/*
* Get4points
*/
static void Get4points(
const AKFVEC v[], /*!< (i) : input vectors */
const int16 n, /*!< (i) : number of vectors */
AKFVEC out[] /*!< (o) : */
){
int16 i, j;
AKFLOAT temp;
AKFLOAT d;
AKFVEC dv[AKFS_HBUF_SIZE];
AKFVEC cross;
AKFVEC tempv;
/* out 0 */
out[0] = v[0];
/* out 1 */
d = 0.0;
for(i = 1; i < n; i++){
temp = CalcR(&v[i], &out[0]);
if(d < temp){
d = temp;
out[1] = v[i];
}
}
/* out 2 */
d = 0.0;
for(j = 0; j < 3; j++){
dv[0].v[j] = out[1].v[j] - out[0].v[j];
}
for(i = 1; i < n; i++){
for(j = 0; j < 3; j++){
dv[i].v[j] = v[i].v[j] - out[0].v[j];
}
tempv.v[0] = dv[0].v[1]*dv[i].v[2] - dv[0].v[2]*dv[i].v[1];
tempv.v[1] = dv[0].v[2]*dv[i].v[0] - dv[0].v[0]*dv[i].v[2];
tempv.v[2] = dv[0].v[0]*dv[i].v[1] - dv[0].v[1]*dv[i].v[0];
temp = tempv.u.x * tempv.u.x
+ tempv.u.y * tempv.u.y
+ tempv.u.z * tempv.u.z;
if(d < temp){
d = temp;
out[2] = v[i];
cross = tempv;
}
}
/* out 3 */
d = 0.0;
for(i = 1; i < n; i++){
temp = dv[i].u.x * cross.u.x
+ dv[i].u.y * cross.u.y
+ dv[i].u.z * cross.u.z;
temp = fabs(temp);
if(d < temp){
d = temp;
out[3] = v[i];
}
}
}
/*
* CheckInitFvec
*/
static int16 CheckInitFvec(
const AKFVEC *v /*!< [in] vector */
){
int16 i;
for(i = 0; i < 3; i++){
if(AKFS_FMAX <= v->v[i]){
return 1; /* initvalue */
}
}
return 0; /* not initvalue */
}
/*
* AKFS_AOC
*/
int16 AKFS_AOC( /*!< (o) : calibration success(1), failure(0) */
AKFS_AOC_VAR* haocv, /*!< (i/o) : a set of variables */
const AKFVEC* hdata, /*!< (i) : vectors of data */
AKFVEC* ho /*!< (i/o) : offset */
){
int16 i, j;
int16 num;
AKFLOAT tempf;
AKFVEC tempho;
AKFVEC fourpoints[4];
AKFVEC var;
AKFVEC mean;
/* buffer new data */
for(i = 1; i < AKFS_HBUF_SIZE; i++){
haocv->hbuf[AKFS_HBUF_SIZE-i] = haocv->hbuf[AKFS_HBUF_SIZE-i-1];
}
haocv->hbuf[0] = *hdata;
/* Check Init */
num = 0;
for(i = AKFS_HBUF_SIZE; 3 < i; i--){
if(CheckInitFvec(&haocv->hbuf[i-1]) == 0){
num = i;
break;
}
}
if(num < 4){
return AKFS_ERROR;
}
/* get 4 points */
Get4points(haocv->hbuf, num, fourpoints);
/* estimate offset */
if(0 != From4Points2Sphere(fourpoints, &tempho, &haocv->hraoc)){
return AKFS_ERROR;
}
/* check distance */
for(i = 0; i < 4; i++){
for(j = (i+1); j < 4; j++){
tempf = CalcR(&fourpoints[i], &fourpoints[j]);
if((tempf < haocv->hraoc)||(tempf < AKFS_HR_TH)){
return AKFS_ERROR;
}
}
}
/* update offset buffer */
for(i = 1; i < AKFS_HOBUF_SIZE; i++){
haocv->hobuf[AKFS_HOBUF_SIZE-i] = haocv->hobuf[AKFS_HOBUF_SIZE-i-1];
}
haocv->hobuf[0] = tempho;
/* clear hbuf */
for(i = (AKFS_HBUF_SIZE>>1); i < AKFS_HBUF_SIZE; i++) {
for(j = 0; j < 3; j++) {
haocv->hbuf[i].v[j]= AKFS_FMAX;
}
}
/* Check Init */
if(CheckInitFvec(&haocv->hobuf[AKFS_HOBUF_SIZE-1]) == 1){
return AKFS_ERROR;
}
/* Check ovar */
tempf = haocv->hraoc * AKFS_HO_TH;
MeanVar(haocv->hobuf, AKFS_HOBUF_SIZE, &mean, &var);
if ((var.u.x >= tempf) || (var.u.y >= tempf) || (var.u.z >= tempf)){
return AKFS_ERROR;
}
*ho = mean;
return AKFS_SUCCESS;
}
/*
* AKFS_InitAOC
*/
void AKFS_InitAOC(
AKFS_AOC_VAR* haocv
){
int16 i, j;
/* Initialize buffer */
for(i = 0; i < AKFS_HBUF_SIZE; i++) {
for(j = 0; j < 3; j++) {
haocv->hbuf[i].v[j]= AKFS_FMAX;
}
}
for(i = 0; i < AKFS_HOBUF_SIZE; i++) {
for(j = 0; j < 3; j++) {
haocv->hobuf[i].v[j]= AKFS_FMAX;
}
}
haocv->hraoc = 0.0;
}