1 // Gmsh - Copyright (C) 1997-2021 C. Geuzaine, J.-F. Remacle
2 //
3 // See the LICENSE.txt file in the Gmsh root directory for license information.
4 // Please report all issues on https://gitlab.onelab.info/gmsh/gmsh/issues.
5
6 #ifndef SVECTOR3_H
7 #define SVECTOR3_H
8
9 #include "SPoint3.h"
10 #include <string>
11 #include <stdio.h>
12 #include <array>
13 //#include "GmshMessage.h"
14
15 // concrete class for vector of size 3
16 class SVector3 {
17 protected:
18 SPoint3 P;
19
20 public:
SVector3()21 SVector3() : P() {}
22 // Construct from 2 SPoints, vector from p1 to p2
SVector3(const SPoint3 & p1,const SPoint3 & p2)23 SVector3(const SPoint3 &p1, const SPoint3 &p2) : P(p2 - p1) {}
24 // Construct from a single SPoint, vector from origin to p1
SVector3(const SPoint3 & p1)25 SVector3(const SPoint3 &p1) : P(p1) {}
SVector3(double x,double y,double z)26 SVector3(double x, double y, double z) : P(x, y, z) {}
SVector3(double v)27 SVector3(double v) : P(v, v, v) {}
SVector3(const double * array)28 SVector3(const double *array) : P(array) {}
SVector3(const SVector3 & v)29 SVector3(const SVector3 &v) : P(v.P) {}
x(void)30 double x(void) const { return P.x(); }
y(void)31 double y(void) const { return P.y(); }
z(void)32 double z(void) const { return P.z(); }
norm()33 double norm() const { return std::sqrt(this->normSq()); }
normSq()34 double normSq() const { return P[0] * P[0] + P[1] * P[1] + P[2] * P[2]; }
35 // Beware that " w = v.normalize() " produces the vector
36 // w = (v.norm(), v.norm(), v.norm()), which is NOT a unit vector!
37 // Use " w = v.unit() " to affect to "w" the unit vector parallel to "v".
normalize()38 double normalize()
39 {
40 double n = norm();
41 if(n) {
42 P[0] /= n;
43 P[1] /= n;
44 P[2] /= n;
45 }
46 return n;
47 }
unit()48 SVector3 unit() const
49 {
50 SVector3 y(*this);
51 y.normalize();
52 return y;
53 }
negate()54 void negate()
55 {
56 P[0] = -P[0];
57 P[1] = -P[1];
58 P[2] = -P[2];
59 }
60 // why both [] and (), why not
61 double &operator[](int i) { return P[i]; }
62 double operator[](int i) const { return P[i]; }
operator()63 double &operator()(int i) { return P[i]; }
operator()64 double operator()(int i) const { return P[i]; }
65 SVector3 &operator+=(const SVector3 &a)
66 {
67 P[0] += a[0];
68 P[1] += a[1];
69 P[2] += a[2];
70 return *this;
71 }
72 SVector3 &operator-=(const SVector3 &a)
73 {
74 P[0] -= a[0];
75 P[1] -= a[1];
76 P[2] -= a[2];
77 return *this;
78 }
79 SVector3 &operator*=(const SVector3 &a)
80 {
81 P[0] *= a[0];
82 P[1] *= a[1];
83 P[2] *= a[2];
84 return *this;
85 }
86 SVector3 &operator*=(const double v)
87 {
88 P[0] *= v;
89 P[1] *= v;
90 P[2] *= v;
91 return *this;
92 }
93 SVector3 &operator=(const SVector3 &a)
94 {
95 P[0] = a[0];
96 P[1] = a[1];
97 P[2] = a[2];
98 return *this;
99 }
100 SVector3 &operator=(double v)
101 {
102 P[0] = v;
103 P[1] = v;
104 P[2] = v;
105 return *this;
106 }
107 operator double *() { return P; }
108 void print(const std::string &name = "") const
109 {
110 // Msg::Info("Vector \'%s\': %f %f %f", name.c_str(), P[0], P[1], P[2]);
111 }
112
113 // Needed to allow the initialization of a SPoint3 from a SPoint3, a distance
114 // and a direction
point()115 SPoint3 point() const { return P; }
getMaxValue(double & val)116 int getMaxValue(double &val) const
117 {
118 if((P[0] >= P[1]) && (P[0] >= P[2])) {
119 val = P[0];
120 return 0;
121 }
122 else if((P[1] >= P[0]) && (P[1] >= P[2])) {
123 val = P[1];
124 return 1;
125 }
126 else {
127 val = P[2];
128 return 2;
129 }
130 }
data()131 const double *data() const { return P.data(); }
data()132 double *data() { return P.data(); }
axpy(const double a,const SVector3 & y)133 void axpy(const double a, const SVector3 &y)
134 {
135 for(int i = 0; i < 3; i++) { P[i] += (a * y[i]); }
136 }
137 // implicit conversion to std::array<double,3>
138 operator std::array<double, 3>() const { return {{P[0], P[1], P[2]}}; }
139 };
140
dot(const SVector3 & a,const SVector3 & b)141 inline double dot(const SVector3 &a, const SVector3 &b)
142 {
143 return a.x() * b.x() + a.y() * b.y() + a.z() * b.z();
144 }
145
norm(const SVector3 & v)146 inline double norm(const SVector3 &v) { return sqrt(dot(v, v)); }
147
normSq(const SVector3 & v)148 inline double normSq(const SVector3 &v) { return dot(v, v); }
149
crossprod(const SVector3 & a,const SVector3 & b)150 inline SVector3 crossprod(const SVector3 &a, const SVector3 &b)
151 {
152 return SVector3(a.y() * b.z() - b.y() * a.z(),
153 -(a.x() * b.z() - b.x() * a.z()),
154 a.x() * b.y() - b.x() * a.y());
155 }
156
angle(const SVector3 & a,const SVector3 & b)157 inline double angle(const SVector3 &a, const SVector3 &b)
158 {
159 double cosTheta = dot(a, b);
160 double sinTheta = norm(crossprod(a, b));
161 return atan2(sinTheta, cosTheta);
162 }
163
signedAngle(const SVector3 & a,const SVector3 & b,const SVector3 & n)164 inline double signedAngle(const SVector3 &a, const SVector3 &b,
165 const SVector3 &n)
166 {
167 double cosTheta = dot(a, b);
168 double sinTheta = dot(crossprod(a, b), n);
169 return atan2(sinTheta, cosTheta);
170 }
171
172 inline SVector3 operator*(double m, const SVector3 &v)
173 {
174 return SVector3(v[0] * m, v[1] * m, v[2] * m);
175 }
176
177 inline SVector3 operator*(const SVector3 &v, double m)
178 {
179 return SVector3(v[0] * m, v[1] * m, v[2] * m);
180 }
181
182 inline SVector3 operator*(const SVector3 &v1, const SVector3 &v2)
183 {
184 return SVector3(v1[0] * v2[0], v1[1] * v2[1], v1[2] * v2[2]);
185 }
186
187 inline SVector3 operator+(const SVector3 &a, const SVector3 &b)
188 {
189 return SVector3(a[0] + b[0], a[1] + b[1], a[2] + b[2]);
190 }
191
192 inline SPoint3 operator+(const SPoint3 &a, const SVector3 &b)
193 {
194 return SPoint3(a[0] + b[0], a[1] + b[1], a[2] + b[2]);
195 }
196
197 inline SVector3 operator-(const SVector3 &a, const SVector3 &b)
198 {
199 return SVector3(a[0] - b[0], a[1] - b[1], a[2] - b[2]);
200 }
201
202 inline SPoint3 operator-(const SPoint3 &a, const SVector3 &b)
203 {
204 return SPoint3(a[0] - b[0], a[1] - b[1], a[2] - b[2]);
205 }
206
207 inline SVector3 operator-(const SVector3 &a)
208 {
209 return SVector3(-a[0], -a[1], -a[2]);
210 }
211
buildOrthoBasis_naive(SVector3 & dir,SVector3 & dir1,SVector3 & dir2)212 inline void buildOrthoBasis_naive(SVector3 &dir, SVector3 &dir1, SVector3 &dir2)
213 {
214 dir.normalize();
215 if(dir[1] != 0.0 && dir[2] != 0.0) {
216 dir1 = SVector3(1.0, 0.0, -dir[0] / dir[2]);
217 dir2 =
218 SVector3(dir[0] / dir[2],
219 -(dir[0] * dir[0] + dir[2] * dir[2]) / (dir[1] * dir[2]), 1.0);
220 }
221 else if(dir[0] != 0.0 && dir[2] != 0.0) {
222 dir1 = SVector3(-dir[1] / dir[0], 1.0, 0.0);
223 dir2 = SVector3(1.0, dir[1] / dir[0],
224 -(dir[1] * dir[1] + dir[0] * dir[0]) / (dir[0] * dir[2]));
225 }
226 else if(dir[0] != 0.0 && dir[1] != 0.0) {
227 dir1 = SVector3(0.0, -dir[2] / dir[1], 1.0);
228 dir2 = SVector3(-(dir[1] * dir[1] + dir[2] * dir[2]) / (dir[0] * dir[1]),
229 1.0, dir[2] / dir[1]);
230 }
231 else if(dir[0] == 0.0 && dir[1] == 0.0) {
232 dir1 = SVector3(0.0, 1.0, 0.0);
233 dir2 = SVector3(1.0, 0.0, 0.0);
234 }
235 else if(dir[1] == 0.0 && dir[2] == 0.0) {
236 dir1 = SVector3(0.0, 1.0, 0.0);
237 dir2 = SVector3(0.0, 0.0, 1.0);
238 }
239 else if(dir[0] == 0.0 && dir[2] == 0.0) {
240 dir1 = SVector3(1.0, 0.0, 0.0);
241 dir2 = SVector3(0.0, 0.0, 1.0);
242 }
243 else {
244 dir1 = SVector3(1.0, 0.0, 0.0);
245 dir2 = SVector3(0.0, 0.0, 1.0);
246 // Msg::Error("Problem with computing orthoBasis");
247 }
248 // printf("XYZ =%g %g %g r=%g dir0 = %g %g %g dir1 = %g %g %g dir2 =%g %g
249 // %g\n", x,y,z,d, dir[0], dir[1], dir[2], dir1[0], dir1[1], dir1[2],
250 // dir2[0], dir2[1], dir2[2] ); printf("0x1 =%g 1x2=%g 2x1=%g \n", dot(dir,
251 // dir1), dot(dir1,dir2), dot(dir2,dir));
252 dir1.normalize();
253 dir2.normalize();
254 }
255
256 // given a normal, build tangent and binormal
buildOrthoBasis(SVector3 & normal,SVector3 & tangent,SVector3 & binormal)257 inline void buildOrthoBasis(SVector3 &normal, SVector3 &tangent,
258 SVector3 &binormal)
259 {
260 // pick any Unit-Vector that's not parallel to normal:
261 normal.normalize();
262 if(fabs(normal[0]) > fabs(normal[1]))
263 tangent = SVector3(0.0, 1.0, 0.0);
264 else
265 tangent = SVector3(1.0, 0.0, 0.0);
266
267 // build a binormal from tangent and normal:
268 binormal = crossprod(tangent, normal);
269 double t1 = binormal.normalize();
270
271 // and correct the tangent from the binormal and the normal.
272 tangent = crossprod(normal, binormal);
273 double t2 = tangent.normalize();
274
275 if(t1 == 0.0 || t2 == 0.0) buildOrthoBasis_naive(normal, tangent, binormal);
276 }
277
278 // given a normal and guess tangent, build binormal
buildOrthoBasis2(SVector3 & normal,SVector3 & tangent,SVector3 & binormal)279 inline void buildOrthoBasis2(SVector3 &normal, SVector3 &tangent,
280 SVector3 &binormal)
281 {
282 normal.normalize();
283 tangent.normalize();
284
285 // build a binormal from tangent and normal:
286 binormal = crossprod(tangent, normal);
287 double t1 = binormal.normalize();
288
289 // and correct the tangent from the binormal and the normal.
290 tangent = crossprod(normal, binormal);
291 double t2 = tangent.normalize();
292
293 if(t1 == 0.0 || t2 == 0.0) buildOrthoBasis_naive(normal, tangent, binormal);
294 }
295
296 #endif
297