//--------------------------------------------------------------------------------------------------
void q3Body::SetToSleep( )
{
m_flags &= ~eAwake;
m_sleepTime = r32( 0.0 );
q3Identity( m_linearVelocity );
q3Identity( m_angularVelocity );
q3Identity( m_force );
q3Identity( m_torque );
}
//--------------------------------------------------------------------------------------------------
// q3Body
//--------------------------------------------------------------------------------------------------
q3Body::q3Body( const q3BodyDef& def, q3Scene* scene )
{
m_linearVelocity = def.linearVelocity;
m_angularVelocity = def.angularVelocity;
q3Identity( m_force );
q3Identity( m_torque );
m_q.Set( q3Normalize( def.axis ), def.angle );
m_tx.rotation = m_q.ToMat3( );
m_tx.position = def.position;
m_sleepTime = r32( 0.0 );
m_gravityScale = def.gravityScale;
m_layers = def.layers;
m_userData = def.userData;
m_scene = scene;
m_flags = 0;
m_linearDamping = def.linearDamping;
m_angularDamping = def.angularDamping;
if ( def.bodyType == eDynamicBody )
m_flags |= q3Body::eDynamic;
else
{
if ( def.bodyType == eStaticBody )
{
m_flags |= q3Body::eStatic;
q3Identity( m_linearVelocity );
q3Identity( m_angularVelocity );
q3Identity( m_force );
q3Identity( m_torque );
}
else if ( def.bodyType == eKinematicBody )
m_flags |= q3Body::eKinematic;
}
if ( def.allowSleep )
m_flags |= eAllowSleep;
if ( def.awake )
m_flags |= eAwake;
if ( def.active )
m_flags |= eActive;
if ( def.lockAxisX )
m_flags |= eLockAxisX;
if ( def.lockAxisY )
m_flags |= eLockAxisY;
if ( def.lockAxisZ )
m_flags |= eLockAxisZ;
m_boxes = NULL;
m_contactList = NULL;
}
//--------------------------------------------------------------------------------------------------
void q3Box::ComputeMass( q3MassData* md ) const
{
// Calculate inertia tensor
r32 ex2 = r32( 4.0 ) * e.x * e.x;
r32 ey2 = r32( 4.0 ) * e.y * e.y;
r32 ez2 = r32( 4.0 ) * e.z * e.z;
r32 mass = r32( 8.0 ) * e.x * e.y * e.z * density;
r32 x = r32( 1.0 / 12.0 ) * mass * (ey2 + ez2);
r32 y = r32( 1.0 / 12.0 ) * mass * (ex2 + ez2);
r32 z = r32( 1.0 / 12.0 ) * mass * (ex2 + ey2);
q3Mat3 I = q3Diagonal( x, y, z );
// Transform tensor to local space
I = local.rotation * I * q3Transpose( local.rotation );
q3Mat3 identity;
q3Identity( identity );
I += (identity * q3Dot( local.position, local.position ) - q3OuterProduct( local.position, local.position )) * mass;
md->center = local.position;
md->inertia = I;
md->mass = mass;
}
void Demo( )
{
scene.SetIterations( 20 );
scene.SetAllowSleep( true );
scene.RemoveAllBodies( );
//seed = 23;
//printf( "seed: %d\n", seed );
srand( seed++ );
// Create the floor
q3BodyDef bodyDef;
//bodyDef.axis.Set( q3RandomFloat( -1.0f, 1.0f ), q3RandomFloat( -1.0f, 1.0f ), q3RandomFloat( -1.0f, 1.0f ) );
//bodyDef.angle = q3PI * q3RandomFloat( -1.0f, 1.0f );
q3Body* body = scene.CreateBody( bodyDef );
q3BoxDef boxDef;
boxDef.SetRestitution( 0 );
q3Transform tx;
q3Identity( tx );
boxDef.Set( tx, q3Vec3( 50.0f, 1.0f, 50.0f ) );
body->AddBox( boxDef );
// Create boxes
for ( i32 i = 0; i < 10; ++i )
{
bodyDef.position.Set( 0.0f, 1.2f * (i + 1), -0.0f );
//bodyDef.axis.Set( 0.0f, 1.0f, 0.0f );
//bodyDef.angle = q3PI * q3RandomFloat( -1.0f, 1.0f );
//bodyDef.angularVelocity.Set( 3.0f, 3.0f, 3.0f );
//bodyDef.linearVelocity.Set( 2.0f, 0.0f, 0.0f );
bodyDef.bodyType = eDynamicBody;
body = scene.CreateBody( bodyDef );
boxDef.Set( tx, q3Vec3( 1.0f, 1.0f, 1.0f ) );
body->AddBox( boxDef );
}
}
//--------------------------------------------------------------------------------------------------
void q3Body::CalculateMassData( )
{
q3Mat3 inertia = q3Diagonal( r32( 0.0 ) );
m_invInertiaModel = q3Diagonal( r32( 0.0 ) );
m_invInertiaWorld = q3Diagonal( r32( 0.0 ) );
m_invMass = r32( 0.0 );
m_mass = r32( 0.0 );
r32 mass = r32( 0.0 );
if ( m_flags & eStatic || m_flags &eKinematic )
{
q3Identity( m_localCenter );
m_worldCenter = m_tx.position;
return;
}
q3Vec3 lc;
q3Identity( lc );
for ( q3Box* box = m_boxes; box; box = box->next)
{
if ( box->density == r32( 0.0 ) )
continue;
q3MassData md;
box->ComputeMass( &md );
mass += md.mass;
inertia += md.inertia;
lc += md.center * md.mass;
}
if ( mass > r32( 0.0 ) )
{
m_mass = mass;
m_invMass = r32( 1.0 ) / mass;
lc *= m_invMass;
q3Mat3 identity;
q3Identity( identity );
inertia -= (identity * q3Dot( lc, lc ) - q3OuterProduct( lc, lc )) * mass;
m_invInertiaModel = q3Inverse( inertia );
if ( m_flags & eLockAxisX )
q3Identity( m_invInertiaModel.ex );
if ( m_flags & eLockAxisY )
q3Identity( m_invInertiaModel.ey );
if ( m_flags & eLockAxisZ )
q3Identity( m_invInertiaModel.ez );
}
else
{
// Force all dynamic bodies to have some mass
m_invMass = r32( 1.0 );
m_invInertiaModel = q3Diagonal( r32( 0.0 ) );
m_invInertiaWorld = q3Diagonal( r32( 0.0 ) );
}
m_localCenter = lc;
m_worldCenter = q3Mul( m_tx, lc );
}
