DG_INLINE dgFloat32 dgSolver::CalculateJointForce(const dgJointInfo* const jointInfo, dgSoaMatrixElement* const massMatrix, const dgSoaFloat* const internalForces) const
{
dgSoaVector6 forceM0;
dgSoaVector6 forceM1;
dgSoaFloat preconditioner0;
dgSoaFloat preconditioner1;
dgSoaFloat accNorm(m_soaZero);
dgSoaFloat normalForce[DG_CONSTRAINT_MAX_ROWS + 1];
const dgBodyProxy* const bodyProxyArray = m_bodyProxyArray;
for (dgInt32 i = 0; i < DG_SOA_WORD_GROUP_SIZE; i++) {
const dgInt32 m0 = jointInfo[i].m_m0;
const dgInt32 m1 = jointInfo[i].m_m1;
forceM0.m_linear.m_x[i] = internalForces[m0][0];
forceM0.m_linear.m_y[i] = internalForces[m0][1];
forceM0.m_linear.m_z[i] = internalForces[m0][2];
forceM0.m_angular.m_x[i] = internalForces[m0][4];
forceM0.m_angular.m_y[i] = internalForces[m0][5];
forceM0.m_angular.m_z[i] = internalForces[m0][6];
forceM1.m_linear.m_x[i] = internalForces[m1][0];
forceM1.m_linear.m_y[i] = internalForces[m1][1];
forceM1.m_linear.m_z[i] = internalForces[m1][2];
forceM1.m_angular.m_x[i] = internalForces[m1][4];
forceM1.m_angular.m_y[i] = internalForces[m1][5];
forceM1.m_angular.m_z[i] = internalForces[m1][6];
preconditioner0[i] = jointInfo[i].m_preconditioner0 * bodyProxyArray[m0].m_weight;
preconditioner1[i] = jointInfo[i].m_preconditioner1 * bodyProxyArray[m1].m_weight;
}
forceM0.m_linear.m_x = forceM0.m_linear.m_x * preconditioner0;
forceM0.m_linear.m_y = forceM0.m_linear.m_y * preconditioner0;
forceM0.m_linear.m_z = forceM0.m_linear.m_z * preconditioner0;
forceM0.m_angular.m_x = forceM0.m_angular.m_x * preconditioner0;
forceM0.m_angular.m_y = forceM0.m_angular.m_y * preconditioner0;
forceM0.m_angular.m_z = forceM0.m_angular.m_z * preconditioner0;
forceM1.m_linear.m_x = forceM1.m_linear.m_x * preconditioner1;
forceM1.m_linear.m_y = forceM1.m_linear.m_y * preconditioner1;
forceM1.m_linear.m_z = forceM1.m_linear.m_z * preconditioner1;
forceM1.m_angular.m_x = forceM1.m_angular.m_x * preconditioner1;
forceM1.m_angular.m_y = forceM1.m_angular.m_y * preconditioner1;
forceM1.m_angular.m_z = forceM1.m_angular.m_z * preconditioner1;
const dgInt32 rowsCount = jointInfo->m_pairCount;
normalForce[0] = m_soaOne;
for (dgInt32 j = 0; j < rowsCount; j++) {
dgSoaMatrixElement* const row = &massMatrix[j];
dgSoaFloat a;
a = row->m_coordenateAccel.MulSub(row->m_JMinv.m_jacobianM0.m_linear.m_x, forceM0.m_linear.m_x);
a = a.MulSub(row->m_JMinv.m_jacobianM0.m_linear.m_y, forceM0.m_linear.m_y);
a = a.MulSub(row->m_JMinv.m_jacobianM0.m_linear.m_z, forceM0.m_linear.m_z);
a = a.MulSub(row->m_JMinv.m_jacobianM0.m_angular.m_x, forceM0.m_angular.m_x);
a = a.MulSub(row->m_JMinv.m_jacobianM0.m_angular.m_y, forceM0.m_angular.m_y);
a = a.MulSub(row->m_JMinv.m_jacobianM0.m_angular.m_z, forceM0.m_angular.m_z);
a = a.MulSub(row->m_JMinv.m_jacobianM1.m_linear.m_x, forceM1.m_linear.m_x);
a = a.MulSub(row->m_JMinv.m_jacobianM1.m_linear.m_y, forceM1.m_linear.m_y);
a = a.MulSub(row->m_JMinv.m_jacobianM1.m_linear.m_z, forceM1.m_linear.m_z);
a = a.MulSub(row->m_JMinv.m_jacobianM1.m_angular.m_x, forceM1.m_angular.m_x);
a = a.MulSub(row->m_JMinv.m_jacobianM1.m_angular.m_y, forceM1.m_angular.m_y);
a = a.MulSub(row->m_JMinv.m_jacobianM1.m_angular.m_z, forceM1.m_angular.m_z);
a = a.MulSub(row->m_force, row->m_diagDamp);
dgSoaFloat f(row->m_force.MulAdd(row->m_invJinvMJt, a));
dgSoaFloat frictionNormal;
for (dgInt32 k = 0; k < DG_SOA_WORD_GROUP_SIZE; k++) {
dgAssert(row->m_normalForceIndex.m_i[k] >= -1);
dgAssert(row->m_normalForceIndex.m_i[k] <= rowsCount);
const dgInt32 frictionIndex = dgInt32(row->m_normalForceIndex.m_i[k] + 1);
frictionNormal[k] = normalForce[frictionIndex][k];
}
dgSoaFloat lowerFrictionForce(frictionNormal * row->m_lowerBoundFrictionCoefficent);
dgSoaFloat upperFrictionForce(frictionNormal * row->m_upperBoundFrictionCoefficent);
a = a.AndNot((f > upperFrictionForce) | (f < lowerFrictionForce));
f = f.GetMax(lowerFrictionForce).GetMin(upperFrictionForce);
accNorm = accNorm + a * a;
dgSoaFloat deltaForce(f - row->m_force);
row->m_force = f;
normalForce[j + 1] = f;
dgSoaFloat deltaForce0(deltaForce * preconditioner0);
dgSoaFloat deltaForce1(deltaForce * preconditioner1);
forceM0.m_linear.m_x = forceM0.m_linear.m_x.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_x, deltaForce0);
forceM0.m_linear.m_y = forceM0.m_linear.m_y.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_y, deltaForce0);
forceM0.m_linear.m_z = forceM0.m_linear.m_z.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_z, deltaForce0);
forceM0.m_angular.m_x = forceM0.m_angular.m_x.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_x, deltaForce0);
forceM0.m_angular.m_y = forceM0.m_angular.m_y.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_y, deltaForce0);
forceM0.m_angular.m_z = forceM0.m_angular.m_z.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_z, deltaForce0);
forceM1.m_linear.m_x = forceM1.m_linear.m_x.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_x, deltaForce1);
forceM1.m_linear.m_y = forceM1.m_linear.m_y.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_y, deltaForce1);
forceM1.m_linear.m_z = forceM1.m_linear.m_z.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_z, deltaForce1);
forceM1.m_angular.m_x = forceM1.m_angular.m_x.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_x, deltaForce1);
forceM1.m_angular.m_y = forceM1.m_angular.m_y.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_y, deltaForce1);
forceM1.m_angular.m_z = forceM1.m_angular.m_z.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_z, deltaForce1);
}
const dgFloat32 tol = dgFloat32(0.5f);
const dgFloat32 tol2 = tol * tol;
dgSoaFloat maxAccel(accNorm);
for (dgInt32 i = 0; (i < 4) && (maxAccel.AddHorizontal() > tol2); i++) {
maxAccel = m_soaZero;
for (dgInt32 j = 0; j < rowsCount; j++) {
dgSoaMatrixElement* const row = &massMatrix[j];
dgSoaFloat a;
a = row->m_coordenateAccel.MulSub(row->m_JMinv.m_jacobianM0.m_linear.m_x, forceM0.m_linear.m_x);
a = a.MulSub(row->m_JMinv.m_jacobianM0.m_linear.m_y, forceM0.m_linear.m_y);
a = a.MulSub(row->m_JMinv.m_jacobianM0.m_linear.m_z, forceM0.m_linear.m_z);
a = a.MulSub(row->m_JMinv.m_jacobianM0.m_angular.m_x, forceM0.m_angular.m_x);
a = a.MulSub(row->m_JMinv.m_jacobianM0.m_angular.m_y, forceM0.m_angular.m_y);
a = a.MulSub(row->m_JMinv.m_jacobianM0.m_angular.m_z, forceM0.m_angular.m_z);
a = a.MulSub(row->m_JMinv.m_jacobianM1.m_linear.m_x, forceM1.m_linear.m_x);
a = a.MulSub(row->m_JMinv.m_jacobianM1.m_linear.m_y, forceM1.m_linear.m_y);
a = a.MulSub(row->m_JMinv.m_jacobianM1.m_linear.m_z, forceM1.m_linear.m_z);
a = a.MulSub(row->m_JMinv.m_jacobianM1.m_angular.m_x, forceM1.m_angular.m_x);
a = a.MulSub(row->m_JMinv.m_jacobianM1.m_angular.m_y, forceM1.m_angular.m_y);
a = a.MulSub(row->m_JMinv.m_jacobianM1.m_angular.m_z, forceM1.m_angular.m_z);
a = a.MulSub(row->m_force, row->m_diagDamp);
dgSoaFloat f(row->m_force.MulAdd(row->m_invJinvMJt, a));
dgSoaFloat frictionNormal;
for (dgInt32 k = 0; k < DG_SOA_WORD_GROUP_SIZE; k++) {
dgAssert(row->m_normalForceIndex.m_i[k] >= -1);
dgAssert(row->m_normalForceIndex.m_i[k] <= rowsCount);
const dgInt32 frictionIndex = dgInt32 (row->m_normalForceIndex.m_i[k] + 1);
frictionNormal[k] = normalForce[frictionIndex][k];
}
dgSoaFloat lowerFrictionForce(frictionNormal * row->m_lowerBoundFrictionCoefficent);
dgSoaFloat upperFrictionForce(frictionNormal * row->m_upperBoundFrictionCoefficent);
a = a.AndNot((f > upperFrictionForce) | (f < lowerFrictionForce));
f = f.GetMax(lowerFrictionForce).GetMin(upperFrictionForce);
maxAccel = maxAccel + a * a;
dgSoaFloat deltaForce(f - row->m_force);
row->m_force = f;
normalForce[j + 1] = f;
dgSoaFloat deltaForce0(deltaForce * preconditioner0);
dgSoaFloat deltaForce1(deltaForce * preconditioner1);
forceM0.m_linear.m_x = forceM0.m_linear.m_x.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_x, deltaForce0);
forceM0.m_linear.m_y = forceM0.m_linear.m_y.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_y, deltaForce0);
forceM0.m_linear.m_z = forceM0.m_linear.m_z.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_z, deltaForce0);
forceM0.m_angular.m_x = forceM0.m_angular.m_x.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_x, deltaForce0);
forceM0.m_angular.m_y = forceM0.m_angular.m_y.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_y, deltaForce0);
forceM0.m_angular.m_z = forceM0.m_angular.m_z.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_z, deltaForce0);
forceM1.m_linear.m_x = forceM1.m_linear.m_x.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_x, deltaForce1);
forceM1.m_linear.m_y = forceM1.m_linear.m_y.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_y, deltaForce1);
forceM1.m_linear.m_z = forceM1.m_linear.m_z.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_z, deltaForce1);
forceM1.m_angular.m_x = forceM1.m_angular.m_x.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_x, deltaForce1);
forceM1.m_angular.m_y = forceM1.m_angular.m_y.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_y, deltaForce1);
forceM1.m_angular.m_z = forceM1.m_angular.m_z.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_z, deltaForce1);
}
}
return accNorm.AddHorizontal();
}
dgFloat32 dgWorldDynamicUpdate::CalculateJointForceJacobi1(const dgJointInfo* const jointInfo, const dgBodyInfo* const bodyArray, dgJacobian* const internalForces, dgJacobianMatrixElement* const matrixRow, dgFloat32 restAcceleration) const
{
dgVector accNorm(dgVector::m_zero);
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
dgVector linearM0(internalForces[m0].m_linear);
dgVector angularM0(internalForces[m0].m_angular);
dgVector linearM1(internalForces[m1].m_linear);
dgVector angularM1(internalForces[m1].m_angular);
const dgVector scale0(jointInfo->m_scale0);
const dgVector scale1(jointInfo->m_scale1);
const dgInt32 index = jointInfo->m_pairStart;
const dgInt32 rowsCount = jointInfo->m_pairCount;
dgFloat32 cacheForce[DG_CONSTRAINT_MAX_ROWS + 4];
cacheForce[0] = dgFloat32(1.0f);
cacheForce[1] = dgFloat32(1.0f);
cacheForce[2] = dgFloat32(1.0f);
cacheForce[3] = dgFloat32(1.0f);
dgFloat32* const normalForce = &cacheForce[4];
dgVector maxAccel(1.0e10f);
dgFloat32 prevError = dgFloat32(1.0e20f);
dgVector firstPass(dgVector::m_one);
for (dgInt32 j = 0; (j < 5) && (maxAccel.GetScalar() > restAcceleration) && (prevError - maxAccel.GetScalar()) > dgFloat32(1.0e-2f); j++) {
prevError = maxAccel.GetScalar();
maxAccel = dgVector::m_zero;
for (dgInt32 i = 0; i < rowsCount; i++) {
dgJacobianMatrixElement* const row = &matrixRow[index + i];
dgAssert(row->m_Jt.m_jacobianM0.m_linear.m_w == dgFloat32(0.0f));
dgAssert(row->m_Jt.m_jacobianM0.m_angular.m_w == dgFloat32(0.0f));
dgAssert(row->m_Jt.m_jacobianM1.m_linear.m_w == dgFloat32(0.0f));
dgAssert(row->m_Jt.m_jacobianM1.m_angular.m_w == dgFloat32(0.0f));
dgVector diag(row->m_JMinv.m_jacobianM0.m_linear * linearM0 + row->m_JMinv.m_jacobianM0.m_angular * angularM0 +
row->m_JMinv.m_jacobianM1.m_linear * linearM1 + row->m_JMinv.m_jacobianM1.m_angular * angularM1);
dgVector accel(row->m_coordenateAccel - row->m_force * row->m_diagDamp - (diag.AddHorizontal()).GetScalar());
dgVector force(row->m_force + row->m_invJMinvJt * accel.GetScalar());
const dgInt32 frictionIndex = row->m_normalForceIndex;
dgAssert(((frictionIndex < 0) && (normalForce[frictionIndex] == dgFloat32(1.0f))) || ((frictionIndex >= 0) && (normalForce[frictionIndex] >= dgFloat32(0.0f))));
const dgFloat32 frictionNormal = normalForce[frictionIndex];
dgVector lowerFrictionForce(frictionNormal * row->m_lowerBoundFrictionCoefficent);
dgVector upperFrictionForce(frictionNormal * row->m_upperBoundFrictionCoefficent);
accel = accel.AndNot((force > upperFrictionForce) | (force < lowerFrictionForce));
dgVector accelAbs(accel.Abs());
maxAccel = maxAccel.GetMax(accelAbs);
accNorm = accNorm.GetMax(accelAbs * firstPass);
dgAssert(maxAccel.m_x >= dgAbsf(accel.m_x));
dgFloat32 f = (force.GetMax(lowerFrictionForce).GetMin(upperFrictionForce)).GetScalar();
dgVector deltaForce(f - row->m_force);
row->m_force = f;
normalForce[i] = f;
dgVector deltaforce0(scale0 * deltaForce);
dgVector deltaforce1(scale1 * deltaForce);
linearM0 += row->m_Jt.m_jacobianM0.m_linear * deltaforce0;
angularM0 += row->m_Jt.m_jacobianM0.m_angular * deltaforce0;
linearM1 += row->m_Jt.m_jacobianM1.m_linear * deltaforce1;
angularM1 += row->m_Jt.m_jacobianM1.m_angular * deltaforce1;
}
firstPass = dgVector::m_zero;
}
for (dgInt32 i = 0; i < rowsCount; i++) {
dgJacobianMatrixElement* const row = &matrixRow[index + i];
row->m_maxImpact = dgMax(dgAbsf(row->m_force), row->m_maxImpact);
}
return accNorm.GetScalar();
}
