/demo/glm/gtx/dual_quaternion.inl

https://gitlab.com/gazdik/FTGL3 · C++ Header · 421 lines · 337 code · 43 blank · 41 comment · 14 complexity · 414c4bc33dde037cab792eccc4494ce0 MD5 · raw file 

    

  1. ///////////////////////////////////////////////////////////////////////////////////

    2. 

  2. /// OpenGL Mathematics (glm.g-truc.net)

    3. 

  3. ///

    4. 

  4. /// Copyright (c) 2005 - 2014 G-Truc Creation (www.g-truc.net)

    5. 

  5. /// Permission is hereby granted, free of charge, to any person obtaining a copy

    6. 

  6. /// of this software and associated documentation files (the "Software"), to deal

    7. 

  7. /// in the Software without restriction, including without limitation the rights

    8. 

  8. /// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell

    9. 

  9. /// copies of the Software, and to permit persons to whom the Software is

    10. 

  10. /// furnished to do so, subject to the following conditions:

    11. 

  11. /// 

    12. 

  12. /// The above copyright notice and this permission notice shall be included in

    13. 

  13. /// all copies or substantial portions of the Software.

    14. 

  14. /// 

    15. 

  15. /// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR

    16. 

  16. /// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,

    17. 

  17. /// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE

    18. 

  18. /// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER

    19. 

  19. /// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,

    20. 

  20. /// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN

    21. 

  21. /// THE SOFTWARE.

    22. 

  22. ///

    23. 

  23. /// @ref gtx_dual_quaternion

    24. 

  24. /// @file glm/gtx/dual_quaternion.inl

    25. 

  25. /// @date 2013-02-10 / 2013-02-13

    26. 

  26. /// @author Maksim Vorobiev (msomeone@gmail.com)

    27. 

  27. ///////////////////////////////////////////////////////////////////////////////////

    28. 

  28. 

    29. 

  29. #include "../geometric.hpp"

    30. 

  30. #include <limits>

    31. 

  31. 

    32. 

  32. namespace glm{

    33. 

  33. namespace detail

    34. 

  34. {

    35. 

  35. 	template <typename T, precision P>

    36. 

  36. 	GLM_FUNC_QUALIFIER GLM_CONSTEXPR int tdualquat<T, P>::length() const

    37. 

  37. 	{

    38. 

  38. 		return 8;

    39. 

  39. 	}

    40. 

  40. 

    41. 

  41. 	template <typename T, precision P>

    42. 

  42. 	GLM_FUNC_QUALIFIER tdualquat<T, P>::tdualquat() :

    43. 

  43. 		real(tquat<T, P>()),

    44. 

  44. 		dual(tquat<T, P>(T(0), T(0), T(0), T(0)))

    45. 

  45. 	{}

    46. 

  46. 

    47. 

  47. 	template <typename T, precision P>

    48. 

  48. 	GLM_FUNC_QUALIFIER tdualquat<T, P>::tdualquat

    49. 

  49. 	(

    50. 

  50. 		tquat<T, P> const & r

    51. 

  51. 	) :

    52. 

  52. 		real(r),

    53. 

  53. 		dual(tquat<T, P>(T(0), T(0), T(0), T(0)))

    54. 

  54. 	{}

    55. 

  55. 

    56. 

  56. 	template <typename T, precision P>

    57. 

  57. 	GLM_FUNC_QUALIFIER tdualquat<T, P>::tdualquat

    58. 

  58. 	(

    59. 

  59. 		tquat<T, P> const & r,

    60. 

  60. 		tquat<T, P> const & d

    61. 

  61. 	) :

    62. 

  62. 		real(r),

    63. 

  63. 		dual(d)

    64. 

  64. 	{}

    65. 

  65. 

    66. 

  66. 	template <typename T, precision P>

    67. 

  67. 	GLM_FUNC_QUALIFIER tdualquat<T, P>::tdualquat

    68. 

  68. 	(

    69. 

  69. 		tquat<T, P> const & q,

    70. 

  70. 		tvec3<T, P> const& p

    71. 

  71. 	) :

    72. 

  72. 		real(q),

    73. 

  73. 		dual(

    74. 

  74. 			T(-0.5) * ( p.x*q.x + p.y*q.y + p.z*q.z),

    75. 

  75. 			T(+0.5) * ( p.x*q.w + p.y*q.z - p.z*q.y),

    76. 

  76. 			T(+0.5) * (-p.x*q.z + p.y*q.w + p.z*q.x),

    77. 

  77. 			T(+0.5) * ( p.x*q.y - p.y*q.x + p.z*q.w))

    78. 

  78. 	{}

    79. 

  79. 

    80. 

  80. 	//////////////////////////////////////////////////////////////

    81. 

  81. 	// tdualquat conversions

    82. 

  82. 	template <typename T, precision P>

    83. 

  83. 	GLM_FUNC_QUALIFIER tdualquat<T, P>::tdualquat

    84. 

  84. 	(

    85. 

  85. 		tmat2x4<T, P> const & m

    86. 

  86. 	)

    87. 

  87. 	{

    88. 

  88. 		*this = dualquat_cast(m);

    89. 

  89. 	}

    90. 

  90. 

    91. 

  91. 	template <typename T, precision P>

    92. 

  92. 	GLM_FUNC_QUALIFIER tdualquat<T, P>::tdualquat

    93. 

  93. 	(

    94. 

  94. 		tmat3x4<T, P> const & m

    95. 

  95. 	)

    96. 

  96. 	{

    97. 

  97. 		*this = dualquat_cast(m);

    98. 

  98. 	}

    99. 

  99. 

    100. 

  100. 	//////////////////////////////////////////////////////////////

    101. 

  101. 	// tdualquat<T, P> accesses

    102. 

  102. 

    103. 

  103. 	template <typename T, precision P>

    104. 

  104. 	GLM_FUNC_QUALIFIER typename tdualquat<T, P>::part_type & tdualquat<T, P>::operator [] (int i)

    105. 

  105. 	{

    106. 

  106. 		assert(i >= 0 && i < this->length());

    107. 

  107. 		return (&real)[i];

    108. 

  108. 	}

    109. 

  109. 

    110. 

  110. 	template <typename T, precision P>

    111. 

  111. 	GLM_FUNC_QUALIFIER typename tdualquat<T, P>::part_type const & tdualquat<T, P>::operator [] (int i) const

    112. 

  112. 	{

    113. 

  113. 		assert(i >= 0 && i < this->length());

    114. 

  114. 		return (&real)[i];

    115. 

  115. 	}

    116. 

  116. 

    117. 

  117. 	//////////////////////////////////////////////////////////////

    118. 

  118. 	// tdualquat<valType> operators

    119. 

  119. 

    120. 

  120. 	template <typename T, precision P>

    121. 

  121. 	GLM_FUNC_QUALIFIER tdualquat<T, P> & tdualquat<T, P>::operator *=

    122. 

  122. 	(

    123. 

  123. 		T const & s

    124. 

  124. 	)

    125. 

  125. 	{

    126. 

  126. 		this->real *= s;

    127. 

  127. 		this->dual *= s;

    128. 

  128. 		return *this;

    129. 

  129. 	}

    130. 

  130. 

    131. 

  131. 	template <typename T, precision P>

    132. 

  132. 	GLM_FUNC_QUALIFIER tdualquat<T, P> & tdualquat<T, P>::operator /=

    133. 

  133. 	(

    134. 

  134. 		T const & s

    135. 

  135. 	)

    136. 

  136. 	{

    137. 

  137. 		this->real /= s;

    138. 

  138. 		this->dual /= s;

    139. 

  139. 		return *this;

    140. 

  140. 	}

    141. 

  141. 

    142. 

  142. 	//////////////////////////////////////////////////////////////

    143. 

  143. 	// tquat<valType> external operators

    144. 

  144. 

    145. 

  145. 	template <typename T, precision P>

    146. 

  146. 	GLM_FUNC_QUALIFIER detail::tdualquat<T, P> operator-

    147. 

  147. 	(

    148. 

  148. 		detail::tdualquat<T, P> const & q

    149. 

  149. 	)

    150. 

  150. 	{

    151. 

  151. 		return detail::tdualquat<T, P>(-q.real,-q.dual);

    152. 

  152. 	}

    153. 

  153. 

    154. 

  154. 	template <typename T, precision P>

    155. 

  155. 	GLM_FUNC_QUALIFIER detail::tdualquat<T, P> operator+

    156. 

  156. 	(

    157. 

  157. 		detail::tdualquat<T, P> const & q,

    158. 

  158. 		detail::tdualquat<T, P> const & p

    159. 

  159. 	)

    160. 

  160. 	{

    161. 

  161. 		return detail::tdualquat<T, P>(q.real + p.real,q.dual + p.dual);

    162. 

  162. 	}

    163. 

  163. 

    164. 

  164. 	template <typename T, precision P>

    165. 

  165. 	GLM_FUNC_QUALIFIER detail::tdualquat<T, P> operator*

    166. 

  166. 	(

    167. 

  167. 		detail::tdualquat<T, P> const & p,

    168. 

  168. 		detail::tdualquat<T, P> const & o

    169. 

  169. 	)

    170. 

  170. 	{

    171. 

  171. 		return detail::tdualquat<T, P>(p.real * o.real,p.real * o.dual + p.dual * o.real);

    172. 

  172. 	}

    173. 

  173. 

    174. 

  174. 	// Transformation

    175. 

  175. 	template <typename T, precision P>

    176. 

  176. 	GLM_FUNC_QUALIFIER detail::tvec3<T, P> operator*

    177. 

  177. 	(

    178. 

  178. 		detail::tdualquat<T, P> const & q,

    179. 

  179. 		detail::tvec3<T, P> const & v

    180. 

  180. 	)

    181. 

  181. 	{

    182. 

  182. 		detail::tvec3<T, P> const real_v3(q.real.x,q.real.y,q.real.z);

    183. 

  183. 		detail::tvec3<T, P> const dual_v3(q.dual.x,q.dual.y,q.dual.z);

    184. 

  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. 

  185. 	}

    186. 

  186. 

    187. 

  187. 	template <typename T, precision P>

    188. 

  188. 	GLM_FUNC_QUALIFIER detail::tvec3<T, P> operator*

    189. 

  189. 	(

    190. 

  190. 		detail::tvec3<T, P> const & v,

    191. 

  191. 		detail::tdualquat<T, P> const & q

    192. 

  192. 	)

    193. 

  193. 	{

    194. 

  194. 		return glm::inverse(q) * v;

    195. 

  195. 	}

    196. 

  196. 

    197. 

  197. 	template <typename T, precision P>

    198. 

  198. 	GLM_FUNC_QUALIFIER detail::tvec4<T, P> operator*

    199. 

  199. 	(

    200. 

  200. 		detail::tdualquat<T, P> const & q,

    201. 

  201. 		detail::tvec4<T, P> const & v

    202. 

  202. 	)

    203. 

  203. 	{

    204. 

  204. 		return detail::tvec4<T, P>(q * detail::tvec3<T, P>(v), v.w);

    205. 

  205. 	}

    206. 

  206. 

    207. 

  207. 	template <typename T, precision P>

    208. 

  208. 	GLM_FUNC_QUALIFIER detail::tvec4<T, P> operator*

    209. 

  209. 	(

    210. 

  210. 		detail::tvec4<T, P> const & v,

    211. 

  211. 		detail::tdualquat<T, P> const & q

    212. 

  212. 	)

    213. 

  213. 	{

    214. 

  214. 		return glm::inverse(q) * v;

    215. 

  215. 	}

    216. 

  216. 

    217. 

  217. 	template <typename T, precision P>

    218. 

  218. 	GLM_FUNC_QUALIFIER detail::tdualquat<T, P> operator*

    219. 

  219. 	(

    220. 

  220. 		detail::tdualquat<T, P> const & q,

    221. 

  221. 		T const & s

    222. 

  222. 	)

    223. 

  223. 	{

    224. 

  224. 		return detail::tdualquat<T, P>(q.real * s, q.dual * s);

    225. 

  225. 	}

    226. 

  226. 

    227. 

  227. 	template <typename T, precision P>

    228. 

  228. 	GLM_FUNC_QUALIFIER detail::tdualquat<T, P> operator*

    229. 

  229. 	(

    230. 

  230. 		T const & s,

    231. 

  231. 		detail::tdualquat<T, P> const & q

    232. 

  232. 	)

    233. 

  233. 	{

    234. 

  234. 		return q * s;

    235. 

  235. 	}

    236. 

  236. 

    237. 

  237. 	template <typename T, precision P>

    238. 

  238. 	GLM_FUNC_QUALIFIER detail::tdualquat<T, P> operator/

    239. 

  239. 	(

    240. 

  240. 		detail::tdualquat<T, P> const & q,

    241. 

  241. 		T const & s

    242. 

  242. 	)

    243. 

  243. 	{

    244. 

  244. 		return detail::tdualquat<T, P>(q.real / s, q.dual / s);

    245. 

  245. 	}

    246. 

  246. 

    247. 

  247. 	//////////////////////////////////////

    248. 

  248. 	// Boolean operators

    249. 

  249. 	template <typename T, precision P>

    250. 

  250. 	GLM_FUNC_QUALIFIER bool operator==

    251. 

  251. 	(

    252. 

  252. 		detail::tdualquat<T, P> const & q1,

    253. 

  253. 		detail::tdualquat<T, P> const & q2

    254. 

  254. 	)

    255. 

  255. 	{

    256. 

  256. 		return (q1.real == q2.real) && (q1.dual == q2.dual);

    257. 

  257. 	}

    258. 

  258. 

    259. 

  259. 	template <typename T, precision P>

    260. 

  260. 	GLM_FUNC_QUALIFIER bool operator!=

    261. 

  261. 	(

    262. 

  262. 		detail::tdualquat<T, P> const & q1,

    263. 

  263. 		detail::tdualquat<T, P> const & q2

    264. 

  264. 	)

    265. 

  265. 	{

    266. 

  266. 		return (q1.real != q2.dual) || (q1.real != q2.dual);

    267. 

  267. 	}

    268. 

  268. 	}//namespace detail

    269. 

  269. 

    270. 

  270. 	////////////////////////////////////////////////////////

    271. 

  271. 	template <typename T, precision P>

    272. 

  272. 	GLM_FUNC_QUALIFIER detail::tdualquat<T, P> normalize

    273. 

  273. 	(

    274. 

  274. 		detail::tdualquat<T, P> const & q

    275. 

  275. 	)

    276. 

  276. 	{

    277. 

  277. 		return q / length(q.real);

    278. 

  278. 	}

    279. 

  279. 

    280. 

  280. 	template <typename T, precision P>

    281. 

  281. 	GLM_FUNC_QUALIFIER detail::tdualquat<T, P> lerp

    282. 

  282. 	(

    283. 

  283. 		detail::tdualquat<T, P> const & x,

    284. 

  284. 		detail::tdualquat<T, P> const & y,

    285. 

  285. 		T const & a

    286. 

  286. 	)

    287. 

  287. 	{

    288. 

  288. 		// Dual Quaternion Linear blend aka DLB:

    289. 

  289. 		// Lerp is only defined in [0, 1]

    290. 

  290. 		assert(a >= static_cast<T>(0));

    291. 

  291. 		assert(a <= static_cast<T>(1));

    292. 

  292. 		T const k = dot(x.real,y.real) < static_cast<T>(0) ? -a : a;

    293. 

  293. 		T const one(1);

    294. 

  294. 		return detail::tdualquat<T, P>(x * (one - a) + y * k);

    295. 

  295. 	}

    296. 

  296. 

    297. 

  297. 	template <typename T, precision P>

    298. 

  298. 	GLM_FUNC_QUALIFIER detail::tdualquat<T, P> inverse

    299. 

  299. 	(

    300. 

  300. 		detail::tdualquat<T, P> const & q

    301. 

  301. 	)

    302. 

  302. 	{

    303. 

  303. 		const glm::detail::tquat<T, P> real = conjugate(q.real);

    304. 

  304. 		const glm::detail::tquat<T, P> dual = conjugate(q.dual);

    305. 

  305. 		return detail::tdualquat<T, P>(real, dual + (real * (-2.0f * dot(real,dual))));

    306. 

  306. 	}

    307. 

  307. 

    308. 

  308. 	template <typename T, precision P>

    309. 

  309. 	GLM_FUNC_QUALIFIER detail::tmat2x4<T, P> mat2x4_cast

    310. 

  310. 	(

    311. 

  311. 		detail::tdualquat<T, P> const & x

    312. 

  312. 	)

    313. 

  313. 	{

    314. 

  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. 

  315. 	}

    316. 

  316. 

    317. 

  317. 	template <typename T, precision P>

    318. 

  318. 	GLM_FUNC_QUALIFIER detail::tmat3x4<T, P> mat3x4_cast

    319. 

  319. 	(

    320. 

  320. 		detail::tdualquat<T, P> const & x

    321. 

  321. 	)

    322. 

  322. 	{

    323. 

  323. 		detail::tquat<T, P> r = x.real / length2(x.real);

    324. 

  324. 		

    325. 

  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. 

  326. 		r *= static_cast<T>(2);

    327. 

  327. 		

    328. 

  328. 		T const xy = r.x * x.real.y;

    329. 

  329. 		T const xz = r.x * x.real.z;

    330. 

  330. 		T const yz = r.y * x.real.z;

    331. 

  331. 		T const wx = r.w * x.real.x;

    332. 

  332. 		T const wy = r.w * x.real.y;

    333. 

  333. 		T const wz = r.w * x.real.z;

    334. 

  334. 		

    335. 

  335. 		detail::tvec4<T, P> const a(

    336. 

  336. 			rr.w + rr.x - rr.y - rr.z,

    337. 

  337. 			xy - wz,

    338. 

  338. 			xz + wy,

    339. 

  339. 			-(x.dual.w * r.x - x.dual.x * r.w + x.dual.y * r.z - x.dual.z * r.y));

    340. 

  340. 		

    341. 

  341. 		detail::tvec4<T, P> const b(

    342. 

  342. 			xy + wz,

    343. 

  343. 			rr.w + rr.y - rr.x - rr.z,

    344. 

  344. 			yz - wx,

    345. 

  345. 			-(x.dual.w * r.y - x.dual.x * r.z - x.dual.y * r.w + x.dual.z * r.x));

    346. 

  346. 		

    347. 

  347. 		detail::tvec4<T, P> const c(

    348. 

  348. 			xz - wy,

    349. 

  349. 			yz + wx,

    350. 

  350. 			rr.w + rr.z - rr.x - rr.y,

    351. 

  351. 			-(x.dual.w * r.z + x.dual.x * r.y - x.dual.y * r.x - x.dual.z * r.w));

    352. 

  352. 		

    353. 

  353. 		return detail::tmat3x4<T, P>(a, b, c);

    354. 

  354. 	}

    355. 

  355. 

    356. 

  356. 	template <typename T, precision P>

    357. 

  357. 	GLM_FUNC_QUALIFIER detail::tdualquat<T, P> dualquat_cast

    358. 

  358. 	(

    359. 

  359. 		detail::tmat2x4<T, P> const & x

    360. 

  360. 	)

    361. 

  361. 	{

    362. 

  362. 		return detail::tdualquat<T, P>(

    363. 

  363. 			detail::tquat<T, P>( x[0].w, x[0].x, x[0].y, x[0].z ),

    364. 

  364. 			detail::tquat<T, P>( x[1].w, x[1].x, x[1].y, x[1].z ));

    365. 

  365. 	}

    366. 

  366. 

    367. 

  367. 	template <typename T, precision P>

    368. 

  368. 	GLM_FUNC_QUALIFIER detail::tdualquat<T, P> dualquat_cast

    369. 

  369. 	(

    370. 

  370. 		detail::tmat3x4<T, P> const & x

    371. 

  371. 	)

    372. 

  372. 	{

    373. 

  373. 		detail::tquat<T, P> real;

    374. 

  374. 		

    375. 

  375. 		T const trace = x[0].x + x[1].y + x[2].z;

    376. 

  376. 		if(trace > T(0))

    377. 

  377. 		{

    378. 

  378. 			T const r = sqrt(T(1) + trace);

    379. 

  379. 			T const invr = static_cast<T>(0.5) / r;

    380. 

  380. 			real.w = static_cast<T>(0.5) * r;

    381. 

  381. 			real.x = (x[2].y - x[1].z) * invr;

    382. 

  382. 			real.y = (x[0].z - x[2].x) * invr;

    383. 

  383. 			real.z = (x[1].x - x[0].y) * invr;

    384. 

  384. 		}

    385. 

  385. 		else if(x[0].x > x[1].y && x[0].x > x[2].z)

    386. 

  386. 		{

    387. 

  387. 			T const r = sqrt(T(1) + x[0].x - x[1].y - x[2].z);

    388. 

  388. 			T const invr = static_cast<T>(0.5) / r;

    389. 

  389. 			real.x = static_cast<T>(0.5)*r;

    390. 

  390. 			real.y = (x[1].x + x[0].y) * invr;

    391. 

  391. 			real.z = (x[0].z + x[2].x) * invr;

    392. 

  392. 			real.w = (x[2].y - x[1].z) * invr;

    393. 

  393. 		}

    394. 

  394. 		else if(x[1].y > x[2].z)

    395. 

  395. 		{

    396. 

  396. 			T const r = sqrt(T(1) + x[1].y - x[0].x - x[2].z);

    397. 

  397. 			T const invr = static_cast<T>(0.5) / r;

    398. 

  398. 			real.x = (x[1].x + x[0].y) * invr;

    399. 

  399. 			real.y = static_cast<T>(0.5) * r;

    400. 

  400. 			real.z = (x[2].y + x[1].z) * invr;

    401. 

  401. 			real.w = (x[0].z - x[2].x) * invr;

    402. 

  402. 		}

    403. 

  403. 		else

    404. 

  404. 		{

    405. 

  405. 			T const r = sqrt(T(1) + x[2].z - x[0].x - x[1].y);

    406. 

  406. 			T const invr = static_cast<T>(0.5) / r;

    407. 

  407. 			real.x = (x[0].z + x[2].x) * invr;

    408. 

  408. 			real.y = (x[2].y + x[1].z) * invr;

    409. 

  409. 			real.z = static_cast<T>(0.5) * r;

    410. 

  410. 			real.w = (x[1].x - x[0].y) * invr;

    411. 

  411. 		}

    412. 

  412. 		

    413. 

  413. 		detail::tquat<T, P> dual;

    414. 

  414. 		dual.x =  T(0.5) * ( x[0].w * real.w + x[1].w * real.z - x[2].w * real.y);

    415. 

  415. 		dual.y =  T(0.5) * (-x[0].w * real.z + x[1].w * real.w + x[2].w * real.x);

    416. 

  416. 		dual.z =  T(0.5) * ( x[0].w * real.y - x[1].w * real.x + x[2].w * real.w);

    417. 

  417. 		dual.w = -T(0.5) * ( x[0].w * real.x + x[1].w * real.y + x[2].w * real.z);

    418. 

  418. 		return detail::tdualquat<T, P>(real, dual);

    419. 

  419. 	}

    420. 

  420. 

    421. 

  421. }//namespace glm
    422.