From cf2f1956de48a3671cd8ce25a2e987af7ba933d5 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 2 Apr 2006 20:15:24 +0000 Subject: [PATCH] fixed a crashing bug in new vehicle physics, and removed some debugging code in contact/friction physics code. --- .../CollisionDispatch/CollisionWorld.cpp | 16 ++- .../MinkowskiPenetrationDepthSolver.cpp | 9 +- .../ConstraintSolver/ContactConstraint.cpp | 16 +-- .../ConstraintSolver/ContactSolverInfo.h | 4 +- .../SimpleConstraintSolver.cpp | 130 +++++++++--------- 5 files changed, 85 insertions(+), 90 deletions(-) diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp b/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp index 99e2bc5e9c7..5264662b519 100644 --- a/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp +++ b/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp @@ -124,14 +124,16 @@ void CollisionWorld::RemoveCollisionObject(CollisionObject* collisionObject) //bool removeFromBroadphase = false; { - /*BroadphaseInterface* scene = */GetBroadphase(); - BroadphaseProxy* bp = collisionObject->m_broadphaseHandle; - // - // only clear the cached algorithms - // - GetBroadphase()->CleanProxyFromPairs(bp); - GetBroadphase()->DestroyProxy(bp); + BroadphaseProxy* bp = collisionObject->m_broadphaseHandle; + if (bp) + { + // + // only clear the cached algorithms + // + GetBroadphase()->CleanProxyFromPairs(bp); + GetBroadphase()->DestroyProxy(bp); + } } diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp index d482be27659..23b92c63792 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp +++ b/extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp @@ -128,7 +128,7 @@ bool MinkowskiPenetrationDepthSolver::CalcPenDepth(SimplexSolverInterface& simpl } } - +//#define DEBUG_DRAW 1 #ifdef DEBUG_DRAW if (debugDraw) { @@ -165,7 +165,9 @@ bool MinkowskiPenetrationDepthSolver::CalcPenDepth(SimplexSolverInterface& simpl MyResult res; gjkdet.GetClosestPoints(input,res,debugDraw); - + //the penetration depth is over-estimated, relax it + float penetration_relaxation= 0.1f; + minNorm*=penetration_relaxation; if (res.m_hasResult) { @@ -185,3 +187,6 @@ bool MinkowskiPenetrationDepthSolver::CalcPenDepth(SimplexSolverInterface& simpl } return res.m_hasResult; } + + + diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp index 38c82fdb07d..e8e8d37f6d6 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp @@ -123,18 +123,6 @@ float resolveSingleCollision( // printf("distance=%f\n",distance); - if (distance>0.f) - { - contactPoint.m_appliedImpulse = 0.f; - contactPoint.m_accumulatedTangentImpulse0 = 0.f; - contactPoint.m_accumulatedTangentImpulse1 = 0.f; - - return 0.f; - } - -#define MAXPENETRATIONPERFRAME -0.05 - distance = distance < MAXPENETRATIONPERFRAME? MAXPENETRATIONPERFRAME:distance; - const SimdVector3& normal = contactPoint.m_normalWorldOnB; SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); @@ -153,7 +141,7 @@ float resolveSingleCollision( SimdScalar Kfps = 1.f / solverInfo.m_timeStep ; float damping = solverInfo.m_damping ; - float Kerp = solverInfo.m_tau; + float Kerp = solverInfo.m_erp; if (useGlobalSettingContacts) { @@ -168,7 +156,7 @@ float resolveSingleCollision( float clipDist = distance + allowedPenetration; float dist = (clipDist > 0.f) ? 0.f : clipDist; //distance = 0.f; - SimdScalar positionalError = Kcor *-clipDist; + SimdScalar positionalError = Kcor *-dist; //jacDiagABInv; SimdScalar velocityError = -(1.0f + restitution) * rel_vel;// * damping; diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/ContactSolverInfo.h b/extern/bullet/BulletDynamics/ConstraintSolver/ContactSolverInfo.h index ec006dab64b..6c6325df8a6 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/ContactSolverInfo.h +++ b/extern/bullet/BulletDynamics/ConstraintSolver/ContactSolverInfo.h @@ -23,11 +23,12 @@ struct ContactSolverInfo inline ContactSolverInfo() { m_tau = 0.4f; - m_damping = 0.9f; + m_damping = 1.0f; m_friction = 0.3f; m_restitution = 0.f; m_maxErrorReduction = 20.f; m_numIterations = 10; + m_erp = 0.4f; m_sor = 1.3f; } @@ -39,6 +40,7 @@ struct ContactSolverInfo int m_numIterations; float m_maxErrorReduction; float m_sor; + float m_erp; }; diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp index 7fd42d04775..acfca631b72 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp @@ -26,13 +26,6 @@ subject to the following restrictions: #include "JacobianEntry.h" #include "GEN_MinMax.h" -//debugging -bool doApplyImpulse = true; - - - -bool useImpulseFriction = true;//true;//false; - @@ -97,57 +90,59 @@ float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const Conta for (int i=0;iGetContactPoint(i); - const SimdVector3& pos1 = cp.GetPositionWorldOnA(); - const SimdVector3& pos2 = cp.GetPositionWorldOnB(); + if (cp.GetDistance() <= 0.f) + { + const SimdVector3& pos1 = cp.GetPositionWorldOnA(); + const SimdVector3& pos2 = cp.GetPositionWorldOnB(); - SimdVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition(); - SimdVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition(); - - //this jacobian entry is re-used for all iterations - JacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(), - body1->getCenterOfMassTransform().getBasis().transpose(), - rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(), - body1->getInvInertiaDiagLocal(),body1->getInvMass()); + SimdVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition(); + SimdVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition(); + + //this jacobian entry is re-used for all iterations + JacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(), + body1->getCenterOfMassTransform().getBasis().transpose(), + rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(), + body1->getInvInertiaDiagLocal(),body1->getInvMass()); - SimdScalar jacDiagAB = jac.getDiagonal(); - - cp.m_jacDiagABInv = 1.f / jacDiagAB; + SimdScalar jacDiagAB = jac.getDiagonal(); + + cp.m_jacDiagABInv = 1.f / jacDiagAB; - //for friction - cp.m_prevAppliedImpulse = cp.m_appliedImpulse; + //for friction + cp.m_prevAppliedImpulse = cp.m_appliedImpulse; - float relaxation = info.m_damping; - cp.m_appliedImpulse *= relaxation; - - //re-calculate friction direction every frame, todo: check if this is really needed - SimdPlaneSpace1(cp.m_normalWorldOnB,cp.m_frictionWorldTangential0,cp.m_frictionWorldTangential1); + float relaxation = info.m_damping; + cp.m_appliedImpulse *= relaxation; + + //re-calculate friction direction every frame, todo: check if this is really needed + SimdPlaneSpace1(cp.m_normalWorldOnB,cp.m_frictionWorldTangential0,cp.m_frictionWorldTangential1); -#ifdef NO_FRICTION_WARMSTART - cp.m_accumulatedTangentImpulse0 = 0.f; - cp.m_accumulatedTangentImpulse1 = 0.f; -#endif //NO_FRICTION_WARMSTART - float denom0 = body0->ComputeImpulseDenominator(pos1,cp.m_frictionWorldTangential0); - float denom1 = body1->ComputeImpulseDenominator(pos2,cp.m_frictionWorldTangential0); - float denom = relaxation/(denom0+denom1); - cp.m_jacDiagABInvTangent0 = denom; + #ifdef NO_FRICTION_WARMSTART + cp.m_accumulatedTangentImpulse0 = 0.f; + cp.m_accumulatedTangentImpulse1 = 0.f; + #endif //NO_FRICTION_WARMSTART + float denom0 = body0->ComputeImpulseDenominator(pos1,cp.m_frictionWorldTangential0); + float denom1 = body1->ComputeImpulseDenominator(pos2,cp.m_frictionWorldTangential0); + float denom = relaxation/(denom0+denom1); + cp.m_jacDiagABInvTangent0 = denom; - denom0 = body0->ComputeImpulseDenominator(pos1,cp.m_frictionWorldTangential1); - denom1 = body1->ComputeImpulseDenominator(pos2,cp.m_frictionWorldTangential1); - denom = relaxation/(denom0+denom1); - cp.m_jacDiagABInvTangent1 = denom; + denom0 = body0->ComputeImpulseDenominator(pos1,cp.m_frictionWorldTangential1); + denom1 = body1->ComputeImpulseDenominator(pos2,cp.m_frictionWorldTangential1); + denom = relaxation/(denom0+denom1); + cp.m_jacDiagABInvTangent1 = denom; - SimdVector3 totalImpulse = -#ifndef NO_FRICTION_WARMSTART - cp.m_frictionWorldTangential0*cp.m_accumulatedTangentImpulse0+ - cp.m_frictionWorldTangential1*cp.m_accumulatedTangentImpulse1+ -#endif //NO_FRICTION_WARMSTART - cp.m_normalWorldOnB*cp.m_appliedImpulse; - - //apply previous frames impulse on both bodies - body0->applyImpulse(totalImpulse, rel_pos1); - body1->applyImpulse(-totalImpulse, rel_pos2); + SimdVector3 totalImpulse = + #ifndef NO_FRICTION_WARMSTART + cp.m_frictionWorldTangential0*cp.m_accumulatedTangentImpulse0+ + cp.m_frictionWorldTangential1*cp.m_accumulatedTangentImpulse1+ + #endif //NO_FRICTION_WARMSTART + cp.m_normalWorldOnB*cp.m_appliedImpulse; + //apply previous frames impulse on both bodies + body0->applyImpulse(totalImpulse, rel_pos1); + body1->applyImpulse(-totalImpulse, rel_pos2); + } } } @@ -166,26 +161,29 @@ float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const Conta j=i; ManifoldPoint& cp = manifoldPtr->GetContactPoint(j); - - if (iter == 0) - { - if (debugDrawer) - debugDrawer->DrawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.GetDistance(),cp.GetLifeTime(),color); - } - + if (cp.GetDistance() <= 0.f) { + if (iter == 0) + { + if (debugDrawer) + debugDrawer->DrawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.GetDistance(),cp.GetLifeTime(),color); + } - //float dist = cp.GetDistance(); - //printf("dist(%i)=%f\n",j,dist); - float impulse = resolveSingleCollision( - *body0,*body1, - cp, - info); - - if (maxImpulse < impulse) - maxImpulse = impulse; + { + + //float dist = cp.GetDistance(); + //printf("dist(%i)=%f\n",j,dist); + float impulse = resolveSingleCollision( + *body0,*body1, + cp, + info); + + if (maxImpulse < impulse) + maxImpulse = impulse; + + } } } } @@ -208,7 +206,7 @@ float SimpleConstraintSolver::SolveFriction(PersistentManifold* manifoldPtr, con int j=i; ManifoldPoint& cp = manifoldPtr->GetContactPoint(j); - + if (cp.GetDistance() <= 0.f) { resolveSingleFriction(