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
Demos/AllBulletDemos/DemoEntries.cpp
View File

@@ -143,7 +143,7 @@ btDemoEntry g_demoEntries[] =
{"SoftBody Cluster Stack Mixed",SoftDemo27::Create}, {"SoftBody Cluster Stack Mixed",SoftDemo27::Create},
// {"SliderConstraint",SliderConstraintDemo::Create}, // {"SliderConstraint",SliderConstraintDemo::Create},
// {"CcdPhysicsDemo", CcdPhysicsDemo::Create}, {"CcdPhysicsDemo", CcdPhysicsDemo::Create},
// {"ConcaveRaycastDemo",ConcaveRaycastDemo::Create}, // {"ConcaveRaycastDemo",ConcaveRaycastDemo::Create},
//{"BspDemo", BspDemo::Create}, //{"BspDemo", BspDemo::Create},
// {"Raytracer Test",Raytracer::Create}, // {"Raytracer Test",Raytracer::Create},

6
Demos/AllBulletDemos/Main.cpp
View File

@@ -71,7 +71,7 @@ void setDefaultSettings()
{ {
viewX = 0.0f; viewX = 0.0f;
viewY = 0.0f; viewY = 0.0f;
framePeriod = 1;//16;//16;//todo: test if this value should be 0 framePeriod = 6;//16;//16;//todo: test if this value should be 0
width = 800;//640; width = 800;//640;
height = 600;//1024;//480; height = 600;//1024;//480;
@@ -87,8 +87,8 @@ void setDefaultSettings()
gUseSplitImpulse = 0; gUseSplitImpulse = 0;
gUseWarmstarting = 1; gUseWarmstarting = 1;
gRandomizeConstraints = 1; gRandomizeConstraints = 1;
gErp = 0.2f; gErp = 0.4f;
gSlop=0.01f; gSlop=0.0f;
gErp2 = 0.1f; gErp2 = 0.1f;
gWarmStartingParameter = 0.85f; gWarmStartingParameter = 0.85f;

1
Demos/BasicDemo/BasicDemo.cpp
View File

@@ -13,6 +13,7 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
///create 125 (5x5x5) dynamic object ///create 125 (5x5x5) dynamic object
#define ARRAY_SIZE_X 5 #define ARRAY_SIZE_X 5
#define ARRAY_SIZE_Y 5 #define ARRAY_SIZE_Y 5

6
Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp
View File

@@ -192,7 +192,7 @@ extern btScalar gJitterVelocityDampingFactor;
extern int gNumManifold; extern int gNumManifold;
extern int gOverlappingPairs; extern int gOverlappingPairs;
extern int gTotalContactPoints;
void CcdPhysicsDemo::clientMoveAndDisplay() void CcdPhysicsDemo::clientMoveAndDisplay()
{ {
@@ -282,10 +282,10 @@ void CcdPhysicsDemo::clientMoveAndDisplay()
#ifdef PRINT_CONTACT_STATISTICS #ifdef PRINT_CONTACT_STATISTICS
printf("num manifolds: %i\n",gNumManifold); printf("num manifolds: %i\n",gNumManifold);
printf("num gOverlappingPairs: %i\n",gOverlappingPairs); printf("num gOverlappingPairs: %i\n",gOverlappingPairs);
printf("num gTotalContactPoints : %i\n",gTotalContactPoints );
#endif //PRINT_CONTACT_STATISTICS #endif //PRINT_CONTACT_STATISTICS
gTotalContactPoints = 0;
glutSwapBuffers(); glutSwapBuffers();
} }

4
Demos/MultiThreadedDemo/MultiThreadedDemo.cpp
View File

@@ -133,7 +133,7 @@ extern btScalar gJitterVelocityDampingFactor;
extern int gNumManifold; extern int gNumManifold;
extern int gOverlappingPairs; extern int gOverlappingPairs;
extern int gTotalContactPoints;
void MultiThreadedDemo::clientMoveAndDisplay() void MultiThreadedDemo::clientMoveAndDisplay()
{ {
@@ -201,7 +201,7 @@ void MultiThreadedDemo::clientMoveAndDisplay()
#endif #endif
glFlush(); glFlush();
gTotalContactPoints = 0;
glutSwapBuffers(); glutSwapBuffers();
} }

6
Demos/SoftDemo/SoftDemo.cpp
View File

@@ -97,7 +97,7 @@ void SoftDemo::createStack( btCollisionShape* boxShape, float halfCubeSize, int
extern int gNumManifold; extern int gNumManifold;
extern int gOverlappingPairs; extern int gOverlappingPairs;
extern int gTotalContactPoints;
void SoftDemo::clientMoveAndDisplay() void SoftDemo::clientMoveAndDisplay()
{ {
@@ -200,10 +200,10 @@ void SoftDemo::clientMoveAndDisplay()
#ifdef PRINT_CONTACT_STATISTICS #ifdef PRINT_CONTACT_STATISTICS
printf("num manifolds: %i\n",gNumManifold); printf("num manifolds: %i\n",gNumManifold);
printf("num gOverlappingPairs: %i\n",gOverlappingPairs); printf("num gOverlappingPairs: %i\n",gOverlappingPairs);
printf("num gTotalContactPoints : %i\n",gTotalContactPoints );
#endif //PRINT_CONTACT_STATISTICS #endif //PRINT_CONTACT_STATISTICS
gTotalContactPoints = 0;
glutSwapBuffers(); glutSwapBuffers();
} }

94
Extras/quickstep/btOdeContactJoint.cpp
View File

@@ -35,7 +35,7 @@ int m_numRows = 3;
void btOdeContactJoint::GetInfo1(Info1 *info) void btOdeContactJoint::GetInfo1(Info1 *info)
{ {
info->m = m_numRows; info->m_numConstraintRows = m_numRows;
//friction adds another 2... //friction adds another 2...
info->nub = 0; info->nub = 0;
@@ -107,16 +107,12 @@ void btOdeContactJoint::GetInfo2(Info2 *info)
btVector3 relativePositionA; btVector3 relativePositionA;
{ {
relativePositionA = point.getPositionWorldOnA() - m_body0->m_centerOfMassPosition; relativePositionA = point.getPositionWorldOnA() - m_body0->m_centerOfMassPosition;
dVector3 c1;
c1[0] = relativePositionA.x();
c1[1] = relativePositionA.y();
c1[2] = relativePositionA.z();
// set jacobian for normal // set jacobian for normal
info->J1l[0] = normal[0]; info->m_J1linearAxis[0] = normal[0];
info->J1l[1] = normal[1]; info->m_J1linearAxis[1] = normal[1];
info->J1l[2] = normal[2]; info->m_J1linearAxis[2] = normal[2];
dCROSS (info->J1a,=,c1,normal); dCROSS (info->m_J1angularAxis,=,relativePositionA,normal);
} }
@@ -126,30 +122,24 @@ void btOdeContactJoint::GetInfo2(Info2 *info)
// if (GetBody1()) // if (GetBody1())
{ {
dVector3 c2;
btVector3 posBody1 = m_body1 ? m_body1->m_centerOfMassPosition : btVector3(0,0,0); btVector3 posBody1 = m_body1 ? m_body1->m_centerOfMassPosition : btVector3(0,0,0);
relativePositionB = point.getPositionWorldOnB() - posBody1; relativePositionB = point.getPositionWorldOnB() - posBody1;
// for (i=0; i<3; i++) c2[i] = j->contact.geom.pos[i] - // for (i=0; i<3; i++) c2[i] = j->contact.geom.pos[i] -
// j->node[1].body->pos[i]; // j->node[1].body->pos[i];
c2[0] = relativePositionB.x();
c2[1] = relativePositionB.y();
c2[2] = relativePositionB.z();
info->J2l[0] = -normal[0]; info->m_J2linearAxis[0] = -normal[0];
info->J2l[1] = -normal[1]; info->m_J2linearAxis[1] = -normal[1];
info->J2l[2] = -normal[2]; info->m_J2linearAxis[2] = -normal[2];
dCROSS (info->J2a,= -,c2,normal); dCROSS (info->m_J2angularAxis,= -,relativePositionB,normal);
} }
} }
btScalar k = info->fps * info->erp;
float depth = -point.getDistance(); float depth = -point.getDistance();
// if (depth < 0.f) // if (depth < 0.f)
// depth = 0.f; // depth = 0.f;
info->c[0] = k * depth; info->m_constraintError[0] = depth * info->fps * info->erp ;
//float maxvel = .2f; //float maxvel = .2f;
// if (info->c[0] > maxvel) // if (info->c[0] > maxvel)
@@ -164,12 +154,12 @@ void btOdeContactJoint::GetInfo2(Info2 *info)
// set LCP limits for normal // set LCP limits for normal
info->lo[0] = 0; info->m_lowerLimit[0] = 0;
info->hi[0] = 1e30f;//dInfinity; info->m_higherLimit[0] = 1e30f;//dInfinity;
info->lo[1] = 0; info->m_lowerLimit[1] = 0;
info->hi[1] = 0.f; info->m_higherLimit[1] = 0.f;
info->lo[2] = 0.f; info->m_lowerLimit[2] = 0.f;
info->hi[2] = 0.f; info->m_higherLimit[2] = 0.f;
#define DO_THE_FRICTION_2 #define DO_THE_FRICTION_2
#ifdef DO_THE_FRICTION_2 #ifdef DO_THE_FRICTION_2
@@ -197,15 +187,15 @@ void btOdeContactJoint::GetInfo2(Info2 *info)
dPlaneSpace1 (normal,t1,t2); dPlaneSpace1 (normal,t1,t2);
info->J1l[s+0] = t1[0]; info->m_J1linearAxis[s+0] = t1[0];
info->J1l[s+1] = t1[1]; info->m_J1linearAxis[s+1] = t1[1];
info->J1l[s+2] = t1[2]; info->m_J1linearAxis[s+2] = t1[2];
dCROSS (info->J1a+s,=,c1,t1); dCROSS (info->m_J1angularAxis+s,=,c1,t1);
// if (1) { //j->node[1].body) { // if (1) { //j->node[1].body) {
info->J2l[s+0] = -t1[0]; info->m_J2linearAxis[s+0] = -t1[0];
info->J2l[s+1] = -t1[1]; info->m_J2linearAxis[s+1] = -t1[1];
info->J2l[s+2] = -t1[2]; info->m_J2linearAxis[s+2] = -t1[2];
dCROSS (info->J2a+s,= -,c2,t1); dCROSS (info->m_J2angularAxis+s,= -,c2,t1);
// } // }
// set right hand side // set right hand side
// if (0) {//j->contact.surface.mode & dContactMotion1) { // if (0) {//j->contact.surface.mode & dContactMotion1) {
@@ -216,8 +206,8 @@ void btOdeContactJoint::GetInfo2(Info2 *info)
//1e30f //1e30f
info->lo[1] = -friction;//-j->contact.surface.mu; info->m_lowerLimit[1] = -friction;//-j->contact.surface.mu;
info->hi[1] = friction;//j->contact.surface.mu; info->m_higherLimit[1] = friction;//j->contact.surface.mu;
// if (1)//j->contact.surface.mode & dContactApprox1_1) // if (1)//j->contact.surface.mode & dContactApprox1_1)
info->findex[1] = 0; info->findex[1] = 0;
@@ -233,15 +223,15 @@ void btOdeContactJoint::GetInfo2(Info2 *info)
// second friction direction // second friction direction
if (m_numRows >= 3) { if (m_numRows >= 3) {
info->J1l[s2+0] = t2[0]; info->m_J1linearAxis[s2+0] = t2[0];
info->J1l[s2+1] = t2[1]; info->m_J1linearAxis[s2+1] = t2[1];
info->J1l[s2+2] = t2[2]; info->m_J1linearAxis[s2+2] = t2[2];
dCROSS (info->J1a+s2,=,c1,t2); dCROSS (info->m_J1angularAxis+s2,=,c1,t2);
// if (1) { //j->node[1].body) { // if (1) { //j->node[1].body) {
info->J2l[s2+0] = -t2[0]; info->m_J2linearAxis[s2+0] = -t2[0];
info->J2l[s2+1] = -t2[1]; info->m_J2linearAxis[s2+1] = -t2[1];
info->J2l[s2+2] = -t2[2]; info->m_J2linearAxis[s2+2] = -t2[2];
dCROSS (info->J2a+s2,= -,c2,t2); dCROSS (info->m_J2angularAxis+s2,= -,c2,t2);
// } // }
// set right hand side // set right hand side
@@ -251,18 +241,18 @@ void btOdeContactJoint::GetInfo2(Info2 *info)
// set LCP bounds and friction index. this depends on the approximation // set LCP bounds and friction index. this depends on the approximation
// mode // mode
// if (0){//j->contact.surface.mode & dContactMu2) { // if (0){//j->contact.surface.mode & dContactMu2) {
//info->lo[2] = -j->contact.surface.mu2; //info->m_lowerLimit[2] = -j->contact.surface.mu2;
//info->hi[2] = j->contact.surface.mu2; //info->m_higherLimit[2] = j->contact.surface.mu2;
// } // }
// else { // else {
info->lo[2] = -friction; info->m_lowerLimit[2] = -friction;
info->hi[2] = friction; info->m_higherLimit[2] = friction;
// } // }
// if (0)//j->contact.surface.mode & dContactApprox1_2) if (1)//j->contact.surface.mode & dContactApprox1_2)
// { {
// info->findex[2] = 0; info->findex[2] = 0;
// } }
// set slip (constraint force mixing) // set slip (constraint force mixing)
// if (0) //j->contact.surface.mode & dContactSlip2) // if (0) //j->contact.surface.mode & dContactSlip2)

8
Extras/quickstep/btOdeJoint.h
View File

@@ -44,7 +44,7 @@ public:
struct Info1 { struct Info1 {
int m,nub; int m_numConstraintRows,nub;
}; };
// info returned by getInfo2 function // info returned by getInfo2 function
@@ -58,7 +58,7 @@ public:
// n*3 jacobian sub matrices, stored by rows. these matrices will have // n*3 jacobian sub matrices, stored by rows. these matrices will have
// been initialized to 0 on entry. if the second body is zero then the // been initialized to 0 on entry. if the second body is zero then the
// J2xx pointers may be 0. // J2xx pointers may be 0.
btScalar *J1l,*J1a,*J2l,*J2a; btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis;
// elements to jump from one row to the next in J's // elements to jump from one row to the next in J's
int rowskip; int rowskip;
@@ -66,10 +66,10 @@ public:
// right hand sides of the equation J*v = c + cfm * lambda. cfm is the // right hand sides of the equation J*v = c + cfm * lambda. cfm is the
// "constraint force mixing" vector. c is set to zero on entry, cfm is // "constraint force mixing" vector. c is set to zero on entry, cfm is
// set to a constant value (typically very small or zero) value on entry. // set to a constant value (typically very small or zero) value on entry.
btScalar *c,*cfm; btScalar *m_constraintError,*cfm;
// lo and hi limits for variables (set to -/+ infinity on entry). // lo and hi limits for variables (set to -/+ infinity on entry).
btScalar *lo,*hi; btScalar *m_lowerLimit,*m_higherLimit;
// findex vector for variables. see the LCP solver interface for a // findex vector for variables. see the LCP solver interface for a
// description of what this does. this is set to -1 on entry. // description of what this does. this is set to -1 on entry.

9
Extras/quickstep/btOdeQuickstepConstraintSolver.cpp
View File

@@ -58,9 +58,7 @@ class btOdeJoint;
//to bridge with ODE quickstep, we make a temp copy of the rigidbodies in each simultion island //to bridge with ODE quickstep, we make a temp copy of the rigidbodies in each simultion island
btOdeQuickstepConstraintSolver::btOdeQuickstepConstraintSolver(): btOdeQuickstepConstraintSolver::btOdeQuickstepConstraintSolver()
m_cfm(0.f),//1e-5f),
m_erp(0.4f)
{ {
} }
@@ -124,7 +122,7 @@ btScalar btOdeQuickstepConstraintSolver::solveGroup(btCollisionObject** /*bodies
// printf(" numJoints > numManifolds * 4 + numConstraints"); // printf(" numJoints > numManifolds * 4 + numConstraints");
m_SorLcpSolver.SolveInternal1(m_cfm,m_erp,m_odeBodies,numBodies,m_joints,numJoints,infoGlobal,stackAlloc); ///do m_SorLcpSolver.SolveInternal1(m_odeBodies,numBodies,m_joints,numJoints,infoGlobal,stackAlloc); ///do
//write back resulting velocities //write back resulting velocities
for (int i=0;i<numBodies;i++) for (int i=0;i<numBodies;i++)
@@ -264,8 +262,9 @@ void btOdeQuickstepConstraintSolver::ConvertConstraint(btPersistentManifold* man
{ {
manifold->refreshContactPoints(((btRigidBody*)manifold->getBody0())->getCenterOfMassTransform(), /* manifold->refreshContactPoints(((btRigidBody*)manifold->getBody0())->getCenterOfMassTransform(),
((btRigidBody*)manifold->getBody1())->getCenterOfMassTransform()); ((btRigidBody*)manifold->getBody1())->getCenterOfMassTransform());
*/
int bodyId0 = _bodyId0,bodyId1 = _bodyId1; int bodyId0 = _bodyId0,bodyId1 = _bodyId1;

17
Extras/quickstep/btOdeQuickstepConstraintSolver.h
View File

@@ -40,9 +40,6 @@ private:
int m_CurJoint; int m_CurJoint;
int m_CurTypedJoint; int m_CurTypedJoint;
float m_cfm;
float m_erp;
btSorLcpSolver m_SorLcpSolver; btSorLcpSolver m_SorLcpSolver;
btAlignedObjectArray<btOdeSolverBody*> m_odeBodies; btAlignedObjectArray<btOdeSolverBody*> m_odeBodies;
@@ -74,19 +71,7 @@ public:
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 solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher);
///setConstraintForceMixing, the cfm adds some positive value to the main diagonal
///This can improve convergence (make matrix positive semidefinite), but it can make the simulation look more 'springy'
void setConstraintForceMixing(float cfm) {
m_cfm = cfm;
}
///setErrorReductionParamter sets the maximum amount of error reduction
///which limits energy addition during penetration depth recovery
void setErrorReductionParamter(float erp)
{
m_erp = erp;
}
///clear internal cached data and reset random seed ///clear internal cached data and reset random seed
void reset() void reset()
{ {

236
Extras/quickstep/btOdeTypedJoint.cpp
View File

@@ -80,7 +80,7 @@ OdeP2PJoint::OdeP2PJoint(
void OdeP2PJoint::GetInfo1(Info1 *info) void OdeP2PJoint::GetInfo1(Info1 *info)
{ {
info->m = 3; info->m_numConstraintRows = 3;
info->nub = 3; info->nub = 3;
} }
@@ -133,24 +133,24 @@ void OdeP2PJoint::GetInfo2(Info2 *info)
int s = info->rowskip; int s = info->rowskip;
// set jacobian // set jacobian
info->J1l[0] = 1; info->m_J1linearAxis[0] = 1;
info->J1l[s+1] = 1; info->m_J1linearAxis[s+1] = 1;
info->J1l[2*s+2] = 1; info->m_J1linearAxis[2*s+2] = 1;
btVector3 a1,a2; btVector3 a1,a2;
a1 = body0_trans.getBasis()*p2pconstraint->getPivotInA(); a1 = body0_trans.getBasis()*p2pconstraint->getPivotInA();
//dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA); //dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA);
dCROSSMAT (info->J1a,a1,s,-,+); dCROSSMAT (info->m_J1angularAxis,a1,s,-,+);
if (m_body1) if (m_body1)
{ {
info->J2l[0] = -1; info->m_J2linearAxis[0] = -1;
info->J2l[s+1] = -1; info->m_J2linearAxis[s+1] = -1;
info->J2l[2*s+2] = -1; info->m_J2linearAxis[2*s+2] = -1;
a2 = body1_trans.getBasis()*p2pconstraint->getPivotInB(); a2 = body1_trans.getBasis()*p2pconstraint->getPivotInB();
//dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB); //dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB);
dCROSSMAT (info->J2a,a2,s,+,-); dCROSSMAT (info->m_J2angularAxis,a2,s,+,-);
} }
@@ -160,7 +160,7 @@ void OdeP2PJoint::GetInfo2(Info2 *info)
{ {
for (int j=0; j<3; j++) for (int j=0; j<3; j++)
{ {
info->c[j] = k * (a2[j] + body1_trans.getOrigin()[j] - info->m_constraintError[j] = k * (a2[j] + body1_trans.getOrigin()[j] -
a1[j] - body0_trans.getOrigin()[j]); a1[j] - body0_trans.getOrigin()[j]);
} }
} }
@@ -168,7 +168,7 @@ void OdeP2PJoint::GetInfo2(Info2 *info)
{ {
for (int j=0; j<3; j++) for (int j=0; j<3; j++)
{ {
info->c[j] = k * (p2pconstraint->getPivotInB()[j] - a1[j] - info->m_constraintError[j] = k * (p2pconstraint->getPivotInB()[j] - a1[j] -
body0_trans.getOrigin()[j]); body0_trans.getOrigin()[j]);
} }
} }
@@ -193,8 +193,8 @@ int bt_get_limit_motor_info2(
if (powered || limit) if (powered || limit)
{ {
btScalar *J1 = rotational ? info->J1a : info->J1l; btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
btScalar *J2 = rotational ? info->J2a : info->J2l; btScalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
J1[srow+0] = ax1[0]; J1[srow+0] = ax1[0];
J1[srow+1] = ax1[1]; J1[srow+1] = ax1[1];
@@ -232,12 +232,12 @@ int bt_get_limit_motor_info2(
ltd = c.cross(ax1); ltd = c.cross(ax1);
info->J1a[srow+0] = ltd[0]; info->m_J1angularAxis[srow+0] = ltd[0];
info->J1a[srow+1] = ltd[1]; info->m_J1angularAxis[srow+1] = ltd[1];
info->J1a[srow+2] = ltd[2]; info->m_J1angularAxis[srow+2] = ltd[2];
info->J2a[srow+0] = ltd[0]; info->m_J2angularAxis[srow+0] = ltd[0];
info->J2a[srow+1] = ltd[1]; info->m_J2angularAxis[srow+1] = ltd[1];
info->J2a[srow+2] = ltd[2]; info->m_J2angularAxis[srow+2] = ltd[2];
} }
// if we're limited low and high simultaneously, the joint motor is // if we're limited low and high simultaneously, the joint motor is
@@ -250,37 +250,37 @@ int bt_get_limit_motor_info2(
info->cfm[row] = 0.0f;//limot->m_normalCFM; info->cfm[row] = 0.0f;//limot->m_normalCFM;
if (! limit) if (! limit)
{ {
info->c[row] = limot->m_targetVelocity; info->m_constraintError[row] = limot->m_targetVelocity;
info->lo[row] = -limot->m_maxMotorForce; info->m_lowerLimit[row] = -limot->m_maxMotorForce;
info->hi[row] = limot->m_maxMotorForce; info->m_higherLimit[row] = limot->m_maxMotorForce;
} }
} }
if (limit) if (limit)
{ {
btScalar k = info->fps * limot->m_ERP; btScalar k = info->fps * limot->m_ERP;
info->c[row] = -k * limot->m_currentLimitError; info->m_constraintError[row] = -k * limot->m_currentLimitError;
info->cfm[row] = 0.0f;//limot->m_stopCFM; info->cfm[row] = 0.0f;//limot->m_stopCFM;
if (limot->m_loLimit == limot->m_hiLimit) if (limot->m_loLimit == limot->m_hiLimit)
{ {
// limited low and high simultaneously // limited low and high simultaneously
info->lo[row] = -dInfinity; info->m_lowerLimit[row] = -dInfinity;
info->hi[row] = dInfinity; info->m_higherLimit[row] = dInfinity;
} }
else else
{ {
if (limit == 1) if (limit == 1)
{ {
// low limit // low limit
info->lo[row] = 0; info->m_lowerLimit[row] = 0;
info->hi[row] = SIMD_INFINITY; info->m_higherLimit[row] = SIMD_INFINITY;
} }
else else
{ {
// high limit // high limit
info->lo[row] = -SIMD_INFINITY; info->m_lowerLimit[row] = -SIMD_INFINITY;
info->hi[row] = 0; info->m_higherLimit[row] = 0;
} }
// deal with bounce // deal with bounce
@@ -309,7 +309,8 @@ int bt_get_limit_motor_info2(
if (vel < 0) if (vel < 0)
{ {
btScalar newc = -limot->m_bounce* vel; btScalar newc = -limot->m_bounce* vel;
if (newc > info->c[row]) info->c[row] = newc; if (newc > info->m_constraintError[row])
info->m_constraintError[row] = newc;
} }
} }
else else
@@ -318,7 +319,8 @@ int bt_get_limit_motor_info2(
if (vel > 0) if (vel > 0)
{ {
btScalar newc = -limot->m_bounce * vel; btScalar newc = -limot->m_bounce * vel;
if (newc < info->c[row]) info->c[row] = newc; if (newc < info->m_constraintError[row])
info->m_constraintError[row] = newc;
} }
} }
} }
@@ -349,7 +351,7 @@ void OdeD6Joint::GetInfo1(Info1 *info)
btGeneric6DofConstraint * d6constraint = this->getD6Constraint(); btGeneric6DofConstraint * d6constraint = this->getD6Constraint();
//prepare constraint //prepare constraint
d6constraint->calculateTransforms(); d6constraint->calculateTransforms();
info->m = 3; info->m_numConstraintRows = 3;
info->nub = 3; info->nub = 3;
//test angular limits //test angular limits
@@ -358,7 +360,7 @@ void OdeD6Joint::GetInfo1(Info1 *info)
//if(i==2) continue; //if(i==2) continue;
if(d6constraint->testAngularLimitMotor(i)) if(d6constraint->testAngularLimitMotor(i))
{ {
info->m++; info->m_numConstraintRows++;
} }
} }
@@ -390,25 +392,25 @@ int OdeD6Joint::setLinearLimits(Info2 *info)
int s = info->rowskip; int s = info->rowskip;
// set jacobian // set jacobian
info->J1l[0] = 1; info->m_J1linearAxis[0] = 1;
info->J1l[s+1] = 1; info->m_J1linearAxis[s+1] = 1;
info->J1l[2*s+2] = 1; info->m_J1linearAxis[2*s+2] = 1;
btVector3 a1,a2; btVector3 a1,a2;
a1 = body0_trans.getBasis()*d6constraint->getFrameOffsetA().getOrigin(); a1 = body0_trans.getBasis()*d6constraint->getFrameOffsetA().getOrigin();
//dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA); //dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA);
dCROSSMAT (info->J1a,a1,s,-,+); dCROSSMAT (info->m_J1angularAxis,a1,s,-,+);
if (m_body1) if (m_body1)
{ {
info->J2l[0] = -1; info->m_J2linearAxis[0] = -1;
info->J2l[s+1] = -1; info->m_J2linearAxis[s+1] = -1;
info->J2l[2*s+2] = -1; info->m_J2linearAxis[2*s+2] = -1;
a2 = body1_trans.getBasis()*d6constraint->getFrameOffsetB().getOrigin(); a2 = body1_trans.getBasis()*d6constraint->getFrameOffsetB().getOrigin();
//dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB); //dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB);
dCROSSMAT (info->J2a,a2,s,+,-); dCROSSMAT (info->m_J2angularAxis,a2,s,+,-);
} }
@@ -418,7 +420,7 @@ int OdeD6Joint::setLinearLimits(Info2 *info)
{ {
for (int j=0; j<3; j++) for (int j=0; j<3; j++)
{ {
info->c[j] = k * (a2[j] + body1_trans.getOrigin()[j] - info->m_constraintError[j] = k * (a2[j] + body1_trans.getOrigin()[j] -
a1[j] - body0_trans.getOrigin()[j]); a1[j] - body0_trans.getOrigin()[j]);
} }
} }
@@ -426,7 +428,7 @@ int OdeD6Joint::setLinearLimits(Info2 *info)
{ {
for (int j=0; j<3; j++) for (int j=0; j<3; j++)
{ {
info->c[j] = k * (d6constraint->getCalculatedTransformB().getOrigin()[j] - a1[j] - info->m_constraintError[j] = k * (d6constraint->getCalculatedTransformB().getOrigin()[j] - a1[j] -
body0_trans.getOrigin()[j]); body0_trans.getOrigin()[j]);
} }
} }
@@ -485,19 +487,19 @@ OdeSliderJoint::OdeSliderJoint(
void OdeSliderJoint::GetInfo1(Info1* info) void OdeSliderJoint::GetInfo1(Info1* info)
{ {
info->nub = 4; info->nub = 4;
info->m = 4; // Fixed 2 linear + 2 angular info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
btSliderConstraint * slider = this->getSliderConstraint(); btSliderConstraint * slider = this->getSliderConstraint();
//prepare constraint //prepare constraint
slider->calculateTransforms(); slider->calculateTransforms();
slider->testLinLimits(); slider->testLinLimits();
if(slider->getSolveLinLimit() || slider->getPoweredLinMotor()) if(slider->getSolveLinLimit() || slider->getPoweredLinMotor())
{ {
info->m++; // limit 3rd linear as well info->m_numConstraintRows++; // limit 3rd linear as well
} }
slider->testAngLimits(); slider->testAngLimits();
if(slider->getSolveAngLimit() || slider->getPoweredAngMotor()) if(slider->getSolveAngLimit() || slider->getPoweredAngMotor())
{ {
info->m++; // limit 3rd angular as well info->m_numConstraintRows++; // limit 3rd angular as well
} }
} // OdeSliderJoint::GetInfo1() } // OdeSliderJoint::GetInfo1()
@@ -523,20 +525,20 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
btVector3 p = trA.getBasis().getColumn(1); btVector3 p = trA.getBasis().getColumn(1);
btVector3 q = trA.getBasis().getColumn(2); btVector3 q = trA.getBasis().getColumn(2);
// set the two slider rows // set the two slider rows
info->J1a[0] = p[0]; info->m_J1angularAxis[0] = p[0];
info->J1a[1] = p[1]; info->m_J1angularAxis[1] = p[1];
info->J1a[2] = p[2]; info->m_J1angularAxis[2] = p[2];
info->J1a[s+0] = q[0]; info->m_J1angularAxis[s+0] = q[0];
info->J1a[s+1] = q[1]; info->m_J1angularAxis[s+1] = q[1];
info->J1a[s+2] = q[2]; info->m_J1angularAxis[s+2] = q[2];
if(m_body1) if(m_body1)
{ {
info->J2a[0] = -p[0]; info->m_J2angularAxis[0] = -p[0];
info->J2a[1] = -p[1]; info->m_J2angularAxis[1] = -p[1];
info->J2a[2] = -p[2]; info->m_J2angularAxis[2] = -p[2];
info->J2a[s+0] = -q[0]; info->m_J2angularAxis[s+0] = -q[0];
info->J2a[s+1] = -q[1]; info->m_J2angularAxis[s+1] = -q[1];
info->J2a[s+2] = -q[2]; info->m_J2angularAxis[s+2] = -q[2];
} }
// compute the right hand side of the constraint equation. set relative // compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the slider back into alignment. // body velocities along p and q to bring the slider back into alignment.
@@ -564,8 +566,8 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
{ {
u = ax2.cross(ax1); u = ax2.cross(ax1);
} }
info->c[0] = k * u.dot(p); info->m_constraintError[0] = k * u.dot(p);
info->c[1] = k * u.dot(q); info->m_constraintError[1] = k * u.dot(q);
// pull out pos and R for both bodies. also get the connection // pull out pos and R for both bodies. also get the connection
// vector c = pos2-pos1. // vector c = pos2-pos1.
// next two rows. we want: vel2 = vel1 + w1 x c ... but this would // next two rows. we want: vel2 = vel1 + w1 x c ... but this would
@@ -585,19 +587,19 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin(); c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
btVector3 tmp = btScalar(0.5) * c.cross(p); btVector3 tmp = btScalar(0.5) * c.cross(p);
for (i=0; i<3; i++) info->J1a[s2+i] = tmp[i]; for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmp[i];
for (i=0; i<3; i++) info->J2a[s2+i] = tmp[i]; for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = tmp[i];
tmp = btScalar(0.5) * c.cross(q); tmp = btScalar(0.5) * c.cross(q);
for (i=0; i<3; i++) info->J1a[s3+i] = tmp[i]; for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = tmp[i];
for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i]; for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = tmp[i];
for (i=0; i<3; i++) info->J2l[s2+i] = -p[i]; for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i];
for (i=0; i<3; i++) info->J2l[s3+i] = -q[i]; for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i];
} }
for (i=0; i<3; i++) info->J1l[s2+i] = p[i]; for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
for (i=0; i<3; i++) info->J1l[s3+i] = q[i]; for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
// compute two elements of right hand side. we want to align the offset // compute two elements of right hand side. we want to align the offset
// point (in body 2's frame) with the center of body 1. // point (in body 2's frame) with the center of body 1.
btVector3 ofs; // offset point in global coordinates btVector3 ofs; // offset point in global coordinates
@@ -610,8 +612,8 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
ofs = trA.getOrigin() - trB.getOrigin(); ofs = trA.getOrigin() - trB.getOrigin();
} }
k = info->fps * info->erp * slider->getSoftnessOrthoLin(); k = info->fps * info->erp * slider->getSoftnessOrthoLin();
info->c[2] = k * p.dot(ofs); info->m_constraintError[2] = k * p.dot(ofs);
info->c[3] = k * q.dot(ofs); info->m_constraintError[3] = k * q.dot(ofs);
int nrow = 3; // last filled row int nrow = 3; // last filled row
int srow; int srow;
// check linear limits linear // check linear limits linear
@@ -639,14 +641,14 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
{ {
nrow++; nrow++;
srow = nrow * info->rowskip; srow = nrow * info->rowskip;
info->J1l[srow+0] = ax1[0]; info->m_J1linearAxis[srow+0] = ax1[0];
info->J1l[srow+1] = ax1[1]; info->m_J1linearAxis[srow+1] = ax1[1];
info->J1l[srow+2] = ax1[2]; info->m_J1linearAxis[srow+2] = ax1[2];
if(m_body1) if(m_body1)
{ {
info->J2l[srow+0] = -ax1[0]; info->m_J2linearAxis[srow+0] = -ax1[0];
info->J2l[srow+1] = -ax1[1]; info->m_J2linearAxis[srow+1] = -ax1[1];
info->J2l[srow+2] = -ax1[2]; info->m_J2linearAxis[srow+2] = -ax1[2];
} }
// linear torque decoupling step: // linear torque decoupling step:
// //
@@ -664,12 +666,12 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
dVector3 ltd; // Linear Torque Decoupling vector (a torque) dVector3 ltd; // Linear Torque Decoupling vector (a torque)
c = btScalar(0.5) * c; c = btScalar(0.5) * c;
dCROSS (ltd,=,c,ax1); dCROSS (ltd,=,c,ax1);
info->J1a[srow+0] = ltd[0]; info->m_J1angularAxis[srow+0] = ltd[0];
info->J1a[srow+1] = ltd[1]; info->m_J1angularAxis[srow+1] = ltd[1];
info->J1a[srow+2] = ltd[2]; info->m_J1angularAxis[srow+2] = ltd[2];
info->J2a[srow+0] = ltd[0]; info->m_J2angularAxis[srow+0] = ltd[0];
info->J2a[srow+1] = ltd[1]; info->m_J2angularAxis[srow+1] = ltd[1];
info->J2a[srow+2] = ltd[2]; info->m_J2angularAxis[srow+2] = ltd[2];
} }
// right-hand part // right-hand part
btScalar lostop = slider->getLowerLinLimit(); btScalar lostop = slider->getLowerLinLimit();
@@ -683,9 +685,9 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
info->cfm[nrow] = btScalar(0.0); info->cfm[nrow] = btScalar(0.0);
if(!limit) if(!limit)
{ {
info->c[nrow] = slider->getTargetLinMotorVelocity(); info->m_constraintError[nrow] = slider->getTargetLinMotorVelocity();
info->lo[nrow] = -slider->getMaxLinMotorForce() * info->fps; info->m_lowerLimit[nrow] = -slider->getMaxLinMotorForce() * info->fps;
info->hi[nrow] = slider->getMaxLinMotorForce() * info->fps; info->m_higherLimit[nrow] = slider->getMaxLinMotorForce() * info->fps;
} }
} }
if(limit) if(limit)
@@ -693,32 +695,32 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
k = info->fps * info->erp; k = info->fps * info->erp;
if(m_body1) if(m_body1)
{ {
info->c[nrow] = k * limit_err; info->m_constraintError[nrow] = k * limit_err;
} }
else else
{ {
info->c[nrow] = - k * limit_err; info->m_constraintError[nrow] = - k * limit_err;
} }
info->cfm[nrow] = btScalar(0.0); // stop_cfm; info->cfm[nrow] = btScalar(0.0); // stop_cfm;
if(lostop == histop) if(lostop == histop)
{ {
// limited low and high simultaneously // limited low and high simultaneously
info->lo[nrow] = -SIMD_INFINITY; info->m_lowerLimit[nrow] = -SIMD_INFINITY;
info->hi[nrow] = SIMD_INFINITY; info->m_higherLimit[nrow] = SIMD_INFINITY;
} }
else else
{ {
if(limit == 1) if(limit == 1)
{ {
// low limit // low limit
info->lo[nrow] = 0; info->m_lowerLimit[nrow] = 0;
info->hi[nrow] = SIMD_INFINITY; info->m_higherLimit[nrow] = SIMD_INFINITY;
} }
else else
{ {
// high limit // high limit
info->lo[nrow] = -SIMD_INFINITY; info->m_lowerLimit[nrow] = -SIMD_INFINITY;
info->hi[nrow] = 0; info->m_higherLimit[nrow] = 0;
} }
} }
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that) // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
@@ -738,7 +740,8 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
if(vel < 0) if(vel < 0)
{ {
btScalar newc = -bounce * vel; btScalar newc = -bounce * vel;
if (newc > info->c[nrow]) info->c[nrow] = newc; if (newc > info->m_constraintError[nrow])
info->m_constraintError[nrow] = newc;
} }
} }
else else
@@ -747,11 +750,12 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
if(vel > 0) if(vel > 0)
{ {
btScalar newc = -bounce * vel; btScalar newc = -bounce * vel;
if(newc < info->c[nrow]) info->c[nrow] = newc; if(newc < info->m_constraintError[nrow])
info->m_constraintError[nrow] = newc;
} }
} }
} }
info->c[nrow] *= slider->getSoftnessLimLin(); info->m_constraintError[nrow] *= slider->getSoftnessLimLin();
} // if(limit) } // if(limit)
} // if linear limit } // if linear limit
// check angular limits // check angular limits
@@ -779,14 +783,14 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
{ {
nrow++; nrow++;
srow = nrow * info->rowskip; srow = nrow * info->rowskip;
info->J1a[srow+0] = ax1[0]; info->m_J1angularAxis[srow+0] = ax1[0];
info->J1a[srow+1] = ax1[1]; info->m_J1angularAxis[srow+1] = ax1[1];
info->J1a[srow+2] = ax1[2]; info->m_J1angularAxis[srow+2] = ax1[2];
if(m_body1) if(m_body1)
{ {
info->J2a[srow+0] = -ax1[0]; info->m_J2angularAxis[srow+0] = -ax1[0];
info->J2a[srow+1] = -ax1[1]; info->m_J2angularAxis[srow+1] = -ax1[1];
info->J2a[srow+2] = -ax1[2]; info->m_J2angularAxis[srow+2] = -ax1[2];
} }
btScalar lostop = slider->getLowerAngLimit(); btScalar lostop = slider->getLowerAngLimit();
btScalar histop = slider->getUpperAngLimit(); btScalar histop = slider->getUpperAngLimit();
@@ -799,9 +803,9 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
info->cfm[nrow] = btScalar(0.0); info->cfm[nrow] = btScalar(0.0);
if(!limit) if(!limit)
{ {
info->c[nrow] = slider->getTargetAngMotorVelocity(); info->m_constraintError[nrow] = slider->getTargetAngMotorVelocity();
info->lo[nrow] = -slider->getMaxAngMotorForce() * info->fps; info->m_lowerLimit[nrow] = -slider->getMaxAngMotorForce() * info->fps;
info->hi[nrow] = slider->getMaxAngMotorForce() * info->fps; info->m_higherLimit[nrow] = slider->getMaxAngMotorForce() * info->fps;
} }
} }
if(limit) if(limit)
@@ -809,32 +813,32 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
k = info->fps * info->erp; k = info->fps * info->erp;
if (m_body1) if (m_body1)
{ {
info->c[nrow] = k * limit_err; info->m_constraintError[nrow] = k * limit_err;
} }
else else
{ {
info->c[nrow] = -k * limit_err; info->m_constraintError[nrow] = -k * limit_err;
} }
info->cfm[nrow] = btScalar(0.0); // stop_cfm; info->cfm[nrow] = btScalar(0.0); // stop_cfm;
if(lostop == histop) if(lostop == histop)
{ {
// limited low and high simultaneously // limited low and high simultaneously
info->lo[nrow] = -SIMD_INFINITY; info->m_lowerLimit[nrow] = -SIMD_INFINITY;
info->hi[nrow] = SIMD_INFINITY; info->m_higherLimit[nrow] = SIMD_INFINITY;
} }
else else
{ {
if (limit == 1) if (limit == 1)
{ {
// low limit // low limit
info->lo[nrow] = 0; info->m_lowerLimit[nrow] = 0;
info->hi[nrow] = SIMD_INFINITY; info->m_higherLimit[nrow] = SIMD_INFINITY;
} }
else else
{ {
// high limit // high limit
info->lo[nrow] = -SIMD_INFINITY; info->m_lowerLimit[nrow] = -SIMD_INFINITY;
info->hi[nrow] = 0; info->m_higherLimit[nrow] = 0;
} }
} }
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
@@ -854,7 +858,7 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
if(vel < 0) if(vel < 0)
{ {
btScalar newc = -bounce * vel; btScalar newc = -bounce * vel;
if (newc > info->c[nrow]) info->c[nrow] = newc; if (newc > info->m_constraintError[nrow]) info->m_constraintError[nrow] = newc;
} }
} }
else else
@@ -863,11 +867,11 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
if(vel > 0) if(vel > 0)
{ {
btScalar newc = -bounce * vel; btScalar newc = -bounce * vel;
if(newc < info->c[nrow]) info->c[nrow] = newc; if(newc < info->m_constraintError[nrow]) info->m_constraintError[nrow] = newc;
} }
} }
} }
info->c[nrow] *= slider->getSoftnessLimAng(); info->m_constraintError[nrow] *= slider->getSoftnessLimAng();
} // if(limit) } // if(limit)
} // if angular limit or powered } // if angular limit or powered
} // OdeSliderJoint::GetInfo2() } // OdeSliderJoint::GetInfo2()

