int dComplemtaritySolver::BuildJacobianMatrix (int jointCount, dBilateralJoint ** const jointArray, dFloat timestep, dJacobianPair * const jacobianArray, dJacobianColum * const jacobianColumnArray, int maxRowCount)
{
int rowCount = 0;
dParamInfo constraintParams;
constraintParams.m_timestep = timestep;
constraintParams.m_timestepInv = 1.0f / timestep;
// calculate Jacobian derivative for each active joint
for (int j = 0; j < jointCount; j ++)
{
dBilateralJoint * const joint = jointArray[j];
constraintParams.m_count = 0;
joint->JacobianDerivative (&constraintParams);
int dofCount = constraintParams.m_count;
joint->m_count = dofCount;
joint->m_start = rowCount;
// complete the derivative matrix for this joint
int index = joint->m_start;
dBodyState * const state0 = joint->m_state0;
dBodyState * const state1 = joint->m_state1;
const dMatrix & invInertia0 = state0->m_invInertia;
const dMatrix & invInertia1 = state1->m_invInertia;
dFloat invMass0 = state0->m_invMass;
dFloat invMass1 = state1->m_invMass;
dFloat weight = 0.9f;
for (int i = 0; i < dofCount; i ++)
{
dJacobianPair * const row = &jacobianArray[index];
dJacobianColum * const col = &jacobianColumnArray[index];
jacobianArray[rowCount] = constraintParams.m_jacobians[i];
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 tmpDiag (JMinvIM0linear.CompProduct (row->m_jacobian_IM0.m_linear) + JMinvIM0angular.CompProduct (row->m_jacobian_IM0.m_angular) + JMinvIM1linear.CompProduct (row->m_jacobian_IM1.m_linear) + JMinvIM1angular.CompProduct (row->m_jacobian_IM1.m_angular));
dVector tmpAccel (JMinvIM0linear.CompProduct (state0->m_externalForce) + JMinvIM0angular.CompProduct (state0->m_externalTorque) + JMinvIM1linear.CompProduct (state1->m_externalForce) + JMinvIM1angular.CompProduct (state1->m_externalTorque));
dFloat extenalAcceleration = - (tmpAccel[0] + tmpAccel[1] + tmpAccel[2]);
col->m_diagDamp = 1.0f;
col->m_coordenateAccel = constraintParams.m_jointAccel[i];
col->m_jointLowFriction = constraintParams.m_jointLowFriction[i];
col->m_jointHighFriction = constraintParams.m_jointHighFriction[i];
col->m_deltaAccel = extenalAcceleration;
col->m_coordenateAccel += extenalAcceleration;
col->m_force = joint->m_jointFeebackForce[i] * weight;
dFloat stiffness = COMPLEMENTARITY_PSD_DAMP_TOL * col->m_diagDamp;
dFloat diag = (tmpDiag[0] + tmpDiag[1] + tmpDiag[2]);
dAssert (diag > dFloat (0.0f));
col->m_diagDamp = diag * stiffness;
diag *= (dFloat (1.0f) + stiffness);
col->m_invDJMinvJt = dFloat (1.0f) / diag;
index ++;
rowCount ++;
dAssert (rowCount < maxRowCount);
}
}
return rowCount;
}
void dgWorldDynamicUpdate::BuildJacobianMatrix (const dgBodyInfo* const bodyInfoArray, const dgJointInfo* const jointInfo, dgJacobian* const internalForces, dgJacobianMatrixElement* const matrixRow, dgFloat32 forceImpulseScale) const
{
const dgInt32 index = jointInfo->m_pairStart;
const dgInt32 count = jointInfo->m_pairCount;
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
const dgBody* const body0 = bodyInfoArray[m0].m_body;
const dgBody* const body1 = bodyInfoArray[m1].m_body;
const bool isBilateral = jointInfo->m_joint->IsBilateral();
const dgVector invMass0(body0->m_invMass[3]);
const dgMatrix& invInertia0 = body0->m_invWorldInertiaMatrix;
const dgVector invMass1(body1->m_invMass[3]);
const dgMatrix& invInertia1 = body1->m_invWorldInertiaMatrix;
dgVector force0(dgVector::m_zero);
dgVector torque0(dgVector::m_zero);
if (body0->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
force0 = ((dgDynamicBody*)body0)->m_externalForce;
torque0 = ((dgDynamicBody*)body0)->m_externalTorque;
}
dgVector force1(dgVector::m_zero);
dgVector torque1(dgVector::m_zero);
if (body1->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
force1 = ((dgDynamicBody*)body1)->m_externalForce;
torque1 = ((dgDynamicBody*)body1)->m_externalTorque;
}
const dgVector scale0(jointInfo->m_scale0);
const dgVector scale1(jointInfo->m_scale1);
dgJacobian forceAcc0;
dgJacobian forceAcc1;
forceAcc0.m_linear = dgVector::m_zero;
forceAcc0.m_angular = dgVector::m_zero;
forceAcc1.m_linear = dgVector::m_zero;
forceAcc1.m_angular = dgVector::m_zero;
for (dgInt32 i = 0; i < count; 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));
row->m_JMinv.m_jacobianM0.m_linear = row->m_Jt.m_jacobianM0.m_linear * invMass0;
row->m_JMinv.m_jacobianM0.m_angular = invInertia0.RotateVector(row->m_Jt.m_jacobianM0.m_angular);
row->m_JMinv.m_jacobianM1.m_linear = row->m_Jt.m_jacobianM1.m_linear * invMass1;
row->m_JMinv.m_jacobianM1.m_angular = invInertia1.RotateVector(row->m_Jt.m_jacobianM1.m_angular);
dgVector tmpAccel(row->m_JMinv.m_jacobianM0.m_linear * force0 + row->m_JMinv.m_jacobianM0.m_angular * torque0 +
row->m_JMinv.m_jacobianM1.m_linear * force1 + row->m_JMinv.m_jacobianM1.m_angular * torque1);
dgAssert(tmpAccel.m_w == dgFloat32(0.0f));
dgFloat32 extenalAcceleration = -(tmpAccel.AddHorizontal()).GetScalar();
row->m_deltaAccel = extenalAcceleration * forceImpulseScale;
row->m_coordenateAccel += extenalAcceleration * forceImpulseScale;
dgAssert(row->m_jointFeebackForce);
const dgFloat32 force = row->m_jointFeebackForce->m_force * forceImpulseScale;
row->m_force = isBilateral ? dgClamp(force, row->m_lowerBoundFrictionCoefficent, row->m_upperBoundFrictionCoefficent) : force;
row->m_force0 = row->m_force;
row->m_maxImpact = dgFloat32(0.0f);
dgVector jMinvM0linear (scale0 * row->m_JMinv.m_jacobianM0.m_linear);
dgVector jMinvM0angular (scale0 * row->m_JMinv.m_jacobianM0.m_angular);
dgVector jMinvM1linear (scale1 * row->m_JMinv.m_jacobianM1.m_linear);
dgVector jMinvM1angular (scale1 * row->m_JMinv.m_jacobianM1.m_angular);
dgVector tmpDiag(jMinvM0linear * row->m_Jt.m_jacobianM0.m_linear + jMinvM0angular * row->m_Jt.m_jacobianM0.m_angular +
jMinvM1linear * row->m_Jt.m_jacobianM1.m_linear + jMinvM1angular * row->m_Jt.m_jacobianM1.m_angular);
dgAssert (tmpDiag.m_w == dgFloat32 (0.0f));
dgFloat32 diag = (tmpDiag.AddHorizontal()).GetScalar();
dgAssert(diag > dgFloat32(0.0f));
row->m_diagDamp = diag * row->m_stiffness;
diag *= (dgFloat32(1.0f) + row->m_stiffness);
row->m_jMinvJt = diag;
row->m_invJMinvJt = dgFloat32(1.0f) / diag;
dgAssert(dgCheckFloat(row->m_force));
dgVector val(row->m_force);
forceAcc0.m_linear += row->m_Jt.m_jacobianM0.m_linear * val;
forceAcc0.m_angular += row->m_Jt.m_jacobianM0.m_angular * val;
forceAcc1.m_linear += row->m_Jt.m_jacobianM1.m_linear * val;
forceAcc1.m_angular += row->m_Jt.m_jacobianM1.m_angular * val;
}
forceAcc0.m_linear = forceAcc0.m_linear * scale0;
forceAcc0.m_angular = forceAcc0.m_angular * scale0;
forceAcc1.m_linear = forceAcc1.m_linear * scale1;
forceAcc1.m_angular = forceAcc1.m_angular * scale1;
if (!body0->m_equilibrium) {
internalForces[m0].m_linear += forceAcc0.m_linear;
internalForces[m0].m_angular += forceAcc0.m_angular;
}
if (!body1->m_equilibrium) {
internalForces[m1].m_linear += forceAcc1.m_linear;
internalForces[m1].m_angular += forceAcc1.m_angular;
}
}
