88 Quaternion(
double inI,
double inJ,
double inK,
double inS) noexcept :
89 components_{ inI, inJ, inK, inS }
103 components_{ 0.0, 0.0, 0.0, 0.0 }
105 if (inArray.
size() == 3)
108 eulerToQuat(inArray[0], inArray[1], inArray[2]);
110 else if (inArray.
size() == 4)
116 else if (inArray.
size() == 9)
139 const double halfAngle = inAngle / 2.0;
140 const double sinHalfAngle =
std::sin(halfAngle);
142 components_[0] = normAxis.
x * sinHalfAngle;
143 components_[1] = normAxis.
y * sinHalfAngle;
144 components_[2] = normAxis.
z * sinHalfAngle;
145 components_[3] =
std::cos(halfAngle);
179 eyeTimesScalar.
zeros();
180 eyeTimesScalar(0, 0) = inQuat2.
s();
181 eyeTimesScalar(1, 1) = inQuat2.
s();
182 eyeTimesScalar(2, 2) = inQuat2.
s();
184 NdArray<double> epsilonHat = linalg::hat<double>(inQuat2.
i(), inQuat2.
j(), inQuat2.
k());
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();
219 return {-
i(), -
j(), -
k(),
s()};
229 double i() const noexcept
231 return components_[0];
266 double j() const noexcept
268 return components_[1];
278 double k() const noexcept
280 return components_[2];
295 if (inPercent < 0.0 || inPercent > 1.0)
309 const double oneMinus = 1.0 - inPercent;
310 std::array<double, 4> newComponents{};
313 inQuat2.components_.begin(), newComponents.begin(),
314 [inPercent, oneMinus](
double component1,
double component2) ->
double
316 return oneMinus * component1 + inPercent * component2;
319 return {newComponents[0], newComponents[1], newComponents[2], newComponents[3]};
333 return nlerp(*
this, inQuat2, inPercent);
344 return std::asin(2 * (
s() *
j() -
k() *
i()));
364 return std::atan2(2 * (
s() *
i() +
j() *
k()),
379 if (inVector.
size() != 3)
384 return *
this * inVector;
398 return *
this * inVec3;
408 double s() const noexcept
410 return components_[3];
425 if (inPercent < 0 || inPercent > 1)
445 if (dotProduct < 0.0)
451 constexpr
double DOT_THRESHOLD = 0.9995;
452 if (dotProduct > DOT_THRESHOLD) {
455 return nlerp(inQuat1, inQuat2, inPercent);
458 dotProduct =
clip(dotProduct, -1.0, 1.0);
459 const double theta0 = std::acos(dotProduct);
460 const double theta = theta0 * inPercent;
480 return slerp(*
this, inQuat2, inPercent);
509 const double q0 =
i();
510 const double q1 =
j();
511 const double q2 =
k();
512 const double q3 =
s();
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;
541 auto componentsCopy = components_;
556 const Vec3 eulerAxis = { 1.0, 0.0, 0.0 };
566 double yaw() const noexcept
568 return std::atan2(2 * (
s() *
k() +
i() *
j()),
583 const Vec3 eulerAxis = { 0.0, 1.0, 0.0 };
598 const Vec3 eulerAxis = { 0.0, 0.0, 1.0 };
613 const auto comparitor = [](
double value1,
double value2) noexcept ->
bool
619 inRhs.components_.begin(), comparitor);
633 return !(*
this == inRhs);
648 inRhs.components_.begin(), components_.begin(), std::plus<double>());
681 inRhs.components_.begin(), components_.begin(), std::minus<double>());
725 double q0 = inRhs.
s() *
i();
726 q0 += inRhs.i() *
s();
727 q0 -= inRhs.j() *
k();
728 q0 += inRhs.k() *
j();
730 double q1 = inRhs.s() *
j();
731 q1 += inRhs.i() *
k();
732 q1 += inRhs.j() *
s();
733 q1 -= inRhs.k() *
i();
735 double q2 = inRhs.s() *
k();
736 q2 -= inRhs.i() *
j();
737 q2 += inRhs.j() *
i();
738 q2 += inRhs.k() *
s();
740 double q3 = inRhs.s() *
s();
741 q3 -= inRhs.i() *
i();
742 q3 -= inRhs.j() *
j();
743 q3 -= inRhs.k() *
k();
768 [&inScalar](
double& component)
770 component *= inScalar;
818 if (inVec.
size() != 3)
823 const auto p =
Quaternion(inVec[0], inVec[1], inVec[2], 0.0);
824 const auto pPrime = *
this * p * this->
inverse();
827 rotatedVec *=
norm(inVec).item();
884 inOStream << inQuat.
str();
890 std::array<double, 4> components_{ {0.0, 0.0, 0.0, 1.0} };
896 void normalize() noexcept
898 double sumOfSquares = 0.0;
900 [&sumOfSquares](
double component) noexcept ->
void
902 sumOfSquares += utils::sqr(component);
907 [&
norm](
double& component) noexcept ->
void
921 void eulerToQuat(
double roll,
double pitch,
double yaw) noexcept
923 const double halfPhi =
roll / 2.0;
924 const double halfTheta =
pitch / 2.0;
925 const double halfPsi =
yaw / 2.0;
946 void dcmToQuat(
const NdArray<double>& dcm)
948 const Shape inShape = dcm.shape();
949 if (!(inShape.rows == 3 && inShape.cols == 3))
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);
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]);
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]);
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]);
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]);
#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