void dgWorldDynamicUpdate::CalculateJointForceJacobi0(const dgJointInfo* const jointInfo, const dgBodyInfo* const bodyArray, dgJacobian* const internalForces, dgJacobianMatrixElement* const matrixRow) const
{
dgVector linearM0(dgVector::m_zero);
dgVector angularM0(dgVector::m_zero);
dgVector linearM1(dgVector::m_zero);
dgVector angularM1(dgVector::m_zero);
const dgFloat32 scale0 = jointInfo->m_scale0;
const dgFloat32 scale1 = jointInfo->m_scale1;
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
const dgInt32 index = jointInfo->m_pairStart;
const dgInt32 rowsCount = jointInfo->m_pairCount;
//dgTrace (("%d %d ->", m0, m1));
for (dgInt32 i = 0; i < rowsCount; i++) {
dgJacobianMatrixElement* const row = &matrixRow[index + i];
//dgTrace (("%f ", row->m_force));
dgFloat32 deltaForce = row->m_force - row->m_force0;
row->m_force0 = row->m_force;
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;
}
//dgTrace (("\n"));
internalForces[m0].m_linear += linearM0;
internalForces[m0].m_angular += angularM0;
internalForces[m1].m_linear += linearM1;
internalForces[m1].m_angular += angularM1;
}
void dComplentaritySolver::CalculateReactionsForces (int bodyCount, dBodyState** const bodyArray, int jointCount, dBilateralJoint** const jointArray, dFloat timestepSrc, dJacobianPair* const jacobianArray, dJacobianColum* const jacobianColumnArray)
{
dJacobian stateVeloc[COMPLEMENTARITY_STACK_ENTRIES];
dJacobian internalForces [COMPLEMENTARITY_STACK_ENTRIES];
int stateIndex = 0;
dVector zero(dFloat (0.0f), dFloat (0.0f), dFloat (0.0f), dFloat (0.0f));
for (int i = 0; i < bodyCount; i ++) {
dBodyState* const state = bodyArray[i];
stateVeloc[stateIndex].m_linear = state->m_veloc;
stateVeloc[stateIndex].m_angular = state->m_omega;
internalForces[stateIndex].m_linear = zero;
internalForces[stateIndex].m_angular = zero;
state->m_myIndex = stateIndex;
stateIndex ++;
dAssert (stateIndex < int (sizeof (stateVeloc)/sizeof (stateVeloc[0])));
}
for (int i = 0; i < jointCount; i ++) {
dJacobian y0;
dJacobian y1;
y0.m_linear = zero;
y0.m_angular = zero;
y1.m_linear = zero;
y1.m_angular = zero;
dBilateralJoint* const constraint = jointArray[i];
int first = constraint->m_start;
int count = constraint->m_count;
for (int j = 0; j < count; j ++) {
dJacobianPair* const row = &jacobianArray[j + first];
const dJacobianColum* const col = &jacobianColumnArray[j + first];
dFloat val = col->m_force;
y0.m_linear += row->m_jacobian_IM0.m_linear.Scale(val);
y0.m_angular += row->m_jacobian_IM0.m_angular.Scale(val);
y1.m_linear += row->m_jacobian_IM1.m_linear.Scale(val);
y1.m_angular += row->m_jacobian_IM1.m_angular.Scale(val);
}
int m0 = constraint->m_state0->m_myIndex;
int m1 = constraint->m_state1->m_myIndex;
internalForces[m0].m_linear += y0.m_linear;
internalForces[m0].m_angular += y0.m_angular;
internalForces[m1].m_linear += y1.m_linear;
internalForces[m1].m_angular += y1.m_angular;
}
dFloat invTimestepSrc = dFloat (1.0f) / timestepSrc;
dFloat invStep = dFloat (0.25f);
dFloat timestep = timestepSrc * invStep;
dFloat invTimestep = invTimestepSrc * dFloat (4.0f);
int maxPasses = 5;
dFloat firstPassCoef = dFloat (0.0f);
dFloat maxAccNorm = dFloat (1.0e-2f);
for (int step = 0; step < 4; step ++) {
dJointAccelerationDecriptor joindDesc;
joindDesc.m_timeStep = timestep;
joindDesc.m_invTimeStep = invTimestep;
joindDesc.m_firstPassCoefFlag = firstPassCoef;
for (int i = 0; i < jointCount; i ++) {
dBilateralJoint* const constraint = jointArray[i];
joindDesc.m_rowsCount = constraint->m_count;
joindDesc.m_rowMatrix = &jacobianArray[constraint->m_start];
joindDesc.m_colMatrix = &jacobianColumnArray[constraint->m_start];
constraint->JointAccelerations (&joindDesc);
}
firstPassCoef = dFloat (1.0f);
dFloat accNorm = dFloat (1.0e10f);
for (int passes = 0; (passes < maxPasses) && (accNorm > maxAccNorm); passes ++) {
accNorm = dFloat (0.0f);
for (int i = 0; i < jointCount; i ++) {
dBilateralJoint* const constraint = jointArray[i];
int index = constraint->m_start;
int rowsCount = constraint->m_count;
int m0 = constraint->m_state0->m_myIndex;
int m1 = constraint->m_state1->m_myIndex;
dVector linearM0 (internalForces[m0].m_linear);
dVector angularM0 (internalForces[m0].m_angular);
dVector linearM1 (internalForces[m1].m_linear);
dVector angularM1 (internalForces[m1].m_angular);
dBodyState* const state0 = constraint->m_state0;
dBodyState* const state1 = constraint->m_state1;
const dMatrix& invInertia0 = state0->m_invInertia;
const dMatrix& invInertia1 = state1->m_invInertia;
dFloat invMass0 = state0->m_invMass;
dFloat invMass1 = state1->m_invMass;
for (int k = 0; k < rowsCount; k ++) {
dJacobianPair* const row = &jacobianArray[index];
dJacobianColum* const col = &jacobianColumnArray[index];
dVector JMinvIM0linear (row->m_jacobian_IM0.m_linear.Scale (invMass0));
dVector JMinvIM1linear (row->m_jacobian_IM1.m_linear.Scale (invMass1));
dVector JMinvIM0angular = invInertia0.UnrotateVector(row->m_jacobian_IM0.m_angular);
dVector JMinvIM1angular = invInertia1.UnrotateVector(row->m_jacobian_IM1.m_angular);
dVector acc (JMinvIM0linear.CompProduct(linearM0) + JMinvIM0angular.CompProduct(angularM0) + JMinvIM1linear.CompProduct(linearM1) + JMinvIM1angular.CompProduct(angularM1));
dFloat a = col->m_coordenateAccel - acc.m_x - acc.m_y - acc.m_z - col->m_force * col->m_diagDamp;
dFloat f = col->m_force + col->m_invDJMinvJt * a;
dFloat lowerFrictionForce = col->m_jointLowFriction;
dFloat upperFrictionForce = col->m_jointHighFriction;
if (f > upperFrictionForce) {
a = dFloat (0.0f);
f = upperFrictionForce;
} else if (f < lowerFrictionForce) {
a = dFloat (0.0f);
f = lowerFrictionForce;
}
accNorm = dMax (accNorm, dAbs (a));
dFloat prevValue = f - col->m_force;
col->m_force = f;
linearM0 += row->m_jacobian_IM0.m_linear.Scale (prevValue);
angularM0 += row->m_jacobian_IM0.m_angular.Scale (prevValue);
linearM1 += row->m_jacobian_IM1.m_linear.Scale (prevValue);
angularM1 += row->m_jacobian_IM1.m_angular.Scale (prevValue);
index ++;
}
internalForces[m0].m_linear = linearM0;
internalForces[m0].m_angular = angularM0;
internalForces[m1].m_linear = linearM1;
internalForces[m1].m_angular = angularM1;
}
}
for (int i = 0; i < bodyCount; i ++) {
dBodyState* const state = bodyArray[i];
//int index = state->m_myIndex;
dAssert (state->m_myIndex == i);
dVector force (state->m_externalForce + internalForces[i].m_linear);
dVector torque (state->m_externalTorque + internalForces[i].m_angular);
state->IntegrateForce(timestep, force, torque);
}
}
for (int i = 0; i < jointCount; i ++) {
dBilateralJoint* const constraint = jointArray[i];
int first = constraint->m_start;
int count = constraint->m_count;
for (int j = 0; j < count; j ++) {
const dJacobianColum* const col = &jacobianColumnArray[j + first];
dFloat val = col->m_force;
constraint->m_jointFeebackForce[j] = val;
}
}
for (int i = 0; i < jointCount; i ++) {
dBilateralJoint* const constraint = jointArray[i];
constraint->UpdateSolverForces (jacobianArray);
}
for (int i = 0; i < bodyCount; i ++) {
dBodyState* const state = bodyArray[i];
dAssert (state->m_myIndex == i);
state->ApplyNetForceAndTorque (invTimestepSrc, stateVeloc[i].m_linear, stateVeloc[i].m_angular);
}
}
dgFloat32 dgWorldDynamicUpdate::CalculateJointForceDanzig(const dgJointInfo* const jointInfo, const dgBodyInfo* const bodyArray, dgJacobian* const internalForces, dgJacobianMatrixElement* const matrixRow, dgFloat32 restAcceleration) const
{
dgFloat32 massMatrix[DG_CONSTRAINT_MAX_ROWS * DG_CONSTRAINT_MAX_ROWS];
dgFloat32 f[DG_CONSTRAINT_MAX_ROWS];
dgFloat32 b[DG_CONSTRAINT_MAX_ROWS];
dgFloat32 low[DG_CONSTRAINT_MAX_ROWS];
dgFloat32 high[DG_CONSTRAINT_MAX_ROWS];
dgFloat32 cacheForce[DG_CONSTRAINT_MAX_ROWS + 4];
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
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];
const dgInt32 index = jointInfo->m_pairStart;
const dgInt32 rowsCount = jointInfo->m_pairCount;
dgAssert (rowsCount <= DG_CONSTRAINT_MAX_ROWS);
dgVector accNorm(dgVector::m_zero);
for (dgInt32 i = 0; i < rowsCount; i++) {
const dgJacobianMatrixElement* const row_i = &matrixRow[index + i];
dgFloat32* const massMatrixRow = &massMatrix[rowsCount * i];
dgJacobian JMinvM0(row_i->m_JMinv.m_jacobianM0);
dgJacobian JMinvM1(row_i->m_JMinv.m_jacobianM1);
dgVector diag(JMinvM0.m_linear * row_i->m_Jt.m_jacobianM0.m_linear + JMinvM0.m_angular * row_i->m_Jt.m_jacobianM0.m_angular +
JMinvM1.m_linear * row_i->m_Jt.m_jacobianM1.m_linear + JMinvM1.m_angular * row_i->m_Jt.m_jacobianM1.m_angular);
diag = diag.AddHorizontal();
dgFloat32 val = diag.GetScalar() + row_i->m_diagDamp;
dgAssert(val > dgFloat32(0.0f));
massMatrixRow[i] = val;
for (dgInt32 j = i + 1; j < rowsCount; j++) {
const dgJacobianMatrixElement* const row_j = &matrixRow[index + j];
dgVector offDiag(JMinvM0.m_linear * row_j->m_Jt.m_jacobianM0.m_linear + JMinvM0.m_angular * row_j->m_Jt.m_jacobianM0.m_angular +
JMinvM1.m_linear * row_j->m_Jt.m_jacobianM1.m_linear + JMinvM1.m_angular * row_j->m_Jt.m_jacobianM1.m_angular);
offDiag = offDiag.AddHorizontal();
dgFloat32 val1 = offDiag.GetScalar();
massMatrixRow[j] = val1;
massMatrix[j * rowsCount + i] = val1;
}
}
const dgJacobian& y0 = internalForces[m0];
const dgJacobian& y1 = internalForces[m1];
for (dgInt32 i = 0; i < rowsCount; i++) {
const dgJacobianMatrixElement* const row = &matrixRow[index + i];
dgJacobian JMinvM0(row->m_JMinv.m_jacobianM0);
dgJacobian JMinvM1(row->m_JMinv.m_jacobianM1);
dgVector diag(JMinvM0.m_linear * y0.m_linear + JMinvM0.m_angular * y0.m_angular +
JMinvM1.m_linear * y1.m_linear + JMinvM1.m_angular * y1.m_angular);
dgVector accel(row->m_coordenateAccel - row->m_force * row->m_diagDamp - (diag.AddHorizontal()).GetScalar());
dgInt32 frictionIndex = row->m_normalForceIndex;
dgAssert(((frictionIndex < 0) && (normalForce[frictionIndex] == dgFloat32(1.0f))) || ((frictionIndex >= 0) && (normalForce[frictionIndex] >= dgFloat32(0.0f))));
normalForce[i] = row->m_force;
dgFloat32 frictionNormal = normalForce[frictionIndex];
dgFloat32 lowerFrictionForce = frictionNormal * row->m_lowerBoundFrictionCoefficent;
dgFloat32 upperFrictionForce = frictionNormal * row->m_upperBoundFrictionCoefficent;
//f[i] = row->m_force;
f[i] = dgFloat32 (0.0f);
b[i] = accel.GetScalar();
low[i] = lowerFrictionForce - row->m_force;
high[i] = upperFrictionForce - row->m_force;
dgVector force(row->m_force + row->m_invJMinvJt * accel.GetScalar());
accel = accel.AndNot((force > upperFrictionForce) | (force < lowerFrictionForce));
accNorm = accNorm.GetMax(accel.Abs());
}
dgSolveDantzigLCP(rowsCount, massMatrix, f, b, low, high);
dgVector linearM0(internalForces[m0].m_linear);
dgVector angularM0(internalForces[m0].m_angular);
dgVector linearM1(internalForces[m1].m_linear);
dgVector angularM1(internalForces[m1].m_angular);
for (dgInt32 i = 0; i < rowsCount; i++) {
dgJacobianMatrixElement* const row = &matrixRow[index + i];
dgVector jointForce (f[i]);
row->m_force = f[i] + row->m_force;
linearM0 += row->m_Jt.m_jacobianM0.m_linear * jointForce;
angularM0 += row->m_Jt.m_jacobianM0.m_angular * jointForce;
linearM1 += row->m_Jt.m_jacobianM1.m_linear * jointForce;
angularM1 += row->m_Jt.m_jacobianM1.m_angular * jointForce;
}
internalForces[m0].m_linear = linearM0;
internalForces[m0].m_angular = angularM0;
internalForces[m1].m_linear = linearM1;
internalForces[m1].m_angular = angularM1;
return accNorm.GetScalar();
}
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();
}