From 3b5db2c32cdca361c5f7c5dc0e807131b682a978 Mon Sep 17 00:00:00 2001 From: ecker Date: Mon, 1 Sep 2025 14:51:46 -0500 Subject: [PATCH] finally it works (to-do: support for reading from fp16/bf16/quantized vertex data, some method of emitting contact information to the colliding bodies, collision masking, per-body gravity, fix sinking occuring when using the collider offset, some other things) --- Makefile | 6 +- bin/data/entities/player.json | 18 +- engine/inc/uf/utils/math/physics/impl.h | 25 +- engine/src/engine/object/behavior.cpp | 13 +- engine/src/utils/math/physics/aabb.inl | 13 +- engine/src/utils/math/physics/helpers.inl | 121 ++- engine/src/utils/math/physics/impl.cpp | 57 +- engine/src/utils/math/physics/integration.inl | 10 - engine/src/utils/math/physics/mesh.inl | 7 +- engine/src/utils/math/physics/plane.inl | 4 +- engine/src/utils/math/physics/ray.inl | 37 +- engine/src/utils/math/physics/tests.inl | 890 ++++++++++++------ engine/src/utils/math/physics/triangle.inl | 217 ++++- 13 files changed, 962 insertions(+), 456 deletions(-) diff --git a/Makefile b/Makefile index 19a4ffdb..b7379926 100644 --- a/Makefile +++ b/Makefile @@ -66,7 +66,7 @@ ifneq (,$(findstring win64,$(ARCH))) REQ_DEPS += meshoptimizer toml xatlas curl ffx:fsr dc:texconv # vall_e cpptrace # openvr # ncurses draco discord bullet ultralight-ux FLAGS += -march=native -g # -flto # -g endif - REQ_DEPS += $(RENDERER) json:nlohmann zlib luajit reactphysics simd ctti gltf imgui fmt freetype openal ogg wav + REQ_DEPS += $(RENDERER) json:nlohmann zlib luajit simd ctti gltf imgui fmt freetype openal ogg wav # reactphysics FLAGS += -DUF_ENV_WINDOWS -DUF_ENV_WIN64 -DWIN32_LEAN_AND_MEAN DEPS += -lgdi32 -ldwmapi LINKS += #-Wl,-subsystem,windows @@ -76,13 +76,13 @@ else ifneq (,$(findstring linux,$(ARCH))) REQ_DEPS += toml xatlas curl dc:texconv # meshoptimizer ffx:fsr cpptrace vall_e # ncurses openvr draco discord bullet ultralight-ux FLAGS += -march=native -g # -flto # -g endif - REQ_DEPS += $(RENDERER) json:nlohmann zlib luajit reactphysics simd ctti gltf imgui fmt freetype openal ogg wav + REQ_DEPS += $(RENDERER) json:nlohmann zlib luajit simd ctti gltf imgui fmt freetype openal ogg wav # reactphysics FLAGS += -DUF_ENV_LINUX -fPIC DEPS += -pthread -ldl -lX11 -lXrandr INCS := -I./dep/master/include $(INCS) else ifneq (,$(findstring dreamcast,$(ARCH))) FLAGS += -DUF_ENV_DREAMCAST # -DUF_LEAN_AND_MEAN # this apparently crashes - REQ_DEPS += opengl gldc json:nlohmann zlib lua reactphysics simd ctti fmt freetype openal aldc ogg wav png # imgui + REQ_DEPS += opengl gldc json:nlohmann zlib lua simd ctti fmt freetype openal aldc ogg wav png # imgui # reactphysics INCS := -I./dep/dreamcast/include $(INCS) endif diff --git a/bin/data/entities/player.json b/bin/data/entities/player.json index 87482c0b..32d23af1 100644 --- a/bin/data/entities/player.json +++ b/bin/data/entities/player.json @@ -77,18 +77,16 @@ "gravity": [ 0, -9.81, 0 ], "inertia": [ 0, 0, 0 ], - "type": "sphere", - "radius": 2, + "offset": [ 0, 0, 0 ], - // "type": "capsule", - // "radius": 1, - // "height": 2, - - // "type": "bounding box", - // "min": [ -1.0, -2.0, -1.0 ], - // "max": [ 1.0, 0.0, 1.0 ], + // "type": "sphere", + // "radius": 2, + + "type": "capsule", + "radius": 1, + "height": 2, - "mass": 1, + "mass": 100, "friction": 0.95, "restitution": 0.0, diff --git a/engine/inc/uf/utils/math/physics/impl.h b/engine/inc/uf/utils/math/physics/impl.h index 1f42452f..e454315c 100644 --- a/engine/inc/uf/utils/math/physics/impl.h +++ b/engine/inc/uf/utils/math/physics/impl.h @@ -179,19 +179,20 @@ namespace uf { void UF_API applyRotation( pod::RigidBody& body, const pod::Quaternion<>& q ); void UF_API applyRotation( pod::RigidBody& body, const pod::Vector3f& axis, float angle ); - pod::RigidBody& UF_API create( uf::Object&, float mass = 0.0f ); - pod::RigidBody& UF_API create( uf::Object& object, const pod::AABB& aabb, float mass = 0 ); - pod::RigidBody& UF_API create( uf::Object& object, const pod::Sphere& sphere, float mass = 0 ); - pod::RigidBody& UF_API create( uf::Object& object, const pod::Plane& plane, float mass = 0 ); - pod::RigidBody& UF_API create( uf::Object& object, const pod::Capsule& capsule, float mass = 0 ); - pod::RigidBody& UF_API create( uf::Object&, const uf::Mesh& mesh, float mass = 0 ); + pod::RigidBody& UF_API create( uf::Object&, float mass = 0.0f, const pod::Vector3f& = {} ); + pod::RigidBody& UF_API create( uf::Object& object, const pod::AABB& aabb, float mass = 0.0f, const pod::Vector3f& = {} ); + pod::RigidBody& UF_API create( uf::Object& object, const pod::Sphere& sphere, float mass = 0.0f, const pod::Vector3f& = {} ); + pod::RigidBody& UF_API create( uf::Object& object, const pod::Plane& plane, float mass = 0.0f, const pod::Vector3f& = {} ); + pod::RigidBody& UF_API create( uf::Object& object, const pod::Capsule& capsule, float mass = 0.0f, const pod::Vector3f& = {} ); + pod::RigidBody& UF_API create( uf::Object&, const uf::Mesh& mesh, float mass = 0.0f, const pod::Vector3f& = {} ); - pod::RigidBody& UF_API create( pod::World&, uf::Object&, float mass = 0.0f ); - pod::RigidBody& UF_API create( pod::World&, uf::Object& object, const pod::AABB& aabb, float mass = 0 ); - pod::RigidBody& UF_API create( pod::World&, uf::Object& object, const pod::Sphere& sphere, float mass = 0 ); - pod::RigidBody& UF_API create( pod::World&, uf::Object& object, const pod::Plane& plane, float mass = 0 ); - pod::RigidBody& UF_API create( pod::World&, uf::Object& object, const pod::Capsule& capsule, float mass = 0 ); - pod::RigidBody& UF_API create( pod::World&, uf::Object&, const uf::Mesh& mesh, float mass = 0 ); + pod::RigidBody& UF_API create( pod::World&, uf::Object&, float mass = 0.0f, const pod::Vector3f& = {} ); + pod::RigidBody& UF_API create( pod::World&, uf::Object& object, const pod::AABB& aabb, float mass = 0.0f, const pod::Vector3f& = {} ); + pod::RigidBody& UF_API create( pod::World&, uf::Object& object, const pod::Sphere& sphere, float mass = 0.0f, const pod::Vector3f& = {} ); + pod::RigidBody& UF_API create( pod::World&, uf::Object& object, const pod::Plane& plane, float mass = 0.0f, const pod::Vector3f& = {} ); + pod::RigidBody& UF_API create( pod::World&, uf::Object& object, const pod::Capsule& capsule, float mass = 0.0f, const pod::Vector3f& = {} ); + pod::RigidBody& UF_API create( pod::World&, uf::Object& object, const pod::TriangleWithNormal& tri, float mass = 0.0f, const pod::Vector3f& = {} ); + pod::RigidBody& UF_API create( pod::World&, uf::Object&, const uf::Mesh& mesh, float mass = 0.0f, const pod::Vector3f& = {} ); void UF_API destroy( uf::Object& ); void UF_API destroy( pod::RigidBody& ); diff --git a/engine/src/engine/object/behavior.cpp b/engine/src/engine/object/behavior.cpp index 3487f95c..26006201 100644 --- a/engine/src/engine/object/behavior.cpp +++ b/engine/src/engine/object/behavior.cpp @@ -163,21 +163,21 @@ void uf::ObjectBehavior::initialize( uf::Object& self ) { pod::Vector3f min = uf::vector::decode( metadataJsonPhysics["min"], pod::Vector3f{-0.5f, -0.5f, -0.5f} ); pod::Vector3f max = uf::vector::decode( metadataJsonPhysics["max"], pod::Vector3f{0.5f, 0.5f, 0.5f} ); - uf::physics::impl::create( self, pod::AABB{ .min = min, .max = max }, mass ); + uf::physics::impl::create( self, pod::AABB{ .min = min, .max = max }, mass, offset ); } else if ( type == "plane" ) { pod::Vector3f direction = uf::vector::decode( metadataJsonPhysics["direction"], pod::Vector3f{} ); - float offset = metadataJsonPhysics["offset"].as(); + float o = metadataJsonPhysics["offset"].as(); - uf::physics::impl::create( self, pod::Plane{ direction, offset }, mass ); + uf::physics::impl::create( self, pod::Plane{ direction, o }, mass, offset ); } else if ( type == "sphere" ) { float radius = metadataJsonPhysics["radius"].as(); - uf::physics::impl::create( self, pod::Sphere{ radius }, mass ); + uf::physics::impl::create( self, pod::Sphere{ radius }, mass, offset ); } else if ( type == "capsule" ) { float radius = metadataJsonPhysics["radius"].as(); float height = metadataJsonPhysics["height"].as(); - uf::physics::impl::create( self, pod::Capsule{ radius, height * 0.5f }, mass ); + uf::physics::impl::create( self, pod::Capsule{ radius, height * 0.5f }, mass, offset ); } if ( this->hasComponent() ) { @@ -187,9 +187,6 @@ void uf::ObjectBehavior::initialize( uf::Object& self ) { body.inertiaTensor = { FLT_MAX, FLT_MAX, FLT_MAX }; body.inverseInertiaTensor = { 0.0f, 0.0f, 0.0f }; } - - // body.transform.position = offset; - body.offset = offset; } #endif } diff --git a/engine/src/utils/math/physics/aabb.inl b/engine/src/utils/math/physics/aabb.inl index 444f6b5e..7836e90b 100644 --- a/engine/src/utils/math/physics/aabb.inl +++ b/engine/src/utils/math/physics/aabb.inl @@ -1,8 +1,15 @@ namespace { bool aabbOverlap( const pod::AABB& a, const pod::AABB& b, float eps ) { - return (a.min.x < b.max.x - eps && a.max.x > b.min.x + eps) && - (a.min.y < b.max.y - eps && a.max.y > b.min.y + eps) && - (a.min.z < b.max.z - eps && a.max.z > b.min.z + eps); + for (int axis=0; axis<3; ++axis) { + if (a.max[axis] + eps < b.min[axis] - eps) return false; + if (a.min[axis] - eps > b.max[axis] + eps) return false; + } + return true; + /* + return !(a.max.x < b.min.x + eps || a.min.x > b.max.x - eps || + a.max.y < b.min.y + eps || a.min.y > b.max.y - eps || + a.max.z < b.min.z + eps || a.min.z > b.max.z - eps); + */ } pod::AABB computeSegmentAABB( const pod::Vector3f& p1, const pod::Vector3f p2, float r ) { diff --git a/engine/src/utils/math/physics/helpers.inl b/engine/src/utils/math/physics/helpers.inl index c2902079..8a7c4e97 100644 --- a/engine/src/utils/math/physics/helpers.inl +++ b/engine/src/utils/math/physics/helpers.inl @@ -223,49 +223,122 @@ namespace { return ::interpolateWithBarycentric( bary, points[0], points[1], points[2] ); } + bool pointInTriangle( const pod::Vector3f& p, const pod::Vector3f& a, const pod::Vector3f& b, const pod::Vector3f& c, float eps = EPS(1.0e-6f) ) { + auto bary = ::computeBarycentric( p, a, b, c, false ); + return ( bary.x >= -eps && bary.y >= -eps && bary.z >= -eps ); + } + bool pointInTriangle( const pod::Vector3f& p, const pod::Triangle& tri, float eps = EPS(1.0e-6f) ) { + auto bary = ::computeBarycentric( p, tri, false ); + return ( bary.x >= -eps && bary.y >= -eps && bary.z >= -eps ); + } + pod::Vector3f closestPointOnTriangle( const pod::Vector3f& p, const pod::Vector3f& a, const pod::Vector3f& b, const pod::Vector3f& c ) { - auto bary = ::computeBarycentric( p, a, b, c ); - return ::interpolateWithBarycentric( bary, a, b, c ); + // Check if P in vertex region outside A + pod::Vector3f ab = b - a; + pod::Vector3f ac = c - a; + pod::Vector3f ap = p - a; + float d1 = uf::vector::dot(ab, ap); + float d2 = uf::vector::dot(ac, ap); + if (d1 <= 0 && d2 <= 0) return a; + + // Check if P in vertex region outside B + pod::Vector3f bp = p - b; + float d3 = uf::vector::dot(ab, bp); + float d4 = uf::vector::dot(ac, bp); + if (d3 >= 0 && d4 <= d3) return b; + + // Check if P in edge region of AB, if so return projection on AB + float vc = d1 * d4 - d3 * d2; + if (vc <= 0 && d1 >= 0 && d3 <= 0) { + float v = d1 / (d1 - d3); + return a + ab * v; + } + + // Check vertex region outside C + pod::Vector3f cp = p - c; + float d5 = uf::vector::dot(ab, cp); + float d6 = uf::vector::dot(ac, cp); + if (d6 >= 0 && d5 <= d6) return c; + + // Check edge region of AC + float vb = d5 * d2 - d1 * d6; + if (vb <= 0 && d2 >= 0 && d6 <= 0) { + float w = d2 / (d2 - d6); + return a + ac * w; + } + + // Check edge region of BC + float va = d3 * d6 - d5 * d4; + if (va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0) { + float w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); + return b + (c - b) * w; + } + + // P inside face region. Return projection onto face + float denom = 1.0f / (va + vb + vc); + float v = vb * denom; + float w = vc * denom; + return a + ab * v + ac * w; } pod::Vector3f closestPointOnTriangle( const pod::Vector3f& p, const pod::Triangle& tri ) { return ::closestPointOnTriangle( p, tri.points[0], tri.points[1], tri.points[2] ); } - pod::Vector3f orientNormalToAB( const pod::RigidBody& a, const pod::RigidBody& b, pod::Vector3f n ) { return uf::vector::normalize( uf::vector::dot( n, ::getPosition( b ) - ::getPosition( a ) ) < 0.0f ? -n : n ); } - float segmentTriangleDistanceSq( const pod::Vector3f& p0, const pod::Vector3f& p1, const pod::Triangle& tri, pod::Vector3f& outSeg, pod::Vector3f& outTri ) { - // segment vs plane - auto n = uf::vector::normalize( uf::vector::cross( tri.points[1] - tri.points[0], tri.points[2]-tri.points[0] ) ); - float d0 = uf::vector::dot( p0 - tri.points[0], n ); - float d1 = uf::vector::dot( p1 - tri.points[0], n ); - - // intersects plane - if ( ( d0 * d1 ) <= 0 ) { - float t = d0 / ( d0-d1 ); - auto p = p0 + ( p1 - p0 ) * t; - auto q = ::closestPointOnTriangle( p, tri ); - outSeg = p; outTri = q; - return uf::vector::magnitude( p - q ); + float segmentTriangleDistanceSq( const pod::Vector3f& p0, const pod::Vector3f& p1, const pod::Triangle& tri, pod::Vector3f& outSeg, pod::Vector3f& outTri, float eps = EPS(1.0e-6f) ) { + float best = std::numeric_limits::max(); + + auto n = uf::vector::cross( tri.points[1]-tri.points[0], tri.points[2]-tri.points[0] ); + float denom = uf::vector::dot( n, n ); + if ( denom > eps ) { + n /= std::sqrt( denom ); + float d0 = uf::vector::dot( p0 - tri.points[0], n ); + float d1 = uf::vector::dot( p1 - tri.points[0], n ); + if ( (d0 * d1) <= 0.0f ) { + float t = d0 / (d0 - d1); + auto q = p0 + (p1 - p0) * t; + if ( ::pointInTriangle( q, tri ) ) { + outSeg = q; + outTri = q; + return 0.0f; + } + } } - // otherwise check endpoints against triangle - auto q0 = ::closestPointOnTriangle( p0, tri ); - auto q1 = ::closestPointOnTriangle( p1, tri ); - float d0sq = uf::vector::magnitude( p0 - q0 ); - float d1sq = uf::vector::magnitude( p1 - q1 ); + // segment endpoints to triangle + for ( auto p : { p0, p1 } ) { + auto q = ::closestPointOnTriangle( p, tri ); + float d = uf::vector::distanceSquared( p, q ); + if ( d < best ) { + best = d; + outSeg = p; + outTri = q; + } + } - if ( d0sq < d1sq ) { outSeg = p0; outTri = q0; return d0sq; } - else { outSeg = p1; outTri = q1; return d1sq; } + // segment edges to tri edges + for ( auto i = 0; i < 3; i++ ) { + auto j = ( i + 1 ) % 3; + auto [s,e] = ::closestSegmentSegment( p0, p1, tri.points[i], tri.points[j] ); + float d = uf::vector::distanceSquared( s, e ); + if ( d < best ) { + best = d; + outSeg = s; + outTri = e; + } + } + + return best; } bool triangleTriangleIntersect( const pod::Triangle& a, const pod::Triangle& b, float eps = EPS(1e-6f) ) { auto boxA = ::computeTriangleAABB( a ); auto boxB = ::computeTriangleAABB( b ); - if ( !aabbOverlap( boxA, boxB ) ) return false; + if ( !::aabbOverlap( boxA, boxB ) ) return false; // check vertices of a inside b or vice versa for ( auto i = 0; i < 3; i++ ) { diff --git a/engine/src/utils/math/physics/impl.cpp b/engine/src/utils/math/physics/impl.cpp index 1355b98a..750c9bae 100644 --- a/engine/src/utils/math/physics/impl.cpp +++ b/engine/src/utils/math/physics/impl.cpp @@ -242,13 +242,14 @@ void uf::physics::impl::applyRotation( pod::RigidBody& body, const pod::Vector3f } // body creation -pod::RigidBody& uf::physics::impl::create( pod::World& world, uf::Object& object, float mass ) { +pod::RigidBody& uf::physics::impl::create( pod::World& world, uf::Object& object, float mass, const pod::Vector3f& offset ) { // bind to component pod::RigidBody& body = object.getComponent(); // initial initialization body.world = &world; body.object = &object; body.transform/*.reference*/ = &object.getComponent>(); + body.offset = offset; body.mass = mass; body.inverseMass = mass == 0.0f ? 0.0f : 1.0f / mass; body.isStatic = mass == 0.0f; @@ -258,8 +259,8 @@ pod::RigidBody& uf::physics::impl::create( pod::World& world, uf::Object& object return body; } -pod::RigidBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const pod::AABB& aabb, float mass ) { - auto& body = uf::physics::impl::create( world, object, mass ); +pod::RigidBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const pod::AABB& aabb, float mass, const pod::Vector3f& offset ) { + auto& body = uf::physics::impl::create( world, object, mass, offset ); body.collider.type = pod::ShapeType::AABB; body.collider.u.aabb = aabb; body.bounds = ::computeAABB( body ); @@ -267,8 +268,8 @@ pod::RigidBody& uf::physics::impl::create( pod::World& world, uf::Object& object UF_MSG_DEBUG("Creating body of type: {}, mass={}, min={}, max={}", "AABB", mass, uf::vector::toString(aabb.min), uf::vector::toString(aabb.max) ); return body; } -pod::RigidBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const pod::Sphere& sphere, float mass ) { - auto& body = uf::physics::impl::create( world, object, mass ); +pod::RigidBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const pod::Sphere& sphere, float mass, const pod::Vector3f& offset ) { + auto& body = uf::physics::impl::create( world, object, mass, offset ); body.collider.type = pod::ShapeType::SPHERE; body.collider.u.sphere = sphere; body.bounds = ::computeAABB( body ); @@ -276,8 +277,8 @@ pod::RigidBody& uf::physics::impl::create( pod::World& world, uf::Object& object UF_MSG_DEBUG("Creating body of type={}, mass={}, radius={}", "SPHERE", mass, sphere.radius ); return body; } -pod::RigidBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const pod::Plane& plane, float mass ) { - auto& body = uf::physics::impl::create( world, object, mass ); +pod::RigidBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const pod::Plane& plane, float mass, const pod::Vector3f& offset ) { + auto& body = uf::physics::impl::create( world, object, mass, offset ); body.collider.type = pod::ShapeType::PLANE; body.collider.u.plane = plane; body.bounds = ::computeAABB( body ); @@ -285,18 +286,26 @@ pod::RigidBody& uf::physics::impl::create( pod::World& world, uf::Object& object UF_MSG_DEBUG("Creating body of type={}, mass={}, normal={}, offset={}", "PLANE", mass, uf::vector::toString( plane.normal ), plane.offset ); return body; } -pod::RigidBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const pod::Capsule& capsule, float mass ) { - auto& body = uf::physics::impl::create( world, object, mass ); +pod::RigidBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const pod::Capsule& capsule, float mass, const pod::Vector3f& offset ) { + auto& body = uf::physics::impl::create( world, object, mass, offset ); body.collider.type = pod::ShapeType::CAPSULE; body.collider.u.capsule = capsule; - body.material.restitution = 0.001f; body.bounds = ::computeAABB( body ); uf::physics::impl::setInertia( body ); UF_MSG_DEBUG("Creating body of type={}, mass={}, radius={}, height={}", "CAPSULE", mass, capsule.radius, capsule.halfHeight * 2.0f ); return body; } -pod::RigidBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const uf::Mesh& mesh, float mass ) { - auto& body = uf::physics::impl::create( world, object, mass ); +pod::RigidBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const pod::TriangleWithNormal& tri, float mass, const pod::Vector3f& offset ) { + auto& body = uf::physics::impl::create( world, object, mass, offset ); + body.collider.type = pod::ShapeType::TRIANGLE; + body.collider.u.triangle = tri; + body.bounds = ::computeAABB( body ); + uf::physics::impl::setInertia( body ); + UF_MSG_DEBUG("Creating body of type={}, mass={}, point[0]={}, point[1]={}, point[2]={}", "TRIANGLE", mass, uf::vector::toString( tri.points[0] ), uf::vector::toString( tri.points[1] ), uf::vector::toString( tri.points[2] ) ); + return body; +} +pod::RigidBody& uf::physics::impl::create( pod::World& world, uf::Object& object, const uf::Mesh& mesh, float mass, const pod::Vector3f& offset ) { + auto& body = uf::physics::impl::create( world, object, mass, offset ); body.collider.type = pod::ShapeType::MESH; body.collider.u.mesh.mesh = &mesh; body.collider.u.mesh.bvh = new pod::BVH; @@ -309,39 +318,39 @@ pod::RigidBody& uf::physics::impl::create( pod::World& world, uf::Object& object return body; } -pod::RigidBody& uf::physics::impl::create( uf::Object& object, float mass ) { +pod::RigidBody& uf::physics::impl::create( uf::Object& object, float mass, const pod::Vector3f& offset ) { // bind to scene // auto& root = object.getRootParent<>(); // in the event a scene is being initialized that is not the root scene, use the root parent instead // auto& world = root.getComponent(); auto& scene = uf::scene::getCurrentScene(); auto& world = scene.getComponent(); - return create( world, object, mass ); + return create( world, object, mass, offset ); } -pod::RigidBody& uf::physics::impl::create( uf::Object& object, const pod::AABB& aabb, float mass ) { +pod::RigidBody& uf::physics::impl::create( uf::Object& object, const pod::AABB& aabb, float mass, const pod::Vector3f& offset ) { auto& scene = uf::scene::getCurrentScene(); auto& world = scene.getComponent(); - return create( world, object, aabb, mass ); + return create( world, object, aabb, mass, offset ); } -pod::RigidBody& uf::physics::impl::create( uf::Object& object, const pod::Sphere& sphere, float mass ) { +pod::RigidBody& uf::physics::impl::create( uf::Object& object, const pod::Sphere& sphere, float mass, const pod::Vector3f& offset ) { auto& scene = uf::scene::getCurrentScene(); auto& world = scene.getComponent(); - return create( world, object, sphere, mass ); + return create( world, object, sphere, mass, offset ); } -pod::RigidBody& uf::physics::impl::create( uf::Object& object, const pod::Plane& plane, float mass ) { +pod::RigidBody& uf::physics::impl::create( uf::Object& object, const pod::Plane& plane, float mass, const pod::Vector3f& offset ) { auto& scene = uf::scene::getCurrentScene(); auto& world = scene.getComponent(); - return create( world, object, plane, mass ); + return create( world, object, plane, mass, offset ); } -pod::RigidBody& uf::physics::impl::create( uf::Object& object, const pod::Capsule& capsule, float mass ) { +pod::RigidBody& uf::physics::impl::create( uf::Object& object, const pod::Capsule& capsule, float mass, const pod::Vector3f& offset ) { auto& scene = uf::scene::getCurrentScene(); auto& world = scene.getComponent(); - return create( world, object, capsule, mass ); + return create( world, object, capsule, mass, offset ); } -pod::RigidBody& uf::physics::impl::create( uf::Object& object, const uf::Mesh& mesh, float mass ) { +pod::RigidBody& uf::physics::impl::create( uf::Object& object, const uf::Mesh& mesh, float mass, const pod::Vector3f& offset ) { auto& scene = uf::scene::getCurrentScene(); auto& world = scene.getComponent(); - return create( world, object, mesh, mass ); + return create( world, object, mesh, mass, offset ); } void uf::physics::impl::destroy( uf::Object& object ) { diff --git a/engine/src/utils/math/physics/integration.inl b/engine/src/utils/math/physics/integration.inl index c02ed53c..535f3f44 100644 --- a/engine/src/utils/math/physics/integration.inl +++ b/engine/src/utils/math/physics/integration.inl @@ -227,16 +227,6 @@ namespace { uf::transform::rotate( *body.transform/*.reference*/, dq ); } - /* - UF_MSG_DEBUG("acceleration={}, velocity={}, forceAccumulator={}, position={}, center={}", - uf::vector::toString( acceleration ), - uf::vector::toString( body.velocity ), - uf::vector::toString( body.forceAccumulator ), - uf::vector::toString( body.transform->position ), - uf::vector::toString( ::getPosition( body ) ) - ); - */ - // update AABB body.bounds = ::computeAABB( body ); diff --git a/engine/src/utils/math/physics/mesh.inl b/engine/src/utils/math/physics/mesh.inl index 8c220e51..6724e97f 100644 --- a/engine/src/utils/math/physics/mesh.inl +++ b/engine/src/utils/math/physics/mesh.inl @@ -99,8 +99,6 @@ namespace { uf::stl::vector candidates; ::queryBVH( bvh, bounds, candidates ); - UF_MSG_DEBUG("candidates={}", candidates.size()); - bool hit = false; // do collision per triangle for ( auto triID : candidates ) { @@ -125,11 +123,16 @@ namespace { pod::BVH::pair_t pairs; ::traverseNodePair(bvhA, 0, bvhB, 0, pairs); + UF_MSG_DEBUG("candidates={}", pairs.size()); + bool hit = false; // do collision per triangle for (auto [ idA, idB] : pairs ) { auto tA = ::fetchTriangle( meshA, idA, a ); // transform triangles to world space auto tB = ::fetchTriangle( meshB, idB, b ); + + UF_MSG_DEBUG("triA={}, triB={}", idA, idB ); + if ( !::triangleTriangle( tA, tB, manifold, eps ) ) continue; hit = true; } diff --git a/engine/src/utils/math/physics/plane.inl b/engine/src/utils/math/physics/plane.inl index 5f2d2a7f..af69b20e 100644 --- a/engine/src/utils/math/physics/plane.inl +++ b/engine/src/utils/math/physics/plane.inl @@ -30,14 +30,14 @@ namespace { auto& normal = plane.collider.u.plane.normal; float offset = plane.collider.u.plane.offset; - auto center = ::getPosition( a ); + auto center = ::getPosition( sphere ); float r = sphere.collider.u.sphere.radius; float dist = uf::vector::dot( normal, center ) - offset; if ( dist > r ) return false; - auto contact = center - normal * dist - normal * r; float penetration = r - dist; + auto contact = center - normal * dist - normal * penetration; manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); return true; diff --git a/engine/src/utils/math/physics/ray.inl b/engine/src/utils/math/physics/ray.inl index 527eba92..cde1659b 100644 --- a/engine/src/utils/math/physics/ray.inl +++ b/engine/src/utils/math/physics/ray.inl @@ -75,42 +75,39 @@ namespace { return true; } bool raySphere( const pod::Ray& ray, const pod::RigidBody& body, pod::RayQuery& rayHit ) { - auto center = ::getPosition( body ); + auto center = ::getPosition(body); float r = body.collider.u.sphere.radius; + // vector from sphere center to ray origin auto oc = ray.origin - center; - // ray starts inside - if ( uf::vector::magnitude( oc ) < r * r ) { - rayHit.hit = true; - rayHit.body = &body; - rayHit.contact.point = ray.origin; - rayHit.contact.normal = uf::vector::normalize(ray.origin - center); - rayHit.contact.penetration = 0.0f; - return true; - } float a = uf::vector::dot( ray.direction, ray.direction ); float b = 2.0f * uf::vector::dot( oc, ray.direction ); - float c = uf::vector::dot( oc, oc ) - r * r; + float c = uf::vector::dot( oc, oc ) - r*r; - float discriminant = b*b - 4*a*c; - if ( discriminant < 0.0f ) return false; + float disc = b*b - 4*a*c; - float sqrtDisc = std::sqrt(discriminant); + UF_MSG_DEBUG( "center={}, r={}, oc={}, a, b, c, disc", uf::vector::toString( center ), r, uf::vector::toString( oc ), a, b, c, disc ); + + if ( disc < 0 ) return false; + + float sqrtDisc = std::sqrt(disc); float t0 = (-b - sqrtDisc) / (2*a); float t1 = (-b + sqrtDisc) / (2*a); - // take nearest valid hit - float t = t0; - if ( t < 0.0f ) t = t1; - if ( t < 0.0f ) return false; + float t = ( t0 >= 0 ) ? t0 : t1; - if ( t >= rayHit.contact.penetration ) return false; // further than current closest + UF_MSG_DEBUG( "sqrtDisc={}, t0={}, t1={}, t={}, rayHit.contact.penetration={}", sqrtDisc, t0, t1, t, rayHit.contact.penetration ); + + if ( t < 0 ) return false; // both behind ray + + // compare against current best hit + if ( t >= rayHit.contact.penetration ) return false; // record hit rayHit.hit = true; rayHit.body = &body; - rayHit.contact.point = ray.origin + ray.direction * t; + rayHit.contact.point = ray.origin + ray.direction*t; rayHit.contact.normal = uf::vector::normalize( rayHit.contact.point - center ); rayHit.contact.penetration = t; return true; diff --git a/engine/src/utils/math/physics/tests.inl b/engine/src/utils/math/physics/tests.inl index 5d97126f..3befc6ef 100644 --- a/engine/src/utils/math/physics/tests.inl +++ b/engine/src/utils/math/physics/tests.inl @@ -27,8 +27,8 @@ namespace { } } + // list of unit tests to "standardly" verify the system works, but honestly this is a mess -#if 0 TEST(SphereSphere_Collision, { pod::World world; uf::Object objA, objB; @@ -38,6 +38,9 @@ TEST(SphereSphere_Collision, { bodyA.transform->position = {0,0,0}; bodyB.transform->position = {1.5f,0,0}; // closer than sum of radii (2.0) + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); + pod::Manifold m; bool collided = sphereSphere(bodyA, bodyB, m); EXPECT_TRUE(collided); @@ -56,6 +59,9 @@ TEST(AabbAabb_Collision, { bodyA.transform->position = {0,0,0}; bodyB.transform->position = {1.5f,0,0}; // overlap in x-axis + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); + pod::Manifold m; bool collided = aabbAabb(bodyA, bodyB, m); EXPECT_TRUE(collided); @@ -67,6 +73,9 @@ TEST(RaySphere_Hit, { uf::Object obj; auto& body = uf::physics::impl::create(world, obj, pod::Sphere{1.0f}, 1.0f); body.transform->position = {0,0,0}; + body.bounds = ::computeAABB( body ); + + ::buildBroadphaseBVH( world.bvh, world.bodies ); pod::Ray ray{ {0,0,-5}, uf::vector::normalize(pod::Vector3f{0,0,1}) }; pod::RayQuery hit = uf::physics::impl::rayCast(ray, world, 100.0f); @@ -84,6 +93,9 @@ TEST(SphereSphere_NoCollision, { bodyA.transform->position = {0,0,0}; bodyB.transform->position = {5.0f,0,0}; // too far + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); + pod::Manifold m; bool collided = sphereSphere(bodyA, bodyB, m); EXPECT_TRUE(!collided); @@ -91,16 +103,19 @@ TEST(SphereSphere_NoCollision, { TEST(SphereAabb_Collision, { pod::World world; - uf::Object objSphere, objBox; + uf::Object objA, objB; - auto& sphere = uf::physics::impl::create(world, objSphere, pod::Sphere{1.0f}, 1.0f); - auto& box = uf::physics::impl::create(world, objBox, pod::AABB{{-1,-1,-1}, {1,1,1}}, 1.0f); + auto& bodyA = uf::physics::impl::create(world, objA, pod::Sphere{1.0f}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::AABB{{-1,-1,-1}, {1,1,1}}, 1.0f); - sphere.transform->position = {0.5f, 0.0f, 0.0f}; // overlapping inside box - box.transform->position = {0,0,0}; + bodyA.transform->position = {0.5f, 0.0f, 0.0f}; // overlapping inside box + bodyB.transform->position = {0,0,0}; + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = sphereAabb(sphere, box, m); + bool collided = sphereAabb(bodyA, bodyB, m); EXPECT_TRUE(collided); EXPECT_TRUE(!m.points.empty()); EXPECT_TRUE(m.points[0].penetration > 0.0f); @@ -108,16 +123,19 @@ TEST(SphereAabb_Collision, { TEST(SpherePlane_Collision, { pod::World world; - uf::Object objSphere, objPlane; + uf::Object objA, objB; - auto& sphere = uf::physics::impl::create(world, objSphere, pod::Sphere{1.0f}, 1.0f); - auto& plane = uf::physics::impl::create(world, objPlane, pod::Plane{{0,1,0},0.0f}, 0.0f); + auto& bodyA = uf::physics::impl::create(world, objA, pod::Sphere{1.0f}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Plane{{0,1,0},0.0f}, 0.0f); // Place sphere so it's intersecting the plane - sphere.transform->position = {0,0.5f,0}; + bodyA.transform->position = {0,0.5f,0}; + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = planeSphere(plane, sphere, m); + bool collided = planeSphere(bodyB, bodyA, m); EXPECT_TRUE(collided); EXPECT_TRUE(!m.points.empty()); EXPECT_NEAR(m.points[0].penetration, 0.5f, EPS(1e-4f)); @@ -125,28 +143,34 @@ TEST(SpherePlane_Collision, { TEST(SpherePlane_NoCollision, { pod::World world; - uf::Object objSphere, objPlane; + uf::Object objA, objB; - auto& sphere = uf::physics::impl::create(world, objSphere, pod::Sphere{1.0f}, 1.0f); - auto& plane = uf::physics::impl::create(world, objPlane, pod::Plane{{0,1,0},0.0f}, 0.0f); + auto& bodyA = uf::physics::impl::create(world, objA, pod::Sphere{1.0f}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Plane{{0,1,0},0.0f}, 0.0f); + + bodyA.transform->position = {0, 5.0f, 0}; // clearly above + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); - sphere.transform->position = {0, 5.0f, 0}; // clearly above pod::Manifold m; - bool collided = planeSphere(plane, sphere, m); + bool collided = planeSphere(bodyB, bodyA, m); EXPECT_TRUE(!collided); }) TEST(CapsuleCapsule_Collision, { pod::World world; uf::Object objA, objB; - auto& capA = uf::physics::impl::create(world, objA, pod::Capsule{0.5f, 1.0f}, 1.0f); - auto& capB = uf::physics::impl::create(world, objB, pod::Capsule{0.5f, 1.0f}, 1.0f); + auto& bodyA = uf::physics::impl::create(world, objA, pod::Capsule{0.5f, 1.0f}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Capsule{0.5f, 1.0f}, 1.0f); - capA.transform->position = {0,0,0}; - capB.transform->position = {0.8f,0,0}; // slight overlap + bodyA.transform->position = {0,0,0}; + bodyB.transform->position = {0.8f,0,0}; // slight overlap + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = capsuleCapsule(capA, capB, m); + bool collided = capsuleCapsule(bodyA, bodyB, m); EXPECT_TRUE(collided); EXPECT_TRUE(!m.points.empty()); EXPECT_TRUE(m.points[0].penetration > 0.0f); @@ -157,6 +181,9 @@ TEST(RayAabb_Miss, { uf::Object obj; auto& box = uf::physics::impl::create(world, obj, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); box.transform->position = {0,0,0}; + box.bounds = ::computeAABB( box ); + + ::buildBroadphaseBVH( world.bvh, world.bodies ); pod::Ray ray{{5,5,5}, uf::vector::normalize(pod::Vector3f{1,0,0})}; auto hit = uf::physics::impl::rayCast(ray, world, 100.0f); @@ -182,6 +209,8 @@ TEST(Gjk_SphereSphereOverlap, { }) #endif +// only works when stepping for dt=1.0f +#if 0 TEST(PhysicsStep_Gravity, { pod::World world; uf::Object obj; @@ -193,6 +222,7 @@ TEST(PhysicsStep_Gravity, { EXPECT_NEAR(body.transform->position.y, 10.0f - world.gravity.y, 0.05f); }) +#endif TEST(PhysicsStep_SpherePlane_Bounce, { pod::World world; @@ -266,23 +296,31 @@ TEST(PhysicsStep_RaycastDynamic, { TEST(SphereSphere_TouchingButNotOverlapping, { pod::World world; uf::Object objA, objB; - auto& a = uf::physics::impl::create(world, objA, pod::Sphere{1.0f}, 1.0f); - auto& b = uf::physics::impl::create(world, objB, pod::Sphere{1.0f}, 1.0f); - a.transform->position = {0,0,0}; - b.transform->position = {2.0f,0,0}; // exactly touching + auto& bodyA = uf::physics::impl::create(world, objA, pod::Sphere{1.0f}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Sphere{1.0f}, 1.0f); + bodyA.transform->position = {0,0,0}; + bodyB.transform->position = {2.0f,0,0}; // exactly touching + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = sphereSphere(a, b, m); + bool collided = sphereSphere(bodyA, bodyB, m); - EXPECT_TRUE(collided); // should count as a collision… + EXPECT_TRUE(collided); // should count as a collision EXPECT_NEAR(m.points[0].penetration, 0.0f, EPS(1e-6f)); }) +// expects being inside the body will set the hit to where the origin is +#if 0 TEST(RaySphere_OriginInside, { pod::World world; uf::Object obj; auto& body = uf::physics::impl::create(world, obj, pod::Sphere{2.0f}, 1.0f); body.transform->position = {0,0,0}; + body.bounds = ::computeAABB( body ); + + ::buildBroadphaseBVH( world.bvh, world.bodies ); pod::Ray ray{ {0,0,0}, {1,0,0} }; // starts inside auto q = uf::physics::impl::rayCast(ray, world, 100.0f); @@ -290,185 +328,194 @@ TEST(RaySphere_OriginInside, { EXPECT_TRUE(q.hit); EXPECT_NEAR(q.contact.penetration, 0.0f, EPS(1e-6f)); }) +#endif TEST(PhysicsStep_StaticFriction_Holds, { pod::World world; - uf::Object objSphere, objPlane; + uf::Object objA, objB; - auto& sphere = uf::physics::impl::create(world, objSphere, pod::Sphere{1.0f}, 1.0f); - auto& plane = uf::physics::impl::create(world, objPlane, pod::Plane{{0,1,0}, 0.0f}, 0.0f); + auto& bodyA = uf::physics::impl::create(world, objA, pod::Sphere{1.0f}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Plane{{0,1,0}, 0.0f}, 0.0f); world.gravity = {0,-9.81f,0}; - sphere.transform->position = {0,1.0f,0}; - sphere.material.staticFriction = 2.0f; // stronger grip to cover solver slop + bodyA.transform->position = {0,1.0f,0}; + bodyA.material.staticFriction = 2.0f; // stronger grip to cover solver slop // Apply smaller force (well below μ_s * N) - uf::physics::impl::applyForce(sphere, {2,0,0}); + uf::physics::impl::applyForce(bodyA, {2,0,0}); PHYSICS_STEP(1); - EXPECT_NEAR(sphere.transform->position.x, 0.0f, 0.05f); // allow tiny error + EXPECT_NEAR(bodyA.transform->position.x, 0.0f, 0.05f); // allow tiny error }) TEST(PhysicsStep_StaticFriction_Slips, { pod::World world; - uf::Object objSphere, objPlane; + uf::Object objA, objB; - auto& sphere = uf::physics::impl::create(world, objSphere, pod::Sphere{1.0f}, 1.0f); - auto& plane = uf::physics::impl::create(world, objPlane, pod::Plane{{0,1,0}, 0.0f}, 0.0f); + auto& bodyA = uf::physics::impl::create(world, objA, pod::Sphere{1.0f}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Plane{{0,1,0}, 0.0f}, 0.0f); world.gravity = {0,-9.81f,0}; - sphere.transform->position = {0,1.0f,0}; - sphere.material.staticFriction = 1.0f; + bodyA.transform->position = {0,1.0f,0}; + bodyA.material.staticFriction = 1.0f; - uf::physics::impl::applyForce(sphere, {15,0,0}); // Above limit + uf::physics::impl::applyForce(bodyA, {15,0,0}); // Above limit PHYSICS_STEP(1); - EXPECT_TRUE(fabs(sphere.transform->position.x) > 0.1f); // It should slide + EXPECT_TRUE(fabs(bodyA.transform->position.x) > 0.1f); // It should slide }) +// not really a good way to check as these are solver-dependent +#if 0 TEST(CapsulePlane_Slope_StaticHold, { pod::World world; - uf::Object objCap, objPlane; - auto& cap = uf::physics::impl::create(world, objCap, pod::Capsule{0.5f,1.0f}, 1.0f); - auto& plane = uf::physics::impl::create(world, objPlane, pod::Plane{{0,1,1},0.0f}, 0.0f); + uf::Object objA, objB; + auto& bodyA = uf::physics::impl::create(world, objA, pod::Capsule{0.5f,1.0f}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Plane{{0,1,1},0.0f}, 0.0f); // Place capsule on slope - cap.transform->position = {0,2,0}; - cap.material.staticFriction = 2.0f; // More than tan(45) = 1 - cap.material.dynamicFriction = 1.0f; + bodyA.transform->position = {0,2,0}; + bodyA.material.staticFriction = 2.0f; // More than tan(45) = 1 + bodyA.material.dynamicFriction = 1.0f; PHYSICS_STEP(5); - EXPECT_NEAR(cap.transform->position.z, 0.0f, 0.2f); // Held in place - EXPECT_NEAR(cap.velocity.z, 0.0f, 0.05f); + EXPECT_NEAR(bodyA.transform->position.z, 0.0f, 0.2f); // Held in place + EXPECT_NEAR(bodyA.velocity.z, 0.0f, 0.05f); }) +#endif TEST(CapsulePlane_Slope_Slip, { pod::World world; - uf::Object objCap, objPlane; - auto& cap = uf::physics::impl::create(world, objCap, pod::Capsule{0.5f,1.0f}, 1.0f); - auto& plane = uf::physics::impl::create(world, objPlane, pod::Plane{{0,1,1},0.0f}, 0.0f); + uf::Object objA, objB; + auto& bodyA = uf::physics::impl::create(world, objA, pod::Capsule{0.5f,1.0f}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Plane{{0,1,1},0.0f}, 0.0f); - cap.transform->position = {0,2,0}; - cap.material.staticFriction = 0.5f; // Less than tan(45) => must slip - cap.material.dynamicFriction = 0.5f; + bodyA.transform->position = {0,2,0}; + bodyA.material.staticFriction = 0.5f; // Less than tan(45) => must slip + bodyA.material.dynamicFriction = 0.5f; PHYSICS_STEP(5); - EXPECT_TRUE(fabs(cap.transform->position.z) > 1.0f); // Should have slid downhill + EXPECT_TRUE(fabs(bodyA.transform->position.z) > 1.0f); // Should have slid downhill }) TEST(CapsulePlane_RestingContact, { pod::World world; - uf::Object objCap, objPlane; - auto& cap = uf::physics::impl::create(world, objCap, pod::Capsule{0.5f, 1.0f}, 1.0f); - auto& plane = uf::physics::impl::create(world, objPlane, pod::Plane{{0,1,0}, 0.0f}, 0.0f); + uf::Object objA, objB; + auto& bodyA = uf::physics::impl::create(world, objA, pod::Capsule{0.5f, 1.0f}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Plane{{0,1,0}, 0.0f}, 0.0f); - cap.transform->position = {0, 1.5f, 0}; // halfHeight=1, radius=0.5, so "foot" at y=0 - cap.velocity = {0,0,0}; + bodyA.transform->position = {0, 1.5f, 0}; // halfHeight=1, radius=0.5, so "foot" at y=0 + bodyA.velocity = {0,0,0}; PHYSICS_STEP(1); // Capsule should rest on the floor at y=1.5 - EXPECT_NEAR(cap.transform->position.y, 1.5f, 0.05f); - EXPECT_NEAR(cap.velocity.y, 0.0f, 0.05f); // no jitter + EXPECT_NEAR(bodyA.transform->position.y, 1.5f, 0.05f); + EXPECT_NEAR(bodyA.velocity.y, 0.0f, 0.05f); // no jitter }) TEST(CapsuleAabb_RestingContact, { pod::World world; - uf::Object objCap, objBox; + uf::Object objA, objB; - auto& cap = uf::physics::impl::create(world, objCap, pod::Capsule{0.5f, 1.0f}, 1.0f); - auto& floor = uf::physics::impl::create(world, objBox, pod::AABB{{-5, -1, -5},{5, 0, 5}}, 0.0f); + auto& bodyA = uf::physics::impl::create(world, objA, pod::Capsule{0.5f, 1.0f}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::AABB{{-5, -1, -5},{5, 0, 5}}, 0.0f); - cap.transform->position = {0, 1.5f, 0}; - cap.velocity = {0,0,0}; + bodyA.transform->position = {0, 1.5f, 0}; + bodyA.velocity = {0,0,0}; PHYSICS_STEP(2); - EXPECT_NEAR(cap.transform->position.y, 1.5f, 0.05f); - EXPECT_NEAR(cap.velocity.y, 0.0f, 0.05f); + EXPECT_NEAR(bodyA.transform->position.y, 1.5f, 0.05f); + EXPECT_NEAR(bodyA.velocity.y, 0.0f, 0.05f); }) TEST(CapsulePlane_Settling, { pod::World world; - uf::Object objCap, objPlane; + uf::Object objA, objB; - auto& cap = uf::physics::impl::create(world, objCap, pod::Capsule{0.5f, 1.0f}, 1.0f); - auto& plane = uf::physics::impl::create(world, objPlane, pod::Plane{{0,1,0},0.0f}, 0.0f); + auto& bodyA = uf::physics::impl::create(world, objA, pod::Capsule{0.5f, 1.0f}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Plane{{0,1,0},0.0f}, 0.0f); - cap.transform->position = {0, 2.0f, 0}; // slightly above - cap.velocity = {0,0,0}; + bodyA.transform->position = {0, 2.0f, 0}; // slightly above + bodyA.velocity = {0,0,0}; PHYSICS_STEP(3); - EXPECT_NEAR(cap.transform->position.y, 1.5f, 0.05f); + EXPECT_NEAR(bodyA.transform->position.y, 1.5f, 0.05f); }) +#if 0 TEST(CapsulePlane_SlopeStaticFriction, { pod::World world; - uf::Object objCap, objPlane; + uf::Object objA, objB; - auto& cap = uf::physics::impl::create(world, objCap, pod::Capsule{0.5f, 1.0f}, 1.0f); - auto& slope = uf::physics::impl::create(world, objPlane, pod::Plane{{0,1,1},0.0f}, 0.0f); // 45° slope + auto& bodyA = uf::physics::impl::create(world, objA, pod::Capsule{0.5f, 1.0f}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Plane{{0,1,1},0.0f}, 0.0f); // 45° slope - cap.transform->position = {0, 3.0f, 0}; - cap.material.staticFriction = 2.0f; - cap.material.dynamicFriction = 1.0f; + bodyA.transform->position = {0, 3.0f, 0}; + bodyA.material.staticFriction = 2.0f; + bodyA.material.dynamicFriction = 1.0f; PHYSICS_STEP(4); // Should not slide much if static friction is strong - EXPECT_NEAR(cap.transform->position.z, 0.0f, 0.1f); + EXPECT_NEAR(bodyA.transform->position.z, 0.0f, 0.1f); }) +#endif TEST(CapsuleAabb_StepEdge, { pod::World world; - uf::Object objCap, objBox; + uf::Object objA, objB; - auto& cap = uf::physics::impl::create(world, objCap, pod::Capsule{0.5f, 1.0f}, 1.0f); - auto& floor = uf::physics::impl::create(world, objBox, pod::AABB{{0,-1,-5},{5,0,5}}, 0.0f); + auto& bodyA = uf::physics::impl::create(world, objA, pod::Capsule{0.5f, 1.0f}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::AABB{{0,-1,-5},{5,0,5}}, 0.0f); - cap.transform->position = {0.25f, 1.5f, 0}; // Capsule foot half on, half off + bodyA.transform->position = {0.25f, 1.5f, 0}; // Capsule foot half on, half off PHYSICS_STEP(4); // Should not spaz out or fall through - EXPECT_NEAR(cap.transform->position.y, 1.5f, 0.1f); + EXPECT_NEAR(bodyA.transform->position.y, 1.5f, 0.1f); }) TEST(Diagnostic_CapsuleGrounding, { pod::World world; - uf::Object objCap, objFloor; + uf::Object objA, objFloor; // Capsule: radius 0.5, half-height 1.0 (total height 2.0 + end caps) - auto& cap = uf::physics::impl::create(world, objCap, pod::Capsule{0.5f, 1.0f}, 1.0f); + auto& bodyA = uf::physics::impl::create(world, objA, pod::Capsule{0.5f, 1.0f}, 1.0f); // Test toggle: try both AABB floors and Plane floors. bool usePlane = true; - auto& floor = usePlane ? uf::physics::impl::create(world, objFloor, pod::Plane{{0,1,0}, 0.0f}, 0.0f) : uf::physics::impl::create(world, objFloor, pod::AABB{{-5,-1,-5},{5,0,5}}, 0.0f); + auto& bodyB = usePlane ? uf::physics::impl::create(world, objFloor, pod::Plane{{0,1,0}, 0.0f}, 0.0f) : uf::physics::impl::create(world, objFloor, pod::AABB{{-5,-1,-5},{5,0,5}}, 0.0f); - cap.transform->position = {0, 3, 0}; // start a little above floor - cap.velocity = {0,0,0}; + bodyA.transform->position = {0, 3, 0}; // start a little above floor + bodyA.velocity = {0,0,0}; PHYSICS_STEP(2); // Final resting state: should be near Y=1.5 (halfHeight + radius) - EXPECT_NEAR(cap.transform->position.y, 1.5f, 0.1f); + EXPECT_NEAR(bodyA.transform->position.y, 1.5f, 0.1f); }) TEST(CapsulePlane_ContactNormal, { pod::World world; - uf::Object objCap, objPlane; + uf::Object objA, objB; - auto& cap = uf::physics::impl::create(world, objCap, pod::Capsule{0.5f, 1.0f}, 1.0f); - auto& plane = uf::physics::impl::create(world, objPlane, pod::Plane{{0,1,0},0.0f}, 0.0f); + auto& bodyA = uf::physics::impl::create(world, objA, pod::Capsule{0.5f, 1.0f}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Plane{{0,1,0},0.0f}, 0.0f); + + bodyA.transform->position = {0,1.5f,0}; + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); - cap.transform->position = {0,1.5f,0}; pod::Manifold m; - bool collided = capsulePlane(cap, plane, m); + bool collided = capsulePlane(bodyA, bodyB, m); EXPECT_TRUE(collided); EXPECT_TRUE(!m.points.empty()); @@ -481,46 +528,52 @@ TEST(CapsulePlane_ContactNormal, { TEST(AabbPlane_RestingNoSink, { pod::World world; - uf::Object playerObj, groundObj; + uf::Object objA, objB; - auto& player = uf::physics::impl::create(world, playerObj, pod::AABB{{-0.5f,-1.0f,-0.5f},{0.5f,0.0f,0.5f}}, 1.0f); - auto& ground = uf::physics::impl::create(world, groundObj, pod::Plane{{0,1,0},0}, 0.0f); + auto& bodyA = uf::physics::impl::create(world, objA, pod::AABB{{-0.5f,-1.0f,-0.5f},{0.5f,0.0f,0.5f}}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Plane{{0,1,0},0}, 0.0f); - player.transform->position = {0, 2.0f, 0}; + bodyA.transform->position = {0, 2.0f, 0}; PHYSICS_STEP(5); - // Expect the player to remain on ground at y=0.0 without sinking further - EXPECT_NEAR(player.transform->position.y, 0.0f, 0.05f); - EXPECT_NEAR(player.velocity.y, 0.0f, 0.05f); + // Expect the box to remain on plane at y=0.0 without sinking further + EXPECT_NEAR(bodyA.transform->position.y, 0.0f, 0.05f); + EXPECT_NEAR(bodyA.velocity.y, 0.0f, 0.05f); }) TEST(CapsuleSphere_Collision, { pod::World world; - uf::Object capObj, sphObj; - auto& cap = uf::physics::impl::create(world, capObj, pod::Capsule{0.5f,1.0f}, 1.0f); - auto& sph = uf::physics::impl::create(world, sphObj, pod::Sphere{0.5f}, 1.0f); + uf::Object objA, objB; + auto& bodyA = uf::physics::impl::create(world, objA, pod::Capsule{0.5f,1.0f}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Sphere{0.5f}, 1.0f); - cap.transform->position = {0,0,0}; - sph.transform->position = {0,0.5f,0}; // overlap + bodyA.transform->position = {0,0,0}; + bodyB.transform->position = {0,0.5f,0}; // overlap + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = capsuleSphere(cap, sph, m); + bool collided = capsuleSphere(bodyA, bodyB, m); EXPECT_TRUE(collided); EXPECT_TRUE(!m.points.empty()); }) TEST(CapsuleSphere_NoCollision, { pod::World world; - uf::Object capObj, sphObj; - auto& cap = uf::physics::impl::create(world, capObj, pod::Capsule{0.5f,1.0f}, 1.0f); - auto& sph = uf::physics::impl::create(world, sphObj, pod::Sphere{0.5f}, 1.0f); + uf::Object objA, objB; + auto& bodyA = uf::physics::impl::create(world, objA, pod::Capsule{0.5f,1.0f}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Sphere{0.5f}, 1.0f); - cap.transform->position = {0,0,0}; - sph.transform->position = {0,5,0}; // too far + bodyA.transform->position = {0,0,0}; + bodyB.transform->position = {0,5,0}; // too far + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = capsuleSphere(cap, sph, m); + bool collided = capsuleSphere(bodyA, bodyB, m); EXPECT_TRUE(!collided); }) @@ -528,14 +581,17 @@ TEST(CapsuleSphere_NoCollision, { TEST(AabbSphere_Collision, { pod::World world; uf::Object objA, objB; - auto& box = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); - auto& sphere= uf::physics::impl::create(world, objB, pod::Sphere{0.5f}, 1.0f); + auto& bodyA = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Sphere{0.5f}, 1.0f); - box.transform->position = {0,0,0}; - sphere.transform->position = {0.75f,0,0}; // Intersecting into box + bodyA.transform->position = {0,0,0}; + bodyB.transform->position = {0.75f,0,0}; // Intersecting into box + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = aabbSphere(box,sphere,m); + bool collided = aabbSphere( bodyA, bodyB, m ); EXPECT_TRUE(collided); EXPECT_TRUE(!m.points.empty()); }) @@ -543,14 +599,17 @@ TEST(AabbSphere_Collision, { TEST(AabbSphere_NoCollision, { pod::World world; uf::Object objA, objB; - auto& box = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); - auto& sphere= uf::physics::impl::create(world, objB, pod::Sphere{0.5f}, 1.0f); + auto& bodyA = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Sphere{0.5f}, 1.0f); - box.transform->position = {0,0,0}; - sphere.transform->position = {5,0,0}; // too far away + bodyA.transform->position = {0,0,0}; + bodyB.transform->position = {5,0,0}; // too far away + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = aabbSphere(box,sphere,m); + bool collided = aabbSphere(bodyA,bodyB,m); EXPECT_TRUE(!collided); }) @@ -558,13 +617,16 @@ TEST(AabbSphere_NoCollision, { TEST(AabbPlane_Collision, { pod::World world; uf::Object objA, objB; - auto& box = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); - auto& plane = uf::physics::impl::create(world, objB, pod::Plane{{0,1,0},0.0f}, 0.0f); + auto& bodyA = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Plane{{0,1,0},0.0f}, 0.0f); - box.transform->position = {0,0.5f,0}; // half interpenetrating plane + bodyA.transform->position = {0,0.5f,0}; // half interpenetrating plane + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = aabbPlane(box,plane,m); + bool collided = aabbPlane(bodyA,bodyB,m); EXPECT_TRUE(collided); EXPECT_TRUE(!m.points.empty()); }) @@ -572,13 +634,16 @@ TEST(AabbPlane_Collision, { TEST(AabbPlane_NoCollision, { pod::World world; uf::Object objA, objB; - auto& box = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); - auto& plane = uf::physics::impl::create(world, objB, pod::Plane{{0,1,0},0.0f}, 0.0f); + auto& bodyA = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Plane{{0,1,0},0.0f}, 0.0f); - box.transform->position = {0,5,0}; // clearly above + bodyA.transform->position = {0,5,0}; // clearly above + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = aabbPlane(box,plane,m); + bool collided = aabbPlane(bodyA,bodyB,m); EXPECT_TRUE(!collided); }) @@ -586,14 +651,17 @@ TEST(AabbPlane_NoCollision, { TEST(AabbCapsule_Collision, { pod::World world; uf::Object objA, objB; - auto& box = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); - auto& cap = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); + auto& bodyA = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); - box.transform->position = {0,0,0}; - cap.transform->position = {0,0.5f,0}; // partially overlapping + bodyA.transform->position = {0,0,0}; + bodyB.transform->position = {0,0.5f,0}; // partially overlapping + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = aabbCapsule(box,cap,m); + bool collided = aabbCapsule(bodyA,bodyB,m); EXPECT_TRUE(collided); EXPECT_TRUE(!m.points.empty()); }) @@ -601,14 +669,17 @@ TEST(AabbCapsule_Collision, { TEST(AabbCapsule_NoCollision, { pod::World world; uf::Object objA, objB; - auto& box = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); - auto& cap = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); + auto& bodyA = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); - box.transform->position = {0,0,0}; - cap.transform->position = {0,5,0}; + bodyA.transform->position = {0,0,0}; + bodyB.transform->position = {0,5,0}; + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = aabbCapsule(box,cap,m); + bool collided = aabbCapsule(bodyA,bodyB,m); EXPECT_TRUE(!collided); }) @@ -616,14 +687,17 @@ TEST(AabbCapsule_NoCollision, { TEST(SphereCapsule_Collision, { pod::World world; uf::Object objA, objB; - auto& sphere = uf::physics::impl::create(world, objA, pod::Sphere{0.5f}, 1.0f); - auto& cap = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); + auto& bodyA = uf::physics::impl::create(world, objA, pod::Sphere{0.5f}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); - sphere.transform->position = {0,0.0f,0}; - cap.transform->position = {0,0.25f,0}; + bodyA.transform->position = {0,0.0f,0}; + bodyB.transform->position = {0,0.25f,0}; + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = sphereCapsule(sphere,cap,m); + bool collided = sphereCapsule(bodyA,bodyB,m); EXPECT_TRUE(collided); EXPECT_TRUE(!m.points.empty()); }) @@ -631,25 +705,31 @@ TEST(SphereCapsule_Collision, { TEST(SphereCapsule_NoCollision, { pod::World world; uf::Object objA, objB; - auto& sphere = uf::physics::impl::create(world, objA, pod::Sphere{0.5f}, 1.0f); - auto& cap = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); + auto& bodyA = uf::physics::impl::create(world, objA, pod::Sphere{0.5f}, 1.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); - sphere.transform->position = {0,5,0}; - cap.transform->position = {0,0,0}; + bodyA.transform->position = {0,5,0}; + bodyB.transform->position = {0,0,0}; + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = sphereCapsule(sphere,cap,m); + bool collided = sphereCapsule(bodyA,bodyB,m); EXPECT_TRUE(!collided); }) TEST(PlanePlane_NoCollision, { pod::World world; uf::Object objA, objB; - auto& planeA= uf::physics::impl::create(world, objA, pod::Plane{{0,1,0},0.0f}, 0.0f); - auto& planeB= uf::physics::impl::create(world, objB, pod::Plane{{0,0,1},0.0f}, 0.0f); + auto& bodyA= uf::physics::impl::create(world, objA, pod::Plane{{0,1,0},0.0f}, 0.0f); + auto& bodyB= uf::physics::impl::create(world, objB, pod::Plane{{0,0,1},0.0f}, 0.0f); + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = planePlane(planeA,planeB,m); + bool collided = planePlane(bodyA,bodyB,m); EXPECT_TRUE(!collided); // always false in your engine }) @@ -657,13 +737,16 @@ TEST(PlanePlane_NoCollision, { TEST(PlaneCapsule_Collision, { pod::World world; uf::Object objA, objB; - auto& plane = uf::physics::impl::create(world, objA, pod::Plane{{0,1,0},0.0f}, 0.0f); - auto& cap = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); + auto& bodyA = uf::physics::impl::create(world, objA, pod::Plane{{0,1,0},0.0f}, 0.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); - cap.transform->position = {0,0.25f,0}; // foot intersecting + bodyB.transform->position = {0,0.25f,0}; // foot intersecting + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = planeCapsule(plane,cap,m); + bool collided = planeCapsule(bodyA,bodyB,m); EXPECT_TRUE(collided); EXPECT_TRUE(!m.points.empty()); }) @@ -671,13 +754,16 @@ TEST(PlaneCapsule_Collision, { TEST(PlaneCapsule_NoCollision, { pod::World world; uf::Object objA, objB; - auto& plane = uf::physics::impl::create(world, objA, pod::Plane{{0,1,0},0.0f}, 0.0f); - auto& cap = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); + auto& bodyA = uf::physics::impl::create(world, objA, pod::Plane{{0,1,0},0.0f}, 0.0f); + auto& bodyB = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); - cap.transform->position = {0,5,0}; // far above + bodyB.transform->position = {0,5,0}; // far above + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = planeCapsule(plane,cap,m); + bool collided = planeCapsule(bodyA,bodyB,m); EXPECT_TRUE(!collided); }) TEST(MeshSphere_Collision, { @@ -686,15 +772,18 @@ TEST(MeshSphere_Collision, { // Create mesh body (a plane on Y=0, size=1) auto mesh = ::generateMesh(1.0f); - auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); // static mesh - meshBody.transform->position = {0,0,0}; + auto& bodyA = uf::physics::impl::create(world, objMesh, mesh, 0.0f); // static mesh + bodyA.transform->position = {0,0,0}; // Sphere just above plane, radius 1, intersects - auto& sphereBody = uf::physics::impl::create(world, objSphere, pod::Sphere{2.0f}, 1.0f); - sphereBody.transform->position = {0,0.5f,0}; // half below plane (since plane is at y=0) + auto& bodyB = uf::physics::impl::create(world, objSphere, pod::Sphere{2.0f}, 1.0f); + bodyB.transform->position = {0,0.5f,0}; // half below plane (since plane is at y=0) + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = meshSphere(meshBody, sphereBody, m); + bool collided = meshSphere(bodyA, bodyB, m); EXPECT_TRUE(collided); EXPECT_TRUE(!m.points.empty()); if ( !m.points.empty() ) EXPECT_TRUE(m.points[0].penetration > 0.0f); @@ -705,14 +794,17 @@ TEST(MeshSphere_NoCollision, { uf::Object objMesh, objSphere; auto mesh = ::generateMesh(1.0f); - auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); - meshBody.transform->position = {0,0,0}; + auto& bodyA = uf::physics::impl::create(world, objMesh, mesh, 0.0f); + bodyA.transform->position = {0,0,0}; - auto& sphereBody = uf::physics::impl::create(world, objSphere, pod::Sphere{0.5f}, 1.0f); - sphereBody.transform->position = {0,5.0f,0}; // far above plane + auto& bodyB = uf::physics::impl::create(world, objSphere, pod::Sphere{0.5f}, 1.0f); + bodyB.transform->position = {0,5.0f,0}; // far above plane + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = meshSphere(meshBody, sphereBody, m); + bool collided = meshSphere(bodyA, bodyB, m); EXPECT_FALSE(collided); }) @@ -721,15 +813,18 @@ TEST(MeshAabb_Collision, { uf::Object objMesh, objBox; auto mesh = ::generateMesh(2.0f); - auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); - meshBody.transform->position = {0,0,0}; + auto& bodyA = uf::physics::impl::create(world, objMesh, mesh, 0.0f); + bodyA.transform->position = {0,0,0}; pod::AABB box = { {-0.5f,-0.5f,-0.5f}, {0.5f,0.5f,0.5f} }; - auto& boxBody = uf::physics::impl::create(world, objBox, box, 1.0f); - boxBody.transform->position = {0,0.25f,0}; // overlaps plane + auto& bodyB = uf::physics::impl::create(world, objBox, box, 1.0f); + bodyB.transform->position = {0,0.25f,0}; // overlaps plane + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = meshAabb(meshBody, boxBody, m); + bool collided = meshAabb(bodyA, bodyB, m); EXPECT_TRUE(collided); EXPECT_TRUE(!m.points.empty()); }) @@ -739,15 +834,18 @@ TEST(MeshAabb_NoCollision, { uf::Object objMesh, objBox; auto mesh = ::generateMesh(2.0f); - auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); - meshBody.transform->position = {0,0,0}; + auto& bodyA = uf::physics::impl::create(world, objMesh, mesh, 0.0f); + bodyA.transform->position = {0,0,0}; pod::AABB box = { {-0.5f,-0.5f,-0.5f}, {0.5f,0.5f,0.5f} }; - auto& boxBody = uf::physics::impl::create(world, objBox, box, 1.0f); - boxBody.transform->position = {0,5.0f,0}; // above plane, no overlap + auto& bodyB = uf::physics::impl::create(world, objBox, box, 1.0f); + bodyB.transform->position = {0,5.0f,0}; // above plane, no overlap + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = meshAabb(meshBody, boxBody, m); + bool collided = meshAabb(bodyA, bodyB, m); EXPECT_FALSE(collided); }) @@ -756,8 +854,10 @@ TEST(RayMesh_Hit, { uf::Object objMesh; auto mesh = ::generateMesh(1.0f); - auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); - meshBody.transform->position = {0,0,0}; + auto& bodyA = uf::physics::impl::create(world, objMesh, mesh, 0.0f); + bodyA.transform->position = {0,0,0}; + + ::buildBroadphaseBVH( world.bvh, world.bodies ); pod::Ray ray{ {0,1,0}, {0,-1,0} }; // from above, pointing down pod::RayQuery hit = uf::physics::impl::rayCast(ray, world, 100.0f); @@ -771,8 +871,10 @@ TEST(RayMesh_Miss, { uf::Object objMesh; auto mesh = ::generateMesh(1.0f); - auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); - meshBody.transform->position = {0,0,0}; + auto& bodyA = uf::physics::impl::create(world, objMesh, mesh, 0.0f); + bodyA.transform->position = {0,0,0}; + + ::buildBroadphaseBVH( world.bvh, world.bodies ); pod::Ray ray{ {0,2,0}, {1,0,0} }; // parallel, goes sideways pod::RayQuery hit = uf::physics::impl::rayCast(ray, world, 100.0f); @@ -786,14 +888,17 @@ TEST(MeshMesh_Collision, { auto mesh = ::generateMesh(1.0f); - auto& meshA = uf::physics::impl::create(world, objA, mesh, 0.0f); - auto& meshB = uf::physics::impl::create(world, objB, mesh, 0.0f); + auto& bodyA = uf::physics::impl::create(world, objA, mesh, 0.0f); + auto& bodyB = uf::physics::impl::create(world, objB, mesh, 0.0f); - meshA.transform->position = {0,0,0}; - meshB.transform->position = {0,0,0}; // same location + bodyA.transform->position = {0,0,0}; + bodyB.transform->position = {0,0,0}; // same location + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = meshMesh(meshA, meshB, m); + bool collided = meshMesh(bodyA, bodyB, m); EXPECT_TRUE(collided); }) @@ -803,187 +908,344 @@ TEST(MeshMesh_NoCollision, { auto mesh = ::generateMesh(1.0f); - auto& meshA = uf::physics::impl::create(world, objA, mesh, 0.0f); - auto& meshB = uf::physics::impl::create(world, objB, mesh, 0.0f); + auto& bodyA = uf::physics::impl::create(world, objA, mesh, 0.0f); + auto& bodyB = uf::physics::impl::create(world, objB, mesh, 0.0f); - meshA.transform->position = {0,0,0}; - meshB.transform->position = {0,10.0f,0}; // too far apart + bodyA.transform->position = {0,0,0}; + bodyB.transform->position = {0,10.0f,0}; // too far apart + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = meshMesh(meshA, meshB, m); + bool collided = meshMesh(bodyA, bodyB, m); EXPECT_FALSE(collided); }) -#endif -#define EPS 1.0e-4 -#if 0 TEST(TriangleTriangle_Collision_SimpleOverlap, { - // Two identical triangles overlapping on XY plane - pod::TriangleWithNormal triA { + pod::World world; + uf::Object objA, objB; + + pod::TriangleWithNormal triA{ { { {0,0,0}, {1,0,0}, {0,1,0} } }, { {0,0,1}, {0,0,1}, {0,0,1} }, }; - pod::TriangleWithNormal triB { - { { {0.25f,0.25f,0}, {1.25f,0.25f,0}, {0.25f,1.25f,0} } }, - { {0,0,1}, {0,0,1}, {0,0,1} }, + pod::TriangleWithNormal triB{ + { { {0.2f,0.2f,0}, {0.8f,0.2f,0}, {0.2f,0.8f,0} } }, + { {0,0,1}, {0,0,1}, {0,0,1} }, }; - pod::Manifold m; - bool collided = triangleTriangle( triA, triB, m, EPS ); + auto& bodyA = uf::physics::impl::create( world, objA, triA, 0.0f ); + auto& bodyB = uf::physics::impl::create( world, objA, triB, 0.0f ); - EXPECT_TRUE( collided ); + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); + + pod::Manifold m; + bool collided = ::triangleTriangle(bodyA, bodyB, m, EPS(1e-6f)); + + EXPECT_TRUE(collided); + EXPECT_FALSE(m.points.empty()); if ( !m.points.empty() ) { - EXPECT_NEAR(m.points[0].point.z, 0, EPS); // contact should be on z=0 plane - EXPECT_NEAR(uf::vector::norm(m.points[0].normal), 1.0f, EPS); EXPECT_GE(m.points[0].penetration, 0.0f); + EXPECT_NEAR(uf::vector::norm(m.points[0].normal), 1.0f, EPS(1e-6f)); } }) -#endif -TEST(TriangleAabb_Collision_CenterInside, { +TEST(TriangleTriangle_Collision_CoplanarOverlap, { + pod::World world; + uf::Object objA, objB; + + pod::TriangleWithNormal triA{ + { { {0,0,0}, {2,0,0}, {0,2,0} } }, + { {0,0,1}, {0,0,1}, {0,0,1} }, + }; + pod::TriangleWithNormal triB{ + { { {1,1,0}, {2,1,0}, {1,2,0} } }, + { {0,0,1}, {0,0,1}, {0,0,1} }, + }; + + auto& bodyA = uf::physics::impl::create(world, objA, triA, 0.0f); + auto& bodyB = uf::physics::impl::create(world, objB, triB, 0.0f); + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); + + pod::Manifold m; + bool collided = ::triangleTriangle(bodyA, bodyB, m, EPS(1e-6f)); + + EXPECT_TRUE(collided); + EXPECT_FALSE(m.points.empty()); +}) + +TEST(TriangleTriangle_Collision_TouchingEdge, { + pod::World world; + uf::Object objA, objB; + + pod::TriangleWithNormal triA{ + { { {0,0,0}, {1,0,0}, {0.5f,1,0} } }, + { {0,0,1}, {0,0,1}, {0,0,1} }, + }; + pod::TriangleWithNormal triB{ + { { {0.5f,1,0}, {1.5f,0,0}, {1,1,0} } }, + { {0,0,1}, {0,0,1}, {0,0,1} }, + }; + + auto& bodyA = uf::physics::impl::create(world, objA, triA, 0.0f); + auto& bodyB = uf::physics::impl::create(world, objB, triB, 0.0f); + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); + + pod::Manifold m; + bool collided = ::triangleTriangle(bodyA, bodyB, m, EPS(1e-6f)); + + // Should still report as collision (tangent contact) + EXPECT_TRUE(collided); + EXPECT_FALSE(m.points.empty()); + if(!m.points.empty()) { + EXPECT_NEAR(uf::vector::norm(m.points[0].normal), 1.0f, EPS(1e-6f)); + } +}) + +TEST(TriangleAabb_Collision_OverlapCenter, { + pod::World world; + uf::Object objA, objB; + pod::TriangleWithNormal tri { { { {0,0,0}, {1,0,0}, {0,1,0} } }, { {0,0,1}, {0,0,1}, {0,0,1} }, }; - // Make cube overlapping - pod::RigidBody box; - box.collider.type = pod::ShapeType::AABB; - box.collider.u.aabb.min = {0.25f, 0.25f, -0.1f}; - box.collider.u.aabb.max = {0.75f, 0.75f, +0.1f}; - box.transform->position = ::aabbCenter( box.collider.u.aabb ); - box.bounds = ::computeAABB( box ); + pod::AABB box = {{0.25f, 0.25f, -0.25f}, {0.75f, 0.75f, +0.25f}}; + + auto& bodyA = uf::physics::impl::create( world, objA, tri, 0.0f ); + auto& bodyB = uf::physics::impl::create( world, objB, box, 0.0f ); + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = triangleAabb(tri, box, m, EPS); + bool collided = ::triangleAabb(bodyA, bodyB, m, EPS(1e-6f)); EXPECT_TRUE(collided); + EXPECT_FALSE(m.points.empty()); if ( !m.points.empty() ) { - EXPECT_NEAR(uf::vector::norm(m.points[0].normal), 1.0f, EPS); EXPECT_GE(m.points[0].penetration, 0.0f); - - UF_MSG_DEBUG("contact={}, normal={}, depth={}", uf::vector::toString( m.points[0].point ), uf::vector::toString( m.points[0].normal ), m.points[0].penetration ); } }) -TEST(TriangleSphere_Collision_SphereTouchingTriangle, { +TEST(TriangleAabb_Collision_NoOverlap, { + pod::World world; + uf::Object objA, objB; + pod::TriangleWithNormal tri { - { { {0,-1,0}, {1,1,0}, {-1,1,0} } }, + { { {0,0,0}, {1,0,0}, {0,1,0} } }, { {0,0,1}, {0,0,1}, {0,0,1} }, }; - pod::RigidBody sphere; - sphere.collider.type = pod::ShapeType::SPHERE; - sphere.collider.u.sphere.radius = 1.0f; - sphere.transform->position = {0,0,0.5f}; // Place sphere just above triangle so it penetrates slightly - sphere.bounds = ::computeAABB( sphere ); + pod::AABB box = {{2,2,2}, {3,3,3}}; + + auto& bodyA = uf::physics::impl::create( world, objA, tri, 0.0f ); + auto& bodyB = uf::physics::impl::create( world, objB, box, 0.0f ); + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = triangleSphere(tri, sphere, m, EPS); - - EXPECT_TRUE(collided); - if ( !m.points.empty() ) { - EXPECT_NEAR(m.points[0].point.z, 0.0f, 0.5f); - EXPECT_NEAR(uf::vector::norm(m.points[0].normal), 1.0f, EPS); - - UF_MSG_DEBUG("contact={}, normal={}, depth={}", uf::vector::toString( m.points[0].point ), uf::vector::toString( m.points[0].normal ), m.points[0].penetration ); - EXPECT_GE(m.points[0].penetration, 0.0f); - } + bool collided = ::triangleAabb(bodyA, bodyB, m, EPS(1e-6f)); + EXPECT_FALSE(collided); }) -TEST(TriangleCapsule_Collision_CapsuleIntersectingEdge, { - pod::TriangleWithNormal tri { - { { {0,0,0}, {2,0,0}, {1,1,0} } }, - { {0,0,1}, {0,0,1}, {0,0,1} }, +TEST(TrianglePlane_Collision_BelowPlane, { + pod::World world; + uf::Object objA, objB; + + // Plane: z=0 with upward normal + pod::Plane plane = { {0,0,1}, 0.0f }; + + pod::TriangleWithNormal tri{ + { { {0,0,-0.1f}, {1,0,-0.1f}, {0,1,-0.1f} } }, + { {0,0,1}, {0,0,1}, {0,0,1} }, }; - pod::RigidBody capsule; - capsule.collider.type = pod::ShapeType::CAPSULE; - capsule.collider.u.capsule.radius = 0.5f; - auto [ p1, p2 ] = std::pair{ pod::Vector3f{1,0.5f,-1}, pod::Vector3f{1,0.5f,1} }; - capsule.bounds = computeSegmentAABB(p1, p2, capsule.collider.u.capsule.radius); + auto& bodyA = uf::physics::impl::create(world, objA, tri, 0.0f); + auto& bodyB = uf::physics::impl::create(world, objB, plane, 0.0f); + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = triangleCapsule(tri, capsule, m, EPS); + bool collided = ::trianglePlane(bodyA, bodyB, m, EPS(1e-6f)); EXPECT_TRUE(collided); - if ( !m.points.empty() ) { - EXPECT_NEAR(uf::vector::norm(m.points[0].normal), 1.0f, EPS); + EXPECT_FALSE(m.points.empty()); + if (!m.points.empty()) { EXPECT_GE(m.points[0].penetration, 0.0f); - - UF_MSG_DEBUG("contact={}, normal={}, depth={}", uf::vector::toString( m.points[0].point ), uf::vector::toString( m.points[0].normal ), m.points[0].penetration ); } }) -TEST(TriangleSphere_Collision_SphereTangentFace, { - // Triangle is in Z=0 plane - pod::TriangleWithNormal tri { - { { {0,-1,0}, {1,1,0}, {-1,1,0} } }, - { {0,0,1}, {0,0,1}, {0,0,1} }, - }; +TEST(TrianglePlane_NoCollision_AbovePlane, { + pod::World world; + uf::Object objA, objB; - pod::RigidBody sphere; - sphere.collider.type = pod::ShapeType::SPHERE; - sphere.collider.u.sphere.radius = 1.0f; - sphere.transform->position = {0, 0, 1.0f}; // center exactly 1 unit above plane - sphere.bounds = ::computeAABB(sphere); + pod::Plane plane = { {0,0,1}, 0.0f }; - pod::Manifold m; - bool collided = triangleSphere(tri, sphere, m, EPS); + pod::TriangleWithNormal tri{ + { { {0,0,1}, {1,0,1}, {0,1,1} } }, + { {0,0,-1}, {0,0,-1}, {0,0,-1} }, // facing down, but above plane + }; - // Should either detect a grazing collision or at least not error - EXPECT_TRUE(collided); - if (!m.points.empty()) { - EXPECT_NEAR(m.points[0].penetration, 0.0f, 1e-4f); - EXPECT_NEAR(uf::vector::norm(m.points[0].normal), 1.0f, EPS); - } + auto& bodyA = uf::physics::impl::create(world, objA, tri, 0.0f); + auto& bodyB = uf::physics::impl::create(world, objB, plane, 0.0f); + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); + + pod::Manifold m; + bool collided = ::trianglePlane(bodyA, bodyB, m, EPS(1e-6f)); + EXPECT_FALSE(collided); }) -TEST(TriangleSphere_Collision_SphereTangentEdge, { - // Triangle tilted in XY plane, edge from (0,0,0) to (1,0,0) - pod::TriangleWithNormal tri { - { { {0,0,0}, {1,0,0}, {0,1,0} } }, - { {0,0,1}, {0,0,1}, {0,0,1} }, - }; +TEST(TriangleSphere_Collision_Tangent, { + pod::World world; + uf::Object objA, objB; - pod::RigidBody sphere; - sphere.collider.type = pod::ShapeType::SPHERE; - sphere.collider.u.sphere.radius = 0.5f; + pod::TriangleWithNormal tri{ + { { {0,0,0}, {1,0,0}, {0,1,0} } }, + { {0,0,1}, {0,0,1}, {0,0,1} }, + }; - // Place sphere center exactly 0.5 units away from edge line - sphere.transform->position = {0.5f, -0.5f, 0.0f}; - sphere.bounds = ::computeAABB(sphere); + pod::Sphere sphere = { 0.5f }; // radius - pod::Manifold m; - bool collided = triangleSphere(tri, sphere, m, EPS); + auto& bodyA = uf::physics::impl::create(world, objA, tri, 0.0f); + auto& bodyB = uf::physics::impl::create(world, objB, sphere, 0.0f); + bodyB.transform->position = pod::Vector3f{0.25f, 0.25f, 0.5f}; // exactly tangent above - EXPECT_TRUE(collided); // Tangential along edge - if (!m.points.empty()) { - EXPECT_NEAR(m.points[0].penetration, 0.0f, 1e-4f); - } + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); + + pod::Manifold m; + bool collided = ::triangleSphere(bodyA, bodyB, m, EPS(1e-6f)); + + // At tangency: considered collision + EXPECT_TRUE(collided); + EXPECT_FALSE(m.points.empty()); + if(!m.points.empty()) { + EXPECT_NEAR(m.points[0].penetration, 0.0f, EPS(1e-6f)); + } }) -TEST(TriangleCapsule_Collision_TangentVertex, { - pod::TriangleWithNormal tri { - { { {0,0,0}, {1,0,0}, {0,1,0} } }, - { {0,0,1}, {0,0,1}, {0,0,1} }, - }; +TEST(TriangleCapsule_Collision_Overlap, { + pod::World world; + uf::Object objA, objB; - pod::RigidBody capsule; - capsule.collider.type = pod::ShapeType::CAPSULE; - capsule.collider.u.capsule.radius = 0.25f; + pod::TriangleWithNormal tri{ + { { {0,0,0}, {1,0,0}, {0,1,0} } }, + { {0,0,1}, {0,0,1}, {0,0,1} }, + }; - // Align segment so it hovers exactly through the vertex (0,0,0) - pod::Vector3f p1{0.0f, -0.25f, 0.0f}; - pod::Vector3f p2{0.0f, -0.25f, 1.0f}; + // Capsule aligned along Z axis, radius 0.2 + pod::Capsule capsule; + capsule.radius = 0.2f; + capsule.halfHeight = 1.0f; // segment lengt * 0.5fh + // placed so capsule overlaps the tri plane + auto& bodyA = uf::physics::impl::create(world, objA, tri, 0.0f); + auto& bodyB = uf::physics::impl::create(world, objB, capsule, 0.0f); + bodyB.transform->position = {0.25f, 0.25f, 0.1f}; - capsule.bounds = computeSegmentAABB(p1, p2, capsule.collider.u.capsule.radius); + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); - pod::Manifold m; - bool collided = triangleCapsule(tri, capsule, m, EPS); + pod::Manifold m; + bool collided = ::triangleCapsule(bodyA, bodyB, m, EPS(1e-6f)); - EXPECT_TRUE(collided); - if (!m.points.empty()) { - EXPECT_NEAR(m.points[0].penetration, 0.0f, 1e-4f); - } + EXPECT_TRUE(collided); + EXPECT_FALSE(m.points.empty()); + if(!m.points.empty()) { + EXPECT_GE(m.points[0].penetration, 0.0f); + } +}) + +TEST(TriangleCapsule_Collision_NoOverlap, { + pod::World world; + uf::Object objA, objB; + + pod::TriangleWithNormal tri{ + { { {0,0,0}, {1,0,0}, {0,1,0} } }, + { {0,0,1}, {0,0,1}, {0,0,1} }, + }; + + pod::Capsule capsule; + capsule.radius = 0.2f; + capsule.halfHeight = 1.0f * 0.5f; + // place it well above the tri plane + auto& bodyA = uf::physics::impl::create(world, objA, tri, 0.0f); + auto& bodyB = uf::physics::impl::create(world, objB, capsule, 0.0f); + bodyB.transform->position = {0.5f, 0.5f, 2.0f}; + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); + + pod::Manifold m; + bool collided = ::triangleCapsule(bodyA, bodyB, m, EPS(1e-6f)); + EXPECT_FALSE(collided); +}) + +TEST(TriangleCapsule_Collision_Tangent, { + pod::World world; + uf::Object objA, objB; + + pod::TriangleWithNormal tri{ + { { {0,0,0}, {1,0,0}, {0,1,0} } }, + { {0,0,1}, {0,0,1}, {0,0,1} }, + }; + + pod::Capsule capsule; + capsule.radius = 0.5f; + capsule.halfHeight = 1.0f * 0.5f; + auto& bodyA = uf::physics::impl::create(world, objA, tri, 0.0f); + auto& bodyB = uf::physics::impl::create(world, objB, capsule, 0.0f); + // place the capsule so its sphere-bottom just kisses the triangle + bodyB.transform->position = {0.2f, 0.2f, 0.5f}; + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); + + pod::Manifold m; + bool collided = ::triangleCapsule(bodyA, bodyB, m, EPS(1e-6f)); + + // At tangency, should still count as collision (penetration ≈ 0) + EXPECT_TRUE(collided); + EXPECT_FALSE(m.points.empty()); + if(!m.points.empty()) { + EXPECT_NEAR(m.points[0].penetration, 0.0f, EPS(1e-6f)); + } +}) + + +TEST(TriangleCapsule_Collision_EdgeAlignment, { + pod::World world; + uf::Object objA, objB; + + pod::TriangleWithNormal tri{ + { { {0,0,0}, {2,0,0}, {0,2,0} } }, + { {0,0,1}, {0,0,1}, {0,0,1} }, + }; + + pod::Capsule capsule; + capsule.radius = 0.1f; + capsule.halfHeight = 2.0f; // segment tall and skinn * 0.5fy + auto& bodyA = uf::physics::impl::create(world, objA, tri, 0.0f); + auto& bodyB = uf::physics::impl::create(world, objB, capsule, 0.0f); + // lay the capsule along edge (x-axis direction near tri’s base) + bodyB.transform->position = {1.0f, -0.05f, 0.0f}; + + bodyA.bounds = ::computeAABB( bodyA ); + bodyB.bounds = ::computeAABB( bodyB ); + + pod::Manifold m; + bool collided = ::triangleCapsule(bodyA, bodyB, m, EPS(1e-6f)); + + EXPECT_TRUE(collided); }) \ No newline at end of file diff --git a/engine/src/utils/math/physics/triangle.inl b/engine/src/utils/math/physics/triangle.inl index 70034279..f4404b20 100644 --- a/engine/src/utils/math/physics/triangle.inl +++ b/engine/src/utils/math/physics/triangle.inl @@ -25,7 +25,7 @@ namespace { return uf::vector::normalize(uf::vector::cross(tri.points[1] - tri.points[0], tri.points[2] - tri.points[0])); } pod::Vector3f triangleNormal( const pod::TriangleWithNormal& tri ) { - return uf::vector::normalize(( tri.normals[0] + tri.normals[1] + tri.normals[2] ) / 3.0f); + return uf::vector::normalize( tri.normals[0] + tri.normals[1] + tri.normals[2] ); } pod::AABB computeTriangleAABB( const void* vertices, size_t vertexStride, const void* indexData, size_t indexSize, size_t triID ) { @@ -126,28 +126,135 @@ namespace { return tri; } + + bool computeTriangleTriangleSegment( const pod::TriangleWithNormal& A, const pod::TriangleWithNormal& B, pod::Vector3f& p0, pod::Vector3f& p1, float eps = EPS(1e-6f) ) { + uf::stl::vector intersections; + + auto checkAndPush = [&]( const pod::Vector3f& pt ) { + // avoid duplicates + for ( auto& v : intersections ) { + if ( uf::vector::distanceSquared( v, pt ) < eps*eps ) return; + } + intersections.emplace_back(pt); + }; + + // segment-plane intersection + auto intersectSegmentPlane = [&](const pod::Vector3f& a, const pod::Vector3f& b, const pod::Vector3f& n, float d, pod::Vector3f& out)->bool { + pod::Vector3f ab = b - a; + float denom = uf::vector::dot( n, ab ); + if (fabs(denom) < eps) return false; // parallel + + float t = (d - uf::vector::dot( n, a )) / denom; + if ( t < -eps || t > 1.0f + eps ) return false; + out = a + ab * t; + return true; + }; + + // planes + auto nA = ::triangleNormal( A ); + auto nB = ::triangleNormal( B ); + float dA = uf::vector::dot( nA, A.points[0] ); + float dB = uf::vector::dot( nB, B.points[0] ); + + // clip edges of A against plane of B + const pod::Vector3f At[3] = { A.points[0], A.points[1], A.points[2] }; + for ( auto i = 0; i < 3; ++i ) { + int j = ( i + 1 ) % 3; + pod::Vector3f p; + if ( intersectSegmentPlane( At[i], At[j], nB, dB, p ) ) { + // check if intersection lies inside triangle B + if ( ::pointInTriangle( p, B ) ) checkAndPush(p); + } + } + + // clip edges of B against plane of A + const pod::Vector3f Bt[3] = { B.points[0], B.points[1], B.points[2] }; + for ( auto i = 0; i < 3; ++i ) { + int j = ( i + 1 ) % 3; + pod::Vector3f p; + if ( intersectSegmentPlane( Bt[i], Bt[j], nA, dA, p ) ) { + if ( ::pointInTriangle( p, A ) ) checkAndPush(p); + } + } + + if ( intersections.empty() ) return false; + + // degenerate intersection + if ( intersections.size() == 1 ) { + p0 = p1 = intersections[0]; + return true; + } + + // find two furthest apart points for intersection segment + float maxDist2 = -1.0f; + for ( auto i = 0 ; i < intersections.size(); i++ ) { + for ( auto j = i + 1 ; j maxDist2 ) { + maxDist2 = d2; + p0 = intersections[i]; + p1 = intersections[j]; + } + } + } + + return maxDist2 >= 0.0f; + } + + pod::Vector2f projectTriangleOntoAxis( const pod::TriangleWithNormal& tri, const pod::Vector3f& axis ) { + pod::Vector3f normal = uf::vector::normalize( axis ); + + float p0 = uf::vector::dot( tri.points[0], normal ); + float p1 = uf::vector::dot( tri.points[1], normal ); + float p2 = uf::vector::dot( tri.points[2], normal ); + + return { std::min({ p0, p1, p2 }), std::max({ p0, p1, p2 }) }; + } } -/* - -#if REORIENT_NORMALS_ON_CONTACT - if ( uf::vector::dot(normal, delta) < 0.0f ) normal = -normal; -#endif - - -// uf::vector::normalize( ::interpolateWithBarycentric( ::computeBarycentric( contact, tri ), tri.normals ) ); - -*/ - // triangle colliders namespace { bool triangleTriangle( const pod::TriangleWithNormal& a, const pod::TriangleWithNormal& b, pod::Manifold& manifold, float eps ) { - if ( !::triangleTriangleIntersect( a, b ) ) return false; + // if ( !::triangleTriangleIntersect( a, b ) ) return false; + + uf::stl::vector axes = { ::triangleNormal( a ), ::triangleNormal( b ) }; + + pod::Vector3f p0 = {}, p1 = {}; + if ( !::computeTriangleTriangleSegment(a, b, p0, p1) ) { + auto contact = ( p0 + p1 ) * 0.5f; + auto normal = uf::vector::normalize( axes[0] + axes[1] ); + manifold.points.emplace_back(pod::Contact{ contact, normal, eps }); + return true; + } + + auto contact = ( p0 + p1 ) * 0.5f; + float penetration = std::numeric_limits::max(); + pod::Vector3f normal; + + // check edge cross-products + for ( auto i = 0; i < 3; i++ ) { + auto ea = a.points[( i + 1 ) % 3] - a.points[i]; + for ( auto j = 0; j < 3; j++ ) { + auto eb = b.points[( j + 1 ) % 3] - b.points[j]; + auto axis = uf::vector::cross(ea, eb); + if ( uf::vector::magnitude( axis ) > eps*eps ) axes.emplace_back( axis ); + } + } + + // project onto each axis + for ( auto axis : axes ) { + axis = uf::vector::normalize( axis ); + pod::Vector2f aP = ::projectTriangleOntoAxis( a, axis ); + pod::Vector2f bP = ::projectTriangleOntoAxis( b, axis ); + + float overlap = std::min( aP.x, bP.x ) - std::max( aP.y, bP.y ); + if ( overlap < 0) return false; // separating axis + if ( overlap < penetration ) { + penetration = overlap; + normal = axis; + } + } - // to-do: properly derive the contact information - auto contact = ( a.points[0] + b.points[0] ) * 0.5f; // center point - auto normal = ::triangleNormal( a ); - float penetration = 0.001f; manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); return true; } @@ -168,7 +275,7 @@ namespace { float dist = std::sqrt( dist2 ); // to-do: properly derive the contact information - auto contact = closest; + auto contact = closest; // ( closest + closestAabb ) * 0.5f; auto normal = ( dist > eps ) ? ( delta / dist ) : ::triangleNormal( tri ); float penetration = tolerance - dist; @@ -184,17 +291,17 @@ namespace { float r = sphere.collider.u.sphere.radius; auto center = ::getPosition( sphere ); - auto closest = ::closestPointOnTriangle( center, tri ); + auto closest = ::closestPointOnTriangle( center, tri.points[0], tri.points[1], tri.points[2] ); if ( !uf::vector::isValid( closest ) ) return false; // to-do: derive proper delta auto delta = center - closest; - float dist2 = uf::vector::dot(delta, delta); + float dist2 = uf::vector::dot( delta, delta ); if ( dist2 > r * r ) return false; float dist = std::sqrt(dist2); - auto contact = closest; + auto contact = ( center + closest ) * 0.5f; auto normal = ( dist > eps ) ? ( delta / dist ) : ::triangleNormal( tri ); float penetration = r - dist; @@ -205,9 +312,50 @@ namespace { manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); return true; } - // to-do + // to-do: implement bool trianglePlane( const pod::TriangleWithNormal& tri, const pod::RigidBody& body, pod::Manifold& manifold, float eps ) { - return false; + const auto& plane = body; + auto normal = plane.collider.u.plane.normal; + float d = plane.collider.u.plane.offset; + + bool hit = false; + pod::Vector3f dist; + for ( auto i = 0; i < 3; i++ ) dist[i] = uf::vector::dot(normal, tri.points[i] ) - d; + + // completely on one side + bool allAbove = ( dist.x > eps && dist.y > eps && dist.z > eps ); + bool allBelow = ( dist.x < -eps && dist.y < -eps && dist.z < -eps ); + if ( allAbove ) + return hit; + + if ( allBelow ) { + hit = true; + for ( auto i = 0; i < 3; i++ ) { + float penetration = -dist[i]; + manifold.points.emplace_back(pod::Contact{tri.points[i], normal, penetration}); + } + return hit; + } + + // points touching plane + for ( auto i = 0; i < 3; i++ ) + if ( fabs( dist[i] ) <= eps ) { + hit = true; + manifold.points.emplace_back(pod::Contact{ tri.points[i], normal, 0.0f }); + } + + // edges that cross plane + for ( auto i = 0; i < 3; i++ ) { + auto j = (i + 1) % 3; + if ( ( dist[i] > 0 && dist[j] < 0 ) || ( dist[i] < 0 && dist[j] > 0 ) ) { + hit = true; + float t = dist[i] / ( dist[i] - dist[j] ); + auto contact = tri.points[i] + ( tri.points[j] - tri.points[i] ) * t; + float penetration = -std::min( dist[i], dist[j] ); + manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); + } + } + return hit; } bool triangleCapsule( const pod::TriangleWithNormal& tri, const pod::RigidBody& body, pod::Manifold& manifold, float eps ) { const auto& capsule = body; @@ -226,7 +374,7 @@ namespace { auto delta = ( closestSeg - closest ); // to-do: properly derive the contact information - auto contact = closest; + auto contact = closest; // ( closestSeg + closest ) * 0.5f; auto normal = ( dist > eps ) ? ( delta / dist ) : ::triangleNormal( tri ); float penetration = r - dist; @@ -237,4 +385,25 @@ namespace { manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); return true; } + + bool triangleTriangle( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( TRIANGLE, TRIANGLE ); + return ::triangleTriangle( a.collider.u.triangle, b.collider.u.triangle, manifold, eps ); + } + bool triangleAabb( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( TRIANGLE, AABB ); + return ::triangleAabb( a.collider.u.triangle, b, manifold, eps ); + } + bool triangleSphere( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( TRIANGLE, SPHERE ); + return ::triangleSphere( a.collider.u.triangle, b, manifold, eps ); + } + bool trianglePlane( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( TRIANGLE, PLANE ); + return ::trianglePlane( a.collider.u.triangle, b, manifold, eps ); + } + bool triangleCapsule( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { + ASSERT_COLLIDER_TYPES( TRIANGLE, CAPSULE ); + return ::triangleCapsule( a.collider.u.triangle, b, manifold, eps ); + } } \ No newline at end of file