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); }