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 FactorizeLCP(const dgJointInfo* const jointInfoArray, const dgJacobian* const internalForces, dgJacobianMatrixElement* const matrixRow, dgForcePair& accel)
{
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];
const dgInt32 first = jointInfo->m_pairStart;
dgInt32 clampedValue = m_dof - 1;
//dgSpatialVector& accel = force.m_joint;
for (dgInt32 i = 0; i < m_dof; i++) {
dgInt32 k = m_sourceJacobianIndex[i];
const dgJacobianMatrixElement* const row = &matrixRow[first + k];
//dgFloat32 force = row->m_force + row->m_invJMinvJt * residual.GetScalar();
dgFloat32 force = row->m_force;
if ((force >= row->m_lowerBoundFrictionCoefficent) && (force <= row->m_upperBoundFrictionCoefficent)) {
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();
accel.m_joint[i] = -residual.GetScalar();
} else {
dgAssert (0);
dgSwap (m_sourceJacobianIndex[i], m_sourceJacobianIndex[clampedValue]);
i --;
m_dof --;
clampedValue --;
}
}
GetJacobians(jointInfo, matrixRow);
}
Factorize();
}
DG_INLINE void UpdateFactorizeLCP(const dgJointInfo* const jointInfoArray, dgJacobianMatrixElement* const matrixRow, dgForcePair& force, dgForcePair& accel)
{
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 first = jointInfo->m_pairStart;
dgInt32 clampedValue = m_dof - 1;
for (dgInt32 i = 0; i < m_dof; i++) {
dgInt32 k = m_sourceJacobianIndex[i];
const dgJacobianMatrixElement* const row = &matrixRow[first + k];
dgFloat32 f = force.m_joint[i] + row->m_force;
if (f < row->m_lowerBoundFrictionCoefficent) {
force.m_joint[i] = dgFloat32 (0.0f);
dgSwap(accel.m_joint[i], accel.m_joint[clampedValue]);
dgSwap(force.m_joint[i], force.m_joint[clampedValue]);
dgSwap(m_sourceJacobianIndex[i], m_sourceJacobianIndex[clampedValue]);
i--;
m_dof--;
clampedValue--;
} else if (f > row->m_upperBoundFrictionCoefficent) {
force.m_joint[i] = dgFloat32 (0.0f);
dgSwap(accel.m_joint[i], accel.m_joint[clampedValue]);
dgSwap(force.m_joint[i], force.m_joint[clampedValue]);
dgSwap(m_sourceJacobianIndex[i], m_sourceJacobianIndex[clampedValue]);
i--;
m_dof--;
clampedValue--;
}
}
GetJacobians(jointInfo, matrixRow);
}
Factorize();
}
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();
}
