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