Sync countdown (#59)

* Initial commit

* Address review comments

* I have been enlightened by Tokyo Friend Park II - Ketteiban - Minna de Chousen! Taikan Attraction!

* Implement RamStream::split, fix endianness bug

* Button stream cleanup

source/egg/math/Math.cc

@@ -4,6 +4,16 @@
 
 namespace EGG::Mathf {
 
+// sin/cos struct
+struct SinCosEntry {
+    f32 sinVal, cosVal, sinDt, cosDt;
+};
+
+// atan struct
+struct AtanEntry {
+    f32 atanVal, atanDt;
+};
+
 static constexpr SinCosEntry sSinCosTbl[256 + 1] = {
         {0.000000f, 1.000000f, 0.024541f, -0.000301f},
         {0.024541f, 0.999699f, 0.024526f, -0.000903f},
@@ -264,6 +274,42 @@ static constexpr SinCosEntry sSinCosTbl[256 + 1] = {
         {-0.000000f, 1.000000f, 0.024541f, -0.000301f},
 };
 
+static constexpr AtanEntry sArcTanTbl[32 + 1] = {
+        {0.000000000f, 1.272825321f},
+        {1.272825321f, 1.270345790f},
+        {2.543171111f, 1.265415586f},
+        {3.808586697f, 1.258091595f},
+        {5.066678293f, 1.248457103f},
+        {6.315135396f, 1.236619467f},
+        {7.551754863f, 1.222707202f},
+        {8.774462065f, 1.206866624f},
+        {9.981328688f, 1.189258212f},
+        {11.170586901f, 1.170052841f},
+        {12.340639741f, 1.149428034f},
+        {13.490067775f, 1.127564381f},
+        {14.617632156f, 1.104642222f},
+        {15.722274378f, 1.080838675f},
+        {16.803113053f, 1.056325088f},
+        {17.859438141f, 1.031264918f},
+        {18.890703059f, 1.005812061f},
+        {19.896515121f, 0.980109621f},
+        {20.876624742f, 0.954289072f},
+        {21.830913814f, 0.928469801f},
+        {22.759383615f, 0.902758952f},
+        {23.662142567f, 0.877251558f},
+        {24.539394125f, 0.852030871f},
+        {25.391424996f, 0.827168886f},
+        {26.218593881f, 0.802726967f},
+        {27.021320848f, 0.778756582f},
+        {27.800077430f, 0.755300081f},
+        {28.555377511f, 0.732391496f},
+        {29.287769007f, 0.710057351f},
+        {29.997826358f, 0.688317453f},
+        {30.686143811f, 0.667185647f},
+        {31.353329458f, 0.646670542f},
+        {32.000000000f, 0.626776175f},
+};
+
 f32 sqrt(f32 x) {
     return x > 0.0 ? frsqrt(x) * x : 0.0;
 }
@@ -308,6 +354,49 @@ f32 CosFIdx(f32 fidx) {
     return sSinCosTbl[idx].cosVal + r * sSinCosTbl[idx].cosDt;
 }
 
+f32 AtanFIdx_(f32 x) {
+    x *= 32.0f;
+    u16 idx = static_cast<u16>(x);
+    f32 r = x - static_cast<f32>(idx);
+    return sArcTanTbl[idx].atanVal + r * sArcTanTbl[idx].atanDt;
+}
+
+f32 Atan2FIdx(f32 y, f32 x) {
+    if (x == 0.0f && y == 0.0f) {
+        return 0.0f;
+    }
+
+    if (x >= 0.0f) {
+        if (y >= 0.0f) {
+            if (x >= y) {
+                return 0.0f + AtanFIdx_(y / x);
+            } else {
+                return 64.0f - AtanFIdx_(x / y);
+            }
+        } else {
+            if (x >= -y) {
+                return 0.0f - AtanFIdx_(-y / x);
+            } else {
+                return -64.0f + AtanFIdx_(x / -y);
+            }
+        }
+    } else {
+        if (y >= 0.0f) {
+            if (-x >= y) {
+                return 128.0f - AtanFIdx_(y / -x);
+            } else {
+                return 64.0f + AtanFIdx_(-x / y);
+            }
+        } else {
+            if (-x >= -y) {
+                return -128.0f + AtanFIdx_(-y / -x);
+            } else {
+                return -64.0f - AtanFIdx_(-x / -y);
+            }
+        }
+    }
+}
+
 // Takes in radians
 f32 sin(f32 x) {
     return SinFIdx(x * RAD2FIDX);
@@ -322,6 +411,10 @@ f32 acos(f32 x) {
     return std::acos(x);
 }
 
+f32 atan2(f32 y, f32 x) {
+    return Atan2FIdx(y, x) * FIDX2RAD;
+}
+
 f32 abs(f32 x) {
     return std::abs(x);
 }

source/egg/math/Math.hh

@@ -7,6 +7,7 @@
 #define RAD2DEG (180.0f / F_PI)
 #define DEG2FIDX (256.0f / 360.0f)
 #define RAD2FIDX (128.0f / F_PI)
+#define FIDX2RAD (F_PI / 128.0f)
 
 namespace EGG::Mathf {
 
@@ -15,21 +16,19 @@ f32 frsqrt(f32 x);
 
 f32 SinFIdx(f32 fidx);
 f32 CosFIdx(f32 fidx);
+f32 AtanFIdx_(f32 fidx);
+f32 Atan2FIdx(f32 x, f32 y);
 f32 sin(f32 x);
 f32 cos(f32 x);
 f32 acos(f32 x);
+f32 atan2(f32 x, f32 y);
 
 f32 abs(f32 x);
 
 f32 fma(f32 x, f32 y, f32 z);
 
 f64 force25Bit(f64 x);
 
-// sin/cos struct
-struct SinCosEntry {
-    f32 sinVal, cosVal, sinDt, cosDt;
-};
-
 // frsqrte matching
 struct BaseAndDec {
     int base;

source/egg/math/Matrix.cc

@@ -222,6 +222,20 @@ Matrix34f Matrix34f::inverseTo() const {
     return ret;
 }
 
+Matrix34f Matrix34f::transpose() const {
+    // NOTE: 4th element in each row is not meant to be used reliably
+    Matrix34f ret = *this;
+
+    ret(0, 1) = mtx[1][0];
+    ret(0, 2) = mtx[2][0];
+    ret(1, 0) = mtx[0][1];
+    ret(1, 2) = mtx[2][1];
+    ret(2, 0) = mtx[0][2];
+    ret(2, 1) = mtx[1][2];
+
+    return ret;
+}
+
 void Matrix34f::setAxisRotation(f32 angle, const EGG::Vector3f &axis) {
     EGG::Quatf q;
     q.setAxisRotation(angle, axis);

source/egg/math/Matrix.hh

@@ -36,6 +36,7 @@ public:
     Vector3f ps_multVector(const Vector3f &vec) const;
     Vector3f multVector33(const Vector3f &vec) const;
     Matrix34f inverseTo() const;
+    Matrix34f transpose() const;
 
     static const Matrix34f ident;
     static const Matrix34f zero;

source/egg/math/Quat.cc

@@ -36,22 +36,45 @@ void Quatf::normalise() {
     }
 }
 
+void Quatf::makeVectorRotation(const Vector3f &from, const Vector3f &to) {
+    f32 t0 = std::max(0.0f, (from.dot(to) + 1) * 2.0f);
+    t0 = Mathf::sqrt(t0);
+
+    if (t0 <= FLT_EPSILON) {
+        *this = Quatf::ident;
+    } else {
+        const f32 inv = 1.0f / t0;
+        w = t0 * 0.5f;
+        v = from.cross(to) * inv;
+    }
+}
+
 Quatf Quatf::conjugate() const {
     return Quatf(w, -v);
 }
 
 Vector3f Quatf::rotateVector(const Vector3f &vec) const {
     Quatf conj = conjugate();
     Quatf res = *this * vec;
-    res *= conj;
-    return res.v;
+    Quatf ret;
+
+    ret.v.x = (res.v.y * conj.v.z + (res.v.x * conj.w + res.w * conj.v.x)) - res.v.z * conj.v.y;
+    ret.v.y = (res.v.z * conj.v.x + (res.v.y * conj.w + res.w * conj.v.y)) - res.v.x * conj.v.z;
+    ret.v.z = (res.v.x * conj.v.y + (res.v.z * conj.w + res.w * conj.v.z)) - res.v.y * conj.v.x;
+
+    return ret.v;
 }
 
 Vector3f Quatf::rotateVectorInv(const Vector3f &vec) const {
     Quatf conj = conjugate();
     Quatf res = conj * vec;
-    res *= *this;
-    return res.v;
+    Quatf ret;
+
+    ret.v.x = (res.v.y * v.z + (res.v.x * w + res.w * v.x)) - res.v.z * v.y;
+    ret.v.y = (res.v.z * v.x + (res.v.y * w + res.w * v.y)) - res.v.x * v.z;
+    ret.v.z = (res.v.x * v.y + (res.v.z * w + res.w * v.z)) - res.v.y * v.x;
+
+    return ret.v;
 }
 
 Quatf Quatf::slerpTo(const Quatf &q1, f32 t) const {
@@ -96,6 +119,24 @@ void Quatf::setAxisRotation(f32 angle, const EGG::Vector3f &axis) {
     v = axis * s;
 }
 
+Quatf Quatf::multSwap(const Vector3f &vec) const {
+    f32 _w = -(v.dot(vec));
+    f32 _x = (w * vec.x + v.y * vec.z) - v.z * vec.y;
+    f32 _y = (w * vec.y + v.z * vec.x) - v.x * vec.z;
+    f32 _z = (w * vec.z + v.x * vec.y) - v.y * vec.x;
+
+    return Quatf(_w, _x, _y, _z);
+}
+
+Quatf Quatf::multSwap(const Quatf &q) const {
+    f32 _w = ((w * q.w - v.x * q.v.x) - v.y * q.v.y) - v.z * q.v.z;
+    f32 _x = (v.y * q.v.z + (v.x * q.w + w * q.v.x)) - v.z * q.v.y;
+    f32 _y = (v.z * q.v.x + (v.y * q.w + w * q.v.y)) - v.x * q.v.z;
+    f32 _z = (v.x * q.v.y + (v.z * q.w + w * q.v.z)) - v.y * q.v.x;
+
+    return Quatf(_w, _x, _y, _z);
+}
+
 void Quatf::read(Stream &stream) {
     v.read(stream);
     w = stream.read_f32();

source/egg/math/Quat.hh

@@ -61,13 +61,16 @@ struct Quatf {
 
     void setRPY(const Vector3f &rpy);
     void normalise();
+    void makeVectorRotation(const Vector3f &from, const Vector3f &to);
     Quatf conjugate() const;
     Vector3f rotateVector(const Vector3f &vec) const;
     Vector3f rotateVectorInv(const Vector3f &vec) const;
     Quatf slerpTo(const Quatf &q2, f32 t) const;
     f32 dot() const;
     f32 dot(const Quatf &q) const;
-    void setAxisRotation(f32 angle, const EGG::Vector3f &axis);
+    void setAxisRotation(f32 angle, const Vector3f &axis);
+    Quatf multSwap(const Vector3f &v) const;
+    Quatf multSwap(const Quatf &q) const;
 
     void read(Stream &stream);
 

source/egg/util/Stream.cc

@@ -83,6 +83,14 @@ RamStream::RamStream(u8 *buffer, u32 size) {
 
 RamStream::~RamStream() = default;
 
+RamStream RamStream::split(u32 size) {
+    RamStream stream = RamStream(m_buffer + m_index, size);
+    m_index += size;
+    assert(!eof());
+
+    return stream;
+}
+
 void RamStream::read(void *output, u32 size) {
     u8 *buffer = reinterpret_cast<u8 *>(output);
     for (size_t i = 0; i < size; ++i) {

source/egg/util/Stream.hh

@@ -54,6 +54,8 @@ public:
     RamStream(u8 *buffer, u32 size);
     ~RamStream() override;
 
+    RamStream split(u32 size);
+
     void read(void *output, u32 size) override;
     void write(void *input, u32 size) override;
     bool eof() override;

source/game/kart/KartCollide.cc

@@ -9,8 +9,7 @@
 
 namespace Kart {
 
-KartCollide::KartCollide()
-    : m_offRoad(false), m_groundBoostPanelOrRamp(false), m_notTrickable(false) {}
+KartCollide::KartCollide() : m_notTrickable(false) {}
 
 KartCollide::~KartCollide() = default;
 
@@ -168,13 +167,15 @@ void KartCollide::processFloor(CollisionData &collisionData, Hitbox & /*hitbox*/
     const Field::CollisionDirector::CollisionEntry *closestColEntry =
             Field::CollisionDirector::Instance()->closestCollisionEntry();
 
-    if (!(closestColEntry->attribute & 0x2000)) {
+    u16 attribute = closestColEntry->attribute;
+    if (!(attribute & 0x2000)) {
         m_notTrickable = true;
     }
 
-    collisionData.intensity = (closestColEntry->attribute >> 0xb) & 3;
+    collisionData.intensity = (attribute >> 0xb) & 3;
+    collisionData.rotFactor += param()->stats().kclRot[attribute & 0x1f];
     collisionData.closestFloorFlags = closestColEntry->typeMask;
-    collisionData.closestFloorSettings = (closestColEntry->attribute >> 5) & 7;
+    collisionData.closestFloorSettings = (attribute >> 5) & 7;
 
     if (!(*maskOut & KCL_TYPE_BIT(COL_TYPE_BOOST_RAMP))) {
         m_notTrickable = true;
@@ -198,7 +199,8 @@ void KartCollide::applySomeFloorMoment(f32 down, f32 rate, CollisionGroup *hitbo
     EGG::Matrix34f rotMat;
     rotMat.makeQ(dynamics()->mainRot());
     EGG::Matrix34f tmp = rotMat.multiplyTo(dynamics()->invInertiaTensor());
-    tmp = tmp.multiplyTo(rotMat);
+    EGG::Matrix34f rotMatTrans = rotMat.transpose();
+    tmp = tmp.multiplyTo(rotMatTrans);
 
     EGG::Vector3f crossVec = colData.relPos.cross(colData.floorNrm);
     crossVec = tmp.multVector(crossVec);
@@ -236,7 +238,7 @@ void KartCollide::applySomeFloorMoment(f32 down, f32 rate, CollisionGroup *hitbo
     if (dVar5 < EGG::Mathf::abs(rejNorm)) {
         rejNorm_ = dVar5;
         if (rejNorm < 0.0f) {
-            rejNorm = -rate * EGG::Mathf::abs(scalar);
+            rejNorm_ = -rate * EGG::Mathf::abs(scalar);
         }
     }