PhysicsBodyComponent.cpp

Go to the documentation of this file.

#include "Engine/Component/PhysicsBodyComponent.h"
#include "Engine/Component/Collision/CollisionComponent.h"
#include "Engine/Core/Engine.h"
#include "Engine/Ecs/EcsData.h"
#include "Engine/Ecs/RegistryManager.h"
#include "Engine/Entity/Entity.h"
#include "Engine/Math/Converter.h"
#include "Engine/Physics/PhysicsEngine.h"
#include "Engine/World/SceneGraph.h"
 
#include <btBulletDynamicsCommon.h>
#include <glm/gtc/type_ptr.hpp>
#include <plog/Log.h>
#include <algorithm>
 
namespace EngineCore {
 
    PhysicsBodyComponent::PhysicsBodyComponent(Scene* scene, entt::entity entity,
                                               std::shared_ptr<SceneNode> sceneNode)
        : LogicComponent(scene)
        , entity_(entity)
        , sceneNode_(sceneNode)
    {
    }
 
    PhysicsBodyComponent::~PhysicsBodyComponent() {
        // Cleanup happens in endPlay(), but just in case
        if (rigidBody_) {
            destroyRigidBody();
        }
        if (compoundShape_) {
            delete compoundShape_;
            compoundShape_ = nullptr;
        }
    }
 
    void PhysicsBodyComponent::beginPlay() {
        LogicComponent::beginPlay();
 
        // Scan for existing CollisionComponents on the same entity
        Entity* owner = getOwningEntity();
        if (owner) {
            auto existingCollisions = owner->getComponents<CollisionComponent>();
            for (CollisionComponent* collision : existingCollisions) {
                if (collision->getBodyComponent() == nullptr) {
                    registerCollision(collision);
                }
            }
        }
 
        isInitialized_ = true;
 
        // Build initial shape if we have any collisions registered
        if (!collisions_.empty()) {
            rebuildCollisionShape();
            createRigidBody();
        }
    }
 
    void PhysicsBodyComponent::endPlay() {
        // Clean up collision registrations
        for (CollisionComponent* collision : collisions_) {
            collision->bodyComponent_ = nullptr;
        }
        collisions_.clear();
 
        // Clear ECS component pointers BEFORE removing to prevent double-free
        // (PhysicsEngine::onPhysicsComponentDestroyed will try to delete them)
        auto& registry = Ecs::RegistryManager::get();
 
        // Use the SceneNode's entity (same as updateEcsComponent)
        auto sceneNode = sceneNode_.lock();
        if (sceneNode) {
            entt::entity nodeEntity = sceneNode->getEntity();
            if (registry.valid(nodeEntity) && registry.all_of<Ecs::SimulatesPhysics>(nodeEntity)) {
                auto& physicsComp = registry.get<Ecs::SimulatesPhysics>(nodeEntity);
                physicsComp.rigidBody = nullptr;
                physicsComp.collisionShape = nullptr;
                registry.remove<Ecs::SimulatesPhysics>(nodeEntity);
            }
        }
 
        destroyRigidBody();
 
        if (compoundShape_) {
            delete compoundShape_;
            compoundShape_ = nullptr;
        }
 
        isInitialized_ = false;
 
        LogicComponent::endPlay();
    }
 
    // =========================================================================
    // Physics Properties
    // =========================================================================
 
    void PhysicsBodyComponent::setMass(float mass) {
        mass_ = mass;
        if (rigidBody_ && !isStatic_) {
            btVector3 localInertia(0, 0, 0);
            if (mass_ > 0.0f && compoundShape_) {
                compoundShape_->calculateLocalInertia(mass_, localInertia);
            }
            rigidBody_->setMassProps(mass_, localInertia);
            rigidBody_->updateInertiaTensor();
        }
    }
 
    void PhysicsBodyComponent::setFriction(float friction) {
        friction_ = friction;
        if (rigidBody_) {
            rigidBody_->setFriction(friction_);
        }
    }
 
    void PhysicsBodyComponent::setRestitution(float restitution) {
        restitution_ = restitution;
        if (rigidBody_) {
            rigidBody_->setRestitution(restitution_);
        }
    }
 
