diff options
Diffstat (limited to 'thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h')
| -rw-r--r-- | thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h | 567 |
1 files changed, 267 insertions, 300 deletions
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h index 169b1b94ad..1597809db3 100644 --- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h +++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h @@ -23,7 +23,6 @@ email: projectileman@yahoo.com http://gimpact.sf.net */ - #ifndef B3_GENERIC_6DOF_CONSTRAINT_H #define B3_GENERIC_6DOF_CONSTRAINT_H @@ -33,88 +32,83 @@ http://gimpact.sf.net 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; + //! 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_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; - } - - + 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; - } + 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; - } + bool needApplyTorques() + { + if (m_currentLimit == 0 && m_enableMotor == false) return false; + return true; + } //! calculates error /*! @@ -123,104 +117,98 @@ public: int testLimitValue(b3Scalar test_value); //! apply the correction impulses for two bodies - b3Scalar solveAngularLimits(b3Scalar timeStep,b3Vector3& axis, b3Scalar jacDiagABInv,b3RigidBodyData * body0, b3RigidBodyData * body1); - + 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 + 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); + 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_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; + 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_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++) + 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 + //! 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; - } + 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); - - + 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 @@ -229,8 +217,7 @@ enum b36DofFlags B3_6DOF_FLAGS_CFM_STOP = 2, B3_6DOF_FLAGS_ERP_STOP = 4 }; -#define B3_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis - +#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 /*! @@ -268,240 +255,229 @@ This brings support for limit parameters and motors. </li> </ul> */ -B3_ATTRIBUTE_ALIGNED16(class) b3Generic6DofConstraint : public b3TypedConstraint +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 - //!@} + //!@{ + 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 - //!@} + //! 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]; + //!@{ + 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 + //! 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; - bool m_useLinearReferenceFrameA; - bool m_useOffsetForConstraintFrame; - - int m_flags; + b3Vector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes - //!@} + bool m_useLinearReferenceFrameA; + bool m_useOffsetForConstraintFrame; - b3Generic6DofConstraint& operator=(b3Generic6DofConstraint& other) - { - b3Assert(0); - (void) other; - return *this; - } + int m_flags; + //!@} - 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); + b3Generic6DofConstraint& operator=(b3Generic6DofConstraint& other) + { + b3Assert(0); + (void)other; + return *this; + } - int setLinearLimits(b3ConstraintInfo2 *info, int row, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB); + 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(); - - + void calculateAngleInfo(); public: - B3_DECLARE_ALIGNED_ALLOCATOR(); - - b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB ,bool useLinearReferenceFrameA,const b3RigidBodyData* bodies); - + + 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 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; - } + const b3Transform& getCalculatedTransformA() const + { + return m_calculatedTransformA; + } - //! Gets the global transform of the offset for body B - /*! + //! 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; - } + const b3Transform& getCalculatedTransformB() const + { + return m_calculatedTransformB; + } - b3Transform & getFrameOffsetB() - { - return m_frameInB; - } + const b3Transform& getFrameOffsetA() const + { + return m_frameInA; + } + const b3Transform& getFrameOffsetB() const + { + return m_frameInB; + } + b3Transform& getFrameOffsetA() + { + return m_frameInA; + } - virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies); + b3Transform& getFrameOffsetB() + { + return m_frameInB; + } - void getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies); + virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies); - virtual void getInfo2 (b3ConstraintInfo2* info,const b3RigidBodyData* bodies); + void getInfo1NonVirtual(b3ConstraintInfo1 * 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); + 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); + void updateRHS(b3Scalar timeStep); //! Get the rotation axis in global coordinates - b3Vector3 getAxis(int axis_index) const; + b3Vector3 getAxis(int axis_index) const; - //! Get the relative Euler angle - /*! + //! Get the relative Euler angle + /*! \pre b3Generic6DofConstraint::calculateTransforms() must be called previously. */ - b3Scalar getAngle(int axis_index) const; + 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); + 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); + bool testAngularLimitMotor(int axis_index); - void setLinearLowerLimit(const b3Vector3& linearLower) - { - m_linearLimits.m_lowerLimit = linearLower; - } + void setLinearLowerLimit(const b3Vector3& linearLower) + { + m_linearLimits.m_lowerLimit = linearLower; + } - void getLinearLowerLimit(b3Vector3& linearLower) + void getLinearLowerLimit(b3Vector3 & linearLower) { linearLower = m_linearLimits.m_lowerLimit; } - void setLinearUpperLimit(const b3Vector3& linearUpper) + void setLinearUpperLimit(const b3Vector3& linearUpper) { m_linearLimits.m_upperLimit = linearUpper; } - void getLinearUpperLimit(b3Vector3& linearUpper) + void getLinearUpperLimit(b3Vector3 & linearUpper) { linearUpper = m_linearLimits.m_upperLimit; } - void setAngularLowerLimit(const b3Vector3& angularLower) - { - for(int i = 0; i < 3; i++) + void setAngularLowerLimit(const b3Vector3& angularLower) + { + for (int i = 0; i < 3; i++) m_angularLimits[i].m_loLimit = b3NormalizeAngle(angularLower[i]); - } + } - void getAngularLowerLimit(b3Vector3& angularLower) + void getAngularLowerLimit(b3Vector3 & angularLower) { - for(int i = 0; i < 3; i++) + 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++) + void setAngularUpperLimit(const b3Vector3& angularUpper) + { + for (int i = 0; i < 3; i++) m_angularLimits[i].m_hiLimit = b3NormalizeAngle(angularUpper[i]); - } + } - void getAngularUpperLimit(b3Vector3& angularUpper) + void getAngularUpperLimit(b3Vector3 & angularUpper) { - for(int i = 0; i < 3; i++) + 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 - { + 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; - } - } + m_angularLimits[axis - 3].m_loLimit = lo; + m_angularLimits[axis - 3].m_hiLimit = hi; + } + } //! Test limit /*! @@ -510,41 +486,32 @@ public: - limited means upper > lower - limitIndex: first 3 are linear, next 3 are angular */ - bool isLimited(int limitIndex) - { - if(limitIndex<3) - { + bool isLimited(int limitIndex) + { + if (limitIndex < 3) + { return m_linearLimits.isLimited(limitIndex); + } + return m_angularLimits[limitIndex - 3].isLimited(); + } - } - return m_angularLimits[limitIndex-3].isLimited(); - } - - virtual void calcAnchorPos(const b3RigidBodyData* bodies); // overridable + 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); + 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). + ///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); + 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); + 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 +#endif //B3_GENERIC_6DOF_CONSTRAINT_H |