Add drawing RigidBody center of mass and attached colliders

#2325 #3247

Source/Engine/Physics/Actors/RigidBody.cpp

@@ -338,6 +338,34 @@ void RigidBody::AddMovement(const Vector3& translation, const Quaternion& rotati
     SetTransform(t);
 }
 
+#if USE_EDITOR
+
+#include "Engine/Debug/DebugDraw.h"
+
+void RigidBody::OnDebugDrawSelected()
+{
+    // Draw center of mass
+    if (!_centerOfMassOffset.IsZero())
+    {
+        DEBUG_DRAW_WIRE_SPHERE(BoundingSphere(GetPosition() + (GetOrientation() * (GetCenterOfMass() - GetCenterOfMassOffset())), 5.0f), Color::Red, 0, false);
+    }
+    DEBUG_DRAW_WIRE_SPHERE(BoundingSphere(GetPosition() + (GetOrientation() * GetCenterOfMass()), 2.5f), Color::Aqua, 0, false);
+
+    // Draw all attached colliders
+    for (Actor* child : Children)
+    {
+        const auto collider = Cast<Collider>(child);
+        if (collider && collider->GetAttachedRigidBody() == this)
+        {
+            collider->OnDebugDrawSelf();
+        }
+    }
+
+    Actor::OnDebugDrawSelected();
+}
+
+#endif
+
 void RigidBody::OnCollisionEnter(const Collision& c)
 {
     CollisionEnter(c);

Source/Engine/Physics/Actors/RigidBody.h

@@ -487,6 +487,9 @@ public:
     void Serialize(SerializeStream& stream, const void* otherObj) override;
     void Deserialize(DeserializeStream& stream, ISerializeModifier* modifier) override;
     void AddMovement(const Vector3& translation, const Quaternion& rotation) override;
+#if USE_EDITOR
+    void OnDebugDrawSelected() override;
+#endif
 
     // [IPhysicsActor]
     void* GetPhysicsActor() const override;

Source/Engine/Physics/Colliders/BoxCollider.cpp

@@ -111,18 +111,11 @@ namespace
     }
 }
 
-void BoxCollider::OnDebugDrawSelected()
+void BoxCollider::OnDebugDrawSelf()
 {
     const Color color = Color::GreenYellow;
     DEBUG_DRAW_WIRE_BOX(_bounds, color * 0.3f, 0, false);
 
-    if (_contactOffset > 0)
-    {
-        OrientedBoundingBox contactBounds = _bounds;
-        contactBounds.Extents += Vector3(_contactOffset) / contactBounds.Transformation.Scale;
-        DEBUG_DRAW_WIRE_BOX(contactBounds, Color::Blue.AlphaMultiplied(0.2f), 0, false);
-    }
-
     Vector3 corners[8];
     _bounds.GetCorners(corners);
     const float margin = Math::Min(1.0f, _bounds.GetSize().MinValue() * 0.01f);
@@ -146,8 +139,19 @@ void BoxCollider::OnDebugDrawSelected()
     {
         DEBUG_DRAW_WIRE_BOX(_bounds, wiresColor, 0, true);
     }
+}
+
+void BoxCollider::OnDebugDrawSelected()
+{
+    OnDebugDrawSelf();
+
+    if (_contactOffset > 0)
+    {
+        OrientedBoundingBox contactBounds = _bounds;
+        contactBounds.Extents += Vector3(_contactOffset) / contactBounds.Transformation.Scale;
+        DEBUG_DRAW_WIRE_BOX(contactBounds, Color::Blue.AlphaMultiplied(0.2f), 0, false);
+    }
 
-    // Base
     Collider::OnDebugDrawSelected();
 }
 

Source/Engine/Physics/Colliders/BoxCollider.h

@@ -52,6 +52,7 @@ public:
     // [Collider]
 #if USE_EDITOR
     void OnDebugDraw() override;
+    void OnDebugDrawSelf() override;
     void OnDebugDrawSelected() override;
 #endif
     bool IntersectsItself(const Ray& ray, Real& distance, Vector3& normal) override;

