include/PSX/GTE/gte.hpp

@@ -15,7 +15,7 @@ namespace JabyEngine {
             output: Output vector
             flag:   flag output
         */
-        void rot_trans(const SVECTOR& input, VECTOR& output, int32_t& flag) {
+        static void rot_trans(const SVECTOR& input, VECTOR& output, int32_t& flag) {
             ldv0(input);
             rt();
             stlvnl(output);
@@ -28,7 +28,7 @@ namespace JabyEngine {
             Sets a 3x3 matrix m as a constant rotation matrix.
             matrix: The rotation matrix to set
         */
-        void set_rot_matrix(const MATRIX& matrix) {
+        static void set_rot_matrix(const MATRIX& matrix) {
             __asm__ volatile("lw   $12, 0(%0)"  :: "r"(&matrix) : "$12", "$13", "$14"); 
             __asm__ volatile("lw   $13, 4(%0)"  :: "r"(&matrix) : "$12", "$13", "$14"); 
             __asm__ volatile("ctc2 $12, $0"     :: "r"(&matrix) : "$12", "$13", "$14"); 
@@ -46,7 +46,7 @@ namespace JabyEngine {
 
             Sets a constant parallel transfer vector specified by m
         */
-        void set_trans_matrix(const MATRIX& matrix)  {
+        static void set_trans_matrix(const MATRIX& matrix)  {
             __asm__ volatile("lw   $12, 20(%0)" :: "r"(&matrix) : "$12", "$13", "$14");
             __asm__ volatile("lw   $13, 24(%0)" :: "r"(&matrix) : "$12", "$13", "$14");
             __asm__ volatile("ctc2 $12, $5"     :: "r"(&matrix) : "$12", "$13", "$14");
@@ -73,7 +73,7 @@ namespace JabyEngine {
 
             Load GTE-offset.
         */
-        void set_geom_offset(int32_t off_x, int32_t off_y) {
+        static void set_geom_offset(int32_t off_x, int32_t off_y) {
             __asm__ volatile("sll  $12, %0, 16" :: "r"(off_x), "r"(off_y) : "$12", "$13");
             __asm__ volatile("sll  $13, %1, 16" :: "r"(off_x), "r"(off_y) : "$12", "$13");
             __asm__ volatile("ctc2 $12, $24"    :: "r"(off_x), "r"(off_y) : "$12", "$13");
@@ -85,7 +85,7 @@ namespace JabyEngine {
             
             Load distance from viewpoint to screen.
         */
-        void set_geom_screen(int32_t h) {
+        static void set_geom_screen(int32_t h) {
             __asm__ volatile("ctc2 %0, $26" :: "r"(h));
         }
     }

include/PSX/GTE/gte_types.hpp

@@ -1,5 +1,6 @@
 #pragma once
 #include "../GPU/gpu_types.hpp"
+#include "../../math.h"
 
 namespace JabyEngine {
     namespace GTE {
@@ -49,5 +50,24 @@ namespace JabyEngine {
 
         using VECTOR  = internal::VECTOR<int32_t>;
         using SVECTOR = internal::VECTOR<int16_t>;
+
+        static constexpr auto one_degree       = FULL_CIRCLE/360;
+        static constexpr auto one_tenth_degree = FULL_CIRCLE/3600;
+
+        static constexpr int16_t in_degree(int32_t degree) {
+            return degree*one_degree;
+        }
+
+        static constexpr int16_t in_degree10(int32_t degree) {
+            return degree*one_tenth_degree;
+        }
+    }
+
+    static constexpr unsigned long long operator""_DEG(unsigned long long degree) {
+        return GTE::in_degree(degree);
+    }
+
+    static constexpr unsigned long long operator""_DEG10(unsigned long long degree) {
+        return GTE::in_degree10(degree);
     }
 }

include/math.h

@@ -0,0 +1,10 @@
+#ifndef __MATH_H__
+#define __MATH_H__
+#include "stdint.h"
+
+#define FULL_CIRCLE 32768
+
+int32_t sin(int32_t value);
+int32_t cos(int32_t value);
+
+#endif // !__MATH_H__

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 {