diff options
Diffstat (limited to 'thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h')
| -rw-r--r-- | thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h | 517 |
1 files changed, 0 insertions, 517 deletions
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h deleted file mode 100644 index 1597809db3..0000000000 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h +++ /dev/null @@ -1,517 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -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. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/// 2009 March: b3Generic6DofConstraint refactored by Roman Ponomarev -/// Added support for generic constraint solver through getInfo1/getInfo2 methods - -/* -2007-09-09 -b3Generic6DofConstraint Refactored by Francisco Le?n -email: projectileman@yahoo.com -http://gimpact.sf.net -*/ - -#ifndef B3_GENERIC_6DOF_CONSTRAINT_H -#define B3_GENERIC_6DOF_CONSTRAINT_H - -#include "Bullet3Common/b3Vector3.h" -#include "b3JacobianEntry.h" -#include "b3TypedConstraint.h" - -struct b3RigidBodyData; - -//! Rotation Limit structure for generic joints -class b3RotationalLimitMotor -{ -public: - //! limit_parameters - //!@{ - b3Scalar m_loLimit; //!< joint limit - b3Scalar m_hiLimit; //!< joint limit - b3Scalar m_targetVelocity; //!< target motor velocity - b3Scalar m_maxMotorForce; //!< max force on motor - b3Scalar m_maxLimitForce; //!< max force on limit - b3Scalar m_damping; //!< Damping. - b3Scalar m_limitSoftness; //! Relaxation factor - b3Scalar m_normalCFM; //!< Constraint force mixing factor - b3Scalar m_stopERP; //!< Error tolerance factor when joint is at limit - b3Scalar m_stopCFM; //!< Constraint force mixing factor when joint is at limit - b3Scalar m_bounce; //!< restitution factor - bool m_enableMotor; - - //!@} - - //! temp_variables - //!@{ - b3Scalar m_currentLimitError; //! How much is violated this limit - b3Scalar m_currentPosition; //! current value of angle - int m_currentLimit; //!< 0=free, 1=at lo limit, 2=at hi limit - b3Scalar m_accumulatedImpulse; - //!@} - - b3RotationalLimitMotor() - { - m_accumulatedImpulse = 0.f; - m_targetVelocity = 0; - m_maxMotorForce = 6.0f; - m_maxLimitForce = 300.0f; - m_loLimit = 1.0f; - m_hiLimit = -1.0f; - m_normalCFM = 0.f; - m_stopERP = 0.2f; - m_stopCFM = 0.f; - m_bounce = 0.0f; - m_damping = 1.0f; - m_limitSoftness = 0.5f; - m_currentLimit = 0; - m_currentLimitError = 0; - m_enableMotor = false; - } - - b3RotationalLimitMotor(const b3RotationalLimitMotor& limot) - { - m_targetVelocity = limot.m_targetVelocity; - m_maxMotorForce = limot.m_maxMotorForce; - m_limitSoftness = limot.m_limitSoftness; - m_loLimit = limot.m_loLimit; - m_hiLimit = limot.m_hiLimit; - m_normalCFM = limot.m_normalCFM; - m_stopERP = limot.m_stopERP; - m_stopCFM = limot.m_stopCFM; - m_bounce = limot.m_bounce; - m_currentLimit = limot.m_currentLimit; - m_currentLimitError = limot.m_currentLimitError; - m_enableMotor = limot.m_enableMotor; - } - - //! Is limited - bool isLimited() - { - if (m_loLimit > m_hiLimit) return false; - return true; - } - - //! Need apply correction - bool needApplyTorques() - { - if (m_currentLimit == 0 && m_enableMotor == false) return false; - return true; - } - - //! calculates error - /*! - calculates m_currentLimit and m_currentLimitError. - */ - int testLimitValue(b3Scalar test_value); - - //! apply the correction impulses for two bodies - b3Scalar solveAngularLimits(b3Scalar timeStep, b3Vector3& axis, b3Scalar jacDiagABInv, b3RigidBodyData* body0, b3RigidBodyData* body1); -}; - -class b3TranslationalLimitMotor -{ -public: - b3Vector3 m_lowerLimit; //!< the constraint lower limits - b3Vector3 m_upperLimit; //!< the constraint upper limits - b3Vector3 m_accumulatedImpulse; - //! Linear_Limit_parameters - //!@{ - b3Vector3 m_normalCFM; //!< Constraint force mixing factor - b3Vector3 m_stopERP; //!< Error tolerance factor when joint is at limit - b3Vector3 m_stopCFM; //!< Constraint force mixing factor when joint is at limit - b3Vector3 m_targetVelocity; //!< target motor velocity - b3Vector3 m_maxMotorForce; //!< max force on motor - b3Vector3 m_currentLimitError; //! How much is violated this limit - b3Vector3 m_currentLinearDiff; //! Current relative offset of constraint frames - b3Scalar m_limitSoftness; //!< Softness for linear limit - b3Scalar m_damping; //!< Damping for linear limit - b3Scalar m_restitution; //! Bounce parameter for linear limit - //!@} - bool m_enableMotor[3]; - int m_currentLimit[3]; //!< 0=free, 1=at lower limit, 2=at upper limit - - b3TranslationalLimitMotor() - { - m_lowerLimit.setValue(0.f, 0.f, 0.f); - m_upperLimit.setValue(0.f, 0.f, 0.f); - m_accumulatedImpulse.setValue(0.f, 0.f, 0.f); - m_normalCFM.setValue(0.f, 0.f, 0.f); - m_stopERP.setValue(0.2f, 0.2f, 0.2f); - m_stopCFM.setValue(0.f, 0.f, 0.f); - - m_limitSoftness = 0.7f; - m_damping = b3Scalar(1.0f); - m_restitution = b3Scalar(0.5f); - for (int i = 0; i < 3; i++) - { - m_enableMotor[i] = false; - m_targetVelocity[i] = b3Scalar(0.f); - m_maxMotorForce[i] = b3Scalar(0.f); - } - } - - b3TranslationalLimitMotor(const b3TranslationalLimitMotor& other) - { - m_lowerLimit = other.m_lowerLimit; - m_upperLimit = other.m_upperLimit; - m_accumulatedImpulse = other.m_accumulatedImpulse; - - m_limitSoftness = other.m_limitSoftness; - m_damping = other.m_damping; - m_restitution = other.m_restitution; - m_normalCFM = other.m_normalCFM; - m_stopERP = other.m_stopERP; - m_stopCFM = other.m_stopCFM; - - for (int i = 0; i < 3; i++) - { - m_enableMotor[i] = other.m_enableMotor[i]; - m_targetVelocity[i] = other.m_targetVelocity[i]; - m_maxMotorForce[i] = other.m_maxMotorForce[i]; - } - } - - //! Test limit - /*! - - free means upper < lower, - - locked means upper == lower - - limited means upper > lower - - limitIndex: first 3 are linear, next 3 are angular - */ - inline bool isLimited(int limitIndex) - { - return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); - } - inline bool needApplyForce(int limitIndex) - { - if (m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false; - return true; - } - int testLimitValue(int limitIndex, b3Scalar test_value); - - b3Scalar solveLinearAxis( - b3Scalar timeStep, - b3Scalar jacDiagABInv, - b3RigidBodyData& body1, const b3Vector3& pointInA, - b3RigidBodyData& body2, const b3Vector3& pointInB, - int limit_index, - const b3Vector3& axis_normal_on_a, - const b3Vector3& anchorPos); -}; - -enum b36DofFlags -{ - B3_6DOF_FLAGS_CFM_NORM = 1, - B3_6DOF_FLAGS_CFM_STOP = 2, - B3_6DOF_FLAGS_ERP_STOP = 4 -}; -#define B3_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis - -/// b3Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space -/*! -b3Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'. -currently this limit supports rotational motors<br> -<ul> -<li> For Linear limits, use b3Generic6DofConstraint.setLinearUpperLimit, b3Generic6DofConstraint.setLinearLowerLimit. You can set the parameters with the b3TranslationalLimitMotor structure accsesible through the b3Generic6DofConstraint.getTranslationalLimitMotor method. -At this moment translational motors are not supported. May be in the future. </li> - -<li> For Angular limits, use the b3RotationalLimitMotor structure for configuring the limit. -This is accessible through b3Generic6DofConstraint.getLimitMotor method, -This brings support for limit parameters and motors. </li> - -<li> Angulars limits have these possible ranges: -<table border=1 > -<tr> - <td><b>AXIS</b></td> - <td><b>MIN ANGLE</b></td> - <td><b>MAX ANGLE</b></td> -</tr><tr> - <td>X</td> - <td>-PI</td> - <td>PI</td> -</tr><tr> - <td>Y</td> - <td>-PI/2</td> - <td>PI/2</td> -</tr><tr> - <td>Z</td> - <td>-PI</td> - <td>PI</td> -</tr> -</table> -</li> -</ul> - -*/ -B3_ATTRIBUTE_ALIGNED16(class) -b3Generic6DofConstraint : public b3TypedConstraint -{ -protected: - //! relative_frames - //!@{ - b3Transform m_frameInA; //!< the constraint space w.r.t body A - b3Transform m_frameInB; //!< the constraint space w.r.t body B - //!@} - - //! Jacobians - //!@{ - // b3JacobianEntry m_jacLinear[3];//!< 3 orthogonal linear constraints - // b3JacobianEntry m_jacAng[3];//!< 3 orthogonal angular constraints - //!@} - - //! Linear_Limit_parameters - //!@{ - b3TranslationalLimitMotor m_linearLimits; - //!@} - - //! hinge_parameters - //!@{ - b3RotationalLimitMotor m_angularLimits[3]; - //!@} - -protected: - //! temporal variables - //!@{ - b3Transform m_calculatedTransformA; - b3Transform m_calculatedTransformB; - b3Vector3 m_calculatedAxisAngleDiff; - b3Vector3 m_calculatedAxis[3]; - b3Vector3 m_calculatedLinearDiff; - b3Scalar m_timeStep; - b3Scalar m_factA; - b3Scalar m_factB; - bool m_hasStaticBody; - - b3Vector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes - - bool m_useLinearReferenceFrameA; - bool m_useOffsetForConstraintFrame; - - int m_flags; - - //!@} - - b3Generic6DofConstraint& operator=(b3Generic6DofConstraint& other) - { - b3Assert(0); - (void)other; - return *this; - } - - 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); - - int setLinearLimits(b3ConstraintInfo2 * info, int row, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB); - - // tests linear limits - void calculateLinearInfo(); - - //! calcs the euler angles between the two bodies. - void calculateAngleInfo(); - -public: - B3_DECLARE_ALIGNED_ALLOCATOR(); - - b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies); - - //! Calcs global transform of the offsets - /*! - Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies. - \sa b3Generic6DofConstraint.getCalculatedTransformA , b3Generic6DofConstraint.getCalculatedTransformB, b3Generic6DofConstraint.calculateAngleInfo - */ - void calculateTransforms(const b3Transform& transA, const b3Transform& transB, const b3RigidBodyData* bodies); - - void calculateTransforms(const b3RigidBodyData* bodies); - - //! Gets the global transform of the offset for body A - /*! - \sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo. - */ - const b3Transform& getCalculatedTransformA() const - { - return m_calculatedTransformA; - } - - //! Gets the global transform of the offset for body B - /*! - \sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo. - */ - const b3Transform& getCalculatedTransformB() const - { - return m_calculatedTransformB; - } - - const b3Transform& getFrameOffsetA() const - { - return m_frameInA; - } - - const b3Transform& getFrameOffsetB() const - { - return m_frameInB; - } - - b3Transform& getFrameOffsetA() - { - return m_frameInA; - } - - b3Transform& getFrameOffsetB() - { - return m_frameInB; - } - - virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies); - - void getInfo1NonVirtual(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies); - - virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies); - - 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); - - void updateRHS(b3Scalar timeStep); - - //! Get the rotation axis in global coordinates - b3Vector3 getAxis(int axis_index) const; - - //! Get the relative Euler angle - /*! - \pre b3Generic6DofConstraint::calculateTransforms() must be called previously. - */ - b3Scalar getAngle(int axis_index) const; - - //! Get the relative position of the constraint pivot - /*! - \pre b3Generic6DofConstraint::calculateTransforms() must be called previously. - */ - b3Scalar getRelativePivotPosition(int axis_index) const; - - void setFrames(const b3Transform& frameA, const b3Transform& frameB, const b3RigidBodyData* bodies); - - //! Test angular limit. - /*! - Calculates angular correction and returns true if limit needs to be corrected. - \pre b3Generic6DofConstraint::calculateTransforms() must be called previously. - */ - bool testAngularLimitMotor(int axis_index); - - void setLinearLowerLimit(const b3Vector3& linearLower) - { - m_linearLimits.m_lowerLimit = linearLower; - } - - void getLinearLowerLimit(b3Vector3 & linearLower) - { - linearLower = m_linearLimits.m_lowerLimit; - } - - void setLinearUpperLimit(const b3Vector3& linearUpper) - { - m_linearLimits.m_upperLimit = linearUpper; - } - - void getLinearUpperLimit(b3Vector3 & linearUpper) - { - linearUpper = m_linearLimits.m_upperLimit; - } - - void setAngularLowerLimit(const b3Vector3& angularLower) - { - for (int i = 0; i < 3; i++) - m_angularLimits[i].m_loLimit = b3NormalizeAngle(angularLower[i]); - } - - void getAngularLowerLimit(b3Vector3 & angularLower) - { - for (int i = 0; i < 3; i++) - angularLower[i] = m_angularLimits[i].m_loLimit; - } - - void setAngularUpperLimit(const b3Vector3& angularUpper) - { - for (int i = 0; i < 3; i++) - m_angularLimits[i].m_hiLimit = b3NormalizeAngle(angularUpper[i]); - } - - void getAngularUpperLimit(b3Vector3 & angularUpper) - { - for (int i = 0; i < 3; i++) - angularUpper[i] = m_angularLimits[i].m_hiLimit; - } - - //! Retrieves the angular limit informacion - b3RotationalLimitMotor* getRotationalLimitMotor(int index) - { - return &m_angularLimits[index]; - } - - //! Retrieves the limit informacion - b3TranslationalLimitMotor* getTranslationalLimitMotor() - { - return &m_linearLimits; - } - - //first 3 are linear, next 3 are angular - void setLimit(int axis, b3Scalar lo, b3Scalar hi) - { - if (axis < 3) - { - m_linearLimits.m_lowerLimit[axis] = lo; - m_linearLimits.m_upperLimit[axis] = hi; - } - else - { - lo = b3NormalizeAngle(lo); - hi = b3NormalizeAngle(hi); - m_angularLimits[axis - 3].m_loLimit = lo; - m_angularLimits[axis - 3].m_hiLimit = hi; - } - } - - //! Test limit - /*! - - free means upper < lower, - - locked means upper == lower - - limited means upper > lower - - limitIndex: first 3 are linear, next 3 are angular - */ - bool isLimited(int limitIndex) - { - if (limitIndex < 3) - { - return m_linearLimits.isLimited(limitIndex); - } - return m_angularLimits[limitIndex - 3].isLimited(); - } - - virtual void calcAnchorPos(const b3RigidBodyData* bodies); // overridable - - int get_limit_motor_info2(b3RotationalLimitMotor * limot, - const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB, - b3ConstraintInfo2* info, int row, b3Vector3& ax1, int rotational, int rotAllowed = false); - - // access for UseFrameOffset - bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; } - void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; } - - ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). - ///If no axis is provided, it uses the default axis for this constraint. - virtual void setParam(int num, b3Scalar value, int axis = -1); - ///return the local value of parameter - virtual b3Scalar getParam(int num, int axis = -1) const; - - void setAxis(const b3Vector3& axis1, const b3Vector3& axis2, const b3RigidBodyData* bodies); -}; - -#endif //B3_GENERIC_6DOF_CONSTRAINT_H |