Optimizations (#161)

- cache body sim indices and split out fat aabb
- deferred restitution
- the manifold point normal velocity is only computed for restitution or
hit events, otherwise it is zero
This commit is contained in:
Erin Catto 2026-09-20 15:54:52 -07:00 • committed by GitHub
commit 9e5a4cde86
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
49 changed files with 3395 additions and 864 deletions

View file

@ -69,18 +69,41 @@ b3BodyState* b3GetBodyState( b3World* world, b3Body* body )
return NULL;
}
void b3RefreshBodyContactIndices( b3World* world, b3Body* body )
{
int encodedBodySimIndex = b3EncodeBodySimIndex( body );
int contactKey = body->headContactKey;
while ( contactKey != B3_NULL_INDEX )
{
int edgeIndex = contactKey & 1;
int contactId = contactKey >> 1;
b3Contact* contact = b3Array_Get( world->contacts, contactId );
contactKey = contact->edges[edgeIndex].nextKey;
if ( edgeIndex == 0 )
{
contact->encodedBodySimA = encodedBodySimIndex;
}
else
{
contact->encodedBodySimB = encodedBodySimIndex;
}
}
}
void b3SyncBodyFlags( b3World* world, b3Body* body )
{
// Never sync transient flags
uint32_t flags = body->flags & ~b3_bodyTransientFlags;
b3BodySim* bodySim = b3GetBodySim( world, body );
bodySim->flags = flags;
// Preserve the fast flag for contact recycling.
bodySim->flags = ( bodySim->flags & b3_isFast ) | ( body->flags & ~b3_bodyTransientFlags );
b3BodyState* bodyState = b3GetBodyState( world, body );
if ( bodyState != NULL )
{
bodyState->flags = flags;
bodyState->flags = body->flags & ~b3_bodyTransientFlags;
}
}
@ -410,6 +433,7 @@ void b3DestroyBody( b3BodyId bodyId )
b3Body* movedBody = b3Array_Get( world->bodies, movedId );
B3_ASSERT( movedBody->localIndex == movedIndex );
movedBody->localIndex = body->localIndex;
b3RefreshBodyContactIndices( world, movedBody );
}
// Remove body state from awake set
@ -1120,17 +1144,11 @@ void b3Body_SetTransform( b3BodyId bodyId, b3Pos position, b3Quat rotation )
b3AABB aabb = b3ComputeFatShapeAABB( shape, transform, speculativeDistance );
shape->aabb = aabb;
if ( b3AABB_Contains( shape->fatAABB, aabb ) == false )
b3AABB* shapeFatAABB = world->fatAABBs.data + shapeId;
if ( b3AABB_Contains( *shapeFatAABB, aabb ) == false )
{
float margin = shape->aabbMargin;
b3AABB fatAABB;
fatAABB.lowerBound.x = aabb.lowerBound.x - margin;
fatAABB.lowerBound.y = aabb.lowerBound.y - margin;
fatAABB.lowerBound.z = aabb.lowerBound.z - margin;
fatAABB.upperBound.x = aabb.upperBound.x + margin;
fatAABB.upperBound.y = aabb.upperBound.y + margin;
fatAABB.upperBound.z = aabb.upperBound.z + margin;
shape->fatAABB = fatAABB;
b3AABB fatAABB = b3AABB_Inflate( aabb, shape->aabbMargin );
*shapeFatAABB = fatAABB;
// The body could be disabled
if ( shape->proxyKey != B3_NULL_INDEX )
@ -1717,7 +1735,7 @@ void b3Body_SetType( b3BodyId bodyId, b3BodyType type )
shapeId = shape->nextShapeId;
b3DestroyShapeProxy( shape, &world->broadPhase );
bool forcePairCreation = true;
b3CreateShapeProxy( shape, &world->broadPhase, type, transform, forcePairCreation );
b3CreateShapeProxy( world, shape, type, transform, forcePairCreation );
}
// Relink all joints
@ -2263,7 +2281,7 @@ void b3Body_Enable( b3BodyId bodyId )
b3Shape* shape = b3Array_Get( world->shapes, shapeId );
shapeId = shape->nextShapeId;
b3CreateShapeProxy( shape, &world->broadPhase, proxyType, transform, forcePairCreation );
b3CreateShapeProxy( world, shape, proxyType, transform, forcePairCreation );
}
if ( setId != b3_staticSet )

View file

@ -3,6 +3,8 @@
#pragma once
#include "physics_world.h"
#include "box3d/constants.h"
#include "box3d/math_functions.h"
#include "box3d/types.h"
@ -29,7 +31,7 @@ enum b3BodyFlags
// This body has fixed rotation around the z-axis
b3_lockAngularZ = 0x00000020,
// This flag is used for debug draw
// This flag is used for debug draw and contact recycling. Only lives on b3BodySim.
b3_isFast = 0x00000040,
// This dynamic body does a final CCD pass against all body types, but not other bullets
@ -44,8 +46,9 @@ enum b3BodyFlags
// This body has no limit on angular velocity
b3_allowFastRotation = 0x00000400,
// This body need's to have its AABB increased
b3_enlargeBounds = 0x00000800,
// This bullet body needs to have its AABB increased. Needed because bullets don't follow
// the standard broad-phase update.
b3_enlargeBulletBounds = 0x00000800,
// This body is dynamic so the solver should write to it.
// This prevents writing to kinematic bodies that causes a multithreaded sharing
@ -68,7 +71,7 @@ enum b3BodyFlags
b3_fixedRotation = b3_lockAngularX | b3_lockAngularY | b3_lockAngularZ,
// These flags are transient per time step. These may be different across b3Body, b3BodySim, and b3BodyState.
b3_bodyTransientFlags = b3_isFast | b3_isSpeedCapped | b3_hadTimeOfImpact,
b3_bodyTransientFlags = b3_isSpeedCapped | b3_hadTimeOfImpact,
};
// Body organizational details that are not used in the solver.
@ -241,6 +244,38 @@ bool b3WakeBodyWithLock( b3World* world, b3Body* body );
void b3UpdateBodyMassData( b3World* world, b3Body* body );
void b3SyncBodyFlags( b3World* world, b3Body* body );
void b3RefreshBodyContactIndices( b3World* world, b3Body* body );
// Encode the body sim index for storage in the contact.
static inline int b3EncodeBodySimIndex( const b3Body* body )
{
if ( body->setIndex == b3_awakeSet )
{
return body->localIndex;
}
if ( body->setIndex == b3_staticSet )
{
return -( body->localIndex + 2 );
}
return B3_NULL_INDEX;
}
static inline bool b3IsStaticSimIndex( int encodedBodySimIndex )
{
return encodedBodySimIndex < B3_NULL_INDEX;
}
static inline int b3DecodeAwakeIndex( int encodedBodySimIndex )
{
return encodedBodySimIndex >= 0 ? encodedBodySimIndex : B3_NULL_INDEX;
}
static inline int b3SleepBodySimIndex( int encodedBodySimIndex )
{
return encodedBodySimIndex >= 0 ? B3_NULL_INDEX : encodedBodySimIndex;
}
// Make a sweep relative to a base position to keep TOI in float precision far from the origin.
static inline b3Sweep b3MakeRelativeSweep( const b3BodySim* bodySim, b3Pos base )

View file

