Page MenuHome
Differential D1739 Diff 5899 extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

Changeset View

Standalone View

View Options

extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

Show All 39 Lines
#ifdef _BT_USE_CENTER_LIMIT_ #ifdef _BT_USE_CENTER_LIMIT_
m_limit(), m_limit(),
#endif #endif
m_angularOnly(false), m_angularOnly(false),
m_enableAngularMotor(false), m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA), m_useReferenceFrameA(useReferenceFrameA),
m_flags(0) m_flags(0),
m_normalCFM(0),
m_normalERP(0),
m_stopCFM(0),
m_stopERP(0)
{ {
m_rbAFrame.getOrigin() = pivotInA; m_rbAFrame.getOrigin() = pivotInA;
// since no frame is given, assume this to be zero angle and just pick rb transform axis // since no frame is given, assume this to be zero angle and just pick rb transform axis
btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
btVector3 rbAxisA2; btVector3 rbAxisA2;
btScalar projection = axisInA.dot(rbAxisA1); btScalar projection = axisInA.dot(rbAxisA1);
Show All 39 Lines
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),
#ifdef _BT_USE_CENTER_LIMIT_ #ifdef _BT_USE_CENTER_LIMIT_
m_limit(), m_limit(),
#endif #endif
m_angularOnly(false), m_enableAngularMotor(false), m_angularOnly(false), m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA), m_useReferenceFrameA(useReferenceFrameA),
m_flags(0) m_flags(0),
m_normalCFM(0),
m_normalERP(0),
m_stopCFM(0),
m_stopERP(0)
{ {
// since no frame is given, assume this to be zero angle and just pick rb transform axis // since no frame is given, assume this to be zero angle and just pick rb transform axis
// fixed axis in worldspace // fixed axis in worldspace
btVector3 rbAxisA1, rbAxisA2; btVector3 rbAxisA1, rbAxisA2;
btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
m_rbAFrame.getOrigin() = pivotInA; m_rbAFrame.getOrigin() = pivotInA;
Show All 33 Lines
#ifdef _BT_USE_CENTER_LIMIT_ #ifdef _BT_USE_CENTER_LIMIT_
m_limit(), m_limit(),
#endif #endif
m_angularOnly(false), m_angularOnly(false),
m_enableAngularMotor(false), m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA), m_useReferenceFrameA(useReferenceFrameA),
m_flags(0) m_flags(0),
m_normalCFM(0),
m_normalERP(0),
m_stopCFM(0),
m_stopERP(0)
{ {
#ifndef _BT_USE_CENTER_LIMIT_ #ifndef _BT_USE_CENTER_LIMIT_
//start with free //start with free
m_lowerLimit = btScalar(1.0f); m_lowerLimit = btScalar(1.0f);
m_upperLimit = btScalar(-1.0f); m_upperLimit = btScalar(-1.0f);
m_biasFactor = 0.3f; m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f; m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f; m_limitSoftness = 0.9f;
Show All 9 Lines
#ifdef _BT_USE_CENTER_LIMIT_ #ifdef _BT_USE_CENTER_LIMIT_
m_limit(), m_limit(),
#endif #endif
m_angularOnly(false), m_angularOnly(false),
m_enableAngularMotor(false), m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA), m_useReferenceFrameA(useReferenceFrameA),
m_flags(0) m_flags(0),
m_normalCFM(0),
m_normalERP(0),
m_stopCFM(0),
m_stopERP(0)
{ {
///not providing rigidbody B means implicitly using worldspace for body B ///not providing rigidbody B means implicitly using worldspace for body B
m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin()); m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
#ifndef _BT_USE_CENTER_LIMIT_ #ifndef _BT_USE_CENTER_LIMIT_
//start with free //start with free
m_lowerLimit = btScalar(1.0f); m_lowerLimit = btScalar(1.0f);
m_upperLimit = btScalar(-1.0f); m_upperLimit = btScalar(-1.0f);
▲ Show 20 Lines • Show All 91 Lines • ▼ Show 20 Linesvoid btHingeConstraint::buildJacobian()
} }
} }
#endif //__SPU__ #endif //__SPU__
static inline btScalar btNormalizeAnglePositive(btScalar angle)
{
return btFmod(btFmod(angle, btScalar(2.0*SIMD_PI)) + btScalar(2.0*SIMD_PI), btScalar(2.0*SIMD_PI));
}
static btScalar btShortestAngularDistance(btScalar accAngle, btScalar curAngle)
{
btScalar result = btNormalizeAngle(btNormalizeAnglePositive(btNormalizeAnglePositive(curAngle) -
btNormalizeAnglePositive(accAngle)));
return result;
}
static btScalar btShortestAngleUpdate(btScalar accAngle, btScalar curAngle)
{
btScalar tol(0.3);
btScalar result = btShortestAngularDistance(accAngle, curAngle);
if (btFabs(result) > tol)
return curAngle;
else
return accAngle + result;
return curAngle;
}
btScalar btHingeAccumulatedAngleConstraint::getAccumulatedHingeAngle()
{
btScalar hingeAngle = getHingeAngle();
m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,hingeAngle);
return m_accumulatedAngle;
}
void btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle(btScalar accAngle)
{
m_accumulatedAngle = accAngle;
}
void btHingeAccumulatedAngleConstraint::getInfo1(btConstraintInfo1* info)
{
//update m_accumulatedAngle
btScalar curHingeAngle = getHingeAngle();
m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,curHingeAngle);
btHingeConstraint::getInfo1(info);
}
void btHingeConstraint::getInfo1(btConstraintInfo1* info) void btHingeConstraint::getInfo1(btConstraintInfo1* info)
{ {
if (m_useSolveConstraintObsolete) if (m_useSolveConstraintObsolete)
{ {
info->m_numConstraintRows = 0; info->m_numConstraintRows = 0;
info->nub = 0; info->nub = 0;
} }
else else
{ {
info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
▲ Show 20 Lines • Show All 110 Lines • ▼ Show 20 Lines#endif //#if 0
btVector3 a2 = pivotBInW - transB.getOrigin(); btVector3 a2 = pivotBInW - transB.getOrigin();
{ {
btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip); btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip);
btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip); btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip);
a2.getSkewSymmetricMatrix(angular0,angular1,angular2); a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
} }
// linear RHS // linear RHS
btScalar k = info->fps * info->erp; btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp;
btScalar k = info->fps * normalErp;
if (!m_angularOnly) if (!m_angularOnly)
{ {
for(i = 0; i < 3; i++) for(i = 0; i < 3; i++)
{ {
info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]); info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]);
} }
} }
// make rotations around X and Y equal // make rotations around X and Y equal
▲ Show 20 Lines • Show All 80 Lines • ▼ Show 20 Lines if(limit || powered)
btScalar lostop = getLowerLimit(); btScalar lostop = getLowerLimit();
btScalar histop = getUpperLimit(); btScalar histop = getUpperLimit();
if(limit && (lostop == histop)) if(limit && (lostop == histop))
{ // the joint motor is ineffective { // the joint motor is ineffective
powered = 0; powered = 0;
} }
info->m_constraintError[srow] = btScalar(0.0f); info->m_constraintError[srow] = btScalar(0.0f);
btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp; btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
if(powered) if(powered)
{ {
if(m_flags & BT_HINGE_FLAGS_CFM_NORM) if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
{ {
info->cfm[srow] = m_normalCFM; info->cfm[srow] = m_normalCFM;
} }
btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP); btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
▲ Show 20 Lines • Show All 79 Lines • ▼ Show 20 Lines
void btHingeConstraint::updateRHS(btScalar timeStep) void btHingeConstraint::updateRHS(btScalar timeStep)
{ {
(void)timeStep; (void)timeStep;
} }
btScalar btHingeConstraint::getHingeAngle() btScalar btHingeConstraint::getHingeAngle()
{ {
return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
} }
btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB) btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB)
{ {
const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0); const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
▲ Show 20 Lines • Show All 176 Lines • ▼ Show 20 Lines#if 1
{ // to make constraint between static and dynamic objects more rigid { // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation // remove wA (or wB) from equation
tmpB *= factB; tmpB *= factB;
tmpA *= factA; tmpA *= factA;
} }
for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i]; for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i]; for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
btScalar k = info->fps * info->erp; btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM)? m_normalERP : info->erp;
btScalar k = info->fps * normalErp;
if (!m_angularOnly) if (!m_angularOnly)
{ {
for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i]; for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i]; for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i]; for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i];
for (i=0; i<3; i++) info->m_J2linearAxis[s0+i] = -p[i]; for (i=0; i<3; i++) info->m_J2linearAxis[s0+i] = -p[i];
▲ Show 20 Lines • Show All 41 Lines • ▼ Show 20 Lines#if 1
// = (erp*fps) * theta // = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta) // = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish, // ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so // theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2) // angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular // 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. // velocity to p and q to find the right hand side.
k = info->fps * info->erp; k = info->fps * normalErp;//??
btVector3 u = ax1A.cross(ax1B); btVector3 u = ax1A.cross(ax1B);
info->m_constraintError[s3] = k * u.dot(p); info->m_constraintError[s3] = k * u.dot(p);
info->m_constraintError[s4] = k * u.dot(q); info->m_constraintError[s4] = k * u.dot(q);
#endif #endif
// check angular limits // check angular limits
nrow = 4; // last filled row nrow = 4; // last filled row
int srow; int srow;
btScalar limit_err = btScalar(0.0); btScalar limit_err = btScalar(0.0);
Show All 28 Lines if(limit || powered)
btScalar lostop = getLowerLimit(); btScalar lostop = getLowerLimit();
btScalar histop = getUpperLimit(); btScalar histop = getUpperLimit();
if(limit && (lostop == histop)) if(limit && (lostop == histop))
{ // the joint motor is ineffective { // the joint motor is ineffective
powered = 0; powered = 0;
} }
info->m_constraintError[srow] = btScalar(0.0f); info->m_constraintError[srow] = btScalar(0.0f);
btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp; btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
if(powered) if(powered)
{ {
if(m_flags & BT_HINGE_FLAGS_CFM_NORM) if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
{ {
info->cfm[srow] = m_normalCFM; info->cfm[srow] = m_normalCFM;
} }
btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP); btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
▲ Show 20 Lines • Show All 84 Lines • ▼ Show 20 Lines switch(num)
case BT_CONSTRAINT_STOP_CFM : case BT_CONSTRAINT_STOP_CFM :
m_stopCFM = value; m_stopCFM = value;
m_flags |= BT_HINGE_FLAGS_CFM_STOP; m_flags |= BT_HINGE_FLAGS_CFM_STOP;
break; break;
case BT_CONSTRAINT_CFM : case BT_CONSTRAINT_CFM :
m_normalCFM = value; m_normalCFM = value;
m_flags |= BT_HINGE_FLAGS_CFM_NORM; m_flags |= BT_HINGE_FLAGS_CFM_NORM;
break; break;
case BT_CONSTRAINT_ERP:
m_normalERP = value;
m_flags |= BT_HINGE_FLAGS_ERP_NORM;
break;
default : default :
btAssertConstrParams(0); btAssertConstrParams(0);
} }
} }
else else
{ {
btAssertConstrParams(0); btAssertConstrParams(0);
} }
Show All 14 Lines switch(num)
case BT_CONSTRAINT_STOP_CFM : case BT_CONSTRAINT_STOP_CFM :
btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP); btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP);
retVal = m_stopCFM; retVal = m_stopCFM;
break; break;
case BT_CONSTRAINT_CFM : case BT_CONSTRAINT_CFM :
btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM); btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM);
retVal = m_normalCFM; retVal = m_normalCFM;
break; break;
case BT_CONSTRAINT_ERP:
btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_NORM);
retVal = m_normalERP;
break;
default : default :
btAssertConstrParams(0); btAssertConstrParams(0);
} }
} }
else else
{ {
btAssertConstrParams(0); btAssertConstrParams(0);
} }
return retVal; return retVal;
} }