Merge pull request #59 from h4570/feature/math-matrix

Added some matrix/math funcs

src/engine/include/models/math/matrix.hpp

@@ -26,14 +26,64 @@ public:
            float m31, float m32, float m33, float m34,
            float m41, float m42, float m43, float m44);
     Matrix(const Matrix &v);
-    Matrix operator*(Matrix &v);
+    Matrix operator*(const Matrix &v);
+    void operator*=(const Matrix &v);
     Matrix();
     ~Matrix();
 
     void lookAt(Vector3 &t_up, Vector3 &t_position, Vector3 &t_target);
     void identity();
-    void makeZRotation(float t_radians);
+    inline void unit() { identity(); };
-    void translate(float t_x, float t_y, float t_z);
+
+    // Translation
+
+    void translate(const Vector3 &t_val);
+
+    inline void translation(const Vector3 &t_val)
+    {
+        identity();
+        translate(t_val);
+    }
+
+    // Rotation
+
+    void rotateX(const float &t_radians);
+    void rotateY(const float &t_radians);
+    void rotateZ(const float &t_radians);
+
+    inline void rotationX(const float &t_radians)
+    {
+        identity();
+        rotateX(t_radians);
+    }
+
+    inline void rotationY(const float &t_radians)
+    {
+        identity();
+        rotateY(t_radians);
+    }
+
+    inline void rotationZ(const float &t_radians)
+    {
+        identity();
+        rotateZ(t_radians);
+    }
+
+    inline void rotate(const Vector3 &t_val)
+    {
+        rotateX(t_val.x);
+        rotateY(t_val.y);
+        rotateX(t_val.z);
+    }
+
+    inline void rotation(const Vector3 &t_val)
+    {
+        identity();
+        rotate(t_val);
+    }
+
+    //
+
     void setPerspective(ScreenSettings &screen);
     const void print() const;
 };

src/engine/include/utils/math.hpp

@@ -28,6 +28,8 @@ public:
     const static float PI = 3.1415926535897932384626433832795F;
     const static float HALF_PI = 1.5707963267948966192313216916398F;
     static float cos(float x);
+    static float asin(float x);
+    static float mod(float x, float y);
     static inline float sin(float x) { return cos(x - HALF_PI); };
     static float sqrt(float x);
     static float invSqrt(float x);

src/engine/models/math/matrix.cpp

@@ -84,17 +84,41 @@ void Matrix::identity()
         : "r"(this->data));
 }
 
-void Matrix::translate(float x, float y, float z)
+void Matrix::translate(const Vector3 &t_val)
 {
-    this->identity();
+    this->data[12] += t_val.x; // 3,0
-    this->data[(3 << 2) + 0] = x; // 3,0
+    this->data[13] += t_val.y; // 3,1
-    this->data[(3 << 2) + 1] = y; // 3,1
+    this->data[14] += t_val.z; // 3,2
-    this->data[(3 << 2) + 2] = z; // 3,2
 }
 
-void Matrix::makeZRotation(float t_radians)
+void Matrix::rotateX(const float &t_radians)
 {
-    this->identity();
+    Matrix temp = Matrix();
+    temp.identity();
+    float c = Math::cos(t_radians);
+    float s = Math::sin(t_radians);
+    this->data[5] = c;  // 1,1
+    this->data[6] = s;  // 1,2
+    this->data[9] = -s; // 2,1
+    this->data[10] = c; // 2,2
+}
+
+void Matrix::rotateY(const float &t_radians)
+{
+    Matrix temp = Matrix();
+    temp.identity();
+    float c = Math::cos(t_radians);
+    float s = Math::sin(t_radians);
+    this->data[0] = c;  // 0,0
+    this->data[2] = -s; // 0,3
+    this->data[8] = s;  // 2,0
+    this->data[10] = c; // 2,2
+}
+
+void Matrix::rotateZ(const float &t_radians)
+{
+    Matrix temp = Matrix();
+    temp.identity();
     float c = Math::cos(t_radians);
     float s = Math::sin(t_radians);
     this->data[0] = c;  // 0,0
@@ -103,7 +127,7 @@ void Matrix::makeZRotation(float t_radians)
     this->data[5] = c;  // 1,1
 }
 