@ -149,8 +149,8 @@ void b3AddContactToGraph( b3World* world, b3Contact* contact )
b3GraphColor* color = graph->colors + colorIndex;
contact->colorIndex = colorIndex;
contact->localIndex = isScalar ? color->contacts.count : color->convexContacts.count;
contact->bodySimIndexA = bodyA->type == b3_staticBody ? B3_NULL_INDEX : bodyA->localIndex;
contact->bodySimIndexB = bodyB->type == b3_staticBody ? B3_NULL_INDEX : bodyB->localIndex;
contact->encodedBodySimA = b3EncodeBodySimIndex( bodyA );
contact->encodedBodySimB = b3EncodeBodySimIndex( bodyB );
if ( isScalar )
{

View file

@ -210,6 +210,8 @@ void b3CreateContact( b3World* world, b3Shape* shapeA, b3Shape* shapeB, int chil
contact->shapeIdA = shapeIdA;
contact->shapeIdB = shapeIdB;
contact->childIndex = childIndex;
contact->encodedBodySimA = b3EncodeBodySimIndex( bodyA );
contact->encodedBodySimB = b3EncodeBodySimIndex( bodyB );
// Both bodies must enable recycling
if ( ( bodyA->flags & b3_bodyEnableContactRecycling ) != 0 && ( bodyB->flags & b3_bodyEnableContactRecycling ) != 0 )

View file

@ -126,9 +126,9 @@ typedef struct b3Contact
// Back index into b3World::contacts
int contactId;
// These are transient and cached for improved performance. B3_NULL_INDEX for static bodies.
int bodySimIndexA;
int bodySimIndexB;
// Encoded body sim indices to avoid a cache miss accessing the body in the narrow phase.
int encodedBodySimA;
int encodedBodySimB;
// b3ContactFlags
uint32_t flags;

File diff suppressed because it is too large Load diff

View file

@ -10,7 +10,7 @@ typedef struct b3ManifoldConstraintPoint
{
b3Vec3 rA, rB;
float baseSeparation;
float relativeVelocity;
float restitutionVelocity;
float normalImpulse;
float totalNormalImpulse;
float normalMass;
@ -46,7 +46,6 @@ typedef struct b3ContactConstraint
b3Softness softness;
b3Matrix3 rollingMass;
float friction;
float restitution;
float rollingResistance;
int manifoldCount;
} b3ContactConstraint;
@ -56,18 +55,18 @@ int b3GetWideContactConstraintByteCount( void );
// Overflow contacts don't fit into the constraint graph coloring
void b3PrepareContacts_Overflow( b3StepContext* context );
void b3WarmStartContacts_Overflow( b3StepContext* context );
void b3SolveContacts_Overflow( b3StepContext* context, bool useBias );
void b3ApplyRestitution_Overflow( b3StepContext* context );
void b3PushContacts_Overflow( b3StepContext* context );
void b3SolveContacts_Overflow( b3StepContext* context );
void b3StoreImpulses_Overflow( b3StepContext* context );
void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context );
void b3WarmStartContacts_Mesh( b3SolverBlock block, b3StepContext* context );
void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context, bool useBias );
void b3ApplyRestitution_Mesh( b3SolverBlock block, b3StepContext* context );
void b3PushContacts_Mesh( b3SolverBlock block, b3StepContext* context );
void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context );
void b3StoreImpulses_Mesh( b3SolverBlock block, b3StepContext* context, int workerIndex );
void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context );
void b3WarmStartContacts_Convex( b3SolverBlock block, b3StepContext* context );
void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context, bool useBias );
void b3ApplyRestitution_Convex( b3SolverBlock block, b3StepContext* context );
void b3PushContacts_Convex( b3SolverBlock block, b3StepContext* context );
void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context );
void b3StoreImpulses_Convex( b3SolverBlock block, b3StepContext* context, int workerIndex );

View file

@ -138,7 +138,7 @@ int b3GetMaxManifoldPoints( void )
static b3AllocFcn* b3_allocFcn = NULL;
static b3FreeFcn* b3_freeFcn = NULL;
b3AtomicInt b3_byteCount;
static b3AtomicI64 b3_byteCount;
void b3SetAllocator( b3AllocFcn* allocFcn, b3FreeFcn* freeFcn )
{
@ -153,13 +153,11 @@ void* b3Alloc( size_t size )
return NULL;
}
// This could cause some sharing issues, however Box3D rarely calls b3Alloc.
// todo this is not true, Box3D allocates a lot.
b3AtomicFetchAddInt( &b3_byteCount, (int)size );
b3AtomicFetchAddI64( &b3_byteCount, (int)size );
// Allocation must be a multiple of B3_ALIGNMENT (required by spec).
// https://en.cppreference.com/w/c/memory/aligned_alloc
int alignedSize = ( ( (int)size - 1 ) | ( B3_ALIGNMENT - 1 ) ) + 1;
size_t alignedSize = ( ( size - 1 ) | ( B3_ALIGNMENT - 1 ) ) + 1;
if ( b3_allocFcn != NULL )
{
@ -204,7 +202,8 @@ void b3Free( void* mem, size_t size )
if ( b3_freeFcn != NULL )
{
b3_freeFcn( mem );
size_t alignedSize = ( ( size - 1 ) | ( B3_ALIGNMENT - 1 ) ) + 1;
b3_freeFcn( mem, alignedSize );
}
else
{
@ -215,7 +214,7 @@ void b3Free( void* mem, size_t size )
#endif
}
b3AtomicFetchAddInt( &b3_byteCount, -(int)size );
b3AtomicFetchAddI64( &b3_byteCount, -(int64_t)size );
}
void* b3GrowAlloc( void* oldMem, int oldSize, int newSize )
@ -237,9 +236,9 @@ void* b3GrowAllocZeroed( void* oldMem, int oldSize, int newSize )
return newMem;
}
int b3GetByteCount( void )
int64_t b3GetByteCount( void )
{
return b3AtomicLoadInt( &b3_byteCount );
return b3AtomicLoadI64( &b3_byteCount );
}
void* b3AllocZero( size_t size )

View file

@ -113,9 +113,17 @@ typedef struct b3AtomicU32
uint32_t value;
} b3AtomicU32;
typedef struct b3AtomicI64
{
// 64-bit atomic wants 8-byte alignment
_Alignas( 8 ) int64_t value;
} b3AtomicI64;
// Minimum memory alignment used for all allocations
#define B3_ALIGNMENT 16
#define B3_RESTRICT restrict
// Returns the number of elements of an array
#define B3_ARRAY_COUNT( A ) (int)( sizeof( A ) / sizeof( A[0] ) )

View file

