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