551 lines
16 KiB
C++
551 lines
16 KiB
C++
/*
|
|
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
|