void dgSolver::CalculateJointsForce(dgInt32 threadID)
{
const dgInt32* const soaRowStart = m_soaRowStart;
const dgBodyInfo* const bodyArray = m_bodyArray;
dgSoaMatrixElement* const massMatrix = &m_massMatrix[0];
dgRightHandSide* const rightHandSide = &m_world->GetSolverMemory().m_righHandSizeBuffer[0];
dgSoaFloat* const internalForces = (dgSoaFloat*)&m_world->GetSolverMemory().m_internalForcesBuffer[0];
dgFloat32 accNorm = dgFloat32(0.0f);
const dgInt32 step = m_threadCounts;
const dgInt32 jointCount = m_jointCount;
for (dgInt32 i = threadID; i < jointCount; i += step) {
const dgInt32 rowStart = soaRowStart[i];
dgJointInfo* const jointInfo = &m_jointArray[i * DG_SOA_WORD_GROUP_SIZE];
bool isSleeping = true;
dgFloat32 accel2 = dgFloat32(0.0f);
for (dgInt32 j = 0; (j < DG_WORK_GROUP_SIZE) && isSleeping; j++) {
const dgInt32 m0 = jointInfo[j].m_m0;
const dgInt32 m1 = jointInfo[j].m_m1;
const dgBody* const body0 = bodyArray[m0].m_body;
const dgBody* const body1 = bodyArray[m1].m_body;
isSleeping &= body0->m_resting;
isSleeping &= body1->m_resting;
}
if (!isSleeping) {
accel2 = CalculateJointForce(jointInfo, &massMatrix[rowStart], internalForces);
accNorm += accel2;
for (dgInt32 j = 0; j < DG_SOA_WORD_GROUP_SIZE; j++) {
const dgJointInfo* const joint = &jointInfo[j];
if (joint->m_joint) {
dgInt32 const rowCount = joint->m_pairCount;
dgInt32 const rowStartBase = joint->m_pairStart;
for (dgInt32 k = 0; k < rowCount; k++) {
const dgSoaMatrixElement* const row = &massMatrix[rowStart + k];
rightHandSide[k + rowStartBase].m_force = row->m_force[j];
rightHandSide[k + rowStartBase].m_maxImpact = dgMax(dgAbs(row->m_force[j]), rightHandSide[k + rowStartBase].m_maxImpact);
}
}
}
}
}
m_accelNorm[threadID] = accNorm;
}
void dgSolver::CalculateJointsForce(dgInt32 threadID)
{
const dgInt32* const soaRowStart = m_soaRowStart;
const dgBodyInfo* const bodyArray = m_bodyArray;
dgSoaMatrixElement* const massMatrix = &m_massMatrix[0];
dgRightHandSide* const rightHandSide = &m_world->GetSolverMemory().m_righHandSizeBuffer[0];
dgJacobian* const internalForces = &m_world->GetSolverMemory().m_internalForcesBuffer[0];
dgFloat32 accNorm = dgFloat32(0.0f);
const dgInt32 step = m_threadCounts;
const dgInt32 jointCount = m_jointCount;
for (dgInt32 i = threadID; i < jointCount; i += step) {
const dgInt32 rowStart = soaRowStart[i];
dgJointInfo* const jointInfo = &m_jointArray[i * DG_SOA_WORD_GROUP_SIZE];
bool isSleeping = true;
dgFloat32 accel2 = dgFloat32(0.0f);
for (dgInt32 j = 0; (j < DG_SOA_WORD_GROUP_SIZE) && isSleeping; j++) {
const dgInt32 m0 = jointInfo[j].m_m0;
const dgInt32 m1 = jointInfo[j].m_m1;
const dgBody* const body0 = bodyArray[m0].m_body;
const dgBody* const body1 = bodyArray[m1].m_body;
isSleeping &= body0->m_resting;
isSleeping &= body1->m_resting;
}
if (!isSleeping) {
accel2 = CalculateJointForce(jointInfo, &massMatrix[rowStart], internalForces);
for (dgInt32 j = 0; j < DG_SOA_WORD_GROUP_SIZE; j++) {
const dgJointInfo* const joint = &jointInfo[j];
if (joint->m_joint) {
dgInt32 const rowCount = joint->m_pairCount;
dgInt32 const rowStartBase = joint->m_pairStart;
for (dgInt32 k = 0; k < rowCount; k++) {
const dgSoaMatrixElement* const row = &massMatrix[rowStart + k];
rightHandSide[k + rowStartBase].m_force = row->m_force[j];
rightHandSide[k + rowStartBase].m_maxImpact = dgMax(dgAbs(row->m_force[j]), rightHandSide[k + rowStartBase].m_maxImpact);
}
}
}
}
dgSoaVector6 forceM0;
dgSoaVector6 forceM1;
forceM0.m_linear.m_x = m_soaZero;
forceM0.m_linear.m_y = m_soaZero;
forceM0.m_linear.m_z = m_soaZero;
forceM0.m_angular.m_x = m_soaZero;
forceM0.m_angular.m_y = m_soaZero;
forceM0.m_angular.m_z = m_soaZero;
forceM1.m_linear.m_x = m_soaZero;
forceM1.m_linear.m_y = m_soaZero;
forceM1.m_linear.m_z = m_soaZero;
forceM1.m_angular.m_x = m_soaZero;
forceM1.m_angular.m_y = m_soaZero;
forceM1.m_angular.m_z = m_soaZero;
const dgInt32 rowsCount = jointInfo->m_pairCount;
for (dgInt32 j = 0; j < rowsCount; j++) {
dgSoaMatrixElement* const row = &massMatrix[rowStart + j];
dgSoaFloat f(row->m_force);
forceM0.m_linear.m_x = forceM0.m_linear.m_x.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_x, f);
forceM0.m_linear.m_y = forceM0.m_linear.m_y.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_y, f);
forceM0.m_linear.m_z = forceM0.m_linear.m_z.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_z, f);
forceM0.m_angular.m_x = forceM0.m_angular.m_x.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_x, f);
forceM0.m_angular.m_y = forceM0.m_angular.m_y.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_y, f);
forceM0.m_angular.m_z = forceM0.m_angular.m_z.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_z, f);
forceM1.m_linear.m_x = forceM1.m_linear.m_x.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_x, f);
forceM1.m_linear.m_y = forceM1.m_linear.m_y.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_y, f);
forceM1.m_linear.m_z = forceM1.m_linear.m_z.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_z, f);
forceM1.m_angular.m_x = forceM1.m_angular.m_x.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_x, f);
forceM1.m_angular.m_y = forceM1.m_angular.m_y.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_y, f);
forceM1.m_angular.m_z = forceM1.m_angular.m_z.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_z, f);
}
dgBodyProxy* const bodyProxyArray = m_bodyProxyArray;
dgJacobian* const tempInternalForces = &m_world->GetSolverMemory().m_internalForcesBuffer[m_cluster->m_bodyCount];
for (dgInt32 j = 0; j < DG_SOA_WORD_GROUP_SIZE; j++) {
const dgJointInfo* const joint = &jointInfo[j];
if (joint->m_joint) {
dgJacobian m_body0Force;
dgJacobian m_body1Force;
m_body0Force.m_linear = dgVector(forceM0.m_linear.m_x[j], forceM0.m_linear.m_y[j], forceM0.m_linear.m_z[j], dgFloat32(0.0f));
m_body0Force.m_angular = dgVector(forceM0.m_angular.m_x[j], forceM0.m_angular.m_y[j], forceM0.m_angular.m_z[j], dgFloat32(0.0f));
m_body1Force.m_linear = dgVector(forceM1.m_linear.m_x[j], forceM1.m_linear.m_y[j], forceM1.m_linear.m_z[j], dgFloat32(0.0f));
m_body1Force.m_angular = dgVector(forceM1.m_angular.m_x[j], forceM1.m_angular.m_y[j], forceM1.m_angular.m_z[j], dgFloat32(0.0f));
const dgInt32 m0 = jointInfo[j].m_m0;
const dgInt32 m1 = jointInfo[j].m_m1;
if (m0) {
dgScopeSpinPause lock(&bodyProxyArray[m0].m_lock);
tempInternalForces[m0].m_linear += m_body0Force.m_linear;
tempInternalForces[m0].m_angular += m_body0Force.m_angular;
}
if (m1) {
dgScopeSpinPause lock(&bodyProxyArray[m1].m_lock);
tempInternalForces[m1].m_linear += m_body1Force.m_linear;
tempInternalForces[m1].m_angular += m_body1Force.m_angular;
}
}
}
accNorm += accel2;
}
m_accelNorm[threadID] = accNorm;
}
