src/BulletDynamics/ConstraintSolver/btSliderConstraint.h

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 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.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 /*
  17 Added by Roman Ponomarev (rponom@gmail.com)
  18 April 04, 2008
  19
  20 TODO:
  21  - add clamping od accumulated impulse to improve stability
  22  - add conversion for ODE constraint solver
  23 */
  24
  25 #ifndef BT_SLIDER_CONSTRAINT_H
  26 #define BT_SLIDER_CONSTRAINT_H
  27
  28
  29
  30 #include "LinearMath/btVector3.h"
  31 #include "btJacobianEntry.h"
  32 #include "btTypedConstraint.h"
  33
  34
  35
  36 class btRigidBody;
  37
  38
  39
  40 #define SLIDER_CONSTRAINT_DEF_SOFTNESS          (btScalar(1.0))
  41 #define SLIDER_CONSTRAINT_DEF_DAMPING           (btScalar(1.0))
  42 #define SLIDER_CONSTRAINT_DEF_RESTITUTION       (btScalar(0.7))
  43 #define SLIDER_CONSTRAINT_DEF_CFM                       (btScalar(0.f))
  44
  45
  46 enum btSliderFlags
  47 {
  48         BT_SLIDER_FLAGS_CFM_DIRLIN = (1 << 0),
  49         BT_SLIDER_FLAGS_ERP_DIRLIN = (1 << 1),
  50         BT_SLIDER_FLAGS_CFM_DIRANG = (1 << 2),
  51         BT_SLIDER_FLAGS_ERP_DIRANG = (1 << 3),
  52         BT_SLIDER_FLAGS_CFM_ORTLIN = (1 << 4),
  53         BT_SLIDER_FLAGS_ERP_ORTLIN = (1 << 5),
  54         BT_SLIDER_FLAGS_CFM_ORTANG = (1 << 6),
  55         BT_SLIDER_FLAGS_ERP_ORTANG = (1 << 7),
  56         BT_SLIDER_FLAGS_CFM_LIMLIN = (1 << 8),
  57         BT_SLIDER_FLAGS_ERP_LIMLIN = (1 << 9),
  58         BT_SLIDER_FLAGS_CFM_LIMANG = (1 << 10),
  59         BT_SLIDER_FLAGS_ERP_LIMANG = (1 << 11)
  60 };
  61
  62
  63 ATTRIBUTE_ALIGNED16(class) btSliderConstraint : public btTypedConstraint
  64 {
  65 protected:
  66         ///for backwards compatibility during the transition to 'getInfo/getInfo2'
  67         bool            m_useSolveConstraintObsolete;
  68         bool            m_useOffsetForConstraintFrame;
  69         btTransform     m_frameInA;
  70     btTransform m_frameInB;
  71         // use frameA fo define limits, if true
  72         bool m_useLinearReferenceFrameA;
  73         // linear limits
  74         btScalar m_lowerLinLimit;
  75         btScalar m_upperLinLimit;
  76         // angular limits
  77         btScalar m_lowerAngLimit;
  78         btScalar m_upperAngLimit;
  79         // softness, restitution and damping for different cases
  80         // DirLin - moving inside linear limits
  81         // LimLin - hitting linear limit
  82         // DirAng - moving inside angular limits
  83         // LimAng - hitting angular limit
  84         // OrthoLin, OrthoAng - against constraint axis
  85         btScalar m_softnessDirLin;
  86         btScalar m_restitutionDirLin;
  87         btScalar m_dampingDirLin;
  88         btScalar m_cfmDirLin;
  89
  90         btScalar m_softnessDirAng;
  91         btScalar m_restitutionDirAng;
  92         btScalar m_dampingDirAng;
  93         btScalar m_cfmDirAng;
  94
  95         btScalar m_softnessLimLin;
  96         btScalar m_restitutionLimLin;
  97         btScalar m_dampingLimLin;
  98         btScalar m_cfmLimLin;
  99
 100         btScalar m_softnessLimAng;
 101         btScalar m_restitutionLimAng;
 102         btScalar m_dampingLimAng;
 103         btScalar m_cfmLimAng;
 104
 105         btScalar m_softnessOrthoLin;
 106         btScalar m_restitutionOrthoLin;
 107         btScalar m_dampingOrthoLin;
 108         btScalar m_cfmOrthoLin;
 109
 110         btScalar m_softnessOrthoAng;
 111         btScalar m_restitutionOrthoAng;
 112         btScalar m_dampingOrthoAng;
 113         btScalar m_cfmOrthoAng;
 114
 115         // for interlal use
 116         bool m_solveLinLim;
 117         bool m_solveAngLim;
 118
 119         int m_flags;
 120
 121         btJacobianEntry m_jacLin[3];
 122         btScalar                m_jacLinDiagABInv[3];
 123
 124     btJacobianEntry     m_jacAng[3];
 125
 126         btScalar m_timeStep;
 127     btTransform m_calculatedTransformA;
 128     btTransform m_calculatedTransformB;
 129
 130         btVector3 m_sliderAxis;
 131         btVector3 m_realPivotAInW;
 132         btVector3 m_realPivotBInW;
 133         btVector3 m_projPivotInW;
 134         btVector3 m_delta;
 135         btVector3 m_depth;
 136         btVector3 m_relPosA;
 137         btVector3 m_relPosB;
 138
 139         btScalar m_linPos;
 140         btScalar m_angPos;
 141
 142         btScalar m_angDepth;
 143         btScalar m_kAngle;
 144
 145         bool     m_poweredLinMotor;
 146     btScalar m_targetLinMotorVelocity;
 147     btScalar m_maxLinMotorForce;
 148     btScalar m_accumulatedLinMotorImpulse;
 149
 150         bool     m_poweredAngMotor;
 151     btScalar m_targetAngMotorVelocity;
 152     btScalar m_maxAngMotorForce;
 153     btScalar m_accumulatedAngMotorImpulse;
 154
 155         //------------------------
 156         void initParams();
 157 public:
 158         BT_DECLARE_ALIGNED_ALLOCATOR();
 159
 160         // constructors
 161     btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
 162     btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA);
 163
 164         // overrides
 165
 166     virtual void getInfo1 (btConstraintInfo1* info);
 167
 168         void getInfo1NonVirtual(btConstraintInfo1* info);
 169
 170         virtual void getInfo2 (btConstraintInfo2* info);
 171
 172         void getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass);
 173
 174
 175         // access
 176     const btRigidBody& getRigidBodyA() const { return m_rbA; }
 177     const btRigidBody& getRigidBodyB() const { return m_rbB; }
 178     const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; }
 179     const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; }
 180     const btTransform & getFrameOffsetA() const { return m_frameInA; }
 181     const btTransform & getFrameOffsetB() const { return m_frameInB; }
 182     btTransform & getFrameOffsetA() { return m_frameInA; }
 183     btTransform & getFrameOffsetB() { return m_frameInB; }
 184     btScalar getLowerLinLimit() { return m_lowerLinLimit; }
 185     void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; }
 186     btScalar getUpperLinLimit() { return m_upperLinLimit; }
 187     void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; }
 188     btScalar getLowerAngLimit() { return m_lowerAngLimit; }
 189     void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); }
 190     btScalar getUpperAngLimit() { return m_upperAngLimit; }
 191     void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); }
 192         bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; }
 193         btScalar getSoftnessDirLin() { return m_softnessDirLin; }
 194         btScalar getRestitutionDirLin() { return m_restitutionDirLin; }
 195         btScalar getDampingDirLin() { return m_dampingDirLin ; }
 196         btScalar getSoftnessDirAng() { return m_softnessDirAng; }
 197         btScalar getRestitutionDirAng() { return m_restitutionDirAng; }
 198         btScalar getDampingDirAng() { return m_dampingDirAng; }
 199         btScalar getSoftnessLimLin() { return m_softnessLimLin; }
 200         btScalar getRestitutionLimLin() { return m_restitutionLimLin; }
 201         btScalar getDampingLimLin() { return m_dampingLimLin; }
 202         btScalar getSoftnessLimAng() { return m_softnessLimAng; }
 203         btScalar getRestitutionLimAng() { return m_restitutionLimAng; }
 204         btScalar getDampingLimAng() { return m_dampingLimAng; }
 205         btScalar getSoftnessOrthoLin() { return m_softnessOrthoLin; }
 206         btScalar getRestitutionOrthoLin() { return m_restitutionOrthoLin; }
 207         btScalar getDampingOrthoLin() { return m_dampingOrthoLin; }
 208         btScalar getSoftnessOrthoAng() { return m_softnessOrthoAng; }
 209         btScalar getRestitutionOrthoAng() { return m_restitutionOrthoAng; }
 210         btScalar getDampingOrthoAng() { return m_dampingOrthoAng; }
 211         void setSoftnessDirLin(btScalar softnessDirLin) { m_softnessDirLin = softnessDirLin; }
 212         void setRestitutionDirLin(btScalar restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; }
 213         void setDampingDirLin(btScalar dampingDirLin) { m_dampingDirLin = dampingDirLin; }
 214         void setSoftnessDirAng(btScalar softnessDirAng) { m_softnessDirAng = softnessDirAng; }
 215         void setRestitutionDirAng(btScalar restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; }
 216         void setDampingDirAng(btScalar dampingDirAng) { m_dampingDirAng = dampingDirAng; }
 217         void setSoftnessLimLin(btScalar softnessLimLin) { m_softnessLimLin = softnessLimLin; }
 218         void setRestitutionLimLin(btScalar restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; }
 219         void setDampingLimLin(btScalar dampingLimLin) { m_dampingLimLin = dampingLimLin; }
 220         void setSoftnessLimAng(btScalar softnessLimAng) { m_softnessLimAng = softnessLimAng; }
 221         void setRestitutionLimAng(btScalar restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; }
 222         void setDampingLimAng(btScalar dampingLimAng) { m_dampingLimAng = dampingLimAng; }
 223         void setSoftnessOrthoLin(btScalar softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; }
 224         void setRestitutionOrthoLin(btScalar restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; }
 225         void setDampingOrthoLin(btScalar dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; }
 226         void setSoftnessOrthoAng(btScalar softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; }
 227         void setRestitutionOrthoAng(btScalar restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; }
 228         void setDampingOrthoAng(btScalar dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; }
 229         void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; }
 230         bool getPoweredLinMotor() { return m_poweredLinMotor; }
 231         void setTargetLinMotorVelocity(btScalar targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; }
 232         btScalar getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; }
 233         void setMaxLinMotorForce(btScalar maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; }
 234         btScalar getMaxLinMotorForce() { return m_maxLinMotorForce; }
 235         void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; }
 236         bool getPoweredAngMotor() { return m_poweredAngMotor; }
 237         void setTargetAngMotorVelocity(btScalar targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; }
 238         btScalar getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; }
 239         void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; }
 240         btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; }
 241
 242         btScalar getLinearPos() const { return m_linPos; }
 243         btScalar getAngularPos() const { return m_angPos; }
 244
 245
 246
 247         // access for ODE solver
 248         bool getSolveLinLimit() { return m_solveLinLim; }
 249         btScalar getLinDepth() { return m_depth[0]; }
 250         bool getSolveAngLimit() { return m_solveAngLim; }
 251         btScalar getAngDepth() { return m_angDepth; }
 252         // shared code used by ODE solver
 253         void    calculateTransforms(const btTransform& transA,const btTransform& transB);
 254         void    testLinLimits();
 255         void    testAngLimits();
 256         // access for PE Solver
 257         btVector3 getAncorInA();
 258         btVector3 getAncorInB();
 259         // access for UseFrameOffset
 260         bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
 261         void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
 262
 263         void setFrames(const btTransform& frameA, const btTransform& frameB)
 264         {
 265                 m_frameInA=frameA;
 266                 m_frameInB=frameB;
 267                 calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
 268                 buildJacobian();
 269         }
 270
 271
 272         ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 273         ///If no axis is provided, it uses the default axis for this constraint.
 274         virtual void    setParam(int num, btScalar value, int axis = -1);
 275         ///return the local value of parameter
 276         virtual btScalar getParam(int num, int axis = -1) const;
 277
 278         virtual int     calculateSerializeBufferSize() const;
 279
 280         ///fills the dataBuffer and returns the struct name (and 0 on failure)
 281         virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
 282
 283
 284 };
 285
 286 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 287 struct btSliderConstraintData
 288 {
 289         btTypedConstraintData   m_typeConstraintData;
 290         btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
 291         btTransformFloatData m_rbBFrame;
 292
 293         float   m_linearUpperLimit;
 294         float   m_linearLowerLimit;
 295
 296         float   m_angularUpperLimit;
 297         float   m_angularLowerLimit;
 298
 299         int     m_useLinearReferenceFrameA;
 300         int m_useOffsetForConstraintFrame;
 301
 302 };
 303
 304
 305 SIMD_FORCE_INLINE               int     btSliderConstraint::calculateSerializeBufferSize() const
 306 {
 307         return sizeof(btSliderConstraintData);
 308 }
 309
 310         ///fills the dataBuffer and returns the struct name (and 0 on failure)
 311 SIMD_FORCE_INLINE       const char*     btSliderConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 312 {
 313
 314         btSliderConstraintData* sliderData = (btSliderConstraintData*) dataBuffer;
 315         btTypedConstraint::serialize(&sliderData->m_typeConstraintData,serializer);
 316
 317         m_frameInA.serializeFloat(sliderData->m_rbAFrame);
 318         m_frameInB.serializeFloat(sliderData->m_rbBFrame);
 319
 320         sliderData->m_linearUpperLimit = float(m_upperLinLimit);
 321         sliderData->m_linearLowerLimit = float(m_lowerLinLimit);
 322
 323         sliderData->m_angularUpperLimit = float(m_upperAngLimit);
 324         sliderData->m_angularLowerLimit = float(m_lowerAngLimit);
 325
 326         sliderData->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA;
 327         sliderData->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame;
 328
 329         return "btSliderConstraintData";
 330 }
 331
 332
 333
 334 #endif //BT_SLIDER_CONSTRAINT_H
 335