PhysicsSystem.hpp

Go to the documentation of this file.

 
#pragma once
 
#ifndef GLM_ENABLE_EXPERIMENTAL
    #define GLM_ENABLE_EXPERIMENTAL 1
#endif
#include "components/ECS/ECS.hpp"
#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtx/euler_angles.hpp>
#include <glm/gtx/quaternion.hpp>
#include <mutex>
#include <optional>
#include <vector>
#include <unordered_map>
#include <functional>
#include <memory>
 
#include <components/GameComponents/BasicComponents.hpp>
#include <components/GameComponents/CharacterComponent.hpp>
 
#include "components/JoltSafe.hpp"
 
namespace vex {

    struct BodyIDHasher {
        std::size_t operator()(const JPH::BodyID& id) const {
            return std::hash<uint32_t>{}(id.GetIndexAndSequenceNumber());
        }
    };

    class BPLayerInterfaceImpl final : public JPH::BroadPhaseLayerInterface {
    public:
        // @brief Returns the number of broad phase layers.
        unsigned int GetNumBroadPhaseLayers() const override { return 1; }
        // @brief Returns the broad phase layer for a given object layer.
        JPH::BroadPhaseLayer GetBroadPhaseLayer(JPH::ObjectLayer inLayer) const override { return JPH::BroadPhaseLayer(0); }
#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
        // @brief Returns the name of a broad phase layer for profiling purposes.
        const char* GetBroadPhaseLayerName(JPH::BroadPhaseLayer inLayer) const override { return "Default"; }
#endif
    };

    class ObjectVsBroadPhaseLayerFilterImpl final : public JPH::ObjectVsBroadPhaseLayerFilter {
    public:
        // @brief Determines if an object layer should collide with a broad phase layer.
        bool ShouldCollide(JPH::ObjectLayer, JPH::BroadPhaseLayer) const override { return true; }
    };

    class ObjectLayerPairFilterImpl final : public JPH::ObjectLayerPairFilter {
    public:
        // @brief Determines if two object layers should collide.
        bool ShouldCollide(JPH::ObjectLayer, JPH::ObjectLayer) const override { return true; }
    };

    class MyActivationListener : public JPH::BodyActivationListener {
    public:
        // @brief Called when a body is activated.
        void OnBodyActivated(const JPH::BodyID& inBodyID, JPH::uint64 inBodyUserData) override {}
        // @brief Called **change** Called when a body is deactivated.
        void OnBodyDeactivated(const JPH::BodyID& inBodyID, JPH::uint64 inBodyUserData) override {}
    };

    enum class ShapeType {
        BOX, ROUNDED_BOX, SPHERE, CAPSULE, CYLINDER, CONVEX_HULL, MESH
    };

    enum class BodyType {
        STATIC, DYNAMIC, KINEMATIC, SENSOR
    };

    struct CollisionHit {
        glm::vec3 position;
        glm::vec3 normal;
        float impulse;
    };

    struct RaycastHit {
        JPH::BodyID bodyId;
        float distance;
        glm::vec3 position;
        glm::vec3 normal;
    };

    struct PhysicsComponent {
        ShapeType shape = ShapeType::BOX;
        BodyType bodyType = BodyType::STATIC;
        float mass = 1.0f;
        float friction = 0.5f;
        float bounce = 0.1f;
        uint8_t objectLayer = 0;
        JPH::BodyID bodyId = JPH::BodyID(JPH::BodyID::cInvalidBodyID);
        float linearDamping = 0.05f;
        float angularDamping = 0.05f;
        bool isSensor = false;
        bool allowSleeping = true;
        bool debugDraw = true;
 
        glm::vec3 boxHalfExtents = {0.5f, 0.5f, 0.5f};
        float roundedRadius = 0.05f;
        float sphereRadius = 0.5f;
        float capsuleRadius = 0.5f;
        float capsuleHeight = 1.0f;
        float cylinderRadius = 0.5f;
        float cylinderHeight = 1.0f;
        std::vector<JPH::Vec3> convexPoints;
        std::vector<glm::vec3> meshVertices;
        std::vector<uint32_t> meshIndices;
 
        // Collision callbacks
        std::function<void(vex::Entity self, vex::Entity other, const CollisionHit& hit)> onCollisionEnter;
        std::function<void(vex::Entity self, vex::Entity other, const CollisionHit& hit)> onCollisionStay;
        std::function<void(vex::Entity self, vex::Entity other)> onCollisionExit;
 
        #if DEBUG
        bool updated = false;
        #endif

        PhysicsComponent() = default;
 
        explicit PhysicsComponent(ShapeType s) : shape(s) {}

