NumCpp  2.6.2
A Templatized Header Only C++ Implementation of the Python NumPy Library
Quaternion.hpp
Go to the documentation of this file.
1 
28 #pragma once
29 
32 #include "NumCpp/Core/Types.hpp"
35 #include "NumCpp/Functions/dot.hpp"
38 #include "NumCpp/Linalg/hat.hpp"
39 #include "NumCpp/NdArray.hpp"
41 #include "NumCpp/Utils/num2str.hpp"
42 #include "NumCpp/Utils/sqr.hpp"
43 #include "NumCpp/Vector/Vec3.hpp"
44 
45 #include <array>
46 #include <cmath>
47 #include <iostream>
48 #include <string>
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  // 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:
205  NdArray<double> angularVelocity(const Quaternion& inQuat2, double inTime) const
206  {
207  return angularVelocity(*this, inQuat2, inTime);
208  }
209 
210  //============================================================================
211  // Method Description:
217  Quaternion conjugate() const noexcept
218  {
219  return {-i(), -j(), -k(), s()};
220  }
221 
222  //============================================================================
223  // Method Description:
229  double i() const noexcept
230  {
231  return components_[0];
232  }
233 
234  //============================================================================
235  // Method Description:
241  static Quaternion identity() noexcept
242  {
243  return {};
244  }
245 
246  //============================================================================
247  // Method Description:
253  Quaternion inverse() const noexcept
254  {
256  return conjugate();
257  }
258 
259  //============================================================================
260  // Method Description:
266  double j() const noexcept
267  {
268  return components_[1];
269  }
270 
271  //============================================================================
272  // Method Description:
278  double k() const noexcept
279  {
280  return components_[2];
281  }
282 
283  //============================================================================
284  // Method Description:
293  static Quaternion nlerp(const Quaternion& inQuat1, const Quaternion& inQuat2, double inPercent)
294  {
295  if (inPercent < 0.0 || inPercent > 1.0)
296  {
297  THROW_INVALID_ARGUMENT_ERROR("input percent must be of the range [0,1].");
298  }
299 
300  if (utils::essentiallyEqual(inPercent, 0.0))
301  {
302  return inQuat1;
303  }
304  if (utils::essentiallyEqual(inPercent, 1.0))
305  {
306  return inQuat2;
307  }
308 
309  const double oneMinus = 1.0 - inPercent;
310  std::array<double, 4> newComponents{};
311 
312  stl_algorithms::transform(inQuat1.components_.begin(), inQuat1.components_.end(),
313  inQuat2.components_.begin(), newComponents.begin(),
314  [inPercent, oneMinus](double component1, double component2) -> double
315  {
316  return oneMinus * component1 + inPercent * component2;
317  });
318 
319  return {newComponents[0], newComponents[1], newComponents[2], newComponents[3]};
320  }
321 
322  //============================================================================
323  // Method Description:
331  Quaternion nlerp(const Quaternion& inQuat2, double inPercent) const
332  {
333  return nlerp(*this, inQuat2, inPercent);
334  }
335 
336  //============================================================================
337  // Method Description:
342  double pitch() const noexcept
343  {
344  return std::asin(2 * (s() * j() - k() * i()));
345  }
346 
347  //============================================================================
348  // Method Description:
351  void print() const
352  {
353  std::cout << *this;
354  }
355 
356  //============================================================================
357  // Method Description:
362  double roll() const noexcept
363  {
364  return std::atan2(2 * (s() * i() + j() * k()),
365  1 - 2 * (utils::sqr(i()) + utils::sqr(j())));
366  }
367 
368  //============================================================================
369  // Method Description:
377  NdArray<double> rotate(const NdArray<double>& inVector) const
378  {
379  if (inVector.size() != 3)
380  {
381  THROW_INVALID_ARGUMENT_ERROR("input inVector must be a cartesion vector of length = 3.");
382  }
383 
384  return *this * inVector;
385  }
386 
387  //============================================================================
388  // Method Description:
396  Vec3 rotate(const Vec3& inVec3) const
397  {
398  return *this * inVec3;
399  }
400 
401  //============================================================================
402  // Method Description:
408  double s() const noexcept
409  {
410  return components_[3];
411  }
412 
413  //============================================================================
414  // Method Description:
423  static Quaternion slerp(const Quaternion& inQuat1, const Quaternion& inQuat2, double inPercent)
424  {
425  if (inPercent < 0 || inPercent > 1)
426  {
427  THROW_INVALID_ARGUMENT_ERROR("input percent must be of the range [0, 1]");
428  }
429 
430  if (inPercent == 0)
431  {
432  return inQuat1;
433  }
434  if (inPercent == 1)
435  {
436  return inQuat2;
437  }
438 
439  double dotProduct = dot<double>(inQuat1.toNdArray(), inQuat2.toNdArray()).item();
440 
441  // If the dot product is negative, the quaternions
442  // have opposite handed-ness and slerp won't take
443  // the shorter path. Fix by reversing one quaternion.
444  Quaternion quat1Copy(inQuat1);
445  if (dotProduct < 0.0)
446  {
447  quat1Copy *= -1;
448  dotProduct *= -1;
449  }
450 
451  constexpr double DOT_THRESHOLD = 0.9995;
452  if (dotProduct > DOT_THRESHOLD) {
453  // If the inputs are too close for comfort, linearly interpolate
454  // and normalize the result.
455  return nlerp(inQuat1, inQuat2, inPercent);
456  }
457 
458  dotProduct = clip(dotProduct, -1.0, 1.0); // Robustness: Stay within domain of acos()
459  const double theta0 = std::acos(dotProduct); // angle between input vectors
460  const double theta = theta0 * inPercent; // angle between v0 and result
461 
462  const double s0 = std::cos(theta) - dotProduct * std::sin(theta) / std::sin(theta0); // == sin(theta_0 - theta) / sin(theta_0)
463  const double s1 = std::sin(theta) / std::sin(theta0);
464 
465  NdArray<double> interpQuat = (quat1Copy.toNdArray() * s0) + (inQuat2.toNdArray() * s1);
466  return Quaternion(interpQuat);
467  }
468 
469  //============================================================================
470  // Method Description:
478  Quaternion slerp(const Quaternion& inQuat2, double inPercent) const
479  {
480  return slerp(*this, inQuat2, inPercent);
481  }
482 
483  //============================================================================
484  // Method Description:
490  std::string str() const
491  {
492  std::string output = "[" + utils::num2str(i()) + ", " + utils::num2str(j()) +
493  ", " + utils::num2str(k()) + ", " + utils::num2str(s()) + "]\n";
494 
495  return output;
496  }
497 
498  //============================================================================
499  // Method Description:
506  {
507  NdArray<double> dcm(3);
508 
509  const double q0 = i();
510  const double q1 = j();
511  const double q2 = k();
512  const double q3 = s();
513 
514  const double q0sqr = utils::sqr(q0);
515  const double q1sqr = utils::sqr(q1);
516  const double q2sqr = utils::sqr(q2);
517  const double q3sqr = utils::sqr(q3);
518 
519  dcm(0, 0) = q3sqr + q0sqr - q1sqr - q2sqr;
520  dcm(0, 1) = 2 * (q0 * q1 - q3 * q2);
521  dcm(0, 2) = 2 * (q0 * q2 + q3 * q1);
522  dcm(1, 0) = 2 * (q0 * q1 + q3 * q2);
523  dcm(1, 1) = q3sqr + q1sqr - q0sqr - q2sqr;
524  dcm(1, 2) = 2 * (q1 * q2 - q3 * q0);
525  dcm(2, 0) = 2 * (q0 * q2 - q3 * q1);
526  dcm(2, 1) = 2 * (q1 * q2 + q3 * q0);
527  dcm(2, 2) = q3sqr + q2sqr - q0sqr - q1sqr;
528 
529  return dcm;
530  }
531 
532  //============================================================================
533  // Method Description:
540  {
541  auto componentsCopy = components_;
542  return NdArray<double>(componentsCopy);
543  }
544 
545  //============================================================================
546  // Method Description:
554  static Quaternion xRotation(double inAngle) noexcept
555  {
556  const Vec3 eulerAxis = { 1.0, 0.0, 0.0 };
557  return Quaternion(eulerAxis, inAngle);
558  }
559 
560  //============================================================================
561  // Method Description:
566  double yaw() const noexcept
567  {
568  return std::atan2(2 * (s() * k() + i() * j()),
569  1 - 2 * (utils::sqr(j()) + utils::sqr(k())));
570  }
571 
572  //============================================================================
573  // Method Description:
581  static Quaternion yRotation(double inAngle) noexcept
582  {
583  const Vec3 eulerAxis = { 0.0, 1.0, 0.0 };
584  return Quaternion(eulerAxis, inAngle);
585  }
586 
587  //============================================================================
588  // Method Description:
596  static Quaternion zRotation(double inAngle) noexcept
597  {
598  const Vec3 eulerAxis = { 0.0, 0.0, 1.0 };
599  return Quaternion(eulerAxis, inAngle);
600  }
601 
602  //============================================================================
603  // Method Description:
611  bool operator==(const Quaternion& inRhs) const noexcept
612  {
613  const auto comparitor = [](double value1, double value2) noexcept -> bool
614  {
615  return utils::essentiallyEqual(value1, value2);
616  };
617 
618  return stl_algorithms::equal(components_.begin(), components_.end(),
619  inRhs.components_.begin(), comparitor);
620  }
621 
622  //============================================================================
623  // Method Description:
631  bool operator!=(const Quaternion& inRhs) const noexcept
632  {
633  return !(*this == inRhs);
634  }
635 
636  //============================================================================
637  // Method Description:
645  Quaternion& operator+=(const Quaternion& inRhs) noexcept
646  {
647  stl_algorithms::transform(components_.begin(), components_.end(),
648  inRhs.components_.begin(), components_.begin(), std::plus<double>());
649 
650  normalize();
651 
652  return *this;
653  }
654 
655  //============================================================================
656  // Method Description:
664  Quaternion operator+(const Quaternion& inRhs) const noexcept
665  {
666  return Quaternion(*this) += inRhs;
667  }
668 
669  //============================================================================
670  // Method Description:
678  Quaternion& operator-=(const Quaternion& inRhs) noexcept
679  {
680  stl_algorithms::transform(components_.begin(), components_.end(),
681  inRhs.components_.begin(), components_.begin(), std::minus<double>());
682 
683  normalize();
684 
685  return *this;
686  }
687 
688  //============================================================================
689  // Method Description:
697  Quaternion operator-(const Quaternion& inRhs) const noexcept
698  {
699  return Quaternion(*this) -= inRhs;
700  }
701 
702  //============================================================================
703  // Method Description:
709  Quaternion operator-() const noexcept
710  {
711  return Quaternion(*this) *= -1.0;
712  }
713 
714  //============================================================================
715  // Method Description:
723  Quaternion& operator*=(const Quaternion& inRhs) noexcept
724  {
725  double q0 = inRhs.s() * i();
726  q0 += inRhs.i() * s();
727  q0 -= inRhs.j() * k();
728  q0 += inRhs.k() * j();
729 
730  double q1 = inRhs.s() * j();
731  q1 += inRhs.i() * k();
732  q1 += inRhs.j() * s();
733  q1 -= inRhs.k() * i();
734 
735  double q2 = inRhs.s() * k();
736  q2 -= inRhs.i() * j();
737  q2 += inRhs.j() * i();
738  q2 += inRhs.k() * s();
739 
740  double q3 = inRhs.s() * s();
741  q3 -= inRhs.i() * i();
742  q3 -= inRhs.j() * j();
743  q3 -= inRhs.k() * k();
744 
745  components_[0] = q0;
746  components_[1] = q1;
747  components_[2] = q2;
748  components_[3] = q3;
749 
750  normalize();
751 
752  return *this;
753  }
754 
755  //============================================================================
756  // Method Description:
765  Quaternion& operator*=(double inScalar) noexcept
766  {
767  stl_algorithms::for_each(components_.begin(), components_.end(),
768  [&inScalar](double& component)
769  {
770  component *= inScalar;
771  });
772 
773  normalize();
774 
775  return *this;
776  }
777 
778  //============================================================================
779  // Method Description:
787  Quaternion operator*(const Quaternion& inRhs) const noexcept
788  {
789  return Quaternion(*this) *= inRhs;
790  }
791 
792  //============================================================================
793  // Method Description:
802  Quaternion operator*(double inScalar) const noexcept
803  {
804  return Quaternion(*this) *= inScalar;
805  }
806 
807  //============================================================================
808  // Method Description:
817  {
818  if (inVec.size() != 3)
819  {
820  THROW_INVALID_ARGUMENT_ERROR("input vector must be a cartesion vector of length = 3.");
821  }
822 
823  const auto p = Quaternion(inVec[0], inVec[1], inVec[2], 0.0);
824  const auto pPrime = *this * p * this->inverse();
825 
826  NdArray<double> rotatedVec = { pPrime.i(), pPrime.j(), pPrime.k() };
827  rotatedVec *= norm(inVec).item();
828  return rotatedVec;
829  }
830 
831  //============================================================================
832  // Method Description:
840  Vec3 operator*(const Vec3& inVec3) const
841  {
842  return *this * inVec3.toNdArray();
843  }
844 
845  //============================================================================
846  // Method Description:
854  Quaternion& operator/=(const Quaternion& inRhs) noexcept
855  {
856  return *this *= inRhs.conjugate();
857  }
858 
859  //============================================================================
860  // Method Description:
868  Quaternion operator/(const Quaternion& inRhs) const noexcept
869  {
870  return Quaternion(*this) /= inRhs;
871  }
872 
873  //============================================================================
874  // Method Description:
882  friend std::ostream& operator<<(std::ostream& inOStream, const Quaternion& inQuat)
883  {
884  inOStream << inQuat.str();
885  return inOStream;
886  }
887 
888  private:
889  //====================================Attributes==============================
890  std::array<double, 4> components_{ {0.0, 0.0, 0.0, 1.0} };
891 
892  //============================================================================
893  // Method Description:
896  void normalize() noexcept
897  {
898  double sumOfSquares = 0.0;
899  stl_algorithms::for_each(components_.begin(), components_.end(),
900  [&sumOfSquares](double component) noexcept -> void
901  {
902  sumOfSquares += utils::sqr(component);
903  });
904 
905  const double norm = std::sqrt(sumOfSquares);
906  stl_algorithms::for_each(components_.begin(), components_.end(),
907  [&norm](double& component) noexcept -> void
908  {
909  component /= norm;
910  });
911  }
912 
913  //============================================================================
914  // Method Description:
921  void eulerToQuat(double roll, double pitch, double yaw) noexcept
922  {
923  const double halfPhi = roll / 2.0;
924  const double halfTheta = pitch / 2.0;
925  const double halfPsi = yaw / 2.0;
926 
927  components_[0] = std::sin(halfPhi) * std::cos(halfTheta) * std::cos(halfPsi);
928  components_[0] -= std::cos(halfPhi) * std::sin(halfTheta) * std::sin(halfPsi);
929 
930  components_[1] = std::cos(halfPhi) * std::sin(halfTheta) * std::cos(halfPsi);
931  components_[1] += std::sin(halfPhi) * std::cos(halfTheta) * std::sin(halfPsi);
932 
933  components_[2] = std::cos(halfPhi) * std::cos(halfTheta) * std::sin(halfPsi);
934  components_[2] -= std::sin(halfPhi) * std::sin(halfTheta) * std::cos(halfPsi);
935 
936  components_[3] = std::cos(halfPhi) * std::cos(halfTheta) * std::cos(halfPsi);
937  components_[3] += std::sin(halfPhi) * std::sin(halfTheta) * std::sin(halfPsi);
938  }
939 
940  //============================================================================
941  // Method Description:
946  void dcmToQuat(const NdArray<double>& dcm)
947  {
948  const Shape inShape = dcm.shape();
949  if (!(inShape.rows == 3 && inShape.cols == 3))
950  {
951  THROW_INVALID_ARGUMENT_ERROR("input direction cosine matrix must have shape = (3,3).");
952  }
953 
954  NdArray<double> checks(1, 4);
955  checks[0] = 1 + dcm(0, 0) + dcm(1, 1) + dcm(2, 2);
956  checks[1] = 1 + dcm(0, 0) - dcm(1, 1) - dcm(2, 2);
957  checks[2] = 1 - dcm(0, 0) + dcm(1, 1) - dcm(2, 2);
958  checks[3] = 1 - dcm(0, 0) - dcm(1, 1) + dcm(2, 2);
959 
960  const uint32 maxIdx = argmax(checks).item();
961 
962  switch (maxIdx)
963  {
964  case 0:
965  {
966  components_[3] = 0.5 * std::sqrt(1 + dcm(0, 0) + dcm(1, 1) + dcm(2, 2));
967  components_[0] = (dcm(2, 1) - dcm(1, 2)) / (4 * components_[3]);
968  components_[1] = (dcm(0, 2) - dcm(2, 0)) / (4 * components_[3]);
969  components_[2] = (dcm(1, 0) - dcm(0, 1)) / (4 * components_[3]);
970 
971  break;
972  }
973  case 1:
974  {
975  components_[0] = 0.5 * std::sqrt(1 + dcm(0, 0) - dcm(1, 1) - dcm(2, 2));
976  components_[1] = (dcm(1, 0) + dcm(0, 1)) / (4 * components_[0]);
977  components_[2] = (dcm(2, 0) + dcm(0, 2)) / (4 * components_[0]);
978  components_[3] = (dcm(2, 1) - dcm(1, 2)) / (4 * components_[0]);
979 
980  break;
981  }
982  case 2:
983  {
984  components_[1] = 0.5 * std::sqrt(1 - dcm(0, 0) + dcm(1, 1) - dcm(2, 2));
985  components_[0] = (dcm(1, 0) + dcm(0, 1)) / (4 * components_[1]);
986  components_[2] = (dcm(2, 1) + dcm(1, 2)) / (4 * components_[1]);
987  components_[3] = (dcm(0, 2) - dcm(2, 0)) / (4 * components_[1]);
988 
989  break;
990  }
991  case 3:
992  {
993  components_[2] = 0.5 * std::sqrt(1 - dcm(0, 0) - dcm(1, 1) + dcm(2, 2));
994  components_[0] = (dcm(2, 0) + dcm(0, 2)) / (4 * components_[2]);
995  components_[1] = (dcm(2, 1) + dcm(1, 2)) / (4 * components_[2]);
996  components_[3] = (dcm(1, 0) - dcm(0, 1)) / (4 * components_[2]);
997 
998  break;
999  }
1000  }
1001  }
1002  };
1003  } // namespace rotations
1004 } // namespace nc
#define THROW_INVALID_ARGUMENT_ERROR(msg)
Definition: Error.hpp:36
Holds 1D and 2D arrays, the main work horse of the NumCpp library.
Definition: NdArrayCore.hpp:72
NdArray< dtype > & put(int32 inIndex, value_type inValue)
Definition: NdArrayCore.hpp:3856
size_type size() const noexcept
Definition: NdArrayCore.hpp:4497
const_iterator cbegin() const noexcept
Definition: NdArrayCore.hpp:1270
NdArray< dtype > transpose() const
Definition: NdArrayCore.hpp:4841
const_iterator cend() const noexcept
Definition: NdArrayCore.hpp:1614
NdArray< dtype > & zeros() noexcept
Definition: NdArrayCore.hpp:4859
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:323
Vec3 normalize() const noexcept
Definition: Vec3.hpp:275
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:408
std::string str() const
Definition: Quaternion.hpp:490
NdArray< double > operator*(const NdArray< double > &inVec) const
Definition: Quaternion.hpp:816
NdArray< double > angularVelocity(const Quaternion &inQuat2, double inTime) const
Definition: Quaternion.hpp:205
Quaternion & operator*=(double inScalar) noexcept
Definition: Quaternion.hpp:765
double roll() const noexcept
Definition: Quaternion.hpp:362
NdArray< double > rotate(const NdArray< double > &inVector) const
Definition: Quaternion.hpp:377
static Quaternion xRotation(double inAngle) noexcept
Definition: Quaternion.hpp:554
static Quaternion nlerp(const Quaternion &inQuat1, const Quaternion &inQuat2, double inPercent)
Definition: Quaternion.hpp:293
Vec3 rotate(const Vec3 &inVec3) const
Definition: Quaternion.hpp:396
Quaternion(double inI, double inJ, double inK, double inS) noexcept
Definition: Quaternion.hpp:88
Quaternion operator-() const noexcept
Definition: Quaternion.hpp:709
NdArray< double > toNdArray() const
Definition: Quaternion.hpp:539
Quaternion operator+(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:664
double i() const noexcept
Definition: Quaternion.hpp:229
double yaw() const noexcept
Definition: Quaternion.hpp:566
double pitch() const noexcept
Definition: Quaternion.hpp:342
Quaternion & operator-=(const Quaternion &inRhs) noexcept
Definition: Quaternion.hpp:678
Quaternion slerp(const Quaternion &inQuat2, double inPercent) const
Definition: Quaternion.hpp:478
friend std::ostream & operator<<(std::ostream &inOStream, const Quaternion &inQuat)
Definition: Quaternion.hpp:882
static Quaternion slerp(const Quaternion &inQuat1, const Quaternion &inQuat2, double inPercent)
Definition: Quaternion.hpp:423
static NdArray< double > angularVelocity(const Quaternion &inQuat1, const Quaternion &inQuat2, double inTime)
Definition: Quaternion.hpp:170
void print() const
Definition: Quaternion.hpp:351
Quaternion(const NdArray< double > &inAxis, double inAngle)
Definition: Quaternion.hpp:155
bool operator==(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:611
Quaternion & operator/=(const Quaternion &inRhs) noexcept
Definition: Quaternion.hpp:854
Quaternion(double roll, double pitch, double yaw) noexcept
Definition: Quaternion.hpp:74
Vec3 operator*(const Vec3 &inVec3) const
Definition: Quaternion.hpp:840
Quaternion inverse() const noexcept
Definition: Quaternion.hpp:253
double k() const noexcept
Definition: Quaternion.hpp:278
NdArray< double > toDCM() const
Definition: Quaternion.hpp:505
Quaternion & operator*=(const Quaternion &inRhs) noexcept
Definition: Quaternion.hpp:723
static Quaternion zRotation(double inAngle) noexcept
Definition: Quaternion.hpp:596
Quaternion operator/(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:868
Quaternion nlerp(const Quaternion &inQuat2, double inPercent) const
Definition: Quaternion.hpp:331
static Quaternion yRotation(double inAngle) noexcept
Definition: Quaternion.hpp:581
Quaternion(const Vec3 &inAxis, double inAngle) noexcept
Definition: Quaternion.hpp:134
double j() const noexcept
Definition: Quaternion.hpp:266
Quaternion operator*(double inScalar) const noexcept
Definition: Quaternion.hpp:802
Quaternion operator-(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:697
Quaternion operator*(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:787
bool operator!=(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:631
Quaternion(const NdArray< double > &inArray)
Definition: Quaternion.hpp:102
Quaternion conjugate() const noexcept
Definition: Quaternion.hpp:217
static Quaternion identity() noexcept
Definition: Quaternion.hpp:241
Quaternion & operator+=(const Quaternion &inRhs) noexcept
Definition: Quaternion.hpp:645
OutputIt transform(InputIt first, InputIt last, OutputIt destination, UnaryOperation unaryFunction)
Definition: StlAlgorithms.hpp:702
void for_each(InputIt first, InputIt last, UnaryFunction f)
Definition: StlAlgorithms.hpp:213
bool equal(InputIt1 first1, InputIt1 last1, InputIt2 first2) noexcept
Definition: StlAlgorithms.hpp:135
OutputIt copy(InputIt first, InputIt last, OutputIt destination) noexcept
Definition: StlAlgorithms.hpp:95
std::string num2str(dtype inNumber)
Definition: num2str.hpp:46
bool essentiallyEqual(dtype inValue1, dtype inValue2) noexcept
Definition: essentiallyEqual.hpp:52
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:47
auto sin(dtype inValue) noexcept
Definition: sin.hpp:51
dtype clip(dtype inValue, dtype inMinValue, dtype inMaxValue)
Definition: clip.hpp:53
auto cos(dtype inValue) noexcept
Definition: cos.hpp:51
auto sqrt(dtype inValue) noexcept
Definition: sqrt.hpp:50
NdArray< double > norm(const NdArray< dtype > &inArray, Axis inAxis=Axis::NONE)
Definition: norm.hpp:52
std::uint32_t uint32
Definition: Types.hpp:40