Real-Time Physics Simulation Forum

Code: Select all

	if (fabsf(x)>=fabsf(y))
	{
		// x or z is the largest magnitude component, swap them
		AMG3DScalar sInvLength = 1 / (x*x + z*z);
		(*pTangent1).x = -z * sInvLength;
		(*pTangent1).y = 0.0f;
		(*pTangent1).z = x * sInvLength;
		(*pTangent2).x = y * (*pTangent1).z;
		(*pTangent2).y = z*(*pTangent1).x - x*(*pTangent1).z;
		(*pTangent2).z = -y * (*pTangent1).x;
	}
	else
	{
		// y or z is the largest magnitude component, swap them
		AMG3DScalar sInvLength = 1 / (y*y + z*z);
		(*pTangent1).x = 0.0f;
		(*pTangent1).y = z * sInvLength;
		(*pTangent1).z = -y * sInvLength;
		(*pTangent2).x = y*(*pTangent1).z - z*(*pTangent1).y;
		(*pTangent2).y = -x * (*pTangent1).z;
		(*pTangent2).z = x * (*pTangent1).y;
	}

Code: Select all

inline void GenerateOrthonormalBasis( const Vec3& w, Vec3 *u, Vec3 *v )
{
  real absX = std::abs( w.x );
  if(absX > std::abs( w.y ) &&
     absX > std::abs( w.z ))
  {
    u->x = -w.y;
    u->y = w.x;
    u->z = 0.0f;
  }
  else
  {
    u->x = 0.0f;
    u->y = w.z;
    u->z = -w.y;
  }

  u->NormalizeThis( );
  *v = Cross( w, *u );
  v->NormalizeThis( );
}

Code: Select all

void AMG3DRigidBodyCollisionResponder::computeMasses(const AMG3DScalar& dt)
{
	if(m_bSkipCollision)
		return;

	if(dt<=0.0f)
		return;
	
	const AMG3DScalar k_allowedPenetration = 0.01f;
	AMG3DScalar k_biasFactor = (AMG3DPhysicsWorld::g_AMG3D_bPositionCorrection) ? 0.1f : 0.0f;

	for(int i=0; i<m_cdContactData.iNumContacts; ++i) {
		AMG3DVector4 normal = m_cdContactData.contacts[i].vContactNormal;
		
		m_rap[i] = m_cdContactData.contacts[i].vContactPoint - m_pRigidBody1->m_vPosition;
		m_rbp[i] = m_cdContactData.contacts[i].vContactPoint - m_pRigidBody2->m_vPosition;

		m_rapcrossn[i] = m_rap[i].cross(normal);
		m_rbpcrossn[i] = m_rbp[i].cross(normal);

		// Compute the normal mass
		AMG3DScalar Kn = m_pRigidBody1->m_sInvMass + m_pRigidBody2->m_sInvMass;
		Kn += ( m_pRigidBody1->m_matInvIWorld * m_rapcrossn[i].cross(m_rap[i]) + 
				m_pRigidBody2->m_matInvIWorld*m_rbpcrossn[i].cross(m_rbp[i]) ).dot(normal);
		m_cdContactData.contacts[i].massNormal = 1.0f/Kn;

		// Compute the bias (prevents sinking, useful for stacking)
		m_cdContactData.contacts[i].bias = k_biasFactor / dt * 
							AMG3D_MATH_MAX(0.0f, m_cdContactData.contacts[i].sPenetrationDepth - k_allowedPenetration);

		// Calculate the tangent vector. To Do: Research relative velocity tangent vector
		AMG3DVector4 vap = m_pRigidBody1->m_vVelocity + m_pRigidBody1->m_vAngularVelocity.cross(m_rap[i]);
		AMG3DVector4 vbp = m_pRigidBody2->m_vVelocity + m_pRigidBody2->m_vAngularVelocity.cross(m_rbp[i]);

		// Relative velocity
		AMG3DVector4 vab = vap - vbp;

		AMG3DVector4 vabn = normal*(vab.dot(normal));
		AMG3DVector4 vabt = vab - vabn;

		//if(vabt.magnitude()>0) {
		//	vabt.normalize();
		//	m_t1 = vabt;
		//	AMG3DVector4Cross(&m_t2, normal, m_t1);
		//}
		//else {
			
			normal.buildOrthoBasis(&m_t1, &m_t2);
		//}

		m_rapcrosst1[i] = m_rap[i].cross(m_t1);
		m_rbpcrosst1[i] = m_rbp[i].cross(m_t1);

		// Compute the tangent mass in the first direction
		AMG3DScalar Kt1 = m_pRigidBody1->m_sInvMass + m_pRigidBody2->m_sInvMass;
		Kt1 += ( m_pRigidBody1->m_matInvIWorld * m_rapcrosst1[i].cross(m_rap[i]) + 
				 m_pRigidBody2->m_matInvIWorld * m_rbpcrosst1[i].cross(m_rbp[i]) ).dot(m_t1);
		m_cdContactData.contacts[i].massTangent1 = 1.0f/Kt1;

		m_rapcrosst2[i] = m_rap[i].cross(m_t2);
		m_rbpcrosst2[i] = m_rbp[i].cross(m_t2);

		// Compute the tangent mass in the second direction
		AMG3DScalar Kt2 = m_pRigidBody1->m_sInvMass + m_pRigidBody2->m_sInvMass;
		Kt2 += ( m_pRigidBody1->m_matInvIWorld * m_rapcrosst2[i].cross(m_rap[i]) + 
				 m_pRigidBody2->m_matInvIWorld * m_rbpcrosst2[i].cross(m_rbp[i]) ).dot(m_t2);
		m_cdContactData.contacts[i].massTangent2 = 1.0f/Kt2;
		
		if(AMG3DPhysicsWorld::g_AMG3D_bWarmStarting) {
			//AMG3DVector4 P = m_cdContactData.contacts[i].Pn*normal + m_cdContactData.contacts[i].Pt1*m_t1 + m_cdContactData.contacts[i].Pt2*m_t2;

			// Project old friction impulse onto new tangent basis
			AMG3DVector4 oldt1 = m_cdContactData.contacts[i].Pt1 * m_cdContactData.contacts[i].t1; 
			AMG3DVector4 oldt2 = m_cdContactData.contacts[i].Pt2 * m_cdContactData.contacts[i].t2;

			AMG3DVector4 P = m_cdContactData.contacts[i].Pn*normal + oldt1.dot(m_t1) * m_t1 + oldt2.dot(m_t2) * m_t2;

			// Store old tangent vectors to be used for next frame
			m_cdContactData.contacts[i].t1 = m_t1;
			m_cdContactData.contacts[i].t2 = m_t2;

			m_pRigidBody1->m_vVelocity			+= P*m_pRigidBody1->m_sInvMass;
			m_pRigidBody1->m_vAngularVelocity	+= m_pRigidBody1->m_matInvIWorld*m_rap[i].cross(P);
		
			m_pRigidBody2->m_vVelocity			-= P*m_pRigidBody2->m_sInvMass;
			m_pRigidBody2->m_vAngularVelocity	-= m_pRigidBody2->m_matInvIWorld*m_rbp[i].cross(P);
		}	
	}
}

