float G::Interval::getDistance(const G::Point2f& pt, G::Point2f* close_pt)const
{
G::Line_2d interline(*this);
G::Point2f line_close_pt;
float lineDist = interline.getDistance(pt, &line_close_pt);
if ( !belongRectangleArea(line_close_pt) )
{
G::Point2f first = getFirst();
G::Point2f second = getSecond();
float first_dist = G::distance(pt, first);
float second_dist = G::distance(pt, second);
if (first_dist < second_dist)
{
lineDist = first_dist;
*close_pt = first;
}
else
{
lineDist = second_dist;
*close_pt = second;
}
}
else
{
*close_pt = line_close_pt;
}
return lineDist;
}
bool NavigationDefault::execute(int rid, Position FinalPos, Position &TargetPos)
{
Vector2D currentLoc = robotLoc(rid,true);
Vector2D nearestOBS;
bool foundNearest = false;
//for oppROBOTS
for(int i=0; i < PLAYERS_MAX_NUM ;i++)
{
if(!_wm->oppRobot[i].isValid)
continue;
Vector2D rbTemp = _wm->oppRobot[i].pos.loc;
if(intersectOBS(rbTemp,currentLoc,FinalPos.loc))
{
if(!foundNearest)
{
nearestOBS = rbTemp;
foundNearest = true;
}
else if (currentLoc.dist2(rbTemp) <= currentLoc.dist2(nearestOBS) )
{
nearestOBS = rbTemp;
}
}
}
//for ourROBOT
for(int i=0; i < PLAYERS_MAX_NUM ; i++)
{
if(!_wm->oppRobot[i].isValid)
continue;
if(i==rid)
continue;
Vector2D rbTemp = _wm->ourRobot[i].pos.loc;
if(intersectOBS(rbTemp,currentLoc,FinalPos.loc))
{
if(!foundNearest)
{
nearestOBS = rbTemp;
foundNearest = true;
}
else if (currentLoc.dist2(rbTemp) <= currentLoc.dist2(nearestOBS) )
{
nearestOBS = rbTemp;
}
}
}
//check ball
if(_wm->ball.isValid)
{
Vector2D balltmp = _wm->ball.pos.loc;
if(intersectOBS(balltmp,currentLoc,FinalPos.loc)){
if(!foundNearest)
{
nearestOBS = balltmp;
foundNearest = true;
}
else if (currentLoc.dist2(balltmp) <= currentLoc.dist2(nearestOBS) )
{
nearestOBS = balltmp;
}
}
}
if(!foundNearest)
TargetPos = FinalPos;
else {
Line2D interline(currentLoc,nearestOBS);
Circle2D obs(nearestOBS,300);
Vector2D res1,res2;
obs.intersection(interline.perpendicular(nearestOBS),&res1,&res2);
if(FinalPos.loc.dist2(res1) <= FinalPos.loc.dist2(res2))
{
FinalPos.loc = res1;
TargetPos = FinalPos;
}else
{
FinalPos.loc = res2;
TargetPos = FinalPos;
}
}
return true;
}
