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

Added new EPA penetration depth solver

Browse Source 
improved gjk/minkowski sampling pd method
added original solver variant, use btSequentialImpulseConstraintSolver2
Added Pierre Terdimans PD testbed
This commit is contained in:
ejcoumans
2006-11-16 06:14:12 +00:00
parent 25f3d00f01
commit b07bb88a2d
9 changed files with 858 additions and 708 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1320
src/BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp
View File

File diff suppressed because it is too large Load Diff

6
src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
View File

@@ -14,8 +14,8 @@ subject to the following restrictions:
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef EPA_PENETRATION_DEPTH_H
#define EPA_PENETRATION_DEPTH_H
#ifndef BT_GJP_EPA_PENETRATION_DEPTH_H
#define BT_GJP_EPA_PENETRATION_DEPTH_H
#include "btConvexPenetrationDepthSolver.h"
@@ -35,5 +35,5 @@ class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver
};
#endif // EPA_PENETRATION_DEPTH_H
#endif // BT_GJP_EPA_PENETRATION_DEPTH_H

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

@@ -63,8 +63,14 @@ bool MyContactDestroyedCallback(void* userPersistentData)
return true;
}
btSequentialImpulseConstraintSolver2::btSequentialImpulseConstraintSolver2()
{
setSolverMode(SOLVER_USE_WARMSTARTING);
}
btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
:m_solverMode(SOLVER_RANDMIZE_ORDER)
{
gContactDestroyedCallback = &MyContactDestroyedCallback;
@@ -92,18 +98,13 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man
int totalPoints = 0;
{
int j;
for (j=0;j<numManifolds;j++)
{
prepareConstraints(manifoldPtr[j],info,debugDrawer);
}
}
{
int j;
for (j=0;j<numManifolds;j++)
{
btPersistentManifold* manifold = manifoldPtr[j];
prepareConstraints(manifold,info,debugDrawer);
for (int p=0;p<manifoldPtr[j]->getNumContacts();p++)
{
gOrder[totalPoints].m_manifoldIndex = j;
@@ -113,36 +114,111 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man
}
}
//should traverse the contacts random order...
int iteration;
{
for ( iteration = 0;iteration<numiter-1;iteration++)
{
int j;
if (m_solverMode & SOLVER_RANDMIZE_ORDER)
{
if ((iteration & 7) == 0) {
for (j=0; j<totalPoints; ++j) {
btOrderIndex tmp = gOrder[j];
int swapi = btRandInt2(j+1);
gOrder[j] = gOrder[swapi];
gOrder[swapi] = tmp;
}
}
}
for (j=0;j<totalPoints;j++)
{
btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
solve( (btRigidBody*)manifold->getBody0(),
(btRigidBody*)manifold->getBody1()
,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
}
for (j=0;j<totalPoints;j++)
{
btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
solveFriction((btRigidBody*)manifold->getBody0(),
(btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
}
}
}
#ifdef USE_PROFILE
btProfiler::endBlock("solve");
#endif //USE_PROFILE
return 0.f;
}
/// btSequentialImpulseConstraintSolver Sequentially applies impulses
float btSequentialImpulseConstraintSolver2::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
{
btContactSolverInfo info = infoGlobal;
int numiter = infoGlobal.m_numIterations;
#ifdef USE_PROFILE
btProfiler::beginBlock("solve");
#endif //USE_PROFILE
{
int j;
for (j=0;j<numManifolds;j++)
{
btPersistentManifold* manifold = manifoldPtr[j];
prepareConstraints(manifold,info,debugDrawer);
for (int p=0;p<manifoldPtr[j]->getNumContacts();p++)
{
//interleaving here gives better results
solve( (btRigidBody*)manifold->getBody0(),
(btRigidBody*)manifold->getBody1()
,manifoldPtr[j]->getContactPoint(p),info,0,debugDrawer);
}
}
}
//should traverse the contacts random order...
int iteration;
for ( iteration = 0;iteration<numiter-1;iteration++)
{
int j;
if ((iteration & 7) == 0) {
for (j=0; j<totalPoints; ++j) {
btOrderIndex tmp = gOrder[j];
int swapi = btRandInt2(j+1);
gOrder[j] = gOrder[swapi];
gOrder[swapi] = tmp;
for (j=0;j<numManifolds;j++)
{
btPersistentManifold* manifold = manifoldPtr[j];
for (int p=0;p<manifold->getNumContacts();p++)
{
solve( (btRigidBody*)manifold->getBody0(),
(btRigidBody*)manifold->getBody1()
,manifold->getContactPoint(p),info,iteration,debugDrawer);
}
}
for (j=0;j<totalPoints;j++)
{
btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
solve( (btRigidBody*)manifold->getBody0(),
(btRigidBody*)manifold->getBody1()
,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
}
for (j=0;j<totalPoints;j++)
}
for ( iteration = 0;iteration<numiter-1;iteration++)
{
int j;
for (j=0;j<numManifolds;j++)
{
btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
solveFriction((btRigidBody*)manifold->getBody0(),
(btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
btPersistentManifold* manifold = manifoldPtr[j];
for (int p=0;p<manifold->getNumContacts();p++)
{
solveFriction((btRigidBody*)manifold->getBody0(),
(btRigidBody*)manifold->getBody1(),manifold->getContactPoint(p),info,iteration,debugDrawer);
}
}
}
#ifdef USE_PROFILE
btProfiler::endBlock("solve");
@@ -264,7 +340,14 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
float relaxation = info.m_damping;
cpd->m_appliedImpulse =0.f;//*= relaxation;
if (m_solverMode & SOLVER_USE_WARMSTARTING)
{
cpd->m_appliedImpulse *= relaxation;
} else
{
cpd->m_appliedImpulse =0.f;
}
//for friction
cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;

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

@@ -30,6 +30,7 @@ class btIDebugDraw;
class btSequentialImpulseConstraintSolver : public btConstraintSolver
{
protected:
float solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
float solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
void prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer);
@@ -37,8 +38,18 @@ class btSequentialImpulseConstraintSolver : public btConstraintSolver
ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
//choose between several modes, different friction model etc.
int m_solverMode;
public:
enum eSolverMode
{
SOLVER_RANDMIZE_ORDER = 1,
SOLVER_FRICTION_SEPARATE = 2,
SOLVER_USE_WARMSTARTING = 4
};
btSequentialImpulseConstraintSolver();
///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody
@@ -59,7 +70,29 @@ public:
virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
void setSolverMode(int mode)
{
m_solverMode = mode;
}
int getSolverMode() const
{
return m_solverMode;
}
};
/// Small variation on btSequentialImpulseConstraintSolver: warmstarting, separate friction, non-randomized ordering
class btSequentialImpulseConstraintSolver2 : public btSequentialImpulseConstraintSolver
{
public:
btSequentialImpulseConstraintSolver2();
virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
};
#endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H