本文整理汇总了C++中WayPoints::insert方法的典型用法代码示例。如果您正苦于以下问题:C++ WayPoints::insert方法的具体用法?C++ WayPoints::insert怎么用?C++ WayPoints::insert使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类WayPoints
的用法示例。
在下文中一共展示了WayPoints::insert方法的1个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: addPoints
/**
* @brief Adds points to the band if two existing ones are too far apart (ROBOT_RADIUS)
*
* @param road ...
* @return void
*/
void ElasticBand::addPoints(WayPoints &road, const CurrentTarget ¤tTarget)
{
int offset = 1;
for (int i = 0; i < road.size() - offset; i++)
{
if (i > 0 and road[i].isVisible == false)
break;
WayPoint &w = road[i];
WayPoint &wNext = road[i + 1];
float dist = (w.pos - wNext.pos).norm2();
if (dist > ROAD_STEP_SEPARATION) //SHOULD GET FROM IM
{
float l = 0.9 * ROAD_STEP_SEPARATION / dist; //Crucial que el punto se ponga mas cerca que la condición de entrada
WayPoint wNew((w.pos * (1 - l)) + (wNext.pos * l));
road.insert(i + 1, wNew);
}
}
//ELIMINATED AS REQUESTED BY MANSO
//Move point before last to orient the robot. This works but only if the robots approaches from the lower quadrants
//The angle formed by this point and the last one has to be the same es specified in the target
//We solve this equations for (x,z)
// (x' -x)/(z'-z) = tg(a) = t
// sqr(x'-x) + sqr(z'-z) = sqr(r)
// z = z' - (r/(sqrt(t*t -1)))
// x = x' - r(sqrt(1-(1/t*t+1)))
// if( (currentTarget.hasRotation() == true) and (road.last().hasRotation == false) )
// {
// qDebug() << __FUNCTION__ << "computing rotation" << road.last().pos;
// float radius = 500;
// float ta = tan(currentTarget.getRotation().y());
// float xx = road.last().pos.x() - radius*sqrt(1.f - (1.f/(ta*ta+1)));
// float zz = road.last().pos.z() - (radius/sqrt(ta*ta+1));
// WayPoint wNew( QVec::vec3(xx,road.last().pos.y(),zz) );
// road.insert(road.end()-1,wNew);
// road.last().hasRotation = true;
// qDebug() << __FUNCTION__ << "after rotation" << wNew.pos << currentTarget.getRotation().y() << ta;
//
// }
//else
//qDebug() << road.last().hasRotation << road.last().pos << (road.end()-2)->pos << currentTarget.getRotation().y();
}