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

updated Bullet sequential impulse constraint solver, so it matches 100% ODE PGS quickstep solver innerloop, mainly by renaming variables...

Browse Source
This commit is contained in:
erwin.coumans
2008-11-26 00:27:35 +00:00
parent 09aa2dbbe7
commit 82047e601e
28 changed files with 646 additions and 1135 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
View File

@@ -94,8 +94,6 @@ void btConeTwistConstraint::buildJacobian()
for (int i=0;i<3;i++)
{
new (&m_jac[i]) btJacobianEntry(
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
pivotAInW - m_rbA.getCenterOfMassPosition(),
pivotBInW - m_rbB.getCenterOfMassPosition(),
normal[i],

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

@@ -52,8 +52,7 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
btVector3 vel = vel1 - vel2;
btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
body2.getCenterOfMassTransform().getBasis().transpose(),
btJacobianEntry jac(
rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
body2.getInvInertiaDiagLocal(),body2.getInvMass());

6
src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
View File

@@ -39,6 +39,7 @@ struct btContactSolverInfoData
btScalar m_sor;
btScalar m_erp;//used as Baumgarte factor
btScalar m_erp2;//used in Split Impulse
btScalar m_globalCfm;//constraint force mixing
int m_splitImpulse;
btScalar m_splitImpulsePenetrationThreshold;
btScalar m_linearSlop;
@@ -63,9 +64,10 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_restitution = btScalar(0.);
m_maxErrorReduction = btScalar(20.);
m_numIterations = 10;
m_erp = btScalar(0.2);
m_erp = btScalar(0.4);
m_erp2 = btScalar(0.1);
m_sor = btScalar(1.3);
m_globalCfm = btScalar(0.);
m_sor = btScalar(1.);
m_splitImpulse = false;
m_splitImpulsePenetrationThreshold = -0.02f;
m_linearSlop = btScalar(0.0);

3
src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
View File

@@ -345,8 +345,7 @@ void btGeneric6DofConstraint::buildLinearJacobian(
const btVector3 & pivotAInW,const btVector3 & pivotBInW)
{
new (&jacLinear) btJacobianEntry(
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
pivotAInW - m_rbA.getCenterOfMassPosition(),
pivotBInW - m_rbB.getCenterOfMassPosition(),
normalWorld,

2
src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
View File

@@ -181,8 +181,6 @@ void btHingeConstraint::buildJacobian()
for (int i=0;i<3;i++)
{
new (&m_jac[i]) btJacobianEntry(
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
pivotAInW - m_rbA.getCenterOfMassPosition(),
pivotBInW - m_rbB.getCenterOfMassPosition(),
normal[i],

16
src/BulletDynamics/ConstraintSolver/btJacobianEntry.h
View File

@@ -34,8 +34,6 @@ public:
btJacobianEntry() {};
//constraint between two different rigidbodies
btJacobianEntry(
const btMatrix3x3& world2A,
const btMatrix3x3& world2B,
const btVector3& rel_pos1,const btVector3& rel_pos2,
const btVector3& jointAxis,
const btVector3& inertiaInvA,
@@ -44,8 +42,8 @@ public:
const btScalar massInvB)
:m_linearJointAxis(jointAxis)
{
m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis));
m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis));
m_aJ = (rel_pos1.cross(m_linearJointAxis));
m_bJ = (rel_pos2.cross(-m_linearJointAxis));
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
@@ -61,8 +59,8 @@ public:
const btVector3& inertiaInvB)
:m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
{
m_aJ= world2A*jointAxis;
m_bJ = world2B*-jointAxis;
m_aJ= jointAxis;
m_bJ = -jointAxis;
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
@@ -95,8 +93,8 @@ public:
const btScalar massInvA)
:m_linearJointAxis(jointAxis)
{
m_aJ= world2A*(rel_pos1.cross(jointAxis));
m_bJ = world2A*(rel_pos2.cross(-jointAxis));
m_aJ= (rel_pos1.cross(jointAxis));
m_bJ = (rel_pos2.cross(-jointAxis));
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = btVector3(btScalar(0.),btScalar(0.),btScalar(0.));
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
@@ -130,7 +128,7 @@ public:
return sum[0]+sum[1]+sum[2];
}
btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB)
btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB) const
{
btVector3 linrel = linvelA - linvelB;
btVector3 angvela = angvelA * m_aJ;

3
src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
View File

@@ -48,8 +48,7 @@ void btPoint2PointConstraint::buildJacobian()
{
normal[i] = 1;
new (&m_jac[i]) btJacobianEntry(
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
normal,

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

File diff suppressed because it is too large Load Diff

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

@@ -38,52 +38,45 @@ class btSequentialImpulseConstraintSolver : public btConstraintSolver
protected:
btScalar solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
btScalar solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
void prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer);
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];
ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
unsigned long m_btSeed2;
void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
void resolveSplitPenetrationImpulseCacheFriendly(
btSolverBody& body1,
btSolverBody& body2,
const btSolverConstraint& contactConstraint,
const btContactSolverInfo& solverInfo);
void resolveSingleConstraintRow(
btSolverBody& body1,
btSolverBody& body2,
const btSolverConstraint& contactConstraint);
void resolveSingleFrictionCacheFriendly(
btSolverBody& body1,
btSolverBody& body2,
const btSolverConstraint& contactConstraint,
const btContactSolverInfo& solverInfo,
btScalar appliedNormalImpulse);
public:
btSequentialImpulseConstraintSolver();
///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody
///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
void setContactSolverFunc(ContactSolverFunc func,int type0,int type1)
{
m_contactDispatch[type0][type1] = func;
}
///Advanced: Override the default friction solving function for contacts, for certain types of rigidbody
///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
void SetFrictionSolverFunc(ContactSolverFunc func,int type0,int type1)
{
m_frictionDispatch[type0][type1] = func;
}
virtual ~btSequentialImpulseConstraintSolver();
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
///clear internal cached data and reset random seed
virtual void reset();
btScalar solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
unsigned long btRand2();

2
src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
View File

@@ -119,8 +119,6 @@ void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, co
{
normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
new (&m_jacLin[i]) btJacobianEntry(
rbA.getCenterOfMassTransform().getBasis().transpose(),
rbB.getCenterOfMassTransform().getBasis().transpose(),
m_relPosA,
m_relPosB,
normalWorld,

9
src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp
View File

@@ -61,9 +61,10 @@ void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
//this jacobian entry could be re-used for all iterations
btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
btJacobianEntry jacA(
rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB);
btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
btJacobianEntry jacB(rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB);
//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
@@ -150,9 +151,9 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint(
//this jacobian entry could be re-used for all iterations
btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
btJacobianEntry jacA(rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB);
btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
btJacobianEntry jacB(rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB);
//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);

106
src/BulletDynamics/ConstraintSolver/btSolverBody.h
View File

@@ -23,6 +23,7 @@ class btRigidBody;
#include "LinearMath/btAlignedAllocator.h"
#include "LinearMath/btTransformUtil.h"
#ifndef _USE_JACOBIAN
///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
ATTRIBUTE_ALIGNED16 (struct) btSolverBody
@@ -33,6 +34,10 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
btVector3 m_angularVelocity;
btVector3 m_linearVelocity;
btVector3 m_deltaLinearVelocity;
btVector3 m_deltaAngularVelocity;
btVector3 m_centerOfMassPosition;
btVector3 m_pushVelocity;
btVector3 m_turnVelocity;
@@ -49,10 +54,12 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
}
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
{
if (m_invMass)
//if (m_invMass)
{
m_deltaLinearVelocity += linearComponent*impulseMagnitude;
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
m_linearVelocity += linearComponent*impulseMagnitude;
m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
}
@@ -60,7 +67,7 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
{
if (m_invMass)
//if (m_invMass)
{
m_pushVelocity += linearComponent*impulseMagnitude;
m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
@@ -109,7 +116,100 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
};
#else
///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_angularVelocity1;
btVector3 m_linearVelocity1;
btVector3 m_deltaAngularVelocity;
btVector3 m_deltaLinearVelocity;
btVector3 m_centerOfMassPosition;
btVector3 m_pushVelocity;
btVector3 m_turnVelocity;
float m_angularFactor;
float m_invMass;
float m_friction;
btRigidBody* m_originalBody;
SIMD_FORCE_INLINE void getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
{
velocity = (m_linearVelocity1 + m_deltaLinearVelocity) + (m_angularVelocity1+m_deltaAngularVelocity).cross(rel_pos);
}
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
{
if (m_invMass)
{
m_deltaLinearVelocity += linearComponent*impulseMagnitude;
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
}
}
SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
{
if (m_invMass)
{
m_pushVelocity += linearComponent*impulseMagnitude;
m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
}
}
void writebackVelocity()
{
if (m_invMass)
{
m_originalBody->setLinearVelocity(m_linearVelocity1 + m_deltaLinearVelocity);
m_originalBody->setAngularVelocity(m_angularVelocity1+m_deltaAngularVelocity);
//m_originalBody->setCompanionId(-1);
}
}
void writebackVelocity(btScalar timeStep)
{
if (m_invMass)
{
m_originalBody->setLinearVelocity(m_linearVelocity1 + m_deltaLinearVelocity);
m_originalBody->setAngularVelocity(m_angularVelocity1+m_deltaAngularVelocity);
//correct the position/orientation based on push/turn recovery
btTransform newTransform;
btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
m_originalBody->setWorldTransform(newTransform);
//m_originalBody->setCompanionId(-1);
}
}
void readVelocity()
{
if (m_invMass)
{
m_linearVelocity1 = m_originalBody->getLinearVelocity();
m_angularVelocity1 = m_originalBody->getAngularVelocity();
}
}
};
#endif //USE_JAC
#endif //BT_SOLVER_BODY_H

16
src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
View File

@@ -19,6 +19,7 @@ subject to the following restrictions:
class btRigidBody;
#include "LinearMath/btVector3.h"
#include "LinearMath/btMatrix3x3.h"
#include "btJacobianEntry.h"
//#define NO_FRICTION_TANGENTIALS 1
@@ -27,6 +28,11 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
{
BT_DECLARE_ALIGNED_ALLOCATOR();
#ifdef _USE_JACOBIAN
btJacobianEntry m_jac;
#endif
btVector3 m_relpos1CrossNormal;
btVector3 m_contactNormal;
@@ -34,10 +40,10 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
btVector3 m_angularComponentA;
btVector3 m_angularComponentB;
mutable btScalar m_appliedPushImpulse;
mutable btScalar m_appliedPushImpulse;
mutable btScalar m_appliedImpulse;
int m_solverBodyIdA;
int m_solverBodyIdB;
@@ -51,8 +57,10 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
int m_constraintType;
int m_frictionIndex;
void* m_originalContactPoint;
int m_unusedPadding[1];
btScalar m_rhs;
btScalar m_cfm;
btScalar m_lowerLimit;
btScalar m_upperLimit;
enum btSolverConstraintType
{