    void PhysicsBodyComponent::setKinematic(bool kinematic) {
        isKinematic_ = kinematic;
        updateEcsComponent();
 
        if (rigidBody_) {
            int flags = rigidBody_->getCollisionFlags();
            if (kinematic) {
                flags |= btCollisionObject::CF_KINEMATIC_OBJECT;
                rigidBody_->setActivationState(DISABLE_DEACTIVATION);
            } else {
                flags &= ~btCollisionObject::CF_KINEMATIC_OBJECT;
                rigidBody_->setActivationState(ACTIVE_TAG);
            }
            rigidBody_->setCollisionFlags(flags);
        }
    }
 
    void PhysicsBodyComponent::setStatic(bool isStatic) {
        isStatic_ = isStatic;
        updateEcsComponent();
 
        // Static objects need mass = 0 and rebuild
        if (isStatic && rigidBody_) {
            btVector3 localInertia(0, 0, 0);
            rigidBody_->setMassProps(0.0f, localInertia);
            int flags = rigidBody_->getCollisionFlags();
            flags |= btCollisionObject::CF_STATIC_OBJECT;
            rigidBody_->setCollisionFlags(flags);
        }
    }
 
    // =========================================================================
    // Transform Control
    // =========================================================================
 
    void PhysicsBodyComponent::teleport(const glm::vec3& position) {
        auto sceneNode = sceneNode_.lock();
        if (!sceneNode) return;
 
        // Extract current rotation from world matrix
        glm::mat4 worldMat = sceneNode->getWorldMatrix();
        glm::quat currentRotation = glm::quat_cast(glm::mat3(worldMat));
        teleport(position, currentRotation);
    }
 
    void PhysicsBodyComponent::teleport(const glm::vec3& position, const glm::quat& rotation) {
        auto sceneNode = sceneNode_.lock();
        if (!sceneNode) return;
 
        // Update scene node
        sceneNode->setWorldPosition(position);
        sceneNode->setWorldRotation(rotation);
 
        // Update Bullet transform
        if (rigidBody_) {
            btTransform newTransform;
            newTransform.setIdentity();
            newTransform.setOrigin(bt::getVec3(position));
            newTransform.setRotation(bt::getQuat(rotation));
 
            rigidBody_->setWorldTransform(newTransform);
            rigidBody_->getMotionState()->setWorldTransform(newTransform);
 
            // Clear velocities on teleport
            rigidBody_->setLinearVelocity(btVector3(0, 0, 0));
            rigidBody_->setAngularVelocity(btVector3(0, 0, 0));
            rigidBody_->clearForces();
 
            // Wake up the body
            rigidBody_->activate(true);
        }
 
        // Mark transform dirty in ECS (use SceneNode's entity)
        auto& registry = Ecs::RegistryManager::get();
        registry.emplace_or_replace<Ecs::TransformDirty>(sceneNode->getEntity());
        registry.emplace_or_replace<Ecs::TransformDirtyRenderer>(sceneNode->getEntity());
    }
 
    void PhysicsBodyComponent::setLinearVelocity(const glm::vec3& velocity) {
        if (rigidBody_) {
            rigidBody_->setLinearVelocity(bt::getVec3(velocity));
            rigidBody_->activate(true);
        }
    }
 
    glm::vec3 PhysicsBodyComponent::getLinearVelocity() const {
        if (rigidBody_) {
            return glm::getVec3(rigidBody_->getLinearVelocity());
        }
        return glm::vec3(0.0f);
    }
 
    void PhysicsBodyComponent::setAngularVelocity(const glm::vec3& velocity) {
        if (rigidBody_) {
            rigidBody_->setAngularVelocity(bt::getVec3(velocity));
            rigidBody_->activate(true);
        }
    }
 
    glm::vec3 PhysicsBodyComponent::getAngularVelocity() const {
        if (rigidBody_) {
            return glm::getVec3(rigidBody_->getAngularVelocity());
        }
        return glm::vec3(0.0f);
    }
 
    // =========================================================================
    // Collision Management
    // =========================================================================
 
    void PhysicsBodyComponent::registerCollision(CollisionComponent* collision) {
        if (!collision) return;
 
        // Check if already registered
        auto it = std::find(collisions_.begin(), collisions_.end(), collision);
        if (it != collisions_.end()) return;
 
        collisions_.push_back(collision);
        collision->bodyComponent_ = this;
        shapeDirty_ = true;
 
        // If already initialized, rebuild immediately
        if (isInitialized_) {
            rebuildCollisionShape();
            if (!rigidBody_) {
                createRigidBody();
            } else {
                // Update existing rigid body with new shape
                PhysicsEngine* physics = getPhysicsEngine();
                if (physics) {
                    btDiscreteDynamicsWorld* world = physics->getDynamicsWorld();
                    world->removeRigidBody(rigidBody_);
                    rigidBody_->setCollisionShape(compoundShape_);
                    world->addRigidBody(rigidBody_);
                }
            }
        }
    }
 
