dali-physics/third-party/bullet3/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h

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