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);
178 eyeTimesScalar.
zeros();
179 eyeTimesScalar(0, 0) = inQuat2.
s();
180 eyeTimesScalar(1, 1) = inQuat2.
s();
181 eyeTimesScalar(2, 2) = inQuat2.
s();
183 NdArray<double> epsilonHat = linalg::hat<double>(inQuat2.
i(), inQuat2.
j(), inQuat2.
k());
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();
216 return {-
i(), -
j(), -
k(),
s()};
225 double i() const noexcept
227 return components_[0];
259 double j() const noexcept
261 return components_[1];
270 double k() const noexcept
272 return components_[2];
286 if (inPercent < 0.0 || inPercent > 1.0)
300 const double oneMinus = 1.0 - inPercent;
301 std::array<double, 4> newComponents{};
304 inQuat2.components_.begin(), newComponents.begin(),
305 [inPercent, oneMinus](
double component1,
double component2) ->
double
307 return oneMinus * component1 + inPercent * component2;
310 return {newComponents[0], newComponents[1], newComponents[2], newComponents[3]};
323 return nlerp(*
this, inQuat2, inPercent);
334 return std::asin(2 * (
s() *
j() -
k() *
i()));
354 return std::atan2(2 * (
s() *
i() +
j() *
k()),
367 if (inVector.
size() != 3)
372 return *
this * inVector;
384 return *
this * inVec3;
393 double s() const noexcept
395 return components_[3];
409 if (inPercent < 0 || inPercent > 1)
429 if (dotProduct < 0.0)
435 constexpr
double DOT_THRESHOLD = 0.9995;
436 if (dotProduct > DOT_THRESHOLD) {
439 return nlerp(inQuat1, inQuat2, inPercent);
442 dotProduct =
clip(dotProduct, -1.0, 1.0);
443 const double theta0 = std::acos(dotProduct);
444 const double theta = theta0 * inPercent;
463 return slerp(*
this, inQuat2, inPercent);
490 const double q0 =
i();
491 const double q1 =
j();
492 const double q2 =
k();
493 const double q3 =
s();
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;
521 auto componentsCopy = components_;
534 const Vec3 eulerAxis = { 1.0, 0.0, 0.0 };
544 double yaw() const noexcept
546 return std::atan2(2 * (
s() *
k() +
i() *
j()),
559 const Vec3 eulerAxis = { 0.0, 1.0, 0.0 };
572 const Vec3 eulerAxis = { 0.0, 0.0, 1.0 };
585 const auto comparitor = [](
double value1,
double value2) noexcept ->
bool
591 inRhs.components_.begin(), comparitor);
603 return !(*
this == inRhs);
616 inRhs.components_.begin(), components_.begin(), std::plus<double>());
645 inRhs.components_.begin(), components_.begin(), std::minus<double>());
684 double q0 = inRhs.
s() *
i();
685 q0 += inRhs.i() *
s();
686 q0 -= inRhs.j() *
k();
687 q0 += inRhs.k() *
j();
689 double q1 = inRhs.s() *
j();
690 q1 += inRhs.i() *
k();
691 q1 += inRhs.j() *
s();
692 q1 -= inRhs.k() *
i();
694 double q2 = inRhs.s() *
k();
695 q2 -= inRhs.i() *
j();
696 q2 += inRhs.j() *
i();
697 q2 += inRhs.k() *
s();
699 double q3 = inRhs.s() *
s();
700 q3 -= inRhs.i() *
i();
701 q3 -= inRhs.j() *
j();
702 q3 -= inRhs.k() *
k();
725 [inScalar](
double& component)
727 component *= inScalar;
769 if (inVec.
size() != 3)
774 const auto p =
Quaternion(inVec[0], inVec[1], inVec[2], 0.0);
775 const auto pPrime = *
this * p * this->
inverse();
828 inOStream << inQuat.
str();
834 std::array<double, 4> components_{ {0.0, 0.0, 0.0, 1.0} };
840 void normalize() noexcept
842 double sumOfSquares = 0.0;
844 [&sumOfSquares](
double component) noexcept ->
void
846 sumOfSquares += utils::sqr(component);
851 [
norm](
double& component) noexcept ->
void
865 void eulerToQuat(
double roll,
double pitch,
double yaw) noexcept
867 const double halfPhi =
roll / 2.0;
868 const double halfTheta =
pitch / 2.0;
869 const double halfPsi =
yaw / 2.0;
890 void dcmToQuat(
const NdArray<double>& dcm)
892 const Shape inShape = dcm.shape();
893 if (!(inShape.rows == 3 && inShape.cols == 3))
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);
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]);
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]);
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]);
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]);
#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