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: b3Generic6DofConstraint refactored by Roman Ponomarev
17 /// Added support for generic constraint solver through getInfo1/getInfo2 methods
21 b3Generic6DofConstraint Refactored by Francisco Le?n
22 email: projectileman@yahoo.com
26 #ifndef B3_GENERIC_6DOF_CONSTRAINT_H
27 #define B3_GENERIC_6DOF_CONSTRAINT_H
29 #include "Bullet3Common/b3Vector3.h"
30 #include "b3JacobianEntry.h"
31 #include "b3TypedConstraint.h"
33 struct b3RigidBodyData;
35 //! Rotation Limit structure for generic joints
36 class b3RotationalLimitMotor
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
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;
64 b3RotationalLimitMotor()
66 m_accumulatedImpulse = 0.f;
68 m_maxMotorForce = 6.0f;
69 m_maxLimitForce = 300.0f;
77 m_limitSoftness = 0.5f;
79 m_currentLimitError = 0;
80 m_enableMotor = false;
83 b3RotationalLimitMotor(const b3RotationalLimitMotor& limot)
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;
102 if (m_loLimit > m_hiLimit) return false;
106 //! Need apply correction
107 bool needApplyTorques()
109 if (m_currentLimit == 0 && m_enableMotor == false) return false;
115 calculates m_currentLimit and m_currentLimitError.
117 int testLimitValue(b3Scalar test_value);
119 //! apply the correction impulses for two bodies
120 b3Scalar solveAngularLimits(b3Scalar timeStep, b3Vector3& axis, b3Scalar jacDiagABInv, b3RigidBodyData* body0, b3RigidBodyData* body1);
123 class b3TranslationalLimitMotor
126 b3Vector3 m_lowerLimit; //!< the constraint lower limits
127 b3Vector3 m_upperLimit; //!< the constraint upper limits
128 b3Vector3 m_accumulatedImpulse;
129 //! Linear_Limit_parameters
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
142 bool m_enableMotor[3];
143 int m_currentLimit[3]; //!< 0=free, 1=at lower limit, 2=at upper limit
145 b3TranslationalLimitMotor()
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);
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++)
159 m_enableMotor[i] = false;
160 m_targetVelocity[i] = b3Scalar(0.f);
161 m_maxMotorForce[i] = b3Scalar(0.f);
165 b3TranslationalLimitMotor(const b3TranslationalLimitMotor& other)
167 m_lowerLimit = other.m_lowerLimit;
168 m_upperLimit = other.m_upperLimit;
169 m_accumulatedImpulse = other.m_accumulatedImpulse;
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;
178 for (int i = 0; i < 3; i++)
180 m_enableMotor[i] = other.m_enableMotor[i];
181 m_targetVelocity[i] = other.m_targetVelocity[i];
182 m_maxMotorForce[i] = other.m_maxMotorForce[i];
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
193 inline bool isLimited(int limitIndex)
195 return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
197 inline bool needApplyForce(int limitIndex)
199 if (m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
202 int testLimitValue(int limitIndex, b3Scalar test_value);
204 b3Scalar solveLinearAxis(
206 b3Scalar jacDiagABInv,
207 b3RigidBodyData& body1, const b3Vector3& pointInA,
208 b3RigidBodyData& body2, const b3Vector3& pointInB,
210 const b3Vector3& axis_normal_on_a,
211 const b3Vector3& anchorPos);
216 B3_6DOF_FLAGS_CFM_NORM = 1,
217 B3_6DOF_FLAGS_CFM_STOP = 2,
218 B3_6DOF_FLAGS_ERP_STOP = 4
220 #define B3_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
222 /// b3Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
224 b3Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'.
225 currently this limit supports rotational motors<br>
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>
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>
234 <li> Angulars limits have these possible ranges:
238 <td><b>MIN ANGLE</b></td>
239 <td><b>MAX ANGLE</b></td>
258 B3_ATTRIBUTE_ALIGNED16(class)
259 b3Generic6DofConstraint : public b3TypedConstraint
264 b3Transform m_frameInA; //!< the constraint space w.r.t body A
265 b3Transform m_frameInB; //!< the constraint space w.r.t body B
270 // b3JacobianEntry m_jacLinear[3];//!< 3 orthogonal linear constraints
271 // b3JacobianEntry m_jacAng[3];//!< 3 orthogonal angular constraints
274 //! Linear_Limit_parameters
276 b3TranslationalLimitMotor m_linearLimits;
281 b3RotationalLimitMotor m_angularLimits[3];
285 //! temporal variables
287 b3Transform m_calculatedTransformA;
288 b3Transform m_calculatedTransformB;
289 b3Vector3 m_calculatedAxisAngleDiff;
290 b3Vector3 m_calculatedAxis[3];
291 b3Vector3 m_calculatedLinearDiff;
295 bool m_hasStaticBody;
297 b3Vector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
299 bool m_useLinearReferenceFrameA;
300 bool m_useOffsetForConstraintFrame;
306 b3Generic6DofConstraint& operator=(b3Generic6DofConstraint& other)
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);
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);
317 // tests linear limits
318 void calculateLinearInfo();
320 //! calcs the euler angles between the two bodies.
321 void calculateAngleInfo();
324 B3_DECLARE_ALIGNED_ALLOCATOR();
326 b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies);
328 //! Calcs global transform of the offsets
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
333 void calculateTransforms(const b3Transform& transA, const b3Transform& transB, const b3RigidBodyData* bodies);
335 void calculateTransforms(const b3RigidBodyData* bodies);
337 //! Gets the global transform of the offset for body A
339 \sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo.
341 const b3Transform& getCalculatedTransformA() const
343 return m_calculatedTransformA;
346 //! Gets the global transform of the offset for body B
348 \sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo.
350 const b3Transform& getCalculatedTransformB() const
352 return m_calculatedTransformB;
355 const b3Transform& getFrameOffsetA() const
360 const b3Transform& getFrameOffsetB() const
365 b3Transform& getFrameOffsetA()
370 b3Transform& getFrameOffsetB()
375 virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
377 void getInfo1NonVirtual(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
379 virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies);
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);
383 void updateRHS(b3Scalar timeStep);
385 //! Get the rotation axis in global coordinates
386 b3Vector3 getAxis(int axis_index) const;
388 //! Get the relative Euler angle
390 \pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
392 b3Scalar getAngle(int axis_index) const;
394 //! Get the relative position of the constraint pivot
396 \pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
398 b3Scalar getRelativePivotPosition(int axis_index) const;
400 void setFrames(const b3Transform& frameA, const b3Transform& frameB, const b3RigidBodyData* bodies);
402 //! Test angular limit.
404 Calculates angular correction and returns true if limit needs to be corrected.
405 \pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
407 bool testAngularLimitMotor(int axis_index);
409 void setLinearLowerLimit(const b3Vector3& linearLower)
411 m_linearLimits.m_lowerLimit = linearLower;
414 void getLinearLowerLimit(b3Vector3 & linearLower)
416 linearLower = m_linearLimits.m_lowerLimit;
419 void setLinearUpperLimit(const b3Vector3& linearUpper)
421 m_linearLimits.m_upperLimit = linearUpper;
424 void getLinearUpperLimit(b3Vector3 & linearUpper)
426 linearUpper = m_linearLimits.m_upperLimit;
429 void setAngularLowerLimit(const b3Vector3& angularLower)
431 for (int i = 0; i < 3; i++)
432 m_angularLimits[i].m_loLimit = b3NormalizeAngle(angularLower[i]);
435 void getAngularLowerLimit(b3Vector3 & angularLower)
437 for (int i = 0; i < 3; i++)
438 angularLower[i] = m_angularLimits[i].m_loLimit;
441 void setAngularUpperLimit(const b3Vector3& angularUpper)
443 for (int i = 0; i < 3; i++)
444 m_angularLimits[i].m_hiLimit = b3NormalizeAngle(angularUpper[i]);
447 void getAngularUpperLimit(b3Vector3 & angularUpper)
449 for (int i = 0; i < 3; i++)
450 angularUpper[i] = m_angularLimits[i].m_hiLimit;
453 //! Retrieves the angular limit informacion
454 b3RotationalLimitMotor* getRotationalLimitMotor(int index)
456 return &m_angularLimits[index];
459 //! Retrieves the limit informacion
460 b3TranslationalLimitMotor* getTranslationalLimitMotor()
462 return &m_linearLimits;
465 //first 3 are linear, next 3 are angular
466 void setLimit(int axis, b3Scalar lo, b3Scalar hi)
470 m_linearLimits.m_lowerLimit[axis] = lo;
471 m_linearLimits.m_upperLimit[axis] = hi;
475 lo = b3NormalizeAngle(lo);
476 hi = b3NormalizeAngle(hi);
477 m_angularLimits[axis - 3].m_loLimit = lo;
478 m_angularLimits[axis - 3].m_hiLimit = hi;
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
489 bool isLimited(int limitIndex)
493 return m_linearLimits.isLimited(limitIndex);
495 return m_angularLimits[limitIndex - 3].isLimited();
498 virtual void calcAnchorPos(const b3RigidBodyData* bodies); // overridable
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);
504 // access for UseFrameOffset
505 bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
506 void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
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;
514 void setAxis(const b3Vector3& axis1, const b3Vector3& axis2, const b3RigidBodyData* bodies);
517 #endif //B3_GENERIC_6DOF_CONSTRAINT_H