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 #include "common/math/vec.h"
18
19 #include "common/math/macros.h"
20
findOrthogonalVector(float inX,float inY,float inZ,float * outX,float * outY,float * outZ)21 void findOrthogonalVector(float inX, float inY, float inZ, float *outX,
22 float *outY, float *outZ) {
23 CHRE_ASSERT_NOT_NULL(outX);
24 CHRE_ASSERT_NOT_NULL(outY);
25 CHRE_ASSERT_NOT_NULL(outZ);
26 float x, y, z;
27
28 // discard the one with the smallest absolute value
29 if (fabsf(inX) <= fabsf(inY) && fabsf(inX) <= fabsf(inZ)) {
30 x = 0.0f;
31 y = inZ;
32 z = -inY;
33 } else if (fabsf(inY) <= fabsf(inZ)) {
34 x = inZ;
35 y = 0.0f;
36 z = -inX;
37 } else {
38 x = inY;
39 y = -inX;
40 z = 0.0f;
41 }
42
43 float magSquared = x * x + y * y + z * z;
44 CHRE_ASSERT(magSquared > 0);
45 // Only set invMag if magSquared is non-zero.
46 float invMag = 1.0f;
47 if (magSquared > 0) {
48 invMag = 1.0f / sqrtf(magSquared);
49 }
50 *outX = x * invMag;
51 *outY = y * invMag;
52 *outZ = z * invMag;
53 }
54
vecAdd(float * u,const float * v,const float * w,size_t dim)55 void vecAdd(float *u, const float *v, const float *w, size_t dim) {
56 CHRE_ASSERT_NOT_NULL(u);
57 CHRE_ASSERT_NOT_NULL(v);
58 CHRE_ASSERT_NOT_NULL(w);
59 size_t i;
60 for (i = 0; i < dim; i++) {
61 u[i] = v[i] + w[i];
62 }
63 }
64
vecAddInPlace(float * v,const float * w,size_t dim)65 void vecAddInPlace(float *v, const float *w, size_t dim) {
66 CHRE_ASSERT_NOT_NULL(v);
67 CHRE_ASSERT_NOT_NULL(w);
68 size_t i;
69 for (i = 0; i < dim; i++) {
70 v[i] += w[i];
71 }
72 }
73
vecSub(float * u,const float * v,const float * w,size_t dim)74 void vecSub(float *u, const float *v, const float *w, size_t dim) {
75 CHRE_ASSERT_NOT_NULL(u);
76 CHRE_ASSERT_NOT_NULL(v);
77 CHRE_ASSERT_NOT_NULL(w);
78 size_t i;
79 for (i = 0; i < dim; i++) {
80 u[i] = v[i] - w[i];
81 }
82 }
83
vecScalarMul(float * u,const float * v,float c,size_t dim)84 void vecScalarMul(float *u, const float *v, float c, size_t dim) {
85 CHRE_ASSERT_NOT_NULL(u);
86 CHRE_ASSERT_NOT_NULL(v);
87 size_t i;
88 for (i = 0; i < dim; i++) {
89 u[i] = c * v[i];
90 }
91 }
92
vecScalarMulInPlace(float * v,float c,size_t dim)93 void vecScalarMulInPlace(float *v, float c, size_t dim) {
94 CHRE_ASSERT_NOT_NULL(v);
95 size_t i;
96 for (i = 0; i < dim; i++) {
97 v[i] *= c;
98 }
99 }
100
vecNorm(const float * v,size_t dim)101 float vecNorm(const float *v, size_t dim) {
102 CHRE_ASSERT_NOT_NULL(v);
103 float norm_sq = vecNormSquared(v, dim);
104 return sqrtf(norm_sq);
105 }
106
vecNormSquared(const float * v,size_t dim)107 float vecNormSquared(const float *v, size_t dim) {
108 CHRE_ASSERT_NOT_NULL(v);
109 return vecDot(v, v, dim);
110 }
111
vecDot(const float * v,const float * w,size_t dim)112 float vecDot(const float *v, const float *w, size_t dim) {
113 CHRE_ASSERT_NOT_NULL(v);
114 CHRE_ASSERT_NOT_NULL(w);
115 size_t i;
116 float result = 0;
117 for (i = 0; i < dim; ++i) {
118 result += v[i] * w[i];
119 }
120 return result;
121 }
122
vecMaxAbsoluteValue(const float * v,size_t dim)123 float vecMaxAbsoluteValue(const float *v, size_t dim) {
124 CHRE_ASSERT_NOT_NULL(v);
125 float max = NANO_ABS(v[0]);
126 float tmp;
127 size_t i;
128 for (i = 1; i < dim; ++i) {
129 tmp = NANO_ABS(v[i]);
130 if(tmp > max) {
131 max = tmp;
132 }
133 }
134 return max;
135 }
136