void
Joint::applyPassiveInternalWrenches()
{
double f = getFriction();
DBGP("Friction coeffs: " << f0 << " " << f1);
DBGP("Friction force: " << f);
if (f != 0.0) applyInternalWrench(f);
f = getSpringForce();
DBGP("Spring force: " << f);
applyInternalWrench(-f);
}
/**
* This is the method that does the heavy lifting in this class.
* Called by step, it calculates what force the spring is experiencing,
* and applies that force to the rigid bodies it connects to.
*/
void tgBulletCompressionSpring::calculateAndApplyForce(double dt)
{
// Create variables to hold the results of these computations
btVector3 force(0.0, 0.0, 0.0);
// the following ONLY includes forces due to K, not due to damping.
double magnitude = getSpringForce();
// hold these variables so we don't have to call the accessor function twice.
const double currLength = getCurrentSpringLength();
const btVector3 unitVector = getAnchorDirectionUnitVector();
// Calculate the damping force for this timestep.
// Take an approximated derivative to estimate the velocity of the
// tip of the compression spring:
// TO-DO: MAKE THIS A BETTER APPROXIMATION TO THE DERIVATIVE!!
const double changeInDeltaX = currLength - m_prevLength;
m_velocity = changeInDeltaX / dt;
// The damping force for this timestep
// Like with spring force, a positive velocity should result in a
// force acting against the movement of the tip of the spring.
m_dampingForce = - getCoefD() * m_velocity;
// Debugging
#if (0)
std::cout << "Length: " << getCurrentSpringLength() << " rl: " << getRestLength() <<std::endl;
std::cout << "SpringForce: " << magnitude << " DampingForce: " << m_dampingForce <<std::endl;
#endif
// Add the damping force to the force from the spring.
magnitude += m_dampingForce;
// Project the force into the direction of the line between the two anchors
force = unitVector * magnitude;
// Store the calculated length as the previous length
m_prevLength = currLength;
//Now Apply it to the connected two bodies
btVector3 point1 = this->anchor1->getRelativePosition();
this->anchor1->attachedBody->activate();
this->anchor1->attachedBody->applyImpulse(force*dt,point1);
btVector3 point2 = this->anchor2->getRelativePosition();
this->anchor2->attachedBody->activate();
this->anchor2->attachedBody->applyImpulse(-force*dt,point2);
}
