void ompl::base::SO3StateSpace::enforceBounds(State *state) const
{
// see http://stackoverflow.com/questions/11667783/quaternion-and-normalization/12934750#12934750
StateType *qstate = static_cast<StateType*>(state);
double nrmsq = quaternionNormSquared(*qstate);
double error = std::abs(1.0 - nrmsq);
const double epsilon = 2.107342e-08;
if (error < epsilon)
{
double scale = 2.0 / (1.0 + nrmsq);
qstate->x *= scale;
qstate->y *= scale;
qstate->z *= scale;
qstate->w *= scale;
}
else
{
if (nrmsq < 1e-6)
qstate->setIdentity();
else
{
double scale = 1.0 / std::sqrt(nrmsq);
qstate->x *= scale;
qstate->y *= scale;
qstate->z *= scale;
qstate->w *= scale;
}
}
}
void ompl::base::SO3StateSpace::enforceBounds(State *state) const
{
StateType *qstate = static_cast<StateType*>(state);
double nrm = norm(qstate);
if (fabs(nrm) < MAX_QUATERNION_NORM_ERROR)
qstate->setIdentity();
else if (fabs(nrm - 1.0) > MAX_QUATERNION_NORM_ERROR)
{
qstate->x /= nrm;
qstate->y /= nrm;
qstate->z /= nrm;
qstate->w /= nrm;
}
}
