2017-08-01 12:30:58 +00:00
|
|
|
/*
|
|
|
|
Bullet Continuous Collision Detection and Physics Library
|
2022-01-06 22:37:49 +00:00
|
|
|
Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
|
2017-08-01 12:30:58 +00:00
|
|
|
|
|
|
|
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: btGeneric6DofConstraint refactored by Roman Ponomarev
|
|
|
|
/// Added support for generic constraint solver through getInfo1/getInfo2 methods
|
|
|
|
|
|
|
|
/*
|
|
|
|
2007-09-09
|
|
|
|
btGeneric6DofConstraint Refactored by Francisco Le?n
|
|
|
|
email: projectileman@yahoo.com
|
|
|
|
http://gimpact.sf.net
|
|
|
|
*/
|
|
|
|
|
|
|
|
#ifndef BT_GENERIC_6DOF_CONSTRAINT_H
|
|
|
|
#define BT_GENERIC_6DOF_CONSTRAINT_H
|
|
|
|
|
|
|
|
#include "LinearMath/btVector3.h"
|
|
|
|
#include "btJacobianEntry.h"
|
|
|
|
#include "btTypedConstraint.h"
|
|
|
|
|
|
|
|
class btRigidBody;
|
|
|
|
|
|
|
|
#ifdef BT_USE_DOUBLE_PRECISION
|
2019-01-03 13:26:51 +00:00
|
|
|
#define btGeneric6DofConstraintData2 btGeneric6DofConstraintDoubleData2
|
|
|
|
#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintDoubleData2"
|
2017-08-01 12:30:58 +00:00
|
|
|
#else
|
2019-01-03 13:26:51 +00:00
|
|
|
#define btGeneric6DofConstraintData2 btGeneric6DofConstraintData
|
|
|
|
#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintData"
|
|
|
|
#endif //BT_USE_DOUBLE_PRECISION
|
2017-08-01 12:30:58 +00:00
|
|
|
|
|
|
|
//! Rotation Limit structure for generic joints
|
|
|
|
class btRotationalLimitMotor
|
|
|
|
{
|
|
|
|
public:
|
2019-01-03 13:26:51 +00:00
|
|
|
//! limit_parameters
|
|
|
|
//!@{
|
|
|
|
btScalar m_loLimit; //!< joint limit
|
|
|
|
btScalar m_hiLimit; //!< joint limit
|
|
|
|
btScalar m_targetVelocity; //!< target motor velocity
|
|
|
|
btScalar m_maxMotorForce; //!< max force on motor
|
|
|
|
btScalar m_maxLimitForce; //!< max force on limit
|
|
|
|
btScalar m_damping; //!< Damping.
|
|
|
|
btScalar m_limitSoftness; //! Relaxation factor
|
|
|
|
btScalar m_normalCFM; //!< Constraint force mixing factor
|
|
|
|
btScalar m_stopERP; //!< Error tolerance factor when joint is at limit
|
|
|
|
btScalar m_stopCFM; //!< Constraint force mixing factor when joint is at limit
|
|
|
|
btScalar m_bounce; //!< restitution factor
|
|
|
|
bool m_enableMotor;
|
|
|
|
|
|
|
|
//!@}
|
|
|
|
|
|
|
|
//! temp_variables
|
|
|
|
//!@{
|
|
|
|
btScalar m_currentLimitError; //! How much is violated this limit
|
|
|
|
btScalar m_currentPosition; //! current value of angle
|
|
|
|
int m_currentLimit; //!< 0=free, 1=at lo limit, 2=at hi limit
|
|
|
|
btScalar m_accumulatedImpulse;
|
|
|
|
//!@}
|
|
|
|
|
|
|
|
btRotationalLimitMotor()
|
|
|
|
{
|
|
|
|
m_accumulatedImpulse = 0.f;
|
|
|
|
m_targetVelocity = 0;
|
|
|
|
m_maxMotorForce = 6.0f;
|
|
|
|
m_maxLimitForce = 300.0f;
|
|
|
|
m_loLimit = 1.0f;
|
|
|
|
m_hiLimit = -1.0f;
|
2017-08-01 12:30:58 +00:00
|
|
|
m_normalCFM = 0.f;
|
|
|
|
m_stopERP = 0.2f;
|
|
|
|
m_stopCFM = 0.f;
|
2019-01-03 13:26:51 +00:00
|
|
|
m_bounce = 0.0f;
|
|
|
|
m_damping = 1.0f;
|
|
|
|
m_limitSoftness = 0.5f;
|
|
|
|
m_currentLimit = 0;
|
|
|
|
m_currentLimitError = 0;
|
|
|
|
m_enableMotor = false;
|
|
|
|
}
|
|
|
|
|
|
|
|
btRotationalLimitMotor(const btRotationalLimitMotor& 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;
|
2017-08-01 12:30:58 +00:00
|
|
|
m_normalCFM = limot.m_normalCFM;
|
|
|
|
m_stopERP = limot.m_stopERP;
|
2019-01-03 13:26:51 +00:00
|
|
|
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;
|
|
|
|
}
|
2017-08-01 12:30:58 +00:00
|
|
|
|
|
|
|
//! Is limited
|
2019-01-03 13:26:51 +00:00
|
|
|
bool isLimited() const
|
|
|
|
{
|
|
|
|
if (m_loLimit > m_hiLimit) return false;
|
|
|
|
return true;
|
|
|
|
}
|
2017-08-01 12:30:58 +00:00
|
|
|
|
|
|
|
//! Need apply correction
|
2019-01-03 13:26:51 +00:00
|
|
|
bool needApplyTorques() const
|
|
|
|
{
|
|
|
|
if (m_currentLimit == 0 && m_enableMotor == false) return false;
|
|
|
|
return true;
|
|
|
|
}
|
2017-08-01 12:30:58 +00:00
|
|
|
|
|
|
|
//! calculates error
|
|
|
|
/*!
|
|
|
|
calculates m_currentLimit and m_currentLimitError.
|
|
|
|
*/
|
|
|
|
int testLimitValue(btScalar test_value);
|
|
|
|
|
|
|
|
//! apply the correction impulses for two bodies
|
2019-01-03 13:26:51 +00:00
|
|
|
btScalar solveAngularLimits(btScalar timeStep, btVector3& axis, btScalar jacDiagABInv, btRigidBody* body0, btRigidBody* body1);
|
2017-08-01 12:30:58 +00:00
|
|
|
};
|
|
|
|
|
|
|
|
class btTranslationalLimitMotor
|
|
|
|
{
|
|
|
|
public:
|
2019-01-03 13:26:51 +00:00
|
|
|
btVector3 m_lowerLimit; //!< the constraint lower limits
|
|
|
|
btVector3 m_upperLimit; //!< the constraint upper limits
|
|
|
|
btVector3 m_accumulatedImpulse;
|
|
|
|
//! Linear_Limit_parameters
|
|
|
|
//!@{
|
|
|
|
btScalar m_limitSoftness; //!< Softness for linear limit
|
|
|
|
btScalar m_damping; //!< Damping for linear limit
|
|
|
|
btScalar m_restitution; //! Bounce parameter for linear limit
|
|
|
|
btVector3 m_normalCFM; //!< Constraint force mixing factor
|
|
|
|
btVector3 m_stopERP; //!< Error tolerance factor when joint is at limit
|
|
|
|
btVector3 m_stopCFM; //!< Constraint force mixing factor when joint is at limit
|
|
|
|
//!@}
|
|
|
|
bool m_enableMotor[3];
|
|
|
|
btVector3 m_targetVelocity; //!< target motor velocity
|
|
|
|
btVector3 m_maxMotorForce; //!< max force on motor
|
|
|
|
btVector3 m_currentLimitError; //! How much is violated this limit
|
|
|
|
btVector3 m_currentLinearDiff; //! Current relative offset of constraint frames
|
|
|
|
int m_currentLimit[3]; //!< 0=free, 1=at lower limit, 2=at upper limit
|
|
|
|
|
|
|
|
btTranslationalLimitMotor()
|
|
|
|
{
|
|
|
|
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);
|
2017-08-01 12:30:58 +00:00
|
|
|
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);
|
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
m_limitSoftness = 0.7f;
|
|
|
|
m_damping = btScalar(1.0f);
|
|
|
|
m_restitution = btScalar(0.5f);
|
|
|
|
for (int i = 0; i < 3; i++)
|
2017-08-01 12:30:58 +00:00
|
|
|
{
|
|
|
|
m_enableMotor[i] = false;
|
|
|
|
m_targetVelocity[i] = btScalar(0.f);
|
|
|
|
m_maxMotorForce[i] = btScalar(0.f);
|
|
|
|
}
|
2019-01-03 13:26:51 +00:00
|
|
|
}
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
btTranslationalLimitMotor(const btTranslationalLimitMotor& other)
|
|
|
|
{
|
|
|
|
m_lowerLimit = other.m_lowerLimit;
|
|
|
|
m_upperLimit = other.m_upperLimit;
|
|
|
|
m_accumulatedImpulse = other.m_accumulatedImpulse;
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
m_limitSoftness = other.m_limitSoftness;
|
|
|
|
m_damping = other.m_damping;
|
|
|
|
m_restitution = other.m_restitution;
|
2017-08-01 12:30:58 +00:00
|
|
|
m_normalCFM = other.m_normalCFM;
|
|
|
|
m_stopERP = other.m_stopERP;
|
|
|
|
m_stopCFM = other.m_stopCFM;
|
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
for (int i = 0; i < 3; i++)
|
2017-08-01 12:30:58 +00:00
|
|
|
{
|
|
|
|
m_enableMotor[i] = other.m_enableMotor[i];
|
|
|
|
m_targetVelocity[i] = other.m_targetVelocity[i];
|
|
|
|
m_maxMotorForce[i] = other.m_maxMotorForce[i];
|
|
|
|
}
|
2019-01-03 13:26:51 +00:00
|
|
|
}
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
//! Test limit
|
2017-08-01 12:30:58 +00:00
|
|
|
/*!
|
|
|
|
- free means upper < lower,
|
|
|
|
- locked means upper == lower
|
|
|
|
- limited means upper > lower
|
|
|
|
- limitIndex: first 3 are linear, next 3 are angular
|
|
|
|
*/
|
2019-01-03 13:26:51 +00:00
|
|
|
inline bool isLimited(int limitIndex) const
|
|
|
|
{
|
|
|
|
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
|
|
|
|
}
|
|
|
|
inline bool needApplyForce(int limitIndex) const
|
|
|
|
{
|
|
|
|
if (m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
|
|
|
|
return true;
|
|
|
|
}
|
2017-08-01 12:30:58 +00:00
|
|
|
int testLimitValue(int limitIndex, btScalar test_value);
|
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
btScalar solveLinearAxis(
|
|
|
|
btScalar timeStep,
|
|
|
|
btScalar jacDiagABInv,
|
|
|
|
btRigidBody& body1, const btVector3& pointInA,
|
|
|
|
btRigidBody& body2, const btVector3& pointInB,
|
|
|
|
int limit_index,
|
|
|
|
const btVector3& axis_normal_on_a,
|
|
|
|
const btVector3& anchorPos);
|
2017-08-01 12:30:58 +00:00
|
|
|
};
|
|
|
|
|
|
|
|
enum bt6DofFlags
|
|
|
|
{
|
|
|
|
BT_6DOF_FLAGS_CFM_NORM = 1,
|
|
|
|
BT_6DOF_FLAGS_CFM_STOP = 2,
|
|
|
|
BT_6DOF_FLAGS_ERP_STOP = 4
|
|
|
|
};
|
2019-01-03 13:26:51 +00:00
|
|
|
#define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
|
2017-08-01 12:30:58 +00:00
|
|
|
|
|
|
|
/// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
|
|
|
|
/*!
|
|
|
|
btGeneric6DofConstraint 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 btGeneric6DofConstraint.setLinearUpperLimit, btGeneric6DofConstraint.setLinearLowerLimit. You can set the parameters with the btTranslationalLimitMotor structure accsesible through the btGeneric6DofConstraint.getTranslationalLimitMotor method.
|
|
|
|
At this moment translational motors are not supported. May be in the future. </li>
|
|
|
|
|
|
|
|
<li> For Angular limits, use the btRotationalLimitMotor structure for configuring the limit.
|
|
|
|
This is accessible through btGeneric6DofConstraint.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>
|
|
|
|
|
|
|
|
*/
|
2019-01-03 13:26:51 +00:00
|
|
|
ATTRIBUTE_ALIGNED16(class)
|
|
|
|
btGeneric6DofConstraint : public btTypedConstraint
|
2017-08-01 12:30:58 +00:00
|
|
|
{
|
|
|
|
protected:
|
|
|
|
//! relative_frames
|
2019-01-03 13:26:51 +00:00
|
|
|
//!@{
|
|
|
|
btTransform m_frameInA; //!< the constraint space w.r.t body A
|
|
|
|
btTransform m_frameInB; //!< the constraint space w.r.t body B
|
|
|
|
//!@}
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
//! Jacobians
|
|
|
|
//!@{
|
|
|
|
btJacobianEntry m_jacLinear[3]; //!< 3 orthogonal linear constraints
|
|
|
|
btJacobianEntry m_jacAng[3]; //!< 3 orthogonal angular constraints
|
|
|
|
//!@}
|
2017-08-01 12:30:58 +00:00
|
|
|
|
|
|
|
//! Linear_Limit_parameters
|
2019-01-03 13:26:51 +00:00
|
|
|
//!@{
|
|
|
|
btTranslationalLimitMotor m_linearLimits;
|
2017-08-01 12:30:58 +00:00
|
|
|
//!@}
|
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
//! hinge_parameters
|
|
|
|
//!@{
|
|
|
|
btRotationalLimitMotor m_angularLimits[3];
|
|
|
|
//!@}
|
2017-08-01 12:30:58 +00:00
|
|
|
|
|
|
|
protected:
|
2019-01-03 13:26:51 +00:00
|
|
|
//! temporal variables
|
|
|
|
//!@{
|
|
|
|
btScalar m_timeStep;
|
|
|
|
btTransform m_calculatedTransformA;
|
|
|
|
btTransform m_calculatedTransformB;
|
|
|
|
btVector3 m_calculatedAxisAngleDiff;
|
|
|
|
btVector3 m_calculatedAxis[3];
|
|
|
|
btVector3 m_calculatedLinearDiff;
|
|
|
|
btScalar m_factA;
|
|
|
|
btScalar m_factB;
|
|
|
|
bool m_hasStaticBody;
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
bool m_useLinearReferenceFrameA;
|
|
|
|
bool m_useOffsetForConstraintFrame;
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
int m_flags;
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
//!@}
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other)
|
|
|
|
{
|
|
|
|
btAssert(0);
|
|
|
|
(void)other;
|
|
|
|
return *this;
|
|
|
|
}
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
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);
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
int setLinearLimits(btConstraintInfo2 * info, int row, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB);
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
void buildLinearJacobian(
|
|
|
|
btJacobianEntry & jacLinear, const btVector3& normalWorld,
|
|
|
|
const btVector3& pivotAInW, const btVector3& pivotBInW);
|
|
|
|
|
|
|
|
void buildAngularJacobian(btJacobianEntry & jacAngular, const btVector3& jointAxisW);
|
2017-08-01 12:30:58 +00:00
|
|
|
|
|
|
|
// tests linear limits
|
|
|
|
void calculateLinearInfo();
|
|
|
|
|
|
|
|
//! calcs the euler angles between the two bodies.
|
2019-01-03 13:26:51 +00:00
|
|
|
void calculateAngleInfo();
|
2017-08-01 12:30:58 +00:00
|
|
|
|
|
|
|
public:
|
|
|
|
BT_DECLARE_ALIGNED_ALLOCATOR();
|
2019-01-03 13:26:51 +00:00
|
|
|
|
2017-08-01 12:30:58 +00:00
|
|
|
///for backwards compatibility during the transition to 'getInfo/getInfo2'
|
2019-01-03 13:26:51 +00:00
|
|
|
bool m_useSolveConstraintObsolete;
|
|
|
|
|
|
|
|
btGeneric6DofConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA);
|
|
|
|
btGeneric6DofConstraint(btRigidBody & rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
|
2017-08-01 12:30:58 +00:00
|
|
|
|
|
|
|
//! 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 btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo
|
|
|
|
*/
|
2019-01-03 13:26:51 +00:00
|
|
|
void calculateTransforms(const btTransform& transA, const btTransform& transB);
|
2017-08-01 12:30:58 +00:00
|
|
|
|
|
|
|
void calculateTransforms();
|
|
|
|
|
|
|
|
//! Gets the global transform of the offset for body A
|
2019-01-03 13:26:51 +00:00
|
|
|
/*!
|
2017-08-01 12:30:58 +00:00
|
|
|
\sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
|
|
|
|
*/
|
2019-01-03 13:26:51 +00:00
|
|
|
const btTransform& getCalculatedTransformA() const
|
|
|
|
{
|
|
|
|
return m_calculatedTransformA;
|
|
|
|
}
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
//! Gets the global transform of the offset for body B
|
|
|
|
/*!
|
2017-08-01 12:30:58 +00:00
|
|
|
\sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
|
|
|
|
*/
|
2019-01-03 13:26:51 +00:00
|
|
|
const btTransform& getCalculatedTransformB() const
|
|
|
|
{
|
|
|
|
return m_calculatedTransformB;
|
|
|
|
}
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
const btTransform& getFrameOffsetA() const
|
|
|
|
{
|
|
|
|
return m_frameInA;
|
|
|
|
}
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
const btTransform& getFrameOffsetB() const
|
|
|
|
{
|
|
|
|
return m_frameInB;
|
|
|
|
}
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
btTransform& getFrameOffsetA()
|
|
|
|
{
|
|
|
|
return m_frameInA;
|
|
|
|
}
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
btTransform& getFrameOffsetB()
|
|
|
|
{
|
|
|
|
return m_frameInB;
|
|
|
|
}
|
2017-08-01 12:30:58 +00:00
|
|
|
|
|
|
|
//! performs Jacobian calculation, and also calculates angle differences and axis
|
2019-01-03 13:26:51 +00:00
|
|
|
virtual void buildJacobian();
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
virtual void getInfo1(btConstraintInfo1 * info);
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
void getInfo1NonVirtual(btConstraintInfo1 * info);
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
virtual void getInfo2(btConstraintInfo2 * info);
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB);
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
void updateRHS(btScalar timeStep);
|
2017-08-01 12:30:58 +00:00
|
|
|
|
|
|
|
//! Get the rotation axis in global coordinates
|
|
|
|
/*!
|
|
|
|
\pre btGeneric6DofConstraint.buildJacobian must be called previously.
|
|
|
|
*/
|
2019-01-03 13:26:51 +00:00
|
|
|
btVector3 getAxis(int axis_index) const;
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
//! Get the relative Euler angle
|
|
|
|
/*!
|
2017-08-01 12:30:58 +00:00
|
|
|
\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
|
|
|
|
*/
|
2019-01-03 13:26:51 +00:00
|
|
|
btScalar getAngle(int axis_index) const;
|
2017-08-01 12:30:58 +00:00
|
|
|
|
|
|
|
//! Get the relative position of the constraint pivot
|
2019-01-03 13:26:51 +00:00
|
|
|
/*!
|
2017-08-01 12:30:58 +00:00
|
|
|
\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
|
|
|
|
*/
|
|
|
|
btScalar getRelativePivotPosition(int axis_index) const;
|
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
void setFrames(const btTransform& frameA, const btTransform& frameB);
|
2017-08-01 12:30:58 +00:00
|
|
|
|
|
|
|
//! Test angular limit.
|
|
|
|
/*!
|
|
|
|
Calculates angular correction and returns true if limit needs to be corrected.
|
|
|
|
\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
|
|
|
|
*/
|
2019-01-03 13:26:51 +00:00
|
|
|
bool testAngularLimitMotor(int axis_index);
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
void setLinearLowerLimit(const btVector3& linearLower)
|
|
|
|
{
|
|
|
|
m_linearLimits.m_lowerLimit = linearLower;
|
|
|
|
}
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
void getLinearLowerLimit(btVector3 & linearLower) const
|
2017-08-01 12:30:58 +00:00
|
|
|
{
|
|
|
|
linearLower = m_linearLimits.m_lowerLimit;
|
|
|
|
}
|
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
void setLinearUpperLimit(const btVector3& linearUpper)
|
2017-08-01 12:30:58 +00:00
|
|
|
{
|
|
|
|
m_linearLimits.m_upperLimit = linearUpper;
|
|
|
|
}
|
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
void getLinearUpperLimit(btVector3 & linearUpper) const
|
2017-08-01 12:30:58 +00:00
|
|
|
{
|
|
|
|
linearUpper = m_linearLimits.m_upperLimit;
|
|
|
|
}
|
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
void setAngularLowerLimit(const btVector3& angularLower)
|
|
|
|
{
|
|
|
|
for (int i = 0; i < 3; i++)
|
2017-08-01 12:30:58 +00:00
|
|
|
m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
|
2019-01-03 13:26:51 +00:00
|
|
|
}
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
void getAngularLowerLimit(btVector3 & angularLower) const
|
2017-08-01 12:30:58 +00:00
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
for (int i = 0; i < 3; i++)
|
2017-08-01 12:30:58 +00:00
|
|
|
angularLower[i] = m_angularLimits[i].m_loLimit;
|
|
|
|
}
|
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
void setAngularUpperLimit(const btVector3& angularUpper)
|
|
|
|
{
|
|
|
|
for (int i = 0; i < 3; i++)
|
2017-08-01 12:30:58 +00:00
|
|
|
m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
|
2019-01-03 13:26:51 +00:00
|
|
|
}
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
void getAngularUpperLimit(btVector3 & angularUpper) const
|
2017-08-01 12:30:58 +00:00
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
for (int i = 0; i < 3; i++)
|
2017-08-01 12:30:58 +00:00
|
|
|
angularUpper[i] = m_angularLimits[i].m_hiLimit;
|
|
|
|
}
|
|
|
|
|
|
|
|
//! Retrieves the angular limit informacion
|
2019-01-03 13:26:51 +00:00
|
|
|
btRotationalLimitMotor* getRotationalLimitMotor(int index)
|
|
|
|
{
|
|
|
|
return &m_angularLimits[index];
|
|
|
|
}
|
|
|
|
|
|
|
|
//! Retrieves the limit informacion
|
|
|
|
btTranslationalLimitMotor* getTranslationalLimitMotor()
|
|
|
|
{
|
|
|
|
return &m_linearLimits;
|
|
|
|
}
|
|
|
|
|
|
|
|
//first 3 are linear, next 3 are angular
|
|
|
|
void setLimit(int axis, btScalar lo, btScalar hi)
|
|
|
|
{
|
|
|
|
if (axis < 3)
|
|
|
|
{
|
|
|
|
m_linearLimits.m_lowerLimit[axis] = lo;
|
|
|
|
m_linearLimits.m_upperLimit[axis] = hi;
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2017-08-01 12:30:58 +00:00
|
|
|
lo = btNormalizeAngle(lo);
|
|
|
|
hi = btNormalizeAngle(hi);
|
2019-01-03 13:26:51 +00:00
|
|
|
m_angularLimits[axis - 3].m_loLimit = lo;
|
|
|
|
m_angularLimits[axis - 3].m_hiLimit = hi;
|
|
|
|
}
|
|
|
|
}
|
2017-08-01 12:30:58 +00:00
|
|
|
|
|
|
|
//! Test limit
|
|
|
|
/*!
|
|
|
|
- free means upper < lower,
|
|
|
|
- locked means upper == lower
|
|
|
|
- limited means upper > lower
|
|
|
|
- limitIndex: first 3 are linear, next 3 are angular
|
|
|
|
*/
|
2019-01-03 13:26:51 +00:00
|
|
|
bool isLimited(int limitIndex) const
|
|
|
|
{
|
|
|
|
if (limitIndex < 3)
|
|
|
|
{
|
2017-08-01 12:30:58 +00:00
|
|
|
return m_linearLimits.isLimited(limitIndex);
|
2019-01-03 13:26:51 +00:00
|
|
|
}
|
|
|
|
return m_angularLimits[limitIndex - 3].isLimited();
|
|
|
|
}
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
virtual void calcAnchorPos(void); // overridable
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
int get_limit_motor_info2(btRotationalLimitMotor * limot,
|
|
|
|
const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB,
|
|
|
|
btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
|
2017-08-01 12:30:58 +00:00
|
|
|
|
|
|
|
// access for UseFrameOffset
|
|
|
|
bool getUseFrameOffset() const { return m_useOffsetForConstraintFrame; }
|
|
|
|
void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
|
2019-01-03 13:26:51 +00:00
|
|
|
|
2017-08-01 12:30:58 +00:00
|
|
|
bool getUseLinearReferenceFrameA() const { return m_useLinearReferenceFrameA; }
|
|
|
|
void setUseLinearReferenceFrameA(bool linearReferenceFrameA) { m_useLinearReferenceFrameA = linearReferenceFrameA; }
|
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
|
2017-08-01 12:30:58 +00:00
|
|
|
///If no axis is provided, it uses the default axis for this constraint.
|
2019-01-03 13:26:51 +00:00
|
|
|
virtual void setParam(int num, btScalar value, int axis = -1);
|
2017-08-01 12:30:58 +00:00
|
|
|
///return the local value of parameter
|
2019-01-03 13:26:51 +00:00
|
|
|
virtual btScalar getParam(int num, int axis = -1) const;
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
void setAxis(const btVector3& axis1, const btVector3& axis2);
|
2017-08-01 12:30:58 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
virtual int getFlags() const
|
|
|
|
{
|
|
|
|
return m_flags;
|
2017-08-01 12:30:58 +00:00
|
|
|
}
|
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
virtual int calculateSerializeBufferSize() const;
|
2017-08-01 12:30:58 +00:00
|
|
|
|
|
|
|
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
2019-01-03 13:26:51 +00:00
|
|
|
virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
|
2017-08-01 12:30:58 +00:00
|
|
|
};
|
|
|
|
|
|
|
|
struct btGeneric6DofConstraintData
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
btTypedConstraintData m_typeConstraintData;
|
|
|
|
btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
2017-08-01 12:30:58 +00:00
|
|
|
btTransformFloatData m_rbBFrame;
|
2019-01-03 13:26:51 +00:00
|
|
|
|
|
|
|
btVector3FloatData m_linearUpperLimit;
|
|
|
|
btVector3FloatData m_linearLowerLimit;
|
|
|
|
|
|
|
|
btVector3FloatData m_angularUpperLimit;
|
|
|
|
btVector3FloatData m_angularLowerLimit;
|
|
|
|
|
|
|
|
int m_useLinearReferenceFrameA;
|
2017-08-01 12:30:58 +00:00
|
|
|
int m_useOffsetForConstraintFrame;
|
|
|
|
};
|
|
|
|
|
|
|
|
struct btGeneric6DofConstraintDoubleData2
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
btTypedConstraintDoubleData m_typeConstraintData;
|
|
|
|
btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
2017-08-01 12:30:58 +00:00
|
|
|
btTransformDoubleData m_rbBFrame;
|
2019-01-03 13:26:51 +00:00
|
|
|
|
|
|
|
btVector3DoubleData m_linearUpperLimit;
|
|
|
|
btVector3DoubleData m_linearLowerLimit;
|
|
|
|
|
|
|
|
btVector3DoubleData m_angularUpperLimit;
|
|
|
|
btVector3DoubleData m_angularLowerLimit;
|
|
|
|
|
|
|
|
int m_useLinearReferenceFrameA;
|
2017-08-01 12:30:58 +00:00
|
|
|
int m_useOffsetForConstraintFrame;
|
|
|
|
};
|
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
SIMD_FORCE_INLINE int btGeneric6DofConstraint::calculateSerializeBufferSize() const
|
2017-08-01 12:30:58 +00:00
|
|
|
{
|
|
|
|
return sizeof(btGeneric6DofConstraintData2);
|
|
|
|
}
|
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
///fills the dataBuffer and returns the struct name (and 0 on failure)
|
|
|
|
SIMD_FORCE_INLINE const char* btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
|
2017-08-01 12:30:58 +00:00
|
|
|
{
|
|
|
|
btGeneric6DofConstraintData2* dof = (btGeneric6DofConstraintData2*)dataBuffer;
|
2019-01-03 13:26:51 +00:00
|
|
|
btTypedConstraint::serialize(&dof->m_typeConstraintData, serializer);
|
2017-08-01 12:30:58 +00:00
|
|
|
|
|
|
|
m_frameInA.serialize(dof->m_rbAFrame);
|
|
|
|
m_frameInB.serialize(dof->m_rbBFrame);
|
|
|
|
|
|
|
|
int i;
|
2019-01-03 13:26:51 +00:00
|
|
|
for (i = 0; i < 3; i++)
|
2017-08-01 12:30:58 +00:00
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
dof->m_angularLowerLimit.m_floats[i] = m_angularLimits[i].m_loLimit;
|
|
|
|
dof->m_angularUpperLimit.m_floats[i] = m_angularLimits[i].m_hiLimit;
|
2017-08-01 12:30:58 +00:00
|
|
|
dof->m_linearLowerLimit.m_floats[i] = m_linearLimits.m_lowerLimit[i];
|
|
|
|
dof->m_linearUpperLimit.m_floats[i] = m_linearLimits.m_upperLimit[i];
|
|
|
|
}
|
2019-01-03 13:26:51 +00:00
|
|
|
|
|
|
|
dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA ? 1 : 0;
|
2017-08-01 12:30:58 +00:00
|
|
|
dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;
|
|
|
|
|
|
|
|
return btGeneric6DofConstraintDataName;
|
|
|
|
}
|
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
#endif //BT_GENERIC_6DOF_CONSTRAINT_H
|