Adding angular properties to objects

includes/math/geometry.hpp

@@ -4,6 +4,7 @@
 #include <vector>
 #include "vectors.hpp"
 #include "quaternions.hpp"
+#include "matrices.hpp"
 
 class Object;
 
@@ -27,10 +28,12 @@ class Geometry
     virtual GeometryType GetType() const = 0;
     virtual bool CollidesWith(const Geometry &other) const = 0;
 
+    virtual Matrix3x3 GetLocalInertiaMatrix() const = 0;
     const Vec3 &GetLocalOffset() const { return offset; }
     const Quat4 &GetLocalRotation() const { return rotation; }
     Vec3 GetWorldCenter() const;
     Quat4 GetWorldRotation() const;
+
 
     void SetOwner(Object *obj) { object = obj; }
 
@@ -82,6 +85,8 @@ class Box : public Geometry
 
     std::vector<Vec3> GetNormalAxis() const;
 
+    Matrix3x3 GetLocalInertiaMatrix() const override;
+
     const Vec3 &GetHalfExtends() const { return halfExtends; }
     void SetHalfExtends(double _x, double _y, double _z)
     {
@@ -103,6 +108,7 @@ class Sphere : public Geometry {
 
     GeometryType GetType() const override { return GeometryType::SPHERE; }
     bool CollidesWith(const Geometry &other) const override;
+    Matrix3x3 GetLocalInertiaMatrix() const override;
 
     double GetRadius() const { return radius; }
     void SetRadius(double _r) { this->radius = _r; }

includes/math/matrices.hpp

@@ -0,0 +1,100 @@
+#ifndef MATRICES_HPP
+#define MATRICES_HPP
+
+#include "vectors.hpp"
+#include "quaternions.hpp"
+
+struct Matrix3x3
+{
+    double data[9];
+
+    Matrix3x3()
+    {
+        data[0] = 0.0;
+        data[1] = 0.0;
+        data[2] = 0.0;
+        data[3] = 0.0;
+        data[4] = 0.0;
+        data[5] = 0.0;
+        data[6] = 0.0;
+        data[7] = 0.0;
+        data[8] = 0.0;
+    };
+
+    Matrix3x3(double d0, double d1, double d2,
+              double d3, double d4, double d5,
+              double d6, double d7, double d8)
+    {
+        data[0] = d0;
+        data[1] = d1;
+        data[2] = d2;
+        data[3] = d3;
+        data[4] = d4;
+        data[5] = d5;
+        data[6] = d6;
+        data[7] = d7;
+        data[8] = d8;
+    }
+    double &operator()(int row, int col)
+    {
+        return data[row * 3 + col];
+    }
+    const double &operator()(int row, int col) const
+    {
+        return data[row * 3 + col];
+    }
+
+    Matrix3x3 &operator*=(double scalar)
+    {
+        for (int i = 0; i < 3; i++)
+            for (int j = 0; j < 3; j++)
+                (*this)(i, j) *= scalar;
+        return *this;
+    }
+    Matrix3x3 &operator*=(const Matrix3x3 &other);
+    Matrix3x3 &operator+=(const Matrix3x3 &other)
+    {
+        for (int i = 0; i < 9; i++)
+            data[i] += other.data[i];
+        return *this;
+    }
+    Matrix3x3 &operator-=(const Matrix3x3 &other)
+    {
+        for (int i = 0; i < 9; i++)
+            data[i] -= other.data[i];
+        return *this;
+    }
+
+    Quat4 ToQuaternion() const;
+
+    Matrix3x3 Transpose() const
+    {
+        return Matrix3x3(
+            data[0], data[3], data[6],
+            data[1], data[4], data[7],
+            data[2], data[5], data[8]);
+    }
+    static Matrix3x3 Identity()
+    {
+        Matrix3x3 identity;
+        identity(0, 0) = 1.0;
+        identity(1, 1) = 1.0;
+        identity(2, 2) = 1.0;
+        return identity;
+    }
+};
+
+inline Matrix3x3 operator*(Matrix3x3 l, const Matrix3x3 &r) { return l *= r; }
+inline Matrix3x3 operator*(Matrix3x3 l, double scalar) { return l *= scalar; }
+inline Vec3 operator*(const Vec3 &vec, const Matrix3x3 &matrix)
+{
+    return Vec3(
+        matrix(0, 0) * vec.x + matrix(0, 1) * vec.y + matrix(0, 2) * vec.z,
+        matrix(1, 0) * vec.x + matrix(1, 1) * vec.y + matrix(1, 2) * vec.z,
+        matrix(2, 0) * vec.x + matrix(2, 1) * vec.y + matrix(2, 2) * vec.z);
+}
+inline Vec3 operator*(const Matrix3x3 &m, const Vec3 &vec) { return vec * m; }
+inline Matrix3x3 operator+(Matrix3x3 l, const Matrix3x3 &r) { return l += r; }
+inline Matrix3x3 operator-(Matrix3x3 l, const Matrix3x3 &r) { return l -= r; }
+
+#endif

includes/math/quaternions.hpp

@@ -3,6 +3,8 @@
 
 #include "vectors.hpp"
 
+struct Matrix3x3;
+
 struct Quat4
 {
     double w, x, y, z;
@@ -61,6 +63,8 @@ struct Quat4
         z = w1 * z2 + w2 * z1 + x1 * y2 - y1 * x2;
         return *this;
     }
+
+    Matrix3x3 ToMatrix() const;
 };
 
 inline Quat4 operator+(Quat4 l, const Quat4 &r) { return l += r; }

includes/math/vectors.hpp

@@ -4,6 +4,12 @@
 #include <cmath>
 #include "config.hpp"
 
+// Forward declaraction
+struct Matrix3x3;
+
+// Forward declaraction
+struct Matrix3x3;
+
 // Vectors
 struct Vec3
 {
@@ -43,6 +49,8 @@ struct Vec3
         return *this;
     }
 
+    Matrix3x3 SelfInverseOuter() const;
+
     constexpr double Dot(const Vec3 &other) const
     {
         return x * other.x + y * other.y + z * other.z;

includes/physic/object.hpp

@@ -6,17 +6,24 @@
 #include "force.hpp"
 #include "vectors.hpp"
 #include "quaternions.hpp"
+#include "matrices.hpp"
 
 class Geometry;
 
 class Object
 {
 private:
+    // Linear property
     double mass;
-    Quat4 rotation;
     Vec3 position;
     Vec3 velocity;
 
+    // Angular property
+    Matrix3x3 inertia_matrix;
+    Quat4 rotation;
+    Quat4 angular_velocity;
+
+    // Overal property
     std::vector<Force> forces;
     std::unique_ptr<Geometry> geometry;
 
@@ -25,13 +32,13 @@ class Object
 public:
     Object(double mass, std::unique_ptr<Geometry> geom)
         : mass(mass), position(Vec3()), rotation(Quat4()), velocity(Vec3()), geometry(std::move(geom)) { SetGeometryOwner(); }
-    Object(double mass, Vec3 position, Quat4 rotation, std::unique_ptr<Geometry> geom)
-        : mass(mass), position(position), rotation(rotation), geometry(std::move(geom)), velocity(Vec3()) { SetGeometryOwner(); }
 
     double GetMass() const { return mass; }
     const Vec3 &GetVelocity() const { return velocity; }
     const Vec3 &GetPosition() const { return position; }
+    const Matrix3x3 &GetInertiaMatrix() const { return inertia_matrix; }
     const Quat4 &GetRotation() const { return rotation; }
+    const Quat4 &GetAngularVelocity() const { return angular_velocity; }
     const std::vector<Force> &GetForces() const { return forces; }
 
     void SetMass(double m)

sources/math/geometry.cpp

@@ -148,6 +148,39 @@ bool Sphere::CollidesWith(const Geometry &other) const
     return false;
 }
 
+// Box specifics
+Matrix3x3 Box::GetLocalInertiaMatrix() const
+{
+    double mass = this->object->GetMass();
+    Matrix3x3 inertiaMatrix;
+    const Vec3 fullExtends = this->GetHalfExtends() * 2.0;
+    const Vec3 offset = this->GetLocalOffset();
+
+    inertiaMatrix(0, 0) = 1.0 / 12.0 * mass * (fullExtends.y * fullExtends.y + fullExtends.z * fullExtends.z);
+    inertiaMatrix(1, 1) = 1.0 / 12.0 * mass * (fullExtends.x * fullExtends.x + fullExtends.z * fullExtends.z);
+    inertiaMatrix(2, 2) = 1.0 / 12.0 * mass * (fullExtends.y * fullExtends.y + fullExtends.x * fullExtends.x);
+
+    Matrix3x3 rotationMatrix = this->GetWorldRotation().ToMatrix();
+    Matrix3x3 rotatedMatrix = (rotationMatrix * inertiaMatrix) * rotationMatrix.Transpose();
+    Matrix3x3 res = rotatedMatrix + (Matrix3x3::Identity() * offset.LengthSquared() - offset.SelfInverseOuter()) * mass;
+
+    return res;
+}
+
+Matrix3x3 Sphere::GetLocalInertiaMatrix() const
+{
+    double mass = this->object->GetMass();
+    double radius = this->GetRadius();
+    const Vec3 offset = this->GetLocalOffset();
+
+    double sphere_angular_inertia = 2.0 / 5.0 * mass * radius * radius;
+    Matrix3x3 inertiaLocal = Matrix3x3::Identity() * sphere_angular_inertia;
+    Matrix3x3 offsetOuter = offset.SelfInverseOuter();
+    Matrix3x3 paralelAxis = (Matrix3x3::Identity() * offset.LengthSquared() - offsetOuter) * mass;
+    Matrix3x3 res = inertiaLocal + paralelAxis;
+    return res;
+}
+
 // General functions
 Vec3 Geometry::GetWorldCenter() const { return offset + object->GetPosition(); }
 Quat4 Geometry::GetWorldRotation() const

sources/math/matrices.cpp

@@ -0,0 +1,58 @@
+#include "matrices.hpp"
+
+Matrix3x3 &Matrix3x3::operator*=(const Matrix3x3 &other)
+{
+    Matrix3x3 result;
+    for (int row = 0; row < 3; ++row)
+    {
+        for (int col = 0; col < 3; ++col)
+        {
+            result(row, col) = 0.0;
+            for (int k = 0; k < 3; ++k)
+            {
+                result(row, col) += (*this)(row, k) * other(k, col);
+            }
+        }
+    }
+    *this = result;
+    return *this;
+}
+Quat4 Matrix3x3::ToQuaternion() const
+{
+    // Convert the 3x3 matrix to a quaternion
+    double trace = data[0] + data[4] + data[8];
+    double w, x, y, z;
+    if (trace > 0)
+    {
+        double s = sqrt(trace + 1.0) * 2; // S= 4 * w
+        w = 0.25 * s;
+        x = (data[7] - data[5]) / s;
+        y = (data[2] - data[6]) / s;
+        z = (data[3] - data[1]) / s;
+    }
+    else if ((data[0] > data[4]) && (data[0] > data[8]))
+    {
+        double s = sqrt(1.0 + data[0] - data[4] - data[8]) * 2; // S= 4 * x
+        w = (data[7] - data[5]) / s;
+        x = 0.25 * s;
+        y = (data[1] + data[3]) / s;
+        z = (data[2] + data[6]) / s;
+    }
+    else if (data[4] > data[8])
+    {
+        double s = sqrt(1.0 + data[4] - data[0] - data[8]) * 2; // S= 4 * y
+        w = (data[2] - data[6]) / s;
+        x = (data[1] + data[3]) / s;
+        y = 0.25 * s;
+        z = (data[5] + data[7]) / s;
+    }
+    else
+    {
+        double s = sqrt(1.0 + data[8] - data[0] - data[4]) * 2; // S= 4 * z
+        w = (data[3] - data[1]) / s;
+        x = (data[2] + data[6]) / s;
+        y = (data[5] + data[7]) / s;
+        z = 0.25 * s;
+    }
+    return Quat4(w, x, y, z);
+}

sources/math/quaternions.cpp

@@ -0,0 +1,19 @@
+#include "quaternions.hpp"
+#include "matrices.hpp"
+
+Matrix3x3 Quat4::ToMatrix() const
+{
+    {
+        Matrix3x3 M;
+        M(0, 0) = 1 - 2 * (y * y + z * z);
+        M(0, 1) = 2 * (x * y - w * z);
+        M(0, 2) = 2 * (x * z + w * y);
+        M(1, 0) = 2 * (x * y + w * z);
+        M(1, 1) = 1 - 2 * (x * x + z * z);
+        M(1, 2) = 2 * (y * z - w * x);
+        M(2, 0) = 2 * (x * z - w * y);
+        M(2, 1) = 2 * (y * z + w * x);
+        M(2, 2) = 1 - 2 * (x * x + y * y);
+        return M;
+    }
+}

sources/math/vectors.cpp

@@ -0,0 +1,10 @@
+#include "vectors.hpp"
+#include "matrices.hpp"
+
+Matrix3x3 Vec3::SelfInverseOuter() const
+{
+    return Matrix3x3(
+        x * x, x * y, x * z,
+        y * x, y * y, y * z,
+        z * x, z * y, z * z);
+}

sources/physic/object.cpp

@@ -4,6 +4,9 @@
 void Object::SetGeometryOwner()
 {
     if (geometry)
+    {
         geometry->SetOwner(this);
+        inertia_matrix = geometry->GetLocalInertiaMatrix();
+    }
 }
 Object::~Object() = default;

tests/test_collision.cpp

@@ -3,8 +3,8 @@
 #include "object.hpp"
 
 
-Object obj1(1, Vec3(), Quat4(), nullptr);
-Object obj2(1, Vec3(), Quat4(), nullptr);
+Object obj1(1, nullptr);
+Object obj2(1, nullptr);
 
 TEST(BoxBoxCollision, OverlappingBoxes) {
     Box b1(Vec3(1, 1, 1), &obj1);

tests/test_collision_2.cpp

@@ -3,7 +3,7 @@
 #include "math/geometry.hpp"
 #include "physic/object.hpp"
 
-Object obj(1, Vec3(), Quat4(), nullptr);
+Object obj(1, nullptr);
 
 TEST(SphereCollisionTest, OverlappingSpheresCollide) {
     Sphere s1(2.0, &obj);