2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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:
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.
16 /// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev
17 /// Added support for generic constraint solver through getInfo1/getInfo2 methods
21 btGeneric6DofConstraint Refactored by Francisco Le?n
22 email: projectileman@yahoo.com
27 #ifndef BT_GENERIC_6DOF_CONSTRAINT_H
28 #define BT_GENERIC_6DOF_CONSTRAINT_H
30 #include "LinearMath/btVector3.h"
31 #include "btJacobianEntry.h"
32 #include "btTypedConstraint.h"
39 //! Rotation Limit structure for generic joints
40 class btRotationalLimitMotor
45 btScalar m_loLimit;//!< joint limit
46 btScalar m_hiLimit;//!< joint limit
47 btScalar m_targetVelocity;//!< target motor velocity
48 btScalar m_maxMotorForce;//!< max force on motor
49 btScalar m_maxLimitForce;//!< max force on limit
50 btScalar m_damping;//!< Damping.
51 btScalar m_limitSoftness;//! Relaxation factor
52 btScalar m_normalCFM;//!< Constraint force mixing factor
53 btScalar m_stopERP;//!< Error tolerance factor when joint is at limit
54 btScalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit
55 btScalar m_bounce;//!< restitution factor
62 btScalar m_currentLimitError;//! How much is violated this limit
63 btScalar m_currentPosition; //! current value of angle
64 int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
65 btScalar m_accumulatedImpulse;
68 btRotationalLimitMotor()
70 m_accumulatedImpulse = 0.f;
72 m_maxMotorForce = 0.1f;
73 m_maxLimitForce = 300.0f;
81 m_limitSoftness = 0.5f;
83 m_currentLimitError = 0;
84 m_enableMotor = false;
87 btRotationalLimitMotor(const btRotationalLimitMotor & limot)
89 m_targetVelocity = limot.m_targetVelocity;
90 m_maxMotorForce = limot.m_maxMotorForce;
91 m_limitSoftness = limot.m_limitSoftness;
92 m_loLimit = limot.m_loLimit;
93 m_hiLimit = limot.m_hiLimit;
94 m_normalCFM = limot.m_normalCFM;
95 m_stopERP = limot.m_stopERP;
96 m_stopCFM = limot.m_stopCFM;
97 m_bounce = limot.m_bounce;
98 m_currentLimit = limot.m_currentLimit;
99 m_currentLimitError = limot.m_currentLimitError;
100 m_enableMotor = limot.m_enableMotor;
108 if(m_loLimit > m_hiLimit) return false;
112 //! Need apply correction
113 bool needApplyTorques()
115 if(m_currentLimit == 0 && m_enableMotor == false) return false;
121 calculates m_currentLimit and m_currentLimitError.
123 int testLimitValue(btScalar test_value);
125 //! apply the correction impulses for two bodies
126 btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
132 class btTranslationalLimitMotor
135 btVector3 m_lowerLimit;//!< the constraint lower limits
136 btVector3 m_upperLimit;//!< the constraint upper limits
137 btVector3 m_accumulatedImpulse;
138 //! Linear_Limit_parameters
140 btScalar m_limitSoftness;//!< Softness for linear limit
141 btScalar m_damping;//!< Damping for linear limit
142 btScalar m_restitution;//! Bounce parameter for linear limit
143 btVector3 m_normalCFM;//!< Constraint force mixing factor
144 btVector3 m_stopERP;//!< Error tolerance factor when joint is at limit
145 btVector3 m_stopCFM;//!< Constraint force mixing factor when joint is at limit
147 bool m_enableMotor[3];
148 btVector3 m_targetVelocity;//!< target motor velocity
149 btVector3 m_maxMotorForce;//!< max force on motor
150 btVector3 m_currentLimitError;//! How much is violated this limit
151 btVector3 m_currentLinearDiff;//! Current relative offset of constraint frames
152 int m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
154 btTranslationalLimitMotor()
156 m_lowerLimit.setValue(0.f,0.f,0.f);
157 m_upperLimit.setValue(0.f,0.f,0.f);
158 m_accumulatedImpulse.setValue(0.f,0.f,0.f);
159 m_normalCFM.setValue(0.f, 0.f, 0.f);
160 m_stopERP.setValue(0.2f, 0.2f, 0.2f);
161 m_stopCFM.setValue(0.f, 0.f, 0.f);
163 m_limitSoftness = 0.7f;
164 m_damping = btScalar(1.0f);
165 m_restitution = btScalar(0.5f);
166 for(int i=0; i < 3; i++)
168 m_enableMotor[i] = false;
169 m_targetVelocity[i] = btScalar(0.f);
170 m_maxMotorForce[i] = btScalar(0.f);
174 btTranslationalLimitMotor(const btTranslationalLimitMotor & other )
176 m_lowerLimit = other.m_lowerLimit;
177 m_upperLimit = other.m_upperLimit;
178 m_accumulatedImpulse = other.m_accumulatedImpulse;
180 m_limitSoftness = other.m_limitSoftness ;
181 m_damping = other.m_damping;
182 m_restitution = other.m_restitution;
183 m_normalCFM = other.m_normalCFM;
184 m_stopERP = other.m_stopERP;
185 m_stopCFM = other.m_stopCFM;
187 for(int i=0; i < 3; i++)
189 m_enableMotor[i] = other.m_enableMotor[i];
190 m_targetVelocity[i] = other.m_targetVelocity[i];
191 m_maxMotorForce[i] = other.m_maxMotorForce[i];
197 - free means upper < lower,
198 - locked means upper == lower
199 - limited means upper > lower
200 - limitIndex: first 3 are linear, next 3 are angular
202 inline bool isLimited(int limitIndex)
204 return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
206 inline bool needApplyForce(int limitIndex)
208 if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
211 int testLimitValue(int limitIndex, btScalar test_value);
214 btScalar solveLinearAxis(
216 btScalar jacDiagABInv,
217 btRigidBody& body1,const btVector3 &pointInA,
218 btRigidBody& body2,const btVector3 &pointInB,
220 const btVector3 & axis_normal_on_a,
221 const btVector3 & anchorPos);
228 BT_6DOF_FLAGS_CFM_NORM = 1,
229 BT_6DOF_FLAGS_CFM_STOP = 2,
230 BT_6DOF_FLAGS_ERP_STOP = 4
232 #define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
235 /// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
237 btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'.
238 currently this limit supports rotational motors<br>
240 <li> For Linear limits, use btGeneric6DofConstraint.setLinearUpperLimit, btGeneric6DofConstraint.setLinearLowerLimit. You can set the parameters with the btTranslationalLimitMotor structure accsesible through the btGeneric6DofConstraint.getTranslationalLimitMotor method.
241 At this moment translational motors are not supported. May be in the future. </li>
243 <li> For Angular limits, use the btRotationalLimitMotor structure for configuring the limit.
244 This is accessible through btGeneric6DofConstraint.getLimitMotor method,
245 This brings support for limit parameters and motors. </li>
247 <li> Angulars limits have these possible ranges:
251 <td><b>MIN ANGLE</b></td>
252 <td><b>MAX ANGLE</b></td>
271 ATTRIBUTE_ALIGNED16(class) btGeneric6DofConstraint : public btTypedConstraint
277 btTransform m_frameInA;//!< the constraint space w.r.t body A
278 btTransform m_frameInB;//!< the constraint space w.r.t body B
283 btJacobianEntry m_jacLinear[3];//!< 3 orthogonal linear constraints
284 btJacobianEntry m_jacAng[3];//!< 3 orthogonal angular constraints
287 //! Linear_Limit_parameters
289 btTranslationalLimitMotor m_linearLimits;
295 btRotationalLimitMotor m_angularLimits[3];
300 //! temporal variables
303 btTransform m_calculatedTransformA;
304 btTransform m_calculatedTransformB;
305 btVector3 m_calculatedAxisAngleDiff;
306 btVector3 m_calculatedAxis[3];
307 btVector3 m_calculatedLinearDiff;
310 bool m_hasStaticBody;
312 btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
314 bool m_useLinearReferenceFrameA;
315 bool m_useOffsetForConstraintFrame;
321 btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other)
329 int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
331 int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
333 void buildLinearJacobian(
334 btJacobianEntry & jacLinear,const btVector3 & normalWorld,
335 const btVector3 & pivotAInW,const btVector3 & pivotBInW);
337 void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW);
339 // tests linear limits
340 void calculateLinearInfo();
342 //! calcs the euler angles between the two bodies.
343 void calculateAngleInfo();
349 BT_DECLARE_ALIGNED_ALLOCATOR();
351 ///for backwards compatibility during the transition to 'getInfo/getInfo2'
352 bool m_useSolveConstraintObsolete;
354 btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
355 btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
357 //! Calcs global transform of the offsets
359 Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
360 \sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo
362 void calculateTransforms(const btTransform& transA,const btTransform& transB);
364 void calculateTransforms();
366 //! Gets the global transform of the offset for body A
368 \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
370 const btTransform & getCalculatedTransformA() const
372 return m_calculatedTransformA;
375 //! Gets the global transform of the offset for body B
377 \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
379 const btTransform & getCalculatedTransformB() const
381 return m_calculatedTransformB;
384 const btTransform & getFrameOffsetA() const
389 const btTransform & getFrameOffsetB() const
395 btTransform & getFrameOffsetA()
400 btTransform & getFrameOffsetB()
406 //! performs Jacobian calculation, and also calculates angle differences and axis
407 virtual void buildJacobian();
409 virtual void getInfo1 (btConstraintInfo1* info);
411 void getInfo1NonVirtual (btConstraintInfo1* info);
413 virtual void getInfo2 (btConstraintInfo2* info);
415 void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
418 void updateRHS(btScalar timeStep);
420 //! Get the rotation axis in global coordinates
422 \pre btGeneric6DofConstraint.buildJacobian must be called previously.
424 btVector3 getAxis(int axis_index) const;
426 //! Get the relative Euler angle
428 \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
430 btScalar getAngle(int axis_index) const;
432 //! Get the relative position of the constraint pivot
434 \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
436 btScalar getRelativePivotPosition(int axis_index) const;
438 void setFrames(const btTransform & frameA, const btTransform & frameB);
440 //! Test angular limit.
442 Calculates angular correction and returns true if limit needs to be corrected.
443 \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
445 bool testAngularLimitMotor(int axis_index);
447 void setLinearLowerLimit(const btVector3& linearLower)
449 m_linearLimits.m_lowerLimit = linearLower;
452 void getLinearLowerLimit(btVector3& linearLower)
454 linearLower = m_linearLimits.m_lowerLimit;
457 void setLinearUpperLimit(const btVector3& linearUpper)
459 m_linearLimits.m_upperLimit = linearUpper;
462 void getLinearUpperLimit(btVector3& linearUpper)
464 linearUpper = m_linearLimits.m_upperLimit;
467 void setAngularLowerLimit(const btVector3& angularLower)
469 for(int i = 0; i < 3; i++)
470 m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
473 void getAngularLowerLimit(btVector3& angularLower)
475 for(int i = 0; i < 3; i++)
476 angularLower[i] = m_angularLimits[i].m_loLimit;
479 void setAngularUpperLimit(const btVector3& angularUpper)
481 for(int i = 0; i < 3; i++)
482 m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
485 void getAngularUpperLimit(btVector3& angularUpper)
487 for(int i = 0; i < 3; i++)
488 angularUpper[i] = m_angularLimits[i].m_hiLimit;
491 //! Retrieves the angular limit informacion
492 btRotationalLimitMotor * getRotationalLimitMotor(int index)
494 return &m_angularLimits[index];
497 //! Retrieves the limit informacion
498 btTranslationalLimitMotor * getTranslationalLimitMotor()
500 return &m_linearLimits;
503 //first 3 are linear, next 3 are angular
504 void setLimit(int axis, btScalar lo, btScalar hi)
508 m_linearLimits.m_lowerLimit[axis] = lo;
509 m_linearLimits.m_upperLimit[axis] = hi;
513 lo = btNormalizeAngle(lo);
514 hi = btNormalizeAngle(hi);
515 m_angularLimits[axis-3].m_loLimit = lo;
516 m_angularLimits[axis-3].m_hiLimit = hi;
522 - free means upper < lower,
523 - locked means upper == lower
524 - limited means upper > lower
525 - limitIndex: first 3 are linear, next 3 are angular
527 bool isLimited(int limitIndex)
531 return m_linearLimits.isLimited(limitIndex);
534 return m_angularLimits[limitIndex-3].isLimited();
537 virtual void calcAnchorPos(void); // overridable
539 int get_limit_motor_info2( btRotationalLimitMotor * limot,
540 const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
541 btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
543 // access for UseFrameOffset
544 bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
545 void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
547 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
548 ///If no axis is provided, it uses the default axis for this constraint.
549 virtual void setParam(int num, btScalar value, int axis = -1);
550 ///return the local value of parameter
551 virtual btScalar getParam(int num, int axis = -1) const;
553 void setAxis( const btVector3& axis1, const btVector3& axis2);
556 virtual int calculateSerializeBufferSize() const;
558 ///fills the dataBuffer and returns the struct name (and 0 on failure)
559 virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
564 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
565 struct btGeneric6DofConstraintData
567 btTypedConstraintData m_typeConstraintData;
568 btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
569 btTransformFloatData m_rbBFrame;
571 btVector3FloatData m_linearUpperLimit;
572 btVector3FloatData m_linearLowerLimit;
574 btVector3FloatData m_angularUpperLimit;
575 btVector3FloatData m_angularLowerLimit;
577 int m_useLinearReferenceFrameA;
578 int m_useOffsetForConstraintFrame;
581 SIMD_FORCE_INLINE int btGeneric6DofConstraint::calculateSerializeBufferSize() const
583 return sizeof(btGeneric6DofConstraintData);
586 ///fills the dataBuffer and returns the struct name (and 0 on failure)
587 SIMD_FORCE_INLINE const char* btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
590 btGeneric6DofConstraintData* dof = (btGeneric6DofConstraintData*)dataBuffer;
591 btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
593 m_frameInA.serializeFloat(dof->m_rbAFrame);
594 m_frameInB.serializeFloat(dof->m_rbBFrame);
600 dof->m_angularLowerLimit.m_floats[i] = float(m_angularLimits[i].m_loLimit);
601 dof->m_angularUpperLimit.m_floats[i] = float(m_angularLimits[i].m_hiLimit);
602 dof->m_linearLowerLimit.m_floats[i] = float(m_linearLimits.m_lowerLimit[i]);
603 dof->m_linearUpperLimit.m_floats[i] = float(m_linearLimits.m_upperLimit[i]);
606 dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0;
607 dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;
609 return "btGeneric6DofConstraintData";
616 #endif //BT_GENERIC_6DOF_CONSTRAINT_H