Refactor system dynamics and rigid body handling, add UI improvements…

… for simulation control, and enhance external force/step time support.

README.md

@@ -71,6 +71,30 @@ $$
     \begin{bmatrix} g^* \cr 0 \cr \gamma \end{bmatrix}
 $$
 
+## Unconstrained Demo Body
+
+### Initial Position, Orientation, Velocity & Angular Velocity
+
+1 x 1 x 1 m 10 kilogram box. 
+
+Mass: $ m = 10 kg $
+
+Moment of Inertia:  $ I = 60 kgm^2 $
+
+$$
+    r_1 = \begin{bmatrix} 0 \cr 0 \cr 0 \end{bmatrix} \quad
+    p_1 = \begin{bmatrix} 1 \cr 0 \cr 0 \cr 0 \end{bmatrix} \quad
+    \dot r_1 = \begin{bmatrix} 0 \cr 0 \cr 0 \end{bmatrix} \quad
+    \dot p_1 = \begin{bmatrix} 0 \cr 0 \cr 0 \cr 0 \end{bmatrix}
+$$
+
+### Mass & Inertia Matrix
+
+$$
+    N_1 = \begin{bmatrix} 10 & 0 & 0 \cr 0 & 10 & 0 \cr 0 & 0 & 10 \end{bmatrix} \quad
+    J_1^* = \begin{bmatrix} 0 & 0 & 0 & 0 \cr 0 & 60 & 0 & 0 \cr 0 & 0 & 60 & 0 \cr 0 & 0 & 0 & 60 \end{bmatrix} \quad
+$$
+
 # Variable Mapping Table
 
 | **Variable from Book**     | **Description**                                   | **Corresponding Name in Code**      | **Additional Notes**                       |

include/dynamics.h

