/* ************************************************************************* */
Unit3 Unit3::retract(const Vector2& v) const {
// Compute the 3D xi_hat vector
Vector3 xi_hat = basis() * v;
double theta = xi_hat.norm();
// Treat case of very small v differently
if (theta < std::numeric_limits<double>::epsilon()) {
return Unit3(cos(theta) * p_ + xi_hat);
}
Vector3 exp_p_xi_hat =
cos(theta) * p_ + xi_hat * (sin(theta) / theta);
return Unit3(exp_p_xi_hat);
}
/* ************************************************************************* */
Unit3 Rot3::unrotate(const Unit3& p,
OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const {
Matrix32 Dp;
Unit3 q = Unit3(unrotate(p.point3(Dp)));
if (Hp) *Hp = q.basis().transpose() * matrix().transpose () * Dp;
if (HR) *HR = q.basis().transpose() * q.skew();
return q;
}
/* ************************************************************************* */
Unit3 Unit3::Random(boost::mt19937 & rng) {
// TODO allow any engine without including all of boost :-(
boost::uniform_on_sphere<double> randomDirection(3);
// This variate_generator object is required for versions of boost somewhere
// around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng).
boost::variate_generator<boost::mt19937&, boost::uniform_on_sphere<double> > generator(
rng, randomDirection);
vector<double> d = generator();
return Unit3(d[0], d[1], d[2]);
}
