From 101783126cbeae22b8a1cf60da26bb3ceba8a9ec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?J=C3=A9r=C3=B4me=20Leclercq?= Date: Fri, 13 Oct 2017 15:14:37 +0200 Subject: [PATCH] Physics2D: Add DebugDraw method --- include/Nazara/Physics2D/PhysWorld2D.hpp | 27 ++++++ include/Nazara/Physics2D/RigidBody2D.hpp | 4 + src/Nazara/Physics2D/PhysWorld2D.cpp | 108 +++++++++++++++++++++++ src/Nazara/Physics2D/RigidBody2D.cpp | 9 ++ 4 files changed, 148 insertions(+) diff --git a/include/Nazara/Physics2D/PhysWorld2D.hpp b/include/Nazara/Physics2D/PhysWorld2D.hpp index 939943df5..e0fa516d3 100644 --- a/include/Nazara/Physics2D/PhysWorld2D.hpp +++ b/include/Nazara/Physics2D/PhysWorld2D.hpp @@ -8,6 +8,7 @@ #define NAZARA_PHYSWORLD2D_HPP #include +#include #include #include #include @@ -30,8 +31,16 @@ namespace Nz using ContactPostSolveCallback = std::function; using ContactStartCallback = std::function; + using DebugDrawCircleCallback = std::function; + using DebugDrawDotCallback = std::function; + using DebugDrawPolygonCallback = std::function; + using DebugDrawSegmentCallback = std::function; + using DebugDrawTickSegmentCallback = std::function; + using DebugDrawGetColorCallback = std::function; + public: struct Callback; + struct DebugDrawOptions; struct NearestQueryResult; struct RaycastHit; @@ -40,6 +49,8 @@ namespace Nz PhysWorld2D(PhysWorld2D&&) = delete; ///TODO ~PhysWorld2D(); + void DebugDraw(const DebugDrawOptions& options, bool drawShapes = true, bool drawConstraints = true, bool drawCollisions = true); + float GetDamping() const; Vector2f GetGravity() const; cpSpace* GetHandle() const; @@ -74,6 +85,22 @@ namespace Nz void* userdata; }; + struct DebugDrawOptions + { + Color constraintColor; + Color collisionPointColor; + Color shapeOutlineColor; + + DebugDrawCircleCallback circleCallback; + DebugDrawGetColorCallback colorCallback; + DebugDrawDotCallback dotCallback; + DebugDrawPolygonCallback polygonCallback; + DebugDrawSegmentCallback segmentCallback; + DebugDrawTickSegmentCallback thickSegmentCallback; + + void* userdata; + }; + struct NearestQueryResult { Nz::RigidBody2D* nearestBody; diff --git a/include/Nazara/Physics2D/RigidBody2D.hpp b/include/Nazara/Physics2D/RigidBody2D.hpp index 1319d8e64..5b6116f1c 100644 --- a/include/Nazara/Physics2D/RigidBody2D.hpp +++ b/include/Nazara/Physics2D/RigidBody2D.hpp @@ -13,6 +13,7 @@ #include #include #include +#include struct cpBody; @@ -44,6 +45,7 @@ namespace Nz float GetMomentOfInertia() const; Vector2f GetPosition() const; float GetRotation() const; + std::size_t GetShapeIndex(cpShape* shape) const; void* GetUserdata() const; Vector2f GetVelocity() const; PhysWorld2D* GetWorld() const; @@ -67,6 +69,8 @@ namespace Nz NazaraSignal(OnRigidBody2DMove, RigidBody2D* /*oldPointer*/, RigidBody2D* /*newPointer*/); NazaraSignal(OnRigidBody2DRelease, RigidBody2D* /*rigidBody*/); + static constexpr std::size_t InvalidShapeIndex = std::numeric_limits::max(); + private: void CopyBodyData(cpBody* body); cpBody* Create(float mass = 1.f, float moment = 1.f); diff --git a/src/Nazara/Physics2D/PhysWorld2D.cpp b/src/Nazara/Physics2D/PhysWorld2D.cpp index 06075a782..a1ab49f71 100644 --- a/src/Nazara/Physics2D/PhysWorld2D.cpp +++ b/src/Nazara/Physics2D/PhysWorld2D.cpp @@ -3,11 +3,83 @@ // For conditions of distribution and use, see copyright notice in Config.hpp #include +#include #include #include namespace Nz { + namespace + { + Color CpDebugColorToColor(cpSpaceDebugColor c) + { + return Color{ static_cast(c.r * 255.f), static_cast(c.g * 255.f), static_cast(c.b * 255.f), static_cast(c.a * 255.f) }; + } + + cpSpaceDebugColor ColorToCpDebugColor(Color c) + { + return cpSpaceDebugColor{ c.r / 255.f, c.g / 255.f, c.b / 255.f, c.a / 255.f }; + } + + void DrawCircle(cpVect pos, cpFloat angle, cpFloat radius, cpSpaceDebugColor outlineColor, cpSpaceDebugColor fillColor, cpDataPointer userdata) + { + auto drawOptions = static_cast(userdata); + if (drawOptions->circleCallback) + drawOptions->circleCallback(Vector2f(float(pos.x), float(pos.y)), float(angle), float(radius), CpDebugColorToColor(outlineColor), CpDebugColorToColor(fillColor), drawOptions->userdata); + }; + + void DrawDot(cpFloat size, cpVect pos, cpSpaceDebugColor color, cpDataPointer userdata) + { + auto drawOptions = static_cast(userdata); + if (drawOptions->dotCallback) + drawOptions->dotCallback(Vector2f(float(pos.x), float(pos.y)), float(size), CpDebugColorToColor(color), drawOptions->userdata); + }; + + using DebugDrawPolygonCallback = std::function; + + void DrawPolygon(int vertexCount, const cpVect* vertices, cpFloat radius, cpSpaceDebugColor outlineColor, cpSpaceDebugColor fillColor, cpDataPointer userdata) + { + auto drawOptions = static_cast(userdata); + if (drawOptions->polygonCallback) + { + //TODO: constexpr if to prevent copy/cast if sizeof(cpVect) == sizeof(Vector2f) + + StackAllocation stackAlloc = NazaraStackAllocation(vertexCount * sizeof(Vector2f)); + Vector2f* vec = static_cast(stackAlloc.GetPtr()); + for (int i = 0; i < vertexCount; ++i) + vec[i].Set(float(vertices[i].x), float(vertices[i].y)); + + drawOptions->polygonCallback(vec, vertexCount, float(radius), CpDebugColorToColor(outlineColor), CpDebugColorToColor(fillColor), drawOptions->userdata); + } + }; + + void DrawSegment(cpVect a, cpVect b, cpSpaceDebugColor color, cpDataPointer userdata) + { + auto drawOptions = static_cast(userdata); + if (drawOptions->segmentCallback) + drawOptions->segmentCallback(Vector2f(float(a.x), float(a.y)), Vector2f(float(b.x), float(b.y)), CpDebugColorToColor(color), drawOptions->userdata); + }; + + void DrawThickSegment(cpVect a, cpVect b, cpFloat radius, cpSpaceDebugColor outlineColor, cpSpaceDebugColor fillColor, cpDataPointer userdata) + { + auto drawOptions = static_cast(userdata); + if (drawOptions->thickSegmentCallback) + drawOptions->thickSegmentCallback(Vector2f(float(a.x), float(a.y)), Vector2f(float(b.x), float(b.y)), float(radius), CpDebugColorToColor(outlineColor), CpDebugColorToColor(fillColor), drawOptions->userdata); + }; + + cpSpaceDebugColor GetColorForShape(cpShape* shape, cpDataPointer userdata) + { + auto drawOptions = static_cast(userdata); + if (drawOptions->colorCallback) + { + RigidBody2D& rigidBody = *static_cast(cpShapeGetUserData(shape)); + return ColorToCpDebugColor(drawOptions->colorCallback(rigidBody, rigidBody.GetShapeIndex(shape), drawOptions->userdata)); + } + else + return cpSpaceDebugColor{255.f, 0.f, 0.f, 255.f}; + }; + } + PhysWorld2D::PhysWorld2D() : m_stepSize(0.005f), m_timestepAccumulator(0.f) @@ -21,6 +93,42 @@ namespace Nz cpSpaceFree(m_handle); } + void PhysWorld2D::DebugDraw(const DebugDrawOptions& options, bool drawShapes, bool drawConstraints, bool drawCollisions) + { + auto ColorToCpDebugColor = [](Color c) -> cpSpaceDebugColor + { + return cpSpaceDebugColor{ c.r / 255.f, c.g / 255.f, c.b / 255.f, c.a / 255.f }; + }; + + cpSpaceDebugDrawOptions drawOptions; + drawOptions.collisionPointColor = ColorToCpDebugColor(options.collisionPointColor); + drawOptions.constraintColor = ColorToCpDebugColor(options.constraintColor); + drawOptions.shapeOutlineColor = ColorToCpDebugColor(options.shapeOutlineColor); + drawOptions.data = const_cast(&options); //< Yeah, I know, shame :bell: but it won't be used for writing anyway + + std::underlying_type_t drawFlags = 0; + if (drawCollisions) + drawFlags |= CP_SPACE_DEBUG_DRAW_COLLISION_POINTS; + + if (drawConstraints) + drawFlags |= CP_SPACE_DEBUG_DRAW_CONSTRAINTS; + + if (drawShapes) + drawFlags |= CP_SPACE_DEBUG_DRAW_SHAPES; + + drawOptions.flags = static_cast(drawFlags); + + // Callback trampoline + drawOptions.colorForShape = GetColorForShape; + drawOptions.drawCircle = DrawCircle; + drawOptions.drawDot = DrawDot; + drawOptions.drawFatSegment = DrawThickSegment; + drawOptions.drawPolygon = DrawPolygon; + drawOptions.drawSegment = DrawSegment; + + cpSpaceDebugDraw(m_handle, &drawOptions); + } + float PhysWorld2D::GetDamping() const { return float(cpSpaceGetDamping(m_handle)); diff --git a/src/Nazara/Physics2D/RigidBody2D.cpp b/src/Nazara/Physics2D/RigidBody2D.cpp index b21810c4f..a38e3f3f6 100644 --- a/src/Nazara/Physics2D/RigidBody2D.cpp +++ b/src/Nazara/Physics2D/RigidBody2D.cpp @@ -184,6 +184,15 @@ namespace Nz return FromRadians(static_cast(cpBodyGetAngle(m_handle))); } + std::size_t RigidBody2D::GetShapeIndex(cpShape* shape) const + { + auto it = std::find(m_shapes.begin(), m_shapes.end(), shape); + if (it == m_shapes.end()) + return InvalidShapeIndex; + + return std::distance(m_shapes.begin(), it); + } + void* RigidBody2D::GetUserdata() const { return m_userData;