diff options
author | Erwin Coumans <blender@erwincoumans.com> | 2008-09-03 06:27:16 +0400 |
---|---|---|
committer | Erwin Coumans <blender@erwincoumans.com> | 2008-09-03 06:27:16 +0400 |
commit | 1926e846500212d11061c23cacdbd08d88e375da (patch) | |
tree | ad72fe64632cfc22a4878e065578b66d52de218d /extern/bullet2/src/BulletDynamics/ConstraintSolver | |
parent | 33ac84e888ee3e3217f15f955e1b389a9c4bb230 (diff) |
Finally upgraded to latest Bullet subversion, about to release 2.71. Some recent changes in extern/bullet2 need to be re-applied, will check with Benoit. Ray tests in 0_FPS_Template.blend is broken, didn't figure out why yet.
HELP BUILD SYSTEM MAINTAINERS: Please help with updating all build systems: the newly added files need to be added. Note that the src/SoftBody has been added for future extension of real-time soft bodies.
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/ConstraintSolver')
22 files changed, 2575 insertions, 832 deletions
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index 2289621e8e3..e11b49d9420 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -19,24 +19,20 @@ Written by: Marcus Hennix #include "btConeTwistConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" -#include "LinearMath/btSimdMinMax.h" +#include "LinearMath/btMinMax.h" #include <new> btConeTwistConstraint::btConeTwistConstraint() +:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE) { } btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame,const btTransform& rbBFrame) - :btTypedConstraint(rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), + :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), m_angularOnly(false) { - // flip axis for correct angles - m_rbBFrame.getBasis()[1][0] *= btScalar(-1.); - m_rbBFrame.getBasis()[1][1] *= btScalar(-1.); - m_rbBFrame.getBasis()[1][2] *= btScalar(-1.); - m_swingSpan1 = btScalar(1e30); m_swingSpan2 = btScalar(1e30); m_twistSpan = btScalar(1e30); @@ -49,20 +45,11 @@ btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, } btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame) - :btTypedConstraint(rbA),m_rbAFrame(rbAFrame), + :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame), m_angularOnly(false) { m_rbBFrame = m_rbAFrame; - // flip axis for correct angles - m_rbBFrame.getBasis()[1][0] *= btScalar(-1.); - m_rbBFrame.getBasis()[1][1] *= btScalar(-1.); - m_rbBFrame.getBasis()[1][2] *= btScalar(-1.); - - m_rbBFrame.getBasis()[2][0] *= btScalar(-1.); - m_rbBFrame.getBasis()[2][1] *= btScalar(-1.); - m_rbBFrame.getBasis()[2][2] *= btScalar(-1.); - m_swingSpan1 = btScalar(1e30); m_swingSpan2 = btScalar(1e30); m_twistSpan = btScalar(1e30); @@ -142,7 +129,7 @@ void btConeTwistConstraint::buildJacobian() btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1); btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2); - btScalar EllipseAngle = btFabs(swing1)* RMaxAngle1Sq + btFabs(swing2) * RMaxAngle2Sq; + btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq; if (EllipseAngle > 1.0f) { @@ -246,7 +233,7 @@ void btConeTwistConstraint::solveConstraint(btScalar timeStep) // Clamp the accumulated impulse btScalar temp = m_accSwingLimitImpulse; - m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, 0.0f ); + m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) ); impulseMag = m_accSwingLimitImpulse - temp; btVector3 impulse = m_swingAxis * impulseMag; @@ -264,7 +251,7 @@ void btConeTwistConstraint::solveConstraint(btScalar timeStep) // Clamp the accumulated impulse btScalar temp = m_accTwistLimitImpulse; - m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, 0.0f ); + m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) ); impulseMag = m_accTwistLimitImpulse - temp; btVector3 impulse = m_twistAxis * impulseMag; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h index 874669c80b3..f121919c8f9 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -30,6 +30,9 @@ class btRigidBody; ///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc) class btConeTwistConstraint : public btTypedConstraint { +#ifdef IN_PARALLELL_SOLVER +public: +#endif btJacobianEntry m_jac[3]; //3 orthogonal linear constraints btTransform m_rbAFrame; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h index 7e8458c2c7b..7a8e9c1953d 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef CONSTRAINT_SOLVER_H #define CONSTRAINT_SOLVER_H -#include "../../LinearMath/btScalar.h" +#include "LinearMath/btScalar.h" class btPersistentManifold; class btRigidBody; @@ -26,7 +26,7 @@ struct btContactSolverInfo; struct btBroadphaseProxy; class btIDebugDraw; class btStackAlloc; - +class btDispatcher; /// btConstraintSolver provides solver interface class btConstraintSolver { @@ -35,8 +35,15 @@ public: virtual ~btConstraintSolver() {} - virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc) = 0; + virtual void prepareSolve (int /* numBodies */, int /* numManifolds */) {;} + + ///solve a group of constraints + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher) = 0; + + virtual void allSolved (const btContactSolverInfo& /* info */,class btIDebugDraw* /* debugDrawer */, btStackAlloc* /* stackAlloc */) {;} + ///clear internal cached data and reset random seed + virtual void reset() = 0; }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp index bb3fe832592..4d7cd05feb7 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @@ -192,8 +192,8 @@ btScalar resolveSingleFriction( j1 = -vrel * cpd->m_jacDiagABInvTangent0; btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse0; cpd->m_accumulatedTangentImpulse0 = oldTangentImpulse + j1; - GEN_set_min(cpd->m_accumulatedTangentImpulse0, limit); - GEN_set_max(cpd->m_accumulatedTangentImpulse0, -limit); + btSetMin(cpd->m_accumulatedTangentImpulse0, limit); + btSetMax(cpd->m_accumulatedTangentImpulse0, -limit); j1 = cpd->m_accumulatedTangentImpulse0 - oldTangentImpulse; } @@ -206,8 +206,8 @@ btScalar resolveSingleFriction( j2 = -vrel * cpd->m_jacDiagABInvTangent1; btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse1; cpd->m_accumulatedTangentImpulse1 = oldTangentImpulse + j2; - GEN_set_min(cpd->m_accumulatedTangentImpulse1, limit); - GEN_set_max(cpd->m_accumulatedTangentImpulse1, -limit); + btSetMin(cpd->m_accumulatedTangentImpulse1, limit); + btSetMax(cpd->m_accumulatedTangentImpulse1, -limit); j2 = cpd->m_accumulatedTangentImpulse1 - oldTangentImpulse; } @@ -237,6 +237,12 @@ btScalar resolveSingleFrictionOriginal( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, + const btContactSolverInfo& solverInfo); + +btScalar resolveSingleFrictionOriginal( + btRigidBody& body1, + btRigidBody& body2, + btManifoldPoint& contactPoint, const btContactSolverInfo& solverInfo) { @@ -270,8 +276,8 @@ btScalar resolveSingleFrictionOriginal( // calculate j that moves us to zero relative velocity btScalar j = -vrel * cpd->m_jacDiagABInvTangent0; btScalar total = cpd->m_accumulatedTangentImpulse0 + j; - GEN_set_min(total, limit); - GEN_set_max(total, -limit); + btSetMin(total, limit); + btSetMax(total, -limit); j = total - cpd->m_accumulatedTangentImpulse0; cpd->m_accumulatedTangentImpulse0 = total; body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1); @@ -290,8 +296,8 @@ btScalar resolveSingleFrictionOriginal( // calculate j that moves us to zero relative velocity btScalar j = -vrel * cpd->m_jacDiagABInvTangent1; btScalar total = cpd->m_accumulatedTangentImpulse1 + j; - GEN_set_min(total, limit); - GEN_set_max(total, -limit); + btSetMin(total, limit); + btSetMax(total, -limit); j = total - cpd->m_accumulatedTangentImpulse1; cpd->m_accumulatedTangentImpulse1 = total; body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1); @@ -388,8 +394,8 @@ btScalar resolveSingleCollisionCombined( (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2))); btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction; - GEN_set_min(friction_impulse, normal_impulse); - GEN_set_max(friction_impulse, -normal_impulse); + btSetMin(friction_impulse, normal_impulse); + btSetMax(friction_impulse, -normal_impulse); body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1); body2.applyImpulse(lat_vel * friction_impulse, rel_pos2); } @@ -404,6 +410,12 @@ btScalar resolveSingleFrictionEmpty( btRigidBody& body1, btRigidBody& body2, btManifoldPoint& contactPoint, + const btContactSolverInfo& solverInfo); + +btScalar resolveSingleFrictionEmpty( + btRigidBody& body1, + btRigidBody& body2, + btManifoldPoint& contactPoint, const btContactSolverInfo& solverInfo) { (void)contactPoint; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h index 0834deddeac..826e79f78bd 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; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index c3c73e300f4..916d4581f79 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -16,10 +16,43 @@ subject to the following restrictions: #ifndef CONTACT_SOLVER_INFO #define CONTACT_SOLVER_INFO +enum btSolverMode +{ + SOLVER_RANDMIZE_ORDER = 1, + SOLVER_FRICTION_SEPARATE = 2, + SOLVER_USE_WARMSTARTING = 4, + SOLVER_CACHE_FRIENDLY = 8 +}; + +struct btContactSolverInfoData +{ + + + btScalar m_tau; + btScalar m_damping; + btScalar m_friction; + btScalar m_timeStep; + btScalar m_restitution; + int m_numIterations; + btScalar m_maxErrorReduction; + btScalar m_sor; + btScalar m_erp;//used as Baumgarte factor + btScalar m_erp2;//used in Split Impulse + int m_splitImpulse; + btScalar m_splitImpulsePenetrationThreshold; + btScalar m_linearSlop; + btScalar m_warmstartingFactor; + + int m_solverMode; -struct btContactSolverInfo + +}; + +struct btContactSolverInfo : public btContactSolverInfoData { + + inline btContactSolverInfo() { m_tau = btScalar(0.6); @@ -28,20 +61,15 @@ struct btContactSolverInfo m_restitution = btScalar(0.); m_maxErrorReduction = btScalar(20.); m_numIterations = 10; - m_erp = btScalar(0.4); + m_erp = btScalar(0.2); + m_erp2 = btScalar(0.1); m_sor = btScalar(1.3); + m_splitImpulse = false; + m_splitImpulsePenetrationThreshold = -0.02f; + m_linearSlop = btScalar(0.0); + m_warmstartingFactor=btScalar(0.85); + m_solverMode = SOLVER_RANDMIZE_ORDER | SOLVER_CACHE_FRIENDLY | SOLVER_USE_WARMSTARTING; } - - btScalar m_tau; - btScalar m_damping; - btScalar m_friction; - btScalar m_timeStep; - btScalar m_restitution; - int m_numIterations; - btScalar m_maxErrorReduction; - btScalar m_sor; - btScalar m_erp; - }; #endif //CONTACT_SOLVER_INFO diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index 747d10d1f8b..077b326d13a 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -4,14 +4,20 @@ 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, +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. */ +/* +2007-09-09 +Refactored by Francisco Le?n +email: projectileman@yahoo.com +http://gimpact.sf.net +*/ #include "btGeneric6DofConstraint.h" @@ -19,371 +25,504 @@ subject to the following restrictions: #include "LinearMath/btTransformUtil.h" #include <new> + 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 -btGeneric6DofConstraint::btGeneric6DofConstraint() +btScalar btGetMatrixElem(const btMatrix3x3& mat, int index); +btScalar btGetMatrixElem(const btMatrix3x3& mat, int index) { + int i = index%3; + int j = index/3; + return mat[i][j]; } -btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB) -: btTypedConstraint(rbA, rbB) -, m_frameInA(frameInA) -, m_frameInB(frameInB) +///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html +bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz); +bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) { - //free means upper < lower, - //locked means upper == lower - //limited means upper > lower - //so start all locked - for (int i=0; i<6;++i) +// // rot = cy*cz -cy*sz sy +// // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx +// // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy +// + + if (btGetMatrixElem(mat,2) < btScalar(1.0)) + { + if (btGetMatrixElem(mat,2) > btScalar(-1.0)) + { + xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8)); + xyz[1] = btAsin(btGetMatrixElem(mat,2)); + xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); + return true; + } + else + { + // WARNING. Not unique. XA - ZA = -atan2(r10,r11) + xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + xyz[1] = -SIMD_HALF_PI; + xyz[2] = btScalar(0.0); + return false; + } + } + else + { + // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) + xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + xyz[1] = SIMD_HALF_PI; + xyz[2] = 0.0; + + } + + + return false; +} + + + +//////////////////////////// btRotationalLimitMotor //////////////////////////////////// + + +int btRotationalLimitMotor::testLimitValue(btScalar test_value) +{ + if(m_loLimit>m_hiLimit) + { + m_currentLimit = 0;//Free from violation + return 0; + } + + if (test_value < m_loLimit) { - m_lowerLimit[i] = btScalar(0.0); - m_upperLimit[i] = btScalar(0.0); - m_accumulatedImpulse[i] = btScalar(0.0); + m_currentLimit = 1;//low limit violation + m_currentLimitError = test_value - m_loLimit; + return 1; } + else if (test_value> m_hiLimit) + { + m_currentLimit = 2;//High limit violation + m_currentLimitError = test_value - m_hiLimit; + return 2; + }; + m_currentLimit = 0;//Free from violation + return 0; + } -void btGeneric6DofConstraint::buildJacobian() +btScalar btRotationalLimitMotor::solveAngularLimits( + btScalar timeStep,btVector3& axis,btScalar jacDiagABInv, + btRigidBody * body0, btRigidBody * body1) { - btVector3 localNormalInA(0,0,0); + if (needApplyTorques()==false) return 0.0f; - const btVector3& pivotInA = m_frameInA.getOrigin(); - const btVector3& pivotInB = m_frameInB.getOrigin(); + btScalar target_velocity = m_targetVelocity; + btScalar maxMotorForce = m_maxMotorForce; - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin(); + //current error correction + if (m_currentLimit!=0) + { + target_velocity = -m_ERP*m_currentLimitError/(timeStep); + maxMotorForce = m_maxLimitForce; + } - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + maxMotorForce *= timeStep; - int i; - //linear part - for (i=0;i<3;i++) - { - if (isLimited(i)) - { - localNormalInA[i] = 1; - btVector3 normalWorld = m_rbA.getCenterOfMassTransform().getBasis() * localNormalInA; - - - // Create linear atom - new (&m_jacLinear[i]) btJacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getCenterOfMassTransform()*pivotInA - m_rbA.getCenterOfMassPosition(), - m_rbB.getCenterOfMassTransform()*pivotInB - m_rbB.getCenterOfMassPosition(), - normalWorld, - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); - - //optionally disable warmstarting -#ifdef GENERIC_D6_DISABLE_WARMSTARTING - m_accumulatedImpulse[i] = btScalar(0.); -#endif //GENERIC_D6_DISABLE_WARMSTARTING - - // Apply accumulated impulse - btVector3 impulse_vector = m_accumulatedImpulse[i] * normalWorld; - - m_rbA.applyImpulse( impulse_vector, rel_pos1); - m_rbB.applyImpulse(-impulse_vector, rel_pos2); - - localNormalInA[i] = 0; - } - } + // current velocity difference + btVector3 vel_diff = body0->getAngularVelocity(); + if (body1) + { + vel_diff -= body1->getAngularVelocity(); + } - // angular part - for (i=0;i<3;i++) - { - if (isLimited(i+3)) - { - btVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] ); - btVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] ); - // Dirk: This is IMO mathematically the correct way, but we should consider axisA and axisB being near parallel maybe - btVector3 axis = kSign[i] * axisA.cross(axisB); - // Create angular atom - new (&m_jacAng[i]) btJacobianEntry(axis, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); + btScalar rel_vel = axis.dot(vel_diff); -#ifdef GENERIC_D6_DISABLE_WARMSTARTING - m_accumulatedImpulse[i + 3] = btScalar(0.); -#endif //GENERIC_D6_DISABLE_WARMSTARTING + // correction velocity + btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel); - // Apply accumulated impulse - btVector3 impulse_vector = m_accumulatedImpulse[i + 3] * axis; - m_rbA.applyTorqueImpulse( impulse_vector); - m_rbB.applyTorqueImpulse(-impulse_vector); - } - } -} + if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON ) + { + return 0.0f;//no need for applying force + } + + + // correction impulse + btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv; + + // clip correction impulse + btScalar clippedMotorImpulse; + + //todo: should clip against accumulated impulse + if (unclippedMotorImpulse>0.0f) + { + clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse; + } + else + { + clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse; + } + + + // sort with accumulated impulses + btScalar lo = btScalar(-1e30); + btScalar hi = btScalar(1e30); + + btScalar oldaccumImpulse = m_accumulatedImpulse; + btScalar sum = oldaccumImpulse + clippedMotorImpulse; + m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; + + clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; + + + + btVector3 motorImp = clippedMotorImpulse * axis; + + + body0->applyTorqueImpulse(motorImp); + if (body1) body1->applyTorqueImpulse(-motorImp); + + return clippedMotorImpulse; + -btScalar getMatrixElem(const btMatrix3x3& mat,int index) -{ - int row = index%3; - int col = index / 3; - return mat[row][col]; } -///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html -bool MatrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) +//////////////////////////// End btRotationalLimitMotor //////////////////////////////////// + +//////////////////////////// btTranslationalLimitMotor //////////////////////////////////// +btScalar btTranslationalLimitMotor::solveLinearAxis( + btScalar timeStep, + btScalar jacDiagABInv, + btRigidBody& body1,const btVector3 &pointInA, + btRigidBody& body2,const btVector3 &pointInB, + int limit_index, + const btVector3 & axis_normal_on_a, + const btVector3 & anchorPos) { - // rot = cy*cz -cy*sz sy - // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx - // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy -/// 0..8 +///find relative velocity +// btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition(); +// btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition(); + btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition(); + btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition(); + + btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); + btVector3 vel = vel1 - vel2; + + btScalar rel_vel = axis_normal_on_a.dot(vel); + + + +/// apply displacement correction - if (getMatrixElem(mat,2) < btScalar(1.0)) +//positional error (zeroth order error) + btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a); + btScalar lo = btScalar(-1e30); + btScalar hi = btScalar(1e30); + + btScalar minLimit = m_lowerLimit[limit_index]; + btScalar maxLimit = m_upperLimit[limit_index]; + + //handle the limits + if (minLimit < maxLimit) { - if (getMatrixElem(mat,2) > btScalar(-1.0)) { - xyz[0] = btAtan2(-getMatrixElem(mat,5),getMatrixElem(mat,8)); - xyz[1] = btAsin(getMatrixElem(mat,2)); - xyz[2] = btAtan2(-getMatrixElem(mat,1),getMatrixElem(mat,0)); - return true; + if (depth > maxLimit) + { + depth -= maxLimit; + lo = btScalar(0.); + + } + else + { + if (depth < minLimit) + { + depth -= minLimit; + hi = btScalar(0.); + } + else + { + return 0.0f; + } + } } - else - { - // WARNING. Not unique. XA - ZA = -atan2(r10,r11) - xyz[0] = -btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4)); - xyz[1] = -SIMD_HALF_PI; - xyz[2] = btScalar(0.0); - return false; - } - } - else - { - // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) - xyz[0] = btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4)); - xyz[1] = SIMD_HALF_PI; - xyz[2] = 0.0; - } - - return false; + + btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv; + + + + + btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index]; + btScalar sum = oldNormalImpulse + normalImpulse; + m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; + normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; + + btVector3 impulse_vector = axis_normal_on_a * normalImpulse; + body1.applyImpulse( impulse_vector, rel_pos1); + body2.applyImpulse(-impulse_vector, rel_pos2); + return normalImpulse; +} + +//////////////////////////// btTranslationalLimitMotor //////////////////////////////////// + + +btGeneric6DofConstraint::btGeneric6DofConstraint() + :btTypedConstraint(D6_CONSTRAINT_TYPE), + m_useLinearReferenceFrameA(true) +{ +} + +btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) + : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB) + , m_frameInA(frameInA) + , m_frameInB(frameInB), + m_useLinearReferenceFrameA(useLinearReferenceFrameA) +{ + } -void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) + + + +void btGeneric6DofConstraint::calculateAngleInfo() { - btScalar tau = btScalar(0.1); - btScalar damping = btScalar(1.0); + btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis(); + + matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff); + + + + // in euler angle mode we do not actually constrain the angular velocity + // along the axes axis[0] and axis[2] (although we do use axis[1]) : + // + // to get constrain w2-w1 along ...not + // ------ --------------------- ------ + // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] + // d(angle[1])/dt = 0 ax[1] + // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] + // + // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. + // to prove the result for angle[0], write the expression for angle[0] from + // GetInfo1 then take the derivative. to prove this for angle[2] it is + // easier to take the euler rate expression for d(angle[2])/dt with respect + // to the components of w and set that to 0. + + btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0); + btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2); + + m_calculatedAxis[1] = axis2.cross(axis0); + m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2); + m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]); + + +// if(m_debugDrawer) +// { +// +// char buff[300]; +// sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ", +// m_calculatedAxisAngleDiff[0], +// m_calculatedAxisAngleDiff[1], +// m_calculatedAxisAngleDiff[2]); +// m_debugDrawer->reportErrorWarning(buff); +// } - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin(); +} - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - - btVector3 localNormalInA(0,0,0); - int i; +void btGeneric6DofConstraint::calculateTransforms() +{ + m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA; + m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB; - // linear - for (i=0;i<3;i++) - { - if (isLimited(i)) - { - btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); - btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); - - localNormalInA.setValue(0,0,0); - localNormalInA[i] = 1; - btVector3 normalWorld = m_rbA.getCenterOfMassTransform().getBasis() * localNormalInA; - - btScalar jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal(); - - //velocity error (first order error) - btScalar rel_vel = m_jacLinear[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, - m_rbB.getLinearVelocity(),angvelB); - - //positional error (zeroth order error) - btScalar depth = -(pivotAInW - pivotBInW).dot(normalWorld); - btScalar lo = btScalar(-1e30); - btScalar hi = btScalar(1e30); - - //handle the limits - if (m_lowerLimit[i] < m_upperLimit[i]) - { - { - if (depth > m_upperLimit[i]) - { - depth -= m_upperLimit[i]; - lo = btScalar(0.); - - } else - { - if (depth < m_lowerLimit[i]) - { - depth -= m_lowerLimit[i]; - hi = btScalar(0.); - } else - { - continue; - } - } - } - } + calculateAngleInfo(); +} - btScalar normalImpulse= (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv; - 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; - m_rbA.applyImpulse( impulse_vector, rel_pos1); - m_rbB.applyImpulse(-impulse_vector, rel_pos2); - - localNormalInA[i] = 0; - } - } - btVector3 axis; - btScalar angle; - btTransform frameAWorld = m_rbA.getCenterOfMassTransform() * m_frameInA; - btTransform frameBWorld = m_rbB.getCenterOfMassTransform() * m_frameInB; +void btGeneric6DofConstraint::buildLinearJacobian( + btJacobianEntry & jacLinear,const btVector3 & normalWorld, + const btVector3 & pivotAInW,const btVector3 & pivotBInW) +{ + new (&jacLinear) btJacobianEntry( + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + pivotAInW - m_rbA.getCenterOfMassPosition(), + pivotBInW - m_rbB.getCenterOfMassPosition(), + normalWorld, + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); - btTransformUtil::calculateDiffAxisAngle(frameAWorld,frameBWorld,axis,angle); - btQuaternion diff(axis,angle); - btMatrix3x3 diffMat (diff); - btVector3 xyz; - ///this is not perfect, we can first check which axis are limited, and choose a more appropriate order - MatrixToEulerXYZ(diffMat,xyz); +} + +void btGeneric6DofConstraint::buildAngularJacobian( + btJacobianEntry & jacAngular,const btVector3 & jointAxisW) +{ + new (&jacAngular) btJacobianEntry(jointAxisW, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); - // angular - for (i=0;i<3;i++) - { - if (isLimited(i+3)) - { - btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); - btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); - - btScalar jacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal(); - - //velocity error (first order error) - btScalar rel_vel = m_jacAng[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, - m_rbB.getLinearVelocity(),angvelB); - - //positional error (zeroth order error) - btVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] ); - btVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] ); - - btScalar rel_pos = kSign[i] * axisA.dot(axisB); - - 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_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); - - btScalar projAngle = btScalar(-1.)*xyz[i]; - - if (projAngle < loLimit) - { - hi = btScalar(0.); - rel_pos = (loLimit - projAngle); - } else - { - if (projAngle > hiLimit) - { - lo = btScalar(0.); - rel_pos = (hiLimit - projAngle); - } else - { - continue; - } - } - } - - //impulse - - btScalar normalImpulse= -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv; - 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) - btVector3 axis = kSign[i] * axisA.cross(axisB); - btVector3 impulse_vector = axis * normalImpulse; - - m_rbA.applyTorqueImpulse( impulse_vector); - m_rbB.applyTorqueImpulse(-impulse_vector); - } - } } -void btGeneric6DofConstraint::updateRHS(btScalar timeStep) +bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index) { - (void)timeStep; + btScalar angle = m_calculatedAxisAngleDiff[axis_index]; + //test limits + m_angularLimits[axis_index].testLimitValue(angle); + return m_angularLimits[axis_index].needApplyTorques(); } -btScalar btGeneric6DofConstraint::computeAngle(int axis) const - { - btScalar angle = btScalar(0.f); +void btGeneric6DofConstraint::buildJacobian() +{ - switch (axis) - { - case 0: - { - btVector3 v1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(1); - btVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1); - btVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2); + // Clear accumulated impulses for the next simulation step + m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); + int i; + for(i = 0; i < 3; i++) + { + m_angularLimits[i].m_accumulatedImpulse = btScalar(0.); + } + //calculates transform + calculateTransforms(); + +// const btVector3& pivotAInW = m_calculatedTransformA.getOrigin(); +// const btVector3& pivotBInW = m_calculatedTransformB.getOrigin(); + calcAnchorPos(); + btVector3 pivotAInW = m_AnchorPos; + btVector3 pivotBInW = m_AnchorPos; + +// not used here +// btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); +// btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + + btVector3 normalWorld; + //linear part + for (i=0;i<3;i++) + { + if (m_linearLimits.isLimited(i)) + { + if (m_useLinearReferenceFrameA) + normalWorld = m_calculatedTransformA.getBasis().getColumn(i); + else + normalWorld = m_calculatedTransformB.getBasis().getColumn(i); - btScalar s = v1.dot(w2); - btScalar c = v1.dot(v2); + buildLinearJacobian( + m_jacLinear[i],normalWorld , + pivotAInW,pivotBInW); - angle = btAtan2( s, c ); - } - break; + } + } - case 1: - { - btVector3 w1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(2); - btVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2); - btVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0); + // angular part + for (i=0;i<3;i++) + { + //calculates error angle + if (testAngularLimitMotor(i)) + { + normalWorld = this->getAxis(i); + // Create angular atom + buildAngularJacobian(m_jacAng[i],normalWorld); + } + } - btScalar s = w1.dot(u2); - btScalar c = w1.dot(w2); - angle = btAtan2( s, c ); - } - break; +} - case 2: - { - btVector3 u1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(0); - btVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0); - btVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1); - btScalar s = u1.dot(v2); - btScalar c = u1.dot(u2); +void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) +{ + m_timeStep = timeStep; - angle = btAtan2( s, c ); - } - break; - default: - btAssert ( 0 ) ; - - break ; - } + //calculateTransforms(); + + int i; + + // linear + + btVector3 pointInA = m_calculatedTransformA.getOrigin(); + btVector3 pointInB = m_calculatedTransformB.getOrigin(); + + btScalar jacDiagABInv; + btVector3 linear_axis; + for (i=0;i<3;i++) + { + if (m_linearLimits.isLimited(i)) + { + jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal(); + + if (m_useLinearReferenceFrameA) + linear_axis = m_calculatedTransformA.getBasis().getColumn(i); + else + linear_axis = m_calculatedTransformB.getBasis().getColumn(i); + + m_linearLimits.solveLinearAxis( + m_timeStep, + jacDiagABInv, + m_rbA,pointInA, + m_rbB,pointInB, + i,linear_axis, m_AnchorPos); + + } + } + + // angular + btVector3 angular_axis; + btScalar angularJacDiagABInv; + for (i=0;i<3;i++) + { + if (m_angularLimits[i].needApplyTorques()) + { + + // get axis + angular_axis = getAxis(i); + + angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal(); + + m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,&m_rbB); + } + } +} + +void btGeneric6DofConstraint::updateRHS(btScalar timeStep) +{ + (void)timeStep; + +} + +btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const +{ + return m_calculatedAxis[axis_index]; +} - return angle; +btScalar btGeneric6DofConstraint::getAngle(int axis_index) const +{ + return m_calculatedAxisAngleDiff[axis_index]; +} + +void btGeneric6DofConstraint::calcAnchorPos(void) +{ + btScalar imA = m_rbA.getInvMass(); + btScalar imB = m_rbB.getInvMass(); + btScalar weight; + if(imB == btScalar(0.0)) + { + weight = btScalar(1.0); + } + else + { + weight = imA / (imA + imB); } + const btVector3& pA = m_calculatedTransformA.getOrigin(); + const btVector3& pB = m_calculatedTransformB.getOrigin(); + m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight); + return; +} // btGeneric6DofConstraint::calcAnchorPos() diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index b114e54fa69..f0718d2d4a0 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -4,116 +4,433 @@ 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, +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. */ +/* +2007-09-09 +btGeneric6DofConstraint Refactored by Francisco Le?n +email: projectileman@yahoo.com +http://gimpact.sf.net +*/ + #ifndef GENERIC_6DOF_CONSTRAINT_H #define GENERIC_6DOF_CONSTRAINT_H -#include "../../LinearMath/btVector3.h" +#include "LinearMath/btVector3.h" #include "btJacobianEntry.h" #include "btTypedConstraint.h" class btRigidBody; +//! Rotation Limit structure for generic joints +class btRotationalLimitMotor +{ +public: + //! limit_parameters + //!@{ + btScalar m_loLimit;//!< joint limit + btScalar m_hiLimit;//!< joint limit + btScalar m_targetVelocity;//!< target motor velocity + btScalar m_maxMotorForce;//!< max force on motor + btScalar m_maxLimitForce;//!< max force on limit + btScalar m_damping;//!< Damping. + btScalar m_limitSoftness;//! Relaxation factor + btScalar m_ERP;//!< Error tolerance factor when joint is at limit + btScalar m_bounce;//!< restitution factor + bool m_enableMotor; + + //!@} + + //! temp_variables + //!@{ + btScalar m_currentLimitError;//! How much is violated this limit + int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit + btScalar m_accumulatedImpulse; + //!@} + + btRotationalLimitMotor() + { + m_accumulatedImpulse = 0.f; + m_targetVelocity = 0; + m_maxMotorForce = 0.1f; + m_maxLimitForce = 300.0f; + m_loLimit = -SIMD_INFINITY; + m_hiLimit = SIMD_INFINITY; + m_ERP = 0.5f; + m_bounce = 0.0f; + m_damping = 1.0f; + m_limitSoftness = 0.5f; + m_currentLimit = 0; + m_currentLimitError = 0; + m_enableMotor = false; + } + + btRotationalLimitMotor(const btRotationalLimitMotor & limot) + { + m_targetVelocity = limot.m_targetVelocity; + m_maxMotorForce = limot.m_maxMotorForce; + m_limitSoftness = limot.m_limitSoftness; + m_loLimit = limot.m_loLimit; + m_hiLimit = limot.m_hiLimit; + m_ERP = limot.m_ERP; + m_bounce = limot.m_bounce; + m_currentLimit = limot.m_currentLimit; + m_currentLimitError = limot.m_currentLimitError; + m_enableMotor = limot.m_enableMotor; + } + + + + //! Is limited + bool isLimited() + { + if(m_loLimit>=m_hiLimit) return false; + return true; + } + + //! Need apply correction + bool needApplyTorques() + { + if(m_currentLimit == 0 && m_enableMotor == false) return false; + return true; + } + + //! calculates error + /*! + calculates m_currentLimit and m_currentLimitError. + */ + int testLimitValue(btScalar test_value); + + //! apply the correction impulses for two bodies + btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1); + + +}; + + + +class btTranslationalLimitMotor +{ +public: + btVector3 m_lowerLimit;//!< the constraint lower limits + btVector3 m_upperLimit;//!< the constraint upper limits + btVector3 m_accumulatedImpulse; + //! Linear_Limit_parameters + //!@{ + btScalar m_limitSoftness;//!< Softness for linear limit + btScalar m_damping;//!< Damping for linear limit + btScalar m_restitution;//! Bounce parameter for linear limit + //!@} + + btTranslationalLimitMotor() + { + m_lowerLimit.setValue(0.f,0.f,0.f); + m_upperLimit.setValue(0.f,0.f,0.f); + m_accumulatedImpulse.setValue(0.f,0.f,0.f); + + m_limitSoftness = 0.7f; + m_damping = btScalar(1.0f); + m_restitution = btScalar(0.5f); + } + + btTranslationalLimitMotor(const btTranslationalLimitMotor & other ) + { + m_lowerLimit = other.m_lowerLimit; + m_upperLimit = other.m_upperLimit; + m_accumulatedImpulse = other.m_accumulatedImpulse; + + m_limitSoftness = other.m_limitSoftness ; + m_damping = other.m_damping; + m_restitution = other.m_restitution; + } + + //! Test limit + /*! + - free means upper < lower, + - locked means upper == lower + - limited means upper > lower + - limitIndex: first 3 are linear, next 3 are angular + */ + inline bool isLimited(int limitIndex) + { + return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); + } + + + btScalar solveLinearAxis( + btScalar timeStep, + btScalar jacDiagABInv, + btRigidBody& body1,const btVector3 &pointInA, + btRigidBody& body2,const btVector3 &pointInB, + int limit_index, + const btVector3 & axis_normal_on_a, + const btVector3 & anchorPos); + + +}; /// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space -/// btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked' -/// Work in progress (is still a Hinge actually) +/*! +btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'. +currently this limit supports rotational motors<br> +<ul> +<li> For Linear limits, use btGeneric6DofConstraint.setLinearUpperLimit, btGeneric6DofConstraint.setLinearLowerLimit. You can set the parameters with the btTranslationalLimitMotor structure accsesible through the btGeneric6DofConstraint.getTranslationalLimitMotor method. +At this moment translational motors are not supported. May be in the future. </li> + +<li> For Angular limits, use the btRotationalLimitMotor structure for configuring the limit. +This is accessible through btGeneric6DofConstraint.getLimitMotor method, +This brings support for limit parameters and motors. </li> + +<li> Angulars limits have these possible ranges: +<table border=1 > +<tr + + <td><b>AXIS</b></td> + <td><b>MIN ANGLE</b></td> + <td><b>MAX ANGLE</b></td> + <td>X</td> + <td>-PI</td> + <td>PI</td> + <td>Y</td> + <td>-PI/2</td> + <td>PI/2</td> + <td>Z</td> + <td>-PI/2</td> + <td>PI/2</td> +</tr> +</table> +</li> +</ul> + +*/ class btGeneric6DofConstraint : public btTypedConstraint { - btJacobianEntry m_jacLinear[3]; // 3 orthogonal linear constraints - btJacobianEntry m_jacAng[3]; // 3 orthogonal angular constraints +protected: + + //! relative_frames + //!@{ + btTransform m_frameInA;//!< the constraint space w.r.t body A + btTransform m_frameInB;//!< the constraint space w.r.t body B + //!@} + + //! Jacobians + //!@{ + btJacobianEntry m_jacLinear[3];//!< 3 orthogonal linear constraints + btJacobianEntry m_jacAng[3];//!< 3 orthogonal angular constraints + //!@} - btTransform m_frameInA; // the constraint space w.r.t body A - btTransform m_frameInB; // the constraint space w.r.t body B + //! Linear_Limit_parameters + //!@{ + btTranslationalLimitMotor m_linearLimits; + //!@} + + + //! hinge_parameters + //!@{ + btRotationalLimitMotor m_angularLimits[3]; + //!@} + + +protected: + //! temporal variables + //!@{ + btScalar m_timeStep; + btTransform m_calculatedTransformA; + btTransform m_calculatedTransformB; + btVector3 m_calculatedAxisAngleDiff; + btVector3 m_calculatedAxis[3]; + + btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes + + bool m_useLinearReferenceFrameA; + + //!@} + + btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other) + { + btAssert(0); + (void) other; + return *this; + } + + + + void buildLinearJacobian( + btJacobianEntry & jacLinear,const btVector3 & normalWorld, + const btVector3 & pivotAInW,const btVector3 & pivotBInW); + + void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW); + + + //! calcs the euler angles between the two bodies. + void calculateAngleInfo(); - btScalar m_lowerLimit[6]; // the constraint lower limits - btScalar m_upperLimit[6]; // the constraint upper limits - 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); - - void updateRHS(btScalar timeStep); - - btScalar computeAngle(int axis) const; - - void setLinearLowerLimit(const btVector3& linearLower) - { - m_lowerLimit[0] = linearLower.getX(); - m_lowerLimit[1] = linearLower.getY(); - m_lowerLimit[2] = linearLower.getZ(); - } - - void setLinearUpperLimit(const btVector3& linearUpper) - { - m_upperLimit[0] = linearUpper.getX(); - m_upperLimit[1] = linearUpper.getY(); - m_upperLimit[2] = linearUpper.getZ(); - } - - void setAngularLowerLimit(const btVector3& angularLower) - { - m_lowerLimit[3] = angularLower.getX(); - m_lowerLimit[4] = angularLower.getY(); - m_lowerLimit[5] = angularLower.getZ(); - } - - void setAngularUpperLimit(const btVector3& angularUpper) - { - m_upperLimit[3] = angularUpper.getX(); - m_upperLimit[4] = angularUpper.getY(); - m_upperLimit[5] = angularUpper.getZ(); - } - - //first 3 are linear, next 3 are angular - void SetLimit(int axis, btScalar lo, btScalar hi) - { - m_lowerLimit[axis] = lo; - m_upperLimit[axis] = hi; - } - - //free means upper < lower, - //locked means upper == lower - //limited means upper > lower - //limitIndex: first 3 are linear, next 3 are angular - bool isLimited(int limitIndex) - { - return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); - } - - const btRigidBody& getRigidBodyA() const - { - return m_rbA; - } - const btRigidBody& getRigidBodyB() const - { - return m_rbB; - } - + btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); + + btGeneric6DofConstraint(); + + //! Calcs global transform of the offsets + /*! + Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies. + \sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo + */ + void calculateTransforms(); + + //! Gets the global transform of the offset for body A + /*! + \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo. + */ + const btTransform & getCalculatedTransformA() const + { + return m_calculatedTransformA; + } + + //! Gets the global transform of the offset for body B + /*! + \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo. + */ + const btTransform & getCalculatedTransformB() const + { + return m_calculatedTransformB; + } + + const btTransform & getFrameOffsetA() const + { + return m_frameInA; + } + + const btTransform & getFrameOffsetB() const + { + return m_frameInB; + } + + + btTransform & getFrameOffsetA() + { + return m_frameInA; + } + + btTransform & getFrameOffsetB() + { + return m_frameInB; + } + + + //! performs Jacobian calculation, and also calculates angle differences and axis + virtual void buildJacobian(); + + virtual void solveConstraint(btScalar timeStep); + + void updateRHS(btScalar timeStep); + + //! Get the rotation axis in global coordinates + /*! + \pre btGeneric6DofConstraint.buildJacobian must be called previously. + */ + btVector3 getAxis(int axis_index) const; + + //! Get the relative Euler angle + /*! + \pre btGeneric6DofConstraint.buildJacobian must be called previously. + */ + btScalar getAngle(int axis_index) const; + + //! Test angular limit. + /*! + Calculates angular correction and returns true if limit needs to be corrected. + \pre btGeneric6DofConstraint.buildJacobian must be called previously. + */ + bool testAngularLimitMotor(int axis_index); + + void setLinearLowerLimit(const btVector3& linearLower) + { + m_linearLimits.m_lowerLimit = linearLower; + } + + void setLinearUpperLimit(const btVector3& linearUpper) + { + m_linearLimits.m_upperLimit = linearUpper; + } + + void setAngularLowerLimit(const btVector3& angularLower) + { + m_angularLimits[0].m_loLimit = angularLower.getX(); + m_angularLimits[1].m_loLimit = angularLower.getY(); + m_angularLimits[2].m_loLimit = angularLower.getZ(); + } + + void setAngularUpperLimit(const btVector3& angularUpper) + { + m_angularLimits[0].m_hiLimit = angularUpper.getX(); + m_angularLimits[1].m_hiLimit = angularUpper.getY(); + m_angularLimits[2].m_hiLimit = angularUpper.getZ(); + } + + //! Retrieves the angular limit informacion + btRotationalLimitMotor * getRotationalLimitMotor(int index) + { + return &m_angularLimits[index]; + } + + //! Retrieves the limit informacion + btTranslationalLimitMotor * getTranslationalLimitMotor() + { + return &m_linearLimits; + } + + //first 3 are linear, next 3 are angular + void setLimit(int axis, btScalar lo, btScalar hi) + { + if(axis<3) + { + m_linearLimits.m_lowerLimit[axis] = lo; + m_linearLimits.m_upperLimit[axis] = hi; + } + else + { + m_angularLimits[axis-3].m_loLimit = lo; + m_angularLimits[axis-3].m_hiLimit = hi; + } + } + + //! Test limit + /*! + - free means upper < lower, + - locked means upper == lower + - limited means upper > lower + - limitIndex: first 3 are linear, next 3 are angular + */ + bool isLimited(int limitIndex) + { + if(limitIndex<3) + { + return m_linearLimits.isLimited(limitIndex); + + } + return m_angularLimits[limitIndex-3].isLimited(); + } + + const btRigidBody& getRigidBodyA() const + { + return m_rbA; + } + const btRigidBody& getRigidBodyB() const + { + return m_rbB; + } + + virtual void calcAnchorPos(void); // overridable }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index 27e30987549..114abce24c7 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -17,58 +17,185 @@ subject to the following restrictions: #include "btHingeConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" +#include "LinearMath/btMinMax.h" #include <new> -btHingeConstraint::btHingeConstraint(): + +btHingeConstraint::btHingeConstraint() +: btTypedConstraint (HINGE_CONSTRAINT_TYPE), m_enableAngularMotor(false) { } btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, - btVector3& axisInA,btVector3& axisInB) -:btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB), -m_axisInA(axisInA), -m_axisInB(-axisInB), -m_angularOnly(false), -m_enableAngularMotor(false) + btVector3& axisInA,btVector3& axisInB) + :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), + m_angularOnly(false), + m_enableAngularMotor(false) { + m_rbAFrame.getOrigin() = pivotInA; + + // since no frame is given, assume this to be zero angle and just pick rb transform axis + btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); + + btVector3 rbAxisA2; + btScalar projection = axisInA.dot(rbAxisA1); + if (projection >= 1.0f - SIMD_EPSILON) { + rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2); + rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); + } else if (projection <= -1.0f + SIMD_EPSILON) { + rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2); + rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); + } else { + rbAxisA2 = axisInA.cross(rbAxisA1); + rbAxisA1 = rbAxisA2.cross(axisInA); + } + + m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), + rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), + rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); + + btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); + btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); + btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); + + m_rbBFrame.getOrigin() = pivotInB; + m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(), + rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(), + rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() ); + + //start with free + m_lowerLimit = btScalar(1e30); + m_upperLimit = btScalar(-1e30); + m_biasFactor = 0.3f; + m_relaxationFactor = 1.0f; + m_limitSoftness = 0.9f; + m_solveLimit = false; } btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA) -:btTypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), -m_axisInA(axisInA), -//fixed axis in worldspace -m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA), +:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false) +{ + + // since no frame is given, assume this to be zero angle and just pick rb transform axis + // fixed axis in worldspace + btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); + btScalar projection = rbAxisA1.dot(axisInA); + if (projection > SIMD_EPSILON) + rbAxisA1 = rbAxisA1*projection - axisInA; + else + rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); + + btVector3 rbAxisA2 = axisInA.cross(rbAxisA1); + + m_rbAFrame.getOrigin() = pivotInA; + m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), + rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), + rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); + + + btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA; + + btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); + btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); + btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); + + + m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA); + m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), + rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), + rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); + + //start with free + m_lowerLimit = btScalar(1e30); + m_upperLimit = btScalar(-1e30); + m_biasFactor = 0.3f; + m_relaxationFactor = 1.0f; + m_limitSoftness = 0.9f; + m_solveLimit = false; +} + +btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, + const btTransform& rbAFrame, const btTransform& rbBFrame) +:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), m_angularOnly(false), m_enableAngularMotor(false) { - + // flip axis + m_rbBFrame.getBasis()[0][2] *= btScalar(-1.); + m_rbBFrame.getBasis()[1][2] *= btScalar(-1.); + m_rbBFrame.getBasis()[2][2] *= btScalar(-1.); + + //start with free + m_lowerLimit = btScalar(1e30); + m_upperLimit = btScalar(-1e30); + m_biasFactor = 0.3f; + m_relaxationFactor = 1.0f; + m_limitSoftness = 0.9f; + m_solveLimit = false; +} + + + +btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame) +:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame), +m_angularOnly(false), +m_enableAngularMotor(false) +{ + ///not providing rigidbody B means implicitly using worldspace for body B + + // flip axis + m_rbBFrame.getBasis()[0][2] *= btScalar(-1.); + m_rbBFrame.getBasis()[1][2] *= btScalar(-1.); + m_rbBFrame.getBasis()[2][2] *= btScalar(-1.); + + m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin()); + + //start with free + m_lowerLimit = btScalar(1e30); + m_upperLimit = btScalar(-1e30); + m_biasFactor = 0.3f; + m_relaxationFactor = 1.0f; + m_limitSoftness = 0.9f; + m_solveLimit = false; } void btHingeConstraint::buildJacobian() { m_appliedImpulse = btScalar(0.); - btVector3 normal(0,0,0); - if (!m_angularOnly) { + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); + btVector3 relPos = pivotBInW - pivotAInW; + + btVector3 normal[3]; + if (relPos.length2() > SIMD_EPSILON) + { + normal[0] = relPos.normalized(); + } + else + { + normal[0].setValue(btScalar(1.0),0,0); + } + + btPlaneSpace1(normal[0], normal[1], normal[2]); + for (int i=0;i<3;i++) { - normal[i] = 1; new (&m_jac[i]) btJacobianEntry( m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(), - m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(), - normal, + pivotAInW - m_rbA.getCenterOfMassPosition(), + pivotBInW - m_rbB.getCenterOfMassPosition(), + normal[i], m_rbA.getInvInertiaDiagLocal(), m_rbA.getInvMass(), m_rbB.getInvInertiaDiagLocal(), m_rbB.getInvMass()); - normal[i] = 0; } } @@ -79,12 +206,12 @@ void btHingeConstraint::buildJacobian() btVector3 jointAxis0local; btVector3 jointAxis1local; - btPlaneSpace1(m_axisInA,jointAxis0local,jointAxis1local); + btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); - getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; + getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; - btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; + btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); new (&m_jacAng[0]) btJacobianEntry(jointAxis0, m_rbA.getCenterOfMassTransform().getBasis().transpose(), @@ -105,44 +232,70 @@ void btHingeConstraint::buildJacobian() m_rbB.getInvInertiaDiagLocal()); + // Compute limit information + btScalar hingeAngle = getHingeAngle(); + + //set bias, sign, clear accumulator + m_correction = btScalar(0.); + m_limitSign = btScalar(0.); + m_solveLimit = false; + m_accLimitImpulse = btScalar(0.); + + if (m_lowerLimit < m_upperLimit) + { + if (hingeAngle <= m_lowerLimit*m_limitSoftness) + { + m_correction = (m_lowerLimit - hingeAngle); + m_limitSign = 1.0f; + m_solveLimit = true; + } + else if (hingeAngle >= m_upperLimit*m_limitSoftness) + { + m_correction = m_upperLimit - hingeAngle; + m_limitSign = -1.0f; + m_solveLimit = true; + } + } + + //Compute K = J*W*J' for hinge axis + btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + + getRigidBodyB().computeAngularImpulseDenominator(axisA)); } void btHingeConstraint::solveConstraint(btScalar timeStep) { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); - btVector3 normal(0,0,0); btScalar tau = btScalar(0.3); - btScalar damping = btScalar(1.); -//linear part + //linear part if (!m_angularOnly) { + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + + btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); + btVector3 vel = vel1 - vel2; + for (int i=0;i<3;i++) { - normal[i] = 1; + const btVector3& normal = m_jac[i].m_linearJointAxis; btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - - btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; btScalar rel_vel; rel_vel = normal.dot(vel); //positional error (zeroth order error) btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - btScalar impulse = depth*tau/timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping; + btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; m_appliedImpulse += impulse; btVector3 impulse_vector = normal * impulse; m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); - - normal[i] = 0; } } @@ -151,8 +304,8 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) ///solve angular part // get axes in world space - btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; - btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB; + btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2); const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); @@ -174,7 +327,7 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) getRigidBodyB().computeAngularImpulseDenominator(normal); // scale for mass and relaxation //todo: expose this 0.9 factor to developer - velrelOrthog *= (btScalar(1.)/denom) * btScalar(0.9); + velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor; } //solve angular positional correction @@ -190,10 +343,28 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) m_rbA.applyTorqueImpulse(-velrelOrthog+angularError); m_rbB.applyTorqueImpulse(velrelOrthog-angularError); + + // solve limit + if (m_solveLimit) + { + btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign; + + btScalar impulseMag = amplitude * m_kHinge; + + // Clamp the accumulated impulse + btScalar temp = m_accLimitImpulse; + m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) ); + impulseMag = m_accLimitImpulse - temp; + + + btVector3 impulse = axisA * impulseMag * m_limitSign; + m_rbA.applyTorqueImpulse(impulse); + m_rbB.applyTorqueImpulse(-impulse); + } } //apply motor - if (m_enableAngularMotor) + if (m_enableAngularMotor) { //todo: add limits too btVector3 angularLimit(0,0,0); @@ -204,10 +375,7 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) btScalar desiredMotorVel = m_motorTargetVelocity; btScalar motor_relvel = desiredMotorVel - projRelVel; - btScalar denom3 = getRigidBodyA().computeAngularImpulseDenominator(axisA) + - getRigidBodyB().computeAngularImpulseDenominator(axisA); - - btScalar unclippedMotorImpulse = (btScalar(1.)/denom3) * motor_relvel;; + btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;; //todo: should clip against accumulated impulse btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; @@ -227,3 +395,12 @@ void btHingeConstraint::updateRHS(btScalar timeStep) } +btScalar btHingeConstraint::getHingeAngle() +{ + const btVector3 refAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0); + const btVector3 refAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1); + const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1); + + return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1) ); +} + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 5c1ceafbc5b..4fa9972f6d8 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -13,39 +13,61 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ +/* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */ + #ifndef HINGECONSTRAINT_H #define HINGECONSTRAINT_H -#include "../../LinearMath/btVector3.h" +#include "LinearMath/btVector3.h" #include "btJacobianEntry.h" #include "btTypedConstraint.h" class btRigidBody; - /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space /// axis defines the orientation of the hinge axis class btHingeConstraint : public btTypedConstraint { +#ifdef IN_PARALLELL_SOLVER +public: +#endif btJacobianEntry m_jac[3]; //3 orthogonal linear constraints btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor - btVector3 m_pivotInA; - btVector3 m_pivotInB; - btVector3 m_axisInA; - btVector3 m_axisInB; + btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis. + btTransform m_rbBFrame; - bool m_angularOnly; + btScalar m_motorTargetVelocity; + btScalar m_maxMotorImpulse; + + btScalar m_limitSoftness; + btScalar m_biasFactor; + btScalar m_relaxationFactor; - btScalar m_motorTargetVelocity; - btScalar m_maxMotorImpulse; + btScalar m_lowerLimit; + btScalar m_upperLimit; + + btScalar m_kHinge; + + btScalar m_limitSign; + btScalar m_correction; + + btScalar m_accLimitImpulse; + + bool m_angularOnly; bool m_enableAngularMotor; + bool m_solveLimit; + public: - btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,btVector3& axisInA,btVector3& axisInB); + btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB); btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA); + + btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame); + + btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame); btHingeConstraint(); @@ -76,6 +98,61 @@ public: m_maxMotorImpulse = maxMotorImpulse; } + void setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) + { + m_lowerLimit = low; + m_upperLimit = high; + + m_limitSoftness = _softness; + m_biasFactor = _biasFactor; + m_relaxationFactor = _relaxationFactor; + + } + + btScalar getLowerLimit() const + { + return m_lowerLimit; + } + + btScalar getUpperLimit() const + { + return m_upperLimit; + } + + + btScalar getHingeAngle(); + + + const btTransform& getAFrame() { return m_rbAFrame; }; + const btTransform& getBFrame() { return m_rbBFrame; }; + + inline int getSolveLimit() + { + return m_solveLimit; + } + + inline btScalar getLimitSign() + { + return m_limitSign; + } + + inline bool getAngularOnly() + { + return m_angularOnly; + } + inline bool getEnableAngularMotor() + { + return m_enableAngularMotor; + } + inline btScalar getMotorTargetVelosity() + { + return m_motorTargetVelocity; + } + inline btScalar getMaxMotorImpulse() + { + return m_maxMotorImpulse; + } + }; #endif //HINGECONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h index aae3ed0373f..bfeb24c2dfb 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 "../Dynamics/btRigidBody.h" +#include "LinearMath/btVector3.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" //notes: diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp index aacb0a3ea66..2b69ad90438 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp @@ -21,18 +21,19 @@ subject to the following restrictions: btPoint2PointConstraint::btPoint2PointConstraint() +:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE) { } btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB) -:btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB) +:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB) { } btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA) -:btTypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)) +:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)) { } @@ -99,6 +100,16 @@ void btPoint2PointConstraint::solveConstraint(btScalar timeStep) btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal btScalar impulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv; + + btScalar impulseClamp = m_setting.m_impulseClamp; + if (impulseClamp > 0) + { + if (impulse < -impulseClamp) + impulse = -impulseClamp; + if (impulse > impulseClamp) + impulse = impulseClamp; + } + m_appliedImpulse+=impulse; btVector3 impulse_vector = normal * impulse; m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h index 71da8ac0347..c9d5968530c 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h @@ -16,7 +16,7 @@ subject to the following restrictions: #ifndef POINT2POINTCONSTRAINT_H #define POINT2POINTCONSTRAINT_H -#include "../../LinearMath/btVector3.h" +#include "LinearMath/btVector3.h" #include "btJacobianEntry.h" #include "btTypedConstraint.h" @@ -26,16 +26,21 @@ struct btConstraintSetting { btConstraintSetting() : m_tau(btScalar(0.3)), - m_damping(btScalar(1.)) + m_damping(btScalar(1.)), + m_impulseClamp(btScalar(0.)) { } btScalar m_tau; btScalar m_damping; + btScalar m_impulseClamp; }; /// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space class btPoint2PointConstraint : public btTypedConstraint { +#ifdef IN_PARALLELL_SOLVER +public: +#endif btJacobianEntry m_jac[3]; //3 orthogonal linear constraints btVector3 m_pivotInA; @@ -70,6 +75,15 @@ public: m_pivotInB = pivotB; } + const btVector3& getPivotInA() const + { + return m_pivotInA; + } + + const btVector3& getPivotInB() const + { + return m_pivotInB; + } }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 14b36ad44fd..b8afbd6aac5 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -13,6 +13,9 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ +//#define COMPUTE_IMPULSE_DENOM 1 +//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms. +//#define FORCE_REFESH_CONTACT_MANIFOLDS 1 #include "btSequentialImpulseConstraintSolver.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" @@ -30,11 +33,9 @@ subject to the following restrictions: #include "btSolverBody.h" #include "btSolverConstraint.h" + #include "LinearMath/btAlignedObjectArray.h" -#ifdef USE_PROFILE -#include "LinearMath/btQuickprof.h" -#endif //USE_PROFILE int totalCpd = 0; @@ -64,7 +65,7 @@ unsigned long btSequentialImpulseConstraintSolver::btRand2() int btSequentialImpulseConstraintSolver::btRandInt2 (int n) { // seems good; xor-fold and modulus - const unsigned long un = n; + const unsigned long un = static_cast<unsigned long>(n); unsigned long r = btRand2(); // note: probably more aggressive than it needs to be -- might be @@ -91,12 +92,12 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n) - +bool MyContactDestroyedCallback(void* userPersistentData); bool MyContactDestroyedCallback(void* userPersistentData) { assert (userPersistentData); btConstraintPersistentData* cpd = (btConstraintPersistentData*)userPersistentData; - delete cpd; + btAlignedFree(cpd); totalCpd--; //printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData); return true; @@ -105,8 +106,7 @@ bool MyContactDestroyedCallback(void* userPersistentData) btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() -:m_solverMode(SOLVER_RANDMIZE_ORDER | SOLVER_CACHE_FRIENDLY), //not using SOLVER_USE_WARMSTARTING, -m_btSeed2(0) +:m_btSeed2(0) { gContactDestroyedCallback = &MyContactDestroyedCallback; @@ -121,27 +121,42 @@ m_btSeed2(0) } } - -void initSolverBody(btSolverBody* solverBody, btRigidBody* rigidbody) +btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver() { -/* 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); +void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject); +void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject) +{ + btRigidBody* rb = btRigidBody::upcast(collisionObject); + if (rb) + { + solverBody->m_angularVelocity = rb->getAngularVelocity() ; + solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin(); + solverBody->m_friction = collisionObject->getFriction(); + solverBody->m_invMass = rb->getInvMass(); + solverBody->m_linearVelocity = rb->getLinearVelocity(); + solverBody->m_originalBody = rb; + solverBody->m_angularFactor = rb->getAngularFactor(); + } else + { + solverBody->m_angularVelocity.setValue(0,0,0); + solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin(); + solverBody->m_friction = collisionObject->getFriction(); + solverBody->m_invMass = 0.f; + solverBody->m_linearVelocity.setValue(0,0,0); + solverBody->m_originalBody = 0; + solverBody->m_angularFactor = 1.f; + } + solverBody->m_pushVelocity.setValue(0.f,0.f,0.f); + solverBody->m_turnVelocity.setValue(0.f,0.f,0.f); +} + + +int gNumSplitImpulseRecoveries = 0; + +btScalar restitutionCurve(btScalar rel_vel, btScalar restitution); btScalar restitutionCurve(btScalar rel_vel, btScalar restitution) { btScalar rest = restitution * -rel_vel; @@ -149,30 +164,95 @@ btScalar restitutionCurve(btScalar rel_vel, btScalar restitution) } +void resolveSplitPenetrationImpulseCacheFriendly( + btSolverBody& body1, + btSolverBody& body2, + const btSolverConstraint& contactConstraint, + const btContactSolverInfo& solverInfo); + +//SIMD_FORCE_INLINE +void resolveSplitPenetrationImpulseCacheFriendly( + btSolverBody& body1, + btSolverBody& body2, + const btSolverConstraint& contactConstraint, + const btContactSolverInfo& solverInfo) +{ + (void)solverInfo; + + if (contactConstraint.m_penetration < solverInfo.m_splitImpulsePenetrationThreshold) + { + gNumSplitImpulseRecoveries++; + btScalar normalImpulse; + // 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_pushVelocity) + + contactConstraint.m_relpos1CrossNormal.dot(body1.m_turnVelocity); + btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_pushVelocity) + + contactConstraint.m_relpos2CrossNormal.dot(body2.m_turnVelocity); + + rel_vel = vel1Dotn-vel2Dotn; + + + btScalar positionalError = -contactConstraint.m_penetration * solverInfo.m_erp2/solverInfo.m_timeStep; + // 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; + normalImpulse = penetrationImpulse+velocityImpulse; + + // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse + btScalar oldNormalImpulse = contactConstraint.m_appliedPushImpulse; + btScalar sum = oldNormalImpulse + normalImpulse; + contactConstraint.m_appliedPushImpulse = btScalar(0.) > sum ? btScalar(0.): sum; + + normalImpulse = contactConstraint.m_appliedPushImpulse - oldNormalImpulse; + + body1.internalApplyPushImpulse(contactConstraint.m_contactNormal*body1.m_invMass, contactConstraint.m_angularComponentA,normalImpulse); + + body2.internalApplyPushImpulse(contactConstraint.m_contactNormal*body2.m_invMass, contactConstraint.m_angularComponentB,-normalImpulse); + + } + +} //velocity + friction //response between two dynamic objects with friction -SIMD_FORCE_INLINE btScalar resolveSingleCollisionCombinedCacheFriendly( + +btScalar resolveSingleCollisionCombinedCacheFriendly( btSolverBody& body1, btSolverBody& body2, - btSolverConstraint& contactConstraint, + const btSolverConstraint& contactConstraint, + const btContactSolverInfo& solverInfo); + +//SIMD_FORCE_INLINE +btScalar resolveSingleCollisionCombinedCacheFriendly( + btSolverBody& body1, + btSolverBody& body2, + const btSolverConstraint& contactConstraint, const btContactSolverInfo& solverInfo) { (void)solverInfo; - btScalar normalImpulse(0.f); + btScalar normalImpulse; + { - 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); + + // 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) @@ -182,50 +262,51 @@ SIMD_FORCE_INLINE btScalar resolveSingleCollisionCombinedCacheFriendly( rel_vel = vel1Dotn-vel2Dotn; + btScalar positionalError = 0.f; + if (!solverInfo.m_splitImpulse || (contactConstraint.m_penetration > solverInfo.m_splitImpulsePenetrationThreshold)) + { + positionalError = -contactConstraint.m_penetration * solverInfo.m_erp/solverInfo.m_timeStep; + } - 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; + 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, + body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass, contactConstraint.m_angularComponentA,normalImpulse); - } - if (body2.m_invMass) - { - body2.internalApplyImpulse(contactConstraint.m_contactNormal*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( +btScalar resolveSingleFrictionCacheFriendly( btSolverBody& body1, btSolverBody& body2, - btSolverConstraint& contactConstraint, + const btSolverConstraint& contactConstraint, + const btContactSolverInfo& solverInfo, + btScalar appliedNormalImpulse); + +//SIMD_FORCE_INLINE +btScalar resolveSingleFrictionCacheFriendly( + btSolverBody& body1, + btSolverBody& body2, + const btSolverConstraint& contactConstraint, const btContactSolverInfo& solverInfo, btScalar appliedNormalImpulse) { @@ -252,22 +333,42 @@ SIMD_FORCE_INLINE btScalar resolveSingleFrictionCacheFriendly( // calculate j that moves us to zero relative velocity j1 = -rel_vel * contactConstraint.m_jacDiagABInv; +#define CLAMP_ACCUMULATED_FRICTION_IMPULSE 1 +#ifdef CLAMP_ACCUMULATED_FRICTION_IMPULSE btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse; contactConstraint.m_appliedImpulse = oldTangentImpulse + j1; - GEN_set_min(contactConstraint.m_appliedImpulse, limit); - GEN_set_max(contactConstraint.m_appliedImpulse, -limit); + + if (limit < contactConstraint.m_appliedImpulse) + { + contactConstraint.m_appliedImpulse = limit; + } else + { + if (contactConstraint.m_appliedImpulse < -limit) + contactConstraint.m_appliedImpulse = -limit; + } j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse; +#else + if (limit < j1) + { + j1 = limit; + } else + { + if (j1 < -limit) + j1 = -limit; + } + +#endif //CLAMP_ACCUMULATED_FRICTION_IMPULSE + + //GEN_set_min(contactConstraint.m_appliedImpulse, limit); + //GEN_set_max(contactConstraint.m_appliedImpulse, -limit); + + } - 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); - } + body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1); + + body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1); } return 0.f; @@ -309,7 +410,6 @@ btScalar resolveSingleFrictionCacheFriendly( const btVector3& rel_pos2 = contactConstraint.m_rel_posB; - //if (contactConstraint.m_appliedVelocityImpulse > 0.f) if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON) { lat_rel_vel = btSqrt(lat_rel_vel); @@ -319,7 +419,7 @@ btScalar resolveSingleFrictionCacheFriendly( 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; + btScalar normal_impulse = contactConstraint.m_appliedImpulse * combinedFriction; GEN_set_min(friction_impulse, normal_impulse); GEN_set_max(friction_impulse, -normal_impulse); @@ -333,39 +433,111 @@ btScalar resolveSingleFrictionCacheFriendly( #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 btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation) +{ + + btRigidBody* body0=btRigidBody::upcast(colObj0); + btRigidBody* body1=btRigidBody::upcast(colObj1); + + btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand(); + solverConstraint.m_contactNormal = normalAxis; + + 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_originalContactPoint = 0; + + solverConstraint.m_appliedImpulse = btScalar(0.); + solverConstraint.m_appliedPushImpulse = 0.f; + solverConstraint.m_penetration = 0.f; + { + btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; + solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0); + } + { + btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; + solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0); + } + +#ifdef COMPUTE_IMPULSE_DENOM + btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal); + btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal); +#else + btVector3 vec; + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + if (body0) + { + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = body0->getInvMass() + normalAxis.dot(vec); + } + if (body1) + { + vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = body1->getInvMass() + normalAxis.dot(vec); + } + + +#endif //COMPUTE_IMPULSE_DENOM + btScalar denom = relaxation/(denom0+denom1); + solverConstraint.m_jacDiagABInv = denom; + + +} + + + +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) { + BT_PROFILE("solveGroupCacheFriendlySetup"); (void)stackAlloc; (void)debugDrawer; + if (!(numConstraints + numManifolds)) { // printf("empty\n"); return 0.f; } + btPersistentManifold* manifold = 0; + btCollisionObject* colObj0=0,*colObj1=0; + + //btRigidBody* rb0=0,*rb1=0; + + +#ifdef FORCE_REFESH_CONTACT_MANIFOLDS 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(); - + manifold = manifoldPtr[i]; + rb1 = (btRigidBody*)manifold->getBody1(); + rb0 = (btRigidBody*)manifold->getBody0(); + manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform()); } - + END_PROFILE("refreshManifolds"); +#endif //FORCE_REFESH_CONTACT_MANIFOLDS + + - BEGIN_PROFILE("gatherSolverData"); //int sizeofSB = sizeof(btSolverBody); //int sizeofSC = sizeof(btSolverConstraint); @@ -382,10 +554,12 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio //todo: use stack allocator for this temp memory - int minReservation = numManifolds*2; +// int minReservation = numManifolds*2; - tmpSolverBodyPool.reserve(minReservation); + //m_tmpSolverBodyPool.reserve(minReservation); + //don't convert all bodies, only the one we need so solver the constraints +/* { for (int i=0;i<numBodies;i++) { @@ -393,26 +567,26 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio if (rb && (rb->getIslandTag() >= 0)) { btAssert(rb->getCompanionId() < 0); - int solverBodyId = tmpSolverBodyPool.size(); - btSolverBody& solverBody = tmpSolverBodyPool.expand(); + int solverBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); initSolverBody(&solverBody,rb); rb->setCompanionId(solverBodyId); } } } - +*/ - tmpSolverConstraintPool.reserve(minReservation); - tmpSolverFrictionConstraintPool.reserve(minReservation); + //m_tmpSolverConstraintPool.reserve(minReservation); + //m_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(); - + manifold = manifoldPtr[i]; + colObj0 = (btCollisionObject*)manifold->getBody0(); + colObj1 = (btCollisionObject*)manifold->getBody1(); int solverBodyIdA=-1; int solverBodyIdB=-1; @@ -422,61 +596,108 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio - if (rb0->getIslandTag() >= 0) + if (colObj0->getIslandTag() >= 0) { - solverBodyIdA = rb0->getCompanionId(); + if (colObj0->getCompanionId() >= 0) + { + //body has already been converted + solverBodyIdA = colObj0->getCompanionId(); + } else + { + solverBodyIdA = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&solverBody,colObj0); + colObj0->setCompanionId(solverBodyIdA); + } } else { //create a static body - solverBodyIdA = tmpSolverBodyPool.size(); - btSolverBody& solverBody = tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,rb0); + solverBodyIdA = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&solverBody,colObj0); } - if (rb1->getIslandTag() >= 0) + if (colObj1->getIslandTag() >= 0) { - solverBodyIdB = rb1->getCompanionId(); + if (colObj1->getCompanionId() >= 0) + { + solverBodyIdB = colObj1->getCompanionId(); + } else + { + solverBodyIdB = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&solverBody,colObj1); + colObj1->setCompanionId(solverBodyIdB); + } } else { //create a static body - solverBodyIdB = tmpSolverBodyPool.size(); - btSolverBody& solverBody = tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,rb1); + solverBodyIdB = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&solverBody,colObj1); } } + btVector3 rel_pos1; + btVector3 rel_pos2; + btScalar relaxation; + 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(); + rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); + rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); - btScalar relaxation = 1.f; + relaxation = 1.f; + btScalar rel_vel; + btVector3 vel; + + int frictionIndex = m_tmpSolverConstraintPool.size(); { - btSolverConstraint& solverConstraint = tmpSolverConstraintPool.expand(); + btSolverConstraint& solverConstraint = m_tmpSolverConstraintPool.expand(); + btRigidBody* rb0 = btRigidBody::upcast(colObj0); + btRigidBody* rb1 = btRigidBody::upcast(colObj1); solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D; - + solverConstraint.m_originalContactPoint = &cp; + btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0 : btVector3(0,0,0); + btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1 : btVector3(0,0,0); { - //can be optimized, the cross products are already calculated +#ifdef COMPUTE_IMPULSE_DENOM btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); +#else + btVector3 vec; + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + if (rb0) + { + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec); + } + if (rb1) + { + vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec); + } +#endif //COMPUTE_IMPULSE_DENOM + btScalar denom = relaxation/(denom0+denom1); solverConstraint.m_jacDiagABInv = denom; } @@ -486,121 +707,115 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB); - btVector3 vel1 = rb0->getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = rb1->getVelocityInLocalPoint(rel_pos2); + btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); + btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); - btVector3 vel = vel1 - vel2; - btScalar rel_vel; + vel = vel1 - vel2; + rel_vel = cp.m_normalWorldOnB.dot(vel); + solverConstraint.m_penetration = btMin(cp.getDistance()+infoGlobal.m_linearSlop,btScalar(0.)); + //solverConstraint.m_penetration = cp.getDistance(); - 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.)) + solverConstraint.m_restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); + if (solverConstraint.m_restitution <= btScalar(0.)) { - rest = 0.f; + solverConstraint.m_restitution = 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; + if (solverConstraint.m_restitution > penVel) { - btVector3 ftorqueAxis0 = rel_pos1.cross(solverConstraint.m_contactNormal); - solverConstraint.m_relpos1CrossNormal = ftorqueAxis0; - solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis0; + solverConstraint.m_penetration = btScalar(0.); } + + + + ///warm starting (or zero if disabled) + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; + if (rb0) + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); + if (rb1) + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse); + } else { - btVector3 ftorqueAxis0 = rel_pos2.cross(solverConstraint.m_contactNormal); - solverConstraint.m_relpos2CrossNormal = ftorqueAxis0; - solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis0; + solverConstraint.m_appliedImpulse = 0.f; } - } - - - { - - 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_appliedPushImpulse = 0.f; + + solverConstraint.m_frictionIndex = m_tmpSolverFrictionConstraintPool.size(); + if (!cp.m_lateralFrictionInitialized) + { + cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; + btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); + if (lat_rel_vel > SIMD_EPSILON)//0.0f) + { + cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); + cp.m_lateralFrictionDir2.normalize();//?? + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + } else + { + //re-calculate friction direction every frame, todo: check if this is really needed + + btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + } + cp.m_lateralFrictionInitialized = true; + + } else + { + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + } - 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; + btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex]; + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; + if (rb0) + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); + if (rb1) + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse); + } else + { + frictionConstraint1.m_appliedImpulse = 0.f; } + } { - btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal); - solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis1; + btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; + if (rb0) + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); + if (rb1) + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse); + } else + { + frictionConstraint2.m_appliedImpulse = 0.f; } } + } + } } } } } - END_PROFILE("gatherSolverData"); - - BEGIN_PROFILE("prepareConstraints"); - + btContactSolverInfo info = infoGlobal; { @@ -612,57 +827,60 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio } } - btAlignedObjectArray<int> gOrderTmpConstraintPool; - btAlignedObjectArray<int> gOrderFrictionConstraintPool; + - int numConstraintPool = tmpSolverConstraintPool.size(); - int numFrictionPool = tmpSolverFrictionConstraintPool.size(); + int numConstraintPool = m_tmpSolverConstraintPool.size(); + int numFrictionPool = m_tmpSolverFrictionConstraintPool.size(); ///todo: use stack allocator for such temporarily memory, same for solver bodies/constraints - gOrderTmpConstraintPool.resize(numConstraintPool); - gOrderFrictionConstraintPool.resize(numFrictionPool); + m_orderTmpConstraintPool.resize(numConstraintPool); + m_orderFrictionConstraintPool.resize(numFrictionPool); { int i; for (i=0;i<numConstraintPool;i++) { - gOrderTmpConstraintPool[i] = i; + m_orderTmpConstraintPool[i] = i; } for (i=0;i<numFrictionPool;i++) { - gOrderFrictionConstraintPool[i] = i; + m_orderFrictionConstraintPool[i] = i; } } + return 0.f; - END_PROFILE("prepareConstraints"); - +} - BEGIN_PROFILE("solveConstraints"); +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/) +{ + BT_PROFILE("solveGroupCacheFriendlyIterations"); + int numConstraintPool = m_tmpSolverConstraintPool.size(); + int numFrictionPool = m_tmpSolverFrictionConstraintPool.size(); //should traverse the contacts random order... int iteration; { - for ( iteration = 0;iteration<info.m_numIterations;iteration++) + for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) { int j; - if (m_solverMode & SOLVER_RANDMIZE_ORDER) + if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) { if ((iteration & 7) == 0) { for (j=0; j<numConstraintPool; ++j) { - int tmp = gOrderTmpConstraintPool[j]; + int tmp = m_orderTmpConstraintPool[j]; int swapi = btRandInt2(j+1); - gOrderTmpConstraintPool[j] = gOrderTmpConstraintPool[swapi]; - gOrderTmpConstraintPool[swapi] = tmp; + m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; + m_orderTmpConstraintPool[swapi] = tmp; } for (j=0; j<numFrictionPool; ++j) { - int tmp = gOrderFrictionConstraintPool[j]; + int tmp = m_orderFrictionConstraintPool[j]; int swapi = btRandInt2(j+1); - gOrderFrictionConstraintPool[j] = gOrderFrictionConstraintPool[swapi]; - gOrderFrictionConstraintPool[swapi] = tmp; + m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; + m_orderFrictionConstraintPool[swapi] = tmp; } } } @@ -674,86 +892,145 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0)) { - tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity(); + m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity(); } if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0)) { - tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity(); + m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity(); } - constraint->solveConstraint(info.m_timeStep); + constraint->solveConstraint(infoGlobal.m_timeStep); if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0)) { - tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity(); + m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity(); } if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0)) { - tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity(); + m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity(); } } { - int numPoolConstraints = tmpSolverConstraintPool.size(); + int numPoolConstraints = m_tmpSolverConstraintPool.size(); for (j=0;j<numPoolConstraints;j++) { - btSolverConstraint& solveManifold = tmpSolverConstraintPool[gOrderTmpConstraintPool[j]]; - resolveSingleCollisionCombinedCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA], - tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info); + + const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]]; + resolveSingleCollisionCombinedCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], + m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal); } } { - int numFrictionPoolConstraints = tmpSolverFrictionConstraintPool.size(); - for (j=0;j<numFrictionPoolConstraints;j++) + int numFrictionPoolConstraints = m_tmpSolverFrictionConstraintPool.size(); + + for (j=0;j<numFrictionPoolConstraints;j++) { - btSolverConstraint& solveManifold = tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]]; - btScalar appliedNormalImpulse = tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + const btSolverConstraint& solveManifold = m_tmpSolverFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; + btScalar totalImpulse = m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse+ + m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedPushImpulse; - resolveSingleFrictionCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA], - tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info,appliedNormalImpulse); + resolveSingleFrictionCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], + m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal, + totalImpulse); } } } + + if (infoGlobal.m_splitImpulse) + { + + for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) + { + { + int numPoolConstraints = m_tmpSolverConstraintPool.size(); + int j; + for (j=0;j<numPoolConstraints;j++) + { + const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]]; + + resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], + m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal); + } + } + } + + } + } - - for ( i=0;i<tmpSolverBodyPool.size();i++) + + return 0.f; +} + + +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) +{ + int i; + + solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); + solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); + + int numPoolConstraints = m_tmpSolverConstraintPool.size(); + int j; + for (j=0;j<numPoolConstraints;j++) { - tmpSolverBodyPool[i].writebackVelocity(); + + const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j]; + btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint; + btAssert(pt); + pt->m_appliedImpulse = solveManifold.m_appliedImpulse; + pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse; + + //do a callback here? + } - END_PROFILE("solveConstraints"); + if (infoGlobal.m_splitImpulse) + { + for ( i=0;i<m_tmpSolverBodyPool.size();i++) + { + m_tmpSolverBodyPool[i].writebackVelocity(infoGlobal.m_timeStep); + } + } else + { + for ( i=0;i<m_tmpSolverBodyPool.size();i++) + { + m_tmpSolverBodyPool[i].writebackVelocity(); + } + } -// printf("tmpSolverConstraintPool.size() = %i\n",tmpSolverConstraintPool.size()); +// printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size()); /* - printf("tmpSolverBodyPool.size() = %i\n",tmpSolverBodyPool.size()); - printf("tmpSolverConstraintPool.size() = %i\n",tmpSolverConstraintPool.size()); - printf("tmpSolverFrictionConstraintPool.size() = %i\n",tmpSolverFrictionConstraintPool.size()); + printf("m_tmpSolverBodyPool.size() = %i\n",m_tmpSolverBodyPool.size()); + printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size()); + printf("m_tmpSolverFrictionConstraintPool.size() = %i\n",m_tmpSolverFrictionConstraintPool.size()); - printf("tmpSolverBodyPool.capacity() = %i\n",tmpSolverBodyPool.capacity()); - printf("tmpSolverConstraintPool.capacity() = %i\n",tmpSolverConstraintPool.capacity()); - printf("tmpSolverFrictionConstraintPool.capacity() = %i\n",tmpSolverFrictionConstraintPool.capacity()); + printf("m_tmpSolverBodyPool.capacity() = %i\n",m_tmpSolverBodyPool.capacity()); + printf("m_tmpSolverConstraintPool.capacity() = %i\n",m_tmpSolverConstraintPool.capacity()); + printf("m_tmpSolverFrictionConstraintPool.capacity() = %i\n",m_tmpSolverFrictionConstraintPool.capacity()); */ - tmpSolverBodyPool.resize(0); - tmpSolverConstraintPool.resize(0); - tmpSolverFrictionConstraintPool.resize(0); + m_tmpSolverBodyPool.resize(0); + m_tmpSolverConstraintPool.resize(0); + m_tmpSolverFrictionConstraintPool.resize(0); return 0.f; } /// btSequentialImpulseConstraintSolver Sequentially applies impulses -btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) +btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/) { - - if (getSolverMode() & SOLVER_CACHE_FRIENDLY) + BT_PROFILE("solveGroup"); + if (infoGlobal.m_solverMode & SOLVER_CACHE_FRIENDLY) { //you need to provide at least some bodies //btSimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY @@ -762,16 +1039,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); } - - BEGIN_PROFILE("prepareConstraints"); btContactSolverInfo info = infoGlobal; int numiter = infoGlobal.m_numIterations; -#ifdef USE_PROFILE - btProfiler::beginBlock("solve"); -#endif //USE_PROFILE int totalPoints = 0; @@ -801,10 +1073,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod } } - END_PROFILE("prepareConstraints"); - - - BEGIN_PROFILE("solveConstraints"); //should traverse the contacts random order... int iteration; @@ -813,7 +1081,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod for ( iteration = 0;iteration<numiter;iteration++) { int j; - if (m_solverMode & SOLVER_RANDMIZE_ORDER) + if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) { if ((iteration & 7) == 0) { for (j=0; j<totalPoints; ++j) { @@ -845,16 +1113,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod solveFriction((btRigidBody*)manifold->getBody0(), (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer); } + } } - END_PROFILE("solveConstraints"); - - -#ifdef USE_PROFILE - btProfiler::endBlock("solve"); -#endif //USE_PROFILE - @@ -878,13 +1140,14 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop { +#ifdef FORCE_REFESH_CONTACT_MANIFOLDS manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform()); - +#endif //FORCE_REFESH_CONTACT_MANIFOLDS int numpoints = manifoldPtr->getNumContacts(); gTotalContactPoints += numpoints; - btVector3 color(0,1,0); + for (int i=0;i<numpoints ;i++) { btManifoldPoint& cp = manifoldPtr->getContactPoint(i); @@ -925,7 +1188,9 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol } else { - cpd = new btConstraintPersistentData; + //todo: should this be in a pool? + void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16); + cpd = new (mem)btConstraintPersistentData; assert(cpd); totalCpd ++; @@ -972,10 +1237,9 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol cpd->m_penetration = btScalar(0.); } - btScalar relaxation = info.m_damping; - if (m_solverMode & SOLVER_USE_WARMSTARTING) + if (info.m_solverMode & SOLVER_USE_WARMSTARTING) { cpd->m_appliedImpulse *= relaxation; } else @@ -1060,16 +1324,12 @@ btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRig { - btVector3 color(0,1,0); + { if (cp.getDistance() <= btScalar(0.)) { - if (iter == 0) - { - if (debugDrawer) - debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color); - } + { @@ -1098,16 +1358,12 @@ btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBo { - btVector3 color(0,1,0); + { if (cp.getDistance() <= btScalar(0.)) { - if (iter == 0) - { - if (debugDrawer) - debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color); - } + { @@ -1136,7 +1392,7 @@ btScalar btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,b { - btVector3 color(0,1,0); + { if (cp.getDistance() <= btScalar(0.)) @@ -1156,3 +1412,11 @@ btScalar btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,b } return btScalar(0.); } + + +void btSequentialImpulseConstraintSolver::reset() +{ + m_btSeed2 = 0; +} + + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 13e70c41be4..7143bc41991 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -19,7 +19,8 @@ subject to the following restrictions: #include "btConstraintSolver.h" class btIDebugDraw; #include "btContactConstraint.h" - +#include "btSolverBody.h" +#include "btSolverConstraint.h" /// btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses @@ -29,29 +30,29 @@ class btIDebugDraw; class btSequentialImpulseConstraintSolver : public btConstraintSolver { + btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool; + btAlignedObjectArray<btSolverConstraint> m_tmpSolverConstraintPool; + btAlignedObjectArray<btSolverConstraint> m_tmpSolverFrictionConstraintPool; + btAlignedObjectArray<int> m_orderTmpConstraintPool; + btAlignedObjectArray<int> m_orderFrictionConstraintPool; + + protected: 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); + void addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation); ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]; ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]; - //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: - enum eSolverMode - { - SOLVER_RANDMIZE_ORDER = 1, - SOLVER_FRICTION_SEPARATE = 2, - SOLVER_USE_WARMSTARTING = 4, - SOLVER_CACHE_FRIENDLY = 8 - }; - + btSequentialImpulseConstraintSolver(); ///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody @@ -68,25 +69,22 @@ public: m_frictionDispatch[type0][type1] = func; } - virtual ~btSequentialImpulseConstraintSolver() {} + virtual ~btSequentialImpulseConstraintSolver(); - virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc); + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher); virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); + btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); + btScalar solveGroupCacheFriendlySetup(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); + ///clear internal cached data and reset random seed + virtual void reset(); - void setSolverMode(int mode) - { - m_solverMode = mode; - } + btScalar solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); - int getSolverMode() const - { - return m_solverMode; - } + unsigned long btRand2(); int btRandInt2 (int n); @@ -102,7 +100,9 @@ public: }; - +#ifndef BT_PREFER_SIMD +typedef btSequentialImpulseConstraintSolver btSequentialImpulseConstraintSolverPrefered; +#endif #endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp new file mode 100644 index 00000000000..e7f07a428eb --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -0,0 +1,414 @@ +/* +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. +*/ + +/* +Added by Roman Ponomarev (rponom@gmail.com) +April 04, 2008 +*/ + +//----------------------------------------------------------------------------- + +#include "btSliderConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btTransformUtil.h" +#include <new> + +//----------------------------------------------------------------------------- + +void btSliderConstraint::initParams() +{ + m_lowerLinLimit = btScalar(1.0); + m_upperLinLimit = btScalar(-1.0); + m_lowerAngLimit = btScalar(0.); + m_upperAngLimit = btScalar(0.); + m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingDirLin = btScalar(0.); + m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingDirAng = btScalar(0.); + m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING; + m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING; + m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING; + m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING; + + m_poweredLinMotor = false; + m_targetLinMotorVelocity = btScalar(0.); + m_maxLinMotorForce = btScalar(0.); + m_accumulatedLinMotorImpulse = btScalar(0.0); + + m_poweredAngMotor = false; + m_targetAngMotorVelocity = btScalar(0.); + m_maxAngMotorForce = btScalar(0.); + m_accumulatedAngMotorImpulse = btScalar(0.0); + +} // btSliderConstraint::initParams() + +//----------------------------------------------------------------------------- + +btSliderConstraint::btSliderConstraint() + :btTypedConstraint(SLIDER_CONSTRAINT_TYPE), + m_useLinearReferenceFrameA(true) +{ + initParams(); +} // btSliderConstraint::btSliderConstraint() + +//----------------------------------------------------------------------------- + +btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) + : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB) + , m_frameInA(frameInA) + , m_frameInB(frameInB), + m_useLinearReferenceFrameA(useLinearReferenceFrameA) +{ + initParams(); +} // btSliderConstraint::btSliderConstraint() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::buildJacobian() +{ + if(m_useLinearReferenceFrameA) + { + buildJacobianInt(m_rbA, m_rbB, m_frameInA, m_frameInB); + } + else + { + buildJacobianInt(m_rbB, m_rbA, m_frameInB, m_frameInA); + } +} // btSliderConstraint::buildJacobian() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB) +{ + //calculate transforms + m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA; + m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB; + m_realPivotAInW = m_calculatedTransformA.getOrigin(); + m_realPivotBInW = m_calculatedTransformB.getOrigin(); + m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X + m_delta = m_realPivotBInW - m_realPivotAInW; + m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; + m_relPosA = m_projPivotInW - rbA.getCenterOfMassPosition(); + m_relPosB = m_realPivotBInW - rbB.getCenterOfMassPosition(); + btVector3 normalWorld; + int i; + //linear part + for(i = 0; i < 3; i++) + { + normalWorld = m_calculatedTransformA.getBasis().getColumn(i); + new (&m_jacLin[i]) btJacobianEntry( + rbA.getCenterOfMassTransform().getBasis().transpose(), + rbB.getCenterOfMassTransform().getBasis().transpose(), + m_relPosA, + m_relPosB, + normalWorld, + rbA.getInvInertiaDiagLocal(), + rbA.getInvMass(), + rbB.getInvInertiaDiagLocal(), + rbB.getInvMass() + ); + m_jacLinDiagABInv[i] = btScalar(1.) / m_jacLin[i].getDiagonal(); + m_depth[i] = m_delta.dot(normalWorld); + } + testLinLimits(); + // angular part + for(i = 0; i < 3; i++) + { + normalWorld = m_calculatedTransformA.getBasis().getColumn(i); + new (&m_jacAng[i]) btJacobianEntry( + normalWorld, + rbA.getCenterOfMassTransform().getBasis().transpose(), + rbB.getCenterOfMassTransform().getBasis().transpose(), + rbA.getInvInertiaDiagLocal(), + rbB.getInvInertiaDiagLocal() + ); + } + testAngLimits(); + btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); + m_kAngle = btScalar(1.0 )/ (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA)); + // clear accumulator for motors + m_accumulatedLinMotorImpulse = btScalar(0.0); + m_accumulatedAngMotorImpulse = btScalar(0.0); +} // btSliderConstraint::buildJacobianInt() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::solveConstraint(btScalar timeStep) +{ + m_timeStep = timeStep; + if(m_useLinearReferenceFrameA) + { + solveConstraintInt(m_rbA, m_rbB); + } + else + { + solveConstraintInt(m_rbB, m_rbA); + } +} // btSliderConstraint::solveConstraint() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) +{ + int i; + // linear + btVector3 velA = rbA.getVelocityInLocalPoint(m_relPosA); + btVector3 velB = rbB.getVelocityInLocalPoint(m_relPosB); + btVector3 vel = velA - velB; + for(i = 0; i < 3; i++) + { + const btVector3& normal = m_jacLin[i].m_linearJointAxis; + btScalar rel_vel = normal.dot(vel); + // calculate positional error + btScalar depth = m_depth[i]; + // get parameters + btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin); + btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin); + btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin); + // calcutate and apply impulse + btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i]; + btVector3 impulse_vector = normal * normalImpulse; + rbA.applyImpulse( impulse_vector, m_relPosA); + rbB.applyImpulse(-impulse_vector, m_relPosB); + if(m_poweredLinMotor && (!i)) + { // apply linear motor + if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce) + { + btScalar desiredMotorVel = m_targetLinMotorVelocity; + btScalar motor_relvel = desiredMotorVel + rel_vel; + normalImpulse = -motor_relvel * m_jacLinDiagABInv[i]; + // clamp accumulated impulse + btScalar new_acc = m_accumulatedLinMotorImpulse + btFabs(normalImpulse); + if(new_acc > m_maxLinMotorForce) + { + new_acc = m_maxLinMotorForce; + } + btScalar del = new_acc - m_accumulatedLinMotorImpulse; + if(normalImpulse < btScalar(0.0)) + { + normalImpulse = -del; + } + else + { + normalImpulse = del; + } + m_accumulatedLinMotorImpulse = new_acc; + // apply clamped impulse + impulse_vector = normal * normalImpulse; + rbA.applyImpulse( impulse_vector, m_relPosA); + rbB.applyImpulse(-impulse_vector, m_relPosB); + } + } + } + // angular + // get axes in world space + btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); + btVector3 axisB = m_calculatedTransformB.getBasis().getColumn(0); + + const btVector3& angVelA = rbA.getAngularVelocity(); + const btVector3& angVelB = rbB.getAngularVelocity(); + + btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA); + btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB); + + btVector3 angAorthog = angVelA - angVelAroundAxisA; + btVector3 angBorthog = angVelB - angVelAroundAxisB; + btVector3 velrelOrthog = angAorthog-angBorthog; + //solve orthogonal angular velocity correction + btScalar len = velrelOrthog.length(); + if (len > btScalar(0.00001)) + { + btVector3 normal = velrelOrthog.normalized(); + btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal); + velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; + } + //solve angular positional correction + btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep); + btScalar len2 = angularError.length(); + if (len2>btScalar(0.00001)) + { + btVector3 normal2 = angularError.normalized(); + btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2); + angularError *= (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; + } + // apply impulse + rbA.applyTorqueImpulse(-velrelOrthog+angularError); + rbB.applyTorqueImpulse(velrelOrthog-angularError); + btScalar impulseMag; + //solve angular limits + if(m_solveAngLim) + { + impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / m_timeStep; + impulseMag *= m_kAngle * m_softnessLimAng; + } + else + { + impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / m_timeStep; + impulseMag *= m_kAngle * m_softnessDirAng; + } + btVector3 impulse = axisA * impulseMag; + rbA.applyTorqueImpulse(impulse); + rbB.applyTorqueImpulse(-impulse); + //apply angular motor + if(m_poweredAngMotor) + { + if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce) + { + btVector3 velrel = angVelAroundAxisA - angVelAroundAxisB; + btScalar projRelVel = velrel.dot(axisA); + + btScalar desiredMotorVel = m_targetAngMotorVelocity; + btScalar motor_relvel = desiredMotorVel - projRelVel; + + btScalar angImpulse = m_kAngle * motor_relvel; + // clamp accumulated impulse + btScalar new_acc = m_accumulatedAngMotorImpulse + btFabs(angImpulse); + if(new_acc > m_maxAngMotorForce) + { + new_acc = m_maxAngMotorForce; + } + btScalar del = new_acc - m_accumulatedAngMotorImpulse; + if(angImpulse < btScalar(0.0)) + { + angImpulse = -del; + } + else + { + angImpulse = del; + } + m_accumulatedAngMotorImpulse = new_acc; + // apply clamped impulse + btVector3 motorImp = angImpulse * axisA; + m_rbA.applyTorqueImpulse(motorImp); + m_rbB.applyTorqueImpulse(-motorImp); + } + } +} // btSliderConstraint::solveConstraint() + +//----------------------------------------------------------------------------- + +//----------------------------------------------------------------------------- + +void btSliderConstraint::calculateTransforms(void){ + if(m_useLinearReferenceFrameA) + { + m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA; + m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB; + } + else + { + m_calculatedTransformA = m_rbB.getCenterOfMassTransform() * m_frameInB; + m_calculatedTransformB = m_rbA.getCenterOfMassTransform() * m_frameInA; + } + m_realPivotAInW = m_calculatedTransformA.getOrigin(); + m_realPivotBInW = m_calculatedTransformB.getOrigin(); + m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X + m_delta = m_realPivotBInW - m_realPivotAInW; + m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; + btVector3 normalWorld; + int i; + //linear part + for(i = 0; i < 3; i++) + { + normalWorld = m_calculatedTransformA.getBasis().getColumn(i); + m_depth[i] = m_delta.dot(normalWorld); + } +} // btSliderConstraint::calculateTransforms() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::testLinLimits(void) +{ + m_solveLinLim = false; + if(m_lowerLinLimit <= m_upperLinLimit) + { + if(m_depth[0] > m_upperLinLimit) + { + m_depth[0] -= m_upperLinLimit; + m_solveLinLim = true; + } + else if(m_depth[0] < m_lowerLinLimit) + { + m_depth[0] -= m_lowerLinLimit; + m_solveLinLim = true; + } + else + { + m_depth[0] = btScalar(0.); + } + } + else + { + m_depth[0] = btScalar(0.); + } +} // btSliderConstraint::testLinLimits() + +//----------------------------------------------------------------------------- + + +void btSliderConstraint::testAngLimits(void) +{ + m_angDepth = btScalar(0.); + m_solveAngLim = false; + if(m_lowerAngLimit <= m_upperAngLimit) + { + const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1); + const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2); + const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1); + btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); + if(rot < m_lowerAngLimit) + { + m_angDepth = rot - m_lowerAngLimit; + m_solveAngLim = true; + } + else if(rot > m_upperAngLimit) + { + m_angDepth = rot - m_upperAngLimit; + m_solveAngLim = true; + } + } +} // btSliderConstraint::testAngLimits() + + +//----------------------------------------------------------------------------- + + + +btVector3 btSliderConstraint::getAncorInA(void) +{ + btVector3 ancorInA; + ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis; + ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA; + return ancorInA; +} // btSliderConstraint::getAncorInA() + +//----------------------------------------------------------------------------- + +btVector3 btSliderConstraint::getAncorInB(void) +{ + btVector3 ancorInB; + ancorInB = m_frameInB.getOrigin(); + return ancorInB; +} // btSliderConstraint::getAncorInB(); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h new file mode 100644 index 00000000000..f69dfcf3aa7 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -0,0 +1,215 @@ +/* +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. +*/ + +/* +Added by Roman Ponomarev (rponom@gmail.com) +April 04, 2008 + +TODO: + - add clamping od accumulated impulse to improve stability + - add conversion for ODE constraint solver +*/ + +#ifndef SLIDER_CONSTRAINT_H +#define SLIDER_CONSTRAINT_H + +//----------------------------------------------------------------------------- + +#include "LinearMath/btVector3.h" +#include "btJacobianEntry.h" +#include "btTypedConstraint.h" + +//----------------------------------------------------------------------------- + +class btRigidBody; + +//----------------------------------------------------------------------------- + +#define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0)) +#define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0)) +#define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7)) + +//----------------------------------------------------------------------------- + +class btSliderConstraint : public btTypedConstraint +{ +protected: + btTransform m_frameInA; + btTransform m_frameInB; + // use frameA fo define limits, if true + bool m_useLinearReferenceFrameA; + // linear limits + btScalar m_lowerLinLimit; + btScalar m_upperLinLimit; + // angular limits + btScalar m_lowerAngLimit; + btScalar m_upperAngLimit; + // softness, restitution and damping for different cases + // DirLin - moving inside linear limits + // LimLin - hitting linear limit + // DirAng - moving inside angular limits + // LimAng - hitting angular limit + // OrthoLin, OrthoAng - against constraint axis + btScalar m_softnessDirLin; + btScalar m_restitutionDirLin; + btScalar m_dampingDirLin; + btScalar m_softnessDirAng; + btScalar m_restitutionDirAng; + btScalar m_dampingDirAng; + btScalar m_softnessLimLin; + btScalar m_restitutionLimLin; + btScalar m_dampingLimLin; + btScalar m_softnessLimAng; + btScalar m_restitutionLimAng; + btScalar m_dampingLimAng; + btScalar m_softnessOrthoLin; + btScalar m_restitutionOrthoLin; + btScalar m_dampingOrthoLin; + btScalar m_softnessOrthoAng; + btScalar m_restitutionOrthoAng; + btScalar m_dampingOrthoAng; + + // for interlal use + bool m_solveLinLim; + bool m_solveAngLim; + + btJacobianEntry m_jacLin[3]; + btScalar m_jacLinDiagABInv[3]; + + btJacobianEntry m_jacAng[3]; + + btScalar m_timeStep; + btTransform m_calculatedTransformA; + btTransform m_calculatedTransformB; + + btVector3 m_sliderAxis; + btVector3 m_realPivotAInW; + btVector3 m_realPivotBInW; + btVector3 m_projPivotInW; + btVector3 m_delta; + btVector3 m_depth; + btVector3 m_relPosA; + btVector3 m_relPosB; + + btScalar m_angDepth; + btScalar m_kAngle; + + bool m_poweredLinMotor; + btScalar m_targetLinMotorVelocity; + btScalar m_maxLinMotorForce; + btScalar m_accumulatedLinMotorImpulse; + + bool m_poweredAngMotor; + btScalar m_targetAngMotorVelocity; + btScalar m_maxAngMotorForce; + btScalar m_accumulatedAngMotorImpulse; + + //------------------------ + void initParams(); +public: + // constructors + btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); + btSliderConstraint(); + // overrides + virtual void buildJacobian(); + virtual void solveConstraint(btScalar timeStep); + // access + const btRigidBody& getRigidBodyA() const { return m_rbA; } + const btRigidBody& getRigidBodyB() const { return m_rbB; } + const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; } + const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; } + const btTransform & getFrameOffsetA() const { return m_frameInA; } + const btTransform & getFrameOffsetB() const { return m_frameInB; } + btTransform & getFrameOffsetA() { return m_frameInA; } + btTransform & getFrameOffsetB() { return m_frameInB; } + btScalar getLowerLinLimit() { return m_lowerLinLimit; } + void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; } + btScalar getUpperLinLimit() { return m_upperLinLimit; } + void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; } + btScalar getLowerAngLimit() { return m_lowerAngLimit; } + void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = lowerLimit; } + btScalar getUpperAngLimit() { return m_upperAngLimit; } + void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = upperLimit; } + bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; } + btScalar getSoftnessDirLin() { return m_softnessDirLin; } + btScalar getRestitutionDirLin() { return m_restitutionDirLin; } + btScalar getDampingDirLin() { return m_dampingDirLin ; } + btScalar getSoftnessDirAng() { return m_softnessDirAng; } + btScalar getRestitutionDirAng() { return m_restitutionDirAng; } + btScalar getDampingDirAng() { return m_dampingDirAng; } + btScalar getSoftnessLimLin() { return m_softnessLimLin; } + btScalar getRestitutionLimLin() { return m_restitutionLimLin; } + btScalar getDampingLimLin() { return m_dampingLimLin; } + btScalar getSoftnessLimAng() { return m_softnessLimAng; } + btScalar getRestitutionLimAng() { return m_restitutionLimAng; } + btScalar getDampingLimAng() { return m_dampingLimAng; } + btScalar getSoftnessOrthoLin() { return m_softnessOrthoLin; } + btScalar getRestitutionOrthoLin() { return m_restitutionOrthoLin; } + btScalar getDampingOrthoLin() { return m_dampingOrthoLin; } + btScalar getSoftnessOrthoAng() { return m_softnessOrthoAng; } + btScalar getRestitutionOrthoAng() { return m_restitutionOrthoAng; } + btScalar getDampingOrthoAng() { return m_dampingOrthoAng; } + void setSoftnessDirLin(btScalar softnessDirLin) { m_softnessDirLin = softnessDirLin; } + void setRestitutionDirLin(btScalar restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; } + void setDampingDirLin(btScalar dampingDirLin) { m_dampingDirLin = dampingDirLin; } + void setSoftnessDirAng(btScalar softnessDirAng) { m_softnessDirAng = softnessDirAng; } + void setRestitutionDirAng(btScalar restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; } + void setDampingDirAng(btScalar dampingDirAng) { m_dampingDirAng = dampingDirAng; } + void setSoftnessLimLin(btScalar softnessLimLin) { m_softnessLimLin = softnessLimLin; } + void setRestitutionLimLin(btScalar restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; } + void setDampingLimLin(btScalar dampingLimLin) { m_dampingLimLin = dampingLimLin; } + void setSoftnessLimAng(btScalar softnessLimAng) { m_softnessLimAng = softnessLimAng; } + void setRestitutionLimAng(btScalar restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; } + void setDampingLimAng(btScalar dampingLimAng) { m_dampingLimAng = dampingLimAng; } + void setSoftnessOrthoLin(btScalar softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; } + void setRestitutionOrthoLin(btScalar restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; } + void setDampingOrthoLin(btScalar dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; } + void setSoftnessOrthoAng(btScalar softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; } + void setRestitutionOrthoAng(btScalar restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; } + void setDampingOrthoAng(btScalar dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; } + void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; } + bool getPoweredLinMotor() { return m_poweredLinMotor; } + void setTargetLinMotorVelocity(btScalar targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; } + btScalar getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; } + void setMaxLinMotorForce(btScalar maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; } + btScalar getMaxLinMotorForce() { return m_maxLinMotorForce; } + void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; } + bool getPoweredAngMotor() { return m_poweredAngMotor; } + void setTargetAngMotorVelocity(btScalar targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; } + btScalar getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; } + void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; } + btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; } + + // access for ODE solver + bool getSolveLinLimit() { return m_solveLinLim; } + btScalar getLinDepth() { return m_depth[0]; } + bool getSolveAngLimit() { return m_solveAngLim; } + btScalar getAngDepth() { return m_angDepth; } + // internal + void buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB); + void solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB); + // shared code used by ODE solver + void calculateTransforms(void); + void testLinLimits(void); + void testAngLimits(void); + // access for PE Solver + btVector3 getAncorInA(void); + btVector3 getAncorInB(void); +}; + +//----------------------------------------------------------------------------- + +#endif //SLIDER_CONSTRAINT_H + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h index e7d26645c6a..057d3fac827 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; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h index 0ab536f42b3..b3f0c9d7444 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h @@ -19,38 +19,78 @@ subject to the following restrictions: class btRigidBody; #include "LinearMath/btVector3.h" #include "LinearMath/btMatrix3x3.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btAlignedAllocator.h" +#include "LinearMath/btTransformUtil.h" - - +///btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. ATTRIBUTE_ALIGNED16 (struct) btSolverBody { - btVector3 m_centerOfMassPosition; - btVector3 m_linearVelocity; + BT_DECLARE_ALIGNED_ALLOCATOR(); + btVector3 m_angularVelocity; - btRigidBody* m_originalBody; + float m_angularFactor; float m_invMass; float m_friction; - float m_angularFactor; - - inline void getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const + btRigidBody* m_originalBody; + btVector3 m_linearVelocity; + btVector3 m_centerOfMassPosition; + + btVector3 m_pushVelocity; + btVector3 m_turnVelocity; + + + SIMD_FORCE_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) + SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) + { + if (m_invMass) + { + m_linearVelocity += linearComponent*impulseMagnitude; + m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + } + + SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) { - m_linearVelocity += linearComponent*impulseMagnitude; - m_angularVelocity += angularComponent*impulseMagnitude*m_angularFactor; + if (m_invMass) + { + m_pushVelocity += linearComponent*impulseMagnitude; + m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } } + void writebackVelocity() { if (m_invMass) { m_originalBody->setLinearVelocity(m_linearVelocity); m_originalBody->setAngularVelocity(m_angularVelocity); + + //m_originalBody->setCompanionId(-1); + } + } + + + void writebackVelocity(btScalar timeStep) + { + if (m_invMass) + { + m_originalBody->setLinearVelocity(m_linearVelocity); + m_originalBody->setAngularVelocity(m_angularVelocity); + + //correct the position/orientation based on push/turn recovery + btTransform newTransform; + btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform); + m_originalBody->setWorldTransform(newTransform); + + //m_originalBody->setCompanionId(-1); } } @@ -69,3 +109,5 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody }; #endif //BT_SOLVER_BODY_H + + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h index f1f40ffdf19..2c71360c5b9 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h @@ -1,5 +1,3 @@ - - /* Bullet Continuous Collision Detection and Physics Library Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ @@ -27,24 +25,34 @@ class btRigidBody; ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint { + BT_DECLARE_ALIGNED_ALLOCATOR(); + btVector3 m_relpos1CrossNormal; - btVector3 m_relpos2CrossNormal; btVector3 m_contactNormal; + + btVector3 m_relpos2CrossNormal; btVector3 m_angularComponentA; + btVector3 m_angularComponentB; - btScalar m_appliedVelocityImpulse; + mutable btScalar m_appliedPushImpulse; + + mutable btScalar m_appliedImpulse; 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]; + void* m_originalContactPoint; + int m_unusedPadding[1]; + enum btSolverConstraintType { @@ -61,3 +69,4 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint #endif //BT_SOLVER_CONSTRAINT_H + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index a15b3e026cd..6e8b552dbbc 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -19,18 +19,20 @@ subject to the following restrictions: static btRigidBody s_fixed(0, 0,0); -btTypedConstraint::btTypedConstraint() -: m_userConstraintType(-1), +btTypedConstraint::btTypedConstraint(btTypedConstraintType type) +:m_userConstraintType(-1), m_userConstraintId(-1), +m_constraintType (type), m_rbA(s_fixed), m_rbB(s_fixed), m_appliedImpulse(btScalar(0.)) { s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); } -btTypedConstraint::btTypedConstraint(btRigidBody& rbA) -: m_userConstraintType(-1), +btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA) +:m_userConstraintType(-1), m_userConstraintId(-1), +m_constraintType (type), m_rbA(rbA), m_rbB(s_fixed), m_appliedImpulse(btScalar(0.)) @@ -40,9 +42,10 @@ m_appliedImpulse(btScalar(0.)) } -btTypedConstraint::btTypedConstraint(btRigidBody& rbA,btRigidBody& rbB) -: m_userConstraintType(-1), +btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB) +:m_userConstraintType(-1), m_userConstraintId(-1), +m_constraintType (type), m_rbA(rbA), m_rbB(rbB), m_appliedImpulse(btScalar(0.)) diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index dfee6e80d0e..c50ec6ec579 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -17,7 +17,17 @@ subject to the following restrictions: #define TYPED_CONSTRAINT_H class btRigidBody; -#include "../../LinearMath/btScalar.h" +#include "LinearMath/btScalar.h" + +enum btTypedConstraintType +{ + POINT2POINT_CONSTRAINT_TYPE, + HINGE_CONSTRAINT_TYPE, + CONETWIST_CONSTRAINT_TYPE, + D6_CONSTRAINT_TYPE, + VEHICLE_CONSTRAINT_TYPE, + SLIDER_CONSTRAINT_TYPE +}; ///TypedConstraint is the baseclass for Bullet constraints and vehicles class btTypedConstraint @@ -25,6 +35,8 @@ class btTypedConstraint int m_userConstraintType; int m_userConstraintId; + btTypedConstraintType m_constraintType; + btTypedConstraint& operator=(btTypedConstraint& other) { btAssert(0); @@ -40,11 +52,11 @@ protected: public: - btTypedConstraint(); + btTypedConstraint(btTypedConstraintType type); virtual ~btTypedConstraint() {}; - btTypedConstraint(btRigidBody& rbA); + btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA); - btTypedConstraint(btRigidBody& rbA,btRigidBody& rbB); + btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB); virtual void buildJacobian() = 0; @@ -59,7 +71,7 @@ public: return m_rbB; } - btRigidBody& getRigidBodyA() + btRigidBody& getRigidBodyA() { return m_rbA; } @@ -83,14 +95,26 @@ public: m_userConstraintId = uid; } - int getUserConstraintId() + int getUserConstraintId() const { return m_userConstraintId; } - btScalar getAppliedImpulse() + + int getUid() const + { + return m_userConstraintId; + } + + btScalar getAppliedImpulse() const { return m_appliedImpulse; } + + btTypedConstraintType getConstraintType () const + { + return m_constraintType; + } + }; #endif //TYPED_CONSTRAINT_H |