From 47d7f7cc7e091142c08d11dc7d2e493c5d34f536 Mon Sep 17 00:00:00 2001 From: Erin Catto Date: Sat, 29 Aug 2026 21:47:53 -0700 Subject: [PATCH] clockwise option, joint is awake (#130) - b3MeshDef gets stride and clockwise option - Added b3Joint_IsAwake - Added b3Body_TimeOfImpactMover - #116 - Smoother mover collision: #141 - Shape casts now ignore backside collision on mesh and height-field triangles, same as ray casts. --- .github/issue_template.md | 11 +- .gitignore | 1 + include/box3d/box3d.h | 37 ++- include/box3d/constants.h | 7 + include/box3d/math_functions.h | 23 +- include/box3d/types.h | 56 +++- samples/mover.cpp | 6 + samples/mover.h | 1 + samples/sample_character.cpp | 225 +++++++++++++++- samples/sample_issues.cpp | 84 ++++++ samples/sample_joint.cpp | 65 ++++- src/body.c | 109 +++++++- src/capsule.c | 2 +- src/compound.c | 8 +- src/convex_manifold.c | 2 - src/distance.c | 40 ++- src/height_field.c | 213 ++++++++------- src/hull.c | 2 +- src/joint.c | 85 +++--- src/mesh.c | 235 +++++++++++----- src/mesh_contact.c | 2 - src/physics_world.c | 6 +- src/recording.c | 3 + src/recording.h | 4 +- src/recording_replay.c | 6 +- src/shape.c | 35 +-- src/sphere.c | 2 +- src/spherical_joint.c | 1 + test/CMakeLists.txt | 1 + test/main.c | 2 + test/test_body.c | 91 +++++++ test/test_body_query.c | 417 ++++++++++++++++++++++++++++ test/test_height_field.c | 151 +++++++++++ test/test_mesh.c | 355 ++++++++++++++++++++++++ test/test_mover.c | 477 ++++++++++++++++++++++++++++++++- 35 files changed, 2464 insertions(+), 301 deletions(-) create mode 100644 test/test_mesh.c diff --git a/.github/issue_template.md b/.github/issue_template.md index abbe499..5c930c6 100644 --- a/.github/issue_template.md +++ b/.github/issue_template.md @@ -1 +1,10 @@ -Thank you for filing an issue! + +## Reporting bugs + +Bugs can be filed by humans or LLMs. Please help me reproduce your bug. Include a sample, unit test, or recording if possible. Recordings +should make note of the Box3D commit used (the SHA). + +## Feature requests + +Please don't disguise feature requests as bugs. Feature requests must be human typed because they usually involve discussion. +I'm likely to close feature requests otherwise. diff --git a/.gitignore b/.gitignore index dc1cd05..9e9caaf 100644 --- a/.gitignore +++ b/.gitignore @@ -14,3 +14,4 @@ CMakeUserPresets.json *.b3rec Superluminal/ build-dp/ +AGENTS.md diff --git a/include/box3d/box3d.h b/include/box3d/box3d.h index a6eba4d..2e82590 100644 --- a/include/box3d/box3d.h +++ b/include/box3d/box3d.h @@ -82,6 +82,7 @@ B3_API b3TreeStats b3World_OverlapShape( b3WorldId worldId, b3Pos origin, const /// Cast a ray into the world to collect shapes in the path of the ray. /// Your callback function controls whether you get the closest point, any point, or n-points. +/// Ignores back-side collision on meshes and height-fields. /// @note The callback function may receive shapes in any order /// @param worldId The world to cast the ray against /// @param origin The start point of the ray @@ -94,19 +95,20 @@ B3_API b3TreeStats b3World_CastRay( b3WorldId worldId, b3Pos origin, b3Vec3 tran b3CastResultFcn* fcn, void* context ); /// Cast a ray into the world to collect the closest hit. This is a convenience function. Ignores initial overlap. -/// This is less general than b3World_CastRay() and does not allow for custom filtering. +/// This is less general than b3World_CastRay() and does not allow for custom filtering. Ignores back-side collision +/// on meshes and height-fields. B3_API b3RayResult b3World_CastRayClosest( b3WorldId worldId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter ); /// Cast a shape through the world. Similar to a cast ray except that a shape is cast instead of a point. /// The proxy points are relative to the origin and the hit points come back as world positions, so the -/// cast stays precise far from the world origin. +/// cast stays precise far from the world origin. Ignores back-side collision on meshes and height-fields. /// @see b3World_CastRay B3_API b3TreeStats b3World_CastShape( b3WorldId worldId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation, b3QueryFilter filter, b3CastResultFcn* fcn, void* context ); /// Cast a capsule mover through the world. This is a special shape cast that handles sliding along other shapes while reducing /// clipping. This is not a good source of information about what the mover is touching. Instead use the planes returned by -/// b3World_CollideMover. +/// b3World_CollideMover. Ignores back-side collision on meshes and height-fields. /// @param worldId World to cast the mover against /// @param origin World position the mover capsule is relative to /// @param mover Capsule mover, relative to the origin @@ -120,6 +122,7 @@ B3_API float b3World_CastMover( b3WorldId worldId, b3Pos origin, const b3Capsule /// Collide a capsule mover with the world, gathering collision planes that can be fed to b3SolvePlanes. Useful for /// kinematic character movement. The mover and the returned planes are relative to the origin. +/// Ignores back-side collision on meshes and height-fields. B3_API void b3World_CollideMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mover, b3QueryFilter filter, b3PlaneResultFcn* fcn, void* context ); @@ -759,14 +762,25 @@ B3_API int b3Body_GetContactData( b3BodyId bodyId, b3ContactData* contactData, i /// If there are no shapes attached then the returned AABB is empty and centered on the body origin. B3_API b3AABB b3Body_ComputeAABB( b3BodyId bodyId ); +/// The minimum distance from any shape to the shape centroid. +B3_API float b3Body_GetMinExtent( b3BodyId bodyId ); + +/// The maximum extent vector from any point on the body shapes to the center of mass. +B3_API b3Vec3 b3Body_GetMaxExtent( b3BodyId bodyId ); + +/// The maximum extent vector from any point on the body shapes to the body origin. Conservative. +B3_API b3Vec3 b3Body_GetMaxExtentOrigin( b3BodyId bodyId ); + /// Get the closest point on a body to a world target. B3_API float b3Body_GetClosestPoint( b3BodyId bodyId, b3Vec3* result, b3Vec3 target ); /// Cast a ray at a specific body using a specified body transform. +/// Ignores back-side collision on meshes and height-fields. B3_API b3BodyCastResult b3Body_CastRay( b3BodyId bodyId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter, float maxFraction, b3WorldTransform bodyTransform ); /// Cast a shape at a specific body using a specified body transform. +/// Ignores back-side collision on meshes and height-fields. B3_API b3BodyCastResult b3Body_CastShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation, b3QueryFilter filter, float maxFraction, bool canEncroach, b3WorldTransform bodyTransform ); @@ -776,9 +790,17 @@ B3_API bool b3Body_OverlapShape( b3BodyId bodyId, b3Pos origin, const b3ShapePro b3WorldTransform bodyTransform ); /// Collide a character mover with a specific body using a specified body transform. +/// Only considers convex shapes on the body. B3_API int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int planeCapacity, b3Pos origin, const b3Capsule* mover, b3QueryFilter filter, b3WorldTransform bodyTransform ); +/// Perform a time of impact between a character mover and a body using specified sweep transforms. +/// Initial overlap of any shape on the body is ignored. A non-overlapped shape can still be hit. +/// Only considers convex shapes on the body. +B3_API b3BodyTOIResult b3Body_TimeOfImpactMover( b3BodyId bodyId, b3Pos origin, const b3Capsule* mover, b3Vec3 moverTranslation, + b3QueryFilter filter, b3WorldTransform bodyTransform1, + b3WorldTransform bodyTransform2 ); + /** @} */ // body /** @@ -786,7 +808,7 @@ B3_API int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, * Functions to create, destroy, and access. * Shapes bind raw geometry to bodies and hold material properties including friction and restitution. * You may add multiple shapes to a single body. There are no hard limits on shape count per body. - * + * * When you create a shape on a body the center of mass moves. This can lead to the body linear velocity * changing if the angular velocity is non-zero. * @{ @@ -1076,6 +1098,9 @@ B3_API void* b3Joint_GetUserData( b3JointId jointId ); /// Wake the bodies connect to this joint B3_API void b3Joint_WakeBodies( b3JointId jointId ); +/// Is the joint awake? If true then an attached body is awake. +B3_API bool b3Joint_IsAwake( b3JointId jointId ); + /// Get the current constraint force for this joint B3_API b3Vec3 b3Joint_GetConstraintForce( b3JointId jointId ); @@ -1552,7 +1577,9 @@ B3_API void b3SphericalJoint_EnableMotor( b3JointId jointId, bool enableMotor ); /// Is the spherical joint motor enabled? B3_API bool b3SphericalJoint_IsMotorEnabled( b3JointId jointId ); -/// Set the spherical joint motor velocity in radians per second +/// Set the spherical joint motor velocity in radians per second. This is the relative angular +/// velocity between the two bodies in world space. +/// motorVelocity = angularVelocityB - angularVelocityA B3_API void b3SphericalJoint_SetMotorVelocity( b3JointId jointId, b3Vec3 motorVelocity ); /// Get the spherical joint motor velocity in radians per second diff --git a/include/box3d/constants.h b/include/box3d/constants.h index 3ac6106..855d018 100644 --- a/include/box3d/constants.h +++ b/include/box3d/constants.h @@ -137,3 +137,10 @@ B3_API float b3GetStallThreshold( void ); #ifndef B3_RESTITUTION_ITERATIONS #define B3_RESTITUTION_ITERATIONS 1 #endif + +/// This is the limit on how many mesh or heightfield triangles a single convex shape can collide with. +/// Increasing this will increase stack usage, so be careful. I recommend to simplify your collision data +/// before increasing this. For example, using render mesh for collision often leads to poor performance. +#ifndef B3_MAX_MESH_CONTACT_TRIANGLES +#define B3_MAX_MESH_CONTACT_TRIANGLES 256 +#endif diff --git a/include/box3d/math_functions.h b/include/box3d/math_functions.h index 60c1292..a12f250 100644 --- a/include/box3d/math_functions.h +++ b/include/box3d/math_functions.h @@ -276,14 +276,12 @@ B3_INLINE float b3DistanceSquared( b3Vec3 a, b3Vec3 b ) B3_FORCE_INLINE b3Vec3 b3Normalize( b3Vec3 a ) { float lengthSquared = a.x * a.x + a.y * a.y + a.z * a.z; - if ( lengthSquared > 1000.0f * FLT_MIN ) { float s = 1.0f / sqrtf( lengthSquared ); b3Vec3 u = { s * a.x, s * a.y, s * a.z }; return u; } - return B3_LITERAL( b3Vec3 ){ 0.0f, 0.0f, 0.0f }; } @@ -291,15 +289,17 @@ B3_FORCE_INLINE b3Vec3 b3Normalize( b3Vec3 a ) /// if the input is very small. B3_INLINE b3Vec3 b3GetLengthAndNormalize( float* length, b3Vec3 a ) { - *length = b3Length( a ); - if ( *length < FLT_EPSILON ) + float lengthSquared = a.x * a.x + a.y * a.y + a.z * a.z; + if ( lengthSquared > 1000.0f * FLT_MIN ) { - return b3Vec3_zero; + *length = sqrtf( lengthSquared ); + float s = 1.0f / *length; + b3Vec3 u = { s * a.x, s * a.y, s * a.z }; + return u; } - float invLength = 1.0f / *length; - b3Vec3 n = { invLength * a.x, invLength * a.y, invLength * a.z }; - return n; + *length = 0.0f; + return B3_LITERAL( b3Vec3 ){ 0.0f, 0.0f, 0.0f }; } /// Get a unit vector that is perpendicular to the supplied vector. @@ -756,9 +756,10 @@ B3_INLINE b3WorldTransform b3MakeWorldTransform( b3Transform t ) return w; } -/// Translate a local AABB by a world origin, rounding outward so the float box always contains -/// the double box. Far from the origin a plain conversion could clip a shape out of its own box. -/// In float mode the origin is float and the rounding is a no-op. +/// Translate a local AABB by a world position, rounding outward so the single precision box +/// encloses the double precision box. Far from the origin a plain conversion could clip a +/// shape out of its own box. +/// In single precision mode the the rounding is a no-op. B3_INLINE b3AABB b3OffsetAABB( b3AABB localBox, b3Pos origin ) { b3AABB out; diff --git a/include/box3d/types.h b/include/box3d/types.h index 90d8d4a..19850d2 100644 --- a/include/box3d/types.h +++ b/include/box3d/types.h @@ -8,6 +8,7 @@ #include "id.h" #include "math_functions.h" +#include #include #define B3_DEFAULT_CATEGORY_BITS UINT64_MAX @@ -912,7 +913,9 @@ typedef struct b3SphericalJointDef /// The maximum motor torque, typically in newton-meters. Non-negative number. float maxMotorTorque; - /// The desired motor angular velocity in radians per second. + /// The desired motor angular velocity in radians per second. This is the relative angular + /// velocity between the two bodies in world space. + /// motorVelocity = angularVelocityB - angularVelocityA b3Vec3 motorVelocity; } b3SphericalJointDef; @@ -1620,31 +1623,31 @@ typedef enum b3TOIState b3_toiStateSeparated } b3TOIState; -/// Time of impact output +/// Time of impact output. typedef struct b3TOIOutput { - /// The type of result + /// The type of result. b3TOIState state; - /// The hit point + /// The hit point. A shared point if overlapped. b3Vec3 point; - /// The hit normal + /// The hit normal. Zero if overlapped. b3Vec3 normal; - /// The sweep time of the collision + /// The sweep time of the collision. 0 if overlapped. float fraction; - /// The final distance + /// The final distance. 0 if overlapped. float distance; - /// Number of outer iterations + /// Number of outer iterations. int distanceIterations; - /// Total number of push back iterations + /// Total number of push back iterations. int pushBackIterations; - /// Total number of root iterations + /// Total number of root iterations. int rootIterations; /// Indicates that the time of impact detected initial @@ -1817,6 +1820,15 @@ typedef struct b3PlaneResult /// Closest point on the shape. May not be unique. b3Vec3 point; + /// The index of the mesh or height field triangle hit. + int triangleIndex; + + /// The index of the compound child shape. + int childIndex; + + /// The material index. + int materialIndex; + } b3PlaneResult; /// These are collision planes that can be fed to b3SolvePlanes. Normally @@ -1857,6 +1869,22 @@ typedef struct b3BodyPlaneResult b3PlaneResult result; } b3BodyPlaneResult; +/// Body time of impact result for movers. +typedef struct b3BodyTOIResult +{ + /// The hit point in world space. + b3Pos point; + + /// The hit normal. Points from the body to the mover. + b3Vec3 normal; + + /// The sweep time of the collision. + float fraction; + + /// The hit shape. + b3ShapeId shapeId; +} b3BodyTOIResult; + /// Used to collect collision planes for character movers. /// Return true to continue gathering planes. typedef bool b3PlaneResultFcn( b3ShapeId shapeId, const b3PlaneResult* plane, int planeCount, void* context ); @@ -2068,7 +2096,10 @@ typedef struct b3MeshDef /// Triangle vertices. b3Vec3* vertices; - /// Triangle vertex indices. 3 for each triangle. CCW winding. + /// Stride between vertices. Use 0 for contiguous vertices. + size_t stride; + + /// Triangle vertex indices. 3 for each triangle. CCW winding unless CW is indicated below. int32_t* indices; /// Triangle material index. 1 per triangle. Indexes into b3ShapeDef::materials. @@ -2094,6 +2125,9 @@ typedef struct b3MeshDef /// Compute triangle adjacency information using shared edges bool identifyEdges; + + /// Input indices have clockWise winding order. + bool clockWiseWinding; } b3MeshDef; /// 64-bit mesh version. Useful for validating serialized data. diff --git a/samples/mover.cpp b/samples/mover.cpp index e2efd4c..4db9a13 100644 --- a/samples/mover.cpp +++ b/samples/mover.cpp @@ -221,6 +221,12 @@ void CharacterMover::SolveMove( float timeStep, b3Vec3 forward, b3Vec3 right, b3 continue; } + MoverShapeUserData* userData = static_cast( b3Shape_GetUserData( m_planeExtras[i].shapeId ) ); + if ( userData != nullptr && userData->canMoverPush == false ) + { + continue; + } + b3Pos point = m_planeExtras[i].point; b3Vec3 normal = b3Neg( m_planes[i].plane.normal ); diff --git a/samples/mover.h b/samples/mover.h index bde81ef..724b59b 100644 --- a/samples/mover.h +++ b/samples/mover.h @@ -11,6 +11,7 @@ struct MoverShapeUserData { float maxPush; bool clipVelocity; + bool canMoverPush; }; struct PlaneExtra diff --git a/samples/sample_character.cpp b/samples/sample_character.cpp index cbe2cbc..77b1c86 100644 --- a/samples/sample_character.cpp +++ b/samples/sample_character.cpp @@ -312,10 +312,19 @@ public: static int sampleMoverOverlap = RegisterSample( "Character", "MoverOverlap", MoverOverlap::Create ); -class BasicMover : public Sample +// What the mover was last struck with. Stays on screen until the next hit. +struct MoverImpact +{ + b3Pos point; + b3Vec3 normal; + float speed; + bool valid; +}; + +class GeometricMover : public Sample { public: - explicit BasicMover( SampleContext* context ) + explicit GeometricMover( SampleContext* context ) : Sample( context ) { b3Pos moverPosition = { 7.5f, 0.75f, 9.0f }; @@ -396,6 +405,19 @@ public: b3CreateHeightFieldShape( body, &shapeDef, m_heightField ); } + // The tree is longer than the height field is wide, so topple it along the diagonal. + { + b3Vec3 axis = { 1.0f, 0.0f, -1.0f }; + m_treeSpin = 0.5f * b3Normalize( axis ); + m_treeSpawn = { 22.0f, 1.5f, 2.0f }; + m_treeTimer = 0.0f; + m_treeShapeUserData.maxPush = 10.0f; + m_treeShapeUserData.clipVelocity = true; + m_treeShapeUserData.canMoverPush = false; + m_treeImpact = {}; + CreateFallingTree(); + } + { b3BodyDef bodyDef = b3DefaultBodyDef(); bodyDef.position = { 0.0f, 1.4f, 6.0f }; @@ -403,6 +425,7 @@ public: b3ShapeDef shapeDef = b3DefaultShapeDef(); m_enemyShape.maxPush = 1.0f; m_enemyShape.clipVelocity = true; + m_enemyShape.canMoverPush = false; b3Capsule capsule = { { 0.0f, -0.5f, 0.0f }, @@ -424,6 +447,7 @@ public: b3ShapeDef shapeDef = b3DefaultShapeDef(); m_friendlyShape.maxPush = 0.01f; m_friendlyShape.clipVelocity = false; + m_friendlyShape.canMoverPush = false; b3Capsule capsule = { { 0.0f, -0.5f, 0.0f }, @@ -449,6 +473,28 @@ public: b3CreateSphereShape( body, &shapeDef, &sphere ); } + // Falls through the mover start position on a loop to feed the mover time of impact. + { + m_fallingExtent = { 1.0f, 0.1, 1.0f }; + m_fallingSpawn = { 10.5f, 30.0f, 0.0f }; + m_fallingSpin = { 0.0f, 0.0f, 0.0f }; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = m_fallingSpawn; + bodyDef.angularVelocity = m_fallingSpin; + m_fallingBoxId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.customColor = b3_colorGold; + b3BoxHull box = b3MakeBoxHull( m_fallingExtent.x, m_fallingExtent.y, m_fallingExtent.z ); + b3CreateHullShape( m_fallingBoxId, &shapeDef, &box.base ); + + m_respawnTimer = 0.0f; + m_impactTransform = { b3Pos_zero, b3Quat_identity }; + m_boxImpact = {}; + } + { b3BodyDef bodyDef = b3DefaultBodyDef(); bodyDef.position = { 7.0f, 2.0f, -3.0f }; @@ -519,7 +565,7 @@ public: // m_mouseDelta = { 0.0f, 0.0f }; } - ~BasicMover() override + ~GeometricMover() override { m_camera->m_thirdPerson = false; sapp_lock_mouse( false ); @@ -556,32 +602,201 @@ public: } } + // One tree from the falling trees benchmark: a stack of shrinking cylinders sharing a body. + void CreateFallingTree() + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = m_treeSpawn; + bodyDef.sleepThreshold = 0.2f; + m_treeId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.9f; + shapeDef.baseMaterial.rollingResistance = 0.05f; + shapeDef.updateBodyMass = false; + shapeDef.userData = &m_treeShapeUserData; + + float y = 1.0f; + float r = 0.75f; + float l = 1.5f; + for ( int i = 0; i < 22; ++i ) + { + b3HullData* hull = b3CreateCylinder( l + 2.0f * r, r, y - r, 6 ); + b3CreateHullShape( m_treeId, &shapeDef, hull ); + b3DestroyHull( hull ); + + y += l + 2.0f * r; + r = 0.95f * r; + } + + b3Body_ApplyMassFromShapes( m_treeId ); + + b3Pos center = b3Body_GetWorldCenter( m_treeId ); + b3Body_SetAngularVelocity( m_treeId, m_treeSpin ); + b3Body_SetLinearVelocity( m_treeId, b3Cross( m_treeSpin, b3SubPos( center, m_treeSpawn ) ) ); + } + + // Spin about the base so the trunk sweeps down through anything standing in the fall line. + void ResetFallingTree() + { + b3DestroyBody( m_treeId ); + CreateFallingTree(); + } + + // The normal points from the body to the mover, so approach gives a positive speed. + MoverImpact MakeImpact( b3BodyId bodyId, const b3BodyTOIResult* toi ) + { + b3Vec3 pointVelocity = b3Body_GetWorldPointVelocity( bodyId, toi->point ); + float speed = b3Dot( toi->normal, b3Sub( pointVelocity, m_mover.m_velocity ) ); + + return { toi->point, toi->normal, speed, true }; + } + + void DrawImpact( const MoverImpact* impact, b3HexColor hexColor ) + { + if ( impact->valid == false ) + { + return; + } + + Vec4 color = MakeColor( hexColor ); + b3Pos tip = impact->point + 0.5f * impact->normal; + + DrawPoint( impact->point, 6.0f, color ); + DrawArrow( impact->point, tip, color ); + DrawString3D( tip, color, " %.1f m/s", impact->speed ); + } + + // True once the body has held still for a second. + bool SettledForOneSecond( b3BodyId bodyId, float* timer ) + { + if ( b3Body_IsAwake( bodyId ) ) + { + *timer = 0.0f; + return false; + } + + *timer += m_context->hertz > 0.0f ? 1.0f / m_context->hertz : 0.0f; + if ( *timer < 1.0f ) + { + return false; + } + + *timer = 0.0f; + return true; + } + + // Same sweep as the box, but the tree reports what the character was struck with. + void UpdateFallingTree( b3Pos moverStart, b3WorldTransform treeStart ) + { + b3WorldTransform treeEnd = b3Body_GetTransform( m_treeId ); + b3Vec3 moverTranslation = b3SubPos( m_mover.m_transform.p, moverStart ); + + b3BodyTOIResult toi = b3Body_TimeOfImpactMover( m_treeId, moverStart, &m_mover.m_capsule, moverTranslation, + b3DefaultQueryFilter(), treeStart, treeEnd ); + + if ( toi.fraction > 0.0f ) + { + m_treeImpact = MakeImpact( m_treeId, &toi ); + } + + if ( SettledForOneSecond( m_treeId, &m_treeTimer ) ) + { + ResetFallingTree(); + } + } + + // Sweep the mover capsule against the box motion from this step and keep the box pose at first + // touch. Both sweeps cover the same interval, so the mover translation comes from this step too. + void UpdateFallingBox( b3Pos moverStart, b3WorldTransform boxStart ) + { + b3WorldTransform boxEnd = b3Body_GetTransform( m_fallingBoxId ); + b3Vec3 moverTranslation = b3SubPos( m_mover.m_transform.p, moverStart ); + + b3BodyTOIResult toi = b3Body_TimeOfImpactMover( m_fallingBoxId, moverStart, &m_mover.m_capsule, moverTranslation, + b3DefaultQueryFilter(), boxStart, boxEnd ); + + // A zero fraction means the box was already resting against the mover, so keep the pose from + // the step that first reached it. + if ( toi.fraction > 0.0f ) + { + m_impactTransform.p = boxStart.p + toi.fraction * b3SubPos( boxEnd.p, boxStart.p ); + m_impactTransform.q = b3NLerp( boxStart.q, boxEnd.q, toi.fraction ); + m_boxImpact = MakeImpact( m_fallingBoxId, &toi ); + } + + if ( SettledForOneSecond( m_fallingBoxId, &m_respawnTimer ) ) + { + RespawnFallingBox(); + } + } + + void RespawnFallingBox() + { + b3Body_SetTransform( m_fallingBoxId, m_fallingSpawn, b3Quat_identity ); + b3Body_SetLinearVelocity( m_fallingBoxId, b3Vec3_zero ); + b3Body_SetAngularVelocity( m_fallingBoxId, m_fallingSpin ); + b3Body_SetAwake( m_fallingBoxId, true ); + } + void Step() override { + b3Pos moverStart = m_mover.m_transform.p; + b3WorldTransform boxStart = b3Body_GetTransform( m_fallingBoxId ); + b3WorldTransform treeStart = b3Body_GetTransform( m_treeId ); + m_mover.Step( &m_ignoreShapeId, 1, m_clipVelocity ); DrawTextLine( "third person (T) = %d", m_camera->m_thirdPerson ); DrawTextLine( "deltaX = %g, deltaY = %g", m_mouseDelta.x, m_mouseDelta.y ); Sample::Step(); + + if ( m_didStep ) + { + UpdateFallingBox( moverStart, boxStart ); + UpdateFallingTree( moverStart, treeStart ); + } + + if ( m_boxImpact.valid ) + { + DrawCube( m_impactTransform, 2.0f * m_fallingExtent, MakeColorAlpha( b3_colorYellow, 0.4f ) ); + } + + DrawImpact( &m_boxImpact, b3_colorYellow ); + DrawImpact( &m_treeImpact, b3_colorCyan ); } static Sample* Create( SampleContext* context ) { - return new BasicMover( context ); + return new GeometricMover( context ); } CharacterMover m_mover; MoverShapeUserData m_enemyShape; MoverShapeUserData m_friendlyShape; + MoverShapeUserData m_treeShapeUserData; b3MeshData* m_levelMesh; b3MeshData* m_stairs; b3MeshData* m_torus; b3HeightFieldData* m_heightField; b3ShapeId m_ignoreShapeId; + b3BodyId m_fallingBoxId; + b3BodyId m_treeId; + b3WorldTransform m_impactTransform; + MoverImpact m_boxImpact; + MoverImpact m_treeImpact; + b3Pos m_treeSpawn; + b3Vec3 m_treeSpin; + float m_treeTimer; + b3Pos m_fallingSpawn; + b3Vec3 m_fallingExtent; + b3Vec3 m_fallingSpin; + float m_respawnTimer; bool m_clipVelocity; }; -static int sampleMover = RegisterSample( "Character", "Mover", BasicMover::Create ); +static int sampleMover = RegisterSample( "Character", "Geometric Mover", GeometricMover::Create ); struct ClosestShapeCastContext { diff --git a/samples/sample_issues.cpp b/samples/sample_issues.cpp index d3c7110..6d2cd4b 100644 --- a/samples/sample_issues.cpp +++ b/samples/sample_issues.cpp @@ -1302,3 +1302,87 @@ public: static int sampleSlideTwistOffCenterShape = RegisterSample( "Issues", "Slide Twist Off Center Shape", SlideTwistOffCenterShape::Create ); + +// Very large dynamic bodies have overflow when computing the inertia tensor. +class HugeBox : public Sample +{ +public: + explicit HugeBox( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 0.0f, 25.0f, 10.0f, b3Pos_zero ); + } + + AddGroundBox( 400.0f ); + + { + float a = 100.0f; + + b3BoxHull cube = b3MakeCubeHull( a ); + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.name = "cube"; + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, a, 0.0f }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( { 0.0f, 0.0f, 1.0f }, 0.2f ); + m_bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( m_bodyId, &shapeDef, &cube.base ); + } + } + + static Sample* Create( SampleContext* context ) + { + return new HugeBox( context ); + } + + b3BodyId m_bodyId; +}; + +static int sampleHugeBox = RegisterSample( "Issues", "Huge Box", HugeBox::Create ); + +// This demonstrates this cap on mesh complexity. B3_MAX_MESH_CONTACT_TRIANGLES +// It generates this warning: +// Box3D: WARNING: complex mesh detected, triangle buffer capacity of 256 reached +class HeightfieldIssue : public Sample +{ +public: + explicit HeightfieldIssue( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( -90.0f, 5.0f, 50.0f, { 50.0f, 10.0f, 50.0f } ); + } + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BodyDef bodyDef = b3DefaultBodyDef(); + + m_heightField = b3CreateGrid( 100, 100, { 1, 1, 1 }, false ); + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHeightFieldShape( groundId, &shapeDef, m_heightField ); + + b3HullData* hull = b3CreateCylinder( 20, 5, -10, 24 ); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 50, 20, 50 }; + bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Vec3_axisX, B3_PI / 4 ); + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, hull ); + b3DestroyHull( hull ); + } + + ~HeightfieldIssue() override + { + b3DestroyHeightField( m_heightField ); + } + + static Sample* Create( SampleContext* context ) + { + return new HeightfieldIssue( context ); + } + + b3HeightFieldData* m_heightField; +}; +static int sampleIssueIndex = RegisterSample( "Issues", "Heightfield", HeightfieldIssue::Create ); diff --git a/samples/sample_joint.cpp b/samples/sample_joint.cpp index a9b5223..1139019 100644 --- a/samples/sample_joint.cpp +++ b/samples/sample_joint.cpp @@ -1953,6 +1953,67 @@ public: static int sampleBridgeIndex = RegisterSample( "Joints", "Bridge", Bridge::Create ); +class FoldOut : public Sample +{ +public: + explicit FoldOut( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 20.0f, 10.0f, 28.0f, { 0.0f, 7.0f, 0.0f } ); + } + + float halfLength = 2.0f; + float spacing = 1.0f; + + b3BoxHull box = b3MakeBoxHull( halfLength, 0.1f, 0.25f ); + + b3Quat axisQuat = b3ComputeQuatBetweenUnitVectors( b3Vec3_axisZ, b3Vec3_axisY ); + + b3RevoluteJointDef jointDef = b3DefaultRevoluteJointDef(); + jointDef.base.localFrameA.p = { -halfLength, 1.0f, 0.0f }; + jointDef.base.localFrameA.q = axisQuat; + jointDef.base.localFrameB.q = axisQuat; + jointDef.base.drawScale = 0.5f; + jointDef.base.constraintHertz = 240.0f; + jointDef.enableMotor = true; + jointDef.maxMotorTorque = 1000.0f; + + b3BodyId prevBodyId = AddGroundBox( 20.0f ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position.y = 0.5f * spacing; + bodyDef.type = b3_dynamicBody; + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density *= 2.0f; + + for ( int i = 0; i < 6; ++i ) + { + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + + float x = ( i & 1 ) ? halfLength : -halfLength; + + jointDef.base.bodyIdA = prevBodyId; + jointDef.base.bodyIdB = bodyId; + jointDef.base.localFrameB.p = { x, -0.5f * spacing, 0.0f }; + b3CreateRevoluteJoint( m_worldId, &jointDef ); + + bodyDef.position.y += spacing; + jointDef.base.localFrameA.p = { -x, 0.5f * spacing, 0.0f }; + prevBodyId = bodyId; + } + } + + static Sample* Create( SampleContext* context ) + { + return new FoldOut( context ); + } +}; + +static int sampleFoldOut = RegisterSample( "Joints", "Fold Out", FoldOut::Create ); + // This test ensures joints work correctly with bodies that have motion locks class MotionLocks : public Sample { @@ -2000,6 +2061,8 @@ public: assert( m_count < m_capacity ); bodyDef.position = position; + bodyDef.angularDamping = 0.5f; + m_bodyIds[m_count] = b3CreateBody( m_worldId, &bodyDef ); b3CreateHullShape( m_bodyIds[m_count], &shapeDef, &box.base ); @@ -2403,7 +2466,7 @@ public: ~Driving() override { - if (m_camera->m_thirdPerson) + if ( m_camera->m_thirdPerson ) { m_camera->m_thirdPerson = false; sapp_lock_mouse( false ); diff --git a/src/body.c b/src/body.c index bce16de..f517e7b 100644 --- a/src/body.c +++ b/src/body.c @@ -779,6 +779,87 @@ int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int pla return resultCount; } +b3BodyTOIResult b3Body_TimeOfImpactMover( b3BodyId bodyId, b3Pos origin, const b3Capsule* mover, b3Vec3 moverTranslation, + b3QueryFilter filter, b3WorldTransform bodyTransform1, b3WorldTransform bodyTransform2 ) +{ + b3BodyTOIResult result = { 0 }; + result.fraction = 1.0f; + + b3World* world = b3GetUnlockedWorld( bodyId.world0 ); + if ( world == NULL ) + { + return result; + } + + b3Transform xf1 = b3ToRelativeTransform( bodyTransform1, origin ); + b3Transform xf2 = b3ToRelativeTransform( bodyTransform2, origin ); + + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + b3Vec3 localCenter = bodySim->localCenter; + + b3Vec3 capsulePoints[2] = { mover->center1, mover->center2 }; + b3TOIInput input = { 0 }; + input.proxyB = (b3ShapeProxy){ + .points = capsulePoints, + .count = 2, + .radius = mover->radius, + }; + input.sweepA.c1 = b3TransformPoint( xf1, localCenter ); + input.sweepA.c2 = b3TransformPoint( xf2, localCenter ); + input.sweepA.q1 = bodyTransform1.q; + input.sweepA.q2 = bodyTransform2.q; + input.sweepA.localCenter = localCenter; + + input.sweepB.c1 = b3Vec3_zero; + input.sweepB.c2 = moverTranslation; + input.sweepB.q1 = b3Quat_identity; + input.sweepB.q2 = b3Quat_identity; + input.sweepB.localCenter = b3Vec3_zero; + + input.maxFraction = 1.0f; + + int shapeId = body->headShapeId; + while ( shapeId != B3_NULL_INDEX ) + { + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + shapeId = shape->nextShapeId; + + if ( b3ShouldQueryCollide( &shape->filter, &filter ) == false ) + { + continue; + } + + b3ShapeType type = shape->type; + if ( type != b3_sphereShape && type != b3_capsuleShape && type != b3_hullShape ) + { + continue; + } + + input.proxyA = b3MakeShapeProxy( shape ); + + b3TOIOutput output = b3TimeOfImpact( &input ); + B3_VALIDATE( output.state != b3_toiStateUnknown ); + + // Mimic behavior in b3ContinuousQueryCallback. Ignore shapes that initially overlap. + if (0.0f < output.fraction && output.fraction < result.fraction) + { + input.maxFraction = output.fraction; + + result.point = b3OffsetPos( origin, output.point ); + result.normal = output.normal; + result.fraction = output.fraction; + result.shapeId = (b3ShapeId){ + .index1 = shape->id + 1, + .world0 = world->worldId, + .generation = shape->generation, + }; + } + } + + return result; +} + void b3UpdateBodyMassData( b3World* world, b3Body* body ) { b3BodySim* bodySim = b3GetBodySim( world, body ); @@ -2303,7 +2384,7 @@ bool b3Body_IsBullet( b3BodyId bodyId ) return ( body->flags & b3_isBullet ) != 0; } -void b3Body_AllowFastRotation(b3BodyId bodyId, bool flag) +void b3Body_AllowFastRotation( b3BodyId bodyId, bool flag ) { b3World* world = b3GetUnlockedWorld( bodyId.world0 ); if ( world == NULL ) @@ -2327,7 +2408,7 @@ void b3Body_AllowFastRotation(b3BodyId bodyId, bool flag) b3SyncBodyFlags( world, body ); } -bool b3Body_IsFastRotationAllowed(b3BodyId bodyId) +bool b3Body_IsFastRotationAllowed( b3BodyId bodyId ) { b3World* world = b3GetWorld( bodyId.world0 ); b3Body* body = b3GetBodyFullId( world, bodyId ); @@ -2481,3 +2562,27 @@ bool b3ShouldBodiesCollide( b3World* world, b3Body* bodyA, b3Body* bodyB ) return true; } + +float b3Body_GetMinExtent( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + return bodySim->minExtent; +} + +b3Vec3 b3Body_GetMaxExtent( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + return bodySim->maxExtent; +} + +b3Vec3 b3Body_GetMaxExtentOrigin( b3BodyId bodyId ) +{ + b3World* world = b3GetWorld( bodyId.world0 ); + b3Body* body = b3GetBodyFullId( world, bodyId ); + b3BodySim* bodySim = b3GetBodySim( world, body ); + return b3Add( bodySim->maxExtent, b3Abs( bodySim->localCenter ) ); +} diff --git a/src/capsule.c b/src/capsule.c index 249e8cb..2927740 100644 --- a/src/capsule.c +++ b/src/capsule.c @@ -380,6 +380,6 @@ int b3CollideMoverAndCapsule( b3PlaneResult* result, const b3Capsule* shape, con } b3Plane plane = { normal, totalRadius - distance }; - *result = (b3PlaneResult){ plane, approach.point1 }; + *result = (b3PlaneResult){ plane, approach.point1, 0, 0, 0 }; return 1; } diff --git a/src/compound.c b/src/compound.c index 1df1f1e..597b78b 100644 --- a/src/compound.c +++ b/src/compound.c @@ -399,7 +399,9 @@ b3CompoundData* b3CreateCompound( const b3CompoundDef* def ) // No effort to share mesh materials. It would be easier to do if the number of materials was limited. B3_ASSERT( meshData->materialCount == meshDef->materialCount ); - for ( int j = 0; j < meshDef->materialCount; ++j ) + int meshMaterialCount = b3MinInt( meshDef->materialCount, B3_MAX_COMPOUND_MESH_MATERIALS ); + + for ( int j = 0; j < meshMaterialCount; ++j ) { // Look for an existing material. b3MaterialMap_itr materialItr = @@ -1168,6 +1170,10 @@ static bool b3CompoundMoverCallback( int proxyId, uint64_t userData, void* conte { planes[i].plane.normal = b3RotateVector( child.transform.q, planes[i].plane.normal ); planes[i].point = b3TransformPoint( child.transform, planes[i].point ); + + planes[i].childIndex = childIndex; + int childMaterialIndex = b3MinInt( planes[i].materialIndex, B3_MAX_COMPOUND_MESH_MATERIALS - 1 ); + planes[i].materialIndex = child.materialIndices[childMaterialIndex]; } moverContext->planeCount += planeCount; diff --git a/src/convex_manifold.c b/src/convex_manifold.c index d3b998a..2f71e54 100644 --- a/src/convex_manifold.c +++ b/src/convex_manifold.c @@ -278,7 +278,6 @@ static void b3ReduceManifoldPoints( b3LocalManifold* manifold, int capacity, b3L // Step 2: Find farthest point in 2D bestScore = 0.0f; bestIndex = B3_NULL_INDEX; - float maxDistanceSquared = 0.0f; for ( int index = 0; index < count; ++index ) { @@ -286,7 +285,6 @@ static void b3ReduceManifoldPoints( b3LocalManifold* manifold, int capacity, b3L b3Vec3 d = b3Sub( p, a ); b3Vec3 v = b3MulSub( d, b3Dot( d, normal ), normal ); float distanceSquared = b3LengthSquared( v ); - maxDistanceSquared = b3MaxFloat( maxDistanceSquared, distanceSquared ); float separation = b3MaxFloat( 0.0f, -points[index].separation ); float score = distanceSquared + 4.0f * separation * separation; if ( bias * score > bestScore ) diff --git a/src/distance.c b/src/distance.c index 3d558ab..ffe0c0e 100644 --- a/src/distance.c +++ b/src/distance.c @@ -847,6 +847,10 @@ b3DistanceOutput b3ShapeDistance( const b3DistanceInput* input, b3SimplexCache* b3ComputeWitnessPoints( &simplex, &localPointA, &localPointB ); distanceOutput.pointA = localPointA; distanceOutput.pointB = localPointB; + distanceOutput.normal = b3Vec3_zero; + distanceOutput.distance = 0.0f; + distanceOutput.iterations = iteration; + distanceOutput.simplexCount = simplexIndex; return distanceOutput; } @@ -943,6 +947,10 @@ b3DistanceOutput b3ShapeDistance( const b3DistanceInput* input, b3SimplexCache* b3ComputeWitnessPoints( &simplex, &localPointA, &localPointB ); distanceOutput.pointA = localPointA; distanceOutput.pointB = localPointB; + distanceOutput.normal = b3Vec3_zero; + distanceOutput.distance = 0.0f; + distanceOutput.iterations = iteration; + distanceOutput.simplexCount = simplexIndex; B3_VALIDATE( b3Distance( localPointA, localPointB ) < FLT_EPSILON ); return distanceOutput; } @@ -984,25 +992,27 @@ b3DistanceOutput b3ShapeDistance( const b3DistanceInput* input, b3SimplexCache* simplex.count += 1; } + // Build witness points. + b3Vec3 localPointA, localPointB; + b3ComputeWitnessPoints( &simplex, &localPointA, &localPointB ); + + distanceOutput.pointA = localPointA; + distanceOutput.pointB = localPointB; + distanceOutput.iterations = iteration; + distanceOutput.simplexCount = simplexIndex; + normal = b3Normalize( normal ); if ( b3IsNormalized( normal ) == false ) { // Treat as overlap + distanceOutput.distance = 0.0f; + distanceOutput.normal = b3Vec3_zero; return distanceOutput; } - // Build witness points and safe cache - b3Vec3 localPointA, localPointB; - b3ComputeWitnessPoints( &simplex, &localPointA, &localPointB ); - b3WriteCache( cache, &simplex ); - // Results stay in frame A - distanceOutput.pointA = localPointA; - distanceOutput.pointB = localPointB; distanceOutput.distance = b3Distance( localPointA, localPointB ); distanceOutput.normal = normal; - distanceOutput.iterations = iteration; - distanceOutput.simplexCount = simplexIndex; // Apply radii if requested if ( input->useRadii ) @@ -1016,10 +1026,12 @@ b3DistanceOutput b3ShapeDistance( const b3DistanceInput* input, b3SimplexCache* distanceOutput.pointB = b3Sub( distanceOutput.pointB, b3MulSV( rB, normal ) ); } + // Simplex is useful, cache it. + b3WriteCache( cache, &simplex ); + return distanceOutput; } - // Separation function: // f(t) = (c2 + t * dp2 - c1 - t * dp1 ) * n @@ -1703,6 +1715,14 @@ b3TOIOutput b3TimeOfImpact( const b3TOIInput* input ) { output.state = b3_toiStateOverlapped; output.fraction = 0.0f; + + // Averaged hit point + b3Vec3 pA = b3MulAdd( worldPointA, proxyA.radius, worldNormal ); + b3Vec3 pB = b3MulAdd( worldPointB, -proxyB.radius, worldNormal ); + output.point = b3Lerp( pA, pB, 0.5f ); + output.point = b3Add( output.point, origin ); + output.normal = worldNormal; + break; } diff --git a/src/height_field.c b/src/height_field.c index cf22c6a..37ff65f 100644 --- a/src/height_field.c +++ b/src/height_field.c @@ -608,6 +608,7 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightFieldData* heightField, const b3Vec3 shapeTranslation = input->translation; b3Vec3 scale = heightField->scale; + // The shape start is the center of the proxy. b3Vec3 shapeStart = b3AABB_Center( shapeBounds ); b3Vec3 shapeDelta = b3MulSV( input->maxFraction, shapeTranslation ); b3Vec3 shapeEnd = b3Add( shapeStart, shapeDelta ); @@ -768,9 +769,12 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightFieldData* heightField, const castBounds.lowerBound = b3Sub( b3Min( centerStart, centerEnd ), shapeExtents ); castBounds.upperBound = b3Add( b3Max( centerStart, centerEnd ), shapeExtents ); + // The ray origin is the center of the shape. b3V32 rayOrigin = b3LoadV( &shapeStart.x ); b3V32 rayTranslation = b3LoadV( &shapeTranslation.x ); + bool clockwise = heightField->clockwise; + while ( true ) { int column1, column2; @@ -827,6 +831,11 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightFieldData* heightField, const b3Vec3 point21 = corners[2]; b3Vec3 point22 = corners[3]; + if ( clockwise ) + { + B3_SWAP( point12, point21 ); + } + // I know the min/max x and z values, but not the min/max heights. b3AABB bounds; bounds.lowerBound = b3Min( b3Min( point11, point12 ), b3Min( point21, point22 ) ); @@ -846,18 +855,8 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightFieldData* heightField, const // Ray cast { b3V32 vertex1 = b3LoadV( &point11.x ); - b3V32 vertex2, vertex3; - - if ( heightField->clockwise ) - { - vertex2 = b3LoadV( &point12.x ); - vertex3 = b3LoadV( &point21.x ); - } - else - { - vertex2 = b3LoadV( &point21.x ); - vertex3 = b3LoadV( &point12.x ); - } + b3V32 vertex2 = b3LoadV( &point21.x ); + b3V32 vertex3 = b3LoadV( &point12.x ); float alpha = b3IntersectRayTriangle( rayOrigin, rayTranslation, vertex1, vertex2, vertex3 ); B3_ASSERT( 0 <= alpha && alpha <= 1.0f ); @@ -866,7 +865,7 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightFieldData* heightField, const { b3Vec3 edge1 = b3Sub( point21, point11 ); b3Vec3 edge2 = b3Sub( point12, point11 ); - b3Vec3 normal = heightField->clockwise ? b3Cross( edge2, edge1 ) : b3Cross( edge1, edge2 ); + b3Vec3 normal = b3Cross( edge1, edge2 ); result.point = b3MulAdd( shapeStart, alpha, shapeTranslation ); result.normal = b3Normalize( normal ); @@ -880,18 +879,8 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightFieldData* heightField, const { b3V32 vertex1 = b3LoadV( &point22.x ); - b3V32 vertex2, vertex3; - - if ( heightField->clockwise ) - { - vertex2 = b3LoadV( &point21.x ); - vertex3 = b3LoadV( &point12.x ); - } - else - { - vertex2 = b3LoadV( &point12.x ); - vertex3 = b3LoadV( &point21.x ); - } + b3V32 vertex2 = b3LoadV( &point12.x ); + b3V32 vertex3 = b3LoadV( &point21.x ); float alpha = b3IntersectRayTriangle( rayOrigin, rayTranslation, vertex1, vertex2, vertex3 ); B3_ASSERT( 0 <= alpha && alpha <= 1.0f ); @@ -900,7 +889,7 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightFieldData* heightField, const { b3Vec3 edge1 = b3Sub( point22, point21 ); b3Vec3 edge2 = b3Sub( point12, point21 ); - b3Vec3 normal = heightField->clockwise ? b3Cross( edge2, edge1 ) : b3Cross( edge1, edge2 ); + b3Vec3 normal = b3Cross( edge1, edge2 ); result.point = b3MulAdd( shapeStart, alpha, shapeTranslation ); result.normal = b3Normalize( normal ); @@ -915,44 +904,55 @@ b3CastOutput b3ShapeCastHeightField( const b3HeightFieldData* heightField, const else { // Shape cast - // todo back-side culling { - // Shift origin to first vertex - b3Vec3 origin = point11; - b3Vec3 triangleVertices[] = { b3Vec3_zero, b3Sub( point21, origin ), b3Sub( point12, origin ) }; - pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; - pairInput.maxFraction = bestFraction; - pairInput.transform.p = b3Neg( origin ); + // shapeStart is the center of the proxy. + float signedVolume = b3SignedVolume( point11, point21, point12, shapeStart ); - b3CastOutput pairOutput = b3ShapeCast( &pairInput ); - - if ( pairOutput.hit ) + if ( signedVolume >= 0.0f ) { - bestFraction = pairOutput.fraction; - result = pairOutput; - result.point = b3Add( result.point, origin ); - result.triangleIndex = triangleIndex1; - result.materialIndex = materialIndex; + // Shift origin to first vertex + b3Vec3 origin = point11; + b3Vec3 triangleVertices[] = { b3Vec3_zero, b3Sub( point21, origin ), b3Sub( point12, origin ) }; + pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + pairInput.maxFraction = bestFraction; + pairInput.transform.p = b3Neg( origin ); + + b3CastOutput pairOutput = b3ShapeCast( &pairInput ); + + if ( pairOutput.hit ) + { + bestFraction = pairOutput.fraction; + result = pairOutput; + result.point = b3Add( result.point, origin ); + result.triangleIndex = triangleIndex1; + result.materialIndex = materialIndex; + } } } { - // Shift origin to first vertex - b3Vec3 origin = point21; - b3Vec3 triangleVertices[] = { b3Vec3_zero, b3Sub( point22, origin ), b3Sub( point12, origin ) }; - pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; - pairInput.maxFraction = bestFraction; - pairInput.transform.p = b3Neg( origin ); + // shapeStart is the center of the proxy. + float signedVolume = b3SignedVolume( point21, point22, point12, shapeStart ); - b3CastOutput pairOutput = b3ShapeCast( &pairInput ); - - if ( pairOutput.hit ) + if ( signedVolume >= 0.0f ) { - bestFraction = pairOutput.fraction; - result = pairOutput; - result.point = b3Add( result.point, origin ); - result.triangleIndex = triangleIndex2; - result.materialIndex = materialIndex; + // Shift origin to first vertex + b3Vec3 origin = point21; + b3Vec3 triangleVertices[] = { b3Vec3_zero, b3Sub( point22, origin ), b3Sub( point12, origin ) }; + pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + pairInput.maxFraction = bestFraction; + pairInput.transform.p = b3Neg( origin ); + + b3CastOutput pairOutput = b3ShapeCast( &pairInput ); + + if ( pairOutput.hit ) + { + bestFraction = pairOutput.fraction; + result = pairOutput; + result.point = b3Add( result.point, origin ); + result.triangleIndex = triangleIndex2; + result.materialIndex = materialIndex; + } } } } @@ -1055,8 +1055,7 @@ bool b3OverlapHeightField( const b3HeightFieldData* shape, b3Transform shapeTran b3SimplexCache cache = { 0 }; - // Outer loop on rows and inner loop on columns so that triangle indices - // increase monotonically. + // Outer loop on rows and inner loop on columns so that triangle indices increase monotonically. for ( int row = minRow; row <= maxRow; ++row ) { if ( row < 0 || shape->rowCount - 1 <= row ) @@ -1210,6 +1209,7 @@ int b3CollideMoverAndHeightField( b3PlaneResult* planes, int capacity, const b3H b3SimplexCache cache = { 0 }; float radius = mover->radius; + b3Vec3 center = b3Lerp( mover->center1, mover->center2, 0.5f ); b3V32 center1 = b3LoadV( &mover->center1.x ); b3V32 center2 = b3LoadV( &mover->center2.x ); b3V32 r = b3SplatV( radius ); @@ -1229,6 +1229,8 @@ int b3CollideMoverAndHeightField( b3PlaneResult* planes, int capacity, const b3H int minCol = (int)floorf( localMinX / scale.x ); int maxCol = (int)floorf( localMaxX / scale.x ); + bool clockWise = shape->clockwise; + int planeCount = 0; // Outer loop on rows and inner loop on columns so that triangle indices @@ -1262,63 +1264,84 @@ int b3CollideMoverAndHeightField( b3PlaneResult* planes, int capacity, const b3H b3Vec3 point21 = corners[2]; b3Vec3 point22 = corners[3]; + if ( clockWise ) + { + B3_SWAP( point12, point21 ); + } + b3V32 v11 = b3LoadV( &point11.x ); b3V32 v12 = b3LoadV( &point12.x ); b3V32 v21 = b3LoadV( &point21.x ); b3V32 v22 = b3LoadV( &point22.x ); - if ( b3TestBoundsTriangleOverlap( boundsCenter, boundsExtent, v11, v21, v12 ) ) + bool overlap1 = b3TestBoundsTriangleOverlap( boundsCenter, boundsExtent, v11, v21, v12 ); + if ( overlap1 ) { - b3Vec3 triangleVertices[] = { point11, point21, point12 }; - distanceInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + float signedVolume = b3SignedVolume( point11, point21, point12, center ); - // reset the cache - cache.count = 0; - - // get distance between triangle and mover - b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 ); - - if ( distanceOutput.distance == 0.0f ) + // Front side? + if ( signedVolume >= 0.0f ) { - // todo SAT - } - else if ( distanceOutput.distance <= mover->radius ) - { - b3Plane plane = { distanceOutput.normal, mover->radius - distanceOutput.distance }; - planes[planeCount] = (b3PlaneResult){ plane, distanceOutput.pointA }; - planeCount += 1; + b3Vec3 triangleVertices[] = { point11, point21, point12 }; + distanceInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; - if ( planeCount == capacity ) + // reset the cache + cache.count = 0; + + // get distance between triangle and mover + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + if ( distanceOutput.distance == 0.0f ) { - return planeCount; + // deep overlap + } + else if ( distanceOutput.distance <= mover->radius ) + { + int triangleIndex = 2 * cellIndex; + b3Plane plane = { distanceOutput.normal, mover->radius - distanceOutput.distance }; + planes[planeCount] = (b3PlaneResult){ plane, distanceOutput.pointA, triangleIndex, 0, material }; + planeCount += 1; + + if ( planeCount == capacity ) + { + return planeCount; + } } } } - if ( b3TestBoundsTriangleOverlap( boundsCenter, boundsExtent, v21, v22, v12 ) ) + bool overlap2 = b3TestBoundsTriangleOverlap( boundsCenter, boundsExtent, v21, v22, v12 ); + if ( overlap2 ) { - b3Vec3 triangleVertices[] = { point22, point12, point21 }; - distanceInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + float signedVolume = b3SignedVolume( point22, point12, point21, center ); - // reset the cache - cache.count = 0; - - // get distance between triangle and mover - b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 ); - - if ( distanceOutput.distance == 0.0f ) + // Front side? + if ( signedVolume >= 0.0f ) { - // todo SAT - } - else if ( distanceOutput.distance <= mover->radius ) - { - b3Plane plane = { distanceOutput.normal, mover->radius - distanceOutput.distance }; - planes[planeCount] = (b3PlaneResult){ plane, distanceOutput.pointA }; - planeCount += 1; + b3Vec3 triangleVertices[] = { point22, point12, point21 }; + distanceInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; - if ( planeCount == capacity ) + // reset the cache + cache.count = 0; + + // get distance between triangle and mover + b3DistanceOutput distanceOutput = b3ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + if ( distanceOutput.distance == 0.0f ) { - return planeCount; + // deep overlap + } + else if ( distanceOutput.distance <= mover->radius ) + { + int triangleIndex = 2 * cellIndex + 1; + b3Plane plane = { distanceOutput.normal, mover->radius - distanceOutput.distance }; + planes[planeCount] = (b3PlaneResult){ plane, distanceOutput.pointA, triangleIndex, 0, material }; + planeCount += 1; + + if ( planeCount == capacity ) + { + return planeCount; + } } } } diff --git a/src/hull.c b/src/hull.c index 0782a5c..84cd8ed 100644 --- a/src/hull.c +++ b/src/hull.c @@ -2655,7 +2655,7 @@ int b3CollideMoverAndHull( b3PlaneResult* result, const b3HullData* shape, const if ( distanceOutput.distance <= totalRadius ) { b3Plane plane = { distanceOutput.normal, totalRadius - distanceOutput.distance }; - *result = (b3PlaneResult){ plane, distanceOutput.pointA }; + *result = (b3PlaneResult){ plane, distanceOutput.pointA, 0, 0, 0 }; return 1; } diff --git a/src/joint.c b/src/joint.c index 5875f55..f2d76c6 100644 --- a/src/joint.c +++ b/src/joint.c @@ -120,7 +120,7 @@ b3ExplosionDef b3DefaultExplosionDef( void ) b3Joint* b3GetJointFullId( b3World* world, b3JointId jointId ) { int id = jointId.index1 - 1; - b3Joint* joint = b3Array_Get( world->joints, id ); + b3Joint* joint = b3Array_Get( world->joints, id ); B3_ASSERT( joint->jointId == id && joint->generation == jointId.generation ); return joint; } @@ -131,11 +131,11 @@ b3JointSim* b3GetJointSim( b3World* world, b3Joint* joint ) { B3_ASSERT( 0 <= joint->colorIndex && joint->colorIndex < B3_GRAPH_COLOR_COUNT ); b3GraphColor* color = world->constraintGraph.colors + joint->colorIndex; - return b3Array_Get( color->jointSims, joint->localIndex ); + return b3Array_Get( color->jointSims, joint->localIndex ); } - b3SolverSet* set = b3Array_Get( world->solverSets, joint->setIndex ); - return b3Array_Get( set->jointSims, joint->localIndex ); + b3SolverSet* set = b3Array_Get( world->solverSets, joint->setIndex ); + return b3Array_Get( set->jointSims, joint->localIndex ); } b3JointSim* b3GetJointSimCheckType( b3JointId jointId, b3JointType type ) @@ -171,10 +171,10 @@ static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3Joint int jointId = b3AllocId( &world->jointIdPool ); if ( jointId == world->joints.count ) { - b3Array_Push( world->joints, (b3Joint){ 0 } ); + b3Array_Push( world->joints, (b3Joint){ 0 } ); } - b3Joint* joint = b3Array_Get( world->joints, jointId ); + b3Joint* joint = b3Array_Get( world->joints, jointId ); joint->jointId = jointId; joint->userData = def->userData; joint->generation += 1; @@ -195,7 +195,7 @@ static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3Joint int keyA = ( jointId << 1 ) | 0; if ( bodyA->headJointKey != B3_NULL_INDEX ) { - b3Joint* jointA = b3Array_Get( world->joints, bodyA->headJointKey >> 1 ); + b3Joint* jointA = b3Array_Get( world->joints, bodyA->headJointKey >> 1 ); b3JointEdge* edgeA = jointA->edges + ( bodyA->headJointKey & 1 ); edgeA->prevKey = keyA; } @@ -210,7 +210,7 @@ static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3Joint int keyB = ( jointId << 1 ) | 1; if ( bodyB->headJointKey != B3_NULL_INDEX ) { - b3Joint* jointB = b3Array_Get( world->joints, bodyB->headJointKey >> 1 ); + b3Joint* jointB = b3Array_Get( world->joints, bodyB->headJointKey >> 1 ); b3JointEdge* edgeB = jointB->edges + ( bodyB->headJointKey & 1 ); edgeB->prevKey = keyB; } @@ -222,7 +222,7 @@ static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3Joint if ( bodyA->setIndex == b3_disabledSet || bodyB->setIndex == b3_disabledSet ) { // if either body is disabled, create in disabled set - b3SolverSet* set = b3Array_Get( world->solverSets, b3_disabledSet ); + b3SolverSet* set = b3Array_Get( world->solverSets, b3_disabledSet ); joint->setIndex = b3_disabledSet; joint->localIndex = set->jointSims.count; @@ -236,7 +236,7 @@ static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3Joint else if ( bodyA->type != b3_dynamicBody && bodyB->type != b3_dynamicBody ) { // joint is not attached to a dynamic body - b3SolverSet* set = b3Array_Get( world->solverSets, b3_staticSet ); + b3SolverSet* set = b3Array_Get( world->solverSets, b3_staticSet ); joint->setIndex = b3_staticSet; joint->localIndex = set->jointSims.count; @@ -271,7 +271,7 @@ static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3Joint // joint should go into the sleeping set (not static set) int setIndex = maxSetIndex; - b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); joint->setIndex = setIndex; joint->localIndex = set->jointSims.count; @@ -292,10 +292,10 @@ static b3JointPair b3CreateJoint( b3World* world, const b3JointDef* def, b3Joint // fix potentially invalid set index setIndex = bodyA->setIndex; - b3SolverSet* mergedSet = b3Array_Get( world->solverSets, setIndex ); + b3SolverSet* mergedSet = b3Array_Get( world->solverSets, setIndex ); // Careful! The joint sim pointer was orphaned by the set merge. - jointSim = b3Array_Get( mergedSet->jointSims, joint->localIndex ); + jointSim = b3Array_Get( mergedSet->jointSims, joint->localIndex ); } B3_ASSERT( joint->setIndex == setIndex ); @@ -359,7 +359,7 @@ static void b3DestroyContactsBetweenBodies( b3World* world, b3Body* bodyA, b3Bod int contactId = contactKey >> 1; int edgeIndex = contactKey & 1; - b3Contact* contact = b3Array_Get( world->contacts, contactId ); + b3Contact* contact = b3Array_Get( world->contacts, contactId ); contactKey = contact->edges[edgeIndex].nextKey; int otherEdgeIndex = edgeIndex ^ 1; @@ -437,7 +437,7 @@ b3JointId b3CreateDistanceJoint( b3WorldId worldId, const b3DistanceJointDef* de { B3_CHECK_JOINT_DEF( def ); b3World* world = b3GetUnlockedWorldFromId( worldId ); - if (world == NULL) + if ( world == NULL ) { return (b3JointId){ 0 }; } @@ -505,7 +505,7 @@ b3JointId b3CreateFilterJoint( b3WorldId worldId, const b3FilterJointDef* def ) { B3_CHECK_JOINT_DEF( def ); b3World* world = b3GetUnlockedWorldFromId( worldId ); - if (world == NULL) + if ( world == NULL ) { return (b3JointId){ 0 }; } @@ -523,7 +523,7 @@ b3JointId b3CreateParallelJoint( b3WorldId worldId, const b3ParallelJointDef* de { B3_CHECK_JOINT_DEF( def ); b3World* world = b3GetUnlockedWorldFromId( worldId ); - if (world == NULL) + if ( world == NULL ) { return (b3JointId){ 0 }; } @@ -552,7 +552,7 @@ b3JointId b3CreatePrismaticJoint( b3WorldId worldId, const b3PrismaticJointDef* B3_ASSERT( def->lowerTranslation <= def->upperTranslation ); b3World* world = b3GetUnlockedWorldFromId( worldId ); - if (world == NULL) + if ( world == NULL ) { return (b3JointId){ 0 }; } @@ -583,7 +583,7 @@ b3JointId b3CreateRevoluteJoint( b3WorldId worldId, const b3RevoluteJointDef* de B3_CHECK_JOINT_DEF( def ); b3World* world = b3GetUnlockedWorldFromId( worldId ); - if (world == NULL) + if ( world == NULL ) { return (b3JointId){ 0 }; } @@ -620,7 +620,7 @@ b3JointId b3CreateSphericalJoint( b3WorldId worldId, const b3SphericalJointDef* B3_ASSERT( b3IsValidQuat( def->targetRotation ) ); b3World* world = b3GetUnlockedWorldFromId( worldId ); - if (world == NULL) + if ( world == NULL ) { return (b3JointId){ 0 }; } @@ -661,7 +661,7 @@ b3JointId b3CreateWeldJoint( b3WorldId worldId, const b3WeldJointDef* def ) B3_ASSERT( 0.0f <= def->linearDampingRatio ); b3World* world = b3GetUnlockedWorldFromId( worldId ); - if (world == NULL) + if ( world == NULL ) { return (b3JointId){ 0 }; } @@ -687,7 +687,7 @@ b3JointId b3CreateWheelJoint( b3WorldId worldId, const b3WheelJointDef* def ) B3_ASSERT( def->lowerSuspensionLimit <= def->upperSuspensionLimit ); b3World* world = b3GetUnlockedWorldFromId( worldId ); - if (world == NULL) + if ( world == NULL ) { return (b3JointId){ 0 }; } @@ -730,20 +730,20 @@ void b3DestroyJointInternal( b3World* world, b3Joint* joint, bool wakeBodies ) int idA = edgeA->bodyId; int idB = edgeB->bodyId; - b3Body* bodyA = b3Array_Get( world->bodies, idA ); - b3Body* bodyB = b3Array_Get( world->bodies, idB ); + b3Body* bodyA = b3Array_Get( world->bodies, idA ); + b3Body* bodyB = b3Array_Get( world->bodies, idB ); // Remove from body A if ( edgeA->prevKey != B3_NULL_INDEX ) { - b3Joint* prevJoint = b3Array_Get( world->joints, edgeA->prevKey >> 1 ); + b3Joint* prevJoint = b3Array_Get( world->joints, edgeA->prevKey >> 1 ); b3JointEdge* prevEdge = prevJoint->edges + ( edgeA->prevKey & 1 ); prevEdge->nextKey = edgeA->nextKey; } if ( edgeA->nextKey != B3_NULL_INDEX ) { - b3Joint* nextJoint = b3Array_Get( world->joints, edgeA->nextKey >> 1 ); + b3Joint* nextJoint = b3Array_Get( world->joints, edgeA->nextKey >> 1 ); b3JointEdge* nextEdge = nextJoint->edges + ( edgeA->nextKey & 1 ); nextEdge->prevKey = edgeA->prevKey; } @@ -759,14 +759,14 @@ void b3DestroyJointInternal( b3World* world, b3Joint* joint, bool wakeBodies ) // Remove from body B if ( edgeB->prevKey != B3_NULL_INDEX ) { - b3Joint* prevJoint = b3Array_Get( world->joints, edgeB->prevKey >> 1 ); + b3Joint* prevJoint = b3Array_Get( world->joints, edgeB->prevKey >> 1 ); b3JointEdge* prevEdge = prevJoint->edges + ( edgeB->prevKey & 1 ); prevEdge->nextKey = edgeB->nextKey; } if ( edgeB->nextKey != B3_NULL_INDEX ) { - b3Joint* nextJoint = b3Array_Get( world->joints, edgeB->nextKey >> 1 ); + b3Joint* nextJoint = b3Array_Get( world->joints, edgeB->nextKey >> 1 ); b3JointEdge* nextEdge = nextJoint->edges + ( edgeB->nextKey & 1 ); nextEdge->prevKey = edgeB->prevKey; } @@ -799,14 +799,14 @@ void b3DestroyJointInternal( b3World* world, b3Joint* joint, bool wakeBodies ) } else { - b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); - int movedIndex = b3Array_RemoveSwap( set->jointSims, localIndex ); + b3SolverSet* set = b3Array_Get( world->solverSets, setIndex ); + int movedIndex = b3Array_RemoveSwap( set->jointSims, localIndex ); if ( movedIndex != B3_NULL_INDEX ) { // Fix moved joint b3JointSim* movedJointSim = set->jointSims.data + localIndex; int movedId = movedJointSim->jointId; - b3Joint* movedJoint = b3Array_Get( world->joints, movedId ); + b3Joint* movedJoint = b3Array_Get( world->joints, movedId ); B3_ASSERT( movedJoint->localIndex == movedIndex ); movedJoint->localIndex = localIndex; } @@ -926,8 +926,8 @@ void b3Joint_SetCollideConnected( b3JointId jointId, bool shouldCollide ) joint->collideConnected = shouldCollide; - b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); - b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); + b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); if ( shouldCollide ) { @@ -939,7 +939,7 @@ void b3Joint_SetCollideConnected( b3JointId jointId, bool shouldCollide ) int shapeId = shapeCountA < shapeCountB ? bodyA->headShapeId : bodyB->headShapeId; while ( shapeId != B3_NULL_INDEX ) { - b3Shape* shape = b3Array_Get( world->shapes, shapeId ); + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); if ( shape->proxyKey != B3_NULL_INDEX ) { @@ -989,8 +989,8 @@ void b3Joint_WakeBodies( b3JointId jointId ) world->locked = true; b3Joint* joint = b3GetJointFullId( world, jointId ); - b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); - b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); + b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); b3WakeBody( world, bodyA ); b3WakeBody( world, bodyB ); @@ -998,6 +998,15 @@ void b3Joint_WakeBodies( b3JointId jointId ) world->locked = false; } +bool b3Joint_IsAwake( b3JointId jointId ) +{ + b3World* world = b3GetWorld( jointId.world0 ); + b3Joint* joint = b3GetJointFullId( world, jointId ); + + // Cheaper than checking if the bodies are awake. + return joint->setIndex == b3_awakeSet; +} + void b3GetJointReaction( b3World* world, b3JointSim* sim, float invTimeStep, float* force, float* torque ) { float linearImpulse = 0.0f; @@ -1651,8 +1660,8 @@ void b3SolveJoints_Overflow( b3StepContext* context, bool useBias ) void b3DrawJoint( b3DebugDraw* draw, b3World* world, b3Joint* joint ) { - b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); - b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); + b3Body* bodyA = b3Array_Get( world->bodies, joint->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, joint->edges[1].bodyId ); if ( bodyA->setIndex == b3_disabledSet || bodyB->setIndex == b3_disabledSet ) { return; diff --git a/src/mesh.c b/src/mesh.c index 404b060..312dbab 100644 --- a/src/mesh.c +++ b/src/mesh.c @@ -244,12 +244,12 @@ static bool b3IsConsistent( const b3MeshData* mesh ) const b3MeshNode* child2 = b3GetRightChild( node ); b3AABB bounds2 = b3GetNodeAABB( child2 ); - if ( !b3AABB_Contains( nodeBounds, bounds1 ) ) + if ( b3AABB_Contains( nodeBounds, bounds1 ) == false ) { return false; } - if ( !b3AABB_Contains( nodeBounds, bounds2 ) ) + if ( b3AABB_Contains( nodeBounds, bounds2 ) == false ) { return false; } @@ -329,6 +329,11 @@ bool b3IsValidMesh( const b3MeshData* meshData ) #endif +static inline b3Vec3 b3GetStridedVertex( const b3Vec3* vertices, int index, size_t stride ) +{ + return *(const b3Vec3*)( (const uint8_t*)vertices + index * stride ); +} + // Node for a vertex linked list typedef struct b3VertexNode { @@ -350,15 +355,17 @@ typedef struct b3SpatialHash b3Array( b3VertexNode ) nodes; const b3Vec3* vertices; int vertexCount; + size_t stride; b3VertexMap vertexMap; float cellSize; float tolerance; } b3SpatialHash; -static void b3SpatialHash_Create( b3SpatialHash* h, const b3Vec3* vertices, int vertexCount, float tolerance ) +static void b3CreateSpatialHash( b3SpatialHash* h, const b3Vec3* vertices, int vertexCount, size_t stride, float tolerance ) { h->vertices = vertices; h->vertexCount = vertexCount; + h->stride = stride; h->tolerance = tolerance; h->cellSize = 2.0f * tolerance; b3Array_CreateN( h->nodes, vertexCount ); @@ -369,7 +376,7 @@ static void b3SpatialHash_Create( b3SpatialHash* h, const b3Vec3* vertices, int B3_ASSERT( h->cellSize > 0.0f ); } -static void b3SpatialHash_Destroy( b3SpatialHash* h ) +static void b3DestroySpatialHash( b3SpatialHash* h ) { b3VertexMap_cleanup( &h->vertexMap ); b3Array_Destroy( h->nodes ); @@ -380,7 +387,8 @@ static void b3SpatialHash_Destroy( b3SpatialHash* h ) static int32_t b3SpatialHash_FindDuplicate( b3SpatialHash* h, int32_t currentIndex ) { B3_ASSERT( currentIndex < h->vertexCount ); - b3Vec3 vertex = h->vertices[currentIndex]; + size_t stride = h->stride; + b3Vec3 vertex = b3GetStridedVertex( h->vertices, currentIndex, stride ); float cellSize = h->cellSize; float tolerance = h->tolerance; @@ -420,7 +428,7 @@ static int32_t b3SpatialHash_FindDuplicate( b3SpatialHash* h, int32_t currentInd B3_ASSERT( existingIndex < currentIndex ); B3_ASSERT( existingIndex < h->vertexCount ); - b3Vec3 other = h->vertices[existingIndex]; + b3Vec3 other = b3GetStridedVertex( h->vertices, existingIndex, stride ); // IsEqual inlined: check if vertices are within tolerance if ( fabsf( vertex.x - other.x ) <= tolerance && fabsf( vertex.y - other.y ) <= tolerance && @@ -481,16 +489,18 @@ typedef struct b3WeldData int vertexCount; int indexCount; + size_t stride; } b3WeldData; static int b3WeldVertices( b3WeldData* data, float tolerance ) { int vertexCount = data->vertexCount; int uniqueCount = 0; + size_t stride = data->stride; // Create spatial hash and find duplicates b3SpatialHash spatialHash; - b3SpatialHash_Create( &spatialHash, data->srcVertices, vertexCount, tolerance ); + b3CreateSpatialHash( &spatialHash, data->srcVertices, vertexCount, stride, tolerance ); b3Array( int ) vertexMapping = { 0 }; b3Array_Resize( vertexMapping, vertexCount ); @@ -502,7 +512,7 @@ static int b3WeldVertices( b3WeldData* data, float tolerance ) { // New unique vertex vertexMapping.data[i] = uniqueCount; - data->dstVertices[uniqueCount] = data->srcVertices[i]; + data->dstVertices[uniqueCount] = b3GetStridedVertex( data->srcVertices, i, stride ); uniqueCount += 1; } else @@ -517,11 +527,11 @@ static int b3WeldVertices( b3WeldData* data, float tolerance ) for ( int i = 0; i < indexCount; ++i ) { int srcIndex = data->srcIndices[i]; - B3_ASSERT( srcIndex < vertexCount ); + B3_ASSERT( 0 <= srcIndex && srcIndex < vertexCount ); data->dstIndices[i] = vertexMapping.data[srcIndex]; } - b3SpatialHash_Destroy( &spatialHash ); + b3DestroySpatialHash( &spatialHash ); b3Array_Destroy( vertexMapping ); return uniqueCount; @@ -1555,9 +1565,36 @@ b3MeshData* b3CreatePlatformMesh( b3Vec3 center, float height, float topWidth, f return b3CreateMesh( &def, NULL, 0 ); } +static void b3CopyVerticesWithStride( b3Vec3* dst, const b3Vec3* src, int count, size_t stride ) +{ + if ( stride == sizeof( b3Vec3 ) ) + { + memcpy( dst, src, count * sizeof( b3Vec3 ) ); + return; + } + + for ( int i = 0; i < count; ++i ) + { + dst[i] = *(const b3Vec3*)( (const uint8_t*)src + i * stride ); + } +} + +// Stride larger than this likely indicates stride is uninitialized memory. +#define B3_MAX_STRIDE 4096 + // todo this should fail if the mesh has a height greater than B3_MESH_STACK_SIZE b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, int degenerateCapacity ) { + if ( def->stride != 0 && ( def->stride < sizeof( b3Vec3 ) || B3_MAX_STRIDE < def->stride ) ) + { + return NULL; + } + + if ( ( def->stride & 3 ) != 0 ) + { + return NULL; + } + if ( def->vertexCount < 3 || def->vertices == NULL || def->triangleCount <= 0 || def->indices == NULL ) { return NULL; @@ -1570,6 +1607,7 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, } int vertexCount = def->vertexCount; + size_t stride = def->stride == 0 ? sizeof( b3Vec3 ) : def->stride; b3AABB meshBounds = B3_BOUNDS3_EMPTY; @@ -1584,6 +1622,7 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, { b3Array_Resize( vertices, vertexCount ); b3Array_Resize( indices, 3 * triangleCount ); + b3WeldData data = { .srcVertices = def->vertices, .srcIndices = def->indices, @@ -1591,14 +1630,17 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, .dstIndices = indices.data, .vertexCount = vertexCount, .indexCount = 3 * triangleCount, + .stride = stride, }; + vertices.count = b3WeldVertices( &data, def->weldTolerance ); vertexCount = vertices.count; B3_ASSERT( vertexCount <= def->vertexCount ); } else { - b3Array_Append( vertices, def->vertices, vertexCount ); + vertices.count = vertexCount; + b3CopyVerticesWithStride( vertices.data, def->vertices, vertexCount, stride ); b3Array_Append( indices, def->indices, 3 * triangleCount ); } @@ -1608,6 +1650,7 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, float minArea = 0.01f * B3_LINEAR_SLOP * B3_LINEAR_SLOP; float surfaceArea = 0.0f; int materialCount = 1; + bool clockWise = def->clockWiseWinding; for ( int index = 0; index < triangleCount; ++index ) { @@ -1615,6 +1658,10 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, int index2 = indices.data[3 * index + 1]; int index3 = indices.data[3 * index + 2]; + B3_ASSERT( 0 <= index1 && index1 < vertexCount ); + B3_ASSERT( 0 <= index2 && index2 < vertexCount ); + B3_ASSERT( 0 <= index3 && index3 < vertexCount ); + b3Vec3 vertex1 = vertices.data[index1]; b3Vec3 vertex2 = vertices.data[index2]; b3Vec3 vertex3 = vertices.data[index3]; @@ -1628,11 +1675,12 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, if ( index1 != index2 && index1 != index3 && index2 != index3 ) { - degenerateCount += 1; if ( degenerateTriangleIndices != NULL && degenerateCount < degenerateCapacity ) { - degenerateTriangleIndices[degenerateCount - 1] = index; + degenerateTriangleIndices[degenerateCount] = index; } + + degenerateCount += 1; } continue; @@ -1641,8 +1689,8 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, surfaceArea += area; b3AABB box = { - b3Min( vertex1, b3Min( vertex2, vertex3 ) ), - b3Max( vertex1, b3Max( vertex2, vertex3 ) ), + .lowerBound = b3Min( vertex1, b3Min( vertex2, vertex3 ) ), + .upperBound = b3Max( vertex1, b3Max( vertex2, vertex3 ) ), }; b3Vec3 center = b3AABB_Center( box ); @@ -1668,6 +1716,8 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, if ( b3IsSaneAABB( meshBounds ) == false ) { b3Array_Destroy( primitives ); + b3Array_Destroy( indices ); + b3Array_Destroy( vertices ); return NULL; } @@ -1724,9 +1774,18 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, for ( int index = 0; index < triangleCount; ++index ) { b3Primitive primitive = primitives.data[index]; - triangles[index].index1 = indices.data[3 * primitive.triangleIndex + 0]; - triangles[index].index2 = indices.data[3 * primitive.triangleIndex + 1]; - triangles[index].index3 = indices.data[3 * primitive.triangleIndex + 2]; + + int i1 = 3 * primitive.triangleIndex + 0; + int i2 = 3 * primitive.triangleIndex + 1; + int i3 = 3 * primitive.triangleIndex + 2; + if ( clockWise ) + { + B3_SWAP( i2, i3 ); + } + + triangles[index].index1 = indices.data[i1]; + triangles[index].index2 = indices.data[i2]; + triangles[index].index3 = indices.data[i3]; flags[index] = 0; // Copy material indices if they exist. Otherwise the material indices are all zeroes. @@ -1745,6 +1804,9 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, { b3Array_Destroy( tempNodes ); b3Array_Destroy( primitives ); + b3Array_Destroy( indices ); + b3Array_Destroy( vertices ); + b3Free( mesh, byteCount ); return NULL; } @@ -1762,7 +1824,7 @@ b3MeshData* b3CreateMesh( const b3MeshDef* def, int* degenerateTriangleIndices, b3Array_Destroy( vertices ); mesh->hash = 0; - mesh->hash = b3Hash64NonZero((uint8_t*)mesh, mesh->byteCount ); + mesh->hash = b3Hash64NonZero( (uint8_t*)mesh, mesh->byteCount ); return mesh; } @@ -1896,7 +1958,7 @@ b3CastOutput b3RayCastMesh( const b3Mesh* mesh, const b3RayCastInput* input ) b3V32 scale = b3LoadV( &meshScale.x ); b3V32 invScale = b3DivV( b3_oneV, scale ); - bool clockwise = meshScale.x * meshScale.y * meshScale.z < 0.0f; + bool ccw = meshScale.x * meshScale.y * meshScale.z > 0.0f; // Use the inverse scaled ray for traversal of the BVH b3V32 invScaledRayStart = b3MulV( invScale, rayStart ); @@ -1936,16 +1998,16 @@ b3CastOutput b3RayCastMesh( const b3Mesh* mesh, const b3RayCastInput* input ) b3Vec3 vertex2, vertex3; // The CPU should predict this branch - if ( clockwise ) - { - vertex2 = b3Mul( meshScale, vertices[triangle.index3] ); - vertex3 = b3Mul( meshScale, vertices[triangle.index2] ); - } - else + if ( ccw ) { vertex2 = b3Mul( meshScale, vertices[triangle.index2] ); vertex3 = b3Mul( meshScale, vertices[triangle.index3] ); } + else + { + vertex2 = b3Mul( meshScale, vertices[triangle.index3] ); + vertex3 = b3Mul( meshScale, vertices[triangle.index2] ); + } // Collide ray with triangle in scaled space b3V32 v1 = b3LoadV( &vertex1.x ); @@ -2030,7 +2092,7 @@ b3CastOutput b3ShapeCastMesh( const b3Mesh* mesh, const b3ShapeCastInput* input b3V32 scale = b3LoadV( &meshScale.x ); b3V32 invScale = b3DivV( b3_oneV, scale ); b3V32 absInvScale = b3AbsV( invScale ); - bool clockwise = meshScale.x * meshScale.y * meshScale.z < 0.0f; + bool ccw = meshScale.x * meshScale.y * meshScale.z > 0.0f; // Use the inverse scaled shape cast for traversal of the BVH b3V32 invScaledRayStart = b3MulV( invScale, rayStart ); @@ -2067,21 +2129,21 @@ b3CastOutput b3ShapeCastMesh( const b3Mesh* mesh, const b3ShapeCastInput* input int triangleIndex = triangleOffset + index; b3MeshTriangle triangle = triangles[triangleIndex]; - // Collide ray with triangle in scaled space + // Collide shape with triangle in scaled space b3Vec3 vertex1 = b3Mul( meshScale, vertices[triangle.index1] ); b3Vec3 vertex2, vertex3; // The CPU should predict this branch - if ( clockwise ) - { - vertex2 = b3Mul( meshScale, vertices[triangle.index3] ); - vertex3 = b3Mul( meshScale, vertices[triangle.index2] ); - } - else + if ( ccw ) { vertex2 = b3Mul( meshScale, vertices[triangle.index2] ); vertex3 = b3Mul( meshScale, vertices[triangle.index3] ); } + else + { + vertex2 = b3Mul( meshScale, vertices[triangle.index3] ); + vertex3 = b3Mul( meshScale, vertices[triangle.index2] ); + } b3V32 v1 = b3LoadV( &vertex1.x ); b3V32 v2 = b3LoadV( &vertex2.x ); @@ -2090,43 +2152,54 @@ b3CastOutput b3ShapeCastMesh( const b3Mesh* mesh, const b3ShapeCastInput* input b3V32 triangleMin = b3SubV( b3MinV( v1, b3MinV( v2, v3 ) ), shapeExtent ); b3V32 triangleMax = b3AddV( b3MaxV( v1, b3MaxV( v2, v3 ) ), shapeExtent ); - // Test triangle-ray overlap in scaled space - if ( b3TestBoundsOverlap( triangleMin, triangleMax, rayMin, rayMax ) ) + // Test extended triangle vs ray bounds overlap in scaled space. Skip edge tests. + bool overlap = b3TestBoundsOverlap( triangleMin, triangleMax, rayMin, rayMax ); + if (overlap == false) { - // Collide shape with triangle in scaled space - b3Vec3 origin = vertex1; - b3Vec3 triangleVertices[] = { b3Vec3_zero, b3Sub( vertex2, origin ), b3Sub( vertex3, origin ) }; - b3Transform shiftedOrigin = { b3Neg( origin ), b3Quat_identity }; + continue; + } - b3ShapeCastPairInput pairInput; - pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; - pairInput.proxyB = input->proxy; - pairInput.transform = shiftedOrigin; - pairInput.maxFraction = bestOutput.fraction; - pairInput.translationB = input->translation; - pairInput.canEncroach = input->canEncroach; + // This test is in scaled space. vertices and center are scaled. + float signedVolume = b3SignedVolume( vertex1, vertex2, vertex3, center ); + if ( signedVolume < 0.0f ) + { + // Backside + continue; + } - b3CastOutput pairOutput = b3ShapeCast( &pairInput ); + // Collide shape with triangle in scaled space. + b3Vec3 origin = vertex1; + b3Vec3 triangleVertices[] = { b3Vec3_zero, b3Sub( vertex2, origin ), b3Sub( vertex3, origin ) }; + b3Transform shiftedOrigin = { b3Neg( origin ), b3Quat_identity }; - if ( pairOutput.hit ) - { - pairOutput.point = b3Add( pairOutput.point, origin ); + b3ShapeCastPairInput pairInput; + pairInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; + pairInput.proxyB = input->proxy; + pairInput.transform = shiftedOrigin; + pairInput.maxFraction = bestOutput.fraction; + pairInput.translationB = input->translation; + pairInput.canEncroach = input->canEncroach; - bestOutput = pairOutput; - bestOutput.triangleIndex = triangleIndex; - bestOutput.materialIndex = materialIndices[triangleIndex]; + b3CastOutput pairOutput = b3ShapeCast( &pairInput ); - // Update ray bounds in scaled space - lambda = b3SplatV( pairOutput.fraction ); - rayEnd = b3AddV( rayStart, b3MulV( lambda, rayDelta ) ); - rayMin = b3MinV( rayStart, rayEnd ); - rayMax = b3MaxV( rayStart, rayEnd ); + if ( pairOutput.hit ) + { + pairOutput.point = b3Add( pairOutput.point, origin ); - // Ray bounds in unscaled space - invScaledRayEnd = b3AddV( invScaledRayStart, b3MulV( lambda, invScaledRayDelta ) ); - invScaledRayMin = b3MinV( invScaledRayStart, invScaledRayEnd ); - invScaledRayMax = b3MaxV( invScaledRayStart, invScaledRayEnd ); - } + bestOutput = pairOutput; + bestOutput.triangleIndex = triangleIndex; + bestOutput.materialIndex = materialIndices[triangleIndex]; + + // Update ray bounds in scaled space + lambda = b3SplatV( pairOutput.fraction ); + rayEnd = b3AddV( rayStart, b3MulV( lambda, rayDelta ) ); + rayMin = b3MinV( rayStart, rayEnd ); + rayMax = b3MaxV( rayStart, rayEnd ); + + // Ray bounds in unscaled space + invScaledRayEnd = b3AddV( invScaledRayStart, b3MulV( lambda, invScaledRayDelta ) ); + invScaledRayMin = b3MinV( invScaledRayStart, invScaledRayEnd ); + invScaledRayMax = b3MaxV( invScaledRayStart, invScaledRayEnd ); } } } @@ -2219,7 +2292,7 @@ int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* sh b3SimplexCache cache = { 0 }; float radius = mover->radius; - + b3Vec3 center = b3Lerp( mover->center1, mover->center2, 0.5f ); b3V32 center1 = b3LoadV( &mover->center1.x ); b3V32 center2 = b3LoadV( &mover->center2.x ); b3V32 r = b3SplatV( radius ); @@ -2228,6 +2301,7 @@ int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* sh // Scale may have reflection so min/max may become invalid when unscaled b3Vec3 meshScale = shape->scale; + bool ccw = meshScale.x * meshScale.y * meshScale.z > 0.0f; b3V32 scale = b3LoadV( &meshScale.x ); b3V32 invScale = b3DivV( b3_oneV, scale ); b3V32 temp1 = b3MulV( invScale, boundsMin ); @@ -2242,6 +2316,7 @@ int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* sh const b3MeshNode* node = b3GetRoot( shape->data ); const b3MeshTriangle* triangles = b3GetMeshTriangles( shape->data ); const b3Vec3* vertices = b3GetMeshVertices( shape->data ); + const uint8_t* materialIndices = b3GetMeshMaterialIndices( shape->data ); int planeCount = 0; while ( planeCount < capacity ) @@ -2264,6 +2339,12 @@ int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* sh b3Vec3 vertex1 = vertices[triangle.index1]; b3Vec3 vertex2 = vertices[triangle.index2]; b3Vec3 vertex3 = vertices[triangle.index3]; + + if ( ccw == false ) + { + B3_SWAP( vertex2, vertex3 ); + } + b3V32 v1 = b3LoadV( &vertex1.x ); b3V32 v2 = b3LoadV( &vertex2.x ); b3V32 v3 = b3LoadV( &vertex3.x ); @@ -2271,10 +2352,19 @@ int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* sh // Test triangle bounds overlap in unscaled space if ( b3TestBoundsTriangleOverlap( invScaledBoundsCenter, invScaledBoundsExtent, v1, v2, v3 ) ) { - // Compute shape distance in scaled space. Winding order doesn't matter. - // todo implement one-sided collision? + // Compute shape distance in scaled space. b3Vec3 triangleVertices[] = { b3Mul( meshScale, vertex1 ), b3Mul( meshScale, vertex2 ), b3Mul( meshScale, vertex3 ) }; + + // Vertices and center are in scaled space. + float signedVolume = + b3SignedVolume( triangleVertices[0], triangleVertices[1], triangleVertices[2], center ); + if ( signedVolume < 0.0f ) + { + // Backside + continue; + } + distanceInput.proxyA = (b3ShapeProxy){ triangleVertices, 3, 0.0f }; // reset the cache @@ -2285,12 +2375,13 @@ int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* sh if ( distanceOutput.distance == 0.0f ) { - // todo SAT + // deep overlap } else if ( distanceOutput.distance <= mover->radius ) { b3Plane plane = { distanceOutput.normal, mover->radius - distanceOutput.distance }; - planes[planeCount] = (b3PlaneResult){ plane, distanceOutput.pointA }; + planes[planeCount] = + (b3PlaneResult){ plane, distanceOutput.pointA, triangleIndex, 0, materialIndices[triangleIndex] }; planeCount += 1; if ( planeCount == capacity ) @@ -2325,7 +2416,7 @@ int b3CollideMoverAndMesh( b3PlaneResult* planes, int capacity, const b3Mesh* sh void b3QueryMesh( const b3Mesh* mesh, b3AABB bounds, b3MeshQueryFcn* fcn, void* context ) { b3Vec3 meshScale = mesh->scale; - bool clockwise = meshScale.x * meshScale.y * meshScale.z > 0.0f; + bool ccw = meshScale.x * meshScale.y * meshScale.z > 0.0f; // Scale may have reflection so min/max may become invalid when unscaled b3V32 scale = b3LoadV( &meshScale.x ); @@ -2370,13 +2461,13 @@ void b3QueryMesh( const b3Mesh* mesh, b3AABB bounds, b3MeshQueryFcn* fcn, void* b3V32 v2 = b3LoadV( &vertex2.x ); b3V32 v3 = b3LoadV( &vertex3.x ); - // Perform triangle overlap test in unscaled space. Winding order doesn't matter. + // Perform triangle overlap test in unscaled space. // todo it is possible that some margins are getting scaled if ( b3TestBoundsTriangleOverlap( invScaledBoundsCenter, invScaledBoundsExtent, v1, v2, v3 ) ) { b3Vec3 a = b3Mul( meshScale, vertex1 ); b3Vec3 b, c; - if ( clockwise ) + if ( ccw ) { b = b3Mul( meshScale, vertex2 ); c = b3Mul( meshScale, vertex3 ); diff --git a/src/mesh_contact.c b/src/mesh_contact.c index 60158e4..b3471ce 100644 --- a/src/mesh_contact.c +++ b/src/mesh_contact.c @@ -11,8 +11,6 @@ #include -// This guards against excessive memory usage and complex collision -#define B3_MAX_MESH_CONTACT_TRIANGLES 256 #define B3_MAX_POINTS_PER_TRIANGLE 32 #if B3_ENABLE_VALIDATION diff --git a/src/physics_world.c b/src/physics_world.c index 815ede2..29c13c7 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -609,9 +609,7 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* continue; } - // Update contact respecting shape/body order (A,B). Bodies behind awake-set - // contacts are always either awake or static - inline b3GetBodySim with that - // invariant to skip the cross-TU call and per-call solverSets indirection. + // Update contact respecting shape/body order (A,B) b3Body* bodyA = bodies + shapeA->bodyId; b3Body* bodyB = bodies + shapeB->bodyId; bool isStaticA = bodyA->type == b3_staticBody; @@ -2811,7 +2809,6 @@ void b3World_CollideMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mov b3Vec3 r = { mover->radius, mover->radius, mover->radius }; - // Relative box lifted to world float with outward rounding, conservative for the tree b3AABB relBox; relBox.lowerBound = b3Sub( b3Min( mover->center1, mover->center2 ), r ); relBox.upperBound = b3Add( b3Max( mover->center1, mover->center2 ), r ); @@ -2828,7 +2825,6 @@ void b3World_CollideMover( b3WorldId worldId, b3Pos origin, const b3Capsule* mov if ( world->recording != NULL ) { - // CollideMover returns void: no treestats tail, just the per-shape plane batches. b3RecPatchU32( &recWriter.buf, recWriter.countOffset, recWriter.hitCount ); b3RecQueryCommit( world->recording, b3_recOpQueryCollideMover, &recWriter ); } diff --git a/src/recording.c b/src/recording.c index a6a0407..0f47fc1 100644 --- a/src/recording.c +++ b/src/recording.c @@ -217,6 +217,9 @@ void b3RecW_PLANERESULT( b3RecBuffer* buf, b3PlaneResult v ) b3RecW_VEC3( buf, v.plane.normal ); b3RecW_F32( buf, v.plane.offset ); b3RecW_VEC3( buf, v.point ); + b3RecW_I32( buf, v.triangleIndex ); + b3RecW_I32( buf, v.childIndex ); + b3RecW_I32( buf, v.materialIndex ); } void b3RecW_WORLDID( b3RecBuffer* buf, b3WorldId v ) diff --git a/src/recording.h b/src/recording.h index e0fcc98..7870192 100644 --- a/src/recording.h +++ b/src/recording.h @@ -47,8 +47,8 @@ typedef struct b3World b3World; #define B3_REC_MAGIC 0x43523342u // Major recording version is bumped when writers change. -// Major version 4 added b3ShapeDef::enableSpeculativeContact -#define B3_REC_VERSION_MAJOR 4 +// Major version 5 added b3PlaneResult fields. +#define B3_REC_VERSION_MAJOR 5 // Minor tracks op-stream additions that keep the 48 byte header shape. // Minor version 4 added b3Shape_SetMeshMaterial, b3Shape_SetHull, b3Shape_SetMesh diff --git a/src/recording_replay.c b/src/recording_replay.c index 17c2996..d1a40bc 100644 --- a/src/recording_replay.c +++ b/src/recording_replay.c @@ -288,6 +288,9 @@ b3PlaneResult b3RecR_PLANERESULT( b3RecReader* rdr ) v.plane.normal = b3RecR_VEC3( rdr ); v.plane.offset = b3RecR_F32( rdr ); v.point = b3RecR_VEC3( rdr ); + v.triangleIndex = b3RecR_I32( rdr ); + v.childIndex = b3RecR_I32( rdr ); + v.materialIndex = b3RecR_I32( rdr ); return v; } @@ -1801,7 +1804,8 @@ static bool b3RecReplayPlaneTrampoline( b3ShapeId id, const b3PlaneResult* plane const b3RecRecordedHit* h = &rc->hits[rc->cursor + i]; if ( b3RecVec3Differs( h->plane.plane.normal, planes[i].plane.normal ) || b3RecF32Differs( h->plane.plane.offset, planes[i].plane.offset ) || - b3RecVec3Differs( h->plane.point, planes[i].point ) ) + b3RecVec3Differs( h->plane.point, planes[i].point ) || h->plane.triangleIndex != planes[i].triangleIndex || + h->plane.childIndex != planes[i].childIndex || h->plane.materialIndex != planes[i].materialIndex ) { rc->rdr->diverged = true; } diff --git a/src/shape.c b/src/shape.c index 1894b05..1f0c305 100644 --- a/src/shape.c +++ b/src/shape.c @@ -778,7 +778,7 @@ b3ShapeExtent b3ComputeShapeExtent( const b3Shape* shape, b3Vec3 localCenter ) b3Vec3 c1 = b3Sub( shape->capsule.center1, localCenter ); b3Vec3 c2 = b3Sub( shape->capsule.center2, localCenter ); b3Vec3 r = { radius, radius, radius }; - extent.maxExtent = b3Add( b3Max( c1, c2 ), r ); + extent.maxExtent = b3Add( b3Max( b3Abs( c1 ), b3Abs( c2 ) ), r ); } break; @@ -798,9 +798,9 @@ b3ShapeExtent b3ComputeShapeExtent( const b3Shape* shape, b3Vec3 localCenter ) { float radius = shape->sphere.radius; extent.minExtent = radius; + b3Vec3 h = b3Abs( b3Sub( shape->sphere.center, localCenter ) ); b3Vec3 r = { radius, radius, radius }; - b3Vec3 p = b3Add( b3Sub( shape->sphere.center, localCenter ), r ); - extent.maxExtent = b3Abs( b3Sub( p, localCenter ) ); + extent.maxExtent = b3Add( h, r ); } break; @@ -816,7 +816,7 @@ b3ShapeExtent b3ComputeShapeExtent( const b3Shape* shape, b3Vec3 localCenter ) float r2 = b3Length( b3Sub( aabb.upperBound, localCenter ) ); extent.minExtent = b3MinFloat( r1, r2 ); b3Vec3 p = b3FarthestPointOnAABB( aabb, localCenter ); - extent.maxExtent = b3Abs( p ); + extent.maxExtent = b3Abs( b3Sub( p, localCenter ) ); } break; @@ -939,32 +939,6 @@ bool b3OverlapShape( const b3Shape* shape, b3Transform transform, const b3ShapeP B3_ASSERT( false ); return false; } - -#if 0 - b3Vec3 localPoints[B3_MAX_SHAPE_CAST_POINTS]; - b3ShapeProxy localProxy; - - b3Transform invTransform = b3InvertTransform( transform ); - b3Matrix3 R = b3MakeMatrixFromQuat( invTransform.q ); - - localProxy.count = b3MinInt( proxy->count, B3_MAX_SHAPE_CAST_POINTS ); - for ( int i = 0; i < localProxy.count; ++i ) - { - localPoints[i] = b3Add( b3MulMV( R, proxy->points[i] ), invTransform.p ); - } - - localProxy.points = localPoints; - localProxy.radius = proxy->radius; - - if ( type == b3_meshShape ) - { - return b3OverlapMesh( &localProxy, shape->mesh.data, shape->mesh.scale ); - } - - B3_ASSERT( type == b3_heightShape ); - - return b3OverlapHeightField( &localProxy, shape->heightField ); -#endif } int b3CollideMover( b3PlaneResult* planes, int planeCapacity, const b3Shape* shape, b3Transform transform, @@ -1016,6 +990,7 @@ int b3CollideMover( b3PlaneResult* planes, int planeCapacity, const b3Shape* sha { planes[i].plane.normal = b3RotateVector( transform.q, planes[i].plane.normal ); planes[i].point = b3TransformPoint( transform, planes[i].point ); + planes[i].materialIndex = b3ClampInt( planes[i].materialIndex, 0, shape->materialCount - 1 ); } return planeCount; diff --git a/src/sphere.c b/src/sphere.c index 9e4bdaa..7b2e6cd 100644 --- a/src/sphere.c +++ b/src/sphere.c @@ -242,6 +242,6 @@ int b3CollideMoverAndSphere( b3PlaneResult* result, const b3Sphere* shape, const } b3Plane plane = { normal, totalRadius - distance }; - *result = ( b3PlaneResult ){ plane, shape->center }; + *result = (b3PlaneResult){ plane, shape->center, 0, 0, 0 }; return 1; } diff --git a/src/spherical_joint.c b/src/spherical_joint.c index 3f699ac..c2c1054 100644 --- a/src/spherical_joint.c +++ b/src/spherical_joint.c @@ -478,6 +478,7 @@ void b3SolveSphericalJoint( b3JointSim* base, b3StepContext* context, bool useBi { b3Vec3 cdot = b3Sub( wB, wA ); + // todo it might be more accurate to have the motor velocity in frameA due to sub-stepping. b3Vec3 lambda = b3Neg( b3MulMV( joint->rotationMass, b3Sub( cdot, joint->motorVelocity ) ) ); b3Vec3 newImpulse = b3Add( joint->motorImpulse, lambda ); float length = b3Length( newImpulse ); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index a6c34d5..bf561bf 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -20,6 +20,7 @@ set(BOX3D_TEST_FILES test_macros.h test_manifold.c test_math.c + test_mesh.c test_mover.c test_name_cache.c test_recording.c diff --git a/test/main.c b/test/main.c index 95ccaa3..bd00ecb 100644 --- a/test/main.c +++ b/test/main.c @@ -42,6 +42,7 @@ extern int JointTest( void ); extern int LargeWorldTest( void ); extern int ManifoldTest( void ); extern int MathTest( void ); +extern int MeshTest( void ); extern int MoverTest( void ); extern int NameCacheTest( void ); extern int RecordingTest( void ); @@ -116,6 +117,7 @@ int main( int argc, char** argv ) MAYBE_RUN_TEST( LargeWorldTest ); MAYBE_RUN_TEST( ManifoldTest ); MAYBE_RUN_TEST( MathTest ); + MAYBE_RUN_TEST( MeshTest ); MAYBE_RUN_TEST( MoverTest ); MAYBE_RUN_TEST( NameCacheTest ); MAYBE_RUN_TEST( RecordingTest ); diff --git a/test/test_body.c b/test/test_body.c index 98a3408..7200f61 100644 --- a/test/test_body.c +++ b/test/test_body.c @@ -385,6 +385,96 @@ static int SetMassDataConsistentVelocity( void ) return 0; } +// Extents bound the shapes about the center of mass, per axis. An offset shape must count its +// offset, not just its own size. +static int ShapeExtents( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1.0f; + + // Kinematic bodies measure from the body origin + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_kinematicBody; + + b3BodyId capsuleId = b3CreateBody( worldId, &bodyDef ); + b3Capsule capsule = { { -2.0f, 0.0f, 0.0f }, { -1.0f, 0.0f, 0.0f }, 0.2f }; + b3CreateCapsuleShape( capsuleId, &shapeDef, &capsule ); + + b3Vec3 maxExtent = b3Body_GetMaxExtent( capsuleId ); + ENSURE_SMALL( maxExtent.x - 2.2f, 1e-5f ); + ENSURE_SMALL( maxExtent.y - 0.2f, 1e-5f ); + ENSURE_SMALL( maxExtent.z - 0.2f, 1e-5f ); + ENSURE_SMALL( b3Body_GetMinExtent( capsuleId ) - 0.2f, 1e-5f ); + + b3BodyId sphereId = b3CreateBody( worldId, &bodyDef ); + b3Sphere sphere = { { 1.0f, 2.0f, 3.0f }, 0.5f }; + b3CreateSphereShape( sphereId, &shapeDef, &sphere ); + + maxExtent = b3Body_GetMaxExtent( sphereId ); + ENSURE_SMALL( maxExtent.x - 1.5f, 1e-5f ); + ENSURE_SMALL( maxExtent.y - 2.5f, 1e-5f ); + ENSURE_SMALL( maxExtent.z - 3.5f, 1e-5f ); + ENSURE_SMALL( b3Body_GetMinExtent( sphereId ) - 0.5f, 1e-5f ); + + // Dynamic bodies measure from the center of mass. A light sphere hung off a cube pulls the + // center toward it, so the far side of the sphere is the widest point. + bodyDef.type = b3_dynamicBody; + b3BodyId cubeId = b3CreateBody( worldId, &bodyDef ); + b3BoxHull cube = b3MakeCubeHull( 0.5f ); + b3CreateHullShape( cubeId, &shapeDef, &cube.base ); + b3Sphere offsetSphere = { { 1.0f, 0.0f, 0.0f }, 0.2f }; + b3CreateSphereShape( cubeId, &shapeDef, &offsetSphere ); + + b3Vec3 localCenter = b3Body_GetLocalCenter( cubeId ); + ENSURE( 0.0f < localCenter.x && localCenter.x < 0.5f ); + + maxExtent = b3Body_GetMaxExtent( cubeId ); + ENSURE_SMALL( maxExtent.x - ( 1.2f - localCenter.x ), 1e-5f ); + ENSURE_SMALL( maxExtent.y - 0.5f, 1e-5f ); + ENSURE_SMALL( maxExtent.z - 0.5f, 1e-5f ); + ENSURE_SMALL( b3Body_GetMinExtent( cubeId ) - 0.2f, 1e-5f ); + + b3Vec3 originExtent = b3Body_GetMaxExtentOrigin( cubeId ); + ENSURE_SMALL( originExtent.x - 1.2f, 1e-5f ); + ENSURE_SMALL( originExtent.y - 0.5f, 1e-5f ); + ENSURE_SMALL( originExtent.z - 0.5f, 1e-5f ); + + // A mesh has no mass, so the sphere alone places the center at x = 1 and the far edge of the + // mesh at x = -3 is four units out. + b3Vec3 vertices[6] = { + { -3.0f, 0.0f, -1.0f }, { -2.0f, 0.0f, 1.0f }, { -1.0f, 0.0f, -1.0f }, + { 1.0f, 0.0f, -1.0f }, { 2.0f, 0.0f, 1.0f }, { 3.0f, 0.0f, -1.0f }, + }; + int32_t indices[6] = { 0, 1, 2, 3, 4, 5 }; + b3MeshDef meshDef = { 0 }; + meshDef.vertices = vertices; + meshDef.stride = sizeof( b3Vec3 ); + meshDef.indices = indices; + meshDef.vertexCount = 6; + meshDef.triangleCount = 2; + b3MeshData* mesh = b3CreateMesh( &meshDef, NULL, 0 ); + + b3BodyId meshId = b3CreateBody( worldId, &bodyDef ); + b3CreateSphereShape( meshId, &shapeDef, &offsetSphere ); + b3CreateMeshShape( meshId, &shapeDef, mesh, (b3Vec3){ 1.0f, 1.0f, 1.0f } ); + + localCenter = b3Body_GetLocalCenter( meshId ); + ENSURE_SMALL( localCenter.x - 1.0f, 1e-5f ); + + maxExtent = b3Body_GetMaxExtent( meshId ); + ENSURE_SMALL( maxExtent.x - 4.0f, 1e-4f ); + ENSURE_SMALL( maxExtent.y - 0.2f, 1e-4f ); + ENSURE_SMALL( maxExtent.z - 1.0f, 1e-4f ); + ENSURE_SMALL( b3Body_GetMinExtent( meshId ) - 0.2f, 1e-5f ); + + b3DestroyWorld( worldId ); + b3DestroyMesh( mesh ); + return 0; +} + int BodyTest( void ) { RUN_SUBTEST( FarSingleSphereMass ); @@ -395,5 +485,6 @@ int BodyTest( void ) RUN_SUBTEST( SetMassDataFixedRotation ); RUN_SUBTEST( SetMassDataZeroMass ); RUN_SUBTEST( SetMassDataConsistentVelocity ); + RUN_SUBTEST( ShapeExtents ); return 0; } diff --git a/test/test_body_query.c b/test/test_body_query.c index 8ea43d9..ca05c35 100644 --- a/test/test_body_query.c +++ b/test/test_body_query.c @@ -565,6 +565,412 @@ static int MoverCapacity( void ) return 0; } +// TimeOfImpactMover ------------------------------------------------------------------------ + +// The mover capsule is expressed in the query frame, so a core segment starting at the origin +// stands the character on the query point. Targets sit on the sweep line at y = 0. +static b3Capsule MakeStandingMover( void ) +{ + return (b3Capsule){ { 0.0f, 0.0f, 0.0f }, { 0.0f, 1.0f, 0.0f }, 0.25f }; +} + +static int MoverTOIHitsBox( void ) +{ + b3BodyId bodyId; + b3WorldId worldId = CreateQueryWorld( &bodyId ); + + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3ShapeId shapeId = b3CreateHullShape( bodyId, &shapeDef, &box.base ); + + // Face at x = 4.5, mover radius 0.25, so 4.25 of the 10 unit sweep is free. + b3Capsule mover = MakeStandingMover(); + b3WorldTransform bodyTransform = IdentityAt( 5.0f, 0.0f, 0.0f ); + b3BodyTOIResult result = b3Body_TimeOfImpactMover( bodyId, b3Pos_zero, &mover, (b3Vec3){ 10.0f, 0.0f, 0.0f }, + b3DefaultQueryFilter(), bodyTransform, bodyTransform ); + + ENSURE_SMALL( result.fraction - 0.425f, 1e-2f ); + ENSURE( b3IsNormalized( result.normal ) ); + ENSURE_SMALL( result.normal.x + 1.0f, 1e-3f ); + + // The result carries a shape id, so the hit shape must come back identified. + ENSURE( b3Shape_IsValid( result.shapeId ) ); + ENSURE( result.shapeId.index1 == shapeId.index1 ); + ENSURE( result.shapeId.generation == shapeId.generation ); + + b3DestroyWorld( worldId ); + return 0; +} + +static int MoverTOISeparated( void ) +{ + b3BodyId bodyId; + b3WorldId worldId = CreateQueryWorld( &bodyId ); + + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + + // Sweeping along +Y holds the X gap at 4.25 for the whole interval. + b3Capsule mover = MakeStandingMover(); + b3WorldTransform bodyTransform = IdentityAt( 5.0f, 0.0f, 0.0f ); + b3BodyTOIResult result = b3Body_TimeOfImpactMover( bodyId, b3Pos_zero, &mover, (b3Vec3){ 0.0f, 10.0f, 0.0f }, + b3DefaultQueryFilter(), bodyTransform, bodyTransform ); + + ENSURE_SMALL( result.fraction - 1.0f, 1e-6f ); + ENSURE( b3Shape_IsValid( result.shapeId ) == false ); + + b3DestroyWorld( worldId ); + return 0; +} + +static int MoverTOIOverlapped( void ) +{ + b3BodyId bodyId; + b3WorldId worldId = CreateQueryWorld( &bodyId ); + + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + + // Mover starts buried in the box, so there is no free interval to search. + b3Capsule mover = MakeStandingMover(); + b3WorldTransform bodyTransform = IdentityAt( 0.0f, 0.0f, 0.0f ); + b3BodyTOIResult result = b3Body_TimeOfImpactMover( bodyId, b3Pos_zero, &mover, (b3Vec3){ 10.0f, 0.0f, 0.0f }, + b3DefaultQueryFilter(), bodyTransform, bodyTransform ); + + // Overlap should be ignored. + ENSURE_SMALL( result.fraction - 1.0f, 1e-6f ); + ENSURE( B3_IS_NULL(result.shapeId) ); + + b3DestroyWorld( worldId ); + return 0; +} + +// Two shapes on the sweep line must resolve to the nearer one whatever order the shape list +// hands them to the loop. Shapes are pushed on the head of the list, so the two bodies below +// walk their shapes in opposite orders. +static int MoverTOIClosestShape( void ) +{ + b3BodyId nearFirstId; + b3WorldId worldId = CreateQueryWorld( &nearFirstId ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId nearLastId = b3CreateBody( worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3Sphere nearSphere = { { 5.0f, 0.0f, 0.0f }, 0.5f }; + b3Sphere farSphere = { { 9.0f, 0.0f, 0.0f }, 0.5f }; + + b3ShapeId nearFirstHit = b3CreateSphereShape( nearFirstId, &shapeDef, &nearSphere ); + b3CreateSphereShape( nearFirstId, &shapeDef, &farSphere ); + + b3CreateSphereShape( nearLastId, &shapeDef, &farSphere ); + b3ShapeId nearLastHit = b3CreateSphereShape( nearLastId, &shapeDef, &nearSphere ); + + b3Capsule mover = MakeStandingMover(); + b3WorldTransform bodyTransform = IdentityAt( 0.0f, 0.0f, 0.0f ); + b3Vec3 translation = { 10.0f, 0.0f, 0.0f }; + + b3BodyTOIResult nearFirst = b3Body_TimeOfImpactMover( nearFirstId, b3Pos_zero, &mover, translation, b3DefaultQueryFilter(), + bodyTransform, bodyTransform ); + b3BodyTOIResult nearLast = b3Body_TimeOfImpactMover( nearLastId, b3Pos_zero, &mover, translation, b3DefaultQueryFilter(), + bodyTransform, bodyTransform ); + + ENSURE_SMALL( nearFirst.fraction - 0.425f, 1e-2f ); + ENSURE( nearFirst.shapeId.index1 == nearFirstHit.index1 ); + + ENSURE_SMALL( nearLast.fraction - nearFirst.fraction, 1e-4f ); + ENSURE( nearLast.shapeId.index1 == nearLastHit.index1 ); + + b3DestroyWorld( worldId ); + return 0; +} + +// A shape the sweep clears must not erase a hit found earlier in the list. The off path sphere +// is created first so the list hands it over last. +static int MoverTOIKeepsHitAfterMiss( void ) +{ + b3BodyId bodyId; + b3WorldId worldId = CreateQueryWorld( &bodyId ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3Sphere offPath = { { 5.0f, 0.0f, 20.0f }, 0.5f }; + b3Sphere onPath = { { 5.0f, 0.0f, 0.0f }, 0.5f }; + b3CreateSphereShape( bodyId, &shapeDef, &offPath ); + b3ShapeId onPathId = b3CreateSphereShape( bodyId, &shapeDef, &onPath ); + + b3Capsule mover = MakeStandingMover(); + b3WorldTransform bodyTransform = IdentityAt( 0.0f, 0.0f, 0.0f ); + b3BodyTOIResult result = b3Body_TimeOfImpactMover( bodyId, b3Pos_zero, &mover, (b3Vec3){ 10.0f, 0.0f, 0.0f }, + b3DefaultQueryFilter(), bodyTransform, bodyTransform ); + + ENSURE_SMALL( result.fraction - 0.425f, 1e-2f ); + ENSURE( result.shapeId.index1 == onPathId.index1 ); + + b3DestroyWorld( worldId ); + return 0; +} + +// The mover capsule points live in the query frame, so sliding both the mover and the body by +// the same offset must not move the impact. +static int MoverTOIMoverOffset( void ) +{ + b3BodyId bodyId; + b3WorldId worldId = CreateQueryWorld( &bodyId ); + + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + + b3Vec3 translation = { 10.0f, 0.0f, 0.0f }; + b3QueryFilter filter = b3DefaultQueryFilter(); + + b3Capsule atOrigin = MakeStandingMover(); + b3WorldTransform transformA = IdentityAt( 5.0f, 0.0f, 0.0f ); + b3BodyTOIResult resultA = + b3Body_TimeOfImpactMover( bodyId, b3Pos_zero, &atOrigin, translation, filter, transformA, transformA ); + + b3Capsule offset = { { 2.0f, 0.0f, 0.0f }, { 2.0f, 1.0f, 0.0f }, 0.25f }; + b3WorldTransform transformB = IdentityAt( 7.0f, 0.0f, 0.0f ); + b3BodyTOIResult resultB = b3Body_TimeOfImpactMover( bodyId, b3Pos_zero, &offset, translation, filter, transformB, transformB ); + + ENSURE_SMALL( resultB.fraction - resultA.fraction, 1e-4f ); + ENSURE_SMALL( resultB.point.x - resultA.point.x - 2.0f, 1e-3f ); + + b3DestroyWorld( worldId ); + return 0; +} + +// The body sweeps between the two supplied transforms. A bar spinning a quarter turn about Y +// reaches a mover that its start pose clears. +static int MoverTOIRotatingBody( void ) +{ + b3BodyId bodyId; + b3WorldId worldId = CreateQueryWorld( &bodyId ); + + b3BoxHull bar = b3MakeBoxHull( 2.0f, 0.25f, 0.25f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( bodyId, &shapeDef, &bar.base ); + + // Query origin puts the mover at world (0,0,2.1), just inside the swept end of the bar. + b3Pos origin = { 0.0f, 0.0f, 2.1f }; + b3Capsule mover = MakeStandingMover(); + b3WorldTransform transform1 = IdentityAt( 0.0f, 0.0f, 0.0f ); + b3WorldTransform transform2 = { + .p = (b3Pos){ 0.0f, 0.0f, 0.0f }, + .q = b3MakeQuatFromAxisAngle( (b3Vec3){ 0.0f, 1.0f, 0.0f }, 0.5f * B3_PI ), + }; + + b3BodyTOIResult spinning = + b3Body_TimeOfImpactMover( bodyId, origin, &mover, b3Vec3_zero, b3DefaultQueryFilter(), transform1, transform2 ); + + ENSURE( 0.0f < spinning.fraction && spinning.fraction < 1.0f ); + + // Holding the start pose leaves the bar along X and well clear. + b3BodyTOIResult still = + b3Body_TimeOfImpactMover( bodyId, origin, &mover, b3Vec3_zero, b3DefaultQueryFilter(), transform1, transform1 ); + + ENSURE_SMALL( still.fraction - 1.0f, 1e-6f ); + ENSURE( B3_IS_NULL( still.shapeId ) ); + + b3DestroyWorld( worldId ); + return 0; +} + +// Everything is re-centered on the origin, so a huge origin must not shift the fraction and the +// hit point comes back in the origin frame. +static int MoverTOIFarFromOrigin( void ) +{ + b3BodyId bodyId; + b3WorldId worldId = CreateQueryWorld( &bodyId ); + + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + + b3Pos origin = { 1.0e6f, -2.0e6f, 5.0e5f }; + b3Capsule mover = MakeStandingMover(); + b3WorldTransform bodyTransform = { .p = b3OffsetPos( origin, (b3Vec3){ 5.0f, 0.0f, 0.0f } ), .q = b3Quat_identity }; + b3BodyTOIResult result = b3Body_TimeOfImpactMover( bodyId, origin, &mover, (b3Vec3){ 10.0f, 0.0f, 0.0f }, + b3DefaultQueryFilter(), bodyTransform, bodyTransform ); + + ENSURE_SMALL( result.fraction - 0.425f, 1e-2f ); + ENSURE_SMALL( result.normal.x + 1.0f, 1e-3f ); + ENSURE_SMALL( (result.point.x - origin.x) - 4.4f, 0.5f ); + + b3DestroyWorld( worldId ); + return 0; +} + +static int MoverTOIFilter( void ) +{ + b3BodyId bodyId; + b3WorldId worldId = CreateQueryWorld( &bodyId ); + + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + + // Geometry is on the sweep line, but a zero mask rejects every category. + b3Capsule mover = MakeStandingMover(); + b3QueryFilter filter = b3DefaultQueryFilter(); + filter.maskBits = 0; + b3WorldTransform bodyTransform = IdentityAt( 5.0f, 0.0f, 0.0f ); + b3BodyTOIResult result = b3Body_TimeOfImpactMover( bodyId, b3Pos_zero, &mover, (b3Vec3){ 10.0f, 0.0f, 0.0f }, filter, + bodyTransform, bodyTransform ); + + ENSURE_SMALL( result.fraction - 1.0f, 1e-6f ); + + b3DestroyWorld( worldId ); + return 0; +} + +// A shape the mover starts inside is skipped, but the sweep still runs against the other shapes +// on the body. +static int MoverTOIOverlapSkipsShape( void ) +{ + b3BodyId bodyId; + b3WorldId worldId = CreateQueryWorld( &bodyId ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + b3BoxHull buried = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3CreateHullShape( bodyId, &shapeDef, &buried.base ); + + // Face at x = 4.5 like MoverTOIHitsBox + b3BoxHull onPath = b3MakeOffsetBoxHull( 0.5f, 0.5f, 0.5f, (b3Vec3){ 5.0f, 0.0f, 0.0f } ); + b3ShapeId onPathId = b3CreateHullShape( bodyId, &shapeDef, &onPath.base ); + + b3Capsule mover = MakeStandingMover(); + b3WorldTransform bodyTransform = IdentityAt( 0.0f, 0.0f, 0.0f ); + b3BodyTOIResult result = b3Body_TimeOfImpactMover( bodyId, b3Pos_zero, &mover, (b3Vec3){ 10.0f, 0.0f, 0.0f }, + b3DefaultQueryFilter(), bodyTransform, bodyTransform ); + + ENSURE_SMALL( result.fraction - 0.425f, 1e-2f ); + ENSURE( result.shapeId.index1 == onPathId.index1 ); + ENSURE_SMALL( result.normal.x + 1.0f, 1e-3f ); + + b3DestroyWorld( worldId ); + return 0; +} + +static float RandomFloat( uint32_t* seed, float lower, float upper ) +{ + uint32_t x = *seed; + x ^= x << 13; + x ^= x >> 17; + x ^= x << 5; + *seed = x; + float unit = (float)( x >> 8 ) / 16777216.0f; + return lower + ( upper - lower ) * unit; +} + +static b3Vec3 RandomVector( uint32_t* seed, float lower, float upper ) +{ + b3Vec3 v; + v.x = RandomFloat( seed, lower, upper ); + v.y = RandomFloat( seed, lower, upper ); + v.z = RandomFloat( seed, lower, upper ); + return v; +} + +static b3Vec3 RandomDirection( uint32_t* seed ) +{ + for ( ;; ) + { + b3Vec3 v = RandomVector( seed, -1.0f, 1.0f ); + float length = b3Length( v ); + if ( 0.1f < length && length < 1.0f ) + { + return b3MulSV( 1.0f / length, v ); + } + } +} + +// The mover sweep follows the solver's continuous rule: any time of impact strictly inside the +// sweep counts, whatever the root finder reports. A capsule pivoting around a hull vertex can +// exhaust the iteration cap, and that must not read as a clean miss. Compare against the raw +// sweep built from the same inputs. +static int MoverTOIMatchesSweep( void ) +{ + b3BodyId bodyId; + b3WorldId worldId = CreateQueryWorld( &bodyId ); + + b3BoxHull bar = b3MakeBoxHull( 2.0f, 0.25f, 0.25f ); + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3ShapeId shapeId = b3CreateHullShape( bodyId, &shapeDef, &bar.base ); + + uint32_t seed = 0x9E3779B9u; + int hitCount = 0; + for ( int i = 0; i < 2000; ++i ) + { + b3Capsule mover; + mover.center1 = RandomVector( &seed, -0.2f, 0.2f ); + mover.center2 = b3MulAdd( mover.center1, RandomFloat( &seed, 0.05f, 1.5f ), RandomDirection( &seed ) ); + mover.radius = RandomFloat( &seed, 0.05f, 0.5f ); + b3Vec3 capsulePoints[2] = { mover.center1, mover.center2 }; + + b3Vec3 o = RandomVector( &seed, -3.5f, 3.5f ); + b3Pos origin = { o.x, o.y, o.z }; + b3Vec3 translation = b3MulSV( RandomFloat( &seed, 0.0f, 3.0f ), RandomDirection( &seed ) ); + + b3WorldTransform transform1 = { + .p = b3Pos_zero, + .q = b3MakeQuatFromAxisAngle( RandomDirection( &seed ), RandomFloat( &seed, 0.0f, B3_PI ) ), + }; + b3Quat spin = b3MakeQuatFromAxisAngle( RandomDirection( &seed ), RandomFloat( &seed, 0.001f, B3_PI ) ); + b3Vec3 shift = RandomVector( &seed, -1.0f, 1.0f ); + b3WorldTransform transform2 = { + .p = (b3Pos){ shift.x, shift.y, shift.z }, + .q = b3MulQuat( spin, transform1.q ), + }; + + b3BodyTOIResult result = b3Body_TimeOfImpactMover( bodyId, origin, &mover, translation, b3DefaultQueryFilter(), + transform1, transform2 ); + + // Same sweep the body query builds. The static body keeps its center at the body origin. + b3Transform xf1 = b3ToRelativeTransform( transform1, origin ); + b3Transform xf2 = b3ToRelativeTransform( transform2, origin ); + + b3TOIInput input = { 0 }; + input.proxyA = (b3ShapeProxy){ b3GetHullPoints( &bar.base ), bar.base.vertexCount, 0.0f }; + input.proxyB = (b3ShapeProxy){ capsulePoints, 2, mover.radius }; + input.sweepA.c1 = xf1.p; + input.sweepA.c2 = xf2.p; + input.sweepA.q1 = transform1.q; + input.sweepA.q2 = transform2.q; + input.sweepA.localCenter = b3Vec3_zero; + input.sweepB.c1 = b3Vec3_zero; + input.sweepB.c2 = translation; + input.sweepB.q1 = b3Quat_identity; + input.sweepB.q2 = b3Quat_identity; + input.sweepB.localCenter = b3Vec3_zero; + input.maxFraction = 1.0f; + + b3TOIOutput raw = b3TimeOfImpact( &input ); + + if ( 0.0f < raw.fraction && raw.fraction < 1.0f ) + { + ENSURE_SMALL( result.fraction - raw.fraction, 1e-6f ); + ENSURE( result.shapeId.index1 == shapeId.index1 ); + ENSURE( b3IsNormalized( result.normal ) ); + ENSURE_SMALL( b3Distance( b3SubPos( result.point, origin ), raw.point ), 1e-4f ); + hitCount += 1; + } + else + { + ENSURE_SMALL( result.fraction - 1.0f, 1e-6f ); + ENSURE( B3_IS_NULL( result.shapeId ) ); + } + } + + ENSURE( hitCount > 0 ); + + b3DestroyWorld( worldId ); + return 0; +} + int BodyQueryTest( void ) { RUN_SUBTEST( CastRayHitsSphere ); @@ -589,6 +995,17 @@ int BodyQueryTest( void ) RUN_SUBTEST( MoverSeparated ); RUN_SUBTEST( MoverRotatedBody ); RUN_SUBTEST( MoverCapacity ); + RUN_SUBTEST( MoverTOIHitsBox ); + RUN_SUBTEST( MoverTOISeparated ); + RUN_SUBTEST( MoverTOIOverlapped ); + RUN_SUBTEST( MoverTOIClosestShape ); + RUN_SUBTEST( MoverTOIKeepsHitAfterMiss ); + RUN_SUBTEST( MoverTOIMoverOffset ); + RUN_SUBTEST( MoverTOIRotatingBody ); + RUN_SUBTEST( MoverTOIFarFromOrigin ); + RUN_SUBTEST( MoverTOIFilter ); + RUN_SUBTEST( MoverTOIOverlapSkipsShape ); + RUN_SUBTEST( MoverTOIMatchesSweep ); return 0; } diff --git a/test/test_height_field.c b/test/test_height_field.c index b293579..78ed368 100644 --- a/test/test_height_field.c +++ b/test/test_height_field.c @@ -526,6 +526,153 @@ static int RayCastBruteForce( void ) return 0; } +// Flat 3x3 vertex field at y = 0, one cell material per entry. Caller destroys. +static b3HeightFieldData* MakeFlatField( uint8_t* materials, bool clockwise ) +{ + float heights[9] = { 0 }; + + b3HeightFieldDef def = { 0 }; + def.heights = heights; + def.materialIndices = materials; + def.scale = (b3Vec3){ 1.0f, 1.0f, 1.0f }; + def.countX = 3; + def.countZ = 3; + def.globalMinimumHeight = -1.0f; + def.globalMaximumHeight = 1.0f; + def.clockwiseWinding = clockwise; + + return b3CreateHeightField( &def ); +} + +static int ShapeCastBackside( void ) +{ + uint8_t materials[4] = { 1, 0, 0, 0 }; + b3HeightFieldData* hf = MakeFlatField( materials, false ); + + // Falling onto the front (upper) face reports the hit, the triangle, and its material. + // The surface sits at y = 0, so a sphere of radius 0.2 stops with its center at y = 0.2. + b3Vec3 above = { 0.3f, 5.0f, 0.25f }; + b3ShapeCastInput down = { 0 }; + down.proxy = (b3ShapeProxy){ &above, 1, 0.2f }; + down.translation = (b3Vec3){ 0.0f, -10.0f, 0.0f }; + down.maxFraction = 1.0f; + + b3CastOutput out = b3ShapeCastHeightField( hf, &down ); + ENSURE( out.hit == true ); + ENSURE_SMALL( out.fraction - 0.48f, 1e-2f ); + ENSURE( out.normal.y > 0.99f ); + ENSURE( out.triangleIndex == 0 || out.triangleIndex == 1 ); + ENSURE( out.materialIndex == 1 ); + + // Rising from behind the face is culled + b3Vec3 below = { 0.3f, -5.0f, 0.25f }; + b3ShapeCastInput up = { 0 }; + up.proxy = (b3ShapeProxy){ &below, 1, 0.2f }; + up.translation = (b3Vec3){ 0.0f, 10.0f, 0.0f }; + up.maxFraction = 1.0f; + + b3CastOutput missed = b3ShapeCastHeightField( hf, &up ); + ENSURE( missed.hit == false ); + + b3DestroyHeightField( hf ); + return 0; +} + +// A clockwise height field faces down, see HeightFieldWinding. Shape cast backside +// culling must respect the flag: a rising sphere hits from below, not from above. +static int ShapeCastClockwiseWinding( void ) +{ + uint8_t materials[4] = { 1, 0, 0, 0 }; + b3HeightFieldData* hf = MakeFlatField( materials, true ); + + b3Vec3 below = { 0.3f, -5.0f, 0.25f }; + b3ShapeCastInput up = { 0 }; + up.proxy = (b3ShapeProxy){ &below, 1, 0.2f }; + up.translation = (b3Vec3){ 0.0f, 10.0f, 0.0f }; + up.maxFraction = 1.0f; + + b3CastOutput out = b3ShapeCastHeightField( hf, &up ); + ENSURE( out.hit == true ); + ENSURE_SMALL( out.fraction - 0.48f, 1e-2f ); + ENSURE( out.normal.y < -0.99f ); + ENSURE( out.triangleIndex == 0 || out.triangleIndex == 1 ); + ENSURE( out.materialIndex == 1 ); + + b3Vec3 above = { 0.3f, 5.0f, 0.25f }; + b3ShapeCastInput down = { 0 }; + down.proxy = (b3ShapeProxy){ &above, 1, 0.2f }; + down.translation = (b3Vec3){ 0.0f, -10.0f, 0.0f }; + down.maxFraction = 1.0f; + + b3CastOutput missed = b3ShapeCastHeightField( hf, &down ); + ENSURE( missed.hit == false ); + + b3DestroyHeightField( hf ); + return 0; +} + +// Ray casts run through the shape cast. The default winding faces up, so a ray from below is the +// back side and gets culled. +static int RayCastBackside( void ) +{ + uint8_t materials[4] = { 1, 0, 0, 0 }; + b3HeightFieldData* hf = MakeFlatField( materials, false ); + + b3RayCastInput down = { 0 }; + down.origin = (b3Vec3){ 0.3f, 5.0f, 0.25f }; + down.translation = (b3Vec3){ 0.0f, -10.0f, 0.0f }; + down.maxFraction = 1.0f; + + b3CastOutput out = b3RayCastHeightField( hf, &down ); + ENSURE( out.hit == true ); + ENSURE_SMALL( out.fraction - 0.5f, 1e-3f ); + ENSURE( out.normal.y > 0.99f ); + ENSURE( out.triangleIndex == 0 || out.triangleIndex == 1 ); + ENSURE( out.materialIndex == 1 ); + + b3RayCastInput up = { 0 }; + up.origin = (b3Vec3){ 0.3f, -5.0f, 0.25f }; + up.translation = (b3Vec3){ 0.0f, 10.0f, 0.0f }; + up.maxFraction = 1.0f; + + b3CastOutput missed = b3RayCastHeightField( hf, &up ); + ENSURE( missed.hit == false ); + + b3DestroyHeightField( hf ); + return 0; +} + +// A clockwise field faces down. The winding swap already flips the triangle, so the ray normal +// must not be flipped a second time. +static int RayCastClockwiseWinding( void ) +{ + uint8_t materials[4] = { 1, 0, 0, 0 }; + b3HeightFieldData* hf = MakeFlatField( materials, true ); + + b3RayCastInput up = { 0 }; + up.origin = (b3Vec3){ 0.3f, -5.0f, 0.25f }; + up.translation = (b3Vec3){ 0.0f, 10.0f, 0.0f }; + up.maxFraction = 1.0f; + + b3CastOutput out = b3RayCastHeightField( hf, &up ); + ENSURE( out.hit == true ); + ENSURE_SMALL( out.fraction - 0.5f, 1e-3f ); + ENSURE( out.normal.y < -0.99f ); + ENSURE( out.triangleIndex == 0 || out.triangleIndex == 1 ); + ENSURE( out.materialIndex == 1 ); + + b3RayCastInput down = { 0 }; + down.origin = (b3Vec3){ 0.3f, 5.0f, 0.25f }; + down.translation = (b3Vec3){ 0.0f, -10.0f, 0.0f }; + down.maxFraction = 1.0f; + + b3CastOutput missed = b3RayCastHeightField( hf, &down ); + ENSURE( missed.hit == false ); + + b3DestroyHeightField( hf ); + return 0; +} + int HeightFieldTest( void ) { RUN_SUBTEST( HeightFieldCreate ); @@ -537,6 +684,10 @@ int HeightFieldTest( void ) RUN_SUBTEST( ShapeCastVerticalStraddle ); RUN_SUBTEST( ShapeCastBruteForce ); RUN_SUBTEST( RayCastBruteForce ); + RUN_SUBTEST( ShapeCastBackside ); + RUN_SUBTEST( ShapeCastClockwiseWinding ); + RUN_SUBTEST( RayCastBackside ); + RUN_SUBTEST( RayCastClockwiseWinding ); return 0; } diff --git a/test/test_mesh.c b/test/test_mesh.c new file mode 100644 index 0000000..69330ed --- /dev/null +++ b/test/test_mesh.c @@ -0,0 +1,355 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "test_macros.h" + +#include "box3d/collision.h" +#include "box3d/math_functions.h" + +#include +#include + +// Two quads meeting at a concave crease along the shared edge 1-4. The crease exercises +// edge identification, which reads the baked winding rather than the input winding. +#define VALLEY_VERTEX_COUNT 6 +#define VALLEY_TRIANGLE_COUNT 4 + +static void MakeValley( b3Vec3* vertices, int32_t* indices ) +{ + b3Vec3 v[VALLEY_VERTEX_COUNT] = { + { -1.0f, 1.0f, -1.0f }, { 0.0f, 0.0f, -1.0f }, { 1.0f, 1.0f, -1.0f }, + { -1.0f, 1.0f, 1.0f }, { 0.0f, 0.0f, 1.0f }, { 1.0f, 1.0f, 1.0f }, + }; + + int32_t i[3 * VALLEY_TRIANGLE_COUNT] = { + 0, 3, 1, // left quad + 3, 4, 1, + 1, 4, 2, // right quad + 4, 5, 2, + }; + + memcpy( vertices, v, sizeof( v ) ); + memcpy( indices, i, sizeof( i ) ); +} + +// A vertex laid out the way a renderer would hand it over. The position sits off the +// front of the struct so a non-zero base offset gets exercised too. +typedef struct FatVertex +{ + float weight; + b3Vec3 position; + float uv[2]; +} FatVertex; + +static void MakeFatVertices( FatVertex* fat, const b3Vec3* vertices, int count ) +{ + for ( int i = 0; i < count; ++i ) + { + // Poison the padding so a stride mistake bakes obvious garbage instead of near misses + fat[i].weight = 1000.0f + (float)i; + fat[i].position = vertices[i]; + fat[i].uv[0] = -1000.0f; + fat[i].uv[1] = -2000.0f; + } +} + +static void ReverseWinding( int32_t* indices, int triangleCount ) +{ + for ( int i = 0; i < triangleCount; ++i ) + { + int32_t temp = indices[3 * i + 1]; + indices[3 * i + 1] = indices[3 * i + 2]; + indices[3 * i + 2] = temp; + } +} + +// The hash covers every byte of the mesh block, so this compares the tree, the vertices, +// the baked triangles and the edge flags in one shot. +static bool MeshesMatch( const b3MeshData* mesh1, const b3MeshData* mesh2 ) +{ + return mesh1->byteCount == mesh2->byteCount && mesh1->hash == mesh2->hash; +} + +static bool VerticesMatch( const b3MeshData* mesh, const b3Vec3* expected, int count ) +{ + if ( mesh->vertexCount != count ) + { + return false; + } + + const b3Vec3* vertices = b3GetMeshVertices( mesh ); + for ( int i = 0; i < count; ++i ) + { + if ( b3DistanceSquared( vertices[i], expected[i] ) > FLT_EPSILON ) + { + return false; + } + } + + return true; +} + +// The valley faces up, so every baked triangle should wind counter clockwise about +Y +static bool FacesUp( const b3MeshData* mesh ) +{ + const b3Vec3* vertices = b3GetMeshVertices( mesh ); + const b3MeshTriangle* triangles = b3GetMeshTriangles( mesh ); + + for ( int i = 0; i < mesh->triangleCount; ++i ) + { + b3Vec3 v1 = vertices[triangles[i].index1]; + b3Vec3 v2 = vertices[triangles[i].index2]; + b3Vec3 v3 = vertices[triangles[i].index3]; + + b3Vec3 normal = b3Cross( b3Sub( v2, v1 ), b3Sub( v3, v1 ) ); + if ( normal.y <= 0.0f ) + { + return false; + } + } + + return true; +} + +static bool HasConcaveEdge( const b3MeshData* mesh ) +{ + const uint8_t* flags = b3GetMeshFlags( mesh ); + + for ( int i = 0; i < mesh->triangleCount; ++i ) + { + if ( ( flags[i] & b3_allConcaveEdges ) != 0 ) + { + return true; + } + } + + return false; +} + +static b3MeshDef MakeValleyDef( b3Vec3* vertices, int32_t* indices ) +{ + b3MeshDef def = { 0 }; + def.vertices = vertices; + def.stride = sizeof( b3Vec3 ); + def.indices = indices; + def.vertexCount = VALLEY_VERTEX_COUNT; + def.triangleCount = VALLEY_TRIANGLE_COUNT; + def.identifyEdges = true; + return def; +} + +// Dense input is the reference the other cases are measured against +static int MeshDenseStride( void ) +{ + b3Vec3 vertices[VALLEY_VERTEX_COUNT]; + int32_t indices[3 * VALLEY_TRIANGLE_COUNT]; + MakeValley( vertices, indices ); + + b3MeshDef def = MakeValleyDef( vertices, indices ); + b3MeshData* mesh = b3CreateMesh( &def, NULL, 0 ); + ENSURE( mesh != NULL ); + + ENSURE( mesh->vertexCount == VALLEY_VERTEX_COUNT ); + ENSURE( mesh->triangleCount == VALLEY_TRIANGLE_COUNT ); + ENSURE( mesh->degenerateCount == 0 ); + ENSURE( VerticesMatch( mesh, vertices, VALLEY_VERTEX_COUNT ) ); + ENSURE( FacesUp( mesh ) ); + ENSURE( HasConcaveEdge( mesh ) ); + + b3DestroyMesh( mesh ); + return 0; +} + +// Interleaved input must bake to exactly the same mesh as dense input +static int MeshFatStride( void ) +{ + b3Vec3 vertices[VALLEY_VERTEX_COUNT]; + int32_t indices[3 * VALLEY_TRIANGLE_COUNT]; + MakeValley( vertices, indices ); + + b3MeshDef denseDef = MakeValleyDef( vertices, indices ); + b3MeshData* denseMesh = b3CreateMesh( &denseDef, NULL, 0 ); + ENSURE( denseMesh != NULL ); + + FatVertex fat[VALLEY_VERTEX_COUNT]; + MakeFatVertices( fat, vertices, VALLEY_VERTEX_COUNT ); + + b3MeshDef fatDef = MakeValleyDef( &fat[0].position, indices ); + fatDef.stride = sizeof( FatVertex ); + b3MeshData* fatMesh = b3CreateMesh( &fatDef, NULL, 0 ); + ENSURE( fatMesh != NULL ); + + ENSURE( VerticesMatch( fatMesh, vertices, VALLEY_VERTEX_COUNT ) ); + ENSURE( FacesUp( fatMesh ) ); + ENSURE( MeshesMatch( denseMesh, fatMesh ) ); + + b3DestroyMesh( fatMesh ); + b3DestroyMesh( denseMesh ); + return 0; +} + +// Welding reads the source vertices through the stride as well +static int MeshStrideWeld( void ) +{ + b3Vec3 vertices[VALLEY_VERTEX_COUNT]; + int32_t indices[3 * VALLEY_TRIANGLE_COUNT]; + MakeValley( vertices, indices ); + + // Split the crease so each quad owns a copy of the shared edge + b3Vec3 splitVertices[8]; + memcpy( splitVertices, vertices, sizeof( vertices ) ); + splitVertices[6] = vertices[1]; + splitVertices[7] = vertices[4]; + + // Point the right quad at the copies + int32_t splitIndices[3 * VALLEY_TRIANGLE_COUNT]; + memcpy( splitIndices, indices, sizeof( indices ) ); + splitIndices[6] = 6; + splitIndices[7] = 7; + splitIndices[9] = 7; + + FatVertex fat[8]; + MakeFatVertices( fat, splitVertices, 8 ); + + b3MeshDef def = MakeValleyDef( &fat[0].position, splitIndices ); + def.stride = sizeof( FatVertex ); + def.vertexCount = 8; + def.weldVertices = true; + def.weldTolerance = 0.01f; + + b3MeshData* mesh = b3CreateMesh( &def, NULL, 0 ); + ENSURE( mesh != NULL ); + + ENSURE( mesh->vertexCount == VALLEY_VERTEX_COUNT ); + ENSURE( mesh->triangleCount == VALLEY_TRIANGLE_COUNT ); + ENSURE( VerticesMatch( mesh, vertices, VALLEY_VERTEX_COUNT ) ); + ENSURE( FacesUp( mesh ) ); + ENSURE( HasConcaveEdge( mesh ) ); + + b3DestroyMesh( mesh ); + return 0; +} + +// Clockwise input plus the flag must bake to the same mesh as counter clockwise input +static int MeshClockWise( void ) +{ + b3Vec3 vertices[VALLEY_VERTEX_COUNT]; + int32_t indices[3 * VALLEY_TRIANGLE_COUNT]; + MakeValley( vertices, indices ); + + b3MeshDef ccwDef = MakeValleyDef( vertices, indices ); + b3MeshData* ccwMesh = b3CreateMesh( &ccwDef, NULL, 0 ); + ENSURE( ccwMesh != NULL ); + + int32_t reversed[3 * VALLEY_TRIANGLE_COUNT]; + memcpy( reversed, indices, sizeof( indices ) ); + ReverseWinding( reversed, VALLEY_TRIANGLE_COUNT ); + + b3MeshDef cwDef = MakeValleyDef( vertices, reversed ); + cwDef.clockWiseWinding = true; + b3MeshData* cwMesh = b3CreateMesh( &cwDef, NULL, 0 ); + ENSURE( cwMesh != NULL ); + + ENSURE( FacesUp( cwMesh ) ); + ENSURE( HasConcaveEdge( cwMesh ) ); + ENSURE( MeshesMatch( ccwMesh, cwMesh ) ); + + b3DestroyMesh( cwMesh ); + b3DestroyMesh( ccwMesh ); + return 0; +} + +// Without the flag the same clockwise input must bake inside out +static int MeshClockWiseIgnored( void ) +{ + b3Vec3 vertices[VALLEY_VERTEX_COUNT]; + int32_t indices[3 * VALLEY_TRIANGLE_COUNT]; + MakeValley( vertices, indices ); + ReverseWinding( indices, VALLEY_TRIANGLE_COUNT ); + + b3MeshDef def = MakeValleyDef( vertices, indices ); + b3MeshData* mesh = b3CreateMesh( &def, NULL, 0 ); + ENSURE( mesh != NULL ); + + ENSURE( FacesUp( mesh ) == false ); + + b3DestroyMesh( mesh ); + return 0; +} + +// Winding, stride and welding have to compose +static int MeshClockWiseStrideWeld( void ) +{ + b3Vec3 vertices[VALLEY_VERTEX_COUNT]; + int32_t indices[3 * VALLEY_TRIANGLE_COUNT]; + MakeValley( vertices, indices ); + + b3MeshDef ccwDef = MakeValleyDef( vertices, indices ); + ccwDef.weldVertices = true; + ccwDef.weldTolerance = 0.01f; + b3MeshData* ccwMesh = b3CreateMesh( &ccwDef, NULL, 0 ); + ENSURE( ccwMesh != NULL ); + + int32_t reversed[3 * VALLEY_TRIANGLE_COUNT]; + memcpy( reversed, indices, sizeof( indices ) ); + ReverseWinding( reversed, VALLEY_TRIANGLE_COUNT ); + + FatVertex fat[VALLEY_VERTEX_COUNT]; + MakeFatVertices( fat, vertices, VALLEY_VERTEX_COUNT ); + + b3MeshDef cwDef = MakeValleyDef( &fat[0].position, reversed ); + cwDef.stride = sizeof( FatVertex ); + cwDef.clockWiseWinding = true; + cwDef.weldVertices = true; + cwDef.weldTolerance = 0.01f; + b3MeshData* cwMesh = b3CreateMesh( &cwDef, NULL, 0 ); + ENSURE( cwMesh != NULL ); + + ENSURE( FacesUp( cwMesh ) ); + ENSURE( MeshesMatch( ccwMesh, cwMesh ) ); + + b3DestroyMesh( cwMesh ); + b3DestroyMesh( ccwMesh ); + return 0; +} + +// The built in creators fill their own def, so a missed stride shows up here +static int MeshCreators( void ) +{ + b3Vec3 center = { 1.0f, 2.0f, 3.0f }; + b3Vec3 extent = { 0.5f, 1.0f, 1.5f }; + + b3MeshData* meshes[] = { + b3CreateGridMesh( 4, 4, 1.0f, 2, true ), + b3CreateWaveMesh( 4, 4, 1.0f, 0.5f, 1.0f, 1.0f ), + b3CreateTorusMesh( 8, 6, 2.0f, 0.5f ), + b3CreateBoxMesh( center, extent, true ), + b3CreateHollowBoxMesh( center, extent ), + b3CreatePlatformMesh( center, 2.0f, 1.0f, 2.0f ), + }; + + for ( int i = 0; i < ARRAY_COUNT( meshes ); ++i ) + { + ENSURE( meshes[i] != NULL ); + ENSURE( meshes[i]->vertexCount >= 3 ); + ENSURE( meshes[i]->triangleCount >= 1 ); + ENSURE( b3IsSaneAABB( meshes[i]->bounds ) ); + + b3DestroyMesh( meshes[i] ); + } + + return 0; +} + +int MeshTest( void ) +{ + RUN_SUBTEST( MeshDenseStride ); + RUN_SUBTEST( MeshFatStride ); + RUN_SUBTEST( MeshStrideWeld ); + RUN_SUBTEST( MeshClockWise ); + RUN_SUBTEST( MeshClockWiseIgnored ); + RUN_SUBTEST( MeshClockWiseStrideWeld ); + RUN_SUBTEST( MeshCreators ); + + return 0; +} diff --git a/test/test_mover.c b/test/test_mover.c index 89e16ca..b82ab4e 100644 --- a/test/test_mover.c +++ b/test/test_mover.c @@ -6,6 +6,7 @@ // b3CollideMoverAndSphere / Capsule / Hull are internal #include "shape.h" +#include "box3d/box3d.h" #include "box3d/collision.h" static int ParallelPlanes( void ) @@ -54,15 +55,11 @@ static int GamePlanes( void ) return 0; } -// --------------------------------------------------------------------------- // Mover-collide overlap handling -// // b3CollideMoverAndSphere / Capsule / Hull must never emit a plane with a // degenerate (zero) normal, even when the mover deeply penetrates the shape. // On deep overlap the GJK path returns a {0,0,0} normal; these tests guard the // fix that replaces it with an analytic (sphere/capsule) or dropped (hull) result. -// --------------------------------------------------------------------------- - static int MoverSphereSeparated( void ) { b3Sphere shape = { { 0.0f, 0.0f, 0.0f }, 0.5f }; @@ -233,6 +230,467 @@ static int MoverHullDeepOverlap( void ) return 0; } +// Mover queries report which material a contact plane came from +// b3PlaneResult::materialIndex follows different paths per shape type. Meshes +// report the per triangle material index. Compounds remap the child result +// through the child material table. Convex shapes report index 0. +static b3SurfaceMaterial MakeMaterial( float friction, uint64_t userId ) +{ + b3SurfaceMaterial m = b3DefaultSurfaceMaterial(); + m.friction = friction; + m.userMaterialId = userId; + return m; +} + +// Two separated upward facing triangles on the y = 0 plane, one per material. The bake may +// reorder triangles but always keeps a triangle paired with its material index. +static b3MeshData* MakeTwoMaterialMesh( void ) +{ + b3Vec3 vertices[6] = { + { -3.0f, 0.0f, -1.0f }, { -2.0f, 0.0f, 1.0f }, { -1.0f, 0.0f, -1.0f }, + { 1.0f, 0.0f, -1.0f }, { 2.0f, 0.0f, 1.0f }, { 3.0f, 0.0f, -1.0f }, + }; + int32_t indices[6] = { 0, 1, 2, 3, 4, 5 }; + uint8_t materialIndices[2] = { 0, 1 }; + + b3MeshDef def = { 0 }; + def.vertices = vertices; + def.stride = sizeof( b3Vec3 ); + def.indices = indices; + def.materialIndices = materialIndices; + def.vertexCount = 6; + def.triangleCount = 2; + + return b3CreateMesh( &def, NULL, 0 ); +} + +// Flat 3x3 vertex field at y = 0, one cell material per entry. Caller destroys. +static b3HeightFieldData* MakeFlatField( uint8_t* materials, bool clockwise ) +{ + float heights[9] = { 0 }; + + b3HeightFieldDef def = { 0 }; + def.heights = heights; + def.materialIndices = materials; + def.scale = (b3Vec3){ 1.0f, 1.0f, 1.0f }; + def.countX = 3; + def.countZ = 3; + def.globalMinimumHeight = -1.0f; + def.globalMaximumHeight = 1.0f; + def.clockwiseWinding = clockwise; + + return b3CreateHeightField( &def ); +} + +typedef struct PlaneCapture +{ + b3PlaneResult planes[16]; + int count; +} PlaneCapture; + +static bool CapturePlaneFcn( b3ShapeId shapeId, const b3PlaneResult* planes, int planeCount, void* context ) +{ + (void)shapeId; + PlaneCapture* capture = context; + for ( int i = 0; i < planeCount && capture->count < 16; ++i ) + { + capture->planes[capture->count++] = planes[i]; + } + return true; +} + +static int MoverWorldMeshMaterials( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_staticBody; + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + + b3MeshData* mesh = MakeTwoMaterialMesh(); + ENSURE( mesh != NULL ); + ENSURE( mesh->materialCount == 2 ); + + b3SurfaceMaterial materials[2] = { MakeMaterial( 0.2f, 1 ), MakeMaterial( 0.8f, 2 ) }; + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.materials = materials; + shapeDef.materialCount = 2; + b3CreateMeshShape( bodyId, &shapeDef, mesh, (b3Vec3){ 1.0f, 1.0f, 1.0f } ); + + b3World_Step( worldId, 1.0f / 60.0f, 1 ); + + const uint8_t* bakedMaterialIndices = b3GetMeshMaterialIndices( mesh ); + + // Mover hanging just above the first triangle so its radius reaches 0.05 into the surface + b3Capsule mover = { { -2.0f, 0.15f, 0.0f }, { -2.0f, 0.35f, 0.0f }, 0.2f }; + + PlaneCapture capture = { 0 }; + b3World_CollideMover( worldId, b3Pos_zero, &mover, b3DefaultQueryFilter(), CapturePlaneFcn, &capture ); + + ENSURE( capture.count == 1 ); + ENSURE( capture.planes[0].plane.normal.y > 0.99f ); + ENSURE_SMALL( capture.planes[0].plane.offset - 0.05f, 1e-4f ); + ENSURE_SMALL( capture.planes[0].point.y, 1e-4f ); + ENSURE( capture.planes[0].materialIndex == 0 ); + ENSURE( capture.planes[0].triangleIndex >= 0 ); + ENSURE( capture.planes[0].triangleIndex < mesh->triangleCount ); + ENSURE( bakedMaterialIndices[capture.planes[0].triangleIndex] == 0 ); + + // Same mover over the second triangle + b3Capsule mover2 = { { 2.0f, 0.15f, 0.0f }, { 2.0f, 0.35f, 0.0f }, 0.2f }; + + PlaneCapture capture2 = { 0 }; + b3World_CollideMover( worldId, b3Pos_zero, &mover2, b3DefaultQueryFilter(), CapturePlaneFcn, &capture2 ); + + ENSURE( capture2.count == 1 ); + ENSURE( capture2.planes[0].plane.normal.y > 0.99f ); + ENSURE( capture2.planes[0].materialIndex == 1 ); + ENSURE( bakedMaterialIndices[capture2.planes[0].triangleIndex] == 1 ); + + b3DestroyWorld( worldId ); + b3DestroyMesh( mesh ); + return 0; +} + +static int MoverWorldCompoundMeshMaterials( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_staticBody; + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + + b3MeshData* mesh = MakeTwoMaterialMesh(); + + // The hull material and the two mesh materials are distinct, so the compound material + // table gets one slot per material. Children bake in hull then mesh order. + b3SurfaceMaterial meshMaterials[2] = { MakeMaterial( 0.3f, 101 ), MakeMaterial( 0.6f, 202 ) }; + b3SurfaceMaterial hullMaterial = MakeMaterial( 0.9f, 303 ); + + b3CompoundMeshDef meshChild = { + .meshData = mesh, + .transform = b3Transform_identity, + .scale = { 1.0f, 1.0f, 1.0f }, + .materials = meshMaterials, + .materialCount = 2, + }; + + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3CompoundHullDef hullChild = { + .hull = &box.base, + .transform = { .p = { -8.0f, 0.0f, 0.0f }, .q = b3Quat_identity }, + .material = hullMaterial, + }; + + b3CompoundDef compoundDef = { + .hulls = &hullChild, + .hullCount = 1, + .meshes = &meshChild, + .meshCount = 1, + }; + b3CompoundData* compound = b3CreateCompound( &compoundDef ); + ENSURE( compound != NULL ); + ENSURE( compound->materialCount == 3 ); + + const b3SurfaceMaterial* bakedMaterials = b3GetCompoundMaterials( compound ); + ENSURE( bakedMaterials[0].userMaterialId == 303 ); + ENSURE( bakedMaterials[1].userMaterialId == 101 ); + ENSURE( bakedMaterials[2].userMaterialId == 202 ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3CreateBakedCompoundShape( bodyId, &shapeDef, compound ); + + b3World_Step( worldId, 1.0f / 60.0f, 1 ); + + // Mover on top of the hull child face at y = 0.5 + b3Capsule hullMover = { { -8.0f, 0.65f, 0.0f }, { -8.0f, 0.85f, 0.0f }, 0.2f }; + + PlaneCapture hullCapture = { 0 }; + b3World_CollideMover( worldId, b3Pos_zero, &hullMover, b3DefaultQueryFilter(), CapturePlaneFcn, &hullCapture ); + + ENSURE( hullCapture.count == 1 ); + ENSURE( hullCapture.planes[0].plane.normal.y > 0.99f ); + ENSURE( hullCapture.planes[0].childIndex == 0 ); + ENSURE( hullCapture.planes[0].materialIndex == 0 ); + ENSURE( bakedMaterials[hullCapture.planes[0].materialIndex].userMaterialId == 303 ); + + // Mover over the second mesh triangle. The mesh reports triangle material 1, which the + // compound remaps through the child material table to the shared slot of meshMaterials[1]. + b3Capsule meshMover = { { 2.0f, 0.15f, 0.0f }, { 2.0f, 0.35f, 0.0f }, 0.2f }; + + PlaneCapture meshCapture = { 0 }; + b3World_CollideMover( worldId, b3Pos_zero, &meshMover, b3DefaultQueryFilter(), CapturePlaneFcn, &meshCapture ); + + ENSURE( meshCapture.count == 1 ); + ENSURE( meshCapture.planes[0].plane.normal.y > 0.99f ); + ENSURE( meshCapture.planes[0].childIndex == 1 ); + ENSURE( meshCapture.planes[0].materialIndex == 2 ); + ENSURE( bakedMaterials[meshCapture.planes[0].materialIndex].userMaterialId == 202 ); + + b3DestroyWorld( worldId ); + b3DestroyCompound( compound ); + b3DestroyMesh( mesh ); + return 0; +} + +static int MoverBodyMaterialIndices( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + + // One shape of each convex type, spaced out along X + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + b3Sphere sphere = { { 0.0f, 0.5f, 0.0f }, 0.5f }; + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + + b3BoxHull box = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, + (b3Transform){ .p = { 5.0f, 0.0f, 0.0f }, .q = b3Quat_identity } ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + + b3Capsule capsule = { { 9.0f, 0.0f, 0.0f }, { 11.0f, 0.0f, 0.0f }, 0.3f }; + b3CreateCapsuleShape( bodyId, &shapeDef, &capsule ); + + b3WorldTransform bodyTransform = { .p = b3Pos_zero, .q = b3Quat_identity }; + + // Convex shapes report the base material as index 0 + b3Capsule movers[3] = { + { { 0.0f, 1.15f, 0.0f }, { 0.0f, 1.35f, 0.0f }, 0.2f }, + { { 5.0f, 0.65f, 0.0f }, { 5.0f, 0.85f, 0.0f }, 0.2f }, + { { 10.0f, 0.45f, 0.0f }, { 10.0f, 0.65f, 0.0f }, 0.2f }, + }; + + for ( int i = 0; i < 3; ++i ) + { + b3BodyPlaneResult planes[4]; + int count = b3Body_CollideMover( bodyId, planes, 4, b3Pos_zero, &movers[i], b3DefaultQueryFilter(), bodyTransform ); + + ENSURE( count == 1 ); + ENSURE( b3Shape_IsValid( planes[0].shapeId ) ); + ENSURE( planes[0].result.plane.normal.y > 0.99f ); + ENSURE( planes[0].result.materialIndex == 0 ); + ENSURE( planes[0].result.childIndex == 0 ); + } + + b3DestroyWorld( worldId ); + return 0; +} + +static int MoverBodySkipsMeshAndCompound( void ) +{ + // The body level mover query handles convex shapes only. Mesh and compound shapes are + // skipped, so their material indices are only available through b3World_CollideMover. + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + + b3MeshData* mesh = MakeTwoMaterialMesh(); + ENSURE( mesh != NULL ); + b3CreateMeshShape( bodyId, &shapeDef, mesh, (b3Vec3){ 1.0f, 1.0f, 1.0f } ); + + b3BoxHull box = b3MakeBoxHull( 0.5f, 0.5f, 0.5f ); + b3CompoundHullDef hullChild = { + .hull = &box.base, + .transform = { .p = { 10.0f, 0.0f, 0.0f }, .q = b3Quat_identity }, + }; + b3CompoundDef compoundDef = { + .hulls = &hullChild, + .hullCount = 1, + }; + b3CompoundData* compound = b3CreateCompound( &compoundDef ); + ENSURE( compound != NULL ); + b3CreateBakedCompoundShape( bodyId, &shapeDef, compound ); + + b3WorldTransform bodyTransform = { .p = b3Pos_zero, .q = b3Quat_identity }; + + // Mover over the mesh triangle and mover over the compound hull child both find nothing + b3Capsule meshMover = { { -2.0f, 0.15f, 0.0f }, { -2.0f, 0.35f, 0.0f }, 0.2f }; + b3Capsule compoundMover = { { 10.0f, 0.65f, 0.0f }, { 10.0f, 0.85f, 0.0f }, 0.2f }; + + b3Capsule movers[2] = { meshMover, compoundMover }; + for ( int i = 0; i < 2; ++i ) + { + b3BodyPlaneResult planes[4]; + int count = b3Body_CollideMover( bodyId, planes, 4, b3Pos_zero, &movers[i], b3DefaultQueryFilter(), bodyTransform ); + ENSURE( count == 0 ); + } + + b3DestroyWorld( worldId ); + b3DestroyCompound( compound ); + b3DestroyMesh( mesh ); + return 0; +} + +// One sided mover collision. +// Mover queries keep only triangles facing the mover. The front side follows the +// baked winding: up for a default mesh or height field, down when the height +// field carries clockwiseWinding. A mirror in the mesh scale must not flip it. +static int MoverMeshBackside( void ) +{ + b3MeshData* mesh = MakeTwoMaterialMesh(); + b3Mesh shape = { .data = mesh, .scale = { 1.0f, 1.0f, 1.0f } }; + + b3PlaneResult planes[4]; + + // On the front of the left triangle, so a plane comes back + b3Capsule above = { { -2.0f, 0.15f, 0.0f }, { -2.0f, 0.35f, 0.0f }, 0.2f }; + int count = b3CollideMoverAndMesh( planes, 4, &shape, &above ); + ENSURE( count == 1 ); + ENSURE( planes[0].plane.normal.y > 0.99f ); + ENSURE( planes[0].childIndex == 0 ); + ENSURE( planes[0].triangleIndex >= 0 && planes[0].triangleIndex < mesh->triangleCount ); + + // The same spot seen from behind the face is culled + b3Capsule below = { { -2.0f, -0.35f, 0.0f }, { -2.0f, -0.15f, 0.0f }, 0.2f }; + count = b3CollideMoverAndMesh( planes, 4, &shape, &below ); + ENSURE( count == 0 ); + + b3DestroyMesh( mesh ); + return 0; +} + +static int MoverMeshMirroredScale( void ) +{ + b3MeshData* mesh = MakeTwoMaterialMesh(); + + // Reflecting the scale flips the triangle winding, the collision swaps it back, + // so the front stays up. Local x = -2 maps onto the source right triangle, material 1. + b3Mesh shape = { .data = mesh, .scale = { -1.0f, 1.0f, 1.0f } }; + + b3PlaneResult planes[4]; + + b3Capsule above = { { -2.0f, 0.15f, 0.0f }, { -2.0f, 0.35f, 0.0f }, 0.2f }; + int count = b3CollideMoverAndMesh( planes, 4, &shape, &above ); + ENSURE( count == 1 ); + ENSURE( planes[0].plane.normal.y > 0.99f ); + ENSURE( planes[0].materialIndex == 1 ); + ENSURE( b3GetMeshMaterialIndices( mesh )[planes[0].triangleIndex] == 1 ); + + b3Capsule below = { { -2.0f, -0.35f, 0.0f }, { -2.0f, -0.15f, 0.0f }, 0.2f }; + count = b3CollideMoverAndMesh( planes, 4, &shape, &below ); + ENSURE( count == 0 ); + + b3DestroyMesh( mesh ); + return 0; +} + +static int MoverHeightFieldBackside( void ) +{ + uint8_t materials[4] = { 0, 0, 0, 0 }; + b3HeightFieldData* hf = MakeFlatField( materials, false ); + + b3PlaneResult planes[4]; + + // Standing on the front (upper) face of the default winding + b3Capsule above = { { 0.3f, 0.15f, 0.25f }, { 0.3f, 0.35f, 0.25f }, 0.2f }; + int count = b3CollideMoverAndHeightField( planes, 4, hf, &above ); + ENSURE( count == 1 ); + ENSURE( planes[0].plane.normal.y > 0.99f ); + ENSURE_SMALL( planes[0].plane.offset - 0.05f, 1e-4f ); + + // Under the surface is the back side and gets culled + b3Capsule below = { { 0.3f, -0.35f, 0.25f }, { 0.3f, -0.15f, 0.25f }, 0.2f }; + count = b3CollideMoverAndHeightField( planes, 4, hf, &below ); + ENSURE( count == 0 ); + + b3DestroyHeightField( hf ); + return 0; +} + +static int MoverHeightFieldReport( void ) +{ + uint8_t materials[4] = { 1, 2, 0, 0 }; + b3HeightFieldData* hf = MakeFlatField( materials, false ); + + b3PlaneResult planes[4]; + + // (0.3, 0.25) sits on the x + z <= 1 side of cell (0,0), which holds triangle 0 + b3Capsule first = { { 0.3f, 0.15f, 0.25f }, { 0.3f, 0.35f, 0.25f }, 0.2f }; + int count = b3CollideMoverAndHeightField( planes, 4, hf, &first ); + ENSURE( count == 1 ); + ENSURE( planes[0].triangleIndex == 0 ); + ENSURE( planes[0].childIndex == 0 ); + ENSURE( planes[0].materialIndex == 1 ); + + // (1.3, 0.3) sits on the x + z <= 2 side of cell (0,1), which holds triangle 2 + b3Capsule second = { { 1.3f, 0.15f, 0.3f }, { 1.3f, 0.35f, 0.3f }, 0.2f }; + count = b3CollideMoverAndHeightField( planes, 4, hf, &second ); + ENSURE( count == 1 ); + ENSURE( planes[0].triangleIndex == 2 ); + ENSURE( planes[0].materialIndex == 2 ); + + b3DestroyHeightField( hf ); + return 0; +} + +// A clockwise height field faces down, see HeightFieldWinding. Backside culling +// must respect the flag: below the surface is the front side, above is the back. +static int MoverHeightFieldClockwise( void ) +{ + uint8_t materials[4] = { 0, 0, 0, 0 }; + b3HeightFieldData* hf = MakeFlatField( materials, true ); + + b3PlaneResult planes[4]; + + b3Capsule below = { { 0.3f, -0.35f, 0.25f }, { 0.3f, -0.15f, 0.25f }, 0.2f }; + int count = b3CollideMoverAndHeightField( planes, 4, hf, &below ); + ENSURE( count == 1 ); + ENSURE( planes[0].plane.normal.y < -0.99f ); + ENSURE_SMALL( planes[0].plane.offset - 0.05f, 1e-4f ); + ENSURE( planes[0].triangleIndex == 0 || planes[0].triangleIndex == 1 ); + + b3Capsule above = { { 0.3f, 0.15f, 0.25f }, { 0.3f, 0.35f, 0.25f }, 0.2f }; + count = b3CollideMoverAndHeightField( planes, 4, hf, &above ); + ENSURE( count == 0 ); + + b3DestroyHeightField( hf ); + return 0; +} + +// A mesh can be baked with more materials than the shape carries. The reported index must stay +// inside the shape's material array. +static int MoverWorldMeshMaterialClamp( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_staticBody; + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + + b3MeshData* mesh = MakeTwoMaterialMesh(); + ENSURE( mesh->materialCount == 2 ); + + // Base material only + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3ShapeId shapeId = b3CreateMeshShape( bodyId, &shapeDef, mesh, (b3Vec3){ 1.0f, 1.0f, 1.0f } ); + int shapeMaterialCount = b3Shape_GetMeshMaterialCount( shapeId ); + ENSURE( shapeMaterialCount == 1 ); + + b3World_Step( worldId, 1.0f / 60.0f, 1 ); + + // Over the triangle baked with material 1 + b3Capsule mover = { { 2.0f, 0.15f, 0.0f }, { 2.0f, 0.35f, 0.0f }, 0.2f }; + PlaneCapture capture = { 0 }; + b3World_CollideMover( worldId, b3Pos_zero, &mover, b3DefaultQueryFilter(), CapturePlaneFcn, &capture ); + + ENSURE( capture.count == 1 ); + ENSURE( capture.planes[0].materialIndex == shapeMaterialCount - 1 ); + + b3DestroyWorld( worldId ); + b3DestroyMesh( mesh ); + return 0; +} + int MoverTest( void ) { RUN_SUBTEST( GamePlanes ); @@ -251,5 +709,16 @@ int MoverTest( void ) RUN_SUBTEST( MoverHullTouching ); RUN_SUBTEST( MoverHullDeepOverlap ); + RUN_SUBTEST( MoverWorldMeshMaterials ); + RUN_SUBTEST( MoverWorldCompoundMeshMaterials ); + RUN_SUBTEST( MoverBodyMaterialIndices ); + RUN_SUBTEST( MoverBodySkipsMeshAndCompound ); + RUN_SUBTEST( MoverMeshBackside ); + RUN_SUBTEST( MoverMeshMirroredScale ); + RUN_SUBTEST( MoverHeightFieldBackside ); + RUN_SUBTEST( MoverHeightFieldReport ); + RUN_SUBTEST( MoverHeightFieldClockwise ); + RUN_SUBTEST( MoverWorldMeshMaterialClamp ); + return 0; }