        static PhysicsComponent Box(glm::vec3 halfExtents = {0.5f, 0.5f, 0.5f}, BodyType bodyType = BodyType::STATIC, float mass = 1.0f, float friction = 0.5f, float bounce = 0.1f) {
            PhysicsComponent pc;
            pc.shape = ShapeType::BOX;
            pc.boxHalfExtents = halfExtents;
            pc.mass = mass;
            pc.bodyType = bodyType;
            pc.friction = friction;
            pc.bounce = bounce;
            return pc;
        }

            static PhysicsComponent RoundedBox(glm::vec3 halfExtents, float radius = 0.05f, BodyType bodyType = BodyType::DYNAMIC, float mass = 1.0f, float friction = 0.5f, float bounce = 0.1f) {
                PhysicsComponent pc;
                pc.shape = ShapeType::ROUNDED_BOX;
                pc.boxHalfExtents = halfExtents;
                pc.roundedRadius = radius;
                pc.mass = mass;
                pc.bodyType = bodyType;
                pc.friction = friction;
                pc.bounce = bounce;
                return pc;
            }

        static PhysicsComponent Sphere(float radius = 0.5f, BodyType bodyType = BodyType::STATIC, float mass = 1.0f, float friction = 0.5f, float bounce = 0.1f) {
            PhysicsComponent pc;
            pc.shape = ShapeType::SPHERE;
            pc.sphereRadius = radius;
            pc.mass = mass;
            pc.bodyType = bodyType;
            pc.friction = friction;
            pc.bounce = bounce;
            return pc;
        }

        static PhysicsComponent Capsule(float radius = 0.5f, float height = 1.0f, BodyType bodyType = BodyType::STATIC, float mass = 1.0f, float friction = 0.5f, float bounce = 0.1f) {
            PhysicsComponent pc;
            pc.shape = ShapeType::CAPSULE;
            pc.capsuleRadius = radius;
            pc.capsuleHeight = height;
            pc.mass = mass;
            pc.bodyType = bodyType;
            pc.friction = friction;
            pc.bounce = bounce;
            return pc;
        }

        static PhysicsComponent Cylinder(float radius = 0.5f, float height = 1.0f, BodyType bodyType = BodyType::STATIC, float mass = 1.0f, float friction = 0.5f, float bounce = 0.1f) {
            PhysicsComponent pc;
            pc.shape = ShapeType::CYLINDER;
            pc.cylinderRadius = radius;
            pc.cylinderHeight = height;
            pc.mass = mass;
            pc.bodyType = bodyType;
            pc.friction = friction;
            pc.bounce = bounce;
            return pc;
        }

        static PhysicsComponent ConvexHull(std::vector<JPH::Vec3> points, BodyType bodyType = BodyType::STATIC, float mass = 1.0f, float friction = 0.5f, float bounce = 0.1f) {
            PhysicsComponent pc;
            pc.shape = ShapeType::CONVEX_HULL;
            pc.convexPoints = points;
            pc.mass = mass;
            pc.bodyType = bodyType;
            pc.friction = friction;
            pc.bounce = bounce;
            return pc;
        }

        static PhysicsComponent Mesh(MeshComponent& mesh, BodyType bodyType = BodyType::STATIC, float mass = 1.0f, float friction = 0.5f, float bounce = 0.1f) {
            PhysicsComponent pc;
            pc.shape = ShapeType::MESH;
            pc.bodyType = bodyType;
            pc.mass = mass;
            pc.friction = friction;
            pc.bounce = bounce;
            if (!mesh.meshData.submeshes.empty()) {
                pc.meshVertices.clear();
                pc.meshIndices.clear();
 
                size_t vertexOffset = 0;
 
                for (const auto& sm : mesh.meshData.submeshes) {
                    for (const auto& v : sm.vertices) {
                        pc.meshVertices.push_back(v.position);
                    }
                    for (auto idx : sm.indices) {
                        pc.meshIndices.push_back(static_cast<uint32_t>(idx + vertexOffset));
                    }
                    vertexOffset += sm.vertices.size();
                }
            }
            return pc;
        }

        void addCollisionEnterBinding(std::function<void(vex::Entity, vex::Entity, const CollisionHit&)> callback) {
            onCollisionEnter = callback;
        }

        void addCollisionStayBinding(std::function<void(vex::Entity, vex::Entity, const CollisionHit&)> callback) {
            onCollisionStay = callback;
        }

        void addCollisionExitBinding(std::function<void(vex::Entity, vex::Entity)> callback) {
            onCollisionExit = callback;
        }
    };

    class PhysicsSystem {
    public:
        PhysicsSystem(vex::Registry& registry) : m_registry(registry) {}

