src/fusion-sensor/fusion_util.cpp

   1 /*
   2  * sensord
   3  *
   4  * Copyright (c) 2014 Samsung Electronics Co., Ltd.
   5  *
   6  * Licensed under the Apache License, Version 2.0 (the "License");
   7  * you may not use this file except in compliance with the License.
   8  * You may obtain a copy of the License at
   9  *
  10  * http://www.apache.org/licenses/LICENSE-2.0
  11  *
  12  * Unless required by applicable law or agreed to in writing, software
  13  * distributed under the License is distributed on an "AS IS" BASIS,
  14  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  15  * See the License for the specific language governing permissions and
  16  * limitations under the License.
  17  *
  18  */
  19 #include <errno.h>
  20 #include <math.h>
  21 #include <stdlib.h>
  22
  23 #include "fusion_util.h"
  24
  25 #define RAD2DEGREE (180/M_PI)
  26 #define QUAT (M_PI/4)
  27 #define HALF (M_PI/2)
  28 #define ARCTAN(x, y) ((x) == 0 ? 0 : (y) != 0 ? atan2((x), (y)) : (x) > 0 ? M_PI/2.0 : -M_PI/2.0)
  29
  30 static float clamp(float v)
  31 {
  32         return (v < 0) ? 0.0 : v;
  33 }
  34
  35 int quat_to_matrix(const float *quat, float *R)
  36 {
  37         if (quat == NULL || R == NULL)
  38                 return -EINVAL;
  39
  40         float q0 = quat[3];
  41         float q1 = quat[0];
  42         float q2 = quat[1];
  43         float q3 = quat[2];
  44
  45         float sq_q1 = 2 * q1 * q1;
  46         float sq_q2 = 2 * q2 * q2;
  47         float sq_q3 = 2 * q3 * q3;
  48         float q1_q2 = 2 * q1 * q2;
  49         float q3_q0 = 2 * q3 * q0;
  50         float q1_q3 = 2 * q1 * q3;
  51         float q2_q0 = 2 * q2 * q0;
  52         float q2_q3 = 2 * q2 * q3;
  53         float q1_q0 = 2 * q1 * q0;
  54
  55         R[0] = 1 - sq_q2 - sq_q3;
  56         R[1] = q1_q2 - q3_q0;
  57         R[2] = q1_q3 + q2_q0;
  58         R[3] = q1_q2 + q3_q0;
  59         R[4] = 1 - sq_q1 - sq_q3;
  60         R[5] = q2_q3 - q1_q0;
  61         R[6] = q1_q3 - q2_q0;
  62         R[7] = q2_q3 + q1_q0;
  63         R[8] = 1 - sq_q1 - sq_q2;
  64
  65         return 0;
  66 }
  67
  68 int matrix_to_quat(const float *R, float *quat)
  69 {
  70         if (R == NULL || quat == NULL)
  71                 return -EINVAL;
  72
  73         const float Hx = R[0];
  74         const float My = R[4];
  75         const float Az = R[8];
  76         quat[0] = sqrtf(clamp(Hx - My - Az + 1) * 0.25f);
  77         quat[1] = sqrtf(clamp(-Hx + My - Az + 1) * 0.25f);
  78         quat[2] = sqrtf(clamp(-Hx - My + Az + 1) * 0.25f);
  79         quat[3] = sqrtf(clamp(Hx + My + Az + 1) * 0.25f);
  80         quat[0] = copysignf(quat[0], R[7] - R[5]);
  81         quat[1] = copysignf(quat[1], R[2] - R[6]);
  82         quat[2] = copysignf(quat[2], R[3] - R[1]);
  83
  84         return 0;
  85 }
  86
  87 int calculate_rotation_matrix(float *accel, float *geo, float *R, float *I)
  88 {
  89         if (accel == NULL || geo == NULL || R == NULL || I == NULL)
  90                 return -EINVAL;
  91
  92         float Ax = accel[0];
  93         float Ay = accel[1];
  94         float Az = accel[2];
  95         float Ex = geo[0];
  96         float Ey = geo[1];
  97         float Ez = geo[2];
  98         float Hx = Ey*Az - Ez*Ay;
  99         float Hy = Ez*Ax - Ex*Az;
 100         float Hz = Ex*Ay - Ey*Ax;
 101         float normH =  (float)sqrt(Hx*Hx + Hy*Hy + Hz*Hz);
 102         if (normH < 0.1f)
 103                 return -EINVAL;
 104
 105         float invH = 1.0f / normH;
 106         Hx *= invH;
 107         Hy *= invH;
 108         Hz *= invH;
 109         float invA = 1.0f / (float)sqrt(Ax*Ax + Ay*Ay + Az*Az);
 110         Ax *= invA;
 111         Ay *= invA;
 112         Az *= invA;
 113         float Mx = Ay*Hz - Az*Hy;
 114         float My = Az*Hx - Ax*Hz;
 115         float Mz = Ax*Hy - Ay*Hx;
 116
 117         R[0] = Hx;  R[1] = Hy;  R[2] = Hz;
 118         R[3] = Mx;  R[4] = My;  R[5] = Mz;
 119         R[6] = Ax;  R[7] = Ay;  R[8] = Az;
 120
 121         float invE = 1.0 / (float)sqrt(Ex*Ex + Ey*Ey + Ez*Ez);
 122         float c = (Ex*Mx + Ey*My + Ez*Mz) * invE;
 123         float s = (Ex*Ax + Ey*Ay + Ez*Az) * invE;
 124
 125         I[0] = 1;     I[1] = 0;     I[2] = 0;
 126         I[3] = 0;     I[4] = c;     I[5] = s;
 127         I[6] = 0;     I[7] = -s;    I[8] = c;
 128
 129         return 0;
 130 }
 131
 132 int quat_to_orientation(const float *quat, float &azimuth, float &pitch, float &roll)
 133 {
 134         int error;
 135         float g[3];
 136         float R[9];
 137
 138         error = quat_to_matrix(quat, R);
 139
 140         if (error < 0)
 141                 return error;
 142
 143         float xyz_z = ARCTAN(R[3], R[0]);
 144         float yxz_x = asinf(R[7]);
 145         float yxz_y = ARCTAN(-R[6], R[8]);
 146         float yxz_z = ARCTAN(-R[1], R[4]);
 147
 148         float a = fabs(yxz_x / HALF);
 149         a = a * a;
 150
 151         float p = (fabs(yxz_y) / HALF - 1.0);
 152
 153         if (p < 0)
 154                 p = 0;
 155
 156         float v = 1 + (1 - a) / a * p;
 157
 158         if (v > 20)
 159                 v = 20;
 160
 161         if (yxz_x * yxz_y > 0) {
 162                 if (yxz_z > 0 && xyz_z < 0)
 163                         xyz_z += M_PI * 2;
 164         } else {
 165                 if (yxz_z < 0 && xyz_z > 0)
 166                         xyz_z -= M_PI * 2;
 167         }
 168
 169         g[0] = (1 - a * v) * yxz_z + (a * v) * xyz_z;
 170         g[0] *= -1;
 171
 172         float tmp = R[7];
 173
 174         if (tmp > 1.0f)
 175                 tmp = 1.0f;
 176         else if (tmp < -1.0f)
 177                 tmp = -1.0f;
 178
 179         g[1] = -asinf(tmp);
 180         if (R[8] < 0)
 181                 g[1] = M_PI - g[1];
 182
 183         if (g[1] > M_PI)
 184                 g[1] -= M_PI * 2;
 185
 186         if ((fabs(R[7]) > QUAT))
 187                 g[2] = (float) atan2f(R[6], R[7]);
 188         else
 189                 g[2] = (float) atan2f(R[6], R[8]);
 190
 191         if (g[2] > HALF)
 192                 g[2] = M_PI - g[2];
 193         else if (g[2] < -HALF)
 194                 g[2] = -M_PI - g[2];
 195
 196         g[0] *= RAD2DEGREE;
 197         g[1] *= RAD2DEGREE;
 198         g[2] *= RAD2DEGREE;
 199
 200         if (g[0] < 0)
 201                 g[0] += 360;
 202
 203         azimuth = g[0];
 204         pitch = g[1];
 205         roll = g[2];
 206
 207         return 0;
 208 }
 209