本文整理汇总了C++中StateType::setIdentity方法的典型用法代码示例。如果您正苦于以下问题：C++ StateType::setIdentity方法的具体用法？C++ StateType::setIdentity怎么用？C++ StateType::setIdentity使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类StateType的用法示例。
在下文中一共展示了StateType::setIdentity方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: quaternionNormSquared
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;
}
}
}示例2: if
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;
}
}