1895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* 2895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall $License: 3895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Copyright 2011 InvenSense, Inc. 4895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 5895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Licensed under the Apache License, Version 2.0 (the "License"); 6895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall you may not use this file except in compliance with the License. 7895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall You may obtain a copy of the License at 8895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 9895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall http://www.apache.org/licenses/LICENSE-2.0 10895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 11895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Unless required by applicable law or agreed to in writing, software 12895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall distributed under the License is distributed on an "AS IS" BASIS, 13895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall See the License for the specific language governing permissions and 15895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall limitations under the License. 16895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall $ 17895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 18895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlmath.h" 19895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlMathFunc.h" 20895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlinclude.h" 21895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 22895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 23895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Performs one iteration of the filter, generating a new y(0) 24895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 1 / N / N \\ 25895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * y(0) = ---- * |SUM b(k) * x(k) - | SUM a(k) * y(k)|| for N = length 26895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * a(0) \k=0 \ k=1 // 27895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 28895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The filters A and B should be (sizeof(long) * state->length). 29895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The state variables x and y should be (sizeof(long) * (state->length - 1)) 30895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 31895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * The state variables x and y should be initialized prior to the first call 32895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 33895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param state Contains the length of the filter, filter coefficients and 34895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * filter state variables x and y. 35895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param x New input into the filter. 36895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 37895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_filter_long(struct filter_long *state, long x) 38895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 39895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall const long *b = state->b; 40895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall const long *a = state->a; 41895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long length = state->length; 42895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long long tmp; 43895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int ii; 44895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 45895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* filter */ 46895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = (long long)x *(b[0]); 47895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (ii = 0; ii < length - 1; ii++) { 48895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += ((long long)state->x[ii] * (long long)(b[ii + 1])); 49895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 50895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (ii = 0; ii < length - 1; ii++) { 51895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp -= ((long long)state->y[ii] * (long long)(a[ii + 1])); 52895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 53895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp /= (long long)a[0]; 54895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 55895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* Delay */ 56895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (ii = length - 2; ii > 0; ii--) { 57895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall state->y[ii] = state->y[ii - 1]; 58895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall state->x[ii] = state->x[ii - 1]; 59895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 60895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* New values */ 61895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall state->y[0] = (long)tmp; 62895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall state->x[0] = x; 63895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 64895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 65895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** Performs a multiply and shift by 29. These are good functions to write in assembly on 66895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * with devices with small memory where you want to get rid of the long long which some 67895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * assemblers don't handle well 68895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] a 69895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] b 70895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return ((long long)a*b)>>29 71895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 72895401859313187f15a800e62d43e6bcbf48fadaJP Abgralllong inv_q29_mult(long a, long b) 73895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 74895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long long temp; 75895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long result; 76895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall temp = (long long)a *b; 77895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = (long)(temp >> 29); 78895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 79895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 80895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 81895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** Performs a multiply and shift by 30. These are good functions to write in assembly on 82895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * with devices with small memory where you want to get rid of the long long which some 83895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * assemblers don't handle well 84895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] a 85895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] b 86895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return ((long long)a*b)>>30 87895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 88895401859313187f15a800e62d43e6bcbf48fadaJP Abgralllong inv_q30_mult(long a, long b) 89895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 90895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long long temp; 91895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long result; 92895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall temp = (long long)a *b; 93895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = (long)(temp >> 30); 94895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 95895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 96895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 97895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_q_mult(const long *q1, const long *q2, long *qProd) 98895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 99895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 100895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qProd[0] = (long)(((long long)q1[0] * q2[0] - (long long)q1[1] * q2[1] - 101895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (long long)q1[2] * q2[2] - 102895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (long long)q1[3] * q2[3]) >> 30); 103895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qProd[1] = 104895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (int)(((long long)q1[0] * q2[1] + (long long)q1[1] * q2[0] + 105895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (long long)q1[2] * q2[3] - (long long)q1[3] * q2[2]) >> 30); 106895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qProd[2] = 107895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (long)(((long long)q1[0] * q2[2] - (long long)q1[1] * q2[3] + 108895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (long long)q1[2] * q2[0] + (long long)q1[3] * q2[1]) >> 30); 109895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qProd[3] = 110895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (long)(((long long)q1[0] * q2[3] + (long long)q1[1] * q2[2] - 111895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (long long)q1[2] * q2[1] + (long long)q1[3] * q2[0]) >> 30); 112895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 113895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 114895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_q_add(long *q1, long *q2, long *qSum) 115895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 116895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 117895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qSum[0] = q1[0] + q2[0]; 118895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qSum[1] = q1[1] + q2[1]; 119895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qSum[2] = q1[2] + q2[2]; 120895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qSum[3] = q1[3] + q2[3]; 121895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 122895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 123895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_q_normalize(long *q) 124895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 125895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 126895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall double normSF = 0; 127895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int i; 128895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 4; i++) { 129895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall normSF += ((double)q[i]) / 1073741824L * ((double)q[i]) / 1073741824L; 130895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 131895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (normSF > 0) { 132895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall normSF = 1 / sqrt(normSF); 133895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 4; i++) { 134895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall q[i] = (int)((double)q[i] * normSF); 135895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 136895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 137895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall q[0] = 1073741824L; 138895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall q[1] = 0; 139895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall q[2] = 0; 140895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall q[3] = 0; 141895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 142895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 143895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 144895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_q_invert(const long *q, long *qInverted) 145895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 146895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 147895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qInverted[0] = q[0]; 148895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qInverted[1] = -q[1]; 149895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qInverted[2] = -q[2]; 150895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qInverted[3] = -q[3]; 151895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 152895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 153895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_q_multf(const float *q1, const float *q2, float *qProd) 154895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 155895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 156895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qProd[0] = (q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]); 157895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qProd[1] = (q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]); 158895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qProd[2] = (q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]); 159895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qProd[3] = (q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]); 160895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 161895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 162895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_q_addf(float *q1, float *q2, float *qSum) 163895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 164895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 165895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qSum[0] = q1[0] + q2[0]; 166895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qSum[1] = q1[1] + q2[1]; 167895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qSum[2] = q1[2] + q2[2]; 168895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qSum[3] = q1[3] + q2[3]; 169895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 170895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 171895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_q_normalizef(float *q) 172895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 173895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 174895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float normSF = 0; 175895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float xHalf = 0; 176895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); 177895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (normSF < 2) { 178895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall xHalf = 0.5f * normSF; 179895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall normSF = normSF * (1.5f - xHalf * normSF * normSF); 180895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall normSF = normSF * (1.5f - xHalf * normSF * normSF); 181895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall normSF = normSF * (1.5f - xHalf * normSF * normSF); 182895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall normSF = normSF * (1.5f - xHalf * normSF * normSF); 183895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall q[0] *= normSF; 184895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall q[1] *= normSF; 185895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall q[2] *= normSF; 186895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall q[3] *= normSF; 187895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 188895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall q[0] = 1.0; 189895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall q[1] = 0.0; 190895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall q[2] = 0.0; 191895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall q[3] = 0.0; 192895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 193895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); 194895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 195895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 196895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** Performs a length 4 vector normalization with a square root. 197895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* @param[in,out] vector to normalize. Returns [1,0,0,0] is magnitude is zero. 198895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 199895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_q_norm4(float *q) 200895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 201895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float mag; 202895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); 203895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mag) { 204895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall q[0] /= mag; 205895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall q[1] /= mag; 206895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall q[2] /= mag; 207895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall q[3] /= mag; 208895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 209895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall q[0] = 1.f; 210895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall q[1] = 0.f; 211895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall q[2] = 0.f; 212895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall q[3] = 0.f; 213895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 214895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 215895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 216895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_q_invertf(const float *q, float *qInverted) 217895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 218895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 219895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qInverted[0] = q[0]; 220895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qInverted[1] = -q[1]; 221895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qInverted[2] = -q[2]; 222895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qInverted[3] = -q[3]; 223895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 224895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 225895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 226895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Converts a quaternion to a rotation matrix. 227895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] quat 4-element quaternion in fixed point. One is 2^30. 228895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[out] rot Rotation matrix in fixed point. One is 2^30. The 229895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * First 3 elements of the rotation matrix, represent 230895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the first row of the matrix. Rotation matrix multiplied 231895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * by a 3 element column vector transform a vector from Body 232895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * to World. 233895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 234895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_quaternion_to_rotation(const long *quat, long *rot) 235895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 236895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rot[0] = 237895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_q29_mult(quat[1], quat[1]) + inv_q29_mult(quat[0], 238895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall quat[0]) - 1073741824L; 239895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rot[1] = inv_q29_mult(quat[1], quat[2]) - inv_q29_mult(quat[3], quat[0]); 240895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rot[2] = inv_q29_mult(quat[1], quat[3]) + inv_q29_mult(quat[2], quat[0]); 241895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rot[3] = inv_q29_mult(quat[1], quat[2]) + inv_q29_mult(quat[3], quat[0]); 242895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rot[4] = 243895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_q29_mult(quat[2], quat[2]) + inv_q29_mult(quat[0], 244895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall quat[0]) - 1073741824L; 245895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rot[5] = inv_q29_mult(quat[2], quat[3]) - inv_q29_mult(quat[1], quat[0]); 246895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rot[6] = inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]); 247895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rot[7] = inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]); 248895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rot[8] = 249895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0], 250895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall quat[0]) - 1073741824L; 251895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 252895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 253895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** Converts a 32-bit long to a big endian byte stream */ 254895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallunsigned char *inv_int32_to_big8(long x, unsigned char *big8) 255895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 256895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall big8[0] = (unsigned char)((x >> 24) & 0xff); 257895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall big8[1] = (unsigned char)((x >> 16) & 0xff); 258895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall big8[2] = (unsigned char)((x >> 8) & 0xff); 259895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall big8[3] = (unsigned char)(x & 0xff); 260895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return big8; 261895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 262895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 263895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** Converts a big endian byte stream into a 32-bit long */ 264895401859313187f15a800e62d43e6bcbf48fadaJP Abgralllong inv_big8_to_int32(const unsigned char *big8) 265895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 266895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long x; 267895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall x = ((long)big8[0] << 24) | ((long)big8[1] << 16) | ((long)big8[2] << 8) | 268895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ((long)big8[3]); 269895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return x; 270895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 271895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 272895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** Converts a 16-bit short to a big endian byte stream */ 273895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallunsigned char *inv_int16_to_big8(short x, unsigned char *big8) 274895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 275895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall big8[0] = (unsigned char)((x >> 8) & 0xff); 276895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall big8[1] = (unsigned char)(x & 0xff); 277895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return big8; 278895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 279895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 280895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_matrix_det_inc(float *a, float *b, int *n, int x, int y) 281895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 282895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int k, l, i, j; 283895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0, k = 0; i < *n; i++, k++) { 284895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0, l = 0; j < *n; j++, l++) { 285895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (i == x) 286895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i++; 287895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (j == y) 288895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall j++; 289895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *(b + 10 * k + l) = *(a + 10 * i + j); 290895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 291895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 292895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *n = *n - 1; 293895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 294895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 295895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_matrix_det_incd(double *a, double *b, int *n, int x, int y) 296895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 297895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int k, l, i, j; 298895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0, k = 0; i < *n; i++, k++) { 299895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0, l = 0; j < *n; j++, l++) { 300895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (i == x) 301895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall i++; 302895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (j == y) 303895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall j++; 304895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *(b + 10 * k + l) = *(a + 10 * i + j); 305895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 306895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 307895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *n = *n - 1; 308895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 309895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 310895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallfloat inv_matrix_det(float *p, int *n) 311895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 312895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float d[10][10], sum = 0; 313895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int i, j, m; 314895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall m = *n; 315895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (*n == 2) 316895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return (*p ** (p + 11) - *(p + 1) ** (p + 10)); 317895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0, j = 0; j < m; j++) { 318895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *n = m; 319895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_matrix_det_inc(p, &d[0][0], n, i, j); 320895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sum = 321895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sum + *(p + 10 * i + j) * SIGNM(i + j) * inv_matrix_det(&d[0][0], 322895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall n); 323895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 324895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 325895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return (sum); 326895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 327895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 328895401859313187f15a800e62d43e6bcbf48fadaJP Abgralldouble inv_matrix_detd(double *p, int *n) 329895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 330895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall double d[10][10], sum = 0; 331895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int i, j, m; 332895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall m = *n; 333895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (*n == 2) 334895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return (*p ** (p + 11) - *(p + 1) ** (p + 10)); 335895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0, j = 0; j < m; j++) { 336895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *n = m; 337895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_matrix_det_incd(p, &d[0][0], n, i, j); 338895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sum = 339895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sum + *(p + 10 * i + j) * SIGNM(i + j) * inv_matrix_detd(&d[0][0], 340895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall n); 341895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 342895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 343895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return (sum); 344895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 345895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 346895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** Wraps angle from (-M_PI,M_PI] 347895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] ang Angle in radians to wrap 348895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return Wrapped angle from (-M_PI,M_PI] 349895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 350895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallfloat inv_wrap_angle(float ang) 351895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 352895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (ang > M_PI) 353895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return ang - 2 * (float)M_PI; 354895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else if (ang <= -(float)M_PI) 355895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return ang + 2 * (float)M_PI; 356895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else 357895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return ang; 358895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 359895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 360895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** Finds the minimum angle difference ang1-ang2 such that difference 361895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * is between [-M_PI,M_PI] 362895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] ang1 363895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param[in] ang2 364895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return angle difference ang1-ang2 365895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 366895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallfloat inv_angle_diff(float ang1, float ang2) 367895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 368895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float d; 369895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ang1 = inv_wrap_angle(ang1); 370895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ang2 = inv_wrap_angle(ang2); 371895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall d = ang1 - ang2; 372895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (d > M_PI) 373895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall d -= 2 * (float)M_PI; 374895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else if (d < -(float)M_PI) 375895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall d += 2 * (float)M_PI; 376895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return d; 377895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 378