Compare commits
14 changed files with 178 additions and 275 deletions
|
|
@ -143,8 +143,6 @@ See [`docs/hello.md`](docs/hello.md) for a minimal first program.
|
||||||
|
|
||||||
The Box3D library and samples build and run on Windows, Linux, and Mac.
|
The Box3D library and samples build and run on Windows, Linux, and Mac.
|
||||||
|
|
||||||
Box3D only supports 64-bit desktop platforms. 32-bit builds, such as Win32, are not supported.
|
|
||||||
|
|
||||||
You will need a compiler that supports C17 to build the Box3D library.
|
You will need a compiler that supports C17 to build the Box3D library.
|
||||||
|
|
||||||
You will need a compiler that supports C++20 to build the samples.
|
You will need a compiler that supports C++20 to build the samples.
|
||||||
|
|
|
||||||
|
|
@ -315,9 +315,7 @@ typedef enum b3BodyType
|
||||||
b3_bodyTypeCount,
|
b3_bodyTypeCount,
|
||||||
} b3BodyType;
|
} b3BodyType;
|
||||||
|
|
||||||
/// Motion locks to restrict the body movement. Motion locks are a hack and will likely be removed. The only robust
|
/// Motion locks to restrict the body movement
|
||||||
/// lock is fully locked rotation. You will see softened contacts and other artifacts with this sytem. Other physics
|
|
||||||
/// engines have the same problem and some have stability problems due to this approach.
|
|
||||||
/// @ingroup body
|
/// @ingroup body
|
||||||
typedef struct b3MotionLocks
|
typedef struct b3MotionLocks
|
||||||
{
|
{
|
||||||
|
|
|
||||||
|
|
@ -1211,7 +1211,7 @@ public:
|
||||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||||
bodyDef.type = b3_dynamicBody;
|
bodyDef.type = b3_dynamicBody;
|
||||||
bodyDef.name = "full";
|
bodyDef.name = "full";
|
||||||
bodyDef.position = { 0.0f, 1.0f, 3.0f };
|
bodyDef.position = { 0.0f, 1.0f, 2.0f };
|
||||||
bodyDef.motionLocks.linearX = true;
|
bodyDef.motionLocks.linearX = true;
|
||||||
bodyDef.motionLocks.linearY = true;
|
bodyDef.motionLocks.linearY = true;
|
||||||
bodyDef.motionLocks.linearZ = true;
|
bodyDef.motionLocks.linearZ = true;
|
||||||
|
|
|
||||||
|
|
@ -3093,73 +3093,3 @@ public:
|
||||||
};
|
};
|
||||||
|
|
||||||
static int sampleStaticLoad = RegisterSample( "Issues", "Static Load", StaticLoad::Create );
|
static int sampleStaticLoad = RegisterSample( "Issues", "Static Load", StaticLoad::Create );
|
||||||
|
|
||||||
class TranslationLockedPlatform : public Sample
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
explicit TranslationLockedPlatform( SampleContext* context )
|
|
||||||
: Sample( context )
|
|
||||||
{
|
|
||||||
if ( context->restart == false )
|
|
||||||
{
|
|
||||||
m_camera->SetView( 0.0f, 10.0f, 10.0f, { 0.0f, 0.5f, 0.0f } );
|
|
||||||
}
|
|
||||||
|
|
||||||
context->hertz = 50.0f;
|
|
||||||
context->subStepCount = 4;
|
|
||||||
b3World_SetGravity( m_worldId, { 0.0f, -20.0f, 0.0f } );
|
|
||||||
|
|
||||||
for ( int i = 0; i < 2; ++i )
|
|
||||||
{
|
|
||||||
float x = i == 0 ? -3.0f : 3.0f;
|
|
||||||
b3BodyDef groundDef = b3DefaultBodyDef();
|
|
||||||
groundDef.type = i == 0 ? b3_staticBody : b3_dynamicBody;
|
|
||||||
groundDef.position = { x, 0.0f, 0.0f };
|
|
||||||
groundDef.enableSleep = false;
|
|
||||||
groundDef.motionLocks.linearX = true;
|
|
||||||
groundDef.motionLocks.linearY = true;
|
|
||||||
groundDef.motionLocks.linearZ = true;
|
|
||||||
groundDef.motionLocks.angularX = true;
|
|
||||||
groundDef.motionLocks.angularY = true;
|
|
||||||
b3BodyId groundId = b3CreateBody( m_worldId, &groundDef );
|
|
||||||
|
|
||||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
|
||||||
shapeDef.density = 2.5f;
|
|
||||||
b3BoxHull box = b3MakeBoxHull( 2.0f, 0.05f, 0.5f );
|
|
||||||
b3CreateHullShape( groundId, &shapeDef, &box.base );
|
|
||||||
|
|
||||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
|
||||||
bodyDef.type = b3_dynamicBody;
|
|
||||||
bodyDef.position = { x, 0.65f, 0.0f };
|
|
||||||
bodyDef.enableSleep = false;
|
|
||||||
m_spheres[i] = b3CreateBody( m_worldId, &bodyDef );
|
|
||||||
|
|
||||||
b3Sphere sphere = { b3Vec3_zero, 0.5f };
|
|
||||||
b3CreateSphereShape( m_spheres[i], &shapeDef, &sphere );
|
|
||||||
|
|
||||||
b3MassData massData = b3Body_GetMassData( m_spheres[i] );
|
|
||||||
massData.inertia = b3MulSM( 10.0f / massData.mass, massData.inertia );
|
|
||||||
massData.mass = 10.0f;
|
|
||||||
b3Body_SetMassData( m_spheres[i], massData );
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void Render() override
|
|
||||||
{
|
|
||||||
DrawTextLine( "10 kg spheres dropped onto 0.1 m thick platforms." );
|
|
||||||
DrawTextLine( "Right platform: 1 kg, translation locked, rotation about Z free." );
|
|
||||||
DrawTextLine( "Expected: both gaps settle near zero. Negative means penetration." );
|
|
||||||
DrawTextLine( "Left (static): gap = %.3f m", (float)b3Body_GetPosition( m_spheres[0] ).y - 0.55f );
|
|
||||||
DrawTextLine( "Right (locked): gap = %.3f m", (float)b3Body_GetPosition( m_spheres[1] ).y - 0.55f );
|
|
||||||
}
|
|
||||||
|
|
||||||
static Sample* Create( SampleContext* context )
|
|
||||||
{
|
|
||||||
return new TranslationLockedPlatform( context );
|
|
||||||
}
|
|
||||||
|
|
||||||
b3BodyId m_spheres[2];
|
|
||||||
};
|
|
||||||
|
|
||||||
static int sampleTranslationLockedPlatform =
|
|
||||||
RegisterSample( "Issues", "Translation Locked Platform", TranslationLockedPlatform::Create );
|
|
||||||
|
|
|
||||||
|
|
@ -2035,9 +2035,6 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
m_motionLocks = {};
|
m_motionLocks = {};
|
||||||
m_motionLocks.linearX = true;
|
|
||||||
m_motionLocks.linearY = true;
|
|
||||||
m_motionLocks.linearZ = true;
|
|
||||||
|
|
||||||
for ( int i = 0; i < m_capacity; ++i )
|
for ( int i = 0; i < m_capacity; ++i )
|
||||||
{
|
{
|
||||||
|
|
@ -2048,7 +2045,6 @@ public:
|
||||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||||
bodyDef.type = b3_dynamicBody;
|
bodyDef.type = b3_dynamicBody;
|
||||||
bodyDef.enableSleep = false;
|
bodyDef.enableSleep = false;
|
||||||
bodyDef.motionLocks = m_motionLocks;
|
|
||||||
|
|
||||||
b3BoxHull box = b3MakeBoxHull( 1.0f, 1.0f, 0.5f );
|
b3BoxHull box = b3MakeBoxHull( 1.0f, 1.0f, 0.5f );
|
||||||
|
|
||||||
|
|
@ -2060,7 +2056,6 @@ public:
|
||||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||||
shapeDef.density = 1.0f;
|
shapeDef.density = 1.0f;
|
||||||
|
|
||||||
#if 1
|
|
||||||
// distance joint
|
// distance joint
|
||||||
{
|
{
|
||||||
assert( m_count < m_capacity );
|
assert( m_count < m_capacity );
|
||||||
|
|
@ -2091,28 +2086,30 @@ public:
|
||||||
++m_count;
|
++m_count;
|
||||||
|
|
||||||
// motor joint
|
// motor joint
|
||||||
|
#if 0
|
||||||
{
|
{
|
||||||
assert( m_count < m_capacity );
|
assert( m_bodyCount < e_count );
|
||||||
|
|
||||||
bodyDef.position = position;
|
bodyDef.position = position;
|
||||||
m_bodyIds[m_count] = b3CreateBody( m_worldId, &bodyDef );
|
m_bodyIds[m_bodyCount] = b3CreateBody( m_worldId, &bodyDef );
|
||||||
b3CreateHullShape( m_bodyIds[m_count], &shapeDef, &box.base );
|
b3CreateHullShape( m_bodyIds[m_bodyCount], &shapeDef, &box.base );
|
||||||
|
|
||||||
b3MotorJointDef jointDef = b3DefaultMotorJointDef();
|
b3MotorJointDef jointDef = b3DefaultMotorJointDef();
|
||||||
jointDef.base.bodyIdA = groundId;
|
jointDef.base.bodyIdA = groundId;
|
||||||
jointDef.base.bodyIdB = m_bodyIds[m_count];
|
jointDef.base.bodyIdB = m_bodyIds[m_bodyCount];
|
||||||
jointDef.base.localFrameA.p = position;
|
jointDef.base.localFrameA.p = position;
|
||||||
jointDef.maxSpringForce = 1000.0f;
|
jointDef.maxForce = 1000.0f;
|
||||||
jointDef.maxSpringTorque = 20.0f;
|
jointDef.maxTorque = 20.0f;
|
||||||
jointDef.base.forceThreshold = forceThreshold;
|
jointDef.base.forceThreshold = forceThreshold;
|
||||||
jointDef.base.torqueThreshold = torqueThreshold;
|
jointDef.base.torqueThreshold = torqueThreshold;
|
||||||
jointDef.base.collideConnected = true;
|
jointDef.base.collideConnected = true;
|
||||||
jointDef.base.userData = (void*)(intptr_t)m_count;
|
jointDef.base.userData = (void*)(intptr_t)m_bodyCount;
|
||||||
b3CreateMotorJoint( m_worldId, &jointDef );
|
b3CreateMotorJoint( m_worldId, &jointDef );
|
||||||
}
|
}
|
||||||
|
|
||||||
position.x += 5.0f;
|
position.x += 5.0f;
|
||||||
++m_count;
|
++m_count;
|
||||||
|
#endif
|
||||||
|
|
||||||
// prismatic joint
|
// prismatic joint
|
||||||
{
|
{
|
||||||
|
|
@ -2134,7 +2131,6 @@ public:
|
||||||
jointDef.base.userData = (void*)(intptr_t)m_count;
|
jointDef.base.userData = (void*)(intptr_t)m_count;
|
||||||
b3CreatePrismaticJoint( m_worldId, &jointDef );
|
b3CreatePrismaticJoint( m_worldId, &jointDef );
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
|
|
||||||
position.x += 5.0f;
|
position.x += 5.0f;
|
||||||
++m_count;
|
++m_count;
|
||||||
|
|
@ -2163,7 +2159,6 @@ public:
|
||||||
position.x += 5.0f;
|
position.x += 5.0f;
|
||||||
++m_count;
|
++m_count;
|
||||||
|
|
||||||
#if 1
|
|
||||||
// weld joint
|
// weld joint
|
||||||
{
|
{
|
||||||
assert( m_count < m_capacity );
|
assert( m_count < m_capacity );
|
||||||
|
|
@ -2191,8 +2186,9 @@ public:
|
||||||
++m_count;
|
++m_count;
|
||||||
|
|
||||||
// wheel joint
|
// wheel joint
|
||||||
|
#if 0
|
||||||
{
|
{
|
||||||
assert( m_count < m_capacity );
|
assert( index < e_count );
|
||||||
|
|
||||||
bodyDef.position = position;
|
bodyDef.position = position;
|
||||||
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
|
b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef );
|
||||||
|
|
@ -2204,18 +2200,18 @@ public:
|
||||||
jointDef.base.bodyIdB = bodyId;
|
jointDef.base.bodyIdB = bodyId;
|
||||||
jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot );
|
jointDef.base.localFrameA.p = b3Body_GetLocalPoint( jointDef.base.bodyIdA, pivot );
|
||||||
jointDef.base.localFrameB.p = b3Body_GetLocalPoint( jointDef.base.bodyIdB, pivot );
|
jointDef.base.localFrameB.p = b3Body_GetLocalPoint( jointDef.base.bodyIdB, pivot );
|
||||||
jointDef.suspensionHertz = 1.0f;
|
jointDef.hertz = 1.0f;
|
||||||
jointDef.suspensionDampingRatio = 0.7f;
|
jointDef.dampingRatio = 0.7f;
|
||||||
jointDef.lowerSuspensionLimit = -1.0f;
|
jointDef.lowerTranslation = -1.0f;
|
||||||
jointDef.upperSuspensionLimit = 1.0f;
|
jointDef.upperTranslation = 1.0f;
|
||||||
jointDef.enableSuspensionLimit = true;
|
jointDef.enableLimit = true;
|
||||||
jointDef.enableSpinMotor = true;
|
jointDef.enableMotor = true;
|
||||||
jointDef.maxSpinTorque = 10.0f;
|
jointDef.maxMotorTorque = 10.0f;
|
||||||
jointDef.spinSpeed = 1.0f;
|
jointDef.motorSpeed = 1.0f;
|
||||||
jointDef.base.forceThreshold = forceThreshold;
|
jointDef.base.forceThreshold = forceThreshold;
|
||||||
jointDef.base.torqueThreshold = torqueThreshold;
|
jointDef.base.torqueThreshold = torqueThreshold;
|
||||||
jointDef.base.collideConnected = true;
|
jointDef.base.collideConnected = true;
|
||||||
jointDef.base.userData = (void*)(intptr_t)m_count;
|
jointDef.base.userData = (void*)(intptr_t)index;
|
||||||
b3CreateWheelJoint( m_worldId, &jointDef );
|
b3CreateWheelJoint( m_worldId, &jointDef );
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -2293,7 +2289,7 @@ public:
|
||||||
return new MotionLocks( context );
|
return new MotionLocks( context );
|
||||||
}
|
}
|
||||||
|
|
||||||
static constexpr int m_capacity = 7;
|
static constexpr int m_capacity = 6;
|
||||||
b3BodyId m_bodyIds[m_capacity];
|
b3BodyId m_bodyIds[m_capacity];
|
||||||
int m_count;
|
int m_count;
|
||||||
b3MotionLocks m_motionLocks;
|
b3MotionLocks m_motionLocks;
|
||||||
|
|
|
||||||
99
src/body.c
99
src/body.c
|
|
@ -500,8 +500,7 @@ int b3Body_GetContactData( b3BodyId bodyId, b3ContactData* contactData, int capa
|
||||||
b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA );
|
b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA );
|
||||||
b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB );
|
b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB );
|
||||||
|
|
||||||
contactData[index].contactId =
|
contactData[index].contactId = (b3ContactId){ contact->contactId + 1, bodyId.world0, 0, contact->generation };
|
||||||
(b3ContactId){ contact->contactId + 1, bodyId.world0, 0, contact->generation };
|
|
||||||
contactData[index].shapeIdA = (b3ShapeId){ shapeA->id + 1, bodyId.world0, shapeA->generation };
|
contactData[index].shapeIdA = (b3ShapeId){ shapeA->id + 1, bodyId.world0, shapeA->generation };
|
||||||
contactData[index].shapeIdB = (b3ShapeId){ shapeB->id + 1, bodyId.world0, shapeB->generation };
|
contactData[index].shapeIdB = (b3ShapeId){ shapeB->id + 1, bodyId.world0, shapeB->generation };
|
||||||
contactData[index].manifolds = contact->manifolds;
|
contactData[index].manifolds = contact->manifolds;
|
||||||
|
|
@ -594,8 +593,8 @@ float b3Body_GetClosestPoint( b3BodyId bodyId, b3Vec3* result, b3Vec3 target )
|
||||||
return closestDistance;
|
return closestDistance;
|
||||||
}
|
}
|
||||||
|
|
||||||
b3BodyCastResult b3Body_CastRay( b3BodyId bodyId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter,
|
b3BodyCastResult b3Body_CastRay( b3BodyId bodyId, b3Pos origin, b3Vec3 translation, b3QueryFilter filter, float maxFraction,
|
||||||
float maxFraction, b3WorldTransform bodyTransform )
|
b3WorldTransform bodyTransform )
|
||||||
{
|
{
|
||||||
b3World* world = b3GetUnlockedWorld( bodyId.world0 );
|
b3World* world = b3GetUnlockedWorld( bodyId.world0 );
|
||||||
if ( world == NULL )
|
if ( world == NULL )
|
||||||
|
|
@ -661,8 +660,7 @@ b3BodyCastResult b3Body_CastRay( b3BodyId bodyId, b3Pos origin, b3Vec3 translati
|
||||||
}
|
}
|
||||||
|
|
||||||
b3BodyCastResult b3Body_CastShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation,
|
b3BodyCastResult b3Body_CastShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* proxy, b3Vec3 translation,
|
||||||
b3QueryFilter filter, float maxFraction, bool canEncroach,
|
b3QueryFilter filter, float maxFraction, bool canEncroach, b3WorldTransform bodyTransform )
|
||||||
b3WorldTransform bodyTransform )
|
|
||||||
{
|
{
|
||||||
b3World* world = b3GetUnlockedWorld( bodyId.world0 );
|
b3World* world = b3GetUnlockedWorld( bodyId.world0 );
|
||||||
if ( world == NULL )
|
if ( world == NULL )
|
||||||
|
|
@ -759,8 +757,8 @@ bool b3Body_OverlapShape( b3BodyId bodyId, b3Pos origin, const b3ShapeProxy* pro
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int planeCapacity, b3Pos origin,
|
int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int planeCapacity, b3Pos origin, const b3Capsule* mover,
|
||||||
const b3Capsule* mover, b3QueryFilter filter, b3WorldTransform bodyTransform )
|
b3QueryFilter filter, b3WorldTransform bodyTransform )
|
||||||
{
|
{
|
||||||
b3World* world = b3GetUnlockedWorld( bodyId.world0 );
|
b3World* world = b3GetUnlockedWorld( bodyId.world0 );
|
||||||
if ( world == NULL )
|
if ( world == NULL )
|
||||||
|
|
@ -813,9 +811,8 @@ int b3Body_CollideMover( b3BodyId bodyId, b3BodyPlaneResult* bodyPlanes, int pla
|
||||||
return resultCount;
|
return resultCount;
|
||||||
}
|
}
|
||||||
|
|
||||||
b3BodyTOIResult b3Body_TimeOfImpactMover( b3BodyId bodyId, b3Pos origin, const b3Capsule* mover,
|
b3BodyTOIResult b3Body_TimeOfImpactMover( b3BodyId bodyId, b3Pos origin, const b3Capsule* mover, b3Vec3 moverTranslation,
|
||||||
b3Vec3 moverTranslation, b3QueryFilter filter,
|
b3QueryFilter filter, b3WorldTransform bodyTransform1, b3WorldTransform bodyTransform2 )
|
||||||
b3WorldTransform bodyTransform1, b3WorldTransform bodyTransform2 )
|
|
||||||
{
|
{
|
||||||
b3BodyTOIResult result = { 0 };
|
b3BodyTOIResult result = { 0 };
|
||||||
result.fraction = 1.0f;
|
result.fraction = 1.0f;
|
||||||
|
|
@ -914,8 +911,7 @@ static void b3UpdateIsotropicInertia( b3World* world, b3Body* body, b3BodySim* b
|
||||||
|
|
||||||
float tolerance = world->isotropicInertiaTolerance;
|
float tolerance = world->isotropicInertiaTolerance;
|
||||||
|
|
||||||
bool fixedRotation = ( body->flags & b3_fixedRotation ) == b3_fixedRotation;
|
if ( 2.0f * deviation > 3.0f * tolerance * tolerance * mean * mean )
|
||||||
if ( fixedRotation == false && 2.0f * deviation > 3.0f * tolerance * tolerance * mean * mean )
|
|
||||||
{
|
{
|
||||||
body->flags |= b3_enableGyroscopicTorque;
|
body->flags |= b3_enableGyroscopicTorque;
|
||||||
}
|
}
|
||||||
|
|
@ -927,27 +923,6 @@ static void b3UpdateIsotropicInertia( b3World* world, b3Body* body, b3BodySim* b
|
||||||
b3SyncBodyFlags( world, body );
|
b3SyncBodyFlags( world, body );
|
||||||
}
|
}
|
||||||
|
|
||||||
static void b3UpdateInverseInertia( b3BodySim* bodySim )
|
|
||||||
{
|
|
||||||
float det = b3Det( bodySim->inertia );
|
|
||||||
B3_ASSERT( det >= 0.0f );
|
|
||||||
|
|
||||||
if ( det > 0.0f && ( bodySim->flags & b3_fixedRotation ) != b3_fixedRotation )
|
|
||||||
{
|
|
||||||
// This call is faster than b3Invert
|
|
||||||
bodySim->invInertiaLocal = b3InvertT( bodySim->inertia );
|
|
||||||
|
|
||||||
b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( bodySim->transform.q );
|
|
||||||
bodySim->invInertiaWorld =
|
|
||||||
b3MulMM( b3MulMM( rotationMatrix, bodySim->invInertiaLocal ), b3Transpose( rotationMatrix ) );
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
bodySim->invInertiaLocal = b3Mat3_zero;
|
|
||||||
bodySim->invInertiaWorld = b3Mat3_zero;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void b3UpdateBodyMassData( b3World* world, b3Body* body )
|
void b3UpdateBodyMassData( b3World* world, b3Body* body )
|
||||||
{
|
{
|
||||||
b3BodySim* bodySim = b3GetBodySim( world, body );
|
b3BodySim* bodySim = b3GetBodySim( world, body );
|
||||||
|
|
@ -1049,7 +1024,17 @@ void b3UpdateBodyMassData( b3World* world, b3Body* body )
|
||||||
b3StackFree( &world->stack, masses );
|
b3StackFree( &world->stack, masses );
|
||||||
masses = NULL;
|
masses = NULL;
|
||||||
|
|
||||||
b3UpdateInverseInertia( bodySim );
|
float det = b3Det( bodySim->inertia );
|
||||||
|
B3_ASSERT( det >= 0.0f );
|
||||||
|
|
||||||
|
if ( det > 0.0f )
|
||||||
|
{
|
||||||
|
// This call is faster than b3Invert
|
||||||
|
bodySim->invInertiaLocal = b3InvertT( bodySim->inertia );
|
||||||
|
|
||||||
|
b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( bodySim->transform.q );
|
||||||
|
bodySim->invInertiaWorld = b3MulMM( b3MulMM( rotationMatrix, bodySim->invInertiaLocal ), b3Transpose( rotationMatrix ) );
|
||||||
|
}
|
||||||
|
|
||||||
// Move center of mass.
|
// Move center of mass.
|
||||||
b3Pos oldCenter = bodySim->center;
|
b3Pos oldCenter = bodySim->center;
|
||||||
|
|
@ -1091,6 +1076,14 @@ void b3UpdateBodyMassData( b3World* world, b3Body* body )
|
||||||
edgeKey = contact->edges[edgeIndex].nextKey;
|
edgeKey = contact->edges[edgeIndex].nextKey;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Apply fixed rotation
|
||||||
|
if ( ( bodySim->flags & b3_fixedRotation ) == b3_fixedRotation )
|
||||||
|
{
|
||||||
|
bodySim->inertia = b3Mat3_zero;
|
||||||
|
bodySim->invInertiaLocal = b3Mat3_zero;
|
||||||
|
bodySim->invInertiaWorld = b3Mat3_zero;
|
||||||
|
}
|
||||||
|
|
||||||
b3UpdateIsotropicInertia( world, body, bodySim );
|
b3UpdateIsotropicInertia( world, body, bodySim );
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -1167,8 +1160,7 @@ void b3Body_SetTransform( b3BodyId bodyId, b3Pos position, b3Quat rotation )
|
||||||
bodySim->center = b3TransformWorldPoint( bodySim->transform, bodySim->localCenter );
|
bodySim->center = b3TransformWorldPoint( bodySim->transform, bodySim->localCenter );
|
||||||
|
|
||||||
b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( bodySim->transform.q );
|
b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( bodySim->transform.q );
|
||||||
bodySim->invInertiaWorld =
|
bodySim->invInertiaWorld = b3MulMM( b3MulMM( rotationMatrix, bodySim->invInertiaLocal ), b3Transpose( rotationMatrix ) );
|
||||||
b3MulMM( b3MulMM( rotationMatrix, bodySim->invInertiaLocal ), b3Transpose( rotationMatrix ) );
|
|
||||||
|
|
||||||
bodySim->rotation0 = bodySim->transform.q;
|
bodySim->rotation0 = bodySim->transform.q;
|
||||||
bodySim->center0 = bodySim->center;
|
bodySim->center0 = bodySim->center;
|
||||||
|
|
@ -1934,7 +1926,31 @@ void b3Body_SetMassData( b3BodyId bodyId, b3MassData massData )
|
||||||
state->linearVelocity = b3Add( state->linearVelocity, deltaLinear );
|
state->linearVelocity = b3Add( state->linearVelocity, deltaLinear );
|
||||||
}
|
}
|
||||||
|
|
||||||
b3UpdateInverseInertia( bodySim );
|
float det = b3Det( bodySim->inertia );
|
||||||
|
B3_ASSERT( det >= 0.0f );
|
||||||
|
|
||||||
|
if ( det > 0.0f )
|
||||||
|
{
|
||||||
|
// This call is faster than b3Invert
|
||||||
|
bodySim->invInertiaLocal = b3InvertT( bodySim->inertia );
|
||||||
|
|
||||||
|
b3Matrix3 rotationMatrix = b3MakeMatrixFromQuat( bodySim->transform.q );
|
||||||
|
bodySim->invInertiaWorld = b3MulMM( b3MulMM( rotationMatrix, bodySim->invInertiaLocal ), b3Transpose( rotationMatrix ) );
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
bodySim->invInertiaLocal = b3Mat3_zero;
|
||||||
|
bodySim->invInertiaWorld = b3Mat3_zero;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Apply fixed rotation
|
||||||
|
if ( ( bodySim->flags & b3_fixedRotation ) == b3_fixedRotation )
|
||||||
|
{
|
||||||
|
bodySim->inertia = b3Mat3_zero;
|
||||||
|
bodySim->invInertiaLocal = b3Mat3_zero;
|
||||||
|
bodySim->invInertiaWorld = b3Mat3_zero;
|
||||||
|
}
|
||||||
|
|
||||||
b3UpdateIsotropicInertia( world, body, bodySim );
|
b3UpdateIsotropicInertia( world, body, bodySim );
|
||||||
|
|
||||||
// Update extents using supplied mass center.
|
// Update extents using supplied mass center.
|
||||||
|
|
@ -2237,8 +2253,7 @@ void b3Body_Disable( b3BodyId bodyId )
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
B3_ASSERT( joint->setIndex == set->setIndex || set->setIndex == b3_staticSet ||
|
B3_ASSERT( joint->setIndex == set->setIndex || set->setIndex == b3_staticSet || joint->setIndex == b3_staticSet );
|
||||||
joint->setIndex == b3_staticSet );
|
|
||||||
|
|
||||||
// Remove joint from island
|
// Remove joint from island
|
||||||
b3UnlinkJoint( world, joint );
|
b3UnlinkJoint( world, joint );
|
||||||
|
|
@ -2430,9 +2445,7 @@ void b3Body_SetMotionLocks( b3BodyId bodyId, b3MotionLocks locks )
|
||||||
|
|
||||||
if ( fixedRotation1 != fixedRotation2 )
|
if ( fixedRotation1 != fixedRotation2 )
|
||||||
{
|
{
|
||||||
b3BodySim* bodySim = b3GetBodySim( world, body );
|
b3UpdateBodyMassData( world, body );
|
||||||
b3UpdateInverseInertia( bodySim );
|
|
||||||
b3UpdateIsotropicInertia( world, body, bodySim );
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -66,15 +66,12 @@ enum b3BodyFlags
|
||||||
|
|
||||||
b3_enableGyroscopicTorque = 0x00010000,
|
b3_enableGyroscopicTorque = 0x00010000,
|
||||||
|
|
||||||
|
// All lock flags
|
||||||
|
b3_allLocks = b3_lockLinearX | b3_lockLinearY | b3_lockLinearZ | b3_lockAngularX | b3_lockAngularY | b3_lockAngularZ,
|
||||||
|
|
||||||
// If all these flags are set then the body has fixed rotation
|
// If all these flags are set then the body has fixed rotation
|
||||||
b3_fixedRotation = b3_lockAngularX | b3_lockAngularY | b3_lockAngularZ,
|
b3_fixedRotation = b3_lockAngularX | b3_lockAngularY | b3_lockAngularZ,
|
||||||
|
|
||||||
// If all these flags are set then the body has fixed translation
|
|
||||||
b3_fixedTranslation = b3_lockLinearX | b3_lockLinearY | b3_lockLinearZ,
|
|
||||||
|
|
||||||
// All lock flags
|
|
||||||
b3_allLocks = b3_fixedRotation | b3_fixedTranslation,
|
|
||||||
|
|
||||||
// These flags are transient per time step. These may be different across b3Body, b3BodySim, and b3BodyState.
|
// These flags are transient per time step. These may be different across b3Body, b3BodySim, and b3BodyState.
|
||||||
b3_bodyTransientFlags = b3_isSpeedCapped | b3_hadTimeOfImpact,
|
b3_bodyTransientFlags = b3_isSpeedCapped | b3_hadTimeOfImpact,
|
||||||
};
|
};
|
||||||
|
|
|
||||||
24
src/joint.h
24
src/joint.h
|
|
@ -449,28 +449,20 @@ void b3SolveSphericalJoint( b3JointSim* base, b3StepContext* context, bool useBi
|
||||||
void b3SolveWeldJoint( b3JointSim* base, b3StepContext* context, bool useBias );
|
void b3SolveWeldJoint( b3JointSim* base, b3StepContext* context, bool useBias );
|
||||||
void b3SolveWheelJoint( b3JointSim* base, b3StepContext* context, bool useBias );
|
void b3SolveWheelJoint( b3JointSim* base, b3StepContext* context, bool useBias );
|
||||||
|
|
||||||
void b3DrawDistanceJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA,
|
void b3DrawDistanceJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB );
|
||||||
b3WorldTransform transformB );
|
|
||||||
void b3DrawMoverJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base, b3WorldTransform transformA,
|
void b3DrawMoverJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base, b3WorldTransform transformA,
|
||||||
b3WorldTransform transformB );
|
b3WorldTransform transformB );
|
||||||
void b3DrawPogoJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base, b3WorldTransform transformA,
|
void b3DrawPogoJoint( b3DebugDraw* draw, b3World* world, b3JointSim* base, b3WorldTransform transformA,
|
||||||
b3WorldTransform transformB, float scale );
|
b3WorldTransform transformB, float scale );
|
||||||
void b3DrawParallelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB,
|
void b3DrawParallelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale );
|
||||||
float scale );
|
void b3DrawPrismaticJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale );
|
||||||
void b3DrawPrismaticJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA,
|
void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale );
|
||||||
b3WorldTransform transformB, float scale );
|
void b3DrawSphericalJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale );
|
||||||
void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB,
|
void b3DrawWeldJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale );
|
||||||
float scale );
|
void b3DrawWheelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale );
|
||||||
void b3DrawSphericalJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA,
|
|
||||||
b3WorldTransform transformB, float scale );
|
|
||||||
void b3DrawWeldJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB,
|
|
||||||
float scale );
|
|
||||||
void b3DrawWheelJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB,
|
|
||||||
float scale );
|
|
||||||
|
|
||||||
// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
|
// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
|
||||||
static inline b3Vec3 b3SolvePointConstraint( float mA, float mB, b3Matrix3 iA, b3Matrix3 iB, b3Vec3 rA, b3Vec3 rB,
|
static inline b3Vec3 b3SolvePointConstraint( float mA, float mB, b3Matrix3 iA, b3Matrix3 iB, b3Vec3 rA, b3Vec3 rB, b3Vec3 rhs )
|
||||||
b3Vec3 rhs )
|
|
||||||
{
|
{
|
||||||
float axy = rA.z * iA.cy.y - rA.y * iA.cz.y;
|
float axy = rA.z * iA.cy.y - rA.y * iA.cz.y;
|
||||||
float axz = rA.z * iA.cy.z - rA.y * iA.cz.z;
|
float axz = rA.z * iA.cy.z - rA.y * iA.cz.z;
|
||||||
|
|
|
||||||
10
src/mesh.c
10
src/mesh.c
|
|
@ -572,7 +572,6 @@ typedef struct b3Primitive
|
||||||
b3AABB aabb;
|
b3AABB aabb;
|
||||||
b3Vec3 center;
|
b3Vec3 center;
|
||||||
int triangleIndex;
|
int triangleIndex;
|
||||||
int8_t binIndex[3];
|
|
||||||
} b3Primitive;
|
} b3Primitive;
|
||||||
|
|
||||||
_Static_assert( offsetof( b3Primitive, aabb.upperBound ) == 3 * sizeof( float ) &&
|
_Static_assert( offsetof( b3Primitive, aabb.upperBound ) == 3 * sizeof( float ) &&
|
||||||
|
|
@ -695,10 +694,6 @@ static b3Split b3SplitBinnedSah( int count, b3Primitive* primitives )
|
||||||
B3_ASSERT( 0 <= indexY && indexY < B3_BIN_COUNT );
|
B3_ASSERT( 0 <= indexY && indexY < B3_BIN_COUNT );
|
||||||
B3_ASSERT( 0 <= indexZ && indexZ < B3_BIN_COUNT );
|
B3_ASSERT( 0 <= indexZ && indexZ < B3_BIN_COUNT );
|
||||||
|
|
||||||
primitives[i].binIndex[0] = (int8_t)indexX;
|
|
||||||
primitives[i].binIndex[1] = (int8_t)indexY;
|
|
||||||
primitives[i].binIndex[2] = (int8_t)indexZ;
|
|
||||||
|
|
||||||
const float* p = &primitives[i].aabb.lowerBound.x;
|
const float* p = &primitives[i].aabb.lowerBound.x;
|
||||||
b3FloatW4 lower = b3LoadW4( p );
|
b3FloatW4 lower = b3LoadW4( p );
|
||||||
b3FloatW4 upper = b3LoadW4( p + 2 );
|
b3FloatW4 upper = b3LoadW4( p + 2 );
|
||||||
|
|
@ -777,11 +772,14 @@ static b3Split b3SplitBinnedSah( int count, b3Primitive* primitives )
|
||||||
if ( bestBucket >= 0 )
|
if ( bestBucket >= 0 )
|
||||||
{
|
{
|
||||||
int axis = split.axis;
|
int axis = split.axis;
|
||||||
|
float factor = B3_BIN_COUNT * ( 1.0f - FLT_EPSILON ) /
|
||||||
|
( b3GetByIndex( bounds.upperBound, axis ) - b3GetByIndex( bounds.lowerBound, axis ) );
|
||||||
|
|
||||||
int splitIndex = 0;
|
int splitIndex = 0;
|
||||||
for ( int i = 0; i < count; ++i )
|
for ( int i = 0; i < count; ++i )
|
||||||
{
|
{
|
||||||
int index = primitives[i].binIndex[axis];
|
b3Vec3 center = primitives[i].center;
|
||||||
|
int index = (int)( factor * ( b3GetByIndex( center, axis ) - b3GetByIndex( bounds.lowerBound, axis ) ) );
|
||||||
|
|
||||||
if ( index <= bestBucket )
|
if ( index <= bestBucket )
|
||||||
{
|
{
|
||||||
|
|
|
||||||
|
|
@ -1830,10 +1830,10 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact
|
||||||
|
|
||||||
// Make a temporary copy of previous manifolds
|
// Make a temporary copy of previous manifolds
|
||||||
int oldManifoldCount = contact->manifoldCount;
|
int oldManifoldCount = contact->manifoldCount;
|
||||||
B3_ASSERT( oldManifoldCount <= B3_MAX_MANIFOLDS );
|
b3Manifold* oldManifolds = NULL;
|
||||||
b3Manifold oldManifolds[B3_MAX_MANIFOLDS];
|
|
||||||
if ( oldManifoldCount > 0 )
|
if ( oldManifoldCount > 0 )
|
||||||
{
|
{
|
||||||
|
oldManifolds = b3Bump( &arena, oldManifoldCount * sizeof( b3Manifold ) );
|
||||||
memcpy( oldManifolds, contact->manifolds, oldManifoldCount * sizeof( b3Manifold ) );
|
memcpy( oldManifolds, contact->manifolds, oldManifoldCount * sizeof( b3Manifold ) );
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -1850,7 +1850,12 @@ bool b3ComputeMeshManifolds( b3World* world, int workerIndex, b3Contact* contact
|
||||||
memset( contact->manifolds, 0, contact->manifoldCount * sizeof( b3Manifold ) );
|
memset( contact->manifolds, 0, contact->manifoldCount * sizeof( b3Manifold ) );
|
||||||
}
|
}
|
||||||
|
|
||||||
bool consumed[B3_MAX_MANIFOLDS] = { 0 };
|
bool* consumed = NULL;
|
||||||
|
if ( oldManifoldCount > 0 )
|
||||||
|
{
|
||||||
|
consumed = b3Bump( &arena, oldManifoldCount * sizeof( bool ) );
|
||||||
|
memset( consumed, 0, oldManifoldCount * sizeof( bool ) );
|
||||||
|
}
|
||||||
|
|
||||||
b3Matrix3 matrixB = b3MakeMatrixFromQuat( xfB.q );
|
b3Matrix3 matrixB = b3MakeMatrixFromQuat( xfB.q );
|
||||||
b3Vec3 offsetA = b3SubPos( xfB.p, xfA.p );
|
b3Vec3 offsetA = b3SubPos( xfB.p, xfA.p );
|
||||||
|
|
|
||||||
|
|
@ -253,10 +253,7 @@ b3WorldId b3CreateWorld( const b3WorldDef* def )
|
||||||
|
|
||||||
world->stack = b3CreateStack( 2048 );
|
world->stack = b3CreateStack( 2048 );
|
||||||
|
|
||||||
for ( int i = 0; i < B3_MAX_MANIFOLDS; ++i )
|
b3Array_Reserve( world->manifoldAllocators, 16 );
|
||||||
{
|
|
||||||
world->manifoldAllocators[i] = b3CreateBlockAllocator( ( i + 1 ) * (int)sizeof( b3Manifold ), 0 );
|
|
||||||
}
|
|
||||||
world->manifoldAllocatorMutex = b3CreateMutex();
|
world->manifoldAllocatorMutex = b3CreateMutex();
|
||||||
|
|
||||||
b3CreateBroadPhase( &world->broadPhase, &def->capacity );
|
b3CreateBroadPhase( &world->broadPhase, &def->capacity );
|
||||||
|
|
@ -543,10 +540,11 @@ void b3DestroyWorld( b3WorldId worldId )
|
||||||
b3DestroyIdPool( &world->islandIdPool );
|
b3DestroyIdPool( &world->islandIdPool );
|
||||||
b3DestroyIdPool( &world->solverSetIdPool );
|
b3DestroyIdPool( &world->solverSetIdPool );
|
||||||
|
|
||||||
for ( int i = 0; i < B3_MAX_MANIFOLDS; ++i )
|
for ( int i = 0; i < world->manifoldAllocators.count; ++i )
|
||||||
{
|
{
|
||||||
b3DestroyBlockAllocator( world->manifoldAllocators + i );
|
b3DestroyBlockAllocator( world->manifoldAllocators.data + i );
|
||||||
}
|
}
|
||||||
|
b3Array_Destroy( world->manifoldAllocators );
|
||||||
b3DestroyMutex( world->manifoldAllocatorMutex );
|
b3DestroyMutex( world->manifoldAllocatorMutex );
|
||||||
|
|
||||||
b3DestroyStack( &world->stack );
|
b3DestroyStack( &world->stack );
|
||||||
|
|
@ -2848,18 +2846,20 @@ static uint64_t b3GetWorldByteCount( b3World* world, bool log )
|
||||||
}
|
}
|
||||||
|
|
||||||
// Manifold block allocators, one per manifold point count
|
// Manifold block allocators, one per manifold point count
|
||||||
|
int manifoldArrayBytes = b3Array_ByteCount( world->manifoldAllocators );
|
||||||
int manifoldBlockBytes = 0;
|
int manifoldBlockBytes = 0;
|
||||||
for ( int i = 0; i < B3_MAX_MANIFOLDS; ++i )
|
for ( int i = 0; i < world->manifoldAllocators.count; ++i )
|
||||||
{
|
{
|
||||||
b3BlockAllocator* allocator = world->manifoldAllocators + i;
|
b3BlockAllocator* allocator = world->manifoldAllocators.data + i;
|
||||||
manifoldBlockBytes += b3Array_ByteCount( allocator->blocks );
|
manifoldBlockBytes += b3Array_ByteCount( allocator->blocks );
|
||||||
manifoldBlockBytes += allocator->blocks.count * B3_BLOCK_SIZE * allocator->elementSize;
|
manifoldBlockBytes += allocator->blocks.count * B3_BLOCK_SIZE * allocator->elementSize;
|
||||||
}
|
}
|
||||||
total += (uint64_t)manifoldBlockBytes;
|
total += (uint64_t)manifoldArrayBytes + manifoldBlockBytes;
|
||||||
|
|
||||||
if ( log )
|
if ( log )
|
||||||
{
|
{
|
||||||
b3Log( "manifold allocators" );
|
b3Log( "manifold allocators" );
|
||||||
|
b3Log( "allocator array: %d", manifoldArrayBytes );
|
||||||
b3Log( "blocks: %d", manifoldBlockBytes );
|
b3Log( "blocks: %d", manifoldBlockBytes );
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -131,7 +131,7 @@ typedef struct b3World
|
||||||
b3ConstraintGraph constraintGraph;
|
b3ConstraintGraph constraintGraph;
|
||||||
|
|
||||||
// Manifold allocators have one allocator for each manifold count.
|
// Manifold allocators have one allocator for each manifold count.
|
||||||
b3BlockAllocator manifoldAllocators[B3_MAX_MANIFOLDS];
|
b3Array( b3BlockAllocator ) manifoldAllocators;
|
||||||
b3Mutex* manifoldAllocatorMutex;
|
b3Mutex* manifoldAllocatorMutex;
|
||||||
|
|
||||||
// The body id pool is used to allocate and recycle body ids. Body ids
|
// The body id pool is used to allocate and recycle body ids. Body ids
|
||||||
|
|
@ -353,9 +353,19 @@ static inline b3Manifold* b3AllocateManifolds( b3World* world, int count )
|
||||||
|
|
||||||
B3_ASSERT( count <= B3_MAX_MANIFOLDS );
|
B3_ASSERT( count <= B3_MAX_MANIFOLDS );
|
||||||
|
|
||||||
// Need lock because this is called from the parallel narrow phase.
|
int index = count - 1;
|
||||||
|
|
||||||
|
// Need lock because this is called from the parallel narrow phase
|
||||||
b3LockMutex( world->manifoldAllocatorMutex );
|
b3LockMutex( world->manifoldAllocatorMutex );
|
||||||
b3Manifold* manifolds = b3AllocateElement( world->manifoldAllocators + count - 1 );
|
int currentCount = world->manifoldAllocators.count;
|
||||||
|
for ( int i = currentCount; i < count; ++i )
|
||||||
|
{
|
||||||
|
b3BlockAllocator allocator = b3CreateBlockAllocator( ( i + 1 ) * sizeof( b3Manifold ), 2 * B3_BLOCK_SIZE );
|
||||||
|
b3Array_Push( world->manifoldAllocators, allocator );
|
||||||
|
}
|
||||||
|
|
||||||
|
b3BlockAllocator* allocator = b3Array_Get( world->manifoldAllocators, index );
|
||||||
|
b3Manifold* manifolds = (b3Manifold*)b3AllocateElement( allocator );
|
||||||
b3UnlockMutex( world->manifoldAllocatorMutex );
|
b3UnlockMutex( world->manifoldAllocatorMutex );
|
||||||
memset( manifolds, 0, count * sizeof( b3Manifold ) );
|
memset( manifolds, 0, count * sizeof( b3Manifold ) );
|
||||||
return manifolds;
|
return manifolds;
|
||||||
|
|
@ -370,7 +380,9 @@ static inline void b3FreeManifolds( b3World* world, b3Manifold* manifolds, int c
|
||||||
|
|
||||||
B3_ASSERT( count <= B3_MAX_MANIFOLDS );
|
B3_ASSERT( count <= B3_MAX_MANIFOLDS );
|
||||||
|
|
||||||
|
int index = count - 1;
|
||||||
b3LockMutex( world->manifoldAllocatorMutex );
|
b3LockMutex( world->manifoldAllocatorMutex );
|
||||||
b3FreeElement( world->manifoldAllocators + count - 1, manifolds );
|
b3BlockAllocator* allocator = b3Array_Get( world->manifoldAllocators, index );
|
||||||
|
b3FreeElement( allocator, manifolds );
|
||||||
b3UnlockMutex( world->manifoldAllocatorMutex );
|
b3UnlockMutex( world->manifoldAllocatorMutex );
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -5,9 +5,9 @@
|
||||||
#include "joint.h"
|
#include "joint.h"
|
||||||
#include "math_internal.h"
|
#include "math_internal.h"
|
||||||
#include "physics_world.h"
|
#include "physics_world.h"
|
||||||
#include "recording.h"
|
|
||||||
#include "solver.h"
|
#include "solver.h"
|
||||||
#include "solver_set.h"
|
#include "solver_set.h"
|
||||||
|
#include "recording.h"
|
||||||
|
|
||||||
// needed for dll export
|
// needed for dll export
|
||||||
#include "box3d/box3d.h"
|
#include "box3d/box3d.h"
|
||||||
|
|
@ -240,10 +240,10 @@ b3Vec3 b3GetRevoluteJointTorque( b3World* world, b3JointSim* base )
|
||||||
b3Quat relQ = b3InvMulQuat( joint->frameA.q, joint->frameB.q );
|
b3Quat relQ = b3InvMulQuat( joint->frameA.q, joint->frameB.q );
|
||||||
|
|
||||||
// These are needed for warm starting
|
// These are needed for warm starting
|
||||||
joint->perpAxisX = b3MulSV( 0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ),
|
joint->perpAxisX = b3MulSV(
|
||||||
b3Cross( relQ.v, b3Vec3_axisX ) ) ) );
|
0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) );
|
||||||
joint->perpAxisY = b3MulSV( 0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ),
|
joint->perpAxisY = b3MulSV(
|
||||||
b3Cross( relQ.v, b3Vec3_axisY ) ) ) );
|
0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) );
|
||||||
|
|
||||||
float axialImpulse = joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse;
|
float axialImpulse = joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse;
|
||||||
b3Vec3 angularImpulse =
|
b3Vec3 angularImpulse =
|
||||||
|
|
@ -251,8 +251,8 @@ b3Vec3 b3GetRevoluteJointTorque( b3World* world, b3JointSim* base )
|
||||||
angularImpulse = b3MulAdd( angularImpulse, axialImpulse, joint->rotationAxisZ );
|
angularImpulse = b3MulAdd( angularImpulse, axialImpulse, joint->rotationAxisZ );
|
||||||
|
|
||||||
// todo add pivot torque
|
// todo add pivot torque
|
||||||
b3Vec3 impulse = b3MulAdd(
|
b3Vec3 impulse = b3MulAdd( angularImpulse,
|
||||||
angularImpulse, joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse, axis );
|
joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse, axis );
|
||||||
b3Vec3 torque = b3MulSV( world->inv_h, impulse );
|
b3Vec3 torque = b3MulSV( world->inv_h, impulse );
|
||||||
return torque;
|
return torque;
|
||||||
}
|
}
|
||||||
|
|
@ -312,10 +312,10 @@ void b3PrepareRevoluteJoint( b3JointSim* base, b3StepContext* context )
|
||||||
|
|
||||||
{
|
{
|
||||||
// These are needed for warm starting
|
// These are needed for warm starting
|
||||||
joint->perpAxisX = b3MulSV( 0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ),
|
joint->perpAxisX = b3MulSV(
|
||||||
b3Cross( relQ.v, b3Vec3_axisX ) ) ) );
|
0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) );
|
||||||
joint->perpAxisY = b3MulSV( 0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ),
|
joint->perpAxisY = b3MulSV(
|
||||||
b3Cross( relQ.v, b3Vec3_axisY ) ) ) );
|
0.5f, b3RotateVector( joint->frameA.q, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) );
|
||||||
}
|
}
|
||||||
|
|
||||||
joint->springSoftness = b3MakeSoft( joint->hertz, joint->dampingRatio, context->h );
|
joint->springSoftness = b3MakeSoft( joint->hertz, joint->dampingRatio, context->h );
|
||||||
|
|
@ -432,8 +432,7 @@ void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBia
|
||||||
float massScale = joint->springSoftness.massScale;
|
float massScale = joint->springSoftness.massScale;
|
||||||
float impulseScale = joint->springSoftness.impulseScale;
|
float impulseScale = joint->springSoftness.impulseScale;
|
||||||
|
|
||||||
float deltaImpulse =
|
float deltaImpulse = -massScale * joint->axialMass * ( axialCdot + bias ) - impulseScale * joint->springImpulse;
|
||||||
-massScale * joint->axialMass * ( axialCdot + bias ) - impulseScale * joint->springImpulse;
|
|
||||||
joint->springImpulse += deltaImpulse;
|
joint->springImpulse += deltaImpulse;
|
||||||
|
|
||||||
axialCdot += k * deltaImpulse;
|
axialCdot += k * deltaImpulse;
|
||||||
|
|
@ -445,8 +444,7 @@ void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBia
|
||||||
float cdot = axialCdot - joint->motorSpeed;
|
float cdot = axialCdot - joint->motorSpeed;
|
||||||
float oldImpulse = joint->motorImpulse;
|
float oldImpulse = joint->motorImpulse;
|
||||||
float maxImpulse = joint->maxMotorTorque * context->h;
|
float maxImpulse = joint->maxMotorTorque * context->h;
|
||||||
float deltaImpulse =
|
float deltaImpulse = b3ClampFloat( -joint->axialMass * cdot, -maxImpulse - oldImpulse, maxImpulse - oldImpulse );
|
||||||
b3ClampFloat( -joint->axialMass * cdot, -maxImpulse - oldImpulse, maxImpulse - oldImpulse );
|
|
||||||
joint->motorImpulse = oldImpulse + deltaImpulse;
|
joint->motorImpulse = oldImpulse + deltaImpulse;
|
||||||
|
|
||||||
axialCdot += k * deltaImpulse;
|
axialCdot += k * deltaImpulse;
|
||||||
|
|
@ -478,8 +476,8 @@ void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBia
|
||||||
}
|
}
|
||||||
|
|
||||||
float oldImpulse = joint->lowerImpulse;
|
float oldImpulse = joint->lowerImpulse;
|
||||||
float deltaImpulse = b3MaxFloat(
|
float deltaImpulse =
|
||||||
-massScale * joint->axialMass * ( axialCdot + bias ) - impulseScale * oldImpulse, -oldImpulse );
|
b3MaxFloat( -massScale * joint->axialMass * ( axialCdot + bias ) - impulseScale * oldImpulse, -oldImpulse );
|
||||||
joint->lowerImpulse = oldImpulse + deltaImpulse;
|
joint->lowerImpulse = oldImpulse + deltaImpulse;
|
||||||
|
|
||||||
axialCdot += k * deltaImpulse;
|
axialCdot += k * deltaImpulse;
|
||||||
|
|
@ -507,8 +505,8 @@ void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBia
|
||||||
// sign flipped on Cdot
|
// sign flipped on Cdot
|
||||||
float cdot = -axialCdot;
|
float cdot = -axialCdot;
|
||||||
float oldImpulse = joint->upperImpulse;
|
float oldImpulse = joint->upperImpulse;
|
||||||
float deltaImpulse = b3MaxFloat(
|
float deltaImpulse =
|
||||||
-massScale * joint->axialMass * ( cdot + bias ) - impulseScale * oldImpulse, -oldImpulse );
|
b3MaxFloat( -massScale * joint->axialMass * ( cdot + bias ) - impulseScale * oldImpulse, -oldImpulse );
|
||||||
joint->upperImpulse = oldImpulse + deltaImpulse;
|
joint->upperImpulse = oldImpulse + deltaImpulse;
|
||||||
|
|
||||||
// sign flipped on applied impulse
|
// sign flipped on applied impulse
|
||||||
|
|
@ -536,10 +534,10 @@ void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBia
|
||||||
}
|
}
|
||||||
|
|
||||||
// Collinearity constraint as 2-by-2
|
// Collinearity constraint as 2-by-2
|
||||||
b3Vec3 perpAxisX = b3MulSV(
|
b3Vec3 perpAxisX =
|
||||||
0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) );
|
b3MulSV( 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisX ), b3Cross( relQ.v, b3Vec3_axisX ) ) ) );
|
||||||
b3Vec3 perpAxisY = b3MulSV(
|
b3Vec3 perpAxisY =
|
||||||
0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) );
|
b3MulSV( 0.5f, b3RotateVector( quatA, b3Add( b3MulSV( relQ.s, b3Vec3_axisY ), b3Cross( relQ.v, b3Vec3_axisY ) ) ) );
|
||||||
joint->perpAxisX = perpAxisX;
|
joint->perpAxisX = perpAxisX;
|
||||||
joint->perpAxisY = perpAxisY;
|
joint->perpAxisY = perpAxisY;
|
||||||
|
|
||||||
|
|
@ -608,29 +606,22 @@ void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBia
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB,
|
void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform transformA, b3WorldTransform transformB, float scale )
|
||||||
float scale )
|
|
||||||
{
|
{
|
||||||
b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA );
|
b3WorldTransform frameA = b3MulWorldTransforms( transformA, base->localFrameA );
|
||||||
|
|
||||||
float length1 = 0.1f * scale;
|
float length1 = 0.1f * scale;
|
||||||
draw->DrawSegmentFcn( frameA.p,
|
draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisX ) ) ), b3_colorRed,
|
||||||
b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisX ) ) ),
|
draw->context );
|
||||||
b3_colorRed, draw->context );
|
draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisY ) ) ), b3_colorGreen,
|
||||||
draw->DrawSegmentFcn( frameA.p,
|
draw->context );
|
||||||
b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisY ) ) ),
|
draw->DrawSegmentFcn( frameA.p, b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisZ ) ) ), b3_colorBlue,
|
||||||
b3_colorGreen, draw->context );
|
draw->context );
|
||||||
draw->DrawSegmentFcn( frameA.p,
|
|
||||||
b3OffsetPos( frameA.p, b3MulSV( length1, b3RotateVector( frameA.q, b3Vec3_axisZ ) ) ),
|
|
||||||
b3_colorBlue, draw->context );
|
|
||||||
|
|
||||||
b3WorldTransform frameB = b3MulWorldTransforms( transformB, base->localFrameB );
|
b3WorldTransform frameB = b3MulWorldTransforms( transformB, base->localFrameB );
|
||||||
|
|
||||||
b3RevoluteJoint* joint = &base->revoluteJoint;
|
b3RevoluteJoint* joint = &base->revoluteJoint;
|
||||||
enum
|
enum { kSliceCount = 16 };
|
||||||
{
|
|
||||||
kSliceCount = 16
|
|
||||||
};
|
|
||||||
|
|
||||||
// Twist limit
|
// Twist limit
|
||||||
if ( joint->enableLimit )
|
if ( joint->enableLimit )
|
||||||
|
|
@ -666,8 +657,8 @@ void b3DrawRevoluteJoint( b3DebugDraw* draw, b3JointSim* base, b3WorldTransform
|
||||||
{
|
{
|
||||||
draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex2 ), frameA.p, b3_colorCyan, draw->context );
|
draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex2 ), frameA.p, b3_colorCyan, draw->context );
|
||||||
}
|
}
|
||||||
draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex1 ), b3TransformWorldPoint( frameA, vertex2 ),
|
draw->DrawSegmentFcn( b3TransformWorldPoint( frameA, vertex1 ), b3TransformWorldPoint( frameA, vertex2 ), b3_colorCyan,
|
||||||
b3_colorCyan, draw->context );
|
draw->context );
|
||||||
}
|
}
|
||||||
|
|
||||||
float twistAngle = b3GetTwistAngle( relQ );
|
float twistAngle = b3GetTwistAngle( relQ );
|
||||||
|
|
|
||||||
|
|
@ -182,13 +182,6 @@ static int DeferredMassExtents( void )
|
||||||
// Diagonal inertia with inverses that are exact in float, so tolerances stay tight.
|
// Diagonal inertia with inverses that are exact in float, so tolerances stay tight.
|
||||||
static const b3Matrix3 kDiagInertia = { { 2.0f, 0.0f, 0.0f }, { 0.0f, 4.0f, 0.0f }, { 0.0f, 0.0f, 8.0f } };
|
static const b3Matrix3 kDiagInertia = { { 2.0f, 0.0f, 0.0f }, { 0.0f, 4.0f, 0.0f }, { 0.0f, 0.0f, 8.0f } };
|
||||||
|
|
||||||
static bool HasGyroscopicTorque( b3BodyId bodyId )
|
|
||||||
{
|
|
||||||
b3World* world = b3GetWorld( bodyId.world0 );
|
|
||||||
b3Body* body = b3GetBodyFullId( world, bodyId );
|
|
||||||
return ( body->flags & b3_enableGyroscopicTorque ) != 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
static int SetMassDataRoundTrip( void )
|
static int SetMassDataRoundTrip( void )
|
||||||
{
|
{
|
||||||
b3WorldDef worldDef = b3DefaultWorldDef();
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
||||||
|
|
@ -284,8 +277,8 @@ static int SetMassDataWorldInertiaRotated( void )
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Fixed rotation zeroes only the inverse inertia. The caller's tensor survives so unlocking restores it
|
// Fixed rotation must leave the mass intact but drive the whole angular inertia to zero, even when the
|
||||||
// without recomputing mass from the shapes.
|
// caller hands in a real tensor.
|
||||||
static int SetMassDataFixedRotation( void )
|
static int SetMassDataFixedRotation( void )
|
||||||
{
|
{
|
||||||
b3WorldDef worldDef = b3DefaultWorldDef();
|
b3WorldDef worldDef = b3DefaultWorldDef();
|
||||||
|
|
@ -305,9 +298,9 @@ static int SetMassDataFixedRotation( void )
|
||||||
ENSURE_SMALL( b3Body_GetInverseMass( bodyId ) - 0.2f, 1e-6f );
|
ENSURE_SMALL( b3Body_GetInverseMass( bodyId ) - 0.2f, 1e-6f );
|
||||||
|
|
||||||
b3Matrix3 localInertia = b3Body_GetLocalRotationalInertia( bodyId );
|
b3Matrix3 localInertia = b3Body_GetLocalRotationalInertia( bodyId );
|
||||||
ENSURE_SMALL( localInertia.cx.x - 2.0f, 1e-6f );
|
ENSURE_SMALL( localInertia.cx.x, 1e-6f );
|
||||||
ENSURE_SMALL( localInertia.cy.y - 4.0f, 1e-6f );
|
ENSURE_SMALL( localInertia.cy.y, 1e-6f );
|
||||||
ENSURE_SMALL( localInertia.cz.z - 8.0f, 1e-6f );
|
ENSURE_SMALL( localInertia.cz.z, 1e-6f );
|
||||||
|
|
||||||
b3Matrix3 invWorld = b3Body_GetWorldInverseRotationalInertia( bodyId );
|
b3Matrix3 invWorld = b3Body_GetWorldInverseRotationalInertia( bodyId );
|
||||||
ENSURE_SMALL( invWorld.cx.x, 1e-6f );
|
ENSURE_SMALL( invWorld.cx.x, 1e-6f );
|
||||||
|
|
@ -315,36 +308,9 @@ static int SetMassDataFixedRotation( void )
|
||||||
ENSURE_SMALL( invWorld.cz.z, 1e-6f );
|
ENSURE_SMALL( invWorld.cz.z, 1e-6f );
|
||||||
|
|
||||||
b3MassData md = b3Body_GetMassData( bodyId );
|
b3MassData md = b3Body_GetMassData( bodyId );
|
||||||
ENSURE_SMALL( md.inertia.cx.x - 2.0f, 1e-6f );
|
ENSURE_SMALL( md.inertia.cx.x, 1e-6f );
|
||||||
ENSURE_SMALL( md.inertia.cy.y - 4.0f, 1e-6f );
|
ENSURE_SMALL( md.inertia.cy.y, 1e-6f );
|
||||||
ENSURE_SMALL( md.inertia.cz.z - 8.0f, 1e-6f );
|
ENSURE_SMALL( md.inertia.cz.z, 1e-6f );
|
||||||
|
|
||||||
// The tensor is anisotropic but the body cannot rotate
|
|
||||||
ENSURE( HasGyroscopicTorque( bodyId ) == false );
|
|
||||||
|
|
||||||
b3Body_SetMotionLocks( bodyId, (b3MotionLocks){ 0 } );
|
|
||||||
|
|
||||||
ENSURE_SMALL( b3Body_GetMass( bodyId ) - 5.0f, 1e-6f );
|
|
||||||
|
|
||||||
invWorld = b3Body_GetWorldInverseRotationalInertia( bodyId );
|
|
||||||
ENSURE_SMALL( invWorld.cx.x - 0.5f, 1e-6f );
|
|
||||||
ENSURE_SMALL( invWorld.cy.y - 0.25f, 1e-6f );
|
|
||||||
ENSURE_SMALL( invWorld.cz.z - 0.125f, 1e-6f );
|
|
||||||
|
|
||||||
md = b3Body_GetMassData( bodyId );
|
|
||||||
ENSURE_SMALL( md.inertia.cx.x - 2.0f, 1e-6f );
|
|
||||||
ENSURE_SMALL( md.inertia.cy.y - 4.0f, 1e-6f );
|
|
||||||
ENSURE_SMALL( md.inertia.cz.z - 8.0f, 1e-6f );
|
|
||||||
|
|
||||||
ENSURE( HasGyroscopicTorque( bodyId ) );
|
|
||||||
|
|
||||||
b3Body_SetMotionLocks( bodyId, bodyDef.motionLocks );
|
|
||||||
|
|
||||||
invWorld = b3Body_GetWorldInverseRotationalInertia( bodyId );
|
|
||||||
ENSURE_SMALL( invWorld.cx.x, 1e-6f );
|
|
||||||
ENSURE_SMALL( invWorld.cy.y, 1e-6f );
|
|
||||||
ENSURE_SMALL( invWorld.cz.z, 1e-6f );
|
|
||||||
ENSURE( HasGyroscopicTorque( bodyId ) == false );
|
|
||||||
|
|
||||||
b3DestroyWorld( worldId );
|
b3DestroyWorld( worldId );
|
||||||
return 0;
|
return 0;
|
||||||
|
|
@ -574,6 +540,13 @@ static b3BodyId CreateSpinningHullBody( b3WorldId worldId, float x, const b3Hull
|
||||||
return bodyId;
|
return bodyId;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static bool HasGyroscopicTorque( b3BodyId bodyId )
|
||||||
|
{
|
||||||
|
b3World* world = b3GetWorld( bodyId.world0 );
|
||||||
|
b3Body* body = b3GetBodyFullId( world, bodyId );
|
||||||
|
return ( body->flags & b3_enableGyroscopicTorque ) != 0;
|
||||||
|
}
|
||||||
|
|
||||||
// Bodies within the isotropic tolerance skip the gyroscopic solve, so in free flight the world
|
// Bodies within the isotropic tolerance skip the gyroscopic solve, so in free flight the world
|
||||||
// angular velocity is untouched. Anisotropic bodies precess.
|
// angular velocity is untouched. Anisotropic bodies precess.
|
||||||
static int IsotropicInertiaTolerance( void )
|
static int IsotropicInertiaTolerance( void )
|
||||||
|
|
|
||||||
Loading…
Reference in a new issue