Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

use Dispatcher in ConcaveConvexCollisionAlgorithm (so it uses the registered collision algorithm, not hardcoded convexconcave)

Browse Source 
improved performance of constraint solver by precalculating the cross product/impulse arm
added collision comparison code: ODE box-box, also sphere-triangle
added safety check into GJK, and an assert for AABB's that are very large
write partid/triangle index outside of GJK
This commit is contained in:
ejcoumans
2006-10-28 02:06:19 +00:00
parent 7987be45c5
commit 3fe3b11924
24 changed files with 730 additions and 90 deletions
Download Patch File Download Diff File Expand all files Collapse all files

248
src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
View File

@@ -24,16 +24,7 @@ subject to the following restrictions:
#define ASSERT2 assert
//some values to find stable tresholds
float useGlobalSettingContacts = false;//true;
btScalar contactDamping = 0.2f;
btScalar contactTau = .02f;//0.02f;//*0.02f;
#define USE_INTERNAL_APPLY_IMPULSE 1
//bilateral constraint between two dynamic objects
@@ -75,7 +66,9 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
rel_vel = normal.dot(vel);
//todo: move this into proper structure
btScalar contactDamping = 0.2f;
#ifdef ONLY_USE_LINEAR_MASS
btScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass());
@@ -88,25 +81,17 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
//velocity + friction
//response between two dynamic objects with friction
float resolveSingleCollision(
btRigidBody& body1,
btRigidBody& body2,
btManifoldPoint& contactPoint,
const btContactSolverInfo& solverInfo
)
const btContactSolverInfo& solverInfo)
{
const btVector3& pos1 = contactPoint.getPositionWorldOnA();
const btVector3& pos2 = contactPoint.getPositionWorldOnB();
// printf("distance=%f\n",distance);
const btVector3& normal = contactPoint.m_normalWorldOnB;
const btVector3& normal = contactPoint.m_normalWorldOnB;
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
@@ -117,34 +102,18 @@ float resolveSingleCollision(
btScalar rel_vel;
rel_vel = normal.dot(vel);
btScalar Kfps = 1.f / solverInfo.m_timeStep ;
float damping = solverInfo.m_damping ;
float Kerp = solverInfo.m_erp;
if (useGlobalSettingContacts)
{
damping = contactDamping;
Kerp = contactTau;
}
float Kcor = Kerp *Kfps;
//printf("dist=%f\n",distance);
btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
assert(cpd);
btScalar distance = cpd->m_penetration;//contactPoint.getDistance();
//distance = 0.f;
btScalar distance = cpd->m_penetration;
btScalar positionalError = Kcor *-distance;
//jacDiagABInv;
btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
btScalar velocityImpulse = velocityError * cpd->m_jacDiagABInv;
@@ -158,9 +127,20 @@ float resolveSingleCollision(
normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
#ifdef USE_INTERNAL_APPLY_IMPULSE
if (body1.getInvMass())
{
body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
}
if (body2.getInvMass())
{
body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
}
#else USE_INTERNAL_APPLY_IMPULSE
body1.applyImpulse(normal*(normalImpulse), rel_pos1);
body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
#endif //USE_INTERNAL_APPLY_IMPULSE
return normalImpulse;
}
@@ -169,9 +149,86 @@ float resolveSingleFriction(
btRigidBody& body1,
btRigidBody& body2,
btManifoldPoint& contactPoint,
const btContactSolverInfo& solverInfo
const btContactSolverInfo& solverInfo)
{
)
const btVector3& pos1 = contactPoint.getPositionWorldOnA();
const btVector3& pos2 = contactPoint.getPositionWorldOnB();
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
assert(cpd);
float combinedFriction = cpd->m_friction;
btScalar limit = cpd->m_appliedImpulse * combinedFriction;
//if (contactPoint.m_appliedImpulse>0.f)
//friction
{
//apply friction in the 2 tangential directions
// 1st tangent
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
btVector3 vel = vel1 - vel2;
btScalar j1,j2;
{
btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
// calculate j that moves us to zero relative velocity
j1 = -vrel * cpd->m_jacDiagABInvTangent0;
float total = cpd->m_accumulatedTangentImpulse0 + j1;
GEN_set_min(total, limit);
GEN_set_max(total, -limit);
j1 = total - cpd->m_accumulatedTangentImpulse0;
cpd->m_accumulatedTangentImpulse0 = total;
}
{
// 2nd tangent
btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
// calculate j that moves us to zero relative velocity
j2 = -vrel * cpd->m_jacDiagABInvTangent1;
float total = cpd->m_accumulatedTangentImpulse1 + j2;
GEN_set_min(total, limit);
GEN_set_max(total, -limit);
j2 = total - cpd->m_accumulatedTangentImpulse1;
cpd->m_accumulatedTangentImpulse1 = total;
}
#ifdef USE_INTERNAL_APPLY_IMPULSE
if (body1.getInvMass())
{
body1.internalApplyImpulse(cpd->m_frictionWorldTangential0*body1.getInvMass(),cpd->m_frictionAngularComponent0A,j1);
body1.internalApplyImpulse(cpd->m_frictionWorldTangential1*body1.getInvMass(),cpd->m_frictionAngularComponent1A,j2);
}
if (body2.getInvMass())
{
body2.internalApplyImpulse(cpd->m_frictionWorldTangential0*body2.getInvMass(),cpd->m_frictionAngularComponent0B,-j1);
body2.internalApplyImpulse(cpd->m_frictionWorldTangential1*body2.getInvMass(),cpd->m_frictionAngularComponent1B,-j2);
}
#else USE_INTERNAL_APPLY_IMPULSE
body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1);
body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2);
#endif //USE_INTERNAL_APPLY_IMPULSE
}
return cpd->m_appliedImpulse;
}
float resolveSingleFrictionOriginal(
btRigidBody& body1,
btRigidBody& body2,
btManifoldPoint& contactPoint,
const btContactSolverInfo& solverInfo)
{
const btVector3& pos1 = contactPoint.getPositionWorldOnA();
@@ -232,3 +289,110 @@ float resolveSingleFriction(
}
return cpd->m_appliedImpulse;
}
//velocity + friction
//response between two dynamic objects with friction
float resolveSingleCollisionCombined(
btRigidBody& body1,
btRigidBody& body2,
btManifoldPoint& contactPoint,
const btContactSolverInfo& solverInfo)
{
const btVector3& pos1 = contactPoint.getPositionWorldOnA();
const btVector3& pos2 = contactPoint.getPositionWorldOnB();
const btVector3& normal = contactPoint.m_normalWorldOnB;
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
btVector3 vel = vel1 - vel2;
btScalar rel_vel;
rel_vel = normal.dot(vel);
btScalar Kfps = 1.f / solverInfo.m_timeStep ;
float damping = solverInfo.m_damping ;
float Kerp = solverInfo.m_erp;
float Kcor = Kerp *Kfps;
btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
assert(cpd);
btScalar distance = cpd->m_penetration;
btScalar positionalError = Kcor *-distance;
btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
btScalar velocityImpulse = velocityError * cpd->m_jacDiagABInv;
btScalar normalImpulse = penetrationImpulse+velocityImpulse;
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
float oldNormalImpulse = cpd->m_appliedImpulse;
float sum = oldNormalImpulse + normalImpulse;
cpd->m_appliedImpulse = 0.f > sum ? 0.f: sum;
normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
#ifdef USE_INTERNAL_APPLY_IMPULSE
if (body1.getInvMass())
{
body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
}
if (body2.getInvMass())
{
body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
}
#else USE_INTERNAL_APPLY_IMPULSE
body1.applyImpulse(normal*(normalImpulse), rel_pos1);
body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
#endif //USE_INTERNAL_APPLY_IMPULSE
{
//friction
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
btVector3 vel = vel1 - vel2;
rel_vel = normal.dot(vel);
btVector3 lat_vel = vel - normal * rel_vel;
btScalar lat_rel_vel = lat_vel.length();
float combinedFriction = cpd->m_friction;
if (cpd->m_appliedImpulse > 0)
if (lat_rel_vel > SIMD_EPSILON)
{
lat_vel /= lat_rel_vel;
btVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel);
btVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel);
btScalar friction_impulse = lat_rel_vel /
(body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction;
GEN_set_min(friction_impulse, normal_impulse);
GEN_set_max(friction_impulse, -normal_impulse);
body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
}
}
return normalImpulse;
}
float resolveSingleFrictionEmpty(
btRigidBody& body1,
btRigidBody& body2,
btManifoldPoint& contactPoint,
const btContactSolverInfo& solverInfo)
{
return 0.f;
};

