NumCpp  2.10.1
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::rotations
51 {
52  //================================================================================
53  // Class Description:
55  class Quaternion
56  {
57  public:
58  //============================================================================
59  // Method Description:
62  Quaternion() = default;
63 
64  //============================================================================
65  // Method Description:
72  Quaternion(double roll, double pitch, double yaw) noexcept
73  {
74  eulerToQuat(roll, pitch, yaw);
75  }
76 
77  //============================================================================
78  // Method Description:
86  Quaternion(double inI, double inJ, double inK, double inS) noexcept :
87  components_{ inI, inJ, inK, inS }
88  {
89  normalize();
90  }
91 
92  //============================================================================
93  // Method Description:
98  Quaternion(const std::array<double, 4>& components) noexcept :
99  components_{ components }
100  {
101  normalize();
102  }
103 
104  //============================================================================
105  // Method Description:
112  Quaternion(const NdArray<double>& inArray) :
113  components_{ 0., 0., 0., 0. }
114  {
115  if (inArray.size() == 3)
116  {
117  // euler angles
118  eulerToQuat(inArray[0], inArray[1], inArray[2]);
119  }
120  else if (inArray.size() == 4)
121  {
122  // quaternion i, j, k, s components
123  stl_algorithms::copy(inArray.cbegin(), inArray.cend(), components_.begin());
124  normalize();
125  }
126  else if (inArray.size() == 9)
127  {
128  // direction cosine matrix
129  dcmToQuat(inArray);
130  }
131  else
132  {
133  THROW_INVALID_ARGUMENT_ERROR("input array is not a valid size.");
134  }
135  }
136 
137  //============================================================================
138  // Method Description:
144  Quaternion(const Vec3& inAxis, double inAngle) noexcept
145  {
146  // normalize the input vector
147  Vec3 normAxis = inAxis.normalize();
148 
149  const double halfAngle = inAngle / 2.;
150  const double sinHalfAngle = std::sin(halfAngle);
151 
152  components_[0] = normAxis.x * sinHalfAngle;
153  components_[1] = normAxis.y * sinHalfAngle;
154  components_[2] = normAxis.z * sinHalfAngle;
155  components_[3] = std::cos(halfAngle);
156  }
157 
158  //============================================================================
159  // Method Description:
165  Quaternion(const NdArray<double>& inAxis, double inAngle) :
166  Quaternion(Vec3(inAxis), inAngle)
167  {
168  }
169 
170  //============================================================================
171  // Method Description:
176  [[nodiscard]] double angleOfRotation() const noexcept
177  {
178  return 2. * std::acos(s());
179  }
180 
181  //============================================================================
182  // Method Description:
191  static NdArray<double> angularVelocity(const Quaternion& inQuat1, const Quaternion& inQuat2, double inTime)
192  {
193  NdArray<double> q0 = inQuat1.toNdArray();
194  NdArray<double> q1 = inQuat2.toNdArray();
195 
196  NdArray<double> qDot = q1 - q0;
197  qDot /= inTime;
198 
199  NdArray<double> eyeTimesScalar(3);
200  eyeTimesScalar.zeros();
201  eyeTimesScalar(0, 0) = inQuat2.s();
202  eyeTimesScalar(1, 1) = inQuat2.s();
203  eyeTimesScalar(2, 2) = inQuat2.s();
204 
205  NdArray<double> epsilonHat = linalg::hat<double>(inQuat2.i(), inQuat2.j(), inQuat2.k());
206  NdArray<double> q(4, 3);
207  q.put(Slice(0, 3), Slice(0, 3), eyeTimesScalar + epsilonHat);
208  q(3, 0) = -inQuat2.i();
209  q(3, 1) = -inQuat2.j();
210  q(3, 2) = -inQuat2.k();
211 
212  NdArray<double> omega = q.transpose().dot(qDot.transpose());
213  return omega *= 2.;
214  }
215 
216  //============================================================================
217  // Method Description:
225  [[nodiscard]] NdArray<double> angularVelocity(const Quaternion& inQuat2, double inTime) const
226  {
227  return angularVelocity(*this, inQuat2, inTime);
228  }
229 
230  //============================================================================
231  // Method Description:
236  [[nodiscard]] Vec3 axisOfRotation() const noexcept
237  {
238  const auto halfAngle = angleOfRotation() / 2.;
239  const auto sinHalfAngle = std::sin(halfAngle);
240  auto axis = Vec3(i() / sinHalfAngle, j() / sinHalfAngle, k() / sinHalfAngle);
241 
242  // shouldn't be necessary, but let's be pedantic
243  return axis.normalize();
244  }
245 
246  //============================================================================
247  // Method Description:
252  [[nodiscard]] Quaternion conjugate() const noexcept
253  {
254  return { -i(), -j(), -k(), s() };
255  }
256 
257  //============================================================================
258  // Method Description:
263  [[nodiscard]] double i() const noexcept
264  {
265  return components_[0];
266  }
267 
268  //============================================================================
269  // Method Description:
274  static Quaternion identity() noexcept
275  {
276  return {};
277  }
278 
279  //============================================================================
280  // Method Description:
285  [[nodiscard]] Quaternion inverse() const noexcept
286  {
288  return conjugate();
289  }
290 
291  //============================================================================
292  // Method Description:
297  [[nodiscard]] double j() const noexcept
298  {
299  return components_[1];
300  }
301 
302  //============================================================================
303  // Method Description:
308  [[nodiscard]] double k() const noexcept
309  {
310  return components_[2];
311  }
312 
313  //============================================================================
314  // Method Description:
322  static Quaternion nlerp(const Quaternion& inQuat1, const Quaternion& inQuat2, double inPercent)
323  {
324  if (inPercent < 0. || inPercent > 1.)
325  {
326  THROW_INVALID_ARGUMENT_ERROR("input percent must be of the range [0,1].");
327  }
328 
329  if (utils::essentiallyEqual(inPercent, 0.))
330  {
331  return inQuat1;
332  }
333  if (utils::essentiallyEqual(inPercent, 1.))
334  {
335  return inQuat2;
336  }
337 
338  const double oneMinus = 1. - inPercent;
339  std::array<double, 4> newComponents{};
340 
341  stl_algorithms::transform(inQuat1.components_.begin(),
342  inQuat1.components_.end(),
343  inQuat2.components_.begin(),
344  newComponents.begin(),
345  [inPercent, oneMinus](double component1, double component2) -> double
346  { return oneMinus * component1 + inPercent * component2; });
347 
348  return { newComponents };
349  }
350 
351  //============================================================================
352  // Method Description:
359  [[nodiscard]] Quaternion nlerp(const Quaternion& inQuat2, double inPercent) const
360  {
361  return nlerp(*this, inQuat2, inPercent);
362  }
363 
364  //============================================================================
365  // Method Description:
370  [[nodiscard]] double pitch() const noexcept
371  {
372  return std::asin(2 * (s() * j() - k() * i()));
373  }
374 
375  //============================================================================
376  // Method Description:
382  static Quaternion pitchRotation(double inAngle) noexcept
383  {
384  return { 0., inAngle, 0. };
385  }
386 
387  //============================================================================
388  // Method Description:
391  void print() const
392  {
393  std::cout << *this;
394  }
395 
396  //============================================================================
397  // Method Description:
402  [[nodiscard]] double roll() const noexcept
403  {
404  return std::atan2(2. * (s() * i() + j() * k()), 1. - 2. * (utils::sqr(i()) + utils::sqr(j())));
405  }
406 
407  //============================================================================
408  // Method Description:
414  static Quaternion rollRotation(double inAngle) noexcept
415  {
416  return { inAngle, 0., 0. };
417  }
418 
419  //============================================================================
420  // Method Description:
426  [[nodiscard]] NdArray<double> rotate(const NdArray<double>& inVector) const
427  {
428  if (inVector.size() != 3)
429  {
430  THROW_INVALID_ARGUMENT_ERROR("input inVector must be a cartesion vector of length = 3.");
431  }
432 
433  return *this * inVector;
434  }
435 
436  //============================================================================
437  // Method Description:
443  [[nodiscard]] Vec3 rotate(const Vec3& inVec3) const
444  {
445  return *this * inVec3;
446  }
447 
448  //============================================================================
449  // Method Description:
454  [[nodiscard]] double s() const noexcept
455  {
456  return components_[3];
457  }
458 
459  //============================================================================
460  // Method Description:
468  static Quaternion slerp(const Quaternion& inQuat1, const Quaternion& inQuat2, double inPercent)
469  {
470  if (inPercent < 0 || inPercent > 1)
471  {
472  THROW_INVALID_ARGUMENT_ERROR("input percent must be of the range [0, 1]");
473  }
474 
475  if (utils::essentiallyEqual(inPercent, 0.))
476  {
477  return inQuat1;
478  }
479  if (utils::essentiallyEqual(inPercent, 1.))
480  {
481  return inQuat2;
482  }
483 
484  double dotProduct = dot<double>(inQuat1.toNdArray(), inQuat2.toNdArray()).item();
485 
486  // If the dot product is negative, the quaternions
487  // have opposite handed-ness and slerp won't take
488  // the shorter path. Fix by reversing one quaternion.
489  Quaternion quat1Copy(inQuat1);
490  if (dotProduct < 0.)
491  {
492  quat1Copy *= -1.;
493  dotProduct *= -1.;
494  }
495 
496  constexpr double DOT_THRESHOLD = 0.9995;
497  if (dotProduct > DOT_THRESHOLD)
498  {
499  // If the inputs are too close for comfort, linearly interpolate
500  // and normalize the result.
501  return nlerp(inQuat1, inQuat2, inPercent);
502  }
503 
504  dotProduct = clip(dotProduct, -1., 1.); // Robustness: Stay within domain of acos()
505  const double theta0 = std::acos(dotProduct); // angle between input vectors
506  const double theta = theta0 * inPercent; // angle between v0 and result
507 
508  const double s0 = std::cos(theta) -
509  dotProduct * std::sin(theta) / std::sin(theta0); // == sin(theta_0 - theta) / sin(theta_0)
510  const double s1 = std::sin(theta) / std::sin(theta0);
511 
512  NdArray<double> interpQuat = (quat1Copy.toNdArray() * s0) + (inQuat2.toNdArray() * s1);
513  return Quaternion(interpQuat); // NOLINT(modernize-return-braced-init-list)
514  }
515 
516  //============================================================================
517  // Method Description:
524  [[nodiscard]] Quaternion slerp(const Quaternion& inQuat2, double inPercent) const
525  {
526  return slerp(*this, inQuat2, inPercent);
527  }
528 
529  //============================================================================
530  // Method Description:
535  [[nodiscard]] std::string str() const
536  {
537  std::string output = "[" + utils::num2str(i()) + ", " + utils::num2str(j()) + ", " + utils::num2str(k()) +
538  ", " + utils::num2str(s()) + "]\n";
539 
540  return output;
541  }
542 
543  //============================================================================
544  // Method Description:
549  [[nodiscard]] NdArray<double> toDCM() const
550  {
551  NdArray<double> dcm(3);
552 
553  const double q0 = i();
554  const double q1 = j();
555  const double q2 = k();
556  const double q3 = s();
557 
558  const double q0sqr = utils::sqr(q0);
559  const double q1sqr = utils::sqr(q1);
560  const double q2sqr = utils::sqr(q2);
561  const double q3sqr = utils::sqr(q3);
562 
563  dcm(0, 0) = q3sqr + q0sqr - q1sqr - q2sqr;
564  dcm(0, 1) = 2. * (q0 * q1 - q3 * q2);
565  dcm(0, 2) = 2. * (q0 * q2 + q3 * q1);
566  dcm(1, 0) = 2. * (q0 * q1 + q3 * q2);
567  dcm(1, 1) = q3sqr + q1sqr - q0sqr - q2sqr;
568  dcm(1, 2) = 2. * (q1 * q2 - q3 * q0);
569  dcm(2, 0) = 2. * (q0 * q2 - q3 * q1);
570  dcm(2, 1) = 2. * (q1 * q2 + q3 * q0);
571  dcm(2, 2) = q3sqr + q2sqr - q0sqr - q1sqr;
572 
573  return dcm;
574  }
575 
576  //============================================================================
577  // Method Description:
582  [[nodiscard]] NdArray<double> toNdArray() const
583  {
584  auto componentsCopy = components_;
585  return NdArray<double>(componentsCopy); // NOLINT(modernize-return-braced-init-list)
586  }
587 
588  //============================================================================
589  // Method Description:
595  static Quaternion xRotation(double inAngle) noexcept
596  {
597  const Vec3 eulerAxis = { 1., 0., 0. };
598  return Quaternion(eulerAxis, inAngle); // NOLINT(modernize-return-braced-init-list)
599  }
600 
601  //============================================================================
602  // Method Description:
607  [[nodiscard]] double yaw() const noexcept
608  {
609  return std::atan2(2. * (s() * k() + i() * j()), 1. - 2. * (utils::sqr(j()) + utils::sqr(k())));
610  }
611 
612  //============================================================================
613  // Method Description:
619  static Quaternion yawRotation(double inAngle) noexcept
620  {
621  return { 0., 0., inAngle };
622  }
623 
624  //============================================================================
625  // Method Description:
631  static Quaternion yRotation(double inAngle) noexcept
632  {
633  const Vec3 eulerAxis = { 0., 1., 0. };
634  return Quaternion(eulerAxis, inAngle); // NOLINT(modernize-return-braced-init-list)
635  }
636 
637  //============================================================================
638  // Method Description:
644  static Quaternion zRotation(double inAngle) noexcept
645  {
646  const Vec3 eulerAxis = { 0., 0., 1. };
647  return Quaternion(eulerAxis, inAngle); // NOLINT(modernize-return-braced-init-list)
648  }
649 
650  //============================================================================
651  // Method Description:
657  bool operator==(const Quaternion& inRhs) const noexcept
658  {
659  const auto comparitor = [](double value1, double value2) noexcept -> bool
660  { return utils::essentiallyEqual(value1, value2); };
661 
662  return stl_algorithms::equal(components_.begin(), components_.end(), inRhs.components_.begin(), comparitor);
663  }
664 
665  //============================================================================
666  // Method Description:
672  bool operator!=(const Quaternion& inRhs) const noexcept
673  {
674  return !(*this == inRhs);
675  }
676 
677  //============================================================================
678  // Method Description:
684  Quaternion& operator+=(const Quaternion& inRhs) noexcept
685  {
686  stl_algorithms::transform(components_.begin(),
687  components_.end(),
688  inRhs.components_.begin(),
689  components_.begin(),
690  std::plus<double>()); // NOLINT(modernize-use-transparent-functors)
691 
692  normalize();
693 
694  return *this;
695  }
696 
697  //============================================================================
698  // Method Description:
704  Quaternion operator+(const Quaternion& inRhs) const noexcept
705  {
706  return Quaternion(*this) += inRhs;
707  }
708 
709  //============================================================================
710  // Method Description:
716  Quaternion& operator-=(const Quaternion& inRhs) noexcept
717  {
718  stl_algorithms::transform(components_.begin(),
719  components_.end(),
720  inRhs.components_.begin(),
721  components_.begin(),
722  std::minus<double>()); // NOLINT(modernize-use-transparent-functors)
723 
724  normalize();
725 
726  return *this;
727  }
728 
729  //============================================================================
730  // Method Description:
736  Quaternion operator-(const Quaternion& inRhs) const noexcept
737  {
738  return Quaternion(*this) -= inRhs;
739  }
740 
741  //============================================================================
742  // Method Description:
747  Quaternion operator-() const noexcept
748  {
749  return Quaternion(*this) *= -1.;
750  }
751 
752  //============================================================================
753  // Method Description:
759  Quaternion& operator*=(const Quaternion& inRhs) noexcept
760  {
761  double q0 = inRhs.s() * i();
762  q0 += inRhs.i() * s();
763  q0 -= inRhs.j() * k();
764  q0 += inRhs.k() * j();
765 
766  double q1 = inRhs.s() * j();
767  q1 += inRhs.i() * k();
768  q1 += inRhs.j() * s();
769  q1 -= inRhs.k() * i();
770 
771  double q2 = inRhs.s() * k();
772  q2 -= inRhs.i() * j();
773  q2 += inRhs.j() * i();
774  q2 += inRhs.k() * s();
775 
776  double q3 = inRhs.s() * s();
777  q3 -= inRhs.i() * i();
778  q3 -= inRhs.j() * j();
779  q3 -= inRhs.k() * k();
780 
781  components_[0] = q0;
782  components_[1] = q1;
783  components_[2] = q2;
784  components_[3] = q3;
785 
786  normalize();
787 
788  return *this;
789  }
790 
791  //============================================================================
792  // Method Description:
799  Quaternion& operator*=(double inScalar) noexcept
800  {
801  stl_algorithms::for_each(components_.begin(),
802  components_.end(),
803  [inScalar](double& component) { component *= inScalar; });
804 
805  normalize();
806 
807  return *this;
808  }
809 
810  //============================================================================
811  // Method Description:
817  Quaternion operator*(const Quaternion& inRhs) const noexcept
818  {
819  return Quaternion(*this) *= inRhs;
820  }
821 
822  //============================================================================
823  // Method Description:
830  Quaternion operator*(double inScalar) const noexcept
831  {
832  return Quaternion(*this) *= inScalar;
833  }
834 
835  //============================================================================
836  // Method Description:
843  {
844  if (inVec.size() != 3)
845  {
846  THROW_INVALID_ARGUMENT_ERROR("input vector must be a cartesion vector of length = 3.");
847  }
848 
849  const auto p = Quaternion(inVec[0], inVec[1], inVec[2], 0.);
850  const auto pPrime = *this * p * this->inverse();
851 
852  NdArray<double> rotatedVec = { pPrime.i(), pPrime.j(), pPrime.k() };
853  rotatedVec *= norm(inVec).item();
854  return rotatedVec;
855  }
856 
857  //============================================================================
858  // Method Description:
864  Vec3 operator*(const Vec3& inVec3) const
865  {
866  return *this * inVec3.toNdArray();
867  }
868 
869  //============================================================================
870  // Method Description:
876  Quaternion& operator/=(const Quaternion& inRhs) noexcept
877  {
878  return *this *= inRhs.conjugate();
879  }
880 
881  //============================================================================
882  // Method Description:
888  Quaternion operator/(const Quaternion& inRhs) const noexcept
889  {
890  return Quaternion(*this) /= inRhs;
891  }
892 
893  //============================================================================
894  // Method Description:
901  friend std::ostream& operator<<(std::ostream& inOStream, const Quaternion& inQuat)
902  {
903  inOStream << inQuat.str();
904  return inOStream;
905  }
906 
907  private:
908  //====================================Attributes==============================
909  std::array<double, 4> components_{ { 0., 0., 0., 1. } };
910 
911  //============================================================================
912  // Method Description:
915  void normalize() noexcept
916  {
917  double sumOfSquares = 0.;
918  std::for_each(components_.begin(),
919  components_.end(),
920  [&sumOfSquares](double component) noexcept -> void
921  { sumOfSquares += utils::sqr(component); });
922 
923  const double norm = std::sqrt(sumOfSquares);
924  stl_algorithms::for_each(components_.begin(),
925  components_.end(),
926  [norm](double& component) noexcept -> void { component /= norm; });
927  }
928 
929  //============================================================================
930  // Method Description:
937  void eulerToQuat(double roll, double pitch, double yaw) noexcept
938  {
939  const auto halfPhi = roll / 2.;
940  const auto halfTheta = pitch / 2.;
941  const auto halfPsi = yaw / 2.;
942 
943  const auto sinHalfPhi = std::sin(halfPhi);
944  const auto cosHalfPhi = std::cos(halfPhi);
945 
946  const auto sinHalfTheta = std::sin(halfTheta);
947  const auto cosHalfTheta = std::cos(halfTheta);
948 
949  const auto sinHalfPsi = std::sin(halfPsi);
950  const auto cosHalfPsi = std::cos(halfPsi);
951 
952  components_[0] = sinHalfPhi * cosHalfTheta * cosHalfPsi;
953  components_[0] -= cosHalfPhi * sinHalfTheta * sinHalfPsi;
954 
955  components_[1] = cosHalfPhi * sinHalfTheta * cosHalfPsi;
956  components_[1] += sinHalfPhi * cosHalfTheta * sinHalfPsi;
957 
958  components_[2] = cosHalfPhi * cosHalfTheta * sinHalfPsi;
959  components_[2] -= sinHalfPhi * sinHalfTheta * cosHalfPsi;
960 
961  components_[3] = cosHalfPhi * cosHalfTheta * cosHalfPsi;
962  components_[3] += sinHalfPhi * sinHalfTheta * sinHalfPsi;
963  }
964 
965  //============================================================================
966  // Method Description:
971  void dcmToQuat(const NdArray<double>& dcm)
972  {
973  const Shape inShape = dcm.shape();
974  if (!(inShape.rows == 3 && inShape.cols == 3))
975  {
976  THROW_INVALID_ARGUMENT_ERROR("input direction cosine matrix must have shape = (3,3).");
977  }
978 
979  NdArray<double> checks(1, 4);
980  checks[0] = 1 + dcm(0, 0) + dcm(1, 1) + dcm(2, 2);
981  checks[1] = 1 + dcm(0, 0) - dcm(1, 1) - dcm(2, 2);
982  checks[2] = 1 - dcm(0, 0) + dcm(1, 1) - dcm(2, 2);
983  checks[3] = 1 - dcm(0, 0) - dcm(1, 1) + dcm(2, 2);
984 
985  const uint32 maxIdx = argmax(checks).item();
986 
987  switch (maxIdx)
988  {
989  case 0:
990  {
991  components_[3] = 0.5 * std::sqrt(1 + dcm(0, 0) + dcm(1, 1) + dcm(2, 2));
992  components_[0] = (dcm(2, 1) - dcm(1, 2)) / (4 * components_[3]);
993  components_[1] = (dcm(0, 2) - dcm(2, 0)) / (4 * components_[3]);
994  components_[2] = (dcm(1, 0) - dcm(0, 1)) / (4 * components_[3]);
995 
996  break;
997  }
998  case 1:
999  {
1000  components_[0] = 0.5 * std::sqrt(1 + dcm(0, 0) - dcm(1, 1) - dcm(2, 2));
1001  components_[1] = (dcm(1, 0) + dcm(0, 1)) / (4 * components_[0]);
1002  components_[2] = (dcm(2, 0) + dcm(0, 2)) / (4 * components_[0]);
1003  components_[3] = (dcm(2, 1) - dcm(1, 2)) / (4 * components_[0]);
1004 
1005  break;
1006  }
1007  case 2:
1008  {
1009  components_[1] = 0.5 * std::sqrt(1 - dcm(0, 0) + dcm(1, 1) - dcm(2, 2));
1010  components_[0] = (dcm(1, 0) + dcm(0, 1)) / (4 * components_[1]);
1011  components_[2] = (dcm(2, 1) + dcm(1, 2)) / (4 * components_[1]);
1012  components_[3] = (dcm(0, 2) - dcm(2, 0)) / (4 * components_[1]);
1013 
1014  break;
1015  }
1016  case 3:
1017  {
1018  components_[2] = 0.5 * std::sqrt(1 - dcm(0, 0) - dcm(1, 1) + dcm(2, 2));
1019  components_[0] = (dcm(2, 0) + dcm(0, 2)) / (4 * components_[2]);
1020  components_[1] = (dcm(2, 1) + dcm(1, 2)) / (4 * components_[2]);
1021  components_[3] = (dcm(1, 0) - dcm(0, 1)) / (4 * components_[2]);
1022 
1023  break;
1024  }
1025  }
1026  }
1027  };
1028 } // namespace nc::rotations
#define THROW_INVALID_ARGUMENT_ERROR(msg)
Definition: Error.hpp:37
size_type size() const noexcept
Definition: NdArrayCore.hpp:4415
self_type & zeros() noexcept
Definition: NdArrayCore.hpp:4793
const_iterator cbegin() const noexcept
Definition: NdArrayCore.hpp:1308
self_type & put(index_type inIndex, const value_type &inValue)
Definition: NdArrayCore.hpp:3616
self_type transpose() const
Definition: NdArrayCore.hpp:4775
self_type dot(const self_type &inOtherArray) const
Definition: NdArrayCore.hpp:2642
const_iterator cend() const noexcept
Definition: NdArrayCore.hpp:1616
value_type item() const
Definition: NdArrayCore.hpp:2945
A Class for slicing into NdArrays.
Definition: Slice.hpp:45
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:56
double s() const noexcept
Definition: Quaternion.hpp:454
std::string str() const
Definition: Quaternion.hpp:535
NdArray< double > operator*(const NdArray< double > &inVec) const
Definition: Quaternion.hpp:842
NdArray< double > angularVelocity(const Quaternion &inQuat2, double inTime) const
Definition: Quaternion.hpp:225
Quaternion & operator*=(double inScalar) noexcept
Definition: Quaternion.hpp:799
double angleOfRotation() const noexcept
Definition: Quaternion.hpp:176
double roll() const noexcept
Definition: Quaternion.hpp:402
NdArray< double > rotate(const NdArray< double > &inVector) const
Definition: Quaternion.hpp:426
static Quaternion xRotation(double inAngle) noexcept
Definition: Quaternion.hpp:595
static Quaternion nlerp(const Quaternion &inQuat1, const Quaternion &inQuat2, double inPercent)
Definition: Quaternion.hpp:322
static Quaternion rollRotation(double inAngle) noexcept
Definition: Quaternion.hpp:414
Vec3 rotate(const Vec3 &inVec3) const
Definition: Quaternion.hpp:443
Quaternion(double inI, double inJ, double inK, double inS) noexcept
Definition: Quaternion.hpp:86
Quaternion operator-() const noexcept
Definition: Quaternion.hpp:747
NdArray< double > toNdArray() const
Definition: Quaternion.hpp:582
Quaternion(const std::array< double, 4 > &components) noexcept
Definition: Quaternion.hpp:98
Quaternion operator+(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:704
double i() const noexcept
Definition: Quaternion.hpp:263
double yaw() const noexcept
Definition: Quaternion.hpp:607
double pitch() const noexcept
Definition: Quaternion.hpp:370
Quaternion & operator-=(const Quaternion &inRhs) noexcept
Definition: Quaternion.hpp:716
Quaternion slerp(const Quaternion &inQuat2, double inPercent) const
Definition: Quaternion.hpp:524
friend std::ostream & operator<<(std::ostream &inOStream, const Quaternion &inQuat)
Definition: Quaternion.hpp:901
static Quaternion slerp(const Quaternion &inQuat1, const Quaternion &inQuat2, double inPercent)
Definition: Quaternion.hpp:468
static Quaternion yawRotation(double inAngle) noexcept
Definition: Quaternion.hpp:619
static NdArray< double > angularVelocity(const Quaternion &inQuat1, const Quaternion &inQuat2, double inTime)
Definition: Quaternion.hpp:191
void print() const
Definition: Quaternion.hpp:391
Quaternion(const NdArray< double > &inAxis, double inAngle)
Definition: Quaternion.hpp:165
bool operator==(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:657
Quaternion & operator/=(const Quaternion &inRhs) noexcept
Definition: Quaternion.hpp:876
Quaternion(double roll, double pitch, double yaw) noexcept
Definition: Quaternion.hpp:72
Vec3 operator*(const Vec3 &inVec3) const
Definition: Quaternion.hpp:864
Quaternion inverse() const noexcept
Definition: Quaternion.hpp:285
double k() const noexcept
Definition: Quaternion.hpp:308
NdArray< double > toDCM() const
Definition: Quaternion.hpp:549
Quaternion & operator*=(const Quaternion &inRhs) noexcept
Definition: Quaternion.hpp:759
static Quaternion zRotation(double inAngle) noexcept
Definition: Quaternion.hpp:644
Quaternion operator/(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:888
Quaternion nlerp(const Quaternion &inQuat2, double inPercent) const
Definition: Quaternion.hpp:359
static Quaternion yRotation(double inAngle) noexcept
Definition: Quaternion.hpp:631
Quaternion(const Vec3 &inAxis, double inAngle) noexcept
Definition: Quaternion.hpp:144
double j() const noexcept
Definition: Quaternion.hpp:297
Quaternion operator*(double inScalar) const noexcept
Definition: Quaternion.hpp:830
Quaternion operator-(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:736
Quaternion operator*(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:817
bool operator!=(const Quaternion &inRhs) const noexcept
Definition: Quaternion.hpp:672
Quaternion(const NdArray< double > &inArray)
Definition: Quaternion.hpp:112
Quaternion conjugate() const noexcept
Definition: Quaternion.hpp:252
static Quaternion identity() noexcept
Definition: Quaternion.hpp:274
Vec3 axisOfRotation() const noexcept
Definition: Quaternion.hpp:236
Quaternion & operator+=(const Quaternion &inRhs) noexcept
Definition: Quaternion.hpp:684
static Quaternion pitchRotation(double inAngle) noexcept
Definition: Quaternion.hpp:382
Definition: DCM.hpp:39
OutputIt transform(InputIt first, InputIt last, OutputIt destination, UnaryOperation unaryFunction)
Definition: StlAlgorithms.hpp:775
void for_each(InputIt first, InputIt last, UnaryFunction f)
Definition: StlAlgorithms.hpp:225
bool equal(InputIt1 first1, InputIt1 last1, InputIt2 first2) noexcept
Definition: StlAlgorithms.hpp:140
OutputIt copy(InputIt first, InputIt last, OutputIt destination) noexcept
Definition: StlAlgorithms.hpp:97
std::string num2str(dtype inNumber)
Definition: num2str.hpp:44
bool essentiallyEqual(dtype inValue1, dtype inValue2) noexcept
Definition: essentiallyEqual.hpp:48
constexpr dtype sqr(dtype inValue) noexcept
Definition: sqr.hpp:42
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