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

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2010 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 #ifndef BT_TYPED_CONSTRAINT_H
  17 #define BT_TYPED_CONSTRAINT_H
  18
  19 #include "LinearMath/btScalar.h"
  20 #include "btSolverConstraint.h"
  21 #include "BulletDynamics/Dynamics/btRigidBody.h"
  22
  23 #ifdef BT_USE_DOUBLE_PRECISION
  24 #define btTypedConstraintData2 btTypedConstraintDoubleData
  25 #define btTypedConstraintDataName "btTypedConstraintDoubleData"
  26 #else
  27 #define btTypedConstraintData2 btTypedConstraintFloatData
  28 #define btTypedConstraintDataName "btTypedConstraintFloatData"
  29 #endif  //BT_USE_DOUBLE_PRECISION
  30
  31 class btSerializer;
  32
  33 //Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
  34 enum btTypedConstraintType
  35 {
  36         POINT2POINT_CONSTRAINT_TYPE = 3,
  37         HINGE_CONSTRAINT_TYPE,
  38         CONETWIST_CONSTRAINT_TYPE,
  39         D6_CONSTRAINT_TYPE,
  40         SLIDER_CONSTRAINT_TYPE,
  41         CONTACT_CONSTRAINT_TYPE,
  42         D6_SPRING_CONSTRAINT_TYPE,
  43         GEAR_CONSTRAINT_TYPE,
  44         FIXED_CONSTRAINT_TYPE,
  45         D6_SPRING_2_CONSTRAINT_TYPE,
  46         MAX_CONSTRAINT_TYPE
  47 };
  48
  49 enum btConstraintParams
  50 {
  51         BT_CONSTRAINT_ERP = 1,
  52         BT_CONSTRAINT_STOP_ERP,
  53         BT_CONSTRAINT_CFM,
  54         BT_CONSTRAINT_STOP_CFM
  55 };
  56
  57 #if 1
  58 #define btAssertConstrParams(_par) btAssert(_par)
  59 #else
  60 #define btAssertConstrParams(_par)
  61 #endif
  62
  63 ATTRIBUTE_ALIGNED16(struct)
  64 btJointFeedback
  65 {
  66         BT_DECLARE_ALIGNED_ALLOCATOR();
  67         btVector3 m_appliedForceBodyA;
  68         btVector3 m_appliedTorqueBodyA;
  69         btVector3 m_appliedForceBodyB;
  70         btVector3 m_appliedTorqueBodyB;
  71 };
  72
  73 ///TypedConstraint is the baseclass for Bullet constraints and vehicles
  74 ATTRIBUTE_ALIGNED16(class)
  75 btTypedConstraint : public btTypedObject
  76 {
  77         int m_userConstraintType;
  78
  79         union {
  80                 int m_userConstraintId;
  81                 void* m_userConstraintPtr;
  82         };
  83
  84         btScalar m_breakingImpulseThreshold;
  85         bool m_isEnabled;
  86         bool m_needsFeedback;
  87         int m_overrideNumSolverIterations;
  88
  89         btTypedConstraint& operator=(btTypedConstraint& other)
  90         {
  91                 btAssert(0);
  92                 (void)other;
  93                 return *this;
  94         }
  95
  96 protected:
  97         btRigidBody& m_rbA;
  98         btRigidBody& m_rbB;
  99         btScalar m_appliedImpulse;
 100         btScalar m_dbgDrawSize;
 101         btJointFeedback* m_jointFeedback;
 102
 103         ///internal method used by the constraint solver, don't use them directly
 104         btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
 105
 106 public:
 107         BT_DECLARE_ALIGNED_ALLOCATOR();
 108
 109         virtual ~btTypedConstraint(){};
 110         btTypedConstraint(btTypedConstraintType type, btRigidBody & rbA);
 111         btTypedConstraint(btTypedConstraintType type, btRigidBody & rbA, btRigidBody & rbB);
 112
 113         struct btConstraintInfo1
 114         {
 115                 int m_numConstraintRows, nub;
 116         };
 117
 118         static btRigidBody& getFixedBody();
 119
 120         struct btConstraintInfo2
 121         {
 122                 // integrator parameters: frames per second (1/stepsize), default error
 123                 // reduction parameter (0..1).
 124                 btScalar fps, erp;
 125
 126                 // for the first and second body, pointers to two (linear and angular)
 127                 // n*3 jacobian sub matrices, stored by rows. these matrices will have
 128                 // been initialized to 0 on entry. if the second body is zero then the
 129                 // J2xx pointers may be 0.
 130                 btScalar *m_J1linearAxis, *m_J1angularAxis, *m_J2linearAxis, *m_J2angularAxis;
 131
 132                 // elements to jump from one row to the next in J's
 133                 int rowskip;
 134
 135                 // right hand sides of the equation J*v = c + cfm * lambda. cfm is the
 136                 // "constraint force mixing" vector. c is set to zero on entry, cfm is
 137                 // set to a constant value (typically very small or zero) value on entry.
 138                 btScalar *m_constraintError, *cfm;
 139
 140                 // lo and hi limits for variables (set to -/+ infinity on entry).
 141                 btScalar *m_lowerLimit, *m_upperLimit;
 142
 143                 // number of solver iterations
 144                 int m_numIterations;
 145
 146                 //damping of the velocity
 147                 btScalar m_damping;
 148         };
 149
 150         int getOverrideNumSolverIterations() const
 151         {
 152                 return m_overrideNumSolverIterations;
 153         }
 154
 155         ///override the number of constraint solver iterations used to solve this constraint
 156         ///-1 will use the default number of iterations, as specified in SolverInfo.m_numIterations
 157         void setOverrideNumSolverIterations(int overideNumIterations)
 158         {
 159                 m_overrideNumSolverIterations = overideNumIterations;
 160         }
 161
 162         ///internal method used by the constraint solver, don't use them directly
 163         virtual void buildJacobian(){};
 164
 165         ///internal method used by the constraint solver, don't use them directly
 166         virtual void setupSolverConstraint(btConstraintArray & ca, int solverBodyA, int solverBodyB, btScalar timeStep)
 167         {
 168                 (void)ca;
 169                 (void)solverBodyA;
 170                 (void)solverBodyB;
 171                 (void)timeStep;
 172         }
 173
 174         ///internal method used by the constraint solver, don't use them directly
 175         virtual void getInfo1(btConstraintInfo1 * info) = 0;
 176
 177         ///internal method used by the constraint solver, don't use them directly
 178         virtual void getInfo2(btConstraintInfo2 * info) = 0;
 179
 180         ///internal method used by the constraint solver, don't use them directly
 181         void internalSetAppliedImpulse(btScalar appliedImpulse)
 182         {
 183                 m_appliedImpulse = appliedImpulse;
 184         }
 185         ///internal method used by the constraint solver, don't use them directly
 186         btScalar internalGetAppliedImpulse()
 187         {
 188                 return m_appliedImpulse;
 189         }
 190
 191         btScalar getBreakingImpulseThreshold() const
 192         {
 193                 return m_breakingImpulseThreshold;
 194         }
 195
 196         void setBreakingImpulseThreshold(btScalar threshold)
 197         {
 198                 m_breakingImpulseThreshold = threshold;
 199         }
 200
 201         bool isEnabled() const
 202         {
 203                 return m_isEnabled;
 204         }
 205
 206         void setEnabled(bool enabled)
 207         {
 208                 m_isEnabled = enabled;
 209         }
 210
 211         ///internal method used by the constraint solver, don't use them directly
 212         virtual void solveConstraintObsolete(btSolverBody& /*bodyA*/, btSolverBody& /*bodyB*/, btScalar /*timeStep*/){};
 213
 214         const btRigidBody& getRigidBodyA() const
 215         {
 216                 return m_rbA;
 217         }
 218         const btRigidBody& getRigidBodyB() const
 219         {
 220                 return m_rbB;
 221         }
 222
 223         btRigidBody& getRigidBodyA()
 224         {
 225                 return m_rbA;
 226         }
 227         btRigidBody& getRigidBodyB()
 228         {
 229                 return m_rbB;
 230         }
 231
 232         int getUserConstraintType() const
 233         {
 234                 return m_userConstraintType;
 235         }
 236
 237         void setUserConstraintType(int userConstraintType)
 238         {
 239                 m_userConstraintType = userConstraintType;
 240         };
 241
 242         void setUserConstraintId(int uid)
 243         {
 244                 m_userConstraintId = uid;
 245         }
 246
 247         int getUserConstraintId() const
 248         {
 249                 return m_userConstraintId;
 250         }
 251
 252         void setUserConstraintPtr(void* ptr)
 253         {
 254                 m_userConstraintPtr = ptr;
 255         }
 256
 257         void* getUserConstraintPtr()
 258         {
 259                 return m_userConstraintPtr;
 260         }
 261
 262         void setJointFeedback(btJointFeedback * jointFeedback)
 263         {
 264                 m_jointFeedback = jointFeedback;
 265         }
 266
 267         const btJointFeedback* getJointFeedback() const
 268         {
 269                 return m_jointFeedback;
 270         }
 271
 272         btJointFeedback* getJointFeedback()
 273         {
 274                 return m_jointFeedback;
 275         }
 276
 277         int getUid() const
 278         {
 279                 return m_userConstraintId;
 280         }
 281
 282         bool needsFeedback() const
 283         {
 284                 return m_needsFeedback;
 285         }
 286
 287         ///enableFeedback will allow to read the applied linear and angular impulse
 288         ///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information
 289         void enableFeedback(bool needsFeedback)
 290         {
 291                 m_needsFeedback = needsFeedback;
 292         }
 293
 294         ///getAppliedImpulse is an estimated total applied impulse.
 295         ///This feedback could be used to determine breaking constraints or playing sounds.
 296         btScalar getAppliedImpulse() const
 297         {
 298                 btAssert(m_needsFeedback);
 299                 return m_appliedImpulse;
 300         }
 301
 302         btTypedConstraintType getConstraintType() const
 303         {
 304                 return btTypedConstraintType(m_objectType);
 305         }
 306
 307         void setDbgDrawSize(btScalar dbgDrawSize)
 308         {
 309                 m_dbgDrawSize = dbgDrawSize;
 310         }
 311         btScalar getDbgDrawSize()
 312         {
 313                 return m_dbgDrawSize;
 314         }
 315
 316         ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 317         ///If no axis is provided, it uses the default axis for this constraint.
 318         virtual void setParam(int num, btScalar value, int axis = -1) = 0;
 319
 320         ///return the local value of parameter
 321         virtual btScalar getParam(int num, int axis = -1) const = 0;
 322
 323         virtual int calculateSerializeBufferSize() const;
 324
 325         ///fills the dataBuffer and returns the struct name (and 0 on failure)
 326         virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 327 };
 328
 329 // returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits
 330 // all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI])
 331 SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
 332 {
 333         if (angleLowerLimitInRadians >= angleUpperLimitInRadians)
 334         {
 335                 return angleInRadians;
 336         }
 337         else if (angleInRadians < angleLowerLimitInRadians)
 338         {
 339                 btScalar diffLo = btFabs(btNormalizeAngle(angleLowerLimitInRadians - angleInRadians));
 340                 btScalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians));
 341                 return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI);
 342         }
 343         else if (angleInRadians > angleUpperLimitInRadians)
 344         {
 345                 btScalar diffHi = btFabs(btNormalizeAngle(angleInRadians - angleUpperLimitInRadians));
 346                 btScalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians));
 347                 return (diffLo < diffHi) ? (angleInRadians - SIMD_2_PI) : angleInRadians;
 348         }
 349         else
 350         {
 351                 return angleInRadians;
 352         }
 353 }
 354
 355 // clang-format off
 356
 357 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 358 struct  btTypedConstraintFloatData
 359 {
 360         btRigidBodyFloatData            *m_rbA;
 361         btRigidBodyFloatData            *m_rbB;
 362         char    *m_name;
 363
 364         int     m_objectType;
 365         int     m_userConstraintType;
 366         int     m_userConstraintId;
 367         int     m_needsFeedback;
 368
 369         float   m_appliedImpulse;
 370         float   m_dbgDrawSize;
 371
 372         int     m_disableCollisionsBetweenLinkedBodies;
 373         int     m_overrideNumSolverIterations;
 374
 375         float   m_breakingImpulseThreshold;
 376         int             m_isEnabled;
 377
 378 };
 379
 380
 381
 382 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 383
 384 #define BT_BACKWARDS_COMPATIBLE_SERIALIZATION
 385 #ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
 386 ///this structure is not used, except for loading pre-2.82 .bullet files
 387 struct  btTypedConstraintData
 388 {
 389         btRigidBodyData         *m_rbA;
 390         btRigidBodyData         *m_rbB;
 391         char    *m_name;
 392
 393         int     m_objectType;
 394         int     m_userConstraintType;
 395         int     m_userConstraintId;
 396         int     m_needsFeedback;
 397
 398         float   m_appliedImpulse;
 399         float   m_dbgDrawSize;
 400
 401         int     m_disableCollisionsBetweenLinkedBodies;
 402         int     m_overrideNumSolverIterations;
 403
 404         float   m_breakingImpulseThreshold;
 405         int             m_isEnabled;
 406
 407 };
 408 #endif //BACKWARDS_COMPATIBLE
 409
 410 struct  btTypedConstraintDoubleData
 411 {
 412         btRigidBodyDoubleData           *m_rbA;
 413         btRigidBodyDoubleData           *m_rbB;
 414         char    *m_name;
 415
 416         int     m_objectType;
 417         int     m_userConstraintType;
 418         int     m_userConstraintId;
 419         int     m_needsFeedback;
 420
 421         double  m_appliedImpulse;
 422         double  m_dbgDrawSize;
 423
 424         int     m_disableCollisionsBetweenLinkedBodies;
 425         int     m_overrideNumSolverIterations;
 426
 427         double  m_breakingImpulseThreshold;
 428         int             m_isEnabled;
 429         char    padding[4];
 430
 431 };
 432
 433 // clang-format on
 434
 435 SIMD_FORCE_INLINE int btTypedConstraint::calculateSerializeBufferSize() const
 436 {
 437         return sizeof(btTypedConstraintData2);
 438 }
 439
 440 class btAngularLimit
 441 {
 442 private:
 443         btScalar
 444                 m_center,
 445                 m_halfRange,
 446                 m_softness,
 447                 m_biasFactor,
 448                 m_relaxationFactor,
 449                 m_correction,
 450                 m_sign;
 451
 452         bool
 453                 m_solveLimit;
 454
 455 public:
 456         /// Default constructor initializes limit as inactive, allowing free constraint movement
 457         btAngularLimit()
 458                 : m_center(0.0f),
 459                   m_halfRange(-1.0f),
 460                   m_softness(0.9f),
 461                   m_biasFactor(0.3f),
 462                   m_relaxationFactor(1.0f),
 463                   m_correction(0.0f),
 464                   m_sign(0.0f),
 465                   m_solveLimit(false)
 466         {
 467         }
 468
 469         /// Sets all limit's parameters.
 470         /// When low > high limit becomes inactive.
 471         /// When high - low > 2PI limit is ineffective too becouse no angle can exceed the limit
 472         void set(btScalar low, btScalar high, btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f);
 473
 474         /// Checks conastaint angle against limit. If limit is active and the angle violates the limit
 475         /// correction is calculated.
 476         void test(const btScalar angle);
 477
 478         /// Returns limit's softness
 479         inline btScalar getSoftness() const
 480         {
 481                 return m_softness;
 482         }
 483
 484         /// Returns limit's bias factor
 485         inline btScalar getBiasFactor() const
 486         {
 487                 return m_biasFactor;
 488         }
 489
 490         /// Returns limit's relaxation factor
 491         inline btScalar getRelaxationFactor() const
 492         {
 493                 return m_relaxationFactor;
 494         }
 495
 496         /// Returns correction value evaluated when test() was invoked
 497         inline btScalar getCorrection() const
 498         {
 499                 return m_correction;
 500         }
 501
 502         /// Returns sign value evaluated when test() was invoked
 503         inline btScalar getSign() const
 504         {
 505                 return m_sign;
 506         }
 507
 508         /// Gives half of the distance between min and max limit angle
 509         inline btScalar getHalfRange() const
 510         {
 511                 return m_halfRange;
 512         }
 513
 514         /// Returns true when the last test() invocation recognized limit violation
 515         inline bool isLimit() const
 516         {
 517                 return m_solveLimit;
 518         }
 519
 520         /// Checks given angle against limit. If limit is active and angle doesn't fit it, the angle
 521         /// returned is modified so it equals to the limit closest to given angle.
 522         void fit(btScalar& angle) const;
 523
 524         /// Returns correction value multiplied by sign value
 525         btScalar getError() const;
 526
 527         btScalar getLow() const;
 528
 529         btScalar getHigh() const;
 530 };
 531
 532 #endif  //BT_TYPED_CONSTRAINT_H