From a8b3118e8305fd1c668ea25e07157b625c9747ff Mon Sep 17 00:00:00 2001 From: Joursoir Date: Sat, 10 Apr 2021 17:29:02 +0000 Subject: add glm headers --- src/include/glm/gtx/dual_quaternion.inl | 352 ++++++++++++++++++++++++++++++++ 1 file changed, 352 insertions(+) create mode 100644 src/include/glm/gtx/dual_quaternion.inl (limited to 'src/include/glm/gtx/dual_quaternion.inl') diff --git a/src/include/glm/gtx/dual_quaternion.inl b/src/include/glm/gtx/dual_quaternion.inl new file mode 100644 index 0000000..4fc8b57 --- /dev/null +++ b/src/include/glm/gtx/dual_quaternion.inl @@ -0,0 +1,352 @@ +/// @ref gtx_dual_quaternion + +#include "../geometric.hpp" +#include + +namespace glm +{ + // -- Component accesses -- + + template + GLM_FUNC_QUALIFIER typename tdualquat::part_type & tdualquat::operator[](typename tdualquat::length_type i) + { + assert(i >= 0 && i < this->length()); + return (&real)[i]; + } + + template + GLM_FUNC_QUALIFIER typename tdualquat::part_type const& tdualquat::operator[](typename tdualquat::length_type i) const + { + assert(i >= 0 && i < this->length()); + return (&real)[i]; + } + + // -- Implicit basic constructors -- + +# if GLM_CONFIG_DEFAULTED_FUNCTIONS == GLM_DISABLE + template + GLM_FUNC_QUALIFIER GLM_CONSTEXPR tdualquat::tdualquat() +# if GLM_CONFIG_DEFAULTED_FUNCTIONS != GLM_DISABLE + : real(qua()) + , dual(qua(0, 0, 0, 0)) +# endif + {} + + template + GLM_FUNC_QUALIFIER GLM_CONSTEXPR tdualquat::tdualquat(tdualquat const& d) + : real(d.real) + , dual(d.dual) + {} +# endif + + template + template + GLM_FUNC_QUALIFIER GLM_CONSTEXPR tdualquat::tdualquat(tdualquat const& d) + : real(d.real) + , dual(d.dual) + {} + + // -- Explicit basic constructors -- + + template + GLM_FUNC_QUALIFIER GLM_CONSTEXPR tdualquat::tdualquat(qua const& r) + : real(r), dual(qua(0, 0, 0, 0)) + {} + + template + GLM_FUNC_QUALIFIER GLM_CONSTEXPR tdualquat::tdualquat(qua const& q, vec<3, T, Q> const& p) + : real(q), dual( + T(-0.5) * ( p.x*q.x + p.y*q.y + p.z*q.z), + T(+0.5) * ( p.x*q.w + p.y*q.z - p.z*q.y), + T(+0.5) * (-p.x*q.z + p.y*q.w + p.z*q.x), + T(+0.5) * ( p.x*q.y - p.y*q.x + p.z*q.w)) + {} + + template + GLM_FUNC_QUALIFIER GLM_CONSTEXPR tdualquat::tdualquat(qua const& r, qua const& d) + : real(r), dual(d) + {} + + // -- Conversion constructors -- + + template + template + GLM_FUNC_QUALIFIER GLM_CONSTEXPR tdualquat::tdualquat(tdualquat const& q) + : real(q.real) + , dual(q.dual) + {} + + template + GLM_FUNC_QUALIFIER GLM_CONSTEXPR tdualquat::tdualquat(mat<2, 4, T, Q> const& m) + { + *this = dualquat_cast(m); + } + + template + GLM_FUNC_QUALIFIER GLM_CONSTEXPR tdualquat::tdualquat(mat<3, 4, T, Q> const& m) + { + *this = dualquat_cast(m); + } + + // -- Unary arithmetic operators -- + +# if GLM_CONFIG_DEFAULTED_FUNCTIONS == GLM_DISABLE + template + GLM_FUNC_QUALIFIER tdualquat & tdualquat::operator=(tdualquat const& q) + { + this->real = q.real; + this->dual = q.dual; + return *this; + } +# endif + + template + template + GLM_FUNC_QUALIFIER tdualquat & tdualquat::operator=(tdualquat const& q) + { + this->real = q.real; + this->dual = q.dual; + return *this; + } + + template + template + GLM_FUNC_QUALIFIER tdualquat & tdualquat::operator*=(U s) + { + this->real *= static_cast(s); + this->dual *= static_cast(s); + return *this; + } + + template + template + GLM_FUNC_QUALIFIER tdualquat & tdualquat::operator/=(U s) + { + this->real /= static_cast(s); + this->dual /= static_cast(s); + return *this; + } + + // -- Unary bit operators -- + + template + GLM_FUNC_QUALIFIER tdualquat operator+(tdualquat const& q) + { + return q; + } + + template + GLM_FUNC_QUALIFIER tdualquat operator-(tdualquat const& q) + { + return tdualquat(-q.real, -q.dual); + } + + // -- Binary operators -- + + template + GLM_FUNC_QUALIFIER tdualquat operator+(tdualquat const& q, tdualquat const& p) + { + return tdualquat(q.real + p.real,q.dual + p.dual); + } + + template + GLM_FUNC_QUALIFIER tdualquat operator*(tdualquat const& p, tdualquat const& o) + { + return tdualquat(p.real * o.real,p.real * o.dual + p.dual * o.real); + } + + template + GLM_FUNC_QUALIFIER vec<3, T, Q> operator*(tdualquat const& q, vec<3, T, Q> const& v) + { + vec<3, T, Q> const real_v3(q.real.x,q.real.y,q.real.z); + vec<3, T, Q> const dual_v3(q.dual.x,q.dual.y,q.dual.z); + return (cross(real_v3, cross(real_v3,v) + v * q.real.w + dual_v3) + dual_v3 * q.real.w - real_v3 * q.dual.w) * T(2) + v; + } + + template + GLM_FUNC_QUALIFIER vec<3, T, Q> operator*(vec<3, T, Q> const& v, tdualquat const& q) + { + return glm::inverse(q) * v; + } + + template + GLM_FUNC_QUALIFIER vec<4, T, Q> operator*(tdualquat const& q, vec<4, T, Q> const& v) + { + return vec<4, T, Q>(q * vec<3, T, Q>(v), v.w); + } + + template + GLM_FUNC_QUALIFIER vec<4, T, Q> operator*(vec<4, T, Q> const& v, tdualquat const& q) + { + return glm::inverse(q) * v; + } + + template + GLM_FUNC_QUALIFIER tdualquat operator*(tdualquat const& q, T const& s) + { + return tdualquat(q.real * s, q.dual * s); + } + + template + GLM_FUNC_QUALIFIER tdualquat operator*(T const& s, tdualquat const& q) + { + return q * s; + } + + template + GLM_FUNC_QUALIFIER tdualquat operator/(tdualquat const& q, T const& s) + { + return tdualquat(q.real / s, q.dual / s); + } + + // -- Boolean operators -- + + template + GLM_FUNC_QUALIFIER bool operator==(tdualquat const& q1, tdualquat const& q2) + { + return (q1.real == q2.real) && (q1.dual == q2.dual); + } + + template + GLM_FUNC_QUALIFIER bool operator!=(tdualquat const& q1, tdualquat const& q2) + { + return (q1.real != q2.real) || (q1.dual != q2.dual); + } + + // -- Operations -- + + template + GLM_FUNC_QUALIFIER tdualquat dual_quat_identity() + { + return tdualquat( + qua(static_cast(1), static_cast(0), static_cast(0), static_cast(0)), + qua(static_cast(0), static_cast(0), static_cast(0), static_cast(0))); + } + + template + GLM_FUNC_QUALIFIER tdualquat normalize(tdualquat const& q) + { + return q / length(q.real); + } + + template + GLM_FUNC_QUALIFIER tdualquat lerp(tdualquat const& x, tdualquat const& y, T const& a) + { + // Dual Quaternion Linear blend aka DLB: + // Lerp is only defined in [0, 1] + assert(a >= static_cast(0)); + assert(a <= static_cast(1)); + T const k = dot(x.real,y.real) < static_cast(0) ? -a : a; + T const one(1); + return tdualquat(x * (one - a) + y * k); + } + + template + GLM_FUNC_QUALIFIER tdualquat inverse(tdualquat const& q) + { + const glm::qua real = conjugate(q.real); + const glm::qua dual = conjugate(q.dual); + return tdualquat(real, dual + (real * (-2.0f * dot(real,dual)))); + } + + template + GLM_FUNC_QUALIFIER mat<2, 4, T, Q> mat2x4_cast(tdualquat const& x) + { + return mat<2, 4, T, Q>( x[0].x, x[0].y, x[0].z, x[0].w, x[1].x, x[1].y, x[1].z, x[1].w ); + } + + template + GLM_FUNC_QUALIFIER mat<3, 4, T, Q> mat3x4_cast(tdualquat const& x) + { + qua r = x.real / length2(x.real); + + qua const rr(r.w * x.real.w, r.x * x.real.x, r.y * x.real.y, r.z * x.real.z); + r *= static_cast(2); + + T const xy = r.x * x.real.y; + T const xz = r.x * x.real.z; + T const yz = r.y * x.real.z; + T const wx = r.w * x.real.x; + T const wy = r.w * x.real.y; + T const wz = r.w * x.real.z; + + vec<4, T, Q> const a( + rr.w + rr.x - rr.y - rr.z, + xy - wz, + xz + wy, + -(x.dual.w * r.x - x.dual.x * r.w + x.dual.y * r.z - x.dual.z * r.y)); + + vec<4, T, Q> const b( + xy + wz, + rr.w + rr.y - rr.x - rr.z, + yz - wx, + -(x.dual.w * r.y - x.dual.x * r.z - x.dual.y * r.w + x.dual.z * r.x)); + + vec<4, T, Q> const c( + xz - wy, + yz + wx, + rr.w + rr.z - rr.x - rr.y, + -(x.dual.w * r.z + x.dual.x * r.y - x.dual.y * r.x - x.dual.z * r.w)); + + return mat<3, 4, T, Q>(a, b, c); + } + + template + GLM_FUNC_QUALIFIER tdualquat dualquat_cast(mat<2, 4, T, Q> const& x) + { + return tdualquat( + qua( x[0].w, x[0].x, x[0].y, x[0].z ), + qua( x[1].w, x[1].x, x[1].y, x[1].z )); + } + + template + GLM_FUNC_QUALIFIER tdualquat dualquat_cast(mat<3, 4, T, Q> const& x) + { + qua real; + + T const trace = x[0].x + x[1].y + x[2].z; + if(trace > static_cast(0)) + { + T const r = sqrt(T(1) + trace); + T const invr = static_cast(0.5) / r; + real.w = static_cast(0.5) * r; + real.x = (x[2].y - x[1].z) * invr; + real.y = (x[0].z - x[2].x) * invr; + real.z = (x[1].x - x[0].y) * invr; + } + else if(x[0].x > x[1].y && x[0].x > x[2].z) + { + T const r = sqrt(T(1) + x[0].x - x[1].y - x[2].z); + T const invr = static_cast(0.5) / r; + real.x = static_cast(0.5)*r; + real.y = (x[1].x + x[0].y) * invr; + real.z = (x[0].z + x[2].x) * invr; + real.w = (x[2].y - x[1].z) * invr; + } + else if(x[1].y > x[2].z) + { + T const r = sqrt(T(1) + x[1].y - x[0].x - x[2].z); + T const invr = static_cast(0.5) / r; + real.x = (x[1].x + x[0].y) * invr; + real.y = static_cast(0.5) * r; + real.z = (x[2].y + x[1].z) * invr; + real.w = (x[0].z - x[2].x) * invr; + } + else + { + T const r = sqrt(T(1) + x[2].z - x[0].x - x[1].y); + T const invr = static_cast(0.5) / r; + real.x = (x[0].z + x[2].x) * invr; + real.y = (x[2].y + x[1].z) * invr; + real.z = static_cast(0.5) * r; + real.w = (x[1].x - x[0].y) * invr; + } + + qua dual; + dual.x = static_cast(0.5) * ( x[0].w * real.w + x[1].w * real.z - x[2].w * real.y); + dual.y = static_cast(0.5) * (-x[0].w * real.z + x[1].w * real.w + x[2].w * real.x); + dual.z = static_cast(0.5) * ( x[0].w * real.y - x[1].w * real.x + x[2].w * real.w); + dual.w = -static_cast(0.5) * ( x[0].w * real.x + x[1].w * real.y + x[2].w * real.z); + return tdualquat(real, dual); + } +}//namespace glm -- cgit v1.2.3-18-g5258