You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

792 lines
18 KiB

4 years ago
  1. ///////////////////////////////////////////////////////////////////////////////////
  2. /// OpenGL Mathematics (glm.g-truc.net)
  3. ///
  4. /// Copyright (c) 2005 - 2013 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 gtc_quaternion
  24. /// @file glm/gtc/quaternion.inl
  25. /// @date 2009-05-21 / 2011-06-15
  26. /// @author Christophe Riccio
  27. ///////////////////////////////////////////////////////////////////////////////////
  28. #include <limits>
  29. namespace glm{
  30. namespace detail
  31. {
  32. template <typename T>
  33. GLM_FUNC_QUALIFIER typename tquat<T>::size_type tquat<T>::length() const
  34. {
  35. return 4;
  36. }
  37. template <typename T>
  38. GLM_FUNC_QUALIFIER tquat<T>::tquat() :
  39. x(0),
  40. y(0),
  41. z(0),
  42. w(1)
  43. {}
  44. template <typename T>
  45. GLM_FUNC_QUALIFIER tquat<T>::tquat
  46. (
  47. value_type const & s,
  48. tvec3<T> const & v
  49. ) :
  50. x(v.x),
  51. y(v.y),
  52. z(v.z),
  53. w(s)
  54. {}
  55. template <typename T>
  56. GLM_FUNC_QUALIFIER tquat<T>::tquat
  57. (
  58. value_type const & w,
  59. value_type const & x,
  60. value_type const & y,
  61. value_type const & z
  62. ) :
  63. x(x),
  64. y(y),
  65. z(z),
  66. w(w)
  67. {}
  68. //////////////////////////////////////////////////////////////
  69. // tquat conversions
  70. //template <typename valType>
  71. //GLM_FUNC_QUALIFIER tquat<valType>::tquat
  72. //(
  73. // valType const & pitch,
  74. // valType const & yaw,
  75. // valType const & roll
  76. //)
  77. //{
  78. // tvec3<valType> eulerAngle(pitch * valType(0.5), yaw * valType(0.5), roll * valType(0.5));
  79. // tvec3<valType> c = glm::cos(eulerAngle * valType(0.5));
  80. // tvec3<valType> s = glm::sin(eulerAngle * valType(0.5));
  81. //
  82. // this->w = c.x * c.y * c.z + s.x * s.y * s.z;
  83. // this->x = s.x * c.y * c.z - c.x * s.y * s.z;
  84. // this->y = c.x * s.y * c.z + s.x * c.y * s.z;
  85. // this->z = c.x * c.y * s.z - s.x * s.y * c.z;
  86. //}
  87. template <typename T>
  88. GLM_FUNC_QUALIFIER tquat<T>::tquat
  89. (
  90. tvec3<T> const & eulerAngle
  91. )
  92. {
  93. tvec3<T> c = glm::cos(eulerAngle * value_type(0.5));
  94. tvec3<T> s = glm::sin(eulerAngle * value_type(0.5));
  95. this->w = c.x * c.y * c.z + s.x * s.y * s.z;
  96. this->x = s.x * c.y * c.z - c.x * s.y * s.z;
  97. this->y = c.x * s.y * c.z + s.x * c.y * s.z;
  98. this->z = c.x * c.y * s.z - s.x * s.y * c.z;
  99. }
  100. template <typename T>
  101. GLM_FUNC_QUALIFIER tquat<T>::tquat
  102. (
  103. tmat3x3<T> const & m
  104. )
  105. {
  106. *this = quat_cast(m);
  107. }
  108. template <typename T>
  109. GLM_FUNC_QUALIFIER tquat<T>::tquat
  110. (
  111. tmat4x4<T> const & m
  112. )
  113. {
  114. *this = quat_cast(m);
  115. }
  116. //////////////////////////////////////////////////////////////
  117. // tquat<T> accesses
  118. template <typename T>
  119. GLM_FUNC_QUALIFIER typename tquat<T>::value_type & tquat<T>::operator [] (int i)
  120. {
  121. return (&x)[i];
  122. }
  123. template <typename T>
  124. GLM_FUNC_QUALIFIER typename tquat<T>::value_type const & tquat<T>::operator [] (int i) const
  125. {
  126. return (&x)[i];
  127. }
  128. //////////////////////////////////////////////////////////////
  129. // tquat<valType> operators
  130. template <typename T>
  131. GLM_FUNC_QUALIFIER tquat<T> & tquat<T>::operator *=
  132. (
  133. value_type const & s
  134. )
  135. {
  136. this->w *= s;
  137. this->x *= s;
  138. this->y *= s;
  139. this->z *= s;
  140. return *this;
  141. }
  142. template <typename T>
  143. GLM_FUNC_QUALIFIER tquat<T> & tquat<T>::operator /=
  144. (
  145. value_type const & s
  146. )
  147. {
  148. this->w /= s;
  149. this->x /= s;
  150. this->y /= s;
  151. this->z /= s;
  152. return *this;
  153. }
  154. //////////////////////////////////////////////////////////////
  155. // tquat<valType> external operators
  156. template <typename T>
  157. GLM_FUNC_QUALIFIER detail::tquat<T> operator-
  158. (
  159. detail::tquat<T> const & q
  160. )
  161. {
  162. return detail::tquat<T>(-q.w, -q.x, -q.y, -q.z);
  163. }
  164. template <typename T>
  165. GLM_FUNC_QUALIFIER detail::tquat<T> operator+
  166. (
  167. detail::tquat<T> const & q,
  168. detail::tquat<T> const & p
  169. )
  170. {
  171. return detail::tquat<T>(
  172. q.w + p.w,
  173. q.x + p.x,
  174. q.y + p.y,
  175. q.z + p.z);
  176. }
  177. template <typename T>
  178. GLM_FUNC_QUALIFIER detail::tquat<T> operator*
  179. (
  180. detail::tquat<T> const & q,
  181. detail::tquat<T> const & p
  182. )
  183. {
  184. return detail::tquat<T>(
  185. q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z,
  186. q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y,
  187. q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z,
  188. q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x);
  189. }
  190. // Transformation
  191. template <typename T>
  192. GLM_FUNC_QUALIFIER detail::tvec3<T> operator*
  193. (
  194. detail::tquat<T> const & q,
  195. detail::tvec3<T> const & v
  196. )
  197. {
  198. typename detail::tquat<T>::value_type Two(2);
  199. detail::tvec3<T> uv, uuv;
  200. detail::tvec3<T> QuatVector(q.x, q.y, q.z);
  201. uv = glm::cross(QuatVector, v);
  202. uuv = glm::cross(QuatVector, uv);
  203. uv *= (Two * q.w);
  204. uuv *= Two;
  205. return v + uv + uuv;
  206. }
  207. template <typename T>
  208. GLM_FUNC_QUALIFIER detail::tvec3<T> operator*
  209. (
  210. detail::tvec3<T> const & v,
  211. detail::tquat<T> const & q
  212. )
  213. {
  214. return inverse(q) * v;
  215. }
  216. template <typename T>
  217. GLM_FUNC_QUALIFIER detail::tvec4<T> operator*
  218. (
  219. detail::tquat<T> const & q,
  220. detail::tvec4<T> const & v
  221. )
  222. {
  223. return detail::tvec4<T>(q * detail::tvec3<T>(v), v.w);
  224. }
  225. template <typename T>
  226. GLM_FUNC_QUALIFIER detail::tvec4<T> operator*
  227. (
  228. detail::tvec4<T> const & v,
  229. detail::tquat<T> const & q
  230. )
  231. {
  232. return inverse(q) * v;
  233. }
  234. template <typename T>
  235. GLM_FUNC_QUALIFIER detail::tquat<T> operator*
  236. (
  237. detail::tquat<T> const & q,
  238. typename detail::tquat<T>::value_type const & s
  239. )
  240. {
  241. return detail::tquat<T>(
  242. q.w * s, q.x * s, q.y * s, q.z * s);
  243. }
  244. template <typename T>
  245. GLM_FUNC_QUALIFIER detail::tquat<T> operator*
  246. (
  247. typename detail::tquat<T>::value_type const & s,
  248. detail::tquat<T> const & q
  249. )
  250. {
  251. return q * s;
  252. }
  253. template <typename T>
  254. GLM_FUNC_QUALIFIER detail::tquat<T> operator/
  255. (
  256. detail::tquat<T> const & q,
  257. typename detail::tquat<T>::value_type const & s
  258. )
  259. {
  260. return detail::tquat<T>(
  261. q.w / s, q.x / s, q.y / s, q.z / s);
  262. }
  263. //////////////////////////////////////
  264. // Boolean operators
  265. template <typename T>
  266. GLM_FUNC_QUALIFIER bool operator==
  267. (
  268. detail::tquat<T> const & q1,
  269. detail::tquat<T> const & q2
  270. )
  271. {
  272. return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w);
  273. }
  274. template <typename T>
  275. GLM_FUNC_QUALIFIER bool operator!=
  276. (
  277. detail::tquat<T> const & q1,
  278. detail::tquat<T> const & q2
  279. )
  280. {
  281. return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w);
  282. }
  283. }//namespace detail
  284. ////////////////////////////////////////////////////////
  285. template <typename T>
  286. GLM_FUNC_QUALIFIER T length
  287. (
  288. detail::tquat<T> const & q
  289. )
  290. {
  291. return glm::sqrt(dot(q, q));
  292. }
  293. template <typename T>
  294. GLM_FUNC_QUALIFIER detail::tquat<T> normalize
  295. (
  296. detail::tquat<T> const & q
  297. )
  298. {
  299. typename detail::tquat<T>::value_type len = length(q);
  300. if(len <= typename detail::tquat<T>::value_type(0)) // Problem
  301. return detail::tquat<T>(1, 0, 0, 0);
  302. typename detail::tquat<T>::value_type oneOverLen = typename detail::tquat<T>::value_type(1) / len;
  303. return detail::tquat<T>(q.w * oneOverLen, q.x * oneOverLen, q.y * oneOverLen, q.z * oneOverLen);
  304. }
  305. template <typename T>
  306. GLM_FUNC_QUALIFIER T dot
  307. (
  308. detail::tquat<T> const & q1,
  309. detail::tquat<T> const & q2
  310. )
  311. {
  312. return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
  313. }
  314. template <typename T>
  315. GLM_FUNC_QUALIFIER detail::tquat<T> cross
  316. (
  317. detail::tquat<T> const & q1,
  318. detail::tquat<T> const & q2
  319. )
  320. {
  321. return detail::tquat<T>(
  322. q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z,
  323. q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y,
  324. q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z,
  325. q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x);
  326. }
  327. /*
  328. // (x * sin(1 - a) * angle / sin(angle)) + (y * sin(a) * angle / sin(angle))
  329. template <typename T>
  330. GLM_FUNC_QUALIFIER detail::tquat<T> mix
  331. (
  332. detail::tquat<T> const & x,
  333. detail::tquat<T> const & y,
  334. typename detail::tquat<T>::value_type const & a
  335. )
  336. {
  337. if(a <= typename detail::tquat<T>::value_type(0)) return x;
  338. if(a >= typename detail::tquat<T>::value_type(1)) return y;
  339. float fCos = dot(x, y);
  340. detail::tquat<T> y2(y); //BUG!!! tquat<T> y2;
  341. if(fCos < typename detail::tquat<T>::value_type(0))
  342. {
  343. y2 = -y;
  344. fCos = -fCos;
  345. }
  346. //if(fCos > 1.0f) // problem
  347. float k0, k1;
  348. if(fCos > typename detail::tquat<T>::value_type(0.9999))
  349. {
  350. k0 = typename detail::tquat<T>::value_type(1) - a;
  351. k1 = typename detail::tquat<T>::value_type(0) + a; //BUG!!! 1.0f + a;
  352. }
  353. else
  354. {
  355. typename detail::tquat<T>::value_type fSin = sqrt(T(1) - fCos * fCos);
  356. typename detail::tquat<T>::value_type fAngle = atan(fSin, fCos);
  357. typename detail::tquat<T>::value_type fOneOverSin = T(1) / fSin;
  358. k0 = sin((typename detail::tquat<T>::value_type(1) - a) * fAngle) * fOneOverSin;
  359. k1 = sin((typename detail::tquat<T>::value_type(0) + a) * fAngle) * fOneOverSin;
  360. }
  361. return detail::tquat<T>(
  362. k0 * x.w + k1 * y2.w,
  363. k0 * x.x + k1 * y2.x,
  364. k0 * x.y + k1 * y2.y,
  365. k0 * x.z + k1 * y2.z);
  366. }
  367. template <typename T>
  368. GLM_FUNC_QUALIFIER detail::tquat<T> mix2
  369. (
  370. detail::tquat<T> const & x,
  371. detail::tquat<T> const & y,
  372. T const & a
  373. )
  374. {
  375. bool flip = false;
  376. if(a <= T(0)) return x;
  377. if(a >= T(1)) return y;
  378. T cos_t = dot(x, y);
  379. if(cos_t < T(0))
  380. {
  381. cos_t = -cos_t;
  382. flip = true;
  383. }
  384. T alpha(0), beta(0);
  385. if(T(1) - cos_t < 1e-7)
  386. beta = T(1) - alpha;
  387. else
  388. {
  389. T theta = acos(cos_t);
  390. T sin_t = sin(theta);
  391. beta = sin(theta * (T(1) - alpha)) / sin_t;
  392. alpha = sin(alpha * theta) / sin_t;
  393. }
  394. if(flip)
  395. alpha = -alpha;
  396. return normalize(beta * x + alpha * y);
  397. }
  398. */
  399. template <typename T>
  400. GLM_FUNC_QUALIFIER detail::tquat<T> mix
  401. (
  402. detail::tquat<T> const & x,
  403. detail::tquat<T> const & y,
  404. T const & a
  405. )
  406. {
  407. T cosTheta = dot(x, y);
  408. // Perform a linear interpolation when cosTheta is close to 1 to avoid side effect of sin(angle) becoming a zero denominator
  409. if(cosTheta > T(1) - epsilon<T>())
  410. {
  411. // Linear interpolation
  412. return detail::tquat<T>(
  413. mix(x.w, y.w, a),
  414. mix(x.x, y.x, a),
  415. mix(x.y, y.y, a),
  416. mix(x.z, y.z, a));
  417. }
  418. else
  419. {
  420. // Essential Mathematics, page 467
  421. T angle = acos(cosTheta);
  422. return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle);
  423. }
  424. }
  425. template <typename T>
  426. GLM_FUNC_QUALIFIER detail::tquat<T> lerp
  427. (
  428. detail::tquat<T> const & x,
  429. detail::tquat<T> const & y,
  430. T const & a
  431. )
  432. {
  433. // Lerp is only defined in [0, 1]
  434. assert(a >= T(0));
  435. assert(a <= T(1));
  436. return x * (T(1) - a) + (y * a);
  437. }
  438. template <typename T>
  439. GLM_FUNC_QUALIFIER detail::tquat<T> slerp
  440. (
  441. detail::tquat<T> const & x,
  442. detail::tquat<T> const & y,
  443. T const & a
  444. )
  445. {
  446. detail::tquat<T> z = y;
  447. T cosTheta = dot(x, y);
  448. // If cosTheta < 0, the interpolation will take the long way around the sphere.
  449. // To fix this, one quat must be negated.
  450. if (cosTheta < T(0))
  451. {
  452. z = -y;
  453. cosTheta = -cosTheta;
  454. }
  455. // Perform a linear interpolation when cosTheta is close to 1 to avoid side effect of sin(angle) becoming a zero denominator
  456. if(cosTheta > T(1) - epsilon<T>())
  457. {
  458. // Linear interpolation
  459. return detail::tquat<T>(
  460. mix(x.w, z.w, a),
  461. mix(x.x, z.x, a),
  462. mix(x.y, z.y, a),
  463. mix(x.z, z.z, a));
  464. }
  465. else
  466. {
  467. // Essential Mathematics, page 467
  468. T angle = acos(cosTheta);
  469. return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle);
  470. }
  471. }
  472. template <typename T>
  473. GLM_FUNC_QUALIFIER detail::tquat<T> conjugate
  474. (
  475. detail::tquat<T> const & q
  476. )
  477. {
  478. return detail::tquat<T>(q.w, -q.x, -q.y, -q.z);
  479. }
  480. template <typename T>
  481. GLM_FUNC_QUALIFIER detail::tquat<T> inverse
  482. (
  483. detail::tquat<T> const & q
  484. )
  485. {
  486. return conjugate(q) / dot(q, q);
  487. }
  488. template <typename T>
  489. GLM_FUNC_QUALIFIER detail::tquat<T> rotate
  490. (
  491. detail::tquat<T> const & q,
  492. typename detail::tquat<T>::value_type const & angle,
  493. detail::tvec3<T> const & v
  494. )
  495. {
  496. detail::tvec3<T> Tmp = v;
  497. // Axis of rotation must be normalised
  498. typename detail::tquat<T>::value_type len = glm::length(Tmp);
  499. if(abs(len - T(1)) > T(0.001))
  500. {
  501. T oneOverLen = T(1) / len;
  502. Tmp.x *= oneOverLen;
  503. Tmp.y *= oneOverLen;
  504. Tmp.z *= oneOverLen;
  505. }
  506. #ifdef GLM_FORCE_RADIANS
  507. typename detail::tquat<T>::value_type const AngleRad(angle);
  508. #else
  509. typename detail::tquat<T>::value_type const AngleRad = radians(angle);
  510. #endif
  511. typename detail::tquat<T>::value_type const Sin = sin(AngleRad * T(0.5));
  512. return q * detail::tquat<T>(cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin);
  513. //return gtc::quaternion::cross(q, detail::tquat<T>(cos(AngleRad * T(0.5)), Tmp.x * fSin, Tmp.y * fSin, Tmp.z * fSin));
  514. }
  515. template <typename T>
  516. GLM_FUNC_QUALIFIER detail::tvec3<T> eulerAngles
  517. (
  518. detail::tquat<T> const & x
  519. )
  520. {
  521. return detail::tvec3<T>(pitch(x), yaw(x), roll(x));
  522. }
  523. template <typename valType>
  524. GLM_FUNC_QUALIFIER valType roll
  525. (
  526. detail::tquat<valType> const & q
  527. )
  528. {
  529. #ifdef GLM_FORCE_RADIANS
  530. return valType(atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z));
  531. #else
  532. return glm::degrees(atan(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z));
  533. #endif
  534. }
  535. template <typename valType>
  536. GLM_FUNC_QUALIFIER valType pitch
  537. (
  538. detail::tquat<valType> const & q
  539. )
  540. {
  541. #ifdef GLM_FORCE_RADIANS
  542. return valType(atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z));
  543. #else
  544. return glm::degrees(atan(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z));
  545. #endif
  546. }
  547. template <typename valType>
  548. GLM_FUNC_QUALIFIER valType yaw
  549. (
  550. detail::tquat<valType> const & q
  551. )
  552. {
  553. #ifdef GLM_FORCE_RADIANS
  554. return asin(valType(-2) * (q.x * q.z - q.w * q.y));
  555. #else
  556. return glm::degrees(asin(valType(-2) * (q.x * q.z - q.w * q.y)));
  557. #endif
  558. }
  559. template <typename T>
  560. GLM_FUNC_QUALIFIER detail::tmat3x3<T> mat3_cast
  561. (
  562. detail::tquat<T> const & q
  563. )
  564. {
  565. detail::tmat3x3<T> Result(T(1));
  566. Result[0][0] = 1 - 2 * q.y * q.y - 2 * q.z * q.z;
  567. Result[0][1] = 2 * q.x * q.y + 2 * q.w * q.z;
  568. Result[0][2] = 2 * q.x * q.z - 2 * q.w * q.y;
  569. Result[1][0] = 2 * q.x * q.y - 2 * q.w * q.z;
  570. Result[1][1] = 1 - 2 * q.x * q.x - 2 * q.z * q.z;
  571. Result[1][2] = 2 * q.y * q.z + 2 * q.w * q.x;
  572. Result[2][0] = 2 * q.x * q.z + 2 * q.w * q.y;
  573. Result[2][1] = 2 * q.y * q.z - 2 * q.w * q.x;
  574. Result[2][2] = 1 - 2 * q.x * q.x - 2 * q.y * q.y;
  575. return Result;
  576. }
  577. template <typename T>
  578. GLM_FUNC_QUALIFIER detail::tmat4x4<T> mat4_cast
  579. (
  580. detail::tquat<T> const & q
  581. )
  582. {
  583. return detail::tmat4x4<T>(mat3_cast(q));
  584. }
  585. template <typename T>
  586. GLM_FUNC_QUALIFIER detail::tquat<T> quat_cast
  587. (
  588. detail::tmat3x3<T> const & m
  589. )
  590. {
  591. typename detail::tquat<T>::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2];
  592. typename detail::tquat<T>::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2];
  593. typename detail::tquat<T>::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1];
  594. typename detail::tquat<T>::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2];
  595. int biggestIndex = 0;
  596. typename detail::tquat<T>::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1;
  597. if(fourXSquaredMinus1 > fourBiggestSquaredMinus1)
  598. {
  599. fourBiggestSquaredMinus1 = fourXSquaredMinus1;
  600. biggestIndex = 1;
  601. }
  602. if(fourYSquaredMinus1 > fourBiggestSquaredMinus1)
  603. {
  604. fourBiggestSquaredMinus1 = fourYSquaredMinus1;
  605. biggestIndex = 2;
  606. }
  607. if(fourZSquaredMinus1 > fourBiggestSquaredMinus1)
  608. {
  609. fourBiggestSquaredMinus1 = fourZSquaredMinus1;
  610. biggestIndex = 3;
  611. }
  612. typename detail::tquat<T>::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5);
  613. typename detail::tquat<T>::value_type mult = T(0.25) / biggestVal;
  614. detail::tquat<T> Result;
  615. switch(biggestIndex)
  616. {
  617. case 0:
  618. Result.w = biggestVal;
  619. Result.x = (m[1][2] - m[2][1]) * mult;
  620. Result.y = (m[2][0] - m[0][2]) * mult;
  621. Result.z = (m[0][1] - m[1][0]) * mult;
  622. break;
  623. case 1:
  624. Result.w = (m[1][2] - m[2][1]) * mult;
  625. Result.x = biggestVal;
  626. Result.y = (m[0][1] + m[1][0]) * mult;
  627. Result.z = (m[2][0] + m[0][2]) * mult;
  628. break;
  629. case 2:
  630. Result.w = (m[2][0] - m[0][2]) * mult;
  631. Result.x = (m[0][1] + m[1][0]) * mult;
  632. Result.y = biggestVal;
  633. Result.z = (m[1][2] + m[2][1]) * mult;
  634. break;
  635. case 3:
  636. Result.w = (m[0][1] - m[1][0]) * mult;
  637. Result.x = (m[2][0] + m[0][2]) * mult;
  638. Result.y = (m[1][2] + m[2][1]) * mult;
  639. Result.z = biggestVal;
  640. break;
  641. default: // Silence a -Wswitch-default warning in GCC. Should never actually get here. Assert is just for sanity.
  642. assert(false);
  643. break;
  644. }
  645. return Result;
  646. }
  647. template <typename T>
  648. GLM_FUNC_QUALIFIER detail::tquat<T> quat_cast
  649. (
  650. detail::tmat4x4<T> const & m4
  651. )
  652. {
  653. return quat_cast(detail::tmat3x3<T>(m4));
  654. }
  655. template <typename T>
  656. GLM_FUNC_QUALIFIER T angle
  657. (
  658. detail::tquat<T> const & x
  659. )
  660. {
  661. #ifdef GLM_FORCE_RADIANS
  662. return acos(x.w) * T(2);
  663. #else
  664. return glm::degrees(acos(x.w) * T(2));
  665. #endif
  666. }
  667. template <typename T>
  668. GLM_FUNC_QUALIFIER detail::tvec3<T> axis
  669. (
  670. detail::tquat<T> const & x
  671. )
  672. {
  673. T tmp1 = T(1) - x.w * x.w;
  674. if(tmp1 <= T(0))
  675. return detail::tvec3<T>(0, 0, 1);
  676. T tmp2 = T(1) / sqrt(tmp1);
  677. return detail::tvec3<T>(x.x * tmp2, x.y * tmp2, x.z * tmp2);
  678. }
  679. template <typename valType>
  680. GLM_FUNC_QUALIFIER detail::tquat<valType> angleAxis
  681. (
  682. valType const & angle,
  683. valType const & x,
  684. valType const & y,
  685. valType const & z
  686. )
  687. {
  688. return angleAxis(angle, detail::tvec3<valType>(x, y, z));
  689. }
  690. template <typename valType>
  691. GLM_FUNC_QUALIFIER detail::tquat<valType> angleAxis
  692. (
  693. valType const & angle,
  694. detail::tvec3<valType> const & v
  695. )
  696. {
  697. detail::tquat<valType> result;
  698. #ifdef GLM_FORCE_RADIANS
  699. valType a(angle);
  700. #else
  701. valType a(glm::radians(angle));
  702. #endif
  703. valType s = glm::sin(a * valType(0.5));
  704. result.w = glm::cos(a * valType(0.5));
  705. result.x = v.x * s;
  706. result.y = v.y * s;
  707. result.z = v.z * s;
  708. return result;
  709. }
  710. }//namespace glm