diff options
author | Sergej Reich <sergej.reich@googlemail.com> | 2012-06-08 20:13:01 +0400 |
---|---|---|
committer | Sergej Reich <sergej.reich@googlemail.com> | 2012-06-08 20:13:01 +0400 |
commit | 82d3d9f2ba47bbf2f868b5a970d1fe149eba13e2 (patch) | |
tree | e99d947080f8e787059d86500f01dd6c1ad616e4 /extern/bullet2/src/BulletDynamics/ConstraintSolver | |
parent | 221a7878223e983372ba830e4ca1a17067abf2ba (diff) |
Update Bullet to version 2.80 (bullet svn revision 2537)
Remove Jamfiles and other unused files that stuck around during previous updates.
Add patches for local changes to the patches directory.
Update readme.txt, it had outdated infromation.
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/ConstraintSolver')
24 files changed, 332 insertions, 144 deletions
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index bc371e4062d..755544f0dee 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -638,6 +638,11 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr btTransform trDeltaAB = trB * trPose * trA.inverse(); btQuaternion qDeltaAB = trDeltaAB.getRotation(); btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z()); + float swingAxisLen2 = swingAxis.length2(); + if(btFuzzyZero(swingAxisLen2)) + { + return; + } m_swingAxis = swingAxis; m_swingAxis.normalize(); m_swingCorrection = qDeltaAB.getAngle(); @@ -918,7 +923,7 @@ void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist, if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate. { - qMinTwist = operator-(qTwist); + qMinTwist = -(qTwist); twistAngle = qMinTwist.getAngle(); } if (twistAngle < 0) diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h index c1c5aa851b6..868e62f063e 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -33,8 +33,8 @@ and swing 1 and 2 are along the z and y axes respectively. -#ifndef CONETWISTCONSTRAINT_H -#define CONETWISTCONSTRAINT_H +#ifndef BT_CONETWISTCONSTRAINT_H +#define BT_CONETWISTCONSTRAINT_H #include "LinearMath/btVector3.h" #include "btJacobianEntry.h" @@ -343,4 +343,4 @@ SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer, } -#endif //CONETWISTCONSTRAINT_H +#endif //BT_CONETWISTCONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h index 7a8e9c1953d..6f673102be2 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CONSTRAINT_SOLVER_H -#define CONSTRAINT_SOLVER_H +#ifndef BT_CONSTRAINT_SOLVER_H +#define BT_CONSTRAINT_SOLVER_H #include "LinearMath/btScalar.h" @@ -49,4 +49,4 @@ public: -#endif //CONSTRAINT_SOLVER_H +#endif //BT_CONSTRAINT_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp index d97096d9f26..88859182925 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @@ -68,9 +68,53 @@ void btContactConstraint::buildJacobian() #include "LinearMath/btMinMax.h" #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" -#define ASSERT2 btAssert -#define USE_INTERNAL_APPLY_IMPULSE 1 + +//response between two dynamic objects without friction, assuming 0 penetration depth +btScalar resolveSingleCollision( + btRigidBody* body1, + btCollisionObject* colObj2, + const btVector3& contactPositionWorld, + const btVector3& contactNormalOnB, + const btContactSolverInfo& solverInfo, + btScalar distance) +{ + btRigidBody* body2 = btRigidBody::upcast(colObj2); + + + const btVector3& normal = contactNormalOnB; + + btVector3 rel_pos1 = contactPositionWorld - body1->getWorldTransform().getOrigin(); + btVector3 rel_pos2 = contactPositionWorld - colObj2->getWorldTransform().getOrigin(); + + btVector3 vel1 = body1->getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = body2? body2->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); + btVector3 vel = vel1 - vel2; + btScalar rel_vel; + rel_vel = normal.dot(vel); + + btScalar combinedRestitution = body1->getRestitution() * colObj2->getRestitution(); + btScalar restitution = combinedRestitution* -rel_vel; + + btScalar positionalError = solverInfo.m_erp *-distance /solverInfo.m_timeStep ; + btScalar velocityError = -(1.0f + restitution) * rel_vel;// * damping; + btScalar denom0 = body1->computeImpulseDenominator(contactPositionWorld,normal); + btScalar denom1 = body2? body2->computeImpulseDenominator(contactPositionWorld,normal) : 0.f; + btScalar relaxation = 1.f; + btScalar jacDiagABInv = relaxation/(denom0+denom1); + + btScalar penetrationImpulse = positionalError * jacDiagABInv; + btScalar velocityImpulse = velocityError * jacDiagABInv; + + btScalar normalImpulse = penetrationImpulse+velocityImpulse; + normalImpulse = 0.f > normalImpulse ? 0.f: normalImpulse; + + body1->applyImpulse(normal*(normalImpulse), rel_pos1); + if (body2) + body2->applyImpulse(-normal*(normalImpulse), rel_pos2); + + return normalImpulse; +} //bilateral constraint between two dynamic objects @@ -83,7 +127,7 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, btScalar normalLenSqr = normal.length2(); - ASSERT2(btFabs(normalLenSqr) < btScalar(1.1)); + btAssert(btFabs(normalLenSqr) < btScalar(1.1)); if (normalLenSqr > btScalar(1.1)) { impulse = btScalar(0.); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h index 63c1a417bc1..477c79d1756 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CONTACT_CONSTRAINT_H -#define CONTACT_CONSTRAINT_H +#ifndef BT_CONTACT_CONSTRAINT_H +#define BT_CONTACT_CONSTRAINT_H #include "LinearMath/btVector3.h" #include "btJacobianEntry.h" @@ -57,6 +57,9 @@ public: }; +///very basic collision resolution without friction +btScalar resolveSingleCollision(btRigidBody* body1, class btCollisionObject* colObj2, const btVector3& contactPositionWorld,const btVector3& contactNormalOnB, const struct btContactSolverInfo& solverInfo,btScalar distance); + ///resolveSingleBilateral is an obsolete methods used for vehicle friction between two dynamic objects void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, @@ -65,4 +68,4 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, -#endif //CONTACT_CONSTRAINT_H +#endif //BT_CONTACT_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index db5bb5e4c51..6204cb3d16c 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CONTACT_SOLVER_INFO -#define CONTACT_SOLVER_INFO +#ifndef BT_CONTACT_SOLVER_INFO +#define BT_CONTACT_SOLVER_INFO enum btSolverMode { @@ -84,4 +84,4 @@ struct btContactSolverInfo : public btContactSolverInfoData } }; -#endif //CONTACT_SOLVER_INFO +#endif //BT_CONTACT_SOLVER_INFO diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index 7c5e4f6e7b2..8ff9940bba3 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -130,12 +130,20 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value) { m_currentLimit = 1;//low limit violation m_currentLimitError = test_value - m_loLimit; + if(m_currentLimitError>SIMD_PI) + m_currentLimitError-=SIMD_2_PI; + else if(m_currentLimitError<-SIMD_PI) + m_currentLimitError+=SIMD_2_PI; return 1; } else if (test_value> m_hiLimit) { m_currentLimit = 2;//High limit violation m_currentLimitError = test_value - m_hiLimit; + if(m_currentLimitError>SIMD_PI) + m_currentLimitError-=SIMD_2_PI; + else if(m_currentLimitError<-SIMD_PI) + m_currentLimitError+=SIMD_2_PI; return 2; }; @@ -1067,4 +1075,4 @@ void btGeneric6DofConstraint::setAxis(const btVector3& axis1,const btVector3& ax m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW; calculateTransforms(); -}
\ No newline at end of file +} diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index ab745ca5ad6..a8e7bc22902 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -24,8 +24,8 @@ http://gimpact.sf.net */ -#ifndef GENERIC_6DOF_CONSTRAINT_H -#define GENERIC_6DOF_CONSTRAINT_H +#ifndef BT_GENERIC_6DOF_CONSTRAINT_H +#define BT_GENERIC_6DOF_CONSTRAINT_H #include "LinearMath/btVector3.h" #include "btJacobianEntry.h" @@ -611,4 +611,4 @@ SIMD_FORCE_INLINE const char* btGeneric6DofConstraint::serialize(void* dataBuffe -#endif //GENERIC_6DOF_CONSTRAINT_H +#endif //BT_GENERIC_6DOF_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp index d3503456625..2b38714987b 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp @@ -21,6 +21,19 @@ subject to the following restrictions: btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA) : btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA) { + init(); +} + + +btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB) + : btGeneric6DofConstraint(rbB, frameInB, useLinearReferenceFrameB) +{ + init(); +} + + +void btGeneric6DofSpringConstraint::init() +{ m_objectType = D6_SPRING_CONSTRAINT_TYPE; for(int i = 0; i < 6; i++) diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h index 16ff973e427..31e0cd531ae 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef GENERIC_6DOF_SPRING_CONSTRAINT_H -#define GENERIC_6DOF_SPRING_CONSTRAINT_H +#ifndef BT_GENERIC_6DOF_SPRING_CONSTRAINT_H +#define BT_GENERIC_6DOF_SPRING_CONSTRAINT_H #include "LinearMath/btVector3.h" @@ -39,9 +39,11 @@ protected: btScalar m_equilibriumPoint[6]; btScalar m_springStiffness[6]; btScalar m_springDamping[6]; // between 0 and 1 (1 == no damping) + void init(); void internalUpdateSprings(btConstraintInfo2* info); public: btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); + btGeneric6DofSpringConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB); void enableSpring(int index, bool onOff); void setStiffness(int index, btScalar stiffness); void setDamping(int index, btScalar damping); @@ -93,5 +95,5 @@ SIMD_FORCE_INLINE const char* btGeneric6DofSpringConstraint::serialize(void* dat return "btGeneric6DofConstraintData"; } -#endif // GENERIC_6DOF_SPRING_CONSTRAINT_H +#endif // BT_GENERIC_6DOF_SPRING_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h index 15fd4a014cc..a76452ddb64 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef HINGE2_CONSTRAINT_H -#define HINGE2_CONSTRAINT_H +#ifndef BT_HINGE2_CONSTRAINT_H +#define BT_HINGE2_CONSTRAINT_H @@ -54,5 +54,5 @@ public: -#endif // HINGE2_CONSTRAINT_H +#endif // BT_HINGE2_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index 144beef1cd6..9e3a2baeed9 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -37,15 +37,15 @@ subject to the following restrictions: btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), +#ifdef _BT_USE_CENTER_LIMIT_ + m_limit(), +#endif m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), m_flags(0) -#ifdef _BT_USE_CENTER_LIMIT_ - ,m_limit() -#endif { m_rbAFrame.getOrigin() = pivotInA; @@ -93,14 +93,15 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA) -:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), +:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), +#ifdef _BT_USE_CENTER_LIMIT_ +m_limit(), +#endif +m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), m_flags(0) -#ifdef _BT_USE_CENTER_LIMIT_ -,m_limit() -#endif { // since no frame is given, assume this to be zero angle and just pick rb transform axis @@ -142,15 +143,15 @@ m_flags(0) btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), +#ifdef _BT_USE_CENTER_LIMIT_ +m_limit(), +#endif m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), m_flags(0) -#ifdef _BT_USE_CENTER_LIMIT_ -,m_limit() -#endif { #ifndef _BT_USE_CENTER_LIMIT_ //start with free @@ -168,15 +169,15 @@ m_flags(0) btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame), +#ifdef _BT_USE_CENTER_LIMIT_ +m_limit(), +#endif m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), m_flags(0) -#ifdef _BT_USE_CENTER_LIMIT_ -,m_limit() -#endif { ///not providing rigidbody B means implicitly using worldspace for body B @@ -663,7 +664,7 @@ void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt) btScalar targetAngle = qHinge.getAngle(); if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate. { - qHinge = operator-(qHinge); + qHinge = -(qHinge); targetAngle = qHinge.getAngle(); } if (qHinge.getZ() < 0) diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 50e3f73cb6d..cb2973e1d1d 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -15,8 +15,9 @@ subject to the following restrictions: /* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */ -#ifndef HINGECONSTRAINT_H -#define HINGECONSTRAINT_H +#ifndef BT_HINGECONSTRAINT_H +#define BT_HINGECONSTRAINT_H + #define _BT_USE_CENTER_LIMIT_ 1 @@ -377,4 +378,4 @@ SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btS return btHingeConstraintDataName; } -#endif //HINGECONSTRAINT_H +#endif //BT_HINGECONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h index 22a8af66b8e..f1994a2dfd8 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef JACOBIAN_ENTRY_H -#define JACOBIAN_ENTRY_H +#ifndef BT_JACOBIAN_ENTRY_H +#define BT_JACOBIAN_ENTRY_H #include "LinearMath/btVector3.h" #include "BulletDynamics/Dynamics/btRigidBody.h" @@ -153,4 +153,4 @@ public: }; -#endif //JACOBIAN_ENTRY_H +#endif //BT_JACOBIAN_ENTRY_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h index b589ee68254..b3bda03eec1 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef POINT2POINTCONSTRAINT_H -#define POINT2POINTCONSTRAINT_H +#ifndef BT_POINT2POINTCONSTRAINT_H +#define BT_POINT2POINTCONSTRAINT_H #include "LinearMath/btVector3.h" #include "btJacobianEntry.h" @@ -158,4 +158,4 @@ SIMD_FORCE_INLINE const char* btPoint2PointConstraint::serialize(void* dataBuffe return btPoint2PointConstraintDataName; } -#endif //POINT2POINTCONSTRAINT_H +#endif //BT_POINT2POINTCONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 0fc93cd3756..ab074224028 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -225,7 +225,7 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); - c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); + c.m_appliedPushImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); __m128 impulseMagnitude = deltaImpulse; @@ -740,13 +740,13 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m } } - 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; + m_maxOverrideNumSolverIterations = 0; if (!(numConstraints + numManifolds)) { @@ -788,6 +788,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol { btTypedConstraint* constraint = constraints[j]; constraint->buildJacobian(); + constraint->internalSetAppliedImpulse(0.0f); } } //btRigidBody* rb0=0,*rb1=0; @@ -804,7 +805,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol for (i=0;i<numConstraints;i++) { btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; - constraints[i]->getInfo1(&info1); + if (constraints[i]->isEnabled()) + { + constraints[i]->getInfo1(&info1); + } else + { + info1.m_numConstraintRows = 0; + info1.nub = 0; + } totalNumRows += info1.m_numConstraintRows; } m_tmpSolverNonContactConstraintPool.resize(totalNumRows); @@ -823,23 +831,26 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow]; btTypedConstraint* constraint = constraints[i]; + btRigidBody& rbA = constraint->getRigidBodyA(); + btRigidBody& rbB = constraint->getRigidBodyB(); + int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; + if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations) + m_maxOverrideNumSolverIterations = overrideNumSolverIterations; - btRigidBody& rbA = constraint->getRigidBodyA(); - btRigidBody& rbB = constraint->getRigidBodyB(); - int j; for ( j=0;j<info1.m_numConstraintRows;j++) { memset(¤tConstraintRow[j],0,sizeof(btSolverConstraint)); - currentConstraintRow[j].m_lowerLimit = -FLT_MAX; - currentConstraintRow[j].m_upperLimit = FLT_MAX; + currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY; + currentConstraintRow[j].m_upperLimit = SIMD_INFINITY; currentConstraintRow[j].m_appliedImpulse = 0.f; currentConstraintRow[j].m_appliedPushImpulse = 0.f; currentConstraintRow[j].m_solverBodyA = &rbA; currentConstraintRow[j].m_solverBodyB = &rbB; + currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations; } rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); @@ -872,6 +883,17 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol for ( j=0;j<info1.m_numConstraintRows;j++) { btSolverConstraint& solverConstraint = currentConstraintRow[j]; + + if (solverConstraint.m_upperLimit>=constraints[i]->getBreakingImpulseThreshold()) + { + solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold(); + } + + if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold()) + { + solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold(); + } + solverConstraint.m_originalContactPoint = constraint; { @@ -939,15 +961,20 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btContactSolverInfo info = infoGlobal; - + int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); int numConstraintPool = m_tmpSolverContactConstraintPool.size(); int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints + m_orderNonContactConstraintPool.resize(numNonContactPool); m_orderTmpConstraintPool.resize(numConstraintPool); m_orderFrictionConstraintPool.resize(numFrictionPool); { int i; + for (i=0;i<numNonContactPool;i++) + { + m_orderNonContactConstraintPool[i] = i; + } for (i=0;i<numConstraintPool;i++) { m_orderTmpConstraintPool[i] = i; @@ -965,6 +992,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/) { + int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); int numConstraintPool = m_tmpSolverContactConstraintPool.size(); int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); @@ -973,18 +1001,29 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) { if ((iteration & 7) == 0) { - for (j=0; j<numConstraintPool; ++j) { - int tmp = m_orderTmpConstraintPool[j]; + for (j=0; j<numNonContactPool; ++j) { + int tmp = m_orderNonContactConstraintPool[j]; int swapi = btRandInt2(j+1); - m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; - m_orderTmpConstraintPool[swapi] = tmp; + m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi]; + m_orderNonContactConstraintPool[swapi] = tmp; } - for (j=0; j<numFrictionPool; ++j) { - int tmp = m_orderFrictionConstraintPool[j]; - int swapi = btRandInt2(j+1); - m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; - m_orderFrictionConstraintPool[swapi] = tmp; + //contact/friction constraints are not solved more than + if (iteration< infoGlobal.m_numIterations) + { + for (j=0; j<numConstraintPool; ++j) { + int tmp = m_orderTmpConstraintPool[j]; + int swapi = btRandInt2(j+1); + m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; + m_orderTmpConstraintPool[swapi] = tmp; + } + + for (j=0; j<numFrictionPool; ++j) { + int tmp = m_orderFrictionConstraintPool[j]; + int swapi = btRandInt2(j+1); + m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; + m_orderFrictionConstraintPool[swapi] = tmp; + } } } } @@ -994,36 +1033,41 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration ///solve all joint constraints, using SIMD, if available for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) { - btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; - resolveSingleConstraintRowGenericSIMD(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint); + btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; + if (iteration < constraint.m_overrideNumSolverIterations) + resolveSingleConstraintRowGenericSIMD(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint); } - for (j=0;j<numConstraints;j++) + if (iteration< infoGlobal.m_numIterations) { - constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); - } - - ///solve all contact constraints using SIMD, if available - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - for (j=0;j<numPoolConstraints;j++) - { - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - resolveSingleConstraintRowLowerLimitSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); + for (j=0;j<numConstraints;j++) + { + constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); + } - } - ///solve all friction constraints, using SIMD, if available - int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); - for (j=0;j<numFrictionPoolConstraints;j++) - { - btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; - btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + ///solve all contact constraints using SIMD, if available + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + for (j=0;j<numPoolConstraints;j++) + { + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; + resolveSingleConstraintRowLowerLimitSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); - if (totalImpulse>btScalar(0)) + } + + ///solve all friction constraints, using SIMD, if available + int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); + for (j=0;j<numFrictionPoolConstraints;j++) { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; + btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + + if (totalImpulse>btScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - resolveSingleConstraintRowGenericSIMD(*solveManifold.m_solverBodyA, *solveManifold.m_solverBodyB,solveManifold); + resolveSingleConstraintRowGenericSIMD(*solveManifold.m_solverBodyA, *solveManifold.m_solverBodyB,solveManifold); + } } } } else @@ -1032,34 +1076,38 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration ///solve all joint constraints for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) { - btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; - resolveSingleConstraintRowGeneric(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint); + btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; + if (iteration < constraint.m_overrideNumSolverIterations) + resolveSingleConstraintRowGeneric(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint); } - for (j=0;j<numConstraints;j++) - { - constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); - } - ///solve all contact constraints - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - for (j=0;j<numPoolConstraints;j++) + if (iteration< infoGlobal.m_numIterations) { - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - resolveSingleConstraintRowLowerLimit(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); - } - ///solve all friction constraints - int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); - for (j=0;j<numFrictionPoolConstraints;j++) - { - btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; - btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - - if (totalImpulse>btScalar(0)) + for (j=0;j<numConstraints;j++) { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); + } + ///solve all contact constraints + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + for (j=0;j<numPoolConstraints;j++) + { + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; + resolveSingleConstraintRowLowerLimit(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); + } + ///solve all friction constraints + int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); + for (j=0;j<numFrictionPoolConstraints;j++) + { + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; + btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - resolveSingleConstraintRowGeneric(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); + if (totalImpulse>btScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + + resolveSingleConstraintRowGeneric(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); + } } } } @@ -1111,16 +1159,18 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations( { BT_PROFILE("solveGroupCacheFriendlyIterations"); - - //should traverse the contacts random order... - int iteration; { - for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) + ///this is a special step to resolve penetrations (just for contacts) + solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); + + int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; + + for ( int iteration = 0 ; iteration< maxIterations ; iteration++) + //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--) { solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); } - solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); } return 0.f; } @@ -1151,9 +1201,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo { const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j]; btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint; - btScalar sum = constr->internalGetAppliedImpulse(); - sum += solverConstr.m_appliedImpulse; - constr->internalSetAppliedImpulse(sum); + constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse); + if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold()) + { + constr->setEnabled(false); + } } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index f268ec9ae74..bb377db8db9 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H -#define SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H +#ifndef BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H +#define BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H #include "btConstraintSolver.h" class btIDebugDraw; @@ -33,8 +33,10 @@ protected: btConstraintArray m_tmpSolverNonContactConstraintPool; btConstraintArray m_tmpSolverContactFrictionConstraintPool; btAlignedObjectArray<int> m_orderTmpConstraintPool; + btAlignedObjectArray<int> m_orderNonContactConstraintPool; btAlignedObjectArray<int> m_orderFrictionConstraintPool; btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool; + int m_maxOverrideNumSolverIterations; void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyIdB, btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, @@ -124,5 +126,5 @@ typedef btSequentialImpulseConstraintSolver btSequentialImpulseConstraintSolverP #endif -#endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H +#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h index e9723a7bb40..2edc8d2b2e1 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -22,8 +22,8 @@ TODO: - add conversion for ODE constraint solver */ -#ifndef SLIDER_CONSTRAINT_H -#define SLIDER_CONSTRAINT_H +#ifndef BT_SLIDER_CONSTRAINT_H +#define BT_SLIDER_CONSTRAINT_H @@ -236,7 +236,10 @@ public: btScalar getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; } void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; } btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; } - btScalar getLinearPos() { return m_linPos; } + + btScalar getLinearPos() const { return m_linPos; } + btScalar getAngularPos() const { return m_angPos; } + // access for ODE solver @@ -326,5 +329,5 @@ SIMD_FORCE_INLINE const char* btSliderConstraint::serialize(void* dataBuffer, bt -#endif //SLIDER_CONSTRAINT_H +#endif //BT_SLIDER_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h index 057d3fac827..e8bfabf864e 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef SOLVE_2LINEAR_CONSTRAINT_H -#define SOLVE_2LINEAR_CONSTRAINT_H +#ifndef BT_SOLVE_2LINEAR_CONSTRAINT_H +#define BT_SOLVE_2LINEAR_CONSTRAINT_H #include "LinearMath/btMatrix3x3.h" #include "LinearMath/btVector3.h" @@ -104,4 +104,4 @@ public: }; -#endif //SOLVE_2LINEAR_CONSTRAINT_H +#endif //BT_SOLVE_2LINEAR_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h index 79e45a4383b..179e79d7911 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h @@ -51,6 +51,8 @@ ATTRIBUTE_ALIGNED64 (struct) btSolverConstraint btScalar m_unusedPadding0; }; + int m_overrideNumSolverIterations; + union { int m_frictionIndex; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index 8212c36ff4e..06bde5e7eec 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -25,7 +25,10 @@ btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rb :btTypedObject(type), m_userConstraintType(-1), m_userConstraintId(-1), +m_breakingImpulseThreshold(SIMD_INFINITY), +m_isEnabled(true), m_needsFeedback(false), +m_overrideNumSolverIterations(-1), m_rbA(rbA), m_rbB(getFixedBody()), m_appliedImpulse(btScalar(0.)), @@ -38,7 +41,10 @@ btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rb :btTypedObject(type), m_userConstraintType(-1), m_userConstraintId(-1), +m_breakingImpulseThreshold(SIMD_INFINITY), +m_isEnabled(true), m_needsFeedback(false), +m_overrideNumSolverIterations(-1), m_rbA(rbA), m_rbB(rbB), m_appliedImpulse(btScalar(0.)), @@ -114,6 +120,10 @@ const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali tcd->m_objectType = m_objectType; tcd->m_needsFeedback = m_needsFeedback; + tcd->m_overrideNumSolverIterations = m_overrideNumSolverIterations; + tcd->m_breakingImpulseThreshold = float(m_breakingImpulseThreshold); + tcd->m_isEnabled = m_isEnabled? 1: 0; + tcd->m_userConstraintId =m_userConstraintId; tcd->m_userConstraintType =m_userConstraintType; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index 65e6d558300..a16e869a973 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef TYPED_CONSTRAINT_H -#define TYPED_CONSTRAINT_H +#ifndef BT_TYPED_CONSTRAINT_H +#define BT_TYPED_CONSTRAINT_H class btRigidBody; #include "LinearMath/btScalar.h" @@ -62,7 +62,11 @@ class btTypedConstraint : public btTypedObject void* m_userConstraintPtr; }; - bool m_needsFeedback; + btScalar m_breakingImpulseThreshold; + bool m_isEnabled; + bool m_needsFeedback; + int m_overrideNumSolverIterations; + btTypedConstraint& operator=(btTypedConstraint& other) { @@ -80,7 +84,6 @@ protected: ///internal method used by the constraint solver, don't use them directly btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact); - static btRigidBody& getFixedBody(); public: @@ -92,6 +95,8 @@ public: int m_numConstraintRows,nub; }; + static btRigidBody& getFixedBody(); + struct btConstraintInfo2 { // integrator parameters: frames per second (1/stepsize), default error // reduction parameter (0..1). @@ -126,6 +131,18 @@ public: btScalar m_damping; }; + int getOverrideNumSolverIterations() const + { + return m_overrideNumSolverIterations; + } + + ///override the number of constraint solver iterations used to solve this constraint + ///-1 will use the default number of iterations, as specified in SolverInfo.m_numIterations + void setOverrideNumSolverIterations(int overideNumIterations) + { + m_overrideNumSolverIterations = overideNumIterations; + } + ///internal method used by the constraint solver, don't use them directly virtual void buildJacobian() {}; @@ -155,6 +172,28 @@ public: return m_appliedImpulse; } + + btScalar getBreakingImpulseThreshold() const + { + return m_breakingImpulseThreshold; + } + + void setBreakingImpulseThreshold(btScalar threshold) + { + m_breakingImpulseThreshold = threshold; + } + + bool isEnabled() const + { + return m_isEnabled; + } + + void setEnabled(bool enabled) + { + m_isEnabled=enabled; + } + + ///internal method used by the constraint solver, don't use them directly virtual void solveConstraintObsolete(btRigidBody& /*bodyA*/,btRigidBody& /*bodyB*/,btScalar /*timeStep*/) {}; @@ -302,7 +341,10 @@ struct btTypedConstraintData float m_dbgDrawSize; int m_disableCollisionsBetweenLinkedBodies; - char m_pad4[4]; + int m_overrideNumSolverIterations; + + float m_breakingImpulseThreshold; + int m_isEnabled; }; @@ -407,4 +449,4 @@ public: -#endif //TYPED_CONSTRAINT_H +#endif //BT_TYPED_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp index 40c56e77954..b009f41aec8 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp @@ -27,7 +27,7 @@ subject to the following restrictions: // constructor // anchor, axis1 and axis2 are in world coordinate system // axis1 must be orthogonal to axis2 -btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2) +btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2) : btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true), m_anchor(anchor), m_axis1(axis1), @@ -42,8 +42,8 @@ btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, // new position of X, allowed limits are (-PI,PI); // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs // Build the frame in world coordinate system first - btVector3 zAxis = axis1.normalize(); - btVector3 yAxis = axis2.normalize(); + btVector3 zAxis = m_axis1.normalize(); + btVector3 yAxis = m_axis2.normalize(); btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system btTransform frameInW; frameInW.setIdentity(); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h index a8d404a591c..a86939164ec 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef UNIVERSAL_CONSTRAINT_H -#define UNIVERSAL_CONSTRAINT_H +#ifndef BT_UNIVERSAL_CONSTRAINT_H +#define BT_UNIVERSAL_CONSTRAINT_H @@ -41,7 +41,7 @@ public: // constructor // anchor, axis1 and axis2 are in world coordinate system // axis1 must be orthogonal to axis2 - btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2); + btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2); // access const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); } const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); } @@ -58,5 +58,5 @@ public: -#endif // UNIVERSAL_CONSTRAINT_H +#endif // BT_UNIVERSAL_CONSTRAINT_H |