    void PhysicsBodyComponent::unregisterCollision(CollisionComponent* collision) {
        if (!collision) return;
 
        auto it = std::find(collisions_.begin(), collisions_.end(), collision);
        if (it == collisions_.end()) return;
 
        collisions_.erase(it);
        collision->bodyComponent_ = nullptr;
        shapeDirty_ = true;
 
        // Rebuild if initialized
        if (isInitialized_) {
            if (collisions_.empty()) {
                destroyRigidBody();
            } else {
                rebuildCollisionShape();
                if (rigidBody_) {
                    PhysicsEngine* physics = getPhysicsEngine();
                    if (physics) {
                        btDiscreteDynamicsWorld* world = physics->getDynamicsWorld();
                        world->removeRigidBody(rigidBody_);
                        rigidBody_->setCollisionShape(compoundShape_);
                        world->addRigidBody(rigidBody_);
                    }
                }
            }
        }
    }
 
    void PhysicsBodyComponent::markShapeDirty() {
        shapeDirty_ = true;
    }
 
    void PhysicsBodyComponent::rebuildIfDirty() {
        if (shapeDirty_ && isInitialized_) {
            rebuildCollisionShape();
            shapeDirty_ = false;
        }
    }
 
    // =========================================================================
    // Collision Callbacks
    // =========================================================================
 
    void PhysicsBodyComponent::processCollisions() {
        if (!rigidBody_) return;
 
        PhysicsEngine* physics = getPhysicsEngine();
        if (!physics) return;
 
        btDiscreteDynamicsWorld* world = physics->getDynamicsWorld();
        btDispatcher* dispatcher = world->getDispatcher();
        int numManifolds = dispatcher->getNumManifolds();
 
        std::set<CollisionComponent*> currentCollisions;
 
        for (int i = 0; i < numManifolds; i++) {
            btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
            if (manifold->getNumContacts() == 0) continue;
 
            const btCollisionObject* objA = manifold->getBody0();
            const btCollisionObject* objB = manifold->getBody1();
 
            // Check if this body is involved
            bool isBodyA = (objA == rigidBody_);
            bool isBodyB = (objB == rigidBody_);
            if (!isBodyA && !isBodyB) continue;
 
            // Get the other body
            const btCollisionObject* otherObj = isBodyA ? objB : objA;
 
            // Try to get the CollisionComponent from the other body's user pointer
            void* userPtr = otherObj->getUserPointer();
            if (userPtr) {
                auto* otherBody = static_cast<PhysicsBodyComponent*>(userPtr);
                // Add all collision components from the other body
                for (CollisionComponent* otherCollision : otherBody->collisions_) {
                    currentCollisions.insert(otherCollision);
                }
            }
        }
 
        // Dispatch callbacks
        for (CollisionComponent* other : currentCollisions) {
            bool wasColliding = previousCollisions_.contains(other);
 
            if (wasColliding) {
                // Still colliding - dispatch stay
                for (CollisionComponent* myCollision : collisions_) {
                    myCollision->onCollisionStay(other);
                }
            } else {
                // New collision - dispatch begin
                for (CollisionComponent* myCollision : collisions_) {
                    myCollision->onCollisionBegin(other);
                }
            }
        }
 
        // Check for ended collisions
        for (CollisionComponent* prev : previousCollisions_) {
            if (!currentCollisions.contains(prev)) {
                // Collision ended
                for (CollisionComponent* myCollision : collisions_) {
                    myCollision->onCollisionEnd(prev);
                }
            }
        }
 
        previousCollisions_ = currentCollisions;
    }
 
    // =========================================================================
    // Private Methods
    // =========================================================================
 