9
src/BulletDynamics/ConstraintSolver/btContactConstraint.h
View File

@@ -72,6 +72,15 @@ struct btConstraintPersistentData
float m_penetration;
btVector3 m_frictionWorldTangential0;
btVector3 m_frictionWorldTangential1;
btVector3 m_frictionAngularComponent0A;
btVector3 m_frictionAngularComponent0B;
btVector3 m_frictionAngularComponent1A;
btVector3 m_frictionAngularComponent1B;
//some data doesn't need to be persistent over frames: todo: clean/reuse this
btVector3 m_angularComponentA;
btVector3 m_angularComponentB;
ContactSolverFunc m_contactSolverFunc;
ContactSolverFunc m_frictionSolverFunc;

33
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

@@ -74,8 +74,6 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man
for (j=0;j<numManifolds;j++)
{
int k=j;
//interleaving the preparation with solving impacts the behaviour a lot, todo: find out why
prepareConstraints(manifoldPtr[k],info,debugDrawer);
solve(manifoldPtr[k],info,0,debugDrawer);
}
@@ -232,6 +230,7 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
cpd->m_penetration = 0.f;
}
float relaxation = info.m_damping;
cpd->m_appliedImpulse *= relaxation;
@@ -266,6 +265,36 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
#endif //NO_FRICTION_WARMSTART
cp.m_normalWorldOnB*cpd->m_appliedImpulse;
///
{
btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
cpd->m_angularComponentA = body0->getInvInertiaTensorWorld()*torqueAxis0;
btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
cpd->m_angularComponentB = body1->getInvInertiaTensorWorld()*torqueAxis1;
}
{
btVector3 ftorqueAxis0 = rel_pos1.cross(cpd->m_frictionWorldTangential0);
cpd->m_frictionAngularComponent0A = body0->getInvInertiaTensorWorld()*ftorqueAxis0;
}
{
btVector3 ftorqueAxis1 = rel_pos1.cross(cpd->m_frictionWorldTangential1);
cpd->m_frictionAngularComponent1A = body0->getInvInertiaTensorWorld()*ftorqueAxis1;
}
{
btVector3 ftorqueAxis0 = rel_pos2.cross(cpd->m_frictionWorldTangential0);
cpd->m_frictionAngularComponent0B = body1->getInvInertiaTensorWorld()*ftorqueAxis0;
}
{
btVector3 ftorqueAxis1 = rel_pos2.cross(cpd->m_frictionWorldTangential1);
cpd->m_frictionAngularComponent1B = body1->getInvInertiaTensorWorld()*ftorqueAxis1;
}
///
//apply previous frames impulse on both bodies
body0->applyImpulse(totalImpulse, rel_pos1);
body1->applyImpulse(-totalImpulse, rel_pos2);

