blob: ffdbec424bb0451772c3524049e65d64fd24249c (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
|
/* Copyright (c) 2014 The Chromium OS Authors. All rights reserved.
* Use of this source code is governed by a BSD-style license that can be
* found in the LICENSE file.
*/
/* Header file for common math functions. */
#ifndef __CROS_MATH_UTIL_H
#define __CROS_MATH_UTIL_H
typedef float matrix_3x3_t[3][3];
typedef int vector_3_t[3];
/* Some useful math functions. */
#define SQ(x) ((x) * (x))
#define ABS(x) ((x) >= 0 ? (x) : -(x))
/**
* Find acos(x) in degrees. Argument is clipped to [-1.0, 1.0].
*
* @param x
*
* @return acos(x) in degrees.
*/
float arc_cos(float x);
/**
* Find the cosine of the angle between two vectors.
*
* @param v1
* @param v2
*
* @return Cosine of the angle between v1 and v2.
*/
float cosine_of_angle_diff(const vector_3_t v1, const vector_3_t v2);
/**
* Rotate vector v by rotation matrix R.
*
* @param v Vector to be rotated.
* @param R Pointer to rotation matrix.
* @param res Pointer to the resultant vector.
*/
void rotate(const vector_3_t v, const matrix_3x3_t (* const R),
vector_3_t *res);
#ifdef CONFIG_ACCEL_CALIBRATE
/**
* Multiply two 3x3 matrices.
*
* @param m1
* @param m2
* @param res Pointer to resultant matrix R = a1*a2;
*/
void matrix_multiply(matrix_3x3_t *m1, matrix_3x3_t *m2, matrix_3x3_t *res);
/**
* Given an input matrix and an output matrix, solve for the rotation
* matrix to get from the input matrix to the output matrix. Note, that this
* operation is not guaranteed. In order to successfully calculate the rotation
* matrix, the input must be linearly independent so that the matrix can be
* inverted.
*
* This function solves the following matrix equation for R:
* in * R = out
*
* If input matrix is invertible the resulting rotation matrix is stored in R.
*
* @param in
* @param out
* @param R Pointer to resultant matrix.
*
* @return EC_SUCCESS if successful
*/
int solve_rotation_matrix(matrix_3x3_t *in, matrix_3x3_t *out, matrix_3x3_t *R);
/**
* Calculate magnitude of a vector.
*
* @param v Vector to be measured.
*
* @return Magnitued of vector v.
*/
int vector_magnitude(const vector_3_t v);
#endif
#endif /* __CROS_MATH_UTIL_H */
|
