From 84b1774dda1f3a8d858af08bf11f9996dac0f5ff Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Fri, 31 Aug 2012 19:46:24 +0000 Subject: [PATCH] improve handling of restitution by using the velocity (linear/angular) before applying forces: this is done by re-introducing the btSolverBody and only apply the forces to solver body, and use the original rigid body velocity for restitution computation. warmstarting for contact points was broken, fix in btPersistentManifold enable split impulse by default (at the cost of some performance) add the option for zero-length friction (instead of recomputing friction directions using btPlaneSpace), use the solver mode flag SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS precompute lateral friction directions (in btManifoldResult) remove the mConstraintRow[3] from btManifoldPoint, it just took a lot of memory with no benefits: fixed it in btParallelConstraintSolver --- .../CollisionDispatch/btManifoldResult.cpp | 4 + .../NarrowPhaseCollision/btManifoldPoint.h | 20 +- .../btPersistentManifold.h | 16 +- .../btConeTwistConstraint.cpp | 2 +- .../ConstraintSolver/btConeTwistConstraint.h | 2 +- .../ConstraintSolver/btContactSolverInfo.h | 15 +- .../btGeneric6DofConstraint.cpp | 32 +- .../ConstraintSolver/btJacobianEntry.h | 3 +- .../btSequentialImpulseConstraintSolver.cpp | 564 +++++++++++------- .../btSequentialImpulseConstraintSolver.h | 51 +- .../ConstraintSolver/btSolverBody.h | 134 ++++- .../ConstraintSolver/btSolverConstraint.h | 15 +- .../Dynamics/btDiscreteDynamicsWorld.cpp | 27 +- src/BulletDynamics/Dynamics/btRigidBody.cpp | 21 - src/BulletDynamics/Dynamics/btRigidBody.h | 98 --- .../btParallelConstraintSolver.cpp | 232 +++++-- .../btParallelConstraintSolver.h | 3 + 17 files changed, 730 insertions(+), 509 deletions(-) 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;