//--------------------------------------------------------------------------------------------------
void q3Body::SetAngularVelocity( const q3Vec3 v )
{
// Velocity of static bodies cannot be adjusted
if ( m_flags & eStatic )
assert( false );
if ( q3Dot( v, v ) > r32( 0.0 ) )
{
SetToAwake( );
}
m_angularVelocity = v;
}
void Grabber::ApplyForce(q3Vec3 & lookAt, q3Vec3 & from){
assert(HasObject());
#if GRABBER_USE_FORCE
q3Vec3 objectPoint = mObject->GetWorldPoint(mPoint);
q3Vec3 force = (from + (lookAt * mCallback.mData.t)) - objectPoint;
// Also integrate the error over time
mIntegralError += force * 0.1;
r32 distance = q3Distance({0,0,0},force);
q3Vec3 velocity = mObject->GetLinearVelocity();
if (q3Dot(velocity, force) < 0.0f){
// If the object is moving away from us, damp its velocity
mObject->SetLinearVelocity(velocity * 0.95f);
}
// Always pull it very hard toward us
force *= distance * distance;
force += mIntegralError;
mObject->ApplyForceAtWorldPoint(force, objectPoint);
#else
// THIS BUGS WITH STATIC OBJECTS
auto tx = mObject->GetTransform();
tx.position = (from + (lookAt * mCallback.mData.t));
mObject->SetTransform(tx.position);
auto v = mObject->GetLinearVelocity();
mObject->SetLinearVelocity(q3Vec3( 0, 0, 0 ));
#endif
}
//--------------------------------------------------------------------------------------------------
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 q3ContactManager::TestCollisions( void )
{
q3ContactConstraint* constraint = m_contactList;
while( constraint )
{
q3Box *A = constraint->A;
q3Box *B = constraint->B;
q3Body *bodyA = A->body;
q3Body *bodyB = B->body;
constraint->m_flags &= ~q3ContactConstraint::eIsland;
if( !bodyA->IsAwake( ) && !bodyB->IsAwake( ) )
{
constraint = constraint->next;
continue;
}
if ( !bodyA->CanCollide( bodyB ) )
{
q3ContactConstraint* next = constraint->next;
RemoveContact( constraint );
constraint = next;
continue;
}
// Check if contact should persist
if ( !m_broadphase.TestOverlap( A->broadPhaseIndex, B->broadPhaseIndex ) )
{
q3ContactConstraint* next = constraint->next;
RemoveContact( constraint );
constraint = next;
continue;
}
q3Manifold* manifold = &constraint->manifold;
q3Manifold oldManifold = constraint->manifold;
q3Vec3 ot0 = oldManifold.tangentVectors[ 0 ];
q3Vec3 ot1 = oldManifold.tangentVectors[ 1 ];
constraint->SolveCollision( );
q3ComputeBasis( manifold->normal, manifold->tangentVectors, manifold->tangentVectors + 1 );
for ( i32 i = 0; i < manifold->contactCount; ++i )
{
q3Contact *c = manifold->contacts + i;
c->tangentImpulse[ 0 ] = c->tangentImpulse[ 1 ] = c->normalImpulse = r32( 0.0 );
u8 oldWarmStart = c->warmStarted;
c->warmStarted = u8( 0 );
for ( i32 j = 0; j < oldManifold.contactCount; ++j )
{
q3Contact *oc = oldManifold.contacts + j;
if ( c->fp.key == oc->fp.key )
{
c->normalImpulse = oc->normalImpulse;
// Attempt to re-project old friction solutions
q3Vec3 friction = ot0 * oc->tangentImpulse[ 0 ] + ot1 * oc->tangentImpulse[ 1 ];
c->tangentImpulse[ 0 ] = q3Dot( friction, manifold->tangentVectors[ 0 ] );
c->tangentImpulse[ 1 ] = q3Dot( friction, manifold->tangentVectors[ 1 ] );
c->warmStarted = q3Max( oldWarmStart, u8( oldWarmStart + 1 ) );
break;
}
}
}
if ( m_contactListener )
{
i32 now_colliding = constraint->m_flags & q3ContactConstraint::eColliding;
i32 was_colliding = constraint->m_flags & q3ContactConstraint::eWasColliding;
if ( now_colliding && !was_colliding )
m_contactListener->BeginContact( constraint );
else if ( !now_colliding && was_colliding )
m_contactListener->EndContact( constraint );
}
constraint = constraint->next;
}
}
//--------------------------------------------------------------------------------------------------
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 );
}
//--------------------------------------------------------------------------------------------------
r32 q3HalfSpace::Distance( const q3Vec3& p ) const
{
return q3Dot( normal, p ) - distance;
}
//--------------------------------------------------------------------------------------------------
void q3HalfSpace::Set( const q3Vec3& n, const q3Vec3& p )
{
normal = q3Normalize( n );
distance = q3Dot( normal, p );
}
//--------------------------------------------------------------------------------------------------
void q3HalfSpace::Set( const q3Vec3& a, const q3Vec3& b, const q3Vec3& c )
{
normal = q3Normalize( q3Cross( b - a, c - a ) );
distance = q3Dot( normal, a );
}
