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

Added getInfo1()/ getInfo2() code for joints to support the new SIMD constraint solver

Browse Source
This commit is contained in:
rponom
2008-12-25 02:13:46 +00:00
parent 4be20dc347
commit 1991bf3013
11 changed files with 1232 additions and 341 deletions
Download Patch File Download Diff File Expand all files Collapse all files

218
src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
View File

@@ -24,7 +24,8 @@ Written by: Marcus Hennix
btConeTwistConstraint::btConeTwistConstraint()
:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
m_useSolveConstraintObsolete(true)
m_useSolveConstraintObsolete(false)
//m_useSolveConstraintObsolete(true)
{
}
@@ -33,7 +34,8 @@ btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,
const btTransform& rbAFrame,const btTransform& rbBFrame)
:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
m_angularOnly(false),
m_useSolveConstraintObsolete(true)
m_useSolveConstraintObsolete(false)
// m_useSolveConstraintObsolete(true)
{
m_swingSpan1 = btScalar(1e30);
m_swingSpan2 = btScalar(1e30);
@@ -49,7 +51,8 @@ btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,
btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
m_angularOnly(false),
m_useSolveConstraintObsolete(true)
m_useSolveConstraintObsolete(false)
// m_useSolveConstraintObsolete(true)
{
m_rbBFrame = m_rbAFrame;
@@ -64,6 +67,7 @@ btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform&
}
//-----------------------------------------------------------------------------
void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
{
@@ -71,17 +75,124 @@ void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
{
info->m_numConstraintRows = 0;
info->nub = 0;
} else
}
else
{
btAssert(0);
info->m_numConstraintRows = 3;
info->nub = 3;
calcAngleInfo();
if(m_solveSwingLimit)
{
info->m_numConstraintRows++;
info->nub--;
}
if(m_solveTwistLimit)
{
info->m_numConstraintRows++;
info->nub--;
}
}
} // btConeTwistConstraint::getInfo1()
//-----------------------------------------------------------------------------
void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
{
btAssert(0);
btAssert(!m_useSolveConstraintObsolete);
//retrieve matrices
btTransform body0_trans;
body0_trans = m_rbA.getCenterOfMassTransform();
btTransform body1_trans;
body1_trans = m_rbB.getCenterOfMassTransform();
// set jacobian
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[info->rowskip+1] = 1;
info->m_J1linearAxis[2*info->rowskip+2] = 1;
btVector3 a1 = body0_trans.getBasis() * m_rbAFrame.getOrigin();
{
btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
btVector3 a1neg = -a1;
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
btVector3 a2 = body1_trans.getBasis() * m_rbBFrame.getOrigin();
{
btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
// set right hand side
btScalar k = info->fps * info->erp;
int j;
for (j=0; j<3; j++)
{
info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
}
int row = 3;
int srow = row * info->rowskip;
btVector3 ax1;
// angular limits
if(m_solveSwingLimit)
{
ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor;
btScalar *J1 = info->m_J1angularAxis;
btScalar *J2 = info->m_J2angularAxis;
J1[srow+0] = ax1[0];
J1[srow+1] = ax1[1];
J1[srow+2] = ax1[2];
J2[srow+0] = -ax1[0];
J2[srow+1] = -ax1[1];
J2[srow+2] = -ax1[2];
btScalar k = info->fps * m_biasFactor;
info->m_constraintError[srow] = k * m_swingCorrection;
info->cfm[srow] = 0.0f;
// m_swingCorrection is always positive or 0
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
srow += info->rowskip;
}
if(m_solveTwistLimit)
{
ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor;
btScalar *J1 = info->m_J1angularAxis;
btScalar *J2 = info->m_J2angularAxis;
J1[srow+0] = ax1[0];
J1[srow+1] = ax1[1];
J1[srow+2] = ax1[2];
J2[srow+0] = -ax1[0];
J2[srow+1] = -ax1[1];
J2[srow+2] = -ax1[2];
btScalar k = info->fps * m_biasFactor;
info->m_constraintError[srow] = k * m_twistCorrection;
info->cfm[srow] = 0.0f;
if(m_twistSpan > 0.0f)
{
if(m_twistCorrection > 0.0f)
{
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else
{
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = 0;
}
}
else
{
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
srow += info->rowskip;
}
}
//-----------------------------------------------------------------------------
void btConeTwistConstraint::buildJacobian()
{
@@ -227,6 +338,8 @@ void btConeTwistConstraint::buildJacobian()
}
}
//-----------------------------------------------------------------------------
void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
{
if (m_useSolveConstraintObsolete)
@@ -315,8 +428,101 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
}
//-----------------------------------------------------------------------------
void btConeTwistConstraint::updateRHS(btScalar timeStep)
{
(void)timeStep;
}
//-----------------------------------------------------------------------------
void btConeTwistConstraint::calcAngleInfo()
{
m_swingCorrection = btScalar(0.);
m_twistLimitSign = btScalar(0.);
m_solveTwistLimit = false;
m_solveSwingLimit = false;
btVector3 b1Axis1,b1Axis2,b1Axis3;
btVector3 b2Axis1,b2Axis2;
b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
btScalar swx=btScalar(0.),swy = btScalar(0.);
btScalar thresh = btScalar(10.);
btScalar fact;
// Get Frame into world space
if (m_swingSpan1 >= btScalar(0.05f))
{
b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis2);
swing1 = btAtan2Fast(swy, swx);
fact = (swy*swy + swx*swx) * thresh * thresh;
fact = fact / (fact + btScalar(1.0));
swing1 *= fact;
}
if (m_swingSpan2 >= btScalar(0.05f))
{
b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis3);
swing2 = btAtan2Fast(swy, swx);
fact = (swy*swy + swx*swx) * thresh * thresh;
fact = fact / (fact + btScalar(1.0));
swing2 *= fact;
}
btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);
btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);
btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq;
if (EllipseAngle > 1.0f)
{
m_swingCorrection = EllipseAngle-1.0f;
m_solveSwingLimit = true;
// Calculate necessary axis & factors
m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
m_swingAxis.normalize();
btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
m_swingAxis *= swingAxisSign;
}
// Twist limits
if (m_twistSpan >= btScalar(0.))
{
btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
btVector3 TwistRef = quatRotate(rotationArc,b2Axis2);
btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
// btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.);
if (twist <= -m_twistSpan*lockedFreeFactor)
{
m_twistCorrection = -(twist + m_twistSpan);
m_solveTwistLimit = true;
m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
m_twistAxis.normalize();
m_twistAxis *= -1.0f;
}
else if (twist > m_twistSpan*lockedFreeFactor)
{
m_twistCorrection = (twist - m_twistSpan);
m_solveTwistLimit = true;
m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
m_twistAxis.normalize();
}
}
} // btConeTwistConstraint::calcAngleInfo()
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------

