4 * Copyright (c) 2014 Samsung Electronics Co., Ltd.
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
10 * http://www.apache.org/licenses/LICENSE-2.0
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.
23 #include "fusion_util.h"
25 #define RAD2DEGREE (180/M_PI)
28 #define ARCTAN(x, y) ((x) == 0 ? 0 : (y) != 0 ? atan2((x), (y)) : (x) > 0 ? M_PI/2.0 : -M_PI/2.0)
30 static float clamp(float v)
32 return (v < 0) ? 0.0 : v;
35 int quat_to_matrix(const float *quat, float *R)
37 if(quat == NULL || R == NULL)
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;
55 R[0] = 1 - sq_q2 - sq_q3;
59 R[4] = 1 - sq_q1 - sq_q3;
63 R[8] = 1 - sq_q1 - sq_q2;
68 int matrix_to_quat(const float *R, float *quat)
70 if (R == NULL || quat == NULL)
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]);
87 int calculate_rotation_matrix(float *accel, float *geo, float *R, float *I)
89 if (accel == NULL || geo == NULL || R == NULL || I == NULL)
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);
105 float invH = 1.0f / normH;
109 float invA = 1.0f / (float)sqrt(Ax*Ax + Ay*Ay + Az*Az);
113 float Mx = Ay*Hz - Az*Hy;
114 float My = Az*Hx - Ax*Hz;
115 float Mz = Ax*Hy - Ay*Hx;
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;
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;
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;
132 int quat_to_orientation(const float *quat, float &azimuth, float &pitch, float &roll)
138 error = quat_to_matrix(quat, R);
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]);
148 float a = fabs(yxz_x / HALF);
151 float p = (fabs(yxz_y) / HALF - 1.0);
156 float v = 1 + (1 - a) / a * p;
161 if (yxz_x * yxz_y > 0) {
162 if (yxz_z > 0 && xyz_z < 0)
165 if (yxz_z < 0 && xyz_z > 0)
169 g[0] = (1 - a * v) * yxz_z + (a * v) * xyz_z;
176 else if (tmp < -1.0f)
186 if ((fabs(R[7]) > QUAT))
187 g[2] = (float) atan2f(R[6], R[7]);
189 g[2] = (float) atan2f(R[6], R[8]);
193 else if (g[2] < -HALF)