//--------------------------------------------------------------------------------------------------
void q3Box::Render( const q3Transform& tx, bool awake, q3Render* render ) const
{
q3Transform world = q3Mul( tx, local );
q3Vec3 vertices[ 8 ] = {
q3Vec3( -e.x, -e.y, -e.z ),
q3Vec3( -e.x, -e.y, e.z ),
q3Vec3( -e.x, e.y, -e.z ),
q3Vec3( -e.x, e.y, e.z ),
q3Vec3( e.x, -e.y, -e.z ),
q3Vec3( e.x, -e.y, e.z ),
q3Vec3( e.x, e.y, -e.z ),
q3Vec3( e.x, e.y, e.z )
};
for ( i32 i = 0; i < 36; i += 3 )
{
q3Vec3 a = q3Mul( world, vertices[ kBoxIndices[ i ] ] );
q3Vec3 b = q3Mul( world, vertices[ kBoxIndices[ i + 1 ] ] );
q3Vec3 c = q3Mul( world, vertices[ kBoxIndices[ i + 2 ] ] );
q3Vec3 n = q3Normalize( q3Cross( b - a, c - a ) );
//render->SetPenColor( 0.2f, 0.4f, 0.7f, 0.5f );
//render->SetPenPosition( a.x, a.y, a.z );
//render->Line( b.x, b.y, b.z );
//render->Line( c.x, c.y, c.z );
//render->Line( a.x, a.y, a.z );
render->SetTriNormal( n.x, n.y, n.z );
render->Triangle( a.x, a.y, a.z, b.x, b.y, b.z, c.x, c.y, c.z );
}
}
//--------------------------------------------------------------------------------------------------
void q3Box::ComputeAABB( const q3Transform& tx, q3AABB* aabb ) const
{
q3Transform world = q3Mul( tx, local );
q3Vec3 v[ 8 ] = {
q3Vec3( -e.x, -e.y, -e.z ),
q3Vec3( -e.x, -e.y, e.z ),
q3Vec3( -e.x, e.y, -e.z ),
q3Vec3( -e.x, e.y, e.z ),
q3Vec3( e.x, -e.y, -e.z ),
q3Vec3( e.x, -e.y, e.z ),
q3Vec3( e.x, e.y, -e.z ),
q3Vec3( e.x, e.y, e.z )
};
for ( i32 i = 0; i < 8; ++i )
v[ i ] = q3Mul( world, v[ i ] );
q3Vec3 min( Q3_R32_MAX, Q3_R32_MAX, Q3_R32_MAX );
q3Vec3 max( -Q3_R32_MAX, -Q3_R32_MAX, -Q3_R32_MAX );
for ( i32 i = 0; i < 8; ++i )
{
min = q3Min( min, v[ i ] );
max = q3Max( max, v[ i ] );
}
aabb->min = min;
aabb->max = max;
}
//--------------------------------------------------------------------------------------------------
bool q3Box::Raycast( const q3Transform& tx, q3RaycastData* raycast ) const
{
q3Transform world = q3Mul( tx, local );
q3Vec3 d = q3MulT( world.rotation, raycast->dir );
q3Vec3 p = q3MulT( world, raycast->start );
const r32 epsilon = r32( 1.0e-8 );
r32 tmin = 0;
r32 tmax = raycast->t;
// t = (e[ i ] - p.[ i ]) / d[ i ]
r32 t0;
r32 t1;
q3Vec3 n0;
for ( int i = 0; i < 3; ++i )
{
// Check for ray parallel to and outside of AABB
if ( q3Abs( d[ i ] ) < epsilon )
{
// Detect separating axes
if ( p[ i ] < -e[ i ] || p[ i ] > e[ i ] )
{
return false;
}
}
else
{
r32 d0 = r32( 1.0 ) / d[ i ];
r32 s = q3Sign( d[ i ] );
r32 ei = e[ i ] * s;
q3Vec3 n( 0, 0, 0 );
n[ i ] = -s;
t0 = -(ei + p[ i ]) * d0;
t1 = (ei - p[ i ]) * d0;
if ( t0 > tmin )
{
n0 = n;
tmin = t0;
}
tmax = q3Min( tmax, t1 );
if ( tmin > tmax )
{
return false;
}
}
}
raycast->normal = q3Mul( world.rotation, n0 );
raycast->toi = tmin;
return true;
}
//--------------------------------------------------------------------------------------------------
void q3Body::SynchronizeProxies( )
{
q3BroadPhase* broadphase = &m_scene->m_contactManager.m_broadphase;
m_tx.position = m_worldCenter - q3Mul( m_tx.rotation, m_localCenter );
q3AABB aabb;
q3Transform tx = m_tx;
q3Box* box = m_boxes;
while ( box )
{
box->ComputeAABB( tx, &aabb );
broadphase->Update( box->broadPhaseIndex, aabb );
box = box->next;
}
}
//--------------------------------------------------------------------------------------------------
// q3Box
//--------------------------------------------------------------------------------------------------
bool q3Box::TestPoint( const q3Transform& tx, const q3Vec3& p ) const
{
q3Transform world = q3Mul( tx, local );
q3Vec3 p0 = q3MulT( world, p );
for ( int i = 0; i < 3; ++i )
{
r32 d = p0[ i ];
r32 ei = e[ i ];
if ( d > ei || d < -ei )
{
return false;
}
}
return true;
}
//--------------------------------------------------------------------------------------------------
const q3Box* q3Body::AddBox( const q3BoxDef& def )
{
q3AABB aabb;
q3Box* box = (q3Box*)m_scene->m_heap.Allocate( sizeof( q3Box ) );
box->local = def.m_tx;
box->e = def.m_e;
box->next = m_boxes;
m_boxes = box;
box->ComputeAABB( q3Mul( m_tx, box->local ), &aabb );
box->body = this;
box->friction = def.m_friction;
box->restitution = def.m_restitution;
box->density = def.m_density;
box->sensor = def.m_sensor;
CalculateMassData( );
m_scene->m_contactManager.m_broadphase.InsertBox( box, aabb );
m_scene->m_newBox = true;
return box;
}
//--------------------------------------------------------------------------------------------------
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 );
}
//--------------------------------------------------------------------------------------------------
const q3Vec3 q3Body::GetWorldVector( const q3Vec3& v ) const
{
return q3Mul( m_tx.rotation, v );
}
//--------------------------------------------------------------------------------------------------
const q3Vec3 q3Body::GetWorldPoint( const q3Vec3& p ) const
{
return q3Mul( m_tx, p );
}
