diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index d17e722c2..0ebd5f848 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -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(!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::getInfo2 (btConstraintInfo2* info) -{ - btAssert(0); -} - +//----------------------------------------------------------------------------- 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() + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h index b91522480..56d93c724 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -127,6 +127,9 @@ public: return m_twistLimitSign; } + void calcAngleInfo(); + + }; #endif //CONETWISTCONSTRAINT_H diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index f84576f4f..1ad914f4f 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -25,27 +25,35 @@ http://gimpact.sf.net #include "LinearMath/btTransformUtil.h" #include +//----------------------------------------------------------------------------- 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) - { - btVector3* linear_axis = (btVector3* )&info->m_J1linearAxis[s*i]; - *linear_axis = m_calculatedTransformA.getBasis().getColumn(i); - } - else - { - btVector3* linear_axis = (btVector3* )&info->m_J1linearAxis[s*i]; - *linear_axis = m_calculatedTransformB.getBasis().getColumn(i); + 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) + { + axis = m_calculatedTransformB.getBasis().getColumn(i); + } + else + { + 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); - } - - /*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; - + return row; } - -/*! \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; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index 875d108b2..0ae161d5b 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -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 diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index f28be0e7c..51dfb6cb4 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -21,21 +21,26 @@ subject to the following restrictions: #include #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()); + // clear accumulator + m_accLimitImpulse = btScalar(0.); - // Compute limit information - btScalar hingeAngle = getHingeAngle(); - - //set bias, sign, clear accumulator - m_correction = btScalar(0.); - m_limitSign = btScalar(0.); - m_solveLimit = false; - 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,24 +260,215 @@ void btHingeConstraint::buildJacobian() } } +//----------------------------------------------------------------------------- -void btHingeConstraint::getInfo1 (btConstraintInfo1* info) +void btHingeConstraint::getInfo1(btConstraintInfo1* info) { if (m_useSolveConstraintObsolete) { info->m_numConstraintRows = 0; info->nub = 0; - } else - { - btAssert(0); } -} + else + { + 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() + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 6405381c6..7ea9d9e19 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -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; }; diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index fe22de902..2d617217d 100755 --- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -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); @@ -155,28 +163,358 @@ void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, co //----------------------------------------------------------------------------- -void btSliderConstraint::getInfo1 (btConstraintInfo1* info) +void btSliderConstraint::getInfo1(btConstraintInfo1* info) { - info->m_numConstraintRows = 0; - info->nub = 0; -} - -void btSliderConstraint::getInfo2 (btConstraintInfo2* info) -{ - btAssert(0); -} - - -void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) -{ - m_timeStep = timeStep; - if(m_useLinearReferenceFrameA) + if (m_useSolveConstraintObsolete) { - solveConstraintInt(m_rbA,bodyA, m_rbB,bodyB); + info->m_numConstraintRows = 0; + info->nub = 0; } else { - solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA); + 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(!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) + { + solveConstraintInt(m_rbA,bodyA, m_rbB,bodyB); + } + else + { + 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 - m_delta = m_realPivotBInW - m_realPivotAInW; + 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; @@ -426,7 +771,6 @@ void btSliderConstraint::testLinLimits(void) } // btSliderConstraint::testLinLimits() //----------------------------------------------------------------------------- - void btSliderConstraint::testAngLimits(void) { @@ -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; @@ -450,12 +795,9 @@ void btSliderConstraint::testAngLimits(void) } } } // btSliderConstraint::testAngLimits() - //----------------------------------------------------------------------------- - - btVector3 btSliderConstraint::getAncorInA(void) { btVector3 ancorInA; diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h index 94baf61ad..9357b8fa3 100755 --- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -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; @@ -200,6 +203,7 @@ public: void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; } btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; } btScalar getLinearPos() { return m_linPos; } + // access for ODE solver bool getSolveLinLimit() { return m_solveLinLim; } @@ -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); diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index 6e8b552db..b9f6ffc1e 100644 --- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -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() + + diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index d383239cb..f572f50fa 100644 --- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -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 { diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h index de0cc29a3..5de85c090 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/src/BulletDynamics/Dynamics/btRigidBody.h @@ -232,6 +232,16 @@ public: { m_totalForce += force; } + + const btVector3& getTotalForce() + { + return m_totalForce; + }; + + const btVector3& getTotalTorque() + { + return m_totalTorque; + }; const btVector3& getInvInertiaDiagLocal() {