/// <summary> /// Starting from the goal, recursively follow its parent while inserting it into the path, because it's moving backward /// </summary> /// <param name="goal"></param> /// <param name="pathToAdd"></param> private void GetToRoot(RRTNode goal, ref IPath pathToAdd) { if (goal.IsRoot == false) { IPathSegment segment = new LinePathSegment(goal.Parent.Point, goal.Point); pathToAdd.Insert(0, segment); GetToRoot((RRTNode)goal.Parent, ref pathToAdd); } }
private void ConvertNodeToPath(IPath newNodePath, RRTNode goalNode, List <RobotTwoWheelCommand> inputList) { if (goalNode.IsRoot) { return; } IPathSegment segment = new LinePathSegment(goalNode.Parent.Point, goalNode.Point); newNodePath.Insert(0, segment); inputList.Insert(0, new RobotTwoWheelCommand(goalNode.State.Command.velocity, goalNode.State.Command.turn)); ConvertNodeToPath(newNodePath, goalNode.GetParent(), inputList); }
private Boolean IsLineSegmentClear(LinePathSegment seg, List <Polygon> obstacles) { foreach (Polygon poly in obstacles) { if (poly.IsInside(seg.Start)) { return(false); } if (poly.IsInside(seg.End)) { return(false); } } return(true); }
public Boolean IsEntireLinePathClear(IPath p, List <Polygon> obstacles) { foreach (IPathSegment seg in p) { if (seg is LinePathSegment == false) { return(false); } LinePathSegment lseg = seg as LinePathSegment; if (IsLineSegmentClear(lseg, obstacles) == false) { return(false); } } return(true); }