FIxed debug draw for offset colliders

SHADE_Engine/src/Math/Geometry/SHBox.cpp

@@ -1,5 +1,5 @@
 /****************************************************************************************
- * \file      SHBoundingBox.cpp
+ * \file      SHBox.cpp
  * \author    Diren D Bharwani, diren.dbharwani, 390002520
  * \brief     Implementation for a 3-Dimensional Axis Aligned Bounding Box
  *

SHADE_Engine/src/Math/Geometry/SHBox.h

@@ -1,5 +1,5 @@
 /****************************************************************************************
- * \file      SHBoundingBox.h
+ * \file      SHBox.h
  * \author    Diren D Bharwani, diren.dbharwani, 390002520
  * \brief     Interface for a 3-Dimensional Axis Aligned Bounding Box
  *

SHADE_Engine/src/Physics/System/SHPhysicsDebugDrawSystem.cpp

@@ -24,7 +24,7 @@ namespace SHADE
   /* Static Data Member Definitions                                                    */
   /*-----------------------------------------------------------------------------------*/
 
-  const SHPhysicsDebugDrawSystem::DebugDrawFunction SHPhysicsDebugDrawSystem::drawFunctions[SHPhysicsDebugDrawSystem::NUM_FLAGS] =
+  const SHPhysicsDebugDrawSystem::DebugDrawFunction SHPhysicsDebugDrawSystem::drawFunctions[NUM_FLAGS] =
   {
     SHPhysicsDebugDrawSystem::drawColliders
   , SHPhysicsDebugDrawSystem::drawColliderAABBs
@@ -33,6 +33,8 @@ namespace SHADE
   , SHPhysicsDebugDrawSystem::drawContactNormals
   };
 
+  SHVec3 SHPhysicsDebugDrawSystem::boxVertices[NUM_BOX_VERTICES];
+
   /*-----------------------------------------------------------------------------------*/
   /* Constructors & Destructor Definitions                                             */
   /*-----------------------------------------------------------------------------------*/
@@ -42,7 +44,7 @@ namespace SHADE
   , physicsSystem     { nullptr }
   , rp3dDebugRenderer { nullptr }
   {
-    debugColours[SHUtilities::ConvertEnum(DebugDrawFlags::COLLIDER)]          = 
+    debugColours[SHUtilities::ConvertEnum(DebugDrawFlags::COLLIDER)]          = SHColour::GREEN;
     debugColours[SHUtilities::ConvertEnum(DebugDrawFlags::COLLIDER_AABB)]     = SHColour::YELLOW;
     debugColours[SHUtilities::ConvertEnum(DebugDrawFlags::BROAD_PHASE_AABB)]  = SHColour::CYAN;
     debugColours[SHUtilities::ConvertEnum(DebugDrawFlags::CONTACT_POINTS)]    = SHColour::RED;
@@ -95,6 +97,9 @@ namespace SHADE
 
     SHASSERT(physicsSystem == nullptr, "Non-existent physics system attached to the physics debug draw system!")
     physicsSystem = SHSystemManager::GetSystem<SHPhysicsSystem>();
+
+    // Generate shapes
+    generateBox();
   }
 
   void SHPhysicsDebugDrawSystem::Exit()
@@ -159,21 +164,20 @@ namespace SHADE
     
   }
 