3
src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
View File

@@ -127,6 +127,9 @@ public:
return m_twistLimitSign;
}
void calcAngleInfo();
};
#endif //CONETWISTCONSTRAINT_H

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

@@ -25,27 +25,35 @@ http://gimpact.sf.net
#include "LinearMath/btTransformUtil.h"
#include <new>
//-----------------------------------------------------------------------------
btGeneric6DofConstraint::btGeneric6DofConstraint()
:btTypedConstraint(D6_CONSTRAINT_TYPE),
m_useLinearReferenceFrameA(true),
m_useSolveConstraintObsolete(true)
m_useSolveConstraintObsolete(false)
//m_useSolveConstraintObsolete(true)
{
}
//-----------------------------------------------------------------------------
btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
, m_frameInA(frameInA)
, m_frameInB(frameInB),
m_useLinearReferenceFrameA(useLinearReferenceFrameA),
m_useSolveConstraintObsolete(true)
m_useSolveConstraintObsolete(false)
//m_useSolveConstraintObsolete(true)
{
}
//-----------------------------------------------------------------------------
#define GENERIC_D6_DISABLE_WARMSTARTING 1
//-----------------------------------------------------------------------------
btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
{
@@ -54,6 +62,8 @@ btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
return mat[i][j];
}
//-----------------------------------------------------------------------------
///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
@@ -63,9 +73,10 @@ bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
// // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
//
if (btGetMatrixElem(mat,2) < btScalar(1.0))
btScalar fi = btGetMatrixElem(mat,2);
if (fi < btScalar(1.0f))
{
if (btGetMatrixElem(mat,2) > btScalar(-1.0))
if (fi > btScalar(-1.0f))
{
xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
xyz[1] = btAsin(btGetMatrixElem(mat,2));
@@ -87,18 +98,12 @@ bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
xyz[1] = SIMD_HALF_PI;
xyz[2] = 0.0;
}
return false;
}
//////////////////////////// btRotationalLimitMotor ////////////////////////////////////
int btRotationalLimitMotor::testLimitValue(btScalar test_value)
{
if(m_loLimit>m_hiLimit)
@@ -125,6 +130,7 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value)
}
//-----------------------------------------------------------------------------
btScalar btRotationalLimitMotor::solveAngularLimits(
btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
@@ -211,7 +217,43 @@ btScalar btRotationalLimitMotor::solveAngularLimits(
//////////////////////////// End btRotationalLimitMotor ////////////////////////////////////
//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value)
{
btScalar loLimit = m_lowerLimit[limitIndex];
btScalar hiLimit = m_upperLimit[limitIndex];
if(loLimit > hiLimit)
{
m_currentLimit[limitIndex] = 0;//Free from violation
m_currentLimitError[limitIndex] = btScalar(0.f);
return 0;
}
if (test_value < loLimit)
{
m_currentLimit[limitIndex] = 2;//low limit violation
m_currentLimitError[limitIndex] = test_value - loLimit;
return 2;
}
else if (test_value> hiLimit)
{
m_currentLimit[limitIndex] = 1;//High limit violation
m_currentLimitError[limitIndex] = test_value - hiLimit;
return 1;
};
m_currentLimit[limitIndex] = 0;//Free from violation
m_currentLimitError[limitIndex] = btScalar(0.f);
return 0;
} // btTranslationalLimitMotor::testLimitValue()
//-----------------------------------------------------------------------------
btScalar btTranslationalLimitMotor::solveLinearAxis(
btScalar timeStep,
btScalar jacDiagABInv,
@@ -300,19 +342,10 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
void btGeneric6DofConstraint::calculateAngleInfo()
{
btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
// in euler angle mode we do not actually constrain the angular velocity
// along the axes axis[0] and axis[2] (although we do use axis[1]) :
//
@@ -327,7 +360,6 @@ void btGeneric6DofConstraint::calculateAngleInfo()
// GetInfo1 then take the derivative. to prove this for angle[2] it is
// easier to take the euler rate expression for d(angle[2])/dt with respect
// to the components of w and set that to 0.
btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
@@ -335,28 +367,23 @@ void btGeneric6DofConstraint::calculateAngleInfo()
m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
// if(m_debugDrawer)
// {
//
// char buff[300];
// sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
// m_calculatedAxisAngleDiff[0],
// m_calculatedAxisAngleDiff[1],
// m_calculatedAxisAngleDiff[2]);
// m_debugDrawer->reportErrorWarning(buff);
// }
m_calculatedAxis[0].normalize();
m_calculatedAxis[1].normalize();
m_calculatedAxis[2].normalize();
}
//-----------------------------------------------------------------------------
void btGeneric6DofConstraint::calculateTransforms()
{
m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
calculateLinearInfo();
calculateAngleInfo();
}
//-----------------------------------------------------------------------------
void btGeneric6DofConstraint::buildLinearJacobian(
btJacobianEntry & jacLinear,const btVector3 & normalWorld,
@@ -374,6 +401,8 @@ void btGeneric6DofConstraint::buildLinearJacobian(
m_rbB.getInvMass());
}
//-----------------------------------------------------------------------------
void btGeneric6DofConstraint::buildAngularJacobian(
btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
{
@@ -385,15 +414,18 @@ void btGeneric6DofConstraint::buildAngularJacobian(
}
//-----------------------------------------------------------------------------
bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
{
btScalar angle = m_calculatedAxisAngleDiff[axis_index];
//test limits
m_angularLimits[axis_index].testLimitValue(angle);
return m_angularLimits[axis_index].needApplyTorques();
}
//-----------------------------------------------------------------------------
void btGeneric6DofConstraint::buildJacobian()
{
if (m_useSolveConstraintObsolete)
@@ -452,6 +484,8 @@ void btGeneric6DofConstraint::buildJacobian()
}
}
//-----------------------------------------------------------------------------
void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
{
if (m_useSolveConstraintObsolete)
@@ -462,260 +496,80 @@ void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
{
//prepare constraint
calculateTransforms();
info->m_numConstraintRows = 3;
info->nub = 3;//??
info->m_numConstraintRows = 0;
info->nub = 6;
int i;
//test linear limits
for(i = 0; i < 3; i++)
{
if(m_linearLimits.needApplyForce(i))
{
info->m_numConstraintRows++;
info->nub--;
}
}
//test angular limits
for (int i=0;i<3 ;i++ )
{
//if(i==2) continue;
if(testAngularLimitMotor(i))
{
info->m_numConstraintRows++;
info->nub--;
}
}
}
}
//-----------------------------------------------------------------------------
void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
{
btAssert(!m_useSolveConstraintObsolete);
int row = setLinearLimits(info);
setAngularLimits(info, row);
}
//-----------------------------------------------------------------------------
int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
{
btGeneric6DofConstraint * d6constraint = this;
//retrieve matrices
btTransform body0_trans;
body0_trans = m_rbA.getCenterOfMassTransform();
btTransform body1_trans;
body1_trans = m_rbB.getCenterOfMassTransform();
// anchor points in global coordinates with respect to body PORs.
int s = info->rowskip;
// set jacobian
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[s+1] = 1;
info->m_J1linearAxis[2*s+2] = 1;
/*for (int i=0;i<3;i++)
int row = 0;
//solve linear limits
btRotationalLimitMotor limot;
bool useBasisBodyB = (m_rbB.getInvMass() == btScalar(0.f));
for (int i=0;i<3 ;i++ )
{
if (m_useLinearReferenceFrameA)
if(m_linearLimits.needApplyForce(i))
{ // re-use rotational motor code
limot.m_bounce = btScalar(0.f);
limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
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];
limot.m_maxLimitForce = btScalar(0.f);
limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
btVector3 axis;
if(useBasisBodyB)
{
btVector3* linear_axis = (btVector3* )&info->m_J1linearAxis[s*i];
*linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
axis = m_calculatedTransformB.getBasis().getColumn(i);
}
else
{
btVector3* linear_axis = (btVector3* )&info->m_J1linearAxis[s*i];
*linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
axis = m_calculatedTransformA.getBasis().getColumn(i);
}
row += get_limit_motor_info2(&limot, &m_rbA, &m_rbB, info, row, axis, 0);
}
}
*/
btVector3 a1,a2;
a1 = body0_trans.getBasis()*d6constraint->getFrameOffsetA().getOrigin();
{
btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
btVector3 a1neg = -a1;
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
return row;
}
/*info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[s+1] = -1;
info->m_J2linearAxis[2*s+2] = -1;
*/
a2 = body1_trans.getBasis()*d6constraint->getFrameOffsetB().getOrigin();
{
btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
// set right hand side
btScalar k = info->fps * info->erp;
for (int j=0; j<3; j++)
{
info->m_constraintError[s*j] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
}
return 3;
}
/*! \pre testLimitValue must be called on limot*/
int bt_get_limit_motor_info2(
btRotationalLimitMotor * limot,
btRigidBody * body0, btRigidBody * body1,
btTypedConstraint::btConstraintInfo2 *info, int row, btVector3& ax1, int rotational)
{
int srow = row * info->rowskip;
// if the joint is powered, or has joint limits, add in the extra row
int powered = limot->m_enableMotor;
int limit = limot->m_currentLimit;
if (powered || limit)
{
btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
btScalar *J2 = rotational ? info->m_J2angularAxis : 0;//info->m_J2linearAxis;
J1[srow+0] = ax1[0];
J1[srow+1] = ax1[1];
J1[srow+2] = ax1[2];
if (body1)
{
J2[srow+0] = -ax1[0];
J2[srow+1] = -ax1[1];
J2[srow+2] = -ax1[2];
}
// linear limot torque decoupling step:
//
// if this is a linear limot (e.g. from a slider), we have to be careful
// that the linear constraint forces (+/- ax1) applied to the two bodies
// do not create a torque couple. in other words, the points that the
// constraint force is applied at must lie along the same ax1 axis.
// a torque couple will result in powered or limited slider-jointed free
// bodies from gaining angular momentum.
// the solution used here is to apply the constraint forces at the point
// halfway between the body centers. there is no penalty (other than an
// extra tiny bit of computation) in doing this adjustment. note that we
// only need to do this if the constraint connects two bodies.
btVector3 ltd; // Linear Torque Decoupling vector (a torque)
if (!rotational && body1)
{
btVector3 c;
c[0]=btScalar(0.5)*(body1->getCenterOfMassPosition()[0]
-body0->getCenterOfMassPosition()[0]);
c[1]=btScalar(0.5)*(body1->getCenterOfMassPosition()[1]
-body0->getCenterOfMassPosition()[1]);
c[2]=btScalar(0.5)*(body1->getCenterOfMassPosition()[2]
-body0->getCenterOfMassPosition()[2]);
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];
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;
if (powered)
{
info->cfm[srow] = 0.0f;//limot->m_normalCFM;
if (! limit)
{
info->m_constraintError[srow] = 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_ERP;
info->m_constraintError[srow] = -k * limot->m_currentLimitError;
info->cfm[srow] = 0.0f;//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)
{
// low limit
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else
{
// high limit
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 = body0->getAngularVelocity().dot(ax1);
if (body1)
vel -= body1->getAngularVelocity().dot(ax1);
}
else
{
vel = body0->getLinearVelocity().dot(ax1);
if (body1)
vel -= body1->getLinearVelocity().dot(ax1);
}
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{
// low limit
if (vel < 0)
{
btScalar newc = -limot->m_bounce* vel;
if (newc > info->m_constraintError[srow])
info->m_constraintError[srow] = newc;
}
}
else
{
// high limit - all those computations are reversed
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::setAngularLimits(btConstraintInfo2 *info, int row_offset)
{
@@ -728,7 +582,7 @@ int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_o
if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
{
btVector3 axis = d6constraint->getAxis(i);
row += bt_get_limit_motor_info2(
row += get_limit_motor_info2(
d6constraint->getRotationalLimitMotor(i),
&m_rbA,
&m_rbB,
@@ -739,8 +593,7 @@ int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_o
return row;
}
///////////////////limit motor support
//-----------------------------------------------------------------------------
void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
{
@@ -801,22 +654,30 @@ void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolv
}
}
//-----------------------------------------------------------------------------
void btGeneric6DofConstraint::updateRHS(btScalar timeStep)
{
(void)timeStep;
}
//-----------------------------------------------------------------------------
btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
{
return m_calculatedAxis[axis_index];
}
//-----------------------------------------------------------------------------
btScalar btGeneric6DofConstraint::getAngle(int axis_index) const
{
return m_calculatedAxisAngleDiff[axis_index];
}
//-----------------------------------------------------------------------------
void btGeneric6DofConstraint::calcAnchorPos(void)
{
btScalar imA = m_rbA.getInvMass();
@@ -836,3 +697,160 @@ void btGeneric6DofConstraint::calcAnchorPos(void)
return;
} // btGeneric6DofConstraint::calcAnchorPos()
//-----------------------------------------------------------------------------
void btGeneric6DofConstraint::calculateLinearInfo()
{
m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
for(int i = 0; i < 3; i++)
{
m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
}
} // btGeneric6DofConstraint::calculateLinearInfo()
//-----------------------------------------------------------------------------
int btGeneric6DofConstraint::get_limit_motor_info2(
btRotationalLimitMotor * limot,
btRigidBody * body0, btRigidBody * body1,
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];
}
// linear limot torque decoupling step:
//
// if this is a linear limot (e.g. from a slider), we have to be careful
// that the linear constraint forces (+/- ax1) applied to the two bodies
// do not create a torque couple. in other words, the points that the
// constraint force is applied at must lie along the same ax1 axis.
// a torque couple will result in powered or limited slider-jointed free
// bodies from gaining angular momentum.
// the solution used here is to apply the constraint forces at the point
// at center of mass of two bodies. there is no penalty (other than an
// extra tiny bit of computation) in doing this adjustment. note that we
// only need to do this if the constraint connects two bodies.
btVector3 ltd; // Linear Torque Decoupling vector (a torque)
if(!rotational)
{
btVector3 c = body1->getCenterOfMassPosition() - body0->getCenterOfMassPosition();
ltd = c.cross(ax1);
btScalar miA = body0->getInvMass();
btScalar miB = body1->getInvMass();
btScalar miS = miA + miB;
btScalar factA, factB;
if(miS > btScalar(0.f))
{
factA = miB / miS;
}
else
{
factA = btScalar(0.5f);
}
if(factA > 0.99f) factA = 0.99f;
if(factA < 0.01f) factA = 0.01f;
factB = btScalar(1.0f) - factA;
info->m_J1angularAxis[srow+0] = factA * ltd[0];
info->m_J1angularAxis[srow+1] = factA * ltd[1];
info->m_J1angularAxis[srow+2] = factA * ltd[2];
info->m_J2angularAxis[srow+0] = factB * ltd[0];
info->m_J2angularAxis[srow+1] = factB * ltd[1];
info->m_J2angularAxis[srow+2] = factB * 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;
if (powered)
{
info->cfm[srow] = 0.0f;
if(!limit)
{
info->m_constraintError[srow] = 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_ERP;
info->m_constraintError[srow] = -k * limot->m_currentLimitError;
info->cfm[srow] = 0.0f;
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 = body0->getAngularVelocity().dot(ax1);
if (body1)
vel -= body1->getAngularVelocity().dot(ax1);
}
else
{
vel = body0->getLinearVelocity().dot(ax1);
if (body1)
vel -= body1->getLinearVelocity().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;
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------

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

@@ -30,6 +30,8 @@ http://gimpact.sf.net
class btRigidBody;
//! Rotation Limit structure for generic joints
class btRotationalLimitMotor
{
@@ -92,7 +94,7 @@ public:
//! Is limited
bool isLimited()
{
if(m_loLimit>=m_hiLimit) return false;
if(m_loLimit > m_hiLimit) return false;
return true;
}
@@ -112,7 +114,6 @@ public:
//! apply the correction impulses for two bodies
btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btSolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB);
};
@@ -129,6 +130,11 @@ public:
btScalar m_damping;//!< Damping for linear limit
btScalar m_restitution;//! Bounce parameter for linear 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
int m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
btTranslationalLimitMotor()
{
@@ -139,6 +145,12 @@ public:
m_limitSoftness = 0.7f;
m_damping = btScalar(1.0f);
m_restitution = btScalar(0.5f);
for(int i=0; i < 3; i++)
{
m_enableMotor[i] = false;
m_targetVelocity[i] = btScalar(0.f);
m_maxMotorForce[i] = btScalar(0.f);
}
}
btTranslationalLimitMotor(const btTranslationalLimitMotor & other )
@@ -150,6 +162,12 @@ public:
m_limitSoftness = other.m_limitSoftness ;
m_damping = other.m_damping;
m_restitution = other.m_restitution;
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
@@ -163,6 +181,12 @@ public:
{
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, btScalar test_value);
btScalar solveLinearAxis(
@@ -247,6 +271,7 @@ protected:
btTransform m_calculatedTransformB;
btVector3 m_calculatedAxisAngleDiff;
btVector3 m_calculatedAxis[3];
btVector3 m_calculatedLinearDiff;
btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
@@ -272,6 +297,8 @@ protected:
void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW);
// tests linear limits
void calculateLinearInfo();
//! calcs the euler angles between the two bodies.
void calculateAngleInfo();
@@ -443,6 +470,11 @@ public:
virtual void calcAnchorPos(void); // overridable
int get_limit_motor_info2( btRotationalLimitMotor * limot,
btRigidBody * body0, btRigidBody * body1,
btConstraintInfo2 *info, int row, btVector3& ax1, int rotational);
};
#endif //GENERIC_6DOF_CONSTRAINT_H

287
src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
View File

@@ -21,21 +21,26 @@ subject to the following restrictions:
#include <new>
#include "btSolverBody.h"
//-----------------------------------------------------------------------------
btHingeConstraint::btHingeConstraint()
: btTypedConstraint (HINGE_CONSTRAINT_TYPE),
m_enableAngularMotor(false),
m_useSolveConstraintObsolete(true)
m_useSolveConstraintObsolete(false)
//m_useSolveConstraintObsolete(true)
{
}
//-----------------------------------------------------------------------------
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
btVector3& axisInA,btVector3& axisInB)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
m_angularOnly(false),
m_enableAngularMotor(false),
m_useSolveConstraintObsolete(true)
m_useSolveConstraintObsolete(false)
// m_useSolveConstraintObsolete(true)
{
m_rbAFrame.getOrigin() = pivotInA;
@@ -78,10 +83,12 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
}
//-----------------------------------------------------------------------------
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(true)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false),
m_useSolveConstraintObsolete(false)
// m_useSolveConstraintObsolete(true)
{
// since no frame is given, assume this to be zero angle and just pick rb transform axis
@@ -115,12 +122,15 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,
m_solveLimit = false;
}
//-----------------------------------------------------------------------------
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB,
const btTransform& rbAFrame, const btTransform& rbBFrame)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
m_angularOnly(false),
m_enableAngularMotor(false),
m_useSolveConstraintObsolete(true)
m_useSolveConstraintObsolete(false)
//m_useSolveConstraintObsolete(true)
{
// flip axis
m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
@@ -136,13 +146,14 @@ m_useSolveConstraintObsolete(true)
m_solveLimit = false;
}
//-----------------------------------------------------------------------------
btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
m_angularOnly(false),
m_enableAngularMotor(false),
m_useSolveConstraintObsolete(true)
m_useSolveConstraintObsolete(false)
//m_useSolveConstraintObsolete(true)
{
///not providing rigidbody B means implicitly using worldspace for body B
@@ -162,6 +173,8 @@ m_useSolveConstraintObsolete(true)
m_solveLimit = false;
}
//-----------------------------------------------------------------------------
void btHingeConstraint::buildJacobian()
{
if (m_useSolveConstraintObsolete)
@@ -233,34 +246,11 @@ void btHingeConstraint::buildJacobian()
m_rbA.getInvInertiaDiagLocal(),
m_rbB.getInvInertiaDiagLocal());
// Compute limit information
btScalar hingeAngle = getHingeAngle();
//set bias, sign, clear accumulator
m_correction = btScalar(0.);
m_limitSign = btScalar(0.);
m_solveLimit = false;
// clear accumulator
m_accLimitImpulse = btScalar(0.);
// if (m_lowerLimit < m_upperLimit)
if (m_lowerLimit <= m_upperLimit)
{
// if (hingeAngle <= m_lowerLimit*m_limitSoftness)
if (hingeAngle <= m_lowerLimit)
{
m_correction = (m_lowerLimit - hingeAngle);
m_limitSign = 1.0f;
m_solveLimit = true;
}
// else if (hingeAngle >= m_upperLimit*m_limitSoftness)
else if (hingeAngle >= m_upperLimit)
{
m_correction = m_upperLimit - hingeAngle;
m_limitSign = -1.0f;
m_solveLimit = true;
}
}
// test angular limit
testLimit();
//Compute K = J*W*J' for hinge axis
btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
@@ -270,6 +260,7 @@ void btHingeConstraint::buildJacobian()
}
}
//-----------------------------------------------------------------------------
void btHingeConstraint::getInfo1(btConstraintInfo1* info)
{
@@ -277,17 +268,207 @@ void btHingeConstraint::getInfo1 (btConstraintInfo1* info)
{
info->m_numConstraintRows = 0;
info->nub = 0;
} else
}
else
{
btAssert(0);
info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
info->nub = 1;
//prepare constraint
testLimit();
if(getSolveLimit() || getEnableAngularMotor())
{
info->m_numConstraintRows++; // limit 3rd anguar as well
info->nub--;
}
}
} // btHingeConstraint::getInfo1 ()
//-----------------------------------------------------------------------------
void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
{
btAssert(0);
btAssert(!m_useSolveConstraintObsolete);
int i, s = info->rowskip;
// transforms in world space
btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame;
// pivot point
btVector3 pivotAInW = trA.getOrigin();
btVector3 pivotBInW = trB.getOrigin();
// linear (all fixed)
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[s + 1] = 1;
info->m_J1linearAxis[2 * s + 2] = 1;
btVector3 a1 = pivotAInW - m_rbA.getCenterOfMassTransform().getOrigin();
{
btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s);
btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s);
btVector3 a1neg = -a1;
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin();
{
btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s);
btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s);
a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
// linear RHS
btScalar k = info->fps * info->erp;
for(i = 0; i < 3; i++)
{
info->m_constraintError[i * s] = k * (pivotBInW[i] - pivotAInW[i]);
}
// make rotations around X and Y equal
// the hinge axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the hinge axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the hinge axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
// get hinge axis (Z)
btVector3 ax1 = trA.getBasis().getColumn(2);
// get 2 orthos to hinge axis (X, Y)
btVector3 p = trA.getBasis().getColumn(0);
btVector3 q = trA.getBasis().getColumn(1);
// set the two hinge angular rows
int s3 = 3 * info->rowskip;
int s4 = 4 * info->rowskip;
info->m_J1angularAxis[s3 + 0] = p[0];
info->m_J1angularAxis[s3 + 1] = p[1];
info->m_J1angularAxis[s3 + 2] = p[2];
info->m_J1angularAxis[s4 + 0] = q[0];
info->m_J1angularAxis[s4 + 1] = q[1];
info->m_J1angularAxis[s4 + 2] = q[2];
info->m_J2angularAxis[s3 + 0] = -p[0];
info->m_J2angularAxis[s3 + 1] = -p[1];
info->m_J2angularAxis[s3 + 2] = -p[2];
info->m_J2angularAxis[s4 + 0] = -q[0];
info->m_J2angularAxis[s4 + 1] = -q[1];
info->m_J2angularAxis[s4 + 2] = -q[2];
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the hinge back into alignment.
// if ax1,ax2 are the unit length hinge axes as computed from body1 and
// body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if `theta' is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
btVector3 ax2 = - trB.getBasis().getColumn(2);
btVector3 u = ax1.cross(ax2);
info->m_constraintError[s3] = k * u.dot(p);
info->m_constraintError[s4] = k * u.dot(q);
// check angular limits
int nrow = 4; // last filled row
int srow;
btScalar limit_err = btScalar(0.0);
int limit = 0;
if(getSolveLimit())
{
limit_err = m_correction;
limit = (limit_err > btScalar(0.0)) ? 1 : 2;
}
// if the hinge has joint limits or motor, add in the extra row
int powered = 0;
if(getEnableAngularMotor())
{
powered = 1;
}
if(limit || powered)
{
nrow++;
srow = nrow * info->rowskip;
info->m_J1angularAxis[srow+0] = ax1[0];
info->m_J1angularAxis[srow+1] = ax1[1];
info->m_J1angularAxis[srow+2] = ax1[2];
info->m_J2angularAxis[srow+0] = -ax1[0];
info->m_J2angularAxis[srow+1] = -ax1[1];
info->m_J2angularAxis[srow+2] = -ax1[2];
btScalar lostop = getLowerLimit();
btScalar histop = getUpperLimit();
if(limit && (lostop == histop))
{ // the joint motor is ineffective
powered = 0;
}
if(powered)
{
info->cfm[srow] = btScalar(0.0);
btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp);
info->m_constraintError[srow] = mot_fact * m_motorTargetVelocity;
info->m_lowerLimit[srow] = - m_maxMotorImpulse;
info->m_upperLimit[srow] = m_maxMotorImpulse;
}
if(limit)
{
k = info->fps * info->erp;
info->m_constraintError[srow] = k * limit_err;
info->cfm[srow] = btScalar(0.0);
if(lostop == histop)
{
// limited low and high simultaneously
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else if(limit == 1)
{ // low limit
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else
{ // high limit
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = 0;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
btScalar bounce = m_relaxationFactor;
if(bounce > btScalar(0.0))
{
btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
vel -= m_rbB.getAngularVelocity().dot(ax1);
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if(limit == 1)
{ // low limit
if(vel < 0)
{
btScalar newc = -bounce * vel;
if(newc > info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if(vel > 0)
{
btScalar newc = -bounce * vel;
if(newc < info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
}
info->m_constraintError[srow] *= m_biasFactor;
} // if(limit)
} // if angular limit or powered
}
//-----------------------------------------------------------------------------
void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
{
@@ -435,12 +616,16 @@ void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody
}
//-----------------------------------------------------------------------------
void btHingeConstraint::updateRHS(btScalar timeStep)
{
(void)timeStep;
}
//-----------------------------------------------------------------------------
btScalar btHingeConstraint::getHingeAngle()
{
const btVector3 refAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
@@ -450,3 +635,33 @@ btScalar btHingeConstraint::getHingeAngle()
return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1) );
}
//-----------------------------------------------------------------------------
void btHingeConstraint::testLimit()
{
// Compute limit information
m_hingeAngle = getHingeAngle();
m_correction = btScalar(0.);
m_limitSign = btScalar(0.);
m_solveLimit = false;
if (m_lowerLimit <= m_upperLimit)
{
if (m_hingeAngle <= m_lowerLimit)
{
m_correction = (m_lowerLimit - m_hingeAngle);
m_limitSign = 1.0f;
m_solveLimit = true;
}
else if (m_hingeAngle >= m_upperLimit)
{
m_correction = m_upperLimit - m_hingeAngle;
m_limitSign = -1.0f;
m_solveLimit = true;
}
}
return;
} // btHingeConstraint::testLimit()
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------

3
src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
View File

@@ -53,6 +53,7 @@ public:
btScalar m_correction;
btScalar m_accLimitImpulse;
btScalar m_hingeAngle;
bool m_angularOnly;
bool m_enableAngularMotor;
@@ -127,6 +128,8 @@ public:
btScalar getHingeAngle();
void testLimit();
const btTransform& getAFrame() { return m_rbAFrame; };
const btTransform& getBFrame() { return m_rbBFrame; };

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

@@ -68,7 +68,9 @@ void btSliderConstraint::initParams()
btSliderConstraint::btSliderConstraint()
:btTypedConstraint(SLIDER_CONSTRAINT_TYPE),
m_useLinearReferenceFrameA(true)
m_useLinearReferenceFrameA(true),
m_useSolveConstraintObsolete(false)
// m_useSolveConstraintObsolete(true)
{
initParams();
} // btSliderConstraint::btSliderConstraint()
@@ -79,7 +81,9 @@ btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const
: btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB)
, m_frameInA(frameInA)
, m_frameInB(frameInB),
m_useLinearReferenceFrameA(useLinearReferenceFrameA)
m_useLinearReferenceFrameA(useLinearReferenceFrameA),
m_useSolveConstraintObsolete(false)
// m_useSolveConstraintObsolete(true)
{
initParams();
} // btSliderConstraint::btSliderConstraint()
@@ -88,6 +92,10 @@ btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const
void btSliderConstraint::buildJacobian()
{
if (!m_useSolveConstraintObsolete)
{
return;
}
if(m_useLinearReferenceFrameA)
{
buildJacobianInt(m_rbA, m_rbB, m_frameInA, m_frameInB);
@@ -156,18 +164,347 @@ void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, co
//-----------------------------------------------------------------------------
void btSliderConstraint::getInfo1(btConstraintInfo1* info)
{
if (m_useSolveConstraintObsolete)
{
info->m_numConstraintRows = 0;
info->nub = 0;
}
else
{
info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
info->nub = 2;
//prepare constraint
calculateTransforms();
testLinLimits();
if(getSolveLinLimit() || getPoweredLinMotor())
{
info->m_numConstraintRows++; // limit 3rd linear as well
info->nub--;
}
testAngLimits();
if(getSolveAngLimit() || getPoweredAngMotor())
{
info->m_numConstraintRows++; // limit 3rd angular as well
info->nub--;
}
}
} // btSliderConstraint::getInfo1()
//-----------------------------------------------------------------------------
void btSliderConstraint::getInfo2(btConstraintInfo2* info)
{
btAssert(0);
}
btAssert(!m_useSolveConstraintObsolete);
int i, s = info->rowskip;
const btTransform& trA = getCalculatedTransformA();
const btTransform& trB = getCalculatedTransformB();
btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
// make rotations around Y and Z equal
// the slider axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the slider axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the slider axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
// get slider axis (X)
btVector3 ax1 = trA.getBasis().getColumn(0);
// get 2 orthos to slider axis (Y, Z)
btVector3 p = trA.getBasis().getColumn(1);
btVector3 q = trA.getBasis().getColumn(2);
// set the two slider rows
info->m_J1angularAxis[0] = p[0];
info->m_J1angularAxis[1] = p[1];
info->m_J1angularAxis[2] = p[2];
info->m_J1angularAxis[s+0] = q[0];
info->m_J1angularAxis[s+1] = q[1];
info->m_J1angularAxis[s+2] = q[2];
info->m_J2angularAxis[0] = -p[0];
info->m_J2angularAxis[1] = -p[1];
info->m_J2angularAxis[2] = -p[2];
info->m_J2angularAxis[s+0] = -q[0];
info->m_J2angularAxis[s+1] = -q[1];
info->m_J2angularAxis[s+2] = -q[2];
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the slider back into alignment.
// if ax1,ax2 are the unit length slider axes as computed from body1 and
// body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if "theta" is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
btVector3 ax2 = trB.getBasis().getColumn(0);
btVector3 u = ax1.cross(ax2);
info->m_constraintError[0] = k * u.dot(p);
info->m_constraintError[s] = k * u.dot(q);
// pull out pos and R for both bodies. also get the connection
// vector c = pos2-pos1.
// next two rows. we want: vel2 = vel1 + w1 x c ... but this would
// result in three equations, so we project along the planespace vectors
// so that sliding along the slider axis is disregarded. for symmetry we
// also consider rotation around center of mass of two bodies (factA and factB).
btTransform bodyA_trans = m_rbA.getCenterOfMassTransform();
btTransform bodyB_trans = m_rbB.getCenterOfMassTransform();
int s2 = 2 * s, s3 = 3 * s;
btVector3 c;
btScalar miA = m_rbA.getInvMass();
btScalar miB = m_rbB.getInvMass();
btScalar miS = miA + miB;
btScalar factA, factB;
if(miS > btScalar(0.f))
{
factA = miB / miS;
}
else
{
factA = btScalar(0.5f);
}
if(factA > 0.99f) factA = 0.99f;
if(factA < 0.01f) factA = 0.01f;
factB = btScalar(1.0f) - factA;
c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
btVector3 tmp = c.cross(p);
for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
tmp = c.cross(q);
for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
// compute two elements of right hand side. we want to align the offset
// point (in body 2's frame) with the center of body 1.
btVector3 ofs; // offset point in global coordinates
ofs = trB.getOrigin() - trA.getOrigin();
k = info->fps * info->erp * getSoftnessOrthoLin();
info->m_constraintError[s2] = k * p.dot(ofs);
info->m_constraintError[s3] = k * q.dot(ofs);
int nrow = 3; // last filled row
int srow;
// check linear limits linear
btScalar limit_err = btScalar(0.0);
int limit = 0;
if(getSolveLinLimit())
{
limit_err = getLinDepth() * signFact;
limit = (limit_err > btScalar(0.0)) ? 2 : 1;
}
int powered = 0;
if(getPoweredLinMotor())
{
powered = 1;
}
// if the slider has joint limits or motor, add in the extra row
if (limit || powered)
{
nrow++;
srow = nrow * info->rowskip;
info->m_J1linearAxis[srow+0] = ax1[0];
info->m_J1linearAxis[srow+1] = ax1[1];
info->m_J1linearAxis[srow+2] = ax1[2];
// linear torque decoupling step:
//
// we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
// do not create a torque couple. in other words, the points that the
// constraint force is applied at must lie along the same ax1 axis.
// a torque couple will result in limited slider-jointed free
// bodies from gaining angular momentum.
// the solution used here is to apply the constraint forces at the center of mass of the two bodies
btVector3 ltd; // Linear Torque Decoupling vector (a torque)
// c = btScalar(0.5) * c;
ltd = c.cross(ax1);
info->m_J1angularAxis[srow+0] = factA*ltd[0];
info->m_J1angularAxis[srow+1] = factA*ltd[1];
info->m_J1angularAxis[srow+2] = factA*ltd[2];
info->m_J2angularAxis[srow+0] = factB*ltd[0];
info->m_J2angularAxis[srow+1] = factB*ltd[1];
info->m_J2angularAxis[srow+2] = factB*ltd[2];
// right-hand part
btScalar lostop = getLowerLinLimit();
btScalar histop = getUpperLinLimit();
if(limit && (lostop == histop))
{ // the joint motor is ineffective
powered = 0;
}
info->m_constraintError[srow] = 0.;
info->m_lowerLimit[srow] = 0.;
info->m_upperLimit[srow] = 0.;
if(powered)
{
info->cfm[nrow] = btScalar(0.0);
btScalar tag_vel = getTargetLinMotorVelocity();
btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * info->erp);
info->m_constraintError[srow] += mot_fact * getTargetLinMotorVelocity();
info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
}
if(limit)
{
k = info->fps * info->erp;
info->m_constraintError[srow] += k * limit_err;
info->cfm[srow] = btScalar(0.0); // stop_cfm;
if(lostop == histop)
{ // limited low and high simultaneously
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else if(limit == 1)
{ // low limit
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = 0;
}
else
{ // high limit
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
if(bounce > btScalar(0.0))
{
btScalar vel = m_rbA.getLinearVelocity().dot(ax1);
vel -= m_rbB.getLinearVelocity().dot(ax1);
vel *= signFact;
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if(limit == 1)
{ // low limit
if(vel < 0)
{
btScalar newc = -bounce * vel;
if (newc > info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if(vel > 0)
{
btScalar newc = -bounce * vel;
if(newc < info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
}
info->m_constraintError[srow] *= getSoftnessLimLin();
} // if(limit)
} // if linear limit
// check angular limits
limit_err = btScalar(0.0);
limit = 0;
if(getSolveAngLimit())
{
limit_err = getAngDepth();
limit = (limit_err > btScalar(0.0)) ? 1 : 2;
}
// if the slider has joint limits, add in the extra row
powered = 0;
if(getPoweredAngMotor())
{
powered = 1;
}
if(limit || powered)
{
nrow++;
srow = nrow * info->rowskip;
info->m_J1angularAxis[srow+0] = ax1[0];
info->m_J1angularAxis[srow+1] = ax1[1];
info->m_J1angularAxis[srow+2] = ax1[2];
info->m_J2angularAxis[srow+0] = -ax1[0];
info->m_J2angularAxis[srow+1] = -ax1[1];
info->m_J2angularAxis[srow+2] = -ax1[2];
btScalar lostop = getLowerAngLimit();
btScalar histop = getUpperAngLimit();
if(limit && (lostop == histop))
{ // the joint motor is ineffective
powered = 0;
}
if(powered)
{
info->cfm[srow] = btScalar(0.0);
btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp);
info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
}
if(limit)
{
k = info->fps * info->erp;
info->m_constraintError[srow] += k * limit_err;
info->cfm[srow] = btScalar(0.0); // stop_cfm;
if(lostop == histop)
{
// limited low and high simultaneously
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else if(limit == 1)
{ // low limit
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else
{ // high limit
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = 0;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng());
if(bounce > btScalar(0.0))
{
btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
vel -= m_rbB.getAngularVelocity().dot(ax1);
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if(limit == 1)
{ // low limit
if(vel < 0)
{
btScalar newc = -bounce * vel;
if(newc > info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if(vel > 0)
{
btScalar newc = -bounce * vel;
if(newc < info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
}
info->m_constraintError[srow] *= getSoftnessLimAng();
} // if(limit)
} // if angular limit or powered
} // btSliderConstraint::getInfo2()
//-----------------------------------------------------------------------------
void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
{
if (m_useSolveConstraintObsolete)
{
m_timeStep = timeStep;
if(m_useLinearReferenceFrameA)
@@ -178,6 +515,7 @@ void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBod
{
solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA);
}
}
} // btSliderConstraint::solveConstraint()
//-----------------------------------------------------------------------------
@@ -371,7 +709,7 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& body
//-----------------------------------------------------------------------------
void btSliderConstraint::calculateTransforms(void){
if(m_useLinearReferenceFrameA)
if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
{
m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
@@ -384,7 +722,14 @@ void btSliderConstraint::calculateTransforms(void){
m_realPivotAInW = m_calculatedTransformA.getOrigin();
m_realPivotBInW = m_calculatedTransformB.getOrigin();
m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
{
m_delta = m_realPivotBInW - m_realPivotAInW;
}
else
{
m_delta = m_realPivotAInW - m_realPivotBInW;
}
m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
btVector3 normalWorld;
int i;
@@ -427,7 +772,6 @@ void btSliderConstraint::testLinLimits(void)
//-----------------------------------------------------------------------------
void btSliderConstraint::testAngLimits(void)
{
m_angDepth = btScalar(0.);
@@ -438,6 +782,7 @@ void btSliderConstraint::testAngLimits(void)
const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));
m_angPos = rot;
if(rot < m_lowerAngLimit)
{
m_angDepth = rot - m_lowerAngLimit;
@@ -451,11 +796,8 @@ void btSliderConstraint::testAngLimits(void)
}
} // btSliderConstraint::testAngLimits()
//-----------------------------------------------------------------------------
btVector3 btSliderConstraint::getAncorInA(void)
{
btVector3 ancorInA;

5
src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
View File

@@ -46,6 +46,8 @@ class btRigidBody;
class btSliderConstraint : public btTypedConstraint
{
protected:
///for backwards compatibility during the transition to 'getInfo/getInfo2'
bool m_useSolveConstraintObsolete;
btTransform m_frameInA;
btTransform m_frameInB;
// use frameA fo define limits, if true
@@ -104,6 +106,7 @@ protected:
btVector3 m_relPosB;
btScalar m_linPos;
btScalar m_angPos;
btScalar m_angDepth;
btScalar m_kAngle;
@@ -201,6 +204,7 @@ public:
btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; }
btScalar getLinearPos() { return m_linPos; }
// access for ODE solver
bool getSolveLinLimit() { return m_solveLinLim; }
btScalar getLinDepth() { return m_depth[0]; }
@@ -212,6 +216,7 @@ public:
// shared code used by ODE solver
void calculateTransforms(void);
void testLinLimits(void);
void testLinLimits2(btConstraintInfo2* info);
void testAngLimits(void);
// access for PE Solver
btVector3 getAncorInA(void);

53
src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
View File

@@ -54,3 +54,56 @@ m_appliedImpulse(btScalar(0.))
}
//-----------------------------------------------------------------------------
btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
{
if(lowLim > uppLim)
{
return btScalar(1.0f);
}
else if(lowLim == uppLim)
{
return btScalar(0.0f);
}
btScalar lim_fact = btScalar(1.0f);
btScalar delta_max = vel / timeFact;
if(delta_max > btScalar(0.0f))
{
if((pos > lowLim) && (pos < (lowLim + delta_max)))
{
lim_fact = (pos - lowLim) / delta_max;
}
else if(pos < lowLim)
{
lim_fact = btScalar(0.0f);
}
else
{
lim_fact = btScalar(1.0f);
}
}
else if(delta_max < btScalar(0.0f))
{
if((pos < uppLim) && (pos > (uppLim + delta_max)))
{
lim_fact = (pos - uppLim) / delta_max;
}
else if(pos > uppLim)
{
lim_fact = btScalar(0.0f);
}
else
{
lim_fact = btScalar(1.0f);
}
}
else
{
lim_fact = btScalar(0.0f);
}
return lim_fact;
} // btTypedConstraint::getMotorFactor()

4
src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
View File

@@ -21,6 +21,9 @@ class btRigidBody;
#include "btSolverConstraint.h"
struct btSolverBody;
enum btTypedConstraintType
{
POINT2POINT_CONSTRAINT_TYPE,
@@ -103,6 +106,7 @@ public:
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) = 0;
btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
const btRigidBody& getRigidBodyA() const
{

10
src/BulletDynamics/Dynamics/btRigidBody.h
View File

@@ -233,6 +233,16 @@ public:
m_totalForce += force;
}
const btVector3& getTotalForce()
{
return m_totalForce;
};
const btVector3& getTotalTorque()
{
return m_totalTorque;
};
const btVector3& getInvInertiaDiagLocal()
{
return m_invInertiaLocal;