        ~PhysicsSystem();

        void setDebugRenderer(JPH::DebugRenderer* renderer);

        void drawDebug(bool drawConstraints = true, bool drawWireframe = true);

        bool init(size_t maxBodies = 1024);

        void shutdown();

        void update(float deltaTime);

        void SyncBodies();

        void SetGravityVector(const glm::vec3& gravity) {
            m_physicsSystem->SetGravity(JPH::Vec3(gravity.x, gravity.y, gravity.z));
        }

        void SetGravityFactor(JPH::BodyID bodyId, float gravityFactor) {
            m_physicsSystem->GetBodyInterface().SetGravityFactor(bodyId, gravityFactor);
        }

        void SetFriction(JPH::BodyID bodyId, float friction);

        float GetFriction(JPH::BodyID bodyId);

        void SetBounciness(JPH::BodyID bodyId, float bounciness);

        float GetBounciness(JPH::BodyID bodyId);

        void SetLinearVelocity(JPH::BodyID bodyId, const glm::vec3& velocity);

        glm::vec3 GetLinearVelocity(JPH::BodyID bodyId);

        glm::vec3 GetVelocityAtPosition(JPH::BodyID bodyId, const glm::vec3& point);

        void AddLinearVelocity(JPH::BodyID bodyId, const glm::vec3& velocity);

        void SetAngularVelocity(JPH::BodyID bodyId, const glm::vec3& velocity);

        glm::vec3 GetAngularVelocity(JPH::BodyID bodyId);

        void AddAngularVelocity(JPH::BodyID bodyId, const glm::vec3& velocity);

        void AddTorque(JPH::BodyID bodyId, const glm::vec3& torque);

        void AddForce(JPH::BodyID bodyId, const glm::vec3& force);

        void AddForceAtPosition(JPH::BodyID bodyId, const glm::vec3& force, const glm::vec3& position);

        void AddImpulse(JPH::BodyID bodyId, const glm::vec3& impulse);

        void AddImpulseAtPosition(JPH::BodyID bodyId, const glm::vec3& impulse, const glm::vec3& position);

        void AddAngularImpulse(JPH::BodyID bodyId, const glm::vec3& impulse);

        void SetBodyActive(JPH::BodyID bodyId, bool active);

        bool GetBodyActive(JPH::BodyID bodyId);

        std::optional<JPH::BodyID> CreateBodyForEntity(vex::Entity e, vex::Registry& r, PhysicsComponent& pc);

        std::optional<JPH::BodyID> RecreateBodyForEntity(vex::Entity e, PhysicsComponent& pc);

        void DestroyBodyForEntity(PhysicsComponent& pc);

        bool raycast(const glm::vec3& origin, const glm::vec3& direction, float maxDistance, RaycastHit& hit);

        int GetCollisionSteps() { return collisionSteps; }

        void setCollisionSteps(int steps) { collisionSteps = steps; }

        void setEnableSmoothing(bool enable) { enableSmoothing = enable; }

        glm::vec3 GetPhysicsPosition(JPH::BodyID bodyId);

        glm::quat GetPhysicsRotation(JPH::BodyID bodyId);
 
        // @brief Retrieves the physics component associated with a body ID.
        // @param JPH::BodyID id - the body ID to retrieve the physics component for
        // @return PhysicsComponent& - the physics component associated with the body ID
        PhysicsComponent& getPhysicsComponentByBodyId(JPH::BodyID id);
 
        // @brief Retrieves the entity associated with a body ID.
        // @param JPH::BodyID id - the body ID to retrieve the entity for
        // @return vex::Entity - the entity associated with the body ID
        vex::Entity getEntityByBodyId(JPH::BodyID id);
 
    private:
        friend class MyContactListener;
 
        int collisionSteps = 3;
        bool enableSmoothing = true;
 
        JPH::TempAllocatorImpl* m_tempAllocator = nullptr;
        JPH::JobSystem* m_jobSystem = nullptr;
        JPH::PhysicsSystem* m_physicsSystem = nullptr;
        JPH::DebugRenderer* m_debugRenderer = nullptr;
 
        vex::Registry& m_registry;
 
        BPLayerInterfaceImpl m_bpInterface;
        ObjectVsBroadPhaseLayerFilterImpl m_objVsBpFilter;
        ObjectLayerPairFilterImpl m_objLayerPairFilter;
        std::unique_ptr<MyActivationListener> m_activationListener;
        std::unique_ptr<JPH::ContactListener> m_contactListener;
 
        std::unordered_map<JPH::BodyID, vex::Entity, BodyIDHasher> m_bodyToEntity;
 
