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
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
|
/* 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_EC_MATH_UTIL_H
#define __CROS_EC_MATH_UTIL_H
#include <stdint.h>
#ifdef CONFIG_FPU
typedef float fp_t;
typedef float fp_inter_t;
/* Conversion to/from fixed-point */
#define INT_TO_FP(x) ((float)(x))
#define FP_TO_INT(x) ((int32_t)(x))
/* Float to fixed-point, only for compile-time constants and unit tests */
#define FLOAT_TO_FP(x) ((float)(x))
/* Fixed-point to float, for unit tests */
#define FP_TO_FLOAT(x) ((float)(x))
#else
/* Fixed-point type */
typedef int32_t fp_t;
/* Type used during fp operation */
typedef int64_t fp_inter_t;
/* Number of bits left of decimal point for fixed-point */
#define FP_BITS 16
/* Conversion to/from fixed-point */
#define INT_TO_FP(x) ((fp_t)(x) << FP_BITS)
#define FP_TO_INT(x) ((int32_t)((x) >> FP_BITS))
/* Float to fixed-point, only for compile-time constants and unit tests */
#define FLOAT_TO_FP(x) ((fp_t)((x) * (float)(1<<FP_BITS)))
/* Fixed-point to float, for unit tests */
#define FP_TO_FLOAT(x) ((float)(x) / (float)(1<<FP_BITS))
#endif
/*
* Fixed-point addition and subtraction can be done directly, because they
* work identically.
*/
#ifdef CONFIG_FPU
static inline fp_t fp_mul(fp_t a, fp_t b)
{
return a * b;
}
static inline fp_t fp_div(fp_t a, fp_t b)
{
return a / b;
}
#else
/**
* Multiplication - return (a * b)
*/
static inline fp_t fp_mul(fp_t a, fp_t b)
{
return (fp_t)(((fp_inter_t)a * b) >> FP_BITS);
}
/**
* Division - return (a / b)
*/
static inline fp_t fp_div(fp_t a, fp_t b)
{
return (fp_t)(((fp_inter_t)a << FP_BITS) / b);
}
#endif
/**
* Square (a * a)
*/
static inline fp_t fp_sq(fp_t a)
{
return fp_mul(a, a);
}
/**
* Absolute value
*/
static inline fp_t fp_abs(fp_t a)
{
return (a >= INT_TO_FP(0) ? a : -a);
}
/*
* Fixed point matrix
*
* Note that constant matrices MUST be initialized using FLOAT_TO_FP()
* or INT_TO_FP() for all non-zero values.
*/
typedef fp_t mat33_fp_t[3][3];
/* Integer vector */
typedef int intv3_t[3];
/* For vectors, define which coordinates are in which location. */
enum {
X, Y, Z, W
};
/*
* Return absolute value of x. Note that as a macro expansion, this may have
* side effects if x includes function calls, which is why inline functions
* like fp_abs() are preferred.
*/
#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.
*/
fp_t arc_cos(fp_t x);
/**
* Calculate the dot product of 2 vectors.
*/
fp_inter_t dot_product(const intv3_t v1, const intv3_t v2);
/*
* Calculate the dot product of 2 vectors,
*
* Assume the result vector components fits in 32bit.
*/
void cross_product(const intv3_t v1, const intv3_t v2, intv3_t v);
/**
* Scale a vector by fixed point constant.
*/
void vector_scale(intv3_t v, fp_t s);
/**
* Find the cosine of the angle between two vectors.
*
* The implementation assumes no vector component is greater than
* 2^(31 - FP_BITS/2). For example, 2^23, for FP_BITS=16.
*
* @param v1
* @param v2
*
* @return Cosine of the angle between v1 and v2.
*/
fp_t cosine_of_angle_diff(const intv3_t v1, const intv3_t v2);
/**
* Rotate vector v by rotation matrix R.
*
* @param v Vector to be rotated.
* @param R Rotation matrix.
* @param res Resultant vector.
*/
void rotate(const intv3_t v, const mat33_fp_t R, intv3_t res);
/**
* Rotate vector v by rotation matrix R^-1.
*
* @param v Vector to be rotated.
* @param R Rotation matrix.
* @param res Resultant vector.
*/
void rotate_inv(const intv3_t v, const mat33_fp_t R, intv3_t res);
#endif /* __CROS_EC_MATH_UTIL_H */
|