Code: Select all

void AMG3DRBCuboid::updatePosition(const AMG3DScalar& dt)
{
	if(dt<=0.0f)
		return;

	// May want to update the velocity of a kinematic body
	// e.g. Camera/player interaction
//	if(m_sMass<=0.0f)
//		return;

	// Dynamic and kinematic bodies can move
	if(m_eType==AMG3D_PHYSICS_RIGID_BODY_TYPE_STATIC)
		return;

	if(m_eState==AMG3D_PHYSICS_RIGID_BODY_STATE_SLEEPING)
		return;

	m_vPosition += m_vVelocity*dt;
	
	m_quatOrientation.addScaledVector(m_vAngularVelocity, dt);
	m_quatOrientation.normalize();
	
	updateWorldTransformMatrix();

	m_OBB.transformBy(m_matWorldTransform);

	if(AMG3DPhysicsWorld::g_AMG3D_bAutoClearForces) {
		clearNetForce();
		clearNetTorque();
	}

	if(AMG3DPhysicsWorld::g_AMG3D_bEnableSleeping)
		updateSleepState(dt);
}

Code: Select all

m_quatOrientation.addScaledVector(m_vAngularVelocity, dt);
   m_quatOrientation.normalize();

Code: Select all

void AMG3DQuaternion::addScaledVector(const AMG3DVector4& v, const AMG3DScalar& scale)
{
	// To Do: Derive this
	AMG3DQuaternion q(0, v.x*scale, v.y*scale, v.z*scale);
	q *= *this;
	
	w += q.w * 0.5f;
	x += q.x * 0.5f;
	y += q.y * 0.5f;
	z += q.z * 0.5f;
}

Code: Select all

void AMG3DQuaternion::normalize()
{
	AMG3DScalar s = magnitude();
	
	if(s>0) {
		w /= s;
		x /= s;
		y /= s;
		z /= s;
	}
}

Code: Select all

AMG3DScalar AMG3DQuaternion::magnitude() const
{
	return sqrtf(w*w + x*x + y*y + z*z);
}

Code: Select all

void AMG3DRigidBody::integrateForces(const AMG3DScalar& dt)
{
	if(dt<=0.0f)
		return; 

	// Just check if the body is dynamic, zero mass body = zero acceleration anyway
	// Some forces can wake the body (user added forces), some can't (gravity), leave that to the force updater
//	if(m_sMass<=0.0f)
//		return;

	// Cannot update forces for kinematic or immovable bodies
	if(m_eType!=AMG3D_PHYSICS_RIGID_BODY_TYPE_DYNAMIC)
		return;

	// Body is sleeping, do not integrate forces
//	if(m_eState==AMG3D_PHYSICS_RIGID_BODY_STATE_SLEEPING)
//		return;

	m_vAcceleration			= m_vNetForce*m_sInvMass;
	m_vAngularAcceleration	= m_vNetTorque*m_matInvIWorld;

	m_vVelocity			+= m_vAcceleration*dt;
	m_vAngularVelocity	+= m_vAngularAcceleration*dt;
}