diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index e4aa00e96..d228b5247 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -631,7 +631,7 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, c { rotAllowed = 0; } - row += get_limit_motor_info2UsingFrameOffset(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed); + row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed); } else { @@ -802,7 +802,7 @@ void btGeneric6DofConstraint::calculateLinearInfo() int btGeneric6DofConstraint::get_limit_motor_info2( btRotationalLimitMotor * limot, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB, - btConstraintInfo2 *info, int row, btVector3& ax1, int rotational) + btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed) { int srow = row * info->rowskip; int powered = limot->m_enableMotor; @@ -822,168 +822,51 @@ int btGeneric6DofConstraint::get_limit_motor_info2( } if((!rotational)) { - btVector3 ltd; // Linear Torque Decoupling vector - btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin(); - ltd = c.cross(ax1); - info->m_J1angularAxis[srow+0] = ltd[0]; - info->m_J1angularAxis[srow+1] = ltd[1]; - info->m_J1angularAxis[srow+2] = ltd[2]; - - c = m_calculatedTransformB.getOrigin() - transB.getOrigin(); - ltd = -c.cross(ax1); - info->m_J2angularAxis[srow+0] = ltd[0]; - info->m_J2angularAxis[srow+1] = ltd[1]; - info->m_J2angularAxis[srow+2] = ltd[2]; - } - // if we're limited low and high simultaneously, the joint motor is - // ineffective - if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0; - info->m_constraintError[srow] = btScalar(0.f); - if (powered) - { - info->cfm[srow] = limot->m_normalCFM; - if(!limit) - { - btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity; - - btScalar mot_fact = getMotorFactor( limot->m_currentPosition, - limot->m_loLimit, - limot->m_hiLimit, - tag_vel, - info->fps * info->erp); - info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity; - info->m_lowerLimit[srow] = -limot->m_maxMotorForce; - info->m_upperLimit[srow] = limot->m_maxMotorForce; - } - } - if(limit) - { - btScalar k = info->fps * limot->m_stopERP; - if(!rotational) + if (m_useOffsetForConstraintFrame) { - info->m_constraintError[srow] += k * limot->m_currentLimitError; - } - else + btVector3 tmpA, tmpB, relA, relB; + // get vector from bodyB to frameB in WCS + relB = m_calculatedTransformB.getOrigin() - transB.getOrigin(); + // get its projection to constraint axis + btVector3 projB = ax1 * relB.dot(ax1); + // get vector directed from bodyB to constraint axis (and orthogonal to it) + btVector3 orthoB = relB - projB; + // same for bodyA + relA = m_calculatedTransformA.getOrigin() - transA.getOrigin(); + btVector3 projA = ax1 * relA.dot(ax1); + btVector3 orthoA = relA - projA; + // get desired offset between frames A and B along constraint axis + btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError; + // desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis + btVector3 totalDist = projA + ax1 * desiredOffs - projB; + // get offset vectors relA and relB + relA = orthoA + totalDist * m_factA; + relB = orthoB - totalDist * m_factB; + tmpA = relA.cross(ax1); + tmpB = relB.cross(ax1); + if(m_hasStaticBody && (!rotAllowed)) + { + tmpA *= m_factA; + tmpB *= m_factB; + } + int i; + for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i]; + for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i]; + } else { - info->m_constraintError[srow] += -k * limot->m_currentLimitError; + btVector3 ltd; // Linear Torque Decoupling vector + btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin(); + ltd = c.cross(ax1); + info->m_J1angularAxis[srow+0] = ltd[0]; + info->m_J1angularAxis[srow+1] = ltd[1]; + info->m_J1angularAxis[srow+2] = ltd[2]; + + c = m_calculatedTransformB.getOrigin() - transB.getOrigin(); + ltd = -c.cross(ax1); + info->m_J2angularAxis[srow+0] = ltd[0]; + info->m_J2angularAxis[srow+1] = ltd[1]; + info->m_J2angularAxis[srow+2] = ltd[2]; } - info->cfm[srow] = limot->m_stopCFM; - if (limot->m_loLimit == limot->m_hiLimit) - { // limited low and high simultaneously - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else - { - if (limit == 1) - { - info->m_lowerLimit[srow] = 0; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else - { - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = 0; - } - // deal with bounce - if (limot->m_bounce > 0) - { - // calculate joint velocity - btScalar vel; - if (rotational) - { - vel = angVelA.dot(ax1); -//make sure that if no body -> angVelB == zero vec -// if (body1) - vel -= angVelB.dot(ax1); - } - else - { - vel = linVelA.dot(ax1); -//make sure that if no body -> angVelB == zero vec -// if (body1) - vel -= linVelB.dot(ax1); - } - // only apply bounce if the velocity is incoming, and if the - // resulting c[] exceeds what we already have. - if (limit == 1) - { - if (vel < 0) - { - btScalar newc = -limot->m_bounce* vel; - if (newc > info->m_constraintError[srow]) - info->m_constraintError[srow] = newc; - } - } - else - { - if (vel > 0) - { - btScalar newc = -limot->m_bounce * vel; - if (newc < info->m_constraintError[srow]) - info->m_constraintError[srow] = newc; - } - } - } - } - } - return 1; - } - else return 0; -} - - - -int btGeneric6DofConstraint::get_limit_motor_info2UsingFrameOffset( btRotationalLimitMotor * limot, - const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB, - btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed) -{ - int srow = row * info->rowskip; - int powered = limot->m_enableMotor; - int limit = limot->m_currentLimit; - if (powered || limit) - { // if the joint is powered, or has joint limits, add in the extra row - btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis; - btScalar *J2 = rotational ? info->m_J2angularAxis : 0; - J1[srow+0] = ax1[0]; - J1[srow+1] = ax1[1]; - J1[srow+2] = ax1[2]; - if(rotational) - { - J2[srow+0] = -ax1[0]; - J2[srow+1] = -ax1[1]; - J2[srow+2] = -ax1[2]; - } - if((!rotational)) - { - btVector3 tmpA, tmpB, relA, relB; - // get vector from bodyB to frameB in WCS - relB = m_calculatedTransformB.getOrigin() - transB.getOrigin(); - // get its projection to constraint axis - btVector3 projB = ax1 * relB.dot(ax1); - // get vector directed from bodyB to constraint axis (and orthogonal to it) - btVector3 orthoB = relB - projB; - // same for bodyA - relA = m_calculatedTransformA.getOrigin() - transA.getOrigin(); - btVector3 projA = ax1 * relA.dot(ax1); - btVector3 orthoA = relA - projA; - // get desired offset between frames A and B along constraint axis - btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError; - // desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis - btVector3 totalDist = projA + ax1 * desiredOffs - projB; - // get offset vectors relA and relB - relA = orthoA + totalDist * m_factA; - relB = orthoB - totalDist * m_factB; - tmpA = relA.cross(ax1); - tmpB = relB.cross(ax1); - if(m_hasStaticBody && (!rotAllowed)) - { - tmpA *= m_factA; - tmpB *= m_factB; - } - int i; - for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i]; } // if we're limited low and high simultaneously, the joint motor is // ineffective @@ -1084,6 +967,9 @@ int btGeneric6DofConstraint::get_limit_motor_info2UsingFrameOffset( btRotational + + + ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). ///If no axis is provided, it uses the default axis for this constraint. void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis) diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index 99eb3b26a..50f5966a0 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -515,11 +515,7 @@ public: int get_limit_motor_info2( btRotationalLimitMotor * limot, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB, - btConstraintInfo2 *info, int row, btVector3& ax1, int rotational); - - int get_limit_motor_info2UsingFrameOffset( btRotationalLimitMotor * limot, - const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB, - btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed); + btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false); // access for UseFrameOffset bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; } diff --git a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp index b515e232c..8cf83653d 100644 --- a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp @@ -276,7 +276,7 @@ void btPoint2PointConstraint::setParam(int num, btScalar value, int axis) ///return the local value of parameter btScalar btPoint2PointConstraint::getParam(int num, int axis) const { - btScalar retVal; + btScalar retVal(SIMD_INFINITY); if(axis != -1) { btAssertConstrParams(0); diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index 1b1d8957d..27d159fda 100755 --- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -507,7 +507,7 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra { if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG) { - info->cfm[nrow] = m_cfmDirAng; + info->cfm[srow] = m_cfmDirAng; } btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP); info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity(); @@ -1216,9 +1216,8 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i { if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG) { - info->cfm[nrow] = m_cfmDirAng; + info->cfm[srow] = m_cfmDirAng; } - info->cfm[srow] = btScalar(0.0); btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP); info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity(); info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps; @@ -1365,7 +1364,7 @@ void btSliderConstraint::setParam(int num, btScalar value, int axis) ///return the local value of parameter btScalar btSliderConstraint::getParam(int num, int axis) const { - btScalar retVal; + btScalar retVal(SIMD_INFINITY); switch(num) { case BT_CONSTRAINT_STOP_ERP :