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

94
Extras/quickstep/btOdeContactJoint.cpp
View File

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

8
Extras/quickstep/btOdeJoint.h
View File

@@ -44,7 +44,7 @@ public:
struct Info1 {
int m,nub;
int m_numConstraintRows,nub;
};
// info returned by getInfo2 function
@@ -58,7 +58,7 @@ public:
// 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
// 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
int rowskip;
@@ -66,10 +66,10 @@ public:
// 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
// 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).
btScalar *lo,*hi;
btScalar *m_lowerLimit,*m_higherLimit;
// findex vector for variables. see the LCP solver interface for a
// 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
btOdeQuickstepConstraintSolver::btOdeQuickstepConstraintSolver():
m_cfm(0.f),//1e-5f),
m_erp(0.4f)
btOdeQuickstepConstraintSolver::btOdeQuickstepConstraintSolver()
{
}
@@ -124,7 +122,7 @@ btScalar btOdeQuickstepConstraintSolver::solveGroup(btCollisionObject** /*bodies
// 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
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());
*/
int bodyId0 = _bodyId0,bodyId1 = _bodyId1;

17
Extras/quickstep/btOdeQuickstepConstraintSolver.h
View File

@@ -40,9 +40,6 @@ private:
int m_CurJoint;
int m_CurTypedJoint;
float m_cfm;
float m_erp;
btSorLcpSolver m_SorLcpSolver;
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);
///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
void reset()
{

236
Extras/quickstep/btOdeTypedJoint.cpp
View File

@@ -80,7 +80,7 @@ OdeP2PJoint::OdeP2PJoint(
void OdeP2PJoint::GetInfo1(Info1 *info)
{
info->m = 3;
info->m_numConstraintRows = 3;
info->nub = 3;
}
@@ -133,24 +133,24 @@ void OdeP2PJoint::GetInfo2(Info2 *info)
int s = info->rowskip;
// set jacobian
info->J1l[0] = 1;
info->J1l[s+1] = 1;
info->J1l[2*s+2] = 1;
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[s+1] = 1;
info->m_J1linearAxis[2*s+2] = 1;
btVector3 a1,a2;
a1 = body0_trans.getBasis()*p2pconstraint->getPivotInA();
//dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA);
dCROSSMAT (info->J1a,a1,s,-,+);
dCROSSMAT (info->m_J1angularAxis,a1,s,-,+);
if (m_body1)
{
info->J2l[0] = -1;
info->J2l[s+1] = -1;
info->J2l[2*s+2] = -1;
info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[s+1] = -1;
info->m_J2linearAxis[2*s+2] = -1;
a2 = body1_trans.getBasis()*p2pconstraint->getPivotInB();
//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++)
{
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]);
}
}
@@ -168,7 +168,7 @@ void OdeP2PJoint::GetInfo2(Info2 *info)
{
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]);
}
}
@@ -193,8 +193,8 @@ int bt_get_limit_motor_info2(
if (powered || limit)
{
btScalar *J1 = rotational ? info->J1a : info->J1l;
btScalar *J2 = rotational ? info->J2a : info->J2l;
btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
btScalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
J1[srow+0] = ax1[0];
J1[srow+1] = ax1[1];
@@ -232,12 +232,12 @@ int bt_get_limit_motor_info2(
ltd = c.cross(ax1);
info->J1a[srow+0] = ltd[0];
info->J1a[srow+1] = ltd[1];
info->J1a[srow+2] = ltd[2];
info->J2a[srow+0] = ltd[0];
info->J2a[srow+1] = ltd[1];
info->J2a[srow+2] = ltd[2];
info->m_J1angularAxis[srow+0] = ltd[0];
info->m_J1angularAxis[srow+1] = ltd[1];
info->m_J1angularAxis[srow+2] = ltd[2];
info->m_J2angularAxis[srow+0] = ltd[0];
info->m_J2angularAxis[srow+1] = ltd[1];
info->m_J2angularAxis[srow+2] = ltd[2];
}
// 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;
if (! limit)
{
info->c[row] = limot->m_targetVelocity;
info->lo[row] = -limot->m_maxMotorForce;
info->hi[row] = limot->m_maxMotorForce;
info->m_constraintError[row] = limot->m_targetVelocity;
info->m_lowerLimit[row] = -limot->m_maxMotorForce;
info->m_higherLimit[row] = limot->m_maxMotorForce;
}
}
if (limit)
{
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;
if (limot->m_loLimit == limot->m_hiLimit)
{
// limited low and high simultaneously
info->lo[row] = -dInfinity;
info->hi[row] = dInfinity;
info->m_lowerLimit[row] = -dInfinity;
info->m_higherLimit[row] = dInfinity;
}
else
{
if (limit == 1)
{
// low limit
info->lo[row] = 0;
info->hi[row] = SIMD_INFINITY;
info->m_lowerLimit[row] = 0;
info->m_higherLimit[row] = SIMD_INFINITY;
}
else
{
// high limit
info->lo[row] = -SIMD_INFINITY;
info->hi[row] = 0;
info->m_lowerLimit[row] = -SIMD_INFINITY;
info->m_higherLimit[row] = 0;
}
// deal with bounce
@@ -309,7 +309,8 @@ int bt_get_limit_motor_info2(
if (vel < 0)
{
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
@@ -318,7 +319,8 @@ int bt_get_limit_motor_info2(
if (vel > 0)
{
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();
//prepare constraint
d6constraint->calculateTransforms();
info->m = 3;
info->m_numConstraintRows = 3;
info->nub = 3;
//test angular limits
@@ -358,7 +360,7 @@ void OdeD6Joint::GetInfo1(Info1 *info)
//if(i==2) continue;
if(d6constraint->testAngularLimitMotor(i))
{
info->m++;
info->m_numConstraintRows++;
}
}
@@ -390,25 +392,25 @@ int OdeD6Joint::setLinearLimits(Info2 *info)
int s = info->rowskip;
// set jacobian
info->J1l[0] = 1;
info->J1l[s+1] = 1;
info->J1l[2*s+2] = 1;
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[s+1] = 1;
info->m_J1linearAxis[2*s+2] = 1;
btVector3 a1,a2;
a1 = body0_trans.getBasis()*d6constraint->getFrameOffsetA().getOrigin();
//dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA);
dCROSSMAT (info->J1a,a1,s,-,+);
dCROSSMAT (info->m_J1angularAxis,a1,s,-,+);
if (m_body1)
{
info->J2l[0] = -1;
info->J2l[s+1] = -1;
info->J2l[2*s+2] = -1;
info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[s+1] = -1;
info->m_J2linearAxis[2*s+2] = -1;
a2 = body1_trans.getBasis()*d6constraint->getFrameOffsetB().getOrigin();
//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++)
{
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]);
}
}
@@ -426,7 +428,7 @@ int OdeD6Joint::setLinearLimits(Info2 *info)
{
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]);
}
}
@@ -485,19 +487,19 @@ OdeSliderJoint::OdeSliderJoint(
void OdeSliderJoint::GetInfo1(Info1* info)
{
info->nub = 4;
info->m = 4; // Fixed 2 linear + 2 angular
info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
btSliderConstraint * slider = this->getSliderConstraint();
//prepare constraint
slider->calculateTransforms();
slider->testLinLimits();
if(slider->getSolveLinLimit() || slider->getPoweredLinMotor())
{
info->m++; // limit 3rd linear as well
info->m_numConstraintRows++; // limit 3rd linear as well
}
slider->testAngLimits();
if(slider->getSolveAngLimit() || slider->getPoweredAngMotor())
{
info->m++; // limit 3rd angular as well
info->m_numConstraintRows++; // limit 3rd angular as well
}
} // OdeSliderJoint::GetInfo1()
@@ -523,20 +525,20 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
btVector3 p = trA.getBasis().getColumn(1);
btVector3 q = trA.getBasis().getColumn(2);
// set the two slider rows
info->J1a[0] = p[0];
info->J1a[1] = p[1];
info->J1a[2] = p[2];
info->J1a[s+0] = q[0];
info->J1a[s+1] = q[1];
info->J1a[s+2] = q[2];
info->m_J1angularAxis[0] = p[0];
info->m_J1angularAxis[1] = p[1];
info->m_J1angularAxis[2] = p[2];
info->m_J1angularAxis[s+0] = q[0];
info->m_J1angularAxis[s+1] = q[1];
info->m_J1angularAxis[s+2] = q[2];
if(m_body1)
{
info->J2a[0] = -p[0];
info->J2a[1] = -p[1];
info->J2a[2] = -p[2];
info->J2a[s+0] = -q[0];
info->J2a[s+1] = -q[1];
info->J2a[s+2] = -q[2];
info->m_J2angularAxis[0] = -p[0];
info->m_J2angularAxis[1] = -p[1];
info->m_J2angularAxis[2] = -p[2];
info->m_J2angularAxis[s+0] = -q[0];
info->m_J2angularAxis[s+1] = -q[1];
info->m_J2angularAxis[s+2] = -q[2];
}
// compute the right hand side of the constraint equation. set relative
// 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);
}
info->c[0] = k * u.dot(p);
info->c[1] = k * u.dot(q);
info->m_constraintError[0] = k * u.dot(p);
info->m_constraintError[1] = k * u.dot(q);
// pull out pos and R for both bodies. also get the connection
// vector c = pos2-pos1.
// 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();
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->J2a[s2+i] = tmp[i];
for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmp[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = tmp[i];
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->J2a[s3+i] = tmp[i];
for (i=0; i<3; i++) info->m_J1angularAxis[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->J2l[s3+i] = -q[i];
for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[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->J1l[s3+i] = q[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[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
// point (in body 2's frame) with the center of body 1.
btVector3 ofs; // offset point in global coordinates
@@ -610,8 +612,8 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
ofs = trA.getOrigin() - trB.getOrigin();
}
k = info->fps * info->erp * slider->getSoftnessOrthoLin();
info->c[2] = k * p.dot(ofs);
info->c[3] = k * q.dot(ofs);
info->m_constraintError[2] = k * p.dot(ofs);
info->m_constraintError[3] = k * q.dot(ofs);
int nrow = 3; // last filled row
int srow;
// check linear limits linear
@@ -639,14 +641,14 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
{
nrow++;
srow = nrow * info->rowskip;
info->J1l[srow+0] = ax1[0];
info->J1l[srow+1] = ax1[1];
info->J1l[srow+2] = ax1[2];
info->m_J1linearAxis[srow+0] = ax1[0];
info->m_J1linearAxis[srow+1] = ax1[1];
info->m_J1linearAxis[srow+2] = ax1[2];
if(m_body1)
{
info->J2l[srow+0] = -ax1[0];
info->J2l[srow+1] = -ax1[1];
info->J2l[srow+2] = -ax1[2];
info->m_J2linearAxis[srow+0] = -ax1[0];
info->m_J2linearAxis[srow+1] = -ax1[1];
info->m_J2linearAxis[srow+2] = -ax1[2];
}
// linear torque decoupling step:
//
@@ -664,12 +666,12 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
dVector3 ltd; // Linear Torque Decoupling vector (a torque)
c = btScalar(0.5) * c;
dCROSS (ltd,=,c,ax1);
info->J1a[srow+0] = ltd[0];
info->J1a[srow+1] = ltd[1];
info->J1a[srow+2] = ltd[2];
info->J2a[srow+0] = ltd[0];
info->J2a[srow+1] = ltd[1];
info->J2a[srow+2] = ltd[2];
info->m_J1angularAxis[srow+0] = ltd[0];
info->m_J1angularAxis[srow+1] = ltd[1];
info->m_J1angularAxis[srow+2] = ltd[2];
info->m_J2angularAxis[srow+0] = ltd[0];
info->m_J2angularAxis[srow+1] = ltd[1];
info->m_J2angularAxis[srow+2] = ltd[2];
}
// right-hand part
btScalar lostop = slider->getLowerLinLimit();
@@ -683,9 +685,9 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
info->cfm[nrow] = btScalar(0.0);
if(!limit)
{
info->c[nrow] = slider->getTargetLinMotorVelocity();
info->lo[nrow] = -slider->getMaxLinMotorForce() * info->fps;
info->hi[nrow] = slider->getMaxLinMotorForce() * info->fps;
info->m_constraintError[nrow] = slider->getTargetLinMotorVelocity();
info->m_lowerLimit[nrow] = -slider->getMaxLinMotorForce() * info->fps;
info->m_higherLimit[nrow] = slider->getMaxLinMotorForce() * info->fps;
}
}
if(limit)
@@ -693,32 +695,32 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
k = info->fps * info->erp;
if(m_body1)
{
info->c[nrow] = k * limit_err;
info->m_constraintError[nrow] = k * limit_err;
}
else
{
info->c[nrow] = - k * limit_err;
info->m_constraintError[nrow] = - k * limit_err;
}
info->cfm[nrow] = btScalar(0.0); // stop_cfm;
if(lostop == histop)
{
// limited low and high simultaneously
info->lo[nrow] = -SIMD_INFINITY;
info->hi[nrow] = SIMD_INFINITY;
info->m_lowerLimit[nrow] = -SIMD_INFINITY;
info->m_higherLimit[nrow] = SIMD_INFINITY;
}
else
{
if(limit == 1)
{
// low limit
info->lo[nrow] = 0;
info->hi[nrow] = SIMD_INFINITY;
info->m_lowerLimit[nrow] = 0;
info->m_higherLimit[nrow] = SIMD_INFINITY;
}
else
{
// high limit
info->lo[nrow] = -SIMD_INFINITY;
info->hi[nrow] = 0;
info->m_lowerLimit[nrow] = -SIMD_INFINITY;
info->m_higherLimit[nrow] = 0;
}
}
// 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)
{
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
@@ -747,11 +750,12 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
if(vel > 0)
{
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 linear limit
// check angular limits
@@ -779,14 +783,14 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
{
nrow++;
srow = nrow * info->rowskip;
info->J1a[srow+0] = ax1[0];
info->J1a[srow+1] = ax1[1];
info->J1a[srow+2] = ax1[2];
info->m_J1angularAxis[srow+0] = ax1[0];
info->m_J1angularAxis[srow+1] = ax1[1];
info->m_J1angularAxis[srow+2] = ax1[2];
if(m_body1)
{
info->J2a[srow+0] = -ax1[0];
info->J2a[srow+1] = -ax1[1];
info->J2a[srow+2] = -ax1[2];
info->m_J2angularAxis[srow+0] = -ax1[0];
info->m_J2angularAxis[srow+1] = -ax1[1];
info->m_J2angularAxis[srow+2] = -ax1[2];
}
btScalar lostop = slider->getLowerAngLimit();
btScalar histop = slider->getUpperAngLimit();
@@ -799,9 +803,9 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
info->cfm[nrow] = btScalar(0.0);
if(!limit)
{
info->c[nrow] = slider->getTargetAngMotorVelocity();
info->lo[nrow] = -slider->getMaxAngMotorForce() * info->fps;
info->hi[nrow] = slider->getMaxAngMotorForce() * info->fps;
info->m_constraintError[nrow] = slider->getTargetAngMotorVelocity();
info->m_lowerLimit[nrow] = -slider->getMaxAngMotorForce() * info->fps;
info->m_higherLimit[nrow] = slider->getMaxAngMotorForce() * info->fps;
}
}
if(limit)
@@ -809,32 +813,32 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
k = info->fps * info->erp;
if (m_body1)
{
info->c[nrow] = k * limit_err;
info->m_constraintError[nrow] = k * limit_err;
}
else
{
info->c[nrow] = -k * limit_err;
info->m_constraintError[nrow] = -k * limit_err;
}
info->cfm[nrow] = btScalar(0.0); // stop_cfm;
if(lostop == histop)
{
// limited low and high simultaneously
info->lo[nrow] = -SIMD_INFINITY;
info->hi[nrow] = SIMD_INFINITY;
info->m_lowerLimit[nrow] = -SIMD_INFINITY;
info->m_higherLimit[nrow] = SIMD_INFINITY;
}
else
{
if (limit == 1)
{
// low limit
info->lo[nrow] = 0;
info->hi[nrow] = SIMD_INFINITY;
info->m_lowerLimit[nrow] = 0;
info->m_higherLimit[nrow] = SIMD_INFINITY;
}
else
{
// high limit
info->lo[nrow] = -SIMD_INFINITY;
info->hi[nrow] = 0;
info->m_lowerLimit[nrow] = -SIMD_INFINITY;
info->m_higherLimit[nrow] = 0;
}
}
// 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)
{
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
@@ -863,11 +867,11 @@ void OdeSliderJoint::GetInfo2(Info2 *info)
if(vel > 0)
{
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 angular limit or powered
} // 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
// 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
// compute iMJ = inv(M)*J'
inline void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb,
// various common computations involving the matrix JconstraintAxis
// compute iMJ = inv(M)*JconstraintAxis'
inline void compute_invM_JT (int numConstraintRows, dRealMutablePtr JconstraintAxis, dRealMutablePtr iMJ, int *jb,
//OdeSolverBody* const *body,
const btAlignedObjectArray<btOdeSolverBody*> &body,
dRealPtr invI)
dRealPtr inverseInertiaWorld)
{
int i,j;
dRealMutablePtr iMJ_ptr = iMJ;
dRealMutablePtr J_ptr = J;
for (i=0; i<m; i++) {
dRealMutablePtr J_ptr = JconstraintAxis;
for (i=0; i<numConstraintRows; i++) {
int b1 = jb[i*2];
int b2 = jb[i*2+1];
btScalar k = body[b1]->m_invMass;
for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j];
dMULTIPLY0_331 (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3);
for (j=0; j<3; j++)
iMJ_ptr[j] = body[b1]->m_invMass*J_ptr[j];
dMULTIPLY0_331 (iMJ_ptr + 3, inverseInertiaWorld + 12*b1, J_ptr + 3);
if (b2 >= 0) {
k = body[b2]->m_invMass;
for (j=0; j<3; j++) iMJ_ptr[j+6] = k*J_ptr[j+6];
dMULTIPLY0_331 (iMJ_ptr + 9, invI + 12*b2, J_ptr + 9);
for (j=0; j<3; j++)
iMJ_ptr[j+6] = body[b2]->m_invMass*J_ptr[j+6];//inv mass * constraint (normal) axis
dMULTIPLY0_331 (iMJ_ptr + 9, inverseInertiaWorld + 12*b2, J_ptr + 9);//inverse inertia world * constraint (normal) axis
}
J_ptr += 12;
iMJ_ptr += 12;
@@ -95,7 +96,7 @@ inline void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int
}
#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)
{
int i,j;
@@ -116,7 +117,7 @@ static void multiply_invM_JTSpecial (int m, int nb, dRealMutablePtr iMJ, int *jb
}
dRealPtr iMJ_ptr = iMJ;
for (i=0; i<m; i++) {
for (i=0; i<numConstraintRows; i++) {
int b1 = jb[i*2];
@@ -147,16 +148,16 @@ static void multiply_invM_JTSpecial (int m, int nb, dRealMutablePtr iMJ, int *jb
#endif
// compute out = inv(M)*J'*in.
// compute out = inv(M)*JconstraintAxis'*in.
#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)
{
int i,j;
dSetZero1 (out,6*nb);
dRealPtr iMJ_ptr = iMJ;
for (i=0; i<m; i++) {
for (i=0; i<numConstraintRows; i++) {
int b1 = jb[i*2];
int b2 = jb[i*2+1];
dRealMutablePtr out_ptr = out + b1*6;
@@ -173,13 +174,13 @@ static void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb,
#endif
// compute out = J*in.
inline void multiply_J (int m, dRealMutablePtr J, int *jb,
// compute out = JconstraintAxis*in.
inline void multiply_J (int numConstraintRows, dRealMutablePtr JconstraintAxis, int *jb,
dRealMutablePtr in, dRealMutablePtr out)
{
int i,j;
dRealPtr J_ptr = J;
for (i=0; i<m; i++) {
dRealPtr J_ptr = JconstraintAxis;
for (i=0; i<numConstraintRows; i++) {
int b1 = jb[i*2];
int b2 = jb[i*2+1];
btScalar sum = 0;
@@ -199,14 +200,14 @@ inline void multiply_J (int m, dRealMutablePtr J, int *jb,
// SOR-LCP method
// 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
// 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).
// note: fc is returned as inv(M)*J'*lambda, the constraint force is actually J'*lambda
// this returns lambdaAccumulatedImpulse and fc (the constraint force).
// 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 {
@@ -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,
dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs,
dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
dRealPtr inverseInertiaWorld, dRealMutablePtr lambdaAccumulatedImpulse, dRealMutablePtr invMforce, dRealMutablePtr rhs,
dRealMutablePtr lowerLimit, dRealMutablePtr higherLimit, dRealPtr cfm, int *findex,
int numiter,float overRelax,
btStackAlloc* stackAlloc
)
@@ -237,49 +238,51 @@ void btSorLcpSolver::SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb,
#ifdef WARM_STARTING
// for warm starting, this seems to be necessary to prevent
// 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
dSetZero1 (lambda,m);
dSetZero1 (lambdaAccumulatedImpulse,numConstraintRows);
#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.
dRealAllocaArray (last_lambda,m);
dRealAllocaArray (last_lambda,numConstraintRows);
// a copy of the 'hi' vector in case findex[] is being used
dRealAllocaArray (hicopy,m);
memcpy (hicopy,hi,m*sizeof(float));
// a copy of the 'higherLimit' vector in case findex[] is being used
dRealAllocaArray (hicopy,numConstraintRows);
memcpy (hicopy,higherLimit,numConstraintRows*sizeof(float));
// precompute iMJ = inv(M)*J'
dRealAllocaArray (iMJ,m*12);
compute_invM_JT (m,J,iMJ,jb,body,invI);
// precompute iMJ = inv(M)*JconstraintAxis'
dRealAllocaArray (iMJ,numConstraintRows*12);
compute_invM_JT (numConstraintRows,JconstraintAxis,iMJ,jb,body,inverseInertiaWorld);
// compute fc=(inv(M)*J')*lambda. we will incrementally maintain fc
// as we change lambda.
// compute fc=(inv(M)*JconstraintAxis')*lambdaAccumulatedImpulse. we will incrementally maintain fc
// as we change lambdaAccumulatedImpulse.
#ifdef WARM_STARTING
multiply_invM_JT (m,nb,iMJ,jb,lambda,fc);
multiply_invM_JT (numConstraintRows,nb,iMJ,jb,lambdaAccumulatedImpulse,fc);
#else
dSetZero1 (invMforce,nb*6);
#endif
// precompute 1 / diagonals of A
dRealAllocaArray (Ad,m);
dRealAllocaArray (Ad,numConstraintRows);
dRealPtr iMJ_ptr = iMJ;
dRealMutablePtr J_ptr = J;
for (i=0; i<m; i++) {
dRealMutablePtr J_ptr = JconstraintAxis;
for (i=0; i<numConstraintRows; i++) {
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) {
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;
J_ptr += 12;
Ad[i] = sor_w / sum;//(sum + cfm[i]);
}
// scale J and b by Ad
J_ptr = J;
for (i=0; i<m; i++) {
// scale JconstraintAxis and b by Ad
J_ptr = JconstraintAxis;
for (i=0; i<numConstraintRows; i++) {
for (j=0; j<12; j++) {
J_ptr[0] *= Ad[i];
J_ptr++;
@@ -288,24 +291,24 @@ void btSorLcpSolver::SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb,
}
// scale Ad by CFM
for (i=0; i<m; i++)
for (i=0; i<numConstraintRows; i++)
Ad[i] *= cfm[i];
// order to solve constraint rows in
//IndexError *order = (IndexError*) alloca (m*sizeof(IndexError));
IndexError *order = (IndexError*) ALLOCA (m*sizeof(IndexError));
//IndexError *order = (IndexError*) alloca (numConstraintRows*sizeof(IndexError));
IndexError *order = (IndexError*) ALLOCA (numConstraintRows*sizeof(IndexError));
#ifndef REORDER_CONSTRAINTS
// make sure constraints with findex < 0 come first.
j=0;
for (i=0; i<m; i++)
for (i=0; i<numConstraintRows; i++)
if (findex[i] < 0)
order[j++].index = i;
for (i=0; i<m; i++)
for (i=0; i<numConstraintRows; i++)
if (findex[i] >= 0)
order[j++].index = i;
dIASSERT (j==m);
dIASSERT (j==numConstraintRows);
#endif
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) {
// for the first two iterations, solve the constraints in
// the given order
for (i=0; i<m; i++) {
for (i=0; i<numConstraintRows; i++) {
order[i].error = i;
order[i].findex = findex[i];
order[i].index = i;
@@ -324,13 +327,13 @@ void btSorLcpSolver::SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb,
else {
// sort the constraints so that the ones converging slowest
// get solved last. use the absolute (not relative) error.
for (i=0; i<m; i++) {
float v1 = dFabs (lambda[i]);
for (i=0; i<numConstraintRows; i++) {
float v1 = dFabs (lambdaAccumulatedImpulse[i]);
float v2 = dFabs (last_lambda[i]);
float max = (v1 > v2) ? v1 : v2;
if (max > 0) {
//@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max;
order[i].error = dFabs(lambda[i]-last_lambda[i]);
//@@@ relative error: order[i].error = dFabs(lambdaAccumulatedImpulse[i]-last_lambda[i])/max;
order[i].error = dFabs(lambdaAccumulatedImpulse[i]-last_lambda[i]);
}
else {
order[i].error = dInfinity;
@@ -339,11 +342,11 @@ void btSorLcpSolver::SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb,
order[i].index = i;
}
}
qsort (order,m,sizeof(IndexError),&compare_index_error);
qsort (order,numConstraintRows,sizeof(IndexError),&compare_index_error);
#endif
#ifdef RANDOMLY_REORDER_CONSTRAINTS
if ((iteration & 7) == 0) {
for (i=1; i<m; ++i) {
for (i=1; i<numConstraintRows; ++i) {
IndexError tmp = order[i];
int swapi = dRandInt2(i+1);
order[i] = order[swapi];
@@ -352,19 +355,19 @@ void btSorLcpSolver::SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb,
}
#endif
//@@@ potential optimization: swap lambda and last_lambda pointers rather
// than copying the data. we must make sure lambda is properly
//@@@ potential optimization: swap lambdaAccumulatedImpulse and last_lambda pointers rather
// than copying the data. we must make sure lambdaAccumulatedImpulse is properly
// 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++) {
// @@@ potential optimization: we could pre-sort J and iMJ, thereby
for (int i=0; i<numConstraintRows; i++) {
// @@@ potential optimization: we could pre-sort JconstraintAxis and iMJ, thereby
// linearizing access to those arrays. hmmm, this does not seem
// like a win, but we should think carefully about our memory
// access pattern.
int index = order[i].index;
J_ptr = J + index*12;
J_ptr = JconstraintAxis + index*12;
iMJ_ptr = iMJ + index*12;
// 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
// limit adjustment once per time step, whereas this method performs
// 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.
if (findex[index] >= 0) {
hi[index] = btFabs (hicopy[index] * lambda[findex[index]]);
lo[index] = -hi[index];
higherLimit[index] = btFabs (hicopy[index] * lambdaAccumulatedImpulse[findex[index]]);
lowerLimit[index] = -higherLimit[index];
}
int b1 = jb[index*2];
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
delta -=fc_ptr[0] * J_ptr[0] + fc_ptr[1] * J_ptr[1] +
fc_ptr[2] * J_ptr[2] + fc_ptr[3] * J_ptr[3] +
fc_ptr[4] * J_ptr[4] + fc_ptr[5] * J_ptr[5];
deltaAppliedImpulse -=deltaVelocity[0] * J_ptr[0] + deltaVelocity[1] * J_ptr[1] +
deltaVelocity[2] * J_ptr[2] + deltaVelocity[3] * J_ptr[3] +
deltaVelocity[4] * J_ptr[4] + deltaVelocity[5] * J_ptr[5];
// @@@ potential optimization: handle 1-body constraints in a separate
// loop to avoid the cost of test & jump?
if (b2 >= 0) {
fc_ptr = invMforce + 6*b2;
delta -=fc_ptr[0] * J_ptr[6] + fc_ptr[1] * J_ptr[7] +
fc_ptr[2] * J_ptr[8] + fc_ptr[3] * J_ptr[9] +
fc_ptr[4] * J_ptr[10] + fc_ptr[5] * J_ptr[11];
deltaVelocity = invMforce + 6*b2;
deltaAppliedImpulse -=deltaVelocity[0] * J_ptr[6] + deltaVelocity[1] * J_ptr[7] +
deltaVelocity[2] * J_ptr[8] + deltaVelocity[3] * J_ptr[9] +
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
// to save test+jump penalties here?
float new_lambda = lambda[index] + delta;
if (new_lambda < lo[index]) {
delta = lo[index]-lambda[index];
lambda[index] = lo[index];
float sum = lambdaAccumulatedImpulse[index] + deltaAppliedImpulse;
if (sum < lowerLimit[index]) {
deltaAppliedImpulse = lowerLimit[index]-lambdaAccumulatedImpulse[index];
lambdaAccumulatedImpulse[index] = lowerLimit[index];
}
else if (new_lambda > hi[index]) {
delta = hi[index]-lambda[index];
lambda[index] = hi[index];
else if (sum > higherLimit[index]) {
deltaAppliedImpulse = higherLimit[index]-lambdaAccumulatedImpulse[index];
lambdaAccumulatedImpulse[index] = higherLimit[index];
}
else {
lambda[index] = new_lambda;
lambdaAccumulatedImpulse[index] = sum;
}
//@@@ a trick that may or may not help
//float ramp = (1-((float)(iteration+1)/(float)num_iterations));
//delta *= ramp;
//deltaAppliedImpulse *= ramp;
// update invMforce.
// @@@ potential optimization: SIMD for this and the b2 >= 0 case
fc_ptr = invMforce + 6*b1;
fc_ptr[0] += delta * iMJ_ptr[0];
fc_ptr[1] += delta * iMJ_ptr[1];
fc_ptr[2] += delta * iMJ_ptr[2];
fc_ptr[3] += delta * iMJ_ptr[3];
fc_ptr[4] += delta * iMJ_ptr[4];
fc_ptr[5] += delta * iMJ_ptr[5];
deltaVelocity = invMforce + 6*b1;
deltaVelocity[0] += deltaAppliedImpulse * iMJ_ptr[0];
deltaVelocity[1] += deltaAppliedImpulse * iMJ_ptr[1];
deltaVelocity[2] += deltaAppliedImpulse * iMJ_ptr[2];
deltaVelocity[3] += deltaAppliedImpulse * iMJ_ptr[3];
deltaVelocity[4] += deltaAppliedImpulse * iMJ_ptr[4];
deltaVelocity[5] += deltaAppliedImpulse * iMJ_ptr[5];
// @@@ potential optimization: handle 1-body constraints in a separate
// loop to avoid the cost of test & jump?
if (b2 >= 0) {
fc_ptr = invMforce + 6*b2;
fc_ptr[0] += delta * iMJ_ptr[6];
fc_ptr[1] += delta * iMJ_ptr[7];
fc_ptr[2] += delta * iMJ_ptr[8];
fc_ptr[3] += delta * iMJ_ptr[9];
fc_ptr[4] += delta * iMJ_ptr[10];
fc_ptr[5] += delta * iMJ_ptr[11];
deltaVelocity = invMforce + 6*b2;
deltaVelocity[0] += deltaAppliedImpulse * iMJ_ptr[6];
deltaVelocity[1] += deltaAppliedImpulse * iMJ_ptr[7];
deltaVelocity[2] += deltaAppliedImpulse * iMJ_ptr[8];
deltaVelocity[3] += deltaAppliedImpulse * iMJ_ptr[9];
deltaVelocity[4] += deltaAppliedImpulse * iMJ_ptr[10];
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 (
float global_cfm,
float global_erp,
const btAlignedObjectArray<btOdeSolverBody*> &body, int nb,
void btSorLcpSolver::SolveInternal1 (const btAlignedObjectArray<btOdeSolverBody*> &body, int nb,
btAlignedObjectArray<btOdeJoint*> &joint,
int nj, const btContactSolverInfo& solverInfo,
btStackAlloc* stackAlloc)
@@ -475,16 +477,16 @@ void btSorLcpSolver::SolveInternal1 (
// 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
// 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 (invI,3*4*nb);
dRealAllocaArray (inverseInertiaWorld,3*4*nb);
/* for (i=0; i<nb; i++) {
dMatrix3 tmp;
// compute inertia tensor in global frame
dMULTIPLY2_333 (tmp,body[i]->m_I,body[i]->m_R);
// compute inverse inertia tensor in global frame
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
dCROSS (body[i]->m_tacc,-=,body[i]->getAngularVelocity(),tmp);
}
@@ -497,7 +499,7 @@ void btSorLcpSolver::SolveInternal1 (
// compute inverse inertia tensor in global frame
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
// dMULTIPLY0_331 (tmp,I+i*12,body[i]->m_angularVelocity);
// 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).
// joints with m=0 are inactive and are removed from the joints array
// get joint information (numConstraintRows = total constraint dimension, nub = number of unbounded variables).
// joints with numConstraintRows=0 are inactive and are removed from the joints array
// entirely, so that the code that follows does not consider them.
//@@@ do we really need to save all the info1's
btOdeJoint::Info1 *info = (btOdeJoint::Info1*) ALLOCA (nj*sizeof(btOdeJoint::Info1));
for (i=0, j=0; j<nj; j++) { // i=dest, j=src
joint[j]->GetInfo1 (info+i);
dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m);
if (info[i].m > 0) {
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_numConstraintRows > 0) {
joint[i] = joint[j];
i++;
}
@@ -523,34 +525,34 @@ void btSorLcpSolver::SolveInternal1 (
nj = i;
// create the row offset array
int m = 0;
int *ofs = (int*) ALLOCA (nj*sizeof(int));
int numConstraintRows = 0;
int *constraintRowOffsets = (int*) ALLOCA (nj*sizeof(int));
for (i=0; i<nj; i++) {
ofs[i] = m;
m += info[i].m;
constraintRowOffsets[i] = numConstraintRows;
numConstraintRows += info[i].m_numConstraintRows;
}
// if there are constraints, compute the constraint force
dRealAllocaArray (J,m*12);
int *jb = (int*) ALLOCA (m*2*sizeof(int));
if (m > 0) {
dRealAllocaArray (JconstraintAxis,numConstraintRows*12);
int *jb = (int*) ALLOCA (numConstraintRows*2*sizeof(int));
if (numConstraintRows > 0) {
// create a constraint equation right hand side vector `c', a constraint
// force mixing vector `cfm', and LCP low and high bound vectors, and an
// 'findex' vector.
dRealAllocaArray (c,m);
dRealAllocaArray (cfm,m);
dRealAllocaArray (lo,m);
dRealAllocaArray (hi,m);
dRealAllocaArray (c_rhs,numConstraintRows);
dRealAllocaArray (cfm,numConstraintRows);
dRealAllocaArray (lowerLimit,numConstraintRows);
dRealAllocaArray (higherLimit,numConstraintRows);
int *findex = (int*) ALLOCA (m*sizeof(int));
int *findex = (int*) ALLOCA (numConstraintRows*sizeof(int));
dSetZero1 (c,m);
dSetValue1 (cfm,m,global_cfm);
dSetValue1 (lo,m,-dInfinity);
dSetValue1 (hi,m, dInfinity);
for (i=0; i<m; i++) findex[i] = -1;
dSetZero1 (c_rhs,numConstraintRows);
dSetValue1 (cfm,numConstraintRows,solverInfo.m_globalCfm);
dSetValue1 (lowerLimit,numConstraintRows,-dInfinity);
dSetValue1 (higherLimit,numConstraintRows, dInfinity);
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
// format:
//
@@ -563,30 +565,30 @@ void btSorLcpSolver::SolveInternal1 (
// (lll) = linear jacobian data
// (aaa) = angular jacobian data
//
dSetZero1 (J,m*12);
dSetZero1 (JconstraintAxis,numConstraintRows*12);
btOdeJoint::Info2 Jinfo;
Jinfo.rowskip = 12;
Jinfo.fps = stepsize1;
Jinfo.erp = global_erp;
Jinfo.erp = solverInfo.m_erp;
for (i=0; i<nj; i++) {
Jinfo.J1l = J + ofs[i]*12;
Jinfo.J1a = Jinfo.J1l + 3;
Jinfo.J2l = Jinfo.J1l + 6;
Jinfo.J2a = Jinfo.J1l + 9;
Jinfo.c = c + ofs[i];
Jinfo.cfm = cfm + ofs[i];
Jinfo.lo = lo + ofs[i];
Jinfo.hi = hi + ofs[i];
Jinfo.findex = findex + ofs[i];
Jinfo.m_J1linearAxis = JconstraintAxis + constraintRowOffsets[i]*12;
Jinfo.m_J1angularAxis = Jinfo.m_J1linearAxis + 3;
Jinfo.m_J2linearAxis = Jinfo.m_J1linearAxis + 6;
Jinfo.m_J2angularAxis = Jinfo.m_J1linearAxis + 9;
Jinfo.m_constraintError = c_rhs + constraintRowOffsets[i];
Jinfo.cfm = cfm + constraintRowOffsets[i];
Jinfo.m_lowerLimit = lowerLimit + constraintRowOffsets[i];
Jinfo.m_higherLimit = higherLimit + constraintRowOffsets[i];
Jinfo.findex = findex + constraintRowOffsets[i];
joint[i]->GetInfo2 (&Jinfo);
if (Jinfo.c[0] > solverInfo.m_maxErrorReduction)
Jinfo.c[0] = solverInfo.m_maxErrorReduction;
if (Jinfo.m_constraintError[0] > solverInfo.m_maxErrorReduction)
Jinfo.m_constraintError[0] = solverInfo.m_maxErrorReduction;
// adjust returned findex values for global index numbering
for (j=0; j<info[i].m; j++) {
if (findex[ofs[i] + j] >= 0)
findex[ofs[i] + j] += ofs[i];
for (j=0; j<info[i].m_numConstraintRows; j++) {
if (findex[constraintRowOffsets[i] + j] >= 0)
findex[constraintRowOffsets[i] + j] += constraintRowOffsets[i];
}
}
@@ -595,13 +597,13 @@ void btSorLcpSolver::SolveInternal1 (
for (i=0; i<nj; i++) {
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;
for (j=0; j<info[i].m; j++) {
for (j=0; j<info[i].m_numConstraintRows; j++) {
jb_ptr[0] = b1;
jb_ptr[1] = b2;
jb_ptr += 2;
}
}
dIASSERT (jb_ptr == jb+2*m);
dIASSERT (jb_ptr == jb+2*numConstraintRows);
// compute the right hand side `rhs'
dRealAllocaArray (tmp1,nb*6);
@@ -610,47 +612,48 @@ void btSorLcpSolver::SolveInternal1 (
btScalar body_invMass = body[i]->m_invMass;
for (j=0; j<3; j++)
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++)
tmp1[i*6+3+j] += body[i]->m_angularVelocity[j] * stepsize1;
}
// put J*tmp1 into rhs
dRealAllocaArray (rhs,m);
multiply_J (m,J,jb,tmp1,rhs);
// put JconstraintAxis*tmp1 into rhs
dRealAllocaArray (rhs,numConstraintRows);
multiply_J (numConstraintRows,JconstraintAxis,jb,tmp1,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
for (i=0; i<m; i++)
for (i=0; i<numConstraintRows; i++)
cfm[i] *= stepsize1;
// load lambda from the value saved on the previous iteration
dRealAllocaArray (lambda,m);
// load lambdaAccumulatedImpulse from the value saved on the previous iteration
dRealAllocaArray (lambdaAccumulatedImpulse,numConstraintRows);
#ifdef WARM_STARTING
dSetZero1 (lambda,m); //@@@ shouldn't be necessary
dSetZero1 (lambdaAccumulatedImpulse,numConstraintRows); //@@@ shouldn't be necessary
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
// 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);
/// 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
// 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
// recreated every iteration
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
// 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.
// add stepsize * cforce to the body velocity
for (i=0; i<nb; i++) {
@@ -677,7 +680,7 @@ void btSorLcpSolver::SolveInternal1 (
{
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;
}
//stackAlloc->endBlock(saBlock);//Remo: 10.10.2007

4
Extras/quickstep/btSorLcp.h
View File

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