void dgWorld::BodySetMatrix (dgBody* const body, const dgMatrix& matrix)
{
#define DG_RECURSIVE_SIZE 1024
dgBody* queue[DG_RECURSIVE_SIZE];
dgInt32 index = 1;
queue[0] = body;
m_genericLRUMark ++;
body->m_genericLRUMark = m_genericLRUMark;
dgMatrix relMatrix (body->GetMatrix().Inverse() * matrix);
while (index) {
index --;
dgBody* body1 = queue[index];
dgAssert (body1 != m_sentinelBody);
// why should I do this? I do no remember the reason
//m_broadPhase->Remove (body);
//m_broadPhase->Add (body);
dgMatrix matrix1 (body1->GetMatrix() * relMatrix);
//body1->SetOmega (dgVector (dgFloat32 (0.0f)));
//body1->SetVelocity (dgVector (dgFloat32 (0.0f)));
body1->SetOmega (matrix1.RotateVector (body1->GetOmega()));
body1->SetVelocity (matrix1.RotateVector (body1->GetVelocity()));
body1->SetMatrix (matrix1);
body1->UpdateCollisionMatrix (dgFloat32 (0.0f), 0);
body1->SetSleepState(false);
for (dgBodyMasterListRow::dgListNode* jointNode = body1->m_masterNode->GetInfo().GetFirst(); jointNode; jointNode = jointNode->GetNext()) {
dgBodyMasterListCell& cell = jointNode->GetInfo();
body1 = cell.m_bodyNode;
if (body1 != m_sentinelBody) {
if (body1->m_genericLRUMark != m_genericLRUMark) {
dgConstraint* constraint;
constraint = cell.m_joint;
if (constraint->GetId() != dgConstraint::m_contactConstraint) {
body1->m_genericLRUMark = m_genericLRUMark;
queue[index] = body1;
index ++;
dgAssert (index < DG_RECURSIVE_SIZE);
}
}
}
}
}
}
void dgWorld::BodySetMatrix (dgBody* body, const dgMatrix& matrix)
{
#define DG_RECURSIVE_SIZE 1024
dgInt32 index;
dgBody* queue[DG_RECURSIVE_SIZE];
index = 1;
queue[0] = body;
m_genericLRUMark ++;
body->m_genericLRUMark = m_genericLRUMark;
dgMatrix relMatrix (body->GetMatrix().Inverse() * matrix);
while (index) {
dgBody* body;
index --;
body = queue[index];
_ASSERTE (body != m_sentionelBody);
dgBroadPhaseCollision::Remove (body);
dgBroadPhaseCollision::Add (body);
dgMatrix matrix (body->GetMatrix() * relMatrix);
body->SetVelocity (dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)));
body->SetOmega (dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)));
body->SetMatrix (matrix);
body->m_isInWorld = true;
for (dgBodyMasterListRow::dgListNode* jointNode = body->m_masterNode->GetInfo().GetFirst(); jointNode; jointNode = jointNode->GetNext()) {
dgBodyMasterListCell& cell = jointNode->GetInfo();
body = cell.m_bodyNode;
if (body != m_sentionelBody) {
if (body->m_genericLRUMark != m_genericLRUMark) {
dgConstraint* constraint;
constraint = cell.m_joint;
if (constraint->GetId() != dgContactConstraintId) {
body->m_genericLRUMark = m_genericLRUMark;
queue[index] = body;
index ++;
_ASSERTE (index < DG_RECURSIVE_SIZE);
}
}
}
}
}
}
dgVector dgBilateralConstraint::CalculateGlobalMatrixAndAngle (dgMatrix& globalMatrix0, dgMatrix& globalMatrix1) const
{
dgAssert (m_body0);
dgAssert (m_body1);
const dgMatrix& body0Matrix = m_body0->GetMatrix();
const dgMatrix& body1Matrix = m_body1->GetMatrix();
globalMatrix0 = m_localMatrix0 * body0Matrix;
globalMatrix1 = m_localMatrix1 * body1Matrix;
dgMatrix relMatrix (globalMatrix1 * globalMatrix0.Inverse());
dgAssert (dgAbsf (dgFloat32 (1.0f) - (relMatrix.m_front % relMatrix.m_front)) < 1.0e-5f);
dgAssert (dgAbsf (dgFloat32 (1.0f) - (relMatrix.m_up % relMatrix.m_up)) < 1.0e-5f);
dgAssert (dgAbsf (dgFloat32 (1.0f) - (relMatrix.m_right % relMatrix.m_right)) < 1.0e-5f);
dgVector euler0;
dgVector euler1;
relMatrix.CalcPitchYawRoll (euler0, euler1);
return euler0;
}
dgVector dgBilateralConstraint::CalculateGlobalMatrixAndAngle (dgMatrix& globalMatrix0, dgMatrix& globalMatrix1) const
{
_ASSERTE (m_body0);
_ASSERTE (m_body1);
const dgMatrix& body0Matrix = m_body0->GetMatrix();
const dgMatrix& body1Matrix = m_body1->GetMatrix();
// dgMatrix body1Matrix (dgGetIdentityMatrix());
// if (m_body1) {
// body1Matrix = m_body1->GetMatrix();
// }
globalMatrix0 = m_localMatrix0 * body0Matrix;
globalMatrix1 = m_localMatrix1 * body1Matrix;
dgMatrix relMatrix (globalMatrix1 * globalMatrix0.Inverse());
_ASSERTE (dgAbsf (dgFloat32 (1.0f) - (relMatrix.m_front % relMatrix.m_front)) < 1.0e-5f);
_ASSERTE (dgAbsf (dgFloat32 (1.0f) - (relMatrix.m_up % relMatrix.m_up)) < 1.0e-5f);
_ASSERTE (dgAbsf (dgFloat32 (1.0f) - (relMatrix.m_right % relMatrix.m_right)) < 1.0e-5f);
// _ASSERTE ((relMatrix.m_posit % relMatrix.m_posit) < 1.0e-3f);
return relMatrix.CalcPitchYawRoll ();
}
