DG_INLINE void InitKinematicMassMatrix (const dgJointInfo* const jointInfoArray, dgJacobianMatrixElement* const matrixRow, dgFloat32 correctionFactor)
{
dgAssert (0);
dgAssert((dgUnsigned64(&m_bodyMass) & 0x0f) == 0);
m_bodyMass.SetZero();
m_bodyForce.SetZero();
if (m_body->GetInvMass().m_w != dgFloat32(0.0f)) {
GetInertia();
}
if (m_joint) {
dgAssert(m_parent);
const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index];
dgAssert(jointInfo->m_joint == m_joint);
dgAssert(jointInfo->m_joint->GetBody0() == m_body);
dgAssert(jointInfo->m_joint->GetBody1() == m_parent->m_body);
m_dof = 0;
const dgInt32 count = jointInfo->m_pairCount;
const dgInt32 first = jointInfo->m_pairStart;
for (dgInt32 i = 0; i < count; i++) {
const dgJacobianMatrixElement* const row = &matrixRow[i + first];
m_jointForce[m_dof] = -dgClamp(row->m_penetration * correctionFactor, dgFloat32(-0.25f), dgFloat32(0.25f));
m_sourceJacobianIndex[m_dof] = dgInt8(i);
m_dof++;
}
GetJacobians(jointInfo, matrixRow);
}
Factorize();
}
DG_INLINE void Factorize(const dgJointInfo* const jointInfoArray, dgJacobianMatrixElement* const matrixRow)
{
dgAssert((dgUnsigned64(&m_bodyMass) & 0x0f) == 0);
m_bodyMass.SetZero();
if (m_body->GetInvMass().m_w != dgFloat32 (0.0f)) {
GetInertia();
}
if (m_joint) {
dgAssert (m_parent);
const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index];
dgAssert(jointInfo->m_joint == m_joint);
dgAssert(jointInfo->m_joint->GetBody0() == m_body);
dgAssert(jointInfo->m_joint->GetBody1() == m_parent->m_body);
m_dof = 0;
const dgInt32 count = jointInfo->m_pairCount;
const dgInt32 first = jointInfo->m_pairStart;
for (dgInt32 i = 0; i < count; i++) {
const dgJacobianMatrixElement* const row = &matrixRow[i + first];
if (((row->m_lowerBoundFrictionCoefficent < dgFloat32 (-1.0e9f)) && row->m_upperBoundFrictionCoefficent > dgFloat32 (1.0e9f))) {
m_sourceJacobianIndex[m_dof] = dgInt8(i);
m_dof ++;
}
}
GetJacobians(jointInfo, matrixRow);
}
Factorize();
}
DG_INLINE void EnumerateRows(const dgJointInfo* const jointInfoArray)
{
if (m_joint) {
m_dof = 0;
dgAssert(m_parent);
const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index];
dgAssert(jointInfo->m_joint == m_joint);
const dgInt32 count = jointInfo->m_pairCount;
for (dgInt32 i = 0; i < count; i++) {
m_sourceJacobianIndex[m_dof] = dgInt8(i);
m_dof++;
}
}
}
DG_INLINE void FactorizeLCP(const dgJointInfo* const jointInfoArray, const dgJacobian* const internalForces, dgJacobianMatrixElement* const matrixRow)
{
dgAssert((dgUnsigned64(&m_bodyMass) & 0x0f) == 0);
m_bodyMass.SetZero();
if (m_body->GetInvMass().m_w != dgFloat32(0.0f)) {
GetInertia();
}
if (m_joint) {
dgAssert(m_parent);
const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index];
dgAssert(jointInfo->m_joint == m_joint);
dgAssert(jointInfo->m_joint->GetBody0() == m_body);
dgAssert(jointInfo->m_joint->GetBody1() == m_parent->m_body);
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
const dgJacobian& y0 = internalForces[m0];
const dgJacobian& y1 = internalForces[m1];
m_dof = 0;
const dgInt32 count = jointInfo->m_pairCount;
const dgInt32 first = jointInfo->m_pairStart;
for (dgInt32 i = 0; i < count; i++) {
const dgJacobianMatrixElement* const row = &matrixRow[i + first];
dgVector residual(row->m_JMinv.m_jacobianM0.m_linear.CompProduct4(y0.m_linear) + row->m_JMinv.m_jacobianM0.m_angular.CompProduct4(y0.m_angular) +
row->m_JMinv.m_jacobianM1.m_linear.CompProduct4(y1.m_linear) + row->m_JMinv.m_jacobianM1.m_angular.CompProduct4(y1.m_angular));
residual = dgVector(row->m_coordenateAccel) - residual.AddHorizontal();
dgFloat32 force = row->m_force + row->m_invJMinvJt * residual.GetScalar();
if ((force > row->m_lowerBoundFrictionCoefficent) && (force < row->m_upperBoundFrictionCoefficent)) {
m_sourceJacobianIndex[m_dof] = dgInt8(i);
m_dof++;
}
}
GetJacobians(jointInfo, matrixRow);
}
Factorize();
}
