void Leapfrog::leapfrogAsphericalRotate(const shared_ptr<Node>& node, const Vector3r& M){
Quaternionr& ori(node->ori); DemData& dyn(node->getData<DemData>());
Vector3r& angMom(dyn.angMom); Vector3r& angVel(dyn.angVel); const Vector3r& inertia(dyn.inertia);
// initialize angular momentum; it is in local coordinates, therefore angVel must be rotated
if(isnan(angMom.minCoeff())){
angMom=dyn.inertia.asDiagonal()*node->ori.conjugate()*dyn.angVel;
}
Matrix3r A=ori.conjugate().toRotationMatrix(); // rotation matrix from global to local r.f.
const Vector3r l_n = angMom + dt/2 * M; // global angular momentum at time n
const Vector3r l_b_n = A*l_n; // local angular momentum at time n
const Vector3r angVel_b_n = (l_b_n.array()/inertia.array()).matrix(); // local angular velocity at time n
const Quaternionr dotQ_n=DotQ(angVel_b_n,ori); // dQ/dt at time n
const Quaternionr Q_half = ori + dt/2 * dotQ_n; // Q at time n+1/2
angMom+=dt*M; // global angular momentum at time n+1/2
const Vector3r l_b_half = A*angMom; // local angular momentum at time n+1/2
Vector3r angVel_b_half = (l_b_half.array()/inertia.array()).matrix(); // local angular velocity at time n+1/2
const Quaternionr dotQ_half=DotQ(angVel_b_half,Q_half); // dQ/dt at time n+1/2
ori=ori+dt*dotQ_half; // Q at time n+1
angVel=ori*angVel_b_half; // global angular velocity at time n+1/2
ori.normalize();
}
Vector3r Leapfrog::computeAngAccel(const Vector3r& torque, const Vector3r& inertia, const DemData& dyn){
if(likely(dyn.isBlockedNone())) return (torque.array()/inertia.array()).matrix();
Vector3r ret(Vector3r::Zero());
for(int i=0; i<3; i++) if(!(dyn.isBlockedAxisDOF(i,true))) ret[i]+=torque[i]/inertia[i];
return ret;
}
