diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index 330c06d5c..e5935b959 100755 --- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -217,364 +217,9 @@ void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info) void btSliderConstraint::getInfo2(btConstraintInfo2* info) { -// if(m_useOffsetForConstraintFrame) -// { - getInfo2NonVirtualUsingFrameOffset(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass()); -// } -// else -// { -// getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass()); -// } + getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass()); } -#if 0 // replaced with getInfo2NonVirtualUsingFrameOffset -void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass ) -{ - //prepare constraint - calculateTransforms(transA,transB); - testLinLimits(); - testAngLimits(); - - 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); - // 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(); - btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ? m_softnessOrthoAng : m_softnessOrthoAng * info->erp; - btScalar k = info->fps * currERP; - - 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); - if(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG) - { - info->cfm[0] = m_cfmOrthoAng; - info->cfm[s] = m_cfmOrthoAng; - } - // 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 = transA; - btTransform bodyB_trans = transB; - int s2 = 2 * s, s3 = 3 * s; - btVector3 c; - btScalar miA = rbAinvMass; - btScalar miB = rbBinvMass; - 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(); - currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN) ? m_softnessOrthoLin : m_softnessOrthoLin * info->erp; - k = info->fps * currERP; - - info->m_constraintError[s2] = k * p.dot(ofs); - info->m_constraintError[s3] = k * q.dot(ofs); - if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN) - { - info->cfm[s2] = m_cfmOrthoLin; - info->cfm[s3] = m_cfmOrthoLin; - } - 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.; - 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] += mot_fact * getTargetLinMotorVelocity(); - 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 = 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; - } - 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 -} -#endif void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) @@ -894,7 +539,7 @@ btVector3 btSliderConstraint::getAncorInB(void) } -void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass ) +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(); @@ -1005,7 +650,7 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i int s2 = nrow * s; nrow++; int s3 = nrow * s; - btVector3 tmpA, tmpB, relA, relB, c; + 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 diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h index aad7b83d5..5c8a9d1b0 100755 --- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -168,9 +168,8 @@ public: virtual void getInfo2 (btConstraintInfo2* info); void getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass); - void getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass); - virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); // access