@ -20,6 +20,7 @@
#include "scheduler.h"
#include "sensor.h"
#include "shape.h"
#include "simd.h"
#include "solver.h"
#include "solver_set.h"
@ -290,6 +291,7 @@ b3WorldId b3CreateWorld( const b3WorldDef* def )
int shapeCapacity = b3MaxInt( 16, def->capacity.staticShapeCount + def->capacity.dynamicShapeCount );
b3Array_Reserve( world->shapes, shapeCapacity );
b3Array_Reserve( world->fatAABBs, shapeCapacity );
world->hullDatabase = b3Alloc( sizeof( b3HullMap ) );
b3HullMap_init( world->hullDatabase );
@ -482,6 +484,7 @@ void b3DestroyWorld( b3WorldId worldId )
b3DestroyNameCache( &world->names );
b3Array_Destroy( world->shapes );
b3Array_Destroy( world->fatAABBs );
b3Array_Destroy( world->contacts );
b3Array_Destroy( world->joints );
@ -555,6 +558,24 @@ static inline void b3PrefetchContact( const b3Contact* contact )
b3Prefetch( p + 192 );
}
static inline b3BodySim* b3ResolveContactBodySim( b3World* world, b3BodySim* awakeSims, b3BodySim* staticSims, int encodedIndex,
int bodyId )
{
if ( encodedIndex >= 0 )
{
return awakeSims + encodedIndex;
}
if ( b3IsStaticSimIndex( encodedIndex ) )
{
return staticSims - encodedIndex - 2;
}
b3Body* body = b3Array_Get( world->bodies, bodyId );
b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex );
return b3Array_Get( set->bodySims, body->localIndex );
}
static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* context )
{
b3TracyCZoneNC( collide_task, "Collide Task", b3_colorDodgerBlue, true );
@ -566,7 +587,7 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void*
int* contactIndices = stepContext->awakeContactIndices;
b3Contact* contacts = world->contacts.data;
b3Shape* shapes = world->shapes.data;
b3Body* bodies = world->bodies.data;
const b3AABB* fatAABBs = world->fatAABBs.data;
b3BodySim* awakeSims = world->solverSets.data[b3_awakeSet].bodySims.data;
b3BodySim* staticSims = world->solverSets.data[b3_staticSet].bodySims.data;
@ -595,11 +616,11 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void*
b3Contact* contact = contacts + contactIndex;
B3_VALIDATE( contact->contactId == contactIndex );
b3Shape* shapeA = shapes + contact->shapeIdA;
b3Shape* shapeB = shapes + contact->shapeIdB;
int shapeIdA = contact->shapeIdA;
int shapeIdB = contact->shapeIdB;
// Do proxies still overlap?
bool overlap = b3AABB_Overlaps( shapeA->fatAABB, shapeB->fatAABB );
bool overlap = b3OverlapV( fatAABBs + shapeIdA, fatAABBs + shapeIdB );
if ( overlap == false )
{
// This contact will be destroyed
@ -609,51 +630,29 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void*
continue;
}
b3Shape* shapeA = shapes + shapeIdA;
b3Shape* shapeB = shapes + shapeIdB;
// Update contact respecting shape/body order (A,B)
b3Body* bodyA = bodies + shapeA->bodyId;
b3Body* bodyB = bodies + shapeB->bodyId;
bool isStaticA = bodyA->type == b3_staticBody;
bool isStaticB = bodyB->type == b3_staticBody;
int encodedA = contact->encodedBodySimA;
int encodedB = contact->encodedBodySimB;
b3BodySim* bodySimA = b3ResolveContactBodySim( world, awakeSims, staticSims, encodedA, contact->edges[0].bodyId );
b3BodySim* bodySimB = b3ResolveContactBodySim( world, awakeSims, staticSims, encodedB, contact->edges[1].bodyId );
bool wasTouching = ( contact->flags & b3_simTouchingFlag );
b3BodySim* bodySimA;
b3BodySim* bodySimB;
if ( wasTouching )
{
B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyA->setIndex == b3_staticSet );
B3_ASSERT( bodyB->setIndex == b3_awakeSet || bodyB->setIndex == b3_staticSet );
bodySimA = ( isStaticA ? staticSims : awakeSims ) + bodyA->localIndex;
bodySimB = ( isStaticB ? staticSims : awakeSims ) + bodyB->localIndex;
}
else
{
// There can be non-touching contacts between awake bodies and sleeping bodies.
{
b3SolverSet* set = b3Array_Get( world->solverSets, bodyA->setIndex );
bodySimA = b3Array_Get( set->bodySims, bodyA->localIndex );
}
{
b3SolverSet* set = b3Array_Get( world->solverSets, bodyB->setIndex );
bodySimB = b3Array_Get( set->bodySims, bodyB->localIndex );
}
}
b3WorldTransform transformA = bodySimA->transform;
b3WorldTransform transformB = bodySimB->transform;
bool isFast = ( bodySimA->flags & b3_isFast ) || ( bodySimB->flags & b3_isFast );
bool isFast = ( ( bodySimA->flags | bodySimB->flags ) & b3_isFast ) != 0;
// These are used by the contact solver. If the contact is between an awake body
// and a sleeping body and the contact begins to touch, the these will be invalid
// but fixed when linked in the constraint graph.
contact->bodySimIndexA = isStaticA ? B3_NULL_INDEX : bodyA->localIndex;
contact->bodySimIndexB = isStaticB ? B3_NULL_INDEX : bodyB->localIndex;
float recycleTolerance = wasTouching ? recycleDistance : recycleDistanceNonTouching;
// Contact recycling optimization. Please cite this library if you use this optimization.
// This is inspired by persistent contact manifolds used in some physics engines, such as PhysX.
// However, this allows larger relative motion and has fewer tuning parameters (just one).
if ( isFast == false && recycleDistance > 0.0f &&
( contact->flags & b3_relativeTransformValid ) && ( contact->flags & b3_contactRecycleFlag ) )
if ( isFast == false && recycleDistance > 0.0f && ( contact->flags & b3_relativeTransformValid ) &&
( contact->flags & b3_contactRecycleFlag ) )
{
// The scalar part of b3InvMulQuat is just the quaternion dot product.
// cos(relative_angle/2) = scalar(conj(q1) * q2) = dot(q1, q2)
@ -665,8 +664,8 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void*
b3Transform xf = b3InvMulWorldTransforms( transformA, transformB );
b3Transform xfc = contact->cachedRelativePose;
b3Vec3 maxExtentA = isStaticA ? b3Vec3_zero : bodySimA->maxExtent;
b3Vec3 maxExtentB = isStaticB ? b3Vec3_zero : bodySimB->maxExtent;
b3Vec3 maxExtentA = b3IsStaticSimIndex( encodedA ) ? b3Vec3_zero : bodySimA->maxExtent;
b3Vec3 maxExtentB = b3IsStaticSimIndex( encodedB ) ? b3Vec3_zero : bodySimB->maxExtent;
b3Vec3 maxExtent = b3Max( maxExtentA, maxExtentB );
// Variation of Conservative Advancement
@ -772,14 +771,19 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void*
b3SetBit( &taskContext->contactStateBitSet, contactIndex );
}
for ( int manifoldIndex = 0; manifoldIndex < contact->manifoldCount; ++manifoldIndex )
if ( touching )
{
b3Manifold* manifold = contact->manifolds + manifoldIndex;
for ( int pointIndex = 0; pointIndex < manifold->pointCount; ++pointIndex )
int manifoldCount = contact->manifoldCount;
for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex )
{
// Cache separation
b3ManifoldPoint* mp = manifold->points + pointIndex;
mp->baseSeparation = mp->separation;
b3Manifold* manifold = contact->manifolds + manifoldIndex;
int pointCount = manifold->pointCount;
for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex )
{
// Cache separation
b3ManifoldPoint* mp = manifold->points + pointIndex;
mp->baseSeparation = mp->separation;
}
}
}
}
@ -793,8 +797,6 @@ static void b3AddNonTouchingContact( b3World* world, b3Contact* contact )
b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet );
contact->colorIndex = B3_NULL_INDEX;
contact->localIndex = set->contactIndices.count;
contact->bodySimIndexA = B3_NULL_INDEX;
contact->bodySimIndexB = B3_NULL_INDEX;
b3Array_Push( set->contactIndices, contact->contactId );
}
@ -1358,7 +1360,7 @@ static bool DrawQueryCallback( int proxyId, uint64_t userData, void* context )
if ( draw->drawBounds )
{
draw->DrawBoundsFcn( shape->fatAABB, b3_colorGold, draw->context );
draw->DrawBoundsFcn( world->fatAABBs.data[shapeId], b3_colorGold, draw->context );
}
return true;
@ -1440,7 +1442,7 @@ void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits )
b3WorldTransform transform = { bodySim->center, bodySim->transform.q };
draw->DrawTransformFcn( transform, draw->context );
if (body->type == b3_dynamicBody)
if ( body->type == b3_dynamicBody )
{
b3Vec3 offset = { 0.05f, 0.05f, 0.05f };
b3Pos p = b3TransformWorldPoint( transform, offset );
@ -1563,9 +1565,7 @@ void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits )
{
// Hack inv_dt for single step debugging
float inv_dt = world->inv_dt > 0.0f ? world->inv_dt : 60.0f;
// todo validate
// multiply by one-half due to relax iteration
float force = 0.5f * mp->totalNormalImpulse * inv_dt;
float force = mp->totalNormalImpulse * inv_dt;
b3Pos p1 = p;
b3Pos p2 = b3OffsetPos( p1, b3MulSV( draw->forceScale * force, normal ) );
draw->DrawSegmentFcn( p1, p2, impulseColor, draw->context );
@ -1657,7 +1657,7 @@ void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits )
while ( shapeId != B3_NULL_INDEX )
{
b3Shape* shape = b3Array_Get( world->shapes, shapeId );
aabb = b3AABB_Union( aabb, shape->fatAABB );
aabb = b3AABB_Union( aabb, world->fatAABBs.data[shapeId] );
shapeCount += 1;
shapeId = shape->nextShapeId;
}
@ -3575,6 +3575,21 @@ void b3ValidateSolverSets( b3World* world )
B3_ASSERT( b3GetIdCapacity( &world->islandIdPool ) == world->islands.count );
B3_ASSERT( b3GetIdCapacity( &world->solverSetIdPool ) == world->solverSets.count );
// Ensure the encoded body sim indices are correct.
for ( int contactIndex = 0; contactIndex < world->contacts.count; ++contactIndex )
{
b3Contact* contact = world->contacts.data + contactIndex;
if ( contact->contactId == B3_NULL_INDEX )
{
continue;
}
b3Body* bodyA = b3Array_Get( world->bodies, contact->edges[0].bodyId );
b3Body* bodyB = b3Array_Get( world->bodies, contact->edges[1].bodyId );
B3_ASSERT( contact->encodedBodySimA == b3EncodeBodySimIndex( bodyA ) );
B3_ASSERT( contact->encodedBodySimB == b3EncodeBodySimIndex( bodyB ) );
}
int activeSetCount = 0;
int totalBodyCount = 0;
int totalJointCount = 0;
@ -3626,7 +3641,7 @@ void b3ValidateSolverSets( b3World* world )
B3_ASSERT( body->setIndex == setIndex );
B3_ASSERT( body->localIndex == i );
uint32_t syncedFlags = body->flags & ~b3_bodyTransientFlags;
uint32_t syncedFlags = body->flags & ~( b3_isFast | b3_bodyTransientFlags );
B3_ASSERT( ( bodySim->flags & syncedFlags ) == syncedFlags );
b3BodyState* bodyState = b3GetBodyState( world, body );
@ -3965,30 +3980,6 @@ void b3ValidateContacts( b3World* world )
if ( touching )
{
B3_ASSERT( 0 <= contact->colorIndex && contact->colorIndex < B3_GRAPH_COLOR_COUNT );
// Validate body sim indices
b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA );
b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB );
b3Body* bodyA = b3Array_Get( world->bodies, shapeA->bodyId );
b3Body* bodyB = b3Array_Get( world->bodies, shapeB->bodyId );
if ( bodyA->type == b3_staticBody )
{
B3_ASSERT( contact->bodySimIndexA == B3_NULL_INDEX );
}
else
{
B3_ASSERT( contact->bodySimIndexA == bodyA->localIndex );
}
if ( bodyB->type == b3_staticBody )
{
B3_ASSERT( contact->bodySimIndexB == B3_NULL_INDEX );
}
else
{
B3_ASSERT( contact->bodySimIndexB == bodyB->localIndex );
}
if ( ( contact->flags & b3_simMeshContact ) != 0 || contact->colorIndex == B3_OVERFLOW_INDEX )
{

View file

@ -33,6 +33,7 @@ b3DeclareArray( b3SolverSet );
b3DeclareArray( b3Joint );
b3DeclareArray( b3Contact );
b3DeclareArray( b3Island );
b3DeclareArray( b3AABB );
b3DeclareArray( b3Shape );
b3DeclareArray( b3Sensor );
b3DeclareArray( b3SensorTaskContext );
@ -176,6 +177,9 @@ typedef struct b3World
// These are sparse arrays that point into the pools above
b3Array( b3Shape ) shapes;
// This follows the shape array and is split out for the hot path in the narrow phase.
b3Array( b3AABB ) fatAABBs;
// Compound shapes are static only, so only the broad-phase static pass can meet one. This
// lets a world without compounds skip the shape type read in the pair batch.
int compoundShapeCount;

View file

@ -132,3 +132,34 @@ static inline uint32_t b3AtomicFetchOrU32( uint32_t* a, uint32_t mask )
#error "Unsupported platform"
#endif
}
static inline int64_t b3AtomicFetchAddI64( b3AtomicI64* a, int64_t increment )
{
#if defined( _MSC_VER )
return (int64_t)_InterlockedExchangeAdd64( (__int64*)&a->value, (__int64)increment );
#elif defined( __GNUC__ ) || defined( __clang__ )
return __atomic_fetch_add( &a->value, increment, __ATOMIC_SEQ_CST );
#else
#error "Unsupported platform"
#endif
}
static inline int64_t b3AtomicLoadI64( b3AtomicI64* a )
{
#if defined( _MSC_VER ) && !defined( __clang__ ) && !defined( _M_ARM )
int64_t value = __iso_volatile_load64( (volatile __int64*)&a->value );
#if defined( _M_ARM64 ) || defined( _M_ARM64EC )
__dmb( 0xB );
#else
_ReadWriteBarrier();
#endif
return value;
#elif defined( _MSC_VER ) && !defined( __clang__ )
// 32-bit ARM has no plain atomic 64-bit load
return _InterlockedOr64( (__int64*)&a->value, 0 );
#elif defined( __GNUC__ ) || defined( __clang__ )
return __atomic_load_n( &a->value, __ATOMIC_SEQ_CST );
#else
#error "Unsupported platform"
#endif
}

