Changeset 2430 for code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver

Timestamp:

Dec 13, 2008, 11:45:51 PM (15 years ago)

Author:

rgrieder

Message:

Updated to Bullet 2.73 (first part).

Location:

code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver

Files:

• btConeTwistConstraint.h (modified) (1 diff)
• btContactConstraint.cpp (modified) (1 diff)
• btContactConstraint.h (modified) (1 diff)
• btContactSolverInfo.h (modified) (3 diffs)
• btGeneric6DofConstraint.cpp (modified) (2 diffs)
• btHingeConstraint.cpp (modified) (1 diff)
• btSequentialImpulseConstraintSolver.cpp (modified) (7 diffs)
• btSequentialImpulseConstraintSolver.h (modified) (2 diffs)
• btSolverBody.h (modified) (1 diff)

code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h

@@ -21 +21 @@
 #define CONETWISTCONSTRAINT_H
 
-#include "../../LinearMath/btVector3.h"
+#include "LinearMath/btVector3.h"
 #include "btJacobianEntry.h"
 #include "btTypedConstraint.h"

code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp

@@ -426 +426 @@
 
         return btScalar(0.);
-};
-
+}
+

code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.h

@@ -17 +17 @@
 #define CONTACT_CONSTRAINT_H
 
-//todo: make into a proper class working with the iterative constraint solver
+///@todo: make into a proper class working with the iterative constraint solver
 
 class btRigidBody;

code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

@@ -22 +22 @@
         SOLVER_FRICTION_SEPARATE = 2,
         SOLVER_USE_WARMSTARTING = 4,
-        SOLVER_CACHE_FRIENDLY = 8
+        SOLVER_USE_FRICTION_WARMSTARTING = 8,
+        SOLVER_CACHE_FRIENDLY = 16
 };
 
@@ -45 +46 @@
 
         int                     m_solverMode;
+        int     m_restingContactRestitutionThreshold;
 
 
@@ -69 +71 @@
                 m_linearSlop = btScalar(0.0);
                 m_warmstartingFactor=btScalar(0.85);
-                m_solverMode = SOLVER_RANDMIZE_ORDER | SOLVER_CACHE_FRIENDLY | SOLVER_USE_WARMSTARTING;
+                m_solverMode = SOLVER_CACHE_FRIENDLY |  SOLVER_RANDMIZE_ORDER |  SOLVER_USE_WARMSTARTING;
+                m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution
         }
 };

code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp

@@ -27 +27 @@
 
 
-static const btScalar kSign[] = { btScalar(1.0), btScalar(-1.0), btScalar(1.0) };
-static const int kAxisA[] = { 1, 0, 0 };
-static const int kAxisB[] = { 2, 2, 1 };
 #define GENERIC_D6_DISABLE_WARMSTARTING 1
 
@@ -157 +154 @@
     btScalar clippedMotorImpulse;
 
-    //todo: should clip against accumulated impulse
+    ///@todo: should clip against accumulated impulse
     if (unclippedMotorImpulse>0.0f)
     {

code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

@@ -324 +324 @@
                                         getRigidBodyB().computeAngularImpulseDenominator(normal);
                                 // scale for mass and relaxation
-                                //todo:  expose this 0.9 factor to developer
                                 velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
                         }

code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -438 +438 @@
 
 
-void    btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation)
+btSolverConstraint&     btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation)
 {
 
@@ -492 +492 @@
         solverConstraint.m_jacDiagABInv = denom;
 
-
+        return solverConstraint;
 }
 
@@ -719 +719 @@
 
                                                         solverConstraint.m_friction = cp.m_combinedFriction;
-                                                        solverConstraint.m_restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
-                                                        if (solverConstraint.m_restitution <= btScalar(0.))
+
+                                                        
+                                                        if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
                                                         {
                                                                 solverConstraint.m_restitution = 0.f;
-                                                        };
+                                                        } else
+                                                        {
+                                                                solverConstraint.m_restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
+                                                                if (solverConstraint.m_restitution <= btScalar(0.))
+                                                                {
+                                                                        solverConstraint.m_restitution = 0.f;
+                                                                };
+                                                        }
 
                                                         
@@ -761 +769 @@
                                                                         cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
                                                                         addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-                                                                        cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
