1 /////////////////////////////////////////////////////////////////////////////////// 2 /// OpenGL Mathematics (glm.g-truc.net) 3 /// 4 /// Copyright (c) 2005 - 2014 G-Truc Creation (www.g-truc.net) 5 /// Permission is hereby granted, free of charge, to any person obtaining a copy 6 /// of this software and associated documentation files (the "Software"), to deal 7 /// in the Software without restriction, including without limitation the rights 8 /// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 /// copies of the Software, and to permit persons to whom the Software is 10 /// furnished to do so, subject to the following conditions: 11 /// 12 /// The above copyright notice and this permission notice shall be included in 13 /// all copies or substantial portions of the Software. 14 /// 15 /// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 /// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 /// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 /// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 /// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 /// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 /// THE SOFTWARE. 22 /// 23 /// @ref gtx_dual_quaternion 24 /// @file glm/gtx/dual_quaternion.inl 25 /// @date 2013-02-10 / 2013-02-13 26 /// @author Maksim Vorobiev (msomeone (a] gmail.com) 27 /////////////////////////////////////////////////////////////////////////////////// 28 29 #include "../geometric.hpp" 30 #include <limits> 31 32 namespace glm{ 33 namespace detail 34 { 35 template <typename T, precision P> 36 GLM_FUNC_QUALIFIER GLM_CONSTEXPR int tdualquat<T, P>::length() const 37 { 38 return 8; 39 } 40 41 template <typename T, precision P> 42 GLM_FUNC_QUALIFIER tdualquat<T, P>::tdualquat() : 43 real(tquat<T, P>()), 44 dual(tquat<T, P>(T(0), T(0), T(0), T(0))) 45 {} 46 47 template <typename T, precision P> 48 GLM_FUNC_QUALIFIER tdualquat<T, P>::tdualquat 49 ( 50 tquat<T, P> const & r 51 ) : 52 real(r), 53 dual(tquat<T, P>(T(0), T(0), T(0), T(0))) 54 {} 55 56 template <typename T, precision P> 57 GLM_FUNC_QUALIFIER tdualquat<T, P>::tdualquat 58 ( 59 tquat<T, P> const & r, 60 tquat<T, P> const & d 61 ) : 62 real(r), 63 dual(d) 64 {} 65 66 template <typename T, precision P> 67 GLM_FUNC_QUALIFIER tdualquat<T, P>::tdualquat 68 ( 69 tquat<T, P> const & q, 70 tvec3<T, P> const& p 71 ) : 72 real(q), 73 dual( 74 T(-0.5) * ( p.x*q.x + p.y*q.y + p.z*q.z), 75 T(+0.5) * ( p.x*q.w + p.y*q.z - p.z*q.y), 76 T(+0.5) * (-p.x*q.z + p.y*q.w + p.z*q.x), 77 T(+0.5) * ( p.x*q.y - p.y*q.x + p.z*q.w)) 78 {} 79 80 ////////////////////////////////////////////////////////////// 81 // tdualquat conversions 82 template <typename T, precision P> 83 GLM_FUNC_QUALIFIER tdualquat<T, P>::tdualquat 84 ( 85 tmat2x4<T, P> const & m 86 ) 87 { 88 *this = dualquat_cast(m); 89 } 90 91 template <typename T, precision P> 92 GLM_FUNC_QUALIFIER tdualquat<T, P>::tdualquat 93 ( 94 tmat3x4<T, P> const & m 95 ) 96 { 97 *this = dualquat_cast(m); 98 } 99 100 ////////////////////////////////////////////////////////////// 101 // tdualquat<T, P> accesses 102 103 template <typename T, precision P> 104 GLM_FUNC_QUALIFIER typename tdualquat<T, P>::part_type & tdualquat<T, P>::operator [] (int i) 105 { 106 assert(i >= 0 && i < this->length()); 107 return (&real)[i]; 108 } 109 110 template <typename T, precision P> 111 GLM_FUNC_QUALIFIER typename tdualquat<T, P>::part_type const & tdualquat<T, P>::operator [] (int i) const 112 { 113 assert(i >= 0 && i < this->length()); 114 return (&real)[i]; 115 } 116 117 ////////////////////////////////////////////////////////////// 118 // tdualquat<valType> operators 119 120 template <typename T, precision P> 121 GLM_FUNC_QUALIFIER tdualquat<T, P> & tdualquat<T, P>::operator *= 122 ( 123 T const & s 124 ) 125 { 126 this->real *= s; 127 this->dual *= s; 128 return *this; 129 } 130 131 template <typename T, precision P> 132 GLM_FUNC_QUALIFIER tdualquat<T, P> & tdualquat<T, P>::operator /= 133 ( 134 T const & s 135 ) 136 { 137 this->real /= s; 138 this->dual /= s; 139 return *this; 140 } 141 142 ////////////////////////////////////////////////////////////// 143 // tquat<valType> external operators 144 145 template <typename T, precision P> 146 GLM_FUNC_QUALIFIER detail::tdualquat<T, P> operator- 147 ( 148 detail::tdualquat<T, P> const & q 149 ) 150 { 151 return detail::tdualquat<T, P>(-q.real,-q.dual); 152 } 153 154 template <typename T, precision P> 155 GLM_FUNC_QUALIFIER detail::tdualquat<T, P> operator+ 156 ( 157 detail::tdualquat<T, P> const & q, 158 detail::tdualquat<T, P> const & p 159 ) 160 { 161 return detail::tdualquat<T, P>(q.real + p.real,q.dual + p.dual); 162 } 163 164 template <typename T, precision P> 165 GLM_FUNC_QUALIFIER detail::tdualquat<T, P> operator* 166 ( 167 detail::tdualquat<T, P> const & p, 168 detail::tdualquat<T, P> const & o 169 ) 170 { 171 return detail::tdualquat<T, P>(p.real * o.real,p.real * o.dual + p.dual * o.real); 172 } 173 174 // Transformation 175 template <typename T, precision P> 176 GLM_FUNC_QUALIFIER detail::tvec3<T, P> operator* 177 ( 178 detail::tdualquat<T, P> const & q, 179 detail::tvec3<T, P> const & v 180 ) 181 { 182 detail::tvec3<T, P> const real_v3(q.real.x,q.real.y,q.real.z); 183 detail::tvec3<T, P> const dual_v3(q.dual.x,q.dual.y,q.dual.z); 184 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; 185 } 186 187 template <typename T, precision P> 188 GLM_FUNC_QUALIFIER detail::tvec3<T, P> operator* 189 ( 190 detail::tvec3<T, P> const & v, 191 detail::tdualquat<T, P> const & q 192 ) 193 { 194 return glm::inverse(q) * v; 195 } 196 197 template <typename T, precision P> 198 GLM_FUNC_QUALIFIER detail::tvec4<T, P> operator* 199 ( 200 detail::tdualquat<T, P> const & q, 201 detail::tvec4<T, P> const & v 202 ) 203 { 204 return detail::tvec4<T, P>(q * detail::tvec3<T, P>(v), v.w); 205 } 206 207 template <typename T, precision P> 208 GLM_FUNC_QUALIFIER detail::tvec4<T, P> operator* 209 ( 210 detail::tvec4<T, P> const & v, 211 detail::tdualquat<T, P> const & q 212 ) 213 { 214 return glm::inverse(q) * v; 215 } 216 217 template <typename T, precision P> 218 GLM_FUNC_QUALIFIER detail::tdualquat<T, P> operator* 219 ( 220 detail::tdualquat<T, P> const & q, 221 T const & s 222 ) 223 { 224 return detail::tdualquat<T, P>(q.real * s, q.dual * s); 225 } 226 227 template <typename T, precision P> 228 GLM_FUNC_QUALIFIER detail::tdualquat<T, P> operator* 229 ( 230 T const & s, 231 detail::tdualquat<T, P> const & q 232 ) 233 { 234 return q * s; 235 } 236 237 template <typename T, precision P> 238 GLM_FUNC_QUALIFIER detail::tdualquat<T, P> operator/ 239 ( 240 detail::tdualquat<T, P> const & q, 241 T const & s 242 ) 243 { 244 return detail::tdualquat<T, P>(q.real / s, q.dual / s); 245 } 246 247 ////////////////////////////////////// 248 // Boolean operators 249 template <typename T, precision P> 250 GLM_FUNC_QUALIFIER bool operator== 251 ( 252 detail::tdualquat<T, P> const & q1, 253 detail::tdualquat<T, P> const & q2 254 ) 255 { 256 return (q1.real == q2.real) && (q1.dual == q2.dual); 257 } 258 259 template <typename T, precision P> 260 GLM_FUNC_QUALIFIER bool operator!= 261 ( 262 detail::tdualquat<T, P> const & q1, 263 detail::tdualquat<T, P> const & q2 264 ) 265 { 266 return (q1.real != q2.dual) || (q1.real != q2.dual); 267 } 268 }//namespace detail 269 270 //////////////////////////////////////////////////////// 271 template <typename T, precision P> 272 GLM_FUNC_QUALIFIER detail::tdualquat<T, P> normalize 273 ( 274 detail::tdualquat<T, P> const & q 275 ) 276 { 277 return q / length(q.real); 278 } 279 280 template <typename T, precision P> 281 GLM_FUNC_QUALIFIER detail::tdualquat<T, P> lerp 282 ( 283 detail::tdualquat<T, P> const & x, 284 detail::tdualquat<T, P> const & y, 285 T const & a 286 ) 287 { 288 // Dual Quaternion Linear blend aka DLB: 289 // Lerp is only defined in [0, 1] 290 assert(a >= static_cast<T>(0)); 291 assert(a <= static_cast<T>(1)); 292 T const k = dot(x.real,y.real) < static_cast<T>(0) ? -a : a; 293 T const one(1); 294 return detail::tdualquat<T, P>(x * (one - a) + y * k); 295 } 296 297 template <typename T, precision P> 298 GLM_FUNC_QUALIFIER detail::tdualquat<T, P> inverse 299 ( 300 detail::tdualquat<T, P> const & q 301 ) 302 { 303 const glm::detail::tquat<T, P> real = conjugate(q.real); 304 const glm::detail::tquat<T, P> dual = conjugate(q.dual); 305 return detail::tdualquat<T, P>(real, dual + (real * (-2.0f * dot(real,dual)))); 306 } 307 308 template <typename T, precision P> 309 GLM_FUNC_QUALIFIER detail::tmat2x4<T, P> mat2x4_cast 310 ( 311 detail::tdualquat<T, P> const & x 312 ) 313 { 314 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 ); 315 } 316 317 template <typename T, precision P> 318 GLM_FUNC_QUALIFIER detail::tmat3x4<T, P> mat3x4_cast 319 ( 320 detail::tdualquat<T, P> const & x 321 ) 322 { 323 detail::tquat<T, P> r = x.real / length2(x.real); 324 325 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); 326 r *= static_cast<T>(2); 327 328 T const xy = r.x * x.real.y; 329 T const xz = r.x * x.real.z; 330 T const yz = r.y * x.real.z; 331 T const wx = r.w * x.real.x; 332 T const wy = r.w * x.real.y; 333 T const wz = r.w * x.real.z; 334 335 detail::tvec4<T, P> const a( 336 rr.w + rr.x - rr.y - rr.z, 337 xy - wz, 338 xz + wy, 339 -(x.dual.w * r.x - x.dual.x * r.w + x.dual.y * r.z - x.dual.z * r.y)); 340 341 detail::tvec4<T, P> const b( 342 xy + wz, 343 rr.w + rr.y - rr.x - rr.z, 344 yz - wx, 345 -(x.dual.w * r.y - x.dual.x * r.z - x.dual.y * r.w + x.dual.z * r.x)); 346 347 detail::tvec4<T, P> const c( 348 xz - wy, 349 yz + wx, 350 rr.w + rr.z - rr.x - rr.y, 351 -(x.dual.w * r.z + x.dual.x * r.y - x.dual.y * r.x - x.dual.z * r.w)); 352 353 return detail::tmat3x4<T, P>(a, b, c); 354 } 355 356 template <typename T, precision P> 357 GLM_FUNC_QUALIFIER detail::tdualquat<T, P> dualquat_cast 358 ( 359 detail::tmat2x4<T, P> const & x 360 ) 361 { 362 return detail::tdualquat<T, P>( 363 detail::tquat<T, P>( x[0].w, x[0].x, x[0].y, x[0].z ), 364 detail::tquat<T, P>( x[1].w, x[1].x, x[1].y, x[1].z )); 365 } 366 367 template <typename T, precision P> 368 GLM_FUNC_QUALIFIER detail::tdualquat<T, P> dualquat_cast 369 ( 370 detail::tmat3x4<T, P> const & x 371 ) 372 { 373 detail::tquat<T, P> real; 374 375 T const trace = x[0].x + x[1].y + x[2].z; 376 if(trace > T(0)) 377 { 378 T const r = sqrt(T(1) + trace); 379 T const invr = static_cast<T>(0.5) / r; 380 real.w = static_cast<T>(0.5) * r; 381 real.x = (x[2].y - x[1].z) * invr; 382 real.y = (x[0].z - x[2].x) * invr; 383 real.z = (x[1].x - x[0].y) * invr; 384 } 385 else if(x[0].x > x[1].y && x[0].x > x[2].z) 386 { 387 T const r = sqrt(T(1) + x[0].x - x[1].y - x[2].z); 388 T const invr = static_cast<T>(0.5) / r; 389 real.x = static_cast<T>(0.5)*r; 390 real.y = (x[1].x + x[0].y) * invr; 391 real.z = (x[0].z + x[2].x) * invr; 392 real.w = (x[2].y - x[1].z) * invr; 393 } 394 else if(x[1].y > x[2].z) 395 { 396 T const r = sqrt(T(1) + x[1].y - x[0].x - x[2].z); 397 T const invr = static_cast<T>(0.5) / r; 398 real.x = (x[1].x + x[0].y) * invr; 399 real.y = static_cast<T>(0.5) * r; 400 real.z = (x[2].y + x[1].z) * invr; 401 real.w = (x[0].z - x[2].x) * invr; 402 } 403 else 404 { 405 T const r = sqrt(T(1) + x[2].z - x[0].x - x[1].y); 406 T const invr = static_cast<T>(0.5) / r; 407 real.x = (x[0].z + x[2].x) * invr; 408 real.y = (x[2].y + x[1].z) * invr; 409 real.z = static_cast<T>(0.5) * r; 410 real.w = (x[1].x - x[0].y) * invr; 411 } 412 413 detail::tquat<T, P> dual; 414 dual.x = T(0.5) * ( x[0].w * real.w + x[1].w * real.z - x[2].w * real.y); 415 dual.y = T(0.5) * (-x[0].w * real.z + x[1].w * real.w + x[2].w * real.x); 416 dual.z = T(0.5) * ( x[0].w * real.y - x[1].w * real.x + x[2].w * real.w); 417 dual.w = -T(0.5) * ( x[0].w * real.x + x[1].w * real.y + x[2].w * real.z); 418 return detail::tdualquat<T, P>(real, dual); 419 } 420 421 }//namespace glm 422