View file

@ -51,8 +51,8 @@ typedef struct b3World b3World;
#define B3_REC_VERSION_MAJOR 6
// Minor tracks op-stream additions that keep the 48 byte header shape.
// Minor version 6 moved the broad phase to the tree against tree pair traversal
#define B3_REC_VERSION_MINOR 6
// Minor version 7 made the contact body sim locators persistent
#define B3_REC_VERSION_MINOR 7
// File header, fixed 48 bytes. Contains the registry locator so the player
// can load geometry before replaying any ops.

View file

@ -75,7 +75,7 @@ static float b3ComputeShapeMargin( b3Shape* shape )
return b3MinFloat( B3_MAX_AABB_MARGIN, B3_AABB_MARGIN_FRACTION * margin );
}
static void b3UpdateShapeAABBs( b3Shape* shape, b3WorldTransform transform, b3BodyType proxyType )
static void b3UpdateShapeAABBs( b3Shape* shape, b3AABB* fatAABB, b3WorldTransform transform, b3BodyType proxyType )
{
// Compute a bounding box with a speculative margin
const float speculativeDistance = B3_SPECULATIVE_DISTANCE;
@ -86,14 +86,7 @@ static void b3UpdateShapeAABBs( b3Shape* shape, b3WorldTransform transform, b3Bo
// Smaller margin for static bodies. Cannot be zero due to TOI tolerance.
float margin = proxyType == b3_staticBody ? speculativeDistance : aabbMargin;
b3AABB fatAABB;
fatAABB.lowerBound.x = aabb.lowerBound.x - margin;
fatAABB.lowerBound.y = aabb.lowerBound.y - margin;
fatAABB.lowerBound.z = aabb.lowerBound.z - margin;
fatAABB.upperBound.x = aabb.upperBound.x + margin;
fatAABB.upperBound.y = aabb.upperBound.y + margin;
fatAABB.upperBound.z = aabb.upperBound.z + margin;
shape->fatAABB = fatAABB;
*fatAABB = b3AABB_Inflate( aabb, margin );
}
static b3Shape* b3CreateShapeInternal( b3World* world, b3Body* body, b3WorldTransform bodyTransform, const b3ShapeDef* def,
@ -105,12 +98,15 @@ static b3Shape* b3CreateShapeInternal( b3World* world, b3Body* body, b3WorldTran
if ( shapeId == world->shapes.count )
{
b3Array_Push( world->shapes, (b3Shape){ 0 } );
b3Array_Push( world->fatAABBs, (b3AABB){ 0 } );
}
else
{
B3_ASSERT( world->shapes.data[shapeId].id == B3_NULL_INDEX );
}
B3_ASSERT( world->fatAABBs.count == world->shapes.count );
b3Shape* shape = b3Array_Get( world->shapes, shapeId );
switch ( shapeType )
@ -186,7 +182,7 @@ static b3Shape* b3CreateShapeInternal( b3World* world, b3Body* body, b3WorldTran
shape->localCentroid = b3GetShapeCentroid( shape );
shape->aabbMargin = b3ComputeShapeMargin( shape );
shape->aabb = (b3AABB){ b3Vec3_zero, b3Vec3_zero };
shape->fatAABB = (b3AABB){ b3Vec3_zero, b3Vec3_zero };
world->fatAABBs.data[shapeId] = (b3AABB){ b3Vec3_zero, b3Vec3_zero };
shape->nameId = b3AddName( &world->names, def->name );
shape->generation += 1;
@ -220,7 +216,7 @@ static b3Shape* b3CreateShapeInternal( b3World* world, b3Body* body, b3WorldTran
{
b3BodyType proxyType = body->type;
bool forcePairCreation = def->invokeContactCreation && shape->type != b3_compoundShape;
b3CreateShapeProxy( shape, &world->broadPhase, proxyType, bodyTransform, forcePairCreation );
b3CreateShapeProxy( world, shape, proxyType, bodyTransform, forcePairCreation );
}
// Add to shape doubly linked list
@ -1004,15 +1000,16 @@ int b3CollideMover( b3PlaneResult* planes, int planeCapacity, const b3Shape* sha
return planeCount;
}
void b3CreateShapeProxy( b3Shape* shape, b3BroadPhase* bp, b3BodyType type, b3WorldTransform transform, bool forcePairCreation )
void b3CreateShapeProxy( b3World* world, b3Shape* shape, b3BodyType type, b3WorldTransform transform, bool forcePairCreation )
{
B3_ASSERT( shape->proxyKey == B3_NULL_INDEX );
b3UpdateShapeAABBs( shape, transform, type );
b3AABB* fatAABB = world->fatAABBs.data + shape->id;
b3UpdateShapeAABBs( shape, fatAABB, transform, type );
// Create proxies in the broad-phase.
shape->proxyKey =
b3BroadPhase_CreateProxy( bp, type, shape->fatAABB, shape->filter.categoryBits, shape->id, forcePairCreation );
shape->proxyKey = b3BroadPhase_CreateProxy( &world->broadPhase, type, *fatAABB, shape->filter.categoryBits, shape->id,
forcePairCreation );
B3_ASSERT( B3_PROXY_TYPE( shape->proxyKey ) < b3_bodyTypeCount );
}
@ -1360,25 +1357,26 @@ static void b3ResetProxy( b3World* world, b3Shape* shape, bool wakeBodies, bool
if ( shape->proxyKey != B3_NULL_INDEX )
{
b3BodyType proxyType = B3_PROXY_TYPE( shape->proxyKey );
b3UpdateShapeAABBs( shape, transform, proxyType );
b3AABB* fatAABB = world->fatAABBs.data + shapeId;
b3UpdateShapeAABBs( shape, fatAABB, transform, proxyType );
if ( destroyProxy )
{
b3BroadPhase_DestroyProxy( &world->broadPhase, shape->proxyKey );
bool forcePairCreation = true;
shape->proxyKey = b3BroadPhase_CreateProxy( &world->broadPhase, proxyType, shape->fatAABB, shape->filter.categoryBits,
shape->proxyKey = b3BroadPhase_CreateProxy( &world->broadPhase, proxyType, *fatAABB, shape->filter.categoryBits,
shapeId, forcePairCreation );
}
else
{
b3BroadPhase_MoveProxy( &world->broadPhase, shape->proxyKey, shape->fatAABB );
b3BroadPhase_MoveProxy( &world->broadPhase, shape->proxyKey, *fatAABB );
}
}
else
{
b3BodyType proxyType = body->type;
b3UpdateShapeAABBs( shape, transform, proxyType );
b3UpdateShapeAABBs( shape, world->fatAABBs.data + shapeId, transform, proxyType );
}
b3ValidateSolverSets( world );

View file

@ -37,7 +37,6 @@ typedef struct b3Shape
float aabbMargin;
b3AABB aabb;
b3AABB fatAABB;
b3Vec3 localCentroid;
int materialCount;
@ -74,7 +73,7 @@ static inline b3SurfaceMaterial* b3GetShapeMaterials( const b3Shape* shape )
return shape->materials != NULL ? shape->materials : (b3SurfaceMaterial*)&shape->material;
}
void b3CreateShapeProxy( b3Shape* shape, b3BroadPhase* bp, b3BodyType type, b3WorldTransform transform, bool forcePairCreation );
void b3CreateShapeProxy( b3World* world, b3Shape* shape, b3BodyType type, b3WorldTransform transform, bool forcePairCreation );
void b3DestroyShapeProxy( b3Shape* shape, b3BroadPhase* bp );
void b3DestroyShapeAllocations( b3World* world, b3Shape* shape );

View file

@ -476,6 +476,20 @@ static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b )
return vreinterpretq_f32_u32( vorrq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) );
}
// a & ~b
static inline b3FloatW b3AndNotW( b3FloatW a, b3FloatW b )
{
return vreinterpretq_f32_u32( vbicq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) );
}
static inline b3FloatW b3SoftMaskW( const int* indexA, const int* indexB )
{
int32x4_t zero = vdupq_n_s32( 0 );
uint32x4_t a = vceqq_s32( vld1q_s32( (const int32_t*)indexA ), zero );
uint32x4_t b = vceqq_s32( vld1q_s32( (const int32_t*)indexB ), zero );
return vreinterpretq_f32_u32( vorrq_u32( a, b ) );
}
static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b )
{
return vreinterpretq_f32_u32( vcgtq_f32( a, b ) );
@ -650,6 +664,22 @@ static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b )
return _mm_or_ps( a, b );
}
// a & ~b
static inline b3FloatW b3AndNotW( b3FloatW a, b3FloatW b )
{
// Arguments are reversed
return _mm_andnot_ps( b, a );
}
// This is used to optimize selection of contact softness.
static inline b3FloatW b3SoftMaskW( const int* indexA, const int* indexB )
{
__m128i zero = _mm_setzero_si128();
__m128i a = _mm_cmpeq_epi32( _mm_loadu_si128( (const __m128i*)indexA ), zero );
__m128i b = _mm_cmpeq_epi32( _mm_loadu_si128( (const __m128i*)indexB ), zero );
return _mm_castsi128_ps( _mm_or_si128( a, b ) );
}
static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b )
{
return _mm_cmpgt_ps( a, b );
@ -834,6 +864,27 @@ static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b )
return r;
}
// a & ~b
static inline b3FloatW b3AndNotW( b3FloatW a, b3FloatW b )
{
b3FloatW r;
r.x = a.x != 0.0f && b.x == 0.0f ? 1.0f : 0.0f;
r.y = a.y != 0.0f && b.y == 0.0f ? 1.0f : 0.0f;
r.z = a.z != 0.0f && b.z == 0.0f ? 1.0f : 0.0f;
r.w = a.w != 0.0f && b.w == 0.0f ? 1.0f : 0.0f;
return r;
}
static inline b3FloatW b3SoftMaskW( const int* indexA, const int* indexB )
{
b3FloatW r;
r.x = indexA[0] == 0 || indexB[0] == 0 ? 1.0f : 0.0f;
r.y = indexA[1] == 0 || indexB[1] == 0 ? 1.0f : 0.0f;
r.z = indexA[2] == 0 || indexB[2] == 0 ? 1.0f : 0.0f;
r.w = indexA[3] == 0 || indexB[3] == 0 ? 1.0f : 0.0f;
return r;
}
static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b )
{
b3FloatW r;

View file

@ -24,8 +24,6 @@
#include <stddef.h>
#include <stdio.h>
_Static_assert( B3_RESTITUTION_ITERATIONS >= 1, "must be 1 or more" );
// these are useful for solver testing
#define ITERATIONS 1
#define RELAX_ITERATIONS 1
@ -473,7 +471,7 @@ static bool b3ContinuousQueryCallback( int proxyId, uint64_t userData, void* con
}
// Continuous collision of dynamic versus static
static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext* taskContext )
static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext* taskContext, float dt )
{
b3TracyCZoneNC( ccd, "CCD", b3_colorDarkGoldenRod, true );
@ -481,7 +479,7 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext*
b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet );
b3BodySim* fastBodySim = b3Array_Get( awakeSet->bodySims, bodySimIndex );
B3_ASSERT( fastBodySim->flags & b3_isFast );
B3_VALIDATE( fastBodySim->flags & b3_isFast );
// Re-center the sweep on the fast body so the TOI and the swept query stay in float precision
b3Pos base = fastBodySim->center0;
@ -567,6 +565,16 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext*
fastBodySim->rotation0 = q;
fastBodySim->center0 = center;
// Timeloss means there is a lost gravity contribution. Other forces and torques are ignored for now.
b3BodyState* fastBodyState = b3Array_Get( awakeSet->bodyStates, bodySimIndex );
b3Vec3 v = fastBodyState->linearVelocity;
float timeLoss = ( 1.0f - context.fraction ) * dt;
b3Vec3 dv = b3MulSV( -timeLoss * fastBodySim->gravityScale, world->gravity );
dv.x = ( fastBodyState->flags & b3_lockLinearX ) ? 0.0f : dv.x;
dv.y = ( fastBodyState->flags & b3_lockLinearY ) ? 0.0f : dv.y;
dv.z = ( fastBodyState->flags & b3_lockLinearZ ) ? 0.0f : dv.z;
fastBodyState->linearVelocity = b3Add( v, dv );
// The move event was written before CCD, so correct it with the impact pose
b3BodyMoveEvent* event = b3Array_Get( world->bodyMoveEvents, bodySimIndex );
event->transform = fastBodySim->transform;
@ -583,19 +591,20 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext*
b3AABB aabb = b3ComputeFatShapeAABB( shape, transform, speculativeScalar );
shape->aabb = aabb;
if ( b3AABB_Contains( shape->fatAABB, aabb ) == false )
b3AABB* shapeFatAABB = world->fatAABBs.data + shapeId;
if ( b3AABB_Contains( *shapeFatAABB, aabb ) == false )
{
float marginScalar = shape->aabbMargin;
b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar };
shape->fatAABB = (b3AABB){ b3Sub( aabb.lowerBound, aabbMargin ), b3Add( aabb.upperBound, aabbMargin ) };
fastBodySim->flags |= b3_enlargeBounds;
*shapeFatAABB = b3AABB_Inflate( aabb, shape->aabbMargin );
// Regular bodies mark the hierarchy as moved using atomic operations.
// Bullets are handled separately at a later stage.
if ( isBullet == false )
{
b3BroadPhase_MarkProxyMoved( &world->broadPhase, shape->proxyKey, shape->fatAABB );
b3BroadPhase_MarkProxyMoved( &world->broadPhase, shape->proxyKey, *shapeFatAABB );
}
else
{
fastBodySim->flags |= b3_enlargeBulletBounds;
}
}
@ -618,20 +627,18 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext*
// shape->aabb is still valid from above
if ( b3AABB_Contains( shape->fatAABB, shape->aabb ) == false )
b3AABB* shapeFatAABB = world->fatAABBs.data + shapeId;
if ( b3AABB_Contains( *shapeFatAABB, shape->aabb ) == false )
{
float marginScalar = shape->aabbMargin;
b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar };
shape->fatAABB = (b3AABB){
.lowerBound = b3Sub( shape->aabb.lowerBound, aabbMargin ),
.upperBound = b3Add( shape->aabb.upperBound, aabbMargin ),
};
fastBodySim->flags |= b3_enlargeBounds;
*shapeFatAABB = b3AABB_Inflate( shape->aabb, shape->aabbMargin );
if ( isBullet == false )
{
b3BroadPhase_MarkProxyMoved( &world->broadPhase, shape->proxyKey, shape->fatAABB );
b3BroadPhase_MarkProxyMoved( &world->broadPhase, shape->proxyKey, *shapeFatAABB );
}
else
{
fastBodySim->flags |= b3_enlargeBulletBounds;
}
}
@ -754,10 +761,13 @@ static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex,
// or b3Body_SetMassData.
B3_ASSERT( ( body->flags & b3_dirtyMass ) == 0 );
// Clear the transient flags (fast, speed capped, had TOI). These flags are conditionally set
// as part of the code below.
body->flags &= ~b3_bodyTransientFlags;
body->flags |= ( sim->flags & ( b3_isSpeedCapped | b3_hadTimeOfImpact ) );
body->flags |= ( state->flags & ( b3_isSpeedCapped | b3_hadTimeOfImpact ) );
sim->flags &= ~b3_bodyTransientFlags;
sim->flags &= ~( b3_isFast | b3_bodyTransientFlags );
// The body state flag knows about speed capping (used for debug draw).
body->flags |= ( state->flags & b3_isSpeedCapped );
state->flags &= ~b3_bodyTransientFlags;
if ( enableSleep == false || ( body->flags & b3_enableSleep ) == 0 || sleepVelocity > body->sleepThreshold )
@ -781,7 +791,7 @@ static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex,
}
else
{
b3SolveContinuous( world, simIndex, taskContext );
b3SolveContinuous( world, simIndex, taskContext, timeStep );
}
}
else
@ -839,14 +849,13 @@ static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex,
b3AABB aabb = b3ComputeFatShapeAABB( shape, transform, speculativeScalar );
shape->aabb = aabb;
if ( b3AABB_Contains( shape->fatAABB, aabb ) == false )
b3AABB* shapeFatAABB = world->fatAABBs.data + shapeId;
if ( b3AABB_Contains( *shapeFatAABB, aabb ) == false )
{
float marginScalar = shape->aabbMargin;
b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar };
shape->fatAABB = (b3AABB){ b3Sub( aabb.lowerBound, aabbMargin ), b3Add( aabb.upperBound, aabbMargin ) };
*shapeFatAABB = b3AABB_Inflate( aabb, shape->aabbMargin );
// Mark the hierarchy as moved using atomic operations.
b3BroadPhase_MarkProxyMoved( &world->broadPhase, shape->proxyKey, shape->fatAABB );
b3BroadPhase_MarkProxyMoved( &world->broadPhase, shape->proxyKey, *shapeFatAABB );
}
shapeId = shape->nextShapeId;
@ -1006,13 +1015,11 @@ static void b3ExecuteBlock( b3SolverStage* stage, b3StepContext* context, b3Solv
}
else if ( blockType == b3_graphWideContactBlock )
{
bool useBias = true;
b3SolveContacts_Convex( block, context, useBias );
b3PushContacts_Convex( block, context );
}
else
{
bool useBias = true;
b3SolveContacts_Mesh( block, context, useBias );
b3PushContacts_Mesh( block, context );
}
break;
@ -1028,24 +1035,11 @@ static void b3ExecuteBlock( b3SolverStage* stage, b3StepContext* context, b3Solv
}
else if ( blockType == b3_graphWideContactBlock )
{
bool useBias = false;
b3SolveContacts_Convex( block, context, useBias );
b3SolveContacts_Convex( block, context );
}
else
{
bool useBias = false;
b3SolveContacts_Mesh( block, context, useBias );
}
break;
case b3_stageRestitution:
if ( blockType == b3_graphWideContactBlock )
{
b3ApplyRestitution_Convex( block, context );
}
else if ( blockType == b3_graphContactBlock )
{
b3ApplyRestitution_Mesh( block, context );
b3SolveContacts_Mesh( block, context );
}
break;
@ -1188,7 +1182,6 @@ static void b3SolverTask( void* taskContext )
b3_stageSolve,
b3_stageIntegratePositions,
b3_stageRelax,
b3_stageRestitution,
b3_stageStoreImpulses
*/
@ -1265,7 +1258,7 @@ static void b3SolverTask( void* taskContext )
{
// Overflow constraints have lower priority. Typically these are dynamic-vs-dynamic.
b3SolveJoints_Overflow( context, useBias );
b3SolveContacts_Overflow( context, useBias );
b3PushContacts_Overflow( context );
for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex )
{
@ -1293,7 +1286,7 @@ static void b3SolverTask( void* taskContext )
for ( int j = 0; j < RELAX_ITERATIONS; ++j )
{
b3SolveJoints_Overflow( context, useBias );
b3SolveContacts_Overflow( context, useBias );
b3SolveContacts_Overflow( context );
for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex )
{
@ -1312,25 +1305,6 @@ static void b3SolverTask( void* taskContext )
// integrate velocities / warm start / solve / integrate positions / relax
stageIndex += 1 + activeColorCount + ITERATIONS * activeColorCount + 1 + RELAX_ITERATIONS * activeColorCount;
// Restitution
for ( int iteration = 0; iteration < B3_RESTITUTION_ITERATIONS; ++iteration )
{
b3ApplyRestitution_Overflow( context );
int iterStageIndex = stageIndex;
for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex )
{
syncBits = ( graphSyncIndex << 16 ) | iterStageIndex;
B3_ASSERT( stages[iterStageIndex].type == b3_stageRestitution );
b3ExecuteMainStage( stages + iterStageIndex, context, syncBits );
iterStageIndex += 1;
}
graphSyncIndex += 1;
stageIndex += activeColorCount;
}
profile->applyRestitution += b3GetMillisecondsAndReset( &ticks );
// Store impulses
b3StoreImpulses_Overflow( context );
@ -1420,7 +1394,7 @@ static void b3BulletBodyTask( int startIndex, int endIndex, int workerIndex, voi
for ( int i = startIndex; i < endIndex; ++i )
{
int simIndex = stepContext->bulletBodies[i];
b3SolveContinuous( stepContext->world, simIndex, taskContext );
b3SolveContinuous( stepContext->world, simIndex, taskContext, stepContext->dt );
}
b3TracyCZoneEnd( bullet_body_task );
@ -1689,8 +1663,6 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
stageCount += 1;
// b3_stageRelax
stageCount += RELAX_ITERATIONS * activeColorCount;
// b3_stageRestitution
stageCount += B3_RESTITUTION_ITERATIONS * activeColorCount;
// b3_stageStoreWideImpulses
stageCount += 1;
// b3_stageStoreImpulses
@ -1774,9 +1746,6 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
stage = b3InitStage( stage, b3_stageIntegratePositions, bodyBlocks, bodyDim.count, UINT8_MAX );
stage = b3InitColorStages( stage, b3_stageRelax, RELAX_ITERATIONS, activeColorCount, graphColorBlocks, graphBlockCounts,
activeColorIndices );
// Note: joint blocks mixed in, could have joint limit restitution
stage = b3InitColorStages( stage, b3_stageRestitution, B3_RESTITUTION_ITERATIONS, activeColorCount, graphColorBlocks,
graphBlockCounts, activeColorIndices );
stage = b3InitStage( stage, b3_stageStoreWideImpulses, convexBlocks, convexPrepareDim.count, UINT8_MAX );
stage = b3InitStage( stage, b3_stageStoreImpulses, meshBlocks, meshPrepareDim.count, UINT8_MAX );
@ -2117,13 +2086,13 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
for ( int i = 0; i < bulletBodyCount; ++i )
{
b3BodySim* bulletBodySim = bodySimArray + bulletBodySimIndices[i];
if ( ( bulletBodySim->flags & b3_enlargeBounds ) == 0 )
if ( ( bulletBodySim->flags & b3_enlargeBulletBounds ) == 0 )
{
continue;
}
// Clear flag
bulletBodySim->flags &= ~b3_enlargeBounds;
bulletBodySim->flags &= ~b3_enlargeBulletBounds;
int bodyId = bulletBodySim->bodyId;
B3_ASSERT( 0 <= bodyId && bodyId < world->bodies.count );
@ -2139,10 +2108,11 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
B3_VALIDATE( B3_PROXY_TYPE( proxyKey ) == b3_dynamicBody );
b3AABB treeAABB = b3DynamicTree_GetAABB( dynamicTree, proxyId );
b3AABB shapeFatAABB = world->fatAABBs.data[shapeId];
if ( b3AABB_Contains( treeAABB, shape->fatAABB ) == false )
if ( b3AABB_Contains( treeAABB, shapeFatAABB ) == false )
{
b3DynamicTree_EnlargeProxy( dynamicTree, proxyId, shape->fatAABB );
b3DynamicTree_EnlargeProxy( dynamicTree, proxyId, shapeFatAABB );
}
shapeId = shape->nextShapeId;

View file

@ -76,7 +76,6 @@ typedef enum b3SolverStageType
b3_stageSolve,
b3_stageIntegratePositions,
b3_stageRelax,
b3_stageRestitution,
b3_stageStoreWideImpulses,
b3_stageStoreImpulses,
} b3SolverStageType;

View file

@ -62,7 +62,8 @@ void b3WakeSolverSet( b3World* world, int setIndex )
*state = b3_identityBodyState;
state->flags = body->flags;
// move non-touching contacts from disabled set to awake set
// Move non-touching contacts from disabled set to awake set.
int encodedBodySimIndex = body->localIndex;
int contactKey = body->headContactKey;
while ( contactKey != B3_NULL_INDEX )
{
@ -75,6 +76,21 @@ void b3WakeSolverSet( b3World* world, int setIndex )
if ( contact->setIndex != b3_disabledSet )
{
B3_ASSERT( contact->setIndex == b3_awakeSet || contact->setIndex == setIndex );
// If this contact is awake but not touching I need to fixup the body sim index here.
if ( contact->setIndex == b3_awakeSet )
{
B3_ASSERT( contact->colorIndex == B3_NULL_INDEX );
if ( edgeIndex == 0 )
{
contact->encodedBodySimA = encodedBodySimIndex;
}
else
{
contact->encodedBodySimB = encodedBodySimIndex;
}
}
continue;
}
@ -89,6 +105,15 @@ void b3WakeSolverSet( b3World* world, int setIndex )
contact->localIndex = awakeSet->contactIndices.count;
b3Array_Push( awakeSet->contactIndices, contactId );
if ( edgeIndex == 0 )
{
contact->encodedBodySimA = encodedBodySimIndex;
}
else
{
contact->encodedBodySimB = encodedBodySimIndex;
}
int movedLocalIndex = b3Array_RemoveSwap( disabledSet->contactIndices, localIndex );
if ( movedLocalIndex != B3_NULL_INDEX )
{
@ -227,6 +252,11 @@ void b3TrySleepIsland( b3World* world, int islandId )
b3Body* movedBody = b3Array_Get( world->bodies, movedId );
B3_ASSERT( movedBody->localIndex == movedIndex );
movedBody->localIndex = awakeBodyIndex;
if ( movedBody->islandId != islandId )
{
b3RefreshBodyContactIndices( world, movedBody );
}
}
// destroy state, no need to clone
@ -245,14 +275,9 @@ void b3TrySleepIsland( b3World* world, int islandId )
b3Contact* contact = b3Array_Get( world->contacts, contactId );
B3_ASSERT( contact->setIndex == b3_awakeSet || contact->setIndex == b3_disabledSet );
contactKey = contact->edges[edgeIndex].nextKey;
B3_ASSERT( contact->setIndex == b3_awakeSet );
if ( contact->setIndex == b3_disabledSet )
{
// already moved to disabled set by another body in the island
continue;
}
contactKey = contact->edges[edgeIndex].nextKey;
if ( contact->colorIndex != B3_NULL_INDEX )
{
@ -261,6 +286,15 @@ void b3TrySleepIsland( b3World* world, int islandId )
continue;
}
if ( edgeIndex == 0 )
{
contact->encodedBodySimA = B3_NULL_INDEX;
}
else
{
contact->encodedBodySimB = B3_NULL_INDEX;
}
// the other body may still be awake, it still may go to sleep and then it will be responsible
// for moving this contact to the disabled set.
int otherEdgeIndex = edgeIndex ^ 1;
@ -352,6 +386,8 @@ void b3TrySleepIsland( b3World* world, int islandId )
contact->setIndex = sleepSetId;
contact->colorIndex = B3_NULL_INDEX;
contact->localIndex = sleepContactIndex;
contact->encodedBodySimA = b3SleepBodySimIndex( contact->encodedBodySimA );
contact->encodedBodySimB = b3SleepBodySimIndex( contact->encodedBodySimB );
}
}
@ -530,6 +566,8 @@ void b3MergeSolverSets( b3World* world, int setId1, int setId2 )
void b3TransferBody( b3World* world, b3SolverSet* targetSet, b3SolverSet* sourceSet, b3Body* body )
{
B3_ASSERT( body->headContactKey == B3_NULL_INDEX );
if ( targetSet == sourceSet )
{
return;
@ -543,7 +581,8 @@ void b3TransferBody( b3World* world, b3SolverSet* targetSet, b3SolverSet* source
memcpy( targetSim, sourceSim, sizeof( b3BodySim ) );
// Clear transient body flags
targetSim->flags &= ~( b3_isFast | b3_isSpeedCapped | b3_hadTimeOfImpact );
body->flags &= ~b3_bodyTransientFlags;
targetSim->flags &= ~(b3_isFast | b3_bodyTransientFlags);
// Remove body sim from solver set that owns it
int movedIndex = b3Array_RemoveSwap( sourceSet->bodySims, sourceIndex );
@ -555,6 +594,7 @@ void b3TransferBody( b3World* world, b3SolverSet* targetSet, b3SolverSet* source
b3Body* movedBody = b3Array_Get( world->bodies, movedId );
B3_ASSERT( movedBody->localIndex == movedIndex );
movedBody->localIndex = sourceIndex;
b3RefreshBodyContactIndices( world, movedBody );
}
if ( sourceSet->setIndex == b3_awakeSet )

View file

@ -33,7 +33,7 @@
// Snapshot image magic 'BNS3' and version
#define B3_SNAP_MAGIC 0x33534E42u
#define B3_SNAP_VERSION 5u // broad-phase pair traversal, move buffer removed
#define B3_SNAP_VERSION 7u // same-step restitution
#define B3_SNAP_FLAG_VALIDATION 0x1u
#define B3_SNAP_FLAG_DOUBLE_PRECISION 0x2u
@ -831,8 +831,6 @@ static void b3SerContacts( b3RecBuffer* buf, b3World* world )
// Write raw struct with pointer fields zeroed
b3Contact copy = *c;
copy.manifolds = NULL;
copy.bodySimIndexA = B3_NULL_INDEX;
copy.bodySimIndexB = B3_NULL_INDEX;
if ( copy.flags & b3_simMeshContact )
{
copy.meshContact.triangleCache.data = NULL;
@ -889,8 +887,6 @@ static void b3DesContacts( b3SnapReader* r, b3World* world )
b3Contact* dst = world->contacts.data + i;
b3SnapR_Bytes( r, dst, sizeof( b3Contact ) );
dst->manifolds = NULL;
dst->bodySimIndexA = B3_NULL_INDEX;
dst->bodySimIndexB = B3_NULL_INDEX;
if ( dst->flags & b3_simMeshContact )
{
dst->meshContact.triangleCache.data = NULL;
@ -1062,6 +1058,8 @@ int b3SerializeWorld( b3World* world, b3RecBuffer* buf, b3Recording* rec )
// Shape sparse array with geometry interning
b3SerShapes( buf, world, rec );
b3SerPodArray( buf, world->fatAABBs );
// Contact sparse array with manifold and mesh triangleCache
b3SerContacts( buf, world );
@ -1243,6 +1241,13 @@ bool b3DeserializeIntoShell( const uint8_t* data, int size, b3World* world, b3Re
return false;
}
b3DesPodArray( r, world->fatAABBs );
if ( !r->ok )
{
return false;
}
// 6. Contact sparse array
b3DesContacts( r, world );