btTypedConstraint.h

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2010 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 #ifndef BT_TYPED_CONSTRAINT_H
00017 #define BT_TYPED_CONSTRAINT_H
00018 
00019 
00020 #include "LinearMath/btScalar.h"
00021 #include "btSolverConstraint.h"
00022 #include "BulletDynamics/Dynamics/btRigidBody.h"
00023 
00024 class btSerializer;
00025 
00026 //Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
00027 enum btTypedConstraintType
00028 {
00029         POINT2POINT_CONSTRAINT_TYPE=3,
00030         HINGE_CONSTRAINT_TYPE,
00031         CONETWIST_CONSTRAINT_TYPE,
00032         D6_CONSTRAINT_TYPE,
00033         SLIDER_CONSTRAINT_TYPE,
00034         CONTACT_CONSTRAINT_TYPE,
00035         D6_SPRING_CONSTRAINT_TYPE,
00036         GEAR_CONSTRAINT_TYPE,
00037         MAX_CONSTRAINT_TYPE
00038 };
00039 
00040 
00041 enum btConstraintParams
00042 {
00043         BT_CONSTRAINT_ERP=1,
00044         BT_CONSTRAINT_STOP_ERP,
00045         BT_CONSTRAINT_CFM,
00046         BT_CONSTRAINT_STOP_CFM
00047 };
00048 
00049 #if 1
00050         #define btAssertConstrParams(_par) btAssert(_par) 
00051 #else
00052         #define btAssertConstrParams(_par)
00053 #endif
00054 
00055 
00056 ATTRIBUTE_ALIGNED16(struct)     btJointFeedback
00057 {
00058         btVector3       m_appliedForceBodyA;
00059         btVector3       m_appliedTorqueBodyA;
00060         btVector3       m_appliedForceBodyB;
00061         btVector3       m_appliedTorqueBodyB;
00062 };
00063 
00064 
00066 ATTRIBUTE_ALIGNED16(class) btTypedConstraint : public btTypedObject
00067 {
00068         int     m_userConstraintType;
00069 
00070         union
00071         {
00072                 int     m_userConstraintId;
00073                 void* m_userConstraintPtr;
00074         };
00075 
00076         btScalar        m_breakingImpulseThreshold;
00077         bool            m_isEnabled;
00078         bool            m_needsFeedback;
00079         int                     m_overrideNumSolverIterations;
00080 
00081 
00082         btTypedConstraint&      operator=(btTypedConstraint&    other)
00083         {
00084                 btAssert(0);
00085                 (void) other;
00086                 return *this;
00087         }
00088 
00089 protected:
00090         btRigidBody&    m_rbA;
00091         btRigidBody&    m_rbB;
00092         btScalar        m_appliedImpulse;
00093         btScalar        m_dbgDrawSize;
00094         btJointFeedback*        m_jointFeedback;
00095 
00097         btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
00098         
00099 
00100 public:
00101 
00102         BT_DECLARE_ALIGNED_ALLOCATOR();
00103 
00104         virtual ~btTypedConstraint() {};
00105         btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
00106         btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
00107 
00108         struct btConstraintInfo1 {
00109                 int m_numConstraintRows,nub;
00110         };
00111 
00112         static btRigidBody& getFixedBody();
00113 
00114         struct btConstraintInfo2 {
00115                 // integrator parameters: frames per second (1/stepsize), default error
00116                 // reduction parameter (0..1).
00117                 btScalar fps,erp;
00118 
00119                 // for the first and second body, pointers to two (linear and angular)
00120                 // n*3 jacobian sub matrices, stored by rows. these matrices will have
00121                 // been initialized to 0 on entry. if the second body is zero then the
00122                 // J2xx pointers may be 0.
00123                 btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis;
00124 
00125                 // elements to jump from one row to the next in J's
00126                 int rowskip;
00127 
00128                 // right hand sides of the equation J*v = c + cfm * lambda. cfm is the
00129                 // "constraint force mixing" vector. c is set to zero on entry, cfm is
00130                 // set to a constant value (typically very small or zero) value on entry.
00131                 btScalar *m_constraintError,*cfm;
00132 
00133                 // lo and hi limits for variables (set to -/+ infinity on entry).
00134                 btScalar *m_lowerLimit,*m_upperLimit;
00135 
00136                 // findex vector for variables. see the LCP solver interface for a
00137                 // description of what this does. this is set to -1 on entry.
00138                 // note that the returned indexes are relative to the first index of
00139                 // the constraint.
00140                 int *findex;
00141                 // number of solver iterations
00142                 int m_numIterations;
00143 
00144                 //damping of the velocity
00145                 btScalar        m_damping;
00146         };
00147 
00148         int     getOverrideNumSolverIterations() const
00149         {
00150                 return m_overrideNumSolverIterations;
00151         }
00152 
00155         void setOverrideNumSolverIterations(int overideNumIterations)
00156         {
00157                 m_overrideNumSolverIterations = overideNumIterations;
00158         }
00159 
00161         virtual void    buildJacobian() {};
00162 
00164         virtual void    setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
00165         {
00166         (void)ca;
00167         (void)solverBodyA;
00168         (void)solverBodyB;
00169         (void)timeStep;
00170         }
00171         
00173         virtual void getInfo1 (btConstraintInfo1* info)=0;
00174 
00176         virtual void getInfo2 (btConstraintInfo2* info)=0;
00177 
00179         void    internalSetAppliedImpulse(btScalar appliedImpulse)
00180         {
00181                 m_appliedImpulse = appliedImpulse;
00182         }
00184         btScalar        internalGetAppliedImpulse()
00185         {
00186                 return m_appliedImpulse;
00187         }
00188 
00189 
00190         btScalar        getBreakingImpulseThreshold() const
00191         {
00192                 return  m_breakingImpulseThreshold;
00193         }
00194 
00195         void    setBreakingImpulseThreshold(btScalar threshold)
00196         {
00197                 m_breakingImpulseThreshold = threshold;
00198         }
00199 
00200         bool    isEnabled() const
00201         {
00202                 return m_isEnabled;
00203         }
00204 
00205         void    setEnabled(bool enabled)
00206         {
00207                 m_isEnabled=enabled;
00208         }
00209 
00210 
00212         virtual void    solveConstraintObsolete(btSolverBody& /*bodyA*/,btSolverBody& /*bodyB*/,btScalar        /*timeStep*/) {};
00213 
00214         
00215         const btRigidBody& getRigidBodyA() const
00216         {
00217                 return m_rbA;
00218         }
00219         const btRigidBody& getRigidBodyB() const
00220         {
00221                 return m_rbB;
00222         }
00223 
00224         btRigidBody& getRigidBodyA() 
00225         {
00226                 return m_rbA;
00227         }
00228         btRigidBody& getRigidBodyB()
00229         {
00230                 return m_rbB;
00231         }
00232 
00233         int getUserConstraintType() const
00234         {
00235                 return m_userConstraintType ;
00236         }
00237 
00238         void    setUserConstraintType(int userConstraintType)
00239         {
00240                 m_userConstraintType = userConstraintType;
00241         };
00242 
00243         void    setUserConstraintId(int uid)
00244         {
00245                 m_userConstraintId = uid;
00246         }
00247 
00248         int getUserConstraintId() const
00249         {
00250                 return m_userConstraintId;
00251         }
00252 
00253         void    setUserConstraintPtr(void* ptr)
00254         {
00255                 m_userConstraintPtr = ptr;
00256         }
00257 
00258         void*   getUserConstraintPtr()
00259         {
00260                 return m_userConstraintPtr;
00261         }
00262 
00263         void    setJointFeedback(btJointFeedback* jointFeedback)
00264         {
00265                 m_jointFeedback = jointFeedback;
00266         }
00267 
00268         const btJointFeedback* getJointFeedback() const
00269         {
00270                 return m_jointFeedback;
00271         }
00272 
00273         btJointFeedback* getJointFeedback()
00274         {
00275                 return m_jointFeedback;
00276         }
00277 
00278 
00279         int getUid() const
00280         {
00281                 return m_userConstraintId;   
00282         } 
00283 
00284         bool    needsFeedback() const
00285         {
00286                 return m_needsFeedback;
00287         }
00288 
00291         void    enableFeedback(bool needsFeedback)
00292         {
00293                 m_needsFeedback = needsFeedback;
00294         }
00295 
00298         btScalar        getAppliedImpulse() const
00299         {
00300                 btAssert(m_needsFeedback);
00301                 return m_appliedImpulse;
00302         }
00303 
00304         btTypedConstraintType getConstraintType () const
00305         {
00306                 return btTypedConstraintType(m_objectType);
00307         }
00308         
00309         void setDbgDrawSize(btScalar dbgDrawSize)
00310         {
00311                 m_dbgDrawSize = dbgDrawSize;
00312         }
00313         btScalar getDbgDrawSize()
00314         {
00315                 return m_dbgDrawSize;
00316         }
00317 
00320         virtual void    setParam(int num, btScalar value, int axis = -1) = 0;
00321 
00323         virtual btScalar getParam(int num, int axis = -1) const = 0;
00324         
00325         virtual int     calculateSerializeBufferSize() const;
00326 
00328         virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
00329 
00330 };
00331 
00332 // returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits 
00333 // all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI])
00334 SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
00335 {
00336         if(angleLowerLimitInRadians >= angleUpperLimitInRadians)
00337         {
00338                 return angleInRadians;
00339         }
00340         else if(angleInRadians < angleLowerLimitInRadians)
00341         {
00342                 btScalar diffLo = btFabs(btNormalizeAngle(angleLowerLimitInRadians - angleInRadians));
00343                 btScalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians));
00344                 return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI);
00345         }
00346         else if(angleInRadians > angleUpperLimitInRadians)
00347         {
00348                 btScalar diffHi = btFabs(btNormalizeAngle(angleInRadians - angleUpperLimitInRadians));
00349                 btScalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians));
00350                 return (diffLo < diffHi) ? (angleInRadians - SIMD_2_PI) : angleInRadians;
00351         }
00352         else
00353         {
00354                 return angleInRadians;
00355         }
00356 }
00357 
00359 struct  btTypedConstraintData
00360 {
00361         btRigidBodyData         *m_rbA;
00362         btRigidBodyData         *m_rbB;
00363         char    *m_name;
00364 
00365         int     m_objectType;
00366         int     m_userConstraintType;
00367         int     m_userConstraintId;
00368         int     m_needsFeedback;
00369 
00370         float   m_appliedImpulse;
00371         float   m_dbgDrawSize;
00372 
00373         int     m_disableCollisionsBetweenLinkedBodies;
00374         int     m_overrideNumSolverIterations;
00375 
00376         float   m_breakingImpulseThreshold;
00377         int             m_isEnabled;
00378         
00379 };
00380 
00381 SIMD_FORCE_INLINE       int     btTypedConstraint::calculateSerializeBufferSize() const
00382 {
00383         return sizeof(btTypedConstraintData);
00384 }
00385 
00386 
00387 
00388 class btAngularLimit
00389 {
00390 private:
00391         btScalar 
00392                 m_center,
00393                 m_halfRange,
00394                 m_softness,
00395                 m_biasFactor,
00396                 m_relaxationFactor,
00397                 m_correction,
00398                 m_sign;
00399 
00400         bool
00401                 m_solveLimit;
00402 
00403 public:
00405         btAngularLimit()
00406                 :m_center(0.0f),
00407                 m_halfRange(-1.0f),
00408                 m_softness(0.9f),
00409                 m_biasFactor(0.3f),
00410                 m_relaxationFactor(1.0f),
00411                 m_correction(0.0f),
00412                 m_sign(0.0f),
00413                 m_solveLimit(false)
00414         {}
00415 
00419         void set(btScalar low, btScalar high, btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f);
00420 
00423         void test(const btScalar angle);
00424 
00426         inline btScalar getSoftness() const
00427         {
00428                 return m_softness;
00429         }
00430 
00432         inline btScalar getBiasFactor() const
00433         {
00434                 return m_biasFactor;
00435         }
00436 
00438         inline btScalar getRelaxationFactor() const
00439         {
00440                 return m_relaxationFactor;
00441         }
00442 
00444         inline btScalar getCorrection() const
00445         {
00446                 return m_correction;
00447         }
00448 
00450         inline btScalar getSign() const
00451         {
00452                 return m_sign;
00453         }
00454 
00456         inline btScalar getHalfRange() const
00457         {
00458                 return m_halfRange;
00459         }
00460 
00462         inline bool isLimit() const
00463         {
00464                 return m_solveLimit;
00465         }
00466 
00469         void fit(btScalar& angle) const;
00470 
00472         btScalar getError() const;
00473 
00474         btScalar getLow() const;
00475 
00476         btScalar getHigh() const;
00477 
00478 };
00479 
00480 
00481 
00482 #endif //BT_TYPED_CONSTRAINT_H