btScalar btTranslationalLimitMotor::solveLinearAxis( btScalar timeStep, btScalar jacDiagABInv, btRigidBody& body1,const btVector3 &pointInA, btRigidBody& body2,const btVector3 &pointInB, int limit_index, const btVector3 & axis_normal_on_a, const btVector3 & anchorPos) { ///find relative velocity // btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition(); // btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition(); btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition(); btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition(); btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); btVector3 vel = vel1 - vel2; btScalar rel_vel = axis_normal_on_a.dot(vel); /// apply displacement correction //positional error (zeroth order error) btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a); btScalar lo = btScalar(-BT_LARGE_FLOAT); btScalar hi = btScalar(BT_LARGE_FLOAT); btScalar minLimit = m_lowerLimit[limit_index]; btScalar maxLimit = m_upperLimit[limit_index]; //handle the limits if (minLimit < maxLimit) { { if (depth > maxLimit) { depth -= maxLimit; lo = btScalar(0.); } else { if (depth < minLimit) { depth -= minLimit; hi = btScalar(0.); } else { return 0.0f; } } } } btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv; btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index]; btScalar sum = oldNormalImpulse + normalImpulse; m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; btVector3 impulse_vector = axis_normal_on_a * normalImpulse; body1.applyImpulse( impulse_vector, rel_pos1); body2.applyImpulse(-impulse_vector, rel_pos2); return normalImpulse; }
void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) { int i; // linear btVector3 velA = rbA.getVelocityInLocalPoint(m_relPosA); btVector3 velB = rbB.getVelocityInLocalPoint(m_relPosB); btVector3 vel = velA - velB; for(i = 0; i < 3; i++) { const btVector3& normal = m_jacLin[i].m_linearJointAxis; btScalar rel_vel = normal.dot(vel); // calculate positional error btScalar depth = m_depth[i]; // get parameters btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin); btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin); btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin); // calcutate and apply impulse btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i]; btVector3 impulse_vector = normal * normalImpulse; rbA.applyImpulse( impulse_vector, m_relPosA); rbB.applyImpulse(-impulse_vector, m_relPosB); if(m_poweredLinMotor && (!i)) { // apply linear motor if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce) { btScalar desiredMotorVel = m_targetLinMotorVelocity; btScalar motor_relvel = desiredMotorVel + rel_vel; normalImpulse = -motor_relvel * m_jacLinDiagABInv[i]; // clamp accumulated impulse btScalar new_acc = m_accumulatedLinMotorImpulse + btFabs(normalImpulse); if(new_acc > m_maxLinMotorForce) { new_acc = m_maxLinMotorForce; } btScalar del = new_acc - m_accumulatedLinMotorImpulse; if(normalImpulse < btScalar(0.0)) { normalImpulse = -del; } else { normalImpulse = del; } m_accumulatedLinMotorImpulse = new_acc; // apply clamped impulse impulse_vector = normal * normalImpulse; rbA.applyImpulse( impulse_vector, m_relPosA); rbB.applyImpulse(-impulse_vector, m_relPosB); } } } // angular // get axes in world space btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); btVector3 axisB = m_calculatedTransformB.getBasis().getColumn(0); const btVector3& angVelA = rbA.getAngularVelocity(); const btVector3& angVelB = rbB.getAngularVelocity(); btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA); btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB); btVector3 angAorthog = angVelA - angVelAroundAxisA; btVector3 angBorthog = angVelB - angVelAroundAxisB; btVector3 velrelOrthog = angAorthog-angBorthog; //solve orthogonal angular velocity correction btScalar len = velrelOrthog.length(); if (len > btScalar(0.00001)) { btVector3 normal = velrelOrthog.normalized(); btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal); velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; } //solve angular positional correction btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep); btScalar len2 = angularError.length(); if (len2>btScalar(0.00001)) { btVector3 normal2 = angularError.normalized(); btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2); angularError *= (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; } // apply impulse rbA.applyTorqueImpulse(-velrelOrthog+angularError); rbB.applyTorqueImpulse(velrelOrthog-angularError); btScalar impulseMag; //solve angular limits if(m_solveAngLim) { impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / m_timeStep; impulseMag *= m_kAngle * m_softnessLimAng; } else { impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / m_timeStep; impulseMag *= m_kAngle * m_softnessDirAng; } btVector3 impulse = axisA * impulseMag; rbA.applyTorqueImpulse(impulse); rbB.applyTorqueImpulse(-impulse); //apply angular motor if(m_poweredAngMotor) { if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce) { btVector3 velrel = angVelAroundAxisA - angVelAroundAxisB; btScalar projRelVel = velrel.dot(axisA); btScalar desiredMotorVel = m_targetAngMotorVelocity; btScalar motor_relvel = desiredMotorVel - projRelVel; btScalar angImpulse = m_kAngle * motor_relvel; // clamp accumulated impulse btScalar new_acc = m_accumulatedAngMotorImpulse + btFabs(angImpulse); if(new_acc > m_maxAngMotorForce) { new_acc = m_maxAngMotorForce; } btScalar del = new_acc - m_accumulatedAngMotorImpulse; if(angImpulse < btScalar(0.0)) { angImpulse = -del; } else { angImpulse = del; } m_accumulatedAngMotorImpulse = new_acc; // apply clamped impulse btVector3 motorImp = angImpulse * axisA; m_rbA.applyTorqueImpulse(motorImp); m_rbB.applyTorqueImpulse(-motorImp); } } } // btSliderConstraint::solveConstraint()