diff --git a/Demos/AllBulletDemos/DemoEntries.cpp b/Demos/AllBulletDemos/DemoEntries.cpp index a9f939e9f..6684762bd 100644 --- a/Demos/AllBulletDemos/DemoEntries.cpp +++ b/Demos/AllBulletDemos/DemoEntries.cpp @@ -143,7 +143,7 @@ btDemoEntry g_demoEntries[] = {"SoftBody Cluster Stack Mixed",SoftDemo27::Create}, // {"SliderConstraint",SliderConstraintDemo::Create}, -// {"CcdPhysicsDemo", CcdPhysicsDemo::Create}, + {"CcdPhysicsDemo", CcdPhysicsDemo::Create}, // {"ConcaveRaycastDemo",ConcaveRaycastDemo::Create}, //{"BspDemo", BspDemo::Create}, // {"Raytracer Test",Raytracer::Create}, diff --git a/Demos/AllBulletDemos/Main.cpp b/Demos/AllBulletDemos/Main.cpp index ae05b6d10..ed7083e9e 100644 --- a/Demos/AllBulletDemos/Main.cpp +++ b/Demos/AllBulletDemos/Main.cpp @@ -71,7 +71,7 @@ void setDefaultSettings() { viewX = 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; height = 600;//1024;//480; @@ -87,8 +87,8 @@ void setDefaultSettings() gUseSplitImpulse = 0; gUseWarmstarting = 1; gRandomizeConstraints = 1; - gErp = 0.2f; - gSlop=0.01f; + gErp = 0.4f; + gSlop=0.0f; gErp2 = 0.1f; gWarmStartingParameter = 0.85f; diff --git a/Demos/BasicDemo/BasicDemo.cpp b/Demos/BasicDemo/BasicDemo.cpp index 40daf6944..f6ba2d416 100644 --- a/Demos/BasicDemo/BasicDemo.cpp +++ b/Demos/BasicDemo/BasicDemo.cpp @@ -13,6 +13,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ + ///create 125 (5x5x5) dynamic object #define ARRAY_SIZE_X 5 #define ARRAY_SIZE_Y 5 diff --git a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp index c5933f41a..0f90cc7ca 100644 --- a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp +++ b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp @@ -192,7 +192,7 @@ extern btScalar gJitterVelocityDampingFactor; extern int gNumManifold; extern int gOverlappingPairs; -extern int gTotalContactPoints; + void CcdPhysicsDemo::clientMoveAndDisplay() { @@ -282,10 +282,10 @@ void CcdPhysicsDemo::clientMoveAndDisplay() #ifdef PRINT_CONTACT_STATISTICS printf("num manifolds: %i\n",gNumManifold); printf("num gOverlappingPairs: %i\n",gOverlappingPairs); - printf("num gTotalContactPoints : %i\n",gTotalContactPoints ); + #endif //PRINT_CONTACT_STATISTICS - gTotalContactPoints = 0; + glutSwapBuffers(); } diff --git a/Demos/MultiThreadedDemo/MultiThreadedDemo.cpp b/Demos/MultiThreadedDemo/MultiThreadedDemo.cpp index 27f2660f1..6c633e416 100644 --- a/Demos/MultiThreadedDemo/MultiThreadedDemo.cpp +++ b/Demos/MultiThreadedDemo/MultiThreadedDemo.cpp @@ -133,7 +133,7 @@ extern btScalar gJitterVelocityDampingFactor; extern int gNumManifold; extern int gOverlappingPairs; -extern int gTotalContactPoints; + void MultiThreadedDemo::clientMoveAndDisplay() { @@ -201,7 +201,7 @@ void MultiThreadedDemo::clientMoveAndDisplay() #endif glFlush(); - gTotalContactPoints = 0; + glutSwapBuffers(); } diff --git a/Demos/SoftDemo/SoftDemo.cpp b/Demos/SoftDemo/SoftDemo.cpp index e7a16a28b..69309e574 100644 --- a/Demos/SoftDemo/SoftDemo.cpp +++ b/Demos/SoftDemo/SoftDemo.cpp @@ -97,7 +97,7 @@ void SoftDemo::createStack( btCollisionShape* boxShape, float halfCubeSize, int extern int gNumManifold; extern int gOverlappingPairs; -extern int gTotalContactPoints; + void SoftDemo::clientMoveAndDisplay() { @@ -200,10 +200,10 @@ void SoftDemo::clientMoveAndDisplay() #ifdef PRINT_CONTACT_STATISTICS printf("num manifolds: %i\n",gNumManifold); printf("num gOverlappingPairs: %i\n",gOverlappingPairs); - printf("num gTotalContactPoints : %i\n",gTotalContactPoints ); + #endif //PRINT_CONTACT_STATISTICS - gTotalContactPoints = 0; + glutSwapBuffers(); } diff --git a/Extras/quickstep/btOdeContactJoint.cpp b/Extras/quickstep/btOdeContactJoint.cpp index 5d9c2a55f..d38db8a1b 100644 --- a/Extras/quickstep/btOdeContactJoint.cpp +++ b/Extras/quickstep/btOdeContactJoint.cpp @@ -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) diff --git a/Extras/quickstep/btOdeJoint.h b/Extras/quickstep/btOdeJoint.h index 7fae83067..34a1811e7 100644 --- a/Extras/quickstep/btOdeJoint.h +++ b/Extras/quickstep/btOdeJoint.h @@ -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. diff --git a/Extras/quickstep/btOdeQuickstepConstraintSolver.cpp b/Extras/quickstep/btOdeQuickstepConstraintSolver.cpp index c83864794..b0489195c 100644 --- a/Extras/quickstep/btOdeQuickstepConstraintSolver.cpp +++ b/Extras/quickstep/btOdeQuickstepConstraintSolver.cpp @@ -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;irefreshContactPoints(((btRigidBody*)manifold->getBody0())->getCenterOfMassTransform(), + /* manifold->refreshContactPoints(((btRigidBody*)manifold->getBody0())->getCenterOfMassTransform(), ((btRigidBody*)manifold->getBody1())->getCenterOfMassTransform()); +*/ int bodyId0 = _bodyId0,bodyId1 = _bodyId1; diff --git a/Extras/quickstep/btOdeQuickstepConstraintSolver.h b/Extras/quickstep/btOdeQuickstepConstraintSolver.h index 3a47ba4ab..bc044b292 100644 --- a/Extras/quickstep/btOdeQuickstepConstraintSolver.h +++ b/Extras/quickstep/btOdeQuickstepConstraintSolver.h @@ -40,9 +40,6 @@ private: int m_CurJoint; int m_CurTypedJoint; - float m_cfm; - float m_erp; - btSorLcpSolver m_SorLcpSolver; btAlignedObjectArray 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() { diff --git a/Extras/quickstep/btOdeTypedJoint.cpp b/Extras/quickstep/btOdeTypedJoint.cpp index 022e6a8ed..c289bed6f 100644 --- a/Extras/quickstep/btOdeTypedJoint.cpp +++ b/Extras/quickstep/btOdeTypedJoint.cpp @@ -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() diff --git a/Extras/quickstep/btSorLcp.cpp b/Extras/quickstep/btSorLcp.cpp index d4d87393e..5304bf0e5 100644 --- a/Extras/quickstep/btSorLcp.cpp +++ b/Extras/quickstep/btSorLcp.cpp @@ -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 &body, - dRealPtr invI) + dRealPtr inverseInertiaWorld) { int i,j; dRealMutablePtr iMJ_ptr = iMJ; - dRealMutablePtr J_ptr = J; - for (i=0; im_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 &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= 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= 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 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= 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 &body, int nb, +void btSorLcpSolver::SolveInternal1 (const btAlignedObjectArray &body, int nb, btAlignedObjectArray &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; im_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; jGetInfo1 (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 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; iGetInfo2 (&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= 0) - findex[ofs[i] + j] += ofs[i]; + for (j=0; j= 0) + findex[constraintRowOffsets[i] + j] += constraintRowOffsets[i]; } } @@ -595,13 +597,13 @@ void btSorLcpSolver::SolveInternal1 ( for (i=0; inode[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; jm_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; ilambda,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; ilambda,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; im_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 diff --git a/Extras/quickstep/btSorLcp.h b/Extras/quickstep/btSorLcp.h index 41778de73..46d1a190b 100644 --- a/Extras/quickstep/btSorLcp.h +++ b/Extras/quickstep/btSorLcp.h @@ -39,9 +39,7 @@ public: dRand2_seed = 0; } - void SolveInternal1 (float global_cfm, - float global_erp, - const btAlignedObjectArray &body, int nb, + void SolveInternal1 (const btAlignedObjectArray &body, int nb, btAlignedObjectArray &joint, int nj, const btContactSolverInfo& solverInfo, btStackAlloc* stackAlloc diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index 61dad522a..c33d92459 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -94,8 +94,6 @@ void btConeTwistConstraint::buildJacobian() for (int i=0;i<3;i++) { new (&m_jac[i]) btJacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), pivotAInW - m_rbA.getCenterOfMassPosition(), pivotBInW - m_rbB.getCenterOfMassPosition(), normal[i], diff --git a/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp index 4575c96d2..9b81dc798 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @@ -52,8 +52,7 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, btVector3 vel = vel1 - vel2; - btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(), - body2.getCenterOfMassTransform().getBasis().transpose(), + btJacobianEntry jac( rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(), body2.getInvInertiaDiagLocal(),body2.getInvMass()); diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 7b0e8c6ba..11715708d 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -39,6 +39,7 @@ struct btContactSolverInfoData btScalar m_sor; btScalar m_erp;//used as Baumgarte factor btScalar m_erp2;//used in Split Impulse + btScalar m_globalCfm;//constraint force mixing int m_splitImpulse; btScalar m_splitImpulsePenetrationThreshold; btScalar m_linearSlop; @@ -63,9 +64,10 @@ struct btContactSolverInfo : public btContactSolverInfoData m_restitution = btScalar(0.); m_maxErrorReduction = btScalar(20.); m_numIterations = 10; - m_erp = btScalar(0.2); + m_erp = btScalar(0.4); m_erp2 = btScalar(0.1); - m_sor = btScalar(1.3); + m_globalCfm = btScalar(0.); + m_sor = btScalar(1.); m_splitImpulse = false; m_splitImpulsePenetrationThreshold = -0.02f; m_linearSlop = btScalar(0.0); diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index 6b886cc81..abcb8d753 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -345,8 +345,7 @@ void btGeneric6DofConstraint::buildLinearJacobian( const btVector3 & pivotAInW,const btVector3 & pivotBInW) { new (&jacLinear) btJacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), + pivotAInW - m_rbA.getCenterOfMassPosition(), pivotBInW - m_rbB.getCenterOfMassPosition(), normalWorld, diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index cc2d4b615..ae8ba96a7 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -181,8 +181,6 @@ void btHingeConstraint::buildJacobian() for (int i=0;i<3;i++) { new (&m_jac[i]) btJacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), pivotAInW - m_rbA.getCenterOfMassPosition(), pivotBInW - m_rbB.getCenterOfMassPosition(), normal[i], diff --git a/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h b/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h index bfeb24c2d..e05fdce76 100644 --- a/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h +++ b/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h @@ -34,8 +34,6 @@ public: btJacobianEntry() {}; //constraint between two different rigidbodies btJacobianEntry( - const btMatrix3x3& world2A, - const btMatrix3x3& world2B, const btVector3& rel_pos1,const btVector3& rel_pos2, const btVector3& jointAxis, const btVector3& inertiaInvA, @@ -44,8 +42,8 @@ public: const btScalar massInvB) :m_linearJointAxis(jointAxis) { - m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis)); - m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis)); + m_aJ = (rel_pos1.cross(m_linearJointAxis)); + m_bJ = (rel_pos2.cross(-m_linearJointAxis)); m_0MinvJt = inertiaInvA * m_aJ; m_1MinvJt = inertiaInvB * m_bJ; m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); @@ -61,8 +59,8 @@ public: const btVector3& inertiaInvB) :m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.))) { - m_aJ= world2A*jointAxis; - m_bJ = world2B*-jointAxis; + m_aJ= jointAxis; + m_bJ = -jointAxis; m_0MinvJt = inertiaInvA * m_aJ; m_1MinvJt = inertiaInvB * m_bJ; m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); @@ -95,8 +93,8 @@ public: const btScalar massInvA) :m_linearJointAxis(jointAxis) { - m_aJ= world2A*(rel_pos1.cross(jointAxis)); - m_bJ = world2A*(rel_pos2.cross(-jointAxis)); + m_aJ= (rel_pos1.cross(jointAxis)); + m_bJ = (rel_pos2.cross(-jointAxis)); m_0MinvJt = inertiaInvA * m_aJ; m_1MinvJt = btVector3(btScalar(0.),btScalar(0.),btScalar(0.)); m_Adiag = massInvA + m_0MinvJt.dot(m_aJ); @@ -130,7 +128,7 @@ public: return sum[0]+sum[1]+sum[2]; } - btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB) + btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB) const { btVector3 linrel = linvelA - linvelB; btVector3 angvela = angvelA * m_aJ; diff --git a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp index 2b69ad904..5046328a2 100644 --- a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp @@ -48,8 +48,7 @@ void btPoint2PointConstraint::buildJacobian() { normal[i] = 1; new (&m_jac[i]) btJacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(), m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(), normal, diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 9feec6745..381592d79 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -15,7 +15,6 @@ subject to the following restrictions: //#define COMPUTE_IMPULSE_DENOM 1 //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms. -//#define FORCE_REFESH_CONTACT_MANIFOLDS 1 #include "btSequentialImpulseConstraintSolver.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" @@ -32,25 +31,48 @@ subject to the following restrictions: #include "LinearMath/btQuickprof.h" #include "btSolverBody.h" #include "btSolverConstraint.h" - - #include "LinearMath/btAlignedObjectArray.h" -int totalCpd = 0; - -int gTotalContactPoints = 0; - -struct btOrderIndex +btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() +:m_btSeed2(0) { - int m_manifoldIndex; - int m_pointIndex; -}; +} +btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver() +{ +} +// Project Gauss Seidel or the equivalent Sequential Impulse +void btSequentialImpulseConstraintSolver::resolveSingleConstraintRow(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +{ + float deltaImpulse; + deltaImpulse = c.m_rhs-c.m_appliedImpulse*c.m_cfm; + btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity); + btScalar deltaVel2Dotn = c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity); + btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; + deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; + deltaImpulse += deltaVel2Dotn*c.m_jacDiagABInv; + + btScalar sum = c.m_appliedImpulse + deltaImpulse; + if (sum < c.m_lowerLimit) + { + deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_lowerLimit; + } + else if (sum > c.m_upperLimit) + { + deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_upperLimit; + } + else + { + c.m_appliedImpulse = sum; + } + body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse); + body2.applyImpulse(c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,-deltaImpulse); +} -#define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384 -static btOrderIndex gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS]; unsigned long btSequentialImpulseConstraintSolver::btRand2() @@ -92,44 +114,17 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n) -bool MyContactDestroyedCallback(void* userPersistentData); -bool MyContactDestroyedCallback(void* userPersistentData) -{ - assert (userPersistentData); - btConstraintPersistentData* cpd = (btConstraintPersistentData*)userPersistentData; - btAlignedFree(cpd); - totalCpd--; - //printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData); - return true; -} -btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() -:m_btSeed2(0) -{ - gContactDestroyedCallback = &MyContactDestroyedCallback; - //initialize default friction/contact funcs - int i,j; - for (i=0;im_deltaLinearVelocity.setValue(0.f,0.f,0.f); + solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); + if (rb) { solverBody->m_angularVelocity = rb->getAngularVelocity() ; @@ -156,22 +151,14 @@ void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject int gNumSplitImpulseRecoveries = 0; -btScalar restitutionCurve(btScalar rel_vel, btScalar restitution); -btScalar restitutionCurve(btScalar rel_vel, btScalar restitution) +btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution) { btScalar rest = restitution * -rel_vel; return rest; } - -void resolveSplitPenetrationImpulseCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, - const btSolverConstraint& contactConstraint, - const btContactSolverInfo& solverInfo); - //SIMD_FORCE_INLINE -void resolveSplitPenetrationImpulseCacheFriendly( +void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly( btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& contactConstraint, @@ -225,85 +212,11 @@ void resolveSplitPenetrationImpulseCacheFriendly( } -//velocity + friction -//response between two dynamic objects with friction -btScalar resolveSingleCollisionCombinedCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, - const btSolverConstraint& contactConstraint, - const btContactSolverInfo& solverInfo); + //SIMD_FORCE_INLINE -btScalar resolveSingleCollisionCombinedCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, - const btSolverConstraint& contactConstraint, - const btContactSolverInfo& solverInfo) -{ - (void)solverInfo; - - btScalar normalImpulse; - - { - - - // Optimized version of projected relative velocity, use precomputed cross products with normal - // body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1); - // body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2); - // btVector3 vel = vel1 - vel2; - // btScalar rel_vel = contactConstraint.m_contactNormal.dot(vel); - - btScalar rel_vel; - btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) - + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity); - btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) - + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity); - - rel_vel = vel1Dotn-vel2Dotn; - - btScalar positionalError = 0.f; - if (!solverInfo.m_splitImpulse || (contactConstraint.m_penetration > solverInfo.m_splitImpulsePenetrationThreshold)) - { - positionalError = -contactConstraint.m_penetration * solverInfo.m_erp/solverInfo.m_timeStep; - } - - btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping; - - btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv; - normalImpulse = penetrationImpulse+velocityImpulse; - - - // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse - btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse; - btScalar sum = oldNormalImpulse + normalImpulse; - contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum; - - normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse; - - body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass, - contactConstraint.m_angularComponentA,normalImpulse); - - body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass, - contactConstraint.m_angularComponentB,-normalImpulse); - } - - return normalImpulse; -} - - -#ifndef NO_FRICTION_TANGENTIALS - -btScalar resolveSingleFrictionCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, - const btSolverConstraint& contactConstraint, - const btContactSolverInfo& solverInfo, - btScalar appliedNormalImpulse); - -//SIMD_FORCE_INLINE -btScalar resolveSingleFrictionCacheFriendly( +void btSequentialImpulseConstraintSolver::resolveSingleFrictionCacheFriendly( btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& contactConstraint, @@ -312,7 +225,6 @@ btScalar resolveSingleFrictionCacheFriendly( { (void)solverInfo; - const btScalar combinedFriction = contactConstraint.m_friction; const btScalar limit = appliedNormalImpulse * combinedFriction; @@ -324,12 +236,18 @@ btScalar resolveSingleFrictionCacheFriendly( btScalar j1; { +#ifndef _USE_JACOBIAN btScalar rel_vel; const btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity); const btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity); + rel_vel = vel1Dotn-vel2Dotn; +#else + btScalar rel_vel = contactConstraint.m_jac.getRelativeVelocity(body1.m_linearVelocity1+body1.m_deltaLinearVelocity,body1.m_angularVelocity1+body1.m_deltaAngularVelocity, + body2.m_linearVelocity1+body2.m_deltaLinearVelocity,body2.m_angularVelocity1+body2.m_deltaAngularVelocity); +#endif //_USE_JACOBIAN // calculate j that moves us to zero relative velocity j1 = -rel_vel * contactConstraint.m_jacDiagABInv; @@ -366,76 +284,14 @@ btScalar resolveSingleFrictionCacheFriendly( } - body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1); + body1.applyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1); - body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1); + body2.applyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1); } - return 0.f; } -#else - -//velocity + friction -//response between two dynamic objects with friction -btScalar resolveSingleFrictionCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, - btSolverConstraint& contactConstraint, - const btContactSolverInfo& solverInfo) -{ - - btVector3 vel1; - btVector3 vel2; - btScalar normalImpulse(0.f); - - { - const btVector3& normal = contactConstraint.m_contactNormal; - if (contactConstraint.m_penetration < 0.f) - return 0.f; - - - body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1); - body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2); - btVector3 vel = vel1 - vel2; - btScalar rel_vel; - rel_vel = normal.dot(vel); - - btVector3 lat_vel = vel - normal * rel_vel; - btScalar lat_rel_vel = lat_vel.length2(); - - btScalar combinedFriction = contactConstraint.m_friction; - const btVector3& rel_pos1 = contactConstraint.m_rel_posA; - const btVector3& rel_pos2 = contactConstraint.m_rel_posB; - - - if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON) - { - lat_rel_vel = btSqrt(lat_rel_vel); - - lat_vel /= lat_rel_vel; - btVector3 temp1 = body1.m_invInertiaWorld * rel_pos1.cross(lat_vel); - btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel); - btScalar friction_impulse = lat_rel_vel / - (body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2))); - btScalar normal_impulse = contactConstraint.m_appliedImpulse * combinedFriction; - - GEN_set_min(friction_impulse, normal_impulse); - GEN_set_max(friction_impulse, -normal_impulse); - body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1); - body2.applyImpulse(lat_vel * friction_impulse, rel_pos2); - } - } - - return normalImpulse; -} - -#endif //NO_FRICTION_TANGENTIALS - - - - btSolverConstraint& btSequentialImpulseConstraintSolver::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) { @@ -491,6 +347,14 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c btScalar denom = relaxation/(denom0+denom1); solverConstraint.m_jacDiagABInv = denom; +#ifdef _USE_JACOBIAN + solverConstraint.m_jac = btJacobianEntry ( + rel_pos1,rel_pos2,solverConstraint.m_contactNormal, + body0->getInvInertiaDiagLocal(), + body0->getInvMass(), + body1->getInvInertiaDiagLocal(), + body1->getInvMass()); +#endif //_USE_JACOBIAN return solverConstraint; } @@ -513,72 +377,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol //btRigidBody* rb0=0,*rb1=0; - -#ifdef FORCE_REFESH_CONTACT_MANIFOLDS - - BEGIN_PROFILE("refreshManifolds"); - - int i; - - - - for (i=0;igetBody1(); - rb0 = (btRigidBody*)manifold->getBody0(); - - manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform()); - - } - - END_PROFILE("refreshManifolds"); -#endif //FORCE_REFESH_CONTACT_MANIFOLDS - - - - - - //int sizeofSB = sizeof(btSolverBody); - //int sizeofSC = sizeof(btSolverConstraint); - - //if (1) { - //if m_stackAlloc, try to pack bodies/constraints to speed up solving -// btBlock* sablock; -// sablock = stackAlloc->beginBlock(); - - // int memsize = 16; -// unsigned char* stackMemory = stackAlloc->allocate(memsize); - - //todo: use stack allocator for this temp memory -// int minReservation = numManifolds*2; - - //m_tmpSolverBodyPool.reserve(minReservation); - - //don't convert all bodies, only the one we need so solver the constraints -/* - { - for (int i=0;igetIslandTag() >= 0)) - { - btAssert(rb->getCompanionId() < 0); - int solverBodyId = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,rb); - rb->setCompanionId(solverBodyId); - } - } - } -*/ - - //m_tmpSolverConstraintPool.reserve(minReservation); - //m_tmpSolverFrictionConstraintPool.reserve(minReservation); - { int i; @@ -647,7 +448,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btManifoldPoint& cp = manifold->getContactPoint(j); - if (cp.getDistance() <= btScalar(0.)) + ///this is a bad test and results in jitter -> always solve for those zero-distanc contacts! + ///-> if (cp.getDistance() <= btScalar(0.)) { const btVector3& pos1 = cp.getPositionWorldOnA(); @@ -714,9 +516,16 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol rel_vel = cp.m_normalWorldOnB.dot(vel); - solverConstraint.m_penetration = btMin(cp.getDistance()+infoGlobal.m_linearSlop,btScalar(0.)); + solverConstraint.m_penetration = cp.getDistance()+infoGlobal.m_linearSlop; //solverConstraint.m_penetration = cp.getDistance(); + + + + + + + solverConstraint.m_friction = cp.m_combinedFriction; @@ -733,25 +542,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } - btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep; - - - - if (solverConstraint.m_restitution > penVel) - { - solverConstraint.m_penetration = btScalar(0.); - } - - - ///warm starting (or zero if disabled) if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) { solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; if (rb0) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); if (rb1) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse); + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse); } else { solverConstraint.m_appliedImpulse = 0.f; @@ -759,6 +557,43 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol solverConstraint.m_appliedPushImpulse = 0.f; + { + btScalar rel_vel; + btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0)) + + solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0)); + btScalar vel2Dotn = solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0)) + + solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0)); + + rel_vel = vel1Dotn-vel2Dotn; + + btScalar positionalError = 0.f; + positionalError = -solverConstraint.m_penetration * infoGlobal.m_erp/infoGlobal.m_timeStep; + btScalar velocityError = solverConstraint.m_restitution - rel_vel;// * damping; + + btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; + solverConstraint.m_rhs = (penetrationImpulse+velocityImpulse); + solverConstraint.m_cfm = 0.f; + solverConstraint.m_lowerLimit = 0; + solverConstraint.m_upperLimit = 1e30f; + } + + +#ifdef _USE_JACOBIAN + solverConstraint.m_jac = btJacobianEntry ( + rel_pos1,rel_pos2,cp.m_normalWorldOnB, + rb0->getInvInertiaDiagLocal(), + rb0->getInvMass(), + rb1->getInvInertiaDiagLocal(), + rb1->getInvMass()); +#endif //_USE_JACOBIAN + + /////setup the friction constraints + + + + if (1) + { solverConstraint.m_frictionIndex = m_tmpSolverFrictionConstraintPool.size(); if (!cp.m_lateralFrictionInitialized) { @@ -802,9 +637,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol { frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; if (rb0) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); if (rb1) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse); + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse); } else { frictionConstraint1.m_appliedImpulse = 0.f; @@ -816,15 +651,16 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol { frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; if (rb0) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); if (rb1) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse); + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse); } else { frictionConstraint2.m_appliedImpulse = 0.f; } } } + } } @@ -845,8 +681,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } } - - int numConstraintPool = m_tmpSolverConstraintPool.size(); int numFrictionPool = m_tmpSolverFrictionConstraintPool.size(); @@ -865,8 +699,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } } - - return 0.f; } @@ -930,27 +762,24 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations( } + { int numPoolConstraints = m_tmpSolverConstraintPool.size(); for (j=0;jgetNumContacts();p++) - { - gOrder[totalPoints].m_manifoldIndex = j; - gOrder[totalPoints].m_pointIndex = p; - totalPoints++; - } - } - } - - { - int j; - for (j=0;jbuildJacobian(); - } - } - - - //should traverse the contacts random order... - int iteration; - - { - for ( iteration = 0;iterationsolveConstraint(info.m_timeStep); - } - - for (j=0;jgetBody0(), - (btRigidBody*)manifold->getBody1() - ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); - } - - for (j=0;jgetBody0(), - (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); - } - - } - } - - - - - return btScalar(0.); -} - - - - - - - -void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer) -{ - - (void)debugDrawer; - - btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0(); - btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1(); - - - //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop - { -#ifdef FORCE_REFESH_CONTACT_MANIFOLDS - manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform()); -#endif //FORCE_REFESH_CONTACT_MANIFOLDS - int numpoints = manifoldPtr->getNumContacts(); - - gTotalContactPoints += numpoints; - - - for (int i=0;igetContactPoint(i); - if (cp.getDistance() <= btScalar(0.)) - { - const btVector3& pos1 = cp.getPositionWorldOnA(); - const btVector3& pos2 = cp.getPositionWorldOnB(); - - btVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition(); - btVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition(); - - - //this jacobian entry is re-used for all iterations - btJacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(), - body1->getCenterOfMassTransform().getBasis().transpose(), - rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(), - body1->getInvInertiaDiagLocal(),body1->getInvMass()); - - - btScalar jacDiagAB = jac.getDiagonal(); - - btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; - if (cpd) - { - //might be invalid - cpd->m_persistentLifeTime++; - if (cpd->m_persistentLifeTime != cp.getLifeTime()) - { - //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime()); - new (cpd) btConstraintPersistentData; - cpd->m_persistentLifeTime = cp.getLifeTime(); - - } else - { - //printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime()); - - } - } else - { - - //todo: should this be in a pool? - void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16); - cpd = new (mem)btConstraintPersistentData; - assert(cpd); - - totalCpd ++; - //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd); - cp.m_userPersistentData = cpd; - cpd->m_persistentLifeTime = cp.getLifeTime(); - //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime()); - - } - assert(cpd); - - cpd->m_jacDiagABInv = btScalar(1.) / jacDiagAB; - - //Dependent on Rigidbody A and B types, fetch the contact/friction response func - //perhaps do a similar thing for friction/restutution combiner funcs... - - cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType]; - cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType]; - - btVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - btScalar rel_vel; - rel_vel = cp.m_normalWorldOnB.dot(vel); - - btScalar combinedRestitution = cp.m_combinedRestitution; - - cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations); - cpd->m_friction = cp.m_combinedFriction; - if (cp.m_lifeTime>info.m_restingContactRestitutionThreshold) - { - cpd->m_restitution = 0.f; - } else - { - cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution); - if (cpd->m_restitution <= btScalar(0.)) - { - cpd->m_restitution = btScalar(0.0); - }; - } - - //restitution and penetration work in same direction so - //rel_vel - - btScalar penVel = -cpd->m_penetration/info.m_timeStep; - - if (cpd->m_restitution > penVel) - { - cpd->m_penetration = btScalar(0.); - } - - - btScalar relaxation = info.m_damping; - if (info.m_solverMode & SOLVER_USE_WARMSTARTING) - { - cpd->m_appliedImpulse *= relaxation; - } else - { - cpd->m_appliedImpulse =btScalar(0.); - } - - //for friction - cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse; - - //re-calculate friction direction every frame, todo: check if this is really needed - btPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1); - - -#define NO_FRICTION_WARMSTART 1 - - #ifdef NO_FRICTION_WARMSTART - cpd->m_accumulatedTangentImpulse0 = btScalar(0.); - cpd->m_accumulatedTangentImpulse1 = btScalar(0.); - #endif //NO_FRICTION_WARMSTART - btScalar denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0); - btScalar denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0); - btScalar denom = relaxation/(denom0+denom1); - cpd->m_jacDiagABInvTangent0 = denom; - - - denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1); - denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1); - denom = relaxation/(denom0+denom1); - cpd->m_jacDiagABInvTangent1 = denom; - - btVector3 totalImpulse = - #ifndef NO_FRICTION_WARMSTART - cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+ - cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+ - #endif //NO_FRICTION_WARMSTART - cp.m_normalWorldOnB*cpd->m_appliedImpulse; - - - - /// - { - btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); - cpd->m_angularComponentA = body0->getInvInertiaTensorWorld()*torqueAxis0; - btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); - cpd->m_angularComponentB = body1->getInvInertiaTensorWorld()*torqueAxis1; - } - { - btVector3 ftorqueAxis0 = rel_pos1.cross(cpd->m_frictionWorldTangential0); - cpd->m_frictionAngularComponent0A = body0->getInvInertiaTensorWorld()*ftorqueAxis0; - } - { - btVector3 ftorqueAxis1 = rel_pos1.cross(cpd->m_frictionWorldTangential1); - cpd->m_frictionAngularComponent1A = body0->getInvInertiaTensorWorld()*ftorqueAxis1; - } - { - btVector3 ftorqueAxis0 = rel_pos2.cross(cpd->m_frictionWorldTangential0); - cpd->m_frictionAngularComponent0B = body1->getInvInertiaTensorWorld()*ftorqueAxis0; - } - { - btVector3 ftorqueAxis1 = rel_pos2.cross(cpd->m_frictionWorldTangential1); - cpd->m_frictionAngularComponent1B = body1->getInvInertiaTensorWorld()*ftorqueAxis1; - } - - /// - - - - //apply previous frames impulse on both bodies - body0->applyImpulse(totalImpulse, rel_pos1); - body1->applyImpulse(-totalImpulse, rel_pos2); - } - - } - } -} - - -btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) -{ - btScalar maxImpulse = btScalar(0.); - - { - - - { - if (cp.getDistance() <= btScalar(0.)) - { - - - - { - - //btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; - btScalar impulse = resolveSingleCollisionCombined( - *body0,*body1, - cp, - info); - - if (maxImpulse < impulse) - maxImpulse = impulse; - - } - } - } - } - return maxImpulse; -} - - - -btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) -{ - - btScalar maxImpulse = btScalar(0.); - - { - - - { - if (cp.getDistance() <= btScalar(0.)) - { - - - - { - - btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; - btScalar impulse = cpd->m_contactSolverFunc( - *body0,*body1, - cp, - info); - - if (maxImpulse < impulse) - maxImpulse = impulse; - - } - } - } - } - return maxImpulse; -} - -btScalar btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) -{ - - (void)debugDrawer; - (void)iter; - - - { - - - { - - if (cp.getDistance() <= btScalar(0.)) - { - - btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; - cpd->m_frictionSolverFunc( - *body0,*body1, - cp, - info); - - - } - } - - - } - return btScalar(0.); -} void btSequentialImpulseConstraintSolver::reset() diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 983ff10d8..fb16fdef2 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -38,52 +38,45 @@ class btSequentialImpulseConstraintSolver : public btConstraintSolver protected: - btScalar solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); - btScalar solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); - void prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer); btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation); - - ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]; - ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]; - ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction unsigned long m_btSeed2; + void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject); + btScalar restitutionCurve(btScalar rel_vel, btScalar restitution); + + void resolveSplitPenetrationImpulseCacheFriendly( + btSolverBody& body1, + btSolverBody& body2, + const btSolverConstraint& contactConstraint, + const btContactSolverInfo& solverInfo); + + void resolveSingleConstraintRow( + btSolverBody& body1, + btSolverBody& body2, + const btSolverConstraint& contactConstraint); + + void resolveSingleFrictionCacheFriendly( + btSolverBody& body1, + btSolverBody& body2, + const btSolverConstraint& contactConstraint, + const btContactSolverInfo& solverInfo, + btScalar appliedNormalImpulse); + public: btSequentialImpulseConstraintSolver(); - - ///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody - ///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType - void setContactSolverFunc(ContactSolverFunc func,int type0,int type1) - { - m_contactDispatch[type0][type1] = func; - } - - ///Advanced: Override the default friction solving function for contacts, for certain types of rigidbody - ///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType - void SetFrictionSolverFunc(ContactSolverFunc func,int type0,int type1) - { - m_frictionDispatch[type0][type1] = func; - } - virtual ~btSequentialImpulseConstraintSolver(); - + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher); - - virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); - btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); + btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); - + btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); ///clear internal cached data and reset random seed virtual void reset(); - - btScalar solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); - - unsigned long btRand2(); diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index af2ee3928..2abe61e2a 100755 --- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -119,8 +119,6 @@ void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, co { normalWorld = m_calculatedTransformA.getBasis().getColumn(i); new (&m_jacLin[i]) btJacobianEntry( - rbA.getCenterOfMassTransform().getBasis().transpose(), - rbB.getCenterOfMassTransform().getBasis().transpose(), m_relPosA, m_relPosB, normalWorld, diff --git a/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp index 0c7dbd668..d2feeb885 100644 --- a/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp @@ -61,9 +61,10 @@ void btSolve2LinearConstraint::resolveUnilateralPairConstraint( //this jacobian entry could be re-used for all iterations - btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, + btJacobianEntry jacA( + rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, invInertiaBDiag,invMassB); - btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, + btJacobianEntry jacB(rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, invInertiaBDiag,invMassB); //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); @@ -150,9 +151,9 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint( //this jacobian entry could be re-used for all iterations - btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, + btJacobianEntry jacA(rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, invInertiaBDiag,invMassB); - btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, + btJacobianEntry jacB(rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, invInertiaBDiag,invMassB); //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); diff --git a/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/src/BulletDynamics/ConstraintSolver/btSolverBody.h index 3681e4d11..5b812a0d0 100644 --- a/src/BulletDynamics/ConstraintSolver/btSolverBody.h +++ b/src/BulletDynamics/ConstraintSolver/btSolverBody.h @@ -23,6 +23,7 @@ class btRigidBody; #include "LinearMath/btAlignedAllocator.h" #include "LinearMath/btTransformUtil.h" +#ifndef _USE_JACOBIAN ///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. ATTRIBUTE_ALIGNED16 (struct) btSolverBody @@ -33,6 +34,10 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody btVector3 m_angularVelocity; btVector3 m_linearVelocity; + + btVector3 m_deltaLinearVelocity; + btVector3 m_deltaAngularVelocity; + btVector3 m_centerOfMassPosition; btVector3 m_pushVelocity; btVector3 m_turnVelocity; @@ -49,10 +54,12 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody } //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position - SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) + SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) { - if (m_invMass) + //if (m_invMass) { + m_deltaLinearVelocity += linearComponent*impulseMagnitude; + m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); m_linearVelocity += linearComponent*impulseMagnitude; m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); } @@ -60,7 +67,7 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) { - if (m_invMass) + //if (m_invMass) { m_pushVelocity += linearComponent*impulseMagnitude; m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor); @@ -109,7 +116,100 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody }; +#else +///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. +ATTRIBUTE_ALIGNED16 (struct) btSolverBody +{ + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btMatrix3x3 m_worldInvInertiaTensor; + + btVector3 m_angularVelocity1; + btVector3 m_linearVelocity1; + + btVector3 m_deltaAngularVelocity; + btVector3 m_deltaLinearVelocity; + + btVector3 m_centerOfMassPosition; + btVector3 m_pushVelocity; + btVector3 m_turnVelocity; + + float m_angularFactor; + float m_invMass; + float m_friction; + btRigidBody* m_originalBody; + + + + SIMD_FORCE_INLINE void getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const + { + velocity = (m_linearVelocity1 + m_deltaLinearVelocity) + (m_angularVelocity1+m_deltaAngularVelocity).cross(rel_pos); + } + + //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position + SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) + { + if (m_invMass) + { + m_deltaLinearVelocity += linearComponent*impulseMagnitude; + m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + } + + SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) + { + if (m_invMass) + { + m_pushVelocity += linearComponent*impulseMagnitude; + m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + } + + + void writebackVelocity() + { + if (m_invMass) + { + m_originalBody->setLinearVelocity(m_linearVelocity1 + m_deltaLinearVelocity); + m_originalBody->setAngularVelocity(m_angularVelocity1+m_deltaAngularVelocity); + + //m_originalBody->setCompanionId(-1); + } + } + + + void writebackVelocity(btScalar timeStep) + { + if (m_invMass) + { + m_originalBody->setLinearVelocity(m_linearVelocity1 + m_deltaLinearVelocity); + m_originalBody->setAngularVelocity(m_angularVelocity1+m_deltaAngularVelocity); + + //correct the position/orientation based on push/turn recovery + btTransform newTransform; + btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform); + m_originalBody->setWorldTransform(newTransform); + + //m_originalBody->setCompanionId(-1); + } + } + + void readVelocity() + { + if (m_invMass) + { + m_linearVelocity1 = m_originalBody->getLinearVelocity(); + m_angularVelocity1 = m_originalBody->getAngularVelocity(); + } + } + + + + +}; + +#endif //USE_JAC #endif //BT_SOLVER_BODY_H diff --git a/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h index 2c71360c5..1ebf08df6 100644 --- a/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h @@ -19,6 +19,7 @@ subject to the following restrictions: class btRigidBody; #include "LinearMath/btVector3.h" #include "LinearMath/btMatrix3x3.h" +#include "btJacobianEntry.h" //#define NO_FRICTION_TANGENTIALS 1 @@ -27,6 +28,11 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint { BT_DECLARE_ALIGNED_ALLOCATOR(); +#ifdef _USE_JACOBIAN + btJacobianEntry m_jac; +#endif + + btVector3 m_relpos1CrossNormal; btVector3 m_contactNormal; @@ -34,10 +40,10 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint btVector3 m_angularComponentA; btVector3 m_angularComponentB; - - mutable btScalar m_appliedPushImpulse; + mutable btScalar m_appliedPushImpulse; mutable btScalar m_appliedImpulse; + int m_solverBodyIdA; int m_solverBodyIdB; @@ -51,8 +57,10 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint int m_constraintType; int m_frictionIndex; void* m_originalContactPoint; - int m_unusedPadding[1]; - + btScalar m_rhs; + btScalar m_cfm; + btScalar m_lowerLimit; + btScalar m_upperLimit; enum btSolverConstraintType { diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 6b2090044..54f17c95b 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -113,7 +113,6 @@ void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) btRigidBody* body = btRigidBody::upcast(colObj); if (body) { - btTransform predictedTrans; if (body->getActivationState() != ISLAND_SLEEPING) { if (body->isKinematicObject()) diff --git a/src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.cpp b/src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.cpp index 24eba629d..23104fbea 100644 --- a/src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.cpp +++ b/src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.cpp @@ -449,10 +449,10 @@ btScalar solveContact(btSolverBody& body1,btSolverBody& body2,const btSolverCons 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); - body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass, + body2.applyImpulse(contactConstraint.m_contactNormal*body2.m_invMass, contactConstraint.m_angularComponentB,-normalImpulse); } @@ -1138,9 +1138,9 @@ void processSolverTask(void* userPtr, void* lsMemory) if (constraint.m_appliedImpulse!= 0.f) { 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) - 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); }