working soft bodies

JerboaBurrow · Dec 1, 2023 · 8c29826 · 8c29826
1 parent d373f31
commit 8c29826
Show file tree

Hide file tree

Showing 7 changed files with 73 additions and 26 deletions.
diff --git a/demo/android/perlinWorld/app/src/main/cpp/include/Component/cCollideable.h b/demo/android/perlinWorld/app/src/main/cpp/include/Component/cCollideable.h
@@ -31,10 +31,11 @@ namespace Hop::Object::Component
             double x,
             double y,
             double theta, 
-            double scale
+            double scale,
+            double dt
         )
         {
-            mesh.updateWorldMesh(x, y, theta, scale);
+            mesh.updateWorldMesh(x, y, theta, scale, dt);
             this->x = x;
             this->y = y;
         }

diff --git a/demo/desktop/perlinWorld/sfml/sfmlPerlinWorld.cpp b/demo/desktop/perlinWorld/sfml/sfmlPerlinWorld.cpp
@@ -298,6 +298,8 @@ int main(int argc, char ** argv)
     glClearColor(1.0f,1.0f,1.0f,1.0f);
     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 
+    tp0 = high_resolution_clock::now();
+
     collisions.centreOn(world.get()->getMapCenter());
 
     if (!paused)

diff --git a/include/Collision/collisionMesh.h b/include/Collision/collisionMesh.h
@@ -141,8 +141,6 @@ namespace Hop::System::Physics
             double rox = x-ox;
             double roy = y-oy;
 
-            std::cout << ox << ", " << oy << ", " << x << ", " << y << ", " << fx << ", " << fy << ", " << vx << ", " << vy << "\n";
-
             // spring with relaxed state at ox, oy;
             double ax = (fx-stiffness*rox-damping*vx)/mass;
             double ay = (fy-stiffness*roy-damping*vy)/mass;
@@ -414,11 +412,13 @@ namespace Hop::System::Physics
         );
 
         double bestAngle();
-        void centerOfMass(double & cx, double & cy);
+        void centerOfMassWorld(double & cx, double & cy);
+        void modelToCenterOfMassFrame();
 
         double momentOfInertia();
         void computeRadius();
         double getRadius(){return radius;}
+        double netTorque();
 
         double getX(){return x;}
         double getY(){return y;}
@@ -431,7 +431,7 @@ namespace Hop::System::Physics
         {
             for (auto w : worldVertices)
             {
-                w->applyForce(fx/size(), fy/size());
+                w->applyForce(fx, fy);
             }
         }
 

diff --git a/include/Component/cCollideable.h b/include/Component/cCollideable.h
@@ -30,7 +30,7 @@ namespace Hop::Object::Component
 
         cCollideable(){}
 
-        void updateWorldMesh(
+        void updateMesh(
             double x,
             double y,
             double theta, 
@@ -39,8 +39,8 @@ namespace Hop::Object::Component
         )
         {
             mesh.updateWorldMesh(x, y, theta, scale, dt);
-            this->x = x;
-            this->y = y;
+            this->x = mesh.getX();
+            this->y = mesh.getY();
         }
 
     };

diff --git a/src/Collision/collisionMesh.cpp b/src/Collision/collisionMesh.cpp
@@ -64,7 +64,7 @@ namespace Hop::System::Physics
         computeRadius();
     }
 
-    void CollisionMesh::centerOfMass(double & cx, double & cy)
+    void CollisionMesh::centerOfMassWorld(double & cx, double & cy)
     {
         cx = 0.0;
         cy = 0.0;
@@ -73,15 +73,34 @@ namespace Hop::System::Physics
             cx += worldVertices[i]->x;
             cy += worldVertices[i]->y;
         }
