diff options
Diffstat (limited to 'thirdparty/bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp')
| -rw-r--r-- | thirdparty/bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp | 855 |
1 files changed, 0 insertions, 855 deletions
diff --git a/thirdparty/bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/thirdparty/bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp deleted file mode 100644 index d63cef0316..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ /dev/null @@ -1,855 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* -Added by Roman Ponomarev (rponom@gmail.com) -April 04, 2008 -*/ - - - -#include "btSliderConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" -#include <new> - -#define USE_OFFSET_FOR_CONSTANT_FRAME true - -void btSliderConstraint::initParams() -{ - m_lowerLinLimit = btScalar(1.0); - m_upperLinLimit = btScalar(-1.0); - m_lowerAngLimit = btScalar(0.); - m_upperAngLimit = btScalar(0.); - m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingDirLin = btScalar(0.); - m_cfmDirLin = SLIDER_CONSTRAINT_DEF_CFM; - m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingDirAng = btScalar(0.); - m_cfmDirAng = SLIDER_CONSTRAINT_DEF_CFM; - m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING; - m_cfmOrthoLin = SLIDER_CONSTRAINT_DEF_CFM; - m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING; - m_cfmOrthoAng = SLIDER_CONSTRAINT_DEF_CFM; - m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING; - m_cfmLimLin = SLIDER_CONSTRAINT_DEF_CFM; - m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING; - m_cfmLimAng = SLIDER_CONSTRAINT_DEF_CFM; - - m_poweredLinMotor = false; - m_targetLinMotorVelocity = btScalar(0.); - m_maxLinMotorForce = btScalar(0.); - m_accumulatedLinMotorImpulse = btScalar(0.0); - - m_poweredAngMotor = false; - m_targetAngMotorVelocity = btScalar(0.); - m_maxAngMotorForce = btScalar(0.); - m_accumulatedAngMotorImpulse = btScalar(0.0); - - m_flags = 0; - m_flags = 0; - - m_useOffsetForConstraintFrame = USE_OFFSET_FOR_CONSTANT_FRAME; - - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); -} - - - - - -btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) - : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB), - m_useSolveConstraintObsolete(false), - m_frameInA(frameInA), - m_frameInB(frameInB), - m_useLinearReferenceFrameA(useLinearReferenceFrameA) -{ - initParams(); -} - - - -btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA) - : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB), - m_useSolveConstraintObsolete(false), - m_frameInB(frameInB), - m_useLinearReferenceFrameA(useLinearReferenceFrameA) -{ - ///not providing rigidbody A means implicitly using worldspace for body A - m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB; -// m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin()); - - initParams(); -} - - - - - - -void btSliderConstraint::getInfo1(btConstraintInfo1* info) -{ - if (m_useSolveConstraintObsolete) - { - info->m_numConstraintRows = 0; - info->nub = 0; - } - else - { - info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular - info->nub = 2; - //prepare constraint - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); - testAngLimits(); - testLinLimits(); - if(getSolveLinLimit() || getPoweredLinMotor()) - { - info->m_numConstraintRows++; // limit 3rd linear as well - info->nub--; - } - if(getSolveAngLimit() || getPoweredAngMotor()) - { - info->m_numConstraintRows++; // limit 3rd angular as well - info->nub--; - } - } -} - -void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info) -{ - - info->m_numConstraintRows = 6; // Fixed 2 linear + 2 angular + 1 limit (even if not used) - info->nub = 0; -} - -void btSliderConstraint::getInfo2(btConstraintInfo2* info) -{ - getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass()); -} - - - - - - - -void btSliderConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB) -{ - if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete)) - { - m_calculatedTransformA = transA * m_frameInA; - m_calculatedTransformB = transB * m_frameInB; - } - else - { - m_calculatedTransformA = transB * m_frameInB; - m_calculatedTransformB = transA * m_frameInA; - } - m_realPivotAInW = m_calculatedTransformA.getOrigin(); - m_realPivotBInW = m_calculatedTransformB.getOrigin(); - m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X - if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete) - { - m_delta = m_realPivotBInW - m_realPivotAInW; - } - else - { - m_delta = m_realPivotAInW - m_realPivotBInW; - } - m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; - btVector3 normalWorld; - int i; - //linear part - for(i = 0; i < 3; i++) - { - normalWorld = m_calculatedTransformA.getBasis().getColumn(i); - m_depth[i] = m_delta.dot(normalWorld); - } -} - - - -void btSliderConstraint::testLinLimits(void) -{ - m_solveLinLim = false; - m_linPos = m_depth[0]; - if(m_lowerLinLimit <= m_upperLinLimit) - { - if(m_depth[0] > m_upperLinLimit) - { - m_depth[0] -= m_upperLinLimit; - m_solveLinLim = true; - } - else if(m_depth[0] < m_lowerLinLimit) - { - m_depth[0] -= m_lowerLinLimit; - m_solveLinLim = true; - } - else - { - m_depth[0] = btScalar(0.); - } - } - else - { - m_depth[0] = btScalar(0.); - } -} - - - -void btSliderConstraint::testAngLimits(void) -{ - m_angDepth = btScalar(0.); - m_solveAngLim = false; - if(m_lowerAngLimit <= m_upperAngLimit) - { - const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1); - 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)); - btScalar rot = btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0)); - rot = btAdjustAngleToLimits(rot, m_lowerAngLimit, m_upperAngLimit); - m_angPos = rot; - if(rot < m_lowerAngLimit) - { - m_angDepth = rot - m_lowerAngLimit; - m_solveAngLim = true; - } - else if(rot > m_upperAngLimit) - { - m_angDepth = rot - m_upperAngLimit; - m_solveAngLim = true; - } - } -} - -btVector3 btSliderConstraint::getAncorInA(void) -{ - btVector3 ancorInA; - ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis; - ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA; - return ancorInA; -} - - - -btVector3 btSliderConstraint::getAncorInB(void) -{ - btVector3 ancorInB; - ancorInB = m_frameInB.getOrigin(); - return ancorInB; -} - - -void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass ) -{ - const btTransform& trA = getCalculatedTransformA(); - const btTransform& trB = getCalculatedTransformB(); - - btAssert(!m_useSolveConstraintObsolete); - int i, s = info->rowskip; - - btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f); - - // difference between frames in WCS - btVector3 ofs = trB.getOrigin() - trA.getOrigin(); - // now get weight factors depending on masses - btScalar miA = rbAinvMass; - btScalar miB = rbBinvMass; - bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); - btScalar miS = miA + miB; - btScalar factA, factB; - if(miS > btScalar(0.f)) - { - factA = miB / miS; - } - else - { - factA = btScalar(0.5f); - } - factB = btScalar(1.0f) - factA; - btVector3 ax1, p, q; - btVector3 ax1A = trA.getBasis().getColumn(0); - btVector3 ax1B = trB.getBasis().getColumn(0); - if(m_useOffsetForConstraintFrame) - { - // get the desired direction of slider axis - // as weighted sum of X-orthos of frameA and frameB in WCS - ax1 = ax1A * factA + ax1B * factB; - ax1.normalize(); - // construct two orthos to slider axis - btPlaneSpace1 (ax1, p, q); - } - else - { // old way - use frameA - ax1 = trA.getBasis().getColumn(0); - // get 2 orthos to slider axis (Y, Z) - p = trA.getBasis().getColumn(1); - q = trA.getBasis().getColumn(2); - } - // make rotations around these orthos 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. - 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 ax1A,ax1B are the unit length slider axes as computed from bodyA and - // bodyB, 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(); - btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ? m_softnessOrthoAng : m_softnessOrthoAng * info->erp; - btScalar k = info->fps * currERP; - - btVector3 u = ax1A.cross(ax1B); - info->m_constraintError[0] = k * u.dot(p); - info->m_constraintError[s] = k * u.dot(q); - if(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG) - { - info->cfm[0] = m_cfmOrthoAng; - info->cfm[s] = m_cfmOrthoAng; - } - - int nrow = 1; // last filled row - int srow; - btScalar limit_err; - int limit; - - // next two rows. - // we want: velA + wA x relA == velB + wB x relB ... but this would - // result in three equations, so we project along two orthos to the slider axis - - btTransform bodyA_trans = transA; - btTransform bodyB_trans = transB; - nrow++; - int s2 = nrow * s; - nrow++; - int s3 = nrow * s; - btVector3 tmpA(0,0,0), tmpB(0,0,0), relA(0,0,0), relB(0,0,0), c(0,0,0); - if(m_useOffsetForConstraintFrame) - { - // get vector from bodyB to frameB in WCS - relB = trB.getOrigin() - bodyB_trans.getOrigin(); - // get its projection to slider axis - btVector3 projB = ax1 * relB.dot(ax1); - // get vector directed from bodyB to slider axis (and orthogonal to it) - btVector3 orthoB = relB - projB; - // same for bodyA - relA = trA.getOrigin() - bodyA_trans.getOrigin(); - btVector3 projA = ax1 * relA.dot(ax1); - btVector3 orthoA = relA - projA; - // get desired offset between frames A and B along slider axis - btScalar sliderOffs = m_linPos - m_depth[0]; - // desired vector from projection of center of bodyA to projection of center of bodyB to slider axis - btVector3 totalDist = projA + ax1 * sliderOffs - projB; - // get offset vectors relA and relB - relA = orthoA + totalDist * factA; - relB = orthoB - totalDist * factB; - // now choose average ortho to slider axis - p = orthoB * factA + orthoA * factB; - btScalar len2 = p.length2(); - if(len2 > SIMD_EPSILON) - { - p /= btSqrt(len2); - } - else - { - p = trA.getBasis().getColumn(1); - } - // make one more ortho - q = ax1.cross(p); - // fill two rows - tmpA = relA.cross(p); - tmpB = relB.cross(p); - 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]; - tmpA = relA.cross(q); - tmpB = relB.cross(q); - if(hasStaticBody && getSolveAngLimit()) - { // to make constraint between static and dynamic objects more rigid - // remove wA (or wB) from equation if angular limit is hit - tmpB *= factB; - tmpA *= factA; - } - for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = tmpA[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[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]; - for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i]; - for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i]; - } - else - { // old way - maybe incorrect if bodies are not on the slider axis - // see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0 - 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]; - for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i]; - for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i]; - } - // compute two elements of right hand side - - // k = info->fps * info->erp * getSoftnessOrthoLin(); - currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN) ? m_softnessOrthoLin : m_softnessOrthoLin * info->erp; - k = info->fps * currERP; - - btScalar rhs = k * p.dot(ofs); - info->m_constraintError[s2] = rhs; - rhs = k * q.dot(ofs); - info->m_constraintError[s3] = rhs; - if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN) - { - info->cfm[s2] = m_cfmOrthoLin; - info->cfm[s3] = m_cfmOrthoLin; - } - - - // check linear limits - limit_err = btScalar(0.0); - limit = 0; - if(getSolveLinLimit()) - { - limit_err = getLinDepth() * signFact; - limit = (limit_err > btScalar(0.0)) ? 2 : 1; - } - bool powered = getPoweredLinMotor(); - // 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]; - info->m_J2linearAxis[srow+0] = -ax1[0]; - info->m_J2linearAxis[srow+1] = -ax1[1]; - info->m_J2linearAxis[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. - if(m_useOffsetForConstraintFrame) - { - // this is needed only when bodyA and bodyB are both dynamic. - if(!hasStaticBody) - { - tmpA = relA.cross(ax1); - tmpB = relB.cross(ax1); - info->m_J1angularAxis[srow+0] = tmpA[0]; - info->m_J1angularAxis[srow+1] = tmpA[1]; - info->m_J1angularAxis[srow+2] = tmpA[2]; - info->m_J2angularAxis[srow+0] = -tmpB[0]; - info->m_J2angularAxis[srow+1] = -tmpB[1]; - info->m_J2angularAxis[srow+2] = -tmpB[2]; - } - } - else - { // The old way. May be incorrect if bodies are not on the slider axis - btVector3 ltd; // Linear Torque Decoupling vector (a torque) - 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 = false; - } - info->m_constraintError[srow] = 0.; - info->m_lowerLimit[srow] = 0.; - info->m_upperLimit[srow] = 0.; - currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp; - if(powered) - { - if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN) - { - info->cfm[srow] = m_cfmDirLin; - } - btScalar tag_vel = getTargetLinMotorVelocity(); - btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP); - info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity(); - info->m_lowerLimit[srow] += -getMaxLinMotorForce() / info->fps; - info->m_upperLimit[srow] += getMaxLinMotorForce() / info->fps; - } - if(limit) - { - k = info->fps * currERP; - info->m_constraintError[srow] += k * limit_err; - if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN) - { - info->cfm[srow] = m_cfmLimLin; - } - 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 = linVelA.dot(ax1); - vel -= linVelB.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 = getPoweredAngMotor(); - 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 = false; - } - currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp; - if(powered) - { - if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG) - { - 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(); - info->m_lowerLimit[srow] = -getMaxAngMotorForce() / info->fps; - info->m_upperLimit[srow] = getMaxAngMotorForce() / info->fps; - } - if(limit) - { - k = info->fps * currERP; - info->m_constraintError[srow] += k * limit_err; - if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG) - { - info->cfm[srow] = m_cfmLimAng; - } - 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 -} - - -///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 btSliderConstraint::setParam(int num, btScalar value, int axis) -{ - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : - if(axis < 1) - { - m_softnessLimLin = value; - m_flags |= BT_SLIDER_FLAGS_ERP_LIMLIN; - } - else if(axis < 3) - { - m_softnessOrthoLin = value; - m_flags |= BT_SLIDER_FLAGS_ERP_ORTLIN; - } - else if(axis == 3) - { - m_softnessLimAng = value; - m_flags |= BT_SLIDER_FLAGS_ERP_LIMANG; - } - else if(axis < 6) - { - m_softnessOrthoAng = value; - m_flags |= BT_SLIDER_FLAGS_ERP_ORTANG; - } - else - { - btAssertConstrParams(0); - } - break; - case BT_CONSTRAINT_CFM : - if(axis < 1) - { - m_cfmDirLin = value; - m_flags |= BT_SLIDER_FLAGS_CFM_DIRLIN; - } - else if(axis == 3) - { - m_cfmDirAng = value; - m_flags |= BT_SLIDER_FLAGS_CFM_DIRANG; - } - else - { - btAssertConstrParams(0); - } - break; - case BT_CONSTRAINT_STOP_CFM : - if(axis < 1) - { - m_cfmLimLin = value; - m_flags |= BT_SLIDER_FLAGS_CFM_LIMLIN; - } - else if(axis < 3) - { - m_cfmOrthoLin = value; - m_flags |= BT_SLIDER_FLAGS_CFM_ORTLIN; - } - else if(axis == 3) - { - m_cfmLimAng = value; - m_flags |= BT_SLIDER_FLAGS_CFM_LIMANG; - } - else if(axis < 6) - { - m_cfmOrthoAng = value; - m_flags |= BT_SLIDER_FLAGS_CFM_ORTANG; - } - else - { - btAssertConstrParams(0); - } - break; - } -} - -///return the local value of parameter -btScalar btSliderConstraint::getParam(int num, int axis) const -{ - btScalar retVal(SIMD_INFINITY); - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : - if(axis < 1) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN); - retVal = m_softnessLimLin; - } - else if(axis < 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN); - retVal = m_softnessOrthoLin; - } - else if(axis == 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMANG); - retVal = m_softnessLimAng; - } - else if(axis < 6) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTANG); - retVal = m_softnessOrthoAng; - } - else - { - btAssertConstrParams(0); - } - break; - case BT_CONSTRAINT_CFM : - if(axis < 1) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN); - retVal = m_cfmDirLin; - } - else if(axis == 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG); - retVal = m_cfmDirAng; - } - else - { - btAssertConstrParams(0); - } - break; - case BT_CONSTRAINT_STOP_CFM : - if(axis < 1) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN); - retVal = m_cfmLimLin; - } - else if(axis < 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN); - retVal = m_cfmOrthoLin; - } - else if(axis == 3) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG); - retVal = m_cfmLimAng; - } - else if(axis < 6) - { - btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG); - retVal = m_cfmOrthoAng; - } - else - { - btAssertConstrParams(0); - } - break; - } - return retVal; -} - - - |