summaryrefslogtreecommitdiffstats
path: root/src/math/Quaternion.cpp
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--src/math/Quaternion.cpp177
1 files changed, 177 insertions, 0 deletions
diff --git a/src/math/Quaternion.cpp b/src/math/Quaternion.cpp
new file mode 100644
index 00000000..b0e782e2
--- /dev/null
+++ b/src/math/Quaternion.cpp
@@ -0,0 +1,177 @@
+#include "common.h"
+#include "Quaternion.h"
+
+void
+CQuaternion::Normalise(void)
+{
+ float sq = MagnitudeSqr();
+ if (sq == 0.0f)
+ w = 1.0f;
+ else {
+ float invsqrt = RecipSqrt(sq);
+ x *= invsqrt;
+ y *= invsqrt;
+ z *= invsqrt;
+ w *= invsqrt;
+ }
+}
+
+void
+CQuaternion::Slerp(const CQuaternion &q1, const CQuaternion &q2, float theta, float invSin, float t)
+{
+ if (theta == 0.0f)
+ *this = q2;
+ else {
+ float w1, w2;
+ if (theta > PI / 2) {
+ theta = PI - theta;
+ w1 = Sin((1.0f - t) * theta) * invSin;
+ w2 = -Sin(t * theta) * invSin;
+ } else {
+ w1 = Sin((1.0f - t) * theta) * invSin;
+ w2 = Sin(t * theta) * invSin;
+ }
+ // TODO: VU0 code
+ *this = w1 * q1 + w2 * q2;
+ }
+}
+
+void
+CQuaternion::Multiply(const CQuaternion &q1, const CQuaternion &q2)
+{
+ x = (q2.z * q1.y) - (q1.z * q2.y) + (q1.x * q2.w) + (q2.x * q1.w);
+ y = (q2.x * q1.z) - (q1.x * q2.z) + (q1.y * q2.w) + (q2.y * q1.w);
+ z = (q2.y * q1.x) - (q1.y * q2.x) + (q1.z * q2.w) + (q2.z * q1.w);
+ w = (q2.w * q1.w) - (q2.x * q1.x) - (q2.y * q1.y) - (q2.z * q1.z);
+}
+
+void
+CQuaternion::Get(RwV3d *axis, float *angle)
+{
+ *angle = Acos(w);
+ float s = Sin(*angle);
+
+ axis->x = x * (1.0f / s);
+ axis->y = y * (1.0f / s);
+ axis->z = z * (1.0f / s);
+}
+
+void
+CQuaternion::Set(RwV3d *axis, float angle)
+{
+ float halfCos = Cos(angle * 0.5f);
+ float halfSin = Sin(angle * 0.5f);
+ x = axis->x * halfSin;
+ y = axis->y * halfSin;
+ z = axis->z * halfSin;
+ w = halfCos;
+}
+
+void
+CQuaternion::Get(RwMatrix *matrix)
+{
+ float x2 = x + x;
+ float y2 = y + y;
+ float z2 = z + z;
+
+ float x_2x = x * x2;
+ float x_2y = x * y2;
+ float x_2z = x * z2;
+ float y_2y = y * y2;
+ float y_2z = y * z2;
+ float z_2z = z * z2;
+ float w_2x = w * x2;
+ float w_2y = w * y2;
+ float w_2z = w * z2;
+
+ matrix->right.x = 1.0f - (y_2y + z_2z);
+ matrix->up.x = x_2y - w_2z;
+ matrix->at.x = x_2z + w_2y;
+ matrix->right.y = x_2y + w_2z;
+ matrix->up.y = 1.0f - (x_2x + z_2z);
+ matrix->at.y = y_2z - w_2x;
+ matrix->right.z = x_2z - w_2y;
+ matrix->up.z = y_2z + w_2x;
+ matrix->at.z = 1.0f - (x_2x + y_2y);
+}
+
+void
+CQuaternion::Set(const RwMatrix &matrix)
+{
+ float f, s, m;
+
+ f = matrix.up.y + matrix.right.x + matrix.at.z;
+ if (f >= 0.0f) {
+ s = Sqrt(f + 1.0f);
+ w = 0.5f * s;
+ m = 0.5f / s;
+ x = (matrix.up.z - matrix.at.y) * m;
+ y = (matrix.at.x - matrix.right.z) * m;
+ z = (matrix.right.y - matrix.up.x) * m;
+ return;
+ }
+
+ f = matrix.right.x - matrix.up.y - matrix.at.z;
+ if (f >= 0.0f) {
+ s = Sqrt(f + 1.0f);
+ x = 0.5f * s;
+ m = 0.5f / s;
+ y = (matrix.up.x + matrix.right.y) * m;
+ z = (matrix.at.x + matrix.right.z) * m;
+ w = (matrix.up.z - matrix.at.y) * m;
+ return;
+ }
+
+ f = matrix.up.y - matrix.right.x - matrix.at.z;
+ if (f >= 0.0f) {
+ s = Sqrt(f + 1.0f);
+ y = 0.5f * s;
+ m = 0.5f / s;
+ w = (matrix.at.x - matrix.right.z) * m;
+ x = (matrix.up.x - matrix.right.y) * m;
+ z = (matrix.at.y + matrix.up.z) * m;
+ return;
+ }
+
+ f = matrix.at.z - (matrix.up.y + matrix.right.x);
+ s = Sqrt(f + 1.0f);
+ z = 0.5f * s;
+ m = 0.5f / s;
+ w = (matrix.right.y - matrix.up.x) * m;
+ x = (matrix.at.x + matrix.right.z) * m;
+ y = (matrix.at.y + matrix.up.z) * m;
+}
+
+void
+CQuaternion::Get(float *f1, float *f2, float *f3)
+{
+ RwMatrix matrix;
+
+ Get(&matrix);
+ *f3 = Atan2(matrix.right.y, matrix.up.y);
+ if (*f3 < 0.0f)
+ *f3 += TWOPI;
+ float s = Sin(*f3);
+ float c = Cos(*f3);
+ *f1 = Atan2(-matrix.at.y, s * matrix.right.y + c * matrix.up.y);
+ if (*f1 < 0.0f)
+ *f1 += TWOPI;
+ *f2 = Atan2(-(matrix.right.z * c - matrix.up.z * s), matrix.right.x * c - matrix.up.x * s);
+ if (*f2 < 0.0f)
+ *f2 += TWOPI;
+}
+
+void
+CQuaternion::Set(float f1, float f2, float f3)
+{
+ float c1 = Cos(f1 * 0.5f);
+ float c2 = Cos(f2 * 0.5f);
+ float c3 = Cos(f3 * 0.5f);
+ float s1 = Sin(f1 * 0.5f);
+ float s2 = Sin(f2 * 0.5f);
+ float s3 = Sin(f3 * 0.5f);
+ x = ((c2 * c1) * s3) - ((s2 * s1) * c3);
+ y = ((s1 * c2) * c3) + ((s2 * c1) * s3);
+ z = ((s2 * c1) * c3) - ((s1 * c2) * s3);
+ w = ((c2 * c1) * c3) + ((s2 * s1) * s3);
+} \ No newline at end of file