MercuryDPM  Trunk
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
Quaternion.cc
Go to the documentation of this file.
1 //Copyright (c) 2013-2020, The MercuryDPM Developers Team. All rights reserved.
2 //For the list of developers, see <http://www.MercuryDPM.org/Team>.
3 //
4 //Redistribution and use in source and binary forms, with or without
5 //modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright
7 // notice, this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name MercuryDPM nor the
12 // names of its contributors may be used to endorse or promote products
13 // derived from this software without specific prior written permission.
14 //
15 //THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16 //ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17 //WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 //DISCLAIMED. IN NO EVENT SHALL THE MERCURYDPM DEVELOPERS TEAM BE LIABLE FOR ANY
19 //DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 //(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 //LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 //ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 //(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 //SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 #include "Math/Quaternion.h"
26 
31 {
32  setUnity();
33 }
34 
42 Quaternion::Quaternion(const Mdouble q0, const Mdouble q1, const Mdouble q2, const Mdouble q3)
43 {
44  this->q0 = q0;
45  this->q1 = q1;
46  this->q2 = q2;
47  this->q3 = q3;
48 }
49 
54 {
55  q0 = 1.0;
56  q1 = 0.0;
57  q2 = 0.0;
58  q3 = 0.0;
59 }
60 
67 {
68  return Quaternion(q0 + a.q0, q1 + a.q1, q2 + a.q2, q3 + a.q3);
69 }
70 
77 {
78  return Quaternion(q0 - a.q0, q1 - a.q1, q2 - a.q2, q3 - a.q3);
79 }
80 
87 {
88  return Quaternion(q0 * a, q1 * a, q2 * a, q3 * a);
89 }
90 
97 {
98  return Quaternion(q0 / a, q1 / a, q2 / a, q3 / a);
99 }
100 
107 {
108  q0 += a.q0;
109  q1 += a.q1;
110  q2 += a.q2;
111  q3 += a.q3;
112  return *this;
113 }
114 
121 {
122  q0 -= a.q0;
123  q1 -= a.q1;
124  q2 -= a.q2;
125  q3 -= a.q3;
126  return *this;
127 }
128 
135 {
136  q0 *= a;
137  q1 *= a;
138  q2 *= a;
139  q3 *= a;
140  return *this;
141 }
142 
149 {
150  q0 /= a;
151  q1 /= a;
152  q2 /= a;
153  q3 /= a;
154  return *this;
155 }
156 
162 {
163  const Mdouble length2 = getLengthSquared();
164  if (length2 == 0)
165  {
166  logger(ERROR, "Normalizing a quaternion of length 0");
167  }
168  *this /= sqrt(length2);
169 }
170 
177 {
178  *this /= this->getLength() * length;
179 }
180 
189 {
190  return std::sqrt(getDistanceSquared(a, b));
191 }
192 
202 {
203  return ((a.q0 - b.q0) * (a.q0 - b.q0) + (a.q1 - b.q1) * (a.q1 - b.q1) + (a.q2 - b.q2) * (a.q2 - b.q2) +
204  (a.q3 - b.q3) * (a.q3 - b.q3));
205 }
206 
214 {
215  return (a.q0 * a.q0 + a.q1 * a.q1 + a.q2 * a.q2 + a.q3 * a.q3);
216 }
217 
223 {
224  return (q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
225 }
226 
232 Mdouble Quaternion::getComponent(const int index) const
233 {
234  switch (index)
235  {
236  case 0:
237  return q0;
238  case 1:
239  return q1;
240  case 2:
241  return q2;
242  case 3:
243  return q3;
244  default:
245  logger(ERROR,
246  "[Quaternion::getComponent] Index = %, which is too high for a 3D quaternion (should be 0-2).",
247  index);
248  return 0;
249  }
250 }
251 
258 void Quaternion::setComponent(const int index, const double val)
259 {
260  switch (index)
261  {
262  case 0:
263  q0 = val;
264  break;
265  case 1:
266  q1 = val;
267  break;
268  case 2:
269  q2 = val;
270  break;
271  case 3:
272  q3 = val;
273  break;
274  default:
275  logger(ERROR,
276  "[Quaternion::setComponent] Index = %, which is too high for a 3D quaternion (should be 0-2).",
277  index);
278  }
279 }
280 
290 bool Quaternion::isEqualTo(const Quaternion& other, const double tol) const
291 {
292  return (getDistance(*this, other) <= tol * tol);
293 }
294 
300 {
301  return std::sqrt(getLengthSquared());
302 }
303 
311 {
312  return a.getLength();
313 }
314 
325 {
326  Mdouble Length2 = a.getLengthSquared();
327  if (Length2 != 0.0)
328  return a / std::sqrt(Length2);
329  else
330  return Quaternion(1, 0, 0, 0);
331 }
332 
340 std::ostream& operator<<(std::ostream& os, const Quaternion& a)
341 {
342  os << a.q0 << ' ' << a.q1 << ' ' << a.q2 << ' ' << a.q3;
343  return os;
344 }
345 
353 std::istream& operator>>(std::istream& is, Quaternion& a)
354 {
355  is >> a.q0 >> a.q1 >> a.q2 >> a.q3;
356  return is;
357 }
358 
368 {
369  return Quaternion(b.q0 + a, b.q1 + a, b.q2 + a, b.q3 + a);
370 }
371 
381 {
382  return Quaternion(a - b.q0, a - b.q1, a - b.q2, a - b.q3);
383 }
384 
393 {
394  return Quaternion(-a.q0, -a.q1, -a.q2, -a.q3);
395 }
396 
406 {
407  return Quaternion(b.q0 * a, b.q1 * a, b.q2 * a, b.q3 * a);
408 }
409 
412 {
413  return Quaternion(
414  -q1 * v.X - q2 * v.Y - q3 * v.Z,
415  q0 * v.X - q3 * v.Y + q2 * v.Z,
416  q3 * v.X + q0 * v.Y - q1 * v.Z,
417  -q2 * v.X + q1 * v.Y + q0 * v.Z);
418 }
419 
423 {
424  return 0.5 * Quaternion(
425  -q1 * v.X - q2 * v.Y - q3 * v.Z,
426  q0 * v.X + q3 * v.Y - q2 * v.Z,
427  -q3 * v.X + q0 * v.Y + q1 * v.Z,
428  q2 * v.X - q1 * v.Y + q0 * v.Z);
429 }
430 
432 {
433  *this += angularDisplacementTimeDerivative(angularVelocityDt);
434  //const Quaternion q = *this;
435  normalise();
436  //logger(INFO,"%|%",q,*this);
437 }
438 
439 
442 {
443  return 2.0 * Vec3D(
444  -q1 * q.q0 + q0 * q.q1 - q3 * q.q2 + q2 * q.q3,
445  -q2 * q.q0 + q3 * q.q1 + q0 * q.q2 - q1 * q.q3,
446  -q3 * q.q0 - q2 * q.q1 + q1 * q.q2 + q0 * q.q3);
447 }
448 
453 {
454  Mdouble sinp = 2 * (q0 * q2 - q3 * q1);
455  Mdouble pitch;
456  const Mdouble pi = 3.141592653589793238462643383279502884197169399375105820974944592307816406286208998628034825342117068;
457 
458  if ((std::abs(sinp) < 1))
459  {
460  pitch = std::asin(sinp);
461  }
462  else
463  {
464  pitch = copysign(pi/2., sinp);
465  }
466 
467  return Vec3D(
468  std::atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2)),
469  pitch,
470  std::atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3)));
471 }
472 
474 {
475  Mdouble cp = std::cos(0.5 * e.Y);
476  Mdouble sp = std::sin(0.5 * e.Y);
477  Mdouble cr = std::cos(0.5 * e.X);
478  Mdouble sr = std::sin(0.5 * e.X);
479  Mdouble cy = std::cos(0.5 * e.Z);
480  Mdouble sy = std::sin(0.5 * e.Z);
481  q0 = cr * cp * cy + sr * sp * sy;
482  q1 = sr * cp * cy - cr * sp * sy;
483  q2 = cr * sp * cy + sr * cp * sy;
484  q3 = cr * cp * sy - sr * sp * cy;
485 }
486 
488 {
489  return -std::atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3));
490 }
491 
493 {
494  //assuming theta=phi=0
495  q0 = std::cos(-0.5 * psi);
496  q1 = 0;
497  q2 = 0;
498  q3 = std::sin(-0.5 * psi);
499 }
500 
502 {
503  //logger(ERROR,"o % d % d %",*this,Vec3D(1-2.0*q2*q2-2.0*q3*q3, 2.0*(q1*q2+q3*q0), 2.0*(q1*q3-q2*q0)),Vec3D(q0*q0+q1*q1-q2*q2-q3*q3, 2.0*(q1*q2+q3*q0), 2.0*(q1*q3-q2*q0)));
504  return Vec3D(q0 * q0 - q3 * q3 + q1 * q1 - q2 * q2, 2.0 * (q1 * q2 + q3 * q0), 2.0 * (q1 * q3 - q2 * q0));
505 }
506 
507 //retrieves the rotation matrix, often called A in literature.
509 {
510 
511  Mdouble q00 = q0 * q0;
512  Mdouble q01 = 2 * q0 * q1;
513  Mdouble q02 = 2 * q0 * q2;
514  Mdouble q03 = 2 * q0 * q3;
515  Mdouble q11 = q1 * q1;
516  Mdouble q12 = 2 * q1 * q2;
517  Mdouble q13 = 2 * q1 * q3;
518  Mdouble q22 = q2 * q2;
519  Mdouble q23 = 2 * q2 * q3;
520  Mdouble q33 = q3 * q3;
521  A(0, 0) = q00 + q11 - q22 - q33;
522  A(1, 0) = q12 + q03;
523  A(2, 0) = q13 - q02;
524  A(0, 1) = q12 - q03;
525  A(1, 1) = q00 - q11 + q22 - q33;
526  A(2, 1) = q23 + q01;
527  A(0, 2) = q13 + q02;
528  A(1, 2) = q23 - q01;
529  A(2, 2) = q00 - q11 - q22 + q33;
530 }
531 
537 {
538  //if the normal vector cannot be read properly
539 // if (normal.getLengthSquared() < 1e-20)
540 // {
541 // setUnity();
542 // return;
543 // }
544 
545  normal.normalise();
546 
547  if (normal.X <= -1 + 1e-14)
548  {
549  *this = Quaternion(0, 0, 1, 0);
550  return;
551  }
552 
553  Vec3D half = Vec3D(1, 0, 0) + normal;
554  q0 = half.X;
555  q1 = 0;
556  q2 = -half.Z;
557  q3 = half.Y;
558  normalise();
559  //note, it can technically happen that normalising a normalised vector slightly changes the result.
560 }
561 
562 //Euler rodriguez
563 void Quaternion::rotate(Vec3D& position) const
564 {
565  Mdouble q00 = q0 * q0;
566  Mdouble q01 = 2 * q0 * q1;
567  Mdouble q02 = 2 * q0 * q2;
568  Mdouble q03 = 2 * q0 * q3;
569  Mdouble q11 = q1 * q1;
570  Mdouble q12 = 2 * q1 * q2;
571  Mdouble q13 = 2 * q1 * q3;
572  Mdouble q22 = q2 * q2;
573  Mdouble q23 = 2 * q2 * q3;
574  Mdouble q33 = q3 * q3;
575  position = Matrix3D(
576  q00 + q11 - q22 - q33, q12 - q03, q13 + q02,
577  q12 + q03, q00 - q11 + q22 - q33, q23 - q01,
578  q13 - q02, q23 + q01, q00 - q11 - q22 + q33) * position;
579 }
580 
583 void Quaternion::rotate(SmallVector<3>& position) const
584 {
587  position = A * position;
588 }
589 
592 void Quaternion::rotateBack(Vec3D& position) const
593 {
594  Mdouble q00 = q0 * q0;
595  Mdouble q01 = 2 * q0 * q1;
596  Mdouble q02 = 2 * q0 * q2;
597  Mdouble q03 = 2 * q0 * q3;
598  Mdouble q11 = q1 * q1;
599  Mdouble q12 = 2 * q1 * q2;
600  Mdouble q13 = 2 * q1 * q3;
601  Mdouble q22 = q2 * q2;
602  Mdouble q23 = 2 * q2 * q3;
603  Mdouble q33 = q3 * q3;
604  position = Matrix3D(
605  q00 + q11 - q22 - q33, q12 + q03, q13 - q02,
606  q12 - q03, q00 - q11 + q22 - q33, q23 + q01,
607  q13 + q02, q23 - q01, q00 - q11 - q22 + q33) * position;
608 }
609 
610 
615 Mdouble Quaternion::getDistance(const Vec3D p, const Vec3D p0) const
616 {
617  return (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * (p0.X - p.X) +
618  2.0 * ((q1 * q2 + q0 * q3) * (p0.Y - p.Y) + (q1 * q3 - q0 * q2) * (p0.Z - p.Z));
619 }
620 
627 {
628  Mdouble a = 1 - 2 * q2 * q2 - 2 * q3 * q3;
629  Mdouble b = 2 * q1 * q2 - 2 * q0 * q3;
630  Mdouble c = 2 * q1 * q3 + 2 * q0 * q2;
631  Mdouble d = 2 * q1 * q2 + 2 * q0 * q3;
632  Mdouble e = 1 - 2 * q1 * q1 - 2 * q3 * q3;
633  Mdouble f = 2 * q2 * q3 - 2 * q0 * q1;
634  Mdouble g = 2 * q1 * q3 - 2 * q0 * q2;
635  Mdouble h = 2 * q2 * q3 + 2 * q0 * q1;
636  Mdouble i = 1 - 2 * q1 * q1 - 2 * q2 * q2;
638  invI.XX * a * a + 2 * invI.XY * a * b + 2 * invI.XZ * a * c + invI.YY * b * b + 2 * invI.YZ * b * c +
639  invI.ZZ * c * c,
640  d * (invI.XX * a + invI.XY * b + invI.XZ * c) + e * (invI.XY * a + invI.YY * b + invI.YZ * c) +
641  f * (invI.XZ * a + invI.YZ * b + invI.ZZ * c),
642  g * (invI.XX * a + invI.XY * b + invI.XZ * c) + h * (invI.XY * a + invI.YY * b + invI.YZ * c) +
643  i * (invI.XZ * a + invI.YZ * b + invI.ZZ * c),
644  invI.XX * d * d + 2 * invI.XY * d * e + 2 * invI.XZ * d * f + invI.YY * e * e + 2 * invI.YZ * e * f +
645  invI.ZZ * f * f,
646  g * (invI.XX * d + invI.XY * e + invI.XZ * f) + h * (invI.XY * d + invI.YY * e + invI.YZ * f) +
647  i * (invI.XZ * d + invI.YZ * e + invI.ZZ * f),
648  invI.XX * g * g + 2 * invI.XY * g * h + 2 * invI.XZ * g * i + invI.YY * h * h + 2 * invI.YZ * h * i +
649  invI.ZZ * i * i
650  );
651  return ans;
652 }
653 
655 {
658  I = A * I * A.transpose();
659 }
Implementation of a 3D quaternion (by Vitaliy).
Definition: Quaternion.h:62
Vec3D getEuler() const
Convert a quaternion to Euler angles. See Wikipedia for details.
Definition: Quaternion.cc:452
Mdouble X
the vector components
Definition: Vector.h:65
void rotateBack(Vec3D &position) const
Applies the inverse rotation to a position.
Definition: Quaternion.cc:592
Logger< MERCURY_LOGLEVEL > logger("MercuryKernel")
double Mdouble
Definition: GeneralDefine.h:34
Mdouble getComponent(int index) const
Returns the requested component of this Quaternion.
Definition: Quaternion.cc:232
void normalise()
Makes this Vec3D unit length.
Definition: Vector.cc:123
static Quaternion getUnitQuaternion(const Quaternion &a)
Returns a unit Quaternion based on a.
Definition: Quaternion.cc:324
static Mdouble getDistanceSquared(const Quaternion &a, const Quaternion &b)
Calculates the squared distance between two Quaternion: .
Definition: Quaternion.cc:201
void rotateTensor(SmallMatrix< 3, 3 > I) const
Definition: Quaternion.cc:654
SmallMatrix< numberOfColumns, numberOfRows > transpose() const
Definition: SmallMatrix.h:330
std::istream & operator>>(std::istream &is, Quaternion &a)
Definition: Quaternion.cc:353
Quaternion operator*(Mdouble a) const
Multiplies by a scalar.
Definition: Quaternion.cc:86
Quaternion & operator*=(Mdouble a)
Multiplies *this by a scalar.
Definition: Quaternion.cc:134
const std::complex< Mdouble > i
Definition: ExtendedMath.h:50
MatrixSymmetric3D rotateInverseInertiaTensor(const MatrixSymmetric3D &invI) const
Converts the inverse inertia tensor from the reference frame to the lab frame; see See QuaternionsWou...
Definition: Quaternion.cc:626
Quaternion()
Constructor; sets quaternion value to (1,0,0,0)
Definition: Quaternion.cc:30
Quaternion operator*(const Mdouble a, const Quaternion &b)
Definition: Quaternion.cc:405
static Mdouble getLength(const Quaternion &a)
Calculates the length of a Quaternion: .
Definition: Quaternion.cc:310
Implementation of a 3D vector (by Vitaliy).
Definition: SmallVector.h:61
void setComponent(int index, double val)
Sets the requested component of this Quaternion to the requested value.
Definition: Quaternion.cc:258
void setLength(Mdouble length)
Make this Quaternion a certain length |q|=length.
Definition: Quaternion.cc:176
Mdouble q2
the second component of the quaternion q = (q0,q1,q2,q3)
Definition: Quaternion.h:77
Quaternion angularVelocityBodyFixedFrameToAngularDisplacement(Vec3D v) const
Definition: Quaternion.cc:411
void rotate(Vec3D &position) const
Applies the rotation to a position.
Definition: Quaternion.cc:563
Quaternion angularDisplacementTimeDerivative(Vec3D v) const
Converts an angular momentum v=omega into a quaternion rate of change, q(t+dt)-q(t)/dt.
Definition: Quaternion.cc:422
Vec3D getAxis() const
Converts the quaternions into a normal vector by rotating the vector x=(1,0,0); see See Wiki for deta...
Definition: Quaternion.cc:501
Mdouble cos(Mdouble x)
Definition: ExtendedMath.cc:64
Quaternion operator+(const Quaternion &a) const
Adds another quaternion and returns the result.
Definition: Quaternion.cc:66
Mdouble getAngleZ() const
Converts a quaternion to the rotation angle in the XY plane (for Mercury2D). See Wikipedia for detail...
Definition: Quaternion.cc:487
void setAngleZ(Mdouble psi)
Converts the rotation angle in the XY plane into a quaternion (for Mercury2D). See Wikipedia for deta...
Definition: Quaternion.cc:492
std::ostream & operator<<(std::ostream &os, const Quaternion &a)
Definition: Quaternion.cc:340
Mdouble sin(Mdouble x)
Definition: ExtendedMath.cc:44
static Mdouble getDistance(const Quaternion &a, const Quaternion &b)
Calculates the distance between two Quaternion: .
Definition: Quaternion.cc:188
const Mdouble pi
Definition: ExtendedMath.h:45
void setEuler(const Vec3D &e)
Convert Euler angles to a quaternion. See Wikipedia for details.
Definition: Quaternion.cc:473
Mdouble q3
the third component of the quaternion q = (q0,q1,q2,q3)
Definition: Quaternion.h:81
void setOrientationViaNormal(Vec3D normal)
Used to the the normal of an InfiniteWall that has a normal into the x-direction by default...
Definition: Quaternion.cc:536
Quaternion & operator+=(const Quaternion &a)
Adds another quaternion.
Definition: Quaternion.cc:106
static Mdouble getLengthSquared(const Quaternion &a)
Calculates the squared length of a Quaternion: .
Definition: Quaternion.cc:213
Quaternion & operator-=(const Quaternion &a)
Subtracts another quaternion.
Definition: Quaternion.cc:120
Mdouble Y
Definition: Vector.h:65
Vec3D applyCInverse(Quaternion q) const
Converts quaternion rate of change into an angular momentum omega.
Definition: Quaternion.cc:441
bool isEqualTo(const Quaternion &other, double tol) const
Checks if the length this Quaternion is equal the length of other with a certain tolerance.
Definition: Quaternion.cc:290
void getRotationMatrix(SmallMatrix< 3, 3 > &A) const
Retrieves the rotation matrix.
Definition: Quaternion.cc:508
Quaternion operator+(const Mdouble a, const Quaternion &b)
Definition: Quaternion.cc:367
Mdouble getLengthSquared() const
Calculates the squared length of this Quaternion: .
Definition: Quaternion.cc:222
Implementation of a 3D matrix.
Definition: Matrix.h:37
Quaternion operator-(const Quaternion &a) const
Subtracts another quaternion and returns the result.
Definition: Quaternion.cc:76
void updateAngularDisplacement(Vec3D angularVelocityDt)
Definition: Quaternion.cc:431
Definition: Vector.h:49
Data type for small dense matrix.
Definition: SmallMatrix.h:67
Mdouble Z
Definition: Vector.h:65
Quaternion & operator/=(Mdouble a)
Divides by a scalar.
Definition: Quaternion.cc:148
Mdouble q0
the zeroth component of the quaternion q = (q0,q1,q2,q3)
Definition: Quaternion.h:69
Implementation of a 3D symmetric matrix.
Quaternion operator/(Mdouble a) const
Divides by a scalar.
Definition: Quaternion.cc:96
Mdouble q1
the first component of the quaternion q = (q0,q1,q2,q3)
Definition: Quaternion.h:73
void normalise()
Makes this Quaternion unit length |q|=1.
Definition: Quaternion.cc:161
Mdouble XX
The six distinctive matrix elements.
void setUnity()
Sets quaternion value to (1,0,0,0)
Definition: Quaternion.cc:53
Mdouble getLength() const
Calculates the length of this Quaternion: .
Definition: Quaternion.cc:299
Quaternion operator-(const Mdouble a, const Quaternion &b)
Definition: Quaternion.cc:380