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
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
|
/*
* Copyright (C) 2016 The Android Open Source Project
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
/////////////////////////////////////////////////////////////////////////
/*
* This module contains vector math utilities for the following datatypes:
* -) Vec3 structures for 3-dimensional vectors
* -) Vec4 structures for 4-dimensional vectors
* -) floating point arrays for N-dimensional vectors.
*
* Note that the Vec3 and Vec4 utilties were ported from the Android
* repository and maintain dependenices in that separate codebase. As a
* result, the function signatures were left untouched for compatibility with
* this legacy code, despite certain style violations. In particular, for this
* module the function argument ordering is outputs before inputs. This style
* violation will be addressed once the full set of dependencies in Android
* have been brought into this repository.
*/
#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_VEC_H_
#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_VEC_H_
#include <nanohub_math.h>
#include <stddef.h>
#include "util/nano_assert.h"
#ifdef __cplusplus
extern "C" {
#endif
struct Vec3 {
float x, y, z;
};
struct Vec4 {
float x, y, z, w;
};
#ifndef NANO_ABS
#define NANO_ABS(x) ((x) > 0 ? (x) : -(x))
#endif
#ifndef NANO_MAX
#define NANO_MAX(a, b) ((a) > (b)) ? (a) : (b)
#endif
// 3-DIMENSIONAL VECTOR MATH ///////////////////////////////////////////
static inline void initVec3(struct Vec3 *v, float x, float y, float z) {
ASSERT_NOT_NULL(v);
v->x = x;
v->y = y;
v->z = z;
}
// Updates v as the sum of v and w.
static inline void vec3Add(struct Vec3 *v, const struct Vec3 *w) {
ASSERT_NOT_NULL(v);
ASSERT_NOT_NULL(w);
v->x += w->x;
v->y += w->y;
v->z += w->z;
}
// Updates v as the subtraction of w from v.
static inline void vec3Sub(struct Vec3 *v, const struct Vec3 *w) {
ASSERT_NOT_NULL(v);
ASSERT_NOT_NULL(w);
v->x -= w->x;
v->y -= w->y;
v->z -= w->z;
}
// Scales v by the scalar c, i.e. v = c * v.
static inline void vec3ScalarMul(struct Vec3 *v, float c) {
ASSERT_NOT_NULL(v);
v->x *= c;
v->y *= c;
v->z *= c;
}
// Returns the dot product of v and w.
static inline float vec3Dot(const struct Vec3 *v, const struct Vec3 *w) {
ASSERT_NOT_NULL(v);
ASSERT_NOT_NULL(w);
return v->x * w->x + v->y * w->y + v->z * w->z;
}
// Returns the square of the L2-norm of the given vector.
static inline float vec3NormSquared(const struct Vec3 *v) {
ASSERT_NOT_NULL(v);
return vec3Dot(v, v);
}
// Returns the L2-norm of the given vector.
static inline float vec3Norm(const struct Vec3 *v) {
ASSERT_NOT_NULL(v);
return sqrtf(vec3NormSquared(v));
}
// Normalizes the provided vector to unit norm. If the provided vector has a
// norm of zero, the vector will be unchanged.
static inline void vec3Normalize(struct Vec3 *v) {
ASSERT_NOT_NULL(v);
float norm = vec3Norm(v);
ASSERT(norm > 0);
// Only normalize if norm is non-zero.
if (norm > 0) {
float invNorm = 1.0f / norm;
v->x *= invNorm;
v->y *= invNorm;
v->z *= invNorm;
}
}
// Updates u as the cross product of v and w.
static inline void vec3Cross(struct Vec3 *u, const struct Vec3 *v,
const struct Vec3 *w) {
ASSERT_NOT_NULL(u);
ASSERT_NOT_NULL(v);
ASSERT_NOT_NULL(w);
u->x = v->y * w->z - v->z * w->y;
u->y = v->z * w->x - v->x * w->z;
u->z = v->x * w->y - v->y * w->x;
}
// Finds a vector orthogonal to the vector [inX, inY, inZ] and returns
// this in the components [outX, outY, outZ]. The vector is chosen such
// that the smallest component of [inX, inY, inZ] is set to zero in the
// output vector. For example, for the in vector [0.01, 4.0, 5.0], this
// function will return [0, 5.0, -4.0].
void findOrthogonalVector(float inX, float inY, float inZ, float *outX,
float *outY, float *outZ);
// 4-DIMENSIONAL VECTOR MATH ///////////////////////////////////////////
// Initialize the Vec4 structure with the provided component values.
static inline void initVec4(struct Vec4 *v, float x, float y, float z,
float w) {
ASSERT_NOT_NULL(v);
v->x = x;
v->y = y;
v->z = z;
v->w = w;
}
// N-DIMENSIONAL VECTOR MATH ///////////////////////////////////////////
// Dimension specified by the last argument in all functions below.
// Adds two vectors and returns the sum in the provided vector, i.e.
// u = v + w.
void vecAdd(float *u, const float *v, const float *w, int dim);
// Adds two vectors and returns the sum in the first vector, i.e.
// v = v + w.
void vecAddInPlace(float *v, const float *w, int dim);
// Subtracts two vectors and returns in the provided vector, i.e.
// u = v - w.
void vecSub(float *u, const float *v, const float *w, int dim);
// Scales vector by a scalar and returns in the provided vector, i.e.
// u = c * v.
void vecScalarMul(float *u, const float *v, float c, int dim);
// Scales vector by a scalar and returns in the same vector, i.e.
// v = c * v.
void vecScalarMulInPlace(float *v, float c, int dim);
// Returns the L2-norm of the given vector.
float vecNorm(const float *v, int dim);
// Returns the square of the L2-norm of the given vector.
float vecNormSquared(const float *v, int dim);
// Returns the dot product of v and w.
float vecDot(const float *v, const float *w, int dim);
// Returns the maximum absolute value in vector.
float vecMaxAbsoluteValue(const float *v, int dim);
#ifdef __cplusplus
}
#endif
#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_VEC_H_
|