    void PhysicsBodyComponent::rebuildCollisionShape() {
        // Delete old compound shape (but not the child shapes - they're owned by CollisionComponents)
        if (compoundShape_) {
            delete compoundShape_;
            compoundShape_ = nullptr;
        }
 
        if (collisions_.empty()) return;
 
        // Create new compound shape
        compoundShape_ = new btCompoundShape();
 
        for (CollisionComponent* collision : collisions_) {
            btCollisionShape* shape = collision->getShape();
            if (!shape) {
                // Create and store the shape in the collision component
                shape = collision->createShape();
                collision->shape_ = shape;
            }
 
            if (shape) {
                // Get the local transform for this collision
                glm::mat4 localTransform = collision->getLocalTransform();
                btTransform btLocal;
                btLocal.setFromOpenGLMatrix(glm::value_ptr(localTransform));
 
                compoundShape_->addChildShape(btLocal, shape);
            }
        }
 
        shapeDirty_ = false;
    }
 
    void PhysicsBodyComponent::createRigidBody() {
        PLOGI << "PhysicsBodyComponent::createRigidBody() starting";
        if (rigidBody_ || !compoundShape_) {
            PLOGI << "PhysicsBodyComponent::createRigidBody() early return - rigidBody_=" << (rigidBody_ ? "set" : "null") << " compoundShape_=" << (compoundShape_ ? "set" : "null");
            return;
        }
 
        PhysicsEngine* physics = getPhysicsEngine();
        if (!physics) {
            PLOGI << "PhysicsBodyComponent::createRigidBody() - no physics engine";
            return;
        }
 
        auto sceneNode = sceneNode_.lock();
        if (!sceneNode) {
            PLOGI << "PhysicsBodyComponent::createRigidBody() - sceneNode expired";
            return;
        }
 
        // Get initial transform from scene node
        glm::mat4 worldMatrix = sceneNode->getWorldMatrix();
 
        RigidBodyCreateInfo info;
        info.transform = worldMatrix;
        info.mass = isStatic_ ? 0.0f : mass_;
        info.shape = compoundShape_;
        info.friction = friction_;
        info.restitution = restitution_;
 
        rigidBody_ = physics->createRigidBody(info);
 
        // Set user pointer for collision callbacks
        rigidBody_->setUserPointer(this);
 
        // Apply kinematic flag if needed
        if (isKinematic_) {
            int flags = rigidBody_->getCollisionFlags();
            flags |= btCollisionObject::CF_KINEMATIC_OBJECT;
            rigidBody_->setCollisionFlags(flags);
            rigidBody_->setActivationState(DISABLE_DEACTIVATION);
        }
 
        // Apply trigger mode for all trigger collisions
        bool hasTrigger = false;
        for (CollisionComponent* collision : collisions_) {
            if (collision->isTrigger()) {
                hasTrigger = true;
                break;
            }
        }
        if (hasTrigger) {
            int flags = rigidBody_->getCollisionFlags();
            flags |= btCollisionObject::CF_NO_CONTACT_RESPONSE;
            rigidBody_->setCollisionFlags(flags);
        }
 
        // Update ECS component
        updateEcsComponent();
 
        PLOGI << "PhysicsBodyComponent::createRigidBody() completed - rigidBody created with mass=" << info.mass;
    }
 
    void PhysicsBodyComponent::destroyRigidBody() {
        if (!rigidBody_) return;
 
        PhysicsEngine* physics = getPhysicsEngine();
        if (physics) {
            physics->removeRigidBody(rigidBody_);
        }
 
        rigidBody_ = nullptr;
 
        // Note: ECS component cleanup is handled by PhysicsEngine::onPhysicsComponentDestroyed
        // or we need to remove it manually
    }
 
    void PhysicsBodyComponent::updateEcsComponent() {
        auto& registry = Ecs::RegistryManager::get();
 
        // Use the SceneNode's entity, not the Actor's entity
        // The physics sync loop requires SimulatesPhysics and LocalTransform on the same entity
        auto sceneNode = sceneNode_.lock();
        if (!sceneNode) return;
 
        entt::entity nodeEntity = sceneNode->getEntity();
        if (!registry.valid(nodeEntity)) return;
 
        if (rigidBody_) {
            auto& physicsComp = registry.get_or_emplace<Ecs::SimulatesPhysics>(nodeEntity);
            physicsComp.rigidBody = rigidBody_;
            physicsComp.collisionShape = compoundShape_;
            physicsComp.isKinematic = isKinematic_;
            physicsComp.isStatic = isStatic_;
        }
    }
 
    PhysicsEngine* PhysicsBodyComponent::getPhysicsEngine() const {
        Engine* engine = EngineManager::getInstance().getEngineModule();
        return engine ? engine->getPhysicsEngine() : nullptr;
    }
 
}  // namespace EngineCore