9
3

euler_angles.inl 22 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899
  1. /// @ref gtx_euler_angles
  2. #include "compatibility.hpp" // glm::atan2
  3. namespace glm
  4. {
  5. template<typename T>
  6. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> eulerAngleX
  7. (
  8. T const& angleX
  9. )
  10. {
  11. T cosX = glm::cos(angleX);
  12. T sinX = glm::sin(angleX);
  13. return mat<4, 4, T, defaultp>(
  14. T(1), T(0), T(0), T(0),
  15. T(0), cosX, sinX, T(0),
  16. T(0),-sinX, cosX, T(0),
  17. T(0), T(0), T(0), T(1));
  18. }
  19. template<typename T>
  20. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> eulerAngleY
  21. (
  22. T const& angleY
  23. )
  24. {
  25. T cosY = glm::cos(angleY);
  26. T sinY = glm::sin(angleY);
  27. return mat<4, 4, T, defaultp>(
  28. cosY, T(0), -sinY, T(0),
  29. T(0), T(1), T(0), T(0),
  30. sinY, T(0), cosY, T(0),
  31. T(0), T(0), T(0), T(1));
  32. }
  33. template<typename T>
  34. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> eulerAngleZ
  35. (
  36. T const& angleZ
  37. )
  38. {
  39. T cosZ = glm::cos(angleZ);
  40. T sinZ = glm::sin(angleZ);
  41. return mat<4, 4, T, defaultp>(
  42. cosZ, sinZ, T(0), T(0),
  43. -sinZ, cosZ, T(0), T(0),
  44. T(0), T(0), T(1), T(0),
  45. T(0), T(0), T(0), T(1));
  46. }
  47. template <typename T>
  48. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> derivedEulerAngleX
  49. (
  50. T const & angleX,
  51. T const & angularVelocityX
  52. )
  53. {
  54. T cosX = glm::cos(angleX) * angularVelocityX;
  55. T sinX = glm::sin(angleX) * angularVelocityX;
  56. return mat<4, 4, T, defaultp>(
  57. T(0), T(0), T(0), T(0),
  58. T(0),-sinX, cosX, T(0),
  59. T(0),-cosX,-sinX, T(0),
  60. T(0), T(0), T(0), T(0));
  61. }
  62. template <typename T>
  63. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> derivedEulerAngleY
  64. (
  65. T const & angleY,
  66. T const & angularVelocityY
  67. )
  68. {
  69. T cosY = glm::cos(angleY) * angularVelocityY;
  70. T sinY = glm::sin(angleY) * angularVelocityY;
  71. return mat<4, 4, T, defaultp>(
  72. -sinY, T(0), -cosY, T(0),
  73. T(0), T(0), T(0), T(0),
  74. cosY, T(0), -sinY, T(0),
  75. T(0), T(0), T(0), T(0));
  76. }
  77. template <typename T>
  78. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> derivedEulerAngleZ
  79. (
  80. T const & angleZ,
  81. T const & angularVelocityZ
  82. )
  83. {
  84. T cosZ = glm::cos(angleZ) * angularVelocityZ;
  85. T sinZ = glm::sin(angleZ) * angularVelocityZ;
  86. return mat<4, 4, T, defaultp>(
  87. -sinZ, cosZ, T(0), T(0),
  88. -cosZ, -sinZ, T(0), T(0),
  89. T(0), T(0), T(0), T(0),
  90. T(0), T(0), T(0), T(0));
  91. }
  92. template<typename T>
  93. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> eulerAngleXY
  94. (
  95. T const& angleX,
  96. T const& angleY
  97. )
  98. {
  99. T cosX = glm::cos(angleX);
  100. T sinX = glm::sin(angleX);
  101. T cosY = glm::cos(angleY);
  102. T sinY = glm::sin(angleY);
  103. return mat<4, 4, T, defaultp>(
  104. cosY, -sinX * -sinY, cosX * -sinY, T(0),
  105. T(0), cosX, sinX, T(0),
  106. sinY, -sinX * cosY, cosX * cosY, T(0),
  107. T(0), T(0), T(0), T(1));
  108. }
  109. template<typename T>
  110. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> eulerAngleYX
  111. (
  112. T const& angleY,
  113. T const& angleX
  114. )
  115. {
  116. T cosX = glm::cos(angleX);
  117. T sinX = glm::sin(angleX);
  118. T cosY = glm::cos(angleY);
  119. T sinY = glm::sin(angleY);
  120. return mat<4, 4, T, defaultp>(
  121. cosY, 0, -sinY, T(0),
  122. sinY * sinX, cosX, cosY * sinX, T(0),
  123. sinY * cosX, -sinX, cosY * cosX, T(0),
  124. T(0), T(0), T(0), T(1));
  125. }
  126. template<typename T>
  127. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> eulerAngleXZ
  128. (
  129. T const& angleX,
  130. T const& angleZ
  131. )
  132. {
  133. return eulerAngleX(angleX) * eulerAngleZ(angleZ);
  134. }
  135. template<typename T>
  136. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> eulerAngleZX
  137. (
  138. T const& angleZ,
  139. T const& angleX
  140. )
  141. {
  142. return eulerAngleZ(angleZ) * eulerAngleX(angleX);
  143. }
  144. template<typename T>
  145. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> eulerAngleYZ
  146. (
  147. T const& angleY,
  148. T const& angleZ
  149. )
  150. {
  151. return eulerAngleY(angleY) * eulerAngleZ(angleZ);
  152. }
  153. template<typename T>
  154. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> eulerAngleZY
  155. (
  156. T const& angleZ,
  157. T const& angleY
  158. )
  159. {
  160. return eulerAngleZ(angleZ) * eulerAngleY(angleY);
  161. }
  162. template<typename T>
  163. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> eulerAngleXYZ
  164. (
  165. T const& t1,
  166. T const& t2,
  167. T const& t3
  168. )
  169. {
  170. T c1 = glm::cos(-t1);
  171. T c2 = glm::cos(-t2);
  172. T c3 = glm::cos(-t3);
  173. T s1 = glm::sin(-t1);
  174. T s2 = glm::sin(-t2);
  175. T s3 = glm::sin(-t3);
  176. mat<4, 4, T, defaultp> Result;
  177. Result[0][0] = c2 * c3;
  178. Result[0][1] =-c1 * s3 + s1 * s2 * c3;
  179. Result[0][2] = s1 * s3 + c1 * s2 * c3;
  180. Result[0][3] = static_cast<T>(0);
  181. Result[1][0] = c2 * s3;
  182. Result[1][1] = c1 * c3 + s1 * s2 * s3;
  183. Result[1][2] =-s1 * c3 + c1 * s2 * s3;
  184. Result[1][3] = static_cast<T>(0);
  185. Result[2][0] =-s2;
  186. Result[2][1] = s1 * c2;
  187. Result[2][2] = c1 * c2;
  188. Result[2][3] = static_cast<T>(0);
  189. Result[3][0] = static_cast<T>(0);
  190. Result[3][1] = static_cast<T>(0);
  191. Result[3][2] = static_cast<T>(0);
  192. Result[3][3] = static_cast<T>(1);
  193. return Result;
  194. }
  195. template<typename T>
  196. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> eulerAngleYXZ
  197. (
  198. T const& yaw,
  199. T const& pitch,
  200. T const& roll
  201. )
  202. {
  203. T tmp_ch = glm::cos(yaw);
  204. T tmp_sh = glm::sin(yaw);
  205. T tmp_cp = glm::cos(pitch);
  206. T tmp_sp = glm::sin(pitch);
  207. T tmp_cb = glm::cos(roll);
  208. T tmp_sb = glm::sin(roll);
  209. mat<4, 4, T, defaultp> Result;
  210. Result[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb;
  211. Result[0][1] = tmp_sb * tmp_cp;
  212. Result[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb;
  213. Result[0][3] = static_cast<T>(0);
  214. Result[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb;
  215. Result[1][1] = tmp_cb * tmp_cp;
  216. Result[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb;
  217. Result[1][3] = static_cast<T>(0);
  218. Result[2][0] = tmp_sh * tmp_cp;
  219. Result[2][1] = -tmp_sp;
  220. Result[2][2] = tmp_ch * tmp_cp;
  221. Result[2][3] = static_cast<T>(0);
  222. Result[3][0] = static_cast<T>(0);
  223. Result[3][1] = static_cast<T>(0);
  224. Result[3][2] = static_cast<T>(0);
  225. Result[3][3] = static_cast<T>(1);
  226. return Result;
  227. }
  228. template <typename T>
  229. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> eulerAngleXZX
  230. (
  231. T const & t1,
  232. T const & t2,
  233. T const & t3
  234. )
  235. {
  236. T c1 = glm::cos(t1);
  237. T s1 = glm::sin(t1);
  238. T c2 = glm::cos(t2);
  239. T s2 = glm::sin(t2);
  240. T c3 = glm::cos(t3);
  241. T s3 = glm::sin(t3);
  242. mat<4, 4, T, defaultp> Result;
  243. Result[0][0] = c2;
  244. Result[0][1] = c1 * s2;
  245. Result[0][2] = s1 * s2;
  246. Result[0][3] = static_cast<T>(0);
  247. Result[1][0] =-c3 * s2;
  248. Result[1][1] = c1 * c2 * c3 - s1 * s3;
  249. Result[1][2] = c1 * s3 + c2 * c3 * s1;
  250. Result[1][3] = static_cast<T>(0);
  251. Result[2][0] = s2 * s3;
  252. Result[2][1] =-c3 * s1 - c1 * c2 * s3;
  253. Result[2][2] = c1 * c3 - c2 * s1 * s3;
  254. Result[2][3] = static_cast<T>(0);
  255. Result[3][0] = static_cast<T>(0);
  256. Result[3][1] = static_cast<T>(0);
  257. Result[3][2] = static_cast<T>(0);
  258. Result[3][3] = static_cast<T>(1);
  259. return Result;
  260. }
  261. template <typename T>
  262. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> eulerAngleXYX
  263. (
  264. T const & t1,
  265. T const & t2,
  266. T const & t3
  267. )
  268. {
  269. T c1 = glm::cos(t1);
  270. T s1 = glm::sin(t1);
  271. T c2 = glm::cos(t2);
  272. T s2 = glm::sin(t2);
  273. T c3 = glm::cos(t3);
  274. T s3 = glm::sin(t3);
  275. mat<4, 4, T, defaultp> Result;
  276. Result[0][0] = c2;
  277. Result[0][1] = s1 * s2;
  278. Result[0][2] =-c1 * s2;
  279. Result[0][3] = static_cast<T>(0);
  280. Result[1][0] = s2 * s3;
  281. Result[1][1] = c1 * c3 - c2 * s1 * s3;
  282. Result[1][2] = c3 * s1 + c1 * c2 * s3;
  283. Result[1][3] = static_cast<T>(0);
  284. Result[2][0] = c3 * s2;
  285. Result[2][1] =-c1 * s3 - c2 * c3 * s1;
  286. Result[2][2] = c1 * c2 * c3 - s1 * s3;
  287. Result[2][3] = static_cast<T>(0);
  288. Result[3][0] = static_cast<T>(0);
  289. Result[3][1] = static_cast<T>(0);
  290. Result[3][2] = static_cast<T>(0);
  291. Result[3][3] = static_cast<T>(1);
  292. return Result;
  293. }
  294. template <typename T>
  295. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> eulerAngleYXY
  296. (
  297. T const & t1,
  298. T const & t2,
  299. T const & t3
  300. )
  301. {
  302. T c1 = glm::cos(t1);
  303. T s1 = glm::sin(t1);
  304. T c2 = glm::cos(t2);
  305. T s2 = glm::sin(t2);
  306. T c3 = glm::cos(t3);
  307. T s3 = glm::sin(t3);
  308. mat<4, 4, T, defaultp> Result;
  309. Result[0][0] = c1 * c3 - c2 * s1 * s3;
  310. Result[0][1] = s2* s3;
  311. Result[0][2] =-c3 * s1 - c1 * c2 * s3;
  312. Result[0][3] = static_cast<T>(0);
  313. Result[1][0] = s1 * s2;
  314. Result[1][1] = c2;
  315. Result[1][2] = c1 * s2;
  316. Result[1][3] = static_cast<T>(0);
  317. Result[2][0] = c1 * s3 + c2 * c3 * s1;
  318. Result[2][1] =-c3 * s2;
  319. Result[2][2] = c1 * c2 * c3 - s1 * s3;
  320. Result[2][3] = static_cast<T>(0);
  321. Result[3][0] = static_cast<T>(0);
  322. Result[3][1] = static_cast<T>(0);
  323. Result[3][2] = static_cast<T>(0);
  324. Result[3][3] = static_cast<T>(1);
  325. return Result;
  326. }
  327. template <typename T>
  328. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> eulerAngleYZY
  329. (
  330. T const & t1,
  331. T const & t2,
  332. T const & t3
  333. )
  334. {
  335. T c1 = glm::cos(t1);
  336. T s1 = glm::sin(t1);
  337. T c2 = glm::cos(t2);
  338. T s2 = glm::sin(t2);
  339. T c3 = glm::cos(t3);
  340. T s3 = glm::sin(t3);
  341. mat<4, 4, T, defaultp> Result;
  342. Result[0][0] = c1 * c2 * c3 - s1 * s3;
  343. Result[0][1] = c3 * s2;
  344. Result[0][2] =-c1 * s3 - c2 * c3 * s1;
  345. Result[0][3] = static_cast<T>(0);
  346. Result[1][0] =-c1 * s2;
  347. Result[1][1] = c2;
  348. Result[1][2] = s1 * s2;
  349. Result[1][3] = static_cast<T>(0);
  350. Result[2][0] = c3 * s1 + c1 * c2 * s3;
  351. Result[2][1] = s2 * s3;
  352. Result[2][2] = c1 * c3 - c2 * s1 * s3;
  353. Result[2][3] = static_cast<T>(0);
  354. Result[3][0] = static_cast<T>(0);
  355. Result[3][1] = static_cast<T>(0);
  356. Result[3][2] = static_cast<T>(0);
  357. Result[3][3] = static_cast<T>(1);
  358. return Result;
  359. }
  360. template <typename T>
  361. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> eulerAngleZYZ
  362. (
  363. T const & t1,
  364. T const & t2,
  365. T const & t3
  366. )
  367. {
  368. T c1 = glm::cos(t1);
  369. T s1 = glm::sin(t1);
  370. T c2 = glm::cos(t2);
  371. T s2 = glm::sin(t2);
  372. T c3 = glm::cos(t3);
  373. T s3 = glm::sin(t3);
  374. mat<4, 4, T, defaultp> Result;
  375. Result[0][0] = c1 * c2 * c3 - s1 * s3;
  376. Result[0][1] = c1 * s3 + c2 * c3 * s1;
  377. Result[0][2] =-c3 * s2;
  378. Result[0][3] = static_cast<T>(0);
  379. Result[1][0] =-c3 * s1 - c1 * c2 * s3;
  380. Result[1][1] = c1 * c3 - c2 * s1 * s3;
  381. Result[1][2] = s2 * s3;
  382. Result[1][3] = static_cast<T>(0);
  383. Result[2][0] = c1 * s2;
  384. Result[2][1] = s1 * s2;
  385. Result[2][2] = c2;
  386. Result[2][3] = static_cast<T>(0);
  387. Result[3][0] = static_cast<T>(0);
  388. Result[3][1] = static_cast<T>(0);
  389. Result[3][2] = static_cast<T>(0);
  390. Result[3][3] = static_cast<T>(1);
  391. return Result;
  392. }
  393. template <typename T>
  394. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> eulerAngleZXZ
  395. (
  396. T const & t1,
  397. T const & t2,
  398. T const & t3
  399. )
  400. {
  401. T c1 = glm::cos(t1);
  402. T s1 = glm::sin(t1);
  403. T c2 = glm::cos(t2);
  404. T s2 = glm::sin(t2);
  405. T c3 = glm::cos(t3);
  406. T s3 = glm::sin(t3);
  407. mat<4, 4, T, defaultp> Result;
  408. Result[0][0] = c1 * c3 - c2 * s1 * s3;
  409. Result[0][1] = c3 * s1 + c1 * c2 * s3;
  410. Result[0][2] = s2 *s3;
  411. Result[0][3] = static_cast<T>(0);
  412. Result[1][0] =-c1 * s3 - c2 * c3 * s1;
  413. Result[1][1] = c1 * c2 * c3 - s1 * s3;
  414. Result[1][2] = c3 * s2;
  415. Result[1][3] = static_cast<T>(0);
  416. Result[2][0] = s1 * s2;
  417. Result[2][1] =-c1 * s2;
  418. Result[2][2] = c2;
  419. Result[2][3] = static_cast<T>(0);
  420. Result[3][0] = static_cast<T>(0);
  421. Result[3][1] = static_cast<T>(0);
  422. Result[3][2] = static_cast<T>(0);
  423. Result[3][3] = static_cast<T>(1);
  424. return Result;
  425. }
  426. template <typename T>
  427. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> eulerAngleXZY
  428. (
  429. T const & t1,
  430. T const & t2,
  431. T const & t3
  432. )
  433. {
  434. T c1 = glm::cos(t1);
  435. T s1 = glm::sin(t1);
  436. T c2 = glm::cos(t2);
  437. T s2 = glm::sin(t2);
  438. T c3 = glm::cos(t3);
  439. T s3 = glm::sin(t3);
  440. mat<4, 4, T, defaultp> Result;
  441. Result[0][0] = c2 * c3;
  442. Result[0][1] = s1 * s3 + c1 * c3 * s2;
  443. Result[0][2] = c3 * s1 * s2 - c1 * s3;
  444. Result[0][3] = static_cast<T>(0);
  445. Result[1][0] =-s2;
  446. Result[1][1] = c1 * c2;
  447. Result[1][2] = c2 * s1;
  448. Result[1][3] = static_cast<T>(0);
  449. Result[2][0] = c2 * s3;
  450. Result[2][1] = c1 * s2 * s3 - c3 * s1;
  451. Result[2][2] = c1 * c3 + s1 * s2 *s3;
  452. Result[2][3] = static_cast<T>(0);
  453. Result[3][0] = static_cast<T>(0);
  454. Result[3][1] = static_cast<T>(0);
  455. Result[3][2] = static_cast<T>(0);
  456. Result[3][3] = static_cast<T>(1);
  457. return Result;
  458. }
  459. template <typename T>
  460. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> eulerAngleYZX
  461. (
  462. T const & t1,
  463. T const & t2,
  464. T const & t3
  465. )
  466. {
  467. T c1 = glm::cos(t1);
  468. T s1 = glm::sin(t1);
  469. T c2 = glm::cos(t2);
  470. T s2 = glm::sin(t2);
  471. T c3 = glm::cos(t3);
  472. T s3 = glm::sin(t3);
  473. mat<4, 4, T, defaultp> Result;
  474. Result[0][0] = c1 * c2;
  475. Result[0][1] = s2;
  476. Result[0][2] =-c2 * s1;
  477. Result[0][3] = static_cast<T>(0);
  478. Result[1][0] = s1 * s3 - c1 * c3 * s2;
  479. Result[1][1] = c2 * c3;
  480. Result[1][2] = c1 * s3 + c3 * s1 * s2;
  481. Result[1][3] = static_cast<T>(0);
  482. Result[2][0] = c3 * s1 + c1 * s2 * s3;
  483. Result[2][1] =-c2 * s3;
  484. Result[2][2] = c1 * c3 - s1 * s2 * s3;
  485. Result[2][3] = static_cast<T>(0);
  486. Result[3][0] = static_cast<T>(0);
  487. Result[3][1] = static_cast<T>(0);
  488. Result[3][2] = static_cast<T>(0);
  489. Result[3][3] = static_cast<T>(1);
  490. return Result;
  491. }
  492. template <typename T>
  493. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> eulerAngleZYX
  494. (
  495. T const & t1,
  496. T const & t2,
  497. T const & t3
  498. )
  499. {
  500. T c1 = glm::cos(t1);
  501. T s1 = glm::sin(t1);
  502. T c2 = glm::cos(t2);
  503. T s2 = glm::sin(t2);
  504. T c3 = glm::cos(t3);
  505. T s3 = glm::sin(t3);
  506. mat<4, 4, T, defaultp> Result;
  507. Result[0][0] = c1 * c2;
  508. Result[0][1] = c2 * s1;
  509. Result[0][2] =-s2;
  510. Result[0][3] = static_cast<T>(0);
  511. Result[1][0] = c1 * s2 * s3 - c3 * s1;
  512. Result[1][1] = c1 * c3 + s1 * s2 * s3;
  513. Result[1][2] = c2 * s3;
  514. Result[1][3] = static_cast<T>(0);
  515. Result[2][0] = s1 * s3 + c1 * c3 * s2;
  516. Result[2][1] = c3 * s1 * s2 - c1 * s3;
  517. Result[2][2] = c2 * c3;
  518. Result[2][3] = static_cast<T>(0);
  519. Result[3][0] = static_cast<T>(0);
  520. Result[3][1] = static_cast<T>(0);
  521. Result[3][2] = static_cast<T>(0);
  522. Result[3][3] = static_cast<T>(1);
  523. return Result;
  524. }
  525. template <typename T>
  526. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> eulerAngleZXY
  527. (
  528. T const & t1,
  529. T const & t2,
  530. T const & t3
  531. )
  532. {
  533. T c1 = glm::cos(t1);
  534. T s1 = glm::sin(t1);
  535. T c2 = glm::cos(t2);
  536. T s2 = glm::sin(t2);
  537. T c3 = glm::cos(t3);
  538. T s3 = glm::sin(t3);
  539. mat<4, 4, T, defaultp> Result;
  540. Result[0][0] = c1 * c3 - s1 * s2 * s3;
  541. Result[0][1] = c3 * s1 + c1 * s2 * s3;
  542. Result[0][2] =-c2 * s3;
  543. Result[0][3] = static_cast<T>(0);
  544. Result[1][0] =-c2 * s1;
  545. Result[1][1] = c1 * c2;
  546. Result[1][2] = s2;
  547. Result[1][3] = static_cast<T>(0);
  548. Result[2][0] = c1 * s3 + c3 * s1 * s2;
  549. Result[2][1] = s1 * s3 - c1 * c3 * s2;
  550. Result[2][2] = c2 * c3;
  551. Result[2][3] = static_cast<T>(0);
  552. Result[3][0] = static_cast<T>(0);
  553. Result[3][1] = static_cast<T>(0);
  554. Result[3][2] = static_cast<T>(0);
  555. Result[3][3] = static_cast<T>(1);
  556. return Result;
  557. }
  558. template<typename T>
  559. GLM_FUNC_QUALIFIER mat<4, 4, T, defaultp> yawPitchRoll
  560. (
  561. T const& yaw,
  562. T const& pitch,
  563. T const& roll
  564. )
  565. {
  566. T tmp_ch = glm::cos(yaw);
  567. T tmp_sh = glm::sin(yaw);
  568. T tmp_cp = glm::cos(pitch);
  569. T tmp_sp = glm::sin(pitch);
  570. T tmp_cb = glm::cos(roll);
  571. T tmp_sb = glm::sin(roll);
  572. mat<4, 4, T, defaultp> Result;
  573. Result[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb;
  574. Result[0][1] = tmp_sb * tmp_cp;
  575. Result[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb;
  576. Result[0][3] = static_cast<T>(0);
  577. Result[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb;
  578. Result[1][1] = tmp_cb * tmp_cp;
  579. Result[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb;
  580. Result[1][3] = static_cast<T>(0);
  581. Result[2][0] = tmp_sh * tmp_cp;
  582. Result[2][1] = -tmp_sp;
  583. Result[2][2] = tmp_ch * tmp_cp;
  584. Result[2][3] = static_cast<T>(0);
  585. Result[3][0] = static_cast<T>(0);
  586. Result[3][1] = static_cast<T>(0);
  587. Result[3][2] = static_cast<T>(0);
  588. Result[3][3] = static_cast<T>(1);
  589. return Result;
  590. }
  591. template<typename T>
  592. GLM_FUNC_QUALIFIER mat<2, 2, T, defaultp> orientate2
  593. (
  594. T const& angle
  595. )
  596. {
  597. T c = glm::cos(angle);
  598. T s = glm::sin(angle);
  599. mat<2, 2, T, defaultp> Result;
  600. Result[0][0] = c;
  601. Result[0][1] = s;
  602. Result[1][0] = -s;
  603. Result[1][1] = c;
  604. return Result;
  605. }
  606. template<typename T>
  607. GLM_FUNC_QUALIFIER mat<3, 3, T, defaultp> orientate3
  608. (
  609. T const& angle
  610. )
  611. {
  612. T c = glm::cos(angle);
  613. T s = glm::sin(angle);
  614. mat<3, 3, T, defaultp> Result;
  615. Result[0][0] = c;
  616. Result[0][1] = s;
  617. Result[0][2] = 0.0f;
  618. Result[1][0] = -s;
  619. Result[1][1] = c;
  620. Result[1][2] = 0.0f;
  621. Result[2][0] = 0.0f;
  622. Result[2][1] = 0.0f;
  623. Result[2][2] = 1.0f;
  624. return Result;
  625. }
  626. template<typename T, qualifier Q>
  627. GLM_FUNC_QUALIFIER mat<3, 3, T, Q> orientate3
  628. (
  629. vec<3, T, Q> const& angles
  630. )
  631. {
  632. return mat<3, 3, T, Q>(yawPitchRoll(angles.z, angles.x, angles.y));
  633. }
  634. template<typename T, qualifier Q>
  635. GLM_FUNC_QUALIFIER mat<4, 4, T, Q> orientate4
  636. (
  637. vec<3, T, Q> const& angles
  638. )
  639. {
  640. return yawPitchRoll(angles.z, angles.x, angles.y);
  641. }
  642. template<typename T>
  643. GLM_FUNC_DECL void extractEulerAngleXYZ(mat<4, 4, T, defaultp> const& M,
  644. T & t1,
  645. T & t2,
  646. T & t3)
  647. {
  648. T T1 = glm::atan2<T, defaultp>(M[2][1], M[2][2]);
  649. T C2 = glm::sqrt(M[0][0]*M[0][0] + M[1][0]*M[1][0]);
  650. T T2 = glm::atan2<T, defaultp>(-M[2][0], C2);
  651. T S1 = glm::sin(T1);
  652. T C1 = glm::cos(T1);
  653. T T3 = glm::atan2<T, defaultp>(S1*M[0][2] - C1*M[0][1], C1*M[1][1] - S1*M[1][2 ]);
  654. t1 = -T1;
  655. t2 = -T2;
  656. t3 = -T3;
  657. }
  658. template <typename T>
  659. GLM_FUNC_QUALIFIER void extractEulerAngleYXZ(mat<4, 4, T, defaultp> const & M,
  660. T & t1,
  661. T & t2,
  662. T & t3)
  663. {
  664. T T1 = glm::atan2<T, defaultp>(M[2][0], M[2][2]);
  665. T C2 = glm::sqrt(M[0][1]*M[0][1] + M[1][1]*M[1][1]);
  666. T T2 = glm::atan2<T, defaultp>(-M[2][1], C2);
  667. T S1 = glm::sin(T1);
  668. T C1 = glm::cos(T1);
  669. T T3 = glm::atan2<T, defaultp>(S1*M[1][2] - C1*M[1][0], C1*M[0][0] - S1*M[0][2]);
  670. t1 = T1;
  671. t2 = T2;
  672. t3 = T3;
  673. }
  674. template <typename T>
  675. GLM_FUNC_QUALIFIER void extractEulerAngleXZX(mat<4, 4, T, defaultp> const & M,
  676. T & t1,
  677. T & t2,
  678. T & t3)
  679. {
  680. T T1 = glm::atan2<T, defaultp>(M[0][2], M[0][1]);
  681. T S2 = glm::sqrt(M[1][0]*M[1][0] + M[2][0]*M[2][0]);
  682. T T2 = glm::atan2<T, defaultp>(S2, M[0][0]);
  683. T S1 = glm::sin(T1);
  684. T C1 = glm::cos(T1);
  685. T T3 = glm::atan2<T, defaultp>(C1*M[1][2] - S1*M[1][1], C1*M[2][2] - S1*M[2][1]);
  686. t1 = T1;
  687. t2 = T2;
  688. t3 = T3;
  689. }
  690. template <typename T>
  691. GLM_FUNC_QUALIFIER void extractEulerAngleXYX(mat<4, 4, T, defaultp> const & M,
  692. T & t1,
  693. T & t2,
  694. T & t3)
  695. {
  696. T T1 = glm::atan2<T, defaultp>(M[0][1], -M[0][2]);
  697. T S2 = glm::sqrt(M[1][0]*M[1][0] + M[2][0]*M[2][0]);
  698. T T2 = glm::atan2<T, defaultp>(S2, M[0][0]);
  699. T S1 = glm::sin(T1);
  700. T C1 = glm::cos(T1);
  701. T T3 = glm::atan2<T, defaultp>(-C1*M[2][1] - S1*M[2][2], C1*M[1][1] + S1*M[1][2]);
  702. t1 = T1;
  703. t2 = T2;
  704. t3 = T3;
  705. }
  706. template <typename T>
  707. GLM_FUNC_QUALIFIER void extractEulerAngleYXY(mat<4, 4, T, defaultp> const & M,
  708. T & t1,
  709. T & t2,
  710. T & t3)
  711. {
  712. T T1 = glm::atan2<T, defaultp>(M[1][0], M[1][2]);
  713. T S2 = glm::sqrt(M[0][1]*M[0][1] + M[2][1]*M[2][1]);
  714. T T2 = glm::atan2<T, defaultp>(S2, M[1][1]);
  715. T S1 = glm::sin(T1);
  716. T C1 = glm::cos(T1);
  717. T T3 = glm::atan2<T, defaultp>(C1*M[2][0] - S1*M[2][2], C1*M[0][0] - S1*M[0][2]);
  718. t1 = T1;
  719. t2 = T2;
  720. t3 = T3;
  721. }
  722. template <typename T>
  723. GLM_FUNC_QUALIFIER void extractEulerAngleYZY(mat<4, 4, T, defaultp> const & M,
  724. T & t1,
  725. T & t2,
  726. T & t3)
  727. {
  728. T T1 = glm::atan2<T, defaultp>(M[1][2], -M[1][0]);
  729. T S2 = glm::sqrt(M[0][1]*M[0][1] + M[2][1]*M[2][1]);
  730. T T2 = glm::atan2<T, defaultp>(S2, M[1][1]);
  731. T S1 = glm::sin(T1);
  732. T C1 = glm::cos(T1);
  733. T T3 = glm::atan2<T, defaultp>(-S1*M[0][0] - C1*M[0][2], S1*M[2][0] + C1*M[2][2]);
  734. t1 = T1;
  735. t2 = T2;
  736. t3 = T3;
  737. }
  738. template <typename T>
  739. GLM_FUNC_QUALIFIER void extractEulerAngleZYZ(mat<4, 4, T, defaultp> const & M,
  740. T & t1,
  741. T & t2,
  742. T & t3)
  743. {
  744. T T1 = glm::atan2<T, defaultp>(M[2][1], M[2][0]);
  745. T S2 = glm::sqrt(M[0][2]*M[0][2] + M[1][2]*M[1][2]);
  746. T T2 = glm::atan2<T, defaultp>(S2, M[2][2]);
  747. T S1 = glm::sin(T1);
  748. T C1 = glm::cos(T1);
  749. T T3 = glm::atan2<T, defaultp>(C1*M[0][1] - S1*M[0][0], C1*M[1][1] - S1*M[1][0]);
  750. t1 = T1;
  751. t2 = T2;
  752. t3 = T3;
  753. }
  754. template <typename T>
  755. GLM_FUNC_QUALIFIER void extractEulerAngleZXZ(mat<4, 4, T, defaultp> const & M,
  756. T & t1,
  757. T & t2,
  758. T & t3)
  759. {
  760. T T1 = glm::atan2<T, defaultp>(M[2][0], -M[2][1]);
  761. T S2 = glm::sqrt(M[0][2]*M[0][2] + M[1][2]*M[1][2]);
  762. T T2 = glm::atan2<T, defaultp>(S2, M[2][2]);
  763. T S1 = glm::sin(T1);
  764. T C1 = glm::cos(T1);
  765. T T3 = glm::atan2<T, defaultp>(-C1*M[1][0] - S1*M[1][1], C1*M[0][0] + S1*M[0][1]);
  766. t1 = T1;
  767. t2 = T2;
  768. t3 = T3;
  769. }
  770. template <typename T>
  771. GLM_FUNC_QUALIFIER void extractEulerAngleXZY(mat<4, 4, T, defaultp> const & M,
  772. T & t1,
  773. T & t2,
  774. T & t3)
  775. {
  776. T T1 = glm::atan2<T, defaultp>(M[1][2], M[1][1]);
  777. T C2 = glm::sqrt(M[0][0]*M[0][0] + M[2][0]*M[2][0]);
  778. T T2 = glm::atan2<T, defaultp>(-M[1][0], C2);
  779. T S1 = glm::sin(T1);
  780. T C1 = glm::cos(T1);
  781. T T3 = glm::atan2<T, defaultp>(S1*M[0][1] - C1*M[0][2], C1*M[2][2] - S1*M[2][1]);
  782. t1 = T1;
  783. t2 = T2;
  784. t3 = T3;
  785. }
  786. template <typename T>
  787. GLM_FUNC_QUALIFIER void extractEulerAngleYZX(mat<4, 4, T, defaultp> const & M,
  788. T & t1,
  789. T & t2,
  790. T & t3)
  791. {
  792. T T1 = glm::atan2<T, defaultp>(-M[0][2], M[0][0]);
  793. T C2 = glm::sqrt(M[1][1]*M[1][1] + M[2][1]*M[2][1]);
  794. T T2 = glm::atan2<T, defaultp>(M[0][1], C2);
  795. T S1 = glm::sin(T1);
  796. T C1 = glm::cos(T1);
  797. T T3 = glm::atan2<T, defaultp>(S1*M[1][0] + C1*M[1][2], S1*M[2][0] + C1*M[2][2]);
  798. t1 = T1;
  799. t2 = T2;
  800. t3 = T3;
  801. }
  802. template <typename T>
  803. GLM_FUNC_QUALIFIER void extractEulerAngleZYX(mat<4, 4, T, defaultp> const & M,
  804. T & t1,
  805. T & t2,
  806. T & t3)
  807. {
  808. T T1 = glm::atan2<T, defaultp>(M[0][1], M[0][0]);
  809. T C2 = glm::sqrt(M[1][2]*M[1][2] + M[2][2]*M[2][2]);
  810. T T2 = glm::atan2<T, defaultp>(-M[0][2], C2);
  811. T S1 = glm::sin(T1);
  812. T C1 = glm::cos(T1);
  813. T T3 = glm::atan2<T, defaultp>(S1*M[2][0] - C1*M[2][1], C1*M[1][1] - S1*M[1][0]);
  814. t1 = T1;
  815. t2 = T2;
  816. t3 = T3;
  817. }
  818. template <typename T>
  819. GLM_FUNC_QUALIFIER void extractEulerAngleZXY(mat<4, 4, T, defaultp> const & M,
  820. T & t1,
  821. T & t2,
  822. T & t3)
  823. {
  824. T T1 = glm::atan2<T, defaultp>(-M[1][0], M[1][1]);
  825. T C2 = glm::sqrt(M[0][2]*M[0][2] + M[2][2]*M[2][2]);
  826. T T2 = glm::atan2<T, defaultp>(M[1][2], C2);
  827. T S1 = glm::sin(T1);
  828. T C1 = glm::cos(T1);
  829. T T3 = glm::atan2<T, defaultp>(C1*M[2][0] + S1*M[2][1], C1*M[0][0] + S1*M[0][1]);
  830. t1 = T1;
  831. t2 = T2;
  832. t3 = T3;
  833. }
  834. }//namespace glm