From 2bd90e7c143ddd7a665c4e5d5f58f07d16203c11 Mon Sep 17 00:00:00 2001
From: Diren D Bharwani <diren.bharwani@gmail.com>
Date: Fri, 16 Dec 2022 14:38:46 +0800
Subject: [PATCH] Adjusted physics debug draw to fit new debug draw interface

---
 .../Routines/SHPhysicsPreUpdateRoutine.cpp    |  5 +++--
 .../System/SHPhysicsDebugDrawSystem.cpp       | 19 +++++++++++--------
 .../Physics/System/SHPhysicsDebugDrawSystem.h |  4 +---
 3 files changed, 15 insertions(+), 13 deletions(-)

diff --git a/SHADE_Engine/src/Physics/System/Routines/SHPhysicsPreUpdateRoutine.cpp b/SHADE_Engine/src/Physics/System/Routines/SHPhysicsPreUpdateRoutine.cpp
index a26a414d..88ea8d79 100644
--- a/SHADE_Engine/src/Physics/System/Routines/SHPhysicsPreUpdateRoutine.cpp
+++ b/SHADE_Engine/src/Physics/System/Routines/SHPhysicsPreUpdateRoutine.cpp
@@ -36,12 +36,13 @@ namespace SHADE
     auto* physicsSystem = reinterpret_cast<SHPhysicsSystem*>(GetSystem());
 
     // Get all physics objects & sync transforms
-    for (auto& [entityID, physicsObject] : physicsSystem->physicsObjectManager.GetPhysicsObjects())
+    auto& physicsObjects = physicsSystem->physicsObjectManager.GetPhysicsObjects();
+    for (auto& [entityID, physicsObject] : physicsObjects)
     {
       const auto* TRANSFORM_COMPONENT = SHComponentManager::GetComponent_s<SHTransformComponent>(entityID);
 
       if (!TRANSFORM_COMPONENT || !TRANSFORM_COMPONENT->HasChanged())
-        return;
+        continue;
 
       const SHVec3&       WORLD_POS = TRANSFORM_COMPONENT->GetWorldPosition();
       const SHQuaternion& WORLD_ROT = TRANSFORM_COMPONENT->GetWorldOrientation();
diff --git a/SHADE_Engine/src/Physics/System/SHPhysicsDebugDrawSystem.cpp b/SHADE_Engine/src/Physics/System/SHPhysicsDebugDrawSystem.cpp
index 62654dbb..dda437bc 100644
--- a/SHADE_Engine/src/Physics/System/SHPhysicsDebugDrawSystem.cpp
+++ b/SHADE_Engine/src/Physics/System/SHPhysicsDebugDrawSystem.cpp
@@ -93,6 +93,7 @@ namespace SHADE
   {
     const auto& EVENT_DATA = reinterpret_cast<const SHEventSpec<SHColliderOnDebugDrawEvent>*>(onColliderDrawEvent.get())->data;
 
+    // Add to the container to draw all colliders
     if (EVENT_DATA->debugDrawState)
     {
       if (collidersToDraw.empty())
@@ -115,15 +116,23 @@ namespace SHADE
   {
     for (const auto* SHAPE : collider.GetCollisionShapes())
     {
+      const SHColour& DRAW_COLOUR = DEBUG_DRAW_COLOURS[SHUtilities::ConvertEnum(SHAPE->IsTrigger() ? Colours::TRIGGER : Colours::COLLIDER)];
+
       switch (SHAPE->GetType())
       {
         case SHCollisionShape::Type::SPHERE:
         {
           const SHSphereCollisionShape* SPHERE = dynamic_cast<const SHSphereCollisionShape*>(SHAPE);
 
-          // Compute rotation of sphere
+          // Compute transforms of sphere
+          const SHVec3 POSITION = SPHERE->GetCenter();  // Position offset is already computed here
           const SHVec3 ROTATION = collider.GetOrientation().ToEuler() + SPHERE->GetRotationOffset();
-          drawSphere(debugDrawSystem, *SPHERE, ROTATION);
+          const SHVec3 SCALE { SPHERE->GetWorldRadius() };
+
+          // Compute TRS for the sphere
+          const SHMatrix TRS = SHMatrix::Transform(POSITION, ROTATION, SCALE);
+
+          debugDrawSystem->DrawWireSphere(TRS, DRAW_COLOUR, true);
 
           break;
         }
@@ -140,12 +149,6 @@ namespace SHADE
     }
   }
 
-  void SHPhysicsDebugDrawSystem::drawSphere(SHDebugDrawSystem* debugDrawSystem, const SHSphereCollisionShape& sphere, const SHVec3& rotation) noexcept
-  {
-    const SHColour& DRAW_COLOUR = DEBUG_DRAW_COLOURS[SHUtilities::ConvertEnum(sphere.IsTrigger() ? Colours::TRIGGER : Colours::COLLIDER)];
-    debugDrawSystem->DrawSphere(DRAW_COLOUR, sphere.GetCenter(), rotation, sphere.GetWorldRadius());
-  }
-
   void SHPhysicsDebugDrawSystem::drawContact(SHDebugDrawSystem* debugDrawSystem, const DebugDrawInfo::Contact& contactInfo) noexcept
   {
     static const SHColour& DRAW_COLOUR = DEBUG_DRAW_COLOURS[SHUtilities::ConvertEnum(Colours::CONTACT)];
diff --git a/SHADE_Engine/src/Physics/System/SHPhysicsDebugDrawSystem.h b/SHADE_Engine/src/Physics/System/SHPhysicsDebugDrawSystem.h
index 1eb4e967..79b88d3f 100644
--- a/SHADE_Engine/src/Physics/System/SHPhysicsDebugDrawSystem.h
+++ b/SHADE_Engine/src/Physics/System/SHPhysicsDebugDrawSystem.h
@@ -138,9 +138,7 @@ namespace SHADE
 
     SHEventHandle onColliderDraw(SHEventPtr onColliderDrawEvent);
 
-    static void   drawCollider  (SHDebugDrawSystem* debugDrawSystem, const SHCollider& collider)                                    noexcept;
-    static void   drawSphere    (SHDebugDrawSystem* debugDrawSystem, const SHSphereCollisionShape& sphere, const SHVec3& rotation)  noexcept;
-
+    static void   drawCollider  (SHDebugDrawSystem* debugDrawSystem, const SHCollider& collider)                noexcept;
     static void   drawContact   (SHDebugDrawSystem* debugDrawSystem, const DebugDrawInfo::Contact& contactInfo) noexcept;
     static void   drawRaycast   (SHDebugDrawSystem* debugDrawSystem, const DebugDrawInfo::Raycast& raycastInfo) noexcept;