//------------------------------------------------------------------------------ //! void AnchoredSpring::preStep( double step ) { DBG_BLOCK( os_spr, "AnchoredSpring::prePositionStep(" << step << ")" ); // Compute world anchors. Vec3f worldAnchorA = _bodyA->transform() * _anchorA; Vec3f x = (_anchorB.position() - worldAnchorA); float dist = x.length(); if( dist > CGConstf::epsilon() ) { x *= (1.0f/dist); } else { x = Vec3f( 0.0f, -1.0f, 0.0f ); } x *= (dist - _restLength); // Apply friction using the velocity from B to A. Vec3f deltaV = -_bodyA->velocity( worldAnchorA ); // Compute impulse to apply. // F = -kx - dv = k*(-x) + d*(-v) // p = F * t Vec3f p = x * (_stiffness * (float)step) + deltaV * (_damping * (float)step); _bodyA->applyImpulse( p, worldAnchorA ); /** StdErr << "anchorA=" << _anchorA << " worldAnchorA=" << worldAnchorA << " deltaV=" << deltaV << " bodyA=" << _bodyA->referential() << " vel=" << _bodyA->linearVelocity() << " p=" << p << nl; **/ // Compute the transformation needed to end a the desired orientation. Quatf curO = _bodyA->referential().orientation(); Quatf desO = _anchorB.orientation(); Quatf trfO = desO * curO.getInversed(); // Compute the angular velocity related to this transformation. Vec3f axis; float angle; trfO.toAxisAngle( axis, angle ); Vec3f angularVelocity = axis*(angle/(float)step); Vec3f diffAV = angularVelocity - _bodyA->angularVelocity(); // Clamp angular velocity. float maxAV = _maxTorque*(float)step/_bodyA->mass(); float diffAVLen = diffAV.length(); if( diffAVLen > maxAV ) { diffAV *= maxAV/diffAVLen; } // Apply angular velocity. _bodyA->angularVelocity( _bodyA->angularVelocity() + diffAV ); /** StdErr << "curO=" << curO << " desO=" << desO << " angVel=" << angularVelocity << " body=" << _bodyA->angularVelocity() << " diffAV=" << diffAV << nl; **/ // FIXME. _bodyA->angularVelocity( Vec3f(0.0f) ); }