本文整理汇总了C++中Station::setPosition方法的典型用法代码示例。如果您正苦于以下问题：C++ Station::setPosition方法的具体用法？C++ Station::setPosition怎么用？C++ Station::setPosition使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Station的用法示例。
在下文中一共展示了Station::setPosition方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: GRRTsolve
Trajectory PlanningProblem::GRRTsolve()
{
this->planningResult = false;
randomTree.clear();
float start_time = currentTimeMSec();
int tryExtensionCounter = 0;
randomTree.appendNewStation(NULL, initialState);
for(int step=0; step < MAX_RRT_STEP_TRY/5; step++)
{
Station target;
float toss = uni_rand(0, 1);
if(toss < GOAL_PROB)
target.setPosition(goal.goal_point.getPosition());
else {
Station tempSt = SampleStateUniform();
target.setPosition(tempSt.getPosition());
}
if( !target.isValid() )
continue;
// throw "can not sampled!";
SpatialVertex* near_ver = randomTree.getNearestVertex(target);
if(near_ver == NULL)
continue;
int greedyCounter = 0;
while(greedyCounter < 5){
tryExtensionCounter ++;
Station extended = RRTExtend(near_ver->state, target, agent->radius() * 2);
if(!extended.isValid())
break;
randomTree.appendNewStation(near_ver, extended);
if(Station::dubinDistance(extended, target) < agent->radius() ) {
if((target.getPosition() - goal.goal_point.getPosition()).lenght2D() < agent->radius() /2)
planningResult = true;
break;
}
// if(target != goal.goal_point) break;
greedyCounter ++;
}
cout << "Step = " << step << endl;
}
if(planningResult)
{
float finish_time = currentTimeMSec();
this->planningTime = finish_time - start_time;
// cout << "Greedy RRT Planning succeed in " << planningTime << "mili seconds" << endl;
return buildTrajectoryFromRandomTree();
}
return Trajectory();
}示例2: RRTStep
PlanningProblem::ExtendResult PlanningProblem::RRTStep(float extension_len, float max_len)
{
ExtendResult result;
try {
Station rand_st;
float toss = uni_rand(0, 1);
if(toss < GOAL_PROB)
rand_st.set(goal.goal_point);
else
{
if(max_len < INFINITY) {
Vector2D rand_point = randomSampleFromEllipse(initialState.getPosition().to2D(),
goal.goal_point.getPosition().to2D(),
max_len);
Station temp;
temp.setPosition(Vector3D(rand_point, uni_rand(-M_PI, M_PI)));
rand_st.set(temp);
}
else {
Station temp = SampleStateUniform();
rand_st.set(temp);
}
}
if(!rand_st.isValid())
throw "can not sampled!";
SpatialVertex* near_ver = randomTree.getNearestVertex(rand_st);
// if(near_ver->state.isValid())
if(near_ver == NULL)
throw "can not find nearest!";
Station new_st = RRTExtend((near_ver->state), rand_st, extension_len);
if(!new_st.isValid())
{
result = eTrapped;
throw "can not extend tree!";
}
if(goal.minDistTo(new_st) < agent->radius() * 1)
result = eReached;
else
result = eAdvanced;
randomTree.appendNewStation(near_ver, new_st);
} catch (const char* msg)
{
// cerr << "Exception in RRTStep: " << msg << endl;
}
return result;
}