combattest/Physics.h

#ifndef PHYSICS_CLASS
#define PHYSICS_CLASS
#include <iostream>
#include <random>
#include <vector>
#include <memory>
#include <cmath>
#include <optional>
#include <algorithm>
#include <glad/glad.h>
#include <GLFW/glfw3.h>
#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
// ============================================================================
static constexpr float PHYSICS_LINEAR_DAMPING = 0.995f;
static constexpr float PHYSICS_ANGULAR_DAMPING = 0.990f;
static constexpr float PHYSICS_BAUMGARTE_FACTOR = 0.1f;
static constexpr float PHYSICS_BAUMGARTE_SLOP = 0.02f;
static constexpr float PHYSICS_SLEEP_LINEAR_SQ = 0.05f * 0.05f;     // m/s
static constexpr float PHYSICS_SLEEP_ANGULAR_SQ = 0.08f * 0.08f;    // rad/s
static constexpr float PHYSICS_SLEEP_TIME_THRESHOLD = 0.5f;
static constexpr float PHYSICS_RESTITUTION_THRESHOLD = 1.0f;
static constexpr int   PHYSICS_CONTACT_ITERATIONS = 8;   // per step with no joints around
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
static constexpr float PHYSICS_MIN_IMPACT_IMPULSE = 1.0f;   // newton seconds, softer contacts like resting aren't recorded

// ============================================================================
// Physics body
// ============================================================================
class p {
    friend class Physics;
public:
    p() {
        pointer = nullptr;
        std::cout << "PHYSICS OBJECT INITIALIZED WITHOUT POINTER IN CONSTRUCTOR!!!\n";
    }
    p(t_package* t) : pointer(t) {}

    void SetLinearVelocity(glm::vec3 nf) { linearvelocity = nf; }
    void SetMass(float newmass) { mass = newmass; }

    void EnableCollision() { collision = true; }
    void DisableCollision() { collision = false; }
    void EnableVelocity() { velocity = true; }
    void DisableVelocity() { velocity = false; }

    void AddForce(const glm::vec3& f) { if (velocity) { force += f; WakeUp(); } }
    void AddTorque(const glm::vec3& t) { if (velocity) { torque += t; WakeUp(); } }

    float GetMass()    const noexcept { return mass; }
    float GetInvMass() const noexcept { return velocity ? 1.0f / mass : 0.0f; }

    void WakeUp() { isSleeping = false; sleepTimer = 0.0f; }

public:
    t_package* pointer = nullptr;

    bool collision = true;
    bool velocity = true;
    bool isSleeping = false;
    bool lockRotation = false;
    bool frozen = false;                          // parked until something touches its group
    bool isHitbox = false;                        // only collides with loose objects, never with other characters

    // 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;

    // For drawing it smoothly between physics ticks, see Physics::ApplyInterpolation
    glm::vec3 previousPosition = glm::vec3(0.0f);
    glm::quat previousOrientation = glm::quat(1.0f, 0.0f, 0.0f, 0.0f);
    glm::vec3 simulatedPosition = glm::vec3(0.0f);
    glm::quat simulatedOrientation = glm::quat(1.0f, 0.0f, 0.0f, 0.0f);
    bool hasPrevious = false;        // false until its first tick, so new bodies don't slide in from nowhere
    bool interpolated = false;       // t currently holds the blended pose, not the real one

    // World bounds, kept until t changes. Most bodies sit still most of the time.
    BoundingBox bounds;
    unsigned int boundsVersion = 0;
};

// ============================================================================
// Collision manifold
// ============================================================================
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;

    // Filled in by PrepareContact, these can't change during a step's iterations
    glm::vec3 rA = glm::vec3(0.0f);
    glm::vec3 rB = glm::vec3(0.0f);
    glm::vec3 tangent1 = glm::vec3(0.0f);
    glm::vec3 tangent2 = glm::vec3(0.0f);
    float inverseDenomN = 0.0f;
    float inverseDenomT1 = 0.0f;
    float inverseDenomT2 = 0.0f;
    float targetSpeed = 0.0f;        // how fast it should be separating afterwards, the bounce
    float mu = 0.0f;

    // Running totals over the iterations
    float normalImpulse = 0.0f;
    glm::vec2 tangentImpulse = glm::vec2(0.0f);

    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;
};

// One contact pushing two bodies apart during a step. impulse is in newton seconds.
struct Impact {
    p* a = nullptr;
    p* b = nullptr;
    glm::vec3 point = glm::vec3(0.0f);
    float impulse = 0.0f;
};

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. The broad phase also keeps its order,
    // and is only rebuilt when bodies come or go.
    std::vector<BroadPhaseEntry> broadPhase;
    unsigned int objectsVersion = 1;
    unsigned int broadPhaseVersion = 0;
    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             PrepareContact(Collision& c);
    void             SolveContact(Collision& c);
    void             CorrectContactPosition(Collision& c);
    void             FindCollisions();
    void             CollideTriangles(p* a, p* b);
    void             CollideBoxes(p* a, p* b, const TInTInfo& info);
    void             WakeGroup(int group);
    void             WakeAround(p* removed);
    void             PrepareJoint(BallJoint& joint, float dt);
    void             SolveJoint(BallJoint& joint);
    void             DampJoint(BallJoint& joint, float dt);

public:
    Physics() {}

    const glm::vec3& GetGravity() const noexcept { return gravity; }
    void SetGravity(const glm::vec3& g) noexcept { gravity = g; }

    int  GetObjectAmount() const noexcept { return (int)objects.size(); }
    void AddObject(p* obj) { objects.push_back(obj); objectsVersion++; }
    void AddJoint(BallJoint* joint) { joints.push_back(joint); }
    void RemoveJoint(BallJoint* joint) {
        auto it = std::find(joints.begin(), joints.end(), joint);
        if (it != joints.end()) joints.erase(it);
    }
    const std::vector<p*>& GetObjects() const { return objects; }

    p* PPointerOfObject(t_package* pointer) const {
        for (p* obj : objects)
            if (obj->pointer == pointer) return obj;
        return nullptr;
    }

    void RemoveP(p* obj) {
        objects.erase(std::find(objects.begin(), objects.end(), obj));
        objectsVersion++;
        WakeAround(obj);
    }

    void RemoveObject(t_package* pointer) {
        for (int i = 0; i < (int)objects.size(); i++) {
            if (objects[i]->pointer == pointer) {
                p* removed = objects[i];
                objects.erase(objects.begin() + i);
                objectsVersion++;
                WakeAround(removed);
                delete removed;
                return;
            }
        }
    }

    void Step(float dt);

    // One fixed physics tick, split into substeps so fast things don't pass through each other.
    // Remembers where everything was beforehand, for ApplyInterpolation.
    void Tick(float dt, int substeps);

    // Moves every moving body's t partway from where it was before the last tick (0) to where it is now (1),
    // so things drawn between ticks don't step along at the tick rate. Nothing may move bodies until
    // RemoveInterpolation puts the real positions back.
    void ApplyInterpolation(float alpha);
    void RemoveInterpolation();

    void ResolveCollisions(float dt);

    // Every contact since the last ClearImpacts, for working out what hit what and how hard
    const std::vector<Impact>& GetImpacts() const { return impacts; }
    void ClearImpacts() { impacts.clear(); }

private:
    std::vector<Impact> impacts;
};

#endif