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 #include "util/nano_assert.h" 43 44 #ifdef __cplusplus 45 extern "C" { 46 #endif 47 48 struct Vec3 { 49 float x, y, z; 50 }; 51 52 struct Vec4 { 53 float x, y, z, w; 54 }; 55 56 // 3-DIMENSIONAL VECTOR MATH /////////////////////////////////////////// 57 static inline void initVec3(struct Vec3 *v, float x, float y, float z) { 58 ASSERT_NOT_NULL(v); 59 v->x = x; 60 v->y = y; 61 v->z = z; 62 } 63 64 // Updates v as the sum of v and w. 65 static inline void vec3Add(struct Vec3 *v, const struct Vec3 *w) { 66 ASSERT_NOT_NULL(v); 67 ASSERT_NOT_NULL(w); 68 v->x += w->x; 69 v->y += w->y; 70 v->z += w->z; 71 } 72 73 // Sets u as the sum of v and w. 74 static inline void vec3AddVecs(struct Vec3 *u, const struct Vec3 *v, 75 const struct Vec3 *w) { 76 ASSERT_NOT_NULL(u); 77 ASSERT_NOT_NULL(v); 78 ASSERT_NOT_NULL(w); 79 u->x = v->x + w->x; 80 u->y = v->y + w->y; 81 u->z = v->z + w->z; 82 } 83 84 // Updates v as the subtraction of w from v. 85 static inline void vec3Sub(struct Vec3 *v, const struct Vec3 *w) { 86 ASSERT_NOT_NULL(v); 87 ASSERT_NOT_NULL(w); 88 v->x -= w->x; 89 v->y -= w->y; 90 v->z -= w->z; 91 } 92 93 // Sets u as the difference of v and w. 94 static inline void vec3SubVecs(struct Vec3 *u, const struct Vec3 *v, 95 const struct Vec3 *w) { 96 ASSERT_NOT_NULL(u); 97 ASSERT_NOT_NULL(v); 98 ASSERT_NOT_NULL(w); 99 u->x = v->x - w->x; 100 u->y = v->y - w->y; 101 u->z = v->z - w->z; 102 } 103 104 // Scales v by the scalar c, i.e. v = c * v. 105 static inline void vec3ScalarMul(struct Vec3 *v, float c) { 106 ASSERT_NOT_NULL(v); 107 v->x *= c; 108 v->y *= c; 109 v->z *= c; 110 } 111 112 // Returns the dot product of v and w. 113 static inline float vec3Dot(const struct Vec3 *v, const struct Vec3 *w) { 114 ASSERT_NOT_NULL(v); 115 ASSERT_NOT_NULL(w); 116 return v->x * w->x + v->y * w->y + v->z * w->z; 117 } 118 119 // Returns the square of the L2-norm of the given vector. 120 static inline float vec3NormSquared(const struct Vec3 *v) { 121 ASSERT_NOT_NULL(v); 122 return vec3Dot(v, v); 123 } 124 125 // Returns the L2-norm of the given vector. 126 static inline float vec3Norm(const struct Vec3 *v) { 127 ASSERT_NOT_NULL(v); 128 return sqrtf(vec3NormSquared(v)); 129 } 130 131 // Normalizes the provided vector to unit norm. If the provided vector has a 132 // norm of zero, the vector will be unchanged. 133 static inline void vec3Normalize(struct Vec3 *v) { 134 ASSERT_NOT_NULL(v); 135 float norm = vec3Norm(v); 136 ASSERT(norm > 0); 137 // Only normalize if norm is non-zero. 138 if (norm > 0) { 139 float invNorm = 1.0f / norm; 140 v->x *= invNorm; 141 v->y *= invNorm; 142 v->z *= invNorm; 143 } 144 } 145 146 // Updates u as the cross product of v and w. 147 static inline void vec3Cross(struct Vec3 *u, const struct Vec3 *v, 148 const struct Vec3 *w) { 149 ASSERT_NOT_NULL(u); 150 ASSERT_NOT_NULL(v); 151 ASSERT_NOT_NULL(w); 152 u->x = v->y * w->z - v->z * w->y; 153 u->y = v->z * w->x - v->x * w->z; 154 u->z = v->x * w->y - v->y * w->x; 155 } 156 157 // Finds a vector orthogonal to the vector [inX, inY, inZ] and returns 158 // this in the components [outX, outY, outZ]. The vector is chosen such 159 // that the smallest component of [inX, inY, inZ] is set to zero in the 160 // output vector. For example, for the in vector [0.01, 4.0, 5.0], this 161 // function will return [0, 5.0, -4.0]. 162 void findOrthogonalVector(float inX, float inY, float inZ, float *outX, 163 float *outY, float *outZ); 164 165 166 // 4-DIMENSIONAL VECTOR MATH /////////////////////////////////////////// 167 // Initialize the Vec4 structure with the provided component values. 168 static inline void initVec4(struct Vec4 *v, float x, float y, float z, 169 float w) { 170 ASSERT_NOT_NULL(v); 171 v->x = x; 172 v->y = y; 173 v->z = z; 174 v->w = w; 175 } 176 177 // N-DIMENSIONAL VECTOR MATH /////////////////////////////////////////// 178 // Dimension specified by the last argument in all functions below. 179 180 // Adds two vectors and returns the sum in the provided vector, i.e. 181 // u = v + w. 182 void vecAdd(float *u, const float *v, const float *w, size_t dim); 183 184 // Adds two vectors and returns the sum in the first vector, i.e. 185 // v = v + w. 186 void vecAddInPlace(float *v, const float *w, size_t dim); 187 188 // Subtracts two vectors and returns in the provided vector, i.e. 189 // u = v - w. 190 void vecSub(float *u, const float *v, const float *w, size_t dim); 191 192 // Scales vector by a scalar and returns in the provided vector, i.e. 193 // u = c * v. 194 void vecScalarMul(float *u, const float *v, float c, size_t dim); 195 196 // Scales vector by a scalar and returns in the same vector, i.e. 197 // v = c * v. 198 void vecScalarMulInPlace(float *v, float c, size_t dim); 199 200 // Returns the L2-norm of the given vector. 201 float vecNorm(const float *v, size_t dim); 202 203 // Returns the square of the L2-norm of the given vector. 204 float vecNormSquared(const float *v, size_t dim); 205 206 // Returns the dot product of v and w. 207 float vecDot(const float *v, const float *w, size_t dim); 208 209 // Returns the maximum absolute value in vector. 210 float vecMaxAbsoluteValue(const float *v, size_t dim); 211 212 #ifdef __cplusplus 213 } 214 #endif 215 216 #endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_VEC_H_ 217
