それらしく見えるでしょうか?19行のIbodyと26行のLの値に注意して下さい。
#include <iostream> #include <Eigen/Dense> #include <Eigen/Core> #include <Eigen/Geometry> using namespace std; using namespace Eigen; Matrix3d Ibody; Matrix3d Ibodyinv; Quaterniond q; Quaterniond qdot; Vector3d L; Matrix3d Iinv; Matrix3d R; Vector3d omega; int main(int argc, const char * argv[]) { Ibody << 1.0,0.0,0.0, 0.0,2.0,0.0, 0.0,0.0,4.0; Ibodyinv = Ibody.inverse(); cout << "Ibody: " << endl << Ibody << endl; cout << "Ibodyinv: " << endl << Ibodyinv << endl; Quaterniond q(AngleAxisd(M_PI*0.0, Vector3d::UnitZ())); q.normalize(); L << 0.00, 0.00, 1.00; double dt = 0.1; for(double t=0; t<200.0; t+= dt) { R = q.toRotationMatrix(); Iinv = R * Ibodyinv * R.transpose(); omega = Iinv * L; Quaterniond qq(0.0, omega.x()/2.0, omega.y()/2.0, omega.z()/2.0); qdot = qq * q; cout << "t : " << t << endl; cout << "q : " << q.w() << " " << q.vec().transpose() << endl; cout << "L : " << L.transpose() << endl; cout << "R : " << endl << R << endl; cout << "Iinv : " << endl << Iinv << endl; cout << "omega: " << omega.transpose() << endl; cout << "qdot : " << qdot.w() << " " << qdot.vec().transpose() << endl; q.w() += qdot.w() * dt; q.vec() += qdot.vec() * dt; q.normalize(); } return 0; }
% ./MyEigenTest1 | grep "q :" | colrm 1 6 | cat -n > t % ./MyEigenTest1 | grep "omega:" | colrm 1 6 | cat -n > u % gnuplot gnuplot> plot "t" using 1:2, "t" using 1:5, "u" using 1:2, "u" using 1:3, "u" using 1:4
今度は、IbodyとLの両方を少し変えました。
Ibody << 1.0,0.0,0.0, 0.0,4.0,0.0, 0.0,0.0,2.0; L << 0.01, 0.00, 1.00;
解は安定なようです。