-                                                                        cp.m_lateralFrictionDir2.normalize();//??
-                                                                        addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                                                        if(infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
+                                                                        {
+                                                                                cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
+                                                                                cp.m_lateralFrictionDir2.normalize();//??
+                                                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                                                                cp.m_lateralFrictionInitialized = true;
+                                                                        }
                                                                 } else
                                                                 {
                                                                         //re-calculate friction direction every frame, todo: check if this is really needed
-                                                                        
                                                                         btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
                                                                         addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
                                                                         addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                                                        if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
+                                                                        {
+                                                                                cp.m_lateralFrictionInitialized = true;
+                                                                        }
                                                                 }
-                                                                cp.m_lateralFrictionInitialized = true;
                                                                 
                                                         } else
                                                         {
                                                                 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-                                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                                                if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
+                                                                        addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
                                                         }
 
+                                                        if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
                                                         {
-                                                                btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex];
-                                                                if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
                                                                 {
-                                                                        frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
-                                                                        if (rb0)
-                                                                                m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
-                                                                        if (rb1)
-                                                                                m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
-                                                                } else
+                                                                        btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex];
+                                                                        if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+                                                                        {
+                                                                                frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
+                                                                                if (rb0)
+                                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
+                                                                                if (rb1)
+                                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
+                                                                        } else
+                                                                        {
+                                                                                frictionConstraint1.m_appliedImpulse = 0.f;
+                                                                        }
+                                                                }
                                                                 {
-                                                                        frictionConstraint1.m_appliedImpulse = 0.f;
-                                                                }
-                                                        }
-                                                        {
-                                                                btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
-                                                                if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-                                                                {
-                                                                        frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
-                                                                        if (rb0)
-                                                                                m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
-                                                                        if (rb1)
-                                                                                m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
-                                                                } else
-                                                                {
-                                                                        frictionConstraint2.m_appliedImpulse = 0.f;
+                                                                        btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
+                                                                        if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+                                                                        {
+                                                                                frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
+                                                                                if (rb0)
+                                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
+                                                                                if (rb1)
+                                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
+                                                                        } else
+                                                                        {
+                                                                                frictionConstraint2.m_appliedImpulse = 0.f;
+                                                                        }
                                                                 }
                                                         }
@@ -833 +851 @@
         int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
 
-        ///todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
+        ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
         m_orderTmpConstraintPool.resize(numConstraintPool);
         m_orderFrictionConstraintPool.resize(numFrictionPool);
@@ -985 +1003 @@
                 btAssert(pt);
                 pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
-                pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
-                pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
+                if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
+                {
+                        pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+                        pt->m_appliedImpulseLateral2 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
+                }
 
                 //do a callback here?
@@ -1221 +1242 @@
                                 cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations);
                                 cpd->m_friction = cp.m_combinedFriction;
-                                cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
-                                if (cpd->m_restitution <= btScalar(0.))
-                                {
-                                        cpd->m_restitution = btScalar(0.0);
-
-                                };
+                                if (cp.m_lifeTime>info.m_restingContactRestitutionThreshold)
+                                {
+                                        cpd->m_restitution = 0.f;
+                                } else
+                                {
+                                        cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
+                                        if (cpd->m_restitution <= btScalar(0.))
+                                        {
+                                                cpd->m_restitution = btScalar(0.0);
+                                        };
+                                }
                                 
                                 //restitution and penetration work in same direction so

code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -24 +24 @@
 
 
-/// btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses
-/// The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com
-/// Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP)
-/// Applies impulses for combined restitution and penetration recovery and to simulate friction
+///The btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses
+///The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com
+///Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP)
+///Applies impulses for combined restitution and penetration recovery and to simulate friction
 class btSequentialImpulseConstraintSolver : public btConstraintSolver
 {
@@ -42 +42 @@
         btScalar solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
         void  prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer);
-        void    addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation);
+        btSolverConstraint&     addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation);
 
         ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];

code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h

@@ -25 +25 @@
 
 
-///btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
+///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
 ATTRIBUTE_ALIGNED16 (struct)    btSolverBody
 {
         BT_DECLARE_ALIGNED_ALLOCATOR();
         
+        btMatrix3x3             m_worldInvInertiaTensor;
+
         btVector3               m_angularVelocity;
+        btVector3               m_linearVelocity;
+        btVector3               m_centerOfMassPosition;
+        btVector3               m_pushVelocity;
+        btVector3               m_turnVelocity;
+
         float                   m_angularFactor;
         float                   m_invMass;
         float                   m_friction;
         btRigidBody*    m_originalBody;
-        btVector3               m_linearVelocity;
-        btVector3               m_centerOfMassPosition;
-        
-        btVector3               m_pushVelocity;
-        btVector3               m_turnVelocity;