From cf1cd164de318a120b1c0bef4b535f1adb9a4a57 Mon Sep 17 00:00:00 2001
From: fallenoak <git@fallenoak.me>
Date: Sat, 24 Dec 2022 17:34:02 -0600
Subject: [PATCH] feat(matrix): add C44Matrix constructor for C4Quaternion

---
 tempest/matrix/C44Matrix.cpp | 37 ++++++++++++++++++++++++++++++++++++
 tempest/matrix/C44Matrix.hpp |  2 ++
 2 files changed, 39 insertions(+)

diff --git a/tempest/matrix/C44Matrix.cpp b/tempest/matrix/C44Matrix.cpp
index 098bb8b..285afad 100644
--- a/tempest/matrix/C44Matrix.cpp
+++ b/tempest/matrix/C44Matrix.cpp
@@ -1,4 +1,5 @@
 #include "tempest/matrix/C44Matrix.hpp"
+#include "tempest/Quaternion.hpp"
 #include "tempest/Vector.hpp"
 #include <cmath>
 
@@ -29,6 +30,42 @@ C44Matrix C44Matrix::RotationAroundZ(float angle) {
     return { a0, a1, a2, a3, b0, b1, b2, b3, c0, c1, c2, c3, d0, d1, d2, d3 };
 }
 
+C44Matrix::C44Matrix(const C4Quaternion& rotation) {
+    this->a3 = 0.0f;
+    this->b3 = 0.0f;
+    this->c3 = 0.0f;
+
+    this->d0 = 0.0f;
+    this->d1 = 0.0f;
+    this->d2 = 0.0f;
+    this->d3 = 1.0f;
+
+    float v3 = rotation.x * 2.0f;
+    float v4 = rotation.y * 2.0f;
+    float v5 = rotation.z * 2.0f;
+
+    float v6 = rotation.w * v3;
+    float v7 = rotation.w * v4;
+    float v8 = rotation.w * v5;
+
+    float v10 = rotation.x * v3;
+    float v11 = rotation.x * v4;
+    float v19 = rotation.x * v5;
+    float v12 = rotation.y * v4;
+    float v17 = rotation.y * v5;
+    float v16 = rotation.z * v5;
+
+    this->a0 = 1.0f - (v16 + v12);
+    this->a1 = v11 + v8;
+    this->a2 = v19 - v7;
+    this->b0 = v11 - v8;
+    this->b1 = 1.0f - (v16 + v10);
+    this->b2 = v17 + v6;
+    this->c0 = v7 + v19;
+    this->c1 = v17 - v6;
+    this->c2 = 1.0f - (v10 + v12);
+}
+
 C44Matrix C44Matrix::Adjoint() const {
     float a0 = this->b2 * this->c3 * this->d1 + this->b3 * this->c1 * this->d2 + this->b1 * this->c2 * this->d3 - this->b3 * this->c2 * this->d1 - this->b2 * this->c1 * this->d3 - this->b1 * this->c3 * this->d2;
     float a1 = -(this->a2 * this->c3 * this->d1 + this->a3 * this->c1 * this->d2 + this->a1 * this->c2 * this->d3 - this->a3 * this->c2 * this->d1 - this->a2 * this->c1 * this->d3 - this->a1 * this->c3 * this->d2);
diff --git a/tempest/matrix/C44Matrix.hpp b/tempest/matrix/C44Matrix.hpp
index 1519a0d..5fdac17 100644
--- a/tempest/matrix/C44Matrix.hpp
+++ b/tempest/matrix/C44Matrix.hpp
@@ -2,6 +2,7 @@
 #define TEMPEST_MATRIX_C_44MATRIX_HPP
 
 class C3Vector;
+class C4Quaternion;
 
 class C44Matrix {
     public:
@@ -45,6 +46,7 @@ class C44Matrix {
         , d1(d1)
         , d2(d2)
         , d3(d3) {};
+    C44Matrix(const C4Quaternion& rotation);
     C44Matrix Adjoint() const;
     float Determinant() const;
     C44Matrix Inverse(float det) const;