-Matrix Matrix::operator*(Matrix &t)
+Matrix Matrix::operator*(const Matrix &t)
 {
     Matrix result;
     asm volatile(
@@ -141,6 +165,42 @@ Matrix Matrix::operator*(Matrix &t)
     return result;
 }
 
+void Matrix::operator*=(const Matrix &t)
+{
+    asm volatile(
+        "lqc2         vf1, 0x00(%1) \n\t"
+        "lqc2         vf2, 0x10(%1) \n\t"
+        "lqc2         vf3, 0x20(%1) \n\t"
+        "lqc2         vf4, 0x30(%1) \n\t"
+        "lqc2         vf5, 0x00(%2) \n\t"
+        "lqc2         vf6, 0x10(%2) \n\t"
+        "lqc2         vf7, 0x20(%2) \n\t"
+        "lqc2         vf8, 0x30(%2) \n\t"
+        "vmulax.xyzw  ACC, vf5, vf1 \n\t"
+        "vmadday.xyzw ACC, vf6, vf1 \n\t"
+        "vmaddaz.xyzw ACC, vf7, vf1 \n\t"
+        "vmaddw.xyzw  vf1, vf8, vf1 \n\t"
+        "vmulax.xyzw  ACC, vf5, vf2 \n\t"
+        "vmadday.xyzw ACC, vf6, vf2 \n\t"
+        "vmaddaz.xyzw ACC, vf7, vf2 \n\t"
+        "vmaddw.xyzw  vf2, vf8, vf2 \n\t"
+        "vmulax.xyzw  ACC, vf5, vf3 \n\t"
+        "vmadday.xyzw ACC, vf6, vf3 \n\t"
+        "vmaddaz.xyzw ACC, vf7, vf3 \n\t"
+        "vmaddw.xyzw  vf3, vf8, vf3 \n\t"
+        "vmulax.xyzw  ACC, vf5, vf4 \n\t"
+        "vmadday.xyzw ACC, vf6, vf4 \n\t"
+        "vmaddaz.xyzw ACC, vf7, vf4 \n\t"
+        "vmaddw.xyzw  vf4, vf8, vf4 \n\t"
+        "sqc2         vf1, 0x00(%0) \n\t"
+        "sqc2         vf2, 0x10(%0) \n\t"
+        "sqc2         vf3, 0x20(%0) \n\t"
+        "sqc2         vf4, 0x30(%0) \n\t"
+        :
+        : "r"(this->data), "r"(this->data), "r"(t.data)
+        : "memory");
+}
+
 /** Create empty matrix */
 Matrix::Matrix()
 {

src/engine/utils/math.cpp

@@ -99,3 +99,87 @@ void manyVec3ToNative(VECTOR *o_result, Vector3 *t_vec, int t_amount, float t_fo
     for (int i = 0; i < t_amount; i++)
         vec3ToNative(o_result[i], t_vec[i], t_fourthVal);
 }
+
+static float asin(float x)
+{
+    float r;
+    asm volatile(
+        "lui     $9,  0x3f00        \n\t"
+        ".set noreorder             \n\t"
+        ".align 3                   \n\t"
+        "abs.s   %0,  %1            \n\t"
+        "lui     $8,  0xbe22        \n\t"
+        "mtc1    $9,  $f1           \n\t"
+        "ori     $8,  $8,    0xf983 \n\t"
+        "mtc1    $8,  $f8           \n\t"
+        "lui     $9,  0x4b00        \n\t"
+        "mtc1    $9,  $f3           \n\t"
+        "lui     $8,  0x3f80        \n\t"
+        "mtc1    $8,  $f2           \n\t"
+        "mula.s  %0,  $f8           \n\t"
+        "msuba.s $f3, $f2           \n\t"
+        "madda.s $f3, $f2           \n\t"
+        "lui     $8,  0x40c9        \n\t"
+        "msuba.s %0,  $f8           \n\t"
+        "ori     $8,  0x0fdb        \n\t"
+        "msub.s  %0,  $f1,   $f2    \n\t"
+        "lui     $9,  0xc225        \n\t"
+        "abs.s   %0,  %0            \n\t"
+        "lui     $10, 0x3e80        \n\t"
+        "mtc1    $10, $f7           \n\t"
+        "ori     $9,  0x5de1        \n\t"
+        "sub.s   %0,  %0,    $f7    \n\t"
+        "lui     $10, 0x42a3        \n\t"
+        "mtc1    $8,  $f3           \n\t"
+        "ori     $10, 0x3458        \n\t"
+        "mtc1    $9,  $f4           \n\t"
+        "lui     $8,  0xc299        \n\t"
+        "mtc1    $10, $f5           \n\t"
+        "ori     $8,  0x2663        \n\t"
+        "mul.s   $f8, %0,    %0     \n\t"
+        "lui     $9,  0x421e        \n\t"
+        "mtc1    $8,  $f6           \n\t"
+        "ori     $9,  0xd7bb        \n\t"
+        "mtc1    $9,  $f7           \n\t"
+        "nop                        \n\t"
+        "mul.s   $f1, %0,    $f8    \n\t"
+        "mul.s   $f9, $f8,   $f8    \n\t"
+        "mula.s  $f3, %0            \n\t"
+        "mul.s   $f2, $f1,   $f8    \n\t"
+        "madda.s $f4, $f1           \n\t"
+        "mul.s   $f1, $f1,   $f9    \n\t"
+        "mul.s   %0,  $f2,   $f9    \n\t"
+        "madda.s $f5, $f2           \n\t"
+        "madda.s $f6, $f1           \n\t"
+        "madd.s  %0,  $f7,   %0     \n\t"
+        ".set reorder               \n\t"
+        : "=&f"(r)
+        : "f"(x)
+        : "$f1", "$f2", "$f3", "$f4", "$f5", "$f6", "$f7", "$f8", "$f9", "$8", "$9", "$10");
+    return r;
+}
+
+static float mod(float x, float y)
+{
+    /*
+     * Portable fmod(x,y) implementation for systems
+     * that don't have it. Adapted from code found here:
+     * http://www.opensource.apple.com/source/python/python-3/python/Python/fmod.c
+     */
+    float i, f;
+
+    if (y == 0.0f)
+    {
+        return 0.0f;
+    }
+
+    i = floorf(x / y);
+    f = x - i * y;
+
+    if ((x < 0.0f) != (y < 0.0f))
+    {
+        f = f - y;
+    }
+
+    return f;
+}