diff options
author | Brecht Van Lommel <brechtvanlommel@pandora.be> | 2009-06-09 00:08:19 +0400 |
---|---|---|
committer | Brecht Van Lommel <brechtvanlommel@pandora.be> | 2009-06-09 00:08:19 +0400 |
commit | c8b4cf92067ffeb625aa39003baf5d8f7c3f0025 (patch) | |
tree | c6c50dbc3d90a65fca6c1ca56a93e4a57cf7e154 /extern/bullet2 | |
parent | e93db433a086a3e739c0f4026cd500f0b595b0f1 (diff) | |
parent | d76a6f5231c015c35123d22e1f5c3ffcdfbf9bbd (diff) |
2.50:
svn merge https://svn.blender.org/svnroot/bf-blender/trunk/blender -r19820:HEAD
Notes:
* Game and sequencer RNA, and sequencer header are now out of date
a bit after changes in trunk.
* I didn't know how to port these bugfixes, most likely they are
not needed anymore.
* Fix "duplicate strip" always increase the user count for ipo.
* IPO pinning on sequencer strips was lost during Undo.
Diffstat (limited to 'extern/bullet2')
10 files changed, 359 insertions, 105 deletions
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index 29c8496c36f..50a79451f5d 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -22,12 +22,13 @@ Written by: Marcus Hennix #include "LinearMath/btMinMax.h" #include <new> -//----------------------------------------------------------------------------- + +//#define CONETWIST_USE_OBSOLETE_SOLVER true #define CONETWIST_USE_OBSOLETE_SOLVER false #define CONETWIST_DEF_FIX_THRESH btScalar(.05f) -//----------------------------------------------------------------------------- + btConeTwistConstraint::btConeTwistConstraint() :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE), @@ -63,13 +64,13 @@ void btConeTwistConstraint::init() m_bMotorEnabled = false; m_maxMotorImpulse = btScalar(-1); - setLimit(btScalar(1e30), btScalar(1e30), btScalar(1e30)); + setLimit(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); m_damping = btScalar(0.01); m_fixThresh = CONETWIST_DEF_FIX_THRESH; } -//----------------------------------------------------------------------------- + void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info) { @@ -99,9 +100,9 @@ void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info) info->nub--; } } -} // btConeTwistConstraint::getInfo1() +} -//----------------------------------------------------------------------------- + void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info) { @@ -230,7 +231,7 @@ void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info) } } -//----------------------------------------------------------------------------- + void btConeTwistConstraint::buildJacobian() { @@ -239,6 +240,7 @@ void btConeTwistConstraint::buildJacobian() m_appliedImpulse = btScalar(0.); m_accTwistLimitImpulse = btScalar(0.); m_accSwingLimitImpulse = btScalar(0.); + m_accMotorImpulse = btVector3(0.,0.,0.); if (!m_angularOnly) { @@ -277,7 +279,7 @@ void btConeTwistConstraint::buildJacobian() } } -//----------------------------------------------------------------------------- + void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) { @@ -406,10 +408,10 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver } } - else // no motor: do a little damping + else if (m_damping > SIMD_EPSILON) // no motor: do a little damping { - const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); - const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); + btVector3 angVelA; bodyA.getAngularVelocity(angVelA); + btVector3 angVelB; bodyB.getAngularVelocity(angVelB); btVector3 relVel = angVelB - angVelA; if (relVel.length2() > SIMD_EPSILON) { @@ -490,7 +492,7 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver } -//----------------------------------------------------------------------------- + void btConeTwistConstraint::updateRHS(btScalar timeStep) { @@ -498,7 +500,7 @@ void btConeTwistConstraint::updateRHS(btScalar timeStep) } -//----------------------------------------------------------------------------- + void btConeTwistConstraint::calcAngleInfo() { @@ -584,12 +586,12 @@ void btConeTwistConstraint::calcAngleInfo() m_twistAxis.normalize(); } } -} // btConeTwistConstraint::calcAngleInfo() +} static btVector3 vTwist(1,0,0); // twist axis in constraint's space -//----------------------------------------------------------------------------- + void btConeTwistConstraint::calcAngleInfo2() { @@ -597,13 +599,34 @@ void btConeTwistConstraint::calcAngleInfo2() m_twistLimitSign = btScalar(0.); m_solveTwistLimit = false; m_solveSwingLimit = false; + // compute rotation of A wrt B (in constraint space) + if (m_bMotorEnabled && (!m_useSolveConstraintObsolete)) + { // it is assumed that setMotorTarget() was alredy called + // and motor target m_qTarget is within constraint limits + // TODO : split rotation to pure swing and pure twist + // compute desired transforms in world + btTransform trPose(m_qTarget); + btTransform trA = getRigidBodyA().getCenterOfMassTransform() * m_rbAFrame; + btTransform trB = getRigidBodyB().getCenterOfMassTransform() * m_rbBFrame; + btTransform trDeltaAB = trB * trPose * trA.inverse(); + btQuaternion qDeltaAB = trDeltaAB.getRotation(); + btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z()); + m_swingAxis = swingAxis; + m_swingAxis.normalize(); + m_swingCorrection = qDeltaAB.getAngle(); + if(!btFuzzyZero(m_swingCorrection)) + { + m_solveSwingLimit = true; + } + return; + } + { // compute rotation of A wrt B (in constraint space) btQuaternion qA = getRigidBodyA().getCenterOfMassTransform().getRotation() * m_rbAFrame.getRotation(); btQuaternion qB = getRigidBodyB().getCenterOfMassTransform().getRotation() * m_rbBFrame.getRotation(); btQuaternion qAB = qB.inverse() * qA; - // split rotation into cone and twist // (all this is done from B's perspective. Maybe I should be averaging axes...) btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize(); @@ -756,7 +779,7 @@ void btConeTwistConstraint::calcAngleInfo2() m_twistAngle = btScalar(0.f); } } -} // btConeTwistConstraint::calcAngleInfo2() +} @@ -982,8 +1005,5 @@ void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion & } -//----------------------------------------------------------------------------- -//----------------------------------------------------------------------------- -//----------------------------------------------------------------------------- diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h index 84ea9e04095..8a893d4fb8c 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -17,6 +17,22 @@ Written by: Marcus Hennix +/* +Overview: + +btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc). +It is a fixed translation, 3 degree-of-freedom (DOF) rotational "joint". +It divides the 3 rotational DOFs into swing (movement within a cone) and twist. +Swing is divided into swing1 and swing2 which can have different limits, giving an elliptical shape. +(Note: the cone's base isn't flat, so this ellipse is "embedded" on the surface of a sphere.) + +In the contraint's frame of reference: +twist is along the x-axis, +and swing 1 and 2 are along the z and y axes respectively. +*/ + + + #ifndef CONETWISTCONSTRAINT_H #define CONETWISTCONSTRAINT_H @@ -141,7 +157,18 @@ public: }; } - void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) + // setLimit(), a few notes: + // _softness: + // 0->1, recommend ~0.8->1. + // describes % of limits where movement is free. + // beyond this softness %, the limit is gradually enforced until the "hard" (1.0) limit is reached. + // _biasFactor: + // 0->1?, recommend 0.3 +/-0.3 or so. + // strength with which constraint resists zeroth order (angular, not angular velocity) limit violation. + // __relaxationFactor: + // 0->1, recommend to stay near 1. + // the lower the value, the less the constraint will fight velocities which violate the angular limits. + void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) { m_swingSpan1 = _swingSpan1; m_swingSpan2 = _swingSpan2; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index e99430c00de..a11fc94ea11 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -77,7 +77,7 @@ struct btContactSolverInfo : public btContactSolverInfoData m_splitImpulsePenetrationThreshold = -0.02f; m_linearSlop = btScalar(0.0); m_warmstartingFactor=btScalar(0.85); - m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD ;//SOLVER_RANDMIZE_ORDER + m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_USE_2_FRICTION_DIRECTIONS |SOLVER_SIMD | SOLVER_RANDMIZE_ORDER; m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution } }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index 6cbfe61f700..38e81688f02 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -22,11 +22,13 @@ http://gimpact.sf.net #include "btGeneric6DofConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" +#include "LinearMath/btTransformUtil.h" #include <new> + #define D6_USE_OBSOLETE_METHOD false -//----------------------------------------------------------------------------- + btGeneric6DofConstraint::btGeneric6DofConstraint() :btTypedConstraint(D6_CONSTRAINT_TYPE), @@ -35,7 +37,7 @@ m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD) { } -//----------------------------------------------------------------------------- + btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB) @@ -46,12 +48,12 @@ m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD) { } -//----------------------------------------------------------------------------- + #define GENERIC_D6_DISABLE_WARMSTARTING 1 -//----------------------------------------------------------------------------- + btScalar btGetMatrixElem(const btMatrix3x3& mat, int index); btScalar btGetMatrixElem(const btMatrix3x3& mat, int index) @@ -61,7 +63,7 @@ btScalar btGetMatrixElem(const btMatrix3x3& mat, int index) return mat[i][j]; } -//----------------------------------------------------------------------------- + ///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz); @@ -129,7 +131,7 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value) } -//----------------------------------------------------------------------------- + btScalar btRotationalLimitMotor::solveAngularLimits( btScalar timeStep,btVector3& axis,btScalar jacDiagABInv, @@ -191,8 +193,8 @@ btScalar btRotationalLimitMotor::solveAngularLimits( // sort with accumulated impulses - btScalar lo = btScalar(-1e30); - btScalar hi = btScalar(1e30); + btScalar lo = btScalar(-BT_LARGE_FLOAT); + btScalar hi = btScalar(BT_LARGE_FLOAT); btScalar oldaccumImpulse = m_accumulatedImpulse; btScalar sum = oldaccumImpulse + clippedMotorImpulse; @@ -249,9 +251,9 @@ int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_valu m_currentLimit[limitIndex] = 0;//Free from violation m_currentLimitError[limitIndex] = btScalar(0.f); return 0; -} // btTranslationalLimitMotor::testLimitValue() +} + -//----------------------------------------------------------------------------- btScalar btTranslationalLimitMotor::solveLinearAxis( btScalar timeStep, @@ -283,8 +285,8 @@ btScalar btTranslationalLimitMotor::solveLinearAxis( //positional error (zeroth order error) btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a); - btScalar lo = btScalar(-1e30); - btScalar hi = btScalar(1e30); + btScalar lo = btScalar(-BT_LARGE_FLOAT); + btScalar hi = btScalar(BT_LARGE_FLOAT); btScalar minLimit = m_lowerLimit[limit_index]; btScalar maxLimit = m_upperLimit[limit_index]; @@ -372,7 +374,7 @@ void btGeneric6DofConstraint::calculateAngleInfo() } -//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::calculateTransforms() { @@ -382,7 +384,7 @@ void btGeneric6DofConstraint::calculateTransforms() calculateAngleInfo(); } -//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::buildLinearJacobian( btJacobianEntry & jacLinear,const btVector3 & normalWorld, @@ -400,7 +402,7 @@ void btGeneric6DofConstraint::buildLinearJacobian( m_rbB.getInvMass()); } -//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::buildAngularJacobian( btJacobianEntry & jacAngular,const btVector3 & jointAxisW) @@ -413,17 +415,18 @@ void btGeneric6DofConstraint::buildAngularJacobian( } -//----------------------------------------------------------------------------- + bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index) { btScalar angle = m_calculatedAxisAngleDiff[axis_index]; + m_angularLimits[axis_index].m_currentPosition = angle; //test limits m_angularLimits[axis_index].testLimitValue(angle); return m_angularLimits[axis_index].needApplyTorques(); } -//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::buildJacobian() { @@ -483,7 +486,7 @@ void btGeneric6DofConstraint::buildJacobian() } } -//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info) { @@ -519,7 +522,7 @@ void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info) } } -//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info) { @@ -528,7 +531,7 @@ void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info) setAngularLimits(info, row); } -//----------------------------------------------------------------------------- + int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info) { @@ -542,6 +545,7 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info) { // re-use rotational motor code limot.m_bounce = btScalar(0.f); limot.m_currentLimit = m_linearLimits.m_currentLimit[i]; + limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i]; limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i]; limot.m_damping = m_linearLimits.m_damping; limot.m_enableMotor = m_linearLimits.m_enableMotor[i]; @@ -559,7 +563,7 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info) return row; } -//----------------------------------------------------------------------------- + int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset) { @@ -582,7 +586,7 @@ int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_o return row; } -//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) { @@ -643,7 +647,7 @@ void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolv } } -//----------------------------------------------------------------------------- + void btGeneric6DofConstraint::updateRHS(btScalar timeStep) { @@ -651,21 +655,26 @@ void btGeneric6DofConstraint::updateRHS(btScalar timeStep) } -//----------------------------------------------------------------------------- + btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const { return m_calculatedAxis[axis_index]; } -//----------------------------------------------------------------------------- -btScalar btGeneric6DofConstraint::getAngle(int axis_index) const +btScalar btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const { - return m_calculatedAxisAngleDiff[axis_index]; + return m_calculatedLinearDiff[axisIndex]; } -//----------------------------------------------------------------------------- + +btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const +{ + return m_calculatedAxisAngleDiff[axisIndex]; +} + + void btGeneric6DofConstraint::calcAnchorPos(void) { @@ -684,9 +693,9 @@ void btGeneric6DofConstraint::calcAnchorPos(void) const btVector3& pB = m_calculatedTransformB.getOrigin(); m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight); return; -} // btGeneric6DofConstraint::calcAnchorPos() +} + -//----------------------------------------------------------------------------- void btGeneric6DofConstraint::calculateLinearInfo() { @@ -694,11 +703,12 @@ void btGeneric6DofConstraint::calculateLinearInfo() m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff; for(int i = 0; i < 3; i++) { + m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i]; m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]); } -} // btGeneric6DofConstraint::calculateLinearInfo() +} + -//----------------------------------------------------------------------------- int btGeneric6DofConstraint::get_limit_motor_info2( btRotationalLimitMotor * limot, @@ -721,7 +731,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2( J2[srow+1] = -ax1[1]; J2[srow+2] = -ax1[2]; } - if((!rotational) && limit) + if((!rotational)) { btVector3 ltd; // Linear Torque Decoupling vector btVector3 c = m_calculatedTransformB.getOrigin() - body0->getCenterOfMassPosition(); @@ -745,7 +755,14 @@ int btGeneric6DofConstraint::get_limit_motor_info2( info->cfm[srow] = 0.0f; if(!limit) { - info->m_constraintError[srow] += limot->m_targetVelocity; + btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity; + + btScalar mot_fact = getMotorFactor( limot->m_currentPosition, + limot->m_loLimit, + limot->m_hiLimit, + tag_vel, + info->fps * info->erp); + info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity; info->m_lowerLimit[srow] = -limot->m_maxMotorForce; info->m_upperLimit[srow] = limot->m_maxMotorForce; } @@ -824,6 +841,131 @@ int btGeneric6DofConstraint::get_limit_motor_info2( else return 0; } -//----------------------------------------------------------------------------- -//----------------------------------------------------------------------------- -//----------------------------------------------------------------------------- + + + +btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA) + : btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA) +{ + for(int i = 0; i < 6; i++) + { + m_springEnabled[i] = false; + m_equilibriumPoint[i] = btScalar(0.f); + m_springStiffness[i] = btScalar(0.f); + m_springDamping[i] = btScalar(1.f); + } +} + + +void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff) +{ + btAssert((index >= 0) && (index < 6)); + m_springEnabled[index] = onOff; + if(index < 3) + { + m_linearLimits.m_enableMotor[index] = onOff; + } + else + { + m_angularLimits[index - 3].m_enableMotor = onOff; + } +} + + + +void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness) +{ + btAssert((index >= 0) && (index < 6)); + m_springStiffness[index] = stiffness; +} + + +void btGeneric6DofSpringConstraint::setDamping(int index, btScalar damping) +{ + btAssert((index >= 0) && (index < 6)); + m_springDamping[index] = damping; +} + + +void btGeneric6DofSpringConstraint::setEquilibriumPoint() +{ + calculateTransforms(); + for(int i = 0; i < 3; i++) + { + m_equilibriumPoint[i] = m_calculatedLinearDiff[i]; + } + for(int i = 0; i < 3; i++) + { + m_equilibriumPoint[i + 3] = m_calculatedAxisAngleDiff[i]; + } +} + + + +void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index) +{ + btAssert((index >= 0) && (index < 6)); + calculateTransforms(); + if(index < 3) + { + m_equilibriumPoint[index] = m_calculatedLinearDiff[index]; + } + else + { + m_equilibriumPoint[index + 3] = m_calculatedAxisAngleDiff[index]; + } +} + + + +void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info) +{ + calculateTransforms(); + // it is assumed that calculateTransforms() have been called before this call + int i; + btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity(); + for(i = 0; i < 3; i++) + { + if(m_springEnabled[i]) + { + // get current position of constraint + btScalar currPos = m_calculatedLinearDiff[i]; + // calculate difference + btScalar delta = currPos - m_equilibriumPoint[i]; + // spring force is (delta * m_stiffness) according to Hooke's Law + btScalar force = delta * m_springStiffness[i]; + btScalar velFactor = info->fps * m_springDamping[i]; + m_linearLimits.m_targetVelocity[i] = velFactor * force; + m_linearLimits.m_maxMotorForce[i] = btFabs(force) / info->fps; + } + } + for(i = 0; i < 3; i++) + { + if(m_springEnabled[i + 3]) + { + // get current position of constraint + btScalar currPos = m_calculatedAxisAngleDiff[i]; + // calculate difference + btScalar delta = currPos - m_equilibriumPoint[i+3]; + // spring force is (-delta * m_stiffness) according to Hooke's Law + btScalar force = -delta * m_springStiffness[i+3]; + btScalar velFactor = info->fps * m_springDamping[i+3]; + m_angularLimits[i].m_targetVelocity = velFactor * force; + m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps; + } + } +} + + +void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info) +{ + // this will be called by constraint solver at the constraint setup stage + // set current motor parameters + internalUpdateSprings(info); + // do the rest of job for constraint setup + btGeneric6DofConstraint::getInfo2(info); +} + + + + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index 0ae161d5bdf..8082eb1f132 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -54,6 +54,7 @@ public: //! temp_variables //!@{ btScalar m_currentLimitError;//! How much is violated this limit + btScalar m_currentPosition; //! current value of angle int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit btScalar m_accumulatedImpulse; //!@} @@ -134,6 +135,7 @@ public: btVector3 m_targetVelocity;//!< target motor velocity btVector3 m_maxMotorForce;//!< max force on motor btVector3 m_currentLimitError;//! How much is violated this limit + btVector3 m_currentLinearDiff;//! Current relative offset of constraint frames int m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit btTranslationalLimitMotor() @@ -380,14 +382,21 @@ public: //! Get the relative Euler angle /*! - \pre btGeneric6DofConstraint.buildJacobian must be called previously. + \pre btGeneric6DofConstraint::calculateTransforms() must be called previously. */ btScalar getAngle(int axis_index) const; + //! Get the relative position of the constraint pivot + /*! + \pre btGeneric6DofConstraint::calculateTransforms() must be called previously. + */ + btScalar getRelativePivotPosition(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. + \pre btGeneric6DofConstraint::calculateTransforms() must be called previously. */ bool testAngularLimitMotor(int axis_index); @@ -477,4 +486,34 @@ public: }; + +/// Generic 6 DOF constraint that allows to set spring motors to any translational and rotational DOF + +/// DOF index used in enableSpring() and setStiffness() means: +/// 0 : translation X +/// 1 : translation Y +/// 2 : translation Z +/// 3 : rotation X (3rd Euler rotational around new position of X axis, range [-PI+epsilon, PI-epsilon] ) +/// 4 : rotation Y (2nd Euler rotational around new position of Y axis, range [-PI/2+epsilon, PI/2-epsilon] ) +/// 5 : rotation Z (1st Euler rotational around Z axis, range [-PI+epsilon, PI-epsilon] ) + +class btGeneric6DofSpringConstraint : public btGeneric6DofConstraint +{ +protected: + bool m_springEnabled[6]; + btScalar m_equilibriumPoint[6]; + btScalar m_springStiffness[6]; + btScalar m_springDamping[6]; // between 0 and 1 (1 == no damping) + void internalUpdateSprings(btConstraintInfo2* info); +public: + btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); + void enableSpring(int index, bool onOff); + void setStiffness(int index, btScalar stiffness); + void setDamping(int index, btScalar damping); + void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF + void setEquilibriumPoint(int index); // set the current constraint position/orientation as an equilibrium point for given DOF + virtual void getInfo2 (btConstraintInfo2* info); +}; + + #endif //GENERIC_6DOF_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 685a812d427..dbd09b39238 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -490,7 +490,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m ///warm starting (or zero if disabled) - if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) { solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; if (rb0) @@ -539,9 +539,6 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) { cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel); - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); @@ -550,21 +547,26 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); } + + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); cp.m_lateralFrictionInitialized = true; } else { //re-calculate friction direction every frame, todo: check if this is really needed btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); - - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); } + + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + cp.m_lateralFrictionInitialized = true; } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index 50d06960379..133aed7271b 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -18,14 +18,14 @@ 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() { @@ -62,9 +62,9 @@ void btSliderConstraint::initParams() m_maxAngMotorForce = btScalar(0.); m_accumulatedAngMotorImpulse = btScalar(0.0); -} // btSliderConstraint::initParams() +} + -//----------------------------------------------------------------------------- btSliderConstraint::btSliderConstraint() :btTypedConstraint(SLIDER_CONSTRAINT_TYPE), @@ -73,9 +73,9 @@ btSliderConstraint::btSliderConstraint() // m_useSolveConstraintObsolete(true) { initParams(); -} // btSliderConstraint::btSliderConstraint() +} + -//----------------------------------------------------------------------------- btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB) @@ -86,9 +86,25 @@ btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const // m_useSolveConstraintObsolete(true) { initParams(); -} // btSliderConstraint::btSliderConstraint() +} + + +static btRigidBody s_fixed(0, 0, 0); +btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB) + : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, s_fixed, rbB) + , + m_frameInB(frameInB), + m_useLinearReferenceFrameA(useLinearReferenceFrameB), + m_useSolveConstraintObsolete(false) +// m_useSolveConstraintObsolete(true) +{ + ///not providing rigidbody B means implicitly using worldspace for body B +// m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin()); + + initParams(); +} + -//----------------------------------------------------------------------------- void btSliderConstraint::buildJacobian() { @@ -104,9 +120,9 @@ void btSliderConstraint::buildJacobian() { buildJacobianInt(m_rbB, m_rbA, m_frameInB, m_frameInA); } -} // btSliderConstraint::buildJacobian() +} + -//----------------------------------------------------------------------------- void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB) { @@ -159,9 +175,9 @@ void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, co // clear accumulator for motors m_accumulatedLinMotorImpulse = btScalar(0.0); m_accumulatedAngMotorImpulse = btScalar(0.0); -} // btSliderConstraint::buildJacobianInt() +} + -//----------------------------------------------------------------------------- void btSliderConstraint::getInfo1(btConstraintInfo1* info) { @@ -189,9 +205,9 @@ void btSliderConstraint::getInfo1(btConstraintInfo1* info) info->nub--; } } -} // btSliderConstraint::getInfo1() +} + -//----------------------------------------------------------------------------- void btSliderConstraint::getInfo2(btConstraintInfo2* info) { @@ -499,9 +515,9 @@ void btSliderConstraint::getInfo2(btConstraintInfo2* info) info->m_constraintError[srow] *= getSoftnessLimAng(); } // if(limit) } // if angular limit or powered -} // btSliderConstraint::getInfo2() +} + -//----------------------------------------------------------------------------- void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) { @@ -517,9 +533,9 @@ void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBod solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA); } } -} // btSliderConstraint::solveConstraint() +} + -//----------------------------------------------------------------------------- void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB) { @@ -703,11 +719,11 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& body bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse); } } -} // btSliderConstraint::solveConstraint() +} + + -//----------------------------------------------------------------------------- -//----------------------------------------------------------------------------- void btSliderConstraint::calculateTransforms(void){ if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete)) @@ -740,9 +756,9 @@ void btSliderConstraint::calculateTransforms(void){ normalWorld = m_calculatedTransformA.getBasis().getColumn(i); m_depth[i] = m_delta.dot(normalWorld); } -} // btSliderConstraint::calculateTransforms() +} -//----------------------------------------------------------------------------- + void btSliderConstraint::testLinLimits(void) { @@ -769,9 +785,9 @@ void btSliderConstraint::testLinLimits(void) { m_depth[0] = btScalar(0.); } -} // btSliderConstraint::testLinLimits() +} + -//----------------------------------------------------------------------------- void btSliderConstraint::testAngLimits(void) { @@ -795,9 +811,9 @@ void btSliderConstraint::testAngLimits(void) m_solveAngLim = true; } } -} // btSliderConstraint::testAngLimits() +} -//----------------------------------------------------------------------------- + btVector3 btSliderConstraint::getAncorInA(void) { @@ -805,13 +821,13 @@ btVector3 btSliderConstraint::getAncorInA(void) 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 index 70fbce5d9b2..01cef59ed31 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -25,23 +25,23 @@ TODO: #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 { @@ -126,6 +126,7 @@ protected: public: // constructors btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); + btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB); btSliderConstraint(); // overrides virtual void buildJacobian(); @@ -223,7 +224,7 @@ public: btVector3 getAncorInB(void); }; -//----------------------------------------------------------------------------- + #endif //SLIDER_CONSTRAINT_H diff --git a/extern/bullet2/src/CMakeLists.txt b/extern/bullet2/src/CMakeLists.txt index 043fd3f6e7f..9b8a5a7e00e 100644 --- a/extern/bullet2/src/CMakeLists.txt +++ b/extern/bullet2/src/CMakeLists.txt @@ -1 +1,4 @@ -SUBDIRS( BulletCollision BulletDynamics LinearMath BulletSoftBody ) +ADD_SUBDIRECTORY(BulletCollision) +ADD_SUBDIRECTORY(BulletDynamics) +ADD_SUBDIRECTORY(LinearMath) +ADD_SUBDIRECTORY(BulletSoftBody ) diff --git a/extern/bullet2/src/LinearMath/btScalar.h b/extern/bullet2/src/LinearMath/btScalar.h index 822296164c1..08b2dee8af3 100644 --- a/extern/bullet2/src/LinearMath/btScalar.h +++ b/extern/bullet2/src/LinearMath/btScalar.h @@ -168,8 +168,12 @@ inline int btGetVersion() ///The btScalar type abstracts floating point numbers, to easily switch between double and single floating point precision. #if defined(BT_USE_DOUBLE_PRECISION) typedef double btScalar; +//this number could be bigger in double precision +#define BT_LARGE_FLOAT 1e30 #else typedef float btScalar; +//keep BT_LARGE_FLOAT*BT_LARGE_FLOAT < FLT_MAX +#define BT_LARGE_FLOAT 1e18f #endif |