+  void SHPhysicsDebugDrawSystem::generateBox() noexcept
+  {
+    boxVertices[0] = {  0.5f,  0.5f, -0.5f  }; // TOP_RIGHT_BACK
+    boxVertices[1] = { -0.5f,  0.5f, -0.5f  }; // TOP_LEFT_BACK
+    boxVertices[2] = {  0.5f, -0.5f, -0.5f  }; // BTM_RIGHT_BACK
+    boxVertices[3] = { -0.5f, -0.5f, -0.5f  }; // BTM_LEFT_BACK
+    boxVertices[4] = {  0.5f,  0.5f,  0.5f  }; // TOP_RIGHT_FRONT
+    boxVertices[5] = { -0.5f,  0.5f,  0.5f  }; // TOP_LEFT_FRONT
+    boxVertices[6] = {  0.5f, -0.5f,  0.5f  }; // BTM_RIGHT_FRONT
+    boxVertices[7] = { -0.5f, -0.5f,  0.5f  }; // BTM_LEFT_FRONT
+  }
+
   void SHPhysicsDebugDrawSystem::debugDrawBox(const SHColliderComponent& colliderComponent, const SHCollisionShape& collisionShape) noexcept
   {
-    static constexpr uint32_t NUM_BOX_VERTICES = 8;
-    static const SHVec3 boxVertices[NUM_BOX_VERTICES]
-    {
-      {  0.5f,  0.5f, -0.5f  } // TOP_RIGHT_BACK
-    , { -0.5f,  0.5f, -0.5f  } // TOP_LEFT_BACK
-    , {  0.5f, -0.5f, -0.5f  } // BTM_RIGHT_BACK
-    , { -0.5f, -0.5f, -0.5f  } // BTM_LEFT_BACK
-    , {  0.5f,  0.5f,  0.5f  } // TOP_RIGHT_FRONT
-    , { -0.5f,  0.5f,  0.5f  } // TOP_LEFT_FRONT
-    , {  0.5f, -0.5f,  0.5f  } // BTM_RIGHT_FRONT
-    , { -0.5f, -0.5f,  0.5f  } // BTM_LEFT_FRONT
-    };
-
     auto* debugDrawSystem = SHSystemManager::GetSystem<SHDebugDrawSystem>();
     if (debugDrawSystem == nullptr)
     {
@@ -184,10 +188,16 @@ namespace SHADE
     auto* BOX = reinterpret_cast<const SHBox*>(collisionShape.GetShape());
 
     // Calculate final position & orientation
-    const SHVec3        FINAL_POS = colliderComponent.GetPosition() + collisionShape.GetPositionOffset();
+    const SHVec3        COLLIDER_POS  = colliderComponent.GetPosition();
-    const SHQuaternion  FINAL_ROT = colliderComponent.GetOrientation() * SHQuaternion::FromEuler(collisionShape.GetRotationOffset());
+    const SHVec3        BOX_POS       = collisionShape.GetPositionOffset();
+    const SHQuaternion  COLLIDER_ROT  = colliderComponent.GetOrientation();
+    const SHQuaternion  BOX_ROT       = SHQuaternion::FromEuler(collisionShape.GetRotationOffset());
 
-    const SHMatrix BOX_TRS = SHMatrix::Scale(BOX->GetWorldExtents() * 2.0f) * SHMatrix::Rotate(FINAL_ROT) * SHMatrix::Translate(FINAL_POS);
+
+    const SHMatrix COLLIDER_TR  = SHMatrix::Rotate(COLLIDER_ROT) * SHMatrix::Translate(COLLIDER_POS);
+    const SHMatrix BOX_TRS      = SHMatrix::Scale(BOX->GetWorldExtents() * 2.0f) * SHMatrix::Rotate(BOX_ROT) * SHMatrix::Translate(BOX_POS);
+
+    const SHMatrix FINAL_TRS = BOX_TRS * COLLIDER_TR;
 
     const SHColour COLLIDER_COLOUR = collisionShape.IsTrigger() ? SHColour::PURPLE : SHColour::GREEN;
 
@@ -197,8 +207,8 @@ namespace SHADE
       const uint32_t IDX1 = i;
       const uint32_t IDX2 = i + NUM_BOX_VERTICES / 2;
 
-      transformedVertices[IDX1] = SHVec3::Transform(boxVertices[IDX1], BOX_TRS);
+      transformedVertices[IDX1] = SHVec3::Transform(boxVertices[IDX1], FINAL_TRS);
-      transformedVertices[IDX2] = SHVec3::Transform(boxVertices[IDX2], BOX_TRS);
+      transformedVertices[IDX2] = SHVec3::Transform(boxVertices[IDX2], FINAL_TRS);
 
       // Draw 4 line to connect the quads
       debugDrawSystem->DrawLine(COLLIDER_COLOUR, transformedVertices[IDX1], transformedVertices[IDX2]);
@@ -207,6 +217,7 @@ namespace SHADE
     // A, B, C, D
     std::array backQuad { transformedVertices[0], transformedVertices[1], transformedVertices[3], transformedVertices[2] };
     debugDrawSystem->DrawPoly(COLLIDER_COLOUR, backQuad.begin(), backQuad.end());
+
     // E, F, G, H
     std::array frontQuad { transformedVertices[4], transformedVertices[5], transformedVertices[7], transformedVertices[6] };
     debugDrawSystem->DrawPoly(COLLIDER_COLOUR, frontQuad.begin(), frontQuad.end());
@@ -226,8 +237,10 @@ namespace SHADE
     const SHColour COLLIDER_COLOUR = collisionShape.IsTrigger() ? SHColour::PURPLE : SHColour::GREEN;
 
     // Calculate final position & orientation
-    const SHVec3 FINAL_POS = colliderComponent.GetPosition() + collisionShape.GetPositionOffset();
+    const SHQuaternion  FINAL_ROT = colliderComponent.GetOrientation() * SHQuaternion::FromEuler(collisionShape.GetRotationOffset());
-    debugDrawSystem->DrawSphere(COLLIDER_COLOUR, FINAL_POS, SPHERE->GetWorldRadius());
+    const SHMatrix      TR = SHMatrix::Rotate(FINAL_ROT) * SHMatrix::Translate(colliderComponent.GetPosition());
+
+    debugDrawSystem->DrawSphere(COLLIDER_COLOUR, SHVec3::Transform(collisionShape.GetPositionOffset(), TR), SPHERE->GetWorldRadius());
   }
 
 } // namespace SHADE

