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

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
   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 Written by: Marcus Hennix
  16 */
  17
  18 /*
  19 Overview:
  20
  21 btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc).
  22 It is a fixed translation, 3 degree-of-freedom (DOF) rotational "joint".
  23 It divides the 3 rotational DOFs into swing (movement within a cone) and twist.
  24 Swing is divided into swing1 and swing2 which can have different limits, giving an elliptical shape.
  25 (Note: the cone's base isn't flat, so this ellipse is "embedded" on the surface of a sphere.)
  26
  27 In the contraint's frame of reference:
  28 twist is along the x-axis,
  29 and swing 1 and 2 are along the z and y axes respectively.
  30 */
  31
  32 #ifndef BT_CONETWISTCONSTRAINT_H
  33 #define BT_CONETWISTCONSTRAINT_H
  34
  35 #include "LinearMath/btVector3.h"
  36 #include "btJacobianEntry.h"
  37 #include "btTypedConstraint.h"
  38
  39 #ifdef BT_USE_DOUBLE_PRECISION
  40 #define btConeTwistConstraintData2 btConeTwistConstraintDoubleData
  41 #define btConeTwistConstraintDataName "btConeTwistConstraintDoubleData"
  42 #else
  43 #define btConeTwistConstraintData2 btConeTwistConstraintData
  44 #define btConeTwistConstraintDataName "btConeTwistConstraintData"
  45 #endif  //BT_USE_DOUBLE_PRECISION
  46
  47 class btRigidBody;
  48
  49 enum btConeTwistFlags
  50 {
  51         BT_CONETWIST_FLAGS_LIN_CFM = 1,
  52         BT_CONETWIST_FLAGS_LIN_ERP = 2,
  53         BT_CONETWIST_FLAGS_ANG_CFM = 4
  54 };
  55
  56 ///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)
  57 ATTRIBUTE_ALIGNED16(class)
  58 btConeTwistConstraint : public btTypedConstraint
  59 {
  60 #ifdef IN_PARALLELL_SOLVER
  61 public:
  62 #endif
  63         btJacobianEntry m_jac[3];  //3 orthogonal linear constraints
  64
  65         btTransform m_rbAFrame;
  66         btTransform m_rbBFrame;
  67
  68         btScalar m_limitSoftness;
  69         btScalar m_biasFactor;
  70         btScalar m_relaxationFactor;
  71
  72         btScalar m_damping;
  73
  74         btScalar m_swingSpan1;
  75         btScalar m_swingSpan2;
  76         btScalar m_twistSpan;
  77
  78         btScalar m_fixThresh;
  79
  80         btVector3 m_swingAxis;
  81         btVector3 m_twistAxis;
  82
  83         btScalar m_kSwing;
  84         btScalar m_kTwist;
  85
  86         btScalar m_twistLimitSign;
  87         btScalar m_swingCorrection;
  88         btScalar m_twistCorrection;
  89
  90         btScalar m_twistAngle;
  91
  92         btScalar m_accSwingLimitImpulse;
  93         btScalar m_accTwistLimitImpulse;
  94
  95         bool m_angularOnly;
  96         bool m_solveTwistLimit;
  97         bool m_solveSwingLimit;
  98
  99         bool m_useSolveConstraintObsolete;
 100
 101         // not yet used...
 102         btScalar m_swingLimitRatio;
 103         btScalar m_twistLimitRatio;
 104         btVector3 m_twistAxisA;
 105
 106         // motor
 107         bool m_bMotorEnabled;
 108         bool m_bNormalizedMotorStrength;
 109         btQuaternion m_qTarget;
 110         btScalar m_maxMotorImpulse;
 111         btVector3 m_accMotorImpulse;
 112
 113         // parameters
 114         int m_flags;
 115         btScalar m_linCFM;
 116         btScalar m_linERP;
 117         btScalar m_angCFM;
 118
 119 protected:
 120         void init();
 121
 122         void computeConeLimitInfo(const btQuaternion& qCone,                                           // in
 123                                                           btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit);  // all outs
 124
 125         void computeTwistLimitInfo(const btQuaternion& qTwist,                    // in
 126                                                            btScalar& twistAngle, btVector3& vTwistAxis);  // all outs
 127
 128         void adjustSwingAxisToUseEllipseNormal(btVector3 & vSwingAxis) const;
 129
 130 public:
 131         BT_DECLARE_ALIGNED_ALLOCATOR();
 132
 133         btConeTwistConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& rbAFrame, const btTransform& rbBFrame);
 134
 135         btConeTwistConstraint(btRigidBody & rbA, const btTransform& rbAFrame);
 136
 137         virtual void buildJacobian();
 138
 139         virtual void getInfo1(btConstraintInfo1 * info);
 140
 141         void getInfo1NonVirtual(btConstraintInfo1 * info);
 142
 143         virtual void getInfo2(btConstraintInfo2 * info);
 144
 145         void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB);
 146
 147         virtual void solveConstraintObsolete(btSolverBody & bodyA, btSolverBody & bodyB, btScalar timeStep);
 148
 149         void updateRHS(btScalar timeStep);
 150
 151         const btRigidBody& getRigidBodyA() const
 152         {
 153                 return m_rbA;
 154         }
 155         const btRigidBody& getRigidBodyB() const
 156         {
 157                 return m_rbB;
 158         }
 159
 160         void setAngularOnly(bool angularOnly)
 161         {
 162                 m_angularOnly = angularOnly;
 163         }
 164
 165         bool getAngularOnly() const
 166         {
 167                 return m_angularOnly;
 168         }
 169
 170         void setLimit(int limitIndex, btScalar limitValue)
 171         {
 172                 switch (limitIndex)
 173                 {
 174                         case 3:
 175                         {
 176                                 m_twistSpan = limitValue;
 177                                 break;
 178                         }
 179                         case 4:
 180                         {
 181                                 m_swingSpan2 = limitValue;
 182                                 break;
 183                         }
 184                         case 5:
 185                         {
 186                                 m_swingSpan1 = limitValue;
 187                                 break;
 188                         }
 189                         default:
 190                         {
 191                         }
 192                 };
 193         }
 194
 195         btScalar getLimit(int limitIndex) const
 196         {
 197                 switch (limitIndex)
 198                 {
 199                         case 3:
 200                         {
 201                                 return m_twistSpan;
 202                                 break;
 203                         }
 204                         case 4:
 205                         {
 206                                 return m_swingSpan2;
 207                                 break;
 208                         }
 209                         case 5:
 210                         {
 211                                 return m_swingSpan1;
 212                                 break;
 213                         }
 214                         default:
 215                         {
 216                                 btAssert(0 && "Invalid limitIndex specified for btConeTwistConstraint");
 217                                 return 0.0;
 218                         }
 219                 };
 220         }
 221
 222         // setLimit(), a few notes:
 223         // _softness:
 224         //              0->1, recommend ~0.8->1.
 225         //              describes % of limits where movement is free.
 226         //              beyond this softness %, the limit is gradually enforced until the "hard" (1.0) limit is reached.
 227         // _biasFactor:
 228         //              0->1?, recommend 0.3 +/-0.3 or so.
 229         //              strength with which constraint resists zeroth order (angular, not angular velocity) limit violation.
 230         // __relaxationFactor:
 231         //              0->1, recommend to stay near 1.
 232         //              the lower the value, the less the constraint will fight velocities which violate the angular limits.
 233         void setLimit(btScalar _swingSpan1, btScalar _swingSpan2, btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
 234         {
 235                 m_swingSpan1 = _swingSpan1;
 236                 m_swingSpan2 = _swingSpan2;
 237                 m_twistSpan = _twistSpan;
 238
 239                 m_limitSoftness = _softness;
 240                 m_biasFactor = _biasFactor;
 241                 m_relaxationFactor = _relaxationFactor;
 242         }
 243
 244         const btTransform& getAFrame() const { return m_rbAFrame; };
 245         const btTransform& getBFrame() const { return m_rbBFrame; };
 246
 247         inline int getSolveTwistLimit()
 248         {
 249                 return m_solveTwistLimit;
 250         }
 251
 252         inline int getSolveSwingLimit()
 253         {
 254                 return m_solveSwingLimit;
 255         }
 256
 257         inline btScalar getTwistLimitSign()
 258         {
 259                 return m_twistLimitSign;
 260         }
 261
 262         void calcAngleInfo();
 263         void calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB);
 264
 265         inline btScalar getSwingSpan1() const
 266         {
 267                 return m_swingSpan1;
 268         }
 269         inline btScalar getSwingSpan2() const
 270         {
 271                 return m_swingSpan2;
 272         }
 273         inline btScalar getTwistSpan() const
 274         {
 275                 return m_twistSpan;
 276         }
 277         inline btScalar getLimitSoftness() const
 278         {
 279                 return m_limitSoftness;
 280         }
 281         inline btScalar getBiasFactor() const
 282         {
 283                 return m_biasFactor;
 284         }
 285         inline btScalar getRelaxationFactor() const
 286         {
 287                 return m_relaxationFactor;
 288         }
 289         inline btScalar getTwistAngle() const
 290         {
 291                 return m_twistAngle;
 292         }
 293         bool isPastSwingLimit() { return m_solveSwingLimit; }
 294
 295         btScalar getDamping() const { return m_damping; }
 296         void setDamping(btScalar damping) { m_damping = damping; }
 297
 298         void enableMotor(bool b) { m_bMotorEnabled = b; }
 299         bool isMotorEnabled() const { return m_bMotorEnabled; }
 300         btScalar getMaxMotorImpulse() const { return m_maxMotorImpulse; }
 301         bool isMaxMotorImpulseNormalized() const { return m_bNormalizedMotorStrength; }
 302         void setMaxMotorImpulse(btScalar maxMotorImpulse)
 303         {
 304                 m_maxMotorImpulse = maxMotorImpulse;
 305                 m_bNormalizedMotorStrength = false;
 306         }
 307         void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse)
 308         {
 309                 m_maxMotorImpulse = maxMotorImpulse;
 310                 m_bNormalizedMotorStrength = true;
 311         }
 312
 313         btScalar getFixThresh() { return m_fixThresh; }
 314         void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; }
 315
 316         // setMotorTarget:
 317         // q: the desired rotation of bodyA wrt bodyB.
 318         // note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability)
 319         // note: don't forget to enableMotor()
 320         void setMotorTarget(const btQuaternion& q);
 321         const btQuaternion& getMotorTarget() const { return m_qTarget; }
 322
 323         // same as above, but q is the desired rotation of frameA wrt frameB in constraint space
 324         void setMotorTargetInConstraintSpace(const btQuaternion& q);
 325
 326         btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
 327
 328         ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 329         ///If no axis is provided, it uses the default axis for this constraint.
 330         virtual void setParam(int num, btScalar value, int axis = -1);
 331
 332         virtual void setFrames(const btTransform& frameA, const btTransform& frameB);
 333
 334         const btTransform& getFrameOffsetA() const
 335         {
 336                 return m_rbAFrame;
 337         }
 338
 339         const btTransform& getFrameOffsetB() const
 340         {
 341                 return m_rbBFrame;
 342         }
 343
 344         ///return the local value of parameter
 345         virtual btScalar getParam(int num, int axis = -1) const;
 346
 347         int getFlags() const
 348         {
 349                 return m_flags;
 350         }
 351
 352         virtual int calculateSerializeBufferSize() const;
 353
 354         ///fills the dataBuffer and returns the struct name (and 0 on failure)
 355         virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 356 };
 357
 358 struct btConeTwistConstraintDoubleData
 359 {
 360         btTypedConstraintDoubleData m_typeConstraintData;
 361         btTransformDoubleData m_rbAFrame;
 362         btTransformDoubleData m_rbBFrame;
 363
 364         //limits
 365         double m_swingSpan1;
 366         double m_swingSpan2;
 367         double m_twistSpan;
 368         double m_limitSoftness;
 369         double m_biasFactor;
 370         double m_relaxationFactor;
 371
 372         double m_damping;
 373 };
 374
 375 #ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
 376 ///this structure is not used, except for loading pre-2.82 .bullet files
 377 struct btConeTwistConstraintData
 378 {
 379         btTypedConstraintData m_typeConstraintData;
 380         btTransformFloatData m_rbAFrame;
 381         btTransformFloatData m_rbBFrame;
 382
 383         //limits
 384         float m_swingSpan1;
 385         float m_swingSpan2;
 386         float m_twistSpan;
 387         float m_limitSoftness;
 388         float m_biasFactor;
 389         float m_relaxationFactor;
 390
 391         float m_damping;
 392
 393         char m_pad[4];
 394 };
 395 #endif  //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
 396 //
 397
 398 SIMD_FORCE_INLINE int btConeTwistConstraint::calculateSerializeBufferSize() const
 399 {
 400         return sizeof(btConeTwistConstraintData2);
 401 }
 402
 403 ///fills the dataBuffer and returns the struct name (and 0 on failure)
 404 SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 405 {
 406         btConeTwistConstraintData2* cone = (btConeTwistConstraintData2*)dataBuffer;
 407         btTypedConstraint::serialize(&cone->m_typeConstraintData, serializer);
 408
 409         m_rbAFrame.serialize(cone->m_rbAFrame);
 410         m_rbBFrame.serialize(cone->m_rbBFrame);
 411
 412         cone->m_swingSpan1 = m_swingSpan1;
 413         cone->m_swingSpan2 = m_swingSpan2;
 414         cone->m_twistSpan = m_twistSpan;
 415         cone->m_limitSoftness = m_limitSoftness;
 416         cone->m_biasFactor = m_biasFactor;
 417         cone->m_relaxationFactor = m_relaxationFactor;
 418         cone->m_damping = m_damping;
 419
 420         return btConeTwistConstraintDataName;
 421 }
 422
 423 #endif  //BT_CONETWISTCONSTRAINT_H