Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

remove a little bit of code duplication, small fix (nrow should be srow), and initialize return value to SIMD_INFINITY for nonvalid parameters/axis.

Browse Source
This commit is contained in:
erwin.coumans
2010-02-04 23:06:25 +00:00
parent 5eeb624585
commit 2e094c62ac
4 changed files with 52 additions and 171 deletions
Download Patch File Download Diff File Expand all files Collapse all files

156
src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
View File

@@ -631,7 +631,7 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, c
{
rotAllowed = 0;
}
row += get_limit_motor_info2UsingFrameOffset(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
}
else
{
@@ -801,140 +801,6 @@ 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)
{
int srow = row * info->rowskip;
int powered = limot->m_enableMotor;
int limit = limot->m_currentLimit;
if (powered || limit)
{ // if the joint is powered, or has joint limits, add in the extra row
btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
btScalar *J2 = rotational ? info->m_J2angularAxis : 0;
J1[srow+0] = ax1[0];
J1[srow+1] = ax1[1];
J1[srow+2] = ax1[2];
if(rotational)
{
J2[srow+0] = -ax1[0];
J2[srow+1] = -ax1[1];
J2[srow+2] = -ax1[2];
}
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];
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
if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
info->m_constraintError[srow] = btScalar(0.f);
if (powered)
{
info->cfm[srow] = limot->m_normalCFM;
if(!limit)
{
btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
btScalar mot_fact = getMotorFactor( limot->m_currentPosition,
limot->m_loLimit,
limot->m_hiLimit,
tag_vel,
info->fps * info->erp);
info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
info->m_upperLimit[srow] = limot->m_maxMotorForce;
}
}
if(limit)
{
btScalar k = info->fps * limot->m_stopERP;
if(!rotational)
{
info->m_constraintError[srow] += k * limot->m_currentLimitError;
}
else
{
info->m_constraintError[srow] += -k * limot->m_currentLimitError;
}
info->cfm[srow] = limot->m_stopCFM;
if (limot->m_loLimit == limot->m_hiLimit)
{ // limited low and high simultaneously
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else
{
if (limit == 1)
{
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else
{
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = 0;
}
// deal with bounce
if (limot->m_bounce > 0)
{
// calculate joint velocity
btScalar vel;
if (rotational)
{
vel = angVelA.dot(ax1);
//make sure that if no body -> angVelB == zero vec
// if (body1)
vel -= angVelB.dot(ax1);
}
else
{
vel = linVelA.dot(ax1);
//make sure that if no body -> angVelB == zero vec
// if (body1)
vel -= linVelB.dot(ax1);
}
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{
if (vel < 0)
{
btScalar newc = -limot->m_bounce* vel;
if (newc > info->m_constraintError[srow])
info->m_constraintError[srow] = newc;
}
}
else
{
if (vel > 0)
{
btScalar newc = -limot->m_bounce * vel;
if (newc < info->m_constraintError[srow])
info->m_constraintError[srow] = newc;
}
}
}
}
}
return 1;
}
else return 0;
}
int btGeneric6DofConstraint::get_limit_motor_info2UsingFrameOffset( 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)
{
@@ -955,6 +821,8 @@ int btGeneric6DofConstraint::get_limit_motor_info2UsingFrameOffset( btRotational
J2[srow+2] = -ax1[2];
}
if((!rotational))
{
if (m_useOffsetForConstraintFrame)
{
btVector3 tmpA, tmpB, relA, relB;
// get vector from bodyB to frameB in WCS
@@ -984,6 +852,21 @@ int btGeneric6DofConstraint::get_limit_motor_info2UsingFrameOffset( btRotational
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];
}
}
// if we're limited low and high simultaneously, the joint motor is
// ineffective
@@ -1084,6 +967,9 @@ int btGeneric6DofConstraint::get_limit_motor_info2UsingFrameOffset( btRotational
///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)

6
src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
View File

@@ -515,11 +515,7 @@ public:
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 get_limit_motor_info2UsingFrameOffset( 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);
btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
// access for UseFrameOffset
bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }

2
src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
View File

@@ -276,7 +276,7 @@ void btPoint2PointConstraint::setParam(int num, btScalar value, int axis)
///return the local value of parameter
btScalar btPoint2PointConstraint::getParam(int num, int axis) const
{
btScalar retVal;
btScalar retVal(SIMD_INFINITY);
if(axis != -1)
{
btAssertConstrParams(0);

7
src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
View File

@@ -507,7 +507,7 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
{
if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG)
{
info->cfm[nrow] = m_cfmDirAng;
info->cfm[srow] = m_cfmDirAng;
}
btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP);
info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
@@ -1216,9 +1216,8 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i
{
if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG)
{
info->cfm[nrow] = m_cfmDirAng;
info->cfm[srow] = m_cfmDirAng;
}
info->cfm[srow] = btScalar(0.0);
btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP);
info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
@@ -1365,7 +1364,7 @@ void btSliderConstraint::setParam(int num, btScalar value, int axis)
///return the local value of parameter
btScalar btSliderConstraint::getParam(int num, int axis) const
{
btScalar retVal;
btScalar retVal(SIMD_INFINITY);
switch(num)
{
case BT_CONSTRAINT_STOP_ERP :