NumCpp  2.7.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 
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:
169  static NdArray<double> angularVelocity(const Quaternion& inQuat1, const Quaternion& inQuat2, double inTime)
170  {
171  NdArray<double> q0 = inQuat1.toNdArray();
172  NdArray<double> q1 = inQuat2.toNdArray();
173 
174  NdArray<double> qDot = q1 - q0;
175  qDot /= inTime;
176 
177  NdArray<double> eyeTimesScalar(3);
178  eyeTimesScalar.zeros();
179  eyeTimesScalar(0, 0) = inQuat2.s();
180  eyeTimesScalar(1, 1) = inQuat2.s();
181  eyeTimesScalar(2, 2) = inQuat2.s();
182 
183  NdArray<double> epsilonHat = linalg::hat<double>(inQuat2.i(), inQuat2.j(), inQuat2.k());
184  NdArray<double> q(4, 3);
185  q.put(Slice(0, 3), Slice(0, 3), eyeTimesScalar + epsilonHat);
186  q(3, 0) = -inQuat2.i();
187  q(3, 1) = -inQuat2.j();
188  q(3, 2) = -inQuat2.k();
189 
190  NdArray<double> omega = q.transpose().dot(qDot.transpose());
191  return omega *= 2.0;
192  }
193 
194  //============================================================================
195  // Method Description:
203  NdArray<double> angularVelocity(const Quaternion& inQuat2, double inTime) const
204  {
205  return angularVelocity(*this, inQuat2, inTime);
206  }
207 
208  //============================================================================
209  // Method Description:
214  Quaternion conjugate() const noexcept
215  {
216  return {-i(), -j(), -k(), s()};
217  }
218 
219  //============================================================================
220  // Method Description:
225  double i() const noexcept
226  {
227  return components_[0];
228  }
229 
230  //============================================================================
231  // Method Description:
236  static Quaternion identity() noexcept
237  {
238  return {};
239  }
240 
241  //============================================================================
242  // Method Description:
247  Quaternion inverse() const noexcept
248  {
250  return conjugate();
251  }
252 
253  //============================================================================
254  // Method Description:
259  double j() const noexcept
260  {
261  return components_[1];
262  }
263 
264  //============================================================================
265  // Method Description:
270  double k() const noexcept
271  {
272  return components_[2];
273  }
274 
275  //============================================================================
276  // Method Description:
284  static Quaternion nlerp(const Quaternion& inQuat1, const Quaternion& inQuat2, double inPercent)
285  {
286  if (inPercent < 0.0 || inPercent > 1.0)
287  {
288  THROW_INVALID_ARGUMENT_ERROR("input percent must be of the range [0,1].");
289  }
290 
291  if (utils::essentiallyEqual(inPercent, 0.0))
292  {
293  return inQuat1;
294  }
295  if (utils::essentiallyEqual(inPercent, 1.0))
296  {
297  return inQuat2;
298  }
299 
300  const double oneMinus = 1.0 - inPercent;
301  std::array<double, 4> newComponents{};
302 
303  stl_algorithms::transform(inQuat1.components_.begin(), inQuat1.components_.end(),
304  inQuat2.components_.begin(), newComponents.begin(),
305  [inPercent, oneMinus](double component1, double component2) -> double
306  {
307  return oneMinus * component1 + inPercent * component2;
308  });
309 
310  return {newComponents[0], newComponents[1], newComponents[2], newComponents[3]};
311  }
312 
313  //============================================================================
314  // Method Description:
321  Quaternion nlerp(const Quaternion& inQuat2, double inPercent) const
322  {
323  return nlerp(*this, inQuat2, inPercent);
324  }
325 
326  //============================================================================
327  // Method Description:
332  double pitch() const noexcept
333  {
334  return std::asin(2 * (s() * j() - k() * i()));
335  }
336 
337  //============================================================================
338  // Method Description:
341  void print() const
342  {
343  std::cout << *this;
344  }
345 
346  //============================================================================
347  // Method Description:
352  double roll() const noexcept
353  {
354  return std::atan2(2 * (s() * i() + j() * k()),
355  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  // If the inputs are too close for comfort, linearly interpolate
438  // and normalize the result.
439  return nlerp(inQuat1, inQuat2, inPercent);
440  }
441 
442  dotProduct = clip(dotProduct, -1.0, 1.0); // Robustness: Stay within domain of acos()
443  const double theta0 = std::acos(dotProduct); // angle between input vectors
444  const double theta = theta0 * inPercent; // angle between v0 and result
445 
446  const double s0 = std::cos(theta) - dotProduct * std::sin(theta) / std::sin(theta0); // == sin(theta_0 - theta) / sin(theta_0)
447  const double s1 = std::sin(theta) / std::sin(theta0);
448 
449  NdArray<double> interpQuat = (quat1Copy.toNdArray() * s0) + (inQuat2.toNdArray() * s1);
450  return Quaternion(interpQuat);
451  }
452 
453  //============================================================================
454  // Method Description:
461  Quaternion slerp(const Quaternion& inQuat2, double inPercent) const
462  {
463  return slerp(*this, inQuat2, inPercent);
464  }
465 
466  //============================================================================
467  // Method Description:
472  std::string str() const
473  {
474  std::string output = "[" + utils::num2str(i()) + ", " + utils::num2str(j()) +
475  ", " + utils::num2str(k()) + ", " + utils::num2str(s()) + "]\n";
476 
477  return output;
478  }
479 
480  //============================================================================
481  // Method Description:
487  {
488  NdArray<double> dcm(3);
489 
490  const double q0 = i();
491  const double q1 = j();
492  const double q2 = k();
493  const double q3 = s();
494 
495  const double q0sqr = utils::sqr(q0);
496  const double q1sqr = utils::sqr(q1);
497  const double q2sqr = utils::sqr(q2);
498  const double q3sqr = utils::sqr(q3);
499 
500  dcm(0, 0) = q3sqr + q0sqr - q1sqr - q2sqr;
501  dcm(0, 1) = 2 * (q0 * q1 - q3 * q2);
502  dcm(0, 2) = 2 * (q0 * q2 + q3 * q1);
503  dcm(1, 0) = 2 * (q0 * q1 + q3 * q2);
504  dcm(1, 1) = q3sqr + q1sqr - q0sqr - q2sqr;
505  dcm(1, 2) = 2 * (q1 * q2 - q3 * q0);
506  dcm(2, 0) = 2 * (q0 * q2 - q3 * q1);
507  dcm(2, 1) = 2 * (q1 * q2 + q3 * q0);
508  dcm(2, 2) = q3sqr + q2sqr - q0sqr - q1sqr;
509 
510  return dcm;
511  }
512 
513  //============================================================================
514  // Method Description:
520  {
521  auto componentsCopy = components_;
522  return NdArray<double>(componentsCopy);
523  }
524 
525  //============================================================================
526  // Method Description:
532  static Quaternion xRotation(double inAngle) noexcept
533  {
534  const Vec3 eulerAxis = { 1.0, 0.0, 0.0 };
535  return Quaternion(eulerAxis, inAngle);
536  }
537 
538  //============================================================================
539  // Method Description:
544  double yaw() const noexcept
545  {
546  return std::atan2(2 * (s() * k() + i() * j()),
547  1 - 2 * (utils::sqr(j()) + utils::sqr(k())));
548  }
549 
550  //============================================================================
551  // Method Description:
557  static Quaternion yRotation(double inAngle) noexcept
558  {
559  const Vec3 eulerAxis = { 0.0, 1.0, 0.0 };
560  return Quaternion(eulerAxis, inAngle);
561  }
562 
563  //============================================================================
564  // Method Description:
570  static Quaternion zRotation(double inAngle) noexcept
571  {
572  const Vec3 eulerAxis = { 0.0, 0.0, 1.0 };
573  return Quaternion(eulerAxis, inAngle);
574  }
575 
576  //============================================================================
577  // Method Description:
583  bool operator==(const Quaternion& inRhs) const noexcept
584  {
585  const auto comparitor = [](double value1, double value2) noexcept -> bool
586  {
587  return utils::essentiallyEqual(value1, value2);
588  };
589 
590  return stl_algorithms::equal(components_.begin(), components_.end(),
591  inRhs.components_.begin(), comparitor);
592  }
593 
594  //============================================================================
595  // Method Description:
601  bool operator!=(const Quaternion& inRhs) const noexcept
602  {
603  return !(*this == inRhs);
604  }
605 
606  //============================================================================
607  // Method Description:
613  Quaternion& operator+=(const Quaternion& inRhs) noexcept
614  {
615  stl_algorithms::transform(components_.begin(), components_.end(),
616  inRhs.components_.begin(), components_.begin(), std::plus<double>());
617 
618  normalize();
619 
620  return *this;
621  }
622 
623  //============================================================================
624  // Method Description:
630  Quaternion operator+(const Quaternion& inRhs) const noexcept
631  {
632  return Quaternion(*this) += inRhs;
633  }
634 
635  //============================================================================
636  // Method Description:
642  Quaternion& operator-=(const Quaternion& inRhs) noexcept
643  {
644  stl_algorithms::transform(components_.begin(), components_.end(),
645  inRhs.components_.begin(), components_.begin(), std::minus<double>());
646 
647  normalize();
648 
649  return *this;
650  }
651 
652  //============================================================================
653  // Method Description:
659  Quaternion operator-(const Quaternion& inRhs) const noexcept
660  {
661  return Quaternion(*this) -= inRhs;
662  }
663 
664  //============================================================================
665  // Method Description:
670  Quaternion operator-() const noexcept
671  {
672  return Quaternion(*this) *= -1.0;
673  }
674 
675  //============================================================================
676  // Method Description:
682  Quaternion& operator*=(const Quaternion& inRhs) noexcept
683  {
684  double q0 = inRhs.s() * i();
685  q0 += inRhs.i() * s();
686  q0 -= inRhs.j() * k();
687  q0 += inRhs.k() * j();
688 
689  double q1 = inRhs.s() * j();
690  q1 += inRhs.i() * k();
691  q1 += inRhs.j() * s();
692  q1 -= inRhs.k() * i();
693 
694  double q2 = inRhs.s() * k();
695  q2 -= inRhs.i() * j();
696  q2 += inRhs.j() * i();
697  q2 += inRhs.k() * s();
698 
699  double q3 = inRhs.s() * s();
700  q3 -= inRhs.i() * i();
701  q3 -= inRhs.j() * j();
702  q3 -= inRhs.k() * k();
703 
704  components_[0] = q0;
705  components_[1] = q1;
706  components_[2] = q2;
707  components_[3] = q3;
708 
709  normalize();
710 
711  return *this;
712  }
713 
714  //============================================================================
715  // Method Description:
722  Quaternion& operator*=(double inScalar) noexcept
723  {
724  stl_algorithms::for_each(components_.begin(), components_.end(),
725  [inScalar](double& component)
726  {
727  component *= inScalar;
728  });
729 
730  normalize();
731 
732  return *this;
733  }
734 
735  //============================================================================
736  // Method Description:
742  Quaternion operator*(const Quaternion& inRhs) const noexcept
743  {
744  return Quaternion(*this) *= inRhs;
745  }
746 
747  //============================================================================
748  // Method Description:
755  Quaternion operator*(double inScalar) const noexcept
756  {
757  return Quaternion(*this) *= inScalar;
758  }
759 
760  //============================================================================
761  // Method Description:
768  {
769  if (inVec.size() != 3)
770  {
771  THROW_INVALID_ARGUMENT_ERROR("input vector must be a cartesion vector of length = 3.");
772  }
773 
774  const auto p = Quaternion(inVec[0], inVec[1], inVec[2], 0.0);
775  const auto pPrime = *this * p * this->inverse();
776 
777  NdArray<double> rotatedVec = { pPrime.i(), pPrime.j(), pPrime.k() };
778  rotatedVec *= norm(inVec).item();
779  return rotatedVec;
780  }
781 
782  //============================================================================
783  // Method Description:
789  Vec3 operator*(const Vec3& inVec3) const
790  {
791  return *this * inVec3.toNdArray();
792  }
793 
794  //============================================================================
795  // Method Description:
801  Quaternion& operator/=(const Quaternion& inRhs) noexcept
802  {
803  return *this *= inRhs.conjugate();
804  }
805 
806  //============================================================================
807  // Method Description:
813  Quaternion operator/(const Quaternion& inRhs) const noexcept
814  {
815  return Quaternion(*this) /= inRhs;
816  }
817 
818  //============================================================================
819  // Method Description:
826  friend std::ostream& operator<<(std::ostream& inOStream, const Quaternion& inQuat)
827  {
828  inOStream << inQuat.str();
829  return inOStream;
830  }
831 
832  private:
833  //====================================Attributes==============================
834  std::array<double, 4> components_{ {0.0, 0.0, 0.0, 1.0} };
835 
836  //============================================================================
837  // Method Description:
840  void normalize() noexcept
841  {
842  double sumOfSquares = 0.0;
843  std::for_each(components_.begin(), components_.end(),
844  [&sumOfSquares](double component) noexcept -> void
845  {
846  sumOfSquares += utils::sqr(component);
847  });
848 
849  const double norm = std::sqrt(sumOfSquares);
850  stl_algorithms::for_each(components_.begin(), components_.end(),
851  [norm](double& component) noexcept -> void
852  {
853  component /= norm;
854  });
855  }
856 
857  //============================================================================
858  // Method Description:
865  void eulerToQuat(double roll, double pitch, double yaw) noexcept
866  {
867  const double halfPhi = roll / 2.0;
868  const double halfTheta = pitch / 2.0;
869  const double halfPsi = yaw / 2.0;
870 
871  components_[0] = std::sin(halfPhi) * std::cos(halfTheta) * std::cos(halfPsi);
872  components_[0] -= std::cos(halfPhi) * std::sin(halfTheta) * std::sin(halfPsi);
873 
874  components_[1] = std::cos(halfPhi) * std::sin(halfTheta) * std::cos(halfPsi);
875  components_[1] += std::sin(halfPhi) * std::cos(halfTheta) * std::sin(halfPsi);
876 
877  components_[2] = std::cos(halfPhi) * std::cos(halfTheta) * std::sin(halfPsi);
878  components_[2] -= std::sin(halfPhi) * std::sin(halfTheta) * std::cos(halfPsi);
879 
880  components_[3] = std::cos(halfPhi) * std::cos(halfTheta) * std::cos(halfPsi);
881  components_[3] += std::sin(halfPhi) * std::sin(halfTheta) * std::sin(halfPsi);
882  }
883 
884  //============================================================================
885  // Method Description:
890  void dcmToQuat(const NdArray<double>& dcm)
891  {
892  const Shape inShape = dcm.shape();
893  if (!(inShape.rows == 3 && inShape.cols == 3))
894  {
895  THROW_INVALID_ARGUMENT_ERROR("input direction cosine matrix must have shape = (3,3).");
896  }
897 
898  NdArray<double> checks(1, 4);
899  checks[0] = 1 + dcm(0, 0) + dcm(1, 1) + dcm(2, 2);
900  checks[1] = 1 + dcm(0, 0) - dcm(1, 1) - dcm(2, 2);
901  checks[2] = 1 - dcm(0, 0) + dcm(1, 1) - dcm(2, 2);
902  checks[3] = 1 - dcm(0, 0) - dcm(1, 1) + dcm(2, 2);
903 
904  const uint32 maxIdx = argmax(checks).item();
905 
906  switch (maxIdx)
907  {
908  case 0:
909  {
910  components_[3] = 0.5 * std::sqrt(1 + dcm(0, 0) + dcm(1, 1) + dcm(2, 2));
911  components_[0] = (dcm(2, 1) - dcm(1, 2)) / (4 * components_[3]);
912  components_[1] = (dcm(0, 2) - dcm(2, 0)) / (4 * components_[3]);
913  components_[2] = (dcm(1, 0) - dcm(0, 1)) / (4 * components_[3]);
914 
915  break;
916  }
917  case 1:
918  {
919  components_[0] = 0.5 * std::sqrt(1 + dcm(0, 0) - dcm(1, 1) - dcm(2, 2));
920  components_[1] = (dcm(1, 0) + dcm(0, 1)) / (4 * components_[0]);
921  components_[2] = (dcm(2, 0) + dcm(0, 2)) / (4 * components_[0]);
922  components_[3] = (dcm(2, 1) - dcm(1, 2)) / (4 * components_[0]);
923 
924  break;
925  }
926  case 2:
927  {
928  components_[1] = 0.5 * std::sqrt(1 - dcm(0, 0) + dcm(1, 1) - dcm(2, 2));
929  components_[0] = (dcm(1, 0) + dcm(0, 1)) / (4 * components_[1]);
930  components_[2] = (dcm(2, 1) + dcm(1, 2)) / (4 * components_[1]);
931  components_[3] = (dcm(0, 2) - dcm(2, 0)) / (4 * components_[1]);
932 
933  break;
934  }
935  case 3:
936  {
937  components_[2] = 0.5 * std::sqrt(1 - dcm(0, 0) - dcm(1, 1) + dcm(2, 2));
938  components_[0] = (dcm(2, 0) + dcm(0, 2)) / (4 * components_[2]);
939  components_[1] = (dcm(2, 1) + dcm(1, 2)) / (4 * components_[2]);
940  components_[3] = (dcm(1, 0) - dcm(0, 1)) / (4 * components_[2]);
941 
942  break;
943  }
944  }
945  }
946  };
947  } // namespace rotations
948 } // namespace nc
#define THROW_INVALID_ARGUMENT_ERROR(msg)
Definition: Error.hpp:36
NdArray< dtype > & put(int32 inIndex, value_type inValue)
Definition: NdArrayCore.hpp:3666
size_type size() const noexcept
Definition: NdArrayCore.hpp:4296
const_iterator cbegin() const noexcept
Definition: NdArrayCore.hpp:1216
NdArray< dtype > transpose() const
Definition: NdArrayCore.hpp:4629
const_iterator cend() const noexcept
Definition: NdArrayCore.hpp:1524
value_type item() const
Definition: NdArrayCore.hpp:2931
NdArray< dtype > & zeros() noexcept
Definition: NdArrayCore.hpp:4647
NdArray< dtype > dot(const NdArray< dtype > &inOtherArray) const
Definition: NdArrayCore.hpp:2633
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:393
std::string str() const
Definition: Quaternion.hpp:472
NdArray< double > operator*(const NdArray< double > &inVec) const
Definition: Quaternion.hpp:767
NdArray< double > angularVelocity(const Quaternion &inQuat2, double inTime) const
Definition: Quaternion.hpp:203
Quaternion & operator*=(double inScalar) noexcept
Definition: Quaternion.hpp:722
double roll() const noexcept
Definition: Quaternion.hpp:352
NdArray< double > rotate(const NdArray< double > &inVector) const
Definition: Quaternion.hpp:365
static Quaternion xRotation(double inAngle) noexcept
Definition: Quaternion.hpp:532
static Quaternion nlerp(const Quaternion &inQuat1, const Quaternion &inQuat2, double inPercent)
Definition: Quaternion.hpp:284
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:670
NdArray< double > toNdArray() const
Definition: Quaternion.hpp:519
Quaternion operator+(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:630
double i() const noexcept
Definition: Quaternion.hpp:225
double yaw() const noexcept
Definition: Quaternion.hpp:544
double pitch() const noexcept
Definition: Quaternion.hpp:332
Quaternion & operator-=(const Quaternion &inRhs) noexcept
Definition: Quaternion.hpp:642
Quaternion slerp(const Quaternion &inQuat2, double inPercent) const
Definition: Quaternion.hpp:461
friend std::ostream & operator<<(std::ostream &inOStream, const Quaternion &inQuat)
Definition: Quaternion.hpp:826
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:169
void print() const
Definition: Quaternion.hpp:341
Quaternion(const NdArray< double > &inAxis, double inAngle)
Definition: Quaternion.hpp:155
bool operator==(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:583
Quaternion & operator/=(const Quaternion &inRhs) noexcept
Definition: Quaternion.hpp:801
Quaternion(double roll, double pitch, double yaw) noexcept
Definition: Quaternion.hpp:74
Vec3 operator*(const Vec3 &inVec3) const
Definition: Quaternion.hpp:789
Quaternion inverse() const noexcept
Definition: Quaternion.hpp:247
double k() const noexcept
Definition: Quaternion.hpp:270
NdArray< double > toDCM() const
Definition: Quaternion.hpp:486
Quaternion & operator*=(const Quaternion &inRhs) noexcept
Definition: Quaternion.hpp:682
static Quaternion zRotation(double inAngle) noexcept
Definition: Quaternion.hpp:570
Quaternion operator/(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:813
Quaternion nlerp(const Quaternion &inQuat2, double inPercent) const
Definition: Quaternion.hpp:321
static Quaternion yRotation(double inAngle) noexcept
Definition: Quaternion.hpp:557
Quaternion(const Vec3 &inAxis, double inAngle) noexcept
Definition: Quaternion.hpp:134
double j() const noexcept
Definition: Quaternion.hpp:259
Quaternion operator*(double inScalar) const noexcept
Definition: Quaternion.hpp:755
Quaternion operator-(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:659
Quaternion operator*(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:742
bool operator!=(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:601
Quaternion(const NdArray< double > &inArray)
Definition: Quaternion.hpp:102
Quaternion conjugate() const noexcept
Definition: Quaternion.hpp:214
static Quaternion identity() noexcept
Definition: Quaternion.hpp:236
Quaternion & operator+=(const Quaternion &inRhs) noexcept
Definition: Quaternion.hpp:613
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:46
auto sin(dtype inValue) noexcept
Definition: sin.hpp:49
dtype clip(dtype inValue, dtype inMinValue, dtype inMaxValue)
Definition: clip.hpp:52
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