Optimizations (#168)
- Optimize contact prepare - Back to post restitution stage - Optimize solver math - Bug fixes
This commit is contained in:
parent
9e5a4cde86
commit
79fc55976a
55 changed files with 3248 additions and 1519 deletions
|
|
@ -10,8 +10,8 @@ b3BlockAllocator b3CreateBlockAllocator( int elementSize, int initialCount )
|
|||
B3_ASSERT( elementSize >= (int)sizeof( void* ) );
|
||||
|
||||
// The free list stores a pointer inside each element, so the stride must keep every element aligned.
|
||||
// Blocks come from b3Alloc, so a stride that is a multiple of B3_ALIGNMENT aligns all elements.
|
||||
elementSize = ( ( elementSize - 1 ) | ( B3_ALIGNMENT - 1 ) ) + 1;
|
||||
// Blocks come from b3Alloc, so a stride that is a multiple of B3_BLOCK_ALIGNMENT aligns all elements.
|
||||
elementSize = ( ( elementSize - 1 ) | ( B3_BLOCK_ALIGNMENT - 1 ) ) + 1;
|
||||
|
||||
b3BlockAllocator allocator = { 0 };
|
||||
|
||||
|
|
@ -85,7 +85,7 @@ void b3FreeElement( b3BlockAllocator* allocator, void* element )
|
|||
B3_ASSERT( allocator != NULL );
|
||||
B3_ASSERT( element != NULL );
|
||||
B3_ASSERT( allocator->allocationCount > 0 );
|
||||
B3_ASSERT( ( (uintptr_t)element & ( B3_ALIGNMENT - 1 ) ) == 0 );
|
||||
B3_ASSERT( ( (uintptr_t)element & ( B3_BLOCK_ALIGNMENT - 1 ) ) == 0 );
|
||||
|
||||
allocator->allocationCount -= 1;
|
||||
|
||||
|
|
|
|||
|
|
@ -9,6 +9,7 @@
|
|||
|
||||
#define B3_BLOCK_EXPONENT 8
|
||||
#define B3_BLOCK_SIZE ( 1 << B3_BLOCK_EXPONENT )
|
||||
#define B3_BLOCK_ALIGNMENT 16
|
||||
|
||||
typedef struct b3Block
|
||||
{
|
||||
|
|
@ -26,7 +27,7 @@ typedef struct b3BlockAllocator
|
|||
int allocationCount;
|
||||
} b3BlockAllocator;
|
||||
|
||||
// Element must be large enough to hold a pointer. The element size is rounded up to B3_ALIGNMENT.
|
||||
// Element must be large enough to hold a pointer. The element size is rounded up to B3_BLOCK_ALIGNMENT.
|
||||
b3BlockAllocator b3CreateBlockAllocator( int elementSize, int initialCount );
|
||||
void b3DestroyBlockAllocator( b3BlockAllocator* allocator );
|
||||
|
||||
|
|
|
|||
|
|
@ -97,8 +97,8 @@ void b3SyncBodyFlags( b3World* world, b3Body* body )
|
|||
{
|
||||
b3BodySim* bodySim = b3GetBodySim( world, body );
|
||||
|
||||
// Preserve the fast flag for contact recycling.
|
||||
bodySim->flags = ( bodySim->flags & b3_isFast ) | ( body->flags & ~b3_bodyTransientFlags );
|
||||
// Preserve the sim only flags: fast for contact recycling, time of impact for debug draw.
|
||||
bodySim->flags = ( bodySim->flags & ( b3_isFast | b3_hadTimeOfImpact ) ) | ( body->flags & ~b3_bodyTransientFlags );
|
||||
|
||||
b3BodyState* bodyState = b3GetBodyState( world, body );
|
||||
if ( bodyState != NULL )
|
||||
|
|
@ -2217,7 +2217,7 @@ void b3Body_Disable( b3BodyId bodyId )
|
|||
continue;
|
||||
}
|
||||
|
||||
B3_ASSERT( joint->setIndex == set->setIndex || set->setIndex == b3_staticSet );
|
||||
B3_ASSERT( joint->setIndex == set->setIndex || set->setIndex == b3_staticSet || joint->setIndex == b3_staticSet );
|
||||
|
||||
// Remove joint from island
|
||||
b3UnlinkJoint( world, joint );
|
||||
|
|
@ -2314,7 +2314,7 @@ void b3Body_Enable( b3BodyId bodyId )
|
|||
|
||||
// Transfer joint first
|
||||
int jointSetId;
|
||||
if ( bodyA->setIndex == b3_staticSet && bodyB->setIndex == b3_staticSet )
|
||||
if ( bodyA->type != b3_dynamicBody && bodyB->type != b3_dynamicBody )
|
||||
{
|
||||
jointSetId = b3_staticSet;
|
||||
}
|
||||
|
|
|
|||
31
src/body.h
31
src/body.h
|
|
@ -40,7 +40,7 @@ enum b3BodyFlags
|
|||
// This body was speed capped in the current time step
|
||||
b3_isSpeedCapped = 0x00000100,
|
||||
|
||||
// This body had a time of impact event in the current time step
|
||||
// This body had a time of impact event in the current time step (body sim only)
|
||||
b3_hadTimeOfImpact = 0x00000200,
|
||||
|
||||
// This body has no limit on angular velocity
|
||||
|
|
@ -159,28 +159,30 @@ typedef struct b3Body
|
|||
// according to substep progress. Contacts have reduced stability when anchors are rotated during substeps, especially for
|
||||
// round shapes.
|
||||
//
|
||||
// 56 bytes
|
||||
// todo_erin measure perf padding to 64 bytes
|
||||
// 64 bytes
|
||||
typedef struct b3BodyState
|
||||
{
|
||||
b3Vec3 linearVelocity; // 12
|
||||
b3Vec3 linearVelocity; // 12
|
||||
float padding1;
|
||||
|
||||
b3Vec3 angularVelocity; // 12
|
||||
float padding2;
|
||||
|
||||
// Using delta position reduces round-off error far from the origin
|
||||
b3Vec3 deltaPosition; // 12
|
||||
|
||||
// Using delta rotation because I cannot access the full rotation on static bodies in
|
||||
// the solver and must use zero delta rotation for static bodies (c,s) = (1,0)
|
||||
b3Quat deltaRotation; // 16
|
||||
|
||||
// b3BodyFlags
|
||||
// Important flags: locking, dynamic
|
||||
uint32_t flags; // 4
|
||||
|
||||
// Using delta rotation because I cannot access the full rotation on static bodies in
|
||||
// the solver and must use zero delta rotation for static bodies (c,s) = (1,0)
|
||||
b3Quat deltaRotation; // 16
|
||||
} b3BodyState;
|
||||
|
||||
// Identity body state, notice the deltaRotation is identity
|
||||
static const b3BodyState b3_identityBodyState = {
|
||||
{ 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f }, { { 0.0f, 0.0f, 0.0f }, 1.0f }, 0,
|
||||
{ 0.0f, 0.0f, 0.0f }, 0.0f, { 0.0f, 0.0f, 0.0f }, 0.0f, { 0.0f, 0.0f, 0.0f }, 0, { { 0.0f, 0.0f, 0.0f }, 1.0f },
|
||||
};
|
||||
|
||||
// Body simulation data used for integration of position and velocity
|
||||
|
|
@ -193,6 +195,12 @@ typedef struct b3BodySim
|
|||
// center of mass position in world space
|
||||
b3Pos center;
|
||||
|
||||
// b3BodyFlags
|
||||
uint32_t flags;
|
||||
|
||||
float minExtent;
|
||||
b3Vec3 maxExtent;
|
||||
|
||||
// previous rotation and COM for TOI
|
||||
b3Quat rotation0;
|
||||
b3Pos center0;
|
||||
|
|
@ -210,17 +218,12 @@ typedef struct b3BodySim
|
|||
b3Matrix3 invInertiaLocal;
|
||||
b3Matrix3 invInertiaWorld;
|
||||
|
||||
float minExtent;
|
||||
b3Vec3 maxExtent;
|
||||
float linearDamping;
|
||||
float angularDamping;
|
||||
float gravityScale;
|
||||
|
||||
// Index of b3Body
|
||||
int bodyId;
|
||||
|
||||
// b3BodyFlags
|
||||
uint32_t flags;
|
||||
} b3BodySim;
|
||||
|
||||
// Get a validated body from a world using an id.
|
||||
|
|
|
|||
1352
src/contact_solver.c
1352
src/contact_solver.c
File diff suppressed because it is too large
Load diff
|
|
@ -10,9 +10,10 @@ typedef struct b3ManifoldConstraintPoint
|
|||
{
|
||||
b3Vec3 rA, rB;
|
||||
float baseSeparation;
|
||||
float restitutionVelocity;
|
||||
float relativeVelocity;
|
||||
float normalImpulse;
|
||||
float totalNormalImpulse;
|
||||
float restitutionImpulse;
|
||||
float normalMass;
|
||||
float leverArm;
|
||||
} b3ManifoldConstraintPoint;
|
||||
|
|
@ -46,6 +47,7 @@ typedef struct b3ContactConstraint
|
|||
b3Softness softness;
|
||||
b3Matrix3 rollingMass;
|
||||
float friction;
|
||||
float restitution;
|
||||
float rollingResistance;
|
||||
int manifoldCount;
|
||||
} b3ContactConstraint;
|
||||
|
|
@ -57,16 +59,19 @@ void b3PrepareContacts_Overflow( b3StepContext* context );
|
|||
void b3WarmStartContacts_Overflow( b3StepContext* context );
|
||||
void b3PushContacts_Overflow( b3StepContext* context );
|
||||
void b3SolveContacts_Overflow( b3StepContext* context );
|
||||
void b3ApplyRestitution_Overflow( b3StepContext* context );
|
||||
void b3StoreImpulses_Overflow( b3StepContext* context );
|
||||
|
||||
void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context );
|
||||
void b3WarmStartContacts_Mesh( b3SolverBlock block, b3StepContext* context );
|
||||
void b3PushContacts_Mesh( b3SolverBlock block, b3StepContext* context );
|
||||
void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context );
|
||||
void b3ApplyRestitution_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 b3PushContacts_Convex( b3SolverBlock block, b3StepContext* context );
|
||||
void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context );
|
||||
void b3ApplyRestitution_Convex( b3SolverBlock block, b3StepContext* context );
|
||||
void b3StoreImpulses_Convex( b3SolverBlock block, b3StepContext* context, int workerIndex );
|
||||
|
|
|
|||
|
|
@ -19,7 +19,7 @@
|
|||
#include <stdarg.h>
|
||||
#include <string.h>
|
||||
|
||||
#ifdef BOX2D_PROFILE
|
||||
#ifdef BOX3D_PROFILE
|
||||
|
||||
#include <tracy/TracyC.h>
|
||||
#define b3TracyCAlloc( ptr, size ) TracyCAlloc( ptr, size )
|
||||
|
|
|
|||
|
|
@ -1929,7 +1929,10 @@ int b3DynamicTree_Rebuild( b3DynamicTree* tree, bool fullBuild )
|
|||
else
|
||||
{
|
||||
b3TreeNode node = *root;
|
||||
node.flagIndex &= ~B3_MOVED_NODE;
|
||||
if ( fullBuild == false )
|
||||
{
|
||||
node.flagIndex &= ~B3_MOVED_NODE;
|
||||
}
|
||||
leafIndices[0] = 0;
|
||||
leaves[0] = node;
|
||||
#if B3_TREE_HEURISTIC == 0
|
||||
|
|
@ -1954,7 +1957,10 @@ int b3DynamicTree_Rebuild( b3DynamicTree* tree, bool fullBuild )
|
|||
continue;
|
||||
}
|
||||
|
||||
node.flagIndex &= ~B3_MOVED_NODE;
|
||||
if ( fullBuild == false )
|
||||
{
|
||||
node.flagIndex &= ~B3_MOVED_NODE;
|
||||
}
|
||||
leafIndices[leafCount] = leafCount;
|
||||
leaves[leafCount] = node;
|
||||
#if B3_TREE_HEURISTIC == 0
|
||||
|
|
@ -1977,7 +1983,10 @@ int b3DynamicTree_Rebuild( b3DynamicTree* tree, bool fullBuild )
|
|||
tree->dfsOrdered = true;
|
||||
|
||||
b3DynamicTree_Validate( tree );
|
||||
b3DynamicTree_ValidateNoMoved( tree );
|
||||
if ( fullBuild == false )
|
||||
{
|
||||
b3DynamicTree_ValidateNoMoved( tree );
|
||||
}
|
||||
|
||||
return leafCount;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -327,6 +327,8 @@ b3WorldId b3CreateWorld( const b3WorldDef* def )
|
|||
world->gravity = def->gravity;
|
||||
world->hitEventThreshold = def->hitEventThreshold;
|
||||
world->restitutionThreshold = def->restitutionThreshold;
|
||||
world->restitutionIterations = b3ClampInt( def->restitutionIterations, 0, B3_MAX_RESTITUTION_ITERATIONS );
|
||||
world->enableRestitutionPropagation = def->enableRestitutionPropagation;
|
||||
world->maxLinearSpeed = def->maximumLinearSpeed;
|
||||
world->contactSpeed = def->contactSpeed;
|
||||
world->contactHertz = def->contactHertz;
|
||||
|
|
@ -714,6 +716,7 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void*
|
|||
b3Vec3 rB = b3MulMV( matrixB, mp->anchorB );
|
||||
b3Vec3 dp = b3Add( dc, b3Sub( rB, rA ) );
|
||||
mp->separation = mp->baseSeparation + b3Dot( dp, normal );
|
||||
mp->normalVelocity = 0.0f;
|
||||
mp->persisted = true;
|
||||
}
|
||||
}
|
||||
|
|
@ -1125,7 +1128,6 @@ void b3World_Step( b3WorldId worldId, float timeStep, int subStepCount )
|
|||
context.contactSoftness = b3MakeSoft( contactHertz, world->contactDampingRatio, context.h );
|
||||
context.staticSoftness = b3MakeSoft( 2.0f * contactHertz, 0.5f * world->contactDampingRatio, context.h );
|
||||
|
||||
context.restitutionThreshold = world->restitutionThreshold;
|
||||
context.maxLinearVelocity = world->maxLinearSpeed;
|
||||
context.enableWarmStarting = world->enableWarmStarting;
|
||||
|
||||
|
|
@ -1261,7 +1263,7 @@ static bool DrawQueryCallback( int proxyId, uint64_t userData, void* context )
|
|||
{
|
||||
rgb = b3_colorWheat;
|
||||
}
|
||||
else if ( body->flags & b3_hadTimeOfImpact )
|
||||
else if ( bodySim->flags & b3_hadTimeOfImpact )
|
||||
{
|
||||
rgb = b3_colorLime;
|
||||
}
|
||||
|
|
@ -2079,6 +2081,55 @@ float b3World_GetRestitutionThreshold( b3WorldId worldId )
|
|||
return world->restitutionThreshold;
|
||||
}
|
||||
|
||||
void b3World_SetRestitutionIterations( b3WorldId worldId, int iterations )
|
||||
{
|
||||
b3World* world = b3GetUnlockedWorldFromId( worldId );
|
||||
if ( world == NULL )
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
iterations = b3ClampInt( iterations, 0, B3_MAX_RESTITUTION_ITERATIONS );
|
||||
if ( iterations == world->restitutionIterations )
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
B3_REC( world, WorldSetRestitutionIterations, worldId, iterations );
|
||||
|
||||
world->restitutionIterations = iterations;
|
||||
}
|
||||
|
||||
int b3World_GetRestitutionIterations( b3WorldId worldId )
|
||||
{
|
||||
b3World* world = b3GetWorldFromId( worldId );
|
||||
return world->restitutionIterations;
|
||||
}
|
||||
|
||||
void b3World_EnableRestitutionPropagation( b3WorldId worldId, bool flag )
|
||||
{
|
||||
b3World* world = b3GetUnlockedWorldFromId( worldId );
|
||||
if ( world == NULL )
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if ( flag == world->enableRestitutionPropagation )
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
B3_REC( world, WorldEnableRestitutionPropagation, worldId, flag );
|
||||
|
||||
world->enableRestitutionPropagation = flag;
|
||||
}
|
||||
|
||||
bool b3World_IsRestitutionPropagationEnabled( b3WorldId worldId )
|
||||
{
|
||||
b3World* world = b3GetWorldFromId( worldId );
|
||||
return world->enableRestitutionPropagation;
|
||||
}
|
||||
|
||||
void b3World_SetHitEventThreshold( b3WorldId worldId, float value )
|
||||
{
|
||||
b3World* world = b3GetUnlockedWorldFromId( worldId );
|
||||
|
|
|
|||
|
|
@ -235,6 +235,8 @@ typedef struct b3World
|
|||
b3Vec3 gravity;
|
||||
float hitEventThreshold;
|
||||
float restitutionThreshold;
|
||||
int restitutionIterations;
|
||||
bool enableRestitutionPropagation;
|
||||
float maxLinearSpeed;
|
||||
float contactSpeed;
|
||||
float contactHertz;
|
||||
|
|
@ -301,6 +303,8 @@ b3World* b3GetWorldFromId( b3WorldId id );
|
|||
b3World* b3GetUnlockedWorld( int index );
|
||||
b3World* b3GetWorld( int index );
|
||||
|
||||
void b3World_RebuildStaticTree( b3WorldId worldId );
|
||||
|
||||
void b3ValidateConnectivity( b3World* world );
|
||||
void b3ValidateSolverSets( b3World* world );
|
||||
void b3ValidateContacts( b3World* world );
|
||||
|
|
|
|||
|
|
@ -307,8 +307,8 @@ void b3PreparePrismaticJoint( b3JointSim* base, b3StepContext* context )
|
|||
b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA );
|
||||
b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB );
|
||||
|
||||
B3_ASSERT( bodyB->setIndex == b3_awakeSet );
|
||||
b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex );
|
||||
B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet );
|
||||
b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex );
|
||||
b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex );
|
||||
|
||||
int localIndexA = bodyA->localIndex;
|
||||
|
|
|
|||
|
|
@ -52,7 +52,7 @@ typedef struct b3World b3World;
|
|||
|
||||
// Minor tracks op-stream additions that keep the 48 byte header shape.
|
||||
// Minor version 7 made the contact body sim locators persistent
|
||||
#define B3_REC_VERSION_MINOR 7
|
||||
#define B3_REC_VERSION_MINOR 8
|
||||
|
||||
// File header, fixed 48 bytes. Contains the registry locator so the player
|
||||
// can load geometry before replaying any ops.
|
||||
|
|
|
|||
|
|
@ -38,6 +38,8 @@ B3_REC_OP( 0x0A, WorldSetMaximumLinearSpeed, RET_NONE, ARG( WORLDID, world ) ARG
|
|||
B3_REC_OP( 0x0B, WorldEnableWarmStarting, RET_NONE, ARG( WORLDID, world ) ARG( BOOL, flag ) )
|
||||
B3_REC_OP( 0x0C, WorldRebuildStaticTree, RET_NONE, ARG( WORLDID, world ) )
|
||||
B3_REC_OP( 0x0D, WorldEnableSpeculative, RET_NONE, ARG( WORLDID, world ) ARG( BOOL, flag ) )
|
||||
B3_REC_OP( 0x0E, WorldSetRestitutionIterations, RET_NONE, ARG( WORLDID, world ) ARG( I32, iterations ) )
|
||||
B3_REC_OP( 0x0F, WorldEnableRestitutionPropagation, RET_NONE, ARG( WORLDID, world ) ARG( BOOL, flag ) )
|
||||
|
||||
// Body
|
||||
B3_REC_OP( 0x10, CreateBody, RET_BODYID, ARG( WORLDID, world ) ARG( BODYDEF, def ) )
|
||||
|
|
|
|||
|
|
@ -819,6 +819,17 @@ static void b3RecDispatch_WorldEnableSpeculative( const b3RecArgs_WorldEnableSpe
|
|||
b3World_EnableSpeculative( rdr->replayWorldId, a->flag );
|
||||
}
|
||||
|
||||
static void b3RecDispatch_WorldSetRestitutionIterations( const b3RecArgs_WorldSetRestitutionIterations* a, b3RecReader* rdr )
|
||||
{
|
||||
b3World_SetRestitutionIterations( rdr->replayWorldId, a->iterations );
|
||||
}
|
||||
|
||||
static void b3RecDispatch_WorldEnableRestitutionPropagation( const b3RecArgs_WorldEnableRestitutionPropagation* a,
|
||||
b3RecReader* rdr )
|
||||
{
|
||||
b3World_EnableRestitutionPropagation( rdr->replayWorldId, a->flag );
|
||||
}
|
||||
|
||||
static void b3RecDispatch_CreateBody( const b3RecArgs_CreateBody* a, b3RecReader* rdr )
|
||||
{
|
||||
b3BodyId recId = b3RecR_BODYID( rdr );
|
||||
|
|
|
|||
|
|
@ -122,7 +122,7 @@ b3Scheduler* b3CreateScheduler( int workerCount )
|
|||
scheduler->workerContexts[i].threadIndex = i + 1;
|
||||
|
||||
char name[16];
|
||||
snprintf( name, sizeof( name ), "box2d_worker_%02d", i + 1 );
|
||||
snprintf( name, sizeof( name ), "box3d_worker_%02d", i + 1 );
|
||||
scheduler->threads[i] = b3CreateThread( b3SchedulerWorkerMain, scheduler->workerContexts + i, name );
|
||||
}
|
||||
|
||||
|
|
|
|||
58
src/simd.h
58
src/simd.h
|
|
@ -412,11 +412,26 @@ static inline void b3StoreW( float* data, b3FloatW a )
|
|||
vst1q_f32( data, a );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3UnpackLoW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return vzip1q_f32( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3UnpackHiW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return vzip2q_f32( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3NegW( b3FloatW a )
|
||||
{
|
||||
return vnegq_f32( a );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3AbsW( b3FloatW a )
|
||||
{
|
||||
return vabsq_f32( a );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return vaddq_f32( a, b );
|
||||
|
|
@ -589,6 +604,16 @@ static inline void b3StoreW( float* data, b3FloatW a )
|
|||
_mm_storeu_ps( data, a );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3UnpackLoW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return _mm_unpacklo_ps( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3UnpackHiW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return _mm_unpackhi_ps( a, b );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3NegW( b3FloatW a )
|
||||
{
|
||||
// Create a mask with the sign bit set for each element
|
||||
|
|
@ -598,6 +623,11 @@ static inline b3FloatW b3NegW( b3FloatW a )
|
|||
return _mm_xor_ps( a, mask );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3AbsW( b3FloatW a )
|
||||
{
|
||||
return _mm_andnot_ps( _mm_set1_ps( -0.0f ), a );
|
||||
}
|
||||
|
||||
static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return _mm_add_ps( a, b );
|
||||
|
|
@ -772,11 +802,26 @@ static inline void b3StoreW( float* data, b3FloatW a )
|
|||
data[3] = a.w;
|
||||
}
|
||||
|
||||
static inline b3FloatW b3UnpackLoW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return (b3FloatW){ a.x, b.x, a.y, b.y };
|
||||
}
|
||||
|
||||
static inline b3FloatW b3UnpackHiW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return (b3FloatW){ a.z, b.z, a.w, b.w };
|
||||
}
|
||||
|
||||
static inline b3FloatW b3NegW( b3FloatW a )
|
||||
{
|
||||
return (b3FloatW){ -a.x, -a.y, -a.z, -a.w };
|
||||
}
|
||||
|
||||
static inline b3FloatW b3AbsW( b3FloatW a )
|
||||
{
|
||||
return (b3FloatW){ a.x < 0.0f ? -a.x : a.x, a.y < 0.0f ? -a.y : a.y, a.z < 0.0f ? -a.z : a.z, a.w < 0.0f ? -a.w : a.w };
|
||||
}
|
||||
|
||||
static inline b3FloatW b3AddW( b3FloatW a, b3FloatW b )
|
||||
{
|
||||
return (b3FloatW){ a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w };
|
||||
|
|
@ -976,6 +1021,19 @@ static inline int b3MinIndexW( b3FloatW a, int bitCount )
|
|||
|
||||
#endif
|
||||
|
||||
static inline void b3TransposeW( b3FloatW r0, b3FloatW r1, b3FloatW r2, b3FloatW r3, b3FloatW* c0, b3FloatW* c1, b3FloatW* c2,
|
||||
b3FloatW* c3 )
|
||||
{
|
||||
b3FloatW t0 = b3UnpackLoW( r0, r2 );
|
||||
b3FloatW t1 = b3UnpackLoW( r1, r3 );
|
||||
b3FloatW t2 = b3UnpackHiW( r0, r2 );
|
||||
b3FloatW t3 = b3UnpackHiW( r1, r3 );
|
||||
*c0 = b3UnpackLoW( t0, t1 );
|
||||
*c1 = b3UnpackHiW( t0, t1 );
|
||||
*c2 = b3UnpackLoW( t2, t3 );
|
||||
*c3 = b3UnpackHiW( t2, t3 );
|
||||
}
|
||||
|
||||
#if defined( B3_SIMD_NEON )
|
||||
|
||||
typedef struct b3AABBV
|
||||
|
|
|
|||
51
src/solver.c
51
src/solver.c
|
|
@ -382,7 +382,7 @@ static bool b3ContinuousQueryCallback( int proxyId, uint64_t userData, void* con
|
|||
B3_ASSERT( body->type == b3_staticBody || ( fastBodySim->flags & b3_isBullet ) );
|
||||
|
||||
// Skip bullets
|
||||
if ( bodySim->flags & b3_isBullet )
|
||||
if ( body->flags & b3_isBullet )
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
|
@ -450,7 +450,6 @@ static bool b3ContinuousQueryCallback( int proxyId, uint64_t userData, void* con
|
|||
|
||||
if ( didHit )
|
||||
{
|
||||
fastBodySim->flags |= b3_hadTimeOfImpact;
|
||||
continuousContext->fraction = output.fraction;
|
||||
continuousContext->distanceIterations = b3MaxInt( continuousContext->distanceIterations, output.distanceIterations );
|
||||
continuousContext->pushBackIterations = b3MaxInt( continuousContext->pushBackIterations, output.pushBackIterations );
|
||||
|
|
@ -564,6 +563,7 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext*
|
|||
fastBodySim->center = center;
|
||||
fastBodySim->rotation0 = q;
|
||||
fastBodySim->center0 = center;
|
||||
fastBodySim->flags |= b3_hadTimeOfImpact;
|
||||
|
||||
// Timeloss means there is a lost gravity contribution. Other forces and torques are ignored for now.
|
||||
b3BodyState* fastBodyState = b3Array_Get( awakeSet->bodyStates, bodySimIndex );
|
||||
|
|
@ -1043,6 +1043,17 @@ static void b3ExecuteBlock( b3SolverStage* stage, b3StepContext* context, b3Solv
|
|||
}
|
||||
break;
|
||||
|
||||
case b3_stageRestitution:
|
||||
if ( blockType == b3_graphWideContactBlock )
|
||||
{
|
||||
b3ApplyRestitution_Convex( block, context );
|
||||
}
|
||||
else if ( blockType == b3_graphContactBlock )
|
||||
{
|
||||
b3ApplyRestitution_Mesh( block, context );
|
||||
}
|
||||
break;
|
||||
|
||||
case b3_stageStoreWideImpulses:
|
||||
b3StoreImpulses_Convex( block, context, workerIndex );
|
||||
break;
|
||||
|
|
@ -1305,6 +1316,29 @@ static void b3SolverTask( void* taskContext )
|
|||
// integrate velocities / warm start / solve / integrate positions / relax
|
||||
stageIndex += 1 + activeColorCount + ITERATIONS * activeColorCount + 1 + RELAX_ITERATIONS * activeColorCount;
|
||||
|
||||
int restitutionIterations = context->world->restitutionIterations;
|
||||
if ( restitutionIterations > 0 && b3AtomicLoadInt( &context->anyRestitution ) != 0 )
|
||||
{
|
||||
for ( int j = 0; j < restitutionIterations; ++j )
|
||||
{
|
||||
b3ApplyRestitution_Overflow( context );
|
||||
|
||||
int iterationStageIndex = stageIndex;
|
||||
for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex )
|
||||
{
|
||||
syncBits = ( graphSyncIndex << 16 ) | iterationStageIndex;
|
||||
B3_ASSERT( stages[iterationStageIndex].type == b3_stageRestitution );
|
||||
b3ExecuteMainStage( stages + iterationStageIndex, context, syncBits );
|
||||
iterationStageIndex += 1;
|
||||
}
|
||||
graphSyncIndex += 1;
|
||||
}
|
||||
|
||||
profile->restitution += b3GetMillisecondsAndReset( &ticks );
|
||||
}
|
||||
|
||||
stageIndex += activeColorCount;
|
||||
|
||||
// Store impulses
|
||||
b3StoreImpulses_Overflow( context );
|
||||
|
||||
|
|
@ -1584,15 +1618,6 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
|
|||
|
||||
int colorContactCountW = ( ( colorConvexContactCount - 1 ) >> B3_SIMD_SHIFT ) + 1;
|
||||
color->wideConstraintCount = colorContactCountW;
|
||||
|
||||
// Zero remainder lanes in the tail wide slot so prepare workers don't need to
|
||||
// initialize them.
|
||||
if ( ( colorConvexContactCount & ( B3_SIMD_WIDTH - 1 ) ) != 0 )
|
||||
{
|
||||
memset( (uint8_t*)color->wideConstraints + ( colorContactCountW - 1 ) * wideContactByteCount, 0,
|
||||
wideContactByteCount );
|
||||
}
|
||||
|
||||
wideBase += colorContactCountW;
|
||||
}
|
||||
|
||||
|
|
@ -1663,6 +1688,8 @@ void b3Solve( b3World* world, b3StepContext* stepContext )
|
|||
stageCount += 1;
|
||||
// b3_stageRelax
|
||||
stageCount += RELAX_ITERATIONS * activeColorCount;
|
||||
// b3_stageRestitution
|
||||
stageCount += activeColorCount;
|
||||
// b3_stageStoreWideImpulses
|
||||
stageCount += 1;
|
||||
// b3_stageStoreImpulses
|
||||
|
|
@ -1746,6 +1773,8 @@ 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 );
|
||||
stage = b3InitColorStages( stage, b3_stageRestitution, 1, activeColorCount, graphColorBlocks, graphBlockCounts,
|
||||
activeColorIndices );
|
||||
stage = b3InitStage( stage, b3_stageStoreWideImpulses, convexBlocks, convexPrepareDim.count, UINT8_MAX );
|
||||
stage = b3InitStage( stage, b3_stageStoreImpulses, meshBlocks, meshPrepareDim.count, UINT8_MAX );
|
||||
|
||||
|
|
|
|||
|
|
@ -76,6 +76,7 @@ typedef enum b3SolverStageType
|
|||
b3_stageSolve,
|
||||
b3_stageIntegratePositions,
|
||||
b3_stageRelax,
|
||||
b3_stageRestitution,
|
||||
b3_stageStoreWideImpulses,
|
||||
b3_stageStoreImpulses,
|
||||
} b3SolverStageType;
|
||||
|
|
@ -188,7 +189,6 @@ typedef struct b3StepContext
|
|||
b3Softness contactSoftness;
|
||||
b3Softness staticSoftness;
|
||||
|
||||
float restitutionThreshold;
|
||||
float maxLinearVelocity;
|
||||
|
||||
struct b3World* world;
|
||||
|
|
@ -208,6 +208,8 @@ typedef struct b3StepContext
|
|||
int* bulletBodies;
|
||||
b3AtomicInt bulletBodyCount;
|
||||
|
||||
b3AtomicInt anyRestitution;
|
||||
|
||||
// Contact ids for simplified parallel-for access. Used in narrow-phase.
|
||||
// These contacts may or may not be touching. They are associated with awake bodies.
|
||||
int* awakeContactIndices;
|
||||
|
|
|
|||
|
|
@ -16,6 +16,8 @@ b3WorldDef b3DefaultWorldDef( void )
|
|||
def.gravity.y = -10.0f;
|
||||
def.hitEventThreshold = 1.0f * lengthUnits;
|
||||
def.restitutionThreshold = 1.0f * lengthUnits;
|
||||
def.restitutionIterations = 2;
|
||||
def.enableRestitutionPropagation = false;
|
||||
def.contactSpeed = 3.0f * lengthUnits;
|
||||
def.contactHertz = 30.0f;
|
||||
def.contactDampingRatio = 10.0f;
|
||||
|
|
|
|||
|
|
@ -91,7 +91,7 @@ void b3PrepareWeldJoint( b3JointSim* base, b3StepContext* context )
|
|||
b3Body* bodyA = b3Array_Get( world->bodies, base->bodyIdA );
|
||||
b3Body* bodyB = b3Array_Get( world->bodies, base->bodyIdB );
|
||||
|
||||
B3_ASSERT( bodyB->setIndex == b3_awakeSet );
|
||||
B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyB->setIndex == b3_awakeSet );
|
||||
b3SolverSet* setA = b3Array_Get( world->solverSets, bodyA->setIndex );
|
||||
b3SolverSet* setB = b3Array_Get( world->solverSets, bodyB->setIndex );
|
||||
|
||||
|
|
|
|||
|
|
@ -33,7 +33,7 @@
|
|||
|
||||
// Snapshot image magic 'BNS3' and version
|
||||
#define B3_SNAP_MAGIC 0x33534E42u
|
||||
#define B3_SNAP_VERSION 7u // same-step restitution
|
||||
#define B3_SNAP_VERSION 10u // post solve restitution
|
||||
|
||||
#define B3_SNAP_FLAG_VALIDATION 0x1u
|
||||
#define B3_SNAP_FLAG_DOUBLE_PRECISION 0x2u
|
||||
|
|
@ -470,6 +470,7 @@ static void b3SerWorldConfig( b3RecBuffer* buf, const b3World* world )
|
|||
b3SnapW_Bytes( buf, &world->gravity, sizeof( b3Vec3 ) );
|
||||
b3SnapW_Bytes( buf, &world->hitEventThreshold, sizeof( float ) );
|
||||
b3SnapW_Bytes( buf, &world->restitutionThreshold, sizeof( float ) );
|
||||
b3SnapW_I32( buf, world->restitutionIterations );
|
||||
b3SnapW_Bytes( buf, &world->maxLinearSpeed, sizeof( float ) );
|
||||
b3SnapW_Bytes( buf, &world->contactSpeed, sizeof( float ) );
|
||||
b3SnapW_Bytes( buf, &world->contactHertz, sizeof( float ) );
|
||||
|
|
@ -487,6 +488,7 @@ static void b3SerWorldConfig( b3RecBuffer* buf, const b3World* world )
|
|||
flags |= world->enableWarmStarting ? 0x02u : 0u;
|
||||
flags |= world->enableContinuous ? 0x04u : 0u;
|
||||
flags |= world->enableSpeculative ? 0x08u : 0u;
|
||||
flags |= world->enableRestitutionPropagation ? 0x10u : 0u;
|
||||
b3RecBufAppend( buf, &flags, 1 );
|
||||
}
|
||||
|
||||
|
|
@ -495,6 +497,7 @@ static void b3DesWorldConfig( b3SnapReader* r, b3World* world )
|
|||
b3SnapR_Bytes( r, &world->gravity, sizeof( b3Vec3 ) );
|
||||
b3SnapR_Bytes( r, &world->hitEventThreshold, sizeof( float ) );
|
||||
b3SnapR_Bytes( r, &world->restitutionThreshold, sizeof( float ) );
|
||||
world->restitutionIterations = b3SnapR_I32( r );
|
||||
b3SnapR_Bytes( r, &world->maxLinearSpeed, sizeof( float ) );
|
||||
b3SnapR_Bytes( r, &world->contactSpeed, sizeof( float ) );
|
||||
b3SnapR_Bytes( r, &world->contactHertz, sizeof( float ) );
|
||||
|
|
@ -513,6 +516,7 @@ static void b3DesWorldConfig( b3SnapReader* r, b3World* world )
|
|||
world->enableWarmStarting = ( flags & 0x02u ) != 0;
|
||||
world->enableContinuous = ( flags & 0x04u ) != 0;
|
||||
world->enableSpeculative = ( flags & 0x08u ) != 0;
|
||||
world->enableRestitutionPropagation = ( flags & 0x10u ) != 0;
|
||||
}
|
||||
|
||||
// Shapes carry pointer fields: materials, userData, userShape, and the geometry union.
|
||||
|
|
@ -628,7 +632,7 @@ static void b3DesShapes( b3SnapReader* r, b3World* world, b3RecReader* rdr )
|
|||
// shape with the same geometry. Carrying its handle over avoids tearing down and rebuilding every
|
||||
// GPU mesh on each seek, which the host (a 3D renderer) would otherwise pay for. Handles that are
|
||||
// not reclaimed below belong to shapes that are gone or were replaced, and get released so the host
|
||||
// pool does not leak across seeks. Box2D has no such handles, so its restore skips all of this.
|
||||
// pool does not leak across seeks.
|
||||
int oldShapeCount = world->shapes.count;
|
||||
void** savedUserShape = NULL;
|
||||
uint16_t* savedGeneration = NULL;
|
||||
|
|
|
|||
Loading…
Reference in a new issue