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 (); }