void dgBody::ApplyImpulsesAtPoint (dgInt32 count, dgInt32 strideInBytes, const dgFloat32* const impulseArray, const dgFloat32* const pointArray)
{
dgInt32 stride = strideInBytes / sizeof (dgFloat32);
dgMatrix inertia (CalculateInertiaMatrix());
dgVector impulse (m_veloc.Scale3 (m_mass.m_w));
dgVector angularImpulse (inertia.RotateVector (m_omega));
dgVector com (m_globalCentreOfMass);
for (dgInt32 i = 0; i < count; i ++) {
dgInt32 index = i * stride;
dgVector r (pointArray[index], pointArray[index + 1], pointArray[index + 2], dgFloat32 (0.0f));
dgVector L (impulseArray[index], impulseArray[index + 1], impulseArray[index + 2], dgFloat32 (0.0f));
dgVector Q ((r - com) * L);
impulse += L;
angularImpulse += Q;
}
dgMatrix invInertia (CalculateInvInertiaMatrix());
m_veloc = impulse.Scale3(m_invMass.m_w);
m_omega = invInertia.RotateVector(angularImpulse);
m_sleeping = false;
m_equilibrium = false;
Unfreeze ();
}
void dgBody::AddImpulse (const dgVector& pointDeltaVeloc, const dgVector& pointPosit)
{
dgMatrix invInertia (CalculateInvInertiaMatrix());
// get contact matrix
dgMatrix tmp;
dgVector globalContact (pointPosit - m_globalCentreOfMass);
tmp[0][0] = dgFloat32 (0.0f);
tmp[0][1] = + globalContact[2];
tmp[0][2] = - globalContact[1];
tmp[0][3] = dgFloat32 (0.0f);
tmp[1][0] = -globalContact[2];
tmp[1][1] = dgFloat32 (0.0f);
tmp[1][2] = +globalContact[0];
tmp[1][3] = dgFloat32 (0.0f);
tmp[2][0] = +globalContact[1];
tmp[2][1] = -globalContact[0];
tmp[2][2] = dgFloat32 (0.0f);
tmp[2][3] = dgFloat32 (0.0f);
tmp[3][0] = dgFloat32 (0.0f);
tmp[3][1] = dgFloat32 (0.0f);
tmp[3][2] = dgFloat32 (0.0f);
tmp[3][3] = dgFloat32 (1.0f);
dgMatrix contactMatrix (tmp * invInertia * tmp);
for (dgInt32 i = 0; i < 3; i ++) {
for (dgInt32 j = 0; j < 3; j ++) {
contactMatrix[i][j] *= -dgFloat32 (1.0f);
}
}
contactMatrix[0][0] += m_invMass.m_w;
contactMatrix[1][1] += m_invMass.m_w;
contactMatrix[2][2] += m_invMass.m_w;
contactMatrix = contactMatrix.Symetric3by3Inverse ();
// change of momentum
dgVector changeOfMomentum (contactMatrix.RotateVector (pointDeltaVeloc));
dgVector dv (changeOfMomentum.Scale3 (m_invMass.m_w));
dgVector dw (invInertia.RotateVector (globalContact * changeOfMomentum));
m_veloc += dv;
m_omega += dw;
m_sleeping = false;
m_equilibrium = false;
Unfreeze ();
}
void dgBody::ApplyImpulsePair (const dgVector& linearImpulseIn, const dgVector& angularImpulseIn)
{
dgMatrix inertia (CalculateInertiaMatrix());
dgMatrix invInertia (CalculateInvInertiaMatrix());
dgVector linearImpulse (m_veloc.Scale3 (m_mass.m_w) + linearImpulseIn);
dgVector angularImpulse (inertia.RotateVector (m_omega) + angularImpulseIn);
m_veloc = linearImpulse.Scale3(m_invMass.m_w);
m_omega = invInertia.RotateVector(angularImpulse);
m_sleeping = false;
m_equilibrium = false;
Unfreeze ();
}
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);
}
