void CalculatePickForceAndTorque (const NewtonBody* const body, const dVector& pointOnBodyInGlobalSpace, const dVector& targetPositionInGlobalSpace, dFloat timestep)
{
dVector com;
dMatrix matrix;
dVector omega0;
dVector veloc0;
dVector omega1;
dVector veloc1;
dVector pointVeloc;
const dFloat stiffness = 0.3f;
const dFloat angularDamp = 0.95f;
dFloat invTimeStep = 1.0f / timestep;
NewtonWorld* const world = NewtonBodyGetWorld (body);
NewtonWorldCriticalSectionLock (world, 0);
// calculate the desired impulse
NewtonBodyGetMatrix(body, &matrix[0][0]);
NewtonBodyGetOmega (body, &omega0[0]);
NewtonBodyGetVelocity (body, &veloc0[0]);
NewtonBodyGetPointVelocity (body, &pointOnBodyInGlobalSpace[0], &pointVeloc[0]);
dVector deltaVeloc (targetPositionInGlobalSpace - pointOnBodyInGlobalSpace);
deltaVeloc = deltaVeloc.Scale (stiffness * invTimeStep) - pointVeloc;
for (int i = 0; i < 3; i ++) {
dVector veloc (0.0f, 0.0f, 0.0f, 0.0f);
veloc[i] = deltaVeloc[i];
NewtonBodyAddImpulse (body, &veloc[0], &pointOnBodyInGlobalSpace[0]);
}
// damp angular velocity
NewtonBodyGetOmega (body, &omega1[0]);
NewtonBodyGetVelocity (body, &veloc1[0]);
omega1 = omega1.Scale (angularDamp);
// restore body velocity and angular velocity
NewtonBodySetOmega (body, &omega0[0]);
NewtonBodySetVelocity(body, &veloc0[0]);
// convert the delta velocity change to a external force and torque
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
dVector angularMomentum (Ixx, Iyy, Izz);
angularMomentum = matrix.RotateVector (angularMomentum.CompProduct(matrix.UnrotateVector(omega1 - omega0)));
dVector force ((veloc1 - veloc0).Scale (mass * invTimeStep));
dVector torque (angularMomentum.Scale(invTimeStep));
NewtonBodyAddForce(body, &force[0]);
NewtonBodyAddTorque(body, &torque[0]);
// make sure the body is unfrozen, if it is picked
NewtonBodySetSleepState (body, 0);
NewtonWorldCriticalSectionUnlock (world);
}
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);
}
