Add sin/cos support

src/Library/src/BootLoader/start_boot.cpp

@@ -6,6 +6,7 @@
 #include <PSX/GTE/gte.hpp>
 #include <PSX/System/syscalls.hpp>
 #include "../../reference/inline_n.h"
+#include <math.h>
 
 extern "C" uint32_t __heap_start;
 extern "C" uint32_t __bss_start;
@@ -88,6 +89,25 @@ namespace JabyEngine {
         asm("# PLANSCHI START");
         JabyEngine::GTE::multiply_matrix(m0, m1, m2);
         asm("# PLANSCHI END");
+
+        #define PI 3.14159265 // v 90°???
+
+        static const auto display_stuff = [](int32_t angle) {
+            static const auto wanna_be_float = [](int32_t value) -> int32_t {
+                return (value*10000)/(4096);
+            };
+
+            const auto sin_value = sin(GTE::in_degree(angle));
+            const auto cos_value = cos(GTE::in_degree(angle));
+            
+            printf("Sine of %i = %i (%i)\n", angle, sin_value, wanna_be_float(sin_value));
+            printf("Cos  of %i = %i (%i)\n", angle, cos_value, wanna_be_float(cos_value));
+        };
+
+        display_stuff(25);
+        display_stuff(45);
+        display_stuff(75);
+        display_stuff(90);
     }
 
     void start() {

src/Library/src/GTE/gte.cpp

@@ -1,4 +1,28 @@
 #include <PSX/GTE/gte.hpp>
+#include <math.h>
+
+int32_t sin(int32_t value) {
+    static constexpr int32_t qN = 13;
+    static constexpr int32_t qA = 12;
+    static constexpr int32_t B  = 19900;
+    static constexpr int32_t C  = 3516;
+
+    const auto c = value << (30 - qN);          // Semi-circle info into carry.
+
+    value -= 1<<qN;                             // sine -> cosine calc
+    value  = value       << (31 - qN);          // Mask with PI
+    value  = value       >> (31 - qN);          // Note: SIGNED shift! (to qN)
+    value  = value*value >> (2*qN - 14);        // x=x^2 To Q14
+
+    auto result = B - (value*C >> 14);              // B - x^2*C
+         result = (1 << qA) - (value*result >> 16); // A - x^2*(B-x^2*C)
+
+    return c >= 0 ? result : -result;
+}
+
+int32_t cos(int32_t value) {
+    return sin(value + (FULL_CIRCLE/4));
+}
 
 namespace JabyEngine {
     namespace GTE {