stuff

Physics.h

@@ -12,11 +12,13 @@
 #include <glm/glm.hpp>
 #include <glm/gtc/matrix_transform.hpp>
 #include <glm/gtc/type_ptr.hpp>
+#include <glm/gtc/quaternion.hpp>
 #include "common.h"
 #include "Engine.h"
 #include "Camera.h"
 #include "t.h"
 #include "tFunctions.h"
+#include "CollisionMesh.h"
 
 // ============================================================================
 // Tuneable constants
@@ -29,6 +31,13 @@ static constexpr float PHYSICS_SLEEP_LINEAR_SQ = 0.01f * 0.01f;
 static constexpr float PHYSICS_SLEEP_ANGULAR_SQ = 0.01f * 0.01f;
 static constexpr float PHYSICS_SLEEP_TIME_THRESHOLD = 0.5f;
 static constexpr float PHYSICS_RESTITUTION_THRESHOLD = 1.0f;
+static constexpr int   PHYSICS_JOINT_ITERATIONS = 12;
+static constexpr float PHYSICS_JOINT_BIAS = 0.2f;
+static constexpr float PHYSICS_JOINT_MAX_BIAS_SPEED = 5.0f;
+static constexpr float PHYSICS_JOINT_DAMPING = 5.0f;
+static constexpr int   PHYSICS_MAX_MESH_CONTACTS = 4;
+static constexpr float PHYSICS_MAX_MESH_DEPTH = 0.1f;
+static constexpr float PHYSICS_MESH_PAIR_FRICTION = 0.25f;  // two meshes catch on each other, so they slide easier
 
 // ============================================================================
 // Physics body
@@ -65,15 +74,23 @@ public:
     bool velocity = true;
     bool isSleeping = false;
     bool lockRotation = false;
+    bool frozen = false;                          // parked until something touches its group
+
+    // Set this to collide triangle by triangle, otherwise the body is its t's box
+    const CollisionMesh* collisionMesh = nullptr;
+    int collisionGroup = 0;                       // non zero groups don't collide with themselves
 
     float mass = 1.0f;
     float restitution = 0.2f;
     float friction = 0.5f;
+    float maxSpeed = 0.0f;                        // 0 = no limit
+    float maxAngularSpeed = 0.0f;
 
     glm::vec3 linearvelocity = { 0.0f, 0.0f, 0.0f };
     glm::vec3 angularvelocity = { 0.0f, 0.0f, 0.0f };
     glm::vec3 force = { 0.0f, 0.0f, 0.0f };
     glm::vec3 torque = { 0.0f, 0.0f, 0.0f };
+    glm::mat3 invInertia = glm::mat3(0.0f);       // world space, refreshed at the start of each step
 
     float sleepTimer = 0.0f;
 };
@@ -85,23 +102,87 @@ struct Collision {
     glm::vec3 POI;
     glm::vec3 CN;
     float overlap = 0.0f;
+    float correctionWeight = 1.0f;   // shares positional correction between a pair's contacts
+    float frictionScale = 1.0f;
     p* ObjA = nullptr;
     p* ObjB = nullptr;
 
     Collision(p* a, p* b) : ObjA(a), ObjB(b) {}
 };
 
