You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and dots ('.'), can be up to 35 characters long. Letters must be lowercase.
426 lines
11 KiB
426 lines
11 KiB
/////////////////////////////////////////////////////////////////////////////////// |
|
/// OpenGL Mathematics (glm.g-truc.net) |
|
/// |
|
/// Copyright (c) 2005 - 2013 G-Truc Creation (www.g-truc.net) |
|
/// Permission is hereby granted, free of charge, to any person obtaining a copy |
|
/// of this software and associated documentation files (the "Software"), to deal |
|
/// in the Software without restriction, including without limitation the rights |
|
/// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
|
/// copies of the Software, and to permit persons to whom the Software is |
|
/// furnished to do so, subject to the following conditions: |
|
/// |
|
/// The above copyright notice and this permission notice shall be included in |
|
/// all copies or substantial portions of the Software. |
|
/// |
|
/// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
|
/// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
|
/// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
|
/// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
|
/// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
|
/// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
|
/// THE SOFTWARE. |
|
/// |
|
/// @ref gtx_dual_quaternion |
|
/// @file glm/gtx/dual_quaternion.inl |
|
/// @date 2013-02-10 / 2013-02-13 |
|
/// @author Maksim Vorobiev (msomeone@gmail.com) |
|
/////////////////////////////////////////////////////////////////////////////////// |
|
|
|
#include <limits> |
|
|
|
namespace glm{ |
|
namespace detail |
|
{ |
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER GLM_CONSTEXPR typename tdualquat<T, P>::size_type tdualquat<T, P>::length() const |
|
{ |
|
return 8; |
|
} |
|
|
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER tdualquat<T, P>::tdualquat() : |
|
real(tquat<T, P>()), |
|
dual(tquat<T, P>(T(0), T(0), T(0), T(0))) |
|
{} |
|
|
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER tdualquat<T, P>::tdualquat |
|
( |
|
tquat<T, P> const & r |
|
) : |
|
real(r), |
|
dual(tquat<T, P>(T(0), T(0), T(0), T(0))) |
|
{} |
|
|
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER tdualquat<T, P>::tdualquat |
|
( |
|
tquat<T, P> const & r, |
|
tquat<T, P> const & d |
|
) : |
|
real(r), |
|
dual(d) |
|
{} |
|
|
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER tdualquat<T, P>::tdualquat |
|
( |
|
tquat<T, P> const & q, |
|
tvec3<T, P> 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)) |
|
{} |
|
|
|
////////////////////////////////////////////////////////////// |
|
// tdualquat conversions |
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER tdualquat<T, P>::tdualquat |
|
( |
|
tmat2x4<T, P> const & m |
|
) |
|
{ |
|
*this = dualquat_cast(m); |
|
} |
|
|
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER tdualquat<T, P>::tdualquat |
|
( |
|
tmat3x4<T, P> const & m |
|
) |
|
{ |
|
*this = dualquat_cast(m); |
|
} |
|
|
|
////////////////////////////////////////////////////////////// |
|
// tdualquat<T, P> accesses |
|
|
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER typename tdualquat<T, P>::part_type & tdualquat<T, P>::operator [] (int i) |
|
{ |
|
return (&real)[i]; |
|
} |
|
|
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER typename tdualquat<T, P>::part_type const & tdualquat<T, P>::operator [] (int i) const |
|
{ |
|
return (&real)[i]; |
|
} |
|
|
|
////////////////////////////////////////////////////////////// |
|
// tdualquat<valType> operators |
|
|
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER tdualquat<T, P> & tdualquat<T, P>::operator *= |
|
( |
|
T const & s |
|
) |
|
{ |
|
this->real *= s; |
|
this->dual *= s; |
|
return *this; |
|
} |
|
|
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER tdualquat<T, P> & tdualquat<T, P>::operator /= |
|
( |
|
T const & s |
|
) |
|
{ |
|
this->real /= s; |
|
this->dual /= s; |
|
return *this; |
|
} |
|
|
|
////////////////////////////////////////////////////////////// |
|
// tquat<valType> external operators |
|
|
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER detail::tdualquat<T, P> operator- |
|
( |
|
detail::tdualquat<T, P> const & q |
|
) |
|
{ |
|
return detail::tdualquat<T, P>(-q.real,-q.dual); |
|
} |
|
|
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER detail::tdualquat<T, P> operator+ |
|
( |
|
detail::tdualquat<T, P> const & q, |
|
detail::tdualquat<T, P> const & p |
|
) |
|
{ |
|
return detail::tdualquat<T, P>(q.real + p.real,q.dual + p.dual); |
|
} |
|
|
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER detail::tdualquat<T, P> operator* |
|
( |
|
detail::tdualquat<T, P> const & p, |
|
detail::tdualquat<T, P> const & o |
|
) |
|
{ |
|
return detail::tdualquat<T, P>(p.real * o.real,p.real * o.dual + p.dual * o.real); |
|
} |
|
|
|
// Transformation |
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER detail::tvec3<T, P> operator* |
|
( |
|
detail::tdualquat<T, P> const & q, |
|
detail::tvec3<T, P> const & v |
|
) |
|
{ |
|
detail::tvec3<T, P> const real_v3(q.real.x,q.real.y,q.real.z); |
|
detail::tvec3<T, P> 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 <typename T, precision P> |
|
GLM_FUNC_QUALIFIER detail::tvec3<T, P> operator* |
|
( |
|
detail::tvec3<T, P> const & v, |
|
detail::tdualquat<T, P> const & q |
|
) |
|
{ |
|
return inverse(q) * v; |
|
} |
|
|
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER detail::tvec4<T, P> operator* |
|
( |
|
detail::tdualquat<T, P> const & q, |
|
detail::tvec4<T, P> const & v |
|
) |
|
{ |
|
return detail::tvec4<T, P>(q * detail::tvec3<T, P>(v), v.w); |
|
} |
|
|
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER detail::tvec4<T, P> operator* |
|
( |
|
detail::tvec4<T, P> const & v, |
|
detail::tdualquat<T, P> const & q |
|
) |
|
{ |
|
return inverse(q) * v; |
|
} |
|
|
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER detail::tdualquat<T, P> operator* |
|
( |
|
detail::tdualquat<T, P> const & q, |
|
typename detail::tdualquat<T, P>::T const & s |
|
) |
|
{ |
|
return detail::tdualquat<T, P>(q.real * s, q.dual * s); |
|
} |
|
|
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER detail::tdualquat<T, P> operator* |
|
( |
|
typename detail::tdualquat<T, P>::T const & s, |
|
detail::tdualquat<T, P> const & q |
|
) |
|
{ |
|
return q * s; |
|
} |
|
|
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER detail::tdualquat<T, P> operator/ |
|
( |
|
detail::tdualquat<T, P> const & q, |
|
typename detail::tdualquat<T, P>::T const & s |
|
) |
|
{ |
|
return detail::tdualquat<T, P>(q.real / s, q.dual / s); |
|
} |
|
|
|
////////////////////////////////////// |
|
// Boolean operators |
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER bool operator== |
|
( |
|
detail::tdualquat<T, P> const & q1, |
|
detail::tdualquat<T, P> const & q2 |
|
) |
|
{ |
|
return (q1.real == q2.real) && (q1.dual == q2.dual); |
|
} |
|
|
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER bool operator!= |
|
( |
|
detail::tdualquat<T, P> const & q1, |
|
detail::tdualquat<T, P> const & q2 |
|
) |
|
{ |
|
return (q1.real != q2.dual) || (q1.real != q2.dual); |
|
} |
|
}//namespace detail |
|
|
|
//////////////////////////////////////////////////////// |
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER detail::tdualquat<T, P> normalize |
|
( |
|
detail::tdualquat<T, P> const & q |
|
) |
|
{ |
|
return q / length(q.real); |
|
} |
|
|
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER detail::tdualquat<T, P> lerp |
|
( |
|
detail::tdualquat<T, P> const & x, |
|
detail::tdualquat<T, P> const & y, |
|
typename detail::tdualquat<T, P>::T const & a |
|
) |
|
{ |
|
// Dual Quaternion Linear blend aka DLB: |
|
// Lerp is only defined in [0, 1] |
|
assert(a >= static_cast<T>(0)); |
|
assert(a <= static_cast<T>(1)); |
|
T const k = dot(x.real,y.real) < detail::tdualquat<T, P>::value_type(0) ? -a : a; |
|
T const one(1); |
|
return detail::tdualquat<T, P>(x * (one - a) + y * k); |
|
} |
|
|
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER detail::tdualquat<T, P> inverse |
|
( |
|
detail::tdualquat<T, P> const & q |
|
) |
|
{ |
|
const glm::detail::tquat<T, P> real = conjugate(q.real); |
|
const glm::detail::tquat<T, P> dual = conjugate(q.dual); |
|
return detail::tdualquat<T, P>(real, dual + (real * (-2.0f * dot(real,dual)))); |
|
} |
|
/* |
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER detail::tmat3x3<T, P> mat3_cast |
|
( |
|
detail::tdualquat<T, P> const & x |
|
) |
|
{ |
|
} |
|
*/ |
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER detail::tmat2x4<T, P> mat2x4_cast |
|
( |
|
detail::tdualquat<T, P> const & x |
|
) |
|
{ |
|
return detail::tmat2x4<T, P>( 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 <typename T, precision P> |
|
GLM_FUNC_QUALIFIER detail::tmat3x4<T, P> mat3x4_cast |
|
( |
|
detail::tdualquat<T, P> const & x |
|
) |
|
{ |
|
detail::tquat<T, P> r = x.real / length2(x.real); |
|
|
|
detail::tquat<T, P> 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<T>(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; |
|
|
|
detail::tvec4<T, P> 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)); |
|
|
|
detail::tvec4<T, P> 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)); |
|
|
|
detail::tvec4<T, P> 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 detail::tmat3x4<T, P>(a, b, c); |
|
} |
|
|
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER detail::tdualquat<T, P> dualquat_cast |
|
( |
|
detail::tmat2x4<T, P> const & x |
|
) |
|
{ |
|
return detail::tdualquat<T, P>( |
|
detail::tquat<T, P>( x[0].w, x[0].x, x[0].y, x[0].z ), |
|
detail::tquat<T, P>( x[1].w, x[1].x, x[1].y, x[1].z )); |
|
} |
|
|
|
template <typename T, precision P> |
|
GLM_FUNC_QUALIFIER detail::tdualquat<T, P> dualquat_cast |
|
( |
|
detail::tmat3x4<T, P> const & x |
|
) |
|
{ |
|
detail::tquat<T, P> real; |
|
|
|
T const trace = x[0].x + x[1].y + x[2].z; |
|
if(trace > T(0)) |
|
{ |
|
T const r = sqrt(T(1) + trace); |
|
T const invr = static_cast<T>(0.5) / r; |
|
real.w = static_cast<T>(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<T>(0.5) / r; |
|
real.x = static_cast<T>(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<T>(0.5) / r; |
|
real.x = (x[1].x + x[0].y) * invr; |
|
real.y = static_cast<T>(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<T>(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<T>(0.5) * r; |
|
real.w = (x[1].x - x[0].y) * invr; |
|
} |
|
|
|
detail::tquat<T, P> dual; |
|
dual.x = T(0.5) * ( x[0].w * real.w + x[1].w * real.z - x[2].w * real.y); |
|
dual.y = T(0.5) * (-x[0].w * real.z + x[1].w * real.w + x[2].w * real.x); |
|
dual.z = T(0.5) * ( x[0].w * real.y - x[1].w * real.x + x[2].w * real.w); |
|
dual.w = -T(0.5) * ( x[0].w * real.x + x[1].w * real.y + x[2].w * real.z); |
|
return detail::tdualquat<T, P>(real, dual); |
|
} |
|
|
|
}//namespace glm
|
|
|