mirror of
https://github.com/TorqueGameEngines/Torque3D.git
synced 2026-04-24 22:05:40 +00:00
Bullet Library v2.81
This commit is contained in:
LuisAntonRebollo
parent
64fef8b2ad
commit
1eb94f4828
462 changed files with 59613 additions and 8036 deletions
|
|
@ -28,14 +28,10 @@ http://gimpact.sf.net
|
|||
|
||||
|
||||
#define D6_USE_OBSOLETE_METHOD false
|
||||
#define D6_USE_FRAME_OFFSET true
|
||||
|
||||
|
||||
|
||||
btGeneric6DofConstraint::btGeneric6DofConstraint()
|
||||
:btTypedConstraint(D6_CONSTRAINT_TYPE),
|
||||
m_useLinearReferenceFrameA(true),
|
||||
m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
|
@ -44,13 +40,31 @@ btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody&
|
|||
, m_frameInA(frameInA)
|
||||
, m_frameInB(frameInB),
|
||||
m_useLinearReferenceFrameA(useLinearReferenceFrameA),
|
||||
m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
|
||||
m_flags(0),
|
||||
m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
|
||||
{
|
||||
|
||||
calculateTransforms();
|
||||
}
|
||||
|
||||
|
||||
|
||||
btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
|
||||
: btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB),
|
||||
m_frameInB(frameInB),
|
||||
m_useLinearReferenceFrameA(useLinearReferenceFrameB),
|
||||
m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
|
||||
m_flags(0),
|
||||
m_useSolveConstraintObsolete(false)
|
||||
{
|
||||
///not providing rigidbody A means implicitly using worldspace for body A
|
||||
m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
|
||||
calculateTransforms();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
#define GENERIC_D6_DISABLE_WARMSTARTING 1
|
||||
|
||||
|
||||
|
|
@ -116,12 +130,20 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value)
|
|||
{
|
||||
m_currentLimit = 1;//low limit violation
|
||||
m_currentLimitError = test_value - m_loLimit;
|
||||
if(m_currentLimitError>SIMD_PI)
|
||||
m_currentLimitError-=SIMD_2_PI;
|
||||
else if(m_currentLimitError<-SIMD_PI)
|
||||
m_currentLimitError+=SIMD_2_PI;
|
||||
return 1;
|
||||
}
|
||||
else if (test_value> m_hiLimit)
|
||||
{
|
||||
m_currentLimit = 2;//High limit violation
|
||||
m_currentLimitError = test_value - m_hiLimit;
|
||||
if(m_currentLimitError>SIMD_PI)
|
||||
m_currentLimitError-=SIMD_2_PI;
|
||||
else if(m_currentLimitError<-SIMD_PI)
|
||||
m_currentLimitError+=SIMD_2_PI;
|
||||
return 2;
|
||||
};
|
||||
|
||||
|
|
@ -134,7 +156,7 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value)
|
|||
|
||||
btScalar btRotationalLimitMotor::solveAngularLimits(
|
||||
btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
|
||||
btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB)
|
||||
btRigidBody * body0, btRigidBody * body1 )
|
||||
{
|
||||
if (needApplyTorques()==false) return 0.0f;
|
||||
|
||||
|
|
@ -144,7 +166,7 @@ btScalar btRotationalLimitMotor::solveAngularLimits(
|
|||
//current error correction
|
||||
if (m_currentLimit!=0)
|
||||
{
|
||||
target_velocity = -m_ERP*m_currentLimitError/(timeStep);
|
||||
target_velocity = -m_stopERP*m_currentLimitError/(timeStep);
|
||||
maxMotorForce = m_maxLimitForce;
|
||||
}
|
||||
|
||||
|
|
@ -152,10 +174,8 @@ btScalar btRotationalLimitMotor::solveAngularLimits(
|
|||
|
||||
// current velocity difference
|
||||
|
||||
btVector3 angVelA;
|
||||
bodyA.getAngularVelocity(angVelA);
|
||||
btVector3 angVelB;
|
||||
bodyB.getAngularVelocity(angVelB);
|
||||
btVector3 angVelA = body0->getAngularVelocity();
|
||||
btVector3 angVelB = body1->getAngularVelocity();
|
||||
|
||||
btVector3 vel_diff;
|
||||
vel_diff = angVelA-angVelB;
|
||||
|
|
@ -203,12 +223,8 @@ btScalar btRotationalLimitMotor::solveAngularLimits(
|
|||
|
||||
btVector3 motorImp = clippedMotorImpulse * axis;
|
||||
|
||||
//body0->applyTorqueImpulse(motorImp);
|
||||
//body1->applyTorqueImpulse(-motorImp);
|
||||
|
||||
bodyA.applyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
|
||||
bodyB.applyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
|
||||
|
||||
body0->applyTorqueImpulse(motorImp);
|
||||
body1->applyTorqueImpulse(-motorImp);
|
||||
|
||||
return clippedMotorImpulse;
|
||||
|
||||
|
|
@ -257,8 +273,8 @@ int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_valu
|
|||
btScalar btTranslationalLimitMotor::solveLinearAxis(
|
||||
btScalar timeStep,
|
||||
btScalar jacDiagABInv,
|
||||
btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
|
||||
btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
|
||||
btRigidBody& body1,const btVector3 &pointInA,
|
||||
btRigidBody& body2,const btVector3 &pointInB,
|
||||
int limit_index,
|
||||
const btVector3 & axis_normal_on_a,
|
||||
const btVector3 & anchorPos)
|
||||
|
|
@ -270,10 +286,8 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
|
|||
btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
|
||||
|
||||
btVector3 vel1;
|
||||
bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
|
||||
btVector3 vel2;
|
||||
bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
|
||||
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
btScalar rel_vel = axis_normal_on_a.dot(vel);
|
||||
|
|
@ -326,16 +340,10 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
|
|||
normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
|
||||
|
||||
btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
|
||||
//body1.applyImpulse( impulse_vector, rel_pos1);
|
||||
//body2.applyImpulse(-impulse_vector, rel_pos2);
|
||||
|
||||
btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a);
|
||||
btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a);
|
||||
bodyA.applyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
|
||||
bodyB.applyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
|
||||
|
||||
|
||||
body1.applyImpulse( impulse_vector, rel_pos1);
|
||||
body2.applyImpulse(-impulse_vector, rel_pos2);
|
||||
|
||||
|
||||
|
||||
return normalImpulse;
|
||||
}
|
||||
|
|
@ -384,6 +392,22 @@ void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,cons
|
|||
m_calculatedTransformB = transB * m_frameInB;
|
||||
calculateLinearInfo();
|
||||
calculateAngleInfo();
|
||||
if(m_useOffsetForConstraintFrame)
|
||||
{ // get weight factors depending on masses
|
||||
btScalar miA = getRigidBodyA().getInvMass();
|
||||
btScalar miB = getRigidBodyB().getInvMass();
|
||||
m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
|
||||
btScalar miS = miA + miB;
|
||||
if(miS > btScalar(0.f))
|
||||
{
|
||||
m_factA = miB / miS;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_factA = btScalar(0.5f);
|
||||
}
|
||||
m_factB = btScalar(1.0f) - m_factA;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -544,43 +568,59 @@ void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
|
|||
|
||||
void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
|
||||
{
|
||||
getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(),m_rbA.getAngularVelocity(), m_rbB.getAngularVelocity());
|
||||
btAssert(!m_useSolveConstraintObsolete);
|
||||
|
||||
const btTransform& transA = m_rbA.getCenterOfMassTransform();
|
||||
const btTransform& transB = m_rbB.getCenterOfMassTransform();
|
||||
const btVector3& linVelA = m_rbA.getLinearVelocity();
|
||||
const btVector3& linVelB = m_rbB.getLinearVelocity();
|
||||
const btVector3& angVelA = m_rbA.getAngularVelocity();
|
||||
const btVector3& angVelB = m_rbB.getAngularVelocity();
|
||||
|
||||
if(m_useOffsetForConstraintFrame)
|
||||
{ // for stability better to solve angular limits first
|
||||
int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
|
||||
setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
|
||||
}
|
||||
else
|
||||
{ // leave old version for compatibility
|
||||
int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
|
||||
setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
void btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
|
||||
{
|
||||
|
||||
btAssert(!m_useSolveConstraintObsolete);
|
||||
|
||||
//prepare constraint
|
||||
calculateTransforms(transA,transB);
|
||||
|
||||
|
||||
int i;
|
||||
//test linear limits
|
||||
for(i = 0; i < 3; i++)
|
||||
{
|
||||
if(m_linearLimits.needApplyForce(i))
|
||||
{
|
||||
|
||||
}
|
||||
}
|
||||
//test angular limits
|
||||
for (i=0;i<3 ;i++ )
|
||||
{
|
||||
if(testAngularLimitMotor(i))
|
||||
{
|
||||
|
||||
}
|
||||
testAngularLimitMotor(i);
|
||||
}
|
||||
|
||||
int row = setLinearLimits(info,transA,transB,linVelA,linVelB,angVelA,angVelB);
|
||||
setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
|
||||
if(m_useOffsetForConstraintFrame)
|
||||
{ // for stability better to solve angular limits first
|
||||
int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
|
||||
setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
|
||||
}
|
||||
else
|
||||
{ // leave old version for compatibility
|
||||
int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
|
||||
setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
|
||||
int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
|
||||
{
|
||||
int row = 0;
|
||||
// int row = 0;
|
||||
//solve linear limits
|
||||
btRotationalLimitMotor limot;
|
||||
for (int i=0;i<3 ;i++ )
|
||||
|
|
@ -593,7 +633,6 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info,const btTra
|
|||
limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
|
||||
limot.m_damping = m_linearLimits.m_damping;
|
||||
limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
|
||||
limot.m_ERP = m_linearLimits.m_restitution;
|
||||
limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
|
||||
limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
|
||||
limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
|
||||
|
|
@ -601,9 +640,25 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info,const btTra
|
|||
limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
|
||||
limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
|
||||
btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
|
||||
row += get_limit_motor_info2(&limot,
|
||||
transA,transB,linVelA,linVelB,angVelA,angVelB
|
||||
, info, row, axis, 0);
|
||||
int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT);
|
||||
limot.m_normalCFM = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
|
||||
limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
|
||||
limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
|
||||
if(m_useOffsetForConstraintFrame)
|
||||
{
|
||||
int indx1 = (i + 1) % 3;
|
||||
int indx2 = (i + 2) % 3;
|
||||
int rotAllowed = 1; // rotations around orthos to current axis
|
||||
if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
|
||||
{
|
||||
rotAllowed = 0;
|
||||
}
|
||||
row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
|
||||
}
|
||||
else
|
||||
{
|
||||
row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
return row;
|
||||
|
|
@ -621,10 +676,21 @@ int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_o
|
|||
if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
|
||||
{
|
||||
btVector3 axis = d6constraint->getAxis(i);
|
||||
row += get_limit_motor_info2(
|
||||
d6constraint->getRotationalLimitMotor(i),
|
||||
transA,transB,linVelA,linVelB,angVelA,angVelB,
|
||||
info,row,axis,1);
|
||||
int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT);
|
||||
if(!(flags & BT_6DOF_FLAGS_CFM_NORM))
|
||||
{
|
||||
m_angularLimits[i].m_normalCFM = info->cfm[0];
|
||||
}
|
||||
if(!(flags & BT_6DOF_FLAGS_CFM_STOP))
|
||||
{
|
||||
m_angularLimits[i].m_stopCFM = info->cfm[0];
|
||||
}
|
||||
if(!(flags & BT_6DOF_FLAGS_ERP_STOP))
|
||||
{
|
||||
m_angularLimits[i].m_stopERP = info->erp;
|
||||
}
|
||||
row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
|
||||
transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -633,66 +699,6 @@ int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_o
|
|||
|
||||
|
||||
|
||||
void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
|
||||
{
|
||||
if (m_useSolveConstraintObsolete)
|
||||
{
|
||||
|
||||
|
||||
m_timeStep = timeStep;
|
||||
|
||||
//calculateTransforms();
|
||||
|
||||
int i;
|
||||
|
||||
// linear
|
||||
|
||||
btVector3 pointInA = m_calculatedTransformA.getOrigin();
|
||||
btVector3 pointInB = m_calculatedTransformB.getOrigin();
|
||||
|
||||
btScalar jacDiagABInv;
|
||||
btVector3 linear_axis;
|
||||
for (i=0;i<3;i++)
|
||||
{
|
||||
if (m_linearLimits.isLimited(i))
|
||||
{
|
||||
jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal();
|
||||
|
||||
if (m_useLinearReferenceFrameA)
|
||||
linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
|
||||
else
|
||||
linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
|
||||
|
||||
m_linearLimits.solveLinearAxis(
|
||||
m_timeStep,
|
||||
jacDiagABInv,
|
||||
m_rbA,bodyA,pointInA,
|
||||
m_rbB,bodyB,pointInB,
|
||||
i,linear_axis, m_AnchorPos);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// angular
|
||||
btVector3 angular_axis;
|
||||
btScalar angularJacDiagABInv;
|
||||
for (i=0;i<3;i++)
|
||||
{
|
||||
if (m_angularLimits[i].needApplyTorques())
|
||||
{
|
||||
|
||||
// get axis
|
||||
angular_axis = getAxis(i);
|
||||
|
||||
angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
|
||||
|
||||
m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,bodyA,&m_rbB,bodyB);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btGeneric6DofConstraint::updateRHS(btScalar timeStep)
|
||||
{
|
||||
|
|
@ -701,6 +707,15 @@ void btGeneric6DofConstraint::updateRHS(btScalar timeStep)
|
|||
}
|
||||
|
||||
|
||||
void btGeneric6DofConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
|
||||
{
|
||||
m_frameInA = frameA;
|
||||
m_frameInB = frameB;
|
||||
buildJacobian();
|
||||
calculateTransforms();
|
||||
}
|
||||
|
||||
|
||||
|
||||
btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
|
||||
{
|
||||
|
|
@ -758,7 +773,7 @@ void btGeneric6DofConstraint::calculateLinearInfo()
|
|||
int btGeneric6DofConstraint::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)
|
||||
btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
|
||||
{
|
||||
int srow = row * info->rowskip;
|
||||
int powered = limot->m_enableMotor;
|
||||
|
|
@ -778,18 +793,51 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
|
|||
}
|
||||
if((!rotational))
|
||||
{
|
||||
btVector3 ltd; // Linear Torque Decoupling vector
|
||||
btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
|
||||
ltd = c.cross(ax1);
|
||||
info->m_J1angularAxis[srow+0] = ltd[0];
|
||||
info->m_J1angularAxis[srow+1] = ltd[1];
|
||||
info->m_J1angularAxis[srow+2] = ltd[2];
|
||||
if (m_useOffsetForConstraintFrame)
|
||||
{
|
||||
btVector3 tmpA, tmpB, relA, relB;
|
||||
// get vector from bodyB to frameB in WCS
|
||||
relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
|
||||
// get its projection to constraint axis
|
||||
btVector3 projB = ax1 * relB.dot(ax1);
|
||||
// get vector directed from bodyB to constraint axis (and orthogonal to it)
|
||||
btVector3 orthoB = relB - projB;
|
||||
// same for bodyA
|
||||
relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
|
||||
btVector3 projA = ax1 * relA.dot(ax1);
|
||||
btVector3 orthoA = relA - projA;
|
||||
// get desired offset between frames A and B along constraint axis
|
||||
btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
|
||||
// desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
|
||||
btVector3 totalDist = projA + ax1 * desiredOffs - projB;
|
||||
// get offset vectors relA and relB
|
||||
relA = orthoA + totalDist * m_factA;
|
||||
relB = orthoB - totalDist * m_factB;
|
||||
tmpA = relA.cross(ax1);
|
||||
tmpB = relB.cross(ax1);
|
||||
if(m_hasStaticBody && (!rotAllowed))
|
||||
{
|
||||
tmpA *= m_factA;
|
||||
tmpB *= m_factB;
|
||||
}
|
||||
int i;
|
||||
for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
|
||||
for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
|
||||
} else
|
||||
{
|
||||
btVector3 ltd; // Linear Torque Decoupling vector
|
||||
btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
|
||||
ltd = c.cross(ax1);
|
||||
info->m_J1angularAxis[srow+0] = ltd[0];
|
||||
info->m_J1angularAxis[srow+1] = ltd[1];
|
||||
info->m_J1angularAxis[srow+2] = ltd[2];
|
||||
|
||||
c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
|
||||
ltd = -c.cross(ax1);
|
||||
info->m_J2angularAxis[srow+0] = ltd[0];
|
||||
info->m_J2angularAxis[srow+1] = ltd[1];
|
||||
info->m_J2angularAxis[srow+2] = ltd[2];
|
||||
c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
|
||||
ltd = -c.cross(ax1);
|
||||
info->m_J2angularAxis[srow+0] = ltd[0];
|
||||
info->m_J2angularAxis[srow+1] = ltd[1];
|
||||
info->m_J2angularAxis[srow+2] = ltd[2];
|
||||
}
|
||||
}
|
||||
// if we're limited low and high simultaneously, the joint motor is
|
||||
// ineffective
|
||||
|
|
@ -797,7 +845,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
|
|||
info->m_constraintError[srow] = btScalar(0.f);
|
||||
if (powered)
|
||||
{
|
||||
info->cfm[srow] = 0.0f;
|
||||
info->cfm[srow] = limot->m_normalCFM;
|
||||
if(!limit)
|
||||
{
|
||||
btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
|
||||
|
|
@ -806,7 +854,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
|
|||
limot->m_loLimit,
|
||||
limot->m_hiLimit,
|
||||
tag_vel,
|
||||
info->fps * info->erp);
|
||||
info->fps * limot->m_stopERP);
|
||||
info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
|
||||
info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
|
||||
info->m_upperLimit[srow] = limot->m_maxMotorForce;
|
||||
|
|
@ -814,7 +862,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
|
|||
}
|
||||
if(limit)
|
||||
{
|
||||
btScalar k = info->fps * limot->m_ERP;
|
||||
btScalar k = info->fps * limot->m_stopERP;
|
||||
if(!rotational)
|
||||
{
|
||||
info->m_constraintError[srow] += k * limot->m_currentLimitError;
|
||||
|
|
@ -823,7 +871,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
|
|||
{
|
||||
info->m_constraintError[srow] += -k * limot->m_currentLimitError;
|
||||
}
|
||||
info->cfm[srow] = 0.0f;
|
||||
info->cfm[srow] = limot->m_stopCFM;
|
||||
if (limot->m_loLimit == limot->m_hiLimit)
|
||||
{ // limited low and high simultaneously
|
||||
info->m_lowerLimit[srow] = -SIMD_INFINITY;
|
||||
|
|
@ -891,3 +939,126 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
|
|||
|
||||
|
||||
|
||||
|
||||
|
||||
///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.
|
||||
void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)
|
||||
{
|
||||
if((axis >= 0) && (axis < 3))
|
||||
{
|
||||
switch(num)
|
||||
{
|
||||
case BT_CONSTRAINT_STOP_ERP :
|
||||
m_linearLimits.m_stopERP[axis] = value;
|
||||
m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
|
||||
break;
|
||||
case BT_CONSTRAINT_STOP_CFM :
|
||||
m_linearLimits.m_stopCFM[axis] = value;
|
||||
m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
|
||||
break;
|
||||
case BT_CONSTRAINT_CFM :
|
||||
m_linearLimits.m_normalCFM[axis] = value;
|
||||
m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
|
||||
break;
|
||||
default :
|
||||
btAssertConstrParams(0);
|
||||
}
|
||||
}
|
||||
else if((axis >=3) && (axis < 6))
|
||||
{
|
||||
switch(num)
|
||||
{
|
||||
case BT_CONSTRAINT_STOP_ERP :
|
||||
m_angularLimits[axis - 3].m_stopERP = value;
|
||||
m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
|
||||
break;
|
||||
case BT_CONSTRAINT_STOP_CFM :
|
||||
m_angularLimits[axis - 3].m_stopCFM = value;
|
||||
m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
|
||||
break;
|
||||
case BT_CONSTRAINT_CFM :
|
||||
m_angularLimits[axis - 3].m_normalCFM = value;
|
||||
m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
|
||||
break;
|
||||
default :
|
||||
btAssertConstrParams(0);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
btAssertConstrParams(0);
|
||||
}
|
||||
}
|
||||
|
||||
///return the local value of parameter
|
||||
btScalar btGeneric6DofConstraint::getParam(int num, int axis) const
|
||||
{
|
||||
btScalar retVal = 0;
|
||||
if((axis >= 0) && (axis < 3))
|
||||
{
|
||||
switch(num)
|
||||
{
|
||||
case BT_CONSTRAINT_STOP_ERP :
|
||||
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
|
||||
retVal = m_linearLimits.m_stopERP[axis];
|
||||
break;
|
||||
case BT_CONSTRAINT_STOP_CFM :
|
||||
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
|
||||
retVal = m_linearLimits.m_stopCFM[axis];
|
||||
break;
|
||||
case BT_CONSTRAINT_CFM :
|
||||
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
|
||||
retVal = m_linearLimits.m_normalCFM[axis];
|
||||
break;
|
||||
default :
|
||||
btAssertConstrParams(0);
|
||||
}
|
||||
}
|
||||
else if((axis >=3) && (axis < 6))
|
||||
{
|
||||
switch(num)
|
||||
{
|
||||
case BT_CONSTRAINT_STOP_ERP :
|
||||
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
|
||||
retVal = m_angularLimits[axis - 3].m_stopERP;
|
||||
break;
|
||||
case BT_CONSTRAINT_STOP_CFM :
|
||||
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
|
||||
retVal = m_angularLimits[axis - 3].m_stopCFM;
|
||||
break;
|
||||
case BT_CONSTRAINT_CFM :
|
||||
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
|
||||
retVal = m_angularLimits[axis - 3].m_normalCFM;
|
||||
break;
|
||||
default :
|
||||
btAssertConstrParams(0);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
btAssertConstrParams(0);
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btGeneric6DofConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
|
||||
{
|
||||
btVector3 zAxis = axis1.normalized();
|
||||
btVector3 yAxis = axis2.normalized();
|
||||
btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
|
||||
|
||||
btTransform frameInW;
|
||||
frameInW.setIdentity();
|
||||
frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
|
||||
xAxis[1], yAxis[1], zAxis[1],
|
||||
xAxis[2], yAxis[2], zAxis[2]);
|
||||
|
||||
// now get constraint frame in local coordinate systems
|
||||
m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
|
||||
m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
|
||||
|
||||
calculateTransforms();
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue