28 const static bool mhi=
true;
38 Eigen::Vector3d c_eig=Eigen::Map<const Eigen::Vector3d>(c_.
data);
39 Eigen::Map<Eigen::Matrix3d>(
I.
data)=Eigen::Map<const Eigen::Matrix3d>(Ic.
data)-m_*(c_eig*c_eig.transpose()-c_eig.dot(c_eig)*Eigen::Matrix3d::Identity());
60 Eigen::Vector3d r_eig = Eigen::Map<Eigen::Vector3d>(X.
p.
data);
61 Eigen::Vector3d h_eig = Eigen::Map<const Eigen::Vector3d>(I.
h.
data);
62 Eigen::Vector3d hmr_eig = Eigen::Map<Eigen::Vector3d>(hmr.
data);
63 Eigen::Matrix3d rcrosshcross = h_eig *r_eig.transpose()-r_eig.dot(h_eig)*Eigen::Matrix3d::Identity();
64 Eigen::Matrix3d hmrcrossrcross = r_eig*hmr_eig.transpose()-hmr_eig.dot(r_eig)*Eigen::Matrix3d::Identity();
65 Eigen::Matrix3d R = Eigen::Map<Eigen::Matrix3d>(X.
M.
data);
67 Eigen::Map<Eigen::Matrix3d>(Ib.
data) = R*((Eigen::Map<const Eigen::Matrix3d>(I.
I.
data)+rcrosshcross+hmrcrossrcross)*R.transpose());
76 Eigen::Matrix3d R = Eigen::Map<const Eigen::Matrix3d>(M.
data);
78 Eigen::Map<Eigen::Matrix3d>(Ib.
data) = R.transpose()*(Eigen::Map<const Eigen::Matrix3d>(I.
I.
data)*R);
88 Eigen::Vector3d r_eig = Eigen::Map<const Eigen::Vector3d>(p.
data);
89 Eigen::Vector3d h_eig = Eigen::Map<Eigen::Vector3d>(this->
h.
data);
90 Eigen::Vector3d hmr_eig = Eigen::Map<Eigen::Vector3d>(hmr.
data);
91 Eigen::Matrix3d rcrosshcross = h_eig * r_eig.transpose()-r_eig.dot(h_eig)*Eigen::Matrix3d::Identity();
92 Eigen::Matrix3d hmrcrossrcross = r_eig*hmr_eig.transpose()-hmr_eig.dot(r_eig)*Eigen::Matrix3d::Identity();
94 Eigen::Map<Eigen::Matrix3d>(Ib.
data) = Eigen::Map<Eigen::Matrix3d>(this->
I.
data)+rcrosshcross+hmrcrossrcross;