public virtual bool Equals(IPathSegment other) { LinePathSegment lps = other as LinePathSegment; if (lps != null) { return(lps.start == start && lps.end == end); } else { return(false); } }
/// <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; }