diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index 6745e23da..6bacf5dae 100644 --- a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -92,6 +92,10 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b newPt.m_combinedFriction = calculateCombinedFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); + btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2); + + + //BP mod, store contact triangles. if (isSwapped) { diff --git a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h index 0ce9dd259..0ee3c8f40 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -76,9 +76,7 @@ class btManifoldPoint m_contactCFM2(0.f), m_lifeTime(0) { - mConstraintRow[0].m_accumImpulse = 0.f; - mConstraintRow[1].m_accumImpulse = 0.f; - mConstraintRow[2].m_accumImpulse = 0.f; + } @@ -94,16 +92,16 @@ class btManifoldPoint btScalar m_combinedFriction; btScalar m_combinedRestitution; - //BP mod, store contact triangles. - int m_partId0; - int m_partId1; - int m_index0; - int m_index1; + //BP mod, store contact triangles. + int m_partId0; + int m_partId1; + int m_index0; + int m_index1; mutable void* m_userPersistentData; - btScalar m_appliedImpulse; - bool m_lateralFrictionInitialized; + + btScalar m_appliedImpulse; btScalar m_appliedImpulseLateral1; btScalar m_appliedImpulseLateral2; btScalar m_contactMotion1; @@ -118,8 +116,6 @@ class btManifoldPoint - btConstraintRow mConstraintRow[3]; - btScalar getDistance() const { diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index a5df9e770..80e05a4f2 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -143,10 +143,6 @@ public: m_pointCache[index] = m_pointCache[lastUsedIndex]; //get rid of duplicated userPersistentData pointer m_pointCache[lastUsedIndex].m_userPersistentData = 0; - m_pointCache[lastUsedIndex].mConstraintRow[0].m_accumImpulse = 0.f; - m_pointCache[lastUsedIndex].mConstraintRow[1].m_accumImpulse = 0.f; - m_pointCache[lastUsedIndex].mConstraintRow[2].m_accumImpulse = 0.f; - m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f; m_pointCache[lastUsedIndex].m_lateralFrictionInitialized = false; m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f; @@ -164,9 +160,9 @@ public: #define MAINTAIN_PERSISTENCY 1 #ifdef MAINTAIN_PERSISTENCY int lifeTime = m_pointCache[insertIndex].getLifeTime(); - btScalar appliedImpulse = m_pointCache[insertIndex].mConstraintRow[0].m_accumImpulse; - btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].mConstraintRow[1].m_accumImpulse; - btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].mConstraintRow[2].m_accumImpulse; + btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse; + btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1; + btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2; // bool isLateralFrictionInitialized = m_pointCache[insertIndex].m_lateralFrictionInitialized; @@ -181,9 +177,9 @@ public: m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1; m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2; - m_pointCache[insertIndex].mConstraintRow[0].m_accumImpulse = appliedImpulse; - m_pointCache[insertIndex].mConstraintRow[1].m_accumImpulse = appliedLateralImpulse1; - m_pointCache[insertIndex].mConstraintRow[2].m_accumImpulse = appliedLateralImpulse2; + m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse; + m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1; + m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2; m_pointCache[insertIndex].m_lifeTime = lifeTime; diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index 755544f0d..d6bc2ee7b 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -304,7 +304,7 @@ void btConeTwistConstraint::buildJacobian() -void btConeTwistConstraint::solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar timeStep) +void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) { #ifndef __SPU__ if (m_useSolveConstraintObsolete) diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h index 861dea2f0..581b711a5 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -142,7 +142,7 @@ public: void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB); - virtual void solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar timeStep); + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); void updateRHS(btScalar timeStep); diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 6204cb3d1..44e5554c6 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -16,18 +16,20 @@ subject to the following restrictions: #ifndef BT_CONTACT_SOLVER_INFO #define BT_CONTACT_SOLVER_INFO +#include "LinearMath/btScalar.h" + enum btSolverMode { SOLVER_RANDMIZE_ORDER = 1, SOLVER_FRICTION_SEPARATE = 2, SOLVER_USE_WARMSTARTING = 4, - SOLVER_USE_FRICTION_WARMSTARTING = 8, SOLVER_USE_2_FRICTION_DIRECTIONS = 16, SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32, SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64, SOLVER_CACHE_FRIENDLY = 128, - SOLVER_SIMD = 256, //enabled for Windows, the solver innerloop is branchless SIMD, 40% faster than FPU/scalar version - SOLVER_CUDA = 512 //will be open sourced during Game Developers Conference 2009. Much faster. + SOLVER_SIMD = 256, + SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512, + SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024 }; struct btContactSolverInfoData @@ -67,6 +69,7 @@ struct btContactSolverInfo : public btContactSolverInfoData m_tau = btScalar(0.6); m_damping = btScalar(1.0); m_friction = btScalar(0.3); + m_timeStep = btScalar(1.f/60.f); m_restitution = btScalar(0.); m_maxErrorReduction = btScalar(20.); m_numIterations = 10; @@ -74,12 +77,12 @@ struct btContactSolverInfo : public btContactSolverInfoData m_erp2 = btScalar(0.1); m_globalCfm = btScalar(0.); m_sor = btScalar(1.); - m_splitImpulse = false; - m_splitImpulsePenetrationThreshold = -0.02f; + m_splitImpulse = true; + m_splitImpulsePenetrationThreshold = -.04f; m_linearSlop = btScalar(0.0); m_warmstartingFactor=btScalar(0.85); + //m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD | SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|SOLVER_USE_2_FRICTION_DIRECTIONS|SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | SOLVER_RANDMIZE_ORDER; m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER; - m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit } }; diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index 8ff9940bb..d4b4a9ad4 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -174,10 +174,8 @@ btScalar btRotationalLimitMotor::solveAngularLimits( // current velocity difference - btVector3 angVelA; - body0->internalGetAngularVelocity(angVelA); - btVector3 angVelB; - body1->internalGetAngularVelocity(angVelB); + btVector3 angVelA = body0->getAngularVelocity(); + btVector3 angVelB = body1->getAngularVelocity(); btVector3 vel_diff; vel_diff = angVelA-angVelB; @@ -225,12 +223,8 @@ btScalar btRotationalLimitMotor::solveAngularLimits( btVector3 motorImp = clippedMotorImpulse * axis; - //body0->applyTorqueImpulse(motorImp); - //body1->applyTorqueImpulse(-motorImp); - - body0->internalApplyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse); - body1->internalApplyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse); - + body0->applyTorqueImpulse(motorImp); + body1->applyTorqueImpulse(-motorImp); return clippedMotorImpulse; @@ -292,10 +286,8 @@ btScalar btTranslationalLimitMotor::solveLinearAxis( btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition(); btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition(); - btVector3 vel1; - body1.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1); - btVector3 vel2; - body2.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2); + btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); btVector3 vel = vel1 - vel2; btScalar rel_vel = axis_normal_on_a.dot(vel); @@ -348,16 +340,10 @@ btScalar btTranslationalLimitMotor::solveLinearAxis( normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; btVector3 impulse_vector = axis_normal_on_a * normalImpulse; - //body1.applyImpulse( impulse_vector, rel_pos1); - //body2.applyImpulse(-impulse_vector, rel_pos2); - - btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a); - btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a); - body1.internalApplyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); - body2.internalApplyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); - - + body1.applyImpulse( impulse_vector, rel_pos1); + body2.applyImpulse(-impulse_vector, rel_pos2); + return normalImpulse; } diff --git a/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h b/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h index f1994a2df..125580d19 100644 --- a/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h +++ b/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h @@ -16,8 +16,7 @@ subject to the following restrictions: #ifndef BT_JACOBIAN_ENTRY_H #define BT_JACOBIAN_ENTRY_H -#include "LinearMath/btVector3.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btMatrix3x3.h" //notes: diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 17cf92d5d..08c8ef304 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -18,24 +18,23 @@ subject to the following restrictions: #include "btSequentialImpulseConstraintSolver.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "btContactConstraint.h" -#include "btSolve2LinearConstraint.h" -#include "btContactSolverInfo.h" + #include "LinearMath/btIDebugDraw.h" -#include "btJacobianEntry.h" +//#include "btJacobianEntry.h" #include "LinearMath/btMinMax.h" #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" #include #include "LinearMath/btStackAlloc.h" #include "LinearMath/btQuickprof.h" -#include "btSolverBody.h" -#include "btSolverConstraint.h" +//#include "btSolverBody.h" +//#include "btSolverConstraint.h" #include "LinearMath/btAlignedObjectArray.h" #include //for memset int gNumSplitImpulseRecoveries = 0; +#include "BulletDynamics/Dynamics/btRigidBody.h" + btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() :m_btSeed2(0) { @@ -57,7 +56,7 @@ static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 ) #endif//USE_SIMD // Project Gauss Seidel or the equivalent Sequential Impulse -void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) +void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { #ifdef USE_SIMD __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); @@ -91,7 +90,7 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD( } // Project Gauss Seidel or the equivalent Sequential Impulse - void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) + void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); @@ -120,7 +119,7 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD( body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); } - void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) + void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { #ifdef USE_SIMD __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); @@ -151,7 +150,7 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD( } // Project Gauss Seidel or the equivalent Sequential Impulse - void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) + void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); @@ -175,8 +174,8 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD( void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly( - btRigidBody& body1, - btRigidBody& body2, + btSolverBody& body1, + btSolverBody& body2, const btSolverConstraint& c) { if (c.m_rhsPenetration) @@ -203,7 +202,7 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri } } - void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) + void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { #ifdef USE_SIMD if (!c.m_rhsPenetration) @@ -277,9 +276,10 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n) } -#if 0 + void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject) { + btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0; solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); @@ -289,17 +289,27 @@ void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBod if (rb) { - solverBody->internalGetInvMass() = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor(); + solverBody->m_worldTransform = rb->getWorldTransform(); + solverBody->internalSetInvMass(btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor()); solverBody->m_originalBody = rb; solverBody->m_angularFactor = rb->getAngularFactor(); + solverBody->m_linearFactor = rb->getLinearFactor(); + solverBody->m_linearVelocity = rb->getLinearVelocity(); + solverBody->m_angularVelocity = rb->getAngularVelocity(); } else { - solverBody->internalGetInvMass().setValue(0,0,0); + solverBody->m_worldTransform.setIdentity(); + solverBody->internalSetInvMass(btVector3(0,0,0)); solverBody->m_originalBody = 0; solverBody->m_angularFactor.setValue(1,1,1); + solverBody->m_linearFactor.setValue(1,1,1); + solverBody->m_linearVelocity.setValue(0,0,0); + solverBody->m_angularVelocity.setValue(0,0,0); } + + } -#endif + @@ -313,9 +323,11 @@ btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, -void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection); -void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection) +static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection); +static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection) { + + if (colObj && colObj->hasAnisotropicFriction()) { // transform to local coordinates @@ -326,20 +338,23 @@ void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirec // ... and transform it back to global coordinates frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral; } + } -void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) +void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) { - - btRigidBody* body0=btRigidBody::upcast(colObj0); - btRigidBody* body1=btRigidBody::upcast(colObj1); - + solverConstraint.m_contactNormal = normalAxis; + btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; - solverConstraint.m_solverBodyA = body0 ? body0 : &getFixedBody(); - solverConstraint.m_solverBodyB = body1 ? body1 : &getFixedBody(); + btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody; + btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody; + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; solverConstraint.m_friction = cp.m_combinedFriction; solverConstraint.m_originalContactPoint = 0; @@ -392,11 +407,13 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr { + + btScalar rel_vel; - btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0)) - + solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0)); - btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0)) - + solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0)); + btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?solverBodyA.m_linearVelocity:btVector3(0,0,0)) + + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0)); + btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?solverBodyB.m_linearVelocity:btVector3(0,0,0)) + + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0)); rel_vel = vel1Dotn+vel2Dotn; @@ -408,23 +425,24 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr solverConstraint.m_cfm = cfmSlip; solverConstraint.m_lowerLimit = 0; solverConstraint.m_upperLimit = 1e10f; + } } -btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) +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, btScalar desiredVelocity, btScalar cfmSlip) { btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing(); solverConstraint.m_frictionIndex = frictionIndex; - setupFrictionConstraint(solverConstraint, normalAxis, solverBodyA, solverBodyB, cp, rel_pos1, rel_pos2, + setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); return solverConstraint; } int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body) { -#if 0 + int solverBodyIdA = -1; if (body.getCompanionId() >= 0) @@ -445,29 +463,33 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& return 0;//assume first one is a fixed solver body } } + return solverBodyIdA; -#endif - return 0; + } #include void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, - btCollisionObject* colObj0, btCollisionObject* colObj1, + int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btVector3& vel, btScalar& rel_vel, btScalar& relaxation, btVector3& rel_pos1, btVector3& rel_pos2) { - btRigidBody* rb0 = btRigidBody::upcast(colObj0); - btRigidBody* rb1 = btRigidBody::upcast(colObj1); - + const btVector3& pos1 = cp.getPositionWorldOnA(); const btVector3& pos2 = cp.getPositionWorldOnB(); + btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; + + btRigidBody* rb0 = bodyA->m_originalBody; + btRigidBody* rb1 = bodyB->m_originalBody; + // btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); // btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); - rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); - rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); + rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); relaxation = 1.f; @@ -501,29 +523,27 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra } solverConstraint.m_contactNormal = cp.m_normalWorldOnB; - solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB); - solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB); - - - - - btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); - btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); - vel = vel1 - vel2; - rel_vel = cp.m_normalWorldOnB.dot(vel); - - btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop; - - - solverConstraint.m_friction = cp.m_combinedFriction; + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_relpos2CrossNormal = -torqueAxis1; btScalar restitution = 0.f; + btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop; + + { + btVector3 vel1,vel2; + + vel1 = rb0? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); + vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); + + // btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); + vel = vel1 - vel2; + rel_vel = cp.m_normalWorldOnB.dot(vel); + + + + solverConstraint.m_friction = cp.m_combinedFriction; + - if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold) - { - restitution = 0.f; - } else - { restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); if (restitution <= btScalar(0.)) { @@ -537,9 +557,9 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra { solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; if (rb0) - rb0->internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); + bodyA->internalApplyImpulse(solverConstraint.m_contactNormal*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); if (rb1) - rb1->internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); + bodyB->internalApplyImpulse(solverConstraint.m_contactNormal*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); } else { solverConstraint.m_appliedImpulse = 0.f; @@ -548,33 +568,41 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra 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 vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?bodyA->m_linearVelocity:btVector3(0,0,0)) + + solverConstraint.m_relpos1CrossNormal.dot(rb0?bodyA->m_angularVelocity:btVector3(0,0,0)); + btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?bodyB->m_linearVelocity:btVector3(0,0,0)) + + solverConstraint.m_relpos2CrossNormal.dot(rb1?bodyB->m_angularVelocity:btVector3(0,0,0)); + btScalar rel_vel = vel1Dotn+vel2Dotn; btScalar positionalError = 0.f; btScalar velocityError = restitution - rel_vel;// * damping; + + + btScalar erp = infoGlobal.m_erp2; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + erp = infoGlobal.m_erp; + } if (penetration>0) { positionalError = 0; + velocityError -= penetration / infoGlobal.m_timeStep; } else { - positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep; + positionalError = -penetration * erp/infoGlobal.m_timeStep; } btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) { //combine position and velocity into rhs solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; solverConstraint.m_rhsPenetration = 0.f; + } else { //split position and velocity into rhs and m_rhsPenetration @@ -594,51 +622,46 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, - btRigidBody* rb0, btRigidBody* rb1, + int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal) { - if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) - { - { - btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; - if (rb0) - rb0->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); - if (rb1) - rb1->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse); - } else - { - frictionConstraint1.m_appliedImpulse = 0.f; - } - } - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; - if (rb0) - rb0->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); - if (rb1) - rb1->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse); - } else - { - frictionConstraint2.m_appliedImpulse = 0.f; - } - } - } else - { - btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; - frictionConstraint1.m_appliedImpulse = 0.f; - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; - frictionConstraint2.m_appliedImpulse = 0.f; - } - } + btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; + + btRigidBody* rb0 = bodyA->m_originalBody; + btRigidBody* rb1 = bodyB->m_originalBody; + + { + btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; + if (rb0) + bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); + if (rb1) + bodyB->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse); + } else + { + frictionConstraint1.m_appliedImpulse = 0.f; + } + } + + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; + if (rb0) + bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); + if (rb1) + bodyB->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse); + } else + { + frictionConstraint2.m_appliedImpulse = 0.f; + } + } } @@ -651,12 +674,19 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m colObj0 = (btCollisionObject*)manifold->getBody0(); colObj1 = (btCollisionObject*)manifold->getBody1(); + int solverBodyIdA = getOrInitSolverBody(*colObj0); + int solverBodyIdB = getOrInitSolverBody(*colObj1); + + btRigidBody* bodyA = btRigidBody::upcast(colObj0); + btRigidBody* bodyB = btRigidBody::upcast(colObj1); + + btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB]; + - btRigidBody* solverBodyA = btRigidBody::upcast(colObj0); - btRigidBody* solverBodyB = btRigidBody::upcast(colObj1); ///avoid collision response between two static objects - if ((!solverBodyA || !solverBodyA->getInvMass()) && (!solverBodyB || !solverBodyB->getInvMass())) + if (!solverBodyA || !solverBodyA->m_originalBody && (!solverBodyB || !solverBodyB->m_originalBody)) return; for (int j=0;jgetNumContacts();j++) @@ -676,11 +706,12 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing(); btRigidBody* rb0 = btRigidBody::upcast(colObj0); btRigidBody* rb1 = btRigidBody::upcast(colObj1); - solverConstraint.m_solverBodyA = rb0? rb0 : &getFixedBody(); - solverConstraint.m_solverBodyB = rb1? rb1 : &getFixedBody(); + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_originalContactPoint = &cp; - setupContactConstraint(solverConstraint, colObj0, colObj1, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2); + setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2); // const btVector3& pos1 = cp.getPositionWorldOnA(); // const btVector3& pos2 = cp.getPositionWorldOnB(); @@ -695,46 +726,47 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) { - cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel); + cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel); if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); cp.m_lateralFrictionDir2.normalize();//?? applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); } applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); cp.m_lateralFrictionInitialized = true; } else { - //re-calculate friction direction every frame, todo: check if this is really needed - btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + if (!(infoGlobal.m_solverMode & SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS)) + btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); } applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); cp.m_lateralFrictionInitialized = true; } } else { - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2); + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2); } - setFrictionConstraintImpulse( solverConstraint, rb0, rb1, cp, infoGlobal); + setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); } } @@ -748,39 +780,34 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol m_maxOverrideNumSolverIterations = 0; - if (!(numConstraints + numManifolds)) + + + for (int i = 0; i < numBodies; i++) { - // printf("empty\n"); - return 0.f; + bodies[i]->setCompanionId(-1); } - if (infoGlobal.m_splitImpulse) - { - for (int i = 0; i < numBodies; i++) - { - btRigidBody* body = btRigidBody::upcast(bodies[i]); - if (body) - { - body->internalGetDeltaLinearVelocity().setZero(); - body->internalGetDeltaAngularVelocity().setZero(); - body->internalGetPushVelocity().setZero(); - body->internalGetTurnVelocity().setZero(); - } - } - } - else - { - for (int i = 0; i < numBodies; i++) - { - btRigidBody* body = btRigidBody::upcast(bodies[i]); - if (body) - { - body->internalGetDeltaLinearVelocity().setZero(); - body->internalGetDeltaAngularVelocity().setZero(); - } - } - } + m_tmpSolverBodyPool.reserve(numBodies+1); + m_tmpSolverBodyPool.resize(0); + + btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&fixedBody,0); + + //convert all bodies + + for (int i=0;igetInvMass()) + { + btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; + solverBody.m_linearVelocity += body->getTotalForce()*body->getInvMass()*infoGlobal.m_timeStep; + solverBody.m_angularVelocity += body->getTotalTorque()*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep; + } + } + if (1) { int j; @@ -791,6 +818,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol constraint->internalSetAppliedImpulse(0.0f); } } + //btRigidBody* rb0=0,*rb1=0; //if (1) @@ -834,6 +862,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btRigidBody& rbA = constraint->getRigidBodyA(); btRigidBody& rbB = constraint->getRigidBodyB(); + int solverBodyIdA = getOrInitSolverBody(rbA); + int solverBodyIdB = getOrInitSolverBody(rbB); + + btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; + + + int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations) @@ -848,16 +884,19 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol currentConstraintRow[j].m_upperLimit = SIMD_INFINITY; currentConstraintRow[j].m_appliedImpulse = 0.f; currentConstraintRow[j].m_appliedPushImpulse = 0.f; - currentConstraintRow[j].m_solverBodyA = &rbA; - currentConstraintRow[j].m_solverBodyB = &rbB; + currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA; + currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB; currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations; } - rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); - rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); - rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); - rbB.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); - + bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); + bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); + bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); + bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); + bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); + bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); + bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); + bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); btTypedConstraint::btConstraintInfo2 info2; @@ -967,7 +1006,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool); - m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool); + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool*2); + else + m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool); + m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool); { int i; @@ -996,12 +1039,12 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration int numConstraintPool = m_tmpSolverContactConstraintPool.size(); int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); - int j; - + if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) { if ((iteration & 7) == 0) { - for (j=0; jsolveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); } ///solve all contact constraints using SIMD, if available - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - for (j=0;jbtScalar(0)) + for (int c=0;cbtScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + + resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + } + } + + if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) + { + + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]]; + + if (totalImpulse>btScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + + resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + } + } + } } + } + else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS + { + //solve the friction constraints after all contact constraints, don't interleave them + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + int j; + + for (j=0;jbtScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + + resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + } + } + } } } else { - + //non-SIMD version ///solve all joint constraints - for (j=0;jsolveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); } ///solve all contact constraints int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - for (j=0;jm_appliedImpulse = solveManifold.m_appliedImpulse; - if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) + for (j=0;jm_appliedImpulse = solveManifold.m_appliedImpulse; + // float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + // printf("pt->m_appliedImpulseLateral1 = %f\n", f); pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse; + //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse; + } + //do a callback here? } - - //do a callback here? } numPoolConstraints = m_tmpSolverNonContactConstraintPool.size(); @@ -1209,29 +1309,41 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo } + if (infoGlobal.m_splitImpulse) { - for ( i=0;iinternalWritebackVelocity(infoGlobal.m_timeStep); + { + m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep); + m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity); + m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity); + m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform); + + } } } else { - for ( i=0;iinternalWritebackVelocity(); + { + m_tmpSolverBodyPool[i].writebackVelocity(); + m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity); + m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity); + } } } + m_tmpSolverContactConstraintPool.resizeNoInitialize(0); m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0); - + m_tmpSolverBodyPool.resizeNoInitialize(0); return 0.f; } @@ -1243,14 +1355,12 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod BT_PROFILE("solveGroup"); //you need to provide at least some bodies - btAssert(bodies); - btAssert(numBodies); - + solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); - solveGroupCacheFriendlyFinish(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); + solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal); return 0.f; } @@ -1260,10 +1370,4 @@ void btSequentialImpulseConstraintSolver::reset() m_btSeed2 = 0; } -btRigidBody& btSequentialImpulseConstraintSolver::getFixedBody() -{ - static btRigidBody s_fixed(0, 0,0); - s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); - return s_fixed; -} diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 94dbe27ff..e9e7955cc 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -16,19 +16,23 @@ subject to the following restrictions: #ifndef BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H #define BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H -#include "btConstraintSolver.h" class btIDebugDraw; -#include "btContactConstraint.h" -#include "btSolverBody.h" -#include "btSolverConstraint.h" -#include "btTypedConstraint.h" +class btPersistentManifold; +class btStackAlloc; +class btDispatcher; +class btCollisionObject; +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" +#include "BulletDynamics/ConstraintSolver/btSolverBody.h" +#include "BulletDynamics/ConstraintSolver/btSolverConstraint.h" #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" +#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h" ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method. ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolver : public btConstraintSolver { protected: - + btAlignedObjectArray m_tmpSolverBodyPool; btConstraintArray m_tmpSolverContactConstraintPool; btConstraintArray m_tmpSolverNonContactConstraintPool; btConstraintArray m_tmpSolverContactFrictionConstraintPool; @@ -38,55 +42,54 @@ protected: btAlignedObjectArray m_tmpConstraintSizesPool; int m_maxOverrideNumSolverIterations; - void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyIdB, + void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB, btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.); - btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.); + 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, btScalar desiredVelocity=0., btScalar cfmSlip=0.); - void setupContactConstraint(btSolverConstraint& solverConstraint, btCollisionObject* colObj0, btCollisionObject* colObj1, btManifoldPoint& cp, + void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btVector3& vel, btScalar& rel_vel, btScalar& relaxation, btVector3& rel_pos1, btVector3& rel_pos2); - void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, btRigidBody* rb0, btRigidBody* rb1, + void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal); ///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 convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); void resolveSplitPenetrationSIMD( - btRigidBody& body1, - btRigidBody& body2, + btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint); void resolveSplitPenetrationImpulseCacheFriendly( - btRigidBody& body1, - btRigidBody& body2, + btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint); //internal method - int getOrInitSolverBody(btCollisionObject& body); + int getOrInitSolverBody(btCollisionObject& body); + void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject); - void resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint); + void resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); - void resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint); + void resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); - void resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint); + void resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); - void resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint); + void resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); protected: - static btRigidBody& getFixedBody(); + virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); - virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); + virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal); btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); @@ -122,9 +125,7 @@ public: }; -#ifndef BT_PREFER_SIMD -typedef btSequentialImpulseConstraintSolver btSequentialImpulseConstraintSolverPrefered; -#endif + #endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H diff --git a/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/src/BulletDynamics/ConstraintSolver/btSolverBody.h index 8de515812..75cfe44ad 100644 --- a/src/BulletDynamics/ConstraintSolver/btSolverBody.h +++ b/src/BulletDynamics/ConstraintSolver/btSolverBody.h @@ -19,7 +19,7 @@ subject to the following restrictions: class btRigidBody; #include "LinearMath/btVector3.h" #include "LinearMath/btMatrix3x3.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" + #include "LinearMath/btAlignedAllocator.h" #include "LinearMath/btTransformUtil.h" @@ -105,22 +105,35 @@ operator+(const btSimdScalar& v1, const btSimdScalar& v2) #endif ///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. -ATTRIBUTE_ALIGNED64 (struct) btSolverBodyObsolete +ATTRIBUTE_ALIGNED64 (struct) btSolverBody { BT_DECLARE_ALIGNED_ALLOCATOR(); + btTransform m_worldTransform; btVector3 m_deltaLinearVelocity; btVector3 m_deltaAngularVelocity; btVector3 m_angularFactor; + btVector3 m_linearFactor; btVector3 m_invMass; - btRigidBody* m_originalBody; btVector3 m_pushVelocity; btVector3 m_turnVelocity; + btVector3 m_linearVelocity; + btVector3 m_angularVelocity; + btRigidBody* m_originalBody; + void setWorldTransform(const btTransform& worldTransform) + { + m_worldTransform = worldTransform; + } + + const btTransform& getWorldTransform() const + { + return m_worldTransform; + } SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const { if (m_originalBody) - velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos); + velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos); else velocity.setValue(0,0,0); } @@ -128,7 +141,7 @@ ATTRIBUTE_ALIGNED64 (struct) btSolverBodyObsolete SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const { if (m_originalBody) - angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity; + angVel =m_angularVelocity+m_deltaAngularVelocity; else angVel.setValue(0,0,0); } @@ -137,9 +150,9 @@ ATTRIBUTE_ALIGNED64 (struct) btSolverBodyObsolete //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude) { - //if (m_invMass) + if (m_originalBody) { - m_deltaLinearVelocity += linearComponent*impulseMagnitude; + m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor; m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); } } @@ -148,36 +161,125 @@ ATTRIBUTE_ALIGNED64 (struct) btSolverBodyObsolete { if (m_originalBody) { - m_pushVelocity += linearComponent*impulseMagnitude; + m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor; m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor); } } + + + + const btVector3& getDeltaLinearVelocity() const + { + return m_deltaLinearVelocity; + } + + const btVector3& getDeltaAngularVelocity() const + { + return m_deltaAngularVelocity; + } + + const btVector3& getPushVelocity() const + { + return m_pushVelocity; + } + + const btVector3& getTurnVelocity() const + { + return m_turnVelocity; + } + + + //////////////////////////////////////////////// + ///some internal methods, don't use them + + btVector3& internalGetDeltaLinearVelocity() + { + return m_deltaLinearVelocity; + } + + btVector3& internalGetDeltaAngularVelocity() + { + return m_deltaAngularVelocity; + } + + const btVector3& internalGetAngularFactor() const + { + return m_angularFactor; + } + + const btVector3& internalGetInvMass() const + { + return m_invMass; + } + + void internalSetInvMass(const btVector3& invMass) + { + m_invMass = invMass; + } + btVector3& internalGetPushVelocity() + { + return m_pushVelocity; + } + + btVector3& internalGetTurnVelocity() + { + return m_turnVelocity; + } + + SIMD_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const + { + velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos); + } + + SIMD_FORCE_INLINE void internalGetAngularVelocity(btVector3& angVel) const + { + angVel = m_angularVelocity+m_deltaAngularVelocity; + } + + + //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,const btScalar impulseMagnitude) + { + if (m_originalBody) + { + m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor; + m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + } + + + + void writebackVelocity() { if (m_originalBody) { - m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity); - m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity); + m_linearVelocity +=m_deltaLinearVelocity; + m_angularVelocity += m_deltaAngularVelocity; //m_originalBody->setCompanionId(-1); } } - void writebackVelocity(btScalar timeStep) + void writebackVelocityAndTransform(btScalar timeStep) { (void) timeStep; if (m_originalBody) { - m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity); - m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity); + m_linearVelocity += m_deltaLinearVelocity; + m_angularVelocity += 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); - + if (m_pushVelocity[0]!=0.f || m_pushVelocity[1]!=0 || m_pushVelocity[2]!=0 || m_turnVelocity[0]!=0.f || m_turnVelocity[1]!=0 || m_turnVelocity[2]!=0) + { + btQuaternion orn = m_worldTransform.getRotation(); + btTransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity,timeStep,newTransform); + m_worldTransform = newTransform; + } + //m_worldTransform.setRotation(orn); //m_originalBody->setCompanionId(-1); } } diff --git a/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h index 179e79d79..b85f5c282 100644 --- a/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h @@ -20,6 +20,7 @@ class btRigidBody; #include "LinearMath/btVector3.h" #include "LinearMath/btMatrix3x3.h" #include "btJacobianEntry.h" +#include "LinearMath/btAlignedObjectArray.h" //#define NO_FRICTION_TANGENTIALS 1 #include "btSolverBody.h" @@ -58,16 +59,10 @@ ATTRIBUTE_ALIGNED64 (struct) btSolverConstraint int m_frictionIndex; btScalar m_unusedPadding1; }; - union - { - btRigidBody* m_solverBodyA; - int m_companionIdA; - }; - union - { - btRigidBody* m_solverBodyB; - int m_companionIdB; - }; + + int m_solverBodyIdA; + + int m_solverBodyIdB; union { diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 9019f7b15..24751af5b 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -134,11 +134,8 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal { if (islandId<0) { - if (numManifolds + m_numConstraints) - { - ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); - } + ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id + m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); } else { //also add all non-contact constraints/joints for this island @@ -166,11 +163,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal if (m_solverInfo->m_minimumSolverBatchSize<=1) { - ///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive - if (numManifolds + numCurConstraints) - { - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); - } + m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); } else { @@ -192,15 +185,12 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal } void processConstraints() { - if (m_manifolds.size() + m_constraints.size()>0) - { - btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0; - btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0; - btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0; + btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0; + btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0; + btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0; - m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); - } + m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); m_bodies.resize(0); m_manifolds.resize(0); m_constraints.resize(0); @@ -977,7 +967,8 @@ void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) btRigidBody* body = m_nonStaticRigidBodies[i]; if (!body->isStaticOrKinematicObject()) { - body->integrateVelocities( timeStep); + //don't integrate/update velocities here, it happens in the constraint solver + //damping body->applyDamping(timeStep); diff --git a/src/BulletDynamics/Dynamics/btRigidBody.cpp b/src/BulletDynamics/Dynamics/btRigidBody.cpp index 33efd4717..e9cfe852f 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -250,7 +250,6 @@ void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia) } - void btRigidBody::updateInertiaTensor() { m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose(); @@ -317,26 +316,6 @@ bool btRigidBody::checkCollideWithOverride(const btCollisionObject* co) const return true; } -void btRigidBody::internalWritebackVelocity(btScalar timeStep) -{ - (void) timeStep; - if (m_inverseMass) - { - setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity); - setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity); - - //correct the position/orientation based on push/turn recovery - btTransform newTransform; - btTransformUtil::integrateTransform(getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform); - setWorldTransform(newTransform); - //m_originalBody->setCompanionId(-1); - } -// m_deltaLinearVelocity.setZero(); -// m_deltaAngularVelocity .setZero(); -// m_pushVelocity.setZero(); -// m_turnVelocity.setZero(); -} - void btRigidBody::addConstraintRef(btTypedConstraint* c) diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h index e8927578d..910b8835c 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/src/BulletDynamics/Dynamics/btRigidBody.h @@ -519,105 +519,7 @@ public: return m_rigidbodyFlags; } - const btVector3& getDeltaLinearVelocity() const - { - return m_deltaLinearVelocity; - } - - const btVector3& getDeltaAngularVelocity() const - { - return m_deltaAngularVelocity; - } - - const btVector3& getPushVelocity() const - { - return m_pushVelocity; - } - - const btVector3& getTurnVelocity() const - { - return m_turnVelocity; - } - - - //////////////////////////////////////////////// - ///some internal methods, don't use them - - btVector3& internalGetDeltaLinearVelocity() - { - return m_deltaLinearVelocity; - } - - btVector3& internalGetDeltaAngularVelocity() - { - return m_deltaAngularVelocity; - } - - const btVector3& internalGetAngularFactor() const - { - return m_angularFactor; - } - - const btVector3& internalGetInvMass() const - { - return m_invMass; - } - btVector3& internalGetPushVelocity() - { - return m_pushVelocity; - } - - btVector3& internalGetTurnVelocity() - { - return m_turnVelocity; - } - - SIMD_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const - { - velocity = getLinearVelocity()+m_deltaLinearVelocity + (getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos); - } - - SIMD_FORCE_INLINE void internalGetAngularVelocity(btVector3& angVel) const - { - angVel = getAngularVelocity()+m_deltaAngularVelocity; - } - - - //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,const btScalar impulseMagnitude) - { - if (m_inverseMass) - { - 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_inverseMass) - { - m_pushVelocity += linearComponent*impulseMagnitude; - m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor); - } - } - - void internalWritebackVelocity() - { - if (m_inverseMass) - { - setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity); - setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity); - //m_deltaLinearVelocity.setZero(); - //m_deltaAngularVelocity .setZero(); - //m_originalBody->setCompanionId(-1); - } - } - - - void internalWritebackVelocity(btScalar timeStep); - /////////////////////////////////////////////// diff --git a/src/BulletMultiThreaded/btParallelConstraintSolver.cpp b/src/BulletMultiThreaded/btParallelConstraintSolver.cpp index 10164f8eb..76fd81730 100644 --- a/src/BulletMultiThreaded/btParallelConstraintSolver.cpp +++ b/src/BulletMultiThreaded/btParallelConstraintSolver.cpp @@ -19,7 +19,7 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h" #include "LinearMath/btPoolAllocator.h" - +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" #include "BulletMultiThreaded/vectormath2bullet.h" #include "LinearMath/btQuickprof.h" @@ -56,7 +56,6 @@ unsigned char ATTRIBUTE_ALIGNED128(tmp_buff[TMP_BUFF_BYTES]); btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; const btScalar deltaVel1Dotn = c.m_contactNormal.dot(getBtVector3(body1.mDeltaLinearVelocity)) + c.m_relpos1CrossNormal.dot(getBtVector3(body1.mDeltaAngularVelocity)); const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(getBtVector3(body2.mDeltaLinearVelocity)) + c.m_relpos2CrossNormal.dot(getBtVector3(body2.mDeltaAngularVelocity)); - // const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; @@ -160,6 +159,7 @@ void CustomSolveConstraintsTaskParallel( const PfxParallelGroup *contactParallelGroup,const PfxParallelBatch *contactParallelBatches, PfxConstraintPair *contactPairs,uint32_t numContactPairs, btPersistentManifold* offsetContactManifolds, + btConstraintRow* offsetContactConstraintRows, const PfxParallelGroup *jointParallelGroup,const PfxParallelBatch *jointParallelBatches, PfxConstraintPair *jointPairs,uint32_t numJointPairs, btSolverConstraint* offsetSolverConstraints, @@ -222,8 +222,9 @@ void CustomSolveConstraintsTaskParallel( uint16_t iA = pfxGetRigidBodyIdA(pair); uint16_t iB = pfxGetRigidBodyIdB(pair); - btPersistentManifold& contact = offsetContactManifolds[pfxGetConstraintId1(pair)]; - + uint32_t contactIndex = pfxGetConstraintId1(pair); + btPersistentManifold& contact = offsetContactManifolds[contactIndex]; + btConstraintRow* contactConstraintRows = &offsetContactConstraintRows[contactIndex*12]; PfxSolverBody &solverBodyA = offsetSolverBodies[iA]; PfxSolverBody &solverBodyB = offsetSolverBodies[iB]; @@ -235,9 +236,17 @@ void CustomSolveConstraintsTaskParallel( vmVector3 rA = rotate(solverBodyA.mOrientation,btReadVector3(cp.m_localPointA)); vmVector3 rB = rotate(solverBodyB.mOrientation,btReadVector3(cp.m_localPointB)); - for(int k=0;k<3;k++) { - vmVector3 normal = btReadVector3(cp.mConstraintRow[k].m_normal); - float deltaImpulse = cp.mConstraintRow[k].m_accumImpulse; + float imp[3] = + { + cp.m_appliedImpulse, + cp.m_appliedImpulseLateral1, + cp.m_appliedImpulseLateral2 + }; + for(int k=0;k<3;k++) + { + vmVector3 normal = btReadVector3(contactConstraintRows[j*3+k].m_normal); + contactConstraintRows[j*3+k].m_accumImpulse = imp[k]; + float deltaImpulse = contactConstraintRows[j*3+k].m_accumImpulse; solverBodyA.mDeltaLinearVelocity += deltaImpulse * solverBodyA.mMassInv * normal; solverBodyA.mDeltaAngularVelocity += deltaImpulse * solverBodyA.mInertiaInv * cross(rA,normal); solverBodyB.mDeltaLinearVelocity -= deltaImpulse * solverBodyB.mMassInv * normal; @@ -246,9 +255,9 @@ void CustomSolveConstraintsTaskParallel( } else { btSolveContactConstraint( - cp.mConstraintRow[0], - cp.mConstraintRow[1], - cp.mConstraintRow[2], + contactConstraintRows[j*3], + contactConstraintRows[j*3+1], + contactConstraintRows[j*3+2], btReadVector3(cp.m_localPointA), btReadVector3(cp.m_localPointB), solverBodyA, @@ -334,6 +343,11 @@ void btSetupContactConstraint( const TrbState &stateB, PfxSolverBody &solverBodyA, PfxSolverBody &solverBodyB, + const vmVector3& linVelA, + const vmVector3& angVelA, + const vmVector3& linVelB, + const vmVector3& angVelB, + float separateBias, float timeStep ) @@ -344,11 +358,17 @@ void btSetupContactConstraint( vmMatrix3 K = vmMatrix3::scale(vmVector3(solverBodyA.mMassInv + solverBodyB.mMassInv)) - crossMatrix(rA) * solverBodyA.mInertiaInv * crossMatrix(rA) - crossMatrix(rB) * solverBodyB.mInertiaInv * crossMatrix(rB); + + //use the velocities without the applied (gravity and external) forces for restitution computation + vmVector3 vArestitution = linVelA + cross(angVelA,rA); + vmVector3 vBrestitution = linVelB + cross(angVelB,rB); + vmVector3 vABrestitution = vArestitution-vBrestitution; vmVector3 vA = stateA.getLinearVelocity() + cross(stateA.getAngularVelocity(),rA); vmVector3 vB = stateB.getLinearVelocity() + cross(stateB.getAngularVelocity(),rB); vmVector3 vAB = vA-vB; + vmVector3 tangent1,tangent2; btPlaneSpace1(contactNormal,tangent1,tangent2); @@ -404,6 +424,7 @@ void btSetupContactConstraint( void CustomSetupContactConstraintsTask( PfxConstraintPair *contactPairs,uint32_t numContactPairs, btPersistentManifold* offsetContactManifolds, + btConstraintRow* offsetContactConstraintRows, TrbState *offsetRigStates, PfxSolverBody *offsetSolverBodies, uint32_t numRigidBodies, @@ -422,7 +443,7 @@ void CustomSetupContactConstraintsTask( int id = pfxGetConstraintId1(pair); btPersistentManifold& contact = offsetContactManifolds[id]; - + btConstraintRow* contactConstraintRows = &offsetContactConstraintRows[id*12]; TrbState &stateA = offsetRigStates[iA]; // PfxRigBody &bodyA = offsetRigBodies[iA]; @@ -439,11 +460,40 @@ void CustomSetupContactConstraintsTask( for(int j=0;jgetInvMass()>0.f)) + { + linVelA = rbA->getLinearVelocity(); + angVelA = rbA->getAngularVelocity(); + } else + { + linVelA.setValue(0,0,0); + angVelA.setValue(0,0,0); + } + + if (rbB && (rbB->getInvMass()>0.f)) + { + linVelB = rbB->getLinearVelocity(); + angVelB = rbB->getAngularVelocity(); + } else + { + linVelB.setValue(0,0,0); + angVelB.setValue(0,0,0); + } + + + btSetupContactConstraint( - cp.mConstraintRow[0], - cp.mConstraintRow[1], - cp.mConstraintRow[2], + contactConstraintRows[j*3], + contactConstraintRows[j*3+1], + contactConstraintRows[j*3+2], cp.getDistance(), restitution, friction, @@ -454,6 +504,8 @@ void CustomSetupContactConstraintsTask( stateB, solverBodyA, solverBodyB, + (const vmVector3&)linVelA, (const vmVector3&)angVelA, + (const vmVector3&)linVelB, (const vmVector3&)angVelB, separateBias, timeStep ); @@ -463,6 +515,36 @@ void CustomSetupContactConstraintsTask( } } + +void CustomWritebackContactConstraintsTask( + PfxConstraintPair *contactPairs,uint32_t numContactPairs, + btPersistentManifold* offsetContactManifolds, + btConstraintRow* offsetContactConstraintRows, + TrbState *offsetRigStates, + PfxSolverBody *offsetSolverBodies, + uint32_t numRigidBodies, + float separateBias, + float timeStep) +{ + for(uint32_t i=0;iio); @@ -479,6 +561,7 @@ void SolverThreadFunc(void* userPtr,void* lsMemory) io->solveConstraints.contactPairs, io->solveConstraints.numContactPairs, io->solveConstraints.offsetContactManifolds, + io->solveConstraints.offsetContactConstraintRows, io->solveConstraints.jointParallelGroup, io->solveConstraints.jointParallelBatches, @@ -528,6 +611,49 @@ void SolverThreadFunc(void* userPtr,void* lsMemory) CustomSetupContactConstraintsTask( io->setupContactConstraints.offsetContactPairs+start,batch, io->setupContactConstraints.offsetContactManifolds, + io->setupContactConstraints.offsetContactConstraintRows, + io->setupContactConstraints.offsetRigStates, +// io->setupContactConstraints.offsetRigBodies, + io->setupContactConstraints.offsetSolverBodies, + io->setupContactConstraints.numRigidBodies, + io->setupContactConstraints.separateBias, + io->setupContactConstraints.timeStep); + } + else { + empty = true; + } + } + } + break; + + case PFX_CONSTRAINT_SOLVER_CMD_WRITEBACK_APPLIED_IMPULSES_CONTACT_CONSTRAINTS: + { + bool empty = false; + while(!empty) { + int start,batch; + + criticalsection->lock(); + + start = (int)criticalsection->getSharedParam(0); + batch = (int)criticalsection->getSharedParam(1); + + //PFX_PRINTF("taskId %d start %d num %d\n",arg->taskId,start,batch); + + // 次のバッファをセット + int nextStart = start + batch; + int rest = btMax((int)io->setupContactConstraints.numContactPairs1 - nextStart,0); + int nextBatch = (rest > batch)?batch:rest; + + criticalsection->setSharedParam(0,nextStart); + criticalsection->setSharedParam(1,nextBatch); + + criticalsection->unlock(); + + if(batch > 0) { + CustomWritebackContactConstraintsTask( + io->setupContactConstraints.offsetContactPairs+start,batch, + io->setupContactConstraints.offsetContactManifolds, + io->setupContactConstraints.offsetContactConstraintRows, io->setupContactConstraints.offsetRigStates, // io->setupContactConstraints.offsetRigBodies, io->setupContactConstraints.offsetSolverBodies, @@ -554,6 +680,7 @@ void SolverThreadFunc(void* userPtr,void* lsMemory) void CustomSetupContactConstraintsNew( PfxConstraintPair *contactPairs1,uint32_t numContactPairs, btPersistentManifold *offsetContactManifolds, + btConstraintRow* offsetContactConstraintRows, TrbState *offsetRigStates, PfxSolverBody *offsetSolverBodies, uint32_t numRigidBodies, @@ -561,7 +688,8 @@ void CustomSetupContactConstraintsNew( float timeStep, class btThreadSupportInterface* threadSupport, btCriticalSection* criticalSection, - btConstraintSolverIO *io + btConstraintSolverIO *io , + uint8_t cmd ) { int maxTasks = threadSupport->getNumTasks(); @@ -584,11 +712,12 @@ void CustomSetupContactConstraintsNew( } for(int t=0;tsolveConstraints.jointParallelBatches, io->solveConstraints.jointPairs, io->solveConstraints.numJointPairs, - io->solveConstraints.offsetJoints, + io->solveConstraints.offsetSolverConstraints, - io->solveConstraints.offsetRigStates, + io->solveConstraints.offsetRigStates1, io->solveConstraints.offsetSolverBodies, io->solveConstraints.numRigidBodies, io->solveConstraints.iteration,0,1,0);//arg->taskId,1,0);//,arg->maxTasks,arg->barrier); @@ -794,6 +924,7 @@ void CustomSolveConstraintsParallel( io[t].solveConstraints.contactPairs = contactPairs; io[t].solveConstraints.numContactPairs = numContactPairs; io[t].solveConstraints.offsetContactManifolds = offsetContactManifolds; + io[t].solveConstraints.offsetContactConstraintRows = offsetContactConstraintRows; io[t].solveConstraints.jointParallelGroup = jgroup; io[t].solveConstraints.jointParallelBatches = jbatches; io[t].solveConstraints.jointPairs = jointPairs; @@ -869,6 +1000,7 @@ void CustomSolveConstraintsParallel( void BPE_customConstraintSolverSequentialNew(unsigned int new_num, PfxBroadphasePair *new_pairs1 , btPersistentManifold* offsetContactManifolds, + PfxConstraintRow* offsetContactConstraintRows, TrbState* states,int numRigidBodies, struct PfxSolverBody* solverBodies, PfxConstraintPair* jointPairs, unsigned int numJoints, @@ -920,16 +1052,19 @@ void BPE_customConstraintSolverSequentialNew(unsigned int new_num, PfxBroadphase separateBias, timeStep); #else + CustomSetupContactConstraintsNew( (PfxConstraintPair*)new_pairs1,new_num, offsetContactManifolds, + offsetContactConstraintRows, states, solverBodies, numRigidBodies, separateBias, timeStep, solverThreadSupport, - criticalSection,solverIO + criticalSection,solverIO, + PFX_CONSTRAINT_SOLVER_CMD_SETUP_CONTACT_CONSTRAINTS ); #endif //SEQUENTIAL_SETUP @@ -954,6 +1089,7 @@ void BPE_customConstraintSolverSequentialNew(unsigned int new_num, PfxBroadphase (PfxConstraintPair*)new_pairs1,new_num, jointPairs,numJoints, offsetContactManifolds, + offsetContactConstraintRows, offsetSolverConstraints, states, solverBodies, @@ -968,6 +1104,24 @@ void BPE_customConstraintSolverSequentialNew(unsigned int new_num, PfxBroadphase #endif //SEQUENTIAL } + { + BT_PROFILE("writeback appliedImpulses"); + + CustomSetupContactConstraintsNew( + (PfxConstraintPair*)new_pairs1,new_num, + offsetContactManifolds, + offsetContactConstraintRows, + states, + solverBodies, + numRigidBodies, + separateBias, + timeStep, + solverThreadSupport, + criticalSection,solverIO, + PFX_CONSTRAINT_SOLVER_CMD_WRITEBACK_APPLIED_IMPULSES_CONTACT_CONSTRAINTS + ); + } + } @@ -977,6 +1131,7 @@ struct btParallelSolverMemoryCache btAlignedObjectArray m_mysolverbodies; btAlignedObjectArray m_mypairs; btAlignedObjectArray m_jointPairs; + btAlignedObjectArray m_constraintRows; }; @@ -1057,9 +1212,12 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int //if(state.getMotionMask()&PFX_MOTION_MASK_DYNAMIC) { if (rb && (rb->getInvMass()>0.f)) { - state.setAngularVelocity(vmVector3(rb->getAngularVelocity().getX(),rb->getAngularVelocity().getY(),rb->getAngularVelocity().getZ())); - state.setLinearVelocity(vmVector3(rb->getLinearVelocity().getX(),rb->getLinearVelocity().getY(),rb->getLinearVelocity().getZ())); - + btVector3 angVelPlusForces = rb->getAngularVelocity()+rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*infoGlobal.m_timeStep; + btVector3 linVelPlusForces = rb->getLinearVelocity()+rb->getTotalForce()*rb->getInvMass()*infoGlobal.m_timeStep; + + state.setAngularVelocity((const vmVector3&)angVelPlusForces); + state.setLinearVelocity((const vmVector3&) linVelPlusForces); + state.setMotionType(PfxMotionTypeActive); vmMatrix3 ori(solverBody.mOrientation); vmMatrix3 localInvInertia = vmMatrix3::identity(); @@ -1087,6 +1245,8 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int int totalPoints = 0; #ifndef USE_C_ARRAYS m_memoryCache->m_mypairs.resize(numManifolds); + //4 points per manifold and 3 rows per point makes 12 rows per manifold + m_memoryCache->m_constraintRows.resize(numManifolds*12); m_memoryCache->m_jointPairs.resize(numConstraints); #endif//USE_C_ARRAYS @@ -1196,7 +1356,10 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int btRigidBody& rbA = constraint->getRigidBodyA(); btRigidBody& rbB = constraint->getRigidBodyB(); - + + int idA = constraint->getRigidBodyA().getCompanionId(); + int idB = constraint->getRigidBodyB().getCompanionId(); + int j; for ( j=0;jgetInfo2(&info2); - int idA = constraint->getRigidBodyA().getCompanionId(); - int idB = constraint->getRigidBodyB().getCompanionId(); ///finalize the constraint setup @@ -1246,8 +1404,8 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int btSolverConstraint& solverConstraint = currentConstraintRow[j]; solverConstraint.m_originalContactPoint = constraint; - solverConstraint.m_companionIdA = idA; - solverConstraint.m_companionIdB = idB; + solverConstraint.m_solverBodyIdA = idA; + solverConstraint.m_solverBodyIdB = idB; { const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; @@ -1356,11 +1514,13 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int // PFX_PRINTF("num points PFX = %d\n",total); - + PfxConstraintRow* contactRows = actualNumManifolds? &m_memoryCache->m_constraintRows[0] : 0; + PfxBroadphasePair* actualPairs = m_memoryCache->m_mypairs.size() ? &m_memoryCache->m_mypairs[0] : 0; BPE_customConstraintSolverSequentialNew( actualNumManifolds, - &m_memoryCache->m_mypairs[0], + actualPairs, offsetContactManifolds, + contactRows, &m_memoryCache->m_mystates[0],numRigidBodies, &m_memoryCache->m_mysolverbodies[0], jointPairs,actualNumJoints, diff --git a/src/BulletMultiThreaded/btParallelConstraintSolver.h b/src/BulletMultiThreaded/btParallelConstraintSolver.h index 7c0268e7f..af42a8380 100644 --- a/src/BulletMultiThreaded/btParallelConstraintSolver.h +++ b/src/BulletMultiThreaded/btParallelConstraintSolver.h @@ -188,6 +188,7 @@ class btPersistentManifold; enum { PFX_CONSTRAINT_SOLVER_CMD_SETUP_SOLVER_BODIES, PFX_CONSTRAINT_SOLVER_CMD_SETUP_CONTACT_CONSTRAINTS, + PFX_CONSTRAINT_SOLVER_CMD_WRITEBACK_APPLIED_IMPULSES_CONTACT_CONSTRAINTS, PFX_CONSTRAINT_SOLVER_CMD_SETUP_JOINT_CONSTRAINTS, PFX_CONSTRAINT_SOLVER_CMD_SOLVE_CONSTRAINTS, PFX_CONSTRAINT_SOLVER_CMD_POST_SOLVER @@ -198,6 +199,7 @@ struct PfxSetupContactConstraintsIO { PfxConstraintPair *offsetContactPairs; uint32_t numContactPairs1; btPersistentManifold* offsetContactManifolds; + btConstraintRow* offsetContactConstraintRows; class TrbState *offsetRigStates; struct PfxSolverBody *offsetSolverBodies; uint32_t numRigidBodies; @@ -214,6 +216,7 @@ struct PfxSolveConstraintsIO { PfxConstraintPair *contactPairs; uint32_t numContactPairs; btPersistentManifold *offsetContactManifolds; + btConstraintRow* offsetContactConstraintRows; PfxParallelGroup *jointParallelGroup; PfxParallelBatch *jointParallelBatches; PfxConstraintPair *jointPairs;