NumCpp  2.8.0
A Templatized Header Only C++ Implementation of the Python NumPy Library
Quaternion.hpp
Go to the documentation of this file.
1 #pragma once
29 
30 #include <array>
31 #include <cmath>
32 #include <iostream>
33 #include <string>
34 
37 #include "NumCpp/Core/Types.hpp"
40 #include "NumCpp/Functions/dot.hpp"
43 #include "NumCpp/Linalg/hat.hpp"
44 #include "NumCpp/NdArray.hpp"
46 #include "NumCpp/Utils/num2str.hpp"
47 #include "NumCpp/Utils/sqr.hpp"
48 #include "NumCpp/Vector/Vec3.hpp"
49 
50 namespace nc
51 {
52  namespace rotations
53  {
54  //================================================================================
55  // Class Description:
57  class Quaternion
58  {
59  public:
60  //============================================================================
61  // Method Description:
64  Quaternion() = default;
65 
66  //============================================================================
67  // Method Description:
74  Quaternion(double roll, double pitch, double yaw) noexcept
75  {
76  eulerToQuat(roll, pitch, yaw);
77  }
78 
79  //============================================================================
80  // Method Description:
88  Quaternion(double inI, double inJ, double inK, double inS) noexcept :
89  components_{ inI, inJ, inK, inS }
90  {
91  normalize();
92  }
93 
94  //============================================================================
95  // Method Description:
102  Quaternion(const NdArray<double>& inArray) :
103  components_{ 0.0, 0.0, 0.0, 0.0 }
104  {
105  if (inArray.size() == 3)
106  {
107  // euler angles
108  eulerToQuat(inArray[0], inArray[1], inArray[2]);
109  }
110  else if (inArray.size() == 4)
111  {
112  // quaternion i, j, k, s components
113  stl_algorithms::copy(inArray.cbegin(), inArray.cend(), components_.begin());
114  normalize();
115  }
116  else if (inArray.size() == 9)
117  {
118  // direction cosine matrix
119  dcmToQuat(inArray);
120  }
121  else
122  {
123  THROW_INVALID_ARGUMENT_ERROR("input array is not a valid size.");
124  }
125  }
126 
127  //============================================================================
128  // Method Description:
134  Quaternion(const Vec3& inAxis, double inAngle) noexcept
135  {
136  // normalize the input vector
137  Vec3 normAxis = inAxis.normalize();
138 
139  const double halfAngle = inAngle / 2.0;
140  const double sinHalfAngle = std::sin(halfAngle);
141 
142  components_[0] = normAxis.x * sinHalfAngle;
143  components_[1] = normAxis.y * sinHalfAngle;
144  components_[2] = normAxis.z * sinHalfAngle;
145  components_[3] = std::cos(halfAngle);
146  }
147 
148  //============================================================================
149  // Method Description:
155  Quaternion(const NdArray<double>& inAxis, double inAngle) :
156  Quaternion(Vec3(inAxis), inAngle)
157  {
158  }
159 
160  //============================================================================
161  // Method Description:
170  static NdArray<double> angularVelocity(const Quaternion& inQuat1, const Quaternion& inQuat2, double inTime)
171  {
172  NdArray<double> q0 = inQuat1.toNdArray();
173  NdArray<double> q1 = inQuat2.toNdArray();
174 
175  NdArray<double> qDot = q1 - q0;
176  qDot /= inTime;
177 
178  NdArray<double> eyeTimesScalar(3);
179  eyeTimesScalar.zeros();
180  eyeTimesScalar(0, 0) = inQuat2.s();
181  eyeTimesScalar(1, 1) = inQuat2.s();
182  eyeTimesScalar(2, 2) = inQuat2.s();
183 
184  NdArray<double> epsilonHat = linalg::hat<double>(inQuat2.i(), inQuat2.j(), inQuat2.k());
185  NdArray<double> q(4, 3);
186  q.put(Slice(0, 3), Slice(0, 3), eyeTimesScalar + epsilonHat);
187  q(3, 0) = -inQuat2.i();
188  q(3, 1) = -inQuat2.j();
189  q(3, 2) = -inQuat2.k();
190 
191  NdArray<double> omega = q.transpose().dot(qDot.transpose());
192  return omega *= 2.0;
193  }
194 
195  //============================================================================
196  // Method Description:
204  NdArray<double> angularVelocity(const Quaternion& inQuat2, double inTime) const
205  {
206  return angularVelocity(*this, inQuat2, inTime);
207  }
208 
209  //============================================================================
210  // Method Description:
215  Quaternion conjugate() const noexcept
216  {
217  return { -i(), -j(), -k(), s() };
218  }
219 
220  //============================================================================
221  // Method Description:
226  double i() const noexcept
227  {
228  return components_[0];
229  }
230 
231  //============================================================================
232  // Method Description:
237  static Quaternion identity() noexcept
238  {
239  return {};
240  }
241 
242  //============================================================================
243  // Method Description:
248  Quaternion inverse() const noexcept
249  {
251  return conjugate();
252  }
253 
254  //============================================================================
255  // Method Description:
260  double j() const noexcept
261  {
262  return components_[1];
263  }
264 
265  //============================================================================
266  // Method Description:
271  double k() const noexcept
272  {
273  return components_[2];
274  }
275 
276  //============================================================================
277  // Method Description:
285  static Quaternion nlerp(const Quaternion& inQuat1, const Quaternion& inQuat2, double inPercent)
286  {
287  if (inPercent < 0.0 || inPercent > 1.0)
288  {
289  THROW_INVALID_ARGUMENT_ERROR("input percent must be of the range [0,1].");
290  }
291 
292  if (utils::essentiallyEqual(inPercent, 0.0))
293  {
294  return inQuat1;
295  }
296  if (utils::essentiallyEqual(inPercent, 1.0))
297  {
298  return inQuat2;
299  }
300 
301  const double oneMinus = 1.0 - inPercent;
302  std::array<double, 4> newComponents{};
303 
304  stl_algorithms::transform(inQuat1.components_.begin(),
305  inQuat1.components_.end(),
306  inQuat2.components_.begin(),
307  newComponents.begin(),
308  [inPercent, oneMinus](double component1, double component2) -> double
309  { return oneMinus * component1 + inPercent * component2; });
310 
311  return { newComponents[0], newComponents[1], newComponents[2], newComponents[3] };
312  }
313 
314  //============================================================================
315  // Method Description:
322  Quaternion nlerp(const Quaternion& inQuat2, double inPercent) const
323  {
324  return nlerp(*this, inQuat2, inPercent);
325  }
326 
327  //============================================================================
328  // Method Description:
333  double pitch() const noexcept
334  {
335  return std::asin(2 * (s() * j() - k() * i()));
336  }
337 
338  //============================================================================
339  // Method Description:
342  void print() const
343  {
344  std::cout << *this;
345  }
346 
347  //============================================================================
348  // Method Description:
353  double roll() const noexcept
354  {
355  return std::atan2(2 * (s() * i() + j() * k()), 1 - 2 * (utils::sqr(i()) + utils::sqr(j())));
356  }
357 
358  //============================================================================
359  // Method Description:
365  NdArray<double> rotate(const NdArray<double>& inVector) const
366  {
367  if (inVector.size() != 3)
368  {
369  THROW_INVALID_ARGUMENT_ERROR("input inVector must be a cartesion vector of length = 3.");
370  }
371 
372  return *this * inVector;
373  }
374 
375  //============================================================================
376  // Method Description:
382  Vec3 rotate(const Vec3& inVec3) const
383  {
384  return *this * inVec3;
385  }
386 
387  //============================================================================
388  // Method Description:
393  double s() const noexcept
394  {
395  return components_[3];
396  }
397 
398  //============================================================================
399  // Method Description:
407  static Quaternion slerp(const Quaternion& inQuat1, const Quaternion& inQuat2, double inPercent)
408  {
409  if (inPercent < 0 || inPercent > 1)
410  {
411  THROW_INVALID_ARGUMENT_ERROR("input percent must be of the range [0, 1]");
412  }
413 
414  if (inPercent == 0)
415  {
416  return inQuat1;
417  }
418  if (inPercent == 1)
419  {
420  return inQuat2;
421  }
422 
423  double dotProduct = dot<double>(inQuat1.toNdArray(), inQuat2.toNdArray()).item();
424 
425  // If the dot product is negative, the quaternions
426  // have opposite handed-ness and slerp won't take
427  // the shorter path. Fix by reversing one quaternion.
428  Quaternion quat1Copy(inQuat1);
429  if (dotProduct < 0.0)
430  {
431  quat1Copy *= -1;
432  dotProduct *= -1;
433  }
434 
435  constexpr double DOT_THRESHOLD = 0.9995;
436  if (dotProduct > DOT_THRESHOLD)
437  {
438  // If the inputs are too close for comfort, linearly interpolate
439  // and normalize the result.
440  return nlerp(inQuat1, inQuat2, inPercent);
441  }
442 
443  dotProduct = clip(dotProduct, -1.0, 1.0); // Robustness: Stay within domain of acos()
444  const double theta0 = std::acos(dotProduct); // angle between input vectors
445  const double theta = theta0 * inPercent; // angle between v0 and result
446 
447  const double s0 = std::cos(theta) - dotProduct * std::sin(theta) /
448  std::sin(theta0); // == sin(theta_0 - theta) / sin(theta_0)
449  const double s1 = std::sin(theta) / std::sin(theta0);
450 
451  NdArray<double> interpQuat = (quat1Copy.toNdArray() * s0) + (inQuat2.toNdArray() * s1);
452  return Quaternion(interpQuat);
453  }
454 
455  //============================================================================
456  // Method Description:
463  Quaternion slerp(const Quaternion& inQuat2, double inPercent) const
464  {
465  return slerp(*this, inQuat2, inPercent);
466  }
467 
468  //============================================================================
469  // Method Description:
474  std::string str() const
475  {
476  std::string output = "[" + utils::num2str(i()) + ", " + utils::num2str(j()) + ", " +
477  utils::num2str(k()) + ", " + utils::num2str(s()) + "]\n";
478 
479  return output;
480  }
481 
482  //============================================================================
483  // Method Description:
489  {
490  NdArray<double> dcm(3);
491 
492  const double q0 = i();
493  const double q1 = j();
494  const double q2 = k();
495  const double q3 = s();
496 
497  const double q0sqr = utils::sqr(q0);
498  const double q1sqr = utils::sqr(q1);
499  const double q2sqr = utils::sqr(q2);
500  const double q3sqr = utils::sqr(q3);
501 
502  dcm(0, 0) = q3sqr + q0sqr - q1sqr - q2sqr;
503  dcm(0, 1) = 2 * (q0 * q1 - q3 * q2);
504  dcm(0, 2) = 2 * (q0 * q2 + q3 * q1);
505  dcm(1, 0) = 2 * (q0 * q1 + q3 * q2);
506  dcm(1, 1) = q3sqr + q1sqr - q0sqr - q2sqr;
507  dcm(1, 2) = 2 * (q1 * q2 - q3 * q0);
508  dcm(2, 0) = 2 * (q0 * q2 - q3 * q1);
509  dcm(2, 1) = 2 * (q1 * q2 + q3 * q0);
510  dcm(2, 2) = q3sqr + q2sqr - q0sqr - q1sqr;
511 
512  return dcm;
513  }
514 
515  //============================================================================
516  // Method Description:
522  {
523  auto componentsCopy = components_;
524  return NdArray<double>(componentsCopy);
525  }
526 
527  //============================================================================
528  // Method Description:
534  static Quaternion xRotation(double inAngle) noexcept
535  {
536  const Vec3 eulerAxis = { 1.0, 0.0, 0.0 };
537  return Quaternion(eulerAxis, inAngle);
538  }
539 
540  //============================================================================
541  // Method Description:
546  double yaw() const noexcept
547  {
548  return std::atan2(2 * (s() * k() + i() * j()), 1 - 2 * (utils::sqr(j()) + utils::sqr(k())));
549  }
550 
551  //============================================================================
552  // Method Description:
558  static Quaternion yRotation(double inAngle) noexcept
559  {
560  const Vec3 eulerAxis = { 0.0, 1.0, 0.0 };
561  return Quaternion(eulerAxis, inAngle);
562  }
563 
564  //============================================================================
565  // Method Description:
571  static Quaternion zRotation(double inAngle) noexcept
572  {
573  const Vec3 eulerAxis = { 0.0, 0.0, 1.0 };
574  return Quaternion(eulerAxis, inAngle);
575  }
576 
577  //============================================================================
578  // Method Description:
584  bool operator==(const Quaternion& inRhs) const noexcept
585  {
586  const auto comparitor = [](double value1, double value2) noexcept -> bool
587  { return utils::essentiallyEqual(value1, value2); };
588 
589  return stl_algorithms::equal(components_.begin(),
590  components_.end(),
591  inRhs.components_.begin(),
592  comparitor);
593  }
594 
595  //============================================================================
596  // Method Description:
602  bool operator!=(const Quaternion& inRhs) const noexcept
603  {
604  return !(*this == inRhs);
605  }
606 
607  //============================================================================
608  // Method Description:
614  Quaternion& operator+=(const Quaternion& inRhs) noexcept
615  {
616  stl_algorithms::transform(components_.begin(),
617  components_.end(),
618  inRhs.components_.begin(),
619  components_.begin(),
620  std::plus<double>());
621 
622  normalize();
623 
624  return *this;
625  }
626 
627  //============================================================================
628  // Method Description:
634  Quaternion operator+(const Quaternion& inRhs) const noexcept
635  {
636  return Quaternion(*this) += inRhs;
637  }
638 
639  //============================================================================
640  // Method Description:
646  Quaternion& operator-=(const Quaternion& inRhs) noexcept
647  {
648  stl_algorithms::transform(components_.begin(),
649  components_.end(),
650  inRhs.components_.begin(),
651  components_.begin(),
652  std::minus<double>());
653 
654  normalize();
655 
656  return *this;
657  }
658 
659  //============================================================================
660  // Method Description:
666  Quaternion operator-(const Quaternion& inRhs) const noexcept
667  {
668  return Quaternion(*this) -= inRhs;
669  }
670 
671  //============================================================================
672  // Method Description:
677  Quaternion operator-() const noexcept
678  {
679  return Quaternion(*this) *= -1.0;
680  }
681 
682  //============================================================================
683  // Method Description:
689  Quaternion& operator*=(const Quaternion& inRhs) noexcept
690  {
691  double q0 = inRhs.s() * i();
692  q0 += inRhs.i() * s();
693  q0 -= inRhs.j() * k();
694  q0 += inRhs.k() * j();
695 
696  double q1 = inRhs.s() * j();
697  q1 += inRhs.i() * k();
698  q1 += inRhs.j() * s();
699  q1 -= inRhs.k() * i();
700 
701  double q2 = inRhs.s() * k();
702  q2 -= inRhs.i() * j();
703  q2 += inRhs.j() * i();
704  q2 += inRhs.k() * s();
705 
706  double q3 = inRhs.s() * s();
707  q3 -= inRhs.i() * i();
708  q3 -= inRhs.j() * j();
709  q3 -= inRhs.k() * k();
710 
711  components_[0] = q0;
712  components_[1] = q1;
713  components_[2] = q2;
714  components_[3] = q3;
715 
716  normalize();
717 
718  return *this;
719  }
720 
721  //============================================================================
722  // Method Description:
729  Quaternion& operator*=(double inScalar) noexcept
730  {
731  stl_algorithms::for_each(components_.begin(),
732  components_.end(),
733  [inScalar](double& component) { component *= inScalar; });
734 
735  normalize();
736 
737  return *this;
738  }
739 
740  //============================================================================
741  // Method Description:
747  Quaternion operator*(const Quaternion& inRhs) const noexcept
748  {
749  return Quaternion(*this) *= inRhs;
750  }
751 
752  //============================================================================
753  // Method Description:
760  Quaternion operator*(double inScalar) const noexcept
761  {
762  return Quaternion(*this) *= inScalar;
763  }
764 
765  //============================================================================
766  // Method Description:
773  {
774  if (inVec.size() != 3)
775  {
776  THROW_INVALID_ARGUMENT_ERROR("input vector must be a cartesion vector of length = 3.");
777  }
778 
779  const auto p = Quaternion(inVec[0], inVec[1], inVec[2], 0.0);
780  const auto pPrime = *this * p * this->inverse();
781 
782  NdArray<double> rotatedVec = { pPrime.i(), pPrime.j(), pPrime.k() };
783  rotatedVec *= norm(inVec).item();
784  return rotatedVec;
785  }
786 
787  //============================================================================
788  // Method Description:
794  Vec3 operator*(const Vec3& inVec3) const
795  {
796  return *this * inVec3.toNdArray();
797  }
798 
799  //============================================================================
800  // Method Description:
806  Quaternion& operator/=(const Quaternion& inRhs) noexcept
807  {
808  return *this *= inRhs.conjugate();
809  }
810 
811  //============================================================================
812  // Method Description:
818  Quaternion operator/(const Quaternion& inRhs) const noexcept
819  {
820  return Quaternion(*this) /= inRhs;
821  }
822 
823  //============================================================================
824  // Method Description:
831  friend std::ostream& operator<<(std::ostream& inOStream, const Quaternion& inQuat)
832  {
833  inOStream << inQuat.str();
834  return inOStream;
835  }
836 
837  private:
838  //====================================Attributes==============================
839  std::array<double, 4> components_{ { 0.0, 0.0, 0.0, 1.0 } };
840 
841  //============================================================================
842  // Method Description:
845  void normalize() noexcept
846  {
847  double sumOfSquares = 0.0;
848  std::for_each(components_.begin(),
849  components_.end(),
850  [&sumOfSquares](double component) noexcept -> void
851  { sumOfSquares += utils::sqr(component); });
852 
853  const double norm = std::sqrt(sumOfSquares);
854  stl_algorithms::for_each(components_.begin(),
855  components_.end(),
856  [norm](double& component) noexcept -> void { component /= norm; });
857  }
858 
859  //============================================================================
860  // Method Description:
867  void eulerToQuat(double roll, double pitch, double yaw) noexcept
868  {
869  const double halfPhi = roll / 2.0;
870  const double halfTheta = pitch / 2.0;
871  const double halfPsi = yaw / 2.0;
872 
873  components_[0] = std::sin(halfPhi) * std::cos(halfTheta) * std::cos(halfPsi);
874  components_[0] -= std::cos(halfPhi) * std::sin(halfTheta) * std::sin(halfPsi);
875 
876  components_[1] = std::cos(halfPhi) * std::sin(halfTheta) * std::cos(halfPsi);
877  components_[1] += std::sin(halfPhi) * std::cos(halfTheta) * std::sin(halfPsi);
878 
879  components_[2] = std::cos(halfPhi) * std::cos(halfTheta) * std::sin(halfPsi);
880  components_[2] -= std::sin(halfPhi) * std::sin(halfTheta) * std::cos(halfPsi);
881 
882  components_[3] = std::cos(halfPhi) * std::cos(halfTheta) * std::cos(halfPsi);
883  components_[3] += std::sin(halfPhi) * std::sin(halfTheta) * std::sin(halfPsi);
884  }
885 
886  //============================================================================
887  // Method Description:
892  void dcmToQuat(const NdArray<double>& dcm)
893  {
894  const Shape inShape = dcm.shape();
895  if (!(inShape.rows == 3 && inShape.cols == 3))
896  {
897  THROW_INVALID_ARGUMENT_ERROR("input direction cosine matrix must have shape = (3,3).");
898  }
899 
900  NdArray<double> checks(1, 4);
901  checks[0] = 1 + dcm(0, 0) + dcm(1, 1) + dcm(2, 2);
902  checks[1] = 1 + dcm(0, 0) - dcm(1, 1) - dcm(2, 2);
903  checks[2] = 1 - dcm(0, 0) + dcm(1, 1) - dcm(2, 2);
904  checks[3] = 1 - dcm(0, 0) - dcm(1, 1) + dcm(2, 2);
905 
906  const uint32 maxIdx = argmax(checks).item();
907 
908  switch (maxIdx)
909  {
910  case 0:
911  {
912  components_[3] = 0.5 * std::sqrt(1 + dcm(0, 0) + dcm(1, 1) + dcm(2, 2));
913  components_[0] = (dcm(2, 1) - dcm(1, 2)) / (4 * components_[3]);
914  components_[1] = (dcm(0, 2) - dcm(2, 0)) / (4 * components_[3]);
915  components_[2] = (dcm(1, 0) - dcm(0, 1)) / (4 * components_[3]);
916 
917  break;
918  }
919  case 1:
920  {
921  components_[0] = 0.5 * std::sqrt(1 + dcm(0, 0) - dcm(1, 1) - dcm(2, 2));
922  components_[1] = (dcm(1, 0) + dcm(0, 1)) / (4 * components_[0]);
923  components_[2] = (dcm(2, 0) + dcm(0, 2)) / (4 * components_[0]);
924  components_[3] = (dcm(2, 1) - dcm(1, 2)) / (4 * components_[0]);
925 
926  break;
927  }
928  case 2:
929  {
930  components_[1] = 0.5 * std::sqrt(1 - dcm(0, 0) + dcm(1, 1) - dcm(2, 2));
931  components_[0] = (dcm(1, 0) + dcm(0, 1)) / (4 * components_[1]);
932  components_[2] = (dcm(2, 1) + dcm(1, 2)) / (4 * components_[1]);
933  components_[3] = (dcm(0, 2) - dcm(2, 0)) / (4 * components_[1]);
934 
935  break;
936  }
937  case 3:
938  {
939  components_[2] = 0.5 * std::sqrt(1 - dcm(0, 0) - dcm(1, 1) + dcm(2, 2));
940  components_[0] = (dcm(2, 0) + dcm(0, 2)) / (4 * components_[2]);
941  components_[1] = (dcm(2, 1) + dcm(1, 2)) / (4 * components_[2]);
942  components_[3] = (dcm(1, 0) - dcm(0, 1)) / (4 * components_[2]);
943 
944  break;
945  }
946  }
947  }
948  };
949  } // namespace rotations
950 } // namespace nc
#define THROW_INVALID_ARGUMENT_ERROR(msg)
Definition: Error.hpp:36
NdArray< dtype > & put(int32 inIndex, value_type inValue)
Definition: NdArrayCore.hpp:3663
size_type size() const noexcept
Definition: NdArrayCore.hpp:4289
const_iterator cbegin() const noexcept
Definition: NdArrayCore.hpp:1221
NdArray< dtype > transpose() const
Definition: NdArrayCore.hpp:4650
const_iterator cend() const noexcept
Definition: NdArrayCore.hpp:1529
value_type item() const
Definition: NdArrayCore.hpp:2921
NdArray< dtype > & zeros() noexcept
Definition: NdArrayCore.hpp:4668
NdArray< dtype > dot(const NdArray< dtype > &inOtherArray) const
Definition: NdArrayCore.hpp:2623
A Class for slicing into NdArrays.
Definition: Slice.hpp:44
Holds a 3D vector.
Definition: Vec3.hpp:50
double z
Definition: Vec3.hpp:55
NdArray< double > toNdArray() const
Definition: Vec3.hpp:324
Vec3 normalize() const noexcept
Definition: Vec3.hpp:276
double x
Definition: Vec3.hpp:53
double y
Definition: Vec3.hpp:54
Holds a unit quaternion.
Definition: Quaternion.hpp:58
double s() const noexcept
Definition: Quaternion.hpp:393
std::string str() const
Definition: Quaternion.hpp:474
NdArray< double > operator*(const NdArray< double > &inVec) const
Definition: Quaternion.hpp:772
NdArray< double > angularVelocity(const Quaternion &inQuat2, double inTime) const
Definition: Quaternion.hpp:204
Quaternion & operator*=(double inScalar) noexcept
Definition: Quaternion.hpp:729
double roll() const noexcept
Definition: Quaternion.hpp:353
NdArray< double > rotate(const NdArray< double > &inVector) const
Definition: Quaternion.hpp:365
static Quaternion xRotation(double inAngle) noexcept
Definition: Quaternion.hpp:534
static Quaternion nlerp(const Quaternion &inQuat1, const Quaternion &inQuat2, double inPercent)
Definition: Quaternion.hpp:285
Vec3 rotate(const Vec3 &inVec3) const
Definition: Quaternion.hpp:382
Quaternion(double inI, double inJ, double inK, double inS) noexcept
Definition: Quaternion.hpp:88
Quaternion operator-() const noexcept
Definition: Quaternion.hpp:677
NdArray< double > toNdArray() const
Definition: Quaternion.hpp:521
Quaternion operator+(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:634
double i() const noexcept
Definition: Quaternion.hpp:226
double yaw() const noexcept
Definition: Quaternion.hpp:546
double pitch() const noexcept
Definition: Quaternion.hpp:333
Quaternion & operator-=(const Quaternion &inRhs) noexcept
Definition: Quaternion.hpp:646
Quaternion slerp(const Quaternion &inQuat2, double inPercent) const
Definition: Quaternion.hpp:463
friend std::ostream & operator<<(std::ostream &inOStream, const Quaternion &inQuat)
Definition: Quaternion.hpp:831
static Quaternion slerp(const Quaternion &inQuat1, const Quaternion &inQuat2, double inPercent)
Definition: Quaternion.hpp:407
static NdArray< double > angularVelocity(const Quaternion &inQuat1, const Quaternion &inQuat2, double inTime)
Definition: Quaternion.hpp:170
void print() const
Definition: Quaternion.hpp:342
Quaternion(const NdArray< double > &inAxis, double inAngle)
Definition: Quaternion.hpp:155
bool operator==(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:584
Quaternion & operator/=(const Quaternion &inRhs) noexcept
Definition: Quaternion.hpp:806
Quaternion(double roll, double pitch, double yaw) noexcept
Definition: Quaternion.hpp:74
Vec3 operator*(const Vec3 &inVec3) const
Definition: Quaternion.hpp:794
Quaternion inverse() const noexcept
Definition: Quaternion.hpp:248
double k() const noexcept
Definition: Quaternion.hpp:271
NdArray< double > toDCM() const
Definition: Quaternion.hpp:488
Quaternion & operator*=(const Quaternion &inRhs) noexcept
Definition: Quaternion.hpp:689
static Quaternion zRotation(double inAngle) noexcept
Definition: Quaternion.hpp:571
Quaternion operator/(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:818
Quaternion nlerp(const Quaternion &inQuat2, double inPercent) const
Definition: Quaternion.hpp:322
static Quaternion yRotation(double inAngle) noexcept
Definition: Quaternion.hpp:558
Quaternion(const Vec3 &inAxis, double inAngle) noexcept
Definition: Quaternion.hpp:134
double j() const noexcept
Definition: Quaternion.hpp:260
Quaternion operator*(double inScalar) const noexcept
Definition: Quaternion.hpp:760
Quaternion operator-(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:666
Quaternion operator*(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:747
bool operator!=(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:602
Quaternion(const NdArray< double > &inArray)
Definition: Quaternion.hpp:102
Quaternion conjugate() const noexcept
Definition: Quaternion.hpp:215
static Quaternion identity() noexcept
Definition: Quaternion.hpp:237
Quaternion & operator+=(const Quaternion &inRhs) noexcept
Definition: Quaternion.hpp:614
OutputIt transform(InputIt first, InputIt last, OutputIt destination, UnaryOperation unaryFunction)
Definition: StlAlgorithms.hpp:784
void for_each(InputIt first, InputIt last, UnaryFunction f)
Definition: StlAlgorithms.hpp:227
bool equal(InputIt1 first1, InputIt1 last1, InputIt2 first2) noexcept
Definition: StlAlgorithms.hpp:142
OutputIt copy(InputIt first, InputIt last, OutputIt destination) noexcept
Definition: StlAlgorithms.hpp:99
std::string num2str(dtype inNumber)
Definition: num2str.hpp:46
bool essentiallyEqual(dtype inValue1, dtype inValue2) noexcept
Definition: essentiallyEqual.hpp:51
constexpr dtype sqr(dtype inValue) noexcept
Definition: sqr.hpp:44
Definition: Coordinate.hpp:45
NdArray< uint32 > argmax(const NdArray< dtype > &inArray, Axis inAxis=Axis::NONE)
Definition: argmax.hpp:46
auto sin(dtype inValue) noexcept
Definition: sin.hpp:49
dtype clip(dtype inValue, dtype inMinValue, dtype inMaxValue)
Definition: clip.hpp:50
auto cos(dtype inValue) noexcept
Definition: cos.hpp:49
auto sqrt(dtype inValue) noexcept
Definition: sqrt.hpp:48
NdArray< double > norm(const NdArray< dtype > &inArray, Axis inAxis=Axis::NONE)
Definition: norm.hpp:51
std::uint32_t uint32
Definition: Types.hpp:40