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