void QuaternionAccumApp::drawBall()
{
Vec3f pos = mSpline.getPosition( mSplineValue );
Vec3f delta = pos - mLastPos;
// our axis of rotation is the normal to the spline at this point
Vec3f normal = Vec3f( delta.z, 0, -delta.x );
// rotation amount (in radians) is the distance we've traveled divided by the radius of the ball
float rotation = delta.length() / BALL_RADIUS;
if( rotation ) {
// increment our quaternion by a new quaternion representing how much rotating we did since the last frame
Quatf incQuat( normal, rotation );
mQuat *= incQuat;
mQuat.normalize();
}
gl::translate( Vec3f( 0.0f, BALL_RADIUS, 0.0f ) + pos );
gl::scale( Vec3f( BALL_RADIUS, BALL_RADIUS, BALL_RADIUS ) );
gl::rotate( mQuat );
gl::color( Color( 1, 1, 1 ) );
mBallTexture.enableAndBind();
gl::drawSphere( Vec3f::zero(), 1.0f, 60 );
mBallTexture.disable();
mLastPos = pos;
}
void RestShapeSpringsForceField<Rigid3fTypes>::addForce(const core::MechanicalParams* /* mparams */ /* PARAMS FIRST */, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& /* v */)
{
sofa::helper::WriteAccessor< DataVecDeriv > f1 = f;
sofa::helper::ReadAccessor< DataVecCoord > p1 = x;
sofa::helper::ReadAccessor< DataVecCoord > p0 = *getExtPosition();
f1.resize(p1.size());
if (recompute_indices.getValue())
{
recomputeIndices();
}
const VecReal& k = stiffness.getValue();
const VecReal& k_a = angularStiffness.getValue();
for (unsigned int i=0; i<m_indices.size(); i++)
{
const unsigned int index = m_indices[i];
const unsigned int ext_index = m_ext_indices[i];
// translation
Vec3f dx = p1[index].getCenter() - p0[ext_index].getCenter();
getVCenter(f1[index]) -= dx * (i < k.size() ? k[i] : k[0]) ;
// rotation
Quatf dq = p1[index].getOrientation() * p0[ext_index].getOrientation().inverse();
Vec3f dir;
Real angle=0;
dq.normalize();
if (dq[3] < 0.999999999999999)
dq.quatToAxis(dir, angle);
dq.quatToAxis(dir, angle);
//std::cout<<"dq : "<<dq <<" dir :"<<dir<<" angle :"<<angle<<std::endl;
getVOrientation(f1[index]) -= dir * angle * (i < k_a.size() ? k_a[i] : k_a[0]) ;
}
}
