dali-physics/third-party/bullet3/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.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 /// 2009 March: b3Generic6DofConstraint refactored by Roman Ponomarev
  17 /// Added support for generic constraint solver through getInfo1/getInfo2 methods
  18
  19 /*
  20 2007-09-09
  21 b3Generic6DofConstraint Refactored by Francisco Le?n
  22 email: projectileman@yahoo.com
  23 http://gimpact.sf.net
  24 */
  25
  26 #ifndef B3_GENERIC_6DOF_CONSTRAINT_H
  27 #define B3_GENERIC_6DOF_CONSTRAINT_H
  28
  29 #include "Bullet3Common/b3Vector3.h"
  30 #include "b3JacobianEntry.h"
  31 #include "b3TypedConstraint.h"
  32
  33 struct b3RigidBodyData;
  34
  35 //! Rotation Limit structure for generic joints
  36 class b3RotationalLimitMotor
  37 {
  38 public:
  39         //! limit_parameters
  40         //!@{
  41         b3Scalar m_loLimit;         //!< joint limit
  42         b3Scalar m_hiLimit;         //!< joint limit
  43         b3Scalar m_targetVelocity;  //!< target motor velocity
  44         b3Scalar m_maxMotorForce;   //!< max force on motor
  45         b3Scalar m_maxLimitForce;   //!< max force on limit
  46         b3Scalar m_damping;         //!< Damping.
  47         b3Scalar m_limitSoftness;   //! Relaxation factor
  48         b3Scalar m_normalCFM;       //!< Constraint force mixing factor
  49         b3Scalar m_stopERP;         //!< Error tolerance factor when joint is at limit
  50         b3Scalar m_stopCFM;         //!< Constraint force mixing factor when joint is at limit
  51         b3Scalar m_bounce;          //!< restitution factor
  52         bool m_enableMotor;
  53
  54         //!@}
  55
  56         //! temp_variables
  57         //!@{
  58         b3Scalar m_currentLimitError;  //!  How much is violated this limit
  59         b3Scalar m_currentPosition;    //!  current value of angle
  60         int m_currentLimit;            //!< 0=free, 1=at lo limit, 2=at hi limit
  61         b3Scalar m_accumulatedImpulse;
  62         //!@}
  63
  64         b3RotationalLimitMotor()
  65         {
  66                 m_accumulatedImpulse = 0.f;
  67                 m_targetVelocity = 0;
  68                 m_maxMotorForce = 6.0f;
  69                 m_maxLimitForce = 300.0f;
  70                 m_loLimit = 1.0f;
  71                 m_hiLimit = -1.0f;
  72                 m_normalCFM = 0.f;
  73                 m_stopERP = 0.2f;
  74                 m_stopCFM = 0.f;
  75                 m_bounce = 0.0f;
  76                 m_damping = 1.0f;
  77                 m_limitSoftness = 0.5f;
  78                 m_currentLimit = 0;
  79                 m_currentLimitError = 0;
  80                 m_enableMotor = false;
  81         }
  82
  83         b3RotationalLimitMotor(const b3RotationalLimitMotor& limot)
  84         {
  85                 m_targetVelocity = limot.m_targetVelocity;
  86                 m_maxMotorForce = limot.m_maxMotorForce;
  87                 m_limitSoftness = limot.m_limitSoftness;
  88                 m_loLimit = limot.m_loLimit;
  89                 m_hiLimit = limot.m_hiLimit;
  90                 m_normalCFM = limot.m_normalCFM;
  91                 m_stopERP = limot.m_stopERP;
  92                 m_stopCFM = limot.m_stopCFM;
  93                 m_bounce = limot.m_bounce;
  94                 m_currentLimit = limot.m_currentLimit;
  95                 m_currentLimitError = limot.m_currentLimitError;
  96                 m_enableMotor = limot.m_enableMotor;
  97         }
  98
  99         //! Is limited
 100         bool isLimited()
 101         {
 102                 if (m_loLimit > m_hiLimit) return false;
 103                 return true;
 104         }
 105
 106         //! Need apply correction
 107         bool needApplyTorques()
 108         {
 109                 if (m_currentLimit == 0 && m_enableMotor == false) return false;
 110                 return true;
 111         }
 112
 113         //! calculates  error
 114         /*!
 115         calculates m_currentLimit and m_currentLimitError.
 116         */
 117         int testLimitValue(b3Scalar test_value);
 118
 119         //! apply the correction impulses for two bodies
 120         b3Scalar solveAngularLimits(b3Scalar timeStep, b3Vector3& axis, b3Scalar jacDiagABInv, b3RigidBodyData* body0, b3RigidBodyData* body1);
 121 };
 122
 123 class b3TranslationalLimitMotor
 124 {
 125 public:
 126         b3Vector3 m_lowerLimit;  //!< the constraint lower limits
 127         b3Vector3 m_upperLimit;  //!< the constraint upper limits
 128         b3Vector3 m_accumulatedImpulse;
 129         //! Linear_Limit_parameters
 130         //!@{
 131         b3Vector3 m_normalCFM;          //!< Constraint force mixing factor
 132         b3Vector3 m_stopERP;            //!< Error tolerance factor when joint is at limit
 133         b3Vector3 m_stopCFM;            //!< Constraint force mixing factor when joint is at limit
 134         b3Vector3 m_targetVelocity;     //!< target motor velocity
 135         b3Vector3 m_maxMotorForce;      //!< max force on motor
 136         b3Vector3 m_currentLimitError;  //!  How much is violated this limit
 137         b3Vector3 m_currentLinearDiff;  //!  Current relative offset of constraint frames
 138         b3Scalar m_limitSoftness;       //!< Softness for linear limit
 139         b3Scalar m_damping;             //!< Damping for linear limit
 140         b3Scalar m_restitution;         //! Bounce parameter for linear limit
 141         //!@}
 142         bool m_enableMotor[3];
 143         int m_currentLimit[3];  //!< 0=free, 1=at lower limit, 2=at upper limit
 144
 145         b3TranslationalLimitMotor()
 146         {
 147                 m_lowerLimit.setValue(0.f, 0.f, 0.f);
 148                 m_upperLimit.setValue(0.f, 0.f, 0.f);
 149                 m_accumulatedImpulse.setValue(0.f, 0.f, 0.f);
 150                 m_normalCFM.setValue(0.f, 0.f, 0.f);
 151                 m_stopERP.setValue(0.2f, 0.2f, 0.2f);
 152                 m_stopCFM.setValue(0.f, 0.f, 0.f);
 153
 154                 m_limitSoftness = 0.7f;
 155                 m_damping = b3Scalar(1.0f);
 156                 m_restitution = b3Scalar(0.5f);
 157                 for (int i = 0; i < 3; i++)
 158                 {
 159                         m_enableMotor[i] = false;
 160                         m_targetVelocity[i] = b3Scalar(0.f);
 161                         m_maxMotorForce[i] = b3Scalar(0.f);
 162                 }
 163         }
 164
 165         b3TranslationalLimitMotor(const b3TranslationalLimitMotor& other)
 166         {
 167                 m_lowerLimit = other.m_lowerLimit;
 168                 m_upperLimit = other.m_upperLimit;
 169                 m_accumulatedImpulse = other.m_accumulatedImpulse;
 170
 171                 m_limitSoftness = other.m_limitSoftness;
 172                 m_damping = other.m_damping;
 173                 m_restitution = other.m_restitution;
 174                 m_normalCFM = other.m_normalCFM;
 175                 m_stopERP = other.m_stopERP;
 176                 m_stopCFM = other.m_stopCFM;
 177
 178                 for (int i = 0; i < 3; i++)
 179                 {
 180                         m_enableMotor[i] = other.m_enableMotor[i];
 181                         m_targetVelocity[i] = other.m_targetVelocity[i];
 182                         m_maxMotorForce[i] = other.m_maxMotorForce[i];
 183                 }
 184         }
 185
 186         //! Test limit
 187         /*!
 188     - free means upper < lower,
 189     - locked means upper == lower
 190     - limited means upper > lower
 191     - limitIndex: first 3 are linear, next 3 are angular
 192     */
 193         inline bool isLimited(int limitIndex)
 194         {
 195                 return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
 196         }
 197         inline bool needApplyForce(int limitIndex)
 198         {
 199                 if (m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
 200                 return true;
 201         }
 202         int testLimitValue(int limitIndex, b3Scalar test_value);
 203
 204         b3Scalar solveLinearAxis(
 205                 b3Scalar timeStep,
 206                 b3Scalar jacDiagABInv,
 207                 b3RigidBodyData& body1, const b3Vector3& pointInA,
 208                 b3RigidBodyData& body2, const b3Vector3& pointInB,
 209                 int limit_index,
 210                 const b3Vector3& axis_normal_on_a,
 211                 const b3Vector3& anchorPos);
 212 };
 213
 214 enum b36DofFlags
 215 {
 216         B3_6DOF_FLAGS_CFM_NORM = 1,
 217         B3_6DOF_FLAGS_CFM_STOP = 2,
 218         B3_6DOF_FLAGS_ERP_STOP = 4
 219 };
 220 #define B3_6DOF_FLAGS_AXIS_SHIFT 3  // bits per axis
 221
 222 /// b3Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
 223 /*!
 224 b3Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'.
 225 currently this limit supports rotational motors<br>
 226 <ul>
 227 <li> For Linear limits, use b3Generic6DofConstraint.setLinearUpperLimit, b3Generic6DofConstraint.setLinearLowerLimit. You can set the parameters with the b3TranslationalLimitMotor structure accsesible through the b3Generic6DofConstraint.getTranslationalLimitMotor method.
 228 At this moment translational motors are not supported. May be in the future. </li>
 229
 230 <li> For Angular limits, use the b3RotationalLimitMotor structure for configuring the limit.
 231 This is accessible through b3Generic6DofConstraint.getLimitMotor method,
 232 This brings support for limit parameters and motors. </li>
 233
 234 <li> Angulars limits have these possible ranges:
 235 <table border=1 >
 236 <tr>
 237         <td><b>AXIS</b></td>
 238         <td><b>MIN ANGLE</b></td>
 239         <td><b>MAX ANGLE</b></td>
 240 </tr><tr>
 241         <td>X</td>
 242         <td>-PI</td>
 243         <td>PI</td>
 244 </tr><tr>
 245         <td>Y</td>
 246         <td>-PI/2</td>
 247         <td>PI/2</td>
 248 </tr><tr>
 249         <td>Z</td>
 250         <td>-PI</td>
 251         <td>PI</td>
 252 </tr>
 253 </table>
 254 </li>
 255 </ul>
 256
 257 */
 258 B3_ATTRIBUTE_ALIGNED16(class)
 259 b3Generic6DofConstraint : public b3TypedConstraint
 260 {
 261 protected:
 262         //! relative_frames
 263         //!@{
 264         b3Transform m_frameInA;  //!< the constraint space w.r.t body A
 265         b3Transform m_frameInB;  //!< the constraint space w.r.t body B
 266         //!@}
 267
 268         //! Jacobians
 269         //!@{
 270         //    b3JacobianEntry   m_jacLinear[3];//!< 3 orthogonal linear constraints
 271         //    b3JacobianEntry   m_jacAng[3];//!< 3 orthogonal angular constraints
 272         //!@}
 273
 274         //! Linear_Limit_parameters
 275         //!@{
 276         b3TranslationalLimitMotor m_linearLimits;
 277         //!@}
 278
 279         //! hinge_parameters
 280         //!@{
 281         b3RotationalLimitMotor m_angularLimits[3];
 282         //!@}
 283
 284 protected:
 285         //! temporal variables
 286         //!@{
 287         b3Transform m_calculatedTransformA;
 288         b3Transform m_calculatedTransformB;
 289         b3Vector3 m_calculatedAxisAngleDiff;
 290         b3Vector3 m_calculatedAxis[3];
 291         b3Vector3 m_calculatedLinearDiff;
 292         b3Scalar m_timeStep;
 293         b3Scalar m_factA;
 294         b3Scalar m_factB;
 295         bool m_hasStaticBody;
 296
 297         b3Vector3 m_AnchorPos;  // point betwen pivots of bodies A and B to solve linear axes
 298
 299         bool m_useLinearReferenceFrameA;
 300         bool m_useOffsetForConstraintFrame;
 301
 302         int m_flags;
 303
 304         //!@}
 305
 306         b3Generic6DofConstraint& operator=(b3Generic6DofConstraint& other)
 307         {
 308                 b3Assert(0);
 309                 (void)other;
 310                 return *this;
 311         }
 312
 313         int setAngularLimits(b3ConstraintInfo2 * info, int row_offset, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB);
 314
 315         int setLinearLimits(b3ConstraintInfo2 * info, int row, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB);
 316
 317         // tests linear limits
 318         void calculateLinearInfo();
 319
 320         //! calcs the euler angles between the two bodies.
 321         void calculateAngleInfo();
 322
 323 public:
 324         B3_DECLARE_ALIGNED_ALLOCATOR();
 325
 326         b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies);
 327
 328         //! Calcs global transform of the offsets
 329         /*!
 330         Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
 331         \sa b3Generic6DofConstraint.getCalculatedTransformA , b3Generic6DofConstraint.getCalculatedTransformB, b3Generic6DofConstraint.calculateAngleInfo
 332         */
 333         void calculateTransforms(const b3Transform& transA, const b3Transform& transB, const b3RigidBodyData* bodies);
 334
 335         void calculateTransforms(const b3RigidBodyData* bodies);
 336
 337         //! Gets the global transform of the offset for body A
 338         /*!
 339     \sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo.
 340     */
 341         const b3Transform& getCalculatedTransformA() const
 342         {
 343                 return m_calculatedTransformA;
 344         }
 345
 346         //! Gets the global transform of the offset for body B
 347         /*!
 348     \sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo.
 349     */
 350         const b3Transform& getCalculatedTransformB() const
 351         {
 352                 return m_calculatedTransformB;
 353         }
 354
 355         const b3Transform& getFrameOffsetA() const
 356         {
 357                 return m_frameInA;
 358         }
 359
 360         const b3Transform& getFrameOffsetB() const
 361         {
 362                 return m_frameInB;
 363         }
 364
 365         b3Transform& getFrameOffsetA()
 366         {
 367                 return m_frameInA;
 368         }
 369
 370         b3Transform& getFrameOffsetB()
 371         {
 372                 return m_frameInB;
 373         }
 374
 375         virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
 376
 377         void getInfo1NonVirtual(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
 378
 379         virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies);
 380
 381         void getInfo2NonVirtual(b3ConstraintInfo2 * info, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB, const b3RigidBodyData* bodies);
 382
 383         void updateRHS(b3Scalar timeStep);
 384
 385         //! Get the rotation axis in global coordinates
 386         b3Vector3 getAxis(int axis_index) const;
 387
 388         //! Get the relative Euler angle
 389         /*!
 390         \pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
 391         */
 392         b3Scalar getAngle(int axis_index) const;
 393
 394         //! Get the relative position of the constraint pivot
 395         /*!
 396         \pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
 397         */
 398         b3Scalar getRelativePivotPosition(int axis_index) const;
 399
 400         void setFrames(const b3Transform& frameA, const b3Transform& frameB, const b3RigidBodyData* bodies);
 401
 402         //! Test angular limit.
 403         /*!
 404         Calculates angular correction and returns true if limit needs to be corrected.
 405         \pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
 406         */
 407         bool testAngularLimitMotor(int axis_index);
 408
 409         void setLinearLowerLimit(const b3Vector3& linearLower)
 410         {
 411                 m_linearLimits.m_lowerLimit = linearLower;
 412         }
 413
 414         void getLinearLowerLimit(b3Vector3 & linearLower)
 415         {
 416                 linearLower = m_linearLimits.m_lowerLimit;
 417         }
 418
 419         void setLinearUpperLimit(const b3Vector3& linearUpper)
 420         {
 421                 m_linearLimits.m_upperLimit = linearUpper;
 422         }
 423
 424         void getLinearUpperLimit(b3Vector3 & linearUpper)
 425         {
 426                 linearUpper = m_linearLimits.m_upperLimit;
 427         }
 428
 429         void setAngularLowerLimit(const b3Vector3& angularLower)
 430         {
 431                 for (int i = 0; i < 3; i++)
 432                         m_angularLimits[i].m_loLimit = b3NormalizeAngle(angularLower[i]);
 433         }
 434
 435         void getAngularLowerLimit(b3Vector3 & angularLower)
 436         {
 437                 for (int i = 0; i < 3; i++)
 438                         angularLower[i] = m_angularLimits[i].m_loLimit;
 439         }
 440
 441         void setAngularUpperLimit(const b3Vector3& angularUpper)
 442         {
 443                 for (int i = 0; i < 3; i++)
 444                         m_angularLimits[i].m_hiLimit = b3NormalizeAngle(angularUpper[i]);
 445         }
 446
 447         void getAngularUpperLimit(b3Vector3 & angularUpper)
 448         {
 449                 for (int i = 0; i < 3; i++)
 450                         angularUpper[i] = m_angularLimits[i].m_hiLimit;
 451         }
 452
 453         //! Retrieves the angular limit informacion
 454         b3RotationalLimitMotor* getRotationalLimitMotor(int index)
 455         {
 456                 return &m_angularLimits[index];
 457         }
 458
 459         //! Retrieves the  limit informacion
 460         b3TranslationalLimitMotor* getTranslationalLimitMotor()
 461         {
 462                 return &m_linearLimits;
 463         }
 464
 465         //first 3 are linear, next 3 are angular
 466         void setLimit(int axis, b3Scalar lo, b3Scalar hi)
 467         {
 468                 if (axis < 3)
 469                 {
 470                         m_linearLimits.m_lowerLimit[axis] = lo;
 471                         m_linearLimits.m_upperLimit[axis] = hi;
 472                 }
 473                 else
 474                 {
 475                         lo = b3NormalizeAngle(lo);
 476                         hi = b3NormalizeAngle(hi);
 477                         m_angularLimits[axis - 3].m_loLimit = lo;
 478                         m_angularLimits[axis - 3].m_hiLimit = hi;
 479                 }
 480         }
 481
 482         //! Test limit
 483         /*!
 484     - free means upper < lower,
 485     - locked means upper == lower
 486     - limited means upper > lower
 487     - limitIndex: first 3 are linear, next 3 are angular
 488     */
 489         bool isLimited(int limitIndex)
 490         {
 491                 if (limitIndex < 3)
 492                 {
 493                         return m_linearLimits.isLimited(limitIndex);
 494                 }
 495                 return m_angularLimits[limitIndex - 3].isLimited();
 496         }
 497
 498         virtual void calcAnchorPos(const b3RigidBodyData* bodies);  // overridable
 499
 500         int get_limit_motor_info2(b3RotationalLimitMotor * limot,
 501                                                           const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB,
 502                                                           b3ConstraintInfo2* info, int row, b3Vector3& ax1, int rotational, int rotAllowed = false);
 503
 504         // access for UseFrameOffset
 505         bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
 506         void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
 507
 508         ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 509         ///If no axis is provided, it uses the default axis for this constraint.
 510         virtual void setParam(int num, b3Scalar value, int axis = -1);
 511         ///return the local value of parameter
 512         virtual b3Scalar getParam(int num, int axis = -1) const;
 513
 514         void setAxis(const b3Vector3& axis1, const b3Vector3& axis2, const b3RigidBodyData* bodies);
 515 };
 516
 517 #endif  //B3_GENERIC_6DOF_CONSTRAINT_H