codepad
[
create a new paste
]
login
|
about
Language:
C
C++
D
Haskell
Lua
OCaml
PHP
Perl
Plain Text
Python
Ruby
Scheme
Tcl
void AMG3DRigidBodyCollisionResponder::applyImpulse() { if(m_bSkipCollision) return; for(int i=0; i<m_cdContactData.iNumContacts; ++i) { // =================================================================================================== // The normal impulse calculation // =================================================================================================== AMG3DVector4 normal = m_cdContactData.contacts[i].vContactNormal; AMG3DVector4 rap = m_cdContactData.contacts[i].vContactPoint - m_pRigidBody1->m_vPosition; AMG3DVector4 rbp = m_cdContactData.contacts[i].vContactPoint - m_pRigidBody2->m_vPosition; AMG3DVector4 vap = m_pRigidBody1->m_vVelocity + m_pRigidBody1->m_vAngularVelocity.cross(rap); AMG3DVector4 vbp = m_pRigidBody2->m_vVelocity + m_pRigidBody2->m_vAngularVelocity.cross(rbp); // Relative velocity AMG3DVector4 vab = vap - vbp; // Compute the normal impulse AMG3DScalar vabdotn = vab.dot(normal); AMG3DScalar dPn = (-vabdotn + m_cdContactData.contacts[i].bias) * m_cdContactData.contacts[i].massNormal; if(AMG3DPhysicsWorld::g_AMG3D_bAccumulateImpulses) { // Clamp the accumulated impulse, not each corrective impulse as it will lead to jitter AMG3DScalar Pn0 = m_cdContactData.contacts[i].Pn; m_cdContactData.contacts[i].Pn = AMG3D_MATH_MAX(Pn0+dPn, 0.0f); dPn = m_cdContactData.contacts[i].Pn - Pn0; } else { dPn = AMG3D_MATH_MAX(dPn, 0.0f); } // Apply normal impulse AMG3DVector4 Pn = normal*dPn; // Update final linear and angular velocities with normal impulse m_pRigidBody1->m_vVelocity += Pn*m_pRigidBody1->m_sInvMass; m_pRigidBody1->m_vAngularVelocity += m_pRigidBody1->m_matInvIWorld*rap.cross(Pn); m_pRigidBody2->m_vVelocity -= Pn*m_pRigidBody2->m_sInvMass; m_pRigidBody2->m_vAngularVelocity -= m_pRigidBody2->m_matInvIWorld*rbp.cross(Pn); // =================================================================================================== // The tangent/friction impulse calculation // =================================================================================================== vap = m_pRigidBody1->m_vVelocity + m_pRigidBody1->m_vAngularVelocity.cross(rap); vbp = m_pRigidBody2->m_vVelocity + m_pRigidBody2->m_vAngularVelocity.cross(rbp); // Relative velocity vab = vap - vbp; AMG3DVector4 t1, t2; normal.buildOrthoBasis(&t1, &t2); AMG3DVector4 rapcrosst1 = rap.cross(t1); AMG3DVector4 rbpcrosst1 = rbp.cross(t1); AMG3DVector4 rapcrosst2 = rap.cross(t2); AMG3DVector4 rbpcrosst2 = rbp.cross(t2); // Compute the tangent/friction impulse in the first direction AMG3DScalar vabdott1 = vab.dot(t1); AMG3DScalar dPt1 = -vabdott1 * m_cdContactData.contacts[i].massTangent1; // Compute the tangent/friction impulse in the second direction AMG3DScalar vabdott2 = vab.dot(t2); AMG3DScalar dPt2 = -vabdott2 * m_cdContactData.contacts[i].massTangent2; if(AMG3DPhysicsWorld::g_AMG3D_bAccumulateImpulses) { // Clamp the accumulated impulse, not each corrective impulse as it will lead to jitter AMG3DScalar maxPt = m_sFrictionConstant * m_cdContactData.contacts[i].Pn; // The first tangent direction AMG3DScalar Pt0 = m_cdContactData.contacts[i].Pt1; m_cdContactData.contacts[i].Pt1 = AMG3D_MATH_CLAMP(Pt0 + dPt1, -maxPt, maxPt); dPt1 = m_cdContactData.contacts[i].Pt1 - Pt0; // The second tangent direction Pt0 = m_cdContactData.contacts[i].Pt2; m_cdContactData.contacts[i].Pt2 = AMG3D_MATH_CLAMP(Pt0 + dPt2, -maxPt, maxPt); dPt2 = m_cdContactData.contacts[i].Pt2 - Pt0; } else { AMG3DScalar maxPt = m_sFrictionConstant * dPn; dPt1 = AMG3D_MATH_CLAMP(dPt1, -maxPt, maxPt); dPt2 = AMG3D_MATH_CLAMP(dPt2, -maxPt, maxPt); } // Apply friction/tangent impulse AMG3DVector4 Pt1 = t1*dPt1; AMG3DVector4 Pt2 = t2*dPt2; // Update final linear and angular velocities with friction/tangent impulse m_pRigidBody1->m_vVelocity += Pt1*m_pRigidBody1->m_sInvMass + Pt2*m_pRigidBody1->m_sInvMass; m_pRigidBody1->m_vAngularVelocity += m_pRigidBody1->m_matInvIWorld*rap.cross(Pt1) + m_pRigidBody1->m_matInvIWorld*rap.cross(Pt2); m_pRigidBody2->m_vVelocity -= Pt1*m_pRigidBody2->m_sInvMass + Pt2*m_pRigidBody2->m_sInvMass; m_pRigidBody2->m_vAngularVelocity -= m_pRigidBody2->m_matInvIWorld*rbp.cross(Pt1) + m_pRigidBody2->m_matInvIWorld*rbp.cross(Pt2); } }
Private
[
?
]
Run code
Submit