forked from bartvdbraak/blender
fixed a crashing bug in new vehicle physics, and removed some debugging code in contact/friction physics code.
This commit is contained in:
parent
1d5cca805b
commit
cf2f1956de
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
@ -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;
|
||||
|
||||
|
@ -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;
|
||||
|
||||
};
|
||||
|
||||
|
@ -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;i<numpoints ;i++)
|
||||
{
|
||||
ManifoldPoint& cp = manifoldPtr->GetContactPoint(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();
|
||||
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());
|
||||
//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();
|
||||
SimdScalar jacDiagAB = jac.getDiagonal();
|
||||
|
||||
cp.m_jacDiagABInv = 1.f / jacDiagAB;
|
||||
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;
|
||||
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);
|
||||
//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(
|
||||
|
Loading…
Reference in New Issue
Block a user