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:
parent
f555ee4208
commit
9e5a4cde86
49 changed files with 3395 additions and 864 deletions
52
src/body.c
52
src/body.c
|
|
@ -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 )
|
||||
|
|
|
|||
43
src/body.h
43
src/body.h
|
|
@ -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 )
|
||||
|
|
|
|||
|
|
@ -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 )
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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 )
|
||||
|
|
|
|||
|
|
@ -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
|
|
@ -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 );
|
||||
|
|
|
|||
17
src/core.c
17
src/core.c
|
|
@ -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 )
|
||||
|
|
|
|||
|
|
@ -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] ) )
|
||||
|
||||
|
|
|
|||
|
|
@ -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 )
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
36
src/shape.c
36
src/shape.c
|
|
@ -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 );
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
|
|
|||
51
src/simd.h
51
src/simd.h
|
|
@ -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;
|
||||
|
|
|
|||
132
src/solver.c
132
src/solver.c
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -76,7 +76,6 @@ typedef enum b3SolverStageType
|
|||
b3_stageSolve,
|
||||
b3_stageIntegratePositions,
|
||||
b3_stageRelax,
|
||||
b3_stageRestitution,
|
||||
b3_stageStoreWideImpulses,
|
||||
b3_stageStoreImpulses,
|
||||
} b3SolverStageType;
|
||||
|
|
|
|||
|
|
@ -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 )
|
||||
|
|
|
|||
|
|
@ -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 );
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue