1 // =============================================================================
2 // PROJECT CHRONO - http://projectchrono.org
3 //
4 // Copyright (c) 2014 projectchrono.org
5 // All rights reserved.
6 //
7 // Use of this source code is governed by a BSD-style license that can be found
8 // in the LICENSE file at the top level of the distribution and at
9 // http://projectchrono.org/license-chrono.txt.
10 //
11 // =============================================================================
12 // Authors: Alessandro Tasora, Radu Serban
13 // =============================================================================
14
15 #ifndef CHVECTOR_H
16 #define CHVECTOR_H
17
18 #include <algorithm>
19 #include <cmath>
20 #include <limits>
21
22 #include "chrono/core/ChClassFactory.h"
23 #include "chrono/core/ChMatrix.h"
24 #include "chrono/serialization/ChArchive.h"
25 #include "chrono/serialization/ChArchiveAsciiDump.h"
26
27 namespace chrono {
28
29 /// Definition of general purpose 3d vector variables, such as points in 3D.
30 /// This class implements the vectorial algebra in 3D (Gibbs products).
31 /// ChVector is templated by precision, with default 'double'.
32 ///
33 /// Further info at the @ref mathematical_objects manual page.
34 template <class Real = double>
35 class ChVector {
36 public:
37 // CONSTRUCTORS
38
39 ChVector();
40 ChVector(Real x, Real y, Real z);
41 ChVector(Real a);
42 ChVector(const ChVector<Real>& other);
43
44 /// Copy constructor with type change.
45 template <class RealB>
46 ChVector(const ChVector<RealB>& other);
47
48 /// Access to components
x()49 Real& x() { return m_data[0]; }
y()50 Real& y() { return m_data[1]; }
z()51 Real& z() { return m_data[2]; }
x()52 const Real& x() const { return m_data[0]; }
y()53 const Real& y() const { return m_data[1]; }
z()54 const Real& z() const { return m_data[2]; }
55
56 /// Return const pointer to underlying array storage.
data()57 const Real* data() const { return m_data; }
58
59 // EIGEN INTER-OPERABILITY
60
61 /// Construct a 3d vector from an Eigen vector expression.
62 template <typename Derived>
63 ChVector(const Eigen::MatrixBase<Derived>& vec,
64 typename std::enable_if<(Derived::MaxRowsAtCompileTime == 1 || Derived::MaxColsAtCompileTime == 1),
65 Derived>::type* = 0) {
66 m_data[0] = vec(0);
67 m_data[1] = vec(1);
68 m_data[2] = vec(2);
69 }
70
71 /// View this 3d vector as an Eigen vector.
eigen()72 Eigen::Map<Eigen::Matrix<Real, 3, 1>> eigen() { return Eigen::Map<Eigen::Matrix<Real, 3, 1>>(m_data); }
eigen()73 Eigen::Map<const Eigen::Matrix<Real, 3, 1>> eigen() const { return Eigen::Map<const Eigen::Matrix<Real, 3, 1>>(m_data); }
74
75 /// Assign an Eigen vector expression to this 3d vector.
76 template <typename Derived>
77 ChVector& operator=(const Eigen::MatrixBase<Derived>& vec) {
78 m_data[0] = vec(0);
79 m_data[1] = vec(1);
80 m_data[2] = vec(2);
81 return *this;
82 }
83
84 // SET FUNCTIONS
85
86 /// Set the three values of the vector at once.
87 void Set(Real x, Real y, Real z);
88
89 /// Set the vector as a copy of another vector.
90 void Set(const ChVector<Real>& v);
91
92 /// Set all the vector components ts to the same scalar.
93 void Set(Real s);
94
95 /// Set the vector to the null vector.
96 void SetNull();
97
98 /// Return true if this vector is the null vector.
99 bool IsNull() const;
100
101 /// Return true if this vector is equal to another vector.
102 bool Equals(const ChVector<Real>& other) const;
103
104 /// Return true if this vector is equal to another vector, within a tolerance 'tol'.
105 bool Equals(const ChVector<Real>& other, Real tol) const;
106
107 // VECTOR NORMS
108
109 /// Compute the euclidean norm of the vector, that is its length or magnitude.
110 Real Length() const;
111
112 /// Compute the squared euclidean norm of the vector.
113 Real Length2() const;
114
115 /// Compute the infinity norm of the vector, that is the maximum absolute value of one of its elements.
116 Real LengthInf() const;
117
118 // OPERATORS OVERLOADING
119 //
120 // Note: c++ automatically creates temporary objects to store intermediate
121 // results in long formulas, such as a= b*c*d, so the usage of operators
122 // may give slower results than a wise (less readable however) usage of
123 // Dot(), Cross() etc.. Also pay attention to C++ operator precedence rules!
124
125 /// Subscript operator.
126 Real& operator[](unsigned index);
127 const Real& operator[](unsigned index) const;
128
129 /// Assignment operator (copy from another vector).
130 ChVector<Real>& operator=(const ChVector<Real>& other);
131
132 /// Assignment operator (copy from another vector) with type change.
133 template <class RealB>
134 ChVector<Real>& operator=(const ChVector<RealB>& other);
135
136 /// Operators for sign change.
137 ChVector<Real> operator+() const;
138 ChVector<Real> operator-() const;
139
140 /// Operator for vector sum.
141 ChVector<Real> operator+(const ChVector<Real>& other) const;
142 ChVector<Real>& operator+=(const ChVector<Real>& other);
143
144 /// Operator for vector difference.
145 ChVector<Real> operator-(const ChVector<Real>& other) const;
146 ChVector<Real>& operator-=(const ChVector<Real>& other);
147
148 /// Operator for element-wise multiplication.
149 /// Note that this is neither dot product nor cross product.
150 ChVector<Real> operator*(const ChVector<Real>& other) const;
151 ChVector<Real>& operator*=(const ChVector<Real>& other);
152
153 /// Operator for element-wise division.
154 /// Note that 3D vector algebra is a skew field, non-divisional algebra,
155 /// so this division operation is just an element-by element division.
156 ChVector<Real> operator/(const ChVector<Real>& other) const;
157 ChVector<Real>& operator/=(const ChVector<Real>& other);
158
159 /// Operator for scaling the vector by a scalar value, as V*s
160 ChVector<Real> operator*(Real s) const;
161 ChVector<Real>& operator*=(Real s);
162
163 /// Operator for scaling the vector by inverse of a scalar value, as v/s
164 ChVector<Real> operator/(Real v) const;
165 ChVector<Real>& operator/=(Real v);
166
167 /// Operator for dot product: A^B means the scalar dot-product A*B
168 /// Note: pay attention to operator low precedence (see C++ precedence rules!)
169 Real operator^(const ChVector<Real>& other) const;
170
171 /// Operator for cross product: A%B means the vector cross-product AxB
172 /// Note: pay attention to operator low precedence (see C++ precedence rules!)
173 ChVector<Real> operator%(const ChVector<Real>& other) const;
174 ChVector<Real>& operator%=(const ChVector<Real>& other);
175
176 /// Component-wise comparison operators
177 bool operator<=(const ChVector<Real>& other) const;
178 bool operator>=(const ChVector<Real>& other) const;
179 bool operator<(const ChVector<Real>& other) const;
180 bool operator>(const ChVector<Real>& other) const;
181 bool operator==(const ChVector<Real>& other) const;
182 bool operator!=(const ChVector<Real>& other) const;
183
184 // FUNCTIONS
185
186 /// Set this vector to the sum of A and B: this = A + B
187 void Add(const ChVector<Real>& A, const ChVector<Real>& B);
188
189 /// Set this vector to the difference of A and B: this = A - B
190 void Sub(const ChVector<Real>& A, const ChVector<Real>& B);
191
192 /// Set this vector to the product of a vector A and scalar s: this = A * s
193 void Mul(const ChVector<Real>& A, Real s);
194
195 /// Scale this vector by a scalar: this *= s
196 void Scale(Real s);
197
198 /// Set this vector to the cross product of A and B: this = A x B
199 void Cross(const ChVector<Real>& A, const ChVector<Real>& B);
200
201 /// Return the cross product with another vector: result = this x other
202 ChVector<Real> Cross(const ChVector<Real> other) const;
203
204 /// Return the dot product with another vector: result = this ^ B
205 Real Dot(const ChVector<Real>& B) const;
206
207 /// Normalize this vector in place, so that its euclidean length is 1.
208 /// Return false if the original vector had zero length (in which case the vector
209 /// is set to [1,0,0]) and return true otherwise.
210 bool Normalize();
211
212 /// Return a normalized copy of this vector, with euclidean length = 1.
213 /// Not to be confused with Normalize() which normalizes in place.
214 ChVector<Real> GetNormalized() const;
215
216 /// Impose a new length to the vector, keeping the direction unchanged.
217 void SetLength(Real s);
218
219 /// Use the Gram-Schmidt orthonormalization to find the three
220 /// orthogonal vectors of a coordinate system whose X axis is this vector.
221 /// Vsingular (optional) sets the normal to the plane on which Dz must lie.
222 void DirToDxDyDz(ChVector<Real>& Vx,
223 ChVector<Real>& Vy,
224 ChVector<Real>& Vz,
225 const ChVector<Real>& Vsingular = ChVector<Real>(0, 1, 0)) const;
226
227 /// Return the index of the largest component in absolute value.
228 int GetMaxComponent() const;
229
230 /// Return a unit vector orthogonal to this vector
231 ChVector<Real> GetOrthogonalVector() const;
232
233 /// Method to allow serialization of transient m_data to archives.
234 void ArchiveOUT(ChArchiveOut& marchive);
235
236 /// Method to allow de-serialization of transient m_data from archives.
237 void ArchiveIN(ChArchiveIn& marchive);
238
239 private:
240 Real m_data[3];
241
242 /// Declaration of friend classes
243 template <typename RealB>
244 friend class ChVector;
245 };
246
247 CH_CLASS_VERSION(ChVector<double>, 0)
248
249 // -----------------------------------------------------------------------------
250
251 /// Shortcut for faster use of typical double-precision vectors.
252 /// <pre>
253 /// Instead of writing
254 /// ChVector<double> foo;
255 /// or
256 /// ChVector<> foo;
257 /// you can use the shorter version
258 /// Vector foo;
259 /// </pre>
260 typedef ChVector<double> Vector;
261
262 /// Shortcut for faster use of typical single-precision vectors.
263 /// <pre>
264 /// Instead of writing
265 /// ChVector<float> foo;
266 /// you can use the shorter version
267 /// Vector foo;
268 /// </pre>
269 typedef ChVector<float> VectorF;
270
271 // -----------------------------------------------------------------------------
272 // CONSTANTS
273
274 ChApi extern const ChVector<double> VNULL;
275 ChApi extern const ChVector<double> VECT_X;
276 ChApi extern const ChVector<double> VECT_Y;
277 ChApi extern const ChVector<double> VECT_Z;
278
279 // -----------------------------------------------------------------------------
280 // STATIC VECTOR MATH OPERATIONS
281 //
282 // These functions are here for people which prefer to use static
283 // functions instead of ChVector class' member functions.
284 // NOTE: sometimes a wise adoption of the following functions may
285 // give faster results rather than using overloaded operators +/-/* in
286 // the vector class.
287 // For best readability of our code, it is suggested not to use
288 // these functions - use the member functions or operators of
289 // the ChVector class instead.
290
291 template <class RealA, class RealB>
Vdot(const ChVector<RealA> & va,const ChVector<RealB> & vb)292 RealA Vdot(const ChVector<RealA>& va, const ChVector<RealB>& vb) {
293 return (RealA)((va.x() * vb.x()) + (va.y() * vb.y()) + (va.z() * vb.z()));
294 }
295
296 template <class RealA>
Vset(ChVector<RealA> & v,RealA mx,RealA my,RealA mz)297 void Vset(ChVector<RealA>& v, RealA mx, RealA my, RealA mz) {
298 v.x() = mx;
299 v.y() = my;
300 v.z() = mz;
301 }
302
303 template <class RealA, class RealB>
Vadd(const ChVector<RealA> & va,const ChVector<RealB> & vb)304 ChVector<RealA> Vadd(const ChVector<RealA>& va, const ChVector<RealB>& vb) {
305 ChVector<RealA> result;
306 result.x() = va.x() + vb.x();
307 result.y() = va.y() + vb.y();
308 result.z() = va.z() + vb.z();
309 return result;
310 }
311
312 template <class RealA, class RealB>
Vsub(const ChVector<RealA> & va,const ChVector<RealB> & vb)313 ChVector<RealA> Vsub(const ChVector<RealA>& va, const ChVector<RealB>& vb) {
314 ChVector<RealA> result;
315 result.x() = va.x() - vb.x();
316 result.y() = va.y() - vb.y();
317 result.z() = va.z() - vb.z();
318 return result;
319 }
320
321 template <class RealA, class RealB>
Vcross(const ChVector<RealA> & va,const ChVector<RealB> & vb)322 ChVector<RealA> Vcross(const ChVector<RealA>& va, const ChVector<RealB>& vb) {
323 ChVector<RealA> result;
324 result.x() = (va.y() * vb.z()) - (va.z() * vb.y());
325 result.y() = (va.z() * vb.x()) - (va.x() * vb.z());
326 result.z() = (va.x() * vb.y()) - (va.y() * vb.x());
327 return result;
328 }
329
330 template <class RealA, class RealB>
Vmul(const ChVector<RealA> & va,RealB fact)331 ChVector<RealA> Vmul(const ChVector<RealA>& va, RealB fact) {
332 ChVector<RealA> result;
333 result.x() = va.x() * (RealA)fact;
334 result.y() = va.y() * (RealA)fact;
335 result.z() = va.z() * (RealA)fact;
336 return result;
337 }
338
339 template <class RealA>
Vlength(const ChVector<RealA> & va)340 RealA Vlength(const ChVector<RealA>& va) {
341 return (RealA)va.Length();
342 }
343
344 template <class RealA>
Vnorm(const ChVector<RealA> & va)345 ChVector<RealA> Vnorm(const ChVector<RealA>& va) {
346 ChVector<RealA> result(va);
347 result.Normalize();
348 return result;
349 }
350
351 template <class RealA, class RealB>
Vequal(const ChVector<RealA> & va,const ChVector<RealB> & vb)352 bool Vequal(const ChVector<RealA>& va, const ChVector<RealB>& vb) {
353 return (va == vb);
354 }
355
356 template <class RealA>
Vnotnull(const ChVector<RealA> & va)357 bool Vnotnull(const ChVector<RealA>& va) {
358 return (va.x() != 0 || va.y() != 0 || va.z() != 0);
359 }
360
361 // Gets the zenith angle of a unit vector respect to YZ plane ***OBSOLETE
362 template <class RealA>
VangleYZplane(const ChVector<RealA> & va)363 double VangleYZplane(const ChVector<RealA>& va) {
364 return asin(Vdot(va, ChVector<RealA>(1, 0, 0)));
365 }
366
367 // Gets the zenith angle of a unit vector respect to YZ plane ***OBSOLETE
368 template <class RealA>
VangleYZplaneNorm(const ChVector<RealA> & va)369 double VangleYZplaneNorm(const ChVector<RealA>& va) {
370 return acos(Vdot(va, ChVector<RealA>(1, 0, 0)));
371 }
372
373 // Gets the angle of the projection on the YZ plane respect to
374 // the Y vector, as spinning about X.
375 template <class RealA>
VangleRX(const ChVector<RealA> & va)376 double VangleRX(const ChVector<RealA>& va) {
377 Vector vproj;
378 vproj.x() = 0;
379 vproj.y() = va.y();
380 vproj.z() = va.z();
381 vproj = Vnorm(vproj);
382 if (vproj.x() == 1)
383 return 0;
384 return acos(vproj.y());
385 }
386
387 // The reverse of the two previous functions, gets the vector
388 // given the angle above the normal to YZ plane and the angle
389 // of rotation on X
390 template <class RealA>
VfromPolar(double norm_angle,double pol_angle)391 ChVector<RealA> VfromPolar(double norm_angle, double pol_angle) {
392 ChVector<> res;
393 double projlen;
394 res.x() = cos(norm_angle); // 1) rot 'norm.angle'about z
395 res.y() = sin(norm_angle);
396 res.z() = 0;
397 projlen = res.y();
398 res.y() = projlen * cos(pol_angle);
399 res.z() = projlen * sin(pol_angle);
400 return res;
401 }
402
403 // From non-normalized x direction, to versors DxDyDz.
404 // Vsingular sets the normal to the plane on which Dz must lie.
405 template <class RealA>
XdirToDxDyDz(const ChVector<RealA> & Vxdir,const ChVector<RealA> & Vsingular,ChVector<RealA> & Vx,ChVector<RealA> & Vy,ChVector<RealA> & Vz)406 void XdirToDxDyDz(const ChVector<RealA>& Vxdir,
407 const ChVector<RealA>& Vsingular,
408 ChVector<RealA>& Vx,
409 ChVector<RealA>& Vy,
410 ChVector<RealA>& Vz) {
411 ChVector<RealA> mVnull(0, 0, 0);
412 double zlen;
413
414 if (Vequal(Vxdir, mVnull))
415 Vx = ChVector<RealA>(1, 0, 0);
416 else
417 Vx = Vnorm(Vxdir);
418
419 Vz = Vcross(Vx, Vsingular);
420 zlen = Vlength(Vz);
421
422 // If close to singularity, change reference vector
423 if (zlen < 0.0001) {
424 ChVector<> mVsingular;
425 if (std::abs(Vsingular.z()) < 0.9)
426 mVsingular = ChVector<RealA>(0, 0, 1);
427 if (std::abs(Vsingular.y()) < 0.9)
428 mVsingular = ChVector<RealA>(0, 1, 0);
429 if (std::abs(Vsingular.x()) < 0.9)
430 mVsingular = ChVector<RealA>(1, 0, 0);
431 Vz = Vcross(Vx, mVsingular);
432 zlen = Vlength(Vz); // now should be nonzero length.
433 }
434
435 // normalize Vz
436 Vz = Vmul(Vz, 1.0 / zlen);
437 // compute Vy
438 Vy = Vcross(Vz, Vx);
439 }
440
441 /// Insertion ov 3d vector to output stream.
442 template <typename Real>
443 inline std::ostream& operator<<(std::ostream& out, const ChVector<Real>& v) {
444 out << v.x() << " " << v.y() << " " << v.z();
445 return out;
446 }
447
448 // =============================================================================
449 // IMPLEMENTATION OF ChVector<Real> methods
450 // =============================================================================
451
452 // -----------------------------------------------------------------------------
453 // Constructors
454
455 template <class Real>
ChVector()456 inline ChVector<Real>::ChVector() {
457 m_data[0] = 0;
458 m_data[1] = 0;
459 m_data[2] = 0;
460 }
461
462 template <class Real>
ChVector(Real a)463 inline ChVector<Real>::ChVector(Real a) {
464 m_data[0] = a;
465 m_data[1] = a;
466 m_data[2] = a;
467 }
468
469 template <class Real>
ChVector(Real x,Real y,Real z)470 inline ChVector<Real>::ChVector(Real x, Real y, Real z) {
471 m_data[0] = x;
472 m_data[1] = y;
473 m_data[2] = z;
474 }
475
476 template <class Real>
ChVector(const ChVector<Real> & other)477 inline ChVector<Real>::ChVector(const ChVector<Real>& other) {
478 m_data[0] = other.m_data[0];
479 m_data[1] = other.m_data[1];
480 m_data[2] = other.m_data[2];
481 }
482
483 template <class Real>
484 template <class RealB>
ChVector(const ChVector<RealB> & other)485 inline ChVector<Real>::ChVector(const ChVector<RealB>& other) {
486 m_data[0] = static_cast<Real>(other.m_data[0]);
487 m_data[1] = static_cast<Real>(other.m_data[1]);
488 m_data[2] = static_cast<Real>(other.m_data[2]);
489 }
490
491 // -----------------------------------------------------------------------------
492 // Subscript operators
493
494 template <class Real>
495 inline Real& ChVector<Real>::operator[](unsigned index) {
496 assert(index < 3);
497 return m_data[index];
498 }
499
500 template <class Real>
501 inline const Real& ChVector<Real>::operator[](unsigned index) const {
502 assert(index < 3);
503 return m_data[index];
504 }
505
506 // -----------------------------------------------------------------------------
507 // Assignments
508
509 template <class Real>
510 inline ChVector<Real>& ChVector<Real>::operator=(const ChVector<Real>& other) {
511 if (&other == this)
512 return *this;
513 m_data[0] = other.m_data[0];
514 m_data[1] = other.m_data[1];
515 m_data[2] = other.m_data[2];
516 return *this;
517 }
518
519 template <class Real>
520 template <class RealB>
521 inline ChVector<Real>& ChVector<Real>::operator=(const ChVector<RealB>& other) {
522 m_data[0] = static_cast<Real>(other.m_data[0]);
523 m_data[1] = static_cast<Real>(other.m_data[1]);
524 m_data[2] = static_cast<Real>(other.m_data[2]);
525 return *this;
526 }
527
528 // -----------------------------------------------------------------------------
529 // Sign operators
530
531 template <class Real>
532 inline ChVector<Real> ChVector<Real>::operator+() const {
533 return *this;
534 }
535
536 template <class Real>
537 inline ChVector<Real> ChVector<Real>::operator-() const {
538 return ChVector<Real>(-m_data[0], -m_data[1], -m_data[2]);
539 }
540
541 // -----------------------------------------------------------------------------
542 // Arithmetic operations
543
544 template <class Real>
545 inline ChVector<Real> ChVector<Real>::operator+(const ChVector<Real>& other) const {
546 ChVector<Real> v;
547
548 v.m_data[0] = m_data[0] + other.m_data[0];
549 v.m_data[1] = m_data[1] + other.m_data[1];
550 v.m_data[2] = m_data[2] + other.m_data[2];
551
552 return v;
553 }
554
555 template <class Real>
556 inline ChVector<Real> ChVector<Real>::operator-(const ChVector<Real>& other) const {
557 ChVector<Real> v;
558
559 v.m_data[0] = m_data[0] - other.m_data[0];
560 v.m_data[1] = m_data[1] - other.m_data[1];
561 v.m_data[2] = m_data[2] - other.m_data[2];
562
563 return v;
564 }
565
566 template <class Real>
567 inline ChVector<Real> ChVector<Real>::operator*(const ChVector<Real>& other) const {
568 ChVector<Real> v;
569
570 v.m_data[0] = m_data[0] * other.m_data[0];
571 v.m_data[1] = m_data[1] * other.m_data[1];
572 v.m_data[2] = m_data[2] * other.m_data[2];
573
574 return v;
575 }
576
577 template <class Real>
578 inline ChVector<Real> ChVector<Real>::operator/(const ChVector<Real>& other) const {
579 ChVector<Real> v;
580
581 v.m_data[0] = m_data[0] / other.m_data[0];
582 v.m_data[1] = m_data[1] / other.m_data[1];
583 v.m_data[2] = m_data[2] / other.m_data[2];
584
585 return v;
586 }
587
588 template <class Real>
589 inline ChVector<Real> ChVector<Real>::operator*(Real s) const {
590 ChVector<Real> v;
591
592 v.m_data[0] = m_data[0] * s;
593 v.m_data[1] = m_data[1] * s;
594 v.m_data[2] = m_data[2] * s;
595
596 return v;
597 }
598
599 template <class Real>
600 inline ChVector<Real> ChVector<Real>::operator/(Real s) const {
601 Real oos = 1 / s;
602 ChVector<Real> v;
603
604 v.m_data[0] = m_data[0] * oos;
605 v.m_data[1] = m_data[1] * oos;
606 v.m_data[2] = m_data[2] * oos;
607
608 return v;
609 }
610
611 template <class Real>
612 inline ChVector<Real>& ChVector<Real>::operator+=(const ChVector<Real>& other) {
613 m_data[0] += other.m_data[0];
614 m_data[1] += other.m_data[1];
615 m_data[2] += other.m_data[2];
616
617 return *this;
618 }
619
620 template <class Real>
621 inline ChVector<Real>& ChVector<Real>::operator-=(const ChVector<Real>& other) {
622 m_data[0] -= other.m_data[0];
623 m_data[1] -= other.m_data[1];
624 m_data[2] -= other.m_data[2];
625
626 return *this;
627 }
628
629 template <class Real>
630 inline ChVector<Real>& ChVector<Real>::operator*=(const ChVector<Real>& other) {
631 m_data[0] *= other.m_data[0];
632 m_data[1] *= other.m_data[1];
633 m_data[2] *= other.m_data[2];
634
635 return *this;
636 }
637
638 template <class Real>
639 inline ChVector<Real>& ChVector<Real>::operator/=(const ChVector<Real>& other) {
640 m_data[0] /= other.m_data[0];
641 m_data[1] /= other.m_data[1];
642 m_data[2] /= other.m_data[2];
643
644 return *this;
645 }
646
647 template <class Real>
648 inline ChVector<Real>& ChVector<Real>::operator*=(Real s) {
649 m_data[0] *= s;
650 m_data[1] *= s;
651 m_data[2] *= s;
652
653 return *this;
654 }
655
656 template <class Real>
657 inline ChVector<Real>& ChVector<Real>::operator/=(Real s) {
658 Real oos = 1 / s;
659
660 m_data[0] *= oos;
661 m_data[1] *= oos;
662 m_data[2] *= oos;
663
664 return *this;
665 }
666
667 // -----------------------------------------------------------------------------
668 // Vector operations
669
670 template <class Real>
671 inline Real ChVector<Real>::operator^(const ChVector<Real>& other) const {
672 return this->Dot(other);
673 }
674
675 template <class Real>
676 ChVector<Real> ChVector<Real>::operator%(const ChVector<Real>& other) const {
677 ChVector<Real> v;
678 v.Cross(*this, other);
679 return v;
680 }
681
682 template <class Real>
683 inline ChVector<Real>& ChVector<Real>::operator%=(const ChVector<Real>& other) {
684 this->Cross(*this, other);
685 return *this;
686 }
687
688 // -----------------------------------------------------------------------------
689 // Comparison operations
690
691 template <class Real>
692 inline bool ChVector<Real>::operator<=(const ChVector<Real>& other) const {
693 return m_data[0] <= other.m_data[0] && m_data[1] <= other.m_data[1] && m_data[2] <= other.m_data[2];
694 }
695
696 template <class Real>
697 inline bool ChVector<Real>::operator>=(const ChVector<Real>& other) const {
698 return m_data[0] >= other.m_data[0] && m_data[1] >= other.m_data[1] && m_data[2] >= other.m_data[2];
699 }
700
701 template <class Real>
702 inline bool ChVector<Real>::operator<(const ChVector<Real>& other) const {
703 return m_data[0] < other.m_data[0] && m_data[1] < other.m_data[1] && m_data[2] < other.m_data[2];
704 }
705
706 template <class Real>
707 inline bool ChVector<Real>::operator>(const ChVector<Real>& other) const {
708 return m_data[0] > other.m_data[0] && m_data[1] > other.m_data[1] && m_data[2] > other.m_data[2];
709 }
710
711 template <class Real>
712 inline bool ChVector<Real>::operator==(const ChVector<Real>& other) const {
713 return other.m_data[0] == m_data[0] && other.m_data[1] == m_data[1] && other.m_data[2] == m_data[2];
714 }
715
716 template <class Real>
717 inline bool ChVector<Real>::operator!=(const ChVector<Real>& other) const {
718 return !(*this == other);
719 }
720
721 // -----------------------------------------------------------------------------
722 // Functions
723
724 template <class Real>
Set(Real x,Real y,Real z)725 inline void ChVector<Real>::Set(Real x, Real y, Real z) {
726 m_data[0] = x;
727 m_data[1] = y;
728 m_data[2] = z;
729 }
730
731 template <class Real>
Set(const ChVector<Real> & v)732 inline void ChVector<Real>::Set(const ChVector<Real>& v) {
733 m_data[0] = v.m_data[0];
734 m_data[1] = v.m_data[1];
735 m_data[2] = v.m_data[2];
736 }
737
738 template <class Real>
Set(Real s)739 inline void ChVector<Real>::Set(Real s) {
740 m_data[0] = s;
741 m_data[1] = s;
742 m_data[2] = s;
743 }
744
745 /// Sets the vector as a null vector
746 template <class Real>
SetNull()747 inline void ChVector<Real>::SetNull() {
748 m_data[0] = 0;
749 m_data[1] = 0;
750 m_data[2] = 0;
751 }
752
753 template <class Real>
IsNull()754 inline bool ChVector<Real>::IsNull() const {
755 return m_data[0] == 0 && m_data[1] == 0 && m_data[2] == 0;
756 }
757
758 template <class Real>
Equals(const ChVector<Real> & other)759 inline bool ChVector<Real>::Equals(const ChVector<Real>& other) const {
760 return (other.m_data[0] == m_data[0]) && (other.m_data[1] == m_data[1]) && (other.m_data[2] == m_data[2]);
761 }
762
763 template <class Real>
Equals(const ChVector<Real> & other,Real tol)764 inline bool ChVector<Real>::Equals(const ChVector<Real>& other, Real tol) const {
765 return (std::abs(other.m_data[0] - m_data[0]) < tol) && (std::abs(other.m_data[1] - m_data[1]) < tol) &&
766 (std::abs(other.m_data[2] - m_data[2]) < tol);
767 }
768
769 template <class Real>
Add(const ChVector<Real> & A,const ChVector<Real> & B)770 inline void ChVector<Real>::Add(const ChVector<Real>& A, const ChVector<Real>& B) {
771 m_data[0] = A.m_data[0] + B.m_data[0];
772 m_data[1] = A.m_data[1] + B.m_data[1];
773 m_data[2] = A.m_data[2] + B.m_data[2];
774 }
775
776 template <class Real>
Sub(const ChVector<Real> & A,const ChVector<Real> & B)777 inline void ChVector<Real>::Sub(const ChVector<Real>& A, const ChVector<Real>& B) {
778 m_data[0] = A.m_data[0] - B.m_data[0];
779 m_data[1] = A.m_data[1] - B.m_data[1];
780 m_data[2] = A.m_data[2] - B.m_data[2];
781 }
782
783 template <class Real>
Mul(const ChVector<Real> & A,Real s)784 inline void ChVector<Real>::Mul(const ChVector<Real>& A, Real s) {
785 m_data[0] = A.m_data[0] * s;
786 m_data[1] = A.m_data[1] * s;
787 m_data[2] = A.m_data[2] * s;
788 }
789
790 template <class Real>
Scale(Real s)791 inline void ChVector<Real>::Scale(Real s) {
792 m_data[0] *= s;
793 m_data[1] *= s;
794 m_data[2] *= s;
795 }
796
797 template <class Real>
Cross(const ChVector<Real> & A,const ChVector<Real> & B)798 inline void ChVector<Real>::Cross(const ChVector<Real>& A, const ChVector<Real>& B) {
799 m_data[0] = (A.m_data[1] * B.m_data[2]) - (A.m_data[2] * B.m_data[1]);
800 m_data[1] = (A.m_data[2] * B.m_data[0]) - (A.m_data[0] * B.m_data[2]);
801 m_data[2] = (A.m_data[0] * B.m_data[1]) - (A.m_data[1] * B.m_data[0]);
802 }
803
804 template <class Real>
Cross(const ChVector<Real> other)805 inline ChVector<Real> ChVector<Real>::Cross(const ChVector<Real> other) const {
806 ChVector<Real> v;
807 v.Cross(*this, other);
808 return v;
809 }
810
811 template <class Real>
Dot(const ChVector<Real> & B)812 inline Real ChVector<Real>::Dot(const ChVector<Real>& B) const {
813 return (m_data[0] * B.m_data[0]) + (m_data[1] * B.m_data[1]) + (m_data[2] * B.m_data[2]);
814 }
815
816 template <class Real>
Length()817 inline Real ChVector<Real>::Length() const {
818 return sqrt(Length2());
819 }
820
821 template <class Real>
Length2()822 inline Real ChVector<Real>::Length2() const {
823 return this->Dot(*this);
824 }
825
826 template <class Real>
LengthInf()827 inline Real ChVector<Real>::LengthInf() const {
828 return std::max(std::max(std::abs(m_data[0]), std::abs(m_data[1])), std::abs(m_data[2]));
829 }
830
831 template <class Real>
Normalize()832 inline bool ChVector<Real>::Normalize() {
833 Real length = this->Length();
834 if (length < std::numeric_limits<Real>::min()) {
835 m_data[0] = 1;
836 m_data[1] = 0;
837 m_data[2] = 0;
838 return false;
839 }
840 this->Scale(1 / length);
841 return true;
842 }
843
844 template <class Real>
GetNormalized()845 inline ChVector<Real> ChVector<Real>::GetNormalized() const {
846 ChVector<Real> v(*this);
847 v.Normalize();
848 return v;
849 }
850
851 template <class Real>
SetLength(Real s)852 inline void ChVector<Real>::SetLength(Real s) {
853 Normalize();
854 Scale(s);
855 }
856
857 template <class Real>
DirToDxDyDz(ChVector<Real> & Vx,ChVector<Real> & Vy,ChVector<Real> & Vz,const ChVector<Real> & Vsingular)858 inline void ChVector<Real>::DirToDxDyDz(ChVector<Real>& Vx,
859 ChVector<Real>& Vy,
860 ChVector<Real>& Vz,
861 const ChVector<Real>& Vsingular) const {
862 // set Vx.
863 if (this->IsNull())
864 Vx = ChVector<Real>(1, 0, 0);
865 else
866 Vx = this->GetNormalized();
867
868 Vz.Cross(Vx, Vsingular);
869 Real zlen = Vz.Length();
870
871 // if near singularity, change the singularity reference vector.
872 if (zlen < 0.0001) {
873 ChVector<Real> mVsingular;
874
875 if (std::abs(Vsingular.m_data[0]) < 0.9)
876 mVsingular = ChVector<Real>(1, 0, 0);
877 else if (std::abs(Vsingular.m_data[1]) < 0.9)
878 mVsingular = ChVector<Real>(0, 1, 0);
879 else if (std::abs(Vsingular.m_data[2]) < 0.9)
880 mVsingular = ChVector<Real>(0, 0, 1);
881
882 Vz.Cross(Vx, mVsingular);
883 zlen = Vz.Length(); // now should be nonzero length.
884 }
885
886 // normalize Vz.
887 Vz.Scale(1 / zlen);
888
889 // compute Vy.
890 Vy.Cross(Vz, Vx);
891 }
892
893 template <class Real>
GetMaxComponent()894 inline int ChVector<Real>::GetMaxComponent() const {
895 int idx = 0;
896 Real max = std::abs(m_data[0]);
897 if (std::abs(m_data[1]) > max) {
898 idx = 1;
899 max = m_data[1];
900 }
901 if (std::abs(m_data[2]) > max) {
902 idx = 2;
903 max = m_data[2];
904 }
905 return idx;
906 }
907
908 template <class Real>
GetOrthogonalVector()909 inline ChVector<Real> ChVector<Real>::GetOrthogonalVector() const {
910 int idx1 = this->GetMaxComponent();
911 int idx2 = (idx1 + 1) % 3; // cycle to the next component
912 int idx3 = (idx2 + 1) % 3; // cycle to the next component
913
914 // Construct v2 by rotating in the plane containing the maximum component
915 ChVector<Real> v2(-m_data[idx2], m_data[idx1], m_data[idx3]);
916
917 // Construct the normal vector
918 ChVector<Real> ortho = Cross(v2);
919 ortho.Normalize();
920 return ortho;
921 }
922
923 // -----------------------------------------------------------------------------
924 // Streaming operations
925
926 template <class Real>
ArchiveOUT(ChArchiveOut & marchive)927 inline void ChVector<Real>::ArchiveOUT(ChArchiveOut& marchive) {
928 // suggested: use versioning
929 marchive.VersionWrite<ChVector<double>>(); // must use specialized template (any)
930 // stream out all member m_data
931 marchive << CHNVP(m_data[0], "x");
932 marchive << CHNVP(m_data[1], "y");
933 marchive << CHNVP(m_data[2], "z");
934 }
935
936 template <class Real>
ArchiveIN(ChArchiveIn & marchive)937 inline void ChVector<Real>::ArchiveIN(ChArchiveIn& marchive) {
938 // suggested: use versioning
939 /*int version =*/ marchive.VersionRead<ChVector<double>>(); // must use specialized template (any)
940 // stream in all member m_data
941 marchive >> CHNVP(m_data[0], "x");
942 marchive >> CHNVP(m_data[1], "y");
943 marchive >> CHNVP(m_data[2], "z");
944 }
945
946 // -----------------------------------------------------------------------------
947 // Reversed operators
948
949 /// Operator for scaling the vector by a scalar value, as s*V
950 template <class Real>
951 ChVector<Real> operator*(Real s, const ChVector<Real>& V) {
952 return ChVector<Real>(V.x() * s, V.y() * s, V.z() * s);
953 }
954
955 } // end namespace chrono
956
957 #endif
958