1 /*
2 * Copyright (C) 2016 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 /////////////////////////////////////////////////////////////////////////
18 /*
19 * This module contains vector math utilities for the following datatypes:
20 * -) Vec3 structures for 3-dimensional vectors
21 * -) Vec4 structures for 4-dimensional vectors
22 * -) floating point arrays for N-dimensional vectors.
23 *
24 * Note that the Vec3 and Vec4 utilties were ported from the Android
25 * repository and maintain dependenices in that separate codebase. As a
26 * result, the function signatures were left untouched for compatibility with
27 * this legacy code, despite certain style violations. In particular, for this
28 * module the function argument ordering is outputs before inputs. This style
29 * violation will be addressed once the full set of dependencies in Android
30 * have been brought into this repository.
31 */
32 #ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_VEC_H_
33 #define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_VEC_H_
34
35 #ifdef NANOHUB_NON_CHRE_API
36 #include <nanohub_math.h>
37 #else
38 #include <math.h>
39 #endif // NANOHUB_NON_CHRE_API
40
41 #include <stddef.h>
42
43 #include "chre/util/nanoapp/assert.h"
44
45 #ifdef __cplusplus
46 extern "C" {
47 #endif
48
49 struct Vec3 {
50 float x, y, z;
51 };
52
53 struct Vec4 {
54 float x, y, z, w;
55 };
56
57 // 3-DIMENSIONAL VECTOR MATH ///////////////////////////////////////////
initVec3(struct Vec3 * v,float x,float y,float z)58 static inline void initVec3(struct Vec3 *v, float x, float y, float z) {
59 CHRE_ASSERT_NOT_NULL(v);
60 v->x = x;
61 v->y = y;
62 v->z = z;
63 }
64
65 // Updates v as the sum of v and w.
vec3Add(struct Vec3 * v,const struct Vec3 * w)66 static inline void vec3Add(struct Vec3 *v, const struct Vec3 *w) {
67 CHRE_ASSERT_NOT_NULL(v);
68 CHRE_ASSERT_NOT_NULL(w);
69 v->x += w->x;
70 v->y += w->y;
71 v->z += w->z;
72 }
73
74 // Sets u as the sum of v and w.
vec3AddVecs(struct Vec3 * u,const struct Vec3 * v,const struct Vec3 * w)75 static inline void vec3AddVecs(struct Vec3 *u, const struct Vec3 *v,
76 const struct Vec3 *w) {
77 CHRE_ASSERT_NOT_NULL(u);
78 CHRE_ASSERT_NOT_NULL(v);
79 CHRE_ASSERT_NOT_NULL(w);
80 u->x = v->x + w->x;
81 u->y = v->y + w->y;
82 u->z = v->z + w->z;
83 }
84
85 // Updates v as the subtraction of w from v.
vec3Sub(struct Vec3 * v,const struct Vec3 * w)86 static inline void vec3Sub(struct Vec3 *v, const struct Vec3 *w) {
87 CHRE_ASSERT_NOT_NULL(v);
88 CHRE_ASSERT_NOT_NULL(w);
89 v->x -= w->x;
90 v->y -= w->y;
91 v->z -= w->z;
92 }
93
94 // Sets u as the difference of v and w.
vec3SubVecs(struct Vec3 * u,const struct Vec3 * v,const struct Vec3 * w)95 static inline void vec3SubVecs(struct Vec3 *u, const struct Vec3 *v,
96 const struct Vec3 *w) {
97 CHRE_ASSERT_NOT_NULL(u);
98 CHRE_ASSERT_NOT_NULL(v);
99 CHRE_ASSERT_NOT_NULL(w);
100 u->x = v->x - w->x;
101 u->y = v->y - w->y;
102 u->z = v->z - w->z;
103 }
104
105 // Scales v by the scalar c, i.e. v = c * v.
vec3ScalarMul(struct Vec3 * v,float c)106 static inline void vec3ScalarMul(struct Vec3 *v, float c) {
107 CHRE_ASSERT_NOT_NULL(v);
108 v->x *= c;
109 v->y *= c;
110 v->z *= c;
111 }
112
113 // Returns the dot product of v and w.
vec3Dot(const struct Vec3 * v,const struct Vec3 * w)114 static inline float vec3Dot(const struct Vec3 *v, const struct Vec3 *w) {
115 CHRE_ASSERT_NOT_NULL(v);
116 CHRE_ASSERT_NOT_NULL(w);
117 return v->x * w->x + v->y * w->y + v->z * w->z;
118 }
119
120 // Returns the square of the L2-norm of the given vector.
vec3NormSquared(const struct Vec3 * v)121 static inline float vec3NormSquared(const struct Vec3 *v) {
122 CHRE_ASSERT_NOT_NULL(v);
123 return vec3Dot(v, v);
124 }
125
126 // Returns the L2-norm of the given vector.
vec3Norm(const struct Vec3 * v)127 static inline float vec3Norm(const struct Vec3 *v) {
128 CHRE_ASSERT_NOT_NULL(v);
129 return sqrtf(vec3NormSquared(v));
130 }
131
132 // Normalizes the provided vector to unit norm. If the provided vector has a
133 // norm of zero, the vector will be unchanged.
vec3Normalize(struct Vec3 * v)134 static inline void vec3Normalize(struct Vec3 *v) {
135 CHRE_ASSERT_NOT_NULL(v);
136 float norm = vec3Norm(v);
137 CHRE_ASSERT(norm > 0);
138 // Only normalize if norm is non-zero.
139 if (norm > 0) {
140 float invNorm = 1.0f / norm;
141 v->x *= invNorm;
142 v->y *= invNorm;
143 v->z *= invNorm;
144 }
145 }
146
147 // Updates u as the cross product of v and w.
vec3Cross(struct Vec3 * u,const struct Vec3 * v,const struct Vec3 * w)148 static inline void vec3Cross(struct Vec3 *u, const struct Vec3 *v,
149 const struct Vec3 *w) {
150 CHRE_ASSERT_NOT_NULL(u);
151 CHRE_ASSERT_NOT_NULL(v);
152 CHRE_ASSERT_NOT_NULL(w);
153 u->x = v->y * w->z - v->z * w->y;
154 u->y = v->z * w->x - v->x * w->z;
155 u->z = v->x * w->y - v->y * w->x;
156 }
157
158 // Finds a vector orthogonal to the vector [inX, inY, inZ] and returns
159 // this in the components [outX, outY, outZ]. The vector is chosen such
160 // that the smallest component of [inX, inY, inZ] is set to zero in the
161 // output vector. For example, for the in vector [0.01, 4.0, 5.0], this
162 // function will return [0, 5.0, -4.0].
163 void findOrthogonalVector(float inX, float inY, float inZ, float *outX,
164 float *outY, float *outZ);
165
166
167 // 4-DIMENSIONAL VECTOR MATH ///////////////////////////////////////////
168 // Initialize the Vec4 structure with the provided component values.
initVec4(struct Vec4 * v,float x,float y,float z,float w)169 static inline void initVec4(struct Vec4 *v, float x, float y, float z,
170 float w) {
171 CHRE_ASSERT_NOT_NULL(v);
172 v->x = x;
173 v->y = y;
174 v->z = z;
175 v->w = w;
176 }
177
178 // N-DIMENSIONAL VECTOR MATH ///////////////////////////////////////////
179 // Dimension specified by the last argument in all functions below.
180
181 // Adds two vectors and returns the sum in the provided vector, i.e.
182 // u = v + w.
183 void vecAdd(float *u, const float *v, const float *w, size_t dim);
184
185 // Adds two vectors and returns the sum in the first vector, i.e.
186 // v = v + w.
187 void vecAddInPlace(float *v, const float *w, size_t dim);
188
189 // Subtracts two vectors and returns in the provided vector, i.e.
190 // u = v - w.
191 void vecSub(float *u, const float *v, const float *w, size_t dim);
192
193 // Scales vector by a scalar and returns in the provided vector, i.e.
194 // u = c * v.
195 void vecScalarMul(float *u, const float *v, float c, size_t dim);
196
197 // Scales vector by a scalar and returns in the same vector, i.e.
198 // v = c * v.
199 void vecScalarMulInPlace(float *v, float c, size_t dim);
200
201 // Returns the L2-norm of the given vector.
202 float vecNorm(const float *v, size_t dim);
203
204 // Returns the square of the L2-norm of the given vector.
205 float vecNormSquared(const float *v, size_t dim);
206
207 // Returns the dot product of v and w.
208 float vecDot(const float *v, const float *w, size_t dim);
209
210 // Returns the maximum absolute value in vector.
211 float vecMaxAbsoluteValue(const float *v, size_t dim);
212
213 #ifdef __cplusplus
214 }
215 #endif
216
217 #endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_VEC_H_
218