1/*
2 $License:
3   Copyright 2011 InvenSense, Inc.
4
5 Licensed under the Apache License, Version 2.0 (the "License");
6 you may not use this file except in compliance with the License.
7 You may obtain a copy of the License at
8
9 http://www.apache.org/licenses/LICENSE-2.0
10
11 Unless required by applicable law or agreed to in writing, software
12 distributed under the License is distributed on an "AS IS" BASIS,
13 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 See the License for the specific language governing permissions and
15 limitations under the License.
16  $
17 */
18#include "mlmath.h"
19#include "mlMathFunc.h"
20#include "mlinclude.h"
21
22/**
23 * Performs one iteration of the filter, generating a new y(0)
24 *         1     / N                /  N             \\
25 * y(0) = ---- * |SUM b(k) * x(k) - | SUM a(k) * y(k)|| for N = length
26 *        a(0)   \k=0               \ k=1            //
27 *
28 * The filters A and B should be (sizeof(long) * state->length).
29 * The state variables x and y should be (sizeof(long) * (state->length - 1))
30 *
31 * The state variables x and y should be initialized prior to the first call
32 *
33 * @param state Contains the length of the filter, filter coefficients and
34 *              filter state variables x and y.
35 * @param x New input into the filter.
36 */
37void inv_filter_long(struct filter_long *state, long x)
38{
39    const long *b = state->b;
40    const long *a = state->a;
41    long length = state->length;
42    long long tmp;
43    int ii;
44
45    /* filter */
46    tmp = (long long)x *(b[0]);
47    for (ii = 0; ii < length - 1; ii++) {
48        tmp += ((long long)state->x[ii] * (long long)(b[ii + 1]));
49    }
50    for (ii = 0; ii < length - 1; ii++) {
51        tmp -= ((long long)state->y[ii] * (long long)(a[ii + 1]));
52    }
53    tmp /= (long long)a[0];
54
55    /* Delay */
56    for (ii = length - 2; ii > 0; ii--) {
57        state->y[ii] = state->y[ii - 1];
58        state->x[ii] = state->x[ii - 1];
59    }
60    /* New values */
61    state->y[0] = (long)tmp;
62    state->x[0] = x;
63}
64
65/** Performs a multiply and shift by 29. These are good functions to write in assembly on
66 * with devices with small memory where you want to get rid of the long long which some
67 * assemblers don't handle well
68 * @param[in] a
69 * @param[in] b
70 * @return ((long long)a*b)>>29
71*/
72long inv_q29_mult(long a, long b)
73{
74    long long temp;
75    long result;
76    temp = (long long)a *b;
77    result = (long)(temp >> 29);
78    return result;
79}
80
81/** Performs a multiply and shift by 30. These are good functions to write in assembly on
82 * with devices with small memory where you want to get rid of the long long which some
83 * assemblers don't handle well
84 * @param[in] a
85 * @param[in] b
86 * @return ((long long)a*b)>>30
87*/
88long inv_q30_mult(long a, long b)
89{
90    long long temp;
91    long result;
92    temp = (long long)a *b;
93    result = (long)(temp >> 30);
94    return result;
95}
96
97void inv_q_mult(const long *q1, const long *q2, long *qProd)
98{
99    INVENSENSE_FUNC_START;
100    qProd[0] = (long)(((long long)q1[0] * q2[0] - (long long)q1[1] * q2[1] -
101                       (long long)q1[2] * q2[2] -
102                       (long long)q1[3] * q2[3]) >> 30);
103    qProd[1] =
104        (int)(((long long)q1[0] * q2[1] + (long long)q1[1] * q2[0] +
105               (long long)q1[2] * q2[3] - (long long)q1[3] * q2[2]) >> 30);
106    qProd[2] =
107        (long)(((long long)q1[0] * q2[2] - (long long)q1[1] * q2[3] +
108                (long long)q1[2] * q2[0] + (long long)q1[3] * q2[1]) >> 30);
109    qProd[3] =
110        (long)(((long long)q1[0] * q2[3] + (long long)q1[1] * q2[2] -
111                (long long)q1[2] * q2[1] + (long long)q1[3] * q2[0]) >> 30);
112}
113
114void inv_q_add(long *q1, long *q2, long *qSum)
115{
116    INVENSENSE_FUNC_START;
117    qSum[0] = q1[0] + q2[0];
118    qSum[1] = q1[1] + q2[1];
119    qSum[2] = q1[2] + q2[2];
120    qSum[3] = q1[3] + q2[3];
121}
122
123void inv_q_normalize(long *q)
124{
125    INVENSENSE_FUNC_START;
126    double normSF = 0;
127    int i;
128    for (i = 0; i < 4; i++) {
129        normSF += ((double)q[i]) / 1073741824L * ((double)q[i]) / 1073741824L;
130    }
131    if (normSF > 0) {
132        normSF = 1 / sqrt(normSF);
133        for (i = 0; i < 4; i++) {
134            q[i] = (int)((double)q[i] * normSF);
135        }
136    } else {
137        q[0] = 1073741824L;
138        q[1] = 0;
139        q[2] = 0;
140        q[3] = 0;
141    }
142}
143
144void inv_q_invert(const long *q, long *qInverted)
145{
146    INVENSENSE_FUNC_START;
147    qInverted[0] = q[0];
148    qInverted[1] = -q[1];
149    qInverted[2] = -q[2];
150    qInverted[3] = -q[3];
151}
152
153void inv_q_multf(const float *q1, const float *q2, float *qProd)
154{
155    INVENSENSE_FUNC_START;
156    qProd[0] = (q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]);
157    qProd[1] = (q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]);
158    qProd[2] = (q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]);
159    qProd[3] = (q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]);
160}
161
162void inv_q_addf(float *q1, float *q2, float *qSum)
163{
164    INVENSENSE_FUNC_START;
165    qSum[0] = q1[0] + q2[0];
166    qSum[1] = q1[1] + q2[1];
167    qSum[2] = q1[2] + q2[2];
168    qSum[3] = q1[3] + q2[3];
169}
170
171void inv_q_normalizef(float *q)
172{
173    INVENSENSE_FUNC_START;
174    float normSF = 0;
175    float xHalf = 0;
176    normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
177    if (normSF < 2) {
178        xHalf = 0.5f * normSF;
179        normSF = normSF * (1.5f - xHalf * normSF * normSF);
180        normSF = normSF * (1.5f - xHalf * normSF * normSF);
181        normSF = normSF * (1.5f - xHalf * normSF * normSF);
182        normSF = normSF * (1.5f - xHalf * normSF * normSF);
183        q[0] *= normSF;
184        q[1] *= normSF;
185        q[2] *= normSF;
186        q[3] *= normSF;
187    } else {
188        q[0] = 1.0;
189        q[1] = 0.0;
190        q[2] = 0.0;
191        q[3] = 0.0;
192    }
193    normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
194}
195
196/** Performs a length 4 vector normalization with a square root.
197* @param[in,out] vector to normalize. Returns [1,0,0,0] is magnitude is zero.
198*/
199void inv_q_norm4(float *q)
200{
201    float mag;
202    mag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
203    if (mag) {
204        q[0] /= mag;
205        q[1] /= mag;
206        q[2] /= mag;
207        q[3] /= mag;
208    } else {
209        q[0] = 1.f;
210        q[1] = 0.f;
211        q[2] = 0.f;
212        q[3] = 0.f;
213    }
214}
215
216void inv_q_invertf(const float *q, float *qInverted)
217{
218    INVENSENSE_FUNC_START;
219    qInverted[0] = q[0];
220    qInverted[1] = -q[1];
221    qInverted[2] = -q[2];
222    qInverted[3] = -q[3];
223}
224
225/**
226 * Converts a quaternion to a rotation matrix.
227 * @param[in] quat 4-element quaternion in fixed point. One is 2^30.
228 * @param[out] rot Rotation matrix in fixed point. One is 2^30. The
229 *             First 3 elements of the rotation matrix, represent
230 *             the first row of the matrix. Rotation matrix multiplied
231 *             by a 3 element column vector transform a vector from Body
232 *             to World.
233 */
234void inv_quaternion_to_rotation(const long *quat, long *rot)
235{
236    rot[0] =
237        inv_q29_mult(quat[1], quat[1]) + inv_q29_mult(quat[0],
238                                                      quat[0]) - 1073741824L;
239    rot[1] = inv_q29_mult(quat[1], quat[2]) - inv_q29_mult(quat[3], quat[0]);
240    rot[2] = inv_q29_mult(quat[1], quat[3]) + inv_q29_mult(quat[2], quat[0]);
241    rot[3] = inv_q29_mult(quat[1], quat[2]) + inv_q29_mult(quat[3], quat[0]);
242    rot[4] =
243        inv_q29_mult(quat[2], quat[2]) + inv_q29_mult(quat[0],
244                                                      quat[0]) - 1073741824L;
245    rot[5] = inv_q29_mult(quat[2], quat[3]) - inv_q29_mult(quat[1], quat[0]);
246    rot[6] = inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]);
247    rot[7] = inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]);
248    rot[8] =
249        inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0],
250                                                      quat[0]) - 1073741824L;
251}
252
253/** Converts a 32-bit long to a big endian byte stream */
254unsigned char *inv_int32_to_big8(long x, unsigned char *big8)
255{
256    big8[0] = (unsigned char)((x >> 24) & 0xff);
257    big8[1] = (unsigned char)((x >> 16) & 0xff);
258    big8[2] = (unsigned char)((x >> 8) & 0xff);
259    big8[3] = (unsigned char)(x & 0xff);
260    return big8;
261}
262
263/** Converts a big endian byte stream into a 32-bit long */
264long inv_big8_to_int32(const unsigned char *big8)
265{
266    long x;
267    x = ((long)big8[0] << 24) | ((long)big8[1] << 16) | ((long)big8[2] << 8) |
268        ((long)big8[3]);
269    return x;
270}
271
272/** Converts a 16-bit short to a big endian byte stream */
273unsigned char *inv_int16_to_big8(short x, unsigned char *big8)
274{
275    big8[0] = (unsigned char)((x >> 8) & 0xff);
276    big8[1] = (unsigned char)(x & 0xff);
277    return big8;
278}
279
280void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y)
281{
282    int k, l, i, j;
283    for (i = 0, k = 0; i < *n; i++, k++) {
284        for (j = 0, l = 0; j < *n; j++, l++) {
285            if (i == x)
286                i++;
287            if (j == y)
288                j++;
289            *(b + 10 * k + l) = *(a + 10 * i + j);
290        }
291    }
292    *n = *n - 1;
293}
294
295void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y)
296{
297    int k, l, i, j;
298    for (i = 0, k = 0; i < *n; i++, k++) {
299        for (j = 0, l = 0; j < *n; j++, l++) {
300            if (i == x)
301                i++;
302            if (j == y)
303                j++;
304            *(b + 10 * k + l) = *(a + 10 * i + j);
305        }
306    }
307    *n = *n - 1;
308}
309
310float inv_matrix_det(float *p, int *n)
311{
312    float d[10][10], sum = 0;
313    int i, j, m;
314    m = *n;
315    if (*n == 2)
316        return (*p ** (p + 11) - *(p + 1) ** (p + 10));
317    for (i = 0, j = 0; j < m; j++) {
318        *n = m;
319        inv_matrix_det_inc(p, &d[0][0], n, i, j);
320        sum =
321            sum + *(p + 10 * i + j) * SIGNM(i + j) * inv_matrix_det(&d[0][0],
322                                                                    n);
323    }
324
325    return (sum);
326}
327
328double inv_matrix_detd(double *p, int *n)
329{
330    double d[10][10], sum = 0;
331    int i, j, m;
332    m = *n;
333    if (*n == 2)
334        return (*p ** (p + 11) - *(p + 1) ** (p + 10));
335    for (i = 0, j = 0; j < m; j++) {
336        *n = m;
337        inv_matrix_det_incd(p, &d[0][0], n, i, j);
338        sum =
339            sum + *(p + 10 * i + j) * SIGNM(i + j) * inv_matrix_detd(&d[0][0],
340                                                                     n);
341    }
342
343    return (sum);
344}
345
346/** Wraps angle from (-M_PI,M_PI]
347 * @param[in] ang Angle in radians to wrap
348 * @return Wrapped angle from (-M_PI,M_PI]
349 */
350float inv_wrap_angle(float ang)
351{
352    if (ang > M_PI)
353        return ang - 2 * (float)M_PI;
354    else if (ang <= -(float)M_PI)
355        return ang + 2 * (float)M_PI;
356    else
357        return ang;
358}
359
360/** Finds the minimum angle difference ang1-ang2 such that difference
361 * is between [-M_PI,M_PI]
362 * @param[in] ang1
363 * @param[in] ang2
364 * @return angle difference ang1-ang2
365 */
366float inv_angle_diff(float ang1, float ang2)
367{
368    float d;
369    ang1 = inv_wrap_angle(ang1);
370    ang2 = inv_wrap_angle(ang2);
371    d = ang1 - ang2;
372    if (d > M_PI)
373        d -= 2 * (float)M_PI;
374    else if (d < -(float)M_PI)
375        d += 2 * (float)M_PI;
376    return d;
377}
378