API tools faq
paste
Guest User

Simulation

a guest
Feb 9th, 2015
485
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 10.75 KB | None | 0 0
raw download clone embed print report
  1. //The simulation loop:
  2.  
  3. m_cmContactManager.UpdateContacts();
  4.  
  5.     for ( unsigned int I = 0UL; I < P_TOTAL_POLYGONS; ++I ) {
  6.         CRigidBody* prbBody0 = &m_rbRigidBodies[I];
  7.         for (unsigned int J = I + 1UL; J < P_TOTAL_POLYGONS; ++J) {
  8.             CRigidBody* prbBody1 = &m_rbRigidBodies[J];
  9.  
  10.             if ( prbBody0->IsStatic() &&  prbBody1->IsStatic() ) { continue; }
  11.  
  12.             CDiscreteIntersect::DI_CONTACT_OUTPUT coContactOutput;
  13.             if ( m_geGjkEpa.Intersect(prbBody0, prbBody1, &coContactOutput) ) {
  14.                 CContactManifold::CM_MANIFOLD_CONTACT cContact;
  15.                
  16.                 cContact.vWorldPointOnA = coContactOutput.vPointOnA;
  17.                 cContact.vWorldPointOnB = coContactOutput.vPointOnB;
  18.                 cContact.vWorldNormalOnB = -coContactOutput.vNormalB;
  19.                 cContact.fDistance = coContactOutput.fDistance;
  20.  
  21.                 CMatrix4x4 mInv0 = prbBody0->GetTransform();
  22.                 mInv0.Transpose();
  23.                 mInv0._41 = -mInv0._14;
  24.                 mInv0._42 = -mInv0._24;
  25.                 mInv0._43 = -mInv0._34;
  26.                 mInv0._14 = mInv0._24 = mInv0._34 = ML_ZERO;
  27.  
  28.                 CMatrix4x4 mInv1 = prbBody1->GetTransform();
  29.                 mInv1.Transpose();
  30.                 mInv1._41 = -mInv1._14;
  31.                 mInv1._42 = -mInv1._24;
  32.                 mInv1._43 = -mInv1._34;
  33.                 mInv1._14 = mInv1._24 = mInv1._34 = ML_ZERO;
  34.  
  35.                 cContact.vLocalPointOnA = mInv0 * cContact.vWorldPointOnA;
  36.                 cContact.vLocalPointOnB = mInv1 * cContact.vWorldPointOnB;
  37.  
  38.                 CVector3 vR0 = cContact.vWorldPointOnA - prbBody0->m_vPos;
  39.                 CVector3 vR1 = cContact.vWorldPointOnB - prbBody1->m_vPos;
  40.  
  41.                 CVector3 vVR0 = prbBody0->m_vLinVel + prbBody0->m_vAngVel.Cross(vR0);
  42.                 CVector3 vVR1 = prbBody1->m_vLinVel + prbBody1->m_vAngVel.Cross(vR1);
  43.                 CVector3 vDV = vVR1 - vVR0;
  44.  
  45.                 cContact.vWorldTangents[0] = vDV - (cContact.vWorldNormalOnB * vDV.Dot(cContact.vWorldNormalOnB));
  46.                 float fTanLen = cContact.vWorldTangents[0].LenSq();
  47.                 if ( fTanLen > FLT_EPSILON ) {
  48.                     cContact.vWorldTangents[0] /= CMathLib::Sqrt(fTanLen);
  49.                     cContact.vWorldTangents[1] = cContact.vWorldTangents[0].Cross(cContact.vWorldNormalOnB);
  50.                 }
  51.                 else {
  52.                     CVector3::ComputeBasis(cContact.vWorldNormalOnB, cContact.vWorldTangents[0], cContact.vWorldTangents[1]);
  53.                 }
  54.  
  55.                 m_cmContactManager.AddContact(prbBody0, prbBody1, cContact);
  56.             }
  57.         }
  58.     }
  59.  
  60.     ///*
  61.     for (unsigned int I = 0UL; I < P_TOTAL_POLYGONS; ++I) {
  62.         m_rbRigidBodies[I].ApplyForces(CVector3(ML_ZERO, -10.0f, ML_ZERO), CVector3(ML_ZERO, ML_ZERO, ML_ZERO), fDeltaTime);
  63.     }
  64.  
  65.     m_cmContactManager.PreStep(ML_ONE / fDeltaTime);
  66.    
  67.     m_cmContactManager.WarmStart();
  68.  
  69.     m_cmContactManager.ApplyImpulses();
  70.     //*/
  71.  
  72.     for (unsigned int I = 0UL; I < P_TOTAL_POLYGONS; ++I) {
  73.         m_rbRigidBodies[I].ApplyVelocities(fDeltaTime);
  74.     }
  75.  
  76. //How I'm integrating the bodies:
  77.  
  78. void CRigidBody::ApplyForces(const CVector3& _vForce, const CVector3& _vTorque, float _fDeltaTime) {
  79.     if (m_ui32CollFlags & SIF_STATIC_OBJECT) { return; }
  80.    
  81.     m_vLinVel += _vForce * (m_fInvMass * _fDeltaTime);
  82.     m_vAngVel += (m_mWorldInvInertia * _vTorque) * _fDeltaTime;
  83. }
  84.  
  85. void CRigidBody::ApplyVelocities(float _fDeltaTime) {
  86.     if (m_ui32CollFlags & SIF_STATIC_OBJECT) { return; }
  87.  
  88.     CVector3 vTotalLinVel = m_vLinVel + m_vBiasLinVel;
  89.     CVector3 vTotalAngVel = m_vAngVel + m_vBiasAngVel;
  90.  
  91.     m_vBiasLinVel.Clear();
  92.     m_vBiasAngVel.Clear();
  93.  
  94.     m_vPos += vTotalLinVel * _fDeltaTime;
  95.  
  96.     CQuaternion qDtOrient = CQuaternion(vTotalAngVel.x, vTotalAngVel.y, vTotalAngVel.z, ML_ZERO) * m_qOrient * ML_HALF;
  97.     m_qOrient += qDtOrient * _fDeltaTime;
  98.    
  99.     m_qOrient.Normalize();
  100.     m_qOrient.ToRotationMatrix(m_mOrient);
  101.    
  102.     CMatrix4x4 mInvOrient = m_mOrient;
  103.     mInvOrient.Transpose();
  104.  
  105.     m_mWorldInvInertia = m_mOrient * m_mLocalInvInertia * mInvOrient;
  106.  
  107.     m_mTransform = m_mOrient;
  108.     reinterpret_cast<CVector3&>(m_mTransform._41) = m_vPos;
  109.  
  110.     //Compute this object AABB.
  111.     if (m_psShape) {
  112.         Parent::m_aAabb.ComputeAabbFromAabbAndMatrix(m_psShape->Aabb(), m_mTransform);
  113.         if (m_pboBroadphaseObject) {
  114.             m_pboBroadphaseObject->m_aAabb.ComputeAabbFromAabbAndMatrix(m_aAabb, m_mTransform);
  115.         }
  116.     }
  117. }
  118.  
  119. //Here's how I'm solving the constraint (10 iterations):
  120.  
  121. #ifndef __CONTACT_CONSTRAINT_H__
  122. #define __CONTACT_CONSTRAINT_H__
  123.  
  124. #include "CRigidBody.h"
  125. #include "CContactManifold.h"
  126.  
  127. #define IRPL_BAUMGARTE 0.2f
  128. #define IRPL_ALLOWED_PENETRATION 0.05f
  129.  
  130. class CContactConstraint {
  131.     friend class CContactManager;
  132. public:
  133.     inline void PreStep(float _fInvDt) {
  134.         for (unsigned int I = 0U; I < m_cmContactManifold.m_ui32TotalContacts; ++I) {
  135.             CContactManifold::CM_MANIFOLD_CONTACT& cContact = m_cmContactManifold.m_cContacts[I];
  136.  
  137.             CVector3 vR0 = cContact.vWorldPointOnA - m_prbRigidBody0->m_vPos;
  138.             CVector3 vR1 = cContact.vWorldPointOnB - m_prbRigidBody1->m_vPos;
  139.  
  140.             //Normal:
  141.             {
  142.                 float fMassSum = m_prbRigidBody0->InvMass() + m_prbRigidBody1->InvMass();
  143.                 if (!m_prbRigidBody0->IsStatic()) {
  144.                     fMassSum += (m_prbRigidBody0->m_mWorldInvInertia * vR0.Cross(cContact.vWorldNormalOnB)).Cross(vR0).Dot(cContact.vWorldNormalOnB);
  145.                 }
  146.                 if (!m_prbRigidBody1->IsStatic()) {
  147.                     fMassSum += (m_prbRigidBody1->m_mWorldInvInertia * vR1.Cross(cContact.vWorldNormalOnB)).Cross(vR1).Dot(cContact.vWorldNormalOnB);
  148.                 }
  149.                 cContact.fNormalMass = ML_ONE / fMassSum;
  150.             }
  151.  
  152.  
  153.             for (unsigned int J = 0U; J < 2U; ++J) {
  154.                 //Tangent:
  155.                 const CVector3& vWorldTangent = cContact.vWorldTangents[J];
  156.                 float fMassSum = m_prbRigidBody0->InvMass() + m_prbRigidBody1->InvMass();
  157.                 if (!m_prbRigidBody0->IsStatic()) {
  158.                     fMassSum += (m_prbRigidBody0->m_mWorldInvInertia * vR0.Cross(vWorldTangent)).Cross(vR0).Dot(vWorldTangent);
  159.                 }
  160.                 if (!m_prbRigidBody1->IsStatic()) {
  161.                     fMassSum += (m_prbRigidBody1->m_mWorldInvInertia * vR1.Cross(vWorldTangent)).Cross(vR1).Dot(vWorldTangent);
  162.                 }
  163.                 cContact.fTangentMasses[J] = ML_ONE / fMassSum;
  164.             }
  165.  
  166.             cContact.fBias = IRPL_BAUMGARTE * _fInvDt * CMathLib::Max(cContact.fDistance - IRPL_ALLOWED_PENETRATION, ML_ZERO);
  167.             cContact.fPNB = ML_ZERO;
  168.         }
  169.     }
  170.  
  171.     inline void WarmStart() {
  172.         for (unsigned int I = 0U; I < m_cmContactManifold.m_ui32TotalContacts; ++I) {
  173.             CContactManifold::CM_MANIFOLD_CONTACT& cContact = m_cmContactManifold.m_cContacts[I];
  174.  
  175.             CVector3 vR0 = cContact.vWorldPointOnA - m_prbRigidBody0->m_vPos;
  176.             CVector3 vR1 = cContact.vWorldPointOnB - m_prbRigidBody1->m_vPos;
  177.  
  178.             CVector3 vP = cContact.vWorldNormalOnB * cContact.fPN;
  179.  
  180.             for (unsigned int J = 0U; J < 2U; ++J) {
  181.                 vP += cContact.vWorldTangents[J] * cContact.fPT[J];
  182.             }
  183.  
  184.             if (!m_prbRigidBody0->IsStatic()) {
  185.                 m_prbRigidBody0->m_vLinVel -= vP * m_prbRigidBody0->InvMass();
  186.                 m_prbRigidBody0->m_vAngVel -= m_prbRigidBody0->m_mWorldInvInertia * vR0.Cross(vP);
  187.             }
  188.  
  189.             if (!m_prbRigidBody1->IsStatic()) {
  190.                 m_prbRigidBody1->m_vLinVel += vP * m_prbRigidBody1->InvMass();
  191.                 m_prbRigidBody1->m_vAngVel += m_prbRigidBody1->m_mWorldInvInertia * vR1.Cross(vP);
  192.             }
  193.         }
  194.     }
  195.  
  196.  
  197.     inline void ApplyImpulse() {
  198.         for (unsigned int I = 0U; I < m_cmContactManifold.m_ui32TotalContacts; ++I) {
  199.             CContactManifold::CM_MANIFOLD_CONTACT& cContact = m_cmContactManifold.m_cContacts[I];
  200.  
  201.             CVector3 vR0 = cContact.vWorldPointOnA - m_prbRigidBody0->m_vPos;
  202.             CVector3 vR1 = cContact.vWorldPointOnB - m_prbRigidBody1->m_vPos;
  203.  
  204.             //Tangent:
  205.             {
  206.                 for (unsigned int J = 0U; J < 2U; ++J) {
  207.                     const CVector3& vTangent = cContact.vWorldTangents[J];
  208.  
  209.                     CVector3 vVR0 = m_prbRigidBody0->m_vLinVel + m_prbRigidBody0->m_vAngVel.Cross(vR0);
  210.                     CVector3 vVR1 = m_prbRigidBody1->m_vLinVel + m_prbRigidBody1->m_vAngVel.Cross(vR1);
  211.                     CVector3 vDV = vVR1 - vVR0;
  212.  
  213.                     float fLambda = cContact.fTangentMasses[J] * -vDV.Dot(vTangent);
  214.                     float fMaxLambda = 0.5f * cContact.fPN;
  215.  
  216.                     float fPT0 = cContact.fPT[J];
  217.                     cContact.fPT[J] = CMathLib::Clamp(fPT0 + fLambda, -fMaxLambda, fMaxLambda);
  218.                     fLambda = cContact.fPT[J] - fPT0;
  219.                     CVector3 vPT = vTangent * fLambda;
  220.  
  221.                     if (!m_prbRigidBody0->IsStatic()) {
  222.                         m_prbRigidBody0->m_vLinVel -= vPT * m_prbRigidBody0->InvMass();
  223.                         m_prbRigidBody0->m_vAngVel -= m_prbRigidBody0->m_mWorldInvInertia * vR0.Cross(vPT);
  224.                     }
  225.  
  226.                     if (!m_prbRigidBody1->IsStatic()) {
  227.                         m_prbRigidBody1->m_vLinVel += vPT * m_prbRigidBody1->InvMass();
  228.                         m_prbRigidBody1->m_vAngVel += m_prbRigidBody1->m_mWorldInvInertia * vR1.Cross(vPT);
  229.                     }
  230.                 }
  231.             }
  232.  
  233.             //Normal:
  234.             {
  235.                 CVector3 vVR0 = m_prbRigidBody0->m_vLinVel + m_prbRigidBody0->m_vAngVel.Cross(vR0);
  236.                 CVector3 vVR1 = m_prbRigidBody1->m_vLinVel + m_prbRigidBody1->m_vAngVel.Cross(vR1);
  237.                 CVector3 vDV = vVR1 - vVR0;
  238.                 float fLambda = cContact.fNormalMass * -vDV.Dot(cContact.vWorldNormalOnB);
  239.                 float fPN0 = cContact.fPN;
  240.                 cContact.fPN = CMathLib::Max(fPN0 + fLambda, ML_ZERO);
  241.                 fLambda = cContact.fPN - fPN0;
  242.                 CVector3 vPN = cContact.vWorldNormalOnB * fLambda;
  243.  
  244.                 if (!m_prbRigidBody0->IsStatic()) {
  245.                     m_prbRigidBody0->m_vLinVel -= vPN * m_prbRigidBody0->InvMass();
  246.                     m_prbRigidBody0->m_vAngVel -= m_prbRigidBody0->m_mWorldInvInertia * vR0.Cross(vPN);
  247.                 }
  248.  
  249.                 if (!m_prbRigidBody1->IsStatic()) {
  250.                     m_prbRigidBody1->m_vLinVel += vPN * m_prbRigidBody1->InvMass();
  251.                     m_prbRigidBody1->m_vAngVel += m_prbRigidBody1->m_mWorldInvInertia * vR1.Cross(vPN);
  252.                 }
  253.             }
  254.  
  255.             //Biased Normal:
  256.             {
  257.                 CVector3 vVR0 = m_prbRigidBody0->m_vBiasLinVel + m_prbRigidBody0->m_vBiasAngVel.Cross(vR0);
  258.                 CVector3 vVR1 = m_prbRigidBody1->m_vBiasLinVel + m_prbRigidBody1->m_vBiasAngVel.Cross(vR1);
  259.                 CVector3 vDV = vVR1 - vVR0;
  260.                 float fLambda = cContact.fNormalMass * (-vDV.Dot(cContact.vWorldNormalOnB) + cContact.fBias);
  261.                 float fPNB0 = cContact.fPNB;
  262.                 cContact.fPNB = CMathLib::Max(fPNB0 + fLambda, ML_ZERO);
  263.                 fLambda = cContact.fPNB - fPNB0;
  264.  
  265.                 CVector3 vPN = cContact.vWorldNormalOnB * fLambda;
  266.  
  267.                 if (!m_prbRigidBody0->IsStatic()) {
  268.                     m_prbRigidBody0->m_vBiasLinVel -= vPN * m_prbRigidBody0->InvMass();
  269.                     m_prbRigidBody0->m_vBiasAngVel -= m_prbRigidBody0->m_mWorldInvInertia * vR0.Cross(vPN);
  270.                 }
  271.  
  272.                 if (!m_prbRigidBody1->IsStatic()) {
  273.                     m_prbRigidBody1->m_vBiasLinVel += vPN * m_prbRigidBody1->InvMass();
  274.                     m_prbRigidBody1->m_vBiasAngVel += m_prbRigidBody1->m_mWorldInvInertia * vR1.Cross(vPN);
  275.                 }
  276.             }
  277.         }
  278.     }
  279.     //protected :
  280.     CRigidBody* m_prbRigidBody0;
  281.     CRigidBody* m_prbRigidBody1;
  282.     CContactManifold m_cmContactManifold;
  283. };
  284.  
  285.  
  286. #endif //#ifndef __CONTACT_CONSTRAINT_H__
  287.  
  288. //The inertia tensor of the cubes:
  289.  
  290.                 float fInvDensity = 0.5f;
  291.         float fVolume = vHalfSize.Len() * ML_TWO;
  292.         float fMass = fVolume * fInvDensity;
  293.  
  294.         rbBody.SetMass(fMass);
  295.  
  296.         fMass *= 0.333f;
  297.  
  298.         CMatrix4x4 mLocalInertia;
  299.         mLocalInertia._11 = fMass * vHalfSize.y * vHalfSize.y + vHalfSize.z * vHalfSize.z;
  300.         mLocalInertia._22 = fMass * vHalfSize.y * vHalfSize.x + vHalfSize.z * vHalfSize.z;
  301.         mLocalInertia._33 = fMass * vHalfSize.y * vHalfSize.x + vHalfSize.z * vHalfSize.y;
  302.  
  303.         rbBody.m_mLocalInvInertia = mLocalInertia.Inverse();
Advertisement
Add Comment
Please, Sign In to add comment
Public Pastes
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!