dali-physics/third-party/bullet3/src/BulletDynamics/ConstraintSolver/btHingeConstraint.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 /* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */
  17
  18 #ifndef BT_HINGECONSTRAINT_H
  19 #define BT_HINGECONSTRAINT_H
  20
  21 #define _BT_USE_CENTER_LIMIT_ 1
  22
  23 #include "LinearMath/btVector3.h"
  24 #include "btJacobianEntry.h"
  25 #include "btTypedConstraint.h"
  26
  27 class btRigidBody;
  28
  29 #ifdef BT_USE_DOUBLE_PRECISION
  30 #define btHingeConstraintData btHingeConstraintDoubleData2  //rename to 2 for backwards compatibility, so we can still load the 'btHingeConstraintDoubleData' version
  31 #define btHingeConstraintDataName "btHingeConstraintDoubleData2"
  32 #else
  33 #define btHingeConstraintData btHingeConstraintFloatData
  34 #define btHingeConstraintDataName "btHingeConstraintFloatData"
  35 #endif  //BT_USE_DOUBLE_PRECISION
  36
  37 enum btHingeFlags
  38 {
  39         BT_HINGE_FLAGS_CFM_STOP = 1,
  40         BT_HINGE_FLAGS_ERP_STOP = 2,
  41         BT_HINGE_FLAGS_CFM_NORM = 4,
  42         BT_HINGE_FLAGS_ERP_NORM = 8
  43 };
  44
  45 /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
  46 /// axis defines the orientation of the hinge axis
  47 ATTRIBUTE_ALIGNED16(class)
  48 btHingeConstraint : public btTypedConstraint
  49 {
  50 #ifdef IN_PARALLELL_SOLVER
  51 public:
  52 #endif
  53         btJacobianEntry m_jac[3];     //3 orthogonal linear constraints
  54         btJacobianEntry m_jacAng[3];  //2 orthogonal angular constraints+ 1 for limit/motor
  55
  56         btTransform m_rbAFrame;  // constraint axii. Assumes z is hinge axis.
  57         btTransform m_rbBFrame;
  58
  59         btScalar m_motorTargetVelocity;
  60         btScalar m_maxMotorImpulse;
  61
  62 #ifdef _BT_USE_CENTER_LIMIT_
  63         btAngularLimit m_limit;
  64 #else
  65         btScalar m_lowerLimit;
  66         btScalar m_upperLimit;
  67         btScalar m_limitSign;
  68         btScalar m_correction;
  69
  70         btScalar m_limitSoftness;
  71         btScalar m_biasFactor;
  72         btScalar m_relaxationFactor;
  73
  74         bool m_solveLimit;
  75 #endif
  76
  77         btScalar m_kHinge;
  78
  79         btScalar m_accLimitImpulse;
  80         btScalar m_hingeAngle;
  81         btScalar m_referenceSign;
  82
  83         bool m_angularOnly;
  84         bool m_enableAngularMotor;
  85         bool m_useSolveConstraintObsolete;
  86         bool m_useOffsetForConstraintFrame;
  87         bool m_useReferenceFrameA;
  88
  89         btScalar m_accMotorImpulse;
  90
  91         int m_flags;
  92         btScalar m_normalCFM;
  93         btScalar m_normalERP;
  94         btScalar m_stopCFM;
  95         btScalar m_stopERP;
  96
  97 public:
  98         BT_DECLARE_ALIGNED_ALLOCATOR();
  99
 100         btHingeConstraint(btRigidBody & rbA, btRigidBody & rbB, const btVector3& pivotInA, const btVector3& pivotInB, const btVector3& axisInA, const btVector3& axisInB, bool useReferenceFrameA = false);
 101
 102         btHingeConstraint(btRigidBody & rbA, const btVector3& pivotInA, const btVector3& axisInA, bool useReferenceFrameA = false);
 103
 104         btHingeConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
 105
 106         btHingeConstraint(btRigidBody & rbA, const btTransform& rbAFrame, bool useReferenceFrameA = false);
 107
 108         virtual void buildJacobian();
 109
 110         virtual void getInfo1(btConstraintInfo1 * info);
 111
 112         void getInfo1NonVirtual(btConstraintInfo1 * info);
 113
 114         virtual void getInfo2(btConstraintInfo2 * info);
 115
 116         void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB);
 117
 118         void getInfo2Internal(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB);
 119         void getInfo2InternalUsingFrameOffset(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB);
 120
 121         void updateRHS(btScalar timeStep);
 122
 123         const btRigidBody& getRigidBodyA() const
 124         {
 125                 return m_rbA;
 126         }
 127         const btRigidBody& getRigidBodyB() const
 128         {
 129                 return m_rbB;
 130         }
 131
 132         btRigidBody& getRigidBodyA()
 133         {
 134                 return m_rbA;
 135         }
 136
 137         btRigidBody& getRigidBodyB()
 138         {
 139                 return m_rbB;
 140         }
 141
 142         btTransform& getFrameOffsetA()
 143         {
 144                 return m_rbAFrame;
 145         }
 146
 147         btTransform& getFrameOffsetB()
 148         {
 149                 return m_rbBFrame;
 150         }
 151
 152         void setFrames(const btTransform& frameA, const btTransform& frameB);
 153
 154         void setAngularOnly(bool angularOnly)
 155         {
 156                 m_angularOnly = angularOnly;
 157         }
 158
 159         void enableAngularMotor(bool enableMotor, btScalar targetVelocity, btScalar maxMotorImpulse)
 160         {
 161                 m_enableAngularMotor = enableMotor;
 162                 m_motorTargetVelocity = targetVelocity;
 163                 m_maxMotorImpulse = maxMotorImpulse;
 164         }
 165
 166         // extra motor API, including ability to set a target rotation (as opposed to angular velocity)
 167         // note: setMotorTarget sets angular velocity under the hood, so you must call it every tick to
 168         //       maintain a given angular target.
 169         void enableMotor(bool enableMotor) { m_enableAngularMotor = enableMotor; }
 170         void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; }
 171         void setMotorTargetVelocity(btScalar motorTargetVelocity) { m_motorTargetVelocity = motorTargetVelocity; }
 172         void setMotorTarget(const btQuaternion& qAinB, btScalar dt);  // qAinB is rotation of body A wrt body B.
 173         void setMotorTarget(btScalar targetAngle, btScalar dt);
 174
 175         void setLimit(btScalar low, btScalar high, btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
 176         {
 177 #ifdef _BT_USE_CENTER_LIMIT_
 178                 m_limit.set(low, high, _softness, _biasFactor, _relaxationFactor);
 179 #else
 180                 m_lowerLimit = btNormalizeAngle(low);
 181                 m_upperLimit = btNormalizeAngle(high);
 182                 m_limitSoftness = _softness;
 183                 m_biasFactor = _biasFactor;
 184                 m_relaxationFactor = _relaxationFactor;
 185 #endif
 186         }
 187
 188         btScalar getLimitSoftness() const
 189         {
 190 #ifdef _BT_USE_CENTER_LIMIT_
 191                 return m_limit.getSoftness();
 192 #else
 193                 return m_limitSoftness;
 194 #endif
 195         }
 196
 197         btScalar getLimitBiasFactor() const
 198         {
 199 #ifdef _BT_USE_CENTER_LIMIT_
 200                 return m_limit.getBiasFactor();
 201 #else
 202                 return m_biasFactor;
 203 #endif
 204         }
 205
 206         btScalar getLimitRelaxationFactor() const
 207         {
 208 #ifdef _BT_USE_CENTER_LIMIT_
 209                 return m_limit.getRelaxationFactor();
 210 #else
 211                 return m_relaxationFactor;
 212 #endif
 213         }
 214
 215         void setAxis(btVector3 & axisInA)
 216         {
 217                 btVector3 rbAxisA1, rbAxisA2;
 218                 btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
 219                 btVector3 pivotInA = m_rbAFrame.getOrigin();
 220                 //              m_rbAFrame.getOrigin() = pivotInA;
 221                 m_rbAFrame.getBasis().setValue(rbAxisA1.getX(), rbAxisA2.getX(), axisInA.getX(),
 222                                                                            rbAxisA1.getY(), rbAxisA2.getY(), axisInA.getY(),
 223                                                                            rbAxisA1.getZ(), rbAxisA2.getZ(), axisInA.getZ());
 224
 225                 btVector3 axisInB = m_rbA.getCenterOfMassTransform().getBasis() * axisInA;
 226
 227                 btQuaternion rotationArc = shortestArcQuat(axisInA, axisInB);
 228                 btVector3 rbAxisB1 = quatRotate(rotationArc, rbAxisA1);
 229                 btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
 230
 231                 m_rbBFrame.getOrigin() = m_rbB.getCenterOfMassTransform().inverse()(m_rbA.getCenterOfMassTransform()(pivotInA));
 232
 233                 m_rbBFrame.getBasis().setValue(rbAxisB1.getX(), rbAxisB2.getX(), axisInB.getX(),
 234                                                                            rbAxisB1.getY(), rbAxisB2.getY(), axisInB.getY(),
 235                                                                            rbAxisB1.getZ(), rbAxisB2.getZ(), axisInB.getZ());
 236                 m_rbBFrame.getBasis() = m_rbB.getCenterOfMassTransform().getBasis().inverse() * m_rbBFrame.getBasis();
 237         }
 238
 239         bool hasLimit() const
 240         {
 241 #ifdef _BT_USE_CENTER_LIMIT_
 242                 return m_limit.getHalfRange() > 0;
 243 #else
 244                 return m_lowerLimit <= m_upperLimit;
 245 #endif
 246         }
 247
 248         btScalar getLowerLimit() const
 249         {
 250 #ifdef _BT_USE_CENTER_LIMIT_
 251                 return m_limit.getLow();
 252 #else
 253                 return m_lowerLimit;
 254 #endif
 255         }
 256
 257         btScalar getUpperLimit() const
 258         {
 259 #ifdef _BT_USE_CENTER_LIMIT_
 260                 return m_limit.getHigh();
 261 #else
 262                 return m_upperLimit;
 263 #endif
 264         }
 265
 266         ///The getHingeAngle gives the hinge angle in range [-PI,PI]
 267         btScalar getHingeAngle();
 268
 269         btScalar getHingeAngle(const btTransform& transA, const btTransform& transB);
 270
 271         void testLimit(const btTransform& transA, const btTransform& transB);
 272
 273         const btTransform& getAFrame() const { return m_rbAFrame; };
 274         const btTransform& getBFrame() const { return m_rbBFrame; };
 275
 276         btTransform& getAFrame() { return m_rbAFrame; };
 277         btTransform& getBFrame() { return m_rbBFrame; };
 278
 279         inline int getSolveLimit()
 280         {
 281 #ifdef _BT_USE_CENTER_LIMIT_
 282                 return m_limit.isLimit();
 283 #else
 284                 return m_solveLimit;
 285 #endif
 286         }
 287
 288         inline btScalar getLimitSign()
 289         {
 290 #ifdef _BT_USE_CENTER_LIMIT_
 291                 return m_limit.getSign();
 292 #else
 293                 return m_limitSign;
 294 #endif
 295         }
 296
 297         inline bool getAngularOnly()
 298         {
 299                 return m_angularOnly;
 300         }
 301         inline bool getEnableAngularMotor()
 302         {
 303                 return m_enableAngularMotor;
 304         }
 305         inline btScalar getMotorTargetVelocity()
 306         {
 307                 return m_motorTargetVelocity;
 308         }
 309         inline btScalar getMaxMotorImpulse()
 310         {
 311                 return m_maxMotorImpulse;
 312         }
 313         // access for UseFrameOffset
 314         bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
 315         void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
 316         // access for UseReferenceFrameA
 317         bool getUseReferenceFrameA() const { return m_useReferenceFrameA; }
 318         void setUseReferenceFrameA(bool useReferenceFrameA) { m_useReferenceFrameA = useReferenceFrameA; }
 319
 320         ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 321         ///If no axis is provided, it uses the default axis for this constraint.
 322         virtual void setParam(int num, btScalar value, int axis = -1);
 323         ///return the local value of parameter
 324         virtual btScalar getParam(int num, int axis = -1) const;
 325
 326         virtual int getFlags() const
 327         {
 328                 return m_flags;
 329         }
 330
 331         virtual int calculateSerializeBufferSize() const;
 332
 333         ///fills the dataBuffer and returns the struct name (and 0 on failure)
 334         virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 335 };
 336
 337 //only for backward compatibility
 338 #ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
 339 ///this structure is not used, except for loading pre-2.82 .bullet files
 340 struct btHingeConstraintDoubleData
 341 {
 342         btTypedConstraintData m_typeConstraintData;
 343         btTransformDoubleData m_rbAFrame;  // constraint axii. Assumes z is hinge axis.
 344         btTransformDoubleData m_rbBFrame;
 345         int m_useReferenceFrameA;
 346         int m_angularOnly;
 347         int m_enableAngularMotor;
 348         float m_motorTargetVelocity;
 349         float m_maxMotorImpulse;
 350
 351         float m_lowerLimit;
 352         float m_upperLimit;
 353         float m_limitSoftness;
 354         float m_biasFactor;
 355         float m_relaxationFactor;
 356 };
 357 #endif  //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
 358
 359 ///The getAccumulatedHingeAngle returns the accumulated hinge angle, taking rotation across the -PI/PI boundary into account
 360 ATTRIBUTE_ALIGNED16(class)
 361 btHingeAccumulatedAngleConstraint : public btHingeConstraint
 362 {
 363 protected:
 364         btScalar m_accumulatedAngle;
 365
 366 public:
 367         BT_DECLARE_ALIGNED_ALLOCATOR();
 368
 369         btHingeAccumulatedAngleConstraint(btRigidBody & rbA, btRigidBody & rbB, const btVector3& pivotInA, const btVector3& pivotInB, const btVector3& axisInA, const btVector3& axisInB, bool useReferenceFrameA = false)
 370                 : btHingeConstraint(rbA, rbB, pivotInA, pivotInB, axisInA, axisInB, useReferenceFrameA)
 371         {
 372                 m_accumulatedAngle = getHingeAngle();
 373         }
 374
 375         btHingeAccumulatedAngleConstraint(btRigidBody & rbA, const btVector3& pivotInA, const btVector3& axisInA, bool useReferenceFrameA = false)
 376                 : btHingeConstraint(rbA, pivotInA, axisInA, useReferenceFrameA)
 377         {
 378                 m_accumulatedAngle = getHingeAngle();
 379         }
 380
 381         btHingeAccumulatedAngleConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false)
 382                 : btHingeConstraint(rbA, rbB, rbAFrame, rbBFrame, useReferenceFrameA)
 383         {
 384                 m_accumulatedAngle = getHingeAngle();
 385         }
 386
 387         btHingeAccumulatedAngleConstraint(btRigidBody & rbA, const btTransform& rbAFrame, bool useReferenceFrameA = false)
 388                 : btHingeConstraint(rbA, rbAFrame, useReferenceFrameA)
 389         {
 390                 m_accumulatedAngle = getHingeAngle();
 391         }
 392         btScalar getAccumulatedHingeAngle();
 393         void setAccumulatedHingeAngle(btScalar accAngle);
 394         virtual void getInfo1(btConstraintInfo1 * info);
 395 };
 396
 397 struct btHingeConstraintFloatData
 398 {
 399         btTypedConstraintData m_typeConstraintData;
 400         btTransformFloatData m_rbAFrame;  // constraint axii. Assumes z is hinge axis.
 401         btTransformFloatData m_rbBFrame;
 402         int m_useReferenceFrameA;
 403         int m_angularOnly;
 404
 405         int m_enableAngularMotor;
 406         float m_motorTargetVelocity;
 407         float m_maxMotorImpulse;
 408
 409         float m_lowerLimit;
 410         float m_upperLimit;
 411         float m_limitSoftness;
 412         float m_biasFactor;
 413         float m_relaxationFactor;
 414 };
 415
 416 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 417 struct btHingeConstraintDoubleData2
 418 {
 419         btTypedConstraintDoubleData m_typeConstraintData;
 420         btTransformDoubleData m_rbAFrame;  // constraint axii. Assumes z is hinge axis.
 421         btTransformDoubleData m_rbBFrame;
 422         int m_useReferenceFrameA;
 423         int m_angularOnly;
 424         int m_enableAngularMotor;
 425         double m_motorTargetVelocity;
 426         double m_maxMotorImpulse;
 427
 428         double m_lowerLimit;
 429         double m_upperLimit;
 430         double m_limitSoftness;
 431         double m_biasFactor;
 432         double m_relaxationFactor;
 433         char m_padding1[4];
 434 };
 435
 436 SIMD_FORCE_INLINE int btHingeConstraint::calculateSerializeBufferSize() const
 437 {
 438         return sizeof(btHingeConstraintData);
 439 }
 440
 441 ///fills the dataBuffer and returns the struct name (and 0 on failure)
 442 SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 443 {
 444         btHingeConstraintData* hingeData = (btHingeConstraintData*)dataBuffer;
 445         btTypedConstraint::serialize(&hingeData->m_typeConstraintData, serializer);
 446
 447         m_rbAFrame.serialize(hingeData->m_rbAFrame);
 448         m_rbBFrame.serialize(hingeData->m_rbBFrame);
 449
 450         hingeData->m_angularOnly = m_angularOnly;
 451         hingeData->m_enableAngularMotor = m_enableAngularMotor;
 452         hingeData->m_maxMotorImpulse = float(m_maxMotorImpulse);
 453         hingeData->m_motorTargetVelocity = float(m_motorTargetVelocity);
 454         hingeData->m_useReferenceFrameA = m_useReferenceFrameA;
 455 #ifdef _BT_USE_CENTER_LIMIT_
 456         hingeData->m_lowerLimit = float(m_limit.getLow());
 457         hingeData->m_upperLimit = float(m_limit.getHigh());
 458         hingeData->m_limitSoftness = float(m_limit.getSoftness());
 459         hingeData->m_biasFactor = float(m_limit.getBiasFactor());
 460         hingeData->m_relaxationFactor = float(m_limit.getRelaxationFactor());
 461 #else
 462         hingeData->m_lowerLimit = float(m_lowerLimit);
 463         hingeData->m_upperLimit = float(m_upperLimit);
 464         hingeData->m_limitSoftness = float(m_limitSoftness);
 465         hingeData->m_biasFactor = float(m_biasFactor);
 466         hingeData->m_relaxationFactor = float(m_relaxationFactor);
 467 #endif
 468
 469         // Fill padding with zeros to appease msan.
 470 #ifdef BT_USE_DOUBLE_PRECISION
 471         hingeData->m_padding1[0] = 0;
 472         hingeData->m_padding1[1] = 0;
 473         hingeData->m_padding1[2] = 0;
 474         hingeData->m_padding1[3] = 0;
 475 #endif
 476
 477         return btHingeConstraintDataName;
 478 }
 479
 480 #endif  //BT_HINGECONSTRAINT_H