float MapSearchNode::GetCost(MapSearchNode &successor)
{
    if(IsSameState(successor)) return 0;

    Segment2D seg(vec, successor.vec);
    double d = seg.length();

    auto obs = getObsCircle();
    for(int i=0; i<obs.size(); i++)
    {
        Vector2D sol1, sol2;
        if(obs[i].intersection(seg, &sol1, &sol2) > 1) d += 1000000;
    }

    //qDebug() << "GetCost" << vec.x << vec.y << "TO" << successor.vec.x << successor.vec.y << "=" << d;
    return d;
}
Example #2
0
	bool IsGoal( const SearchInterface& goal )
	{
		return IsSameState(goal);
	}
bool MapSearchNode::IsGoal(MapSearchNode &nodeGoal)
{
    return IsSameState(nodeGoal);
}