From 6dff5a218e6f11c4ea37107eca0757f3c608bcba Mon Sep 17 00:00:00 2001 From: ejcoumans Date: Tue, 12 Dec 2006 03:15:11 +0000 Subject: [PATCH] process contact and non-contact constraints inside the same iteration loop added first draft for hingeConstraint motor --- Demos/ConstraintDemo/ConstraintDemo.cpp | 20 +- Extras/quickstep/OdeConstraintSolver.cpp | 2 +- Extras/quickstep/OdeConstraintSolver.h | 2 +- .../btSimulationIslandManager.cpp | 39 +++- .../btSimulationIslandManager.h | 2 +- .../ConstraintSolver/btConstraintSolver.h | 4 +- .../btGeneric6DofConstraint.cpp | 2 +- .../ConstraintSolver/btHingeConstraint.cpp | 204 +++++++----------- .../ConstraintSolver/btHingeConstraint.h | 15 +- .../btSequentialImpulseConstraintSolver.cpp | 35 ++- .../btSequentialImpulseConstraintSolver.h | 4 +- .../Dynamics/btDiscreteDynamicsWorld.cpp | 122 +++++++---- .../Dynamics/btDiscreteDynamicsWorld.h | 6 +- .../Dynamics/btSimpleDynamicsWorld.cpp | 7 +- src/LinearMath/btScalar.h | 6 +- 15 files changed, 267 insertions(+), 203 deletions(-) diff --git a/Demos/ConstraintDemo/ConstraintDemo.cpp b/Demos/ConstraintDemo/ConstraintDemo.cpp index ac56fcf15..514f8f7de 100644 --- a/Demos/ConstraintDemo/ConstraintDemo.cpp +++ b/Demos/ConstraintDemo/ConstraintDemo.cpp @@ -84,6 +84,8 @@ void ConstraintDemo::initPhysics() btConstraintSolver* constraintSolver = new btSequentialImpulseConstraintSolver(); //ConstraintSolver* solver = new OdeConstraintSolver; m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver); + + //m_dynamicsWorld->setGravity(btVector3(0,0,0)); m_dynamicsWorld->setDebugDrawer(&debugDrawer); @@ -92,16 +94,16 @@ void ConstraintDemo::initPhysics() trans.setIdentity(); trans.setOrigin(btVector3(0,20,0)); - float mass = 0.f; + float mass = 1.f; //point to point constraint (ball socket) { btRigidBody* body0 = localCreateRigidBody( mass,trans,shape); trans.setOrigin(btVector3(2*CUBE_HALF_EXTENTS,20,0)); mass = 1.f; - btRigidBody* body1 = localCreateRigidBody( mass,trans,shape); - body1->setActivationState(DISABLE_DEACTIVATION); - body1->setDamping(0.3,0.3); + btRigidBody* body1 = 0;//localCreateRigidBody( mass,trans,shape); + //body1->setActivationState(DISABLE_DEACTIVATION); + //body1->setDamping(0.3,0.3); btVector3 pivotInA(CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS); btVector3 axisInA(0,0,1); @@ -112,7 +114,15 @@ void ConstraintDemo::initPhysics() body0->getCenterOfMassTransform().getBasis() * axisInA; //btTypedConstraint* p2p = new btPoint2PointConstraint(*body0,*body1,pivotInA,pivotInB); - btTypedConstraint* hinge = new btHingeConstraint(*body0,*body1,pivotInA,pivotInB,axisInA,axisInB); + //btTypedConstraint* hinge = new btHingeConstraint(*body0,*body1,pivotInA,pivotInB,axisInA,axisInB); + btHingeConstraint* hinge = new btHingeConstraint(*body0,pivotInA,axisInA); + + //use zero targetVelocity and a small maxMotorImpulse to simulate joint friction + //float targetVelocity = 0.f; + //float maxMotorImpulse = 0.01; + float targetVelocity = 1.f; + float maxMotorImpulse = 1.0f; + hinge->enableAngularMotor(true,targetVelocity,maxMotorImpulse); m_dynamicsWorld->addConstraint(hinge);//p2p); diff --git a/Extras/quickstep/OdeConstraintSolver.cpp b/Extras/quickstep/OdeConstraintSolver.cpp index bd6c2f877..cc3dd32b6 100644 --- a/Extras/quickstep/OdeConstraintSolver.cpp +++ b/Extras/quickstep/OdeConstraintSolver.cpp @@ -70,7 +70,7 @@ m_erp(0.4f) //iterative lcp and penalty method -float OdeConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +float OdeConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) { m_CurBody = 0; m_CurJoint = 0; diff --git a/Extras/quickstep/OdeConstraintSolver.h b/Extras/quickstep/OdeConstraintSolver.h index fb4c578fe..639cd63a5 100644 --- a/Extras/quickstep/OdeConstraintSolver.h +++ b/Extras/quickstep/OdeConstraintSolver.h @@ -45,7 +45,7 @@ public: virtual ~OdeConstraintSolver() {} - virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info,btIDebugDraw* debugDrawer = 0); + virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info,btIDebugDraw* debugDrawer = 0); ///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' diff --git a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index 9c55e234c..31bb54774 100644 --- a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -268,17 +268,44 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, int startManifoldIndex = 0; int endManifoldIndex = 1; - for (startManifoldIndex=0;startManifoldIndexProcessIsland(startManifold,numIslandManifolds, islandId); + if (numIslandManifolds) { - callback->ProcessIsland(&islandmanifold[startManifoldIndex],numIslandManifolds); + startManifoldIndex = endManifoldIndex; } } } diff --git a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h index 6f55e2476..2dd1a4830 100644 --- a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h +++ b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h @@ -49,7 +49,7 @@ public: { virtual ~IslandCallback() {}; - virtual void ProcessIsland(class btPersistentManifold** manifolds,int numManifolds) = 0; + virtual void ProcessIsland(class btPersistentManifold** manifolds,int numManifolds, int islandId) = 0; }; void buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback); diff --git a/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h index e073797f9..ce90f798c 100644 --- a/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h @@ -18,7 +18,7 @@ subject to the following restrictions: class btPersistentManifold; class btRigidBody; - +class btTypedConstraint; struct btContactSolverInfo; struct btBroadphaseProxy; class btIDebugDraw; @@ -31,7 +31,7 @@ public: virtual ~btConstraintSolver() {} - virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info,class btIDebugDraw* debugDrawer = 0) = 0; + virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer = 0) = 0; }; diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index b5783f824..b2132a8d4 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -289,7 +289,7 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) btScalar loLimit = m_upperLimit[i+3] > -3.1415 ? m_lowerLimit[i+3] : -1e30f; btScalar hiLimit = m_upperLimit[i+3] < 3.1415 ? m_upperLimit[i+3] : 1e30f; - float projAngle = -2.*xyz[i]; + float projAngle = -2.f*xyz[i]; if (projAngle < loLimit) { diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index 7b2a9ba62..f72278e2c 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -19,7 +19,8 @@ subject to the following restrictions: #include "LinearMath/btTransformUtil.h" -btHingeConstraint::btHingeConstraint() +btHingeConstraint::btHingeConstraint(): +m_enableAngularMotor(false) { } @@ -28,7 +29,8 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt :btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB), m_axisInA(axisInA), m_axisInB(-axisInB), -m_angularOnly(false) +m_angularOnly(false), +m_enableAngularMotor(false) { } @@ -39,7 +41,8 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA, m_axisInA(axisInA), //fixed axis in worldspace m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA), -m_angularOnly(false) +m_angularOnly(false), +m_enableAngularMotor(false) { } @@ -73,11 +76,16 @@ void btHingeConstraint::buildJacobian() //these two jointAxis require equal angular velocities for both bodies //this is unused for now, it's a todo - btVector3 axisWorldA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; - btVector3 jointAxis0; - btVector3 jointAxis1; - btPlaneSpace1(axisWorldA,jointAxis0,jointAxis1); + btVector3 jointAxis0local; + btVector3 jointAxis1local; + btPlaneSpace1(m_axisInA,jointAxis0local,jointAxis1local); + + getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; + btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; + btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; + btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; + new (&m_jacAng[0]) btJacobianEntry(jointAxis0, m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(), @@ -90,107 +98,18 @@ void btHingeConstraint::buildJacobian() m_rbA.getInvInertiaDiagLocal(), m_rbB.getInvInertiaDiagLocal()); + new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); + + } void btHingeConstraint::solveConstraint(btScalar timeStep) { -//#define NEW_IMPLEMENTATION - -#ifdef NEW_IMPLEMENTATION - btScalar tau = 0.3f; - btScalar damping = 1.f; - - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; - - // Dirk: Don't we need to update this after each applied impulse - btVector3 angvelA; // = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); - btVector3 angvelB; // = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); - - - if (!m_angularOnly) - { - btVector3 normal(0,0,0); - - for (int i=0;i<3;i++) - { - normal[i] = 1; - btScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal(); - - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - - btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - - // Dirk: Get new angular velocity since it changed after applying an impulse - angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); - angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); - - //velocity error (first order error) - btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, - m_rbB.getLinearVelocity(),angvelB); - - //positional error (zeroth order error) - btScalar depth = -(pivotAInW - pivotBInW).dot(normal); - - btScalar impulse = tau*depth/timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv; - - btVector3 impulse_vector = normal * impulse; - m_rbA.applyImpulse( impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); - m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); - - normal[i] = 0; - } - } - - ///solve angular part - - // get axes in world space - btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; - btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB; - - // constraint axes in world space - btVector3 jointAxis0; - btVector3 jointAxis1; - btPlaneSpace1(axisA,jointAxis0,jointAxis1); - - - // Dirk: Get new angular velocity since it changed after applying an impulse - angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); - angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); - - btScalar jacDiagABInv0 = 1.f / m_jacAng[0].getDiagonal(); - btScalar rel_vel0 = m_jacAng[0].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, - m_rbB.getLinearVelocity(),angvelB); - float tau1 = tau;//0.f; - - btScalar impulse0 = (tau1 * axisB.dot(jointAxis1) / timeStep - damping * rel_vel0) * jacDiagABInv0; - btVector3 angular_impulse0 = jointAxis0 * impulse0; - - m_rbA.applyTorqueImpulse( angular_impulse0); - m_rbB.applyTorqueImpulse(-angular_impulse0); - - - - // Dirk: Get new angular velocity since it changed after applying an impulse - angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); - angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); - - btScalar jacDiagABInv1 = 1.f / m_jacAng[1].getDiagonal(); - btScalar rel_vel1 = m_jacAng[1].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, - m_rbB.getLinearVelocity(),angvelB);; - - btScalar impulse1 = -(tau1 * axisB.dot(jointAxis0) / timeStep + damping * rel_vel1) * jacDiagABInv1; - btVector3 angular_impulse1 = jointAxis1 * impulse1; - - m_rbA.applyTorqueImpulse( angular_impulse1); - m_rbB.applyTorqueImpulse(-angular_impulse1); - -#else - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; @@ -237,37 +156,68 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); - btVector3 angA = angVelA - axisA * axisA.dot(angVelA); - btVector3 angB = angVelB - axisB * axisB.dot(angVelB); - btVector3 velrel = angA-angB; - //solve angular velocity correction - float relaxation = 1.f; - float len = velrel.length(); - if (len > 0.00001f) + btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); + btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); + + btVector3 angAorthog = angVelA - angVelAroundHingeAxisA; + btVector3 angBorthog = angVelB - angVelAroundHingeAxisB; + btVector3 velrelOrthog = angAorthog-angBorthog; { - btVector3 normal = velrel.normalized(); - float denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + - getRigidBodyB().computeAngularImpulseDenominator(normal); - // scale for mass and relaxation - velrel *= (1.f/denom) * 0.9; + //solve orthogonal angular velocity correction + float relaxation = 1.f; + float len = velrelOrthog.length(); + if (len > 0.00001f) + { + btVector3 normal = velrelOrthog.normalized(); + float denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + + getRigidBodyB().computeAngularImpulseDenominator(normal); + // scale for mass and relaxation + //todo: expose this 0.9 factor to developer + velrelOrthog *= (1.f/denom) * 0.9f; + } + + //solve angular positional correction + btVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep); + float len2 = angularError.length(); + if (len2>0.00001f) + { + btVector3 normal2 = angularError.normalized(); + float denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + + getRigidBodyB().computeAngularImpulseDenominator(normal2); + angularError *= (1.f/denom2) * relaxation; + } + + m_rbA.applyTorqueImpulse(-velrelOrthog+angularError); + m_rbB.applyTorqueImpulse(velrelOrthog-angularError); } - //solve angular positional correction - btVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep); - float len2 = angularError.length(); - if (len2>0.00001f) + //apply motor + if (m_enableAngularMotor) { - btVector3 normal2 = angularError.normalized(); - float denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + - getRigidBodyB().computeAngularImpulseDenominator(normal2); - angularError *= (1.f/denom2) * relaxation; - } + //todo: add limits too + btVector3 angularLimit(0,0,0); - m_rbA.applyTorqueImpulse(-velrel+angularError); - m_rbB.applyTorqueImpulse(velrel-angularError); + btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; + btScalar projRelVel = velrel.dot(axisA); + + btScalar desiredMotorVel = m_motorTargetVelocity; + btScalar motor_relvel = desiredMotorVel - projRelVel; + + float denom3 = getRigidBodyA().computeAngularImpulseDenominator(axisA) + + getRigidBodyB().computeAngularImpulseDenominator(axisA); + + btScalar unclippedMotorImpulse = (1.f/denom3) * motor_relvel;; + //todo: should clip against accumulated impulse + btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; + clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; + btVector3 motorImp = clippedMotorImpulse * axisA; + + m_rbA.applyTorqueImpulse(motorImp+angularLimit); + m_rbB.applyTorqueImpulse(-motorImp-angularLimit); + + } } -#endif } diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 3bade2b09..553ec135c 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -29,14 +29,18 @@ class btRigidBody; class btHingeConstraint : public btTypedConstraint { btJacobianEntry m_jac[3]; //3 orthogonal linear constraints - btJacobianEntry m_jacAng[2]; //2 orthogonal angular constraints + btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor btVector3 m_pivotInA; btVector3 m_pivotInB; btVector3 m_axisInA; btVector3 m_axisInB; - bool m_angularOnly; + bool m_angularOnly; + + float m_motorTargetVelocity; + float m_maxMotorImpulse; + bool m_enableAngularMotor; public: @@ -66,7 +70,12 @@ public: m_angularOnly = angularOnly; } - + void enableAngularMotor(bool enableMotor,float targetVelocity,float maxMotorImpulse) + { + m_enableAngularMotor = enableMotor; + m_motorTargetVelocity = targetVelocity; + m_maxMotorImpulse = maxMotorImpulse; + } }; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index ce85fc398..94eece73e 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -23,6 +23,8 @@ subject to the following restrictions: #include "LinearMath/btIDebugDraw.h" #include "btJacobianEntry.h" #include "LinearMath/btMinMax.h" +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" + #ifdef USE_PROFILE #include "LinearMath/btQuickprof.h" @@ -123,7 +125,7 @@ btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() } /// btSequentialImpulseConstraintSolver Sequentially applies impulses -float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) { btContactSolverInfo info = infoGlobal; @@ -151,6 +153,15 @@ float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** ma } } } + + { + int j; + for (j=0;jbuildJacobian(); + } + } //should traverse the contacts random order... int iteration; @@ -171,6 +182,12 @@ float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** ma } } + for (j=0;jsolveConstraint(info.m_timeStep); + } + for (j=0;jbuildJacobian(); + } + } //should traverse the contacts random order... int iteration; @@ -230,6 +255,12 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man { int j; + for (j=0;jsolveConstraint(info.m_timeStep); + } + for (j=0;jgetRigidBodyA(); + const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); + islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag(); + return islandId; -void btDiscreteDynamicsWorld::solveContactConstraints(btContactSolverInfo& solverInfo) +} + +static bool btSortConstraintOnIslandPredicate(const btTypedConstraint* lhs, const btTypedConstraint* rhs) +{ + int rIslandId0,lIslandId0; + rIslandId0 = btGetConstraintIslandId(rhs); + lIslandId0 = btGetConstraintIslandId(lhs); + return lIslandId0 < rIslandId0; +} + +void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { - BEGIN_PROFILE("solveContactConstraints"); + BEGIN_PROFILE("solveConstraints"); struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback { - btContactSolverInfo& m_solverInfo; - btConstraintSolver* m_solver; - btIDebugDraw* m_debugDrawer; + btContactSolverInfo& m_solverInfo; + btConstraintSolver* m_solver; + btTypedConstraint** m_sortedConstraints; + int m_numConstraints; + btIDebugDraw* m_debugDrawer; + + InplaceSolverIslandCallback( btContactSolverInfo& solverInfo, btConstraintSolver* solver, + btTypedConstraint** sortedConstraints, + int numConstraints, btIDebugDraw* debugDrawer) :m_solverInfo(solverInfo), m_solver(solver), + m_sortedConstraints(sortedConstraints), + m_numConstraints(numConstraints), m_debugDrawer(debugDrawer) { } - virtual void ProcessIsland(btPersistentManifold** manifolds,int numManifolds) + virtual void ProcessIsland(btPersistentManifold** manifolds,int numManifolds, int islandId) { - m_solver->solveGroup( manifolds, numManifolds,m_solverInfo,m_debugDrawer); + //also add all non-contact constraints/joints for this island + btTypedConstraint** startConstraint = 0; + int numCurConstraints = 0; + int i; + + //find the first constraint for this island + for (i=0;isolveGroup( manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer); } }; - InplaceSolverIslandCallback solverCallback( solverInfo, m_constraintSolver, m_debugDrawer); - /// solve all the contact points and contact friction + + //sorted version of all btTypedConstraint, based on islandId + std::vector sortedConstraints; + sortedConstraints.resize( m_constraints.size()); + int i; + for (i=0;ibuildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback); - END_PROFILE("solveContactConstraints"); + END_PROFILE("solveConstraints"); } -void btDiscreteDynamicsWorld::solveNoncontactConstraints(btContactSolverInfo& solverInfo) -{ - BEGIN_PROFILE("solveNoncontactConstraints"); - int i; - int numConstraints = int(m_constraints.size()); - - ///constraint preparation: building jacobians - for (i=0;i< numConstraints ; i++ ) - { - btTypedConstraint* constraint = m_constraints[i]; - constraint->buildJacobian(); - } - - //solve the regular non-contact constraints (point 2 point, hinge, generic d6) - for (int g=0;gsolveConstraint( solverInfo.m_timeStep ); - } - } - - END_PROFILE("solveNoncontactConstraints"); - -} void btDiscreteDynamicsWorld::calculateSimulationIslands() { diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index e5cd4438f..9a1676af9 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -66,10 +66,8 @@ protected: void calculateSimulationIslands(); - void solveNoncontactConstraints(btContactSolverInfo& solverInfo); - - void solveContactConstraints(btContactSolverInfo& solverInfo); - + void solveConstraints(btContactSolverInfo& solverInfo); + void updateActivationState(float timeStep); void updateVehicles(float timeStep); diff --git a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index c129dc7e7..9e0d37712 100644 --- a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -63,7 +63,7 @@ int btSimpleDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, floa btContactSolverInfo infoGlobal; infoGlobal.m_timeStep = timeStep; - m_constraintSolver->solveGroup(manifoldPtr, numManifolds,infoGlobal,m_debugDrawer); + m_constraintSolver->solveGroup(manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer); } ///integrate transforms @@ -101,7 +101,10 @@ void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body) { body->setGravity(m_gravity); - addCollisionObject(body); + if (body->getCollisionShape()) + { + addCollisionObject(body); + } } void btSimpleDynamicsWorld::updateAabbs() diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h index 215eed4cd..dd76fb2de 100644 --- a/src/LinearMath/btScalar.h +++ b/src/LinearMath/btScalar.h @@ -54,8 +54,10 @@ subject to the following restrictions: typedef float btScalar; -#if defined (__sun) || defined (__sun__) || defined (__sparc) || defined (__APPLE__) -//use double float precision operation on those platforms for Blender +///older compilers (gcc 3.x) and Sun needs double versions of srqt etc. +///exclude Apple Intel (it's assumed to be a Macbook or newer Intel Dual Core processor) +#if defined (__sun) || defined (__sun__) || defined (__sparc) || (defined (__APPLE__) && ! defined (__i386__)) +//use slow double float precision operation on those platforms SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrt(x); } SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); }