dgMatrix dgCollisionInstance::CalculateInertia () const
{
if (IsType(dgCollision::dgCollisionMesh_RTTI)) {
return dgGetZeroMatrix();
} else {
return m_childShape->CalculateInertiaAndCenterOfMass (m_aligmentMatrix, m_scale, m_localMatrix);
}
}
dgBody::dgBody()
:m_invWorldInertiaMatrix(dgGetZeroMatrix())
,m_matrix (dgGetIdentityMatrix())
,m_rotation(dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f))
,m_mass(dgFloat32 (DG_INFINITE_MASS * 2.0f), dgFloat32 (DG_INFINITE_MASS * 2.0f), dgFloat32 (DG_INFINITE_MASS * 2.0f), dgFloat32 (DG_INFINITE_MASS * 2.0f))
,m_invMass(dgFloat32 (0.0))
,m_veloc(dgFloat32 (0.0))
,m_omega(dgFloat32 (0.0))
,m_minAABB(dgFloat32 (0.0))
,m_maxAABB(dgFloat32 (0.0))
,m_netForce(dgFloat32 (0.0))
,m_netTorque(dgFloat32 (0.0))
,m_localCentreOfMass(dgFloat32 (0.0))
,m_globalCentreOfMass(dgFloat32 (0.0))
,m_aparentMass(dgFloat32 (DG_INFINITE_MASS), dgFloat32 (DG_INFINITE_MASS), dgFloat32 (DG_INFINITE_MASS), dgFloat32 (DG_INFINITE_MASS))
,m_maxAngulaRotationPerSet2(DG_MAX_ANGLE_STEP * DG_MAX_ANGLE_STEP )
,m_criticalSectionLock()
,m_flags(0)
,m_userData(NULL)
,m_world(NULL)
,m_collision(NULL)
,m_broadPhaseNode(NULL)
,m_masterNode(NULL)
,m_broadPhaseaggregateNode(NULL)
,m_destructor(NULL)
,m_matrixUpdate(NULL)
,m_index(0)
,m_uniqueID(0)
,m_bodyGroupId(0)
,m_rtti(m_baseBodyRTTI)
,m_type(0)
,m_dynamicsLru(0)
,m_genericLRUMark(0)
{
m_resting = true;
m_autoSleep = true;
m_collidable = true;
m_collideWithLinkedBodies = true;
m_invWorldInertiaMatrix[3][3] = dgFloat32 (1.0f);
}
dgMatrix dgCollisionInstance::CalculateInertia () const
{
if (IsType(dgCollision::dgCollisionMesh_RTTI)) {
return dgGetZeroMatrix();
} else {
return m_childShape->CalculateInertiaAndCenterOfMass (m_aligmentMatrix, m_scale, m_localMatrix);
// switch (m_scaleType)
// {
// case m_unit:
// case m_uniform:
// case m_nonUniform:
// {
// return m_childShape->CalculateInertiaAndCenterOfMass (m_aligmentMatrix____, m_scale, m_localMatrix);
// }
//
// default:
// {
// return m_childShape->CalculateInertiaAndCenterOfMass (m_aligmentMatrix____, m_scale, m_localMatrix);
// }
// }
}
}
dgBody::dgBody (dgWorld* const world, const dgTree<const dgCollision*, dgInt32>* const collisionCashe, dgDeserialize serializeCallback, void* const userData, dgInt32 revisionNumber)
:m_invWorldInertiaMatrix(dgGetZeroMatrix())
,m_matrix (dgGetIdentityMatrix())
,m_rotation(dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f))
,m_mass(dgFloat32 (DG_INFINITE_MASS * 2.0f), dgFloat32 (DG_INFINITE_MASS * 2.0f), dgFloat32 (DG_INFINITE_MASS * 2.0f), dgFloat32 (DG_INFINITE_MASS * 2.0f))
,m_invMass(dgFloat32 (0.0))
,m_veloc(dgFloat32 (0.0))
,m_omega(dgFloat32 (0.0))
,m_minAABB(dgFloat32 (0.0))
,m_maxAABB(dgFloat32 (0.0))
,m_netForce(dgFloat32 (0.0))
,m_netTorque(dgFloat32 (0.0))
,m_localCentreOfMass(dgFloat32 (0.0))
,m_globalCentreOfMass(dgFloat32 (0.0))
,m_aparentMass(dgFloat32 (DG_INFINITE_MASS), dgFloat32 (DG_INFINITE_MASS), dgFloat32 (DG_INFINITE_MASS), dgFloat32 (DG_INFINITE_MASS))
,m_maxAngulaRotationPerSet2(DG_MAX_ANGLE_STEP * DG_MAX_ANGLE_STEP )
,m_criticalSectionLock()
,m_flags(0)
,m_userData(NULL)
,m_world(world)
,m_collision(NULL)
,m_broadPhaseNode(NULL)
,m_masterNode(NULL)
,m_broadPhaseaggregateNode(NULL)
,m_destructor(NULL)
,m_matrixUpdate(NULL)
,m_index(0)
,m_uniqueID(0)
,m_bodyGroupId(0)
,m_rtti(m_baseBodyRTTI)
,m_type(0)
,m_dynamicsLru(0)
,m_genericLRUMark(0)
{
m_autoSleep = true;
m_collidable = true;
m_collideWithLinkedBodies = true;
m_invWorldInertiaMatrix[3][3] = dgFloat32 (1.0f);
serializeCallback (userData, &m_rotation, sizeof (m_rotation));
serializeCallback (userData, &m_matrix.m_posit, sizeof (m_matrix.m_posit));
serializeCallback (userData, &m_veloc, sizeof (m_veloc));
serializeCallback (userData, &m_omega, sizeof (m_omega));
serializeCallback (userData, &m_localCentreOfMass, sizeof (m_localCentreOfMass));
serializeCallback (userData, &m_aparentMass, sizeof (m_aparentMass));
serializeCallback (userData, &m_flags, sizeof (m_flags));
serializeCallback (userData, &m_maxAngulaRotationPerSet2, sizeof (m_maxAngulaRotationPerSet2));
m_matrix = dgMatrix (m_rotation, m_matrix.m_posit);
dgInt32 id;
serializeCallback (userData, &id, sizeof (id));
dgTree<const dgCollision*, dgInt32>::dgTreeNode* const node = collisionCashe->Find(id);
dgAssert (node);
const dgCollision* const collision = node->GetInfo();
collision->AddRef();
dgCollisionInstance* const instance = new (world->GetAllocator()) dgCollisionInstance (world, serializeCallback, userData, revisionNumber);
instance->m_childShape = collision;
m_collision = instance;
}
void dgCollisionDeformableSolidMesh::ApplyExternalForces (dgFloat32 timestep)
{
dgAssert (m_myBody);
dgBody* const body = GetBody();
dgAssert (body->GetMass().m_w > dgFloat32 (0.0f));
dgAssert (body->GetMass().m_w < dgFloat32 (1.0e5f));
const dgMatrix& matrix = body->GetCollision()->GetGlobalMatrix();
dgFloat32 invMass = body->GetInvMass().m_w;
dgVector velocyStep (body->GetForce().Scale4(invMass * timestep));
//velocyStep = dgVector(0.0f);
dgVector* const veloc = m_particles.m_veloc;
dgFloat32* const unitMass = m_particles.m_unitMass;
/*
invMass = 0;
dgVector w (0.0f, 0.0f, 1.0f, 0.0f);
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
unitMass[i] = 1.0f;
veloc[i] = w * m_posit[i];
invMass += unitMass[i];
}
invMass = 1.0f / invMass;
*/
dgVector com (dgFloat32 (0.0f));
dgVector comVeloc (dgFloat32 (0.0f));
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
dgVector mass (unitMass[i]);
const dgVector& p = m_posit[i];
veloc[i] += velocyStep;
com += p.CompProduct4(mass);
comVeloc += veloc[i].CompProduct4(mass);
}
com = com.Scale4(invMass);
comVeloc = comVeloc.Scale4(invMass);
const dgMatrix& indentity = dgGetIdentityMatrix();
dgMatrix inertiaMatrix (dgGetZeroMatrix());
inertiaMatrix.m_posit = dgVector::m_wOne;
dgVector comAngularMomentum (dgFloat32 (0.0f));
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
dgVector mass (unitMass[i]);
dgVector r (m_posit[i] - com);
dgVector mr (r.CompProduct4(mass));
dgVector relVeloc (veloc[i] - comVeloc);
comAngularMomentum += mr * relVeloc;
dgMatrix inertia (mr, r);
dgVector diagInertia (mr.DotProduct4(r));
inertiaMatrix.m_front += (indentity.m_front.CompProduct4(diagInertia) - inertia.m_front);
inertiaMatrix.m_up += (indentity.m_up.CompProduct4(diagInertia) - inertia.m_up);
inertiaMatrix.m_right += (indentity.m_right.CompProduct4(diagInertia) - inertia.m_right);
dgAssert (inertiaMatrix.TestSymetric3x3());
}
dgVector damp (0.3f);
dgMatrix invInertia (inertiaMatrix.Symetric3by3Inverse());
dgVector omega (invInertia.RotateVector(comAngularMomentum));
for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
dgVector r (m_posit[i] - com);
dgVector deltaVeloc (comVeloc + omega * r - veloc[i]);
veloc[i] += deltaVeloc.CompProduct4(damp);
}
//Sleep (50);
dgMatrix tmp;
dgMatrix transform;
dgVector scale;
inertiaMatrix.PolarDecomposition (transform, scale, tmp, matrix);
body->GetCollision()->SetGlobalMatrix(transform);
body->SetMatrixOriginAndRotation(body->GetCollision()->GetLocalMatrix().Inverse() * transform);
body->SetVelocity(comVeloc);
body->SetOmega(omega);
}
