//-------------------------------------------------------------------------------------------------- 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 ); }