lolengine
/
lol
дзеркало https://github.com/lolengine/lol
1
0
Форк 0
Код Проблеми 0 Релізи 0 Вікі Активність
Ви не можете вибрати більше 25 тем Теми мають розпочинатися з літери або цифри, можуть містити дефіси (-) і не повинні перевищувати 35 символів.
3156 Коміти
5 Гілки
21 MiB
 
 
 
Дерево: 01e1b2736d
Гілки Теги
${ item.name }
Створити гілку ${ searchTerm }
з '01e1b2736d'
${ noResults }
lol/include/lol/private/math/transform.h

596 рядки
16 KiB
Неформатований Звинувачення Історія 

  1. //

  2. //  Lol Engine

  3. //

  4. //  Copyright © 2010—2020 Sam Hocevar <sam@hocevar.net>

  5. //

  6. //  Lol Engine is free software. It comes without any warranty, to

  7. //  the extent permitted by applicable law. You can redistribute it

  8. //  and/or modify it under the terms of the Do What the Fuck You Want

  9. //  to Public License, Version 2, as published by the WTFPL Task Force.

  10. //  See http://www.wtfpl.net/ for more details.

  11. //

  12. 

  13. #pragma once

  14. 

  15. //

  16. // The complex, quaternion and dual quaternion classes

  17. // ———————————————————————————————————————————————————

  18. //

  19. 

  20. #include <lol/vector>

  21. #include <ostream> // std::ostream

  22. #include <cmath>   // std::atan2, std::sqrt

  23. 

  24. #include "ops.h"

  25. #include "matrix.h"

  26. 

  27. namespace lol

  28. {

  29. 

  30. /*

  31.  * 2-element transforms: complex numbers

  32.  */

  33. 

  34. template<typename T>

  35. struct [[nodiscard]] cmplx_t : public linear_ops::base<T>

  36. {

  37.     static int const count = 2;

  38.     typedef T scalar_element;

  39.     typedef T element;

  40.     typedef cmplx_t<T> type;

  41. 

  42.     inline constexpr cmplx_t() = default;

  43.     inline constexpr cmplx_t(cmplx_t<T> const &) = default;

  44.     inline constexpr cmplx_t(T X) : x(X), y(T(0)) {}

  45.     inline constexpr cmplx_t(T X, T Y) : x(X), y(Y) {}

  46. 

  47.     template<typename U>

  48.     explicit inline constexpr cmplx_t(cmplx_t<U> const &z)

  49.       : x(z.x), y(z.y) {}

  50. 

  51.     LOL_COMMON_MEMBER_OPS(x)

  52. 

  53.     inline cmplx_t<T> operator *(cmplx_t<T> const &val) const

  54.     {

  55.         return cmplx_t<T>(x * val.x - y * val.y, x * val.y + y * val.x);

  56.     }

  57. 

  58.     inline cmplx_t<T> operator *=(cmplx_t<T> const &val)

  59.     {

  60.         return *this = (*this) * val;

  61.     }

  62. 

  63.     inline cmplx_t<T> operator ~() const

  64.     {

  65.         return cmplx_t<T>(x, -y);

  66.     }

  67. 

  68.     template<typename U>

  69.     friend std::ostream &operator<<(std::ostream &stream, cmplx_t<U> const &v);

  70. 

  71.     T x, y;

  72. };

  73. 

  74. /*

  75.  * 4-element transforms: quaternions

  76.  */

  77. 

  78. template<typename T>

  79. struct [[nodiscard]] quat_t : public linear_ops::base<T>

  80. {

  81.     static int const count = 4;

  82.     typedef T scalar_element;

  83.     typedef T element;

  84.     typedef quat_t<T> type;

  85. 

  86.     /* Default constructor and copy constructor */

  87.     inline constexpr quat_t() = default;

  88.     inline constexpr quat_t(quat_t<T> const &) = default;

  89. 

  90.     /* Explicit constructor for type conversion */

  91.     template<typename U>

  92.     explicit inline constexpr quat_t(quat_t<U> const &q)

  93.       : w(q.w), x(q.x), y(q.y), z(q.z) {}

  94. 

  95.     /* Various explicit constructors */

  96.     explicit inline constexpr quat_t(T W, T X, T Y, T Z)

  97.       : w(W), x(X), y(Y), z(Z) {}

  98.     explicit inline constexpr quat_t(T W)

  99.       : w(W), x(0), y(0), z(0) {}

  100. 

  101.     /* Construct a unit quaternion from a pure rotation matrix */

  102.     explicit quat_t(mat_t<T,3,3> const &m)

  103.     {

  104.         using std::sqrt;

  105. 

  106.         T tr = m[0][0] + m[1][1] + m[2][2];

  107. 

  108.         if (tr > T(0))

  109.         {

  110.             T const p = T(0.5) * sqrt(T(1) + tr);

  111.             T const q = T(0.25) / p;

  112. 

  113.             w = p;

  114.             x = q * (m[1][2] - m[2][1]);

  115.             y = q * (m[2][0] - m[0][2]);

  116.             z = q * (m[0][1] - m[1][0]);

  117.         }

  118.         else

  119.         {

  120.             int i = (m[0][0] > m[1][1] && m[0][0] > m[2][2]) ? 0

  121.                   : (m[1][1] > m[2][2]) ? 1

  122.                   : 2;

  123.             int j = (i + 1) % 3, k = (i + 2) % 3;

  124. 

  125.             T const p = T(0.5) * sqrt(T(1) - tr + m[i][i] + m[i][i]);

  126.             T const q = T(0.25) / p;

  127. 

  128.             w              = q * (m[j][k] - m[k][j]);

  129.             (*this)[1 + i] = p;

  130.             (*this)[1 + j] = q * (m[i][j] + m[j][i]);

  131.             (*this)[1 + k] = q * (m[k][i] + m[i][k]);

  132.         }

  133.     }

  134. 

  135.     LOL_COMMON_MEMBER_OPS(w)

  136. 

  137.     inline quat_t operator *(quat_t const &val) const

  138.     {

  139.         vec_t<T,3> v1(x, y, z);

  140.         vec_t<T,3> v2(val.x, val.y, val.z);

  141.         vec_t<T,3> v3 = cross(v1, v2) + w * v2 + val.w * v1;

  142.         return quat_t(w * val.w - dot(v1, v2), v3.x, v3.y, v3.z);

  143.     }

  144. 

  145.     inline quat_t operator *=(quat_t const &val)

  146.     {

  147.         return *this = (*this * val);

  148.     }

  149. 

  150.     /* Create a unit quaternion representing a rotation around an axis. */

  151.     static quat_t rotate(T radians, T x, T y, T z);

  152.     static quat_t rotate(T radians, vec_t<T,3> const &v);

  153. 

  154.     /* Create a unit quaternion representing a rotation between two vectors.

  155.      * Input vectors need not be normalised. */

  156.     static quat_t rotate(vec_t<T,3> const &src, vec_t<T,3> const &dst);

  157. 

  158.     /* Convert from Euler angles. The axes in fromeuler_xyx are

  159.      * x, then y', then x", ie. the axes are attached to the model.

  160.      * If you want to rotate around static axes, just reverse the order

  161.      * of the arguments. Angle values are in radians. */

  162.     static quat_t fromeuler_xyx(vec_t<T,3> const &v);

  163.     static quat_t fromeuler_xzx(vec_t<T,3> const &v);

  164.     static quat_t fromeuler_yxy(vec_t<T,3> const &v);

  165.     static quat_t fromeuler_yzy(vec_t<T,3> const &v);

  166.     static quat_t fromeuler_zxz(vec_t<T,3> const &v);

  167.     static quat_t fromeuler_zyz(vec_t<T,3> const &v);

  168.     static quat_t fromeuler_xyx(T phi, T theta, T psi);

  169.     static quat_t fromeuler_xzx(T phi, T theta, T psi);

  170.     static quat_t fromeuler_yxy(T phi, T theta, T psi);

  171.     static quat_t fromeuler_yzy(T phi, T theta, T psi);

  172.     static quat_t fromeuler_zxz(T phi, T theta, T psi);

  173.     static quat_t fromeuler_zyz(T phi, T theta, T psi);

  174. 

  175.     /* Convert from Tait-Bryan angles (incorrectly called Euler angles,

  176.      * but since everyone does it…). The axes in fromeuler_xyz are

  177.      * x, then y', then z", ie. the axes are attached to the model.

  178.      * If you want to apply yaw around x, pitch around y, and roll

  179.      * around z, use fromeuler_xyz. Angle values are in radians.

  180.      * If you want to rotate around static axes, reverse the order in

  181.      * the function name (_zyx instead of _xyz) AND reverse the order

  182.      * of the arguments. */

  183.     static quat_t fromeuler_xyz(vec_t<T,3> const &v);

  184.     static quat_t fromeuler_xzy(vec_t<T,3> const &v);

  185.     static quat_t fromeuler_yxz(vec_t<T,3> const &v);

  186.     static quat_t fromeuler_yzx(vec_t<T,3> const &v);

  187.     static quat_t fromeuler_zxy(vec_t<T,3> const &v);

  188.     static quat_t fromeuler_zyx(vec_t<T,3> const &v);

  189.     static quat_t fromeuler_xyz(T phi, T theta, T psi);

  190.     static quat_t fromeuler_xzy(T phi, T theta, T psi);

  191.     static quat_t fromeuler_yxz(T phi, T theta, T psi);

  192.     static quat_t fromeuler_yzx(T phi, T theta, T psi);

  193.     static quat_t fromeuler_zxy(T phi, T theta, T psi);

  194.     static quat_t fromeuler_zyx(T phi, T theta, T psi);

  195. 

  196.     inline quat_t operator ~() const

  197.     {

  198.         return quat_t(w, -x, -y, -z);

  199.     }

  200. 

  201.     /* Transform vectors or points */

  202.     inline vec_t<T,3> transform(vec_t<T,3> const &v) const

  203.     {

  204.         quat_t p = quat_t(0, v.x, v.y, v.z);

  205.         quat_t q = *this * p / *this;

  206.         return vec_t<T,3>(q.x, q.y, q.z);

  207.     }

  208. 

  209.     inline vec_t<T,4> transform(vec_t<T,4> const &v) const

  210.     {

  211.         quat_t p = quat_t(0, v.x, v.y, v.z);

  212.         quat_t q = *this * p / *this;

  213.         return vec_t<T,4>(q.x, q.y, q.z, v.w);

  214.     }

  215. 

  216.     inline vec_t<T,3> operator *(vec_t<T,3> const &v) const

  217.     {

  218.         return transform(v);

  219.     }

  220. 

  221.     inline vec_t<T,4> operator *(vec_t<T,4> const &v) const

  222.     {

  223.         return transform(v);

  224.     }

  225. 

  226.     inline vec_t<T,3> axis()

  227.     {

  228.         vec_t<T,3> v(x, y, z);

  229.         T n2 = sqlength(v);

  230.         if (n2 <= (T)1e-6)

  231.             return vec_t<T,3>::axis_x;

  232.         return normalize(v);

  233.     }

  234. 

  235.     [[nodiscard]] inline T angle()

  236.     {

  237.         using std::atan2, std::sqrt;

  238. 

  239.         vec_t<T,3> v(x, y, z);

  240.         T n2 = sqlength(v);

  241.         if (n2 <= (T)1e-6)

  242.             return (T)0;

  243.         return (T)2 * atan2(sqrt(n2), w);

  244.     }

  245. 

  246.     template<typename U>

  247.     friend std::ostream &operator<<(std::ostream &stream, quat_t<U> const &v);

  248. 

  249.     /* XXX: storage order is wxyz, unlike vectors! */

  250.     T w, x, y, z;

  251. };

  252. 

  253. /*

  254.  * SQT transforms: scale / rotation / translation

  255.  */

  256. 

  257. template<typename T>

  258. struct [[nodiscard]] sqt_t

  259. {

  260.     /* Default constructor and copy constructor */

  261.     inline constexpr sqt_t() = default;

  262.     inline constexpr sqt_t(sqt_t<T> const &) = default;

  263.     inline constexpr sqt_t<T>& operator =(const sqt_t<T>&) = default;

  264. 

  265.     /* Explicit constructor for type conversion */

  266.     template<typename U>

  267.     explicit inline constexpr sqt_t(sqt_t<U> const &other)

  268.       : q(other.q), t(other.t), s(other.s) {}

  269. 

  270.     /* Various explicit constructors */

  271.     explicit inline constexpr sqt_t(T const &s_,

  272.                                     quat_t<T> const &q_,

  273.                                     vec_t<T,3> const &t_)

  274.       : q(q_), t(t_), s(s_) {}

  275. 

  276.     explicit inline constexpr sqt_t(T const &s_)

  277.       : q(1.f), t(0.f), s(s_) {}

  278. 

  279.     explicit inline constexpr sqt_t(quat_t<T> const &q_)

  280.       : q(q_), t(0.f), s(1.f) {}

  281. 

  282.     explicit inline constexpr sqt_t(vec_t<T,3> const &t_)

  283.       : q(1.f), t(t_), s(1.f) {}

  284. 

  285.     /* Transform vectors or points */

  286.     inline vec_t<T,3> transform(vec_t<T,3> const &v) const

  287.     {

  288.         return t + q.transform(s * v);

  289.     }

  290. 

  291.     inline vec_t<T,4> transform(vec_t<T,4> const &v) const

  292.     {

  293.         // XXX: needs serious testing for w != 1

  294.         vec_t<T,4> tmp = q.transform(vec_t<T,4>(s * v.xyz, v.w));

  295.         return vec_t<T,4>(tmp.xyz, 0.f) + vec_t<T,4>(t, 1.f) * tmp.w;

  296.     }

  297. 

  298.     inline vec_t<T,3> operator *(vec_t<T,3> const &v) const

  299.     {

  300.         return transform(v);

  301.     }

  302. 

  303.     inline vec_t<T,4> operator *(vec_t<T,4> const &v) const

  304.     {

  305.         return transform(v);

  306.     }

  307. 

  308.     /* Compose two SQTs together */

  309.     inline sqt_t<T> operator *(sqt_t<T> const &other) const

  310.     {

  311.         return sqt_t<T>(s * other.s,

  312.                         q * other.q,

  313.                         transform(other.t));

  314.     }

  315. 

  316.     quat_t<T> q;

  317.     vec_t<T,3> t;

  318.     T s;

  319. };

  320. 

  321. /*

  322.  * stdstream method implementations

  323.  */

  324. 

  325. template<typename U>

  326. std::ostream &operator<<(std::ostream &stream, cmplx_t<U> const &c)

  327. {

  328.     return stream << "(" << c.x << ", " << c.y << ")";

  329. }

  330. 

  331. template<typename U>

  332. std::ostream &operator<<(std::ostream &stream, quat_t<U> const &q)

  333. {

  334.     return stream << "(" << q.w << ", " << q.x << ", "

  335.                          << q.y << ", " << q.z << ")";

  336. }

  337. 

  338. /*

  339.  * Common operations on transforms

  340.  */

  341. 

  342. template<typename T> [[nodiscard]]

  343. static inline T dot(cmplx_t<T> const &t1, cmplx_t<T> const &t2)

  344. {

  345.     T ret(0);

  346.     for (size_t i = 0; i < sizeof(t1) / sizeof(T); ++i)

  347.         ret += t1[i] * t2[i];

  348.     return ret;

  349. }

  350. 

  351. template<typename T> [[nodiscard]]

  352. static inline T sqlength(cmplx_t<T> const &t)

  353. {

  354.     return dot(t, t);

  355. }

  356. 

  357. template<typename T> [[nodiscard]]

  358. static inline T length(cmplx_t<T> const &t)

  359. {

  360.     using std::sqrt;

  361.     /* FIXME: this is not very nice */

  362.     return (T)sqrt((double)sqlength(t));

  363. }

  364. 

  365. template<typename T> [[nodiscard]]

  366. static inline T norm(cmplx_t<T> const &t)

  367. {

  368.     return length(t);

  369. }

  370. 

  371. template<typename T>

  372. static inline cmplx_t<T> normalize(cmplx_t<T> const &z)

  373. {

  374.     T norm = (T)length(z);

  375.     return norm ? z / norm : cmplx_t<T>(T(0));

  376. }

  377. 

  378. /* XXX: duplicate */

  379. 

  380. template<typename T> [[nodiscard]]

  381. static inline T dot(quat_t<T> const &t1, quat_t<T> const &t2)

  382. {

  383.     T ret(0);

  384.     for (size_t i = 0; i < sizeof(t1) / sizeof(T); ++i)

  385.         ret += t1[i] * t2[i];

  386.     return ret;

  387. }

  388. 

  389. template<typename T> [[nodiscard]]

  390. static inline T sqlength(quat_t<T> const &t)

  391. {

  392.     return dot(t, t);

  393. }

  394. 

  395. template<typename T> [[nodiscard]]

  396. static inline T length(quat_t<T> const &t)

  397. {

  398.     using std::sqrt;

  399.     /* FIXME: this is not very nice */

  400.     return (T)sqrt(sqlength(t));

  401. }

  402. 

  403. template<typename T> [[nodiscard]]

  404. static inline T norm(quat_t<T> const &t)

  405. {

  406.     return length(t);

  407. }

  408. 

  409. template<typename T>

  410. static inline quat_t<T> normalize(quat_t<T> const &z)

  411. {

  412.     T norm = (T)length(z);

  413.     return norm ? z / norm : quat_t<T>(T(0));

  414. }

  415. 

  416. /*

  417.  * Complex numbers only

  418.  */

  419. 

  420. template<typename T>

  421. static inline cmplx_t<T> inverse(cmplx_t<T> const &z)

  422. {

  423.     return ~z / sqlength(z);

  424. }

  425. 

  426. template<typename T>

  427. static inline cmplx_t<T> operator /(T a, cmplx_t<T> const &b)

  428. {

  429.     return a * inverse(b);

  430. }

  431. 

  432. template<typename T>

  433. static inline cmplx_t<T> operator /(cmplx_t<T> a, cmplx_t<T> const &b)

  434. {

  435.     return a * inverse(b);

  436. }

  437. 

  438. template<typename T> [[nodiscard]]

  439. static inline bool operator ==(cmplx_t<T> const &a, T b)

  440. {

  441.     return (a.x == b) && !a.y;

  442. }

  443. 

  444. template<typename T> [[nodiscard]]

  445. static inline bool operator !=(cmplx_t<T> const &a, T b)

  446. {

  447.     return (a.x != b) || a.y;

  448. }

  449. 

  450. template<typename T> [[nodiscard]]

  451. static inline bool operator ==(T a, cmplx_t<T> const &b) { return b == a; }

  452. 

  453. template<typename T> [[nodiscard]]

  454. static inline bool operator !=(T a, cmplx_t<T> const &b) { return b != a; }

  455. 

  456. /*

  457.  * Quaternions only

  458.  */

  459. 

  460. template<typename T>

  461. static inline quat_t<T> inverse(quat_t<T> const &q)

  462. {

  463.     return ~q / sqlength(q);

  464. }

  465. 

  466. template<typename T>

  467. static inline quat_t<T> operator /(T x, quat_t<T> const &y)

  468. {

  469.     return x * inverse(y);

  470. }

  471. 

  472. template<typename T>

  473. static inline quat_t<T> operator /(quat_t<T> const &x, quat_t<T> const &y)

  474. {

  475.     return x * inverse(y);

  476. }

  477. 

  478. template<typename T>

  479. extern quat_t<T> slerp(quat_t<T> const &qa, quat_t<T> const &qb, T f);

  480. 

  481. /*

  482.  * SQTs only

  483.  */

  484. 

  485. template<typename T>

  486. static inline sqt_t<T> inverse(sqt_t<T> const &tr)

  487. {

  488.     auto inv_s = T(1) / tr.s;

  489.     auto inv_q = inverse(tr.q);

  490.     return sqt_t<T>(inv_s, inv_q, inv_q * tr.t * -inv_s);

  491. }

  492. 

  493. template<typename T>

  494. static inline sqt_t<T> operator /(sqt_t<T> const &x, sqt_t<T> const &y)

  495. {

  496.     return x * inverse(y);

  497. }

  498. 

  499. //

  500. // Generic GLSL-like type names

  501. //

  502. 

  503. #define T_(tleft, tright, suffix) \

  504.     typedef tleft half tright f16##suffix; \

  505.     typedef tleft float tright suffix; \

  506.     typedef tleft double tright d##suffix; \

  507.     typedef tleft ldouble tright f128##suffix; \

  508.     typedef tleft int8_t tright i8##suffix; \

  509.     typedef tleft uint8_t tright u8##suffix; \

  510.     typedef tleft int16_t tright i16##suffix; \

  511.     typedef tleft uint16_t tright u16##suffix; \

  512.     typedef tleft int32_t tright i##suffix; \

  513.     typedef tleft uint32_t tright u##suffix; \

  514.     typedef tleft int64_t tright i64##suffix; \

  515.     typedef tleft uint64_t tright u64##suffix; \

  516.     typedef tleft real tright r##suffix;

  517. 

  518. /* Idiotic hack to put "," inside a macro argument */

  519. #define C_ ,

  520. 

  521. T_(mat_t<, C_ 2 C_ 2>, mat2)

  522. T_(mat_t<, C_ 3 C_ 3>, mat3)

  523. T_(mat_t<, C_ 4 C_ 4>, mat4)

  524. 

  525. T_(mat_t<, C_ 2 C_ 3>, mat2x3)

  526. T_(mat_t<, C_ 2 C_ 4>, mat2x4)

  527. T_(mat_t<, C_ 3 C_ 2>, mat3x2)

  528. T_(mat_t<, C_ 3 C_ 4>, mat3x4)

  529. T_(mat_t<, C_ 4 C_ 2>, mat4x2)

  530. T_(mat_t<, C_ 4 C_ 3>, mat4x3)

  531. 

  532. T_(cmplx_t<, >, cmplx)

  533. T_(quat_t<, >, quat)

  534. T_(sqt_t<, >, sqt)

  535. 

  536. #undef C_

  537. #undef T_

  538. 

  539. static_assert(sizeof(cmplx) == 8, "sizeof(cmplx) == 8");

  540. static_assert(sizeof(dcmplx) == 16, "sizeof(dcmplx) == 16");

  541. 

  542. static_assert(sizeof(quat) == 16, "sizeof(quat) == 16");

  543. static_assert(sizeof(dquat) == 32, "sizeof(dquat) == 32");

  544. 

  545. static_assert(sizeof(imat2) == 16, "sizeof(imat2) == 16");

  546. static_assert(sizeof(mat2) == 16, "sizeof(mat2) == 16");

  547. static_assert(sizeof(dmat2) == 32, "sizeof(dmat2) == 32");

  548. 

  549. static_assert(sizeof(imat3) == 36, "sizeof(imat3) == 36");

  550. static_assert(sizeof(mat3) == 36, "sizeof(mat3) == 36");

  551. static_assert(sizeof(dmat3) == 72, "sizeof(dmat3) == 72");

  552. 

  553. static_assert(sizeof(imat4) == 64, "sizeof(imat4) == 64");

  554. static_assert(sizeof(mat4) == 64, "sizeof(mat4) == 64");

  555. static_assert(sizeof(dmat4) == 128, "sizeof(dmat4) == 128");

  556. 

  557. //

  558. // HLSL/Cg-compliant type names

  559. //

  560. 

  561. typedef mat2 float2x2;

  562. typedef mat3 float3x3;

  563. typedef mat4 float4x4;

  564. typedef mat2x3 float2x3;

  565. typedef mat2x4 float2x4;

  566. typedef mat3x2 float3x2;

  567. typedef mat3x4 float3x4;

  568. typedef mat4x2 float4x2;

  569. typedef mat4x3 float4x3;

  570. 

  571. typedef f16mat2 half2x2;

  572. typedef f16mat3 half3x3;

  573. typedef f16mat4 half4x4;

  574. typedef f16mat2x3 half2x3;

  575. typedef f16mat2x4 half2x4;

  576. typedef f16mat3x2 half3x2;

  577. typedef f16mat3x4 half3x4;

  578. typedef f16mat4x2 half4x2;

  579. typedef f16mat4x3 half4x3;

  580. 

  581. typedef imat2 int2x2;

  582. typedef imat3 int3x3;

  583. typedef imat4 int4x4;

  584. typedef imat2x3 int2x3;

  585. typedef imat2x4 int2x4;

  586. typedef imat3x2 int3x2;

  587. typedef imat3x4 int3x4;

  588. typedef imat4x2 int4x2;

  589. typedef imat4x3 int4x3;

  590. 

  591. } // namespace lol

  592. 

  593. #include "matrix.ipp"

  594. #include "transform.ipp"