diff --git a/include/omath/collision/physx_rigid_body.hpp b/include/omath/collision/physx_rigid_body.hpp new file mode 100644 index 0000000..41a0cb1 --- /dev/null +++ b/include/omath/collision/physx_rigid_body.hpp @@ -0,0 +1,137 @@ +// +// Created by orange-cpp +// +#pragma once + +#ifdef OMATH_ENABLE_PHYSX + +#include "collider_interface.hpp" +#include "physx_world.hpp" +#include +#include +#include + +namespace omath::collision +{ + /// Dynamic rigid body backed by a PhysX PxRigidDynamic actor. + /// Implements ColliderInterface so it can participate in both omath GJK + /// and PhysX simulation-based collision resolution. + /// + /// Ownership: the actor is added to the world's scene on construction + /// and removed + released on destruction. + class PhysXRigidBody final : public ColliderInterface> + { + public: + /// @param world PhysXWorld that owns the scene. + /// @param geometry Shape geometry (PxBoxGeometry, PxSphereGeometry, …). + /// @param origin Initial world-space position. + /// @param density Mass density used to compute mass and inertia. + PhysXRigidBody(PhysXWorld& world, const physx::PxGeometry& geometry, + const VectorType& origin = {}, float density = 1.f) + : m_world(world) + , m_geometry(geometry) + { + const physx::PxTransform pose(physx::PxVec3(origin.x, origin.y, origin.z)); + m_actor = world.get_physics().createRigidDynamic(pose); + + physx::PxShape* shape = world.get_physics().createShape( + geometry, world.get_default_material(), true); + m_actor->attachShape(*shape); + shape->release(); + + physx::PxRigidBodyExt::updateMassAndInertia(*m_actor, density); + world.get_scene().addActor(*m_actor); + } + + ~PhysXRigidBody() override + { + m_world.get_scene().removeActor(*m_actor); + m_actor->release(); + } + + PhysXRigidBody(const PhysXRigidBody&) = delete; + PhysXRigidBody& operator=(const PhysXRigidBody&) = delete; + + // ── ColliderInterface ──────────────────────────────────────────────── + + /// Support function — delegates to the stored geometry type so the body + /// can be used with omath GJK alongside the non-simulated colliders. + [[nodiscard]] + VectorType find_abs_furthest_vertex_position(const VectorType& direction) const override + { + const VectorType o = get_origin(); + switch (m_geometry.getType()) + { + case physx::PxGeometryType::eBOX: + { + const auto& he = m_geometry.box().halfExtents; + return { + o.x + (direction.x >= 0.f ? he.x : -he.x), + o.y + (direction.y >= 0.f ? he.y : -he.y), + o.z + (direction.z >= 0.f ? he.z : -he.z), + }; + } + case physx::PxGeometryType::eSPHERE: + { + const float r = m_geometry.sphere().radius; + const float len = std::sqrt(direction.x * direction.x + + direction.y * direction.y + + direction.z * direction.z); + if (len == 0.f) + return o; + const float inv = r / len; + return { o.x + direction.x * inv, + o.y + direction.y * inv, + o.z + direction.z * inv }; + } + default: + return o; // unsupported geometry — return centre + } + } + + [[nodiscard]] + const VectorType& get_origin() const override + { + const auto& p = m_actor->getGlobalPose().p; + m_cached_origin = { p.x, p.y, p.z }; + return m_cached_origin; + } + + void set_origin(const VectorType& new_origin) override + { + physx::PxTransform pose = m_actor->getGlobalPose(); + pose.p = physx::PxVec3(new_origin.x, new_origin.y, new_origin.z); + m_actor->setGlobalPose(pose); + } + + // ── PhysX-specific API ─────────────────────────────────────────────── + + [[nodiscard]] physx::PxRigidDynamic& get_actor() { return *m_actor; } + [[nodiscard]] const physx::PxRigidDynamic& get_actor() const { return *m_actor; } + + void set_linear_velocity(const VectorType& v) + { + m_actor->setLinearVelocity(physx::PxVec3(v.x, v.y, v.z)); + } + + [[nodiscard]] + VectorType get_linear_velocity() const + { + const auto& v = m_actor->getLinearVelocity(); + return { v.x, v.y, v.z }; + } + + void set_kinematic(bool enabled) + { + m_actor->setRigidBodyFlag(physx::PxRigidBodyFlag::eKINEMATIC, enabled); + } + + private: + PhysXWorld& m_world; + physx::PxGeometryHolder m_geometry; + physx::PxRigidDynamic* m_actor{nullptr}; + mutable VectorType m_cached_origin{}; + }; +} // namespace omath::collision + +#endif // OMATH_ENABLE_PHYSX diff --git a/include/omath/collision/physx_world.hpp b/include/omath/collision/physx_world.hpp new file mode 100644 index 0000000..cb422d5 --- /dev/null +++ b/include/omath/collision/physx_world.hpp @@ -0,0 +1,82 @@ +// +// Created by orange-cpp +// +#pragma once + +#ifdef OMATH_ENABLE_PHYSX + +#include + +namespace omath::collision +{ + /// RAII owner of a PhysX Foundation + Physics + Scene. + /// One world per simulation context; not copyable or movable. + class PhysXWorld final + { + public: + explicit PhysXWorld(physx::PxVec3 gravity = {0.f, -9.81f, 0.f}, + physx::PxU32 cpu_threads = 2) + { + m_foundation = PxCreateFoundation(PX_PHYSICS_VERSION, m_allocator, m_error_callback); + + m_physics = PxCreatePhysics(PX_PHYSICS_VERSION, *m_foundation, + physx::PxTolerancesScale{}); + + physx::PxSceneDesc desc(m_physics->getTolerancesScale()); + desc.gravity = gravity; + desc.cpuDispatcher = physx::PxDefaultCpuDispatcherCreate(cpu_threads); + m_dispatcher = static_cast(desc.cpuDispatcher); + desc.filterShader = physx::PxDefaultSimulationFilterShader; + + m_scene = m_physics->createScene(desc); + + // Default material: static friction 0.5, dynamic friction 0.5, restitution 0. + m_default_material = m_physics->createMaterial(0.5f, 0.5f, 0.f); + } + + ~PhysXWorld() + { + m_scene->release(); + m_dispatcher->release(); + m_default_material->release(); + m_physics->release(); + m_foundation->release(); + } + + PhysXWorld(const PhysXWorld&) = delete; + PhysXWorld& operator=(const PhysXWorld&) = delete; + + /// Advance the simulation by @p dt seconds and block until results are ready. + void step(float dt) + { + m_scene->simulate(dt); + m_scene->fetchResults(true); + } + + [[nodiscard]] physx::PxPhysics& get_physics() { return *m_physics; } + [[nodiscard]] physx::PxScene& get_scene() { return *m_scene; } + [[nodiscard]] physx::PxMaterial& get_default_material() { return *m_default_material; } + + /// Add an infinite static ground plane at y = @p y_level facing +Y. + physx::PxRigidStatic* add_ground_plane(float y_level = 0.f) + { + physx::PxRigidStatic* plane = PxCreatePlane( + *m_physics, + physx::PxPlane(0.f, 1.f, 0.f, -y_level), + *m_default_material); + m_scene->addActor(*plane); + return plane; + } + + private: + physx::PxDefaultAllocator m_allocator{}; + physx::PxDefaultErrorCallback m_error_callback{}; + physx::PxFoundation* m_foundation{nullptr}; + physx::PxPhysics* m_physics{nullptr}; + physx::PxDefaultCpuDispatcher* m_dispatcher{nullptr}; + physx::PxScene* m_scene{nullptr}; + physx::PxMaterial* m_default_material{nullptr}; + }; +} // namespace omath::collision + +#endif // OMATH_ENABLE_PHYSX diff --git a/tests/general/unit_test_physx_colliders.cpp b/tests/general/unit_test_physx_colliders.cpp index 652461a..44f492c 100644 --- a/tests/general/unit_test_physx_colliders.cpp +++ b/tests/general/unit_test_physx_colliders.cpp @@ -6,7 +6,9 @@ #include #include #include +#include #include +#include using namespace omath::collision; using omath::Vector3; @@ -253,4 +255,87 @@ TEST(PhysXSphereGjk, DifferentRadiiNotColliding) EXPECT_FALSE(GjkSphere::is_collide(a, b)); } +// ─── PhysX simulation-based collision resolution ───────────────────────────── + +// Helper: step the world N times with a fixed dt. +static void step_n(omath::collision::PhysXWorld& world, int n, float dt = 1.f / 60.f) +{ + for (int i = 0; i < n; ++i) + world.step(dt); +} + +TEST(PhysXSimulation, BoxFallsAndStopsOnGround) +{ + // A box dropped from y=5 should come to rest at y≈0.5 (half-extent) above the ground plane. + omath::collision::PhysXWorld world; + world.add_ground_plane(0.f); + + omath::collision::PhysXRigidBody box(world, physx::PxBoxGeometry(0.5f, 0.5f, 0.5f), + {0.f, 5.f, 0.f}); + + step_n(world, 300); // ~5 simulated seconds + + EXPECT_NEAR(box.get_origin().y, 0.5f, 0.05f); +} + +TEST(PhysXSimulation, SphereFallsAndStopsOnGround) +{ + // A sphere of radius 1 dropped from y=5 should rest at y≈1. + omath::collision::PhysXWorld world; + world.add_ground_plane(0.f); + + omath::collision::PhysXRigidBody sphere(world, physx::PxSphereGeometry(1.f), + {0.f, 5.f, 0.f}); + + step_n(world, 300); + + EXPECT_NEAR(sphere.get_origin().y, 1.f, 0.05f); +} + +TEST(PhysXSimulation, TwoBoxesCollideSeparate) +{ + // Two boxes launched toward each other — after collision they must be + // further apart than their combined half-extents (no overlap). + omath::collision::PhysXWorld world({0.f, 0.f, 0.f}); // no gravity + + omath::collision::PhysXRigidBody left (world, physx::PxBoxGeometry(0.5f, 0.5f, 0.5f), {-3.f, 0.f, 0.f}); + omath::collision::PhysXRigidBody right(world, physx::PxBoxGeometry(0.5f, 0.5f, 0.5f), { 3.f, 0.f, 0.f}); + + left.set_linear_velocity({ 5.f, 0.f, 0.f}); + right.set_linear_velocity({-5.f, 0.f, 0.f}); + + step_n(world, 120); // 2 simulated seconds + + const float distance = right.get_origin().x - left.get_origin().x; + // Boxes must not be overlapping (combined extents = 1.0). + EXPECT_GE(distance, 1.0f); +} + +TEST(PhysXSimulation, BoxGetOriginMatchesSetOrigin) +{ + // Kinematic teleport — set_origin must immediately reflect in get_origin. + omath::collision::PhysXWorld world; + omath::collision::PhysXRigidBody box(world, physx::PxBoxGeometry(1.f, 1.f, 1.f)); + box.set_kinematic(true); + + box.set_origin({7.f, 3.f, -2.f}); + + EXPECT_NEAR(box.get_origin().x, 7.f, 1e-4f); + EXPECT_NEAR(box.get_origin().y, 3.f, 1e-4f); + EXPECT_NEAR(box.get_origin().z, -2.f, 1e-4f); +} + +TEST(PhysXSimulation, BoxFallsUnderGravity) +{ + // Without a floor, a box should be lower after simulation than its start. + omath::collision::PhysXWorld world; // default gravity -9.81 Y + omath::collision::PhysXRigidBody box(world, physx::PxBoxGeometry(0.5f, 0.5f, 0.5f), + {0.f, 10.f, 0.f}); + + const float y_start = box.get_origin().y; + step_n(world, 60); // 1 simulated second + + EXPECT_LT(box.get_origin().y, y_start); +} + #endif // OMATH_ENABLE_PHYSX