23
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
View File

@@ -502,7 +502,28 @@ void btDiscreteDynamicsWorld::updateAabbs()
btPoint3 minAabb,maxAabb;
colObj->m_collisionShape->getAabb(colObj->m_worldTransform, minAabb,maxAabb);
btSimpleBroadphase* bp = (btSimpleBroadphase*)m_broadphasePairCache;
bp->setAabb(body->m_broadphaseHandle,minAabb,maxAabb);
if ( colObj->m_collisionShape->isInfinite() || ((maxAabb-minAabb).length2() < 1e12f))
{
bp->setAabb(body->m_broadphaseHandle,minAabb,maxAabb);
} else
{
//something went wrong, investigate
//this assert is unwanted in 3D modelers (danger of loosing work)
assert(0);
body->SetActivationState(DISABLE_SIMULATION);
static bool reportMe = true;
if (reportMe)
{
reportMe = false;
printf("Overflow in AABB, object removed from simulation \n");
printf("If you can reproduce this, please email bugs@continuousphysics.com\n");
printf("Please include above information, your Platform, version of OS.\n");
printf("Thanks.\n");
}
}
if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
{
DrawAabb(m_debugDrawer,minAabb,maxAabb,colorvec);

10
src/BulletDynamics/Dynamics/btRigidBody.h
View File

@@ -158,6 +158,16 @@ public:
applyTorqueImpulse(rel_pos.cross(impulse));
}
}
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,float impulseMagnitude)
{
if (m_inverseMass != 0.f)
{
m_linearVelocity += linearComponent*impulseMagnitude;
m_angularVelocity += angularComponent*impulseMagnitude;
}
}
void clearForces()
{