diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 072dc9dd..c7674de7 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -156,3 +156,7 @@ endif() if(MAGNUM_WITH_WEBXR_EXAMPLE) add_subdirectory(webxr) endif() + +if(MAGNUM_WITH_BOX3D_EXAMPLE) + add_subdirectory(box3d) +endif() diff --git a/src/box3d/Box3DExample.cpp b/src/box3d/Box3DExample.cpp new file mode 100644 index 00000000..9b73a235 --- /dev/null +++ b/src/box3d/Box3DExample.cpp @@ -0,0 +1,556 @@ +/* + This file is part of Magnum. + + Original authors — credit is appreciated but not required: + + 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018, 2019, + 2020, 2021, 2022, 2023, 2024, 2025, 2026 + — Vladimír Vondruš + 2026 — Igal Alkon + + This is free and unencumbered software released into the public domain. + + Anyone is free to copy, modify, publish, use, compile, sell, or distribute + this software, either in source code form or as a compiled binary, for any + purpose, commercial or non-commercial, and by any means. + + In jurisdictions that recognize copyright laws, the author or authors of + this software dedicate any and all copyright interest in the software to + the public domain. We make this dedication for the benefit of the public + at large and to the detriment of our heirs and successors. We intend this + dedication to be an overt act of relinquishment in perpetuity of all + present and future rights to this software under copyright law. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +*/ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "Box3DIntegration/Converters.h" +#include "Box3DIntegration/DebugDraw.h" + +namespace Magnum { namespace Examples { + +using namespace Math::Literals; + +static constexpr Int BodyGridSize = 14; +static constexpr Int BodyInitialBufferCapacity = BodyGridSize*BodyGridSize*BodyGridSize; + +static constexpr Float GroundSizeXZ = 16.0f; +static constexpr Float GroundSizeY = 0.5f; +static constexpr Float BoxSizeXYZ = 0.5f; +static constexpr Float SphereRadius = 1.0f; +static constexpr Float BoxMass = 1.0f; + +static constexpr Float BoxDensity = 2.75f; +static constexpr Float SphereDensity = 3.5f; +static constexpr Float ShapeFriction = 0.15f; +static constexpr Float ShapeRestitution = 0.05f; + +static constexpr Float ShootBoxMass = 1.0f; +static constexpr Float ShootSphereMass = 5.0f; +static constexpr Float ShootSpeed = 80.0f; + +static constexpr Float MinZoomIn = 3.0f; +static constexpr Float MaxZoomOut = 100.0f; + +static constexpr Float CameraProjectionNear = 0.001f; +static constexpr Float CameraProjectionFar = 1000.0f; + +static constexpr Float MaxBodySimDistance = 1000.0f; +static constexpr Float MaxSimulationDt = 1.0f/30.0f; + +/* Per-instance data for instanced Phong draws (transform, normals, color) */ +struct InstanceData { + Matrix4 transformationMatrix; + Matrix3x3 normalMatrix; + Color3 color; +}; + +/* Lightweight body record: physics handle + render-only side data. + Pose is always read from Box3D when needed (no cached transform). */ +struct Body { + b3BodyId id = b3_nullBodyId; + Matrix4 primitiveTransformation{Math::IdentityInit}; + Color3 color{0xffffff_rgbf}; + bool isBox = true; +}; + +namespace { + +b3BodyId createBody(const Float mass, const b3WorldId worldId, + const b3BoxHull* boxHull, const b3Sphere* sphereGeom, + const Vector3& position = {}) { + const bool isStatic = (mass <= 0.0f); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = isStatic ? b3_staticBody : b3_dynamicBody; + bodyDef.position = b3Pos(position); + bodyDef.rotation = b3Quat_identity; + + const b3BodyId bodyId = b3CreateBody(worldId, &bodyDef); + CORRADE_INTERNAL_ASSERT(b3Body_IsValid(bodyId)); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = ShapeFriction; + shapeDef.baseMaterial.restitution = ShapeRestitution; + + if (!isStatic) { + shapeDef.density = boxHull ? BoxDensity : SphereDensity; + } + + if (boxHull) { + b3CreateHullShape(bodyId, &shapeDef, &boxHull->base); + } else if(sphereGeom) { + b3CreateSphereShape(bodyId, &shapeDef, sphereGeom); + } + + return bodyId; +} + +void destroyBody(Body& body) { + if (b3Body_IsValid(body.id)) { + b3DestroyBody(body.id); + body.id = b3_nullBodyId; + } +} + +/* Read pose straight from Box3D. Returns false if the body is invalid. */ +bool transformationFromPhysics(const Body& body, Matrix4& outTransformation) { + if (!b3Body_IsValid(body.id)) { + return false; + } + + const b3Pos posB = b3Body_GetPosition(body.id); + const b3Quat rotB = b3Body_GetRotation(body.id); + + const Vector3 pos{posB}; + const Quaternion rot{rotB}; + outTransformation = Matrix4::from(rot.toMatrix(), pos); + return true; +} + +Body& spawnBody(Containers::Array& bodies, const Float mass, + const b3WorldId worldId, const b3BoxHull* boxHull, + const b3Sphere* sphereGeom, const Matrix4& primitiveTransformation, + const Color3& color, const Vector3& position = {}) { + Body& body = arrayAppend(bodies, InPlaceInit); + body.id = createBody(mass, worldId, boxHull, sphereGeom, position); + body.isBox = boxHull != nullptr; + body.color = color; + body.primitiveTransformation = primitiveTransformation; + return body; +} + +} + +class Box3DExample: public Platform::Application { +public: + virtual ~Box3DExample(); + explicit Box3DExample(const Arguments& arguments); + +private: + void drawEvent() override; + void keyPressEvent(KeyEvent& event) override; + void pointerPressEvent(PointerEvent& event) override; + void pointerReleaseEvent(PointerEvent& event) override; + void pointerMoveEvent(PointerMoveEvent& event) override; + void scrollEvent(ScrollEvent& event) override; + void viewportEvent(ViewportEvent& event) override; + + GL::Mesh _box{NoCreate}, _sphere{NoCreate}; + GL::Buffer _boxInstanceBuffer{NoCreate}, _sphereInstanceBuffer{NoCreate}; + Shaders::PhongGL _shader{NoCreate}; + Containers::Array _boxInstanceData, _sphereInstanceData; + + /* Orbit Camera */ + Vector3 _cameraTarget{0.0f, 3.0f, 0.0f}; + Deg _cameraYaw = 45.0_degf; + Deg _cameraPitch = -30.0_degf; + Float _cameraDistance = 65.0f; + Matrix4 _projectionMatrix; + Matrix4 _cameraMatrix; + Vector2 _projectionSize; + + bool _dragging = false; + Vector2 _lastPointerPosition; + + Timeline _timeline; + + /* Box3D world identifier */ + b3WorldId _worldId = b3_nullWorldId; + + /* Reusable shape templates */ + b3BoxHull _boxHull{}; + b3Sphere _sphereGeom{}; + b3BoxHull _groundHull{}; + + Box3DIntegration::DebugDraw _debugDraw{NoCreate}; + + Containers::Array _bodies; + + bool _drawCubes{true}, _drawDebug{false}, _shootBox{false}; + + Matrix4 cameraAbsoluteTransformation() const { + /* Classic orbit camera: translate to target -> yaw -> pitch -> push out along local +Z. */ + return Matrix4::translation(_cameraTarget) + * Matrix4::rotationY(_cameraYaw) + * Matrix4::rotationX(_cameraPitch) + * Matrix4::translation(Vector3::zAxis(_cameraDistance)); + } + + void updateCameraMatrices() { + const Matrix4 absolute = cameraAbsoluteTransformation(); + _cameraMatrix = absolute.inverted(); + } +}; + +Box3DExample::Box3DExample(const Arguments& arguments) : + Platform::Application(arguments, NoCreate) +{ + { + const Vector2 dpiScaling = this->dpiScaling({}); + Configuration conf; + conf.setTitle("Magnum Box3D Example") + .setSize(conf.size(), dpiScaling); + GLConfiguration glConf; + glConf.setSampleCount(dpiScaling.max() < 2.0f ? 8 : 2); + if (!tryCreate(conf, glConf)) + create(conf, glConf.setSampleCount(0)); + } + + _debugDraw.create(BodyInitialBufferCapacity); + + /* Manual camera setup */ + const Vector2i viewport = GL::defaultFramebuffer.viewport().size(); + const Float aspect = Vector2{viewport}.aspectRatio(); + constexpr Rad fov = 35.0_degf; + _projectionMatrix = Matrix4::perspectiveProjection(fov, aspect, CameraProjectionNear, CameraProjectionFar); + const Float halfHeightAt1 = Math::tan(fov*0.5f); + _projectionSize = {2.0f * aspect * halfHeightAt1, 2.0f * halfHeightAt1}; + updateCameraMatrices(); + + /* Instanced Phong shader */ + _shader = Shaders::PhongGL{ + Shaders::PhongGL::Configuration{} + .setFlags(Shaders::PhongGL::Flag::VertexColor | + Shaders::PhongGL::Flag::InstancedTransformation) + }; + + /* Global/world lighting */ + _shader.setAmbientColor(0x333333_rgbf) + .setSpecularColor(0x222222_rgbf) + .setLightColors({0xffffff_rgbf}); + + /* Meshes and instance buffers */ + _box = MeshTools::compile(Primitives::cubeSolid()); + _sphere = MeshTools::compile(Primitives::uvSphereSolid(16, 32)); + + _boxInstanceBuffer = GL::Buffer{}; + _sphereInstanceBuffer = GL::Buffer{}; + + _box.addVertexBufferInstanced(_boxInstanceBuffer, 1, 0, + Shaders::PhongGL::TransformationMatrix{}, + Shaders::PhongGL::NormalMatrix{}, + Shaders::PhongGL::Color3{}); + + _sphere.addVertexBufferInstanced(_sphereInstanceBuffer, 1, 0, + Shaders::PhongGL::TransformationMatrix{}, + Shaders::PhongGL::NormalMatrix{}, + Shaders::PhongGL::Color3{}); + + /* Growable arrays: reserve once so shooting / filling instances stays cheap */ + arrayReserve(_bodies, BodyInitialBufferCapacity + 1); + arrayReserve(_boxInstanceData, BodyInitialBufferCapacity + 1); + arrayReserve(_sphereInstanceData, 64); + + GL::Renderer::enable(GL::Renderer::Feature::DepthTest); + GL::Renderer::enable(GL::Renderer::Feature::FaceCulling); + GL::Renderer::enable(GL::Renderer::Feature::PolygonOffsetFill); + GL::Renderer::setPolygonOffset(2.0f, 0.5f); + + /* Box3D Initialization */ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.gravity = {0.0f, -9.81f, 0.0f}; + + // Workers as the [number of cores] - 1 + const unsigned workers = std::max(1u, std::thread::hardware_concurrency()-1); + worldDef.workerCount = workers; + worldDef.enableSleep = true; + _worldId = b3CreateWorld(&worldDef); + CORRADE_INTERNAL_ASSERT(b3World_IsValid(_worldId)); + + /* Precompute reusable shape geometries (half-extents / radius) */ + _boxHull = b3MakeBoxHull(BoxSizeXYZ, BoxSizeXYZ, BoxSizeXYZ); + _groundHull = b3MakeBoxHull(GroundSizeXZ, GroundSizeY, GroundSizeXZ); + _sphereGeom = {{0.0f, 0.0f, 0.0f}, SphereRadius}; + + /* Ground (static) */ + spawnBody(_bodies, 0.0f, _worldId, &_groundHull, nullptr, + Matrix4::scaling({GroundSizeXZ, GroundSizeY, GroundSizeXZ}), + 0xffffff_rgbf); + + /* Stack of dynamic boxes */ + constexpr Int boxGridSize = BodyGridSize; + constexpr Float boxGridOffset = (boxGridSize - 1)*BoxSizeXYZ; + auto hue = 42.0_degf; + for(Int i = 0; i != boxGridSize; ++i) { + for(Int j = 0; j != boxGridSize; ++j) { + for(Int k = 0; k != boxGridSize; ++k) { + const Vector3 pos{ + static_cast(i) - boxGridOffset, + static_cast(j) + 4.0f, + static_cast(k) - boxGridOffset + }; + spawnBody(_bodies, BoxMass, _worldId, &_boxHull, nullptr, + Matrix4::scaling(Vector3{BoxSizeXYZ}), + Color3::fromHsv({hue += 137.5_degf, 0.75f, 0.9f}), + pos); + } + } + } + + setSwapInterval(1); + setMinimalLoopPeriod(16.0_msec); + _timeline.start(); +} + +Box3DExample::~Box3DExample() { + for(Body& body: _bodies) { + destroyBody(body); + } + + arrayClear(_bodies); + + if(b3World_IsValid(_worldId)) { + b3DestroyWorld(_worldId); + _worldId = b3_nullWorldId; + } +} + +void Box3DExample::viewportEvent(ViewportEvent& event) { + GL::defaultFramebuffer.setViewport({{}, event.framebufferSize()}); + + const Vector2i size = event.framebufferSize(); + const Float aspect = Vector2{size}.aspectRatio(); + constexpr Rad fov = 35.0_degf; + _projectionMatrix = Matrix4::perspectiveProjection(fov, aspect, CameraProjectionNear, CameraProjectionFar); + + /* Keep projectionSize in sync (plane at distance 1) */ + const Float halfHeightAt1 = Math::tan(fov*0.5f); + _projectionSize = {2.0f * aspect * halfHeightAt1, 2.0f * halfHeightAt1}; +} + +void Box3DExample::drawEvent() { + GL::defaultFramebuffer.clear(GL::FramebufferClear::Color|GL::FramebufferClear::Depth); + + /* Upper-bounds the physics step so a hitch won't feed Box3D a huge delta time. */ + const Float dt = Math::min(_timeline.previousFrameDuration(), MaxSimulationDt); + b3World_Step(_worldId, dt, 4); + + /* Update view matrix */ + updateCameraMatrices(); + + if(_drawCubes) { + arrayClear(_boxInstanceData); + arrayClear(_sphereInstanceData); + } + + if(_drawDebug) { + _debugDraw.setTransformationProjectionMatrix( + _projectionMatrix*_cameraMatrix); + } + + /* + * Single pass over all bodies: + * - Read pose from physics; remove invalid bodies + * - Destroy and drop bodies that flew too far + * - Build instanced box/sphere draw data when enabled + * - Emit debug axes / wireframes when enabled + */ + for(std::size_t i = 0; i != _bodies.size();) { + Body& body = _bodies[i]; + + Matrix4 transformation; + if(!transformationFromPhysics(body, transformation)) { + arrayRemoveUnordered(_bodies, i); + continue; + } + + const Vector3 pos = transformation.translation(); + if(pos.dot() > MaxBodySimDistance*MaxBodySimDistance) { + destroyBody(body); + arrayRemoveUnordered(_bodies, i); + continue; + } + + if(_drawCubes) { + const Matrix4 t = + _cameraMatrix*transformation*body.primitiveTransformation; + arrayAppend(body.isBox ? _boxInstanceData : _sphereInstanceData, + InPlaceInit, t, t.normalMatrix(), body.color); + } + + if(_drawDebug) { + _debugDraw.drawAxes(transformation, 0.6f); + + const Vector3 scale = + Math::abs(body.primitiveTransformation.scaling()); + if(body.isBox) + _debugDraw.drawWireframeBox(transformation, scale); + else + _debugDraw.drawWireframeSphere(transformation, scale.x()); + } + + ++i; + } + + if(_drawCubes) { + _shader.setProjectionMatrix(_projectionMatrix); + _shader.setLightPositions({ + _cameraMatrix*Vector4{10.0f, 15.0f, 5.0f, 0.0f} + }); + + _boxInstanceBuffer.setData(_boxInstanceData, GL::BufferUsage::DynamicDraw); + _box.setInstanceCount(_boxInstanceData.size()); + _shader.draw(_box); + + _sphereInstanceBuffer.setData(_sphereInstanceData, GL::BufferUsage::DynamicDraw); + _sphere.setInstanceCount(_sphereInstanceData.size()); + _shader.draw(_sphere); + } + + if(_drawDebug) { + b3DebugDraw draw = _debugDraw.debugDraw(); + draw.drawShapes = false; + b3World_Draw(_worldId, &draw, UINT64_MAX); + _debugDraw.flush(); + } + + swapBuffers(); + _timeline.nextFrame(); + redraw(); +} + +void Box3DExample::keyPressEvent(KeyEvent& event) { + if (event.key() == Key::D) { + if (_drawCubes && _drawDebug) { + _drawDebug = false; + } else if (_drawCubes && !_drawDebug) { + _drawCubes = false; + _drawDebug = true; + } else if (!_drawCubes && _drawDebug) { + _drawCubes = true; + } + } else if (event.key() == Key::S) { + _shootBox ^= true; + } else if (event.key() == Key::Esc) { + exit(); + } else { + return; + } + event.setAccepted(); +} + +void Box3DExample::pointerPressEvent(PointerEvent& event) { + if (!event.isPrimary()) + return; + + /* Right mouse / finger starts orbit drag */ + if (event.pointer() & (Pointer::MouseRight | Pointer::Finger)) { + _dragging = true; + _lastPointerPosition = event.position(); + event.setAccepted(); + return; + } + + /* Left mouse shoots (keeps left free for comfortable orbiting) */ + if (!(event.pointer() & Pointer::MouseLeft)) + return; + + const Vector2 position = event.position() * Vector2{framebufferSize()} / Vector2{windowSize()}; + const Vector2 clickPoint = Vector2::yScale(-1.0f) * + (position / Vector2{framebufferSize()} - Vector2{0.5f}) * + _projectionSize; + const Matrix4 absolute = cameraAbsoluteTransformation(); + const Vector3 direction = + (absolute.rotationScaling() * + Vector3{clickPoint, -1.0f}).normalized(); + + const bool shootBox = _shootBox; + const Body& object = spawnBody( + _bodies, + shootBox ? ShootBoxMass : ShootSphereMass, + _worldId, + shootBox ? &_boxHull : nullptr, + shootBox ? nullptr : &_sphereGeom, + Matrix4::scaling(Vector3{shootBox ? BoxSizeXYZ : SphereRadius}), + shootBox ? 0x880000_rgbf : 0xff4444_rgbf, + absolute.translation()); + + b3Body_SetLinearVelocity(object.id, b3Vec3(direction*ShootSpeed)); + + event.setAccepted(); +} + +void Box3DExample::pointerReleaseEvent(PointerEvent& event) { + if (event.pointer() & (Pointer::MouseRight | Pointer::Finger)) { + _dragging = false; + event.setAccepted(); + } +} + +void Box3DExample::pointerMoveEvent(PointerMoveEvent& event) { + if (!_dragging) + return; + + const Vector2 delta = event.position() - _lastPointerPosition; + _lastPointerPosition = event.position(); + + /* Sensitivity tuned for comfortable orbit */ + constexpr Float sensitivity = 0.3f; + _cameraYaw -= Deg{delta.x() * sensitivity}; + _cameraPitch -= Deg{delta.y() * sensitivity}; + + /* Clamp pitch so we don't flip upside-down (min, max) */ + _cameraPitch = Math::clamp(_cameraPitch, Deg{-89.0f}, Deg{89.0f}); + + event.setAccepted(); +} + +void Box3DExample::scrollEvent(ScrollEvent& event) { + /* Zoom in/out */ + _cameraDistance *= (event.offset().y() > 0 ? 0.9f : 1.1f); + _cameraDistance = Math::clamp(_cameraDistance, MinZoomIn, MaxZoomOut); + event.setAccepted(); +} + +}} + +MAGNUM_APPLICATION_MAIN(Magnum::Examples::Box3DExample) diff --git a/src/box3d/Box3DIntegration/Converters.h b/src/box3d/Box3DIntegration/Converters.h new file mode 100644 index 00000000..46be2754 --- /dev/null +++ b/src/box3d/Box3DIntegration/Converters.h @@ -0,0 +1,61 @@ +#ifndef Magnum_Box3DIntegration_Converters_h +#define Magnum_Box3DIntegration_Converters_h +/* + This file is part of Magnum. + + Copyright © 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018, 2019, + 2020, 2021, 2022, 2023, 2024, 2025, 2026 + Vladimír Vondruš + Copyright © 2026 Igal Alkon + + Permission is hereby granted, free of charge, to any person obtaining a + copy of this software and associated documentation files (the "Software"), + to deal in the Software without restriction, including without limitation + the rights to use, copy, modify, merge, publish, distribute, sublicense, + and/or sell copies of the Software, and to permit persons to whom the + Software is furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included + in all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + DEALINGS IN THE SOFTWARE. +*/ + +#include +#include + +#include + +namespace Magnum { namespace Math { namespace Implementation { + +/* b3Vec3 / b3Pos are the same type in Box3D (b3Pos is an alias). + Fields are float; one specialization covers both names. */ +template<> struct VectorConverter<3, Float, b3Vec3> { + static Vector<3, Float> from(const b3Vec3& other) { + return {other.x, other.y, other.z}; + } + static b3Vec3 to(const Vector<3, Float>& other) { + return {other[0], other[1], other[2]}; + } +}; + +/* b3Quat <-> Quaternion + Box3D stores quaternion as { b3Vec3 v; float s; } (vector part + scalar) */ +template<> struct QuaternionConverter { + static Quaternion from(const b3Quat& other) { + return {{other.v.x, other.v.y, other.v.z}, other.s}; + } + static b3Quat to(const Quaternion& other) { + return {{other.vector().x(), other.vector().y(), other.vector().z()}, other.scalar()}; + } +}; + +}}} + +#endif diff --git a/src/box3d/Box3DIntegration/DebugDraw.cpp b/src/box3d/Box3DIntegration/DebugDraw.cpp new file mode 100644 index 00000000..24dbc3d5 --- /dev/null +++ b/src/box3d/Box3DIntegration/DebugDraw.cpp @@ -0,0 +1,374 @@ +/* + This file is part of Magnum. + + Copyright © 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018, 2019, + 2020, 2021, 2022, 2023, 2024, 2025, 2026 + Vladimír Vondruš + Copyright © 2026 Igal Alkon + + Permission is hereby granted, free of charge, to any person obtaining a + copy of this software and associated documentation files (the "Software"), + to deal in the Software without restriction, including without limitation + the rights to use, copy, modify, merge, publish, distribute, sublicense, + and/or sell copies of the Software, and to permit persons to whom the + Software is furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included + in all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + DEALINGS IN THE SOFTWARE. +*/ + +#include "DebugDraw.h" + +#include +#include +#include +#include + +namespace Magnum { namespace Box3DIntegration { + +using namespace Math::Literals; + +DebugDraw::DebugDraw(const std::size_t initialBufferCapacity) + : _shader{NoCreate}, _buffer{NoCreate}, _mesh{NoCreate} +{ + create(initialBufferCapacity); +} + +DebugDraw::DebugDraw(NoCreateT) noexcept + : _shader{NoCreate}, _buffer{NoCreate}, _mesh{NoCreate} {} + +DebugDraw::~DebugDraw() = default; + +DebugDraw::Color DebugDraw::fromHex(const b3HexColor hex, const float alpha) { + return { + ((hex >> 16) & 0xff) / 255.0f, + ((hex >> 8) & 0xff) / 255.0f, + ( hex & 0xff) / 255.0f, + alpha + }; +} + +Vector3 DebugDraw::fromPos(const b3Pos p) { + /* b3Pos uses double for large-world support */ + return {Float(p.x), Float(p.y), Float(p.z)}; +} + +Matrix4 DebugDraw::fromWorldTransform(const b3WorldTransform& t) { + /* b3WorldTransform = { b3Pos p; b3Quat q; } */ + const Quaternion rot{{Float(t.q.v.x), Float(t.q.v.y), Float(t.q.v.z)}, Float(t.q.s)}; + const Vector3 pos = fromPos(t.p); + return Matrix4::from(rot.toMatrix(), pos); +} + +b3DebugDraw DebugDraw::debugDraw() { + b3DebugDraw d = b3DefaultDebugDraw(); + + d.DrawShapeFcn = drawShapeCallback; + d.DrawSegmentFcn = drawSegmentCallback; + d.DrawTransformFcn = drawTransformCallback; + d.DrawPointFcn = drawPointCallback; + d.DrawSphereFcn = drawSphereCallback; + d.DrawCapsuleFcn = drawCapsuleCallback; + d.DrawBoundsFcn = drawBoundsCallback; + d.DrawBoxFcn = drawBoxCallback; + d.DrawStringFcn = drawStringCallback; + + d.context = this; + + d.drawShapes = true; + d.drawJoints = true; + d.drawJointExtras = false; + d.drawBounds = true; + d.drawMass = false; + d.drawBodyNames = false; + d.drawContacts = false; + d.drawContactNormals = false; + d.drawContactForces = false; + d.drawFrictionForces = false; + d.drawIslands = true; + d.drawGraphColors = false; + d.drawContactFeatures = true; + d.forceScale = 0.35f; + d.jointScale = 1.0f; + + return d; +} + +void DebugDraw::drawAxes(const Matrix4& transformation, const float length) { + const Vector3 o = transformation.translation(); + const Matrix3x3 rot = transformation.rotationScaling(); + + const b3Vec3 origin = {o.x(), o.y(), o.z()}; + const b3Vec3 xAxis = {o.x() + rot[0][0] * length, + o.y() + rot[0][1] * length, + o.z() + rot[0][2] * length}; + const b3Vec3 yAxis = {o.x() + rot[1][0] * length, + o.y() + rot[1][1] * length, + o.z() + rot[1][2] * length}; + const b3Vec3 zAxis = {o.x() + rot[2][0] * length, + o.y() + rot[2][1] * length, + o.z() + rot[2][2] * length}; + + drawSegment(origin, xAxis, {1.0f, 0.0f, 0.0f, 1.0f}); // Red X + drawSegment(origin, yAxis, {0.0f, 1.0f, 0.0f, 1.0f}); // Green Y + drawSegment(origin, zAxis, {0.0f, 0.0f, 1.0f, 1.0f}); // Blue Z +} + +void DebugDraw::drawWireframeBox(const Matrix4& transformation, + const Vector3& halfExtents, + const Color& color) +{ + const Vector3 min = -halfExtents; + const Vector3 max = halfExtents; + + const Vector3 corners[8] = { + {min.x(), min.y(), min.z()}, + {max.x(), min.y(), min.z()}, + {max.x(), max.y(), min.z()}, + {min.x(), max.y(), min.z()}, + {min.x(), min.y(), max.z()}, + {max.x(), min.y(), max.z()}, + {max.x(), max.y(), max.z()}, + {min.x(), max.y(), max.z()}, + }; + + b3Vec3 world[8]; + for(int i = 0; i < 8; ++i) { + const Vector3 w = (transformation * Vector4{corners[i], 1.0f}).xyz(); + world[i] = {w.x(), w.y(), w.z()}; + } + + // Bottom face + drawSegment(world[0], world[1], color); + drawSegment(world[1], world[2], color); + drawSegment(world[2], world[3], color); + drawSegment(world[3], world[0], color); + + // Top face + drawSegment(world[4], world[5], color); + drawSegment(world[5], world[6], color); + drawSegment(world[6], world[7], color); + drawSegment(world[7], world[4], color); + + // Vertical edges + drawSegment(world[0], world[4], color); + drawSegment(world[1], world[5], color); + drawSegment(world[2], world[6], color); + drawSegment(world[3], world[7], color); +} + +void DebugDraw::drawWireframeSphere(const Matrix4& transformation, + const Float radius, + const Color& color) +{ + /* Three great circles (XY, XZ, YZ) give a clear sphere outline. */ + constexpr Int segments = 24; + constexpr Float step = Constants::pi()*2.0f/static_cast(segments); + + auto toWorld = [&](const Vector3& local) { + const Vector3 w = (transformation*Vector4{local, 1.0f}).xyz(); + return b3Vec3{w.x(), w.y(), w.z()}; + }; + + for(Int i = 0; i != segments; ++i) { + const Rad a0{static_cast(i)*step}; + const Rad a1{static_cast(i + 1)*step}; + const Float c0 = Math::cos(a0), s0 = Math::sin(a0); + const Float c1 = Math::cos(a1), s1 = Math::sin(a1); + + /* XY plane */ + drawSegment(toWorld(Vector3{radius*c0, radius*s0, 0.0f}), + toWorld(Vector3{radius*c1, radius*s1, 0.0f}), color); + /* XZ plane */ + drawSegment(toWorld(Vector3{radius*c0, 0.0f, radius*s0}), + toWorld(Vector3{radius*c1, 0.0f, radius*s1}), color); + /* YZ plane */ + drawSegment(toWorld(Vector3{0.0f, radius*c0, radius*s0}), + toWorld(Vector3{0.0f, radius*c1, radius*s1}), color); + } +} + +void DebugDraw::create(const std::size_t initialBufferCapacity) { + _mesh = GL::Mesh{GL::MeshPrimitive::Lines}; + _buffer = GL::Buffer{}; + _shader = Shaders::VertexColorGL3D{}; + + _mesh.addVertexBuffer(_buffer, 0, + Shaders::VertexColorGL3D::Position{}, + Shaders::VertexColorGL3D::Color4{}); + + arrayReserve(_bufferData, initialBufferCapacity); +} + +void DebugDraw::flush() { + if(_bufferData.isEmpty()) return; + + _buffer.setData(_bufferData, GL::BufferUsage::DynamicDraw); + _mesh.setCount(_bufferData.size()); + + _shader.setTransformationProjectionMatrix(_transformationProjectionMatrix) + .draw(_mesh); + + arrayResize(_bufferData, 0); +} + +void DebugDraw::drawSegment(const b3Vec3 p1, const b3Vec3 p2, const Color color) { + const Vector4 c{color.r, color.g, color.b, color.a}; + + arrayAppend(_bufferData, { + Vertex{{p1.x, p1.y, p1.z}, c}, + Vertex{{p2.x, p2.y, p2.z}, c} + }); +} + +void DebugDraw::drawTransform(const b3WorldTransform& xf) { + constexpr float len = 0.5f; + + const Vector3 origin = fromPos(xf.p); + + /* Reconstruct rotation matrix from quaternion */ + const Quaternion q{{(xf.q.v.x), (xf.q.v.y), (xf.q.v.z)}, (xf.q.s)}; + const Matrix3x3 rot = q.toMatrix(); + + const b3Vec3 o = {origin.x(), origin.y(), origin.z()}; + const b3Vec3 xAxis = {o.x + rot[0][0]*len, + o.y + rot[0][1]*len, + o.z + rot[0][2]*len}; + const b3Vec3 yAxis = {o.x + rot[1][0]*len, + o.y + rot[1][1]*len, + o.z + rot[1][2]*len}; + const b3Vec3 zAxis = {o.x + rot[2][0]*len, + o.y + rot[2][1]*len, + o.z + rot[2][2]*len}; + + drawSegment(o, xAxis, {1.0f, 0.0f, 0.0f, 1.0f}); // Red X + drawSegment(o, yAxis, {0.0f, 1.0f, 0.0f, 1.0f}); // Green Y + drawSegment(o, zAxis, {0.0f, 0.0f, 1.0f, 1.0f}); // Blue Z +} + + void DebugDraw::drawPoint(const b3Vec3 p, const float size, const Color color) { + const float s = size * 0.55f; + const float s2 = s * 0.65f; + const float d = s2 * 0.70710678f; // 1/√2 + + // main cross + drawSegment({p.x-s, p.y, p.z}, {p.x+s, p.y, p.z}, color); + drawSegment({p.x, p.y-s, p.z}, {p.x, p.y+s, p.z}, color); + drawSegment({p.x, p.y, p.z-s}, {p.x, p.y, p.z+s}, color); + + // 45° secondary cross → star / diamond look + drawSegment({p.x-d, p.y-d, p.z}, {p.x+d, p.y+d, p.z}, color); + drawSegment({p.x-d, p.y+d, p.z}, {p.x+d, p.y-d, p.z}, color); + drawSegment({p.x-d, p.y, p.z-d}, {p.x+d, p.y, p.z+d}, color); + drawSegment({p.x-d, p.y, p.z+d}, {p.x+d, p.y, p.z-d}, color); + drawSegment({p.x, p.y-d, p.z-d}, {p.x, p.y+d, p.z+d}, color); + drawSegment({p.x, p.y-d, p.z+d}, {p.x, p.y+d, p.z-d}, color); +} + +/* ==== Static Callbacks ==================================================== */ + +void DebugDraw::drawSegmentCallback(const b3Pos p1, const b3Pos p2, const b3HexColor color, void* context) { + auto* dd = static_cast(context); + const b3Vec3 a{Float(p1.x), Float(p1.y), Float(p1.z)}; + const b3Vec3 b{Float(p2.x), Float(p2.y), Float(p2.z)}; + const Color c = fromHex(color); + + dd->drawSegment(a, b, c); +} + +void DebugDraw::drawTransformCallback(b3WorldTransform transform, void* context) { + auto* dd = static_cast(context); + dd->drawTransform(transform); +} + + void DebugDraw::drawPointCallback(const b3Pos p, const float size, const b3HexColor color, void* context) { + auto* dd = static_cast(context); + + Color c = fromHex(color); + + // brighten + c.r = Math::min(1.0f, c.r * 1.25f + 0.15f); + c.g = Math::min(1.0f, c.g * 1.25f + 0.15f); + c.b = Math::min(1.0f, c.b * 1.25f + 0.15f); + c.a = 1.0f; + + const float visualSize = Math::max(size * 1.8f, 0.12f); + dd->drawPoint({Float(p.x), Float(p.y), Float(p.z)}, visualSize, c); +} + +void DebugDraw::drawSphereCallback(const b3Pos p, const float radius, const b3HexColor color, const float alpha, void* context) { + auto* dd = static_cast(context); + const Matrix4 t = Matrix4::translation(fromPos(p)); + dd->drawWireframeSphere(t, radius, fromHex(color, alpha)); +} + +void DebugDraw::drawCapsuleCallback(const b3Pos p1, const b3Pos p2, const float radius, const b3HexColor color, const float alpha, void* context) { + auto* dd = static_cast(context); + const Color c = fromHex(color, alpha); + const Vector3 a = fromPos(p1); + const Vector3 b = fromPos(p2); + + /* Simple wireframe: two spheres + a few longitudinal lines */ + dd->drawWireframeSphere(Matrix4::translation(a), radius, c); + dd->drawWireframeSphere(Matrix4::translation(b), radius, c); + + const Vector3 dir = (b - a).normalized(); + /* Three meridians */ + Vector3 u = Math::cross(dir, Vector3::yAxis()); + if(u.dot() < 1.0e-6f) + u = Math::cross(dir, Vector3::xAxis()); + u = u.normalized()*radius; + const Vector3 v = Math::cross(dir, u); + + for(int i = 0; i < 4; ++i) { + const Float ang = static_cast(i)*Constants::pi()*0.5f; + const Vector3 offset = u*Math::cos(Rad{ang}) + v*Math::sin(Rad{ang}); + dd->drawSegment( + {a.x()+offset.x(), a.y()+offset.y(), a.z()+offset.z()}, + {b.x()+offset.x(), b.y()+offset.y(), b.z()+offset.z()}, + c); + } +} + +void DebugDraw::drawBoundsCallback(b3AABB aabb, b3HexColor color, void* context) { + auto* dd = static_cast(context); + const Vector3 min{(aabb.lowerBound.x), (aabb.lowerBound.y), (aabb.lowerBound.z)}; + const Vector3 max{(aabb.upperBound.x), (aabb.upperBound.y), (aabb.upperBound.z)}; + const Vector3 center = (min + max)*0.5f; + const Vector3 half = (max - min)*0.5f; + dd->drawWireframeBox(Matrix4::translation(center), half, fromHex(color)); +} + +void DebugDraw::drawBoxCallback(const b3Vec3 extents, b3WorldTransform transform, const b3HexColor color, void* context) { + auto* dd = static_cast(context); + const Matrix4 t = fromWorldTransform(transform); + const Vector3 half{(extents.x), (extents.y), (extents.z)}; + dd->drawWireframeBox(t, half, fromHex(color)); +} + +void DebugDraw::drawStringCallback(b3Pos, const char*, b3HexColor, void*) { + /* Text rendering is left to the application */ +} + +bool DebugDraw::drawShapeCallback(void* /*userShape*/, b3WorldTransform transform, + b3HexColor /*color*/, void* context) { + /* When createDebugShape / destroyDebugShape are not used, Box3D still + calls DrawShapeFcn for every shape. We fall back to drawing a small + transform (axes) so the user at least sees body locations. For full + solid/wire shape rendering, the application should supply the + creation/destruction callbacks and store Magnum meshes as userShape. */ + auto* dd = static_cast(context); + dd->drawTransform(transform); + /* Returning true continues drawing subsequent shapes */ + return true; +} + +}} diff --git a/src/box3d/Box3DIntegration/DebugDraw.h b/src/box3d/Box3DIntegration/DebugDraw.h new file mode 100644 index 00000000..4a28396a --- /dev/null +++ b/src/box3d/Box3DIntegration/DebugDraw.h @@ -0,0 +1,134 @@ +#ifndef Magnum_Box3DIntegration_DebugDraw_h +#define Magnum_Box3DIntegration_DebugDraw_h +/* + This file is part of Magnum. + + Copyright © 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018, 2019, + 2020, 2021, 2022, 2023, 2024, 2025, 2026 + Vladimír Vondruš + Copyright © 2026 Igal Alkon + + Permission is hereby granted, free of charge, to any person obtaining a + copy of this software and associated documentation files (the "Software"), + to deal in the Software without restriction, including without limitation + the rights to use, copy, modify, merge, publish, distribute, sublicense, + and/or sell copies of the Software, and to permit persons to whom the + Software is furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included + in all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + DEALINGS IN THE SOFTWARE. +*/ + +#include + +#include +#include +#include +#include + +#include + +namespace Magnum { namespace Box3DIntegration { +/** + * @brief Utility class for rendering debug geometry, lines, shapes and overlays. + */ +class DebugDraw { +public: + struct Color { + float r, g, b, a; + }; + + explicit DebugDraw(std::size_t initialBufferCapacity = 2048); + explicit DebugDraw(NoCreateT) noexcept; + + DebugDraw(const DebugDraw&) = delete; + DebugDraw& operator=(const DebugDraw&) = delete; + ~DebugDraw(); + + /** @brief Set the combined projection * view matrix used for drawing */ + DebugDraw& setTransformationProjectionMatrix(const Matrix4& matrix) { + _transformationProjectionMatrix = matrix; + return *this; + } + + /** + * @brief Builds and returns a b3DebugDraw instance configured with this + * object's drawing callbacks and default debug visualization flags. + * + * @return Configured b3DebugDraw ready for use with the physics world draw API. + */ + b3DebugDraw debugDraw(); + + /** + * @brief Draws RGB coordinate axes at the given transformation. + * @param transformation World transform defining the origin and orientation of the axes. + * @param length Length of each axis segment. + */ + void drawAxes(const Matrix4& transformation, float length = 0.5f); + + /** + * @brief Draws a wireframe box with the specified transform, size, and color. + * @param transformation World transform defining position and orientation of the box. + * @param halfExtents Half-extents of the box along each axis from its center. + * @param color Color used for the wireframe edges. + */ + void drawWireframeBox(const Matrix4& transformation, + const Vector3& halfExtents, + const Color& color = {0.7f, 0.7f, 0.7f, 1.0f}); + + /** + * @brief Draws a wireframe sphere using three great-circle outlines in the XY, XZ, and YZ planes. + * @param transformation World transform defining the position and orientation of the sphere. + * @param radius Radius of the sphere. + * @param color Color used for the wireframe edges. + */ + void drawWireframeSphere(const Matrix4& transformation, + Float radius, + const Color& color = {0.7f, 0.7f, 0.7f, 1.0f}); + + void create(std::size_t initialBufferCapacity = 2048); + void flush(); + +private: + struct Vertex { + Vector3 position; + Vector4 color; + }; + + /* Static callbacks matching b3DebugDraw function pointers */ + static void drawSegmentCallback(b3Pos p1, b3Pos p2, b3HexColor color, void* context); + static void drawTransformCallback(b3WorldTransform transform, void* context); + static void drawPointCallback(b3Pos p, float size, b3HexColor color, void* context); + static void drawSphereCallback(b3Pos p, float radius, b3HexColor color, float alpha, void* context); + static void drawCapsuleCallback(b3Pos p1, b3Pos p2, float radius, b3HexColor color, float alpha, void* context); + static void drawBoundsCallback(b3AABB aabb, b3HexColor color, void* context); + static void drawBoxCallback(b3Vec3 extents, b3WorldTransform transform, b3HexColor color, void* context); + static void drawStringCallback(b3Pos p, const char* s, b3HexColor color, void* context); + static bool drawShapeCallback(void* userShape, b3WorldTransform transform, b3HexColor color, void* context); + + void drawSegment(b3Vec3 p1, b3Vec3 p2, Color color); + void drawTransform(const b3WorldTransform& xf); + void drawPoint(b3Vec3 p, float size, Color color); + + static Color fromHex(b3HexColor hex, float alpha = 1.0f); + static Vector3 fromPos(b3Pos p); + static Matrix4 fromWorldTransform(const b3WorldTransform& t); + + Matrix4 _transformationProjectionMatrix; + Shaders::VertexColorGL3D _shader; + GL::Buffer _buffer; + GL::Mesh _mesh; + Containers::Array _bufferData; +}; + +}} + +#endif diff --git a/src/box3d/CMakeLists.txt b/src/box3d/CMakeLists.txt new file mode 100644 index 00000000..e970da9b --- /dev/null +++ b/src/box3d/CMakeLists.txt @@ -0,0 +1,85 @@ +# +# This file is part of Magnum. +# +# Original authors — credit is appreciated but not required: +# +# 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018, 2019, +# 2020, 2021, 2022, 2023, 2024, 2025 +# — Vladimír Vondruš +# 2026 — Igal Alkon +# +# This is free and unencumbered software released into the public domain. +# +# Anyone is free to copy, modify, publish, use, compile, sell, or distribute +# this software, either in source code form or as a compiled binary, for any +# purpose, commercial or non-commercial, and by any means. +# +# In jurisdictions that recognize copyright laws, the author or authors of +# this software dedicate any and all copyright interest in the software to +# the public domain. We make this dedication for the benefit of the public +# at large and to the detriment of our heirs and successors. We intend this +# dedication to be an overt act of relinquishment in perpetuity of all +# present and future rights to this software under copyright law. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER +# IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN +# CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +# + +cmake_minimum_required(VERSION 3.14) + +project(MagnumBox3DExample CXX) + +# Add module path in case this is project root +if(PROJECT_SOURCE_DIR STREQUAL CMAKE_SOURCE_DIR) + set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/../../modules/" ${CMAKE_MODULE_PATH}) +endif() + +find_package(Corrade REQUIRED Main) +find_package(Magnum REQUIRED + GL + MeshTools + Primitives + SceneGraph + Sdl2Application + Shaders + Trade) + +# Box3D integration (as recommended in https://github.com/erincatto/box3d) +# Uses FetchContent for a self-contained build. Works both standalone and when +# this example is added as a subdirectory (guarded to avoid redeclaration). +if(NOT TARGET box3d::box3d) + include(FetchContent) + FetchContent_Declare(box3d + GIT_REPOSITORY https://github.com/erincatto/box3d.git + GIT_TAG v0.1.0) + FetchContent_MakeAvailable(box3d) +endif() + +# Alternative (after `cmake --install` of Box3D): +# find_package(box3d 0.1 REQUIRED) + +set_directory_properties(PROPERTIES CORRADE_USE_PEDANTIC_FLAGS ON) + +add_executable(magnum-box3d Box3DExample.cpp Box3DIntegration/DebugDraw.cpp) + +target_link_libraries(magnum-box3d PRIVATE + Corrade::Main + Magnum::Application + Magnum::GL + Magnum::Magnum + Magnum::MeshTools + Magnum::Primitives + Magnum::SceneGraph + Magnum::Shaders + Magnum::Trade + box3d::box3d +) + +install(TARGETS magnum-box3d DESTINATION ${MAGNUM_BINARY_INSTALL_DIR}) + +# Make the executable a default target to build & run in Visual Studio +set_property(DIRECTORY ${PROJECT_SOURCE_DIR} PROPERTY VS_STARTUP_PROJECT magnum-box3d)