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; }
bool IsGoal( const SearchInterface& goal ) { return IsSameState(goal); }
bool MapSearchNode::IsGoal(MapSearchNode &nodeGoal) { return IsSameState(nodeGoal); }