void ompl::base::SO3StateSampler::sampleUniformNear(State *state, const State *near, const double distance) { if (distance >= .25 * boost::math::constants::pi<double>()) { sampleUniform(state); return; } double d = rng_.uniform01(); SO3StateSpace::StateType q, *qs = static_cast<SO3StateSpace::StateType*>(state); const SO3StateSpace::StateType *qnear = static_cast<const SO3StateSpace::StateType*>(near); computeAxisAngle(q, rng_.gaussian01(), rng_.gaussian01(), rng_.gaussian01(), 2.*pow(d,1./3.)*distance); quaternionProduct(*qs, *qnear, q); }
void ompl::base::SO3StateSpace::StateType::setAxisAngle(double ax, double ay, double az, double angle) { computeAxisAngle(*this, ax, ay, az, angle); }