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