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
|
|
@ -1,9 +1,9 @@
|
|||
threads,ms
|
||||
1,14441.9
|
||||
2,7238.43
|
||||
3,4870.13
|
||||
4,3697.34
|
||||
5,2998.67
|
||||
6,2533.13
|
||||
7,2209.12
|
||||
8,1983.89
|
||||
1,13564.7
|
||||
2,7058.44
|
||||
3,4749.26
|
||||
4,3596.92
|
||||
5,2921.35
|
||||
6,2478.9
|
||||
7,2163.02
|
||||
8,1919.1
|
||||
|
|
|
|||
|
|
|
@ -1,9 +1,9 @@
|
|||
threads,ms
|
||||
1,1323.84
|
||||
2,691.063
|
||||
3,463.698
|
||||
4,353.723
|
||||
5,284.09
|
||||
6,240.864
|
||||
7,209.11
|
||||
8,185.953
|
||||
1,1341.05
|
||||
2,697.405
|
||||
3,467.98
|
||||
4,355.631
|
||||
5,285.912
|
||||
6,240.761
|
||||
7,210.048
|
||||
8,187.1
|
||||
|
|
|
|||
|
|
|
@ -1,9 +1,9 @@
|
|||
threads,ms
|
||||
1,16467.5
|
||||
2,8687.63
|
||||
3,5966.06
|
||||
4,4569.55
|
||||
5,3727.89
|
||||
6,3175.51
|
||||
7,2801.72
|
||||
8,2512.32
|
||||
1,14783.1
|
||||
2,7859.85
|
||||
3,5367.48
|
||||
4,4115.03
|
||||
5,3368.76
|
||||
6,2874.23
|
||||
7,2535.89
|
||||
8,2277.2
|
||||
|
|
|
|||
|
|
|
@ -1,9 +1,9 @@
|
|||
threads,ms
|
||||
1,1993.6
|
||||
2,1049.01
|
||||
3,712.917
|
||||
4,544.335
|
||||
5,443.056
|
||||
6,384.22
|
||||
7,331.626
|
||||
8,297.982
|
||||
1,1852.7
|
||||
2,978.38
|
||||
3,665.353
|
||||
4,505.116
|
||||
5,411.56
|
||||
6,356.776
|
||||
7,307.866
|
||||
8,276.957
|
||||
|
|
|
|||
|
|
|
@ -1,9 +1,9 @@
|
|||
threads,ms
|
||||
1,7.6329
|
||||
2,9.7115
|
||||
3,10.7648
|
||||
4,11.2398
|
||||
5,12.029
|
||||
6,12.1204
|
||||
7,12.2289
|
||||
8,12.5125
|
||||
1,7.1447
|
||||
2,9.0736
|
||||
3,10.1672
|
||||
4,10.5088
|
||||
5,11.0298
|
||||
6,11.2858
|
||||
7,11.378
|
||||
8,11.4947
|
||||
|
|
|
|||
|
|
|
@ -1,9 +1,9 @@
|
|||
threads,ms
|
||||
1,2075.59
|
||||
2,1084.54
|
||||
3,722.057
|
||||
4,549.644
|
||||
5,437.772
|
||||
6,372.242
|
||||
7,319.13
|
||||
8,283.131
|
||||
1,1947.41
|
||||
2,1002.95
|
||||
3,663.846
|
||||
4,499.942
|
||||
5,405.97
|
||||
6,336.977
|
||||
7,291.483
|
||||
8,255.782
|
||||
|
|
|
|||
|
|
|
@ -1,9 +1,9 @@
|
|||
threads,ms
|
||||
1,2102.47
|
||||
2,1147.85
|
||||
3,803.547
|
||||
4,630.96
|
||||
5,524.168
|
||||
6,452.1
|
||||
7,404.499
|
||||
8,370.325
|
||||
1,2061.72
|
||||
2,1139.07
|
||||
3,794.05
|
||||
4,622.555
|
||||
5,516.441
|
||||
6,446.451
|
||||
7,399.084
|
||||
8,364.345
|
||||
|
|
|
|||
|
|
|
@ -1,9 +1,9 @@
|
|||
threads,ms
|
||||
1,1144.18
|
||||
2,613.407
|
||||
3,423.032
|
||||
4,329.512
|
||||
5,272.901
|
||||
6,239.65
|
||||
7,216.5
|
||||
8,200.217
|
||||
1,1051.05
|
||||
2,568.185
|
||||
3,390.055
|
||||
4,304.593
|
||||
5,251.978
|
||||
6,221.25
|
||||
7,200.171
|
||||
8,185.485
|
||||
|
|
|
|||
|
|
|
@ -1,9 +1,9 @@
|
|||
threads,ms
|
||||
1,3173.87
|
||||
2,1841.25
|
||||
3,1325.07
|
||||
4,1061.04
|
||||
5,902.193
|
||||
6,803.715
|
||||
7,734.266
|
||||
8,689.781
|
||||
1,3152.3
|
||||
2,1832.83
|
||||
3,1315.75
|
||||
4,1058.49
|
||||
5,903.367
|
||||
6,801.254
|
||||
7,738.93
|
||||
8,695.326
|
||||
|
|
|
|||
|
|
|
@ -1,9 +1,9 @@
|
|||
threads,ms
|
||||
1,170.276
|
||||
2,112.037
|
||||
3,88.2708
|
||||
4,78.4451
|
||||
5,74.3577
|
||||
6,71.868
|
||||
7,70.6687
|
||||
8,69.1284
|
||||
1,170.933
|
||||
2,112.916
|
||||
3,89.6041
|
||||
4,79.6461
|
||||
5,75.7754
|
||||
6,73.2911
|
||||
7,71.7742
|
||||
8,70.7292
|
||||
|
|
|
|||
|
|
|
@ -1,9 +1,9 @@
|
|||
threads,ms
|
||||
1,673.356
|
||||
2,395.07
|
||||
3,291.336
|
||||
4,249.378
|
||||
5,225.709
|
||||
6,211.1
|
||||
7,201.452
|
||||
8,194.379
|
||||
1,680.981
|
||||
2,400.129
|
||||
3,296.389
|
||||
4,252.676
|
||||
5,229.879
|
||||
6,214.117
|
||||
7,204.083
|
||||
8,196.839
|
||||
|
|
|
|||
|
|
|
@ -1,9 +1,9 @@
|
|||
threads,ms
|
||||
1,285.24
|
||||
2,174.635
|
||||
3,132.161
|
||||
4,116.509
|
||||
5,107.948
|
||||
6,102.503
|
||||
7,98.9163
|
||||
8,96.5196
|
||||
1,295.508
|
||||
2,183.122
|
||||
3,139.634
|
||||
4,121.466
|
||||
5,113.262
|
||||
6,106.697
|
||||
7,103.854
|
||||
8,101.959
|
||||
|
|
|
|||
|
|
|
@ -1,9 +1,9 @@
|
|||
threads,ms
|
||||
1,21430.4
|
||||
2,11407.7
|
||||
3,7908.09
|
||||
4,6102.55
|
||||
5,5012.68
|
||||
6,4272.59
|
||||
7,3783.33
|
||||
8,3415.5
|
||||
1,19931.1
|
||||
2,10626.7
|
||||
3,7300.85
|
||||
4,5627.16
|
||||
5,4603.18
|
||||
6,3931.89
|
||||
7,3454.58
|
||||
8,3273.54
|
||||
|
|
|
|||
|
|
|
@ -188,7 +188,6 @@ int main( int argc, char** argv )
|
|||
.solveImpulses = FLT_MAX,
|
||||
.integratePositions = FLT_MAX,
|
||||
.relaxImpulses = FLT_MAX,
|
||||
.applyRestitution = FLT_MAX,
|
||||
.storeImpulses = FLT_MAX,
|
||||
.splitIslands = FLT_MAX,
|
||||
.transforms = FLT_MAX,
|
||||
|
|
|
|||
1
data/hulls/axe.txt
Normal file
1
data/hulls/axe.txt
Normal file
File diff suppressed because one or more lines are too long
|
|
@ -4,6 +4,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
|
||||
// Compile-time options. Edit box3d/config.h, or define BOX3D_USER_CONFIG to
|
||||
|
|
@ -93,11 +94,12 @@
|
|||
/// Prototype for user allocation function.
|
||||
/// @param size the allocation size in bytes
|
||||
/// @param alignment the required alignment, guaranteed to be a power of 2
|
||||
typedef void* b3AllocFcn( int32_t size, int32_t alignment );
|
||||
typedef void* b3AllocFcn( size_t size, int32_t alignment );
|
||||
|
||||
/// Prototype for user free function.
|
||||
/// @param mem the memory previously allocated through `b3AllocFcn`
|
||||
typedef void b3FreeFcn( void* mem );
|
||||
/// @param size the size of the allocation in bytes.
|
||||
typedef void b3FreeFcn( void* mem, size_t size );
|
||||
|
||||
/// Prototype for the user assert callback. Return 0 to skip the debugger break.
|
||||
typedef int b3AssertFcn( const char* condition, const char* fileName, int lineNumber );
|
||||
|
|
@ -110,7 +112,7 @@ typedef void b3LogFcn( const char* message );
|
|||
B3_API void b3SetAllocator( b3AllocFcn* allocFcn, b3FreeFcn* freeFcn );
|
||||
|
||||
/// Total bytes allocated by Box3D
|
||||
B3_API int b3GetByteCount( void );
|
||||
B3_API int64_t b3GetByteCount( void );
|
||||
|
||||
/// Override the default assert callback.
|
||||
/// @param assertFcn a non-null assert callback
|
||||
|
|
|
|||
|
|
@ -31,5 +31,3 @@
|
|||
|
||||
// Enable assertions when NDEBUG is defined
|
||||
// #define B3_ENABLE_ASSERT
|
||||
|
||||
// #define B3_RESTITUTION_ITERATIONS
|
||||
|
|
|
|||
|
|
@ -141,11 +141,6 @@ B3_API float b3GetStallThreshold( void );
|
|||
#define B3_MAX_SHAPES ( 1 << B3_SHAPE_POWER )
|
||||
#define B3_MAX_CHILD_SHAPES ( 1 << B3_CHILD_POWER )
|
||||
|
||||
/// Increase this if your application needs more accurate restitution. Doing so will
|
||||
/// slow down the simulation. Must be 1 or more.
|
||||
#ifndef B3_RESTITUTION_ITERATIONS
|
||||
#define B3_RESTITUTION_ITERATIONS 1
|
||||
#endif
|
||||
|
||||
/// This is the limit on how many mesh or heightfield triangles a single convex shape can collide with.
|
||||
/// Increasing this will increase stack usage, so be careful. I recommend to simplify your collision data
|
||||
|
|
|
|||
|
|
@ -537,69 +537,70 @@ typedef struct b3ShapeDef
|
|||
/// @ingroup shape
|
||||
B3_API b3ShapeDef b3DefaultShapeDef( void );
|
||||
|
||||
//! @cond
|
||||
/// Profiling data. Times are in milliseconds.
|
||||
/// @ingroup world
|
||||
typedef struct b3Profile
|
||||
{
|
||||
float step;
|
||||
float pairs;
|
||||
float collide;
|
||||
float solve;
|
||||
float solverSetup;
|
||||
float constraints;
|
||||
float prepareConstraints;
|
||||
float integrateVelocities;
|
||||
float warmStart;
|
||||
float solveImpulses;
|
||||
float integratePositions;
|
||||
float relaxImpulses;
|
||||
float applyRestitution;
|
||||
float storeImpulses;
|
||||
float splitIslands;
|
||||
float transforms;
|
||||
float sensorHits;
|
||||
float jointEvents;
|
||||
float hitEvents;
|
||||
float refit;
|
||||
float bullets;
|
||||
float sleepIslands;
|
||||
float sensors;
|
||||
float step; //
|
||||
float pairs; //
|
||||
float collide; //
|
||||
float solve; //
|
||||
float solverSetup; //
|
||||
float constraints; //
|
||||
float prepareConstraints; //
|
||||
float integrateVelocities; //
|
||||
float warmStart; //
|
||||
float solveImpulses; //
|
||||
float integratePositions; //
|
||||
float relaxImpulses; //
|
||||
float storeImpulses; //
|
||||
float splitIslands; //
|
||||
float transforms; //
|
||||
float sensorHits; //
|
||||
float jointEvents; //
|
||||
float hitEvents; //
|
||||
float refit; //
|
||||
float bullets; //
|
||||
float sleepIslands; //
|
||||
float sensors; //
|
||||
} b3Profile;
|
||||
|
||||
/// Counters that give details of the simulation size.
|
||||
/// @ingroup world
|
||||
typedef struct b3Counters
|
||||
{
|
||||
int bodyCount;
|
||||
int shapeCount;
|
||||
int contactCount;
|
||||
int jointCount;
|
||||
int islandCount;
|
||||
int stackUsed;
|
||||
int arenaCapacity;
|
||||
int staticTreeHeight;
|
||||
int treeHeight;
|
||||
int satCallCount;
|
||||
int satCacheHitCount;
|
||||
int byteCount;
|
||||
int taskCount;
|
||||
int colorCounts[24];
|
||||
int manifoldCounts[B3_CONTACT_MANIFOLD_COUNT_BUCKETS];
|
||||
int64_t byteCount; //
|
||||
int bodyCount; //
|
||||
int shapeCount; //
|
||||
int contactCount; //
|
||||
int jointCount; //
|
||||
int islandCount; //
|
||||
int stackUsed; //
|
||||
int arenaCapacity; //
|
||||
int staticTreeHeight; //
|
||||
int treeHeight; //
|
||||
int satCallCount; //
|
||||
int satCacheHitCount; //
|
||||
int taskCount; //
|
||||
int colorCounts[24]; //
|
||||
int manifoldCounts[B3_CONTACT_MANIFOLD_COUNT_BUCKETS]; //
|
||||
|
||||
/// Number of contacts touched by the collide pass
|
||||
/// graph contacts + awake-set non-touching
|
||||
int awakeContactCount;
|
||||
int awakeContactCount; //
|
||||
|
||||
/// Number of contacts recycled in the most recent step.
|
||||
int recycledContactCount;
|
||||
|
||||
/// Maximum number of time of impact iterations
|
||||
/// Maximum number of time of impact outer iterations
|
||||
int distanceIterations;
|
||||
|
||||
/// Maximum TOI push backs.
|
||||
int pushBackIterations;
|
||||
|
||||
/// Maximum TOI root solver iterations.
|
||||
int rootIterations;
|
||||
} b3Counters;
|
||||
//! @endcond
|
||||
|
||||
/// Joint type enumeration. This is useful because all joint types use b3JointId and sometimes you
|
||||
/// want to get the type of a joint.
|
||||
|
|
@ -2651,12 +2652,14 @@ typedef struct b3ManifoldPoint
|
|||
/// result from the final sub-step.
|
||||
float normalImpulse;
|
||||
|
||||
/// The total normal impulse applied during sub-stepping. This is important
|
||||
/// The total normal impulse applied across sub-stepping and restitution. This is important
|
||||
/// to identify speculative contact points that had an interaction in the time step.
|
||||
/// This includes the warm starting impulse, the sub-step delta impulse, and the restitution
|
||||
/// impulse.
|
||||
float totalNormalImpulse;
|
||||
|
||||
/// Relative normal velocity pre-solve. Used for hit events. If the normal impulse is
|
||||
/// zero then there was no hit. Negative means shapes are approaching.
|
||||
/// Relative normal velocity pre-solve. Negative when approaching. This is only
|
||||
/// computed if hit events are enabled.
|
||||
float normalVelocity;
|
||||
|
||||
/// Local point for matching
|
||||
|
|
|
|||
|
|
@ -621,7 +621,7 @@ void Sample::DrawMetrics()
|
|||
int count = m_profileWriteIndex - m_profileReadIndex;
|
||||
|
||||
// Unroll ring buffer into per-field histories.
|
||||
constexpr int kRowCount = 22;
|
||||
constexpr int kRowCount = 21;
|
||||
float histories[kRowCount][m_profileCapacity];
|
||||
float totals[kRowCount] = {};
|
||||
for ( int i = 0; i < count; ++i )
|
||||
|
|
@ -640,16 +640,15 @@ void Sample::DrawMetrics()
|
|||
histories[9][i] = p.solveImpulses;
|
||||
histories[10][i] = p.integratePositions;
|
||||
histories[11][i] = p.relaxImpulses;
|
||||
histories[12][i] = p.applyRestitution;
|
||||
histories[13][i] = p.storeImpulses;
|
||||
histories[14][i] = p.splitIslands;
|
||||
histories[15][i] = p.transforms;
|
||||
histories[16][i] = p.jointEvents;
|
||||
histories[17][i] = p.hitEvents;
|
||||
histories[18][i] = p.refit;
|
||||
histories[19][i] = p.sleepIslands;
|
||||
histories[20][i] = p.bullets;
|
||||
histories[21][i] = p.sensors;
|
||||
histories[12][i] = p.storeImpulses;
|
||||
histories[13][i] = p.splitIslands;
|
||||
histories[14][i] = p.transforms;
|
||||
histories[15][i] = p.jointEvents;
|
||||
histories[16][i] = p.hitEvents;
|
||||
histories[17][i] = p.refit;
|
||||
histories[18][i] = p.sleepIslands;
|
||||
histories[19][i] = p.bullets;
|
||||
histories[20][i] = p.sensors;
|
||||
|
||||
totals[0] += p.step;
|
||||
totals[1] += p.pairs;
|
||||
|
|
@ -663,16 +662,15 @@ void Sample::DrawMetrics()
|
|||
totals[9] += p.solveImpulses;
|
||||
totals[10] += p.integratePositions;
|
||||
totals[11] += p.relaxImpulses;
|
||||
totals[12] += p.applyRestitution;
|
||||
totals[13] += p.storeImpulses;
|
||||
totals[14] += p.splitIslands;
|
||||
totals[15] += p.transforms;
|
||||
totals[16] += p.jointEvents;
|
||||
totals[17] += p.hitEvents;
|
||||
totals[18] += p.refit;
|
||||
totals[19] += p.sleepIslands;
|
||||
totals[20] += p.bullets;
|
||||
totals[21] += p.sensors;
|
||||
totals[12] += p.storeImpulses;
|
||||
totals[13] += p.splitIslands;
|
||||
totals[14] += p.transforms;
|
||||
totals[15] += p.jointEvents;
|
||||
totals[16] += p.hitEvents;
|
||||
totals[17] += p.refit;
|
||||
totals[18] += p.sleepIslands;
|
||||
totals[19] += p.bullets;
|
||||
totals[20] += p.sensors;
|
||||
}
|
||||
|
||||
// Smoothed over the last few frames so bars don't jitter visibly.
|
||||
|
|
@ -728,13 +726,13 @@ void Sample::DrawMetrics()
|
|||
const ImU32 colorDefault = IM_COL32( 220, 220, 220, 255 );
|
||||
|
||||
const RowDef rows[kRowCount] = {
|
||||
{ "step", 0, colorStep }, { "pairs", 0, colorPairs }, { "collide", 0, colorCollide },
|
||||
{ "solve", 0, colorSolve }, { "setup", 1, colorDefault }, { "constraints", 1, colorDefault },
|
||||
{ "prepare", 2, colorDefault }, { "velocities", 2, colorDefault }, { "warm start", 2, colorDefault },
|
||||
{ "bias", 2, colorDefault }, { "positions", 2, colorDefault }, { "relax", 2, colorDefault },
|
||||
{ "restitution", 2, colorDefault }, { "store", 2, colorDefault }, { "split islands", 2, colorDefault },
|
||||
{ "transforms", 1, colorDefault }, { "joint events", 1, colorDefault }, { "hit events", 1, colorDefault },
|
||||
{ "refit BVH", 1, colorDefault }, { "sleep", 1, colorDefault }, { "bullets", 1, colorDefault },
|
||||
{ "step", 0, colorStep }, { "pairs", 0, colorPairs }, { "collide", 0, colorCollide },
|
||||
{ "solve", 0, colorSolve }, { "setup", 1, colorDefault }, { "constraints", 1, colorDefault },
|
||||
{ "prepare", 2, colorDefault }, { "velocities", 2, colorDefault }, { "warm start", 2, colorDefault },
|
||||
{ "bias", 2, colorDefault }, { "positions", 2, colorDefault }, { "relax", 2, colorDefault },
|
||||
{ "store", 2, colorDefault }, { "split islands", 2, colorDefault },
|
||||
{ "transforms", 1, colorDefault }, { "joint events", 1, colorDefault }, { "hit events", 1, colorDefault },
|
||||
{ "refit BVH", 1, colorDefault }, { "sleep", 1, colorDefault }, { "bullets", 1, colorDefault },
|
||||
{ "sensors", 0, colorSensors },
|
||||
};
|
||||
|
||||
|
|
@ -955,7 +953,7 @@ void Sample::DrawMetrics()
|
|||
ImGui::Text( "toi d/p/r = %d/%d/%d", s.distanceIterations, s.pushBackIterations, s.rootIterations );
|
||||
ImGui::Text( "stack allocator size = %d K", s.stackUsed / 1024 );
|
||||
ImGui::Text( "arena capacity = %d K", s.arenaCapacity / 1024 );
|
||||
ImGui::Text( "total allocation = %d K", s.byteCount / 1024 );
|
||||
ImGui::Text( "total allocation = %d K", (int)(s.byteCount / 1024) );
|
||||
|
||||
ImGui::Separator();
|
||||
b3Capacity c = b3World_GetMaxCapacity( m_worldId );
|
||||
|
|
@ -2175,3 +2173,24 @@ float CastClosestCallback( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float
|
|||
rayContext->hit = true;
|
||||
return fraction;
|
||||
}
|
||||
|
||||
MechanicalEnergy MeasureEnergy( b3WorldId worldId, const b3BodyId* bodyIds, int count )
|
||||
{
|
||||
b3Vec3 gravity = b3World_GetGravity( worldId );
|
||||
|
||||
MechanicalEnergy energy = {};
|
||||
for ( int i = 0; i < count; ++i )
|
||||
{
|
||||
b3MassData massData = b3Body_GetMassData( bodyIds[i] );
|
||||
b3Vec3 v = b3Body_GetLinearVelocity( bodyIds[i] );
|
||||
|
||||
// The inertia tensor is in the body frame, so bring the angular velocity back to it
|
||||
b3Vec3 w = b3InvRotateVector( b3Body_GetRotation( bodyIds[i] ), b3Body_GetAngularVelocity( bodyIds[i] ) );
|
||||
|
||||
energy.linear += 0.5f * massData.mass * b3Dot( v, v );
|
||||
energy.angular += 0.5f * b3Dot( w, b3MulMV( massData.inertia, w ) );
|
||||
energy.potential -= massData.mass * b3Dot( gravity, b3ToVec3( b3Body_GetWorldCenter( bodyIds[i] ) ) );
|
||||
}
|
||||
|
||||
return energy;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -294,3 +294,19 @@ struct CastClosestContext
|
|||
|
||||
float CastClosestCallback( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t materialId, int triangleIndex,
|
||||
int childIndex, void* context );
|
||||
|
||||
struct MechanicalEnergy
|
||||
{
|
||||
float Total() const
|
||||
{
|
||||
return linear + angular + potential;
|
||||
}
|
||||
|
||||
float linear;
|
||||
float angular;
|
||||
float potential;
|
||||
};
|
||||
|
||||
// Kinetic and gravitational potential energy summed over the bodies. Restitution samples
|
||||
// use the total as the invariant that may only decrease.
|
||||
MechanicalEnergy MeasureEnergy( b3WorldId worldId, const b3BodyId* bodyIds, int count );
|
||||
|
|
|
|||
|
|
@ -8,6 +8,10 @@
|
|||
|
||||
#include "box3d/box3d.h"
|
||||
|
||||
#include <ctype.h>
|
||||
#include <iterator>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <vector>
|
||||
|
||||
class Crash : public Sample
|
||||
|
|
@ -132,65 +136,164 @@ public:
|
|||
|
||||
static int sampleMultiplePrismatic = RegisterSample( "Issues", "Multiple Prismatic", MultiplePrismatic::Create );
|
||||
|
||||
// bad hull SM_Waterfall_MED_Wide_01
|
||||
static const b3Vec3 waterfallPoints[] = {
|
||||
{ 0.100183107, -498.925385, -1275.39966 }, { 0.100183107, -498.925415, 0.125000000 },
|
||||
{ 0.100183107, 486.343750, 0.125000000 }, { 0.100183107, 486.343719, -1275.39966 },
|
||||
{ -395.117462, 486.343781, -1462.43750 }, { -395.117462, 486.343750, -96.7426758 },
|
||||
{ -395.117462, -498.925415, -96.7424469 }, { -395.117462, -498.925446, -1462.52612 },
|
||||
{ -186.979691, 486.294891, -1462.47949 }, { -298.250000, 486.294891, 0.125000000 },
|
||||
{ -395.121216, 486.294891, -1462.52612 }, { -186.984360, -498.913361, -1462.48413 },
|
||||
{ -298.250000, -498.913361, 0.125000000 },
|
||||
};
|
||||
|
||||
static const b3Vec3 quadPoints[] = {
|
||||
{ 100.000000, -142.292389, 130.826111 }, { 99.5354385, -71.3011093, 130.826111 },
|
||||
{ 99.5930862, -80.1112213, -100.000000 }, { 100.000000, -142.292389, -100.000000 },
|
||||
{ 99.5930862, -80.1112213, 130.826111 },
|
||||
};
|
||||
|
||||
static const b3Vec3 thinBoxPoints[] = {
|
||||
{ -11.3861933, -24.2451687, -12.0037909 }, { -11.3889809, -24.2466526, -11.9013014 },
|
||||
{ -11.3804407, -24.3151531, -12.0046492 }, { -11.3832273, -24.3166409, -11.9021587 },
|
||||
{ -14.4396200, -24.3636723, -12.1324549 }, { -14.4432650, -24.3655701, -12.0299988 },
|
||||
{ -14.4356947, -24.4337788, -12.1336164 }, { -14.4393377, -24.4356804, -12.0311594 },
|
||||
};
|
||||
|
||||
// Inline point sets are scaled from centimeters. A null point array is a file in data/hulls.
|
||||
struct HullSource
|
||||
{
|
||||
const char* name;
|
||||
const b3Vec3* points;
|
||||
int count;
|
||||
float scale;
|
||||
};
|
||||
|
||||
static const HullSource hullSources[] = {
|
||||
{ "waterfall", waterfallPoints, (int)std::size( waterfallPoints ), 0.01f },
|
||||
{ "quad", quadPoints, (int)std::size( quadPoints ), 0.01f },
|
||||
{ "thin box", thinBoxPoints, (int)std::size( thinBoxPoints ), 0.01f },
|
||||
{ "axe.txt", nullptr, 0, 1.0f },
|
||||
};
|
||||
|
||||
static constexpr int hullSourceCount = (int)std::size( hullSources );
|
||||
|
||||
// Reads x, y, z triples separated by commas or whitespace. A C source repro is
|
||||
// also accepted: only the first initializer list is parsed, so counts and other
|
||||
// numbers in the surrounding code are ignored.
|
||||
static bool LoadHullPoints( const char* path, std::vector<b3Vec3>* points )
|
||||
{
|
||||
points->clear();
|
||||
|
||||
FILE* file = fopen( path, "rb" );
|
||||
if ( file == nullptr )
|
||||
{
|
||||
fprintf( stderr, "Failed to open '%s'\n", path );
|
||||
return false;
|
||||
}
|
||||
|
||||
fseek( file, 0, SEEK_END );
|
||||
long size = ftell( file );
|
||||
fseek( file, 0, SEEK_SET );
|
||||
if ( size < 0 )
|
||||
{
|
||||
fprintf( stderr, "Failed to read '%s'\n", path );
|
||||
fclose( file );
|
||||
return false;
|
||||
}
|
||||
|
||||
std::vector<char> text( size + 1 );
|
||||
size_t readCount = fread( text.data(), 1, size, file );
|
||||
fclose( file );
|
||||
text[readCount] = '\0';
|
||||
|
||||
char* begin = text.data();
|
||||
char* end = begin + readCount;
|
||||
for ( char* c = begin; *c != '\0'; ++c )
|
||||
{
|
||||
if ( *c != '{' )
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
char* prev = c - 1;
|
||||
while ( prev >= begin && isspace( (unsigned char)*prev ) )
|
||||
{
|
||||
--prev;
|
||||
}
|
||||
|
||||
if ( prev < begin || *prev != '=' )
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
begin = c + 1;
|
||||
int depth = 1;
|
||||
for ( end = begin; *end != '\0'; ++end )
|
||||
{
|
||||
if ( *end == '{' )
|
||||
{
|
||||
depth += 1;
|
||||
}
|
||||
else if ( *end == '}' && --depth == 0 )
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
*end = '\0';
|
||||
|
||||
std::vector<float> values;
|
||||
char* cursor = begin;
|
||||
while ( *cursor != '\0' )
|
||||
{
|
||||
char* stop;
|
||||
float value = strtof( cursor, &stop );
|
||||
if ( stop == cursor )
|
||||
{
|
||||
cursor += 1;
|
||||
continue;
|
||||
}
|
||||
|
||||
values.push_back( value );
|
||||
cursor = stop;
|
||||
}
|
||||
|
||||
if ( values.size() % 3 != 0 )
|
||||
{
|
||||
fprintf( stderr, "'%s' has %d values, not a multiple of 3\n", path, (int)values.size() );
|
||||
}
|
||||
|
||||
int count = (int)values.size() / 3;
|
||||
points->resize( count );
|
||||
for ( int i = 0; i < count; ++i )
|
||||
{
|
||||
( *points )[i] = { values[3 * i], values[3 * i + 1], values[3 * i + 2] };
|
||||
}
|
||||
|
||||
return count > 0;
|
||||
}
|
||||
|
||||
class HullCrash : public Sample
|
||||
{
|
||||
public:
|
||||
explicit HullCrash( SampleContext* context )
|
||||
: Sample( context )
|
||||
{
|
||||
m_hull = nullptr;
|
||||
m_sourceIndex = 3;
|
||||
m_maxVertexCount = B3_MAX_HULL_VERTICES;
|
||||
|
||||
LoadPoints();
|
||||
CreateHull();
|
||||
|
||||
if ( m_context->restart == false )
|
||||
{
|
||||
m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Pos_zero );
|
||||
FrameView();
|
||||
}
|
||||
|
||||
m_hull = nullptr;
|
||||
|
||||
#if 0
|
||||
// bad hull SM_Waterfall_MED_Wide_01
|
||||
b3Vec3 points[] = {
|
||||
{ 0.100183107, -498.925385, -1275.39966 }, { 0.100183107, -498.925415, 0.125000000 },
|
||||
{ 0.100183107, 486.343750, 0.125000000 }, { 0.100183107, 486.343719, -1275.39966 },
|
||||
{ -395.117462, 486.343781, -1462.43750 }, { -395.117462, 486.343750, -96.7426758 },
|
||||
{ -395.117462, -498.925415, -96.7424469 }, { -395.117462, -498.925446, -1462.52612 },
|
||||
{ -186.979691, 486.294891, -1462.47949 }, { -298.250000, 486.294891, 0.125000000 },
|
||||
{ -395.121216, 486.294891, -1462.52612 }, { -186.984360, -498.913361, -1462.48413 },
|
||||
{ -298.250000, -498.913361, 0.125000000 },
|
||||
};
|
||||
|
||||
#elif 1
|
||||
b3Vec3 points[] = {
|
||||
{ 100.000000, -142.292389, 130.826111 }, { 99.5354385, -71.3011093, 130.826111 },
|
||||
{ 99.5930862, -80.1112213, -100.000000 }, { 100.000000, -142.292389, -100.000000 },
|
||||
{ 99.5930862, -80.1112213, 130.826111 },
|
||||
};
|
||||
#else
|
||||
b3Vec3 points[] = {
|
||||
{ -11.3861933, -24.2451687, -12.0037909 }, { -11.3889809, -24.2466526, -11.9013014 },
|
||||
{ -11.3804407, -24.3151531, -12.0046492 }, { -11.3832273, -24.3166409, -11.9021587 },
|
||||
{ -14.4396200, -24.3636723, -12.1324549 }, { -14.4432650, -24.3655701, -12.0299988 },
|
||||
{ -14.4356947, -24.4337788, -12.1336164 }, { -14.4393377, -24.4356804, -12.0311594 },
|
||||
};
|
||||
#endif
|
||||
|
||||
static_assert( sizeof( points ) / sizeof( points[0] ) < m_capacity, "bad" );
|
||||
|
||||
m_count = sizeof( points ) / sizeof( points[0] );
|
||||
for ( int i = 0; i < m_count; ++i )
|
||||
{
|
||||
m_points[i] = 0.01f * points[i];
|
||||
}
|
||||
|
||||
// This shift shouldn't be necessary but I'm doing it so the hull
|
||||
// appears on the screen.
|
||||
// for ( int i = 0; i < m_count; ++i )
|
||||
//{
|
||||
// m_points[i] -= m_points[0];
|
||||
// m_points[i] *= 0.01f;
|
||||
//}
|
||||
|
||||
m_hull = b3CreateHull( m_points, m_count, m_count );
|
||||
|
||||
(void)m_hull;
|
||||
}
|
||||
|
||||
~HullCrash() override
|
||||
|
|
@ -201,6 +304,94 @@ public:
|
|||
}
|
||||
}
|
||||
|
||||
void LoadPoints()
|
||||
{
|
||||
const HullSource& source = hullSources[m_sourceIndex];
|
||||
if ( source.points != nullptr )
|
||||
{
|
||||
m_points.assign( source.points, source.points + source.count );
|
||||
}
|
||||
else
|
||||
{
|
||||
char path[64];
|
||||
snprintf( path, sizeof( path ), "data/hulls/%s", source.name );
|
||||
LoadHullPoints( path, &m_points );
|
||||
}
|
||||
|
||||
for ( b3Vec3& point : m_points )
|
||||
{
|
||||
point = source.scale * point;
|
||||
}
|
||||
|
||||
// The reported failure used the point count as the limit
|
||||
m_maxVertexCount = b3ClampInt( (int)m_points.size(), 4, B3_MAX_HULL_VERTICES );
|
||||
}
|
||||
|
||||
void CreateHull()
|
||||
{
|
||||
if ( m_hull != nullptr )
|
||||
{
|
||||
b3DestroyHull( m_hull );
|
||||
m_hull = nullptr;
|
||||
}
|
||||
|
||||
int count = (int)m_points.size();
|
||||
if ( count >= 4 )
|
||||
{
|
||||
m_hull = b3CreateHull( m_points.data(), count, m_maxVertexCount );
|
||||
}
|
||||
}
|
||||
|
||||
void FrameView()
|
||||
{
|
||||
if ( m_points.empty() )
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
b3AABB bounds = b3MakeAABB( m_points.data(), (int)m_points.size(), 0.0f );
|
||||
float radius = b3Length( b3AABB_Extents( bounds ) );
|
||||
m_camera->SetView( 0.0f, 15.0f, b3MaxFloat( 3.0f * radius, 1.0f ), b3ToPos( b3AABB_Center( bounds ) ) );
|
||||
}
|
||||
|
||||
bool DrawControls() override
|
||||
{
|
||||
DrawTextLine( "points = %d", (int)m_points.size() );
|
||||
|
||||
if ( m_hull != nullptr )
|
||||
{
|
||||
DrawTextLine( "hull: vertices = %d, edges = %d, faces = %d", m_hull->vertexCount, m_hull->edgeCount / 2,
|
||||
m_hull->faceCount );
|
||||
}
|
||||
else
|
||||
{
|
||||
DrawTextLine( "hull creation failed" );
|
||||
}
|
||||
|
||||
if ( ImGui::BeginCombo( "Source", hullSources[m_sourceIndex].name ) )
|
||||
{
|
||||
for ( int i = 0; i < hullSourceCount; ++i )
|
||||
{
|
||||
if ( ImGui::Selectable( hullSources[i].name, i == m_sourceIndex ) )
|
||||
{
|
||||
m_sourceIndex = i;
|
||||
LoadPoints();
|
||||
CreateHull();
|
||||
FrameView();
|
||||
}
|
||||
}
|
||||
|
||||
ImGui::EndCombo();
|
||||
}
|
||||
|
||||
if ( ImGui::SliderInt( "Max Vertices", &m_maxVertexCount, 4, B3_MAX_HULL_VERTICES ) )
|
||||
{
|
||||
CreateHull();
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Render() override
|
||||
{
|
||||
if ( m_hull != nullptr )
|
||||
|
|
@ -209,9 +400,9 @@ public:
|
|||
}
|
||||
else
|
||||
{
|
||||
for ( int i = 0; i < m_count; ++i )
|
||||
for ( const b3Vec3& point : m_points )
|
||||
{
|
||||
DrawPoint( b3ToPos( m_points[i] ), 5.0f, MakeColor( b3_colorWhite ) );
|
||||
DrawPoint( b3ToPos( point ), 5.0f, MakeColor( b3_colorWhite ) );
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -225,10 +416,10 @@ public:
|
|||
return new HullCrash( sampleContext );
|
||||
}
|
||||
|
||||
static constexpr int m_capacity = 64;
|
||||
std::vector<b3Vec3> m_points;
|
||||
b3HullData* m_hull;
|
||||
b3Vec3 m_points[m_capacity];
|
||||
int m_count;
|
||||
int m_sourceIndex;
|
||||
int m_maxVertexCount;
|
||||
};
|
||||
|
||||
static int sampleHullCrash = RegisterSample( "Issues", "Hull Crash", HullCrash::Create );
|
||||
|
|
@ -1146,6 +1337,7 @@ public:
|
|||
|
||||
static int sampleWheelStack = RegisterSample( "Issues", "GMod Wheel Stack", WheelStack::Create );
|
||||
|
||||
// There was a problem where this bouncing box could gain energy. This has been fixed by the deferred restitution update.
|
||||
class RestitutionOvershoot : public Sample
|
||||
{
|
||||
public:
|
||||
|
|
@ -1176,6 +1368,7 @@ public:
|
|||
b3BodyDef boxDef = b3DefaultBodyDef();
|
||||
boxDef.type = b3_dynamicBody;
|
||||
boxDef.position = { 0.0f, m_dropHeight, 0.0f };
|
||||
boxDef.safetyFactor = 0.1f;
|
||||
m_boxBody = b3CreateBody( m_worldId, &boxDef );
|
||||
|
||||
b3ShapeDef boxShape = b3DefaultShapeDef();
|
||||
|
|
@ -1186,6 +1379,9 @@ public:
|
|||
m_maxBounceY = 0.0f;
|
||||
m_bounced = false;
|
||||
m_failed = false;
|
||||
m_startEnergy = MeasureEnergy( m_worldId, &m_boxBody, 1 ).Total();
|
||||
m_maxEnergy = m_startEnergy;
|
||||
m_energyFailed = false;
|
||||
}
|
||||
|
||||
void Step() override
|
||||
|
|
@ -1221,9 +1417,29 @@ public:
|
|||
b3Pos markerPoint = { 0.0f, m_dropHeight + m_boxHalf, 0.0f };
|
||||
DrawPlane( b3Vec3_axisY, markerPoint, MakeColor( b3_colorYellow ) );
|
||||
|
||||
// Perfect restitution and no friction, so the total energy may only decrease. It is the honest
|
||||
// invariant here: the height check cannot tell a bounce that gained potential energy from one
|
||||
// that converted it into spin, and this drop does both.
|
||||
MechanicalEnergy energy = MeasureEnergy( m_worldId, &m_boxBody, 1 );
|
||||
float total = energy.Total();
|
||||
|
||||
if ( total > m_maxEnergy )
|
||||
{
|
||||
m_maxEnergy = total;
|
||||
}
|
||||
|
||||
if ( m_bounced && total > m_startEnergy * ( 1.0f + m_energyTolerance ) )
|
||||
{
|
||||
m_energyFailed = true;
|
||||
}
|
||||
|
||||
DrawTextLine( "drop height = %.2f m", m_dropHeight );
|
||||
DrawTextLine( "current y = %.2f m", m_currentY );
|
||||
DrawTextLine( "max bounce = %.2f m", m_maxBounceY );
|
||||
DrawTextLine( "kinetic = %.0f J linear + %.0f J angular", energy.linear, energy.angular );
|
||||
DrawTextLine( "potential = %.0f J", energy.potential );
|
||||
DrawTextLine( "total = %.0f J (%.2f%% of start, peak %.2f%%)", total, 100.0f * total / m_startEnergy,
|
||||
100.0f * m_maxEnergy / m_startEnergy );
|
||||
|
||||
if ( m_bounced == false )
|
||||
{
|
||||
|
|
@ -1237,6 +1453,15 @@ public:
|
|||
{
|
||||
DrawTextLine( "PASS: bounce stays at or below drop height" );
|
||||
}
|
||||
|
||||
if ( m_energyFailed )
|
||||
{
|
||||
DrawTextLine( "FAIL: total energy increased" );
|
||||
}
|
||||
else if ( m_bounced )
|
||||
{
|
||||
DrawTextLine( "PASS: total energy did not increase" );
|
||||
}
|
||||
}
|
||||
|
||||
static Sample* Create( SampleContext* context )
|
||||
|
|
@ -1244,11 +1469,16 @@ public:
|
|||
return new RestitutionOvershoot( context );
|
||||
}
|
||||
|
||||
static constexpr float m_energyTolerance = 0.001f;
|
||||
|
||||
b3BodyId m_boxBody = {};
|
||||
float m_currentY = 0.0f;
|
||||
float m_maxBounceY = 0.0f;
|
||||
float m_startEnergy = 0.0f;
|
||||
float m_maxEnergy = 0.0f;
|
||||
bool m_bounced = false;
|
||||
bool m_failed = false;
|
||||
bool m_energyFailed = false;
|
||||
};
|
||||
|
||||
static int sampleRestitutionOvershoot = RegisterSample( "Issues", "Restitution Overshoot", RestitutionOvershoot::Create );
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
// SPDX-FileCopyrightText: 2025 Erin Catto
|
||||
// SPDX-License-Identifier: MIT
|
||||
|
||||
#include "gfx/debug_adapter.h"
|
||||
#include "gfx/draw.h"
|
||||
#include "human.h"
|
||||
#include "imgui.h"
|
||||
|
|
@ -336,6 +337,528 @@ public:
|
|||
|
||||
static int sampleRestitution = RegisterSample( "Shapes", "Restitution", Restitution::Create );
|
||||
|
||||
|
||||
// Similar to the MeasureSupportedBounce unit test
|
||||
class SphereStackRestitution : public Sample
|
||||
{
|
||||
public:
|
||||
static constexpr float m_impactSpeed = 5.0f;
|
||||
static constexpr int m_maxStackCount = 3;
|
||||
|
||||
explicit SphereStackRestitution( SampleContext* context )
|
||||
: Sample( context )
|
||||
{
|
||||
if ( context->restart == false )
|
||||
{
|
||||
m_camera->SetView( 20.0f, 10.0f, 12.0f, { 0.0f, 2.5f, 0.0f } );
|
||||
}
|
||||
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.position = { 0.0f, -1.0f, 0.0f };
|
||||
b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
shapeDef.baseMaterial.friction = 0.0f;
|
||||
shapeDef.baseMaterial.restitution = 0.0f;
|
||||
b3BoxHull box = b3MakeBoxHull( 40.0f, 1.0f, 40.0f );
|
||||
b3ShapeId groundShapeId = b3CreateHullShape( groundId, &shapeDef, &box.base );
|
||||
SetGroundShape( groundShapeId );
|
||||
|
||||
CreateScene();
|
||||
}
|
||||
|
||||
b3BodyId CreateBall( float y, float velocityY, float restitution, bool hitEvents )
|
||||
{
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
bodyDef.position = { 0.0f, y, 0.0f };
|
||||
bodyDef.linearVelocity = { 0.0f, velocityY, 0.0f };
|
||||
bodyDef.enableSleep = false;
|
||||
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
shapeDef.baseMaterial.friction = 0.0f;
|
||||
shapeDef.baseMaterial.restitution = restitution;
|
||||
shapeDef.enableHitEvents = hitEvents;
|
||||
b3Sphere sphere = { b3Vec3_zero, 0.5f };
|
||||
b3CreateSphereShape( bodyId, &shapeDef, &sphere );
|
||||
|
||||
return bodyId;
|
||||
}
|
||||
|
||||
void CreateScene()
|
||||
{
|
||||
for ( int i = 0; i < m_bodyCount; ++i )
|
||||
{
|
||||
b3DestroyBody( m_bodyIds[i] );
|
||||
}
|
||||
m_bodyCount = 0;
|
||||
|
||||
if ( m_gravity )
|
||||
{
|
||||
b3World_SetGravity( m_worldId, { 0.0f, -10.0f, 0.0f } );
|
||||
}
|
||||
else
|
||||
{
|
||||
b3World_SetGravity( m_worldId, b3Vec3_zero );
|
||||
}
|
||||
|
||||
for ( int i = 0; i < m_stackCount; ++i )
|
||||
{
|
||||
m_bodyIds[m_bodyCount] = CreateBall( 0.5f + 1.0f * i, 0.0f, 0.0f, false );
|
||||
m_bodyCount += 1;
|
||||
}
|
||||
|
||||
// Start half a step of travel above contact so the impact lands mid step, matching the test
|
||||
m_startHeight = 0.5f + 1.0f * m_stackCount + 0.5f * m_impactSpeed * ( 1.0f / 60.0f );
|
||||
|
||||
m_impactorId = CreateBall( m_startHeight, -m_impactSpeed, m_restitution, true );
|
||||
m_bodyIds[m_bodyCount] = m_impactorId;
|
||||
m_bodyCount += 1;
|
||||
|
||||
m_startEnergy = MeasureEnergy( m_worldId, m_bodyIds, m_bodyCount ).Total();
|
||||
m_peakEnergy = m_startEnergy;
|
||||
|
||||
m_coefficient = 0.0f;
|
||||
m_latched = false;
|
||||
m_hit = false;
|
||||
}
|
||||
|
||||
bool DrawControls() override
|
||||
{
|
||||
bool rebuild = false;
|
||||
|
||||
ImGui::PushItemWidth( 6.0f * ImGui::GetFontSize() );
|
||||
|
||||
if ( ImGui::SliderFloat( "Restitution", &m_restitution, 0.0f, 1.0f, "%.2f" ) )
|
||||
{
|
||||
rebuild = true;
|
||||
}
|
||||
|
||||
if ( ImGui::SliderInt( "Stack", &m_stackCount, 0, m_maxStackCount ) )
|
||||
{
|
||||
rebuild = true;
|
||||
}
|
||||
|
||||
ImGui::PopItemWidth();
|
||||
|
||||
if ( ImGui::Checkbox( "Gravity", &m_gravity ) )
|
||||
{
|
||||
rebuild = true;
|
||||
}
|
||||
|
||||
if ( ImGui::Button( "Reset" ) )
|
||||
{
|
||||
rebuild = true;
|
||||
}
|
||||
|
||||
if ( rebuild )
|
||||
{
|
||||
CreateScene();
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Step() override
|
||||
{
|
||||
Sample::Step();
|
||||
|
||||
b3ContactEvents events = b3World_GetContactEvents( m_worldId );
|
||||
if ( events.hitCount > 0 )
|
||||
{
|
||||
m_hit = true;
|
||||
}
|
||||
|
||||
float vy = b3Body_GetLinearVelocity( m_impactorId ).y;
|
||||
|
||||
// The rebound only means something once the impact has happened and the impactor is leaving
|
||||
if ( m_latched == false && m_hit && vy > 0.0f )
|
||||
{
|
||||
m_coefficient = vy / m_impactSpeed;
|
||||
m_latched = true;
|
||||
}
|
||||
|
||||
float supportSpeed = 0.0f;
|
||||
for ( int i = 0; i < m_stackCount; ++i )
|
||||
{
|
||||
supportSpeed = b3MaxFloat( supportSpeed, b3Length( b3Body_GetLinearVelocity( m_bodyIds[i] ) ) );
|
||||
}
|
||||
|
||||
DrawLine( { -2.0f, m_startHeight, 0.0f }, { 2.0f, m_startHeight, 0.0f }, MakeColor( b3_colorRed ) );
|
||||
|
||||
MechanicalEnergy energy = MeasureEnergy( m_worldId, m_bodyIds, m_bodyCount );
|
||||
float total = energy.Total();
|
||||
m_peakEnergy = b3MaxFloat( m_peakEnergy, total );
|
||||
|
||||
DrawTextLine( "impactor vy = %.3f m/s", vy );
|
||||
|
||||
DrawTextLine( "coefficient = %.4f live (target %.2f)", vy / m_impactSpeed, m_restitution );
|
||||
|
||||
if ( m_latched )
|
||||
{
|
||||
DrawTextLine( "coefficient = %.4f at first rebound", m_coefficient );
|
||||
}
|
||||
|
||||
DrawTextLine( "support speed = %.4f m/s", supportSpeed );
|
||||
|
||||
float scale = m_startEnergy != 0.0f ? 100.0f / m_startEnergy : 0.0f;
|
||||
DrawTextLine( "kinetic = %.3f J linear + %.3f J angular", energy.linear, energy.angular );
|
||||
DrawTextLine( "potential = %.3f J", energy.potential );
|
||||
DrawTextLine( "total = %.3f J (%.2f%% of start, peak %.2f%%)", total, scale * total, scale * m_peakEnergy );
|
||||
}
|
||||
|
||||
static Sample* Create( SampleContext* context )
|
||||
{
|
||||
return new SphereStackRestitution( context );
|
||||
}
|
||||
|
||||
b3BodyId m_bodyIds[m_maxStackCount + 1] = {};
|
||||
b3BodyId m_impactorId = b3_nullBodyId;
|
||||
int m_bodyCount = 0;
|
||||
int m_stackCount = 2;
|
||||
float m_restitution = 1.0f;
|
||||
float m_startHeight = 0.0f;
|
||||
float m_coefficient = 0.0f;
|
||||
float m_startEnergy = 0.0f;
|
||||
float m_peakEnergy = 0.0f;
|
||||
bool m_gravity = false;
|
||||
bool m_latched = false;
|
||||
bool m_hit = false;
|
||||
};
|
||||
|
||||
static int sampleSphereStackRestitution = RegisterSample( "Shapes", "Sphere Stack Restitution", SphereStackRestitution::Create );
|
||||
|
||||
// Similar to the MeasureFlatBounce and SpinTest unit tests
|
||||
class BoxRestitution : public Sample
|
||||
{
|
||||
public:
|
||||
static constexpr float m_impactSpeed = 5.0f;
|
||||
|
||||
explicit BoxRestitution( SampleContext* context )
|
||||
: Sample( context )
|
||||
{
|
||||
if ( context->restart == false )
|
||||
{
|
||||
m_camera->SetView( 20.0f, 10.0f, 14.0f, { 0.0f, 3.0f, 0.0f } );
|
||||
}
|
||||
|
||||
// Gravity would bias the measured coefficient
|
||||
b3World_SetGravity( m_worldId, b3Vec3_zero );
|
||||
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.position = { 0.0f, -1.0f, 0.0f };
|
||||
b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
shapeDef.baseMaterial.friction = 0.0f;
|
||||
shapeDef.baseMaterial.restitution = 0.0f;
|
||||
b3BoxHull box = b3MakeBoxHull( 40.0f, 1.0f, 40.0f );
|
||||
b3ShapeId groundShapeId = b3CreateHullShape( groundId, &shapeDef, &box.base );
|
||||
SetGroundShape( groundShapeId );
|
||||
|
||||
CreateScene();
|
||||
}
|
||||
|
||||
void CreateScene()
|
||||
{
|
||||
if ( B3_IS_NON_NULL( m_boxId ) )
|
||||
{
|
||||
b3DestroyBody( m_boxId );
|
||||
}
|
||||
|
||||
// Start half a step of travel above contact so the impact lands mid step, matching the test
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
bodyDef.position = { 0.0f, 0.25f + 0.5f * m_impactSpeed * ( 1.0f / 60.0f ), 0.0f };
|
||||
bodyDef.linearVelocity = { 0.0f, -m_impactSpeed, 0.0f };
|
||||
bodyDef.angularVelocity = { 0.0f, 0.0f, m_spin };
|
||||
bodyDef.enableSleep = false;
|
||||
m_boxId = b3CreateBody( m_worldId, &bodyDef );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
shapeDef.density = 1.0f;
|
||||
shapeDef.baseMaterial.friction = 0.0f;
|
||||
shapeDef.baseMaterial.restitution = m_restitution;
|
||||
shapeDef.enableHitEvents = true;
|
||||
b3BoxHull box = b3MakeBoxHull( 1.0f, 0.25f, 1.0f );
|
||||
b3CreateHullShape( m_boxId, &shapeDef, &box.base );
|
||||
|
||||
m_startEnergy = MeasureEnergy( m_worldId, &m_boxId, 1 ).Total();
|
||||
m_peakEnergy = m_startEnergy;
|
||||
|
||||
m_coefficient = 0.0f;
|
||||
m_latched = false;
|
||||
m_hit = false;
|
||||
}
|
||||
|
||||
bool DrawControls() override
|
||||
{
|
||||
bool rebuild = false;
|
||||
|
||||
ImGui::PushItemWidth( 6.0f * ImGui::GetFontSize() );
|
||||
|
||||
if ( ImGui::SliderFloat( "Restitution", &m_restitution, 0.0f, 1.0f, "%.2f" ) )
|
||||
{
|
||||
rebuild = true;
|
||||
}
|
||||
|
||||
if ( ImGui::SliderFloat( "Spin", &m_spin, 0.0f, 2.0f, "%.2f" ) )
|
||||
{
|
||||
rebuild = true;
|
||||
}
|
||||
|
||||
ImGui::PopItemWidth();
|
||||
|
||||
if ( ImGui::Button( "Reset" ) )
|
||||
{
|
||||
rebuild = true;
|
||||
}
|
||||
|
||||
if ( rebuild )
|
||||
{
|
||||
CreateScene();
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Step() override
|
||||
{
|
||||
Sample::Step();
|
||||
|
||||
b3ContactEvents events = b3World_GetContactEvents( m_worldId );
|
||||
if ( events.hitCount > 0 )
|
||||
{
|
||||
m_hit = true;
|
||||
}
|
||||
|
||||
float vy = b3Body_GetLinearVelocity( m_boxId ).y;
|
||||
b3Vec3 w = b3Body_GetAngularVelocity( m_boxId );
|
||||
|
||||
if ( m_latched == false && m_hit && vy > 0.0f )
|
||||
{
|
||||
m_coefficient = vy / m_impactSpeed;
|
||||
m_latched = true;
|
||||
}
|
||||
|
||||
Vec4 yellow = MakeColor( b3_colorYellow );
|
||||
DrawPoint( b3Body_GetWorldPoint( m_boxId, { -1.0f, -0.25f, -1.0f } ), 8.0f, yellow );
|
||||
DrawPoint( b3Body_GetWorldPoint( m_boxId, { 1.0f, -0.25f, -1.0f } ), 8.0f, yellow );
|
||||
DrawPoint( b3Body_GetWorldPoint( m_boxId, { -1.0f, -0.25f, 1.0f } ), 8.0f, yellow );
|
||||
DrawPoint( b3Body_GetWorldPoint( m_boxId, { 1.0f, -0.25f, 1.0f } ), 8.0f, yellow );
|
||||
|
||||
MechanicalEnergy energy = MeasureEnergy( m_worldId, &m_boxId, 1 );
|
||||
float total = energy.Total();
|
||||
m_peakEnergy = b3MaxFloat( m_peakEnergy, total );
|
||||
|
||||
DrawTextLine( "vy = %.3f m/s", vy );
|
||||
|
||||
DrawTextLine( "coefficient = %.4f live (target %.2f)", vy / m_impactSpeed, m_restitution );
|
||||
|
||||
if ( m_latched )
|
||||
{
|
||||
DrawTextLine( "coefficient = %.4f at first rebound", m_coefficient );
|
||||
}
|
||||
|
||||
// The spin is about z, so any x or y component is residual from the sequential point solve
|
||||
DrawTextLine( "spin in = %.2f, spin out = %.4f (ideal %.2f at e = 1)", m_spin, w.z, -m_spin );
|
||||
DrawTextLine( "off axis spin = %.4f rad/s", sqrtf( w.x * w.x + w.y * w.y ) );
|
||||
|
||||
float scale = m_startEnergy != 0.0f ? 100.0f / m_startEnergy : 0.0f;
|
||||
DrawTextLine( "kinetic = %.3f J linear + %.3f J angular", energy.linear, energy.angular );
|
||||
DrawTextLine( "potential = %.3f J", energy.potential );
|
||||
DrawTextLine( "total = %.3f J (%.2f%% of start, peak %.2f%%)", total, scale * total, scale * m_peakEnergy );
|
||||
}
|
||||
|
||||
static Sample* Create( SampleContext* context )
|
||||
{
|
||||
return new BoxRestitution( context );
|
||||
}
|
||||
|
||||
b3BodyId m_boxId = b3_nullBodyId;
|
||||
float m_restitution = 0.9f;
|
||||
float m_spin = 0.0f;
|
||||
float m_coefficient = 0.0f;
|
||||
float m_startEnergy = 0.0f;
|
||||
float m_peakEnergy = 0.0f;
|
||||
bool m_latched = false;
|
||||
bool m_hit = false;
|
||||
};
|
||||
|
||||
static int sampleBoxRestitution = RegisterSample( "Shapes", "Box Restitution", BoxRestitution::Create );
|
||||
|
||||
// Similar to the ImpulseTest unit test. The hit event marks the bounce and carries the approach speed,
|
||||
// so the impulse the contact reports can be checked against reversing that speed at the coefficient.
|
||||
// The total runs a little over because the contact also carries the weight for the part of the step
|
||||
// it is active.
|
||||
class BounceImpulse : public Sample
|
||||
{
|
||||
public:
|
||||
explicit BounceImpulse( SampleContext* context )
|
||||
: Sample( context )
|
||||
{
|
||||
if ( context->restart == false )
|
||||
{
|
||||
m_camera->SetView( 20.0f, 10.0f, 36.0f, { 0.0f, 8.0f, 0.0f } );
|
||||
}
|
||||
|
||||
b3World_SetGravity( m_worldId, { 0.0f, -10.0f, 0.0f } );
|
||||
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.position = { 0.0f, -1.0f, 0.0f };
|
||||
b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
shapeDef.baseMaterial.friction = 0.0f;
|
||||
shapeDef.baseMaterial.restitution = 0.0f;
|
||||
b3BoxHull box = b3MakeBoxHull( 40.0f, 1.0f, 40.0f );
|
||||
b3ShapeId groundShapeId = b3CreateHullShape( groundId, &shapeDef, &box.base );
|
||||
SetGroundShape( groundShapeId );
|
||||
|
||||
CreateScene();
|
||||
}
|
||||
|
||||
void CreateScene()
|
||||
{
|
||||
if ( B3_IS_NON_NULL( m_ballId ) )
|
||||
{
|
||||
b3DestroyBody( m_ballId );
|
||||
}
|
||||
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
bodyDef.position = { 0.0f, 0.5f + m_dropHeight, 0.0f };
|
||||
bodyDef.enableSleep = false;
|
||||
m_ballId = b3CreateBody( m_worldId, &bodyDef );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
shapeDef.baseMaterial.friction = 0.0f;
|
||||
shapeDef.baseMaterial.restitution = m_restitution;
|
||||
shapeDef.enableHitEvents = true;
|
||||
b3Sphere sphere = { b3Vec3_zero, 0.5f };
|
||||
b3CreateSphereShape( m_ballId, &shapeDef, &sphere );
|
||||
|
||||
m_mass = b3Body_GetMass( m_ballId );
|
||||
|
||||
m_approachSpeed = 0.0f;
|
||||
m_totalImpulse = 0.0f;
|
||||
m_expectedImpulse = 0.0f;
|
||||
m_contactSteps = 0;
|
||||
m_hit = false;
|
||||
m_bouncing = false;
|
||||
}
|
||||
|
||||
bool DrawControls() override
|
||||
{
|
||||
bool rebuild = false;
|
||||
|
||||
ImGui::PushItemWidth( 6.0f * ImGui::GetFontSize() );
|
||||
|
||||
if ( ImGui::SliderFloat( "Restitution", &m_restitution, 0.0f, 1.0f, "%.2f" ) )
|
||||
{
|
||||
rebuild = true;
|
||||
}
|
||||
|
||||
if ( ImGui::SliderFloat( "Height", &m_dropHeight, 0.5f, 50.0f, "%.1f" ) )
|
||||
{
|
||||
rebuild = true;
|
||||
}
|
||||
|
||||
ImGui::PopItemWidth();
|
||||
|
||||
if ( ImGui::Button( "Reset" ) )
|
||||
{
|
||||
rebuild = true;
|
||||
}
|
||||
|
||||
if ( rebuild )
|
||||
{
|
||||
CreateScene();
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Step() override
|
||||
{
|
||||
Sample::Step();
|
||||
|
||||
if ( m_didStep )
|
||||
{
|
||||
b3ContactEvents events = b3World_GetContactEvents( m_worldId );
|
||||
if ( m_hit == false && events.hitCount > 0 )
|
||||
{
|
||||
m_hit = true;
|
||||
m_bouncing = true;
|
||||
m_approachSpeed = events.hitEvents[0].approachSpeed;
|
||||
m_expectedImpulse = ( 1.0f + m_restitution ) * m_mass * m_approachSpeed;
|
||||
}
|
||||
|
||||
// Keep summing until the contact lets go, a slow bounce can take a few steps to leave
|
||||
if ( m_bouncing )
|
||||
{
|
||||
b3ContactData contactData[4];
|
||||
int contactCount = b3Body_GetContactData( m_ballId, contactData, 4 );
|
||||
for ( int c = 0; c < contactCount; ++c )
|
||||
{
|
||||
for ( int m = 0; m < contactData[c].manifoldCount; ++m )
|
||||
{
|
||||
const b3Manifold* manifold = contactData[c].manifolds + m;
|
||||
for ( int p = 0; p < manifold->pointCount; ++p )
|
||||
{
|
||||
m_totalImpulse += manifold->points[p].totalNormalImpulse;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if ( contactCount > 0 )
|
||||
{
|
||||
m_contactSteps += 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_bouncing = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float startHeight = 0.5f + m_dropHeight;
|
||||
DrawLine( { -2.0f, startHeight, 0.0f }, { 2.0f, startHeight, 0.0f }, MakeColor( b3_colorRed ) );
|
||||
|
||||
DrawTextLine( "vy = %.3f m/s", b3Body_GetLinearVelocity( m_ballId ).y );
|
||||
|
||||
if ( m_hit )
|
||||
{
|
||||
DrawTextLine( "approach speed = %.3f m/s", m_approachSpeed );
|
||||
DrawTextLine( "total impulse = %.2f N s over %d steps", m_totalImpulse, m_contactSteps );
|
||||
DrawTextLine( "expected impulse = %.2f N s", m_expectedImpulse );
|
||||
}
|
||||
else
|
||||
{
|
||||
DrawTextLine( "waiting for the hit" );
|
||||
}
|
||||
}
|
||||
|
||||
static Sample* Create( SampleContext* context )
|
||||
{
|
||||
return new BounceImpulse( context );
|
||||
}
|
||||
|
||||
b3BodyId m_ballId = b3_nullBodyId;
|
||||
float m_restitution = 0.5f;
|
||||
float m_dropHeight = 20.0f;
|
||||
float m_mass = 0.0f;
|
||||
float m_approachSpeed = 0.0f;
|
||||
float m_totalImpulse = 0.0f;
|
||||
float m_expectedImpulse = 0.0f;
|
||||
int m_contactSteps = 0;
|
||||
bool m_hit = false;
|
||||
bool m_bouncing = false;
|
||||
};
|
||||
|
||||
static int sampleBounceImpulse = RegisterSample( "Shapes", "Bounce Impulse", BounceImpulse::Create );
|
||||
|
||||
// This shows an optimization when creating many static shapes you can skip having them invoke collision, assuming
|
||||
// dynamic bodies are added after the static bodies.
|
||||
class StaticInvoke : public Sample
|
||||
|
|
|
|||
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 );
|
||||
|
||||
|
|
|
|||
|
|
@ -25,6 +25,7 @@ set(BOX3D_TEST_FILES
|
|||
test_mover.c
|
||||
test_name_cache.c
|
||||
test_recording.c
|
||||
test_restitution.c
|
||||
test_sat.c
|
||||
test_shape.c
|
||||
test_table.c
|
||||
|
|
|
|||
|
|
@ -47,6 +47,7 @@ extern int MeshTest( void );
|
|||
extern int MoverTest( void );
|
||||
extern int NameCacheTest( void );
|
||||
extern int RecordingTest( void );
|
||||
extern int RestitutionTest( void );
|
||||
extern int SeparatingAxisTest( void );
|
||||
extern int ShapeTest( void );
|
||||
extern int TableTest( void );
|
||||
|
|
@ -123,6 +124,7 @@ int main( int argc, char** argv )
|
|||
MAYBE_RUN_TEST( MoverTest );
|
||||
MAYBE_RUN_TEST( NameCacheTest );
|
||||
MAYBE_RUN_TEST( RecordingTest );
|
||||
MAYBE_RUN_TEST( RestitutionTest );
|
||||
MAYBE_RUN_TEST( SeparatingAxisTest );
|
||||
MAYBE_RUN_TEST( ShapeTest );
|
||||
MAYBE_RUN_TEST( TableTest );
|
||||
|
|
|
|||
|
|
@ -18,27 +18,27 @@
|
|||
// Double precision accumulates body positions in double, so the settle/sleep step and the
|
||||
// state hash differ from the float build. Both modes are internally deterministic.
|
||||
#if defined( BOX3D_DOUBLE_PRECISION )
|
||||
#define RAGDOLL_SLEEP_STEP 277
|
||||
#define RAGDOLL_HASH 0xF7B5B2D1
|
||||
#define RAGDOLL_SLEEP_STEP 272
|
||||
#define RAGDOLL_HASH 0xC049067A
|
||||
#define WAVE_PILE_SLEEP_STEP 323
|
||||
#define WAVE_PILE_HASH 0x2200A547
|
||||
#define QUERY_SPAWN_SLEEP_STEP 242
|
||||
#define QUERY_SPAWN_HASH 0x1737F5BC
|
||||
#define QUERY_SPAWN_HIT_COUNT 59
|
||||
#define QUERY_SPAWN_QUERY_HASH 0x5B4429DC
|
||||
#define MESH_DROP_SLEEP_STEP 251
|
||||
#define MESH_DROP_HASH 0x465381C5
|
||||
#define MESH_DROP_SLEEP_STEP 217
|
||||
#define MESH_DROP_HASH 0xCF946437
|
||||
#else
|
||||
#define RAGDOLL_SLEEP_STEP 272
|
||||
#define RAGDOLL_HASH 0xF4992B41
|
||||
#define RAGDOLL_SLEEP_STEP 271
|
||||
#define RAGDOLL_HASH 0x805719D3
|
||||
#define WAVE_PILE_SLEEP_STEP 316
|
||||
#define WAVE_PILE_HASH 0xAA7D15A7
|
||||
#define QUERY_SPAWN_SLEEP_STEP 242
|
||||
#define QUERY_SPAWN_HASH 0xB9F993A5
|
||||
#define QUERY_SPAWN_HIT_COUNT 59
|
||||
#define QUERY_SPAWN_QUERY_HASH 0xE3271F3D
|
||||
#define MESH_DROP_SLEEP_STEP 251
|
||||
#define MESH_DROP_HASH 0xE58C7240
|
||||
#define MESH_DROP_SLEEP_STEP 217
|
||||
#define MESH_DROP_HASH 0x256BC40B
|
||||
#endif
|
||||
|
||||
// The goldens above pin exact values for the default four point manifold. A build that
|
||||
|
|
|
|||
|
|
@ -12,7 +12,7 @@
|
|||
// Direct unit test of the cache: dedup, reverse lookup, empty/null, arbitrary length, no leak.
|
||||
static int CacheUnit( void )
|
||||
{
|
||||
int32_t base = b3GetByteCount();
|
||||
int64_t base = b3GetByteCount();
|
||||
|
||||
b3NameCache cache = b3CreateNameCache();
|
||||
|
||||
|
|
@ -155,7 +155,7 @@ static int NameRoundTrip( void )
|
|||
// the tracked byte count to return to baseline so a re-load that drops duplicate copies leaks nothing.
|
||||
static int RollbackNames( void )
|
||||
{
|
||||
int32_t base = b3GetByteCount();
|
||||
int64_t base = b3GetByteCount();
|
||||
|
||||
b3Recording* rec = b3CreateRecording( 0 );
|
||||
ENSURE( rec != NULL );
|
||||
|
|
|
|||
1135
test/test_restitution.c
Normal file
1135
test/test_restitution.c
Normal file
File diff suppressed because it is too large
Load diff
|
|
@ -2,9 +2,11 @@
|
|||
// SPDX-License-Identifier: MIT
|
||||
|
||||
#include "benchmarks.h"
|
||||
#include "body.h"
|
||||
#include "dynamic_tree.h"
|
||||
#include "overflow_color.h"
|
||||
#include "physics_world.h"
|
||||
#include "recording.h"
|
||||
#include "test_macros.h"
|
||||
|
||||
#include "box3d/box3d.h"
|
||||
|
|
@ -1276,6 +1278,348 @@ static int TestContinuousMoveEvent( void )
|
|||
return 0;
|
||||
}
|
||||
|
||||
// The contact body sim locators are persistent state. Every contact in every set carries the
|
||||
// current encoding for both of its bodies: the awake local index, a static tag, or null for a
|
||||
// body that fell asleep. The scenes below drive each site that can invalidate one. The gate is
|
||||
// b3ValidateSolverSets, which asserts the invariant on every mutation and every step, so a
|
||||
// dropped refresh trips there rather than producing a plausible but wrong simulation. Each scene
|
||||
// also runs at worker counts 1 and 4 with the state hashes compared.
|
||||
|
||||
static b3WorldId CreateLocatorWorld( int workerCount )
|
||||
{
|
||||
b3WorldDef worldDef = b3DefaultWorldDef();
|
||||
worldDef.workerCount = workerCount;
|
||||
return b3CreateWorld( &worldDef );
|
||||
}
|
||||
|
||||
static b3BodyId CreateLocatorBody( b3WorldId worldId, b3BodyType type, b3Pos position, float hx, float hy, float hz )
|
||||
{
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.type = type;
|
||||
bodyDef.position = position;
|
||||
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
b3BoxHull box = b3MakeBoxHull( hx, hy, hz );
|
||||
b3CreateHullShape( bodyId, &shapeDef, &box.base );
|
||||
|
||||
return bodyId;
|
||||
}
|
||||
|
||||
static void StepLocatorWorld( b3WorldId worldId, int stepCount )
|
||||
{
|
||||
for ( int i = 0; i < stepCount; ++i )
|
||||
{
|
||||
b3World_Step( worldId, 1.0f / 60.0f, 4 );
|
||||
}
|
||||
}
|
||||
|
||||
// Steps until the body sleeps or the budget runs out. Returns the steps taken.
|
||||
static int StepUntilAsleep( b3WorldId worldId, b3BodyId bodyId, int maxSteps )
|
||||
{
|
||||
for ( int i = 0; i < maxSteps; ++i )
|
||||
{
|
||||
b3World_Step( worldId, 1.0f / 60.0f, 4 );
|
||||
if ( b3Body_IsAwake( bodyId ) == false )
|
||||
{
|
||||
return i + 1;
|
||||
}
|
||||
}
|
||||
|
||||
return maxSteps;
|
||||
}
|
||||
|
||||
// Sleep and wake: first touch puts contacts in the graph, sleeping nulls the locators of the
|
||||
// island bodies, waking restores their awake indices. The plate beside the stack holds a
|
||||
// non-touching contact across the whole cycle, and that is the contact the wake path has to fix up
|
||||
// itself. A touching contact would get a fresh encoding from the graph add and hide a dropped
|
||||
// refresh.
|
||||
static uint64_t LocatorSleepWakeScene( int workerCount, int* failed )
|
||||
{
|
||||
b3WorldId worldId = CreateLocatorWorld( workerCount );
|
||||
CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ 0.0f, -1.0f, 0.0f }, 5.0f, 1.0f, 5.0f );
|
||||
|
||||
b3BodyId stackId[3];
|
||||
for ( int i = 0; i < 3; ++i )
|
||||
{
|
||||
stackId[i] = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 0.0f, 0.5f + i, 0.0f }, 0.5f, 0.5f, 0.5f );
|
||||
}
|
||||
|
||||
// Beside the top box, close enough that the fat bounds overlap and far enough that the
|
||||
// shapes never touch. Kinematic and awake, so the contact stays in the awake set while the
|
||||
// stack sleeps. Off to the side so it does not catch the body dropped later.
|
||||
b3BodyId plateId = CreateLocatorBody( worldId, b3_kinematicBody, (b3Pos){ 1.04f, 2.5f, 0.0f }, 0.5f, 0.5f, 0.5f );
|
||||
b3Body_EnableSleep( plateId, false );
|
||||
|
||||
int steps = StepUntilAsleep( worldId, stackId[0], 300 );
|
||||
if ( steps == 300 || b3Body_IsAwake( stackId[2] ) == true )
|
||||
{
|
||||
*failed = 1;
|
||||
}
|
||||
|
||||
// Wake without contact so the non-touching contact keeps its stale locator if the wake
|
||||
// path forgets to refresh
|
||||
b3Body_SetAwake( stackId[0], true );
|
||||
StepLocatorWorld( worldId, 5 );
|
||||
|
||||
if ( b3Body_IsAwake( stackId[2] ) == false )
|
||||
{
|
||||
*failed = 1;
|
||||
}
|
||||
|
||||
steps = StepUntilAsleep( worldId, stackId[0], 300 );
|
||||
if ( steps == 300 )
|
||||
{
|
||||
*failed = 1;
|
||||
}
|
||||
|
||||
// Dropping a body on the pile drives the wake path through first touch. The pile can settle
|
||||
// and sleep again, so catch the wake as it happens.
|
||||
CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 0.0f, 8.0f, 0.0f }, 0.5f, 0.5f, 0.5f );
|
||||
|
||||
bool woke = false;
|
||||
for ( int i = 0; i < 180; ++i )
|
||||
{
|
||||
b3World_Step( worldId, 1.0f / 60.0f, 4 );
|
||||
woke = woke || b3Body_IsAwake( stackId[0] );
|
||||
}
|
||||
|
||||
if ( woke == false )
|
||||
{
|
||||
*failed = 1;
|
||||
}
|
||||
|
||||
uint64_t hash = b3HashWorldState( b3GetWorldFromId( worldId ) );
|
||||
b3DestroyWorld( worldId );
|
||||
return hash;
|
||||
}
|
||||
|
||||
// An island falling asleep and a body leaving the awake set both swap the last awake sim into
|
||||
// the hole. The pile that never sleeps supplies a moved body that still owns contacts.
|
||||
static uint64_t LocatorAwakeNeighborScene( int workerCount, int* failed )
|
||||
{
|
||||
b3WorldId worldId = CreateLocatorWorld( workerCount );
|
||||
CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ 0.0f, -1.0f, 0.0f }, 30.0f, 1.0f, 5.0f );
|
||||
|
||||
// This island sleeps
|
||||
b3BodyId sleeperId[2];
|
||||
for ( int i = 0; i < 2; ++i )
|
||||
{
|
||||
sleeperId[i] = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ -10.0f, 0.5f + i, 0.0f }, 0.5f, 0.5f, 0.5f );
|
||||
}
|
||||
|
||||
// Created after the sleepers so its sims sit at the end of the awake set and get swapped
|
||||
// into the holes the sleeping island leaves behind
|
||||
b3BodyId spareId = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 20.0f, 0.5f, 0.0f }, 0.5f, 0.5f, 0.5f );
|
||||
b3Body_EnableSleep( spareId, false );
|
||||
|
||||
b3BodyId awakeId[3];
|
||||
for ( int i = 0; i < 3; ++i )
|
||||
{
|
||||
awakeId[i] = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 10.0f, 0.5f + i, 0.0f }, 0.5f, 0.5f, 0.5f );
|
||||
b3Body_EnableSleep( awakeId[i], false );
|
||||
}
|
||||
|
||||
int steps = StepUntilAsleep( worldId, sleeperId[0], 300 );
|
||||
if ( steps == 300 || b3Body_IsAwake( awakeId[0] ) == false )
|
||||
{
|
||||
*failed = 1;
|
||||
}
|
||||
|
||||
StepLocatorWorld( worldId, 10 );
|
||||
|
||||
// Leaving the awake set swaps a body that owns contacts into the spare's slot
|
||||
b3Body_Disable( spareId );
|
||||
StepLocatorWorld( worldId, 10 );
|
||||
|
||||
b3Body_Enable( spareId );
|
||||
StepLocatorWorld( worldId, 10 );
|
||||
|
||||
uint64_t hash = b3HashWorldState( b3GetWorldFromId( worldId ) );
|
||||
b3DestroyWorld( worldId );
|
||||
return hash;
|
||||
}
|
||||
|
||||
// Destroying a body swaps the last sim of its set into the hole. The moved body keeps its
|
||||
// contacts, so its side of each one has to follow it.
|
||||
static uint64_t LocatorDestroyScene( int workerCount, int* failed )
|
||||
{
|
||||
b3WorldId worldId = CreateLocatorWorld( workerCount );
|
||||
CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ 0.0f, -1.0f, 0.0f }, 5.0f, 1.0f, 5.0f );
|
||||
|
||||
b3BodyId stackId[4];
|
||||
for ( int i = 0; i < 4; ++i )
|
||||
{
|
||||
stackId[i] = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 0.0f, 0.5f + i, 0.0f }, 0.5f, 0.5f, 0.5f );
|
||||
}
|
||||
|
||||
StepLocatorWorld( worldId, 30 );
|
||||
|
||||
// The last awake sim moves into this hole while its contacts live
|
||||
b3DestroyBody( stackId[0] );
|
||||
StepLocatorWorld( worldId, 30 );
|
||||
|
||||
// Three static boxes, one resting box each. Destroying the first moves the last static sim.
|
||||
b3BodyId staticId[3];
|
||||
for ( int i = 0; i < 3; ++i )
|
||||
{
|
||||
float x = 10.0f + 4.0f * i;
|
||||
staticId[i] = CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ x, -1.0f, 0.0f }, 1.0f, 1.0f, 1.0f );
|
||||
CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ x, 0.5f, 0.0f }, 0.5f, 0.5f, 0.5f );
|
||||
}
|
||||
|
||||
StepLocatorWorld( worldId, 30 );
|
||||
|
||||
b3DestroyBody( staticId[0] );
|
||||
StepLocatorWorld( worldId, 30 );
|
||||
|
||||
B3_UNUSED( failed );
|
||||
|
||||
uint64_t hash = b3HashWorldState( b3GetWorldFromId( worldId ) );
|
||||
b3DestroyWorld( worldId );
|
||||
return hash;
|
||||
}
|
||||
|
||||
// A kinematic body parked a hair above a pile keeps a non-touching contact to a body that falls
|
||||
// asleep, which is the one case the locator cannot name and has to leave to the cold path.
|
||||
static uint64_t LocatorKinematicScene( int workerCount, int* failed )
|
||||
{
|
||||
b3WorldId worldId = CreateLocatorWorld( workerCount );
|
||||
CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ 0.0f, -1.0f, 0.0f }, 5.0f, 1.0f, 5.0f );
|
||||
|
||||
b3BodyId boxId = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 0.0f, 0.5f, 0.0f }, 0.5f, 0.5f, 0.5f );
|
||||
|
||||
// Close enough that the fat bounds overlap and a non-touching contact exists
|
||||
b3BodyId plateId = CreateLocatorBody( worldId, b3_kinematicBody, (b3Pos){ 0.0f, 1.52f, 0.0f }, 1.0f, 0.5f, 1.0f );
|
||||
|
||||
int steps = StepUntilAsleep( worldId, boxId, 300 );
|
||||
if ( steps == 300 )
|
||||
{
|
||||
*failed = 1;
|
||||
}
|
||||
|
||||
// Drive the plate down to touch and wake the sleeper
|
||||
b3Body_SetLinearVelocity( plateId, (b3Vec3){ 0.0f, -1.0f, 0.0f } );
|
||||
StepLocatorWorld( worldId, 60 );
|
||||
|
||||
if ( b3Body_IsAwake( boxId ) == false )
|
||||
{
|
||||
*failed = 1;
|
||||
}
|
||||
|
||||
uint64_t hash = b3HashWorldState( b3GetWorldFromId( worldId ) );
|
||||
b3DestroyWorld( worldId );
|
||||
return hash;
|
||||
}
|
||||
|
||||
// Set type and disable move a body between solver sets, which changes the encoding of every
|
||||
// contact it has. A joint across two sleeping sets drives the merge path.
|
||||
static uint64_t LocatorTransferScene( int workerCount, int* failed )
|
||||
{
|
||||
b3WorldId worldId = CreateLocatorWorld( workerCount );
|
||||
CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ 0.0f, -1.0f, 0.0f }, 20.0f, 1.0f, 5.0f );
|
||||
|
||||
b3BodyId boxId = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 0.0f, 0.5f, 0.0f }, 0.5f, 0.5f, 0.5f );
|
||||
StepLocatorWorld( worldId, 30 );
|
||||
|
||||
b3Body_SetType( boxId, b3_staticBody );
|
||||
StepLocatorWorld( worldId, 5 );
|
||||
b3Body_SetType( boxId, b3_dynamicBody );
|
||||
StepLocatorWorld( worldId, 5 );
|
||||
|
||||
b3Body_Disable( boxId );
|
||||
StepLocatorWorld( worldId, 5 );
|
||||
b3Body_Enable( boxId );
|
||||
StepLocatorWorld( worldId, 30 );
|
||||
|
||||
// Two piles far enough apart to sleep in separate sets
|
||||
b3BodyId sleeperId[2];
|
||||
sleeperId[0] = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ -12.0f, 0.5f, 0.0f }, 0.5f, 0.5f, 0.5f );
|
||||
sleeperId[1] = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 12.0f, 0.5f, 0.0f }, 0.5f, 0.5f, 0.5f );
|
||||
|
||||
int steps = StepUntilAsleep( worldId, sleeperId[0], 300 );
|
||||
if ( steps == 300 || b3Body_IsAwake( sleeperId[1] ) == true )
|
||||
{
|
||||
*failed = 1;
|
||||
}
|
||||
|
||||
b3SphericalJointDef jointDef = b3DefaultSphericalJointDef();
|
||||
jointDef.base.bodyIdA = sleeperId[0];
|
||||
jointDef.base.bodyIdB = sleeperId[1];
|
||||
b3CreateSphericalJoint( worldId, &jointDef );
|
||||
|
||||
StepLocatorWorld( worldId, 30 );
|
||||
|
||||
uint64_t hash = b3HashWorldState( b3GetWorldFromId( worldId ) );
|
||||
b3DestroyWorld( worldId );
|
||||
return hash;
|
||||
}
|
||||
|
||||
// The fast flag lives on the body sim and gates contact recycling, but a flag sync rebuilds the
|
||||
// sim flags from the body row, which never carries it. A public setter called on a body in
|
||||
// flight must not drop it, or the recycle path can take a fast pair.
|
||||
static int LocatorFastFlagTest( void )
|
||||
{
|
||||
b3WorldDef worldDef = b3DefaultWorldDef();
|
||||
b3WorldId worldId = b3CreateWorld( &worldDef );
|
||||
|
||||
CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ 0.0f, -1.0f, 0.0f }, 5.0f, 1.0f, 5.0f );
|
||||
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
bodyDef.position = (b3Pos){ -20.0f, 2.0f, 0.0f };
|
||||
bodyDef.linearVelocity = (b3Vec3){ 400.0f, 0.0f, 0.0f };
|
||||
b3BodyId bulletId = b3CreateBody( worldId, &bodyDef );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.1f };
|
||||
b3CreateSphereShape( bulletId, &shapeDef, &sphere );
|
||||
|
||||
b3World_Step( worldId, 1.0f / 60.0f, 4 );
|
||||
|
||||
b3World* world = b3GetWorldFromId( worldId );
|
||||
b3Body* body = b3GetBodyFullId( world, bulletId );
|
||||
b3BodySim* sim = b3GetBodySim( world, body );
|
||||
ENSURE( ( sim->flags & b3_isFast ) != 0 );
|
||||
|
||||
// Any public setter that syncs flags. The locks have to actually change or the setter
|
||||
// returns early and never syncs.
|
||||
b3MotionLocks locks = { 0 };
|
||||
locks.angularX = true;
|
||||
b3Body_SetMotionLocks( bulletId, locks );
|
||||
|
||||
sim = b3GetBodySim( world, b3GetBodyFullId( world, bulletId ) );
|
||||
ENSURE( ( sim->flags & b3_isFast ) != 0 );
|
||||
|
||||
b3World_Step( worldId, 1.0f / 60.0f, 4 );
|
||||
|
||||
b3DestroyWorld( worldId );
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int BodySimLocatorTest( void )
|
||||
{
|
||||
typedef uint64_t ( *LocatorScene )( int workerCount, int* failed );
|
||||
LocatorScene scenes[5] = {
|
||||
LocatorSleepWakeScene,
|
||||
LocatorAwakeNeighborScene,
|
||||
LocatorDestroyScene,
|
||||
LocatorKinematicScene,
|
||||
LocatorTransferScene,
|
||||
};
|
||||
|
||||
for ( int i = 0; i < 5; ++i )
|
||||
{
|
||||
int failed = 0;
|
||||
uint64_t hash1 = scenes[i]( 1, &failed );
|
||||
uint64_t hash4 = scenes[i]( 4, &failed );
|
||||
ENSURE( failed == 0 );
|
||||
ENSURE( hash1 == hash4 );
|
||||
}
|
||||
|
||||
return LocatorFastFlagTest();
|
||||
}
|
||||
|
||||
int WorldTest( void )
|
||||
{
|
||||
RUN_SUBTEST( HelloWorld );
|
||||
|
|
@ -1300,6 +1644,7 @@ int WorldTest( void )
|
|||
RUN_SUBTEST( TestHullDatabase );
|
||||
RUN_SUBTEST( TestEnlargedProxyDestroyed );
|
||||
RUN_SUBTEST( TestCompoundShapeCount );
|
||||
RUN_SUBTEST( BodySimLocatorTest );
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in a new issue