quaternion.inl - Engine C API Reference

quaternion.inl
  1. #include "vector3.h"
  2. #include "matrix4x4.h"
  4. namespace stingray_plugin_foundation {
  6. // Operators
  7. __forceinline Quaternion operator +(const Quaternion &q) {
  8.     return q;
  9. }
  11. __forceinline Quaternion operator -(const Quaternion &q) {
  12.     return quaternion(-q.x, -q.y, -q.z, q.w);
  13. }
  15. __forceinline Quaternion operator +(const Quaternion &lhs, const Quaternion &rhs) {
  16.     return quaternion(lhs.x+rhs.x, lhs.y+rhs.y, lhs.z+rhs.z, lhs.w+rhs.w);
  17. }
  19. __forceinline Quaternion operator -(const Quaternion &lhs, const Quaternion &rhs) {
  20.     return quaternion(lhs.x-rhs.x, lhs.y-rhs.y, lhs.z-rhs.z, lhs.w-rhs.w);
  21. }
  23. __forceinline Quaternion operator *(const Quaternion &lhs, const Quaternion &rhs) {
  24.     return quaternion(
  25.         lhs.w*rhs.x + lhs.x*rhs.w + lhs.y*rhs.z - lhs.z*rhs.y,
  26.         lhs.w*rhs.y + lhs.y*rhs.w + lhs.z*rhs.x - lhs.x*rhs.z,
  27.         lhs.w*rhs.z + lhs.z*rhs.w + lhs.x*rhs.y - lhs.y*rhs.x,
  28.         lhs.w*rhs.w - lhs.x*rhs.x - lhs.y*rhs.y - lhs.z*rhs.z
  29.     );
  30. }
  32. __forceinline Quaternion operator *(const Quaternion &lhs, float rhs) {
  33.     return quaternion(lhs.x*rhs, lhs.y*rhs, lhs.z*rhs, lhs.w*rhs);
  34. }
  36. __forceinline Quaternion operator *(float lhs, const Quaternion &rhs) {
  37.     return rhs * lhs;
  38. }
  40. __forceinline Quaternion operator /(const Quaternion &lhs, float rhs) {
  41.     return quaternion(lhs.x/rhs, lhs.y/rhs, lhs.z/rhs, lhs.w/rhs);
  42. }
  44. __forceinline void operator +=(Quaternion &lhs, const Quaternion &rhs) {
  45.     lhs.x+=rhs.x; lhs.y+=rhs.y; lhs.z+=rhs.z; lhs.w+=rhs.w;
  46. }
  48. __forceinline void operator -=(Quaternion &lhs, const Quaternion &rhs) {
  49.     lhs.x-=rhs.x; lhs.y-=rhs.y; lhs.z-=rhs.z; lhs.w-=rhs.w;
  50. }
  52. __forceinline void operator *=(Quaternion &lhs, const Quaternion &rhs) {
  53.     lhs.x*=rhs.x; lhs.y*=rhs.y; lhs.z*=rhs.z; lhs.w*=rhs.w;
  55. }
  57. __forceinline void operator *=(Quaternion &lhs, float rhs) {
  58.     lhs.x*=rhs; lhs.y*=rhs; lhs.z*=rhs; lhs.w*=rhs;
  59. }
  61. __forceinline bool operator==(const Quaternion &lhs, const Quaternion &rhs)
  62. {
  63.     return lhs.x == rhs.x && lhs.y == rhs.y && lhs.z == rhs.z && lhs.w == rhs.w;
  64. }
  66. __forceinline bool operator!=(const Quaternion &lhs, const Quaternion &rhs)
  67. {
  68.     return !(lhs == rhs);
  69. }
  71. __forceinline bool operator< (const Quaternion &lhs, const Quaternion &rhs)
  72. {
  73.     if (lhs.x != rhs.x) return lhs.x < rhs.x;
  74.     if (lhs.y != rhs.y) return lhs.y < rhs.y;
  75.     if (lhs.z != rhs.z) return lhs.z < rhs.z;
  76.     return lhs.w < rhs.w;
  77. }
  79. __forceinline bool operator<=(const Quaternion &lhs, const Quaternion &rhs)
  80. {
  81.     return (lhs < rhs) || (lhs == rhs);
  82. }
  84. __forceinline bool operator> (const Quaternion &lhs, const Quaternion &rhs)
  85. {
  86.     return (rhs < lhs);
  87. }
  89. __forceinline bool operator>=(const Quaternion &lhs, const Quaternion &rhs)
  90. {
  91.     return !(lhs < rhs);
  92. }
  94. // Creation Methods
  96. __forceinline Quaternion quaternion(const float * list) {
  97.     Quaternion q = { *list, *(list+1), *(list+2), *(list+3) };
  98.     return q;
  99. }
  101. __forceinline Quaternion quaternion(float x, float y, float z, float w) {
  102.     Quaternion q = { x, y, z, w };
  103.     return q;
  104. }
  106. __forceinline const Quaternion &quaternion_identity() {
  107.     static Quaternion q = { 0.f, 0.f, 0.f, 1.f };
  108.     return q;
  109. }
  111. __forceinline Quaternion quaternion(const Vector3 &axis, float theta) {
  112.     Quaternion q;
  114.     float halftheta = theta * 0.5f;
  115.     float cos_ht = cosf( halftheta );
  116.     float sin_ht = sinf( halftheta );
  118.     q.x = axis.x * sin_ht;
  119.     q.y = axis.y * sin_ht;
  120.     q.z = axis.z * sin_ht;
  121.     q.w = cos_ht;
  123.     return q;
  124. }
  126. // Methods
  127. __forceinline Quaternion normalize(const Quaternion &q) {
  128.     float l = math::square_root( dot(q,q) );
  130.     if(l == 0.0f)
  131.         return quaternion(0.f, 0.f, 0.f, 1.f);
  132.     else {
  133.         float c = 1.0f / l;
  134.         return quaternion(q.x*c, q.y*c, q.z*c, q.w*c);
  135.     }
  136. }
  138. __forceinline Quaternion inverse(const Quaternion &q) {
  139.     float l = dot(q,q);
  140.     if (l == 0.0f)
  141.         l = 1.0f;
  142.     float norminv = -1.0f / l;
  143.     return quaternion(q.x*norminv, q.y*norminv, q.z*norminv, q.w*-norminv);
  144. }
  146. __forceinline Quaternion conjugate(const Quaternion &q) {
  147.     return quaternion(-q.x, -q.y, -q.z, q.w);
  148. }
  150. __forceinline float dot(const Quaternion &q1, const Quaternion &q2) {
  151.     return q1.x*q2.x + q1.y*q2.y + q1.z*q2.z + q1.w*q2.w;
  152. }
  154. __forceinline Quaternion slerp(const Quaternion &from, const Quaternion &to, float t) {
  155.     Quaternion q;
  156.     Quaternion q3;
  158.     float d = dot(from, to);
  159.     if (d < 0) {
  160.         d = -d;
  161.         q3 = to * -1.0f;
  162.     } else
  163.         q3 = to;
  165.     // Use linear interpolation for small angles.
  166.     if (d < 0.95f) {
  167.         float clamped_dot = math::clamp(d, -1.0f, 1.0f);
  168.         float angle = acosf(clamped_dot);
  169.         float sina, sinat, sinaomt;
  170.         sina = sinf(angle);
  171.         sinat = sinf(angle*t);
  172.         sinaomt = sinf(angle*(1-t));
  173.         q = (from*sinaomt+q3*sinat)/sina;
  174.     } else
  175.         q = nlerp(from, q3,t);
  177.     return q;
  179. }
  181. __forceinline Quaternion nlerp(const Quaternion &from, const Quaternion &to, float t) {
  182.     Quaternion q;
  183.     if(dot(from, to) < 0)
  184.         q = ((to + from) * t) - from;
  185.     else
  186.         q = from + ((to - from) * t);
  187.     return normalize(q);
  188. }
  190. // lerp for quaternions defaults to nlerp
  191. __forceinline Quaternion lerp(const Quaternion &from, const Quaternion &to, float t) {
  192.     Quaternion q;
  193.     if(dot(from, to) < 0)
  194.         q = ((to + from) * t) - from;
  195.     else
  196.         q = from + ((to - from) * t);
  197.     return normalize(q);
  198. }
  200. // Scales the angle in the rotation the quaternion represents by t.
  201. __forceinline Quaternion scale_angle(const Quaternion &from, float t)
  202. {
  203.     Vector3 axis;
  204.     float theta;
  205.     decompose(from, &axis, &theta);
  206.     return quaternion(axis, theta*t);
  207. }
  209. __forceinline bool is_zero(const Quaternion &q)
  210. {
  211.     return q.x == 0 && q.y == 0 && q.z == 0 && q.w == 0;
  212. }
  214. __forceinline float norm(const Quaternion &q)
  215. {
  216.     return math::square_root(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w);
  217. }
  219. __forceinline float one_norm(const Quaternion &q)
  220. {
  221.     return math::abs(q.x) + math::abs(q.y) + math::abs(q.z) + math::abs(q.w);
  222. }
  224. __forceinline float infinity_norm(const Quaternion &q)
  225. {
  226.     return math::max<float>(
  227.         math::max<float>(math::abs(q.x),
  228.         math::abs(q.y)),
  229.         math::max<float>(math::abs(q.z),
  230.         math::abs(q.w)) );
  231. }
  233. __forceinline Vector3 rotate(const Quaternion &q, const Vector3 &v)
  234. {
  235.     Quaternion v_quat = quaternion(v.x, v.y, v.z, 0.0f);
  236.     Quaternion result(q * v_quat * conjugate(q));
  237.     return vector3(result.x, result.y, result.z);
  238. }
  240. // Conversion Methods
  241. __forceinline Matrix4x4 matrix4x4(const Quaternion &q) {
  242.     const float d = dot(q,q);
  243.     float s;
  244.     if(d)
  245.         s = 2.0f / d;
  246.     else
  247.         s = 1.0f;
  249.     const float xs = q.x * s;
  250.     const float ys = q.y * s;
  251.     const float zs = q.z * s;
  252.     const float wx = q.w * xs;
  253.     const float wy = q.w * ys;
  254.     const float wz = q.w * zs;
  255.     const float xx = q.x * xs;
  256.     const float xy = q.x * ys;
  257.     const float xz = q.x * zs;
  258.     const float yy = q.y * ys;
  259.     const float yz = q.y * zs;
  260.     const float zz = q.z * zs;
  262.     Matrix4x4 m = {
  263.         (1.0f - yy - zz), (xy + wz), (xz - wy), 0.0f,
  264.         (xy - wz), (1.0f - xx - zz), (yz + wx), 0.0f,
  265.         (xz + wy), (yz - wx), (1.0f - xx - yy), 0.0f,
  266.         0.f, 0.f, 0.f, 1.f
  267.     };
  269.     return m;
  270. }
  272. __forceinline Matrix4x4 matrix4x4(const Quaternion &q, const Vector3 &p)
  273. {
  274.     const float d = dot(q,q);
  275.     float s;
  276.     if(d)
  277.         s = 2.0f / d;
  278.     else
  279.         s = 1.0f;
  281.     const float xs = q.x * s;
  282.     const float ys = q.y * s;
  283.     const float zs = q.z * s;
  284.     const float wx = q.w * xs;
  285.     const float wy = q.w * ys;
  286.     const float wz = q.w * zs;
  287.     const float xx = q.x * xs;
  288.     const float xy = q.x * ys;
  289.     const float xz = q.x * zs;
  290.     const float yy = q.y * ys;
  291.     const float yz = q.y * zs;
  292.     const float zz = q.z * zs;
  294.     Matrix4x4 m = {
  295.         (1.0f - yy - zz), (xy + wz), (xz - wy), 0.0f,
  296.         (xy - wz), (1.0f - xx - zz), (yz + wx), 0.0f,
  297.         (xz + wy), (yz - wx), (1.0f - xx - yy), 0.0f,
  298.         p.x, p.y, p.z, 1.f
  299.     };
  301.     return m;
  302. }
  304. __forceinline void decompose(const Quaternion &q, Vector3 *axis, float *theta)
  305. {
  306.     Vector3 a = vector3(q.x, q.y, q.z);
  307.     float s = length(a);
  308.     float c = q.w;
  309.     float half_theta = atan2f(s, c);
  310.     *theta = half_theta * 2.0f;
  311.     *axis = s ? a / s : vector3(1,0,0);
  312. }
  314. __forceinline float angle(const Quaternion &q)
  315. {
  316.     Vector3 a = vector3(q.x, q.y, q.z);
  317.     float s = length(a);
  318.     float c = q.w;
  319.     float half_theta = atan2f(s, c);
  320.     return half_theta * 2.0f;
  321. }
  323. __forceinline void lerp(const Matrix4x4 &m1, const Matrix4x4 &m2, float t, Matrix4x4 &m)
  324. {
  325.     Vector3 p = lerp(translation(m1), translation(m2), t);
  326.     Quaternion q = lerp(quaternion(m1), quaternion(m2), t);
  327.     m = matrix4x4(q, p);
  328. }
  330. inline Quaternion quaternion_orthogonal(const Matrix4x4 &mat) {
  331.     static const int kx = 0;
  332.     static const int ky = 1;
  333.     static const int kz = 2;
  334.     static const int kw = 3;
  336.     struct ShadowMatrix {
  337.         float m[4][4];
  338.     };
  339.     const ShadowMatrix* m = reinterpret_cast<const ShadowMatrix*>(&mat);
  340.     Quaternion r;
  341.     float *q = reinterpret_cast<float *>(&r);
  343.     // This code can be optimized for m[kw][kw] = 1, which
  344.     // should always be true.  This optimization is excluded
  345.     // here for clarity.
  346.     float trace = m->m[kx][kx] + m->m[ky][ky] + m->m[kz][kz] + m->m[kw][kw];
  347.     float four_d;
  348.     int i,j,k;
  350.     if (trace >= 1.0) // w >= 0.5
  351.     {
  352.         four_d = 2.0f*math::square_root(trace);
  353.         q[kw] = -four_d / 4.0f;
  354.         const float one_over_coeff = 1.0f / four_d;
  355.         q[kx] = (m->m[kz][ky] - m->m[ky][kz])*one_over_coeff;
  356.         q[ky] = (m->m[kx][kz] - m->m[kz][kx])*one_over_coeff;
  357.         q[kz] = (m->m[ky][kx] - m->m[kx][ky])*one_over_coeff;
  358.     }
  359.     else
  360.     {
  361.         // Find the largest component
  362.         if (m->m[kx][kx] > m->m[ky][ky])
  363.             i = kx;
  364.         else
  365.             i = ky;
  366.         if (m->m[kz][kz] > m->m[i][i])
  367.             i = kz;
  369.         // Set j and k to next two components
  370.         j = (1 << i) & 3;
  371.         k = (1 << j) & 3;
  373.         four_d = 2.0f*math::square_root(m->m[i][i] - m->m[j][j] - m->m[k][k] + 1.0f );
  374.         q[i] = four_d/4.0f;
  376.         const float one_over_coeff = 1.0f / four_d;
  377.         q[j] = (m->m[j][i] + m->m[i][j])*one_over_coeff;
  378.         q[k] = (m->m[k][i] + m->m[i][k])*one_over_coeff;
  379.         q[kw] = -(m->m[k][j] - m->m[j][k])*one_over_coeff;
  380.     }
  382.     return r;
  383. }
  385. inline Quaternion quaternion(const Matrix4x4 &mat)
  386. {
  387.     static const int kx = 0;
  388.     static const int ky = 1;
  389.     static const int kz = 2;
  390.     static const int kw = 3;
  392.     const Vector3 scale = stingray_plugin_foundation::scale(mat);
  393.     const float *s = reinterpret_cast<const float *>(&scale);
  395.     // If scale is small return an identity quaternion
  396.     const float eps = 1e-5;
  397.     if (scale.x > -eps && scale.x < eps || scale.y > -eps && scale.y < eps || scale.z > -eps && scale.z < eps)
  398.         return quaternion_identity();
  400.     struct ShadowMatrix {
  401.         float m[4][4];
  402.     };
  403.     const ShadowMatrix* m = reinterpret_cast<const ShadowMatrix*>(&mat);
  404.     Quaternion r;
  405.     float *q = reinterpret_cast<float *>(&r);
  407.     // This code can be optimized for m[kw][kw] = 1, which
  408.     // should always be true.  This optimization is excluded
  409.     // here for clarity.
  410.     const float trace = m->m[kx][kx] / s[kx] + m->m[ky][ky] / s[ky] + m->m[kz][kz] / s[kz] + m->m[kw][kw];
  412.     if (trace >= 1.0) // w >= 0.5
  413.     {
  414.         const float four_d = 2.0f*math::square_root(trace);
  415.         q[kw] = -four_d / 4.0f;
  416.         const float one_over_coeff = 1.0f / four_d;
  417.         q[kx] = (m->m[kz][ky] / s[kz] - m->m[ky][kz] / s[ky])*one_over_coeff;
  418.         q[ky] = (m->m[kx][kz] / s[kx] - m->m[kz][kx] / s[kz])*one_over_coeff;
  419.         q[kz] = (m->m[ky][kx] / s[ky] - m->m[kx][ky] / s[kx])*one_over_coeff;
  420.     }
  421.     else
  422.     {
  423.         // Find the largest component
  424.         int i;
  425.         if (m->m[kx][kx] / s[kx] > m->m[ky][ky] / s[ky])
  426.             i = kx;
  427.         else
  428.             i = ky;
  429.         if (m->m[kz][kz] / s[kz] > m->m[i][i] / s[i])
  430.             i = kz;
  432.         // Set j and k to next two components
  433.         const int j = (1 << i) & 3;
  434.         const int k = (1 << j) & 3;
  436.         const float four_d = 2.0f*math::square_root(m->m[i][i] / s[i] - m->m[j][j] / s[j] - m->m[k][k] / s[k] + 1.0f );
  437.         q[i] = four_d/4.0f;
  439.         const float one_over_coeff = 1.0f / four_d;
  440.         q[j] = (m->m[j][i] / s[j] + m->m[i][j] / s[i])*one_over_coeff;
  441.         q[k] = (m->m[k][i] / s[k] + m->m[i][k] / s[i])*one_over_coeff;
  442.         q[kw] = -(m->m[k][j] / s[k] - m->m[j][k] / s[j])*one_over_coeff;
  443.     }
  445.     return r;
  446. }
  448. inline Quaternion quaternion(const Matrix3x3 &mat)
  449. {
  450.     static const int kx = 0;
  451.     static const int ky = 1;
  452.     static const int kz = 2;
  453.     static const int kw = 3;
  455.     struct ShadowMatrix {
  456.         float m[3][3];
  457.     };
  458.     const ShadowMatrix* m = reinterpret_cast<const ShadowMatrix*>(&mat);
  459.     Quaternion r;
  460.     float *q = reinterpret_cast<float *>(&r);
  462.     float trace = m->m[kx][kx] + m->m[ky][ky] + m->m[kz][kz] + 1;
  463.     float four_d;
  464.     int i,j,k;
  466.     if (trace >= 1.0) // w >= 0.5
  467.     {
  468.         four_d = 2.0f*math::square_root(trace);
  469.         q[kw] = -four_d / 4.0f;
  470.         const float one_over_coeff = 1.0f / four_d;
  471.         q[kx] = (m->m[kz][ky] - m->m[ky][kz])*one_over_coeff;
  472.         q[ky] = (m->m[kx][kz] - m->m[kz][kx])*one_over_coeff;
  473.         q[kz] = (m->m[ky][kx] - m->m[kx][ky])*one_over_coeff;
  474.     }
  475.     else
  476.     {
  477.         // Find the largest component
  478.         if (m->m[kx][kx] > m->m[ky][ky])
  479.             i = kx;
  480.         else
  481.             i = ky;
  482.         if (m->m[kz][kz] > m->m[i][i])
  483.             i = kz;
  485.         // Set j and k to next two components
  486.         j = (1 << i) & 3;
  487.         k = (1 << j) & 3;
  489.         four_d = 2.0f*math::square_root(m->m[i][i] - m->m[j][j] - m->m[k][k] + 1.0f );
  490.         q[i] = four_d/4.0f;
  492.         const float one_over_coeff = 1.0f / four_d;
  493.         q[j] = (m->m[j][i] + m->m[i][j])*one_over_coeff;
  494.         q[k] = (m->m[k][i] + m->m[i][k])*one_over_coeff;
  495.         q[kw] = -(m->m[k][j] - m->m[j][k])*one_over_coeff;
  496.     }
  498.     return r;
  499. }
  501. inline Quaternion quaternion(const Vector3 &y, const Vector3 &z)
  502. {
  503.     Vector3 x = cross(y,z);
  504.     Matrix4x4 tm = matrix4x4_identity();
  505.     x_axis(tm) = x;
  506.     y_axis(tm) = y;
  507.     z_axis(tm) = z;
  508.     return quaternion_orthogonal(tm);
  509. }
  511. }
Please send us your comment about this page