2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
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
26 #ifndef BT_GENERIC_6DOF_CONSTRAINT_H
27 #define BT_GENERIC_6DOF_CONSTRAINT_H
29 #include "LinearMath/btVector3.h"
30 #include "btJacobianEntry.h"
31 #include "btTypedConstraint.h"
35 #ifdef BT_USE_DOUBLE_PRECISION
36 #define btGeneric6DofConstraintData2 btGeneric6DofConstraintDoubleData2
37 #define btGeneric6DofConstraintDataName "btGeneric6DofConstraintDoubleData2"
39 #define btGeneric6DofConstraintData2 btGeneric6DofConstraintData
40 #define btGeneric6DofConstraintDataName "btGeneric6DofConstraintData"
41 #endif //BT_USE_DOUBLE_PRECISION
43 //! Rotation Limit structure for generic joints
44 class btRotationalLimitMotor
49 btScalar m_loLimit; //!< joint limit
50 btScalar m_hiLimit; //!< joint limit
51 btScalar m_targetVelocity; //!< target motor velocity
52 btScalar m_maxMotorForce; //!< max force on motor
53 btScalar m_maxLimitForce; //!< max force on limit
54 btScalar m_damping; //!< Damping.
55 btScalar m_limitSoftness; //! Relaxation factor
56 btScalar m_normalCFM; //!< Constraint force mixing factor
57 btScalar m_stopERP; //!< Error tolerance factor when joint is at limit
58 btScalar m_stopCFM; //!< Constraint force mixing factor when joint is at limit
59 btScalar m_bounce; //!< restitution factor
66 btScalar m_currentLimitError; //! How much is violated this limit
67 btScalar m_currentPosition; //! current value of angle
68 int m_currentLimit; //!< 0=free, 1=at lo limit, 2=at hi limit
69 btScalar m_accumulatedImpulse;
72 btRotationalLimitMotor()
74 m_accumulatedImpulse = 0.f;
76 m_maxMotorForce = 6.0f;
77 m_maxLimitForce = 300.0f;
85 m_limitSoftness = 0.5f;
87 m_currentLimitError = 0;
88 m_enableMotor = false;
91 btRotationalLimitMotor(const btRotationalLimitMotor& limot)
93 m_targetVelocity = limot.m_targetVelocity;
94 m_maxMotorForce = limot.m_maxMotorForce;
95 m_limitSoftness = limot.m_limitSoftness;
96 m_loLimit = limot.m_loLimit;
97 m_hiLimit = limot.m_hiLimit;
98 m_normalCFM = limot.m_normalCFM;
99 m_stopERP = limot.m_stopERP;
100 m_stopCFM = limot.m_stopCFM;
101 m_bounce = limot.m_bounce;
102 m_currentLimit = limot.m_currentLimit;
103 m_currentLimitError = limot.m_currentLimitError;
104 m_enableMotor = limot.m_enableMotor;
108 bool isLimited() const
110 if (m_loLimit > m_hiLimit) return false;
114 //! Need apply correction
115 bool needApplyTorques() const
117 if (m_currentLimit == 0 && m_enableMotor == false) return false;
123 calculates m_currentLimit and m_currentLimitError.
125 int testLimitValue(btScalar test_value);
127 //! apply the correction impulses for two bodies
128 btScalar solveAngularLimits(btScalar timeStep, btVector3& axis, btScalar jacDiagABInv, btRigidBody* body0, btRigidBody* body1);
131 class btTranslationalLimitMotor
134 btVector3 m_lowerLimit; //!< the constraint lower limits
135 btVector3 m_upperLimit; //!< the constraint upper limits
136 btVector3 m_accumulatedImpulse;
137 //! Linear_Limit_parameters
139 btScalar m_limitSoftness; //!< Softness for linear limit
140 btScalar m_damping; //!< Damping for linear limit
141 btScalar m_restitution; //! Bounce parameter for linear limit
142 btVector3 m_normalCFM; //!< Constraint force mixing factor
143 btVector3 m_stopERP; //!< Error tolerance factor when joint is at limit
144 btVector3 m_stopCFM; //!< Constraint force mixing factor when joint is at limit
146 bool m_enableMotor[3];
147 btVector3 m_targetVelocity; //!< target motor velocity
148 btVector3 m_maxMotorForce; //!< max force on motor
149 btVector3 m_currentLimitError; //! How much is violated this limit
150 btVector3 m_currentLinearDiff; //! Current relative offset of constraint frames
151 int m_currentLimit[3]; //!< 0=free, 1=at lower limit, 2=at upper limit
153 btTranslationalLimitMotor()
155 m_lowerLimit.setValue(0.f, 0.f, 0.f);
156 m_upperLimit.setValue(0.f, 0.f, 0.f);
157 m_accumulatedImpulse.setValue(0.f, 0.f, 0.f);
158 m_normalCFM.setValue(0.f, 0.f, 0.f);
159 m_stopERP.setValue(0.2f, 0.2f, 0.2f);
160 m_stopCFM.setValue(0.f, 0.f, 0.f);
162 m_limitSoftness = 0.7f;
163 m_damping = btScalar(1.0f);
164 m_restitution = btScalar(0.5f);
165 for (int i = 0; i < 3; i++)
167 m_enableMotor[i] = false;
168 m_targetVelocity[i] = btScalar(0.f);
169 m_maxMotorForce[i] = btScalar(0.f);
173 btTranslationalLimitMotor(const btTranslationalLimitMotor& other)
175 m_lowerLimit = other.m_lowerLimit;
176 m_upperLimit = other.m_upperLimit;
177 m_accumulatedImpulse = other.m_accumulatedImpulse;
179 m_limitSoftness = other.m_limitSoftness;
180 m_damping = other.m_damping;
181 m_restitution = other.m_restitution;
182 m_normalCFM = other.m_normalCFM;
183 m_stopERP = other.m_stopERP;
184 m_stopCFM = other.m_stopCFM;
186 for (int i = 0; i < 3; i++)
188 m_enableMotor[i] = other.m_enableMotor[i];
189 m_targetVelocity[i] = other.m_targetVelocity[i];
190 m_maxMotorForce[i] = other.m_maxMotorForce[i];
196 - free means upper < lower,
197 - locked means upper == lower
198 - limited means upper > lower
199 - limitIndex: first 3 are linear, next 3 are angular
201 inline bool isLimited(int limitIndex) const
203 return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
205 inline bool needApplyForce(int limitIndex) const
207 if (m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
210 int testLimitValue(int limitIndex, btScalar test_value);
212 btScalar solveLinearAxis(
214 btScalar jacDiagABInv,
215 btRigidBody& body1, const btVector3& pointInA,
216 btRigidBody& body2, const btVector3& pointInB,
218 const btVector3& axis_normal_on_a,
219 const btVector3& anchorPos);
224 BT_6DOF_FLAGS_CFM_NORM = 1,
225 BT_6DOF_FLAGS_CFM_STOP = 2,
226 BT_6DOF_FLAGS_ERP_STOP = 4
228 #define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
230 /// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
232 btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'.
233 currently this limit supports rotational motors<br>
235 <li> For Linear limits, use btGeneric6DofConstraint.setLinearUpperLimit, btGeneric6DofConstraint.setLinearLowerLimit. You can set the parameters with the btTranslationalLimitMotor structure accsesible through the btGeneric6DofConstraint.getTranslationalLimitMotor method.
236 At this moment translational motors are not supported. May be in the future. </li>
238 <li> For Angular limits, use the btRotationalLimitMotor structure for configuring the limit.
239 This is accessible through btGeneric6DofConstraint.getLimitMotor method,
240 This brings support for limit parameters and motors. </li>
242 <li> Angulars limits have these possible ranges:
246 <td><b>MIN ANGLE</b></td>
247 <td><b>MAX ANGLE</b></td>
266 ATTRIBUTE_ALIGNED16(class)
267 btGeneric6DofConstraint : public btTypedConstraint
272 btTransform m_frameInA; //!< the constraint space w.r.t body A
273 btTransform m_frameInB; //!< the constraint space w.r.t body B
278 btJacobianEntry m_jacLinear[3]; //!< 3 orthogonal linear constraints
279 btJacobianEntry m_jacAng[3]; //!< 3 orthogonal angular constraints
282 //! Linear_Limit_parameters
284 btTranslationalLimitMotor m_linearLimits;
289 btRotationalLimitMotor m_angularLimits[3];
293 //! temporal variables
296 btTransform m_calculatedTransformA;
297 btTransform m_calculatedTransformB;
298 btVector3 m_calculatedAxisAngleDiff;
299 btVector3 m_calculatedAxis[3];
300 btVector3 m_calculatedLinearDiff;
303 bool m_hasStaticBody;
305 btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
307 bool m_useLinearReferenceFrameA;
308 bool m_useOffsetForConstraintFrame;
314 btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other)
321 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);
323 int setLinearLimits(btConstraintInfo2 * info, int row, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB);
325 void buildLinearJacobian(
326 btJacobianEntry & jacLinear, const btVector3& normalWorld,
327 const btVector3& pivotAInW, const btVector3& pivotBInW);
329 void buildAngularJacobian(btJacobianEntry & jacAngular, const btVector3& jointAxisW);
331 // tests linear limits
332 void calculateLinearInfo();
334 //! calcs the euler angles between the two bodies.
335 void calculateAngleInfo();
338 BT_DECLARE_ALIGNED_ALLOCATOR();
340 ///for backwards compatibility during the transition to 'getInfo/getInfo2'
341 bool m_useSolveConstraintObsolete;
343 btGeneric6DofConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA);
344 btGeneric6DofConstraint(btRigidBody & rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
346 //! Calcs global transform of the offsets
348 Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
349 \sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo
351 void calculateTransforms(const btTransform& transA, const btTransform& transB);
353 void calculateTransforms();
355 //! Gets the global transform of the offset for body A
357 \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
359 const btTransform& getCalculatedTransformA() const
361 return m_calculatedTransformA;
364 //! Gets the global transform of the offset for body B
366 \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
368 const btTransform& getCalculatedTransformB() const
370 return m_calculatedTransformB;
373 const btTransform& getFrameOffsetA() const
378 const btTransform& getFrameOffsetB() const
383 btTransform& getFrameOffsetA()
388 btTransform& getFrameOffsetB()
393 //! performs Jacobian calculation, and also calculates angle differences and axis
394 virtual void buildJacobian();
396 virtual void getInfo1(btConstraintInfo1 * info);
398 void getInfo1NonVirtual(btConstraintInfo1 * info);
400 virtual void getInfo2(btConstraintInfo2 * info);
402 void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB);
404 void updateRHS(btScalar timeStep);
406 //! Get the rotation axis in global coordinates
408 \pre btGeneric6DofConstraint.buildJacobian must be called previously.
410 btVector3 getAxis(int axis_index) const;
412 //! Get the relative Euler angle
414 \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
416 btScalar getAngle(int axis_index) const;
418 //! Get the relative position of the constraint pivot
420 \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
422 btScalar getRelativePivotPosition(int axis_index) const;
424 void setFrames(const btTransform& frameA, const btTransform& frameB);
426 //! Test angular limit.
428 Calculates angular correction and returns true if limit needs to be corrected.
429 \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
431 bool testAngularLimitMotor(int axis_index);
433 void setLinearLowerLimit(const btVector3& linearLower)
435 m_linearLimits.m_lowerLimit = linearLower;
438 void getLinearLowerLimit(btVector3 & linearLower) const
440 linearLower = m_linearLimits.m_lowerLimit;
443 void setLinearUpperLimit(const btVector3& linearUpper)
445 m_linearLimits.m_upperLimit = linearUpper;
448 void getLinearUpperLimit(btVector3 & linearUpper) const
450 linearUpper = m_linearLimits.m_upperLimit;
453 void setAngularLowerLimit(const btVector3& angularLower)
455 for (int i = 0; i < 3; i++)
456 m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
459 void getAngularLowerLimit(btVector3 & angularLower) const
461 for (int i = 0; i < 3; i++)
462 angularLower[i] = m_angularLimits[i].m_loLimit;
465 void setAngularUpperLimit(const btVector3& angularUpper)
467 for (int i = 0; i < 3; i++)
468 m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
471 void getAngularUpperLimit(btVector3 & angularUpper) const
473 for (int i = 0; i < 3; i++)
474 angularUpper[i] = m_angularLimits[i].m_hiLimit;
477 //! Retrieves the angular limit informacion
478 btRotationalLimitMotor* getRotationalLimitMotor(int index)
480 return &m_angularLimits[index];
483 //! Retrieves the limit informacion
484 btTranslationalLimitMotor* getTranslationalLimitMotor()
486 return &m_linearLimits;
489 //first 3 are linear, next 3 are angular
490 void setLimit(int axis, btScalar lo, btScalar hi)
494 m_linearLimits.m_lowerLimit[axis] = lo;
495 m_linearLimits.m_upperLimit[axis] = hi;
499 lo = btNormalizeAngle(lo);
500 hi = btNormalizeAngle(hi);
501 m_angularLimits[axis - 3].m_loLimit = lo;
502 m_angularLimits[axis - 3].m_hiLimit = hi;
508 - free means upper < lower,
509 - locked means upper == lower
510 - limited means upper > lower
511 - limitIndex: first 3 are linear, next 3 are angular
513 bool isLimited(int limitIndex) const
517 return m_linearLimits.isLimited(limitIndex);
519 return m_angularLimits[limitIndex - 3].isLimited();
522 virtual void calcAnchorPos(void); // overridable
524 int get_limit_motor_info2(btRotationalLimitMotor * limot,
525 const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB,
526 btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
528 // access for UseFrameOffset
529 bool getUseFrameOffset() const { return m_useOffsetForConstraintFrame; }
530 void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
532 bool getUseLinearReferenceFrameA() const { return m_useLinearReferenceFrameA; }
533 void setUseLinearReferenceFrameA(bool linearReferenceFrameA) { m_useLinearReferenceFrameA = linearReferenceFrameA; }
535 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
536 ///If no axis is provided, it uses the default axis for this constraint.
537 virtual void setParam(int num, btScalar value, int axis = -1);
538 ///return the local value of parameter
539 virtual btScalar getParam(int num, int axis = -1) const;
541 void setAxis(const btVector3& axis1, const btVector3& axis2);
543 virtual int getFlags() const
548 virtual int calculateSerializeBufferSize() const;
550 ///fills the dataBuffer and returns the struct name (and 0 on failure)
551 virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
554 struct btGeneric6DofConstraintData
556 btTypedConstraintData m_typeConstraintData;
557 btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
558 btTransformFloatData m_rbBFrame;
560 btVector3FloatData m_linearUpperLimit;
561 btVector3FloatData m_linearLowerLimit;
563 btVector3FloatData m_angularUpperLimit;
564 btVector3FloatData m_angularLowerLimit;
566 int m_useLinearReferenceFrameA;
567 int m_useOffsetForConstraintFrame;
570 struct btGeneric6DofConstraintDoubleData2
572 btTypedConstraintDoubleData m_typeConstraintData;
573 btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
574 btTransformDoubleData m_rbBFrame;
576 btVector3DoubleData m_linearUpperLimit;
577 btVector3DoubleData m_linearLowerLimit;
579 btVector3DoubleData m_angularUpperLimit;
580 btVector3DoubleData m_angularLowerLimit;
582 int m_useLinearReferenceFrameA;
583 int m_useOffsetForConstraintFrame;
586 SIMD_FORCE_INLINE int btGeneric6DofConstraint::calculateSerializeBufferSize() const
588 return sizeof(btGeneric6DofConstraintData2);
591 ///fills the dataBuffer and returns the struct name (and 0 on failure)
592 SIMD_FORCE_INLINE const char* btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
594 btGeneric6DofConstraintData2* dof = (btGeneric6DofConstraintData2*)dataBuffer;
595 btTypedConstraint::serialize(&dof->m_typeConstraintData, serializer);
597 m_frameInA.serialize(dof->m_rbAFrame);
598 m_frameInB.serialize(dof->m_rbBFrame);
601 for (i = 0; i < 3; i++)
603 dof->m_angularLowerLimit.m_floats[i] = m_angularLimits[i].m_loLimit;
604 dof->m_angularUpperLimit.m_floats[i] = m_angularLimits[i].m_hiLimit;
605 dof->m_linearLowerLimit.m_floats[i] = m_linearLimits.m_lowerLimit[i];
606 dof->m_linearUpperLimit.m_floats[i] = m_linearLimits.m_upperLimit[i];
609 dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA ? 1 : 0;
610 dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;
612 return btGeneric6DofConstraintDataName;
615 #endif //BT_GENERIC_6DOF_CONSTRAINT_H