NumCpp  2.9.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:
100  Quaternion(const std::array<double, 4>& components) noexcept :
101  components_{ components }
102  {
103  normalize();
104  }
105 
106  //============================================================================
107  // Method Description:
114  Quaternion(const NdArray<double>& inArray) :
115  components_{ 0., 0., 0., 0. }
116  {
117  if (inArray.size() == 3)
118  {
119  // euler angles
120  eulerToQuat(inArray[0], inArray[1], inArray[2]);
121  }
122  else if (inArray.size() == 4)
123  {
124  // quaternion i, j, k, s components
125  stl_algorithms::copy(inArray.cbegin(), inArray.cend(), components_.begin());
126  normalize();
127  }
128  else if (inArray.size() == 9)
129  {
130  // direction cosine matrix
131  dcmToQuat(inArray);
132  }
133  else
134  {
135  THROW_INVALID_ARGUMENT_ERROR("input array is not a valid size.");
136  }
137  }
138 
139  //============================================================================
140  // Method Description:
146  Quaternion(const Vec3& inAxis, double inAngle) noexcept
147  {
148  // normalize the input vector
149  Vec3 normAxis = inAxis.normalize();
150 
151  const double halfAngle = inAngle / 2.;
152  const double sinHalfAngle = std::sin(halfAngle);
153 
154  components_[0] = normAxis.x * sinHalfAngle;
155  components_[1] = normAxis.y * sinHalfAngle;
156  components_[2] = normAxis.z * sinHalfAngle;
157  components_[3] = std::cos(halfAngle);
158  }
159 
160  //============================================================================
161  // Method Description:
167  Quaternion(const NdArray<double>& inAxis, double inAngle) :
168  Quaternion(Vec3(inAxis), inAngle)
169  {
170  }
171 
172  //============================================================================
173  // Method Description:
178  double angleOfRotation() const noexcept
179  {
180  return 2. * std::acos(s());
181  }
182 
183  //============================================================================
184  // Method Description:
193  static NdArray<double> angularVelocity(const Quaternion& inQuat1, const Quaternion& inQuat2, double inTime)
194  {
195  NdArray<double> q0 = inQuat1.toNdArray();
196  NdArray<double> q1 = inQuat2.toNdArray();
197 
198  NdArray<double> qDot = q1 - q0;
199  qDot /= inTime;
200 
201  NdArray<double> eyeTimesScalar(3);
202  eyeTimesScalar.zeros();
203  eyeTimesScalar(0, 0) = inQuat2.s();
204  eyeTimesScalar(1, 1) = inQuat2.s();
205  eyeTimesScalar(2, 2) = inQuat2.s();
206 
207  NdArray<double> epsilonHat = linalg::hat<double>(inQuat2.i(), inQuat2.j(), inQuat2.k());
208  NdArray<double> q(4, 3);
209  q.put(Slice(0, 3), Slice(0, 3), eyeTimesScalar + epsilonHat);
210  q(3, 0) = -inQuat2.i();
211  q(3, 1) = -inQuat2.j();
212  q(3, 2) = -inQuat2.k();
213 
214  NdArray<double> omega = q.transpose().dot(qDot.transpose());
215  return omega *= 2.;
216  }
217 
218  //============================================================================
219  // Method Description:
227  NdArray<double> angularVelocity(const Quaternion& inQuat2, double inTime) const
228  {
229  return angularVelocity(*this, inQuat2, inTime);
230  }
231 
232  //============================================================================
233  // Method Description:
238  Vec3 axisOfRotation() const noexcept
239  {
240  const auto halfAngle = angleOfRotation() / 2.;
241  const auto sinHalfAngle = std::sin(halfAngle);
242  auto axis = Vec3(i() / sinHalfAngle, j() / sinHalfAngle, k() / sinHalfAngle);
243 
244  // shouldn't be necessary, but let's be pedantic
245  axis.normalize();
246 
247  return axis;
248  }
249 
250  //============================================================================
251  // Method Description:
256  Quaternion conjugate() const noexcept
257  {
258  return { -i(), -j(), -k(), s() };
259  }
260 
261  //============================================================================
262  // Method Description:
267  double i() const noexcept
268  {
269  return components_[0];
270  }
271 
272  //============================================================================
273  // Method Description:
278  static Quaternion identity() noexcept
279  {
280  return {};
281  }
282 
283  //============================================================================
284  // Method Description:
289  Quaternion inverse() const noexcept
290  {
292  return conjugate();
293  }
294 
295  //============================================================================
296  // Method Description:
301  double j() const noexcept
302  {
303  return components_[1];
304  }
305 
306  //============================================================================
307  // Method Description:
312  double k() const noexcept
313  {
314  return components_[2];
315  }
316 
317  //============================================================================
318  // Method Description:
326  static Quaternion nlerp(const Quaternion& inQuat1, const Quaternion& inQuat2, double inPercent)
327  {
328  if (inPercent < 0. || inPercent > 1.)
329  {
330  THROW_INVALID_ARGUMENT_ERROR("input percent must be of the range [0,1].");
331  }
332 
333  if (utils::essentiallyEqual(inPercent, 0.))
334  {
335  return inQuat1;
336  }
337  if (utils::essentiallyEqual(inPercent, 1.))
338  {
339  return inQuat2;
340  }
341 
342  const double oneMinus = 1. - inPercent;
343  std::array<double, 4> newComponents{};
344 
345  stl_algorithms::transform(inQuat1.components_.begin(),
346  inQuat1.components_.end(),
347  inQuat2.components_.begin(),
348  newComponents.begin(),
349  [inPercent, oneMinus](double component1, double component2) -> double
350  { return oneMinus * component1 + inPercent * component2; });
351 
352  return { newComponents };
353  }
354 
355  //============================================================================
356  // Method Description:
363  Quaternion nlerp(const Quaternion& inQuat2, double inPercent) const
364  {
365  return nlerp(*this, inQuat2, inPercent);
366  }
367 
368  //============================================================================
369  // Method Description:
374  double pitch() const noexcept
375  {
376  return std::asin(2 * (s() * j() - k() * i()));
377  }
378 
379  //============================================================================
380  // Method Description:
386  static Quaternion pitchRotation(double inAngle) noexcept
387  {
388  return Quaternion(0., inAngle, 0.);
389  }
390 
391  //============================================================================
392  // Method Description:
395  void print() const
396  {
397  std::cout << *this;
398  }
399 
400  //============================================================================
401  // Method Description:
406  double roll() const noexcept
407  {
408  return std::atan2(2. * (s() * i() + j() * k()), 1. - 2. * (utils::sqr(i()) + utils::sqr(j())));
409  }
410 
411  //============================================================================
412  // Method Description:
418  static Quaternion rollRotation(double inAngle) noexcept
419  {
420  return Quaternion(inAngle, 0., 0.);
421  }
422 
423  //============================================================================
424  // Method Description:
430  NdArray<double> rotate(const NdArray<double>& inVector) const
431  {
432  if (inVector.size() != 3)
433  {
434  THROW_INVALID_ARGUMENT_ERROR("input inVector must be a cartesion vector of length = 3.");
435  }
436 
437  return *this * inVector;
438  }
439 
440  //============================================================================
441  // Method Description:
447  Vec3 rotate(const Vec3& inVec3) const
448  {
449  return *this * inVec3;
450  }
451 
452  //============================================================================
453  // Method Description:
458  double s() const noexcept
459  {
460  return components_[3];
461  }
462 
463  //============================================================================
464  // Method Description:
472  static Quaternion slerp(const Quaternion& inQuat1, const Quaternion& inQuat2, double inPercent)
473  {
474  if (inPercent < 0 || inPercent > 1)
475  {
476  THROW_INVALID_ARGUMENT_ERROR("input percent must be of the range [0, 1]");
477  }
478 
479  if (inPercent == 0)
480  {
481  return inQuat1;
482  }
483  if (inPercent == 1)
484  {
485  return inQuat2;
486  }
487 
488  double dotProduct = dot<double>(inQuat1.toNdArray(), inQuat2.toNdArray()).item();
489 
490  // If the dot product is negative, the quaternions
491  // have opposite handed-ness and slerp won't take
492  // the shorter path. Fix by reversing one quaternion.
493  Quaternion quat1Copy(inQuat1);
494  if (dotProduct < 0.)
495  {
496  quat1Copy *= -1.;
497  dotProduct *= -1.;
498  }
499 
500  constexpr double DOT_THRESHOLD = 0.9995;
501  if (dotProduct > DOT_THRESHOLD)
502  {
503  // If the inputs are too close for comfort, linearly interpolate
504  // and normalize the result.
505  return nlerp(inQuat1, inQuat2, inPercent);
506  }
507 
508  dotProduct = clip(dotProduct, -1., 1.); // Robustness: Stay within domain of acos()
509  const double theta0 = std::acos(dotProduct); // angle between input vectors
510  const double theta = theta0 * inPercent; // angle between v0 and result
511 
512  const double s0 = std::cos(theta) - dotProduct * std::sin(theta) /
513  std::sin(theta0); // == sin(theta_0 - theta) / sin(theta_0)
514  const double s1 = std::sin(theta) / std::sin(theta0);
515 
516  NdArray<double> interpQuat = (quat1Copy.toNdArray() * s0) + (inQuat2.toNdArray() * s1);
517  return Quaternion(interpQuat);
518  }
519 
520  //============================================================================
521  // Method Description:
528  Quaternion slerp(const Quaternion& inQuat2, double inPercent) const
529  {
530  return slerp(*this, inQuat2, inPercent);
531  }
532 
533  //============================================================================
534  // Method Description:
539  std::string str() const
540  {
541  std::string output = "[" + utils::num2str(i()) + ", " + utils::num2str(j()) + ", " +
542  utils::num2str(k()) + ", " + utils::num2str(s()) + "]\n";
543 
544  return output;
545  }
546 
547  //============================================================================
548  // Method Description:
554  {
555  NdArray<double> dcm(3);
556 
557  const double q0 = i();
558  const double q1 = j();
559  const double q2 = k();
560  const double q3 = s();
561 
562  const double q0sqr = utils::sqr(q0);
563  const double q1sqr = utils::sqr(q1);
564  const double q2sqr = utils::sqr(q2);
565  const double q3sqr = utils::sqr(q3);
566 
567  dcm(0, 0) = q3sqr + q0sqr - q1sqr - q2sqr;
568  dcm(0, 1) = 2. * (q0 * q1 - q3 * q2);
569  dcm(0, 2) = 2. * (q0 * q2 + q3 * q1);
570  dcm(1, 0) = 2. * (q0 * q1 + q3 * q2);
571  dcm(1, 1) = q3sqr + q1sqr - q0sqr - q2sqr;
572  dcm(1, 2) = 2. * (q1 * q2 - q3 * q0);
573  dcm(2, 0) = 2. * (q0 * q2 - q3 * q1);
574  dcm(2, 1) = 2. * (q1 * q2 + q3 * q0);
575  dcm(2, 2) = q3sqr + q2sqr - q0sqr - q1sqr;
576 
577  return dcm;
578  }
579 
580  //============================================================================
581  // Method Description:
587  {
588  auto componentsCopy = components_;
589  return NdArray<double>(componentsCopy);
590  }
591 
592  //============================================================================
593  // Method Description:
599  static Quaternion xRotation(double inAngle) noexcept
600  {
601  const Vec3 eulerAxis = { 1., 0., 0. };
602  return Quaternion(eulerAxis, inAngle);
603  }
604 
605  //============================================================================
606  // Method Description:
611  double yaw() const noexcept
612  {
613  return std::atan2(2. * (s() * k() + i() * j()), 1. - 2. * (utils::sqr(j()) + utils::sqr(k())));
614  }
615 
616  //============================================================================
617  // Method Description:
623  static Quaternion yawRotation(double inAngle) noexcept
624  {
625  return Quaternion(0., 0., inAngle);
626  }
627 
628  //============================================================================
629  // Method Description:
635  static Quaternion yRotation(double inAngle) noexcept
636  {
637  const Vec3 eulerAxis = { 0., 1., 0. };
638  return Quaternion(eulerAxis, inAngle);
639  }
640 
641  //============================================================================
642  // Method Description:
648  static Quaternion zRotation(double inAngle) noexcept
649  {
650  const Vec3 eulerAxis = { 0., 0., 1. };
651  return Quaternion(eulerAxis, inAngle);
652  }
653 
654  //============================================================================
655  // Method Description:
661  bool operator==(const Quaternion& inRhs) const noexcept
662  {
663  const auto comparitor = [](double value1, double value2) noexcept -> bool
664  { return utils::essentiallyEqual(value1, value2); };
665 
666  return stl_algorithms::equal(components_.begin(),
667  components_.end(),
668  inRhs.components_.begin(),
669  comparitor);
670  }
671 
672  //============================================================================
673  // Method Description:
679  bool operator!=(const Quaternion& inRhs) const noexcept
680  {
681  return !(*this == inRhs);
682  }
683 
684  //============================================================================
685  // Method Description:
691  Quaternion& operator+=(const Quaternion& inRhs) noexcept
692  {
693  stl_algorithms::transform(components_.begin(),
694  components_.end(),
695  inRhs.components_.begin(),
696  components_.begin(),
697  std::plus<double>());
698 
699  normalize();
700 
701  return *this;
702  }
703 
704  //============================================================================
705  // Method Description:
711  Quaternion operator+(const Quaternion& inRhs) const noexcept
712  {
713  return Quaternion(*this) += inRhs;
714  }
715 
716  //============================================================================
717  // Method Description:
723  Quaternion& operator-=(const Quaternion& inRhs) noexcept
724  {
725  stl_algorithms::transform(components_.begin(),
726  components_.end(),
727  inRhs.components_.begin(),
728  components_.begin(),
729  std::minus<double>());
730 
731  normalize();
732 
733  return *this;
734  }
735 
736  //============================================================================
737  // Method Description:
743  Quaternion operator-(const Quaternion& inRhs) const noexcept
744  {
745  return Quaternion(*this) -= inRhs;
746  }
747 
748  //============================================================================
749  // Method Description:
754  Quaternion operator-() const noexcept
755  {
756  return Quaternion(*this) *= -1.;
757  }
758 
759  //============================================================================
760  // Method Description:
766  Quaternion& operator*=(const Quaternion& inRhs) noexcept
767  {
768  double q0 = inRhs.s() * i();
769  q0 += inRhs.i() * s();
770  q0 -= inRhs.j() * k();
771  q0 += inRhs.k() * j();
772 
773  double q1 = inRhs.s() * j();
774  q1 += inRhs.i() * k();
775  q1 += inRhs.j() * s();
776  q1 -= inRhs.k() * i();
777 
778  double q2 = inRhs.s() * k();
779  q2 -= inRhs.i() * j();
780  q2 += inRhs.j() * i();
781  q2 += inRhs.k() * s();
782 
783  double q3 = inRhs.s() * s();
784  q3 -= inRhs.i() * i();
785  q3 -= inRhs.j() * j();
786  q3 -= inRhs.k() * k();
787 
788  components_[0] = q0;
789  components_[1] = q1;
790  components_[2] = q2;
791  components_[3] = q3;
792 
793  normalize();
794 
795  return *this;
796  }
797 
798  //============================================================================
799  // Method Description:
806  Quaternion& operator*=(double inScalar) noexcept
807  {
808  stl_algorithms::for_each(components_.begin(),
809  components_.end(),
810  [inScalar](double& component) { component *= inScalar; });
811 
812  normalize();
813 
814  return *this;
815  }
816 
817  //============================================================================
818  // Method Description:
824  Quaternion operator*(const Quaternion& inRhs) const noexcept
825  {
826  return Quaternion(*this) *= inRhs;
827  }
828 
829  //============================================================================
830  // Method Description:
837  Quaternion operator*(double inScalar) const noexcept
838  {
839  return Quaternion(*this) *= inScalar;
840  }
841 
842  //============================================================================
843  // Method Description:
850  {
851  if (inVec.size() != 3)
852  {
853  THROW_INVALID_ARGUMENT_ERROR("input vector must be a cartesion vector of length = 3.");
854  }
855 
856  const auto p = Quaternion(inVec[0], inVec[1], inVec[2], 0.);
857  const auto pPrime = *this * p * this->inverse();
858 
859  NdArray<double> rotatedVec = { pPrime.i(), pPrime.j(), pPrime.k() };
860  rotatedVec *= norm(inVec).item();
861  return rotatedVec;
862  }
863 
864  //============================================================================
865  // Method Description:
871  Vec3 operator*(const Vec3& inVec3) const
872  {
873  return *this * inVec3.toNdArray();
874  }
875 
876  //============================================================================
877  // Method Description:
883  Quaternion& operator/=(const Quaternion& inRhs) noexcept
884  {
885  return *this *= inRhs.conjugate();
886  }
887 
888  //============================================================================
889  // Method Description:
895  Quaternion operator/(const Quaternion& inRhs) const noexcept
896  {
897  return Quaternion(*this) /= inRhs;
898  }
899 
900  //============================================================================
901  // Method Description:
908  friend std::ostream& operator<<(std::ostream& inOStream, const Quaternion& inQuat)
909  {
910  inOStream << inQuat.str();
911  return inOStream;
912  }
913 
914  private:
915  //====================================Attributes==============================
916  std::array<double, 4> components_{ { 0., 0., 0., 1. } };
917 
918  //============================================================================
919  // Method Description:
922  void normalize() noexcept
923  {
924  double sumOfSquares = 0.;
925  std::for_each(components_.begin(),
926  components_.end(),
927  [&sumOfSquares](double component) noexcept -> void
928  { sumOfSquares += utils::sqr(component); });
929 
930  const double norm = std::sqrt(sumOfSquares);
931  stl_algorithms::for_each(components_.begin(),
932  components_.end(),
933  [norm](double& component) noexcept -> void { component /= norm; });
934  }
935 
936  //============================================================================
937  // Method Description:
944  void eulerToQuat(double roll, double pitch, double yaw) noexcept
945  {
946  const auto halfPhi = roll / 2.;
947  const auto halfTheta = pitch / 2.;
948  const auto halfPsi = yaw / 2.;
949 
950  const auto sinHalfPhi = std::sin(halfPhi);
951  const auto cosHalfPhi = std::cos(halfPhi);
952 
953  const auto sinHalfTheta = std::sin(halfTheta);
954  const auto cosHalfTheta = std::cos(halfTheta);
955 
956  const auto sinHalfPsi = std::sin(halfPsi);
957  const auto cosHalfPsi = std::cos(halfPsi);
958 
959  components_[0] = sinHalfPhi * cosHalfTheta * cosHalfPsi;
960  components_[0] -= cosHalfPhi * sinHalfTheta * sinHalfPsi;
961 
962  components_[1] = cosHalfPhi * sinHalfTheta * cosHalfPsi;
963  components_[1] += sinHalfPhi * cosHalfTheta * sinHalfPsi;
964 
965  components_[2] = cosHalfPhi * cosHalfTheta * sinHalfPsi;
966  components_[2] -= sinHalfPhi * sinHalfTheta * cosHalfPsi;
967 
968  components_[3] = cosHalfPhi * cosHalfTheta * cosHalfPsi;
969  components_[3] += sinHalfPhi * sinHalfTheta * sinHalfPsi;
970  }
971 
972  //============================================================================
973  // Method Description:
978  void dcmToQuat(const NdArray<double>& dcm)
979  {
980  const Shape inShape = dcm.shape();
981  if (!(inShape.rows == 3 && inShape.cols == 3))
982  {
983  THROW_INVALID_ARGUMENT_ERROR("input direction cosine matrix must have shape = (3,3).");
984  }
985 
986  NdArray<double> checks(1, 4);
987  checks[0] = 1 + dcm(0, 0) + dcm(1, 1) + dcm(2, 2);
988  checks[1] = 1 + dcm(0, 0) - dcm(1, 1) - dcm(2, 2);
989  checks[2] = 1 - dcm(0, 0) + dcm(1, 1) - dcm(2, 2);
990  checks[3] = 1 - dcm(0, 0) - dcm(1, 1) + dcm(2, 2);
991 
992  const uint32 maxIdx = argmax(checks).item();
993 
994  switch (maxIdx)
995  {
996  case 0:
997  {
998  components_[3] = 0.5 * std::sqrt(1 + dcm(0, 0) + dcm(1, 1) + dcm(2, 2));
999  components_[0] = (dcm(2, 1) - dcm(1, 2)) / (4 * components_[3]);
1000  components_[1] = (dcm(0, 2) - dcm(2, 0)) / (4 * components_[3]);
1001  components_[2] = (dcm(1, 0) - dcm(0, 1)) / (4 * components_[3]);
1002 
1003  break;
1004  }
1005  case 1:
1006  {
1007  components_[0] = 0.5 * std::sqrt(1 + dcm(0, 0) - dcm(1, 1) - dcm(2, 2));
1008  components_[1] = (dcm(1, 0) + dcm(0, 1)) / (4 * components_[0]);
1009  components_[2] = (dcm(2, 0) + dcm(0, 2)) / (4 * components_[0]);
1010  components_[3] = (dcm(2, 1) - dcm(1, 2)) / (4 * components_[0]);
1011 
1012  break;
1013  }
1014  case 2:
1015  {
1016  components_[1] = 0.5 * std::sqrt(1 - dcm(0, 0) + dcm(1, 1) - dcm(2, 2));
1017  components_[0] = (dcm(1, 0) + dcm(0, 1)) / (4 * components_[1]);
1018  components_[2] = (dcm(2, 1) + dcm(1, 2)) / (4 * components_[1]);
1019  components_[3] = (dcm(0, 2) - dcm(2, 0)) / (4 * components_[1]);
1020 
1021  break;
1022  }
1023  case 3:
1024  {
1025  components_[2] = 0.5 * std::sqrt(1 - dcm(0, 0) - dcm(1, 1) + dcm(2, 2));
1026  components_[0] = (dcm(2, 0) + dcm(0, 2)) / (4 * components_[2]);
1027  components_[1] = (dcm(2, 1) + dcm(1, 2)) / (4 * components_[2]);
1028  components_[3] = (dcm(1, 0) - dcm(0, 1)) / (4 * components_[2]);
1029 
1030  break;
1031  }
1032  }
1033  }
1034  };
1035  } // namespace rotations
1036 } // namespace nc
#define THROW_INVALID_ARGUMENT_ERROR(msg)
Definition: Error.hpp:36
NdArray< dtype > & put(int32 inIndex, value_type inValue)
Definition: NdArrayCore.hpp:3479
size_type size() const noexcept
Definition: NdArrayCore.hpp:4105
const_iterator cbegin() const noexcept
Definition: NdArrayCore.hpp:1221
NdArray< dtype > transpose() const
Definition: NdArrayCore.hpp:4457
const_iterator cend() const noexcept
Definition: NdArrayCore.hpp:1529
value_type item() const
Definition: NdArrayCore.hpp:2809
NdArray< dtype > & zeros() noexcept
Definition: NdArrayCore.hpp:4475
NdArray< dtype > dot(const NdArray< dtype > &inOtherArray) const
Definition: NdArrayCore.hpp:2520
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:458
std::string str() const
Definition: Quaternion.hpp:539
NdArray< double > operator*(const NdArray< double > &inVec) const
Definition: Quaternion.hpp:849
NdArray< double > angularVelocity(const Quaternion &inQuat2, double inTime) const
Definition: Quaternion.hpp:227
Quaternion & operator*=(double inScalar) noexcept
Definition: Quaternion.hpp:806
double angleOfRotation() const noexcept
Definition: Quaternion.hpp:178
double roll() const noexcept
Definition: Quaternion.hpp:406
NdArray< double > rotate(const NdArray< double > &inVector) const
Definition: Quaternion.hpp:430
static Quaternion xRotation(double inAngle) noexcept
Definition: Quaternion.hpp:599
static Quaternion nlerp(const Quaternion &inQuat1, const Quaternion &inQuat2, double inPercent)
Definition: Quaternion.hpp:326
static Quaternion rollRotation(double inAngle) noexcept
Definition: Quaternion.hpp:418
Vec3 rotate(const Vec3 &inVec3) const
Definition: Quaternion.hpp:447
Quaternion(double inI, double inJ, double inK, double inS) noexcept
Definition: Quaternion.hpp:88
Quaternion operator-() const noexcept
Definition: Quaternion.hpp:754
NdArray< double > toNdArray() const
Definition: Quaternion.hpp:586
Quaternion(const std::array< double, 4 > &components) noexcept
Definition: Quaternion.hpp:100
Quaternion operator+(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:711
double i() const noexcept
Definition: Quaternion.hpp:267
double yaw() const noexcept
Definition: Quaternion.hpp:611
double pitch() const noexcept
Definition: Quaternion.hpp:374
Quaternion & operator-=(const Quaternion &inRhs) noexcept
Definition: Quaternion.hpp:723
Quaternion slerp(const Quaternion &inQuat2, double inPercent) const
Definition: Quaternion.hpp:528
friend std::ostream & operator<<(std::ostream &inOStream, const Quaternion &inQuat)
Definition: Quaternion.hpp:908
static Quaternion slerp(const Quaternion &inQuat1, const Quaternion &inQuat2, double inPercent)
Definition: Quaternion.hpp:472
static Quaternion yawRotation(double inAngle) noexcept
Definition: Quaternion.hpp:623
static NdArray< double > angularVelocity(const Quaternion &inQuat1, const Quaternion &inQuat2, double inTime)
Definition: Quaternion.hpp:193
void print() const
Definition: Quaternion.hpp:395
Quaternion(const NdArray< double > &inAxis, double inAngle)
Definition: Quaternion.hpp:167
bool operator==(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:661
Quaternion & operator/=(const Quaternion &inRhs) noexcept
Definition: Quaternion.hpp:883
Quaternion(double roll, double pitch, double yaw) noexcept
Definition: Quaternion.hpp:74
Vec3 operator*(const Vec3 &inVec3) const
Definition: Quaternion.hpp:871
Quaternion inverse() const noexcept
Definition: Quaternion.hpp:289
double k() const noexcept
Definition: Quaternion.hpp:312
NdArray< double > toDCM() const
Definition: Quaternion.hpp:553
Quaternion & operator*=(const Quaternion &inRhs) noexcept
Definition: Quaternion.hpp:766
static Quaternion zRotation(double inAngle) noexcept
Definition: Quaternion.hpp:648
Quaternion operator/(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:895
Quaternion nlerp(const Quaternion &inQuat2, double inPercent) const
Definition: Quaternion.hpp:363
static Quaternion yRotation(double inAngle) noexcept
Definition: Quaternion.hpp:635
Quaternion(const Vec3 &inAxis, double inAngle) noexcept
Definition: Quaternion.hpp:146
double j() const noexcept
Definition: Quaternion.hpp:301
Quaternion operator*(double inScalar) const noexcept
Definition: Quaternion.hpp:837
Quaternion operator-(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:743
Quaternion operator*(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:824
bool operator!=(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:679
Quaternion(const NdArray< double > &inArray)
Definition: Quaternion.hpp:114
Quaternion conjugate() const noexcept
Definition: Quaternion.hpp:256
static Quaternion identity() noexcept
Definition: Quaternion.hpp:278
Vec3 axisOfRotation() const noexcept
Definition: Quaternion.hpp:238
Quaternion & operator+=(const Quaternion &inRhs) noexcept
Definition: Quaternion.hpp:691
static Quaternion pitchRotation(double inAngle) noexcept
Definition: Quaternion.hpp:386
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