TurtleBrains  0.3.5
High quality, portable, C++ framework for rapid 2D game development.
tb_matrix_quaternion.hpp
1 
9 #ifndef TurtleBrains_MatrixQuaternion_hpp
10 #define TurtleBrains_MatrixQuaternion_hpp
11 
12 #include <turtle_brains/math/tb_matrix.hpp>
13 #include <turtle_brains/math/tb_quaternion.hpp>
14 
15 //
16 // This needs more unit testing to confirm it -really- works, but, I find it odd that I had to transpose the resource
17 // during FromMatrix, and _NOT_ transponse from the resource during FromQuaternion. Doing the transpose could make sense
18 // if they used ColumnMajor and TurtleBrains used RowMajor (or opposite), but then I'd expect to have done the same
19 // transposition in both functions...
20 //
21 
22 inline TurtleBrains::Math::Unstable::Quaternion TurtleBrains::Math::Unstable::Quaternion::FromMatrix(const TurtleBrains::Math::Matrix3& matrix)
23 { //http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
24  //https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2015/01/matrix-to-quat.pdf
25  if (matrix(2, 2) < 0.0f)
26  {
27  if (matrix(0, 0) > matrix(1, 1))
28  {
29  const float t = (1.0f + matrix(0, 0) - matrix(1, 1) - matrix(2, 2));
30  Quaternion q(t, matrix(1, 0) + matrix(0, 1), matrix(0, 2) + matrix(2, 0), matrix(2, 1) - matrix(1, 2));
31  q *= (0.5f / sqrt(t));
32  return q;
33  }
34  else
35  {
36  const float t = (1.0f - matrix(0, 0) + matrix(1, 1) - matrix(2, 2));
37  Quaternion q(matrix(1, 0) + matrix(0, 1), t, matrix(2, 1) + matrix(1, 2), matrix(0, 2) - matrix(2, 0));
38  q *= (0.5f / sqrt(t));
39  return q;
40  }
41  }
42  else
43  {
44  if (matrix(0, 0) < -matrix(1, 1))
45  {
46  const float t = (1.0f - matrix(0, 0) - matrix(1, 1) + matrix(2, 2));
47  Quaternion q(matrix(0, 2) + matrix(2, 0), matrix(2, 1) + matrix(1, 2), t, matrix(1, 0) - matrix(0, 1));
48  q *= (0.5f / sqrt(t));
49  return q;
50  }
51  else
52  {
53  const float t = (1.0f + matrix(0, 0) + matrix(1, 1) + matrix(2, 2));
54  Quaternion q(matrix(2, 1) - matrix(1, 2), matrix(0, 2) - matrix(2, 0), matrix(1, 0) - matrix(0, 1), t);
55  q *= (0.5f / sqrt(t));
56  return q;
57  }
58  }
59 }
60 
61 inline TurtleBrains::Math::Unstable::Quaternion TurtleBrains::Math::Unstable::Quaternion::FromMatrix(const TurtleBrains::Math::Matrix4& matrix)
62 {
63  if (matrix(2, 2) < 0.0f)
64  {
65  if (matrix(0, 0) > matrix(1, 1))
66  {
67  const float t = (1.0f + matrix(0, 0) - matrix(1, 1) - matrix(2, 2));
68  Quaternion q(t, matrix(1, 0) + matrix(0, 1), matrix(0, 2) + matrix(2, 0), matrix(2, 1) - matrix(1, 2));
69  q *= 0.5f / sqrt(t);
70  return q;
71  }
72  else
73  {
74  const float t = (1.0f - matrix(0, 0) + matrix(1, 1) - matrix(2, 2));
75  Quaternion q(matrix(1, 0) + matrix(0, 1), t, matrix(2, 1) + matrix(1, 2), matrix(0, 2) - matrix(2, 0));
76  q *= 0.5f / sqrt(t);
77  return q;
78  }
79  }
80  else
81  {
82  if (matrix(0, 0) < -matrix(1, 1))
83  {
84  const float t = (1.0f - matrix(0, 0) - matrix(1, 1) + matrix(2, 2));
85  Quaternion q(matrix(0, 2) + matrix(2, 0), matrix(2, 1) + matrix(1, 2), t, matrix(1, 0) - matrix(0, 1));
86  q *= 0.5f / sqrt(t);
87  return q;
88  }
89  else
90  {
91  const float t = (1.0f + matrix(0, 0) + matrix(1, 1) + matrix(2, 2));
92  Quaternion q(matrix(2, 1) - matrix(1, 2), matrix(0, 2) - matrix(2, 0), matrix(1, 0) - matrix(0, 1), t);
93  q *= 0.5f / sqrt(t);
94  return q;
95  }
96  }
97 }
98 
99 inline TurtleBrains::Math::Matrix3 TurtleBrains::Math::Matrix3::FromQuaternion(const TurtleBrains::Math::Unstable::Quaternion& quaternion)
100 { //http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm
101  const float& qx(quaternion.x);
102  const float& qy(quaternion.y);
103  const float& qz(quaternion.z);
104  const float& qw(quaternion.w);
105 
106  const float qxSq(quaternion.x * quaternion.x);
107  const float qySq(quaternion.y * quaternion.y);
108  const float qzSq(quaternion.z * quaternion.z);
109 
110  //The resource mentioned this expected the quaternion to be normalized; thought if it is not normalized
111  //the sqrt can be thrown away by performing the normalization after calculating the products..
112  return Matrix3(
113  1.0f - 2.0f * qySq - 2.0f * qzSq, 2.0f * qx * qy - 2.0f * qz * qw, 2.0f * qx * qz + 2.0f * qy * qw,
114  2.0f * qx * qy + 2.0f * qz * qw, 1.0f - 2.0f * qxSq - 2.0f * qzSq, 2.0f * qy * qz - 2.0f * qx * qw,
115  2.0f * qx * qz - 2.0f * qy * qw, 2.0f * qy * qz + 2.0f * qx * qw, 1.0f - 2.0f * qxSq - 2.0f * qySq);
116 
117  //This is actually the expected values, but it fails the tests and the above passes. Keeping this block around
118  //until such a time comes that Matrix3 gets some attention. Note: This IS the correct block, 2019-01-01
119  //return Matrix3(
120  // 1.0f - 2.0f * qySq - 2.0f * qzSq, 2.0f * qx * qy + 2.0f * qz * qw, 2.0f * qx * qz - 2.0f * qy * qw,
121  // 2.0f * qx * qy - 2.0f * qz * qw, 1.0f - 2.0f * qxSq - 2.0f * qzSq, 2.0f * qy * qz + 2.0f * qx * qw,
122  // 2.0f * qx * qz + 2.0f * qy * qw, 2.0f * qy * qz - 2.0f * qx * qw, 1.0f - 2.0f * qxSq - 2.0f * qySq);
123 }
124 
125 inline TurtleBrains::Math::Matrix4 TurtleBrains::Math::Matrix4::FromQuaternion(
126  const TurtleBrains::Math::Unstable::Quaternion& quaternion, const TurtleBrains::Math::Vector3& translation)
127 {
128  const float& qx(quaternion.x);
129  const float& qy(quaternion.y);
130  const float& qz(quaternion.z);
131  const float& qw(quaternion.w);
132 
133  const float qxSq(quaternion.x * quaternion.x);
134  const float qySq(quaternion.y * quaternion.y);
135  const float qzSq(quaternion.z * quaternion.z);
136 
137  //The resource mentioned this expected the quaternion to be normalized; thought if it is not normalized
138  //the sqrt can be thrown away by performing the normalization after calculating the products...
139  return Matrix4(
140  1.0f - 2.0f * qySq - 2.0f * qzSq, 2.0f * qx * qy + 2.0f * qz * qw, 2.0f * qx * qz - 2.0f * qy * qw, 0.0f,
141  2.0f * qx * qy - 2.0f * qz * qw, 1.0f - 2.0f * qxSq - 2.0f * qzSq, 2.0f * qy * qz + 2.0f * qx * qw, 0.0f,
142  2.0f * qx * qz + 2.0f * qy * qw, 2.0f * qy * qz - 2.0f * qx * qw, 1.0f - 2.0f * qxSq - 2.0f * qySq, 0.0f,
143  translation.x, translation.y, translation.z, 1.0f);
144 }
145 
146 #endif /* #define TurtleBrains_MatrixQuaternion_hpp */
TurtleBrains::Math::Vector3
Definition: tb_vector.hpp:317
TurtleBrains::Math::Matrix3
Definition: tb_matrix.hpp:111
TurtleBrains::Math::Unstable::Quaternion
Definition: tb_quaternion.hpp:38
TurtleBrains::Math::Matrix4
Definition: tb_matrix.hpp:527
TurtleBrains::Math::Unstable::Quaternion::Quaternion
Quaternion(void)
Definition: tb_quaternion.hpp:74