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