diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp')
| -rw-r--r-- | extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp | 842 |
1 files changed, 720 insertions, 122 deletions
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index eaf172b9395..4366284ea73 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -24,7 +24,13 @@ subject to the following restrictions: #include "btJacobianEntry.h" #include "LinearMath/btMinMax.h" #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +#include <new> +#include "LinearMath/btStackAlloc.h" +#include "LinearMath/btQuickprof.h" +#include "btSolverBody.h" +#include "btSolverConstraint.h" +#include "LinearMath/btAlignedObjectArray.h" #ifdef USE_PROFILE #include "LinearMath/btQuickprof.h" @@ -36,24 +42,26 @@ int gTotalContactPoints = 0; struct btOrderIndex { - short int m_manifoldIndex; - short int m_pointIndex; + int m_manifoldIndex; + int m_pointIndex; }; + + #define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384 static btOrderIndex gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS]; -static unsigned long btSeed2 = 0; -unsigned long btRand2() + + +unsigned long btSequentialImpulseConstraintSolver::btRand2() { - btSeed2 = (1664525L*btSeed2 + 1013904223L) & 0xffffffff; - return btSeed2; + m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff; + return m_btSeed2; } - //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1) -int btRandInt2 (int n) +int btSequentialImpulseConstraintSolver::btRandInt2 (int n) { // seems good; xor-fold and modulus const unsigned long un = n; @@ -82,15 +90,7 @@ int btRandInt2 (int n) -int btRandIntWrong (int n) -{ - float a = float(n) / 4294967296.0f; -// printf("n = %d\n",n); -// printf("a = %f\n",a); - int res = (int) (float(btRand2()) * a); -// printf("res=%d\n",res); - return res; -} + bool MyContactDestroyedCallback(void* userPersistentData) { @@ -102,18 +102,12 @@ bool MyContactDestroyedCallback(void* userPersistentData) return true; } -btSequentialImpulseConstraintSolver3::btSequentialImpulseConstraintSolver3() -{ - btSeed2 = 0; - setSolverMode(SOLVER_RANDMIZE_ORDER); -} btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() -:m_solverMode(SOLVER_USE_WARMSTARTING) +:m_solverMode(SOLVER_RANDMIZE_ORDER | SOLVER_CACHE_FRIENDLY), //not using SOLVER_USE_WARMSTARTING, +m_btSeed2(0) { - btSeed2 = 0; - gContactDestroyedCallback = &MyContactDestroyedCallback; //initialize default friction/contact funcs @@ -127,35 +121,487 @@ btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() } } -/// btSequentialImpulseConstraintSolver Sequentially applies impulses -float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) + +void initSolverBody(btSolverBody* solverBody, btRigidBody* rigidbody) +{ +/* int size = sizeof(btSolverBody); + int sizeofrb = sizeof(btRigidBody); + int sizemanifold = sizeof(btPersistentManifold); + int sizeofmp = sizeof(btManifoldPoint); + int sizeofPersistData = sizeof (btConstraintPersistentData); +*/ + + solverBody->m_angularVelocity = rigidbody->getAngularVelocity(); + solverBody->m_centerOfMassPosition = rigidbody->getCenterOfMassPosition(); + solverBody->m_friction = rigidbody->getFriction(); +// solverBody->m_invInertiaWorld = rigidbody->getInvInertiaTensorWorld(); + solverBody->m_invMass = rigidbody->getInvMass(); + solverBody->m_linearVelocity = rigidbody->getLinearVelocity(); + solverBody->m_originalBody = rigidbody; + solverBody->m_angularFactor = rigidbody->getAngularFactor(); +} + +btScalar penetrationResolveFactor = btScalar(0.9); +btScalar restitutionCurve(btScalar rel_vel, btScalar restitution) +{ + btScalar rest = restitution * -rel_vel; + return rest; +} + + + + + + +//velocity + friction +//response between two dynamic objects with friction +SIMD_FORCE_INLINE btScalar resolveSingleCollisionCombinedCacheFriendly( + btSolverBody& body1, + btSolverBody& body2, + btSolverConstraint& contactConstraint, + const btContactSolverInfo& solverInfo) +{ + (void)solverInfo; + + btScalar normalImpulse(0.f); + { + if (contactConstraint.m_penetration < 0.f) + return 0.f; + + // Optimized version of projected relative velocity, use precomputed cross products with normal + // body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1); + // body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2); + // btVector3 vel = vel1 - vel2; + // btScalar rel_vel = contactConstraint.m_contactNormal.dot(vel); + + btScalar rel_vel; + btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) + + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity); + btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) + + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity); + + rel_vel = vel1Dotn-vel2Dotn; + + + btScalar positionalError = contactConstraint.m_penetration; + btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping; + + btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv; + btScalar normalImpulse = penetrationImpulse+velocityImpulse; + + // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse + btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse; + btScalar sum = oldNormalImpulse + normalImpulse; + contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum; + + btScalar oldVelocityImpulse = contactConstraint.m_appliedVelocityImpulse; + btScalar velocitySum = oldVelocityImpulse + velocityImpulse; + contactConstraint.m_appliedVelocityImpulse = btScalar(0.) > velocitySum ? btScalar(0.): velocitySum; + + normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse; + + if (body1.m_invMass) + { + body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass, + contactConstraint.m_angularComponentA,normalImpulse); + } + if (body2.m_invMass) + { + body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass, + contactConstraint.m_angularComponentB,-normalImpulse); + } + + } + + + + return normalImpulse; +} + + +#ifndef NO_FRICTION_TANGENTIALS + +SIMD_FORCE_INLINE btScalar resolveSingleFrictionCacheFriendly( + btSolverBody& body1, + btSolverBody& body2, + btSolverConstraint& contactConstraint, + const btContactSolverInfo& solverInfo, + btScalar appliedNormalImpulse) { + (void)solverInfo; + - btContactSolverInfo info = infoGlobal; + const btScalar combinedFriction = contactConstraint.m_friction; + + const btScalar limit = appliedNormalImpulse * combinedFriction; + + if (appliedNormalImpulse>btScalar(0.)) + //friction + { + + btScalar j1; + { - int numiter = infoGlobal.m_numIterations; -#ifdef USE_PROFILE - btProfiler::beginBlock("solve"); -#endif //USE_PROFILE + btScalar rel_vel; + const btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) + + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity); + const btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) + + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity); + rel_vel = vel1Dotn-vel2Dotn; + + // calculate j that moves us to zero relative velocity + j1 = -rel_vel * contactConstraint.m_jacDiagABInv; + btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse; + contactConstraint.m_appliedImpulse = oldTangentImpulse + j1; + GEN_set_min(contactConstraint.m_appliedImpulse, limit); + GEN_set_max(contactConstraint.m_appliedImpulse, -limit); + j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse; - int totalPoints = 0; + } + + if (body1.m_invMass) + { + body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1); + } + if (body2.m_invMass) + { + body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1); + } + + } + return 0.f; +} +#else + +//velocity + friction +//response between two dynamic objects with friction +btScalar resolveSingleFrictionCacheFriendly( + btSolverBody& body1, + btSolverBody& body2, + btSolverConstraint& contactConstraint, + const btContactSolverInfo& solverInfo) +{ + + btVector3 vel1; + btVector3 vel2; + btScalar normalImpulse(0.f); + { - int j; - for (j=0;j<numManifolds;j++) + const btVector3& normal = contactConstraint.m_contactNormal; + if (contactConstraint.m_penetration < 0.f) + return 0.f; + + + body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1); + body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2); + btVector3 vel = vel1 - vel2; + btScalar rel_vel; + rel_vel = normal.dot(vel); + + btVector3 lat_vel = vel - normal * rel_vel; + btScalar lat_rel_vel = lat_vel.length2(); + + btScalar combinedFriction = contactConstraint.m_friction; + const btVector3& rel_pos1 = contactConstraint.m_rel_posA; + const btVector3& rel_pos2 = contactConstraint.m_rel_posB; + + + //if (contactConstraint.m_appliedVelocityImpulse > 0.f) + if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON) { - btPersistentManifold* manifold = manifoldPtr[j]; - prepareConstraints(manifold,info,debugDrawer); + lat_rel_vel = btSqrt(lat_rel_vel); + + lat_vel /= lat_rel_vel; + btVector3 temp1 = body1.m_invInertiaWorld * rel_pos1.cross(lat_vel); + btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel); + btScalar friction_impulse = lat_rel_vel / + (body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2))); + btScalar normal_impulse = contactConstraint.m_appliedVelocityImpulse * combinedFriction; + + GEN_set_min(friction_impulse, normal_impulse); + GEN_set_max(friction_impulse, -normal_impulse); + body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1); + body2.applyImpulse(lat_vel * friction_impulse, rel_pos2); + } + } + + return normalImpulse; +} + +#endif //NO_FRICTION_TANGENTIALS + +btAlignedObjectArray<btSolverBody> tmpSolverBodyPool; +btAlignedObjectArray<btSolverConstraint> tmpSolverConstraintPool; +btAlignedObjectArray<btSolverConstraint> tmpSolverFrictionConstraintPool; + + +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) +{ + (void)stackAlloc; + (void)debugDrawer; + + if (!(numConstraints + numManifolds)) + { +// printf("empty\n"); + return 0.f; + } + + BEGIN_PROFILE("refreshManifolds"); + + int i; + for (i=0;i<numManifolds;i++) + { + btPersistentManifold* manifold = manifoldPtr[i]; + btRigidBody* rb0 = (btRigidBody*)manifold->getBody0(); + btRigidBody* rb1 = (btRigidBody*)manifold->getBody1(); - for (int p=0;p<manifoldPtr[j]->getNumContacts();p++) + manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform()); + + } + + END_PROFILE("refreshManifolds"); + + + BEGIN_PROFILE("gatherSolverData"); + + //int sizeofSB = sizeof(btSolverBody); + //int sizeofSC = sizeof(btSolverConstraint); + + + //if (1) + { + //if m_stackAlloc, try to pack bodies/constraints to speed up solving +// btBlock* sablock; +// sablock = stackAlloc->beginBlock(); + + // int memsize = 16; +// unsigned char* stackMemory = stackAlloc->allocate(memsize); + + + //todo: use stack allocator for this temp memory + int minReservation = numManifolds*2; + + tmpSolverBodyPool.reserve(minReservation); + + { + for (int i=0;i<numBodies;i++) { - gOrder[totalPoints].m_manifoldIndex = j; - gOrder[totalPoints].m_pointIndex = p; - totalPoints++; + btRigidBody* rb = btRigidBody::upcast(bodies[i]); + if (rb && (rb->getIslandTag() >= 0)) + { + btAssert(rb->getCompanionId() < 0); + int solverBodyId = tmpSolverBodyPool.size(); + btSolverBody& solverBody = tmpSolverBodyPool.expand(); + initSolverBody(&solverBody,rb); + rb->setCompanionId(solverBodyId); + } + } + } + + + tmpSolverConstraintPool.reserve(minReservation); + tmpSolverFrictionConstraintPool.reserve(minReservation); + { + int i; + + for (i=0;i<numManifolds;i++) + { + btPersistentManifold* manifold = manifoldPtr[i]; + btRigidBody* rb0 = (btRigidBody*)manifold->getBody0(); + btRigidBody* rb1 = (btRigidBody*)manifold->getBody1(); + + + int solverBodyIdA=-1; + int solverBodyIdB=-1; + + if (manifold->getNumContacts()) + { + + + + if (rb0->getIslandTag() >= 0) + { + solverBodyIdA = rb0->getCompanionId(); + } else + { + //create a static body + solverBodyIdA = tmpSolverBodyPool.size(); + btSolverBody& solverBody = tmpSolverBodyPool.expand(); + initSolverBody(&solverBody,rb0); + } + + if (rb1->getIslandTag() >= 0) + { + solverBodyIdB = rb1->getCompanionId(); + } else + { + //create a static body + solverBodyIdB = tmpSolverBodyPool.size(); + btSolverBody& solverBody = tmpSolverBodyPool.expand(); + initSolverBody(&solverBody,rb1); + } + } + + for (int j=0;j<manifold->getNumContacts();j++) + { + + btManifoldPoint& cp = manifold->getContactPoint(j); + + int frictionIndex = tmpSolverConstraintPool.size(); + + if (cp.getDistance() <= btScalar(0.)) + { + + const btVector3& pos1 = cp.getPositionWorldOnA(); + const btVector3& pos2 = cp.getPositionWorldOnB(); + + btVector3 rel_pos1 = pos1 - rb0->getCenterOfMassPosition(); + btVector3 rel_pos2 = pos2 - rb1->getCenterOfMassPosition(); + + + btScalar relaxation = 1.f; + + { + btSolverConstraint& solverConstraint = tmpSolverConstraintPool.expand(); + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D; + + + + { + //can be optimized, the cross products are already calculated + btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); + btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); + btScalar denom = relaxation/(denom0+denom1); + solverConstraint.m_jacDiagABInv = denom; + } + + 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->getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = rb1->getVelocityInLocalPoint(rel_pos2); + + btVector3 vel = vel1 - vel2; + btScalar rel_vel; + rel_vel = cp.m_normalWorldOnB.dot(vel); + + + solverConstraint.m_penetration = cp.getDistance();///btScalar(infoGlobal.m_numIterations); + solverConstraint.m_friction = cp.m_combinedFriction; + btScalar rest = restitutionCurve(rel_vel, cp.m_combinedRestitution); + if (rest <= btScalar(0.)) + { + rest = 0.f; + }; + + btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep; + if (rest > penVel) + { + rest = btScalar(0.); + } + solverConstraint.m_restitution = rest; + + solverConstraint.m_penetration *= -(infoGlobal.m_erp/infoGlobal.m_timeStep); + + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedVelocityImpulse = 0.f; + + + btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*torqueAxis0; + btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*torqueAxis1; + } + + //create 2 '1d axis' constraints for 2 tangential friction directions + + //re-calculate friction direction every frame, todo: check if this is really needed + btVector3 frictionTangential0a, frictionTangential1b; + + btPlaneSpace1(cp.m_normalWorldOnB,frictionTangential0a,frictionTangential1b); + + { + btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand(); + solverConstraint.m_contactNormal = frictionTangential0a; + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D; + solverConstraint.m_frictionIndex = frictionIndex; + + solverConstraint.m_friction = cp.m_combinedFriction; + + solverConstraint.m_appliedImpulse = btScalar(0.); + solverConstraint.m_appliedVelocityImpulse = 0.f; + + btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal); + btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal); + btScalar denom = relaxation/(denom0+denom1); + solverConstraint.m_jacDiagABInv = denom; + + { + btVector3 ftorqueAxis0 = rel_pos1.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos1CrossNormal = ftorqueAxis0; + solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis0; + } + { + btVector3 ftorqueAxis0 = rel_pos2.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos2CrossNormal = ftorqueAxis0; + solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis0; + } + + } + + + { + + btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand(); + solverConstraint.m_contactNormal = frictionTangential1b; + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D; + solverConstraint.m_frictionIndex = frictionIndex; + + solverConstraint.m_friction = cp.m_combinedFriction; + + solverConstraint.m_appliedImpulse = btScalar(0.); + solverConstraint.m_appliedVelocityImpulse = 0.f; + + btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal); + btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal); + btScalar denom = relaxation/(denom0+denom1); + solverConstraint.m_jacDiagABInv = denom; + { + btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; + solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis1; + } + { + btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; + solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis1; + } + } + + } + } } } } + END_PROFILE("gatherSolverData"); + + BEGIN_PROFILE("prepareConstraints"); + + btContactSolverInfo info = infoGlobal; { int j; @@ -166,21 +612,57 @@ float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** ma } } - //should traverse the contacts random order... - int iteration; + btAlignedObjectArray<int> gOrderTmpConstraintPool; + btAlignedObjectArray<int> gOrderFrictionConstraintPool; + int numConstraintPool = tmpSolverConstraintPool.size(); + int numFrictionPool = tmpSolverFrictionConstraintPool.size(); + + ///todo: use stack allocator for such temporarily memory, same for solver bodies/constraints + gOrderTmpConstraintPool.resize(numConstraintPool); + gOrderFrictionConstraintPool.resize(numFrictionPool); { - for ( iteration = 0;iteration<numiter-1;iteration++) + int i; + for (i=0;i<numConstraintPool;i++) + { + gOrderTmpConstraintPool[i] = i; + } + for (i=0;i<numFrictionPool;i++) { + gOrderFrictionConstraintPool[i] = i; + } + } + + + + + END_PROFILE("prepareConstraints"); + + + BEGIN_PROFILE("solveConstraints"); + + //should traverse the contacts random order... + int iteration; + { + for ( iteration = 0;iteration<info.m_numIterations;iteration++) + { + int j; if (m_solverMode & SOLVER_RANDMIZE_ORDER) { if ((iteration & 7) == 0) { - for (j=0; j<totalPoints; ++j) { - btOrderIndex tmp = gOrder[j]; + for (j=0; j<numConstraintPool; ++j) { + int tmp = gOrderTmpConstraintPool[j]; int swapi = btRandInt2(j+1); - gOrder[j] = gOrder[swapi]; - gOrder[swapi] = tmp; + gOrderTmpConstraintPool[j] = gOrderTmpConstraintPool[swapi]; + gOrderTmpConstraintPool[swapi] = tmp; + } + + for (j=0; j<numFrictionPool; ++j) { + int tmp = gOrderFrictionConstraintPool[j]; + int swapi = btRandInt2(j+1); + gOrderFrictionConstraintPool[j] = gOrderFrictionConstraintPool[swapi]; + gOrderFrictionConstraintPool[swapi] = tmp; } } } @@ -188,38 +670,98 @@ float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** ma for (j=0;j<numConstraints;j++) { btTypedConstraint* constraint = constraints[j]; + ///todo: use solver bodies, so we don't need to copy from/to btRigidBody + + if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0)) + { + tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity(); + } + if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0)) + { + tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity(); + } + constraint->solveConstraint(info.m_timeStep); + + if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0)) + { + tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity(); + } + if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0)) + { + tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity(); + } + } - for (j=0;j<totalPoints;j++) { - btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex]; - solve( (btRigidBody*)manifold->getBody0(), - (btRigidBody*)manifold->getBody1() - ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); + int numPoolConstraints = tmpSolverConstraintPool.size(); + for (j=0;j<numPoolConstraints;j++) + { + btSolverConstraint& solveManifold = tmpSolverConstraintPool[gOrderTmpConstraintPool[j]]; + resolveSingleCollisionCombinedCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA], + tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info); + } } - - for (j=0;j<totalPoints;j++) + { - btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex]; - solveFriction((btRigidBody*)manifold->getBody0(), - (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); + int numFrictionPoolConstraints = tmpSolverFrictionConstraintPool.size(); + for (j=0;j<numFrictionPoolConstraints;j++) + { + btSolverConstraint& solveManifold = tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]]; + btScalar appliedNormalImpulse = tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + + resolveSingleFrictionCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA], + tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info,appliedNormalImpulse); + } } + + + } } -#ifdef USE_PROFILE - btProfiler::endBlock("solve"); -#endif //USE_PROFILE + for ( i=0;i<tmpSolverBodyPool.size();i++) + { + tmpSolverBodyPool[i].writebackVelocity(); + } + + END_PROFILE("solveConstraints"); + +// printf("tmpSolverConstraintPool.size() = %i\n",tmpSolverConstraintPool.size()); + +/* + printf("tmpSolverBodyPool.size() = %i\n",tmpSolverBodyPool.size()); + printf("tmpSolverConstraintPool.size() = %i\n",tmpSolverConstraintPool.size()); + printf("tmpSolverFrictionConstraintPool.size() = %i\n",tmpSolverFrictionConstraintPool.size()); + + + printf("tmpSolverBodyPool.capacity() = %i\n",tmpSolverBodyPool.capacity()); + printf("tmpSolverConstraintPool.capacity() = %i\n",tmpSolverConstraintPool.capacity()); + printf("tmpSolverFrictionConstraintPool.capacity() = %i\n",tmpSolverFrictionConstraintPool.capacity()); +*/ + + tmpSolverBodyPool.resize(0); + tmpSolverConstraintPool.resize(0); + tmpSolverFrictionConstraintPool.resize(0); + return 0.f; } - /// btSequentialImpulseConstraintSolver Sequentially applies impulses -float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) { + + if (getSolverMode() & SOLVER_CACHE_FRIENDLY) + { + return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); + } + + + BEGIN_PROFILE("prepareConstraints"); + btContactSolverInfo info = infoGlobal; int numiter = infoGlobal.m_numIterations; @@ -227,21 +769,25 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man btProfiler::beginBlock("solve"); #endif //USE_PROFILE + int totalPoints = 0; + + { - int j; + short j; for (j=0;j<numManifolds;j++) { btPersistentManifold* manifold = manifoldPtr[j]; prepareConstraints(manifold,info,debugDrawer); - for (int p=0;p<manifoldPtr[j]->getNumContacts();p++) + + for (short p=0;p<manifoldPtr[j]->getNumContacts();p++) { - //interleaving here gives better results - solve( (btRigidBody*)manifold->getBody0(), - (btRigidBody*)manifold->getBody1() - ,manifoldPtr[j]->getContactPoint(p),info,0,debugDrawer); + gOrder[totalPoints].m_manifoldIndex = j; + gOrder[totalPoints].m_pointIndex = p; + totalPoints++; } } } + { int j; for (j=0;j<numConstraints;j++) @@ -251,66 +797,77 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man } } + END_PROFILE("prepareConstraints"); + + + BEGIN_PROFILE("solveConstraints"); + //should traverse the contacts random order... int iteration; - for ( iteration = 0;iteration<numiter-1;iteration++) { - int j; - - for (j=0;j<numConstraints;j++) + for ( iteration = 0;iteration<numiter;iteration++) { - btTypedConstraint* constraint = constraints[j]; - constraint->solveConstraint(info.m_timeStep); - } + int j; + if (m_solverMode & SOLVER_RANDMIZE_ORDER) + { + if ((iteration & 7) == 0) { + for (j=0; j<totalPoints; ++j) { + btOrderIndex tmp = gOrder[j]; + int swapi = btRandInt2(j+1); + gOrder[j] = gOrder[swapi]; + gOrder[swapi] = tmp; + } + } + } - for (j=0;j<numManifolds;j++) - { - btPersistentManifold* manifold = manifoldPtr[j]; - for (int p=0;p<manifold->getNumContacts();p++) + for (j=0;j<numConstraints;j++) { + btTypedConstraint* constraint = constraints[j]; + constraint->solveConstraint(info.m_timeStep); + } + + for (j=0;j<totalPoints;j++) + { + btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex]; solve( (btRigidBody*)manifold->getBody0(), (btRigidBody*)manifold->getBody1() - ,manifold->getContactPoint(p),info,iteration,debugDrawer); + ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); } - } - - } - - for ( iteration = 0;iteration<numiter-1;iteration++) - { - int j; - for (j=0;j<numManifolds;j++) - { - btPersistentManifold* manifold = manifoldPtr[j]; - for (int p=0;p<manifold->getNumContacts();p++) + + for (j=0;j<totalPoints;j++) { + btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex]; solveFriction((btRigidBody*)manifold->getBody0(), - (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(p),info,iteration,debugDrawer); + (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); } } } - + END_PROFILE("solveConstraints"); + + #ifdef USE_PROFILE btProfiler::endBlock("solve"); #endif //USE_PROFILE - return 0.f; -} -float penetrationResolveFactor = 0.9f; -btScalar restitutionCurve(btScalar rel_vel, btScalar restitution) -{ - btScalar rest = restitution * -rel_vel; - return rest; + + return btScalar(0.); } + + + + + void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer) { + (void)debugDrawer; + btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0(); btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1(); @@ -327,7 +884,7 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol for (int i=0;i<numpoints ;i++) { btManifoldPoint& cp = manifoldPtr->getContactPoint(i); - if (cp.getDistance() <= 0.f) + if (cp.getDistance() <= btScalar(0.)) { const btVector3& pos1 = cp.getPositionWorldOnA(); const btVector3& pos2 = cp.getPositionWorldOnB(); @@ -376,7 +933,7 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol } assert(cpd); - cpd->m_jacDiagABInv = 1.f / jacDiagAB; + cpd->m_jacDiagABInv = btScalar(1.) / jacDiagAB; //Dependent on Rigidbody A and B types, fetch the contact/friction response func //perhaps do a similar thing for friction/restutution combiner funcs... @@ -390,14 +947,14 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol btScalar rel_vel; rel_vel = cp.m_normalWorldOnB.dot(vel); - float combinedRestitution = cp.m_combinedRestitution; + btScalar combinedRestitution = cp.m_combinedRestitution; - cpd->m_penetration = cp.getDistance(); + cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations); cpd->m_friction = cp.m_combinedFriction; cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution); - if (cpd->m_restitution <= 0.) //0.f) + if (cpd->m_restitution <= btScalar(0.)) { - cpd->m_restitution = 0.0f; + cpd->m_restitution = btScalar(0.0); }; @@ -408,18 +965,18 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol if (cpd->m_restitution > penVel) { - cpd->m_penetration = 0.f; + cpd->m_penetration = btScalar(0.); } - float relaxation = info.m_damping; + btScalar relaxation = info.m_damping; if (m_solverMode & SOLVER_USE_WARMSTARTING) { cpd->m_appliedImpulse *= relaxation; } else { - cpd->m_appliedImpulse =0.f; + cpd->m_appliedImpulse =btScalar(0.); } //for friction @@ -432,12 +989,12 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol #define NO_FRICTION_WARMSTART 1 #ifdef NO_FRICTION_WARMSTART - cpd->m_accumulatedTangentImpulse0 = 0.f; - cpd->m_accumulatedTangentImpulse1 = 0.f; + cpd->m_accumulatedTangentImpulse0 = btScalar(0.); + cpd->m_accumulatedTangentImpulse1 = btScalar(0.); #endif //NO_FRICTION_WARMSTART - float denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0); - float denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0); - float denom = relaxation/(denom0+denom1); + btScalar denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0); + btScalar denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0); + btScalar denom = relaxation/(denom0+denom1); cpd->m_jacDiagABInvTangent0 = denom; @@ -492,16 +1049,54 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol } } -float btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) + +btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) { + btScalar maxImpulse = btScalar(0.); + + { + + btVector3 color(0,1,0); + { + if (cp.getDistance() <= btScalar(0.)) + { - float maxImpulse = 0.f; + if (iter == 0) + { + if (debugDrawer) + debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color); + } + + { + + //btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; + btScalar impulse = resolveSingleCollisionCombined( + *body0,*body1, + cp, + info); + + if (maxImpulse < impulse) + maxImpulse = impulse; + + } + } + } + } + return maxImpulse; +} + + + +btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) +{ + + btScalar maxImpulse = btScalar(0.); { btVector3 color(0,1,0); { - if (cp.getDistance() <= 0.f) + if (cp.getDistance() <= btScalar(0.)) { if (iter == 0) @@ -513,7 +1108,7 @@ float btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* { btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; - float impulse = cpd->m_contactSolverFunc( + btScalar impulse = cpd->m_contactSolverFunc( *body0,*body1, cp, info); @@ -528,16 +1123,19 @@ float btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* return maxImpulse; } -float btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) +btScalar btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) { + (void)debugDrawer; + (void)iter; + { btVector3 color(0,1,0); { - if (cp.getDistance() <= 0.f) + if (cp.getDistance() <= btScalar(0.)) { btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; @@ -552,5 +1150,5 @@ float btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRi } - return 0.f; + return btScalar(0.); } |