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
|
/* 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.
*/
/* Common math functions. */
#include "common.h"
#include "math.h"
#include "math_util.h"
#include "util.h"
/* For cosine lookup table, define the increment and the size of the table. */
#define COSINE_LUT_INCR_DEG 5
#define COSINE_LUT_SIZE ((180 / COSINE_LUT_INCR_DEG) + 1)
/* Lookup table for the value of cosine from 0 degrees to 180 degrees. */
static const float cos_lut[] = {
1.00000, 0.99619, 0.98481, 0.96593, 0.93969,
0.90631, 0.86603, 0.81915, 0.76604, 0.70711,
0.64279, 0.57358, 0.50000, 0.42262, 0.34202,
0.25882, 0.17365, 0.08716, 0.00000, -0.08716,
-0.17365, -0.25882, -0.34202, -0.42262, -0.50000,
-0.57358, -0.64279, -0.70711, -0.76604, -0.81915,
-0.86603, -0.90631, -0.93969, -0.96593, -0.98481,
-0.99619, -1.00000,
};
BUILD_ASSERT(ARRAY_SIZE(cos_lut) == COSINE_LUT_SIZE);
float arc_cos(float x)
{
int i;
/* Cap x if out of range. */
if (x < -1.0)
x = -1.0;
else if (x > 1.0)
x = 1.0;
/*
* Increment through lookup table to find index and then linearly
* interpolate for precision.
*/
/* TODO(crosbug.com/p/25600): Optimize with binary search. */
for (i = 0; i < COSINE_LUT_SIZE-1; i++)
if (x >= cos_lut[i+1])
return COSINE_LUT_INCR_DEG *
(i + (cos_lut[i] - x) / (cos_lut[i] - cos_lut[i+1]));
/*
* Shouldn't be possible to get here because inputs are clipped to
* [-1, 1] and the cos_lut[] table goes over the same range. If we
* are here, throw an assert.
*/
ASSERT(0);
return 0;
}
int vector_magnitude(const vector_3_t v)
{
return sqrtf(SQ(v[0]) + SQ(v[1]) + SQ(v[2]));
}
float cosine_of_angle_diff(const vector_3_t v1, const vector_3_t v2)
{
int dotproduct;
float denominator;
/*
* Angle between two vectors is acos(A dot B / |A|*|B|). To return
* cosine of angle between vectors, then don't do acos operation.
*/
dotproduct = v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
denominator = vector_magnitude(v1) * vector_magnitude(v2);
/* Check for divide by 0 although extremely unlikely. */
if (ABS(denominator) < 0.01F)
return 0.0;
return (float)dotproduct / (denominator);
}
/*
* rotate a vector v
* - support input v and output res are the same vector
*/
void rotate(const vector_3_t v, const matrix_3x3_t R,
vector_3_t res)
{
vector_3_t t;
/* copy input v to temp vector t */
t[0] = v[0];
t[1] = v[1];
t[2] = v[2];
/* start rotate */
res[0] = t[0] * R[0][0] +
t[1] * R[1][0] +
t[2] * R[2][0];
res[1] = t[0] * R[0][1] +
t[1] * R[1][1] +
t[2] * R[2][1];
res[2] = t[0] * R[0][2] +
t[1] * R[1][2] +
t[2] * R[2][2];
}
|
