src/fusion_util.c

   1 /*
   2  * Copyright (c) 2011 Samsung Electronics Co., Ltd All Rights Reserved
   3  *
   4  * Licensed under the Apache License, Version 2.0 (the "License");
   5  * you may not use this file except in compliance with the License.
   6  * You may obtain a copy of the License at
   7  *
   8  * http://www.apache.org/licenses/LICENSE-2.0
   9  *
  10  * Unless required by applicable law or agreed to in writing, software
  11  * distributed under the License is distributed on an "AS IS" BASIS,
  12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13  * See the License for the specific language governing permissions and
  14  * limitations under the License.
  15  */
  16
  17 #include <math.h>
  18 #include <stdlib.h>
  19
  20 float clamp(float v)
  21 {
  22         return (v < 0) ? 0.0 : v;
  23 }
  24
  25 int getAngleChange(float *R, float *prevR, float *angleChange)
  26 {
  27         if (R == NULL || prevR == NULL || angleChange == NULL)
  28                 return -1;
  29
  30         float rd1, rd4, rd6, rd7, rd8;
  31         float ri0, ri1, ri2, ri3, ri4, ri5, ri6, ri7, ri8;
  32         float pri0, pri1, pri2, pri3, pri4, pri5, pri6, pri7, pri8;
  33
  34         ri0 = R[0];
  35         ri1 = R[1];
  36         ri2 = R[2];
  37         ri3 = R[3];
  38         ri4 = R[4];
  39         ri5 = R[5];
  40         ri6 = R[6];
  41         ri7 = R[7];
  42         ri8 = R[8];
  43
  44         pri0 = prevR[0];
  45         pri1 = prevR[1];
  46         pri2 = prevR[2];
  47         pri3 = prevR[3];
  48         pri4 = prevR[4];
  49         pri5 = prevR[5];
  50         pri6 = prevR[6];
  51         pri7 = prevR[7];
  52         pri8 = prevR[8];
  53
  54         rd1 = pri0 * ri1 + pri3 * ri4 + pri6 * ri7;
  55         rd4 = pri1 * ri1 + pri4 * ri4 + pri7 * ri7;
  56         rd6 = pri2 * ri0 + pri5 * ri3 + pri8 * ri6;
  57         rd7 = pri2 * ri1 + pri5 * ri4 + pri8 * ri7;
  58         rd8 = pri2 * ri2 + pri5 * ri5 + pri8 * ri8;
  59
  60         angleChange[0] = atan2(rd1, rd4);
  61         angleChange[1] = asin(-rd7);
  62         angleChange[2] = atan2(-rd6, rd8);
  63
  64         return 0;
  65 }
  66 int quatToMatrix(float *quat, float *R)
  67 {
  68         if (quat == NULL || R == NULL)
  69                 return -1;
  70
  71         float q0 = quat[0];
  72         float q1 = quat[1];
  73         float q2 = quat[2];
  74         float q3 = quat[3];
  75
  76         float sq_q1 = 2 * q1 * q1;
  77         float sq_q2 = 2 * q2 * q2;
  78         float sq_q3 = 2 * q3 * q3;
  79         float q1_q2 = 2 * q1 * q2;
  80         float q3_q0 = 2 * q3 * q0;
  81         float q1_q3 = 2 * q1 * q3;
  82         float q2_q0 = 2 * q2 * q0;
  83         float q2_q3 = 2 * q2 * q3;
  84         float q1_q0 = 2 * q1 * q0;
  85
  86         R[0] = 1 - sq_q2 - sq_q3;
  87         R[1] = q1_q2 - q3_q0;
  88         R[2] = q1_q3 + q2_q0;
  89         R[3] = q1_q2 + q3_q0;
  90         R[4] = 1 - sq_q1 - sq_q3;
  91         R[5] = q2_q3 - q1_q0;
  92         R[6] = q1_q3 - q2_q0;
  93         R[7] = q2_q3 + q1_q0;
  94         R[8] = 1 - sq_q1 - sq_q2;
  95
  96         return 0;
  97 }
  98
  99 int matrixToQuat(float *mat, float *q)
 100 {
 101         if (q == NULL || mat == NULL)
 102                 return -1;
 103
 104         const float Hx = mat[0];
 105         const float My = mat[4];
 106         const float Az = mat[8];
 107         q[0] = sqrtf(clamp(Hx - My - Az + 1) * 0.25f);
 108         q[1] = sqrtf(clamp(-Hx + My - Az + 1) * 0.25f);
 109         q[2] = sqrtf(clamp(-Hx - My + Az + 1) * 0.25f);
 110         q[3] = sqrtf(clamp(Hx + My + Az + 1) * 0.25f);
 111         q[0] = copysignf(q[0], mat[7] - mat[5]);
 112         q[1] = copysignf(q[1], mat[2] - mat[6]);
 113         q[2] = copysignf(q[2], mat[3] - mat[1]);
 114
 115         return 0;
 116 }
 117
 118 int getRotationMatrix(float *accel, float *geo, float *R, float *I)
 119 {
 120         if (accel == NULL || geo == NULL || R == NULL || I == NULL)
 121                 return -1;
 122
 123         float Ax = accel[0];
 124         float Ay = accel[1];
 125         float Az = accel[2];
 126         float Ex = geo[0];
 127         float Ey = geo[1];
 128         float Ez = geo[2];
 129         float Hx = Ey*Az - Ez*Ay;
 130         float Hy = Ez*Ax - Ex*Az;
 131         float Hz = Ex*Ay - Ey*Ax;
 132         float normH =  (float)sqrt(Hx*Hx + Hy*Hy + Hz*Hz);
 133         if (normH < 0.1f)
 134                 return -1;
 135
 136         float invH = 1.0f / normH;
 137         Hx *= invH;
 138         Hy *= invH;
 139         Hz *= invH;
 140         float invA = 1.0f / (float)sqrt(Ax*Ax + Ay*Ay + Az*Az);
 141         Ax *= invA;
 142         Ay *= invA;
 143         Az *= invA;
 144         float Mx = Ay*Hz - Az*Hy;
 145         float My = Az*Hx - Ax*Hz;
 146         float Mz = Ax*Hy - Ay*Hx;
 147
 148         R[0] = Hx;  R[1] = Hy;  R[2] = Hz;
 149         R[3] = Mx;  R[4] = My;  R[5] = Mz;
 150         R[6] = Ax;  R[7] = Ay;  R[8] = Az;
 151
 152         float invE = 1.0 / (float)sqrt(Ex*Ex + Ey*Ey + Ez*Ez);
 153         float c = (Ex*Mx + Ey*My + Ez*Mz) * invE;
 154         float s = (Ex*Ax + Ey*Ay + Ez*Az) * invE;
 155
 156         I[0] = 1;     I[1] = 0;     I[2] = 0;
 157         I[3] = 0;     I[4] = c;     I[5] = s;
 158         I[6] = 0;     I[7] = -s;    I[8] = c;
 159
 160         return 0;
 161 }
 162
 163
 164 int remapCoordinateSystem(float *inR, int X, int Y, float *outR)
 165 {
 166         if (inR == NULL || outR == NULL)
 167                 return -1;
 168
 169         if ((X & 0x7C) != 0 || (Y & 0x7C) != 0)
 170                 return -1;   /* invalid parameter */
 171         if (((X & 0x3) == 0) || ((Y & 0x3) == 0))
 172                 return -1;   /* no axis specified */
 173         if ((X & 0x3) == (Y & 0x3))
 174                 return -1;   /* same axis specified */
 175
 176         int Z = X ^ Y;
 177         int x = (X & 0x3)-1;
 178         int y = (Y & 0x3)-1;
 179         int z = (Z & 0x3)-1;
 180
 181         int axis_y = (z+1)%3;
 182         int axis_z = (z+2)%3;
 183         if (((x^axis_y)|(y^axis_z)) != 0)
 184                 Z ^= 0x80;
 185
 186         char sx = (X >= 0x80) ? 1 : 0;
 187         char sy = (Y >= 0x80) ? 1 : 0;
 188         char sz = (Z >= 0x80) ? 1 : 0;
 189
 190         int i = 0 , j = 0;
 191         for (j = 0 ; j < 3 ; j++) {
 192                 int offset = j * 3;
 193                 for (i = 0 ; i < 3 ; i++) {
 194                         if (x == i)   outR[offset+i] = sx ? -inR[offset+0] : inR[offset+0];
 195                         if (y == i)   outR[offset+i] = sy ? -inR[offset+1] : inR[offset+1];
 196                         if (z == i)   outR[offset+i] = sz ? -inR[offset+2] : inR[offset+2];
 197                 }
 198         }
 199         return 0;
 200 }
 201