-        cx /= float(worldVertices.size());
-        cy /= float(worldVertices.size());
+        cx /= double(worldVertices.size());
+        cy /= double(worldVertices.size());
+    }
+
+    void CollisionMesh::modelToCenterOfMassFrame()
+    {
+        double cx = 0.0;
+        double cy = 0.0;
+        for (unsigned i = 0; i < vertices.size(); i++)
+        {
+            cx += vertices[i]->x;
+            cy += vertices[i]->y;
+        }
+        cx /= double(vertices.size());
+        cy /= double(vertices.size());
+
+        for (unsigned i = 0; i < vertices.size(); i++)
+        {
+            vertices[i]->x -= cx;
+            vertices[i]->y -= cy;
+        }
     }
 
     double CollisionMesh::bestAngle()
     {
         double cx = 0.0;
         double cy = 0.0;
-        centerOfMass(cx, cy);
+        centerOfMassWorld(cx, cy);
         double a = 0.0;
         double b = 0.0;
         double refx, refy;
@@ -100,7 +119,17 @@ namespace Hop::System::Physics
 
         double omega = std::atan2(a, b);
 
-        return omega < 0.0 ? 2.0*M_PI+omega : omega;
+        return omega;
+    }
+
+    double CollisionMesh::netTorque()
+    {
+        double tau = 0.0;
+        for (auto p : worldVertices)
+        {
+            tau += (p->x - x) * p->fx - (p->y - y) * p->fy;
+        }
+        return tau;
     }
 
     void CollisionMesh::updateWorldMeshSoft(
@@ -119,40 +148,43 @@ namespace Hop::System::Physics
             this->y = y;
             this->theta = theta;
             this->scale = scale;
+            modelToCenterOfMassFrame();
         }
 
         double c = std::cos(theta);
         double s = std::sin(theta);
 
         double omega = bestAngle();
+        this->theta = omega;
 
         double co = std::cos(omega);
         double so = std::sin(omega);
 
-        std::vector<uint8_t> inside(worldVertices.size());
 
+        std::vector<uint8_t> inside(worldVertices.size());
+
         for (unsigned i = 0; i < vertices.size(); i++)
         {
             inside[i] = worldVertices[i]->lastInside;
             if (init)
             {
                 worldVertices[i]->setOrigin
                 (
-                    (vertices[i]->x*c + vertices[i]->y*s)*scale + x,
-                    (vertices[i]->y*c - vertices[i]->x*s)*scale + y
+                    (vertices[i]->x*c + vertices[i]->y*s)*this->scale + this->x,
+                    (vertices[i]->y*c - vertices[i]->x*s)*this->scale + this->y
                 );
             }
             else
             {
                 worldVertices[i]->step
                 (
                     dt,
-                    (vertices[i]->x*co + vertices[i]->y*so)*scale + x,
-                    (vertices[i]->y*co - vertices[i]->x*so)*scale + y
+                    (vertices[i]->x*co + vertices[i]->y*so)*this->scale + this->x,
+                    (vertices[i]->y*co - vertices[i]->x*so)*this->scale + this->y
                 );
             }
 
-            worldVertices[i]->r = vertices[i]->r*scale;
+            worldVertices[i]->r = vertices[i]->r*this->scale;
             worldVertices[i]->lastInside = inside[i]; 
 
             Rectangle * lw = dynamic_cast<Rectangle*>(worldVertices[i].get());
@@ -183,13 +215,25 @@ namespace Hop::System::Physics
             }
         }
 
-        if (!init)
+        this->theta = omega;
+        centerOfMassWorld(this->x, this->y);
+
+        if (init)
         {
-            this->theta = omega;
-            centerOfMass(this->x, this->y);
+            double c = std::cos(this->theta);
+            double s = std::sin(this->theta);
+            for (unsigned i = 0; i < vertices.size(); i++)
+            {
+                worldVertices[i]->setOrigin
+                (
+                    (vertices[i]->x*c + vertices[i]->y*s)*this->scale + this->x,
+                    (vertices[i]->y*c - vertices[i]->x*s)*this->scale + this->y
+                );
+            }
         }
 
         computeRadius();
+
     }
 
     void CollisionMesh::computeRadius()

diff --git a/src/System/sPhysics.cpp b/src/System/sPhysics.cpp
@@ -99,7 +99,7 @@ namespace Hop::System::Physics
             if (collideables.hasComponent(id))
             {
                 cCollideable & data = collideables.get(id);
-                data.updateWorldMesh(
+                data.updateMesh(
                     dataT.x,
                     dataT.y,
                     dataT.theta,
@@ -254,7 +254,7 @@ namespace Hop::System::Physics
             if (collideables.hasComponent(*it))
             {
                 cCollideable & data = collideables.get(*it);
-                data.updateWorldMesh(
+                data.updateMesh(
                     dataT.x,
                     dataT.y,
                     dataT.theta,

diff --git a/tests/tetris.lua b/tests/tetris.lua
@@ -44,7 +44,7 @@ for i = 1, 1 do
         ["colour"] = {200/255,250/255,250/255,1.0},
         ["moveable"] = true,
         ["collisionMesh"] = S_mesh,
-        ["meshParameters"] = {1000.0, 0.0, 0.1},
+        ["meshParameters"] = {10000.0, 10.0, 0.1},
         ["name"] = ""
 
     }