+// ============================================================================
+// One angular constraint on a joint, worked out once per step
+// ============================================================================
+struct AngularRow {
+    glm::vec3 axis = glm::vec3(0.0f);
+    float targetSpeed = 0.0f;
+    float inverseDenom = 0.0f;
+    bool oneSided = false;          // a limit only pushes back, the rest also hold the bodies together
+};
+
+// ============================================================================
+// Ball joint with a swing cone and a twist limit, or a one axis hinge
+// Anchors and axes are local to each body: rotated, but not scaled.
+// The twist limit assumes both bodies started out unrotated.
+// ============================================================================
+struct BallJoint {
+    p* parent = nullptr;
+    p* child = nullptr;
+    glm::vec3 anchorParent = glm::vec3(0.0f);
+    glm::vec3 anchorChild = glm::vec3(0.0f);
+    glm::vec3 axisParent = glm::vec3(0.0f, 1.0f, 0.0f);
+    glm::vec3 axisChild = glm::vec3(0.0f, 1.0f, 0.0f);
+    float maxAngle = 0.0f;           // radians between the two axes
+    float maxTwist = 0.0f;           // radians either way around axisChild
+
+    // Hinges ignore the limits above and bend around one axis, like an elbow.
+    // The bend is the angle from hingeRefParent to hingeRefChild around the hinge axis.
+    bool isHinge = false;
+    glm::vec3 hingeAxisParent = glm::vec3(1.0f, 0.0f, 0.0f);
+    glm::vec3 hingeAxisChild = glm::vec3(1.0f, 0.0f, 0.0f);
+    glm::vec3 hingeRefParent = glm::vec3(0.0f, 1.0f, 0.0f);
+    glm::vec3 hingeRefChild = glm::vec3(0.0f, 1.0f, 0.0f);
+    float minBend = 0.0f;
+    float maxBend = 0.0f;
+
+    // Filled in by PrepareJoint, these can't change during a step's iterations
+    glm::vec3 worldAnchorParent = glm::vec3(0.0f);
+    glm::vec3 worldAnchorChild = glm::vec3(0.0f);
+    glm::vec3 bias = glm::vec3(0.0f);
+    glm::mat3 inverseK = glm::mat3(1.0f);
+    AngularRow rows[3];
+    int rowCount = 0;
+};
+
+struct BroadPhaseEntry {
+    p* body = nullptr;
+    BoundingBox bounds;
+};
+
 // ============================================================================
 // Physics world
 // ============================================================================
 class Physics {
 private:
     std::vector<p*> objects;
+    std::vector<BallJoint*> joints;
     glm::vec3 gravity = glm::vec3(0.0f, -9.81f, 0.0f);
 
+    // Kept between steps so they don't get reallocated
+    std::vector<BroadPhaseEntry> broadPhase;
+    std::vector<Collision> collisions;
+    std::vector<Collision> meshContacts;
+    std::vector<int> meshCandidates;
+
     static glm::mat3 ComputeInvInertiaTensor(const p* body);
     static bool      UpdateSleep(p* body, float dt);
-    void             Resolve(Collision* c);
+    void             Resolve(Collision* c, bool firstPass = true);
+    void             FindCollisions();
+    void             CollideTriangles(p* a, p* b);
+    void             WakeGroup(int group);
+    void             PrepareJoint(BallJoint& joint, float dt);
+    void             SolveJoint(BallJoint& joint);
+    void             DampJoint(BallJoint& joint, float dt);
 
 public:
     Physics() {}
@@ -111,6 +192,7 @@ public:
 
     int  GetObjectAmount() const noexcept { return (int)objects.size(); }
     void AddObject(p* obj) { objects.push_back(obj); }
+    void AddJoint(BallJoint* joint) { joints.push_back(joint); }
     const std::vector<p*>& GetObjects() const { return objects; }
 
     p* PPointerOfObject(t_package* pointer) const {
@@ -135,37 +217,7 @@ public:
 
     void Step(float dt);
 
-    void SolveCollisions(std::vector<Collision>& collisions) {
-        for (Collision& col : collisions)
-            Resolve(&col);
-    }
-
-    void ResolveCollisions(float dt) {
-        std::vector<Collision> collisions;
-
-        for (p* a : objects) {
-            if (!a->collision) continue;
-            for (p* b : objects) {
-                if (b <= a || !b->collision)              continue;
-                if (!a->velocity && !b->velocity)         continue;
-                if (a->isSleeping && b->isSleeping)       continue;
-
-                if (TNearT(a->pointer->t, b->pointer->t) &&
-                    BoundingBoxInBoundingBox(a->pointer->t.GetAABB(), b->pointer->t.GetAABB()))
-                {
-                    std::optional<TInTInfo> e = TInT(a->pointer->t, b->pointer->t);
-                    if (e.has_value()) {
-                        Collision tp(a, b);
-                        tp.POI = e.value().POI;
-                        tp.CN = e.value().CN;
-                        tp.overlap = e.value().overlap;
-                        collisions.push_back(tp);
-                    }
-                }
-            }
-        }
-        SolveCollisions(collisions);
-    }
+    void ResolveCollisions(float dt);
 };
 
 #endif