Fixed physics bugs

1. Colliders are now properly deleted along with its component.
2. Forces and velocities are reset on stop.

SHADE_Engine/src/Physics/Components/SHColliderComponent.cpp

@@ -26,7 +26,6 @@ namespace SHADE
 
   SHColliderComponent::SHColliderComponent() noexcept
   : system    { nullptr }
-  , colliders {}
   {}
 
   /*-----------------------------------------------------------------------------------*/
@@ -78,9 +77,15 @@ namespace SHADE
 
   SHBoundingBox* SHColliderComponent::AddBoundingBox(const SHVec3& halfExtents, const SHVec3& posOffset) noexcept
   {
-    const auto TYPE = SHCollider::Type::BOX;
+    if (!system)
+    {
+      SHLOG_ERROR("Physics system does not exist, unable to add Box Collider!")
+      return nullptr;
+    }
 
-    auto boxPair = std::make_pair(SHCollider{TYPE}, true);
+    static constexpr auto TYPE = SHCollider::Type::BOX;
+
+    auto boxPair = std::make_pair(SHCollider{ TYPE }, true);
     auto& collider = colliders.emplace_back(boxPair).first;
 
     const auto* tf = SHComponentManager::GetComponent<SHTransformComponent>(GetEID());
@@ -88,12 +93,6 @@ namespace SHADE
     collider.SetPositionOffset(posOffset);
     collider.SetAsBoundingBox(tf->GetWorldScale() * halfExtents);
 
-    if (!system)
-    {
-      SHLOG_ERROR("Physics system does not exist, unable to add Box Collider!")
-      return nullptr;
-    }
-
     // Notify Physics System
     system->AddCollisionShape(GetEID(), &collider);
 
@@ -102,7 +101,13 @@ namespace SHADE
 
   SHBoundingSphere* SHColliderComponent::AddBoundingSphere(float radius, const SHVec3& posOffset) noexcept
   {
-    const auto TYPE = SHCollider::Type::SPHERE;
+    if (!system)
+    {
+      SHLOG_ERROR("Physics system does not exist, unable to add Sphere Collider!")
+      return nullptr;
+    }
+
+    static constexpr auto TYPE = SHCollider::Type::SPHERE;
 
     auto spherePair = std::make_pair(SHCollider{ TYPE }, true);
     auto& collider = colliders.emplace_back(spherePair).first;
@@ -113,13 +118,7 @@ namespace SHADE
 
     const SHVec3 TF_WORLD_SCALE = tf->GetWorldScale();
     const float MAX_SCALE = SHMath::Max({ TF_WORLD_SCALE.x, TF_WORLD_SCALE.y, TF_WORLD_SCALE.z });
-    collider.SetAsBoundingSphere(MAX_SCALE * 0.5f);
+    collider.SetAsBoundingSphere(MAX_SCALE * 0.5f * radius);
-
-    if (!system)
-    {
-      SHLOG_ERROR("Physics system does not exist, unable to add Sphere Collider!")
-      return nullptr;
-    }
 
     // Notify Physics System
     system->AddCollisionShape(GetEID(), &collider);

SHADE_Engine/src/Physics/Components/SHColliderComponent.h

@@ -44,7 +44,7 @@ namespace SHADE
     /*---------------------------------------------------------------------------------*/
 
     using ColliderDirtyPair = std::pair<SHCollider, bool>;
-    using Colliders = std::vector<ColliderDirtyPair>;
+    using Colliders         = std::vector<ColliderDirtyPair>;
 
   public:
 
@@ -81,10 +81,10 @@ namespace SHADE
     /* Function Members                                                                */
     /*---------------------------------------------------------------------------------*/
 
-    void            OnCreate          ()          override;
+    void              OnCreate          ()          override;
-    void            OnDestroy         ()          override;
+    void              OnDestroy         ()          override;
 
-    void            RemoveCollider    (int index);
+    void              RemoveCollider    (int index);
 
     SHBoundingBox*    AddBoundingBox    (const SHVec3& halfExtents = SHVec3::One, const SHVec3& posOffset = SHVec3::Zero) noexcept;
     SHBoundingSphere* AddBoundingSphere (float radius = 1.0f, const SHVec3& posOffset = SHVec3::Zero) noexcept;

SHADE_Engine/src/Physics/Components/SHRigidBodyComponent.cpp

@@ -28,6 +28,7 @@ namespace SHADE
   , flags       { 0 }
   , dirtyFlags  { 0 }
   , interpolate { true }
+  , rp3dBody    { nullptr }
   , mass        { 1.0f }
   , drag        { 0.01f }
   , angularDrag { 0.01f }
@@ -159,7 +160,13 @@ namespace SHADE
 
   void SHRigidBodyComponent::SetGravityEnabled(bool enableGravity) noexcept
   {
-    constexpr int FLAG_POS = 0;
+    static constexpr int FLAG_POS = 0;
+
+    if (type != Type::DYNAMIC)
+    {
+      SHLOG_WARNING("Cannot enable gravity of a non-dynamic object {}", GetEID())
+      return;
+    }
 
     dirtyFlags |= 1U << FLAG_POS;
     enableGravity ? flags |= (1U << FLAG_POS) : flags &= ~(1U << FLAG_POS);
@@ -167,7 +174,13 @@ namespace SHADE
 
   void SHRigidBodyComponent::SetIsAllowedToSleep(bool isAllowedToSleep) noexcept
   {
-    constexpr int FLAG_POS = 1;
+    static constexpr int FLAG_POS = 1;
+
+    if (type != Type::DYNAMIC)
+    {
+      SHLOG_WARNING("Cannot enable sleeping of a non-dynamic object {}", GetEID())
+      return;
+    }
 
     dirtyFlags |= 1U << 1;
     isAllowedToSleep ? flags |= (1U << FLAG_POS) : flags &= ~(1U << FLAG_POS);
@@ -175,7 +188,13 @@ namespace SHADE
 
   void SHRigidBodyComponent::SetFreezePositionX(bool freezePositionX) noexcept
   {
-    constexpr int FLAG_POS = 2;
+    static constexpr int FLAG_POS = 2;
+
+    if (type == Type::STATIC)
+    {
+      SHLOG_WARNING("Cannot set linear constraints of a static object {}", GetEID())
+      return;
+    }
 
     dirtyFlags |= 1U << 2;
     freezePositionX ? flags |= (1U << FLAG_POS) : flags &= ~(1U << FLAG_POS);
@@ -183,7 +202,13 @@ namespace SHADE
 
   void SHRigidBodyComponent::SetFreezePositionY(bool freezePositionY) noexcept
   {
-    constexpr int FLAG_POS = 3;
+    static constexpr int FLAG_POS = 3;
+
+    if (type == Type::STATIC)
+    {
+      SHLOG_WARNING("Cannot set linear constraints of a static object {}", GetEID())
+      return;
+    }
 
     dirtyFlags |= 1U << 2;
     freezePositionY ? flags |= (1U << FLAG_POS) : flags &= ~(1U << FLAG_POS);
@@ -191,7 +216,13 @@ namespace SHADE
 
   void SHRigidBodyComponent::SetFreezePositionZ(bool freezePositionZ) noexcept
   {
-    constexpr int FLAG_POS = 4;
+    static constexpr int FLAG_POS = 4;
+
+    if (type == Type::STATIC)
+    {
+      SHLOG_WARNING("Cannot set linear constraints of a static object {}", GetEID())
+      return;
+    }
 
     dirtyFlags |= 1U << 2;
     freezePositionZ ? flags |= (1U << FLAG_POS) : flags &= ~(1U << FLAG_POS);
@@ -199,7 +230,13 @@ namespace SHADE
 
   void SHRigidBodyComponent::SetFreezeRotationX(bool freezeRotationX) noexcept
   {
-    constexpr int FLAG_POS = 5;
+    static constexpr int FLAG_POS = 5;
+
+    if (type == Type::STATIC)
+    {
+      SHLOG_WARNING("Cannot set angular constraints of a static object {}", GetEID())
+      return;
+    }
 
     dirtyFlags |= 1U << 3;
     freezeRotationX ? flags |= (1U << FLAG_POS) : flags &= ~(1U << FLAG_POS);
@@ -207,7 +244,13 @@ namespace SHADE
 
   void SHRigidBodyComponent::SetFreezeRotationY(bool freezeRotationY) noexcept
   {
-    constexpr int FLAG_POS = 6;
+    static constexpr int FLAG_POS = 6;
+
+    if (type == Type::STATIC)
+    {
+      SHLOG_WARNING("Cannot set angular constraints of a static object {}", GetEID())
+      return;
+    }
 
     dirtyFlags |= 1U << 3;
     freezeRotationY ? flags |= (1U << FLAG_POS) : flags &= ~(1U << FLAG_POS);
@@ -215,7 +258,13 @@ namespace SHADE
 
   void SHRigidBodyComponent::SetFreezeRotationZ(bool freezeRotationZ) noexcept
   {
-    constexpr int FLAG_POS = 7;
+    static constexpr int FLAG_POS = 7;
+
+    if (type == Type::STATIC)
+    {
+      SHLOG_WARNING("Cannot set angular constraints of a static object {}", GetEID())
+      return;
+    }
 
     dirtyFlags |= 1U << 3;
     freezeRotationZ ? flags |= (1U << FLAG_POS) : flags &= ~(1U << FLAG_POS);
@@ -228,30 +277,60 @@ namespace SHADE
 
   void SHRigidBodyComponent::SetMass(float newMass) noexcept
   {
+    if (type != Type::DYNAMIC)
+    {
+      SHLOG_WARNING("Cannot set mass of a non-dynamic object {}", GetEID())
+      return;
+    }
+
     dirtyFlags |=  1U << 5;
     mass = newMass;
   }
 
   void SHRigidBodyComponent::SetDrag(float newDrag) noexcept
   {
+    if (type != Type::DYNAMIC)
+    {
+      SHLOG_WARNING("Cannot set drag of a non-dynamic object {}", GetEID())
+      return;
+    }
+
     dirtyFlags |=  1U << 6;
     drag = newDrag;
   }
 
   void SHRigidBodyComponent::SetAngularDrag(float newAngularDrag) noexcept
   {
+    if (type != Type::DYNAMIC)
+    {
+      SHLOG_WARNING("Cannot set angular drag of a non-dynamic object {}", GetEID())
+      return;
+    }
+
     dirtyFlags |=  1U << 7;
     angularDrag = newAngularDrag;
   }
 
   void SHRigidBodyComponent::SetLinearVelocity(const SHVec3& newLinearVelocity) noexcept
   {
+    if (type == Type::STATIC)
+    {
+      SHLOG_WARNING("Cannot set linear velocity of a static object {}", GetEID())
+      return;
+    }
+
     dirtyFlags |= 1U << 8;
     linearVelocity = newLinearVelocity;
   }
 
   void SHRigidBodyComponent::SetAngularVelocity(const SHVec3& newAngularVelocity) noexcept
   {
+    if (type == Type::STATIC)
+    {
+      SHLOG_WARNING("Cannot set angular velocity of a static object {}", GetEID())
+      return;
+    }
+
     dirtyFlags |= 1U << 9;
     angularVelocity = newAngularVelocity;
   }
@@ -262,42 +341,90 @@ namespace SHADE
 
   void SHRigidBodyComponent::AddForce(const SHVec3& force) const noexcept
   {
+    if (rp3dBody == nullptr)
+    {
+      SHLOGV_ERROR("Entity {} is missing an rp3dBody!", GetEID())
+      return;
+    }
 
+    rp3dBody->applyWorldForceAtCenterOfMass(force);
   }
 
   void SHRigidBodyComponent::AddForceAtLocalPos(const SHVec3& force, const SHVec3& localPos) const noexcept
   {
+    if (rp3dBody == nullptr)
+    {
+      SHLOGV_ERROR("Entity {} is missing an rp3dBody!", GetEID())
+      return;
+    }
 
+    rp3dBody->applyWorldForceAtLocalPosition(force, localPos);
   }
 
   void SHRigidBodyComponent::AddForceAtWorldPos(const SHVec3& force, const SHVec3& worldPos) const noexcept
   {
+    if (rp3dBody == nullptr)
+    {
+      SHLOGV_ERROR("Entity {} is missing an rp3dBody!", GetEID())
+      return;
+    }
 
+    rp3dBody->applyWorldForceAtWorldPosition(force, worldPos);
   }
 
   void SHRigidBodyComponent::AddRelativeForce(const SHVec3& relativeForce) const noexcept
   {
+    if (rp3dBody == nullptr)
+    {
+      SHLOGV_ERROR("Entity {} is missing an rp3dBody!", GetEID())
+      return;
+    }
 
+    rp3dBody->applyLocalForceAtCenterOfMass(relativeForce);
   }
 
   void SHRigidBodyComponent::AddRelativeForceAtLocalPos(const SHVec3& relativeForce, const SHVec3& localPos) const noexcept
   {
+    if (rp3dBody == nullptr)
+    {
+      SHLOGV_ERROR("Entity {} is missing an rp3dBody!", GetEID())
+      return;
+    }
 
+    rp3dBody->applyLocalForceAtLocalPosition(relativeForce, localPos);
   }
 
   void SHRigidBodyComponent::AddRelativeForceAtWorldPos(const SHVec3& relativeForce, const SHVec3& worldPos) const noexcept
   {
+    if (rp3dBody == nullptr)
+    {
+      SHLOGV_ERROR("Entity {} is missing an rp3dBody!", GetEID())
+      return;
+    }
 
+    rp3dBody->applyLocalForceAtWorldPosition(relativeForce, worldPos);
   }
 
   void SHRigidBodyComponent::AddTorque(const SHVec3& torque) const noexcept
   {
+    if (rp3dBody == nullptr)
+    {
+      SHLOGV_ERROR("Entity {} is missing an rp3dBody!", GetEID())
+      return;
+    }
 
+    rp3dBody->applyWorldTorque(torque);
   }
 
   void SHRigidBodyComponent::AddRelativeTorque(const SHVec3& relativeTorque) const noexcept
   {
+    if (rp3dBody == nullptr)
+    {
+      SHLOGV_ERROR("Entity {} is missing an rp3dBody!", GetEID())
+      return;
+    }
 
+    rp3dBody->applyLocalTorque(relativeTorque);
   }
 
 } // namespace SHADE

SHADE_Engine/src/Physics/SHPhysicsSystem.cpp

@@ -16,6 +16,8 @@
 // Project Headers
 #include "ECS_Base/Managers/SHComponentManager.h"
 #include "ECS_Base/Managers/SHEntityManager.h"
+#include "ECS_Base/Managers/SHSystemManager.h"
+#include "Editor/SHEditor.hpp"
 #include "Math/SHMathHelpers.h"
 #include "Scene/SHSceneManager.h"
 #include "Math/Transform/SHTransformComponent.h"
@@ -205,23 +207,6 @@ namespace SHADE
     factory.destroyPhysicsWorld(world);
   }
 
-  //void SHPhysicsSystem::RemoveRigidBody(EntityID entityID) noexcept
-  //{
-  //  #ifdef _DEBUG
-  //    SHLOG_INFO("Removing a Rigidbody from the Physics World.")
-  //  #endif
-
-  //  auto* physicsObject = GetPhysicsObject(entityID);
-  //  SHASSERT(physicsObject != nullptr, "Physics object has been lost from the world!")
-  //}
-
-  //void SHPhysicsSystem::RemoveCollider(EntityID entityID) noexcept
-  //{
-  //  #ifdef _DEBUG
-  //    SHLOG_INFO("Removing a Collider from the Physics World.")
-  //  #endif
-  //}
-
   void SHPhysicsSystem::AddCollisionShape(EntityID entityID, SHCollider* collider)
   {
     auto* physicsObject = GetPhysicsObject(entityID);
@@ -238,24 +223,55 @@ namespace SHADE
   {
     auto* system = reinterpret_cast<SHPhysicsSystem*>(GetSystem());
 
-    // Update bodies and colliders if component is dirty
-    system->SyncRigidBodyComponents(SHComponentManager::GetDense<SHRigidBodyComponent>());
-    system->SyncColliderComponents(SHComponentManager::GetDense<SHColliderComponent>());
-
     // Sync transforms
-    for (auto& physicsObject : system->map | std::views::values)
+    for (auto& [entityID, physicsObject] : system->map)
     {
       // Ensure a valid physics Object
       if (physicsObject.rp3dBody == nullptr)
         continue;
 
-      const auto* TF = SHComponentManager::GetComponent<SHTransformComponent>(physicsObject.entityID);
+      auto* rigidBodyComponent = SHComponentManager::GetComponent_s<SHRigidBodyComponent>(entityID);
-      if (TF->HasChanged())
+
+      // Clear all forces and velocities if editor is not in play
+      if (SHSystemManager::GetSystem<SHEditor>()->editorState == SHEditor::State::STOP)
       {
-        physicsObject.SetPosition(TF->GetWorldPosition());
+        if (rigidBodyComponent)
-        physicsObject.SetOrientation(TF->GetWorldOrientation());
+        {
+          auto* rp3dRigidBody = reinterpret_cast<rp3d::RigidBody*>(physicsObject.rp3dBody);
+          rp3dRigidBody->resetForce();
+          rp3dRigidBody->resetTorque();
+          rp3dRigidBody->setLinearVelocity(SHVec3::Zero);
+          rp3dRigidBody->setAngularVelocity(SHVec3::Zero);
+        }
+      }
+
+      const auto* transformComponent = SHComponentManager::GetComponent_s<SHTransformComponent>(entityID);
+      if (transformComponent && transformComponent->HasChanged())
+      {
+        const auto WORLD_POS = transformComponent->GetWorldPosition();
+        const auto WORLD_ROT = transformComponent->GetWorldOrientation();
+
+        physicsObject.SetPosition(WORLD_POS);
+        physicsObject.SetOrientation(WORLD_ROT);
+
+        if (rigidBodyComponent)
+        {
+          rigidBodyComponent->position    = WORLD_POS;
+          rigidBodyComponent->orientation = WORLD_ROT;
+        }
+
+        auto* colliderComponent = SHComponentManager::GetComponent_s<SHColliderComponent>(entityID);
+        if (colliderComponent)
+        {
+          colliderComponent->position    = WORLD_POS;
+          colliderComponent->orientation = WORLD_ROT;
+        }
       }
     }
+
+    // Update bodies and colliders if component is dirty
+    system->SyncRigidBodyComponents(SHComponentManager::GetDense<SHRigidBodyComponent>());
+    system->SyncColliderComponents(SHComponentManager::GetDense<SHColliderComponent>());
   }
 
   void SHPhysicsSystem::PhysicsFixedUpdate::Execute(double dt) noexcept
@@ -431,9 +447,9 @@ namespace SHADE
       }
 
       // Convert RP3D Transform to SHADE
-      auto* tfComponent = SHComponentManager::GetComponent<SHTransformComponent>(entityID);
+      auto* transformComponent = SHComponentManager::GetComponent<SHTransformComponent>(entityID);
-      tfComponent->SetWorldPosition(rp3dPos);
+      transformComponent->SetWorldPosition(rp3dPos);
-      tfComponent->SetWorldOrientation(rp3dRot);
+      transformComponent->SetWorldOrientation(rp3dRot);
 
       // Cache transforms
       physicsObject.prevTransform = CURRENT_TF;
@@ -480,6 +496,7 @@ namespace SHADE
           rp3d::Transform{ rigidBodyComponent->position, rigidBodyComponent->orientation }
         );
 
+        rigidBodyComponent->rp3dBody = reinterpret_cast<rp3d::RigidBody*>(physicsObject->rp3dBody);
 
         // Add collision shapes back into the body
         if (colliderComponent != nullptr)
@@ -491,6 +508,8 @@ namespace SHADE
 
       if (ADDED_ID == COLLIDER_ID)
       {
+        SHASSERT(colliderComponent != nullptr, "Collider Component was not added to Entity " + std::to_string(ENTITY_ID) + "!");
+
         colliderComponent->position    = transformComponent->GetWorldPosition();
         colliderComponent->orientation = transformComponent->GetWorldOrientation();
 
@@ -530,7 +549,7 @@ namespace SHADE
       if (REMOVED_ID == RIGID_BODY_ID)
       {
         world->destroyRigidBody(reinterpret_cast<rp3d::RigidBody*>(physicsObject->rp3dBody));
-        physicsObject->rp3dBody = nullptr;
+        physicsObject->rp3dBody       = nullptr;
 
         auto* colliderComponent = SHComponentManager::GetComponent_s<SHColliderComponent>(ENTITY_ID);
         if (colliderComponent != nullptr)
@@ -544,13 +563,23 @@ namespace SHADE
           for (auto& collider : colliderComponent->colliders | std::views::keys)
             physicsObject->AddCollider(&collider);
         }
+
+        // Wake up all physics objects
+        for (auto& [entityID, object] : map)
+        {
+          if (SHComponentManager::HasComponent<SHRigidBodyComponent>(entityID))
+            reinterpret_cast<rp3d::RigidBody*>(object.rp3dBody)->setIsSleeping(false);
+        }
       }
 
       if (REMOVED_ID == COLLIDER_ID)
       {
         // Remove all colliders
         for (uint32_t i = 0; i < physicsObject->rp3dBody->getNbColliders(); ++i)
-          physicsObject->rp3dBody->removeCollider(physicsObject->rp3dBody->getCollider(i));
+        {
+          auto* collider = physicsObject->rp3dBody->getCollider(i);
+          physicsObject->rp3dBody->removeCollider(collider);
+        }
       }
 
       if (physicsObject->rp3dBody == nullptr)