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#ifndef INVENSENSE_INV_MATH_FUNC_H__ 19895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define INVENSENSE_INV_MATH_FUNC_H__ 20895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifdef INV_INCLUDE_LEGACY_HEADERS 21895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlMathFunc_legacy.h" 22895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 23895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 24895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define NUM_ROTATION_MATRIX_ELEMENTS (9) 25895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define ROT_MATRIX_SCALE_LONG (1073741824) 26895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define ROT_MATRIX_SCALE_FLOAT (1073741824.0f) 27895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define ROT_MATRIX_LONG_TO_FLOAT( longval ) \ 28895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ((float) ((longval) / ROT_MATRIX_SCALE_FLOAT )) 29895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define SIGNM(k)((int)(k)&1?-1:1) 30895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 31895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifdef __cplusplus 32895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallextern "C" { 33895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 34895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct filter_long { 35895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int length; 36895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall const long *b; 37895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall const long *a; 38895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long *x; 39895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long *y; 40895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall }; 41895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 42895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall void inv_filter_long(struct filter_long *state, long x); 43895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long inv_q29_mult(long a, long b); 44895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long inv_q30_mult(long a, long b); 45895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall void inv_q_mult(const long *q1, const long *q2, long *qProd); 46895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall void inv_q_add(long *q1, long *q2, long *qSum); 47895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall void inv_q_normalize(long *q); 48895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall void inv_q_invert(const long *q, long *qInverted); 49895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall void inv_q_multf(const float *q1, const float *q2, float *qProd); 50895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall void inv_q_addf(float *q1, float *q2, float *qSum); 51895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall void inv_q_normalizef(float *q); 52895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall void inv_q_norm4(float *q); 53895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall void inv_q_invertf(const float *q, float *qInverted); 54895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall void inv_quaternion_to_rotation(const long *quat, long *rot); 55895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char *inv_int32_to_big8(long x, unsigned char *big8); 56895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long inv_big8_to_int32(const unsigned char *big8); 57895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char *inv_int16_to_big8(short x, unsigned char *big8); 58895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float inv_matrix_det(float *p, int *n); 59895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y); 60895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall double inv_matrix_detd(double *p, int *n); 61895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y); 62895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float inv_wrap_angle(float ang); 63895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float inv_angle_diff(float ang1, float ang2); 64895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 65895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifdef __cplusplus 66895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 67895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 68895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif // INVENSENSE_INV_MATH_FUNC_H__ 69