本文整理汇总了C++中StateType::setYaw方法的典型用法代码示例。如果您正苦于以下问题：C++ StateType::setYaw方法的具体用法？C++ StateType::setYaw怎么用？C++ StateType::setYaw使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类StateType的用法示例。
在下文中一共展示了StateType::setYaw方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: allocState
void ompl::base::DubinsStateSpace::interpolate(const State *from, const DubinsPath& path, double t, State *state) const
{
StateType *s = allocState()->as<StateType>();
double seg = t * path.length(), phi, v;
s->setXY(0., 0.);
s->setYaw(from->as<StateType>()->getYaw());
if (!path.reverse_)
{
for (unsigned int i=0; i<3 && seg>0; ++i)
{
v = std::min(seg, path.length_[i]);
phi = s->getYaw();
seg -= v;
switch(path.type_[i])
{
case DUBINS_LEFT:
s->setXY(s->getX() + sin(phi+v) - sin(phi), s->getY() - cos(phi+v) + cos(phi));
s->setYaw(phi+v);
break;
case DUBINS_RIGHT:
s->setXY(s->getX() - sin(phi-v) + sin(phi), s->getY() + cos(phi-v) - cos(phi));
s->setYaw(phi-v);
break;
case DUBINS_STRAIGHT:
s->setXY(s->getX() + v * cos(phi), s->getY() + v * sin(phi));
break;
}
}
}
else
{
for (unsigned int i=0; i<3 && seg>0; ++i)
{
v = std::min(seg, path.length_[2-i]);
phi = s->getYaw();
seg -= v;
switch(path.type_[2-i])
{
case DUBINS_LEFT:
s->setXY(s->getX() + sin(phi-v) - sin(phi), s->getY() - cos(phi-v) + cos(phi));
s->setYaw(phi-v);
break;
case DUBINS_RIGHT:
s->setXY(s->getX() - sin(phi+v) + sin(phi), s->getY() + cos(phi+v) - cos(phi));
s->setYaw(phi+v);
break;
case DUBINS_STRAIGHT:
s->setXY(s->getX() - v * cos(phi), s->getY() - v * sin(phi));
break;
}
}
}
state->as<StateType>()->setX(s->getX() * rho_ + from->as<StateType>()->getX());
state->as<StateType>()->setY(s->getY() * rho_ + from->as<StateType>()->getY());
getSubSpace(1)->enforceBounds(s->as<SO2StateSpace::StateType>(1));
state->as<StateType>()->setYaw(s->getYaw());
freeState(s);
}