isotropic test
This commit is contained in:
parent
96b3086928
commit
c1779e6ba5
10 changed files with 202 additions and 7 deletions
|
|
@ -173,6 +173,11 @@ typedef struct b3WorldDef
|
|||
/// Maximum linear speed. Usually meters per second.
|
||||
float maximumLinearSpeed;
|
||||
|
||||
/// Use this to disable gyroscopic torque on sphere-like objects automatically.
|
||||
/// A value of 0.2 means up to 20% non-isotropic inertia values are treated as
|
||||
/// isotropic.
|
||||
float isotropicInertiaTolerance;
|
||||
|
||||
/// Optional mixing callback for friction. The default uses sqrt(frictionA * frictionB).
|
||||
b3FrictionCallback* frictionCallback;
|
||||
|
||||
|
|
|
|||
29
src/body.c
29
src/body.c
|
|
@ -892,12 +892,37 @@ b3BodyTOIResult b3Body_TimeOfImpactMover( b3BodyId bodyId, b3Pos origin, const b
|
|||
return result;
|
||||
}
|
||||
|
||||
// https://en.wikipedia.org/wiki/Cauchy_stress_tensor#Stress_deviator_tensor
|
||||
static void b3UpdateIsotropicInertia( b3World* world, b3Body* body )
|
||||
{
|
||||
b3Matrix3 inertia = body->inertia;
|
||||
float mean = ( inertia.cx.x + inertia.cy.y + inertia.cz.z ) / 3.0f;
|
||||
float dx = inertia.cx.x - mean;
|
||||
float dy = inertia.cy.y - mean;
|
||||
float dz = inertia.cz.z - mean;
|
||||
float offDiagonal = inertia.cy.x * inertia.cy.x + inertia.cz.x * inertia.cz.x + inertia.cz.y * inertia.cz.y;
|
||||
float deviation = dx * dx + dy * dy + dz * dz + 2.0f * offDiagonal;
|
||||
float tolerance = world->isotropicInertiaTolerance;
|
||||
|
||||
if ( 2.0f * deviation <= tolerance * tolerance * mean * mean )
|
||||
{
|
||||
body->flags |= b3_isotropicInertia;
|
||||
}
|
||||
else
|
||||
{
|
||||
body->flags &= ~b3_isotropicInertia;
|
||||
}
|
||||
|
||||
b3SyncBodyFlags( world, body );
|
||||
}
|
||||
|
||||
void b3UpdateBodyMassData( b3World* world, b3Body* body )
|
||||
{
|
||||
b3BodySim* bodySim = b3GetBodySim( world, body );
|
||||
|
||||
// Mass is no longer dirty
|
||||
body->flags &= ~b3_dirtyMass;
|
||||
body->flags |= b3_isotropicInertia;
|
||||
b3SyncBodyFlags( world, body );
|
||||
|
||||
// Compute mass data from shapes. Each shape has its own density.
|
||||
|
|
@ -1052,6 +1077,8 @@ void b3UpdateBodyMassData( b3World* world, b3Body* body )
|
|||
bodySim->invInertiaLocal = b3Mat3_zero;
|
||||
bodySim->invInertiaWorld = b3Mat3_zero;
|
||||
}
|
||||
|
||||
b3UpdateIsotropicInertia( world, body );
|
||||
}
|
||||
|
||||
b3Pos b3Body_GetPosition( b3BodyId bodyId )
|
||||
|
|
@ -1917,6 +1944,8 @@ void b3Body_SetMassData( b3BodyId bodyId, b3MassData massData )
|
|||
bodySim->invInertiaWorld = b3Mat3_zero;
|
||||
}
|
||||
|
||||
b3UpdateIsotropicInertia( world, body );
|
||||
|
||||
// Update extents using supplied mass center.
|
||||
bodySim->minExtent = B3_HUGE;
|
||||
bodySim->maxExtent = b3Vec3_zero;
|
||||
|
|
|
|||
|
|
@ -64,6 +64,8 @@ enum b3BodyFlags
|
|||
// data still hasn't been set.
|
||||
b3_dirtyMass = 0x00008000,
|
||||
|
||||
b3_isotropicInertia = 0x00010000,
|
||||
|
||||
// All lock flags
|
||||
b3_allLocks = b3_lockLinearX | b3_lockLinearY | b3_lockLinearZ | b3_lockAngularX | b3_lockAngularY | b3_lockAngularZ,
|
||||
|
||||
|
|
|
|||
|
|
@ -332,6 +332,7 @@ b3WorldId b3CreateWorld( const b3WorldDef* def )
|
|||
world->restitutionIterations = b3ClampInt( def->restitutionIterations, 0, B3_MAX_RESTITUTION_ITERATIONS );
|
||||
world->enableRestitutionPropagation = def->enableRestitutionPropagation;
|
||||
world->maxLinearSpeed = def->maximumLinearSpeed;
|
||||
world->isotropicInertiaTolerance = b3MaxFloat( def->isotropicInertiaTolerance, 0.0f );
|
||||
world->contactSpeed = def->contactSpeed;
|
||||
world->contactHertz = def->contactHertz;
|
||||
world->contactDampingRatio = def->contactDampingRatio;
|
||||
|
|
|
|||
|
|
@ -238,6 +238,7 @@ typedef struct b3World
|
|||
int restitutionIterations;
|
||||
bool enableRestitutionPropagation;
|
||||
float maxLinearSpeed;
|
||||
float isotropicInertiaTolerance;
|
||||
float contactSpeed;
|
||||
float contactHertz;
|
||||
float contactDampingRatio;
|
||||
|
|
|
|||
|
|
@ -106,6 +106,7 @@ static void b3IntegrateVelocitiesTask( b3SolverBlock block, b3StepContext* conte
|
|||
// I * (w2 - w1) + h * cross(w2, I * w2) = 0
|
||||
// This is all done in local coordinates where the Jacobian is easier to compute.
|
||||
// This improves the simulation of long skinny bodies.
|
||||
if ( ( sim->flags & b3_isotropicInertia ) == 0 )
|
||||
{
|
||||
// Get current rotation.
|
||||
b3Quat q0 = sim->transform.q;
|
||||
|
|
|
|||
|
|
@ -25,6 +25,8 @@ b3WorldDef b3DefaultWorldDef( void )
|
|||
// 400 meters per second, faster than the speed of sound
|
||||
def.maximumLinearSpeed = 400.0f * lengthUnits;
|
||||
|
||||
def.isotropicInertiaTolerance = 0.2f;
|
||||
|
||||
def.enableSleep = true;
|
||||
def.enableContinuous = true;
|
||||
def.internalValue = B3_SECRET_COOKIE;
|
||||
|
|
|
|||
|
|
@ -33,7 +33,7 @@
|
|||
|
||||
// Snapshot image magic 'BNS3' and version
|
||||
#define B3_SNAP_MAGIC 0x33534E42u
|
||||
#define B3_SNAP_VERSION 10u // post solve restitution
|
||||
#define B3_SNAP_VERSION 11u // isotropic inertia tolerance
|
||||
|
||||
#define B3_SNAP_FLAG_VALIDATION 0x1u
|
||||
#define B3_SNAP_FLAG_DOUBLE_PRECISION 0x2u
|
||||
|
|
@ -492,6 +492,7 @@ static void b3SerWorldConfig( b3RecBuffer* buf, const b3World* world )
|
|||
b3SnapW_Bytes( buf, &world->restitutionThreshold, sizeof( float ) );
|
||||
b3SnapW_I32( buf, world->restitutionIterations );
|
||||
b3SnapW_Bytes( buf, &world->maxLinearSpeed, sizeof( float ) );
|
||||
b3SnapW_Bytes( buf, &world->isotropicInertiaTolerance, sizeof( float ) );
|
||||
b3SnapW_Bytes( buf, &world->contactSpeed, sizeof( float ) );
|
||||
b3SnapW_Bytes( buf, &world->contactHertz, sizeof( float ) );
|
||||
b3SnapW_Bytes( buf, &world->contactDampingRatio, sizeof( float ) );
|
||||
|
|
@ -519,6 +520,7 @@ static void b3DesWorldConfig( b3SnapReader* r, b3World* world )
|
|||
b3SnapR_Bytes( r, &world->restitutionThreshold, sizeof( float ) );
|
||||
world->restitutionIterations = b3SnapR_I32( r );
|
||||
b3SnapR_Bytes( r, &world->maxLinearSpeed, sizeof( float ) );
|
||||
b3SnapR_Bytes( r, &world->isotropicInertiaTolerance, sizeof( float ) );
|
||||
b3SnapR_Bytes( r, &world->contactSpeed, sizeof( float ) );
|
||||
b3SnapR_Bytes( r, &world->contactHertz, sizeof( float ) );
|
||||
b3SnapR_Bytes( r, &world->contactDampingRatio, sizeof( float ) );
|
||||
|
|
|
|||
152
test/test_body.c
152
test/test_body.c
|
|
@ -475,6 +475,156 @@ static int ShapeExtents( void )
|
|||
return 0;
|
||||
}
|
||||
|
||||
static int CompoundInertiaConsistency( void )
|
||||
{
|
||||
b3WorldDef worldDef = b3DefaultWorldDef();
|
||||
b3WorldId worldId = b3CreateWorld( &worldDef );
|
||||
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Normalize( (b3Vec3){ 0.3f, -0.8f, 0.5f } ), 1.3f );
|
||||
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
b3BoxHull boxA = b3MakeOffsetBoxHull( 0.5f, 0.25f, 0.5f, (b3Vec3){ 1.0f, 1.0f, 0.0f } );
|
||||
b3BoxHull boxB = b3MakeOffsetBoxHull( 0.3f, 0.6f, 0.2f, (b3Vec3){ -0.8f, -0.4f, 0.3f } );
|
||||
b3CreateHullShape( bodyId, &shapeDef, &boxA.base );
|
||||
b3CreateHullShape( bodyId, &shapeDef, &boxB.base );
|
||||
b3Body_ApplyMassFromShapes( bodyId );
|
||||
|
||||
b3Matrix3 local = b3Body_GetLocalRotationalInertia( bodyId );
|
||||
ENSURE( b3AbsFloat( local.cy.x ) > 0.1f );
|
||||
|
||||
b3Matrix3 r = b3MakeMatrixFromQuat( bodyDef.rotation );
|
||||
b3Matrix3 world = b3MulMM( b3MulMM( r, local ), b3Transpose( r ) );
|
||||
b3Matrix3 invWorld = b3Body_GetWorldInverseRotationalInertia( bodyId );
|
||||
b3Matrix3 product = b3MulMM( invWorld, world );
|
||||
|
||||
ENSURE_SMALL( product.cx.x - 1.0f, 1.0e-4f );
|
||||
ENSURE_SMALL( product.cy.y - 1.0f, 1.0e-4f );
|
||||
ENSURE_SMALL( product.cz.z - 1.0f, 1.0e-4f );
|
||||
ENSURE_SMALL( product.cx.y, 1.0e-4f );
|
||||
ENSURE_SMALL( product.cx.z, 1.0e-4f );
|
||||
ENSURE_SMALL( product.cy.x, 1.0e-4f );
|
||||
ENSURE_SMALL( product.cy.z, 1.0e-4f );
|
||||
ENSURE_SMALL( product.cz.x, 1.0e-4f );
|
||||
ENSURE_SMALL( product.cz.y, 1.0e-4f );
|
||||
|
||||
b3Quat q2 = b3MakeQuatFromAxisAngle( b3Normalize( (b3Vec3){ -1.0f, 0.2f, 0.4f } ), -2.1f );
|
||||
b3Body_SetTransform( bodyId, b3Body_GetPosition( bodyId ), q2 );
|
||||
r = b3MakeMatrixFromQuat( q2 );
|
||||
world = b3MulMM( b3MulMM( r, local ), b3Transpose( r ) );
|
||||
invWorld = b3Body_GetWorldInverseRotationalInertia( bodyId );
|
||||
product = b3MulMM( invWorld, world );
|
||||
ENSURE_SMALL( product.cx.x - 1.0f, 1.0e-4f );
|
||||
ENSURE_SMALL( product.cy.y - 1.0f, 1.0e-4f );
|
||||
ENSURE_SMALL( product.cz.z - 1.0f, 1.0e-4f );
|
||||
ENSURE_SMALL( product.cy.x, 1.0e-4f );
|
||||
ENSURE_SMALL( product.cz.y, 1.0e-4f );
|
||||
|
||||
b3DestroyWorld( worldId );
|
||||
return 0;
|
||||
}
|
||||
|
||||
static b3BodyId CreateSpinningHullBody( b3WorldId worldId, float x, const b3HullData* hull )
|
||||
{
|
||||
b3BodyDef bodyDef = b3DefaultBodyDef();
|
||||
bodyDef.type = b3_dynamicBody;
|
||||
bodyDef.position.x = x;
|
||||
bodyDef.rotation = b3MakeQuatFromAxisAngle( b3Normalize( (b3Vec3){ 0.3f, -0.8f, 0.5f } ), 1.3f );
|
||||
bodyDef.angularVelocity = (b3Vec3){ 1.0f, -2.0f, 3.0f };
|
||||
b3BodyId bodyId = b3CreateBody( worldId, &bodyDef );
|
||||
|
||||
b3ShapeDef shapeDef = b3DefaultShapeDef();
|
||||
b3CreateHullShape( bodyId, &shapeDef, hull );
|
||||
return bodyId;
|
||||
}
|
||||
|
||||
static bool IsIsotropic( b3BodyId bodyId )
|
||||
{
|
||||
b3World* world = b3GetWorld( bodyId.world0 );
|
||||
b3Body* body = b3GetBodyFullId( world, bodyId );
|
||||
return ( body->flags & b3_isotropicInertia ) != 0;
|
||||
}
|
||||
|
||||
// Bodies within the isotropic tolerance skip the gyroscopic solve, so in free flight the world
|
||||
// angular velocity is untouched. Anisotropic bodies precess.
|
||||
static int IsotropicInertiaTolerance( void )
|
||||
{
|
||||
b3Vec3 w0 = { 1.0f, -2.0f, 3.0f };
|
||||
|
||||
b3HullData* rock = b3CreateRock( 0.5f );
|
||||
b3HullData* complex = b3CreateComplexHull( 0.5f );
|
||||
b3BoxHull cube = b3MakeCubeHull( 0.5f );
|
||||
b3Transform cubeTransform = { b3Vec3_zero, b3MakeQuatFromAxisAngle( b3Normalize( (b3Vec3){ 1.0f, 2.0f, 3.0f } ), 0.7f ) };
|
||||
b3BoxHull rotatedCube = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, cubeTransform );
|
||||
b3BoxHull plank = b3MakeBoxHull( 0.5f, 1.0f, 0.25f );
|
||||
|
||||
{
|
||||
b3WorldDef worldDef = b3DefaultWorldDef();
|
||||
worldDef.gravity = b3Vec3_zero;
|
||||
b3WorldId worldId = b3CreateWorld( &worldDef );
|
||||
|
||||
b3BodyId rockId = CreateSpinningHullBody( worldId, 0.0f, rock );
|
||||
b3BodyId complexId = CreateSpinningHullBody( worldId, 5.0f, complex );
|
||||
b3BodyId cubeId = CreateSpinningHullBody( worldId, 10.0f, &cube.base );
|
||||
b3BodyId rotatedCubeId = CreateSpinningHullBody( worldId, 15.0f, &rotatedCube.base );
|
||||
b3BodyId plankId = CreateSpinningHullBody( worldId, 20.0f, &plank.base );
|
||||
|
||||
ENSURE( IsIsotropic( rockId ) );
|
||||
ENSURE( IsIsotropic( complexId ) );
|
||||
ENSURE( IsIsotropic( cubeId ) );
|
||||
ENSURE( IsIsotropic( rotatedCubeId ) );
|
||||
ENSURE( IsIsotropic( plankId ) == false );
|
||||
|
||||
for ( int i = 0; i < 10; ++i )
|
||||
{
|
||||
b3World_Step( worldId, 1.0f / 60.0f, 4 );
|
||||
}
|
||||
|
||||
b3Vec3 w = b3Body_GetAngularVelocity( rockId );
|
||||
ENSURE( w.x == w0.x && w.y == w0.y && w.z == w0.z );
|
||||
w = b3Body_GetAngularVelocity( complexId );
|
||||
ENSURE( w.x == w0.x && w.y == w0.y && w.z == w0.z );
|
||||
w = b3Body_GetAngularVelocity( rotatedCubeId );
|
||||
ENSURE( w.x == w0.x && w.y == w0.y && w.z == w0.z );
|
||||
w = b3Body_GetAngularVelocity( plankId );
|
||||
ENSURE( b3Length( b3Sub( w, w0 ) ) > 0.01f );
|
||||
|
||||
b3Body_SetMotionLocks( plankId, (b3MotionLocks){ .angularX = true, .angularY = true, .angularZ = true } );
|
||||
ENSURE( IsIsotropic( plankId ) );
|
||||
|
||||
b3DestroyWorld( worldId );
|
||||
}
|
||||
|
||||
{
|
||||
b3WorldDef worldDef = b3DefaultWorldDef();
|
||||
worldDef.gravity = b3Vec3_zero;
|
||||
worldDef.isotropicInertiaTolerance = 0.0f;
|
||||
b3WorldId worldId = b3CreateWorld( &worldDef );
|
||||
|
||||
b3BodyId rockId = CreateSpinningHullBody( worldId, 0.0f, rock );
|
||||
b3BodyId cubeId = CreateSpinningHullBody( worldId, 10.0f, &cube.base );
|
||||
|
||||
ENSURE( IsIsotropic( rockId ) == false );
|
||||
ENSURE( IsIsotropic( cubeId ) );
|
||||
|
||||
for ( int i = 0; i < 10; ++i )
|
||||
{
|
||||
b3World_Step( worldId, 1.0f / 60.0f, 4 );
|
||||
}
|
||||
|
||||
b3Vec3 w = b3Body_GetAngularVelocity( rockId );
|
||||
ENSURE( b3Length( b3Sub( w, w0 ) ) > 0.001f );
|
||||
|
||||
b3DestroyWorld( worldId );
|
||||
}
|
||||
|
||||
b3DestroyHull( rock );
|
||||
b3DestroyHull( complex );
|
||||
return 0;
|
||||
}
|
||||
|
||||
int BodyTest( void )
|
||||
{
|
||||
RUN_SUBTEST( FarSingleSphereMass );
|
||||
|
|
@ -486,5 +636,7 @@ int BodyTest( void )
|
|||
RUN_SUBTEST( SetMassDataZeroMass );
|
||||
RUN_SUBTEST( SetMassDataConsistentVelocity );
|
||||
RUN_SUBTEST( ShapeExtents );
|
||||
RUN_SUBTEST( CompoundInertiaConsistency );
|
||||
RUN_SUBTEST( IsotropicInertiaTolerance );
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -21,10 +21,10 @@
|
|||
#if defined( BOX3D_DOUBLE_PRECISION )
|
||||
#define RAGDOLL_SLEEP_STEP 267
|
||||
#define RAGDOLL_HASH 0x01EB8FDB
|
||||
#define WAVE_PILE_SLEEP_STEP 333
|
||||
#define WAVE_PILE_HASH 0x80D5266B
|
||||
#define WAVE_PILE_SLEEP_STEP 272
|
||||
#define WAVE_PILE_HASH 0xF7EF20F7
|
||||
#define QUERY_SPAWN_SLEEP_STEP 242
|
||||
#define QUERY_SPAWN_HASH 0x90EE3E7C
|
||||
#define QUERY_SPAWN_HASH 0xCA68A74D
|
||||
#define QUERY_SPAWN_HIT_COUNT 59
|
||||
#define QUERY_SPAWN_QUERY_HASH 0x5B4429DC
|
||||
#define MESH_DROP_SLEEP_STEP 218
|
||||
|
|
@ -32,10 +32,10 @@
|
|||
#else
|
||||
#define RAGDOLL_SLEEP_STEP 265
|
||||
#define RAGDOLL_HASH 0xB98B3638
|
||||
#define WAVE_PILE_SLEEP_STEP 288
|
||||
#define WAVE_PILE_HASH 0xFC1CD2DC
|
||||
#define WAVE_PILE_SLEEP_STEP 275
|
||||
#define WAVE_PILE_HASH 0xA6B9D38B
|
||||
#define QUERY_SPAWN_SLEEP_STEP 242
|
||||
#define QUERY_SPAWN_HASH 0x58F47348
|
||||
#define QUERY_SPAWN_HASH 0x2DA37C5A
|
||||
#define QUERY_SPAWN_HIT_COUNT 59
|
||||
#define QUERY_SPAWN_QUERY_HASH 0xE3271F3D
|
||||
#define MESH_DROP_SLEEP_STEP 218
|
||||
|
|
|
|||
Loading…
Reference in a new issue