24 #ifndef MT_QUATERNION_H
25 #define MT_QUATERNION_H
32 #include <mt/util/assert/assert_template.h>
147 const Scalar& t)
const;
152 Scalar&
at(
size_t n);
155 const Scalar&
at(
size_t n)
const;
208 std::ostream& operator<<(std::ostream& os,
296 inline Quaternion& Quaternion::operator-=(
const Quaternion& q)
318 setValue(m_co[3] * q[0] + m_co[0] * q[3] + m_co[1] * q[2] - m_co[2] * q[1],
319 m_co[3] * q[1] + m_co[1] * q[3] + m_co[2] * q[0] - m_co[0] * q[2],
320 m_co[3] * q[2] + m_co[2] * q[3] + m_co[0] * q[1] - m_co[1] * q[0],
321 m_co[3] * q[3] - m_co[0] * q[0] - m_co[1] * q[1] - m_co[2] * q[2]);
339 return Vector3(q[0], q[1], q[2]);
345 return (
distance(q) == Scalar(0.0)) && (
angle(q) == Scalar(0.0));
349 inline bool Quaternion::operator!=(
const Quaternion& q)
const
351 return !(*
this == q);
359 return m_co[0] * q[0] +
388 const Scalar d2 = diff.
length2();
397 const Scalar d = diff.
length();
404 const Scalar num(
dot(q));
407 Scalar ang_cos(num / den);
415 return acos(ang_cos);
421 return Quaternion(-m_co[0], -m_co[1], -m_co[2], m_co[3]);
432 const Scalar& t)
const
435 Scalar theta =
angle(q);
441 Scalar d = Scalar(1.0) / sin(theta);
442 Scalar s0 = sin((Scalar(1.0) - t) * theta);
443 Scalar s1 = sin(t * theta);
444 return Quaternion((m_co[0] * s0 + q[0] * s1) * d,
445 (m_co[1] * s0 + q[1] * s1) * d,
446 (m_co[2] * s0 + q[2] * s1) * d,
447 (m_co[3] * s0 + q[3] * s1) * d);
460 util::Assert(n < 4, std::range_error(
"Index out of range"));
467 util::Assert(n < 4, std::range_error(
"Index out of range"));
503 inline Quaternion operator-(
const Quaternion& q)
505 return Quaternion(-q[0],
512 inline Quaternion operator+(
const Quaternion& q1,
513 const Quaternion& q2)
520 inline Quaternion operator-(
const Quaternion& q1,
521 const Quaternion& q2)
528 inline Quaternion operator*(
const Quaternion& q1,
529 const Quaternion& q2)
535 inline Quaternion operator*(
const Quaternion& q,
538 return Quaternion(q[3] * v[0] + q[1] * v[2] - q[2] * v[1],
539 q[3] * v[1] + q[2] * v[0] - q[0] * v[2],
540 q[3] * v[2] + q[0] * v[1] - q[1] * v[0],
541 -q[0] * v[0] - q[1] * v[1] - q[2] * v[2]);
545 inline Quaternion operator*(
const Vector3& v,
548 return Quaternion(v[0] * q[3] + v[1] * q[2] - v[2] * q[1],
549 v[1] * q[3] + v[2] * q[0] - v[0] * q[2],
550 v[2] * q[3] + v[0] * q[1] - v[1] * q[0],
551 -v[0] * q[0] - v[1] * q[1] - v[2] * q[2]);
555 inline Quaternion operator*(
const Scalar& s,
563 inline Quaternion operator*(
const Quaternion& q,
570 inline Quaternion operator/(
const Quaternion& q,
578 inline std::ostream& operator<<(std::ostream& os,
581 return os <<
'[' << q[0] <<
' '
590 inline Scalar length2(
const Quaternion& q)
596 inline Scalar length(
const Quaternion& q)
602 inline Quaternion normalize(
const Quaternion& q)
605 return q1.normalize();
609 inline Scalar angleCos(
const Quaternion& q1,
610 const Quaternion& q2)
612 return q1.angleCos(q2);
616 inline Scalar angle(
const Quaternion& q1,
617 const Quaternion& q2)
623 inline Scalar dot(
const Quaternion& q1,
624 const Quaternion& q2)
630 inline Quaternion conjugate(
const Quaternion& q)
636 inline Quaternion inverse(
const Quaternion& q)
642 inline Quaternion slerp(
const Quaternion& q1,
643 const Quaternion& q2,
646 return q1.
slerp(q2, t);
651 #endif // MT_QUATERNION_H