        struct InterpCache {
                glm::vec3 prevPos = {0.0f, 0.0f, 0.0f};
                glm::quat prevRot = {1.0f, 0.0f, 0.0f, 0.0f};
                glm::vec3 currPos = {0.0f, 0.0f, 0.0f};
                glm::quat currRot = {1.0f, 0.0f, 0.0f, 0.0f};
                glm::vec3 lastVisualPos = {0.0f, 0.0f, 0.0f};
                glm::quat lastVisualRot = {1.0f, 0.0f, 0.0f, 0.0f};
                bool desynced = false;
        };
        std::unordered_map<JPH::BodyID, InterpCache, BodyIDHasher> m_interpCache;
 
        float m_fixedDt = 1.0f / 60.0f;
        float m_accumulator = 0.0f;
 
        // @brief Converts Euler angles to a quaternion.
        // @param glm::vec3 eulerDeg - the Euler angles in degrees
        // @return JPH::Quat - the resulting quaternion
        static JPH::Quat EulerToQuat(const glm::vec3& eulerDeg);
        // @brief Converts a quaternion to Euler angles.
        // @param JPH::Quat q - the quaternion to convert
        // @return glm::vec3 - the resulting Euler angles in degrees
        static glm::vec3 QuatToEuler(const JPH::Quat& q);
        // @brief Synchronizes a body's transform with its entity's transform.
        // @param vex::Entity e - the entity to synchronize
        // @param vex::Registry& r - the registry containing the entity
        // @param const JPH::BodyID& id - the ID of the body to synchronize
        // @param float alpha - the interpolation factor (0.0f = no interpolation, 1.0f = full interpolation)
        void SyncBodyToTransform(vex::Entity e, vex::Registry& r, const JPH::BodyID& id, float alpha = 1.0f);
 
        // @brief Handles physics component destruction.
        // @param vex::Registry& reg - the registry containing the entity
        // @param vex::Entity e - the entity to destroy
        void onPhysicsComponentDestroy(vex::Registry& reg, vex::Entity e);
 
        // @brief Initializes a character component.
        // @param vex::Entity e - the entity to initialize
        // @param CharacterComponent& cc - the character component to initialize
        void InitializeCharacter(vex::Entity e, CharacterComponent& cc);
 
        #include <map>

        void WeldVertices(const std::vector<glm::vec3>& inVerts, const std::vector<uint32_t>& inIndices,
                          JPH::VertexList& outVerts, JPH::IndexedTriangleList& outTris);
    };
 
    class MyContactListener : public JPH::ContactListener {
    public:
        MyContactListener(PhysicsSystem& system) : m_system(system) {}
 
        // @brief Validates if a contact should be processed.
        JPH::ValidateResult OnContactValidate(const JPH::Body& inBody1, const JPH::Body& inBody2, JPH::RVec3Arg inBaseOffset, const JPH::CollideShapeResult& inCollisionResult) override {
            return JPH::ValidateResult::AcceptAllContactsForThisBodyPair;
        }
 
        // @brief Called when a new contact is added.
        // @param const JPH::Body& inBody1 - the first body involved in the contact
        // @param const JPH::Body& inBody2 - the second body involved in the contact
        // @param const JPH::ContactManifold& inManifold - the contact manifold
        // @param JPH::ContactSettings& ioSettings - the contact settings
        void OnContactAdded(const JPH::Body& inBody1, const JPH::Body& inBody2, const JPH::ContactManifold& inManifold, JPH::ContactSettings& ioSettings) override;
        // @brief Called when a contact persists.
        // @param const JPH::Body& inBody1 - the first body involved in the contact
        // @param const JPH::Body& inBody2 - the second body involved in the contact
        // @param const JPH::ContactManifold& inManifold - the contact manifold
        // @param JPH::ContactSettings& ioSettings - the contact settings
        void OnContactPersisted(const JPH::Body& inBody1, const JPH::Body& inBody2, const JPH::ContactManifold& inManifold, JPH::ContactSettings& ioSettings) override;
        // @brief Called when a contact is removed.
        // @param const JPH::SubShapeIDPair& inSubShapePair - the sub shape pair involved in the contact
        void OnContactRemoved(const JPH::SubShapeIDPair& inSubShapePair) override;
 
    private:
        PhysicsSystem& m_system;
    };

        class DebugDrawFilter : public JPH::BodyDrawFilter {
        public:
            DebugDrawFilter(PhysicsSystem& system) : m_system(system) {}
 
            virtual bool ShouldDraw(const JPH::Body& body) const override {
                auto& pc = m_system.getPhysicsComponentByBodyId(body.GetID());
                return pc.debugDraw;
            }
 
        private:
            PhysicsSystem& m_system;
        };
}