343
Extras/quickstep/btSorLcp.cpp
View File

@@ -65,29 +65,30 @@ subject to the following restrictions:
// during the solution. depending on the situation, this can help a lot // during the solution. depending on the situation, this can help a lot
// or hardly at all, but it doesn't seem to hurt. // or hardly at all, but it doesn't seem to hurt.
#define RANDOMLY_REORDER_CONSTRAINTS 1 //#define RANDOMLY_REORDER_CONSTRAINTS 1
//*************************************************************************** //***************************************************************************
// various common computations involving the matrix J // various common computations involving the matrix JconstraintAxis
// compute iMJ = inv(M)*J' // compute iMJ = inv(M)*JconstraintAxis'
inline void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb, inline void compute_invM_JT (int numConstraintRows, dRealMutablePtr JconstraintAxis, dRealMutablePtr iMJ, int *jb,
//OdeSolverBody* const *body, //OdeSolverBody* const *body,
const btAlignedObjectArray<btOdeSolverBody*> &body, const btAlignedObjectArray<btOdeSolverBody*> &body,
dRealPtr invI) dRealPtr inverseInertiaWorld)
{ {
int i,j; int i,j;
dRealMutablePtr iMJ_ptr = iMJ; dRealMutablePtr iMJ_ptr = iMJ;
dRealMutablePtr J_ptr = J; dRealMutablePtr J_ptr = JconstraintAxis;
for (i=0; i<m; i++) { for (i=0; i<numConstraintRows; i++) {
int b1 = jb[i*2]; int b1 = jb[i*2];
int b2 = jb[i*2+1]; int b2 = jb[i*2+1];
btScalar k = body[b1]->m_invMass; for (j=0; j<3; j++)
for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j]; iMJ_ptr[j] = body[b1]->m_invMass*J_ptr[j];
dMULTIPLY0_331 (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3); dMULTIPLY0_331 (iMJ_ptr + 3, inverseInertiaWorld + 12*b1, J_ptr + 3);
if (b2 >= 0) { if (b2 >= 0) {
k = body[b2]->m_invMass; for (j=0; j<3; j++)
for (j=0; j<3; j++) iMJ_ptr[j+6] = k*J_ptr[j+6]; iMJ_ptr[j+6] = body[b2]->m_invMass*J_ptr[j+6];//inv mass * constraint (normal) axis
dMULTIPLY0_331 (iMJ_ptr + 9, invI + 12*b2, J_ptr + 9); dMULTIPLY0_331 (iMJ_ptr + 9, inverseInertiaWorld + 12*b2, J_ptr + 9);//inverse inertia world * constraint (normal) axis
} }
J_ptr += 12; J_ptr += 12;
iMJ_ptr += 12; iMJ_ptr += 12;
@@ -95,7 +96,7 @@ inline void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int
} }
#if 0 #if 0
static void multiply_invM_JTSpecial (int m, int nb, dRealMutablePtr iMJ, int *jb, static void multiply_invM_JTSpecial (int numConstraintRows, int nb, dRealMutablePtr iMJ, int *jb,
dRealMutablePtr in, dRealMutablePtr out,int onlyBody1,int onlyBody2) dRealMutablePtr in, dRealMutablePtr out,int onlyBody1,int onlyBody2)
{ {
int i,j; int i,j;
@@ -116,7 +117,7 @@ static void multiply_invM_JTSpecial (int m, int nb, dRealMutablePtr iMJ, int *jb
} }
dRealPtr iMJ_ptr = iMJ; dRealPtr iMJ_ptr = iMJ;
for (i=0; i<m; i++) { for (i=0; i<numConstraintRows; i++) {
int b1 = jb[i*2]; int b1 = jb[i*2];
@@ -147,16 +148,16 @@ static void multiply_invM_JTSpecial (int m, int nb, dRealMutablePtr iMJ, int *jb
#endif #endif
// compute out = inv(M)*J'*in. // compute out = inv(M)*JconstraintAxis'*in.
#if 0 #if 0
static void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb, static void multiply_invM_JT (int numConstraintRows, int nb, dRealMutablePtr iMJ, int *jb,
dRealMutablePtr in, dRealMutablePtr out) dRealMutablePtr in, dRealMutablePtr out)
{ {
int i,j; int i,j;
dSetZero1 (out,6*nb); dSetZero1 (out,6*nb);
dRealPtr iMJ_ptr = iMJ; dRealPtr iMJ_ptr = iMJ;
for (i=0; i<m; i++) { for (i=0; i<numConstraintRows; i++) {
int b1 = jb[i*2]; int b1 = jb[i*2];
int b2 = jb[i*2+1]; int b2 = jb[i*2+1];
dRealMutablePtr out_ptr = out + b1*6; dRealMutablePtr out_ptr = out + b1*6;
@@ -173,13 +174,13 @@ static void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb,
#endif #endif
// compute out = J*in. // compute out = JconstraintAxis*in.
inline void multiply_J (int m, dRealMutablePtr J, int *jb, inline void multiply_J (int numConstraintRows, dRealMutablePtr JconstraintAxis, int *jb,
dRealMutablePtr in, dRealMutablePtr out) dRealMutablePtr in, dRealMutablePtr out)
{ {
int i,j; int i,j;
dRealPtr J_ptr = J; dRealPtr J_ptr = JconstraintAxis;
for (i=0; i<m; i++) { for (i=0; i<numConstraintRows; i++) {
int b1 = jb[i*2]; int b1 = jb[i*2];
int b2 = jb[i*2+1]; int b2 = jb[i*2+1];
btScalar sum = 0; btScalar sum = 0;
@@ -199,14 +200,14 @@ inline void multiply_J (int m, dRealMutablePtr J, int *jb,
// SOR-LCP method // SOR-LCP method
// nb is the number of bodies in the body array. // nb is the number of bodies in the body array.
// J is an m*12 matrix of constraint rows // JconstraintAxis is an numConstraintRows*12 matrix of constraint rows
// jb is an array of first and second body numbers for each constraint row // jb is an array of first and second body numbers for each constraint row
// invI is the global frame inverse inertia for each body (stacked 3x3 matrices) // inverseInertiaWorld is the global frame inverse inertia for each body (stacked 3x3 matrices)
// //
// this returns lambda and fc (the constraint force). // this returns lambdaAccumulatedImpulse and fc (the constraint force).
// note: fc is returned as inv(M)*J'*lambda, the constraint force is actually J'*lambda // note: fc is returned as inv(M)*JconstraintAxis'*lambdaAccumulatedImpulse, the constraint force is actually JconstraintAxis'*lambdaAccumulatedImpulse
// //
// b, lo and hi are modified on exit // b, lowerLimit and higherLimit are modified on exit
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
ATTRIBUTE_ALIGNED16(struct) IndexError { ATTRIBUTE_ALIGNED16(struct) IndexError {
@@ -216,10 +217,10 @@ ATTRIBUTE_ALIGNED16(struct) IndexError {
}; };
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void btSorLcpSolver::SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb, void btSorLcpSolver::SOR_LCP(int numConstraintRows, int nb, dRealMutablePtr JconstraintAxis, int *jb,
const btAlignedObjectArray<btOdeSolverBody*> &body, const btAlignedObjectArray<btOdeSolverBody*> &body,
dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs, dRealPtr inverseInertiaWorld, dRealMutablePtr lambdaAccumulatedImpulse, dRealMutablePtr invMforce, dRealMutablePtr rhs,
dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex, dRealMutablePtr lowerLimit, dRealMutablePtr higherLimit, dRealPtr cfm, int *findex,
int numiter,float overRelax, int numiter,float overRelax,
btStackAlloc* stackAlloc btStackAlloc* stackAlloc
) )
@@ -237,49 +238,51 @@ void btSorLcpSolver::SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb,
#ifdef WARM_STARTING #ifdef WARM_STARTING
// for warm starting, this seems to be necessary to prevent // for warm starting, this seems to be necessary to prevent
// jerkiness in motor-driven joints. i have no idea why this works. // jerkiness in motor-driven joints. i have no idea why this works.
for (i=0; i<m; i++) lambda[i] *= 0.9; for (i=0; i<numConstraintRows; i++) lambdaAccumulatedImpulse[i] *= 0.9;
#else #else
dSetZero1 (lambda,m); dSetZero1 (lambdaAccumulatedImpulse,numConstraintRows);
#endif #endif
// the lambda computed at the previous iteration. // the lambdaAccumulatedImpulse computed at the previous iteration.
// this is used to measure error for when we are reordering the indexes. // this is used to measure error for when we are reordering the indexes.
dRealAllocaArray (last_lambda,m); dRealAllocaArray (last_lambda,numConstraintRows);
// a copy of the 'hi' vector in case findex[] is being used // a copy of the 'higherLimit' vector in case findex[] is being used
dRealAllocaArray (hicopy,m); dRealAllocaArray (hicopy,numConstraintRows);
memcpy (hicopy,hi,m*sizeof(float)); memcpy (hicopy,higherLimit,numConstraintRows*sizeof(float));
// precompute iMJ = inv(M)*J' // precompute iMJ = inv(M)*JconstraintAxis'
dRealAllocaArray (iMJ,m*12); dRealAllocaArray (iMJ,numConstraintRows*12);
compute_invM_JT (m,J,iMJ,jb,body,invI); compute_invM_JT (numConstraintRows,JconstraintAxis,iMJ,jb,body,inverseInertiaWorld);
// compute fc=(inv(M)*J')*lambda. we will incrementally maintain fc // compute fc=(inv(M)*JconstraintAxis')*lambdaAccumulatedImpulse. we will incrementally maintain fc
// as we change lambda. // as we change lambdaAccumulatedImpulse.
#ifdef WARM_STARTING #ifdef WARM_STARTING
multiply_invM_JT (m,nb,iMJ,jb,lambda,fc); multiply_invM_JT (numConstraintRows,nb,iMJ,jb,lambdaAccumulatedImpulse,fc);
#else #else
dSetZero1 (invMforce,nb*6); dSetZero1 (invMforce,nb*6);
#endif #endif
// precompute 1 / diagonals of A // precompute 1 / diagonals of A
dRealAllocaArray (Ad,m); dRealAllocaArray (Ad,numConstraintRows);
dRealPtr iMJ_ptr = iMJ; dRealPtr iMJ_ptr = iMJ;
dRealMutablePtr J_ptr = J; dRealMutablePtr J_ptr = JconstraintAxis;
for (i=0; i<m; i++) { for (i=0; i<numConstraintRows; i++) {
float sum = 0; float sum = 0;
for (j=0; j<6; j++) sum += iMJ_ptr[j] * J_ptr[j]; for (j=0; j<6; j++)
sum += iMJ_ptr[j] * J_ptr[j];
if (jb[i*2+1] >= 0) { if (jb[i*2+1] >= 0) {
for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j]; for (j=6; j<12; j++)
sum += iMJ_ptr[j] * J_ptr[j];
} }
iMJ_ptr += 12; iMJ_ptr += 12;
J_ptr += 12; J_ptr += 12;
Ad[i] = sor_w / sum;//(sum + cfm[i]); Ad[i] = sor_w / sum;//(sum + cfm[i]);
} }
// scale J and b by Ad // scale JconstraintAxis and b by Ad
J_ptr = J; J_ptr = JconstraintAxis;
for (i=0; i<m; i++) { for (i=0; i<numConstraintRows; i++) {
for (j=0; j<12; j++) { for (j=0; j<12; j++) {
J_ptr[0] *= Ad[i]; J_ptr[0] *= Ad[i];
J_ptr++; J_ptr++;
@@ -288,24 +291,24 @@ void btSorLcpSolver::SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb,
} }
// scale Ad by CFM // scale Ad by CFM
for (i=0; i<m; i++) for (i=0; i<numConstraintRows; i++)
Ad[i] *= cfm[i]; Ad[i] *= cfm[i];
// order to solve constraint rows in // order to solve constraint rows in
//IndexError *order = (IndexError*) alloca (m*sizeof(IndexError)); //IndexError *order = (IndexError*) alloca (numConstraintRows*sizeof(IndexError));
IndexError *order = (IndexError*) ALLOCA (m*sizeof(IndexError)); IndexError *order = (IndexError*) ALLOCA (numConstraintRows*sizeof(IndexError));
#ifndef REORDER_CONSTRAINTS #ifndef REORDER_CONSTRAINTS
// make sure constraints with findex < 0 come first. // make sure constraints with findex < 0 come first.
j=0; j=0;
for (i=0; i<m; i++) for (i=0; i<numConstraintRows; i++)
if (findex[i] < 0) if (findex[i] < 0)
order[j++].index = i; order[j++].index = i;
for (i=0; i<m; i++) for (i=0; i<numConstraintRows; i++)
if (findex[i] >= 0) if (findex[i] >= 0)
order[j++].index = i; order[j++].index = i;
dIASSERT (j==m); dIASSERT (j==numConstraintRows);
#endif #endif
for (int iteration=0; iteration < num_iterations; iteration++) { for (int iteration=0; iteration < num_iterations; iteration++) {
@@ -315,7 +318,7 @@ void btSorLcpSolver::SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb,
if (iteration < 2) { if (iteration < 2) {
// for the first two iterations, solve the constraints in // for the first two iterations, solve the constraints in
// the given order // the given order
for (i=0; i<m; i++) { for (i=0; i<numConstraintRows; i++) {
order[i].error = i; order[i].error = i;
order[i].findex = findex[i]; order[i].findex = findex[i];
order[i].index = i; order[i].index = i;
@@ -324,13 +327,13 @@ void btSorLcpSolver::SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb,
else { else {
// sort the constraints so that the ones converging slowest // sort the constraints so that the ones converging slowest
// get solved last. use the absolute (not relative) error. // get solved last. use the absolute (not relative) error.
for (i=0; i<m; i++) { for (i=0; i<numConstraintRows; i++) {
float v1 = dFabs (lambda[i]); float v1 = dFabs (lambdaAccumulatedImpulse[i]);
float v2 = dFabs (last_lambda[i]); float v2 = dFabs (last_lambda[i]);
float max = (v1 > v2) ? v1 : v2; float max = (v1 > v2) ? v1 : v2;
if (max > 0) { if (max > 0) {
//@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max; //@@@ relative error: order[i].error = dFabs(lambdaAccumulatedImpulse[i]-last_lambda[i])/max;
order[i].error = dFabs(lambda[i]-last_lambda[i]); order[i].error = dFabs(lambdaAccumulatedImpulse[i]-last_lambda[i]);
} }
else { else {
order[i].error = dInfinity; order[i].error = dInfinity;
@@ -339,11 +342,11 @@ void btSorLcpSolver::SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb,
order[i].index = i; order[i].index = i;
} }
} }
qsort (order,m,sizeof(IndexError),&compare_index_error); qsort (order,numConstraintRows,sizeof(IndexError),&compare_index_error);
#endif #endif
#ifdef RANDOMLY_REORDER_CONSTRAINTS #ifdef RANDOMLY_REORDER_CONSTRAINTS
if ((iteration & 7) == 0) { if ((iteration & 7) == 0) {
for (i=1; i<m; ++i) { for (i=1; i<numConstraintRows; ++i) {
IndexError tmp = order[i]; IndexError tmp = order[i];
int swapi = dRandInt2(i+1); int swapi = dRandInt2(i+1);
order[i] = order[swapi]; order[i] = order[swapi];
@@ -352,19 +355,19 @@ void btSorLcpSolver::SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb,
} }
#endif #endif
//@@@ potential optimization: swap lambda and last_lambda pointers rather //@@@ potential optimization: swap lambdaAccumulatedImpulse and last_lambda pointers rather
// than copying the data. we must make sure lambda is properly // than copying the data. we must make sure lambdaAccumulatedImpulse is properly
// returned to the caller // returned to the caller
memcpy (last_lambda,lambda,m*sizeof(float)); memcpy (last_lambda,lambdaAccumulatedImpulse,numConstraintRows*sizeof(float));
for (int i=0; i<m; i++) { for (int i=0; i<numConstraintRows; i++) {
// @@@ potential optimization: we could pre-sort J and iMJ, thereby // @@@ potential optimization: we could pre-sort JconstraintAxis and iMJ, thereby
// linearizing access to those arrays. hmmm, this does not seem // linearizing access to those arrays. hmmm, this does not seem
// like a win, but we should think carefully about our memory // like a win, but we should think carefully about our memory
// access pattern. // access pattern.
int index = order[i].index; int index = order[i].index;
J_ptr = J + index*12; J_ptr = JconstraintAxis + index*12;
iMJ_ptr = iMJ + index*12; iMJ_ptr = iMJ + index*12;
// set the limits for this constraint. note that 'hicopy' is used. // set the limits for this constraint. note that 'hicopy' is used.
@@ -372,70 +375,72 @@ void btSorLcpSolver::SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb,
// direct LCP solving method, since that method only performs this // direct LCP solving method, since that method only performs this
// limit adjustment once per time step, whereas this method performs // limit adjustment once per time step, whereas this method performs
// once per iteration per constraint row. // once per iteration per constraint row.
// the constraints are ordered so that all lambda[] values needed have // the constraints are ordered so that all lambdaAccumulatedImpulse[] values needed have
// already been computed. // already been computed.
if (findex[index] >= 0) { if (findex[index] >= 0) {
hi[index] = btFabs (hicopy[index] * lambda[findex[index]]); higherLimit[index] = btFabs (hicopy[index] * lambdaAccumulatedImpulse[findex[index]]);
lo[index] = -hi[index]; lowerLimit[index] = -higherLimit[index];
} }
int b1 = jb[index*2]; int b1 = jb[index*2];
int b2 = jb[index*2+1]; int b2 = jb[index*2+1];
float delta = rhs[index] - lambda[index]*Ad[index];
dRealMutablePtr fc_ptr = invMforce + 6*b1; dRealMutablePtr deltaVelocity = invMforce + 6*b1;
float deltaAppliedImpulse = rhs[index] - lambdaAccumulatedImpulse[index]*Ad[index];
// @@@ potential optimization: SIMD-ize this and the b2 >= 0 case // @@@ potential optimization: SIMD-ize this and the b2 >= 0 case
delta -=fc_ptr[0] * J_ptr[0] + fc_ptr[1] * J_ptr[1] + deltaAppliedImpulse -=deltaVelocity[0] * J_ptr[0] + deltaVelocity[1] * J_ptr[1] +
fc_ptr[2] * J_ptr[2] + fc_ptr[3] * J_ptr[3] + deltaVelocity[2] * J_ptr[2] + deltaVelocity[3] * J_ptr[3] +
fc_ptr[4] * J_ptr[4] + fc_ptr[5] * J_ptr[5]; deltaVelocity[4] * J_ptr[4] + deltaVelocity[5] * J_ptr[5];
// @@@ potential optimization: handle 1-body constraints in a separate // @@@ potential optimization: handle 1-body constraints in a separate
// loop to avoid the cost of test & jump? // loop to avoid the cost of test & jump?
if (b2 >= 0) { if (b2 >= 0) {
fc_ptr = invMforce + 6*b2; deltaVelocity = invMforce + 6*b2;
delta -=fc_ptr[0] * J_ptr[6] + fc_ptr[1] * J_ptr[7] + deltaAppliedImpulse -=deltaVelocity[0] * J_ptr[6] + deltaVelocity[1] * J_ptr[7] +
fc_ptr[2] * J_ptr[8] + fc_ptr[3] * J_ptr[9] + deltaVelocity[2] * J_ptr[8] + deltaVelocity[3] * J_ptr[9] +
fc_ptr[4] * J_ptr[10] + fc_ptr[5] * J_ptr[11]; deltaVelocity[4] * J_ptr[10] + deltaVelocity[5] * J_ptr[11];
} }
// compute lambda and clamp it to [lo,hi]. // compute lambdaAccumulatedImpulse and clamp it to [lowerLimit,higherLimit].
// @@@ potential optimization: does SSE have clamping instructions // @@@ potential optimization: does SSE have clamping instructions
// to save test+jump penalties here? // to save test+jump penalties here?
float new_lambda = lambda[index] + delta; float sum = lambdaAccumulatedImpulse[index] + deltaAppliedImpulse;
if (new_lambda < lo[index]) { if (sum < lowerLimit[index]) {
delta = lo[index]-lambda[index]; deltaAppliedImpulse = lowerLimit[index]-lambdaAccumulatedImpulse[index];
lambda[index] = lo[index]; lambdaAccumulatedImpulse[index] = lowerLimit[index];
} }
else if (new_lambda > hi[index]) { else if (sum > higherLimit[index]) {
delta = hi[index]-lambda[index]; deltaAppliedImpulse = higherLimit[index]-lambdaAccumulatedImpulse[index];
lambda[index] = hi[index]; lambdaAccumulatedImpulse[index] = higherLimit[index];
} }
else { else {
lambda[index] = new_lambda; lambdaAccumulatedImpulse[index] = sum;
} }
//@@@ a trick that may or may not help //@@@ a trick that may or may not help
//float ramp = (1-((float)(iteration+1)/(float)num_iterations)); //float ramp = (1-((float)(iteration+1)/(float)num_iterations));
//delta *= ramp; //deltaAppliedImpulse *= ramp;
// update invMforce. // update invMforce.
// @@@ potential optimization: SIMD for this and the b2 >= 0 case // @@@ potential optimization: SIMD for this and the b2 >= 0 case
fc_ptr = invMforce + 6*b1; deltaVelocity = invMforce + 6*b1;
fc_ptr[0] += delta * iMJ_ptr[0]; deltaVelocity[0] += deltaAppliedImpulse * iMJ_ptr[0];
fc_ptr[1] += delta * iMJ_ptr[1]; deltaVelocity[1] += deltaAppliedImpulse * iMJ_ptr[1];
fc_ptr[2] += delta * iMJ_ptr[2]; deltaVelocity[2] += deltaAppliedImpulse * iMJ_ptr[2];
fc_ptr[3] += delta * iMJ_ptr[3]; deltaVelocity[3] += deltaAppliedImpulse * iMJ_ptr[3];
fc_ptr[4] += delta * iMJ_ptr[4]; deltaVelocity[4] += deltaAppliedImpulse * iMJ_ptr[4];
fc_ptr[5] += delta * iMJ_ptr[5]; deltaVelocity[5] += deltaAppliedImpulse * iMJ_ptr[5];
// @@@ potential optimization: handle 1-body constraints in a separate // @@@ potential optimization: handle 1-body constraints in a separate
// loop to avoid the cost of test & jump? // loop to avoid the cost of test & jump?
if (b2 >= 0) { if (b2 >= 0) {
fc_ptr = invMforce + 6*b2; deltaVelocity = invMforce + 6*b2;
fc_ptr[0] += delta * iMJ_ptr[6]; deltaVelocity[0] += deltaAppliedImpulse * iMJ_ptr[6];
fc_ptr[1] += delta * iMJ_ptr[7]; deltaVelocity[1] += deltaAppliedImpulse * iMJ_ptr[7];
fc_ptr[2] += delta * iMJ_ptr[8]; deltaVelocity[2] += deltaAppliedImpulse * iMJ_ptr[8];
fc_ptr[3] += delta * iMJ_ptr[9]; deltaVelocity[3] += deltaAppliedImpulse * iMJ_ptr[9];
fc_ptr[4] += delta * iMJ_ptr[10]; deltaVelocity[4] += deltaAppliedImpulse * iMJ_ptr[10];
fc_ptr[5] += delta * iMJ_ptr[11]; deltaVelocity[5] += deltaAppliedImpulse * iMJ_ptr[11];
} }
} }
} }
@@ -443,10 +448,7 @@ void btSorLcpSolver::SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb,
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void btSorLcpSolver::SolveInternal1 ( void btSorLcpSolver::SolveInternal1 (const btAlignedObjectArray<btOdeSolverBody*> &body, int nb,
float global_cfm,
float global_erp,
const btAlignedObjectArray<btOdeSolverBody*> &body, int nb,
btAlignedObjectArray<btOdeJoint*> &joint, btAlignedObjectArray<btOdeJoint*> &joint,
int nj, const btContactSolverInfo& solverInfo, int nj, const btContactSolverInfo& solverInfo,
btStackAlloc* stackAlloc) btStackAlloc* stackAlloc)
@@ -475,16 +477,16 @@ void btSorLcpSolver::SolveInternal1 (
// for all bodies, compute the inertia tensor and its inverse in the global // for all bodies, compute the inertia tensor and its inverse in the global
// frame, and compute the rotational force and add it to the torque // frame, and compute the rotational force and add it to the torque
// accumulator. I and invI are a vertical stack of 3x4 matrices, one per body. // accumulator. I and inverseInertiaWorld are a vertical stack of 3x4 matrices, one per body.
dRealAllocaArray (I,3*4*nb); dRealAllocaArray (I,3*4*nb);
dRealAllocaArray (invI,3*4*nb); dRealAllocaArray (inverseInertiaWorld,3*4*nb);
/* for (i=0; i<nb; i++) { /* for (i=0; i<nb; i++) {
dMatrix3 tmp; dMatrix3 tmp;
// compute inertia tensor in global frame // compute inertia tensor in global frame
dMULTIPLY2_333 (tmp,body[i]->m_I,body[i]->m_R); dMULTIPLY2_333 (tmp,body[i]->m_I,body[i]->m_R);
// compute inverse inertia tensor in global frame // compute inverse inertia tensor in global frame
dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R); dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R);
dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp); dMULTIPLY0_333 (inverseInertiaWorld+i*12,body[i]->m_R,tmp);
// compute rotational force // compute rotational force
dCROSS (body[i]->m_tacc,-=,body[i]->getAngularVelocity(),tmp); dCROSS (body[i]->m_tacc,-=,body[i]->getAngularVelocity(),tmp);
} }
@@ -497,7 +499,7 @@ void btSorLcpSolver::SolveInternal1 (
// compute inverse inertia tensor in global frame // compute inverse inertia tensor in global frame
dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R); dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R);
dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp); dMULTIPLY0_333 (inverseInertiaWorld+i*12,body[i]->m_R,tmp);
// compute rotational force // compute rotational force
// dMULTIPLY0_331 (tmp,I+i*12,body[i]->m_angularVelocity); // dMULTIPLY0_331 (tmp,I+i*12,body[i]->m_angularVelocity);
// dCROSS (body[i]->m_tacc,-=,body[i]->m_angularVelocity,tmp); // dCROSS (body[i]->m_tacc,-=,body[i]->m_angularVelocity,tmp);
@@ -506,16 +508,16 @@ void btSorLcpSolver::SolveInternal1 (
// get joint information (m = total constraint dimension, nub = number of unbounded variables). // get joint information (numConstraintRows = total constraint dimension, nub = number of unbounded variables).
// joints with m=0 are inactive and are removed from the joints array // joints with numConstraintRows=0 are inactive and are removed from the joints array
// entirely, so that the code that follows does not consider them. // entirely, so that the code that follows does not consider them.
//@@@ do we really need to save all the info1's //@@@ do we really need to save all the info1's
btOdeJoint::Info1 *info = (btOdeJoint::Info1*) ALLOCA (nj*sizeof(btOdeJoint::Info1)); btOdeJoint::Info1 *info = (btOdeJoint::Info1*) ALLOCA (nj*sizeof(btOdeJoint::Info1));
for (i=0, j=0; j<nj; j++) { // i=dest, j=src for (i=0, j=0; j<nj; j++) { // i=dest, j=src
joint[j]->GetInfo1 (info+i); joint[j]->GetInfo1 (info+i);
dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m); dIASSERT (info[i].m_numConstraintRows >= 0 && info[i].m_numConstraintRows <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m_numConstraintRows);
if (info[i].m > 0) { if (info[i].m_numConstraintRows > 0) {
joint[i] = joint[j]; joint[i] = joint[j];
i++; i++;
} }
@@ -523,34 +525,34 @@ void btSorLcpSolver::SolveInternal1 (
nj = i; nj = i;
// create the row offset array // create the row offset array
int m = 0; int numConstraintRows = 0;
int *ofs = (int*) ALLOCA (nj*sizeof(int)); int *constraintRowOffsets = (int*) ALLOCA (nj*sizeof(int));
for (i=0; i<nj; i++) { for (i=0; i<nj; i++) {
ofs[i] = m; constraintRowOffsets[i] = numConstraintRows;
m += info[i].m; numConstraintRows += info[i].m_numConstraintRows;
} }
// if there are constraints, compute the constraint force // if there are constraints, compute the constraint force
dRealAllocaArray (J,m*12); dRealAllocaArray (JconstraintAxis,numConstraintRows*12);
int *jb = (int*) ALLOCA (m*2*sizeof(int)); int *jb = (int*) ALLOCA (numConstraintRows*2*sizeof(int));
if (m > 0) { if (numConstraintRows > 0) {
// create a constraint equation right hand side vector `c', a constraint // create a constraint equation right hand side vector `c', a constraint
// force mixing vector `cfm', and LCP low and high bound vectors, and an // force mixing vector `cfm', and LCP low and high bound vectors, and an
// 'findex' vector. // 'findex' vector.
dRealAllocaArray (c,m); dRealAllocaArray (c_rhs,numConstraintRows);
dRealAllocaArray (cfm,m); dRealAllocaArray (cfm,numConstraintRows);
dRealAllocaArray (lo,m); dRealAllocaArray (lowerLimit,numConstraintRows);
dRealAllocaArray (hi,m); dRealAllocaArray (higherLimit,numConstraintRows);
int *findex = (int*) ALLOCA (m*sizeof(int)); int *findex = (int*) ALLOCA (numConstraintRows*sizeof(int));
dSetZero1 (c,m); dSetZero1 (c_rhs,numConstraintRows);
dSetValue1 (cfm,m,global_cfm); dSetValue1 (cfm,numConstraintRows,solverInfo.m_globalCfm);
dSetValue1 (lo,m,-dInfinity); dSetValue1 (lowerLimit,numConstraintRows,-dInfinity);
dSetValue1 (hi,m, dInfinity); dSetValue1 (higherLimit,numConstraintRows, dInfinity);
for (i=0; i<m; i++) findex[i] = -1; for (i=0; i<numConstraintRows; i++) findex[i] = -1;
// get jacobian data from constraints. an m*12 matrix will be created // get jacobian data from constraints. an numConstraintRows*12 matrix will be created
// to store the two jacobian blocks from each constraint. it has this // to store the two jacobian blocks from each constraint. it has this
// format: // format:
// //
@@ -563,30 +565,30 @@ void btSorLcpSolver::SolveInternal1 (
// (lll) = linear jacobian data // (lll) = linear jacobian data
// (aaa) = angular jacobian data // (aaa) = angular jacobian data
// //
dSetZero1 (J,m*12); dSetZero1 (JconstraintAxis,numConstraintRows*12);
btOdeJoint::Info2 Jinfo; btOdeJoint::Info2 Jinfo;
Jinfo.rowskip = 12; Jinfo.rowskip = 12;
Jinfo.fps = stepsize1; Jinfo.fps = stepsize1;
Jinfo.erp = global_erp; Jinfo.erp = solverInfo.m_erp;
for (i=0; i<nj; i++) { for (i=0; i<nj; i++) {
Jinfo.J1l = J + ofs[i]*12; Jinfo.m_J1linearAxis = JconstraintAxis + constraintRowOffsets[i]*12;
Jinfo.J1a = Jinfo.J1l + 3; Jinfo.m_J1angularAxis = Jinfo.m_J1linearAxis + 3;
Jinfo.J2l = Jinfo.J1l + 6; Jinfo.m_J2linearAxis = Jinfo.m_J1linearAxis + 6;
Jinfo.J2a = Jinfo.J1l + 9; Jinfo.m_J2angularAxis = Jinfo.m_J1linearAxis + 9;
Jinfo.c = c + ofs[i]; Jinfo.m_constraintError = c_rhs + constraintRowOffsets[i];
Jinfo.cfm = cfm + ofs[i]; Jinfo.cfm = cfm + constraintRowOffsets[i];
Jinfo.lo = lo + ofs[i]; Jinfo.m_lowerLimit = lowerLimit + constraintRowOffsets[i];
Jinfo.hi = hi + ofs[i]; Jinfo.m_higherLimit = higherLimit + constraintRowOffsets[i];
Jinfo.findex = findex + ofs[i]; Jinfo.findex = findex + constraintRowOffsets[i];
joint[i]->GetInfo2 (&Jinfo); joint[i]->GetInfo2 (&Jinfo);
if (Jinfo.c[0] > solverInfo.m_maxErrorReduction) if (Jinfo.m_constraintError[0] > solverInfo.m_maxErrorReduction)
Jinfo.c[0] = solverInfo.m_maxErrorReduction; Jinfo.m_constraintError[0] = solverInfo.m_maxErrorReduction;
// adjust returned findex values for global index numbering // adjust returned findex values for global index numbering
for (j=0; j<info[i].m; j++) { for (j=0; j<info[i].m_numConstraintRows; j++) {
if (findex[ofs[i] + j] >= 0) if (findex[constraintRowOffsets[i] + j] >= 0)
findex[ofs[i] + j] += ofs[i]; findex[constraintRowOffsets[i] + j] += constraintRowOffsets[i];
} }
} }
@@ -595,13 +597,13 @@ void btSorLcpSolver::SolveInternal1 (
for (i=0; i<nj; i++) { for (i=0; i<nj; i++) {
int b1 = (joint[i]->node[0].body) ? (joint[i]->node[0].body->m_odeTag) : -1; int b1 = (joint[i]->node[0].body) ? (joint[i]->node[0].body->m_odeTag) : -1;
int b2 = (joint[i]->node[1].body) ? (joint[i]->node[1].body->m_odeTag) : -1; int b2 = (joint[i]->node[1].body) ? (joint[i]->node[1].body->m_odeTag) : -1;
for (j=0; j<info[i].m; j++) { for (j=0; j<info[i].m_numConstraintRows; j++) {
jb_ptr[0] = b1; jb_ptr[0] = b1;
jb_ptr[1] = b2; jb_ptr[1] = b2;
jb_ptr += 2; jb_ptr += 2;
} }
} }
dIASSERT (jb_ptr == jb+2*m); dIASSERT (jb_ptr == jb+2*numConstraintRows);
// compute the right hand side `rhs' // compute the right hand side `rhs'
dRealAllocaArray (tmp1,nb*6); dRealAllocaArray (tmp1,nb*6);
@@ -610,47 +612,48 @@ void btSorLcpSolver::SolveInternal1 (
btScalar body_invMass = body[i]->m_invMass; btScalar body_invMass = body[i]->m_invMass;
for (j=0; j<3; j++) for (j=0; j<3; j++)
tmp1[i*6+j] = body[i]->m_facc[j] * body_invMass + body[i]->m_linearVelocity[j] * stepsize1; tmp1[i*6+j] = body[i]->m_facc[j] * body_invMass + body[i]->m_linearVelocity[j] * stepsize1;
dMULTIPLY0_331NEW (tmp1 + i*6 + 3,=,invI + i*12,body[i]->m_tacc); dMULTIPLY0_331NEW (tmp1 + i*6 + 3,=,inverseInertiaWorld + i*12,body[i]->m_tacc);
for (j=0; j<3; j++) for (j=0; j<3; j++)
tmp1[i*6+3+j] += body[i]->m_angularVelocity[j] * stepsize1; tmp1[i*6+3+j] += body[i]->m_angularVelocity[j] * stepsize1;
} }
// put J*tmp1 into rhs // put JconstraintAxis*tmp1 into rhs
dRealAllocaArray (rhs,m); dRealAllocaArray (rhs,numConstraintRows);
multiply_J (m,J,jb,tmp1,rhs); multiply_J (numConstraintRows,JconstraintAxis,jb,tmp1,rhs);
// complete rhs // complete rhs
for (i=0; i<m; i++) rhs[i] = c[i]*stepsize1 - rhs[i]; for (i=0; i<numConstraintRows; i++)
rhs[i] = c_rhs[i]*stepsize1 - rhs[i];
// scale CFM // scale CFM
for (i=0; i<m; i++) for (i=0; i<numConstraintRows; i++)
cfm[i] *= stepsize1; cfm[i] *= stepsize1;
// load lambda from the value saved on the previous iteration // load lambdaAccumulatedImpulse from the value saved on the previous iteration
dRealAllocaArray (lambda,m); dRealAllocaArray (lambdaAccumulatedImpulse,numConstraintRows);
#ifdef WARM_STARTING #ifdef WARM_STARTING
dSetZero1 (lambda,m); //@@@ shouldn't be necessary dSetZero1 (lambdaAccumulatedImpulse,numConstraintRows); //@@@ shouldn't be necessary
for (i=0; i<nj; i++) { for (i=0; i<nj; i++) {
memcpy (lambda+ofs[i],joint[i]->lambda,info[i].m * sizeof(btScalar)); memcpy (lambdaAccumulatedImpulse+constraintRowOffsets[i],joint[i]->lambdaAccumulatedImpulse,info[i].numConstraintRows * sizeof(btScalar));
} }
#endif #endif
// solve the LCP problem and get lambda and invM*constraint_force // solve the LCP problem and get lambdaAccumulatedImpulse and invM*constraint_force
dRealAllocaArray (cforce,nb*6); dRealAllocaArray (cforce,nb*6);
/// SOR_LCP /// SOR_LCP
SOR_LCP (m,nb,J,jb,body,invI,lambda,cforce,rhs,lo,hi,cfm,findex,numIter,sOr,stackAlloc); SOR_LCP (numConstraintRows,nb,JconstraintAxis,jb,body,inverseInertiaWorld,lambdaAccumulatedImpulse,cforce,rhs,lowerLimit,higherLimit,cfm,findex,numIter,sOr,stackAlloc);
#ifdef WARM_STARTING #ifdef WARM_STARTING
// save lambda for the next iteration // save lambdaAccumulatedImpulse for the next iteration
//@@@ note that this doesn't work for contact joints yet, as they are //@@@ note that this doesn't work for contact joints yet, as they are
// recreated every iteration // recreated every iteration
for (i=0; i<nj; i++) { for (i=0; i<nj; i++) {
memcpy (joint[i]->lambda,lambda+ofs[i],info[i].m * sizeof(btScalar)); memcpy (joint[i]->lambdaAccumulatedImpulse,lambdaAccumulatedImpulse+constraintRowOffsets[i],info[i].numConstraintRows * sizeof(btScalar));
} }
#endif #endif
// note that the SOR method overwrites rhs and J at this point, so // note that the SOR method overwrites rhs and JconstraintAxis at this point, so
// they should not be used again. // they should not be used again.
// add stepsize * cforce to the body velocity // add stepsize * cforce to the body velocity
for (i=0; i<nb; i++) { for (i=0; i<nb; i++) {
@@ -677,7 +680,7 @@ void btSorLcpSolver::SolveInternal1 (
{ {
body[i]->m_tacc[j] *= solverInfo.m_timeStep; body[i]->m_tacc[j] *= solverInfo.m_timeStep;
} }
dMULTIPLY0_331NEW(angvel,+=,invI + i*12,body[i]->m_tacc); dMULTIPLY0_331NEW(angvel,+=,inverseInertiaWorld + i*12,body[i]->m_tacc);
body[i]->m_angularVelocity = angvel; body[i]->m_angularVelocity = angvel;
} }
//stackAlloc->endBlock(saBlock);//Remo: 10.10.2007 //stackAlloc->endBlock(saBlock);//Remo: 10.10.2007

4
Extras/quickstep/btSorLcp.h
View File

@@ -39,9 +39,7 @@ public:
dRand2_seed = 0; dRand2_seed = 0;
} }
void SolveInternal1 (float global_cfm, void SolveInternal1 (const btAlignedObjectArray<btOdeSolverBody*> &body, int nb,
float global_erp,
const btAlignedObjectArray<btOdeSolverBody*> &body, int nb,
btAlignedObjectArray<btOdeJoint*> &joint, btAlignedObjectArray<btOdeJoint*> &joint,
int nj, const btContactSolverInfo& solverInfo, int nj, const btContactSolverInfo& solverInfo,
btStackAlloc* stackAlloc btStackAlloc* stackAlloc

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

@@ -94,8 +94,6 @@ void btConeTwistConstraint::buildJacobian()
for (int i=0;i<3;i++) for (int i=0;i<3;i++)
{ {
new (&m_jac[i]) btJacobianEntry( new (&m_jac[i]) btJacobianEntry(
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
pivotAInW - m_rbA.getCenterOfMassPosition(), pivotAInW - m_rbA.getCenterOfMassPosition(),
pivotBInW - m_rbB.getCenterOfMassPosition(), pivotBInW - m_rbB.getCenterOfMassPosition(),
normal[i], 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; btVector3 vel = vel1 - vel2;
btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(), btJacobianEntry jac(
body2.getCenterOfMassTransform().getBasis().transpose(),
rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(), rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
body2.getInvInertiaDiagLocal(),body2.getInvMass()); body2.getInvInertiaDiagLocal(),body2.getInvMass());

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

@@ -39,6 +39,7 @@ struct btContactSolverInfoData
btScalar m_sor; btScalar m_sor;
btScalar m_erp;//used as Baumgarte factor btScalar m_erp;//used as Baumgarte factor
btScalar m_erp2;//used in Split Impulse btScalar m_erp2;//used in Split Impulse
btScalar m_globalCfm;//constraint force mixing
int m_splitImpulse; int m_splitImpulse;
btScalar m_splitImpulsePenetrationThreshold; btScalar m_splitImpulsePenetrationThreshold;
btScalar m_linearSlop; btScalar m_linearSlop;
@@ -63,9 +64,10 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_restitution = btScalar(0.); m_restitution = btScalar(0.);
m_maxErrorReduction = btScalar(20.); m_maxErrorReduction = btScalar(20.);
m_numIterations = 10; m_numIterations = 10;
m_erp = btScalar(0.2); m_erp = btScalar(0.4);
m_erp2 = btScalar(0.1); m_erp2 = btScalar(0.1);
m_sor = btScalar(1.3); m_globalCfm = btScalar(0.);
m_sor = btScalar(1.);
m_splitImpulse = false; m_splitImpulse = false;
m_splitImpulsePenetrationThreshold = -0.02f; m_splitImpulsePenetrationThreshold = -0.02f;
m_linearSlop = btScalar(0.0); 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) const btVector3 & pivotAInW,const btVector3 & pivotBInW)
{ {
new (&jacLinear) btJacobianEntry( new (&jacLinear) btJacobianEntry(
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
pivotAInW - m_rbA.getCenterOfMassPosition(), pivotAInW - m_rbA.getCenterOfMassPosition(),
pivotBInW - m_rbB.getCenterOfMassPosition(), pivotBInW - m_rbB.getCenterOfMassPosition(),
normalWorld, normalWorld,

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

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

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

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

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

@@ -48,8 +48,7 @@ void btPoint2PointConstraint::buildJacobian()
{ {
normal[i] = 1; normal[i] = 1;
new (&m_jac[i]) btJacobianEntry( new (&m_jac[i]) btJacobianEntry(
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(), m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(), m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
normal, 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: 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); 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 ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
unsigned long m_btSeed2; 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: public:
btSequentialImpulseConstraintSolver(); 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 ~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 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 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 ///clear internal cached data and reset random seed
virtual void reset(); virtual void reset();
btScalar solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
unsigned long btRand2(); 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); normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
new (&m_jacLin[i]) btJacobianEntry( new (&m_jacLin[i]) btJacobianEntry(
rbA.getCenterOfMassTransform().getBasis().transpose(),
rbB.getCenterOfMassTransform().getBasis().transpose(),
m_relPosA, m_relPosA,
m_relPosB, m_relPosB,
normalWorld, 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 //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); invInertiaBDiag,invMassB);
btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, btJacobianEntry jacB(rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB); invInertiaBDiag,invMassB);
//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); //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 //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); invInertiaBDiag,invMassB);
btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, btJacobianEntry jacB(rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB); invInertiaBDiag,invMassB);
//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); //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/btAlignedAllocator.h"
#include "LinearMath/btTransformUtil.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. ///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
ATTRIBUTE_ALIGNED16 (struct) btSolverBody ATTRIBUTE_ALIGNED16 (struct) btSolverBody
@@ -33,6 +34,10 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
btVector3 m_angularVelocity; btVector3 m_angularVelocity;
btVector3 m_linearVelocity; btVector3 m_linearVelocity;
btVector3 m_deltaLinearVelocity;
btVector3 m_deltaAngularVelocity;
btVector3 m_centerOfMassPosition; btVector3 m_centerOfMassPosition;
btVector3 m_pushVelocity; btVector3 m_pushVelocity;
btVector3 m_turnVelocity; 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 //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_linearVelocity += linearComponent*impulseMagnitude;
m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); 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) SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
{ {
if (m_invMass) //if (m_invMass)
{ {
m_pushVelocity += linearComponent*impulseMagnitude; m_pushVelocity += linearComponent*impulseMagnitude;
m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor); 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 #endif //BT_SOLVER_BODY_H

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

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

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

@@ -113,7 +113,6 @@ void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
btRigidBody* body = btRigidBody::upcast(colObj); btRigidBody* body = btRigidBody::upcast(colObj);
if (body) if (body)
{ {
btTransform predictedTrans;
if (body->getActivationState() != ISLAND_SLEEPING) if (body->getActivationState() != ISLAND_SLEEPING)
{ {
if (body->isKinematicObject()) if (body->isKinematicObject())

8
src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.cpp
View File

@@ -449,10 +449,10 @@ btScalar solveContact(btSolverBody& body1,btSolverBody& body2,const btSolverCons
normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse; normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass, body1.applyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
contactConstraint.m_angularComponentA,normalImpulse); contactConstraint.m_angularComponentA,normalImpulse);
body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass, body2.applyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
contactConstraint.m_angularComponentB,-normalImpulse); contactConstraint.m_angularComponentB,-normalImpulse);
} }
@@ -1138,9 +1138,9 @@ void processSolverTask(void* userPtr, void* lsMemory)
if (constraint.m_appliedImpulse!= 0.f) if (constraint.m_appliedImpulse!= 0.f)
{ {
if (solverBodyA) if (solverBodyA)
solverBodyA->internalApplyImpulse(constraint.m_contactNormal*rb0readonly->getInvMass(),constraint.m_angularComponentA,constraint.m_appliedImpulse); solverBodyA->applyImpulse(constraint.m_contactNormal*rb0readonly->getInvMass(),constraint.m_angularComponentA,constraint.m_appliedImpulse);
if (solverBodyB) if (solverBodyB)
solverBodyB->internalApplyImpulse(constraint.m_contactNormal*rb1readonly->getInvMass(),constraint.m_angularComponentB,-constraint.m_appliedImpulse); solverBodyB->applyImpulse(constraint.m_contactNormal*rb1readonly->getInvMass(),constraint.m_angularComponentB,-constraint.m_appliedImpulse);
} }