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.
This commit is contained in:
Erin Catto 2026-08-29 21:47:53 -07:00 • committed by GitHub
commit 47d7f7cc7e
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
35 changed files with 2466 additions and 303 deletions

View file

@ -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.

1
.gitignore vendored
View file

@ -14,3 +14,4 @@ CMakeUserPresets.json
*.b3rec
Superluminal/
build-dp/
AGENTS.md

View file

@ -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

View file

@ -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

View file

@ -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;

View file

@ -8,6 +8,7 @@
#include "id.h"
#include "math_functions.h"
#include <stddef.h>
#include <stdint.h>
#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.

View file

@ -221,6 +221,12 @@ void CharacterMover::SolveMove( float timeStep, b3Vec3 forward, b3Vec3 right, b3
continue;
}
MoverShapeUserData* userData = static_cast<MoverShapeUserData*>( 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 );

View file

@ -11,6 +11,7 @@ struct MoverShapeUserData
{
float maxPush;
bool clipVelocity;
bool canMoverPush;
};
struct PlaneExtra

View file

@ -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
{

View file

@ -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 );

View file

@ -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 );

View file

@ -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 ) );
}

View file

@ -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;
}

View file

@ -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;

View file

@ -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 )

View file

@ -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;
}

View file

@ -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;
}
}
}
}

View file

@ -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;
}

View file

@ -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;

View file

@ -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 );

View file

@ -11,8 +11,6 @@
#include <stdio.h>
// 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

View file

@ -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 );
}

View file

@ -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 )

View file

@ -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

View file

@ -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;
}

View file

@ -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;

View file

@ -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;
}

View file

@ -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 );

View file

@ -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

View file

@ -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 );

View file

@ -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;
}

View file

@ -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;
}

View file

@ -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;
}

355
test/test_mesh.c Normal file
View file

@ -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 <float.h>
#include <string.h>
// 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;
}

View file

@ -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;
}