void dgSolver::CalculateBodyForce(dgInt32 threadID)
{
const dgBodyProxy* const bodyProxyArray = m_bodyProxyArray;
const dgBodyJacobianPair* const bodyJacobiansPairs = m_bodyJacobiansPairs;
dgSoaFloat* const internalForces = (dgSoaFloat*)&m_world->GetSolverMemory().m_internalForcesBuffer[0];
const dgRightHandSide* const rightHandSide = &m_world->GetSolverMemory().m_righHandSizeBuffer[0];
const dgSoaFloat* const leftHandSide = (dgSoaFloat*)&m_world->GetSolverMemory().m_leftHandSizeBuffer[0].m_Jt.m_jacobianM0;
const dgInt32 step = m_threadCounts;;
const dgInt32 bodyCount = m_cluster->m_bodyCount;
for (dgInt32 i = threadID; i < bodyCount; i += step) {
dgSoaFloat forceAcc (m_soaZero);
const dgBodyProxy* const startJoints = &bodyProxyArray[i];
const dgInt32 jointsCount = dgInt32(startJoints->m_weight);
const dgBodyJacobianPair* const jointsStart = &bodyJacobiansPairs[startJoints->m_jointStart];
for (dgInt32 j = 0; j < jointsCount; j++) {
const dgInt32 rowsCount = jointsStart[j].m_rowCount - 2;
const dgSoaFloat* const lhs = &leftHandSide[jointsStart[j].m_rowStart];
const dgRightHandSide* const rhs = &rightHandSide[jointsStart[j].m_righHandStart];
for (dgInt32 k = 0; k < rowsCount; k += 2) {
forceAcc = forceAcc.MulAdd(lhs[(k + 0) * 4], dgSoaFloat(rhs[k + 0].m_force));
forceAcc = forceAcc.MulAdd(lhs[(k + 1) * 4], dgSoaFloat(rhs[k + 1].m_force));
}
if (jointsStart[j].m_rowCount & 1) {
const dgInt32 k = jointsStart[j].m_rowCount - 1;
forceAcc = forceAcc.MulAdd(lhs[k * 4], dgSoaFloat(rhs[k].m_force));
}
}
internalForces[i] = forceAcc * dgSoaFloat(startJoints->m_invWeight);
}
}
void RigidBody::updateLinearForceAndTorque(float d_t){
Vector3 forceAcc(0,0,0);
Vector3 torqueAcc(0,0,0);
Vector3 r(0,0,0);
Vector3 f(0,0,0);
Vector3 t(0,0,0);
vector<Physics* > mp = getMassPoints();
for (unsigned int i = 0; i < mp.size(); i++)
{
r = mp[i]->getPosition()-getPosition();
f = mp[i]->getAccumulatedForce();
t = r.cross(f);
if(t.length() > 1.0){
t.normalize();
}
if(f.length() > 10.0){
f.normalize();
f*=10.0;
}
forceAcc+=f;
torqueAcc+=t;
mp[i]->clearAccumulatedForce();
}
mTorque = torqueAcc;
mCenterOfMass.applyForce(forceAcc);
}