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