1/// @ref gtx_dual_quaternion 2/// @file glm/gtx/dual_quaternion.inl 3 4#include "../geometric.hpp" 5#include <limits> 6 7namespace glm 8{ 9 // -- Component accesses -- 10 11 template <typename T, precision P> 12 GLM_FUNC_QUALIFIER typename tdualquat<T, P>::part_type & tdualquat<T, P>::operator[](typename tdualquat<T, P>::length_type i) 13 { 14 assert(i >= 0 && i < this->length()); 15 return (&real)[i]; 16 } 17 18 template <typename T, precision P> 19 GLM_FUNC_QUALIFIER typename tdualquat<T, P>::part_type const & tdualquat<T, P>::operator[](typename tdualquat<T, P>::length_type i) const 20 { 21 assert(i >= 0 && i < this->length()); 22 return (&real)[i]; 23 } 24 25 // -- Implicit basic constructors -- 26 27# if !GLM_HAS_DEFAULTED_FUNCTIONS || !defined(GLM_FORCE_NO_CTOR_INIT) 28 template <typename T, precision P> 29 GLM_FUNC_QUALIFIER GLM_CONSTEXPR tdualquat<T, P>::tdualquat() 30# ifndef GLM_FORCE_NO_CTOR_INIT 31 : real(tquat<T, P>()) 32 , dual(tquat<T, P>(0, 0, 0, 0)) 33# endif 34 {} 35# endif 36 37# if !GLM_HAS_DEFAULTED_FUNCTIONS 38 template <typename T, precision P> 39 GLM_FUNC_QUALIFIER GLM_CONSTEXPR tdualquat<T, P>::tdualquat(tdualquat<T, P> const & d) 40 : real(d.real) 41 , dual(d.dual) 42 {} 43# endif//!GLM_HAS_DEFAULTED_FUNCTIONS 44 45 template <typename T, precision P> 46 template <precision Q> 47 GLM_FUNC_QUALIFIER GLM_CONSTEXPR tdualquat<T, P>::tdualquat(tdualquat<T, Q> const & d) 48 : real(d.real) 49 , dual(d.dual) 50 {} 51 52 // -- Explicit basic constructors -- 53 54 template <typename T, precision P> 55 GLM_FUNC_QUALIFIER GLM_CONSTEXPR_CTOR tdualquat<T, P>::tdualquat(ctor) 56 {} 57 58 template <typename T, precision P> 59 GLM_FUNC_QUALIFIER GLM_CONSTEXPR tdualquat<T, P>::tdualquat(tquat<T, P> const & r) 60 : real(r), dual(tquat<T, P>(0, 0, 0, 0)) 61 {} 62 63 template <typename T, precision P> 64 GLM_FUNC_QUALIFIER GLM_CONSTEXPR tdualquat<T, P>::tdualquat(tquat<T, P> const & q, tvec3<T, P> const& p) 65 : real(q), dual( 66 T(-0.5) * ( p.x*q.x + p.y*q.y + p.z*q.z), 67 T(+0.5) * ( p.x*q.w + p.y*q.z - p.z*q.y), 68 T(+0.5) * (-p.x*q.z + p.y*q.w + p.z*q.x), 69 T(+0.5) * ( p.x*q.y - p.y*q.x + p.z*q.w)) 70 {} 71 72 template <typename T, precision P> 73 GLM_FUNC_QUALIFIER GLM_CONSTEXPR tdualquat<T, P>::tdualquat(tquat<T, P> const & r, tquat<T, P> const & d) 74 : real(r), dual(d) 75 {} 76 77 // -- Conversion constructors -- 78 79 template <typename T, precision P> 80 template <typename U, precision Q> 81 GLM_FUNC_QUALIFIER GLM_CONSTEXPR tdualquat<T, P>::tdualquat(tdualquat<U, Q> const & q) 82 : real(q.real) 83 , dual(q.dual) 84 {} 85 86 template <typename T, precision P> 87 GLM_FUNC_QUALIFIER tdualquat<T, P>::tdualquat(tmat2x4<T, P> const & m) 88 { 89 *this = dualquat_cast(m); 90 } 91 92 template <typename T, precision P> 93 GLM_FUNC_QUALIFIER tdualquat<T, P>::tdualquat(tmat3x4<T, P> const & m) 94 { 95 *this = dualquat_cast(m); 96 } 97 98 // -- Unary arithmetic operators -- 99 100# if !GLM_HAS_DEFAULTED_FUNCTIONS 101 template <typename T, precision P> 102 GLM_FUNC_QUALIFIER tdualquat<T, P> & tdualquat<T, P>::operator=(tdualquat<T, P> const & q) 103 { 104 this->real = q.real; 105 this->dual = q.dual; 106 return *this; 107 } 108# endif//!GLM_HAS_DEFAULTED_FUNCTIONS 109 110 template <typename T, precision P> 111 template <typename U> 112 GLM_FUNC_QUALIFIER tdualquat<T, P> & tdualquat<T, P>::operator=(tdualquat<U, P> const & q) 113 { 114 this->real = q.real; 115 this->dual = q.dual; 116 return *this; 117 } 118 119 template <typename T, precision P> 120 template <typename U> 121 GLM_FUNC_QUALIFIER tdualquat<T, P> & tdualquat<T, P>::operator*=(U s) 122 { 123 this->real *= static_cast<T>(s); 124 this->dual *= static_cast<T>(s); 125 return *this; 126 } 127 128 template <typename T, precision P> 129 template <typename U> 130 GLM_FUNC_QUALIFIER tdualquat<T, P> & tdualquat<T, P>::operator/=(U s) 131 { 132 this->real /= static_cast<T>(s); 133 this->dual /= static_cast<T>(s); 134 return *this; 135 } 136 137 // -- Unary bit operators -- 138 139 template <typename T, precision P> 140 GLM_FUNC_QUALIFIER tdualquat<T, P> operator+(tdualquat<T, P> const & q) 141 { 142 return q; 143 } 144 145 template <typename T, precision P> 146 GLM_FUNC_QUALIFIER tdualquat<T, P> operator-(tdualquat<T, P> const & q) 147 { 148 return tdualquat<T, P>(-q.real, -q.dual); 149 } 150 151 // -- Binary operators -- 152 153 template <typename T, precision P> 154 GLM_FUNC_QUALIFIER tdualquat<T, P> operator+(tdualquat<T, P> const & q, tdualquat<T, P> const & p) 155 { 156 return tdualquat<T, P>(q.real + p.real,q.dual + p.dual); 157 } 158 159 template <typename T, precision P> 160 GLM_FUNC_QUALIFIER tdualquat<T, P> operator*(tdualquat<T, P> const & p, tdualquat<T, P> const & o) 161 { 162 return tdualquat<T, P>(p.real * o.real,p.real * o.dual + p.dual * o.real); 163 } 164 165 template <typename T, precision P> 166 GLM_FUNC_QUALIFIER tvec3<T, P> operator*(tdualquat<T, P> const & q, tvec3<T, P> const & v) 167 { 168 tvec3<T, P> const real_v3(q.real.x,q.real.y,q.real.z); 169 tvec3<T, P> const dual_v3(q.dual.x,q.dual.y,q.dual.z); 170 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; 171 } 172 173 template <typename T, precision P> 174 GLM_FUNC_QUALIFIER tvec3<T, P> operator*(tvec3<T, P> const & v, tdualquat<T, P> const & q) 175 { 176 return glm::inverse(q) * v; 177 } 178 179 template <typename T, precision P> 180 GLM_FUNC_QUALIFIER tvec4<T, P> operator*(tdualquat<T, P> const & q, tvec4<T, P> const & v) 181 { 182 return tvec4<T, P>(q * tvec3<T, P>(v), v.w); 183 } 184 185 template <typename T, precision P> 186 GLM_FUNC_QUALIFIER tvec4<T, P> operator*(tvec4<T, P> const & v, tdualquat<T, P> const & q) 187 { 188 return glm::inverse(q) * v; 189 } 190 191 template <typename T, precision P> 192 GLM_FUNC_QUALIFIER tdualquat<T, P> operator*(tdualquat<T, P> const & q, T const & s) 193 { 194 return tdualquat<T, P>(q.real * s, q.dual * s); 195 } 196 197 template <typename T, precision P> 198 GLM_FUNC_QUALIFIER tdualquat<T, P> operator*(T const & s, tdualquat<T, P> const & q) 199 { 200 return q * s; 201 } 202 203 template <typename T, precision P> 204 GLM_FUNC_QUALIFIER tdualquat<T, P> operator/(tdualquat<T, P> const & q, T const & s) 205 { 206 return tdualquat<T, P>(q.real / s, q.dual / s); 207 } 208 209 // -- Boolean operators -- 210 211 template <typename T, precision P> 212 GLM_FUNC_QUALIFIER bool operator==(tdualquat<T, P> const & q1, tdualquat<T, P> const & q2) 213 { 214 return (q1.real == q2.real) && (q1.dual == q2.dual); 215 } 216 217 template <typename T, precision P> 218 GLM_FUNC_QUALIFIER bool operator!=(tdualquat<T, P> const & q1, tdualquat<T, P> const & q2) 219 { 220 return (q1.real != q2.dual) || (q1.real != q2.dual); 221 } 222 223 // -- Operations -- 224 225 template <typename T, precision P> 226 GLM_FUNC_QUALIFIER tdualquat<T, P> normalize(tdualquat<T, P> const & q) 227 { 228 return q / length(q.real); 229 } 230 231 template <typename T, precision P> 232 GLM_FUNC_QUALIFIER tdualquat<T, P> lerp(tdualquat<T, P> const & x, tdualquat<T, P> const & y, T const & a) 233 { 234 // Dual Quaternion Linear blend aka DLB: 235 // Lerp is only defined in [0, 1] 236 assert(a >= static_cast<T>(0)); 237 assert(a <= static_cast<T>(1)); 238 T const k = dot(x.real,y.real) < static_cast<T>(0) ? -a : a; 239 T const one(1); 240 return tdualquat<T, P>(x * (one - a) + y * k); 241 } 242 243 template <typename T, precision P> 244 GLM_FUNC_QUALIFIER tdualquat<T, P> inverse(tdualquat<T, P> const & q) 245 { 246 const glm::tquat<T, P> real = conjugate(q.real); 247 const glm::tquat<T, P> dual = conjugate(q.dual); 248 return tdualquat<T, P>(real, dual + (real * (-2.0f * dot(real,dual)))); 249 } 250 251 template <typename T, precision P> 252 GLM_FUNC_QUALIFIER tmat2x4<T, P> mat2x4_cast(tdualquat<T, P> const & x) 253 { 254 return 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 ); 255 } 256 257 template <typename T, precision P> 258 GLM_FUNC_QUALIFIER tmat3x4<T, P> mat3x4_cast(tdualquat<T, P> const & x) 259 { 260 tquat<T, P> r = x.real / length2(x.real); 261 262 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); 263 r *= static_cast<T>(2); 264 265 T const xy = r.x * x.real.y; 266 T const xz = r.x * x.real.z; 267 T const yz = r.y * x.real.z; 268 T const wx = r.w * x.real.x; 269 T const wy = r.w * x.real.y; 270 T const wz = r.w * x.real.z; 271 272 tvec4<T, P> const a( 273 rr.w + rr.x - rr.y - rr.z, 274 xy - wz, 275 xz + wy, 276 -(x.dual.w * r.x - x.dual.x * r.w + x.dual.y * r.z - x.dual.z * r.y)); 277 278 tvec4<T, P> const b( 279 xy + wz, 280 rr.w + rr.y - rr.x - rr.z, 281 yz - wx, 282 -(x.dual.w * r.y - x.dual.x * r.z - x.dual.y * r.w + x.dual.z * r.x)); 283 284 tvec4<T, P> const c( 285 xz - wy, 286 yz + wx, 287 rr.w + rr.z - rr.x - rr.y, 288 -(x.dual.w * r.z + x.dual.x * r.y - x.dual.y * r.x - x.dual.z * r.w)); 289 290 return tmat3x4<T, P>(a, b, c); 291 } 292 293 template <typename T, precision P> 294 GLM_FUNC_QUALIFIER tdualquat<T, P> dualquat_cast(tmat2x4<T, P> const & x) 295 { 296 return tdualquat<T, P>( 297 tquat<T, P>( x[0].w, x[0].x, x[0].y, x[0].z ), 298 tquat<T, P>( x[1].w, x[1].x, x[1].y, x[1].z )); 299 } 300 301 template <typename T, precision P> 302 GLM_FUNC_QUALIFIER tdualquat<T, P> dualquat_cast(tmat3x4<T, P> const & x) 303 { 304 tquat<T, P> real(uninitialize); 305 306 T const trace = x[0].x + x[1].y + x[2].z; 307 if(trace > static_cast<T>(0)) 308 { 309 T const r = sqrt(T(1) + trace); 310 T const invr = static_cast<T>(0.5) / r; 311 real.w = static_cast<T>(0.5) * r; 312 real.x = (x[2].y - x[1].z) * invr; 313 real.y = (x[0].z - x[2].x) * invr; 314 real.z = (x[1].x - x[0].y) * invr; 315 } 316 else if(x[0].x > x[1].y && x[0].x > x[2].z) 317 { 318 T const r = sqrt(T(1) + x[0].x - x[1].y - x[2].z); 319 T const invr = static_cast<T>(0.5) / r; 320 real.x = static_cast<T>(0.5)*r; 321 real.y = (x[1].x + x[0].y) * invr; 322 real.z = (x[0].z + x[2].x) * invr; 323 real.w = (x[2].y - x[1].z) * invr; 324 } 325 else if(x[1].y > x[2].z) 326 { 327 T const r = sqrt(T(1) + x[1].y - x[0].x - x[2].z); 328 T const invr = static_cast<T>(0.5) / r; 329 real.x = (x[1].x + x[0].y) * invr; 330 real.y = static_cast<T>(0.5) * r; 331 real.z = (x[2].y + x[1].z) * invr; 332 real.w = (x[0].z - x[2].x) * invr; 333 } 334 else 335 { 336 T const r = sqrt(T(1) + x[2].z - x[0].x - x[1].y); 337 T const invr = static_cast<T>(0.5) / r; 338 real.x = (x[0].z + x[2].x) * invr; 339 real.y = (x[2].y + x[1].z) * invr; 340 real.z = static_cast<T>(0.5) * r; 341 real.w = (x[1].x - x[0].y) * invr; 342 } 343 344 tquat<T, P> dual(uninitialize); 345 dual.x = static_cast<T>(0.5) * ( x[0].w * real.w + x[1].w * real.z - x[2].w * real.y); 346 dual.y = static_cast<T>(0.5) * (-x[0].w * real.z + x[1].w * real.w + x[2].w * real.x); 347 dual.z = static_cast<T>(0.5) * ( x[0].w * real.y - x[1].w * real.x + x[2].w * real.w); 348 dual.w = -static_cast<T>(0.5) * ( x[0].w * real.x + x[1].w * real.y + x[2].w * real.z); 349 return tdualquat<T, P>(real, dual); 350 } 351}//namespace glm 352