diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/ConstraintSolver')
19 files changed, 1138 insertions, 333 deletions
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h index ce90f798c04..7e8458c2c7b 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h @@ -16,12 +16,16 @@ subject to the following restrictions: #ifndef CONSTRAINT_SOLVER_H #define CONSTRAINT_SOLVER_H +#include "../../LinearMath/btScalar.h" + class btPersistentManifold; class btRigidBody; +class btCollisionObject; class btTypedConstraint; struct btContactSolverInfo; struct btBroadphaseProxy; class btIDebugDraw; +class btStackAlloc; /// btConstraintSolver provides solver interface class btConstraintSolver @@ -31,7 +35,7 @@ public: virtual ~btConstraintSolver() {} - virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer = 0) = 0; + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc) = 0; }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp index 1b85a0eea42..bb3fe832592 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @@ -30,13 +30,17 @@ subject to the following restrictions: //bilateral constraint between two dynamic objects void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, btRigidBody& body2, const btVector3& pos2, - btScalar distance, const btVector3& normal,btScalar& impulse ,float timeStep) + btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep) { - float normalLenSqr = normal.length2(); - ASSERT2(fabs(normalLenSqr) < 1.1f); - if (normalLenSqr > 1.1f) + (void)timeStep; + (void)distance; + + + btScalar normalLenSqr = normal.length2(); + ASSERT2(btFabs(normalLenSqr) < btScalar(1.1)); + if (normalLenSqr > btScalar(1.1)) { - impulse = 0.f; + impulse = btScalar(0.); return; } btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); @@ -54,24 +58,24 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, body2.getInvInertiaDiagLocal(),body2.getInvMass()); btScalar jacDiagAB = jac.getDiagonal(); - btScalar jacDiagABInv = 1.f / jacDiagAB; + btScalar jacDiagABInv = btScalar(1.) / jacDiagAB; btScalar rel_vel = jac.getRelativeVelocity( body1.getLinearVelocity(), body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(), body2.getLinearVelocity(), body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); - float a; + btScalar a; a=jacDiagABInv; rel_vel = normal.dot(vel); //todo: move this into proper structure - btScalar contactDamping = 0.2f; + btScalar contactDamping = btScalar(0.2); #ifdef ONLY_USE_LINEAR_MASS - btScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass()); + btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass()); impulse = - contactDamping * rel_vel * massTerm; #else btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; @@ -82,19 +86,20 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, //response between two dynamic objects with friction -float resolveSingleCollision( +btScalar resolveSingleCollision( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, const btContactSolverInfo& solverInfo) { - const btVector3& pos1 = contactPoint.getPositionWorldOnA(); - const btVector3& pos2 = contactPoint.getPositionWorldOnB(); + const btVector3& pos1_ = contactPoint.getPositionWorldOnA(); + const btVector3& pos2_ = contactPoint.getPositionWorldOnB(); const btVector3& normal = contactPoint.m_normalWorldOnB; - btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); - btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); + //constant over all iterations + btVector3 rel_pos1 = pos1_ - body1.getCenterOfMassPosition(); + btVector3 rel_pos2 = pos2_ - body2.getCenterOfMassPosition(); btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); @@ -102,11 +107,11 @@ float resolveSingleCollision( btScalar rel_vel; rel_vel = normal.dot(vel); - btScalar Kfps = 1.f / solverInfo.m_timeStep ; + btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ; - // float damping = solverInfo.m_damping ; - float Kerp = solverInfo.m_erp; - float Kcor = Kerp *Kfps; + // btScalar damping = solverInfo.m_damping ; + btScalar Kerp = solverInfo.m_erp; + btScalar Kcor = Kerp *Kfps; btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; assert(cpd); @@ -121,9 +126,9 @@ float resolveSingleCollision( btScalar normalImpulse = penetrationImpulse+velocityImpulse; // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse - float oldNormalImpulse = cpd->m_appliedImpulse; - float sum = oldNormalImpulse + normalImpulse; - cpd->m_appliedImpulse = 0.f > sum ? 0.f: sum; + btScalar oldNormalImpulse = cpd->m_appliedImpulse; + btScalar sum = oldNormalImpulse + normalImpulse; + cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum; normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse; @@ -145,13 +150,15 @@ float resolveSingleCollision( } -float resolveSingleFriction( +btScalar resolveSingleFriction( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, const btContactSolverInfo& solverInfo) { + (void)solverInfo; + const btVector3& pos1 = contactPoint.getPositionWorldOnA(); const btVector3& pos2 = contactPoint.getPositionWorldOnB(); @@ -161,11 +168,11 @@ float resolveSingleFriction( btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; assert(cpd); - float combinedFriction = cpd->m_friction; + btScalar combinedFriction = cpd->m_friction; btScalar limit = cpd->m_appliedImpulse * combinedFriction; - if (cpd->m_appliedImpulse>0.f) + if (cpd->m_appliedImpulse>btScalar(0.)) //friction { //apply friction in the 2 tangential directions @@ -183,7 +190,7 @@ float resolveSingleFriction( // calculate j that moves us to zero relative velocity j1 = -vrel * cpd->m_jacDiagABInvTangent0; - float oldTangentImpulse = cpd->m_accumulatedTangentImpulse0; + btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse0; cpd->m_accumulatedTangentImpulse0 = oldTangentImpulse + j1; GEN_set_min(cpd->m_accumulatedTangentImpulse0, limit); GEN_set_max(cpd->m_accumulatedTangentImpulse0, -limit); @@ -197,7 +204,7 @@ float resolveSingleFriction( // calculate j that moves us to zero relative velocity j2 = -vrel * cpd->m_jacDiagABInvTangent1; - float oldTangentImpulse = cpd->m_accumulatedTangentImpulse1; + btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse1; cpd->m_accumulatedTangentImpulse1 = oldTangentImpulse + j2; GEN_set_min(cpd->m_accumulatedTangentImpulse1, limit); GEN_set_max(cpd->m_accumulatedTangentImpulse1, -limit); @@ -226,13 +233,15 @@ float resolveSingleFriction( } -float resolveSingleFrictionOriginal( +btScalar resolveSingleFrictionOriginal( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, const btContactSolverInfo& solverInfo) { + (void)solverInfo; + const btVector3& pos1 = contactPoint.getPositionWorldOnA(); const btVector3& pos2 = contactPoint.getPositionWorldOnB(); @@ -242,10 +251,10 @@ float resolveSingleFrictionOriginal( btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; assert(cpd); - float combinedFriction = cpd->m_friction; + btScalar combinedFriction = cpd->m_friction; btScalar limit = cpd->m_appliedImpulse * combinedFriction; - //if (contactPoint.m_appliedImpulse>0.f) + //if (contactPoint.m_appliedImpulse>btScalar(0.)) //friction { //apply friction in the 2 tangential directions @@ -260,7 +269,7 @@ float resolveSingleFrictionOriginal( // calculate j that moves us to zero relative velocity btScalar j = -vrel * cpd->m_jacDiagABInvTangent0; - float total = cpd->m_accumulatedTangentImpulse0 + j; + btScalar total = cpd->m_accumulatedTangentImpulse0 + j; GEN_set_min(total, limit); GEN_set_max(total, -limit); j = total - cpd->m_accumulatedTangentImpulse0; @@ -280,7 +289,7 @@ float resolveSingleFrictionOriginal( // calculate j that moves us to zero relative velocity btScalar j = -vrel * cpd->m_jacDiagABInvTangent1; - float total = cpd->m_accumulatedTangentImpulse1 + j; + btScalar total = cpd->m_accumulatedTangentImpulse1 + j; GEN_set_min(total, limit); GEN_set_max(total, -limit); j = total - cpd->m_accumulatedTangentImpulse1; @@ -295,7 +304,7 @@ float resolveSingleFrictionOriginal( //velocity + friction //response between two dynamic objects with friction -float resolveSingleCollisionCombined( +btScalar resolveSingleCollisionCombined( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, @@ -315,11 +324,11 @@ float resolveSingleCollisionCombined( btScalar rel_vel; rel_vel = normal.dot(vel); - btScalar Kfps = 1.f / solverInfo.m_timeStep ; + btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ; - //float damping = solverInfo.m_damping ; - float Kerp = solverInfo.m_erp; - float Kcor = Kerp *Kfps; + //btScalar damping = solverInfo.m_damping ; + btScalar Kerp = solverInfo.m_erp; + btScalar Kcor = Kerp *Kfps; btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; assert(cpd); @@ -334,9 +343,9 @@ float resolveSingleCollisionCombined( btScalar normalImpulse = penetrationImpulse+velocityImpulse; // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse - float oldNormalImpulse = cpd->m_appliedImpulse; - float sum = oldNormalImpulse + normalImpulse; - cpd->m_appliedImpulse = 0.f > sum ? 0.f: sum; + btScalar oldNormalImpulse = cpd->m_appliedImpulse; + btScalar sum = oldNormalImpulse + normalImpulse; + cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum; normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse; @@ -367,7 +376,7 @@ float resolveSingleCollisionCombined( btVector3 lat_vel = vel - normal * rel_vel; btScalar lat_rel_vel = lat_vel.length(); - float combinedFriction = cpd->m_friction; + btScalar combinedFriction = cpd->m_friction; if (cpd->m_appliedImpulse > 0) if (lat_rel_vel > SIMD_EPSILON) @@ -390,12 +399,19 @@ float resolveSingleCollisionCombined( return normalImpulse; } -float resolveSingleFrictionEmpty( + +btScalar resolveSingleFrictionEmpty( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, const btContactSolverInfo& solverInfo) { - return 0.f; + (void)contactPoint; + (void)body1; + (void)body2; + (void)solverInfo; + + + return btScalar(0.); }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h index d88ba0d8ed4..0834deddeac 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h @@ -19,8 +19,8 @@ subject to the following restrictions: //todo: make into a proper class working with the iterative constraint solver class btRigidBody; -#include "LinearMath/btVector3.h" -#include "LinearMath/btScalar.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btScalar.h" struct btContactSolverInfo; class btManifoldPoint; @@ -33,7 +33,7 @@ enum { }; -typedef float (*ContactSolverFunc)(btRigidBody& body1, +typedef btScalar (*ContactSolverFunc)(btRigidBody& body1, btRigidBody& body2, class btManifoldPoint& contactPoint, const btContactSolverInfo& info); @@ -42,15 +42,15 @@ typedef float (*ContactSolverFunc)(btRigidBody& body1, struct btConstraintPersistentData { inline btConstraintPersistentData() - :m_appliedImpulse(0.f), - m_prevAppliedImpulse(0.f), - m_accumulatedTangentImpulse0(0.f), - m_accumulatedTangentImpulse1(0.f), - m_jacDiagABInv(0.f), + :m_appliedImpulse(btScalar(0.)), + m_prevAppliedImpulse(btScalar(0.)), + m_accumulatedTangentImpulse0(btScalar(0.)), + m_accumulatedTangentImpulse1(btScalar(0.)), + m_jacDiagABInv(btScalar(0.)), m_persistentLifeTime(0), - m_restitution(0.f), - m_friction(0.f), - m_penetration(0.f), + m_restitution(btScalar(0.)), + m_friction(btScalar(0.)), + m_penetration(btScalar(0.)), m_contactSolverFunc(0), m_frictionSolverFunc(0) { @@ -58,18 +58,18 @@ struct btConstraintPersistentData /// total applied impulse during most recent frame - float m_appliedImpulse; - float m_prevAppliedImpulse; - float m_accumulatedTangentImpulse0; - float m_accumulatedTangentImpulse1; + btScalar m_appliedImpulse; + btScalar m_prevAppliedImpulse; + btScalar m_accumulatedTangentImpulse0; + btScalar m_accumulatedTangentImpulse1; - float m_jacDiagABInv; - float m_jacDiagABInvTangent0; - float m_jacDiagABInvTangent1; + btScalar m_jacDiagABInv; + btScalar m_jacDiagABInvTangent0; + btScalar m_jacDiagABInvTangent1; int m_persistentLifeTime; - float m_restitution; - float m_friction; - float m_penetration; + btScalar m_restitution; + btScalar m_friction; + btScalar m_penetration; btVector3 m_frictionWorldTangential0; btVector3 m_frictionWorldTangential1; @@ -91,19 +91,28 @@ struct btConstraintPersistentData ///positive distance = separation, negative distance = penetration void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, btRigidBody& body2, const btVector3& pos2, - btScalar distance, const btVector3& normal,btScalar& impulse ,float timeStep); + btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep); ///contact constraint resolution: ///calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint ///positive distance = separation, negative distance = penetration -float resolveSingleCollision( +btScalar resolveSingleCollision( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, const btContactSolverInfo& info); -float resolveSingleFriction( +btScalar resolveSingleFriction( + btRigidBody& body1, + btRigidBody& body2, + btManifoldPoint& contactPoint, + const btContactSolverInfo& solverInfo + ); + + + +btScalar resolveSingleCollisionCombined( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index ed1ba6ac1ba..c3c73e300f4 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -22,25 +22,25 @@ struct btContactSolverInfo inline btContactSolverInfo() { - m_tau = 0.6f; - m_damping = 1.0f; - m_friction = 0.3f; - m_restitution = 0.f; - m_maxErrorReduction = 20.f; + m_tau = btScalar(0.6); + m_damping = btScalar(1.0); + m_friction = btScalar(0.3); + m_restitution = btScalar(0.); + m_maxErrorReduction = btScalar(20.); m_numIterations = 10; - m_erp = 0.4f; - m_sor = 1.3f; + m_erp = btScalar(0.4); + m_sor = btScalar(1.3); } - float m_tau; - float m_damping; - float m_friction; - float m_timeStep; - float m_restitution; + btScalar m_tau; + btScalar m_damping; + btScalar m_friction; + btScalar m_timeStep; + btScalar m_restitution; int m_numIterations; - float m_maxErrorReduction; - float m_sor; - float m_erp; + btScalar m_maxErrorReduction; + btScalar m_sor; + btScalar m_erp; }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index b2132a8d4f3..747d10d1f8b 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -17,8 +17,9 @@ subject to the following restrictions: #include "btGeneric6DofConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" +#include <new> -static const btScalar kSign[] = { 1.0f, -1.0f, 1.0f }; +static const btScalar kSign[] = { btScalar(1.0), btScalar(-1.0), btScalar(1.0) }; static const int kAxisA[] = { 1, 0, 0 }; static const int kAxisB[] = { 2, 2, 1 }; #define GENERIC_D6_DISABLE_WARMSTARTING 1 @@ -38,9 +39,9 @@ btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& //so start all locked for (int i=0; i<6;++i) { - m_lowerLimit[i] = 0.0f; - m_upperLimit[i] = 0.0f; - m_accumulatedImpulse[i] = 0.0f; + m_lowerLimit[i] = btScalar(0.0); + m_upperLimit[i] = btScalar(0.0); + m_accumulatedImpulse[i] = btScalar(0.0); } } @@ -83,7 +84,7 @@ void btGeneric6DofConstraint::buildJacobian() //optionally disable warmstarting #ifdef GENERIC_D6_DISABLE_WARMSTARTING - m_accumulatedImpulse[i] = 0.f; + m_accumulatedImpulse[i] = btScalar(0.); #endif //GENERIC_D6_DISABLE_WARMSTARTING // Apply accumulated impulse @@ -115,7 +116,7 @@ void btGeneric6DofConstraint::buildJacobian() m_rbB.getInvInertiaDiagLocal()); #ifdef GENERIC_D6_DISABLE_WARMSTARTING - m_accumulatedImpulse[i + 3] = 0.f; + m_accumulatedImpulse[i + 3] = btScalar(0.); #endif //GENERIC_D6_DISABLE_WARMSTARTING // Apply accumulated impulse @@ -127,7 +128,7 @@ void btGeneric6DofConstraint::buildJacobian() } } -float getMatrixElem(const btMatrix3x3& mat,int index) +btScalar getMatrixElem(const btMatrix3x3& mat,int index) { int row = index%3; int col = index / 3; @@ -143,9 +144,9 @@ bool MatrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) /// 0..8 - if (getMatrixElem(mat,2) < 1.0f) + if (getMatrixElem(mat,2) < btScalar(1.0)) { - if (getMatrixElem(mat,2) > -1.0f) + if (getMatrixElem(mat,2) > btScalar(-1.0)) { xyz[0] = btAtan2(-getMatrixElem(mat,5),getMatrixElem(mat,8)); xyz[1] = btAsin(getMatrixElem(mat,2)); @@ -157,7 +158,7 @@ bool MatrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) // WARNING. Not unique. XA - ZA = -atan2(r10,r11) xyz[0] = -btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4)); xyz[1] = -SIMD_HALF_PI; - xyz[2] = 0.0f; + xyz[2] = btScalar(0.0); return false; } } @@ -167,16 +168,17 @@ bool MatrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) xyz[0] = btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4)); xyz[1] = SIMD_HALF_PI; xyz[2] = 0.0; - return false; + } - return false; + + return false; } void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) { - btScalar tau = 0.1f; - btScalar damping = 1.0f; + btScalar tau = btScalar(0.1); + btScalar damping = btScalar(1.0); btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin(); btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin(); @@ -199,7 +201,7 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) localNormalInA[i] = 1; btVector3 normalWorld = m_rbA.getCenterOfMassTransform().getBasis() * localNormalInA; - btScalar jacDiagABInv = 1.f / m_jacLinear[i].getDiagonal(); + btScalar jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal(); //velocity error (first order error) btScalar rel_vel = m_jacLinear[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, @@ -207,8 +209,8 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) //positional error (zeroth order error) btScalar depth = -(pivotAInW - pivotBInW).dot(normalWorld); - btScalar lo = -1e30f; - btScalar hi = 1e30f; + btScalar lo = btScalar(-1e30); + btScalar hi = btScalar(1e30); //handle the limits if (m_lowerLimit[i] < m_upperLimit[i]) @@ -217,14 +219,14 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) if (depth > m_upperLimit[i]) { depth -= m_upperLimit[i]; - lo = 0.f; + lo = btScalar(0.); } else { if (depth < m_lowerLimit[i]) { depth -= m_lowerLimit[i]; - hi = 0.f; + hi = btScalar(0.); } else { continue; @@ -234,9 +236,9 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) } btScalar normalImpulse= (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv; - float oldNormalImpulse = m_accumulatedImpulse[i]; - float sum = oldNormalImpulse + normalImpulse; - m_accumulatedImpulse[i] = sum > hi ? 0.f : sum < lo ? 0.f : sum; + btScalar oldNormalImpulse = m_accumulatedImpulse[i]; + btScalar sum = oldNormalImpulse + normalImpulse; + m_accumulatedImpulse[i] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; normalImpulse = m_accumulatedImpulse[i] - oldNormalImpulse; btVector3 impulse_vector = normalWorld * normalImpulse; @@ -267,7 +269,7 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); - btScalar jacDiagABInv = 1.f / m_jacAng[i].getDiagonal(); + btScalar jacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal(); //velocity error (first order error) btScalar rel_vel = m_jacAng[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, @@ -279,27 +281,27 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) btScalar rel_pos = kSign[i] * axisA.dot(axisB); - btScalar lo = -1e30f; - btScalar hi = 1e30f; + btScalar lo = btScalar(-1e30); + btScalar hi = btScalar(1e30); //handle the twist limit if (m_lowerLimit[i+3] < m_upperLimit[i+3]) { //clamp the values - btScalar loLimit = m_upperLimit[i+3] > -3.1415 ? m_lowerLimit[i+3] : -1e30f; - btScalar hiLimit = m_upperLimit[i+3] < 3.1415 ? m_upperLimit[i+3] : 1e30f; + btScalar loLimit = m_lowerLimit[i+3] > -3.1415 ? m_lowerLimit[i+3] : btScalar(-1e30); + btScalar hiLimit = m_upperLimit[i+3] < 3.1415 ? m_upperLimit[i+3] : btScalar(1e30); - float projAngle = -2.f*xyz[i]; + btScalar projAngle = btScalar(-1.)*xyz[i]; if (projAngle < loLimit) { - hi = 0.f; + hi = btScalar(0.); rel_pos = (loLimit - projAngle); } else { if (projAngle > hiLimit) { - lo = 0.f; + lo = btScalar(0.); rel_pos = (hiLimit - projAngle); } else { @@ -311,9 +313,9 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) //impulse btScalar normalImpulse= -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv; - float oldNormalImpulse = m_accumulatedImpulse[i+3]; - float sum = oldNormalImpulse + normalImpulse; - m_accumulatedImpulse[i+3] = sum > hi ? 0.f : sum < lo ? 0.f : sum; + btScalar oldNormalImpulse = m_accumulatedImpulse[i+3]; + btScalar sum = oldNormalImpulse + normalImpulse; + m_accumulatedImpulse[i+3] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; normalImpulse = m_accumulatedImpulse[i+3] - oldNormalImpulse; // Dirk: Not needed - we could actually project onto Jacobian entry here (same as above) @@ -328,12 +330,13 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) void btGeneric6DofConstraint::updateRHS(btScalar timeStep) { + (void)timeStep; } btScalar btGeneric6DofConstraint::computeAngle(int axis) const { - btScalar angle; + btScalar angle = btScalar(0.f); switch (axis) { @@ -375,9 +378,12 @@ btScalar btGeneric6DofConstraint::computeAngle(int axis) const angle = btAtan2( s, c ); } break; - default: assert ( 0 ) ; break ; + default: + btAssert ( 0 ) ; + + break ; } - return angle; + return angle; } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index 329048b5737..b114e54fa69 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -16,9 +16,8 @@ subject to the following restrictions: #ifndef GENERIC_6DOF_CONSTRAINT_H #define GENERIC_6DOF_CONSTRAINT_H -#include "LinearMath/btVector3.h" - -#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h" +#include "../../LinearMath/btVector3.h" +#include "btJacobianEntry.h" #include "btTypedConstraint.h" class btRigidBody; @@ -41,12 +40,19 @@ class btGeneric6DofConstraint : public btTypedConstraint btScalar m_accumulatedImpulse[6]; + btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other) + { + btAssert(0); + (void) other; + return *this; + } public: btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ); btGeneric6DofConstraint(); + virtual void buildJacobian(); virtual void solveConstraint(btScalar timeStep); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index f72278e2cbf..27e30987549 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -17,7 +17,7 @@ subject to the following restrictions: #include "btHingeConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" - +#include <new> btHingeConstraint::btHingeConstraint(): m_enableAngularMotor(false) @@ -49,7 +49,7 @@ m_enableAngularMotor(false) void btHingeConstraint::buildJacobian() { - m_appliedImpulse = 0.f; + m_appliedImpulse = btScalar(0.); btVector3 normal(0,0,0); @@ -115,8 +115,8 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; btVector3 normal(0,0,0); - btScalar tau = 0.3f; - btScalar damping = 1.f; + btScalar tau = btScalar(0.3); + btScalar damping = btScalar(1.); //linear part if (!m_angularOnly) @@ -124,7 +124,7 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) for (int i=0;i<3;i++) { normal[i] = 1; - btScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal(); + btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); @@ -165,27 +165,27 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) btVector3 velrelOrthog = angAorthog-angBorthog; { //solve orthogonal angular velocity correction - float relaxation = 1.f; - float len = velrelOrthog.length(); - if (len > 0.00001f) + btScalar relaxation = btScalar(1.); + btScalar len = velrelOrthog.length(); + if (len > btScalar(0.00001)) { btVector3 normal = velrelOrthog.normalized(); - float denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + + btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + getRigidBodyB().computeAngularImpulseDenominator(normal); // scale for mass and relaxation //todo: expose this 0.9 factor to developer - velrelOrthog *= (1.f/denom) * 0.9f; + velrelOrthog *= (btScalar(1.)/denom) * btScalar(0.9); } //solve angular positional correction - btVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep); - float len2 = angularError.length(); - if (len2>0.00001f) + btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep); + btScalar len2 = angularError.length(); + if (len2>btScalar(0.00001)) { btVector3 normal2 = angularError.normalized(); - float denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + + btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + getRigidBodyB().computeAngularImpulseDenominator(normal2); - angularError *= (1.f/denom2) * relaxation; + angularError *= (btScalar(1.)/denom2) * relaxation; } m_rbA.applyTorqueImpulse(-velrelOrthog+angularError); @@ -204,10 +204,10 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) btScalar desiredMotorVel = m_motorTargetVelocity; btScalar motor_relvel = desiredMotorVel - projRelVel; - float denom3 = getRigidBodyA().computeAngularImpulseDenominator(axisA) + + btScalar denom3 = getRigidBodyA().computeAngularImpulseDenominator(axisA) + getRigidBodyB().computeAngularImpulseDenominator(axisA); - btScalar unclippedMotorImpulse = (1.f/denom3) * motor_relvel;; + btScalar unclippedMotorImpulse = (btScalar(1.)/denom3) * motor_relvel;; //todo: should clip against accumulated impulse btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; @@ -223,6 +223,7 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) void btHingeConstraint::updateRHS(btScalar timeStep) { + (void)timeStep; } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 553ec135c8a..5c1ceafbc5b 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -16,9 +16,8 @@ subject to the following restrictions: #ifndef HINGECONSTRAINT_H #define HINGECONSTRAINT_H -#include "LinearMath/btVector3.h" - -#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h" +#include "../../LinearMath/btVector3.h" +#include "btJacobianEntry.h" #include "btTypedConstraint.h" class btRigidBody; @@ -38,8 +37,8 @@ class btHingeConstraint : public btTypedConstraint bool m_angularOnly; - float m_motorTargetVelocity; - float m_maxMotorImpulse; + btScalar m_motorTargetVelocity; + btScalar m_maxMotorImpulse; bool m_enableAngularMotor; public: @@ -70,7 +69,7 @@ public: m_angularOnly = angularOnly; } - void enableAngularMotor(bool enableMotor,float targetVelocity,float maxMotorImpulse) + void enableAngularMotor(bool enableMotor,btScalar targetVelocity,btScalar maxMotorImpulse) { m_enableAngularMotor = enableMotor; m_motorTargetVelocity = targetVelocity; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h index 384e4f7bab5..aae3ed0373f 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h @@ -16,8 +16,8 @@ subject to the following restrictions: #ifndef JACOBIAN_ENTRY_H #define JACOBIAN_ENTRY_H -#include "LinearMath/btVector3.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "../../LinearMath/btVector3.h" +#include "../Dynamics/btRigidBody.h" //notes: @@ -50,7 +50,7 @@ public: m_1MinvJt = inertiaInvB * m_bJ; m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); - btAssert(m_Adiag > 0.0f); + btAssert(m_Adiag > btScalar(0.0)); } //angular constraint between two different rigidbodies @@ -59,7 +59,7 @@ public: const btMatrix3x3& world2B, const btVector3& inertiaInvA, const btVector3& inertiaInvB) - :m_linearJointAxis(btVector3(0.f,0.f,0.f)) + :m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.))) { m_aJ= world2A*jointAxis; m_bJ = world2B*-jointAxis; @@ -67,7 +67,7 @@ public: m_1MinvJt = inertiaInvB * m_bJ; m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); - btAssert(m_Adiag > 0.0f); + btAssert(m_Adiag > btScalar(0.0)); } //angular constraint between two different rigidbodies @@ -75,7 +75,7 @@ public: const btVector3& axisInB, const btVector3& inertiaInvA, const btVector3& inertiaInvB) - : m_linearJointAxis(btVector3(0.f,0.f,0.f)) + : m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.))) , m_aJ(axisInA) , m_bJ(-axisInB) { @@ -83,7 +83,7 @@ public: m_1MinvJt = inertiaInvB * m_bJ; m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); - btAssert(m_Adiag > 0.0f); + btAssert(m_Adiag > btScalar(0.0)); } //constraint on one rigidbody @@ -98,10 +98,10 @@ public: m_aJ= world2A*(rel_pos1.cross(jointAxis)); m_bJ = world2A*(rel_pos2.cross(-jointAxis)); m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = btVector3(0.f,0.f,0.f); + m_1MinvJt = btVector3(btScalar(0.),btScalar(0.),btScalar(0.)); m_Adiag = massInvA + m_0MinvJt.dot(m_aJ); - btAssert(m_Adiag > 0.0f); + btAssert(m_Adiag > btScalar(0.0)); } btScalar getDiagonal() const { return m_Adiag; } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp index d15bdaad790..aacb0a3ea66 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp @@ -16,7 +16,7 @@ subject to the following restrictions: #include "btPoint2PointConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" - +#include <new> @@ -39,7 +39,7 @@ btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector void btPoint2PointConstraint::buildJacobian() { - m_appliedImpulse = 0.f; + m_appliedImpulse = btScalar(0.); btVector3 normal(0,0,0); @@ -76,7 +76,7 @@ void btPoint2PointConstraint::solveConstraint(btScalar timeStep) for (int i=0;i<3;i++) { normal[i] = 1; - btScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal(); + btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); @@ -110,6 +110,7 @@ void btPoint2PointConstraint::solveConstraint(btScalar timeStep) void btPoint2PointConstraint::updateRHS(btScalar timeStep) { + (void)timeStep; } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h index 8aae8d74ce7..71da8ac0347 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h @@ -16,9 +16,8 @@ subject to the following restrictions: #ifndef POINT2POINTCONSTRAINT_H #define POINT2POINTCONSTRAINT_H -#include "LinearMath/btVector3.h" - -#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h" +#include "../../LinearMath/btVector3.h" +#include "btJacobianEntry.h" #include "btTypedConstraint.h" class btRigidBody; @@ -26,12 +25,12 @@ class btRigidBody; struct btConstraintSetting { btConstraintSetting() : - m_tau(0.3f), - m_damping(1.f) + m_tau(btScalar(0.3)), + m_damping(btScalar(1.)) { } - float m_tau; - float m_damping; + btScalar m_tau; + btScalar m_damping; }; /// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space 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.); } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 0989a86e2cd..13e70c41be4 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -18,7 +18,6 @@ subject to the following restrictions: #include "btConstraintSolver.h" class btIDebugDraw; - #include "btContactConstraint.h" @@ -31,8 +30,8 @@ class btSequentialImpulseConstraintSolver : public btConstraintSolver { protected: - float solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); - float solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); + btScalar solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); + btScalar solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); void prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer); ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]; @@ -40,6 +39,8 @@ protected: //choose between several modes, different friction model etc. int m_solverMode; + ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction + unsigned long m_btSeed2; public: @@ -47,7 +48,8 @@ public: { SOLVER_RANDMIZE_ORDER = 1, SOLVER_FRICTION_SEPARATE = 2, - SOLVER_USE_WARMSTARTING = 4 + SOLVER_USE_WARMSTARTING = 4, + SOLVER_CACHE_FRIENDLY = 8 }; btSequentialImpulseConstraintSolver(); @@ -68,7 +70,12 @@ public: virtual ~btSequentialImpulseConstraintSolver() {} - virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0); + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc); + + virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); + + btScalar solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); + void setSolverMode(int mode) { @@ -79,20 +86,24 @@ public: { return m_solverMode; } -}; - -/// Small variation on btSequentialImpulseConstraintSolver: warmstarting, separate friction, non-randomized ordering -class btSequentialImpulseConstraintSolver3 : public btSequentialImpulseConstraintSolver -{ -public: - btSequentialImpulseConstraintSolver3(); + unsigned long btRand2(); - virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0); + int btRandInt2 (int n); + void setRandSeed(unsigned long seed) + { + m_btSeed2 = seed; + } + unsigned long getRandSeed() const + { + return m_btSeed2; + } }; + + #endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp index edca6c8fa08..0c7dbd668bb 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp @@ -43,12 +43,18 @@ void btSolve2LinearConstraint::resolveUnilateralPairConstraint( btScalar depthB, const btVector3& normalB, btScalar& imp0,btScalar& imp1) { + (void)linvelA; + (void)linvelB; + (void)angvelB; + (void)angvelA; - imp0 = 0.f; - imp1 = 0.f; - btScalar len = fabs(normalA.length())-1.f; - if (fabs(len) >= SIMD_EPSILON) + + imp0 = btScalar(0.); + imp1 = btScalar(0.); + + btScalar len = btFabs(normalA.length()) - btScalar(1.); + if (btFabs(len) >= SIMD_EPSILON) return; btAssert(len < SIMD_EPSILON); @@ -67,7 +73,7 @@ void btSolve2LinearConstraint::resolveUnilateralPairConstraint( const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1)); // btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv - btScalar massTerm = 1.f / (invMassA + invMassB); + btScalar massTerm = btScalar(1.) / (invMassA + invMassB); // calculate rhs (or error) terms @@ -87,7 +93,7 @@ void btSolve2LinearConstraint::resolveUnilateralPairConstraint( btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB); - btScalar invDet = 1.0f / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); + btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; @@ -126,11 +132,18 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint( btScalar& imp0,btScalar& imp1) { - imp0 = 0.f; - imp1 = 0.f; + (void)linvelA; + (void)linvelB; + (void)angvelA; + (void)angvelB; + + - btScalar len = fabs(normalA.length())-1.f; - if (fabs(len) >= SIMD_EPSILON) + imp0 = btScalar(0.); + imp1 = btScalar(0.); + + btScalar len = btFabs(normalA.length()) - btScalar(1.); + if (btFabs(len) >= SIMD_EPSILON) return; btAssert(len < SIMD_EPSILON); @@ -164,7 +177,7 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint( btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB); - btScalar invDet = 1.0f / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); + btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; @@ -178,41 +191,41 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint( //[jA nD] * [imp0] = [dv0] //[nD jB] [imp1] [dv1] - if ( imp0 > 0.0f) + if ( imp0 > btScalar(0.0)) { - if ( imp1 > 0.0f ) + if ( imp1 > btScalar(0.0) ) { //both positive } else { - imp1 = 0.f; + imp1 = btScalar(0.); // now imp0>0 imp1<0 imp0 = dv0 / jacA.getDiagonal(); - if ( imp0 > 0.0f ) + if ( imp0 > btScalar(0.0) ) { } else { - imp0 = 0.f; + imp0 = btScalar(0.); } } } else { - imp0 = 0.f; + imp0 = btScalar(0.); imp1 = dv1 / jacB.getDiagonal(); - if ( imp1 <= 0.0f ) + if ( imp1 <= btScalar(0.0) ) { - imp1 = 0.f; + imp1 = btScalar(0.); // now imp0>0 imp1<0 imp0 = dv0 / jacA.getDiagonal(); - if ( imp0 > 0.0f ) + if ( imp0 > btScalar(0.0) ) { } else { - imp0 = 0.f; + imp0 = btScalar(0.); } } else { @@ -221,7 +234,7 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint( } - +/* void btSolve2LinearConstraint::resolveAngularConstraint( const btMatrix3x3& invInertiaAWS, const btScalar invMassA, const btVector3& linvelA,const btVector3& angvelA, @@ -238,4 +251,5 @@ void btSolve2LinearConstraint::resolveAngularConstraint( const btMatrix3x3& invI { } +*/ diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h index 639e4c9433f..e7d26645c6a 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h @@ -16,8 +16,8 @@ subject to the following restrictions: #ifndef SOLVE_2LINEAR_CONSTRAINT_H #define SOLVE_2LINEAR_CONSTRAINT_H -#include "LinearMath/btMatrix3x3.h" -#include "LinearMath/btVector3.h" +#include "../../LinearMath/btMatrix3x3.h" +#include "../../LinearMath/btVector3.h" class btRigidBody; @@ -85,7 +85,7 @@ public: btScalar depthB, const btVector3& normalB, btScalar& imp0,btScalar& imp1); - +/* void resolveAngularConstraint( const btMatrix3x3& invInertiaAWS, const btScalar invMassA, const btVector3& linvelA,const btVector3& angvelA, @@ -100,6 +100,7 @@ public: btScalar depthB, const btVector3& normalB, btScalar& imp0,btScalar& imp1); +*/ }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h new file mode 100644 index 00000000000..0ab536f42b3 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h @@ -0,0 +1,71 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SOLVER_BODY_H +#define BT_SOLVER_BODY_H + +class btRigidBody; +#include "LinearMath/btVector3.h" +#include "LinearMath/btMatrix3x3.h" + + + + +ATTRIBUTE_ALIGNED16 (struct) btSolverBody +{ + btVector3 m_centerOfMassPosition; + btVector3 m_linearVelocity; + btVector3 m_angularVelocity; + btRigidBody* m_originalBody; + float m_invMass; + float m_friction; + float m_angularFactor; + + inline void getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const + { + velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos); + } + + //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position + inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) + { + m_linearVelocity += linearComponent*impulseMagnitude; + m_angularVelocity += angularComponent*impulseMagnitude*m_angularFactor; + } + + void writebackVelocity() + { + if (m_invMass) + { + m_originalBody->setLinearVelocity(m_linearVelocity); + m_originalBody->setAngularVelocity(m_angularVelocity); + } + } + + void readVelocity() + { + if (m_invMass) + { + m_linearVelocity = m_originalBody->getLinearVelocity(); + m_angularVelocity = m_originalBody->getAngularVelocity(); + } + } + + + + +}; + +#endif //BT_SOLVER_BODY_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h new file mode 100644 index 00000000000..f1f40ffdf19 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h @@ -0,0 +1,63 @@ + + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SOLVER_CONSTRAINT_H +#define BT_SOLVER_CONSTRAINT_H + +class btRigidBody; +#include "LinearMath/btVector3.h" +#include "LinearMath/btMatrix3x3.h" + +//#define NO_FRICTION_TANGENTIALS 1 + +///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. +ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint +{ + btVector3 m_relpos1CrossNormal; + btVector3 m_relpos2CrossNormal; + btVector3 m_contactNormal; + btVector3 m_angularComponentA; + btVector3 m_angularComponentB; + + btScalar m_appliedVelocityImpulse; + int m_solverBodyIdA; + int m_solverBodyIdB; + btScalar m_friction; + btScalar m_restitution; + btScalar m_jacDiagABInv; + btScalar m_penetration; + btScalar m_appliedImpulse; + + int m_constraintType; + int m_frictionIndex; + int m_unusedPadding[2]; + + enum btSolverConstraintType + { + BT_SOLVER_CONTACT_1D = 0, + BT_SOLVER_FRICTION_1D + }; +}; + + + + + + +#endif //BT_SOLVER_CONSTRAINT_H + + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index cb3fa72d440..a15b3e026cd 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -24,18 +24,18 @@ btTypedConstraint::btTypedConstraint() m_userConstraintId(-1), m_rbA(s_fixed), m_rbB(s_fixed), -m_appliedImpulse(0.f) +m_appliedImpulse(btScalar(0.)) { - s_fixed.setMassProps(0.f,btVector3(0.f,0.f,0.f)); + s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); } btTypedConstraint::btTypedConstraint(btRigidBody& rbA) : m_userConstraintType(-1), m_userConstraintId(-1), m_rbA(rbA), m_rbB(s_fixed), -m_appliedImpulse(0.f) +m_appliedImpulse(btScalar(0.)) { - s_fixed.setMassProps(0.f,btVector3(0.f,0.f,0.f)); + s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); } @@ -45,9 +45,9 @@ btTypedConstraint::btTypedConstraint(btRigidBody& rbA,btRigidBody& rbB) m_userConstraintId(-1), m_rbA(rbA), m_rbB(rbB), -m_appliedImpulse(0.f) +m_appliedImpulse(btScalar(0.)) { - s_fixed.setMassProps(0.f,btVector3(0.f,0.f,0.f)); + s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index bc25eaa759c..dfee6e80d0e 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -17,19 +17,25 @@ subject to the following restrictions: #define TYPED_CONSTRAINT_H class btRigidBody; -#include "LinearMath/btScalar.h" +#include "../../LinearMath/btScalar.h" ///TypedConstraint is the baseclass for Bullet constraints and vehicles class btTypedConstraint { int m_userConstraintType; int m_userConstraintId; - + + btTypedConstraint& operator=(btTypedConstraint& other) + { + btAssert(0); + (void) other; + return *this; + } protected: btRigidBody& m_rbA; btRigidBody& m_rbB; - float m_appliedImpulse; + btScalar m_appliedImpulse; public: @@ -81,7 +87,7 @@ public: { return m_userConstraintId; } - float getAppliedImpulse() + btScalar getAppliedImpulse() { return m_appliedImpulse; } |