src/BulletDynamics/ConstraintSolver/btTypedConstraint.h

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