SHADE_Engine/src/Physics/System/SHPhysicsDebugDrawSystem.h

@@ -98,9 +98,14 @@ namespace SHADE
     /*---------------------------------------------------------------------------------*/
 
     static constexpr int NUM_FLAGS = SHUtilities::ConvertEnum(DebugDrawFlags::NUM_FLAGS);
-
     static const DebugDrawFunction  drawFunctions[NUM_FLAGS];
 
+    // SHAPES INFO
+
+    static constexpr size_t NUM_BOX_VERTICES    = 8;
+    static SHVec3 boxVertices[NUM_BOX_VERTICES];
+    
+
     uint8_t                 debugDrawFlags;
     SHPhysicsSystem*        physicsSystem;
     rp3d::DebugRenderer*    rp3dDebugRenderer;
@@ -110,12 +115,20 @@ namespace SHADE
     /* Function Members                                                                */
     /*---------------------------------------------------------------------------------*/
 
+    // Generic Draw Functions
+
     static void drawColliders       (rp3d::DebugRenderer* debugRenderer) noexcept;
     static void drawColliderAABBs   (rp3d::DebugRenderer* debugRenderer) noexcept;
     static void drawBroadPhaseAABBs (rp3d::DebugRenderer* debugRenderer) noexcept;
     static void drawContactPoints   (rp3d::DebugRenderer* debugRenderer) noexcept;
     static void drawContactNormals  (rp3d::DebugRenderer* debugRenderer) noexcept;
 
+    // Shape Generation Functions
+
+    static void generateBox         () noexcept;
+
+    // Shape Draw Functions
+
     static void debugDrawBox        (const SHColliderComponent& colliderComponent, const SHCollisionShape& collisionShape) noexcept;
     static void debugDrawSphere     (const SHColliderComponent& colliderComponent, const SHCollisionShape& collisionShape) noexcept;
   };