void matrix_exponent(hrp::Matrix33& mexp, const hrp::Vector3& omega, double p) {
double w;
hrp::Vector3 a(omega);
hrp::Matrix33 ahat;
w = omega.norm();
if (eps_eq(w, 0)) {
mexp = hrp::Matrix33::Identity();
return;
}
a.normalize();
outer_product_matrix(ahat, a);
//cout << "omega = " << omega << "w = " << w << " a = " << a << "a^ = " << ahat << endl;
mexp = hrp::Matrix33::Identity() + std::sin(w * p) * ahat + (1 - std::cos(w * p)) * (ahat * ahat);
}
// comparison method
bool operator()(const double& x, const double& y) const
{
return eps_eq(x, y, meps);
}