@@ -27,7 +27,7 @@ class Dynamics {
     explicit Dynamics();
 
     void addBody(const std::shared_ptr<RigidBody>& body);
-    void step(double deltaTime);
+    void step();
 
     void debug();
 
@@ -57,7 +57,13 @@ class Dynamics {
     void stopSimulation();
     void resetSimulation();
 
+    void setExternalForces(Vector3d matrix);
+
+    void setStepTime(double stepTime);
+
 private:
+    double m_stepTime = 0.0002;
+
     std::vector<std::shared_ptr<RigidBody>> m_Bodies;
 
     MatrixXd m_SystemMassInertiaMatrix;
@@ -79,7 +85,7 @@ class Dynamics {
     VectorXd m_B;
     VectorXd m_X;
 
-    bool m_isSimulationRunning = true;
+    bool m_isSimulationRunning = false;
 
     void initializeSize();
     void initializeContent();

include/rigid_body.h

@@ -8,11 +8,7 @@ using namespace Eigen;
 
 class RigidBody {
 public:
-    explicit RigidBody(
-            Vector3d position,
-            Vector4d orientation,
-            Matrix3d massMatrix,
-            Matrix3d globalInertiaTensor);
+    explicit RigidBody(double mass, Matrix3d globalInertiaTensor, Vector3d position, Vector4d orientation);
 
     void normalizeOrientation();
 
@@ -40,24 +36,27 @@ class RigidBody {
     void setVelocity(Vector3d velocity);
     void setAngularVelocity(Vector4d angularVelocity);
 
+    void setInertiaTensor(Matrix4d inertiaTensor);
+
 private:
-    Vector3d m_Position;
-    Vector3d m_Velocity;
+    double m_mass;
+
+    Vector3d m_position;
+    Vector3d m_velocity;
 
-    Vector4d m_Orientation;
-    Vector4d m_AngularVelocity;
+    Vector4d m_orientation;
+    Vector4d m_angularVelocity;
 
-    Matrix3d m_GlobalInertiaTensor;
+    Matrix3d m_globalInertiaTensor;
 
-    Matrix3d m_MassMatrix;
-    Matrix4d m_InertiaTensor;
+    Matrix3d m_massMatrix;
+    Matrix4d m_inertiaTensor;
 
     Vector3d m_initialPosition;
     Vector3d m_initialVelocity;
 
     Vector4d m_initialOrientation;
     Vector4d m_initialAngularVelocity;
-
 };
 
 

src/core/application.cpp

@@ -75,70 +75,9 @@ Application::~Application() {
 void Application::run() {
     double lastTime = glfwGetTime();
 
-
-    auto body1 = std::make_shared<RigidBody>(
-                Vector3d(0, 0, 0),
-                Vector4d(1, 0, 0, 0),
-                Matrix3d::Identity() * 10,
-                Matrix3d::Identity() * 60);
-
-    auto body2 = std::make_shared<RigidBody>(
-                Vector3d(0, 0, 0),
-                Vector4d(1, 0, 0, 0),
-                Matrix3d::Identity() * 10,
-                Matrix3d::Identity() * 60);
-    auto body3 = std::make_shared<RigidBody>(
-                Vector3d(0, 0, 0),
-                Vector4d(1, 0, 0, 0),
-                Matrix3d::Identity() * 10,
-                Matrix3d::Identity() * 60);
-    auto body4 = std::make_shared<RigidBody>(
-                Vector3d(0, 0, 0),
-                Vector4d(1, 0, 0, 0),
-                Matrix3d::Identity() * 10,
-                Matrix3d::Identity() * 60);
-    auto body5 = std::make_shared<RigidBody>(
-                Vector3d(0, 0, 0),
-                Vector4d(1, 0, 0, 0),
-                Matrix3d::Identity() * 10,
-                Matrix3d::Identity() * 60);
-    auto body6 = std::make_shared<RigidBody>(
-                Vector3d(0, 0, 0),
-                Vector4d(1, 0, 0, 0),
-                Matrix3d::Identity() * 10,
-                Matrix3d::Identity() * 60);
-
-    auto body7 = std::make_shared<RigidBody>(
-                Vector3d(0, 0, 0),
-                Vector4d(1, 0, 0, 0),
-                Matrix3d::Identity() * 10,
-                Matrix3d::Identity() * 60);
-    auto body8 = std::make_shared<RigidBody>(
-                Vector3d(0, 0, 0),
-                Vector4d(1, 0, 0, 0),
-                Matrix3d::Identity() * 10,
-                Matrix3d::Identity() * 60);
-    auto body9 = std::make_shared<RigidBody>(
-                Vector3d(0, 0, 0),
-                Vector4d(1, 0, 0, 0),
-                Matrix3d::Identity() * 10,
-                Matrix3d::Identity() * 60);
-    auto body10 = std::make_shared<RigidBody>(
-                Vector3d(0, 0, 0),
-                Vector4d(1, 0, 0, 0),
-                Matrix3d::Identity() * 10,
-                Matrix3d::Identity() * 60);
+    auto body1 = std::make_shared<RigidBody>(10.0, Matrix3d::Identity() * 60.0, Vector3d(0.0, 0.0, 0.0), Vector4d(1.0, 0.0, 0.0, 0.0));
 
     m_dynamics->addBody(body1);
-    //m_dynamics->addBody(body2);
-    //m_dynamics->addBody(body3);
-    //m_dynamics->addBody(body4);
-    //m_dynamics->addBody(body5);
-    //m_dynamics->addBody(body6);
-    //m_dynamics->addBody(body7);
-    //m_dynamics->addBody(body8);
-    //m_dynamics->addBody(body9);
-    //m_dynamics->addBody(body10);
 
     while (!glfwWindowShouldClose(m_window.get())) {
         double currentTime = glfwGetTime();
@@ -149,7 +88,7 @@ void Application::run() {
 
         // Step the physics simulation
         if (m_dynamics) {
-             m_dynamics->step(deltaTime);
+             m_dynamics->step();
         }
 
         update();

src/graphics/renderer.cpp

@@ -98,7 +98,7 @@ void Renderer::draw(Dynamics* dynamics) {
 
     glm::vec3 lightPos = glm::vec3(2.0f, 4.0f, 2.0f);
     glm::vec3 lightColor = glm::vec3(1.0f, 1.0f, 1.0f);
-    glm::vec3 objectColor = glm::vec3(1.0f, 0.4f, 0.8f);  // Your pinkish box
+    glm::vec3 objectColor = glm::vec3(0.9529f, 0.2941f, 0.4902f);  // Your pinkish box
 
     glUseProgram(m_GridShaderProgram);
     glUniform3fv(glGetUniformLocation(m_GridShaderProgram, "lightPos"), 1, glm::value_ptr(lightPos));
@@ -118,8 +118,6 @@ void Renderer::draw(Dynamics* dynamics) {
     for (int i = 0; i < dynamics->getBodyCount(); i++) {
         drawBox(dynamics->getBody(i)->getPosition(), Vector3d(1.0, 1.0, 1.0), dynamics->getBody(i)->getOrientation(), Vector3d(1.0, 0.2, 0.8));
     }
-
-
 }
 
 void Renderer::drawBox(const Vector3d& position, const Vector3d& scale, const Vector4d& rotation, const Vector3d& color) const {

src/simulation/dynamics.cpp

@@ -54,7 +54,8 @@ void Dynamics::initializeContent() {
 
         m_VelocityDependentTerm.middleRows(3, 4) = 2 * H * m_Bodies[i]->getAngularVelocity();
 
-        m_GeneralizedExternalForces.tail<4>() = 2 * transformationMatrixG(m_Bodies[i]->getOrientation()).transpose() * m_ExternalTorques;
+        m_GeneralizedExternalForces.segment(m, 3) = m_ExternalForces;
+        m_GeneralizedExternalForces.segment(m + 3, 4) = 2 * L.transpose() * m_ExternalTorques;
 
         m_B.segment(0, 7 * b) = m_GeneralizedExternalForces - m_VelocityDependentTerm;
         m_B.tail(b) = -m_QuaternionNormSquared;
@@ -67,7 +68,7 @@ void Dynamics::initializeContent() {
     m_A.topRightCorner(7 * b, b) = m_QuaternionConstraintMatrix.transpose();
 }
 
-void Dynamics::step(double deltaTime) {
+void Dynamics::step() {
     if (!m_isSimulationRunning) return;
     int b = getBodyCount();
 
@@ -77,14 +78,17 @@ void Dynamics::step(double deltaTime) {
     m_GeneralizedAccelerations = m_X.head(7 * b);
 
     // Integration
-    m_GeneralizedVelocities += m_GeneralizedAccelerations * deltaTime;
-    m_GeneralizedCoordinates += m_GeneralizedVelocities * deltaTime;
+    m_GeneralizedVelocities += m_GeneralizedAccelerations * m_stepTime;
+    m_GeneralizedCoordinates += m_GeneralizedVelocities * m_stepTime;
 
     for (int i = 0; i < b; i++) {
         m_Bodies[i]->setPosition(m_GeneralizedCoordinates.segment(7 * i, 3));
         m_Bodies[i]->setOrientation(m_GeneralizedCoordinates.segment(7 * i + 3, 4));
         m_Bodies[i]->setVelocity(m_GeneralizedVelocities.segment(7 * i, 3));
         m_Bodies[i]->setAngularVelocity(m_GeneralizedVelocities.segment(7 * i + 3, 4));
+
+        m_Bodies[i]->setInertiaTensor(4 * transformationMatrixL(m_GeneralizedCoordinates.segment(7 * i + 3, 4)).transpose() * m_Bodies[i]->getGlobalInertiaTensor() * transformationMatrixL(m_GeneralizedCoordinates.segment(7 * i + 3, 4)));
+
         m_Bodies[i]->normalizeOrientation();
     }
 }
@@ -235,3 +239,11 @@ void Dynamics::resetSimulation() {
     initializeSize();
     initializeContent();
 }
+
+void Dynamics::setExternalForces(Vector3d externalForces) {
+    m_ExternalForces = externalForces;
+}
+
+void Dynamics::setStepTime(double stepTime) {
+    m_stepTime = stepTime;
+}

src/simulation/rigid_body.cpp

@@ -1,78 +1,86 @@
 #include <utility>
+#include <iostream>
 
 #include "rigid_body.h"
 #include "tools.h"
 
 using namespace Eigen;
 
-RigidBody::RigidBody(Vector3d position, Vector4d orientation, Matrix3d massMatrix, Matrix3d globalInertiaTensor)
-    : m_MassMatrix(std::move(massMatrix)),
-      m_GlobalInertiaTensor(std::move(globalInertiaTensor)),
-      m_Position(std::move(position)),
-      m_Orientation(std::move(orientation)){
+RigidBody::RigidBody(double mass, Matrix3d globalInertiaTensor, Vector3d position, Vector4d orientation) {
 
-    m_Velocity = Vector3d::Zero();
-    m_AngularVelocity = Vector4d::Zero();
+    m_mass = mass;
+    m_massMatrix = Matrix3d::Identity() * m_mass;
+
+    m_position = position;
+    m_orientation = orientation;
+
+    m_globalInertiaTensor = globalInertiaTensor;
+
+    m_inertiaTensor = 4 * transformationMatrixL(m_orientation).transpose() * m_globalInertiaTensor * transformationMatrixL(m_orientation);
+
+    m_velocity = Vector3d::Zero();
+    m_angularVelocity = Vector4d::Zero();
 
     m_initialPosition = position;
     m_initialOrientation = orientation;
-    m_initialAngularVelocity = m_AngularVelocity;
-    m_initialVelocity = m_Velocity;
+    m_initialAngularVelocity = m_angularVelocity;
+    m_initialVelocity = m_velocity;
 }
 
 Vector3d RigidBody::getPosition() {
-    return m_Position;
+    return m_position;
 }
 
 Vector4d RigidBody::getOrientation() {
-    return m_Orientation;
+    return m_orientation;
 }
 
 Matrix3d RigidBody::getMassMatrix() {
-    return m_MassMatrix;
+    return m_massMatrix;
 }
 
 Matrix3d RigidBody::getGlobalInertiaTensor() {
-    return m_GlobalInertiaTensor;
+    return m_globalInertiaTensor;
 }
 
 Vector4d RigidBody::getAngularVelocity() {
-    Vector4d angularVelocity = m_AngularVelocity;
-    // angularVelocity[0] = 0.0; // Ensure the first component is 0 when retrieved
-    return angularVelocity;
+    return m_angularVelocity;
 }
 
 Vector3d RigidBody::getVelocity() {
-    return m_Velocity;
+    return m_velocity;
 }
 
 void RigidBody::setPosition(Vector3d position) {
-    m_Position = position;
+    m_position = position;
 }
 
 void RigidBody::setAngularVelocity(Vector4d angularVelocity) {
-    // angularVelocity[0] = 0.0;
-    m_AngularVelocity = angularVelocity;
+    m_angularVelocity = angularVelocity;
 }
 
 void RigidBody::setVelocity(Vector3d velocity) {
-    m_Velocity = velocity;
+    m_velocity = velocity;
 }
 
 void RigidBody::setOrientation(Vector4d orientation) {
-    m_Orientation = orientation;
+    m_orientation = orientation;
+}
+
+void RigidBody::setInertiaTensor(Matrix4d inertiaTensor) {
+    m_inertiaTensor = inertiaTensor;
 }
 
 void RigidBody::normalizeOrientation() {
-    m_Orientation / m_Orientation.norm();
+    m_orientation / m_orientation.norm();
 }
 
 double RigidBody::getQuaternionNormSquared() {
-    return m_AngularVelocity.transpose() * m_AngularVelocity;
+    return m_angularVelocity.transpose() * m_angularVelocity;
 }
 
 Matrix4d RigidBody::getInertiaTensor() {
-    return 4 * transformationMatrixL(m_Orientation).transpose() * m_GlobalInertiaTensor * transformationMatrixL(m_Orientation);
+    return m_inertiaTensor;
 }
 
 Vector3d RigidBody::getInitialPosition() {
@@ -90,6 +98,3 @@ Vector4d RigidBody::getInitialAngularVelocity() {
 Vector3d RigidBody::getInitialVelocity() {
     return m_initialVelocity;
 }
-
-// Matrix4d inertiaTensor = 4 * transformationMatrix.transpose() * globalInertiaTensor * transformationMatrix;
-