diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/ConstraintSolver')
23 files changed, 3032 insertions, 400 deletions
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index 15a4c92de20..09b7388b63e 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -214,7 +214,7 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt } // m_swingCorrection is always positive or 0 info->m_lowerLimit[srow] = 0; - info->m_upperLimit[srow] = SIMD_INFINITY; + info->m_upperLimit[srow] = (m_bMotorEnabled && m_maxMotorImpulse >= 0.0f) ? m_maxMotorImpulse : SIMD_INFINITY; srow += info->rowskip; } } @@ -540,8 +540,8 @@ void btConeTwistConstraint::calcAngleInfo() m_solveTwistLimit = false; m_solveSwingLimit = false; - btVector3 b1Axis1,b1Axis2,b1Axis3; - btVector3 b2Axis1,b2Axis2; + btVector3 b1Axis1(0,0,0),b1Axis2(0,0,0),b1Axis3(0,0,0); + btVector3 b2Axis1(0,0,0),b2Axis2(0,0,0); b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0); b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0); @@ -778,8 +778,10 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2]; target.normalize(); m_swingAxis = -ivB.cross(target); - m_swingCorrection = m_swingAxis.length(); - m_swingAxis.normalize(); + m_swingCorrection = m_swingAxis.length(); + + if (!btFuzzyZero(m_swingCorrection)) + m_swingAxis.normalize(); } } @@ -983,8 +985,8 @@ void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingA void btConeTwistConstraint::setMotorTarget(const btQuaternion &q) { - btTransform trACur = m_rbA.getCenterOfMassTransform(); - btTransform trBCur = m_rbB.getCenterOfMassTransform(); + //btTransform trACur = m_rbA.getCenterOfMassTransform(); + //btTransform trBCur = m_rbB.getCenterOfMassTransform(); // btTransform trABCur = trBCur.inverse() * trACur; // btQuaternion qABCur = trABCur.getRotation(); // btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h index 1735b524dba..b7636180c34 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -170,6 +170,11 @@ public: { m_angularOnly = angularOnly; } + + bool getAngularOnly() const + { + return m_angularOnly; + } void setLimit(int limitIndex,btScalar limitValue) { @@ -196,6 +201,33 @@ public: }; } + btScalar getLimit(int limitIndex) const + { + switch (limitIndex) + { + case 3: + { + return m_twistSpan; + break; + } + case 4: + { + return m_swingSpan2; + break; + } + case 5: + { + return m_swingSpan1; + break; + } + default: + { + btAssert(0 && "Invalid limitIndex specified for btConeTwistConstraint"); + return 0.0; + } + }; + } + // setLimit(), a few notes: // _softness: // 0->1, recommend ~0.8->1. @@ -218,8 +250,8 @@ public: m_relaxationFactor = _relaxationFactor; } - const btTransform& getAFrame() { return m_rbAFrame; }; - const btTransform& getBFrame() { return m_rbBFrame; }; + const btTransform& getAFrame() const { return m_rbAFrame; }; + const btTransform& getBFrame() const { return m_rbBFrame; }; inline int getSolveTwistLimit() { @@ -239,27 +271,43 @@ public: void calcAngleInfo(); void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB); - inline btScalar getSwingSpan1() + inline btScalar getSwingSpan1() const { return m_swingSpan1; } - inline btScalar getSwingSpan2() + inline btScalar getSwingSpan2() const { return m_swingSpan2; } - inline btScalar getTwistSpan() + inline btScalar getTwistSpan() const { return m_twistSpan; } - inline btScalar getTwistAngle() + inline btScalar getLimitSoftness() const + { + return m_limitSoftness; + } + inline btScalar getBiasFactor() const + { + return m_biasFactor; + } + inline btScalar getRelaxationFactor() const + { + return m_relaxationFactor; + } + inline btScalar getTwistAngle() const { return m_twistAngle; } bool isPastSwingLimit() { return m_solveSwingLimit; } + btScalar getDamping() const { return m_damping; } void setDamping(btScalar damping) { m_damping = damping; } void enableMotor(bool b) { m_bMotorEnabled = b; } + bool isMotorEnabled() const { return m_bMotorEnabled; } + btScalar getMaxMotorImpulse() const { return m_maxMotorImpulse; } + bool isMaxMotorImpulseNormalized() const { return m_bNormalizedMotorStrength; } void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; } void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; } @@ -271,6 +319,7 @@ public: // note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability) // note: don't forget to enableMotor() void setMotorTarget(const btQuaternion &q); + const btQuaternion& getMotorTarget() const { return m_qTarget; } // same as above, but q is the desired rotation of frameA wrt frameB in constraint space void setMotorTargetInConstraintSpace(const btQuaternion &q); @@ -297,6 +346,11 @@ public: ///return the local value of parameter virtual btScalar getParam(int num, int axis = -1) const; + int getFlags() const + { + return m_flags; + } + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp index 9d60d9957a5..1098d0c96b6 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @@ -155,8 +155,7 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(), body2.getLinearVelocity(), body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); - btScalar a; - a=jacDiagABInv; + rel_vel = normal.dot(vel); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index c07e9bbd806..a3a0fa6729f 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -89,7 +89,7 @@ struct btContactSolverInfo : public btContactSolverInfoData m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER; m_restingContactRestitutionThreshold = 2;//unused as of 2.81 m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit - m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their BT_ENABLE_GYROPSCOPIC_FORCE flag set (using btRigidBody::setFlag) + m_maxGyroscopicForce = 100.f; ///it is only used for 'explicit' version of gyroscopic force m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows. } }; @@ -111,7 +111,7 @@ struct btContactSolverInfoDoubleData double m_splitImpulseTurnErp; double m_linearSlop; double m_warmstartingFactor; - double m_maxGyroscopicForce; + double m_maxGyroscopicForce;///it is only used for 'explicit' version of gyroscopic force double m_singleAxisRollingFrictionThreshold; int m_numIterations; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp index 3428e0b069d..75d81cc08c2 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp @@ -21,166 +21,17 @@ subject to the following restrictions: btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB) -:btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB) -{ - m_frameInA = frameInA; - m_frameInB = frameInB; - -} - -btFixedConstraint::~btFixedConstraint () +:btGeneric6DofSpring2Constraint(rbA,rbB,frameInA,frameInB) { + setAngularLowerLimit(btVector3(0,0,0)); + setAngularUpperLimit(btVector3(0,0,0)); + setLinearLowerLimit(btVector3(0,0,0)); + setLinearUpperLimit(btVector3(0,0,0)); } - -void btFixedConstraint::getInfo1 (btConstraintInfo1* info) -{ - info->m_numConstraintRows = 6; - info->nub = 0; -} - -void btFixedConstraint::getInfo2 (btConstraintInfo2* info) -{ - //fix the 3 linear degrees of freedom - - const btTransform& transA = m_rbA.getCenterOfMassTransform(); - const btTransform& transB = m_rbB.getCenterOfMassTransform(); - - const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin(); - const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis(); - const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin(); - const btMatrix3x3& worldOrnB = m_rbB.getCenterOfMassTransform().getBasis(); - - info->m_J1linearAxis[0] = 1; - info->m_J1linearAxis[info->rowskip+1] = 1; - info->m_J1linearAxis[2*info->rowskip+2] = 1; - btVector3 a1 = worldOrnA * m_frameInA.getOrigin(); - { - btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); - btVector3 a1neg = -a1; - a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); - } - if (info->m_J2linearAxis) - { - info->m_J2linearAxis[0] = -1; - info->m_J2linearAxis[info->rowskip+1] = -1; - info->m_J2linearAxis[2*info->rowskip+2] = -1; - } - - btVector3 a2 = worldOrnB*m_frameInB.getOrigin(); - { - btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); - a2.getSkewSymmetricMatrix(angular0,angular1,angular2); - } - - // set right hand side for the linear dofs - btScalar k = info->fps * info->erp; - - btVector3 linearError = k*(a2+worldPosB-a1-worldPosA); - int j; - for (j=0; j<3; j++) - { - info->m_constraintError[j*info->rowskip] = linearError[j]; - //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); - } - - btVector3 ivA = transA.getBasis() * m_frameInA.getBasis().getColumn(0); - btVector3 jvA = transA.getBasis() * m_frameInA.getBasis().getColumn(1); - btVector3 kvA = transA.getBasis() * m_frameInA.getBasis().getColumn(2); - btVector3 ivB = transB.getBasis() * m_frameInB.getBasis().getColumn(0); - btVector3 target; - btScalar x = ivB.dot(ivA); - btScalar y = ivB.dot(jvA); - btScalar z = ivB.dot(kvA); - btVector3 swingAxis(0,0,0); - { - if((!btFuzzyZero(y)) || (!(btFuzzyZero(z)))) - { - swingAxis = -ivB.cross(ivA); - } - } - btVector3 vTwist(1,0,0); - - // compute rotation of A wrt B (in constraint space) - btQuaternion qA = transA.getRotation() * m_frameInA.getRotation(); - btQuaternion qB = transB.getRotation() * m_frameInB.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(); - btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize(); - btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize(); - - int row = 3; - int srow = row * info->rowskip; - btVector3 ax1; - // angular limits - { - btScalar *J1 = info->m_J1angularAxis; - btScalar *J2 = info->m_J2angularAxis; - btTransform trA = transA*m_frameInA; - btVector3 twistAxis = trA.getBasis().getColumn(0); - - btVector3 p = trA.getBasis().getColumn(1); - btVector3 q = trA.getBasis().getColumn(2); - int srow1 = srow + info->rowskip; - J1[srow+0] = p[0]; - J1[srow+1] = p[1]; - J1[srow+2] = p[2]; - J1[srow1+0] = q[0]; - J1[srow1+1] = q[1]; - J1[srow1+2] = q[2]; - J2[srow+0] = -p[0]; - J2[srow+1] = -p[1]; - J2[srow+2] = -p[2]; - J2[srow1+0] = -q[0]; - J2[srow1+1] = -q[1]; - J2[srow1+2] = -q[2]; - btScalar fact = info->fps; - info->m_constraintError[srow] = fact * swingAxis.dot(p); - info->m_constraintError[srow1] = fact * swingAxis.dot(q); - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - info->m_lowerLimit[srow1] = -SIMD_INFINITY; - info->m_upperLimit[srow1] = SIMD_INFINITY; - srow = srow1 + info->rowskip; - - { - btQuaternion qMinTwist = qABTwist; - btScalar twistAngle = qABTwist.getAngle(); - - if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate. - { - qMinTwist = -(qABTwist); - twistAngle = qMinTwist.getAngle(); - } - - if (twistAngle > SIMD_EPSILON) - { - twistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z()); - twistAxis.normalize(); - twistAxis = quatRotate(qB, -twistAxis); - } - ax1 = twistAxis; - btScalar *J1 = info->m_J1angularAxis; - btScalar *J2 = info->m_J2angularAxis; - J1[srow+0] = ax1[0]; - J1[srow+1] = ax1[1]; - J1[srow+2] = ax1[2]; - J2[srow+0] = -ax1[0]; - J2[srow+1] = -ax1[1]; - J2[srow+2] = -ax1[2]; - btScalar k = info->fps; - info->m_constraintError[srow] = k * twistAngle; - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - } -}
\ No newline at end of file +btFixedConstraint::~btFixedConstraint () +{ +} diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h index e1c9430fd3c..bff2008b283 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h @@ -16,32 +16,17 @@ subject to the following restrictions: #ifndef BT_FIXED_CONSTRAINT_H #define BT_FIXED_CONSTRAINT_H -#include "btTypedConstraint.h" +#include "btGeneric6DofSpring2Constraint.h" -ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint + +ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btGeneric6DofSpring2Constraint { - btTransform m_frameInA; - btTransform m_frameInB; public: btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB); - - virtual ~btFixedConstraint(); - virtual void getInfo1 (btConstraintInfo1* info); - - virtual void getInfo2 (btConstraintInfo2* info); - - virtual void setParam(int num, btScalar value, int axis = -1) - { - btAssert(0); - } - virtual btScalar getParam(int num, int axis = -1) const - { - btAssert(0); - return 0.f; - } + virtual ~btFixedConstraint(); }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index 431a524169e..bea8629c325 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -111,14 +111,14 @@ public: //! Is limited - bool isLimited() + bool isLimited() const { if(m_loLimit > m_hiLimit) return false; return true; } //! Need apply correction - bool needApplyTorques() + bool needApplyTorques() const { if(m_currentLimit == 0 && m_enableMotor == false) return false; return true; @@ -207,11 +207,11 @@ public: - limited means upper > lower - limitIndex: first 3 are linear, next 3 are angular */ - inline bool isLimited(int limitIndex) + inline bool isLimited(int limitIndex) const { return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); } - inline bool needApplyForce(int limitIndex) + inline bool needApplyForce(int limitIndex) const { if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false; return true; @@ -457,7 +457,7 @@ public: m_linearLimits.m_lowerLimit = linearLower; } - void getLinearLowerLimit(btVector3& linearLower) + void getLinearLowerLimit(btVector3& linearLower) const { linearLower = m_linearLimits.m_lowerLimit; } @@ -467,7 +467,7 @@ public: m_linearLimits.m_upperLimit = linearUpper; } - void getLinearUpperLimit(btVector3& linearUpper) + void getLinearUpperLimit(btVector3& linearUpper) const { linearUpper = m_linearLimits.m_upperLimit; } @@ -478,7 +478,7 @@ public: m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]); } - void getAngularLowerLimit(btVector3& angularLower) + void getAngularLowerLimit(btVector3& angularLower) const { for(int i = 0; i < 3; i++) angularLower[i] = m_angularLimits[i].m_loLimit; @@ -490,7 +490,7 @@ public: m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]); } - void getAngularUpperLimit(btVector3& angularUpper) + void getAngularUpperLimit(btVector3& angularUpper) const { for(int i = 0; i < 3; i++) angularUpper[i] = m_angularLimits[i].m_hiLimit; @@ -532,7 +532,7 @@ public: - limited means upper > lower - limitIndex: first 3 are linear, next 3 are angular */ - bool isLimited(int limitIndex) + bool isLimited(int limitIndex) const { if(limitIndex<3) { @@ -549,8 +549,11 @@ public: btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false); // access for UseFrameOffset - bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; } + bool getUseFrameOffset() const { return m_useOffsetForConstraintFrame; } void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; } + + bool getUseLinearReferenceFrameA() const { return m_useLinearReferenceFrameA; } + void setUseLinearReferenceFrameA(bool linearReferenceFrameA) { m_useLinearReferenceFrameA = linearReferenceFrameA; } ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). ///If no axis is provided, it uses the default axis for this constraint. @@ -560,6 +563,10 @@ public: void setAxis( const btVector3& axis1, const btVector3& axis2); + virtual int getFlags() const + { + return m_flags; + } virtual int calculateSerializeBufferSize() const; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp new file mode 100644 index 00000000000..49ff78c2621 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp @@ -0,0 +1,1121 @@ +/* +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. +*/ + +/* +2014 May: btGeneric6DofSpring2Constraint is created from the original (2.82.2712) btGeneric6DofConstraint by Gabor Puhr and Tamas Umenhoffer +Pros: +- Much more accurate and stable in a lot of situation. (Especially when a sleeping chain of RBs connected with 6dof2 is pulled) +- Stable and accurate spring with minimal energy loss that works with all of the solvers. (latter is not true for the original 6dof spring) +- Servo motor functionality +- Much more accurate bouncing. 0 really means zero bouncing (not true for the original 6odf) and there is only a minimal energy loss when the value is 1 (because of the solvers' precision) +- Rotation order for the Euler system can be set. (One axis' freedom is still limited to pi/2) + +Cons: +- It is slower than the original 6dof. There is no exact ratio, but half speed is a good estimation. (with PGS) +- At bouncing the correct velocity is calculated, but not the correct position. (it is because of the solver can correct position or velocity, but not both.) +*/ + +/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev +/// Added support for generic constraint solver through getInfo1/getInfo2 methods + +/* +2007-09-09 +btGeneric6DofConstraint Refactored by Francisco Le?n +email: projectileman@yahoo.com +http://gimpact.sf.net +*/ + + + +#include "btGeneric6DofSpring2Constraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btTransformUtil.h" +#include <new> + + + +btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder) + : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, rbA, rbB) + , m_frameInA(frameInA) + , m_frameInB(frameInB) + , m_rotateOrder(rotOrder) + , m_flags(0) +{ + calculateTransforms(); +} + + +btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder) + : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, getFixedBody(), rbB) + , m_frameInB(frameInB) + , m_rotateOrder(rotOrder) + , m_flags(0) +{ + ///not providing rigidbody A means implicitly using worldspace for body A + m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB; + calculateTransforms(); +} + + +btScalar btGeneric6DofSpring2Constraint::btGetMatrixElem(const btMatrix3x3& mat, int index) +{ + int i = index%3; + int j = index/3; + return mat[i][j]; +} + +// MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html + +bool btGeneric6DofSpring2Constraint::matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) +{ + // 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 + + btScalar fi = btGetMatrixElem(mat,2); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) + { + 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; +} + +bool btGeneric6DofSpring2Constraint::matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz) +{ + // rot = cy*cz -sz sy*cz + // cy*cx*sz+sx*sy cx*cz sy*cx*sz-cy*sx + // cy*sx*sz-cx*sy sx*cz sy*sx*sz+cx*cy + + btScalar fi = btGetMatrixElem(mat,1); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) + { + xyz[0] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,4)); + xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0)); + xyz[2] = btAsin(-btGetMatrixElem(mat,1)); + return true; + } + else + { + xyz[0] = -btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8)); + xyz[1] = btScalar(0.0); + xyz[2] = SIMD_HALF_PI; + return false; + } + } + else + { + xyz[0] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8)); + xyz[1] = 0.0; + xyz[2] = -SIMD_HALF_PI; + } + return false; +} + +bool btGeneric6DofSpring2Constraint::matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz) +{ + // rot = cy*cz+sy*sx*sz cz*sy*sx-cy*sz cx*sy + // cx*sz cx*cz -sx + // cy*sx*sz-cz*sy sy*sz+cy*cz*sx cy*cx + + btScalar fi = btGetMatrixElem(mat,5); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) + { + xyz[0] = btAsin(-btGetMatrixElem(mat,5)); + xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,8)); + xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + return true; + } + else + { + xyz[0] = SIMD_HALF_PI; + xyz[1] = -btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); + xyz[2] = btScalar(0.0); + return false; + } + } + else + { + xyz[0] = -SIMD_HALF_PI; + xyz[1] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); + xyz[2] = 0.0; + } + return false; +} + +bool btGeneric6DofSpring2Constraint::matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz) +{ + // rot = cy*cz sy*sx-cy*cx*sz cx*sy+cy*sz*sx + // sz cz*cx -cz*sx + // -cz*sy cy*sx+cx*sy*sz cy*cx-sy*sz*sx + + btScalar fi = btGetMatrixElem(mat,3); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) + { + xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,4)); + xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,0)); + xyz[2] = btAsin(btGetMatrixElem(mat,3)); + return true; + } + else + { + xyz[0] = btScalar(0.0); + xyz[1] = -btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8)); + xyz[2] = -SIMD_HALF_PI; + return false; + } + } + else + { + xyz[0] = btScalar(0.0); + xyz[1] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8)); + xyz[2] = SIMD_HALF_PI; + } + return false; +} + +bool btGeneric6DofSpring2Constraint::matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz) +{ + // rot = cz*cy-sz*sx*sy -cx*sz cz*sy+cy*sz*sx + // cy*sz+cz*sx*sy cz*cx sz*sy-cz*xy*sx + // -cx*sy sx cx*cy + + btScalar fi = btGetMatrixElem(mat,7); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) + { + xyz[0] = btAsin(btGetMatrixElem(mat,7)); + xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8)); + xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,4)); + return true; + } + else + { + xyz[0] = -SIMD_HALF_PI; + xyz[1] = btScalar(0.0); + xyz[2] = -btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0)); + return false; + } + } + else + { + xyz[0] = SIMD_HALF_PI; + xyz[1] = btScalar(0.0); + xyz[2] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0)); + } + return false; +} + +bool btGeneric6DofSpring2Constraint::matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz) +{ + // rot = cz*cy cz*sy*sx-cx*sz sz*sx+cz*cx*sy + // cy*sz cz*cx+sz*sy*sx cx*sz*sy-cz*sx + // -sy cy*sx cy*cx + + btScalar fi = btGetMatrixElem(mat,6); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) + { + xyz[0] = btAtan2(btGetMatrixElem(mat,7), btGetMatrixElem(mat,8)); + xyz[1] = btAsin(-btGetMatrixElem(mat,6)); + xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,0)); + return true; + } + else + { + xyz[0] = btScalar(0.0); + xyz[1] = SIMD_HALF_PI; + xyz[2] = -btAtan2(btGetMatrixElem(mat,1),btGetMatrixElem(mat,2)); + return false; + } + } + else + { + xyz[0] = btScalar(0.0); + xyz[1] = -SIMD_HALF_PI; + xyz[2] = btAtan2(-btGetMatrixElem(mat,1),-btGetMatrixElem(mat,2)); + } + return false; +} + +void btGeneric6DofSpring2Constraint::calculateAngleInfo() +{ + btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis(); + switch (m_rotateOrder) + { + case RO_XYZ : matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff); break; + case RO_XZY : matrixToEulerXZY(relative_frame,m_calculatedAxisAngleDiff); break; + case RO_YXZ : matrixToEulerYXZ(relative_frame,m_calculatedAxisAngleDiff); break; + case RO_YZX : matrixToEulerYZX(relative_frame,m_calculatedAxisAngleDiff); break; + case RO_ZXY : matrixToEulerZXY(relative_frame,m_calculatedAxisAngleDiff); break; + case RO_ZYX : matrixToEulerZYX(relative_frame,m_calculatedAxisAngleDiff); break; + default : btAssert(false); + } + // 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. + switch (m_rotateOrder) + { + case RO_XYZ : + { + //Is this the "line of nodes" calculation choosing planes YZ (B coordinate system) and xy (A coordinate system)? (http://en.wikipedia.org/wiki/Euler_angles) + //The two planes are non-homologous, so this is a Tait–Bryan angle formalism and not a proper Euler + //Extrinsic rotations are equal to the reversed order intrinsic rotations so the above xyz extrinsic rotations (axes are fixed) are the same as the zy'x" intrinsic rotations (axes are refreshed after each rotation) + //that is why xy and YZ planes are chosen (this will describe a zy'x" intrinsic rotation) (see the figure on the left at http://en.wikipedia.org/wiki/Euler_angles under Tait–Bryan angles) + // x' = Nperp = N.cross(axis2) + // y' = N = axis2.cross(axis0) + // z' = z + // + // x" = X + // y" = y' + // z" = ?? + //in other words: + //first rotate around z + //second rotate around y'= z.cross(X) + //third rotate around x" = X + //Original XYZ extrinsic rotation order. + //Planes: xy and YZ normals: z, X. Plane intersection (N) is z.cross(X) + 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]); + break; + } + case RO_XZY : + { + //planes: xz,ZY normals: y, X + //first rotate around y + //second rotate around z'= y.cross(X) + //third rotate around x" = X + btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0); + btVector3 axis1 = m_calculatedTransformA.getBasis().getColumn(1); + m_calculatedAxis[2] = axis0.cross(axis1); + m_calculatedAxis[0] = axis1.cross(m_calculatedAxis[2]); + m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0); + break; + } + case RO_YXZ : + { + //planes: yx,XZ normals: z, Y + //first rotate around z + //second rotate around x'= z.cross(Y) + //third rotate around y" = Y + btVector3 axis1 = m_calculatedTransformB.getBasis().getColumn(1); + btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2); + m_calculatedAxis[0] = axis1.cross(axis2); + m_calculatedAxis[1] = axis2.cross(m_calculatedAxis[0]); + m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1); + break; + } + case RO_YZX : + { + //planes: yz,ZX normals: x, Y + //first rotate around x + //second rotate around z'= x.cross(Y) + //third rotate around y" = Y + btVector3 axis0 = m_calculatedTransformA.getBasis().getColumn(0); + btVector3 axis1 = m_calculatedTransformB.getBasis().getColumn(1); + m_calculatedAxis[2] = axis0.cross(axis1); + m_calculatedAxis[0] = axis1.cross(m_calculatedAxis[2]); + m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0); + break; + } + case RO_ZXY : + { + //planes: zx,XY normals: y, Z + //first rotate around y + //second rotate around x'= y.cross(Z) + //third rotate around z" = Z + btVector3 axis1 = m_calculatedTransformA.getBasis().getColumn(1); + btVector3 axis2 = m_calculatedTransformB.getBasis().getColumn(2); + m_calculatedAxis[0] = axis1.cross(axis2); + m_calculatedAxis[1] = axis2.cross(m_calculatedAxis[0]); + m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1); + break; + } + case RO_ZYX : + { + //planes: zy,YX normals: x, Z + //first rotate around x + //second rotate around y' = x.cross(Z) + //third rotate around z" = Z + btVector3 axis0 = m_calculatedTransformA.getBasis().getColumn(0); + btVector3 axis2 = m_calculatedTransformB.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]); + break; + } + default: + btAssert(false); + } + + m_calculatedAxis[0].normalize(); + m_calculatedAxis[1].normalize(); + m_calculatedAxis[2].normalize(); + +} + +void btGeneric6DofSpring2Constraint::calculateTransforms() +{ + calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); +} + +void btGeneric6DofSpring2Constraint::calculateTransforms(const btTransform& transA,const btTransform& transB) +{ + m_calculatedTransformA = transA * m_frameInA; + m_calculatedTransformB = transB * m_frameInB; + calculateLinearInfo(); + calculateAngleInfo(); + + btScalar miA = getRigidBodyA().getInvMass(); + btScalar miB = getRigidBodyB().getInvMass(); + m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); + btScalar miS = miA + miB; + if(miS > btScalar(0.f)) + { + m_factA = miB / miS; + } + else + { + m_factA = btScalar(0.5f); + } + m_factB = btScalar(1.0f) - m_factA; +} + + +void btGeneric6DofSpring2Constraint::testAngularLimitMotor(int axis_index) +{ + btScalar angle = m_calculatedAxisAngleDiff[axis_index]; + angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit); + m_angularLimits[axis_index].m_currentPosition = angle; + m_angularLimits[axis_index].testLimitValue(angle); +} + + +void btGeneric6DofSpring2Constraint::getInfo1 (btConstraintInfo1* info) +{ + //prepare constraint + calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + info->m_numConstraintRows = 0; + info->nub = 0; + int i; + //test linear limits + for(i = 0; i < 3; i++) + { + if (m_linearLimits.m_currentLimit[i]==4) info->m_numConstraintRows += 2; + else if (m_linearLimits.m_currentLimit[i]!=0) info->m_numConstraintRows += 1; + if (m_linearLimits.m_enableMotor[i] ) info->m_numConstraintRows += 1; + if (m_linearLimits.m_enableSpring[i]) info->m_numConstraintRows += 1; + } + //test angular limits + for (i=0;i<3 ;i++ ) + { + testAngularLimitMotor(i); + if (m_angularLimits[i].m_currentLimit==4) info->m_numConstraintRows += 2; + else if (m_angularLimits[i].m_currentLimit!=0) info->m_numConstraintRows += 1; + if (m_angularLimits[i].m_enableMotor ) info->m_numConstraintRows += 1; + if (m_angularLimits[i].m_enableSpring) info->m_numConstraintRows += 1; + } +} + + +void btGeneric6DofSpring2Constraint::getInfo2 (btConstraintInfo2* info) +{ + const btTransform& transA = m_rbA.getCenterOfMassTransform(); + const btTransform& transB = m_rbB.getCenterOfMassTransform(); + const btVector3& linVelA = m_rbA.getLinearVelocity(); + const btVector3& linVelB = m_rbB.getLinearVelocity(); + const btVector3& angVelA = m_rbA.getAngularVelocity(); + const btVector3& angVelB = m_rbB.getAngularVelocity(); + + // for stability better to solve angular limits first + int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB); + setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB); +} + + +int btGeneric6DofSpring2Constraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) +{ + //solve linear limits + btRotationalLimitMotor2 limot; + for (int i=0;i<3 ;i++ ) + { + if(m_linearLimits.m_currentLimit[i] || m_linearLimits.m_enableMotor[i] || m_linearLimits.m_enableSpring[i]) + { // re-use rotational motor code + limot.m_bounce = m_linearLimits.m_bounce[i]; + 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_currentLimitErrorHi = m_linearLimits.m_currentLimitErrorHi[i]; + limot.m_enableMotor = m_linearLimits.m_enableMotor[i]; + limot.m_servoMotor = m_linearLimits.m_servoMotor[i]; + limot.m_servoTarget = m_linearLimits.m_servoTarget[i]; + limot.m_enableSpring = m_linearLimits.m_enableSpring[i]; + limot.m_springStiffness = m_linearLimits.m_springStiffness[i]; + limot.m_springStiffnessLimited = m_linearLimits.m_springStiffnessLimited[i]; + limot.m_springDamping = m_linearLimits.m_springDamping[i]; + limot.m_springDampingLimited = m_linearLimits.m_springDampingLimited[i]; + limot.m_equilibriumPoint = m_linearLimits.m_equilibriumPoint[i]; + limot.m_hiLimit = m_linearLimits.m_upperLimit[i]; + limot.m_loLimit = m_linearLimits.m_lowerLimit[i]; + limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i]; + limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i]; + btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i); + int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT2); + limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP2) ? m_linearLimits.m_stopCFM[i] : info->cfm[0]; + limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP2) ? m_linearLimits.m_stopERP[i] : info->erp; + limot.m_motorCFM = (flags & BT_6DOF_FLAGS_CFM_MOTO2) ? m_linearLimits.m_motorCFM[i] : info->cfm[0]; + limot.m_motorERP = (flags & BT_6DOF_FLAGS_ERP_MOTO2) ? m_linearLimits.m_motorERP[i] : info->erp; + + //rotAllowed is a bit of a magic from the original 6dof. The calculation of it here is something that imitates the original behavior as much as possible. + int indx1 = (i + 1) % 3; + int indx2 = (i + 2) % 3; + int rotAllowed = 1; // rotations around orthos to current axis (it is used only when one of the body is static) + #define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION 1.0e-3 + bool indx1Violated = m_angularLimits[indx1].m_currentLimit == 1 || + m_angularLimits[indx1].m_currentLimit == 2 || + ( m_angularLimits[indx1].m_currentLimit == 3 && ( m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ) || + ( m_angularLimits[indx1].m_currentLimit == 4 && ( m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ); + bool indx2Violated = m_angularLimits[indx2].m_currentLimit == 1 || + m_angularLimits[indx2].m_currentLimit == 2 || + ( m_angularLimits[indx2].m_currentLimit == 3 && ( m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ) || + ( m_angularLimits[indx2].m_currentLimit == 4 && ( m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ); + if( indx1Violated && indx2Violated ) + { + rotAllowed = 0; + } + row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed); + + } + } + return row; +} + + + +int btGeneric6DofSpring2Constraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) +{ + int row = row_offset; + + //order of rotational constraint rows + int cIdx[] = {0, 1, 2}; + switch(m_rotateOrder) + { + case RO_XYZ : cIdx[0] = 0; cIdx[1] = 1; cIdx[2] = 2; break; + case RO_XZY : cIdx[0] = 0; cIdx[1] = 2; cIdx[2] = 1; break; + case RO_YXZ : cIdx[0] = 1; cIdx[1] = 0; cIdx[2] = 2; break; + case RO_YZX : cIdx[0] = 1; cIdx[1] = 2; cIdx[2] = 0; break; + case RO_ZXY : cIdx[0] = 2; cIdx[1] = 0; cIdx[2] = 1; break; + case RO_ZYX : cIdx[0] = 2; cIdx[1] = 1; cIdx[2] = 0; break; + default : btAssert(false); + } + + for (int ii = 0; ii < 3 ; ii++ ) + { + int i = cIdx[ii]; + if(m_angularLimits[i].m_currentLimit || m_angularLimits[i].m_enableMotor || m_angularLimits[i].m_enableSpring) + { + btVector3 axis = getAxis(i); + int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT2); + if(!(flags & BT_6DOF_FLAGS_CFM_STOP2)) + { + m_angularLimits[i].m_stopCFM = info->cfm[0]; + } + if(!(flags & BT_6DOF_FLAGS_ERP_STOP2)) + { + m_angularLimits[i].m_stopERP = info->erp; + } + if(!(flags & BT_6DOF_FLAGS_CFM_MOTO2)) + { + m_angularLimits[i].m_motorCFM = info->cfm[0]; + } + if(!(flags & BT_6DOF_FLAGS_ERP_MOTO2)) + { + m_angularLimits[i].m_motorERP = info->erp; + } + row += get_limit_motor_info2(&m_angularLimits[i],transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1); + } + } + + return row; +} + + +void btGeneric6DofSpring2Constraint::setFrames(const btTransform& frameA, const btTransform& frameB) +{ + m_frameInA = frameA; + m_frameInB = frameB; + buildJacobian(); + calculateTransforms(); +} + + +void btGeneric6DofSpring2Constraint::calculateLinearInfo() +{ + m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin(); + 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]); + } +} + +void btGeneric6DofSpring2Constraint::calculateJacobi(btRotationalLimitMotor2 * limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2 *info, int srow, btVector3& ax1, int rotational, int rotAllowed) +{ + btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis; + btScalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis; + + J1[srow+0] = ax1[0]; + J1[srow+1] = ax1[1]; + J1[srow+2] = ax1[2]; + + J2[srow+0] = -ax1[0]; + J2[srow+1] = -ax1[1]; + J2[srow+2] = -ax1[2]; + + if(!rotational) + { + btVector3 tmpA, tmpB, relA, relB; + // get vector from bodyB to frameB in WCS + relB = m_calculatedTransformB.getOrigin() - transB.getOrigin(); + // same for bodyA + relA = m_calculatedTransformA.getOrigin() - transA.getOrigin(); + tmpA = relA.cross(ax1); + tmpB = relB.cross(ax1); + if(m_hasStaticBody && (!rotAllowed)) + { + tmpA *= m_factA; + tmpB *= m_factB; + } + int i; + for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i]; + for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i]; + } +} + + +int btGeneric6DofSpring2Constraint::get_limit_motor_info2( + btRotationalLimitMotor2 * limot, + const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB, + btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed) +{ + int count = 0; + int srow = row * info->rowskip; + + if (limot->m_currentLimit==4) + { + btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1); + + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1); + if (rotational) { + if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) { + btScalar bounceerror = -limot->m_bounce* vel; + if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror; + } + } else { + if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) { + btScalar bounceerror = -limot->m_bounce* vel; + if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror; + } + } + info->m_lowerLimit[srow] = rotational ? 0 : -SIMD_INFINITY; + info->m_upperLimit[srow] = rotational ? SIMD_INFINITY : 0; + info->cfm[srow] = limot->m_stopCFM; + srow += info->rowskip; + ++count; + + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitErrorHi * (rotational ? -1 : 1); + if (rotational) { + if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) { + btScalar bounceerror = -limot->m_bounce* vel; + if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror; + } + } else { + if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) { + btScalar bounceerror = -limot->m_bounce* vel; + if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror; + } + } + info->m_lowerLimit[srow] = rotational ? -SIMD_INFINITY : 0; + info->m_upperLimit[srow] = rotational ? 0 : SIMD_INFINITY; + info->cfm[srow] = limot->m_stopCFM; + srow += info->rowskip; + ++count; + } else + if (limot->m_currentLimit==3) + { + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1); + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + info->cfm[srow] = limot->m_stopCFM; + srow += info->rowskip; + ++count; + } + + if (limot->m_enableMotor && !limot->m_servoMotor) + { + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + 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 * limot->m_motorERP); + info->m_constraintError[srow] = mot_fact * limot->m_targetVelocity; + info->m_lowerLimit[srow] = -limot->m_maxMotorForce; + info->m_upperLimit[srow] = limot->m_maxMotorForce; + info->cfm[srow] = limot->m_motorCFM; + srow += info->rowskip; + ++count; + } + + if (limot->m_enableMotor && limot->m_servoMotor) + { + btScalar error = limot->m_currentPosition - limot->m_servoTarget; + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + btScalar targetvelocity = error<0 ? -limot->m_targetVelocity : limot->m_targetVelocity; + btScalar tag_vel = -targetvelocity; + btScalar mot_fact; + if(error != 0) + { + btScalar lowLimit; + btScalar hiLimit; + if(limot->m_loLimit > limot->m_hiLimit) + { + lowLimit = error > 0 ? limot->m_servoTarget : -SIMD_INFINITY; + hiLimit = error < 0 ? limot->m_servoTarget : SIMD_INFINITY; + } + else + { + lowLimit = error > 0 && limot->m_servoTarget>limot->m_loLimit ? limot->m_servoTarget : limot->m_loLimit; + hiLimit = error < 0 && limot->m_servoTarget<limot->m_hiLimit ? limot->m_servoTarget : limot->m_hiLimit; + } + mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP); + } + else + { + mot_fact = 0; + } + info->m_constraintError[srow] = mot_fact * targetvelocity * (rotational ? -1 : 1); + info->m_lowerLimit[srow] = -limot->m_maxMotorForce; + info->m_upperLimit[srow] = limot->m_maxMotorForce; + info->cfm[srow] = limot->m_motorCFM; + srow += info->rowskip; + ++count; + } + + if (limot->m_enableSpring) + { + btScalar error = limot->m_currentPosition - limot->m_equilibriumPoint; + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + + //btScalar cfm = 1.0 / ((1.0/info->fps)*limot->m_springStiffness+ limot->m_springDamping); + //if(cfm > 0.99999) + // cfm = 0.99999; + //btScalar erp = (1.0/info->fps)*limot->m_springStiffness / ((1.0/info->fps)*limot->m_springStiffness + limot->m_springDamping); + //info->m_constraintError[srow] = info->fps * erp * error * (rotational ? -1.0 : 1.0); + //info->m_lowerLimit[srow] = -SIMD_INFINITY; + //info->m_upperLimit[srow] = SIMD_INFINITY; + + btScalar dt = BT_ONE / info->fps; + btScalar kd = limot->m_springDamping; + btScalar ks = limot->m_springStiffness; + btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1); +// btScalar erp = 0.1; + btScalar cfm = BT_ZERO; + btScalar mA = BT_ONE / m_rbA.getInvMass(); + btScalar mB = BT_ONE / m_rbB.getInvMass(); + btScalar m = mA > mB ? mB : mA; + btScalar angularfreq = sqrt(ks / m); + + + //limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency) + if(limot->m_springStiffnessLimited && 0.25 < angularfreq * dt) + { + ks = BT_ONE / dt / dt / btScalar(16.0) * m; + } + //avoid damping that would blow up the spring + if(limot->m_springDampingLimited && kd * dt > m) + { + kd = m / dt; + } + btScalar fs = ks * error * dt; + btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt; + btScalar f = (fs+fd); + + info->m_constraintError[srow] = (vel + f * (rotational ? -1 : 1)) ; + + btScalar minf = f < fd ? f : fd; + btScalar maxf = f < fd ? fd : f; + if(!rotational) + { + info->m_lowerLimit[srow] = minf > 0 ? 0 : minf; + info->m_upperLimit[srow] = maxf < 0 ? 0 : maxf; + } + else + { + info->m_lowerLimit[srow] = -maxf > 0 ? 0 : -maxf; + info->m_upperLimit[srow] = -minf < 0 ? 0 : -minf; + } + + info->cfm[srow] = cfm; + srow += info->rowskip; + ++count; + } + + return count; +} + + +//override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). +//If no axis is provided, it uses the default axis for this constraint. +void btGeneric6DofSpring2Constraint::setParam(int num, btScalar value, int axis) +{ + if((axis >= 0) && (axis < 3)) + { + switch(num) + { + case BT_CONSTRAINT_STOP_ERP : + m_linearLimits.m_stopERP[axis] = value; + m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + case BT_CONSTRAINT_STOP_CFM : + m_linearLimits.m_stopCFM[axis] = value; + m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + case BT_CONSTRAINT_ERP : + m_linearLimits.m_motorERP[axis] = value; + m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + case BT_CONSTRAINT_CFM : + m_linearLimits.m_motorCFM[axis] = value; + m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + default : + btAssertConstrParams(0); + } + } + else if((axis >=3) && (axis < 6)) + { + switch(num) + { + case BT_CONSTRAINT_STOP_ERP : + m_angularLimits[axis - 3].m_stopERP = value; + m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + case BT_CONSTRAINT_STOP_CFM : + m_angularLimits[axis - 3].m_stopCFM = value; + m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + case BT_CONSTRAINT_ERP : + m_angularLimits[axis - 3].m_motorERP = value; + m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + case BT_CONSTRAINT_CFM : + m_angularLimits[axis - 3].m_motorCFM = value; + m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + default : + btAssertConstrParams(0); + } + } + else + { + btAssertConstrParams(0); + } +} + +//return the local value of parameter +btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const +{ + btScalar retVal = 0; + if((axis >= 0) && (axis < 3)) + { + switch(num) + { + case BT_CONSTRAINT_STOP_ERP : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_linearLimits.m_stopERP[axis]; + break; + case BT_CONSTRAINT_STOP_CFM : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_linearLimits.m_stopCFM[axis]; + break; + case BT_CONSTRAINT_ERP : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_linearLimits.m_motorERP[axis]; + break; + case BT_CONSTRAINT_CFM : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_linearLimits.m_motorCFM[axis]; + break; + default : + btAssertConstrParams(0); + } + } + else if((axis >=3) && (axis < 6)) + { + switch(num) + { + case BT_CONSTRAINT_STOP_ERP : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_angularLimits[axis - 3].m_stopERP; + break; + case BT_CONSTRAINT_STOP_CFM : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_angularLimits[axis - 3].m_stopCFM; + break; + case BT_CONSTRAINT_ERP : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_angularLimits[axis - 3].m_motorERP; + break; + case BT_CONSTRAINT_CFM : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_angularLimits[axis - 3].m_motorCFM; + break; + default : + btAssertConstrParams(0); + } + } + else + { + btAssertConstrParams(0); + } + return retVal; +} + + + +void btGeneric6DofSpring2Constraint::setAxis(const btVector3& axis1,const btVector3& axis2) +{ + btVector3 zAxis = axis1.normalized(); + btVector3 yAxis = axis2.normalized(); + btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system + + btTransform frameInW; + frameInW.setIdentity(); + frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], + xAxis[1], yAxis[1], zAxis[1], + xAxis[2], yAxis[2], zAxis[2]); + + // now get constraint frame in local coordinate systems + m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW; + m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW; + + calculateTransforms(); +} + +void btGeneric6DofSpring2Constraint::setBounce(int index, btScalar bounce) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_bounce[index] = bounce; + else + m_angularLimits[index - 3].m_bounce = bounce; +} + +void btGeneric6DofSpring2Constraint::enableMotor(int index, bool onOff) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_enableMotor[index] = onOff; + else + m_angularLimits[index - 3].m_enableMotor = onOff; +} + +void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_servoMotor[index] = onOff; + else + m_angularLimits[index - 3].m_servoMotor = onOff; +} + +void btGeneric6DofSpring2Constraint::setTargetVelocity(int index, btScalar velocity) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_targetVelocity[index] = velocity; + else + m_angularLimits[index - 3].m_targetVelocity = velocity; +} + +void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar target) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_servoTarget[index] = target; + else + m_angularLimits[index - 3].m_servoTarget = target; +} + +void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_maxMotorForce[index] = force; + else + m_angularLimits[index - 3].m_maxMotorForce = force; +} + +void btGeneric6DofSpring2Constraint::enableSpring(int index, bool onOff) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_enableSpring[index] = onOff; + else + m_angularLimits[index - 3] .m_enableSpring = onOff; +} + +void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness, bool limitIfNeeded) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) { + m_linearLimits.m_springStiffness[index] = stiffness; + m_linearLimits.m_springStiffnessLimited[index] = limitIfNeeded; + } else { + m_angularLimits[index - 3].m_springStiffness = stiffness; + m_angularLimits[index - 3].m_springStiffnessLimited = limitIfNeeded; + } +} + +void btGeneric6DofSpring2Constraint::setDamping(int index, btScalar damping, bool limitIfNeeded) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) { + m_linearLimits.m_springDamping[index] = damping; + m_linearLimits.m_springDampingLimited[index] = limitIfNeeded; + } else { + m_angularLimits[index - 3].m_springDamping = damping; + m_angularLimits[index - 3].m_springDampingLimited = limitIfNeeded; + } +} + +void btGeneric6DofSpring2Constraint::setEquilibriumPoint() +{ + calculateTransforms(); + int i; + for( i = 0; i < 3; i++) + m_linearLimits.m_equilibriumPoint[i] = m_calculatedLinearDiff[i]; + for(i = 0; i < 3; i++) + m_angularLimits[i].m_equilibriumPoint = m_calculatedAxisAngleDiff[i]; +} + +void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index) +{ + btAssert((index >= 0) && (index < 6)); + calculateTransforms(); + if (index<3) + m_linearLimits.m_equilibriumPoint[index] = m_calculatedLinearDiff[index]; + else + m_angularLimits[index - 3] .m_equilibriumPoint = m_calculatedAxisAngleDiff[index - 3]; +} + +void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index, btScalar val) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_equilibriumPoint[index] = val; + else + m_angularLimits[index - 3] .m_equilibriumPoint = val; +} + + +//////////////////////////// btRotationalLimitMotor2 //////////////////////////////////// + +void btRotationalLimitMotor2::testLimitValue(btScalar test_value) +{ + //we can't normalize the angles here because we would lost the sign that we use later, but it doesn't seem to be a problem + if(m_loLimit > m_hiLimit) { + m_currentLimit = 0; + m_currentLimitError = btScalar(0.f); + } + else if(m_loLimit == m_hiLimit) { + m_currentLimitError = test_value - m_loLimit; + m_currentLimit = 3; + } else { + m_currentLimitError = test_value - m_loLimit; + m_currentLimitErrorHi = test_value - m_hiLimit; + m_currentLimit = 4; + } +} + +//////////////////////////// btTranslationalLimitMotor2 //////////////////////////////////// + +void btTranslationalLimitMotor2::testLimitValue(int limitIndex, btScalar test_value) +{ + btScalar loLimit = m_lowerLimit[limitIndex]; + btScalar hiLimit = m_upperLimit[limitIndex]; + if(loLimit > hiLimit) { + m_currentLimitError[limitIndex] = 0; + m_currentLimit[limitIndex] = 0; + } + else if(loLimit == hiLimit) { + m_currentLimitError[limitIndex] = test_value - loLimit; + m_currentLimit[limitIndex] = 3; + } else { + m_currentLimitError[limitIndex] = test_value - loLimit; + m_currentLimitErrorHi[limitIndex] = test_value - hiLimit; + m_currentLimit[limitIndex] = 4; + } +} + + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h new file mode 100644 index 00000000000..ace4b3c29bf --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h @@ -0,0 +1,674 @@ +/* +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. +*/ + +/* +2014 May: btGeneric6DofSpring2Constraint is created from the original (2.82.2712) btGeneric6DofConstraint by Gabor Puhr and Tamas Umenhoffer +Pros: +- Much more accurate and stable in a lot of situation. (Especially when a sleeping chain of RBs connected with 6dof2 is pulled) +- Stable and accurate spring with minimal energy loss that works with all of the solvers. (latter is not true for the original 6dof spring) +- Servo motor functionality +- Much more accurate bouncing. 0 really means zero bouncing (not true for the original 6odf) and there is only a minimal energy loss when the value is 1 (because of the solvers' precision) +- Rotation order for the Euler system can be set. (One axis' freedom is still limited to pi/2) + +Cons: +- It is slower than the original 6dof. There is no exact ratio, but half speed is a good estimation. +- At bouncing the correct velocity is calculated, but not the correct position. (it is because of the solver can correct position or velocity, but not both.) +*/ + +/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev +/// Added support for generic constraint solver through getInfo1/getInfo2 methods + +/* +2007-09-09 +btGeneric6DofConstraint Refactored by Francisco Le?n +email: projectileman@yahoo.com +http://gimpact.sf.net +*/ + + +#ifndef BT_GENERIC_6DOF_CONSTRAINT2_H +#define BT_GENERIC_6DOF_CONSTRAINT2_H + +#include "LinearMath/btVector3.h" +#include "btJacobianEntry.h" +#include "btTypedConstraint.h" + +class btRigidBody; + + +#ifdef BT_USE_DOUBLE_PRECISION +#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintDoubleData2 +#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintDoubleData2" +#else +#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintData +#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintData" +#endif //BT_USE_DOUBLE_PRECISION + +enum RotateOrder +{ + RO_XYZ=0, + RO_XZY, + RO_YXZ, + RO_YZX, + RO_ZXY, + RO_ZYX +}; + +class btRotationalLimitMotor2 +{ +public: +// upper < lower means free +// upper == lower means locked +// upper > lower means limited + btScalar m_loLimit; + btScalar m_hiLimit; + btScalar m_bounce; + btScalar m_stopERP; + btScalar m_stopCFM; + btScalar m_motorERP; + btScalar m_motorCFM; + bool m_enableMotor; + btScalar m_targetVelocity; + btScalar m_maxMotorForce; + bool m_servoMotor; + btScalar m_servoTarget; + bool m_enableSpring; + btScalar m_springStiffness; + bool m_springStiffnessLimited; + btScalar m_springDamping; + bool m_springDampingLimited; + btScalar m_equilibriumPoint; + + btScalar m_currentLimitError; + btScalar m_currentLimitErrorHi; + btScalar m_currentPosition; + int m_currentLimit; + + btRotationalLimitMotor2() + { + m_loLimit = 1.0f; + m_hiLimit = -1.0f; + m_bounce = 0.0f; + m_stopERP = 0.2f; + m_stopCFM = 0.f; + m_motorERP = 0.9f; + m_motorCFM = 0.f; + m_enableMotor = false; + m_targetVelocity = 0; + m_maxMotorForce = 0.1f; + m_servoMotor = false; + m_servoTarget = 0; + m_enableSpring = false; + m_springStiffness = 0; + m_springStiffnessLimited = false; + m_springDamping = 0; + m_springDampingLimited = false; + m_equilibriumPoint = 0; + + m_currentLimitError = 0; + m_currentLimitErrorHi = 0; + m_currentPosition = 0; + m_currentLimit = 0; + } + + btRotationalLimitMotor2(const btRotationalLimitMotor2 & limot) + { + m_loLimit = limot.m_loLimit; + m_hiLimit = limot.m_hiLimit; + m_bounce = limot.m_bounce; + m_stopERP = limot.m_stopERP; + m_stopCFM = limot.m_stopCFM; + m_motorERP = limot.m_motorERP; + m_motorCFM = limot.m_motorCFM; + m_enableMotor = limot.m_enableMotor; + m_targetVelocity = limot.m_targetVelocity; + m_maxMotorForce = limot.m_maxMotorForce; + m_servoMotor = limot.m_servoMotor; + m_servoTarget = limot.m_servoTarget; + m_enableSpring = limot.m_enableSpring; + m_springStiffness = limot.m_springStiffness; + m_springStiffnessLimited = limot.m_springStiffnessLimited; + m_springDamping = limot.m_springDamping; + m_springDampingLimited = limot.m_springDampingLimited; + m_equilibriumPoint = limot.m_equilibriumPoint; + + m_currentLimitError = limot.m_currentLimitError; + m_currentLimitErrorHi = limot.m_currentLimitErrorHi; + m_currentPosition = limot.m_currentPosition; + m_currentLimit = limot.m_currentLimit; + } + + + bool isLimited() + { + if(m_loLimit > m_hiLimit) return false; + return true; + } + + void testLimitValue(btScalar test_value); +}; + + + +class btTranslationalLimitMotor2 +{ +public: +// upper < lower means free +// upper == lower means locked +// upper > lower means limited + btVector3 m_lowerLimit; + btVector3 m_upperLimit; + btVector3 m_bounce; + btVector3 m_stopERP; + btVector3 m_stopCFM; + btVector3 m_motorERP; + btVector3 m_motorCFM; + bool m_enableMotor[3]; + bool m_servoMotor[3]; + bool m_enableSpring[3]; + btVector3 m_servoTarget; + btVector3 m_springStiffness; + bool m_springStiffnessLimited[3]; + btVector3 m_springDamping; + bool m_springDampingLimited[3]; + btVector3 m_equilibriumPoint; + btVector3 m_targetVelocity; + btVector3 m_maxMotorForce; + + btVector3 m_currentLimitError; + btVector3 m_currentLimitErrorHi; + btVector3 m_currentLinearDiff; + int m_currentLimit[3]; + + btTranslationalLimitMotor2() + { + m_lowerLimit .setValue(0.f , 0.f , 0.f ); + m_upperLimit .setValue(0.f , 0.f , 0.f ); + m_bounce .setValue(0.f , 0.f , 0.f ); + m_stopERP .setValue(0.2f, 0.2f, 0.2f); + m_stopCFM .setValue(0.f , 0.f , 0.f ); + m_motorERP .setValue(0.9f, 0.9f, 0.9f); + m_motorCFM .setValue(0.f , 0.f , 0.f ); + + m_currentLimitError .setValue(0.f , 0.f , 0.f ); + m_currentLimitErrorHi.setValue(0.f , 0.f , 0.f ); + m_currentLinearDiff .setValue(0.f , 0.f , 0.f ); + + for(int i=0; i < 3; i++) + { + m_enableMotor[i] = false; + m_servoMotor[i] = false; + m_enableSpring[i] = false; + m_servoTarget[i] = btScalar(0.f); + m_springStiffness[i] = btScalar(0.f); + m_springStiffnessLimited[i] = false; + m_springDamping[i] = btScalar(0.f); + m_springDampingLimited[i] = false; + m_equilibriumPoint[i] = btScalar(0.f); + m_targetVelocity[i] = btScalar(0.f); + m_maxMotorForce[i] = btScalar(0.f); + + m_currentLimit[i] = 0; + } + } + + btTranslationalLimitMotor2(const btTranslationalLimitMotor2 & other ) + { + m_lowerLimit = other.m_lowerLimit; + m_upperLimit = other.m_upperLimit; + m_bounce = other.m_bounce; + m_stopERP = other.m_stopERP; + m_stopCFM = other.m_stopCFM; + m_motorERP = other.m_motorERP; + m_motorCFM = other.m_motorCFM; + + m_currentLimitError = other.m_currentLimitError; + m_currentLimitErrorHi = other.m_currentLimitErrorHi; + m_currentLinearDiff = other.m_currentLinearDiff; + + for(int i=0; i < 3; i++) + { + m_enableMotor[i] = other.m_enableMotor[i]; + m_servoMotor[i] = other.m_servoMotor[i]; + m_enableSpring[i] = other.m_enableSpring[i]; + m_servoTarget[i] = other.m_servoTarget[i]; + m_springStiffness[i] = other.m_springStiffness[i]; + m_springStiffnessLimited[i] = other.m_springStiffnessLimited[i]; + m_springDamping[i] = other.m_springDamping[i]; + m_springDampingLimited[i] = other.m_springDampingLimited[i]; + m_equilibriumPoint[i] = other.m_equilibriumPoint[i]; + m_targetVelocity[i] = other.m_targetVelocity[i]; + m_maxMotorForce[i] = other.m_maxMotorForce[i]; + + m_currentLimit[i] = other.m_currentLimit[i]; + } + } + + inline bool isLimited(int limitIndex) + { + return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); + } + + void testLimitValue(int limitIndex, btScalar test_value); +}; + +enum bt6DofFlags2 +{ + BT_6DOF_FLAGS_CFM_STOP2 = 1, + BT_6DOF_FLAGS_ERP_STOP2 = 2, + BT_6DOF_FLAGS_CFM_MOTO2 = 4, + BT_6DOF_FLAGS_ERP_MOTO2 = 8 +}; +#define BT_6DOF_FLAGS_AXIS_SHIFT2 4 // bits per axis + + +ATTRIBUTE_ALIGNED16(class) btGeneric6DofSpring2Constraint : public btTypedConstraint +{ +protected: + + btTransform m_frameInA; + btTransform m_frameInB; + + btJacobianEntry m_jacLinear[3]; + btJacobianEntry m_jacAng[3]; + + btTranslationalLimitMotor2 m_linearLimits; + btRotationalLimitMotor2 m_angularLimits[3]; + + RotateOrder m_rotateOrder; + +protected: + + btTransform m_calculatedTransformA; + btTransform m_calculatedTransformB; + btVector3 m_calculatedAxisAngleDiff; + btVector3 m_calculatedAxis[3]; + btVector3 m_calculatedLinearDiff; + btScalar m_factA; + btScalar m_factB; + bool m_hasStaticBody; + int m_flags; + + btGeneric6DofSpring2Constraint& operator=(btGeneric6DofSpring2Constraint&) + { + btAssert(0); + return *this; + } + + int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); + int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); + + void calculateLinearInfo(); + void calculateAngleInfo(); + void testAngularLimitMotor(int axis_index); + + void calculateJacobi(btRotationalLimitMotor2* limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2* info, int srow, btVector3& ax1, int rotational, int rotAllowed); + int get_limit_motor_info2(btRotationalLimitMotor2* limot, + const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB, + btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed = false); + + static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index); + static bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz); + static bool matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz); + static bool matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz); + static bool matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz); + static bool matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz); + static bool matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz); + +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ); + btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ); + + virtual void buildJacobian() {} + virtual void getInfo1 (btConstraintInfo1* info); + virtual void getInfo2 (btConstraintInfo2* info); + virtual int calculateSerializeBufferSize() const; + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + + btRotationalLimitMotor2* getRotationalLimitMotor(int index) { return &m_angularLimits[index]; } + btTranslationalLimitMotor2* getTranslationalLimitMotor() { return &m_linearLimits; } + + // Calculates the global transform for the joint offset for body A an B, and also calculates the angle differences between the bodies. + void calculateTransforms(const btTransform& transA,const btTransform& transB); + void calculateTransforms(); + + // Gets the global transform of the offset for body A + const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; } + // Gets the global transform of the offset for body B + 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; } + + // Get the rotation axis in global coordinates ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously ) + btVector3 getAxis(int axis_index) const { return m_calculatedAxis[axis_index]; } + + // Get the relative Euler angle ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously ) + btScalar getAngle(int axis_index) const { return m_calculatedAxisAngleDiff[axis_index]; } + + // Get the relative position of the constraint pivot ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously ) + btScalar getRelativePivotPosition(int axis_index) const { return m_calculatedLinearDiff[axis_index]; } + + void setFrames(const btTransform & frameA, const btTransform & frameB); + + void setLinearLowerLimit(const btVector3& linearLower) { m_linearLimits.m_lowerLimit = linearLower; } + void getLinearLowerLimit(btVector3& linearLower) { linearLower = m_linearLimits.m_lowerLimit; } + void setLinearUpperLimit(const btVector3& linearUpper) { m_linearLimits.m_upperLimit = linearUpper; } + void getLinearUpperLimit(btVector3& linearUpper) { linearUpper = m_linearLimits.m_upperLimit; } + + void setAngularLowerLimit(const btVector3& angularLower) + { + for(int i = 0; i < 3; i++) + m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]); + } + + void setAngularLowerLimitReversed(const btVector3& angularLower) + { + for(int i = 0; i < 3; i++) + m_angularLimits[i].m_hiLimit = btNormalizeAngle(-angularLower[i]); + } + + void getAngularLowerLimit(btVector3& angularLower) + { + for(int i = 0; i < 3; i++) + angularLower[i] = m_angularLimits[i].m_loLimit; + } + + void getAngularLowerLimitReversed(btVector3& angularLower) + { + for(int i = 0; i < 3; i++) + angularLower[i] = -m_angularLimits[i].m_hiLimit; + } + + void setAngularUpperLimit(const btVector3& angularUpper) + { + for(int i = 0; i < 3; i++) + m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]); + } + + void setAngularUpperLimitReversed(const btVector3& angularUpper) + { + for(int i = 0; i < 3; i++) + m_angularLimits[i].m_loLimit = btNormalizeAngle(-angularUpper[i]); + } + + void getAngularUpperLimit(btVector3& angularUpper) + { + for(int i = 0; i < 3; i++) + angularUpper[i] = m_angularLimits[i].m_hiLimit; + } + + void getAngularUpperLimitReversed(btVector3& angularUpper) + { + for(int i = 0; i < 3; i++) + angularUpper[i] = -m_angularLimits[i].m_loLimit; + } + + //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 + { + lo = btNormalizeAngle(lo); + hi = btNormalizeAngle(hi); + m_angularLimits[axis-3].m_loLimit = lo; + m_angularLimits[axis-3].m_hiLimit = hi; + } + } + + void setLimitReversed(int axis, btScalar lo, btScalar hi) + { + if(axis<3) + { + m_linearLimits.m_lowerLimit[axis] = lo; + m_linearLimits.m_upperLimit[axis] = hi; + } + else + { + lo = btNormalizeAngle(lo); + hi = btNormalizeAngle(hi); + m_angularLimits[axis-3].m_hiLimit = -lo; + m_angularLimits[axis-3].m_loLimit = -hi; + } + } + + bool isLimited(int limitIndex) + { + if(limitIndex<3) + { + return m_linearLimits.isLimited(limitIndex); + } + return m_angularLimits[limitIndex-3].isLimited(); + } + + void setRotationOrder(RotateOrder order) { m_rotateOrder = order; } + RotateOrder getRotationOrder() { return m_rotateOrder; } + + void setAxis( const btVector3& axis1, const btVector3& axis2); + + void setBounce(int index, btScalar bounce); + + void enableMotor(int index, bool onOff); + void setServo(int index, bool onOff); // set the type of the motor (servo or not) (the motor has to be turned on for servo also) + void setTargetVelocity(int index, btScalar velocity); + void setServoTarget(int index, btScalar target); + void setMaxMotorForce(int index, btScalar force); + + void enableSpring(int index, bool onOff); + void setStiffness(int index, btScalar stiffness, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the stiffness in necessary situations where otherwise the spring would move unrealistically too widely + void setDamping(int index, btScalar damping, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the damping in necessary situations where otherwise the spring would blow up + 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 + void setEquilibriumPoint(int index, btScalar val); + + //override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). + //If no axis is provided, it uses the default axis for this constraint. + virtual void setParam(int num, btScalar value, int axis = -1); + virtual btScalar getParam(int num, int axis = -1) const; +}; + + +struct btGeneric6DofSpring2ConstraintData +{ + btTypedConstraintData m_typeConstraintData; + btTransformFloatData m_rbAFrame; + btTransformFloatData m_rbBFrame; + + btVector3FloatData m_linearUpperLimit; + btVector3FloatData m_linearLowerLimit; + btVector3FloatData m_linearBounce; + btVector3FloatData m_linearStopERP; + btVector3FloatData m_linearStopCFM; + btVector3FloatData m_linearMotorERP; + btVector3FloatData m_linearMotorCFM; + btVector3FloatData m_linearTargetVelocity; + btVector3FloatData m_linearMaxMotorForce; + btVector3FloatData m_linearServoTarget; + btVector3FloatData m_linearSpringStiffness; + btVector3FloatData m_linearSpringDamping; + btVector3FloatData m_linearEquilibriumPoint; + char m_linearEnableMotor[4]; + char m_linearServoMotor[4]; + char m_linearEnableSpring[4]; + char m_linearSpringStiffnessLimited[4]; + char m_linearSpringDampingLimited[4]; + char m_padding1[4]; + + btVector3FloatData m_angularUpperLimit; + btVector3FloatData m_angularLowerLimit; + btVector3FloatData m_angularBounce; + btVector3FloatData m_angularStopERP; + btVector3FloatData m_angularStopCFM; + btVector3FloatData m_angularMotorERP; + btVector3FloatData m_angularMotorCFM; + btVector3FloatData m_angularTargetVelocity; + btVector3FloatData m_angularMaxMotorForce; + btVector3FloatData m_angularServoTarget; + btVector3FloatData m_angularSpringStiffness; + btVector3FloatData m_angularSpringDamping; + btVector3FloatData m_angularEquilibriumPoint; + char m_angularEnableMotor[4]; + char m_angularServoMotor[4]; + char m_angularEnableSpring[4]; + char m_angularSpringStiffnessLimited[4]; + char m_angularSpringDampingLimited[4]; + + int m_rotateOrder; +}; + +struct btGeneric6DofSpring2ConstraintDoubleData2 +{ + btTypedConstraintDoubleData m_typeConstraintData; + btTransformDoubleData m_rbAFrame; + btTransformDoubleData m_rbBFrame; + + btVector3DoubleData m_linearUpperLimit; + btVector3DoubleData m_linearLowerLimit; + btVector3DoubleData m_linearBounce; + btVector3DoubleData m_linearStopERP; + btVector3DoubleData m_linearStopCFM; + btVector3DoubleData m_linearMotorERP; + btVector3DoubleData m_linearMotorCFM; + btVector3DoubleData m_linearTargetVelocity; + btVector3DoubleData m_linearMaxMotorForce; + btVector3DoubleData m_linearServoTarget; + btVector3DoubleData m_linearSpringStiffness; + btVector3DoubleData m_linearSpringDamping; + btVector3DoubleData m_linearEquilibriumPoint; + char m_linearEnableMotor[4]; + char m_linearServoMotor[4]; + char m_linearEnableSpring[4]; + char m_linearSpringStiffnessLimited[4]; + char m_linearSpringDampingLimited[4]; + char m_padding1[4]; + + btVector3DoubleData m_angularUpperLimit; + btVector3DoubleData m_angularLowerLimit; + btVector3DoubleData m_angularBounce; + btVector3DoubleData m_angularStopERP; + btVector3DoubleData m_angularStopCFM; + btVector3DoubleData m_angularMotorERP; + btVector3DoubleData m_angularMotorCFM; + btVector3DoubleData m_angularTargetVelocity; + btVector3DoubleData m_angularMaxMotorForce; + btVector3DoubleData m_angularServoTarget; + btVector3DoubleData m_angularSpringStiffness; + btVector3DoubleData m_angularSpringDamping; + btVector3DoubleData m_angularEquilibriumPoint; + char m_angularEnableMotor[4]; + char m_angularServoMotor[4]; + char m_angularEnableSpring[4]; + char m_angularSpringStiffnessLimited[4]; + char m_angularSpringDampingLimited[4]; + + int m_rotateOrder; +}; + +SIMD_FORCE_INLINE int btGeneric6DofSpring2Constraint::calculateSerializeBufferSize() const +{ + return sizeof(btGeneric6DofSpring2ConstraintData2); +} + +SIMD_FORCE_INLINE const char* btGeneric6DofSpring2Constraint::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btGeneric6DofSpring2ConstraintData2* dof = (btGeneric6DofSpring2ConstraintData2*)dataBuffer; + btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer); + + m_frameInA.serialize(dof->m_rbAFrame); + m_frameInB.serialize(dof->m_rbBFrame); + + int i; + for (i=0;i<3;i++) + { + dof->m_angularLowerLimit.m_floats[i] = m_angularLimits[i].m_loLimit; + dof->m_angularUpperLimit.m_floats[i] = m_angularLimits[i].m_hiLimit; + dof->m_angularBounce.m_floats[i] = m_angularLimits[i].m_bounce; + dof->m_angularStopERP.m_floats[i] = m_angularLimits[i].m_stopERP; + dof->m_angularStopCFM.m_floats[i] = m_angularLimits[i].m_stopCFM; + dof->m_angularMotorERP.m_floats[i] = m_angularLimits[i].m_motorERP; + dof->m_angularMotorCFM.m_floats[i] = m_angularLimits[i].m_motorCFM; + dof->m_angularTargetVelocity.m_floats[i] = m_angularLimits[i].m_targetVelocity; + dof->m_angularMaxMotorForce.m_floats[i] = m_angularLimits[i].m_maxMotorForce; + dof->m_angularServoTarget.m_floats[i] = m_angularLimits[i].m_servoTarget; + dof->m_angularSpringStiffness.m_floats[i] = m_angularLimits[i].m_springStiffness; + dof->m_angularSpringDamping.m_floats[i] = m_angularLimits[i].m_springDamping; + dof->m_angularEquilibriumPoint.m_floats[i] = m_angularLimits[i].m_equilibriumPoint; + } + dof->m_angularLowerLimit.m_floats[3] = 0; + dof->m_angularUpperLimit.m_floats[3] = 0; + dof->m_angularBounce.m_floats[3] = 0; + dof->m_angularStopERP.m_floats[3] = 0; + dof->m_angularStopCFM.m_floats[3] = 0; + dof->m_angularMotorERP.m_floats[3] = 0; + dof->m_angularMotorCFM.m_floats[3] = 0; + dof->m_angularTargetVelocity.m_floats[3] = 0; + dof->m_angularMaxMotorForce.m_floats[3] = 0; + dof->m_angularServoTarget.m_floats[3] = 0; + dof->m_angularSpringStiffness.m_floats[3] = 0; + dof->m_angularSpringDamping.m_floats[3] = 0; + dof->m_angularEquilibriumPoint.m_floats[3] = 0; + for (i=0;i<4;i++) + { + dof->m_angularEnableMotor[i] = i < 3 ? ( m_angularLimits[i].m_enableMotor ? 1 : 0 ) : 0; + dof->m_angularServoMotor[i] = i < 3 ? ( m_angularLimits[i].m_servoMotor ? 1 : 0 ) : 0; + dof->m_angularEnableSpring[i] = i < 3 ? ( m_angularLimits[i].m_enableSpring ? 1 : 0 ) : 0; + dof->m_angularSpringStiffnessLimited[i] = i < 3 ? ( m_angularLimits[i].m_springStiffnessLimited ? 1 : 0 ) : 0; + dof->m_angularSpringDampingLimited[i] = i < 3 ? ( m_angularLimits[i].m_springDampingLimited ? 1 : 0 ) : 0; + } + + m_linearLimits.m_lowerLimit.serialize( dof->m_linearLowerLimit ); + m_linearLimits.m_upperLimit.serialize( dof->m_linearUpperLimit ); + m_linearLimits.m_bounce.serialize( dof->m_linearBounce ); + m_linearLimits.m_stopERP.serialize( dof->m_linearStopERP ); + m_linearLimits.m_stopCFM.serialize( dof->m_linearStopCFM ); + m_linearLimits.m_motorERP.serialize( dof->m_linearMotorERP ); + m_linearLimits.m_motorCFM.serialize( dof->m_linearMotorCFM ); + m_linearLimits.m_targetVelocity.serialize( dof->m_linearTargetVelocity ); + m_linearLimits.m_maxMotorForce.serialize( dof->m_linearMaxMotorForce ); + m_linearLimits.m_servoTarget.serialize( dof->m_linearServoTarget ); + m_linearLimits.m_springStiffness.serialize( dof->m_linearSpringStiffness ); + m_linearLimits.m_springDamping.serialize( dof->m_linearSpringDamping ); + m_linearLimits.m_equilibriumPoint.serialize( dof->m_linearEquilibriumPoint ); + for (i=0;i<4;i++) + { + dof->m_linearEnableMotor[i] = i < 3 ? ( m_linearLimits.m_enableMotor[i] ? 1 : 0 ) : 0; + dof->m_linearServoMotor[i] = i < 3 ? ( m_linearLimits.m_servoMotor[i] ? 1 : 0 ) : 0; + dof->m_linearEnableSpring[i] = i < 3 ? ( m_linearLimits.m_enableSpring[i] ? 1 : 0 ) : 0; + dof->m_linearSpringStiffnessLimited[i] = i < 3 ? ( m_linearLimits.m_springStiffnessLimited[i] ? 1 : 0 ) : 0; + dof->m_linearSpringDampingLimited[i] = i < 3 ? ( m_linearLimits.m_springDampingLimited[i] ? 1 : 0 ) : 0; + } + + dof->m_rotateOrder = m_rotateOrder; + + return btGeneric6DofSpring2ConstraintDataName; +} + + + + + +#endif //BT_GENERIC_6DOF_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h index 1b2e0f62ca8..dac59c6889d 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h @@ -63,6 +63,26 @@ public: void setEquilibriumPoint(int index); // set the current constraint position/orientation as an equilibrium point for given DOF void setEquilibriumPoint(int index, btScalar val); + bool isSpringEnabled(int index) const + { + return m_springEnabled[index]; + } + + btScalar getStiffness(int index) const + { + return m_springStiffness[index]; + } + + btScalar getDamping(int index) const + { + return m_springDamping[index]; + } + + btScalar getEquilibriumPoint(int index) const + { + return m_equilibriumPoint[index]; + } + virtual void setAxis( const btVector3& axis1, const btVector3& axis2); virtual void getInfo2 (btConstraintInfo2* info); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp index 29123d526b4..4be2aabe4d3 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp @@ -25,7 +25,7 @@ subject to the following restrictions: // anchor, axis1 and axis2 are in world coordinate system // axis1 must be orthogonal to axis2 btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2) -: btGeneric6DofSpringConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true), +: btGeneric6DofSpring2Constraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(),RO_XYZ), m_anchor(anchor), m_axis1(axis1), m_axis2(axis2) @@ -59,7 +59,7 @@ btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVec setAngularUpperLimit(btVector3(-1.f, 0.f, SIMD_HALF_PI * 0.5f)); // enable suspension enableSpring(2, true); - setStiffness(2, SIMD_PI * SIMD_PI * 4.f); // period 1 sec for 1 kilogramm weel :-) + setStiffness(2, SIMD_PI * SIMD_PI * 4.f); setDamping(2, 0.01f); setEquilibriumPoint(); } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h index 9a004986911..06a8e3ecd14 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h @@ -20,7 +20,7 @@ subject to the following restrictions: #include "LinearMath/btVector3.h" #include "btTypedConstraint.h" -#include "btGeneric6DofSpringConstraint.h" +#include "btGeneric6DofSpring2Constraint.h" @@ -29,7 +29,7 @@ subject to the following restrictions: // 2 rotational degrees of freedom, similar to Euler rotations around Z (axis 1) and X (axis 2) // 1 translational (along axis Z) with suspension spring -ATTRIBUTE_ALIGNED16(class) btHinge2Constraint : public btGeneric6DofSpringConstraint +ATTRIBUTE_ALIGNED16(class) btHinge2Constraint : public btGeneric6DofSpring2Constraint { protected: btVector3 m_anchor; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index c1897413078..76a1509471e 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -45,7 +45,11 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), - m_flags(0) + m_flags(0), + m_normalCFM(0), + m_normalERP(0), + m_stopCFM(0), + m_stopERP(0) { m_rbAFrame.getOrigin() = pivotInA; @@ -101,7 +105,11 @@ m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), -m_flags(0) +m_flags(0), +m_normalCFM(0), +m_normalERP(0), +m_stopCFM(0), +m_stopERP(0) { // since no frame is given, assume this to be zero angle and just pick rb transform axis @@ -151,7 +159,11 @@ m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), -m_flags(0) +m_flags(0), +m_normalCFM(0), +m_normalERP(0), +m_stopCFM(0), +m_stopERP(0) { #ifndef _BT_USE_CENTER_LIMIT_ //start with free @@ -177,7 +189,11 @@ m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), -m_flags(0) +m_flags(0), +m_normalCFM(0), +m_normalERP(0), +m_stopCFM(0), +m_stopERP(0) { ///not providing rigidbody B means implicitly using worldspace for body B @@ -285,8 +301,60 @@ void btHingeConstraint::buildJacobian() #endif //__SPU__ +static inline btScalar btNormalizeAnglePositive(btScalar angle) +{ + return btFmod(btFmod(angle, btScalar(2.0*SIMD_PI)) + btScalar(2.0*SIMD_PI), btScalar(2.0*SIMD_PI)); +} + + + +static btScalar btShortestAngularDistance(btScalar accAngle, btScalar curAngle) +{ + btScalar result = btNormalizeAngle(btNormalizeAnglePositive(btNormalizeAnglePositive(curAngle) - + btNormalizeAnglePositive(accAngle))); + return result; +} + +static btScalar btShortestAngleUpdate(btScalar accAngle, btScalar curAngle) +{ + btScalar tol(0.3); + btScalar result = btShortestAngularDistance(accAngle, curAngle); + + if (btFabs(result) > tol) + return curAngle; + else + return accAngle + result; + + return curAngle; +} + + +btScalar btHingeAccumulatedAngleConstraint::getAccumulatedHingeAngle() +{ + btScalar hingeAngle = getHingeAngle(); + m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,hingeAngle); + return m_accumulatedAngle; +} +void btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle(btScalar accAngle) +{ + m_accumulatedAngle = accAngle; +} + +void btHingeAccumulatedAngleConstraint::getInfo1(btConstraintInfo1* info) +{ + //update m_accumulatedAngle + btScalar curHingeAngle = getHingeAngle(); + m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,curHingeAngle); + + btHingeConstraint::getInfo1(info); + +} + + void btHingeConstraint::getInfo1(btConstraintInfo1* info) { + + if (m_useSolveConstraintObsolete) { info->m_numConstraintRows = 0; @@ -413,7 +481,9 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf a2.getSkewSymmetricMatrix(angular0,angular1,angular2); } // linear RHS - btScalar k = info->fps * info->erp; + btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp; + + btScalar k = info->fps * normalErp; if (!m_angularOnly) { for(i = 0; i < 3; i++) @@ -510,7 +580,7 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf powered = 0; } info->m_constraintError[srow] = btScalar(0.0f); - btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp; + btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp; if(powered) { if(m_flags & BT_HINGE_FLAGS_CFM_NORM) @@ -606,6 +676,8 @@ void btHingeConstraint::updateRHS(btScalar timeStep) } + + btScalar btHingeConstraint::getHingeAngle() { return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); @@ -798,7 +870,8 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i]; for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i]; - btScalar k = info->fps * info->erp; + btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM)? m_normalERP : info->erp; + btScalar k = info->fps * normalErp; if (!m_angularOnly) { @@ -856,7 +929,8 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info // angular_velocity = (erp*fps) * (ax1 x ax2) // ax1 x ax2 is in the plane space of ax1, so we project the angular // velocity to p and q to find the right hand side. - k = info->fps * info->erp; + k = info->fps * normalErp;//?? + btVector3 u = ax1A.cross(ax1B); info->m_constraintError[s3] = k * u.dot(p); info->m_constraintError[s4] = k * u.dot(q); @@ -901,7 +975,7 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info powered = 0; } info->m_constraintError[srow] = btScalar(0.0f); - btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp; + btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp; if(powered) { if(m_flags & BT_HINGE_FLAGS_CFM_NORM) @@ -1002,6 +1076,10 @@ void btHingeConstraint::setParam(int num, btScalar value, int axis) m_normalCFM = value; m_flags |= BT_HINGE_FLAGS_CFM_NORM; break; + case BT_CONSTRAINT_ERP: + m_normalERP = value; + m_flags |= BT_HINGE_FLAGS_ERP_NORM; + break; default : btAssertConstrParams(0); } @@ -1032,6 +1110,10 @@ btScalar btHingeConstraint::getParam(int num, int axis) const btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM); retVal = m_normalCFM; break; + case BT_CONSTRAINT_ERP: + btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_NORM); + retVal = m_normalERP; + break; default : btAssertConstrParams(0); } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 7c33ac24e05..f26e72105ba 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -41,7 +41,8 @@ enum btHingeFlags { BT_HINGE_FLAGS_CFM_STOP = 1, BT_HINGE_FLAGS_ERP_STOP = 2, - BT_HINGE_FLAGS_CFM_NORM = 4 + BT_HINGE_FLAGS_CFM_NORM = 4, + BT_HINGE_FLAGS_ERP_NORM = 8 }; @@ -94,6 +95,7 @@ public: int m_flags; btScalar m_normalCFM; + btScalar m_normalERP; btScalar m_stopCFM; btScalar m_stopERP; @@ -175,6 +177,7 @@ public: // maintain a given angular target. void enableMotor(bool enableMotor) { m_enableAngularMotor = enableMotor; } void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; } + void setMotorTargetVelocity(btScalar motorTargetVelocity) { m_motorTargetVelocity = motorTargetVelocity; } void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B. void setMotorTarget(btScalar targetAngle, btScalar dt); @@ -191,6 +194,33 @@ public: m_relaxationFactor = _relaxationFactor; #endif } + + btScalar getLimitSoftness() const + { +#ifdef _BT_USE_CENTER_LIMIT_ + return m_limit.getSoftness(); +#else + return m_limitSoftness; +#endif + } + + btScalar getLimitBiasFactor() const + { +#ifdef _BT_USE_CENTER_LIMIT_ + return m_limit.getBiasFactor(); +#else + return m_biasFactor; +#endif + } + + btScalar getLimitRelaxationFactor() const + { +#ifdef _BT_USE_CENTER_LIMIT_ + return m_limit.getRelaxationFactor(); +#else + return m_relaxationFactor; +#endif + } void setAxis(btVector3& axisInA) { @@ -217,6 +247,14 @@ public: } + bool hasLimit() const { +#ifdef _BT_USE_CENTER_LIMIT_ + return m_limit.getHalfRange() > 0; +#else + return m_lowerLimit <= m_upperLimit; +#endif + } + btScalar getLowerLimit() const { #ifdef _BT_USE_CENTER_LIMIT_ @@ -236,6 +274,7 @@ public: } + ///The getHingeAngle gives the hinge angle in range [-PI,PI] btScalar getHingeAngle(); btScalar getHingeAngle(const btTransform& transA,const btTransform& transB); @@ -286,13 +325,20 @@ public: // access for UseFrameOffset bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; } void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; } - + // access for UseReferenceFrameA + bool getUseReferenceFrameA() const { return m_useReferenceFrameA; } + void setUseReferenceFrameA(bool useReferenceFrameA) { m_useReferenceFrameA = useReferenceFrameA; } ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). ///If no axis is provided, it uses the default axis for this constraint. virtual void setParam(int num, btScalar value, int axis = -1); ///return the local value of parameter virtual btScalar getParam(int num, int axis = -1) const; + + virtual int getFlags() const + { + return m_flags; + } virtual int calculateSerializeBufferSize() const; @@ -326,6 +372,43 @@ struct btHingeConstraintDoubleData }; #endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION +///The getAccumulatedHingeAngle returns the accumulated hinge angle, taking rotation across the -PI/PI boundary into account +ATTRIBUTE_ALIGNED16(class) btHingeAccumulatedAngleConstraint : public btHingeConstraint +{ +protected: + btScalar m_accumulatedAngle; +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false) + :btHingeConstraint(rbA,rbB,pivotInA,pivotInB, axisInA,axisInB, useReferenceFrameA ) + { + m_accumulatedAngle=getHingeAngle(); + } + + btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false) + :btHingeConstraint(rbA,pivotInA,axisInA, useReferenceFrameA) + { + m_accumulatedAngle=getHingeAngle(); + } + + btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false) + :btHingeConstraint(rbA,rbB, rbAFrame, rbBFrame, useReferenceFrameA ) + { + m_accumulatedAngle=getHingeAngle(); + } + + btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false) + :btHingeConstraint(rbA,rbAFrame, useReferenceFrameA ) + { + m_accumulatedAngle=getHingeAngle(); + } + btScalar getAccumulatedHingeAngle(); + void setAccumulatedHingeAngle(btScalar accAngle); + virtual void getInfo1 (btConstraintInfo1* info); + +}; struct btHingeConstraintFloatData { diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp new file mode 100644 index 00000000000..f110cd48081 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp @@ -0,0 +1,463 @@ +/* +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. +*/ + +#include "btNNCGConstraintSolver.h" + + + + + + +btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +{ + btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer); + + m_pNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size()); + m_pC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size()); + m_pCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size()); + m_pCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size()); + + m_deltafNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size()); + m_deltafC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size()); + m_deltafCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size()); + m_deltafCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size()); + + return val; +} + +btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/) +{ + + int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); + int numConstraintPool = m_tmpSolverContactConstraintPool.size(); + int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); + + if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) + { + if (1) // uncomment this for a bit less random ((iteration & 7) == 0) + { + + for (int j=0; j<numNonContactPool; ++j) { + int tmp = m_orderNonContactConstraintPool[j]; + int swapi = btRandInt2(j+1); + m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi]; + m_orderNonContactConstraintPool[swapi] = tmp; + } + + //contact/friction constraints are not solved more than + if (iteration< infoGlobal.m_numIterations) + { + for (int 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 (int 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; + } + } + } + } + + + btScalar deltaflengthsqr = 0; + + if (infoGlobal.m_solverMode & SOLVER_SIMD) + { + for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) + { + btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; + if (iteration < constraint.m_overrideNumSolverIterations) + { + btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); + m_deltafNC[j] = deltaf; + deltaflengthsqr += deltaf * deltaf; + } + } + } else + { + for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) + { + btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; + if (iteration < constraint.m_overrideNumSolverIterations) + { + btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); + m_deltafNC[j] = deltaf; + deltaflengthsqr += deltaf * deltaf; + } + } + } + + + if (m_onlyForNoneContact) + { + if (iteration==0) + { + for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j]; + } else { + // deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation + btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2; + if (beta>1) + { + for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0; + } else + { + for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) + { + btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; + if (iteration < constraint.m_overrideNumSolverIterations) + { + btScalar additionaldeltaimpulse = beta * m_pNC[j]; + constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse; + m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j]; + btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA]; + btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB]; + const btSolverConstraint& c = constraint; + body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse); + body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse); + } + } + } + } + m_deltafLengthSqrPrev = deltaflengthsqr; + } + + + + if (infoGlobal.m_solverMode & SOLVER_SIMD) + { + + if (iteration< infoGlobal.m_numIterations) + { + for (int j=0;j<numConstraints;j++) + { + if (constraints[j]->isEnabled()) + { + int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep); + int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); + btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; + btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; + constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); + } + } + + ///solve all contact constraints using SIMD, if available + if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) + { + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1; + + for (int c=0;c<numPoolConstraints;c++) + { + btScalar totalImpulse =0; + + { + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]]; + btScalar deltaf = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + m_deltafC[c] = deltaf; + deltaflengthsqr += deltaf*deltaf; + totalImpulse = solveManifold.m_appliedImpulse; + } + bool applyFriction = true; + if (applyFriction) + { + { + + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]]; + + if (totalImpulse>btScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + m_deltafCF[c*multiplier] = deltaf; + deltaflengthsqr += deltaf*deltaf; + } else { + m_deltafCF[c*multiplier] = 0; + } + } + + if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) + { + + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]]; + + if (totalImpulse>btScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + m_deltafCF[c*multiplier+1] = deltaf; + deltaflengthsqr += deltaf*deltaf; + } else { + m_deltafCF[c*multiplier+1] = 0; + } + } + } + } + + } + else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS + { + //solve the friction constraints after all contact constraints, don't interleave them + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + int j; + + for (j=0;j<numPoolConstraints;j++) + { + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; + //resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + m_deltafC[j] = deltaf; + deltaflengthsqr += deltaf*deltaf; + } + + + + ///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; + + if (totalImpulse>btScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + + //resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + m_deltafCF[j] = deltaf; + deltaflengthsqr += deltaf*deltaf; + } else { + m_deltafCF[j] = 0; + } + } + + + int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); + for (j=0;j<numRollingFrictionPoolConstraints;j++) + { + + btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; + btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse; + if (totalImpulse>btScalar(0)) + { + btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; + if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) + rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; + + rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; + rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; + + btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); + m_deltafCRF[j] = deltaf; + deltaflengthsqr += deltaf*deltaf; + } else { + m_deltafCRF[j] = 0; + } + } + + + } + } + + + + } else + { + + if (iteration< infoGlobal.m_numIterations) + { + for (int j=0;j<numConstraints;j++) + { + if (constraints[j]->isEnabled()) + { + int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep); + int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); + btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; + btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; + constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); + } + } + ///solve all contact constraints + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + for (int j=0;j<numPoolConstraints;j++) + { + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; + btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + m_deltafC[j] = deltaf; + deltaflengthsqr += deltaf*deltaf; + } + ///solve all friction constraints + int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); + for (int 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)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + + btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + m_deltafCF[j] = deltaf; + deltaflengthsqr += deltaf*deltaf; + } else { + m_deltafCF[j] = 0; + } + } + + int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); + for (int j=0;j<numRollingFrictionPoolConstraints;j++) + { + btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; + btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse; + if (totalImpulse>btScalar(0)) + { + btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; + if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) + rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; + + rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; + rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; + + btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); + m_deltafCRF[j] = deltaf; + deltaflengthsqr += deltaf*deltaf; + } else { + m_deltafCRF[j] = 0; + } + } + } + } + + + + + if (!m_onlyForNoneContact) + { + if (iteration==0) + { + for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j]; + for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = m_deltafC[j]; + for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = m_deltafCF[j]; + if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) + { + for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = m_deltafCRF[j]; + } + } else + { + // deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation + btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2; + if (beta>1) { + for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0; + for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = 0; + for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = 0; + if ( (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) { + for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = 0; + } + } else { + for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) + { + btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; + if (iteration < constraint.m_overrideNumSolverIterations) { + btScalar additionaldeltaimpulse = beta * m_pNC[j]; + constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse; + m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j]; + btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA]; + btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB]; + const btSolverConstraint& c = constraint; + body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse); + body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse); + } + } + for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) + { + btSolverConstraint& constraint = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; + if (iteration< infoGlobal.m_numIterations) { + btScalar additionaldeltaimpulse = beta * m_pC[j]; + constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse; + m_pC[j] = beta * m_pC[j] + m_deltafC[j]; + btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA]; + btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB]; + const btSolverConstraint& c = constraint; + body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse); + body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse); + } + } + for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) + { + btSolverConstraint& constraint = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; + if (iteration< infoGlobal.m_numIterations) { + btScalar additionaldeltaimpulse = beta * m_pCF[j]; + constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse; + m_pCF[j] = beta * m_pCF[j] + m_deltafCF[j]; + btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA]; + btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB]; + const btSolverConstraint& c = constraint; + body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse); + body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse); + } + } + if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) { + for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) + { + btSolverConstraint& constraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; + if (iteration< infoGlobal.m_numIterations) { + btScalar additionaldeltaimpulse = beta * m_pCRF[j]; + constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse; + m_pCRF[j] = beta * m_pCRF[j] + m_deltafCRF[j]; + btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA]; + btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB]; + const btSolverConstraint& c = constraint; + body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse); + body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse); + } + } + } + } + } + m_deltafLengthSqrPrev = deltaflengthsqr; + } + + return deltaflengthsqr; +} + +btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal) +{ + m_pNC.resizeNoInitialize(0); + m_pC.resizeNoInitialize(0); + m_pCF.resizeNoInitialize(0); + m_pCRF.resizeNoInitialize(0); + + m_deltafNC.resizeNoInitialize(0); + m_deltafC.resizeNoInitialize(0); + m_deltafCF.resizeNoInitialize(0); + m_deltafCRF.resizeNoInitialize(0); + + return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal); +} + + + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h new file mode 100644 index 00000000000..a300929cd5c --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h @@ -0,0 +1,64 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_NNCG_CONSTRAINT_SOLVER_H +#define BT_NNCG_CONSTRAINT_SOLVER_H + +#include "btSequentialImpulseConstraintSolver.h" + +ATTRIBUTE_ALIGNED16(class) btNNCGConstraintSolver : public btSequentialImpulseConstraintSolver +{ +protected: + + btScalar m_deltafLengthSqrPrev; + + btAlignedObjectArray<btScalar> m_pNC; // p for None Contact constraints + btAlignedObjectArray<btScalar> m_pC; // p for Contact constraints + btAlignedObjectArray<btScalar> m_pCF; // p for ContactFriction constraints + btAlignedObjectArray<btScalar> m_pCRF; // p for ContactRollingFriction constraints + + //These are recalculated in every iterations. We just keep these to prevent reallocation in each iteration. + btAlignedObjectArray<btScalar> m_deltafNC; // deltaf for NoneContact constraints + btAlignedObjectArray<btScalar> m_deltafC; // deltaf for Contact constraints + btAlignedObjectArray<btScalar> m_deltafCF; // deltaf for ContactFriction constraints + btAlignedObjectArray<btScalar> m_deltafCRF; // deltaf for ContactRollingFriction constraints + + +protected: + + virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal); + virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + + virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btNNCGConstraintSolver() : btSequentialImpulseConstraintSolver(), m_onlyForNoneContact(false) {} + + virtual btConstraintSolverType getSolverType() const + { + return BT_NNCG_SOLVER; + } + + bool m_onlyForNoneContact; +}; + + + + +#endif //BT_NNCG_CONSTRAINT_SOLVER_H + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h index 91218949498..8fa03d719d7 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h @@ -116,6 +116,11 @@ public: virtual void setParam(int num, btScalar value, int axis = -1); ///return the local value of parameter virtual btScalar getParam(int num, int axis = -1) const; + + virtual int getFlags() const + { + return m_flags; + } virtual int calculateSerializeBufferSize() const; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index f855581c712..8da572bf7d8 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -4,8 +4,8 @@ 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. @@ -22,6 +22,8 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" #include "LinearMath/btIDebugDraw.h" +#include "LinearMath/btCpuFeatureUtility.h" + //#include "btJacobianEntry.h" #include "LinearMath/btMinMax.h" #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" @@ -37,121 +39,253 @@ int gNumSplitImpulseRecoveries = 0; #include "BulletDynamics/Dynamics/btRigidBody.h" -btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() -:m_btSeed2(0) + +///This is the scalar reference implementation of solving a single constraint row, the innerloop of the Projected Gauss Seidel/Sequential Impulse constraint solver +///Below are optional SSE2 and SSE4/FMA3 versions. We assume most hardware has SSE2. For SSE4/FMA3 we perform a CPU feature check. +static btSimdScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) { + btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm; + const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); + const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); + // const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; + deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; + + const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; + if (sum < c.m_lowerLimit) + { + deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse; + c.m_appliedImpulse = c.m_lowerLimit; + } + else if (sum > c.m_upperLimit) + { + deltaImpulse = c.m_upperLimit - c.m_appliedImpulse; + c.m_appliedImpulse = c.m_upperLimit; + } + else + { + c.m_appliedImpulse = sum; + } + + body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); + + return deltaImpulse; } -btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver() + +static btSimdScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) { + btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm; + const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); + const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); + + deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; + const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; + if (sum < c.m_lowerLimit) + { + deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse; + c.m_appliedImpulse = c.m_lowerLimit; + } + else + { + c.m_appliedImpulse = sum; + } + body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); + + return deltaImpulse; } + + #ifdef USE_SIMD #include <emmintrin.h> + + #define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e)) static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 ) { __m128 result = _mm_mul_ps( vec0, vec1); return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( result, 2 ) ) ); } -#endif//USE_SIMD + +#if defined (BT_ALLOW_SSE4) +#include <intrin.h> + +#define USE_FMA 1 +#define USE_FMA3_INSTEAD_FMA4 1 +#define USE_SSE4_DOT 1 + +#define SSE4_DP(a, b) _mm_dp_ps(a, b, 0x7f) +#define SSE4_DP_FP(a, b) _mm_cvtss_f32(_mm_dp_ps(a, b, 0x7f)) + +#if USE_SSE4_DOT +#define DOT_PRODUCT(a, b) SSE4_DP(a, b) +#else +#define DOT_PRODUCT(a, b) btSimdDot3(a, b) +#endif + +#if USE_FMA +#if USE_FMA3_INSTEAD_FMA4 +// a*b + c +#define FMADD(a, b, c) _mm_fmadd_ps(a, b, c) +// -(a*b) + c +#define FMNADD(a, b, c) _mm_fnmadd_ps(a, b, c) +#else // USE_FMA3 +// a*b + c +#define FMADD(a, b, c) _mm_macc_ps(a, b, c) +// -(a*b) + c +#define FMNADD(a, b, c) _mm_nmacc_ps(a, b, c) +#endif +#else // USE_FMA +// c + a*b +#define FMADD(a, b, c) _mm_add_ps(c, _mm_mul_ps(a, b)) +// c - a*b +#define FMNADD(a, b, c) _mm_sub_ps(c, _mm_mul_ps(a, b)) +#endif +#endif // Project Gauss Seidel or the equivalent Sequential Impulse -btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +static btSimdScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) { -#ifdef USE_SIMD __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); - btSimdScalar deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); - __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128)); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); - btSimdScalar resultLowerLess,resultUpperLess; - resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); - 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) ); - __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp); - deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) ); - c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128,body2.internalGetInvMass().mVec128); + btSimdScalar deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse), _mm_set1_ps(c.m_cfm))); + __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128)); + deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv))); + deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv))); + btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse); + btSimdScalar resultLowerLess, resultUpperLess; + resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1); + 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)); + __m128 upperMinApplied = _mm_sub_ps(upperLimit1, cpAppliedImp); + deltaImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied)); + c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1)); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128, body2.internalGetInvMass().mVec128); __m128 impulseMagnitude = deltaImpulse; - body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); - body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); - body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); - body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); + body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude)); + body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude)); + body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude)); + body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude)); + return deltaImpulse; +} + +// Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3 +static btSimdScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +{ +#if defined (BT_ALLOW_SSE4) + __m128 tmp = _mm_set_ps1(c.m_jacDiagABInv); + __m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm); + const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit); + const __m128 upperLimit = _mm_set_ps1(c.m_upperLimit); + const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128)); + const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128)); + deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse); + deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse); + tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse); // sum + const __m128 maskLower = _mm_cmpgt_ps(tmp, lowerLimit); + const __m128 maskUpper = _mm_cmpgt_ps(upperLimit, tmp); + deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), _mm_blendv_ps(_mm_sub_ps(upperLimit, c.m_appliedImpulse), deltaImpulse, maskUpper), maskLower); + c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, _mm_blendv_ps(upperLimit, tmp, maskUpper), maskLower); + body1.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128), deltaImpulse, body1.internalGetDeltaLinearVelocity().mVec128); + body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128); + body2.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128); + body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128); return deltaImpulse; #else - return resolveSingleConstraintRowGeneric(body1,body2,c); + return gResolveSingleConstraintRowGeneric_sse2(body1,body2,c); #endif } -// Project Gauss Seidel or the equivalent Sequential Impulse -btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) -{ - btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); - const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); -// const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; - deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; - deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; - const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; - if (sum < c.m_lowerLimit) - { - deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_lowerLimit; - } - else if (sum > c.m_upperLimit) - { - deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_upperLimit; - } - else - { - c.m_appliedImpulse = sum; - } +static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +{ + __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); + __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); + __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); + btSimdScalar deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse), _mm_set1_ps(c.m_cfm))); + __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128)); + deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv))); + deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv))); + btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse); + btSimdScalar resultLowerLess, resultUpperLess; + resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1); + 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)); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128); + __m128 impulseMagnitude = deltaImpulse; + body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude)); + body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude)); + body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude)); + body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude)); + return deltaImpulse; +} - body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); +// Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3 +static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +{ +#ifdef BT_ALLOW_SSE4 + __m128 tmp = _mm_set_ps1(c.m_jacDiagABInv); + __m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm); + const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit); + const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128)); + const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128)); + deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse); + deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse); + tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse); + const __m128 mask = _mm_cmpgt_ps(tmp, lowerLimit); + deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), deltaImpulse, mask); + c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, tmp, mask); + body1.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128), deltaImpulse, body1.internalGetDeltaLinearVelocity().mVec128); + body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128); + body2.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128); + body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128); return deltaImpulse; +#else + return gResolveSingleConstraintRowLowerLimit_sse2(body1,body2,c); +#endif //BT_ALLOW_SSE4 +} + + +#endif //USE_SIMD + + + +btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +{ +#ifdef USE_SIMD + return m_resolveSingleConstraintRowGeneric(body1, body2, c); +#else + return resolveSingleConstraintRowGeneric(body1,body2,c); +#endif +} + +// Project Gauss Seidel or the equivalent Sequential Impulse +btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +{ + return gResolveSingleConstraintRowGeneric_scalar_reference(body1, body2, c); } btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { #ifdef USE_SIMD - __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); - __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); - __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); - btSimdScalar deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); - __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128)); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); - btSimdScalar resultLowerLess,resultUpperLess; - resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); - 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) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128); - __m128 impulseMagnitude = deltaImpulse; - body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); - body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); - body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); - body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); - return deltaImpulse; + return m_resolveSingleConstraintRowLowerLimit(body1, body2, c); #else return resolveSingleConstraintRowLowerLimit(body1,body2,c); #endif @@ -160,26 +294,7 @@ btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowe btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { - btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); - const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); - - deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; - deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; - const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; - if (sum < c.m_lowerLimit) - { - deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_lowerLimit; - } - else - { - c.m_appliedImpulse = sum; - } - body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); - - return deltaImpulse; + return gResolveSingleConstraintRowLowerLimit_scalar_reference(body1,body2,c); } @@ -248,6 +363,63 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri } + btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() + : m_resolveSingleConstraintRowGeneric(gResolveSingleConstraintRowGeneric_scalar_reference), + m_resolveSingleConstraintRowLowerLimit(gResolveSingleConstraintRowLowerLimit_scalar_reference), + m_btSeed2(0) + { + +#ifdef USE_SIMD + m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2; + m_resolveSingleConstraintRowLowerLimit=gResolveSingleConstraintRowLowerLimit_sse2; +#endif //USE_SIMD + +#ifdef BT_ALLOW_SSE4 + int cpuFeatures = btCpuFeatureUtility::getCpuFeatures(); + if ((cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_FMA3) && (cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_SSE4_1)) + { + m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse4_1_fma3; + m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse4_1_fma3; + } +#endif//BT_ALLOW_SSE4 + + } + + btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver() + { + } + + btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric() + { + return gResolveSingleConstraintRowGeneric_scalar_reference; + } + + btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit() + { + return gResolveSingleConstraintRowLowerLimit_scalar_reference; + } + + +#ifdef USE_SIMD + btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverGeneric() + { + return gResolveSingleConstraintRowGeneric_sse2; + } + btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverLowerLimit() + { + return gResolveSingleConstraintRowLowerLimit_sse2; + } +#ifdef BT_ALLOW_SSE4 + btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverGeneric() + { + return gResolveSingleConstraintRowGeneric_sse4_1_fma3; + } + btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverLowerLimit() + { + return gResolveSingleConstraintRowLowerLimit_sse4_1_fma3; + } +#endif //BT_ALLOW_SSE4 +#endif //USE_SIMD unsigned long btSequentialImpulseConstraintSolver::btRand2() { @@ -308,7 +480,7 @@ void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBod solverBody->m_angularVelocity = rb->getAngularVelocity(); solverBody->m_externalForceImpulse = rb->getTotalForce()*rb->getInvMass()*timeStep; solverBody->m_externalTorqueImpulse = rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*timeStep ; - + } else { solverBody->m_worldTransform.setIdentity(); @@ -340,7 +512,7 @@ btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode) { - + if (colObj && colObj->hasAnisotropicFriction(frictionMode)) { @@ -361,7 +533,7 @@ void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionOb void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) { - + btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; @@ -422,26 +594,26 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr } { - + btScalar rel_vel; - btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0)) + btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0)) + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0)); - btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0)); rel_vel = vel1Dotn+vel2Dotn; // btScalar positionalError = 0.f; - btSimdScalar velocityError = desiredVelocity - rel_vel; - btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv); + btScalar velocityError = desiredVelocity - rel_vel; + btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; solverConstraint.m_rhs = velocityImpulse; solverConstraint.m_rhsPenetration = 0.f; solverConstraint.m_cfm = cfmSlip; solverConstraint.m_lowerLimit = -solverConstraint.m_friction; solverConstraint.m_upperLimit = solverConstraint.m_friction; - + } } @@ -449,7 +621,7 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c { btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing(); solverConstraint.m_frictionIndex = frictionIndex; - setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, + setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); return solverConstraint; } @@ -457,7 +629,7 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB, btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, - btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, + btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) { @@ -503,12 +675,12 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolv } { - + btScalar rel_vel; - btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0)) + btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0)) + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0)); - btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0)); rel_vel = vel1Dotn+vel2Dotn; @@ -521,7 +693,7 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolv solverConstraint.m_cfm = cfmSlip; solverConstraint.m_lowerLimit = -solverConstraint.m_friction; solverConstraint.m_upperLimit = solverConstraint.m_friction; - + } } @@ -536,7 +708,7 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addRollingFrictionConst { btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing(); solverConstraint.m_frictionIndex = frictionIndex; - setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, + setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); return solverConstraint; } @@ -564,7 +736,7 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body.setCompanionId(solverBodyIdA); } else { - + if (m_fixedBodyId<0) { m_fixedBodyId = m_tmpSolverBodyPool.size(); @@ -582,15 +754,15 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& #include <stdio.h> -void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, +void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2) { - - const btVector3& pos1 = cp.getPositionWorldOnA(); - const btVector3& pos2 = cp.getPositionWorldOnB(); + + // const btVector3& pos1 = cp.getPositionWorldOnA(); + // const btVector3& pos2 = cp.getPositionWorldOnB(); btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; @@ -598,23 +770,23 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra btRigidBody* rb0 = bodyA->m_originalBody; btRigidBody* rb1 = bodyB->m_originalBody; -// btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); +// btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); // btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); - //rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + //rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); //rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); relaxation = 1.f; btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); - btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); + btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); { #ifdef COMPUTE_IMPULSE_DENOM btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); -#else +#else btVector3 vec; btScalar denom0 = 0.f; btScalar denom1 = 0.f; @@ -628,7 +800,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec); } -#endif //COMPUTE_IMPULSE_DENOM +#endif //COMPUTE_IMPULSE_DENOM btScalar denom = relaxation/(denom0+denom1); solverConstraint.m_jacDiagABInv = denom; @@ -666,11 +838,11 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra btVector3 vel = vel1 - vel2; btScalar rel_vel = cp.m_normalWorldOnB.dot(vel); - + solverConstraint.m_friction = cp.m_combinedFriction; - + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); if (restitution <= btScalar(0.)) { @@ -700,17 +872,17 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse: btVector3(0,0,0); btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse: btVector3(0,0,0); btVector3 externalTorqueImpulseB = bodyB->m_originalBody ?bodyB->m_externalTorqueImpulse : btVector3(0,0,0); - - btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity+externalForceImpulseA) + + btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity+externalForceImpulseA) + solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity+externalTorqueImpulseA); - btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity+externalForceImpulseB) + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity+externalForceImpulseB) + solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity+externalTorqueImpulseB); btScalar rel_vel = vel1Dotn+vel2Dotn; btScalar positionalError = 0.f; btScalar velocityError = restitution - rel_vel;// * damping; - + btScalar erp = infoGlobal.m_erp2; if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) @@ -755,7 +927,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra -void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, +void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal) { @@ -834,7 +1006,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m btVector3 rel_pos1; btVector3 rel_pos2; btScalar relaxation; - + int frictionIndex = m_tmpSolverContactConstraintPool.size(); btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing(); @@ -848,7 +1020,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m const btVector3& pos1 = cp.getPositionWorldOnA(); const btVector3& pos2 = cp.getPositionWorldOnB(); - rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); + rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); btVector3 vel1;// = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); @@ -856,13 +1028,13 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1,vel1); solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2,vel2 ); - + btVector3 vel = vel1 - vel2; btScalar rel_vel = cp.m_normalWorldOnB.dot(vel); setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2); - + // const btVector3& pos1 = cp.getPositionWorldOnA(); // const btVector3& pos2 = cp.getPositionWorldOnB(); @@ -903,21 +1075,21 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); if (axis1.length()>0.001) addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - + } } ///Bullet has several options to set the friction directions - ///By default, each contact has only a single friction direction that is recomputed automatically very frame + ///By default, each contact has only a single friction direction that is recomputed automatically very frame ///based on the relative linear velocity. ///If the relative velocity it zero, it will automatically compute a friction direction. - + ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS. ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction. /// ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity. /// - ///The user can manually override the friction directions for certain contacts using a contact callback, + ///The user can manually override the friction directions for certain contacts using a contact callback, ///and set the cp.m_lateralFrictionInitialized to true ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) ///this will give a conveyor belt effect @@ -973,9 +1145,9 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m } setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); - - + + } } @@ -1015,7 +1187,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol bool found=false; for (int b=0;b<numBodies;b++) { - + if (&constraint->getRigidBodyA()==bodies[b]) { found = true; @@ -1047,7 +1219,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol bool found=false; for (int b=0;b<numBodies;b++) { - + if (manifoldPtr[i]->getBody0()==bodies[b]) { found = true; @@ -1071,8 +1243,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } } #endif //BT_ADDITIONAL_DEBUG - - + + for (int i = 0; i < numBodies; i++) { bodies[i]->setCompanionId(-1); @@ -1087,6 +1259,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol //convert all bodies + for (int i=0;i<numBodies;i++) { int bodyId = getOrInitSolverBody(*bodies[i],infoGlobal.m_timeStep); @@ -1096,14 +1269,27 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol { btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; btVector3 gyroForce (0,0,0); - if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE) + if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT) { - gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce); + gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce); solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep; } + if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD) + { + gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep); + solverBody.m_externalTorqueImpulse += gyroForce; + } + if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY) + { + gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep); + solverBody.m_externalTorqueImpulse += gyroForce; + + } + + } } - + if (1) { int j; @@ -1123,7 +1309,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol int totalNumRows = 0; int i; - + m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); //calculate the total number of contraint rows for (i=0;i<numConstraints;i++) @@ -1153,14 +1339,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); - + ///setup the btSolverConstraints int currentRow = 0; for (i=0;i<numConstraints;i++) { const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; - + if (info1.m_numConstraintRows) { btAssert(currentRow<totalNumRows); @@ -1268,7 +1454,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } - + { btScalar rel_vel; btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0); @@ -1276,11 +1462,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0,0,0); btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0); - - btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA) + + btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA); - - btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB) + + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB); rel_vel = vel1Dotn+vel2Dotn; @@ -1346,7 +1532,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); int numConstraintPool = m_tmpSolverContactConstraintPool.size(); int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); - + if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) { if (1) // uncomment this for a bit less random ((iteration & 7) == 0) @@ -1359,7 +1545,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration m_orderNonContactConstraintPool[swapi] = tmp; } - //contact/friction constraints are not solved more than + //contact/friction constraints are not solved more than if (iteration< infoGlobal.m_numIterations) { for (int j=0; j<numConstraintPool; ++j) { @@ -1438,7 +1624,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration { btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]]; - + if (totalImpulse>btScalar(0)) { solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); @@ -1463,8 +1649,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); } - - + + ///solve all friction constraints, using SIMD, if available @@ -1483,7 +1669,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration } } - + int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); for (j=0;j<numRollingFrictionPoolConstraints;j++) { @@ -1502,9 +1688,9 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); } } - - } + + } } } else { @@ -1628,10 +1814,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations( 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); } - + } return 0.f; } @@ -1673,7 +1859,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo fb->m_appliedForceBodyB += solverConstr.m_contactNormal2*solverConstr.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep; fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */ - + } constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse); @@ -1694,7 +1880,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp); else m_tmpSolverBodyPool[i].writebackVelocity(); - + m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity( m_tmpSolverBodyPool[i].m_linearVelocity+ m_tmpSolverBodyPool[i].m_externalForceImpulse); @@ -1727,13 +1913,13 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod BT_PROFILE("solveGroup"); //you need to provide at least some bodies - + solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer); solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer); solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal); - + return 0.f; } @@ -1741,5 +1927,3 @@ 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 47dbbe3393d..a6029180983 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -27,6 +27,8 @@ class btCollisionObject; #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" #include "BulletDynamics/ConstraintSolver/btConstraintSolver.h" +typedef btSimdScalar(*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&); + ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method. ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolver : public btConstraintSolver { @@ -43,6 +45,10 @@ protected: btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool; int m_maxOverrideNumSolverIterations; int m_fixedBodyId; + + btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric; + btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit; + void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB, btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, @@ -112,9 +118,7 @@ public: virtual ~btSequentialImpulseConstraintSolver(); virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); - - - + ///clear internal cached data and reset random seed virtual void reset(); @@ -136,6 +140,33 @@ public: { return BT_SEQUENTIAL_IMPULSE_SOLVER; } + + btSingleConstraintRowSolver getActiveConstraintRowSolverGeneric() + { + return m_resolveSingleConstraintRowGeneric; + } + void setConstraintRowSolverGeneric(btSingleConstraintRowSolver rowSolver) + { + m_resolveSingleConstraintRowGeneric = rowSolver; + } + btSingleConstraintRowSolver getActiveConstraintRowSolverLowerLimit() + { + return m_resolveSingleConstraintRowLowerLimit; + } + void setConstraintRowSolverLowerLimit(btSingleConstraintRowSolver rowSolver) + { + m_resolveSingleConstraintRowLowerLimit = rowSolver; + } + + ///Various implementations of solving a single constraint row using a generic equality constraint, using scalar reference, SSE2 or SSE4 + btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric(); + btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric(); + btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric(); + + ///Various implementations of solving a single constraint row using an inequality (lower limit) constraint, using scalar reference, SSE2 or SSE4 + btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit(); + btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit(); + btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit(); }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index aff9f27f594..f8f81bfe6fa 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -539,8 +539,8 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra btScalar tag_vel = getTargetLinMotorVelocity(); btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP); info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity(); - info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps; - info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps; + info->m_lowerLimit[srow] += -getMaxLinMotorForce() / info->fps; + info->m_upperLimit[srow] += getMaxLinMotorForce() / info->fps; } if(limit) { @@ -641,8 +641,8 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra } btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP); info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity(); - info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps; - info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps; + info->m_lowerLimit[srow] = -getMaxAngMotorForce() / info->fps; + info->m_upperLimit[srow] = getMaxAngMotorForce() / info->fps; } if(limit) { diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h index 57ebb47d8e7..628ada4cfb1 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -280,6 +280,11 @@ public: virtual void setParam(int num, btScalar value, int axis = -1); ///return the local value of parameter virtual btScalar getParam(int num, int axis = -1) const; + + virtual int getFlags() const + { + return m_flags; + } virtual int calculateSerializeBufferSize() const; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index 27fdd9d3df8..736a64a1c94 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -24,7 +24,7 @@ subject to the following restrictions: btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA) :btTypedObject(type), m_userConstraintType(-1), -m_userConstraintId(-1), +m_userConstraintPtr((void*)-1), m_breakingImpulseThreshold(SIMD_INFINITY), m_isEnabled(true), m_needsFeedback(false), @@ -41,7 +41,7 @@ m_jointFeedback(0) btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB) :btTypedObject(type), m_userConstraintType(-1), -m_userConstraintId(-1), +m_userConstraintPtr((void*)-1), m_breakingImpulseThreshold(SIMD_INFINITY), m_isEnabled(true), m_needsFeedback(false), diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index b58f984d0fb..b55db0c1bfb 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -44,6 +44,7 @@ enum btTypedConstraintType D6_SPRING_CONSTRAINT_TYPE, GEAR_CONSTRAINT_TYPE, FIXED_CONSTRAINT_TYPE, + D6_SPRING_2_CONSTRAINT_TYPE, MAX_CONSTRAINT_TYPE }; @@ -65,6 +66,7 @@ enum btConstraintParams ATTRIBUTE_ALIGNED16(struct) btJointFeedback { + BT_DECLARE_ALIGNED_ALLOCATOR(); btVector3 m_appliedForceBodyA; btVector3 m_appliedTorqueBodyA; btVector3 m_appliedForceBodyB; |