fix transform coordinates

demos/demo1-wireframes/src/wirescene.cpp

@@ -43,12 +43,12 @@ void WireScene::load() {
         SoftEngine.js:50 drawing 163,36
         SoftEngine.js:50 drawing 76,36
      */
+    //cube->rotateTo(int2fx(13), int2fx(13));
 }
 
 void WireScene::tick(u16 keys) {
+    cube->rotate(2, 2);
     if(keys & KEY_START || keys & KEY_A) {
-        cube->rotate(5, 5);
-    } else if(keys & KEY_B) {
         cube->resetRotation();
     }
 }

engine/include/libgba-sprite-engine/math.h

@@ -13,6 +13,7 @@
     #endif
 
 #include <cmath>
+#include <string>
 
 // fixed point math things that were missing from TONC
 
@@ -34,18 +35,25 @@ INLINE FIXED fxtan(FIXED fxrad);
 INLINE FIXED fxsin(FIXED fxrad);
 INLINE FIXED fxcos(FIXED fxrad);
 INLINE float rnd(float val);
+INLINE std::string fstr(FIXED which);
 
 // ---- impl
 INLINE float rnd(float val) {
     return (float) ((std::floor(val * 100) + .5) / 100);
 }
+INLINE std::string fstr(FIXED which) {
+    char buffer[30];
+    snprintf(buffer, 30, "%4.3f", rnd(fx2float(which)));
+    std::string strObj4(buffer);
+    return strObj4;
+}
 
 INLINE float gr2rad(uint grad) {
     return grad*M_PI/180;
 }
 
 INLINE FIXED gr2lut(uint grad) {
-    return 65535 / (360 / grad);
+    return 65535 / (360 / (grad % 360));
 }
 
 INLINE FIXED rad2lut(float rad) {

engine/include/libgba-sprite-engine/matrixfx.h

@@ -21,11 +21,8 @@ class MatrixFx {
 private:
     FIXED m[MATRIX_DIMENSION];
 
-    inline std::string mstr(int index) {
+    inline std::string mstr(int index) const {
-        char buffer[30];
+        return fstr(m[index]);
-        snprintf(buffer, 30, "%4.3f", rnd(fx2float(m[index])));
-        std::string strObj4(buffer);
-        return strObj4;
     }
 
 public:
@@ -105,24 +102,24 @@ public:
         m[14] = m43;
         m[15] = m44;
     }
-    std::string to_string_m1() {
+    std::string to_string_m1() const {
         return "(" + mstr(0) + "," + mstr(1) + "," + mstr(2) + "," + mstr(3) +")";
     }
-    std::string to_string_m2() {
+    std::string to_string_m2() const {
         return "(" + mstr(4) + "," + mstr(5) + "," + mstr(6) + "," + mstr(7) +")";
     }
-    std::string to_string_m3() {
+    std::string to_string_m3() const {
         return "(" + mstr(8) + "," + mstr(9) + "," + mstr(10) + "," + mstr(11) +")";
     }
-    std::string to_string_m4() {
+    std::string to_string_m4() const {
         return "(" + mstr(12) + "," + mstr(13) + "," + mstr(14) + "," + mstr(15) +")";
     }
 
     inline static VectorFx transformCoordinates(const VectorFx &vector, const MatrixFx &transformation) {
-        FIXED x = fxmul(vector.x(), transformation.mAt(0)) + fxmul(vector.y(), transformation.mAt(4)) + fxmul(vector.z(), transformation.mAt(8) + transformation.mAt(12));
+        FIXED x = fxmul(vector.x(), transformation.mAt(0)) + fxmul(vector.y(), transformation.mAt(4)) + fxmul(vector.z(), transformation.mAt(8)) + transformation.mAt(12);
-        FIXED y = fxmul(vector.x(), transformation.mAt(1)) + fxmul(vector.y(), transformation.mAt(5)) + fxmul(vector.z(), transformation.mAt(9) + transformation.mAt(13));
+        FIXED y = fxmul(vector.x(), transformation.mAt(1)) + fxmul(vector.y(), transformation.mAt(5)) + fxmul(vector.z(), transformation.mAt(9)) + transformation.mAt(13);
-        FIXED z = fxmul(vector.x(), transformation.mAt(2)) + fxmul(vector.y(), transformation.mAt(6)) + fxmul(vector.z(), transformation.mAt(10) + transformation.mAt(14));
+        FIXED z = fxmul(vector.x(), transformation.mAt(2)) + fxmul(vector.y(), transformation.mAt(6)) + fxmul(vector.z(), transformation.mAt(10)) + transformation.mAt(14);
-        FIXED w = fxmul(vector.x(), transformation.mAt(3)) + fxmul(vector.y(), transformation.mAt(7)) + fxmul(vector.z(), transformation.mAt(11) + transformation.mAt(15));
+        FIXED w = fxmul(vector.x(), transformation.mAt(3)) + fxmul(vector.y(), transformation.mAt(7)) + fxmul(vector.z(), transformation.mAt(11)) + transformation.mAt(15);
         return VectorFx(fxdiv(x, w), fxdiv(y, w), fxdiv(z, w));
     }
 
@@ -209,7 +206,7 @@ public:
     }
 
     inline static MatrixFx rotationYawPitchRoll(FIXED yaw, FIXED pitch, FIXED roll) {
-        return MatrixFx::rotationZ(roll) * MatrixFx::rotationX(pitch) * MatrixFx::rotationY(yaw);
+        return (MatrixFx::rotationZ(roll) * MatrixFx::rotationX(pitch)) * MatrixFx::rotationY(yaw);
     }
 
     inline static MatrixFx perspectiveFovLH(FIXED fov, FIXED aspect, FIXED znear, FIXED zfar) {

engine/include/libgba-sprite-engine/mesh.h

@@ -22,15 +22,22 @@ public:
     inline std::vector<std::unique_ptr<VectorFx>> const& vertices() const {
         return verticesArr;
     }
-    inline VectorFx position() { return pos; }
+
-    inline FIXED rotx() { return rot.x(); }
+    inline VectorFx &position() { return pos; }
-    inline FIXED roty() { return rot.y(); }
+    inline VectorFx &rotation() { return rot; }
-    inline FIXED rotz() { return rot.z(); }
+
+    inline FIXED rotx() const { return rot.x(); }
+    inline FIXED roty() const { return rot.y(); }
+    inline FIXED rotz() const { return rot.z(); }
 
     inline void resetRotation() {
         rot.setX(0);
         rot.setY(0);
     }
+    void rotateTo(FIXED x, FIXED y) {
+        rot.setX(x);
+        rot.setY(y);
+    }
     inline void rotate(FIXED x, FIXED y) {
         rot.setX(rot.x() + x);
         rot.setY(rot.y() + y);

engine/include/libgba-sprite-engine/vectorfx.h

@@ -10,6 +10,7 @@
 #include <string>
 #include <deque>
 #include <libgba-sprite-engine/gba/tonc_bios.h>
+#include <libgba-sprite-engine/math.h>
 
 #ifdef CODE_COMPILED_AS_PART_OF_TEST
     #include <libgba-sprite-engine/gba/tonc_math_stub.h>
@@ -21,6 +22,7 @@
 class VectorFx {
 private:
     VECTOR v;
+
 public:
     VectorFx() : v({}) {}
     VectorFx(const VectorFx& other) : v(other.v) {}
@@ -31,7 +33,7 @@ public:
         return VectorFx(int2fx(x), int2fx(y), int2fx(z));
     }
 
-    inline VectorFx toInt() {
+    inline VectorFx toInt() const {
         return VectorFx(fx2int(v.x), fx2int(v.y), fx2int(v.z));
     }
 
@@ -61,15 +63,15 @@ public:
     inline friend VectorFx operator/(const VectorFx &one, const VectorFx &two) {
         return VectorFx(fxdiv(one.v.x, two.v.x), fxdiv(one.v.y, two.v.y), fxdiv(one.v.z, two.v.z));
     }
-    inline FIXED length() {
+    inline FIXED length() const {
         FIXED toRoot = fxmul(v.x, v.x) + fxmul(v.y, v.y) + fxmul(v.z, v.z);
         // tonc's fx() methods are .8fx, and BIOS needs .16 (results are .8??)
         return Sqrt(toRoot << 8);
     }
-    inline VectorFx negate() {
+    inline VectorFx negate() const {
         return VectorFx(-v.x, -v.y, -v.z);
     }
-    inline VectorFx scale(int scale) {
+    inline VectorFx scale(int scale) const {
         FIXED fac = int2fx(scale);
         return VectorFx(fxmul(v.x, fac), fxmul(v.y, fac), fxmul(v.z, fac));
     }
@@ -94,11 +96,11 @@ public:
     inline float floatZ() const { return fx2float(v.z); }
     inline void setZ(FIXED z) { v.z = z; }
 
-    std::string to_string() {
+    std::string to_string() const {
         return "(" + std::to_string(v.x) + "," + std::to_string(v.y) + "," + std::to_string(v.z) + ")";
     }
-    std::string to_stringfl() {
+    std::string to_stringfl() const {
-        return "(" + std::to_string(fx2float(v.x)) + "," + std::to_string(fx2float(v.y)) + "," + std::to_string(fx2float(v.z)) + ")";
+        return "(" + fstr(v.x) + "," + fstr(v.y) + "," + fstr(v.z) + ")";
     }
 };
 

engine/src/gba_engine.cpp

@@ -156,6 +156,8 @@ inline void GBAEngine::plotPixel(int x, int y, u8 clrId) {
     }
 }
 
+int i = 0;
+
 inline VectorFx GBAEngine::project(const VectorFx &coord, const MatrixFx &transMat) {
     auto point = MatrixFx::transformCoordinates(coord, transMat);
 
@@ -172,32 +174,13 @@ void GBAEngine::render() {
         auto worldMatrix = MatrixFx::rotationYawPitchRoll(mesh->roty(), mesh->rotx(), mesh->rotz()) * MatrixFx::translation(mesh->position());
         auto transformMatrix = worldMatrix * viewMatrix * projectionMatrix;
 
-        TextStream::instance().setText(transformMatrix.to_string_m1(), 1, 1);
+        TextStream::instance().setText("rot: " + mesh->rotation().to_stringfl(), 1, 1);
-        TextStream::instance().setText(transformMatrix.to_string_m2(), 2, 1);
-        TextStream::instance().setText(transformMatrix.to_string_m3(), 3, 1);
-        TextStream::instance().setText(transformMatrix.to_string_m4(), 4, 1);
 
-        int i = 0;
         for(auto& vertex : mesh->vertices()) {
             auto projectedPoint = project(*vertex.get(), transformMatrix).toInt();
             plotPixel(projectedPoint.x(), projectedPoint.y(), 1);
-
-            //TextStream::instance().setText(std::to_string(i) + ":" + projectedPoint.to_string(), i, 1);
-            i++;
         }
     }
-
-    /*
-     * according to the unit tests; it should do something like this:
-    plotPixel(150, 40, 1);
-    plotPixel(60, 40, 1);
-    plotPixel(150, 140, 1);
-    plotPixel(93, 26, 1);
-    plotPixel(93, 115, 1);
-    plotPixel(172, 115, 1);
-    plotPixel(60, 140, 1);
-    plotPixel(93, 26, 1);
-    */
 }
 
 void GBAEngine::renderClear() {

test/matrixfxtest.cpp

@@ -43,12 +43,56 @@ TEST_F(MatrixFxSuite, RotationMatriches) {
     assertMatrix(expectedIdMatrix, result, "rotz");
 }
 
-TEST_F(MatrixFxSuite, RotationYawPitchRoll) {
+TEST_F(MatrixFxSuite, RotationYawPitchRoll_WithoutRotations) {
     auto expectedIdMatrix = MatrixFx(float2fx(1.0), 0, 0, 0, 0, float2fx(1.0), 0, 0, 0, 0, float2fx(1.0), 0, 0, 0, 0, float2fx(1.0));
     auto rotYwaPitchRoll = MatrixFx::rotationYawPitchRoll(0, 0, 0);
     assertMatrix(expectedIdMatrix, rotYwaPitchRoll, "rotywa");
 }
 
+TEST_F(MatrixFxSuite, RotationYawPitchRoll_WithRotations) {
+    auto expectedMatrix = MatrixFx::fromFloat(
+            0.9074467814501962, 0, -0.4201670368266409, 0,
+            0.17654033883567982, 0.9074467814501962, 0.38127922523980134, 0,
+            0.38127922523980134, -0.4201670368266409, 0.8234596611643201, 0,
+            0, 0, 0, 1);
+    auto rotYwaPitchRoll = MatrixFx::rotationYawPitchRoll(int2fx(13), int2fx(13), 0);
+
+    assertMatrix(expectedMatrix, rotYwaPitchRoll, "rotywa");
+}
+
+TEST_F(MatrixFxSuite, TransformCoordinates) {
+    // test data taken from Babylon, first coord of cube to be transformed with calculated transfomatrix
+    auto transformation = MatrixFx::fromFloat(
+        -1.6218433160440375,
+        0,
+        0,
+        0,
+        0,
+        2.4327649740660564,
+        0,
+        0,
+        0,
+        0,
+        -1.0101010101010102,
+        -1,
+        0,
+        0,
+        10.090909090909092,
+        10);
+
+    auto result = MatrixFx::transformCoordinates(VectorFx::fromInt(-1, 1, 1), transformation);
+    // expected result: Vector3 {x: 0.18020481289378196, y: 0.2703072193406729, z: 1.0089786756453423}
+    ASSERT_FLOAT_EQ(fx2float(result.x()), 0.1796875);
+    ASSERT_FLOAT_EQ(fx2float(result.y()), 0.26953125);
+    ASSERT_FLOAT_EQ(fx2float(result.z()), 1.0078125);
+
+    result = MatrixFx::transformCoordinates(VectorFx::fromInt(-1, -1, -1), transformation);
+    // expected result: Vector3 {x: 0.14744030145854886, y: -0.22116045218782332, z: 1.0091827364554637}
+    ASSERT_FLOAT_EQ(fx2float(result.x()), 0.14453125);
+    ASSERT_FLOAT_EQ(fx2float(result.y()), -0.21875);
+    ASSERT_FLOAT_EQ(fx2float(result.z()), 1.0078125);
+}
+
 TEST_F(MatrixFxSuite, MeshToTransformMatrix_IntegrationTest) {
     // source:
     Mesh cube;
@@ -83,25 +127,25 @@ TEST_F(MatrixFxSuite, MeshToTransformMatrix_IntegrationTest) {
 
     auto coord = *cube.vertices()[0].get();
     auto point = MatrixFx::transformCoordinates(coord, transformMatrix);
-    ASSERT_EQ(fx2float(point.x()), 0.125f);
+    ASSERT_FLOAT_EQ(fx2float(point.x()), 0);
-    ASSERT_EQ(fx2float(point.y()), 0.25f);
+    ASSERT_FLOAT_EQ(fx2float(point.y()), 0);
-    ASSERT_EQ(fx2float(point.z()), 1.00f);
+    ASSERT_FLOAT_EQ(fx2float(point.z()), 1.0039062);
 
     auto x = fxmul(point.x(), GBA_SCREEN_WIDTH_FX) + fxdiv(GBA_SCREEN_WIDTH_FX, int2fx(2));
     auto y = fxmul(-point.y(), GBA_SCREEN_HEIGHT_FX) + fxdiv(GBA_SCREEN_HEIGHT_FX, int2fx(2));
-    ASSERT_EQ(fx2float(x), 150);
+    ASSERT_EQ(fx2float(x), 120);
-    ASSERT_EQ(fx2float(y), 40);
+    ASSERT_EQ(fx2float(y), 80);
 
-    // dest in Babylon - dest according to for loop below (should print something roughly similar)
+    // dest in Babylon - dest according to for loop below - dest printed using TextStream (should print something roughly similar)
     /*
-     *  163, 36         - 150,40
+     * 0  163, 36         - 150,40        - 165, 34
-     *  76, 36          - 60,40
+     * 1  76, 36          - 60,40         - 75, 34
-     *  163, 123        - 150,140
+     * 2  163, 123        - 150,140       - 165, 125
-     *  155, 115        - 93,26
+     * 3  155, 115        - 93,26         - 75, 34    (same as 0)
-     *  155, 44         - 93,115
+     * 4  155, 44         - 93,115        - 75, 125
-     *  84, 44          - 172,115
+     * 5  84, 44          - 172,115       - 165, 125  (same as 2)
-     *  76, 123         - 60,140
+     * 6  76, 123         - 60,140        - 75, 125   (same as 4)
-     *  84, 115         - 93,26
+     * 7  84, 115         - 93,26         - 165, 34   (same as 0)
      *
      */
     for(auto& vertex : cube.vertices()) {