Source/Engine/Physics/Colliders/CapsuleCollider.cpp

@@ -52,6 +52,17 @@ void CapsuleCollider::DrawPhysicsDebug(RenderView& view)
         DEBUG_DRAW_WIRE_CAPSULE(_transform.LocalToWorld(_center), rotation, radius, height, Color::GreenYellow * 0.8f, 0, true);
 }
 
+void CapsuleCollider::OnDebugDrawSelf()
+{
+    Quaternion rotation;
+    Quaternion::Multiply(_transform.Orientation, Quaternion::Euler(0, 90, 0), rotation);
+    const float minSize = 0.001f;
+    const float radius = Math::Max(Math::Abs(_radius) * _cachedScale, minSize);
+    const float height = Math::Max(Math::Abs(_height) * _cachedScale, minSize);
+    const Vector3 position = _transform.LocalToWorld(_center);
+    DEBUG_DRAW_WIRE_CAPSULE(position, rotation, radius, height, Color::GreenYellow, 0, false);
+}
+
 void CapsuleCollider::OnDebugDrawSelected()
 {
     Quaternion rotation;

Source/Engine/Physics/Colliders/CapsuleCollider.h

@@ -56,6 +56,7 @@ public:
 public:
     // [Collider]
 #if USE_EDITOR
+    void OnDebugDrawSelf() override;
     void OnDebugDrawSelected() override;
 #endif
     bool IntersectsItself(const Ray& ray, Real& distance, Vector3& normal) override;

Source/Engine/Physics/Colliders/Collider.h

@@ -160,6 +160,10 @@ protected:
 
 private:
     void OnMaterialChanged();
+    friend RigidBody;
+#if USE_EDITOR
+    virtual void OnDebugDrawSelf() {}
+#endif
 
 public:
     // [PhysicsColliderActor]

Source/Engine/Physics/Colliders/MeshCollider.cpp

@@ -85,13 +85,18 @@ void MeshCollider::DrawPhysicsDebug(RenderView& view)
     }
 }
 
-void MeshCollider::OnDebugDrawSelected()
+void MeshCollider::OnDebugDrawSelf()
 {
     if (CollisionData && CollisionData->IsLoaded())
     {
         const auto& debugLines = CollisionData->GetDebugLines();
         DEBUG_DRAW_LINES(Span<Float3>(debugLines.Get(), debugLines.Count()), _transform.GetWorld(), Color::GreenYellow, 0, false);
     }
+}
+
+void MeshCollider::OnDebugDrawSelected()
+{
+    OnDebugDrawSelf();
 
     // Base
     Collider::OnDebugDrawSelected();

Source/Engine/Physics/Colliders/MeshCollider.h

@@ -31,6 +31,7 @@ public:
     bool CanAttach(RigidBody* rigidBody) const override;
     bool CanBeTrigger() const override;
 #if USE_EDITOR
+    void OnDebugDrawSelf() override;
     void OnDebugDrawSelected() override;
 #endif
     bool IntersectsItself(const Ray& ray, Real& distance, Vector3& normal) override;

Source/Engine/Physics/Colliders/SphereCollider.cpp

@@ -35,9 +35,14 @@ void SphereCollider::DrawPhysicsDebug(RenderView& view)
         DEBUG_DRAW_WIRE_SPHERE(_sphere, Color::GreenYellow * 0.8f, 0, true);
 }
 
-void SphereCollider::OnDebugDrawSelected()
+void SphereCollider::OnDebugDrawSelf()
 {
     DEBUG_DRAW_WIRE_SPHERE(_sphere, Color::GreenYellow, 0, false);
+}
+
+void SphereCollider::OnDebugDrawSelected()
+{
+    OnDebugDrawSelf();
 
     if (_contactOffset > 0)
     {

Source/Engine/Physics/Colliders/SphereCollider.h

@@ -36,6 +36,7 @@ public:
 public:
     // [Collider]
 #if USE_EDITOR
+    void OnDebugDrawSelf() override;
     void OnDebugDrawSelected() override;
 #endif
     bool IntersectsItself(const Ray& ray, Real& distance, Vector3& normal) override;