23 #ifndef SCMATH_GEOM_QUATERNION_H
24 #define SCMATH_GEOM_QUATERNION_H
29 #include <initializer_list>
84 this->constrainAngle();
95 v(
Vector<T, 3>{vector[1], vector[2], vector[3]}),
98 this->constrainAngle();
111 auto it = ilst.begin();
116 this->constrainAngle();
160 double innerProduct = this->v[0] * other.v[0];
162 innerProduct += (this->v[1] * other.v[1]);
163 innerProduct += (this->v[2] * other.v[2]);
164 innerProduct += (this->w * other.w);
180 n += (this->v[0] * this->v[0]);
181 n += (this->v[1] * this->v[1]);
182 n += (this->v[2] * this->v[2]);
183 n += (this->w * this->w);
195 return *
this / this->
Norm();
214 T _norm = this->
Norm();
216 return this->
Conjugate() / (_norm * _norm);
225 return this->v.
IsZero() &&
226 scmp::WithinTolerance(this->w, (T)1.0, this->eps);
236 auto normal = this->
Norm();
237 return scmp::WithinTolerance(normal, (T) 1.0, this->eps);
250 return Vector<T, 4>{this->w, this->v[0], this->v[1], this->v[2]};
261 return (this->
Conjugate() * vr * (*
this)).Axis();
278 T a = this->w, a2 = a * a;
279 T b = this->v[0], b2 = b * b;
280 T c = this->v[1], c2 = c * c;
281 T d = this->v[2], d2 = d * d;
283 yaw = std::atan2(2 * ((a * b) + (c * d)), a2 - b2 - c2 + d2);
284 pitch = std::asin(2 * ((b * d) - (a * c)));
285 roll = std::atan2(2 * ((a * d) + (b * c)), a2 + b2 - c2 - d2);
298 return Quaternion(this->v + other.v, this->w + other.w);
309 return Quaternion(this->v - other.v, this->w - other.w);
358 T angle = (this->w * other.w) -
361 (this->v * other.w) +
362 (this->v.
Cross(other.v));
374 return (this->v == other.v) &&
375 (scmp::WithinTolerance(this->w, other.w, this->eps));
386 return !(*
this == other);
398 friend std::ostream &
401 outs << q.w <<
" + " << q.v;
406 static constexpr T minRotation = -4 * M_PI;
407 static constexpr T maxRotation = 4 * M_PI;
417 this->w = std::fmod(this->w, this->minRotation);
420 this->w = std::fmod(this->w, this->maxRotation);
478 template <
typename T>
483 std::cos(angle / (T)2.0));
523 template <
typename T>
527 return (p + (q - p) * t).UnitQuaternion();
544 template <
typename T>
552 T sign = dp < 0.0 ? -1.0 : 1.0;
553 T omega = std::acos(dp * sign);
554 T sin_omega = std::sin(omega);
557 return LERP(p, q * sign, t);
560 return (p * std::sin((1.0 - t) * omega) / sin_omega) +
561 (q * sign * std::sin(omega*t) / sin_omega);
Linear algebraic vector class.
Quaternions provide a representation of Orientation and rotations in three dimensions.
Definition: Quaternion.h:61
T Dot(const Quaternion< T > &other) const
Compute the Dot product of two quaternions.
Definition: Quaternion.h:158
Quaternion operator/(const T k) const
Scalar division.
Definition: Quaternion.h:329
bool operator==(const Quaternion< T > &other) const
Quaternion equivalence.
Definition: Quaternion.h:372
T Angle() const
Return the Angle of rotation of this Quaternion.
Definition: Quaternion.h:147
void SetEpsilon(T epsilon)
Set the comparison tolerance for this Quaternion.
Definition: Quaternion.h:126
Quaternion()
Construct an identity Quaternion.
Definition: Quaternion.h:64
Vector< T, 3 > Axis() const
Return the Axis of rotation of this Quaternion.
Definition: Quaternion.h:137
Quaternion operator-(const Quaternion< T > &other) const
Quaternion subtraction.
Definition: Quaternion.h:307
Quaternion(Vector< T, 4 > vector)
Definition: Quaternion.h:94
Quaternion operator*(const Quaternion< T > &other) const
Quaternion Hamilton multiplication.
Definition: Quaternion.h:356
Vector< T, 4 > AsVector() const
Convert to Vector form.
Definition: Quaternion.h:248
friend std::ostream & operator<<(std::ostream &outs, const Quaternion< T > &q)
Output a Quaternion to a stream in the form a + <i, j, k>.
Definition: Quaternion.h:399
Quaterniond MakeQuaternion(Vector3D axis, double angle)
Convience Quaternion construction function.
Quaternionf MakeQuaternion(Vector3F axis, float angle)
Convenience Quaternion construction function.
Quaternion Conjugate() const
Compute the Conjugate of a Quaternion.
Definition: Quaternion.h:202
bool IsIdentity() const
Determine whether this is an identity Quaternion.
Definition: Quaternion.h:224
Quaternion Inverse() const
Compute the Inverse of a Quaternion.
Definition: Quaternion.h:212
bool operator!=(const Quaternion< T > &other) const
Quaternion non-equivalence.
Definition: Quaternion.h:384
Quaternion operator*(const T k) const
Scalar multiplication.
Definition: Quaternion.h:318
Vector< T, 3 > Euler() const
Return Euler angles for this Quaternion.
Definition: Quaternion.h:275
Vector< T, 3 > Rotate(Vector< T, 3 > vr) const
Rotate Vector vr about this Quaternion.
Definition: Quaternion.h:259
bool IsUnitQuaternion() const
Determine whether this is a unit Quaternion.
Definition: Quaternion.h:234
Quaternion(std::initializer_list< T > ilst)
An initializer list containing values for w, x, y, and z.
Definition: Quaternion.h:109
Quaternion UnitQuaternion()
Return the unit Quaternion.
Definition: Quaternion.h:193
T Norm() const
Compute the Norm of a Quaternion.
Definition: Quaternion.h:176
Quaternion(Vector< T, 3 > _axis, T _angle)
Construct a Quaternion with an Axis and Angle of rotation.
Definition: Quaternion.h:82
Quaternion< T > MakeQuaternion(Vector< T, 3 > axis, T angle)
Convience Quaternion construction function.
Definition: Quaternion.h:480
Quaternion operator*(const Vector< T, 3 > &vector) const
Quaternion Hamilton multiplication with a three- dimensional vector.
Definition: Quaternion.h:344
Quaterniond DoubleQuaternionFromEuler(Vector3D euler)
COnstruct a Quaternion from Euler angles.
Quaternionf FloatQuaternionFromEuler(Vector3F euler)
COnstruct a Quaternion from Euler angles.
Quaternion operator+(const Quaternion< T > &other) const
Quaternion addition.
Definition: Quaternion.h:296
Vectors represent a direction and Magnitude.
Definition: Vector.h:57
Vector Cross(const Vector< T, N > &other) const
Compute the cross product of two vectors.
Definition: Vector.h:292
bool IsZero() const
Determine whether this is a zero vector.
Definition: Vector.h:158
void SetEpsilon(T eps)
Set equivalence tolerance.
Definition: Vector.h:148
Vector UnitVector() const
Obtain the unit vector for this vector.
Definition: Vector.h:173
Quaternion< double > Quaterniond
Type alias for a double Quaternion.
Definition: Quaternion.h:437
Quaternion< float > Quaternionf
Type alias for a float Quaternion.
Definition: Quaternion.h:433
Quaternion< T > ShortestSLERP(Quaternion< T > p, Quaternion< T > q, T t)
Shortest distance spherical linear interpolation.
Definition: Quaternion.h:546
void QuaternionSelfTest()
Internal consistency check.
Quaternion< T > LERP(Quaternion< T > p, Quaternion< T > q, T t)
Linear interpolation for two Quaternions.
Definition: Quaternion.h:525
Shimmering Clarity Math & Physics toolkit.
Definition: estimation.h:31
void DefaultEpsilon(double &epsilon)
Get the default epsilon value.