1 files changed, 18 insertions, 0 deletions
diff --git a/src/ta_math.c b/src/ta_math.c
index 53375a5..5b976fc 100644
--- a/src/ta_math.c
+++ b/src/ta_math.c
@@ -16,8 +16,26 @@
  * along with this program.  If not, see <https://www.gnu.org/licenses/>.
  */
 
+#include <string.h>
 #include "ta_math.h"
 
 double ta_rad(double degrees) {
     return degrees * (PI / (CIRCLE_DEG / 2));
+}
+
+void ta_matrix_identity(float *matrix) {
+    for (int index = 0; index < TA_MATRIX_ELEMENTS; index++) {
+        matrix[index] = index % 5 == 0 ? 1.0f : 0.0f;
+    }
+}
+
+void ta_matrix_multiply(float *result, const float *left, const float *right) {
+    float product[TA_MATRIX_ELEMENTS];
+    for (int column = 0; column < 4; column++) for (int row = 0; row < 4; row++) {
+        product[column * 4 + row] = 0.0f;
+        for (int inner = 0; inner < 4; inner++) {
+            product[column * 4 + row] += left[inner * 4 + row] * right[column * 4 + inner];
+        }
+    }
+    memcpy(result, product, sizeof(product));
 }
 \ No newline at end of file