/* * Copyright (c) Contributors to the Open 3D Engine Project. * For complete copyright and license terms please see the LICENSE at the root of this distribution. * * SPDX-License-Identifier: Apache-2.0 OR MIT * */ // Description : Common quaternion class #pragma once #include //---------------------------------------------------------------------- // Quaternion //---------------------------------------------------------------------- template struct Quat_tpl { Vec3_tpl v; F w; //------------------------------- //constructors #if defined(_DEBUG) ILINE Quat_tpl() { if constexpr (sizeof(F) == 4) { uint32* p = alias_cast(&v.x); p[0] = F32NAN; p[1] = F32NAN; p[2] = F32NAN; p[3] = F32NAN; } if constexpr (sizeof(F) == 8) { uint64* p = alias_cast(&v.x); p[0] = F64NAN; p[1] = F64NAN; p[2] = F64NAN; p[3] = F64NAN; } } #else ILINE Quat_tpl() {} #endif //initialize with zeros ILINE Quat_tpl(type_zero) { w = 0, v.x = 0, v.y = 0, v.z = 0; } ILINE Quat_tpl(type_identity) { w = 1, v.x = 0, v.y = 0, v.z = 0; } //ASSIGNMENT OPERATOR of identical Quat types. //The assignment operator has precedence over assignment constructor //Quat q; q=q0; ILINE Quat_tpl& operator = (const Quat_tpl& src) { v.x = src.v.x; v.y = src.v.y; v.z = src.v.z; w = src.w; assert(IsValid()); return *this; } //CONSTRUCTOR to initialize a Quat from 4 floats //Quat q(1,0,0,0); ILINE Quat_tpl(F qw, F qx, F qy, F qz) { w = qw; v.x = qx; v.y = qy; v.z = qz; assert(IsValid()); } //CONSTRUCTOR to initialize a Quat with a scalar and a vector //Quat q(1,Vec3(0,0,0)); ILINE Quat_tpl(F scalar, const Vec3_tpl &vector) { v = vector; w = scalar; assert(IsValid()); }; //CONSTRUCTOR for identical types //Quat q=q0; ILINE Quat_tpl(const Quat_tpl&q) { w = q.w; v.x = q.v.x; v.y = q.v.y; v.z = q.v.z; assert(IsValid()); } //CONSTRUCTOR for AZ::Quaternion explicit ILINE Quat_tpl(const AZ::Quaternion& q) { w = q.GetW(); v.x = q.GetX(); v.y = q.GetY(); v.z = q.GetZ(); assert(IsValid()); } //CONSTRUCTOR for identical types which converts between double/float //Quat q32=q64; //Quatr q64=q32; template ILINE Quat_tpl(const Quat_tpl&q) { assert(q.IsValid()); w = F(q.w); v.x = F(q.v.x); v.y = F(q.v.y); v.z = F(q.v.z); } //CONSTRUCTOR for different types. It converts a Euler Angle into a Quat. //Needs to be 'explicit' because we loose fp-precision in the conversion process //Quat(Ang3(1,2,3)); explicit ILINE Quat_tpl(const Ang3_tpl&ang) { assert(ang.IsValid()); SetRotationXYZ(ang); } //CONSTRUCTOR for different types. It converts a Euler Angle into a Quat and converts between double/float. . //Needs to be 'explicit' because we loose fp-precision in the conversion process //Quat(Ang3r(1,2,3)); template explicit ILINE Quat_tpl(const Ang3_tpl&ang) { assert(ang.IsValid()); SetRotationXYZ(Ang3_tpl(F(ang.x), F(ang.y), F(ang.z))); } //CONSTRUCTOR for different types. It converts a Matrix33 into a Quat. //Needs to be 'explicit' because we loose fp-precision in the conversion process //Quat(m33); explicit ILINE Quat_tpl(const Matrix33_tpl&m) { assert(m.IsOrthonormalRH(0.1f)); F s, p, tr = m.m00 + m.m11 + m.m22; w = 1, v.x = 0, v.y = 0, v.z = 0; if (tr > 0) { s = sqrt_tpl(tr + 1.0f), p = 0.5f / s, w = s * 0.5f, v.x = (m.m21 - m.m12) * p, v.y = (m.m02 - m.m20) * p, v.z = (m.m10 - m.m01) * p; } else if ((m.m00 >= m.m11) && (m.m00 >= m.m22)) { s = sqrt_tpl(m.m00 - m.m11 - m.m22 + 1.0f), p = 0.5f / s, w = (m.m21 - m.m12) * p, v.x = s * 0.5f, v.y = (m.m10 + m.m01) * p, v.z = (m.m20 + m.m02) * p; } else if ((m.m11 >= m.m00) && (m.m11 >= m.m22)) { s = sqrt_tpl(m.m11 - m.m22 - m.m00 + 1.0f), p = 0.5f / s, w = (m.m02 - m.m20) * p, v.x = (m.m01 + m.m10) * p, v.y = s * 0.5f, v.z = (m.m21 + m.m12) * p; } else if ((m.m22 >= m.m00) && (m.m22 >= m.m11)) { s = sqrt_tpl(m.m22 - m.m00 - m.m11 + 1.0f), p = 0.5f / s, w = (m.m10 - m.m01) * p, v.x = (m.m02 + m.m20) * p, v.y = (m.m12 + m.m21) * p, v.z = s * 0.5f; } } //CONSTRUCTOR for different types. It converts a Matrix33 into a Quat and converts between double/float. //Needs to be 'explicit' because we loose fp-precision the conversion process //Quat(m33r); //Quatr(m33); template explicit ILINE Quat_tpl(const Matrix33_tpl&m) { assert(m.IsOrthonormalRH(0.1f)); F1 s, p, tr = m.m00 + m.m11 + m.m22; w = 1, v.x = 0, v.y = 0, v.z = 0; if (tr > 0) { s = sqrt_tpl(tr + 1.0f), p = 0.5f / s, w = F(s * 0.5), v.x = F((m.m21 - m.m12) * p), v.y = F((m.m02 - m.m20) * p), v.z = F((m.m10 - m.m01) * p); } else if ((m.m00 >= m.m11) && (m.m00 >= m.m22)) { s = sqrt_tpl(m.m00 - m.m11 - m.m22 + 1.0f), p = 0.5f / s, w = F((m.m21 - m.m12) * p), v.x = F(s * 0.5), v.y = F((m.m10 + m.m01) * p), v.z = F((m.m20 + m.m02) * p); } else if ((m.m11 >= m.m00) && (m.m11 >= m.m22)) { s = sqrt_tpl(m.m11 - m.m22 - m.m00 + 1.0f), p = 0.5f / s, w = F((m.m02 - m.m20) * p), v.x = F((m.m01 + m.m10) * p), v.y = F(s * 0.5), v.z = F((m.m21 + m.m12) * p); } else if ((m.m22 >= m.m00) && (m.m22 >= m.m11)) { s = sqrt_tpl(m.m22 - m.m00 - m.m11 + 1.0f), p = 0.5f / s, w = F((m.m10 - m.m01) * p), v.x = F((m.m02 + m.m20) * p), v.y = F((m.m12 + m.m21) * p), v.z = F(s * 0.5); } } //CONSTRUCTOR for different types. It converts a Matrix34 into a Quat. //Needs to be 'explicit' because we loose fp-precision in the conversion process //Quat(m34); explicit ILINE Quat_tpl(const Matrix34_tpl&m) { *this = Quat_tpl(Matrix33_tpl(m)); } //CONSTRUCTOR for different types. It converts a Matrix34 into a Quat and converts between double/float. //Needs to be 'explicit' because we loose fp-precision the conversion process //Quat(m34r); //Quatr(m34); template explicit ILINE Quat_tpl(const Matrix34_tpl&m) { *this = Quat_tpl(Matrix33_tpl(m)); } /*! * invert quaternion. * * Example 1: * Quat q=Quat::CreateRotationXYZ(Ang3(1,2,3)); * Quat result = !q; * Quat result = GetInverted(q); * q.Invert(); */ ILINE Quat_tpl operator ! () const { return Quat_tpl(w, -v); } ILINE void Invert(void) { *this = !*this; } ILINE Quat_tpl GetInverted() const { return !(*this); } //flip quaternion. don't confuse this with quaternion-inversion. ILINE Quat_tpl operator - () const { return Quat_tpl(-w, -v); }; //multiplication by a scalar void operator *= (F op) { w *= op; v *= op; } // Exact compare of 2 quats. ILINE bool operator==(const Quat_tpl& q) const { return (v == q.v) && (w == q.w); } ILINE bool operator!=(const Quat_tpl& q) const { return !(*this == q); } //A quaternion is a compressed matrix. Thus there is no problem extracting the rows & columns. ILINE Vec3_tpl GetColumn(uint32 i) { if (i == 0) { return GetColumn0(); } if (i == 1) { return GetColumn1(); } if (i == 2) { return GetColumn2(); } assert(0); //bad index return Vec3(ZERO); } ILINE Vec3_tpl GetColumn0() const {return Vec3_tpl(2 * (v.x * v.x + w * w) - 1, 2 * (v.y * v.x + v.z * w), 2 * (v.z * v.x - v.y * w)); } ILINE Vec3_tpl GetColumn1() const {return Vec3_tpl(2 * (v.x * v.y - v.z * w), 2 * (v.y * v.y + w * w) - 1, 2 * (v.z * v.y + v.x * w)); } ILINE Vec3_tpl GetColumn2() const {return Vec3_tpl(2 * (v.x * v.z + v.y * w), 2 * (v.y * v.z - v.x * w), 2 * (v.z * v.z + w * w) - 1); } ILINE Vec3_tpl GetRow0() const {return Vec3_tpl(2 * (v.x * v.x + w * w) - 1, 2 * (v.x * v.y - v.z * w), 2 * (v.x * v.z + v.y * w)); } ILINE Vec3_tpl GetRow1() const {return Vec3_tpl(2 * (v.y * v.x + v.z * w), 2 * (v.y * v.y + w * w) - 1, 2 * (v.y * v.z - v.x * w)); } ILINE Vec3_tpl GetRow2() const {return Vec3_tpl(2 * (v.z * v.x - v.y * w), 2 * (v.z * v.y + v.x * w), 2 * (v.z * v.z + w * w) - 1); } // These are just copy & pasted components of the GetColumn1() above. ILINE F GetFwdX() const { return (2 * (v.x * v.y - v.z * w)); } ILINE F GetFwdY() const { return (2 * (v.y * v.y + w * w) - 1); } ILINE F GetFwdZ() const { return (2 * (v.z * v.y + v.x * w)); } ILINE F GetRotZ() const { return atan2_tpl(-GetFwdX(), GetFwdY()); } /*! * set identity quaternion * * Example: * Quat q=Quat::CreateIdentity(); * or * q.SetIdentity(); * or * Quat p=Quat(IDENTITY); */ ILINE void SetIdentity(void) { w = 1; v.x = 0; v.y = 0; v.z = 0; } ILINE static Quat_tpl CreateIdentity(void) { return Quat_tpl(1, 0, 0, 0); } // Description: // Check if identity quaternion. ILINE bool IsIdentity() const { return w == 1 && v.x == 0 && v.y == 0 && v.z == 0; } ILINE bool IsUnit(F e = VEC_EPSILON) const { return fabs_tpl(1 - (w * w + v.x * v.x + v.y * v.y + v.z * v.z)) < e; } ILINE bool IsValid([[maybe_unused]] F e = VEC_EPSILON) const { if (!v.IsValid()) { return false; } if (!NumberValid(w)) { return false; } //if (!IsUnit(e)) return false; return true; } ILINE void SetRotationAA(F rad, const Vec3_tpl& axis) { F s, c; sincos_tpl(rad * (F)0.5, &s, &c); SetRotationAA(c, s, axis); } ILINE static Quat_tpl CreateRotationAA(F rad, const Vec3_tpl& axis) { Quat_tpl q; q.SetRotationAA(rad, axis); return q; } ILINE void SetRotationAA(F cosha, F sinha, const Vec3_tpl& axis) { assert(axis.IsUnit(0.001f)); w = cosha; v = axis * sinha; } ILINE static Quat_tpl CreateRotationAA(F cosha, F sinha, const Vec3_tpl& axis) { Quat_tpl q; q.SetRotationAA(cosha, sinha, axis); return q; } /*! * Create rotation-quaternion that around the fixed coordinate axes. * * Example: * Quat q=Quat::CreateRotationXYZ( Ang3(1,2,3) ); * or * q.SetRotationXYZ( Ang3(1,2,3) ); */ ILINE void SetRotationXYZ(const Ang3_tpl& a) { assert(a.IsValid()); F sx, cx; sincos_tpl(F(a.x * F(0.5)), &sx, &cx); F sy, cy; sincos_tpl(F(a.y * F(0.5)), &sy, &cy); F sz, cz; sincos_tpl(F(a.z * F(0.5)), &sz, &cz); w = cx * cy * cz + sx * sy * sz; v.x = cz * cy * sx - sz * sy * cx; v.y = cz * sy * cx + sz * cy * sx; v.z = sz * cy * cx - cz * sy * sx; } ILINE static Quat_tpl CreateRotationXYZ(const Ang3_tpl& a) { assert(a.IsValid()); Quat_tpl q; q.SetRotationXYZ(a); return q; } /*! * Create rotation-quaternion that about the x-axis. * * Example: * Quat q=Quat::CreateRotationX( radiant ); * or * q.SetRotationX( Ang3(1,2,3) ); */ ILINE void SetRotationX(f32 r) { F s, c; sincos_tpl(F(r * F(0.5)), &s, &c); w = c; v.x = s; v.y = 0; v.z = 0; } ILINE static Quat_tpl CreateRotationX(f32 r) { Quat_tpl q; q.SetRotationX(r); return q; } /*! * Create rotation-quaternion that about the y-axis. * * Example: * Quat q=Quat::CreateRotationY( radiant ); * or * q.SetRotationY( radiant ); */ ILINE void SetRotationY(f32 r) { F s, c; sincos_tpl(F(r * F(0.5)), &s, &c); w = c; v.x = 0; v.y = s; v.z = 0; } ILINE static Quat_tpl CreateRotationY(f32 r) { Quat_tpl q; q.SetRotationY(r); return q; } /*! * Create rotation-quaternion that about the z-axis. * * Example: * Quat q=Quat::CreateRotationZ( radiant ); * or * q.SetRotationZ( radiant ); */ ILINE void SetRotationZ(f32 r) { F s, c; sincos_tpl(F(r * F(0.5)), &s, &c); w = c; v.x = 0; v.y = 0; v.z = s; } ILINE static Quat_tpl CreateRotationZ(f32 r) { Quat_tpl q; q.SetRotationZ(r); return q; } /*! * * Create rotation-quaternion that rotates from one vector to another. * Both vectors are assumed to be normalized. * * Example: * Quat q=Quat::CreateRotationV0V1( v0,v1 ); * q.SetRotationV0V1( v0,v1 ); */ ILINE void SetRotationV0V1(const Vec3_tpl& v0, const Vec3_tpl& v1) { assert(v0.IsUnit(0.01f)); assert(v1.IsUnit(0.01f)); F dot = v0.x * v1.x + v0.y * v1.y + v0.z * v1.z + F(1.0); if (dot > F(0.0001)) { F vx = v0.y * v1.z - v0.z * v1.y; F vy = v0.z * v1.x - v0.x * v1.z; F vz = v0.x * v1.y - v0.y * v1.x; F d = isqrt_tpl(dot * dot + vx * vx + vy * vy + vz * vz); w = F(dot * d); v.x = F(vx * d); v.y = F(vy * d); v.z = F(vz * d); return; } w = 0; v = v0.GetOrthogonal().GetNormalized(); } ILINE static Quat_tpl CreateRotationV0V1(const Vec3_tpl& v0, const Vec3_tpl& v1) { Quat_tpl q; q.SetRotationV0V1(v0, v1); return q; } /*! * * \param vdir normalized view direction. * \param roll radiant to rotate about Y-axis. * * Given a view-direction and a radiant to rotate about Y-axis, this function builds a 3x3 look-at quaternion * using only simple vector arithmetic. This function is always using the implicit up-vector Vec3(0,0,1). * The view-direction is always stored in column(1). * IMPORTANT: The view-vector is assumed to be normalized, because all trig-values for the orientation are being * extracted directly out of the vector. This function must NOT be called with a view-direction * that is close to Vec3(0,0,1) or Vec3(0,0,-1). If one of these rules is broken, the function returns a quaternion * with an undefined rotation about the Z-axis. * * Rotation order for the look-at-quaternion is Z-X-Y. (Zaxis=YAW / Xaxis=PITCH / Yaxis=ROLL) * * COORDINATE-SYSTEM * * z-axis * ^ * | * | y-axis * | / * | / * |/ * +---------------> x-axis * * Example: * Quat LookAtQuat=Quat::CreateRotationVDir( Vec3(0,1,0) ); * or * Quat LookAtQuat=Quat::CreateRotationVDir( Vec3(0,1,0), 0.333f ); */ ILINE void SetRotationVDir(const Vec3_tpl& vdir) { assert(vdir.IsUnit(0.01f)); //set default initialization for up-vector w = F(0.70710676908493042); v.x = F(vdir.z * 0.70710676908493042); v.y = F(0.0); v.z = F(0.0); F l = sqrt_tpl(vdir.x * vdir.x + vdir.y * vdir.y); if (l > F(0.00001)) { //calculate LookAt quaternion Vec3_tpl hv = Vec3_tpl (vdir.x / l, vdir.y / l + 1.0f, l + 1.0f); F r = sqrt_tpl(hv.x * hv.x + hv.y * hv.y); F s = sqrt_tpl(hv.z * hv.z + vdir.z * vdir.z); //generate the half-angle sine&cosine F hacos0 = 0.0; F hasin0 = -1.0; if (r > F(0.00001)) { hacos0 = hv.y / r; hasin0 = -hv.x / r; } //yaw F hacos1 = hv.z / s; F hasin1 = vdir.z / s; //pitch w = F(hacos0 * hacos1); v.x = F(hacos0 * hasin1); v.y = F(hasin0 * hasin1); v.z = F(hasin0 * hacos1); } } ILINE static Quat_tpl CreateRotationVDir(const Vec3_tpl& vdir) { Quat_tpl q; q.SetRotationVDir(vdir); return q; } ILINE void SetRotationVDir(const Vec3_tpl& vdir, F r) { SetRotationVDir(vdir); F sy, cy; sincos_tpl(r * (F)0.5, &sy, &cy); F vx = v.x, vy = v.y; v.x = F(vx * cy - v.z * sy); v.y = F(w * sy + vy * cy); v.z = F(v.z * cy + vx * sy); w = F(w * cy - vy * sy); } ILINE static Quat_tpl CreateRotationVDir(const Vec3_tpl& vdir, F roll) { Quat_tpl q; q.SetRotationVDir(vdir, roll); return q; } /*! * normalize quaternion. * * Example 1: * Quat q; q.Normalize(); * * Example 2: * Quat q=Quat(1,2,3,4); * Quat qn=q.GetNormalized(); */ ILINE void Normalize(void) { F d = isqrt_tpl(w * w + v.x * v.x + v.y * v.y + v.z * v.z); w *= d; v.x *= d; v.y *= d; v.z *= d; } ILINE Quat_tpl GetNormalized() const { Quat_tpl t = *this; t.Normalize(); return t; } ILINE void NormalizeSafe(void) { F d = w * w + v.x * v.x + v.y * v.y + v.z * v.z; if (d > 1e-8f) { d = isqrt_tpl(d); w *= d; v.x *= d; v.y *= d; v.z *= d; } else { SetIdentity(); } } ILINE Quat_tpl GetNormalizedSafe() const { Quat_tpl t = *this; t.NormalizeSafe(); return t; } ILINE void NormalizeFast(void) { assert(this->IsValid()); F fInvLen = isqrt_fast_tpl(v.x * v.x + v.y * v.y + v.z * v.z + w * w); v.x *= fInvLen; v.y *= fInvLen; v.z *= fInvLen; w *= fInvLen; } ILINE Quat_tpl GetNormalizedFast() const { Quat_tpl t = *this; t.NormalizeFast(); return t; } /*! * get length of quaternion. * * Example 1: * f32 l=q.GetLength(); */ ILINE F GetLength() const { return sqrt_tpl(w * w + v.x * v.x + v.y * v.y + v.z * v.z); } ILINE static bool IsEquivalent(const Quat_tpl& q1, const Quat_tpl& q2, F qe = RAD_EPSILON) { Quat_tpl q1r = q1; Quat_tpl q2r = q2; f64 rad = acos(min(1.0, fabs_tpl(q1r.v.x * q2r.v.x + q1r.v.y * q2r.v.y + q1r.v.z * q2r.v.z + q1r.w * q2r.w))); bool qdif = rad <= qe; return qdif; } // Exponent of Quaternion. ILINE static Quat_tpl exp(const Vec3_tpl& v) { F lensqr = v.len2(); if (lensqr > F(0)) { F len = sqrt_tpl(lensqr); F s, c; sincos_tpl(len, &s, &c); s /= len; return Quat_tpl(c, v.x * s, v.y * s, v.z * s); } return Quat_tpl (IDENTITY); } // logarithm of a quaternion, imaginary part (the real part of the logarithm is always 0) ILINE static Vec3_tpl log (const Quat_tpl& q) { assert(q.IsValid()); F lensqr = q.v.len2(); if (lensqr > 0.0f) { F len = sqrt_tpl(lensqr); F angle = atan2_tpl(len, q.w) / len; return q.v * angle; } // logarithm of a quaternion, imaginary part (the real part of the logarithm is always 0) return Vec3_tpl(0, 0, 0); } ////////////////////////////////////////////////////////////////////////// //! Logarithm of Quaternion difference. ILINE static Quat_tpl LnDif(const Quat_tpl& q1, const Quat_tpl& q2) { return Quat_tpl(0, log(q2 / q1)); } /*! * linear-interpolation between quaternions (lerp) * * Example: * CQuaternion result,p,q; * result=qlerp( p, q, 0.5f ); */ ILINE void SetNlerp(const Quat_tpl& p, const Quat_tpl& tq, F t) { Quat_tpl q = tq; assert(p.IsValid()); assert(q.IsValid()); if ((p | q) < 0) { q = -q; } v.x = p.v.x * (1.0f - t) + q.v.x * t; v.y = p.v.y * (1.0f - t) + q.v.y * t; v.z = p.v.z * (1.0f - t) + q.v.z * t; w = p.w * (1.0f - t) + q.w * t; Normalize(); } ILINE static Quat_tpl CreateNlerp(const Quat_tpl& p, const Quat_tpl& tq, F t) { Quat_tpl d; d.SetNlerp(p, tq, t); return d; } /*! * spherical-interpolation between quaternions (geometrical slerp) * * Example: * Quat result,p,q; * result.SetSlerp( p, q, 0.5f ); */ ILINE void SetSlerp(const Quat_tpl& tp, const Quat_tpl& tq, F t) { assert(tp.IsValid()); assert(tq.IsUnit()); Quat_tpl p = tp, q = tq; Quat_tpl q2; F cosine = (p | q); if (cosine < 0.0f) { cosine = -cosine; q = -q; } //take shortest arc if (cosine > 0.9999f) { SetNlerp(p, q, t); return; } // from now on, a division by 0 is not possible any more q2.w = q.w - p.w * cosine; q2.v.x = q.v.x - p.v.x * cosine; q2.v.y = q.v.y - p.v.y * cosine; q2.v.z = q.v.z - p.v.z * cosine; F sine = sqrt(q2 | q2); // Actually you can divide by 0 right here. assert(sine != 0.0000f); F s, c; sincos_tpl(atan2_tpl(sine, cosine) * t, &s, &c); w = F(p.w * c + q2.w * s / sine); v.x = F(p.v.x * c + q2.v.x * s / sine); v.y = F(p.v.y * c + q2.v.y * s / sine); v.z = F(p.v.z * c + q2.v.z * s / sine); } ILINE static Quat_tpl CreateSlerp(const Quat_tpl& p, const Quat_tpl& tq, F t) { Quat_tpl d; d.SetSlerp(p, tq, t); return d; } //! squad(p,a,b,q,t) = slerp( slerp(p,q,t),slerp(a,b,t), 2(1-t)t). ILINE void SetSquad(const Quat_tpl& p, const Quat_tpl& a, const Quat_tpl& b, const Quat_tpl& q, F t) { SetSlerp(CreateSlerp(p, q, t), CreateSlerp(a, b, t), 2.0f * (1.0f - t) * t); } ILINE static Quat_tpl CreateSquad(const Quat_tpl& p, const Quat_tpl& a, const Quat_tpl& b, const Quat_tpl& q, F t) { Quat_tpl d; d.SetSquad(p, a, b, q, t); return d; } //useless, please delete ILINE Quat_tpl GetScaled(F scale) const { return CreateNlerp(IDENTITY, *this, scale); } }; /////////////////////////////////////////////////////////////////////////////// // Typedefs // /////////////////////////////////////////////////////////////////////////////// typedef Quat_tpl Quat; //always 32 bit /*! * * The "inner product" or "dot product" operation. * * calculate the "inner product" between two Quaternion. * If both Quaternion are unit-quaternions, the result is the cosine: p*q=cos(angle) * * Example: * Quat p(1,0,0,0),q(1,0,0,0); * f32 cosine = ( p | q ); * */ template ILINE F1 operator | (const Quat_tpl& q, const Quat_tpl& p) { assert(q.v.IsValid()); assert(p.v.IsValid()); return (q.v.x * p.v.x + q.v.y * p.v.y + q.v.z * p.v.z + q.w * p.w); } /*! * * Implements the multiplication operator: Qua=QuatA*QuatB * * AxB = operation B followed by operation A. * A multiplication takes 16 muls and 12 adds. * * Example 1: * Quat p(1,0,0,0),q(1,0,0,0); * Quat result=p*q; * * Example 2: (self-multiplication) * Quat p(1,0,0,0),q(1,0,0,0); * Quat p*=q; */ template Quat_tpl ILINE operator * (const Quat_tpl& q, const Quat_tpl& p) { assert(q.IsValid()); assert(p.IsValid()); return Quat_tpl ( q.w * p.w - (q.v.x * p.v.x + q.v.y * p.v.y + q.v.z * p.v.z), q.v.y * p.v.z - q.v.z * p.v.y + q.w * p.v.x + q.v.x * p.w, q.v.z * p.v.x - q.v.x * p.v.z + q.w * p.v.y + q.v.y * p.w, q.v.x * p.v.y - q.v.y * p.v.x + q.w * p.v.z + q.v.z * p.w ); } template ILINE void operator *= (Quat_tpl& q, const Quat_tpl& p) { assert(q.IsValid()); assert(p.IsValid()); F1 s0 = q.w; q.w = q.w * p.w - (q.v | p.v); q.v = p.v * s0 + q.v * p.w + (q.v % p.v); } /*! * division operator * * Example 1: * Quat p(1,0,0,0),q(1,0,0,0); * Quat result=p/q; * * Example 2: (self-division) * Quat p(1,0,0,0),q(1,0,0,0); * Quat p/=q; */ template ILINE Quat_tpl operator / (const Quat_tpl& q, const Quat_tpl& p) { return (!p * q); } template ILINE void operator /= (Quat_tpl& q, const Quat_tpl& p) { q = (!p * q); } /*! * addition operator * * Example: * Quat p(1,0,0,0),q(1,0,0,0); * Quat result=p+q; * * Example:(self-addition operator) * Quat p(1,0,0,0),q(1,0,0,0); * Quat p-=q; */ template ILINE Quat_tpl operator + (const Quat_tpl& q, const Quat_tpl& p) { return Quat_tpl(q.w + p.w, q.v + p.v); } template ILINE void operator += (Quat_tpl& q, const Quat_tpl& p) { q.w += p.w; q.v += p.v; } /*! * subtraction operator * * Example: * Quat p(1,0,0,0),q(1,0,0,0); * Quat result=p-q; * * Example: (self-subtraction operator) * Quat p(1,0,0,0),q(1,0,0,0); * Quat p-=q; * */ template ILINE Quat_tpl operator - (const Quat_tpl& q, const Quat_tpl& p) { return Quat_tpl(q.w - p.w, q.v - p.v); } template ILINE void operator -= (Quat_tpl& q, const Quat_tpl& p) { q.w -= p.w; q.v -= p.v; } //! Scale quaternion free function. template ILINE Quat_tpl operator * (F t, const Quat_tpl& q) { return Quat_tpl(t * q.w, t * q.v); }; template ILINE Quat_tpl operator * (const Quat_tpl& q, F2 t) { return Quat_tpl(q.w * t, q.v * t); }; template ILINE Quat_tpl operator / (const Quat_tpl& q, F2 t) { return Quat_tpl(q.w / t, q.v / t); }; /*! * * post-multiply of a quaternion and a Vec3 (3D rotations with quaternions) * * Example: * Quat q(1,0,0,0); * Vec3 v(33,44,55); * Vec3 result = q*v; */ template ILINE Vec3_tpl operator * (const Quat_tpl& q, const Vec3_tpl& v) { assert(v.IsValid()); assert(q.IsValid()); //muls=15 / adds=15 Vec3_tpl out, r2; r2.x = (q.v.y * v.z - q.v.z * v.y) + q.w * v.x; r2.y = (q.v.z * v.x - q.v.x * v.z) + q.w * v.y; r2.z = (q.v.x * v.y - q.v.y * v.x) + q.w * v.z; out.x = (r2.z * q.v.y - r2.y * q.v.z); out.x += out.x + v.x; out.y = (r2.x * q.v.z - r2.z * q.v.x); out.y += out.y + v.y; out.z = (r2.y * q.v.x - r2.x * q.v.y); out.z += out.z + v.z; return out; } /*! * pre-multiply of a quaternion and a Vec3 (3D rotations with quaternions) * * Example: * Quat q(1,0,0,0); * Vec3 v(33,44,55); * Vec3 result = v*q; */ template ILINE Vec3_tpl operator * (const Vec3_tpl& v, const Quat_tpl& q) { assert(v.IsValid()); assert(q.IsValid()); //muls=15 / adds=15 Vec3_tpl out, r2; r2.x = (q.v.z * v.y - q.v.y * v.z) + q.w * v.x; r2.y = (q.v.x * v.z - q.v.z * v.x) + q.w * v.y; r2.z = (q.v.y * v.x - q.v.x * v.y) + q.w * v.z; out.x = (r2.y * q.v.z - r2.z * q.v.y); out.x += out.x + v.x; out.y = (r2.z * q.v.x - r2.x * q.v.z); out.y += out.y + v.y; out.z = (r2.x * q.v.y - r2.y * q.v.x); out.z += out.z + v.z; return out; } template ILINE Quat_tpl operator % (const Quat_tpl& q, const Quat_tpl& tp) { Quat_tpl p = tp; if ((p | q) < 0) { p = -p; } return Quat_tpl(q.w + p.w, q.v + p.v); } template ILINE void operator %= (Quat_tpl& q, const Quat_tpl& tp) { Quat_tpl p = tp; if ((p | q) < 0) { p = -p; } q = Quat_tpl(q.w + p.w, q.v + p.v); }