private bool StepCurve(RRTNode node1, RRTNode node2, double delta, System.Drawing.Point middle, double radius, double angle, double theta) { double outerlength = Math.Abs(delta * RRTHelpers.ToRadians * radius); double steps = outerlength / StepWidthCurve; double angleStep = delta / steps; RRTNode start = node1.Clone(); RRTNode end = node2.Clone(); double beforeLength = 0; RRTNode temp = node1; while (temp.Position != end.Position) { if (temp == null || temp.Predecessor == null) { break; } beforeLength += RRTHelpers.CalculateDistance(temp, temp.Predecessor); temp = temp.Predecessor; } RRTNode lastNode = null; bool success = true; for (int i = 0; i < (int)steps; i++) { int NewX = (int)(middle.X - Math.Cos(RRTHelpers.SanatizeAngle(theta * RRTHelpers.ToDegree + i * angleStep) * RRTHelpers.ToRadians) * radius); int NewY = (int)(middle.Y - Math.Sin(RRTHelpers.SanatizeAngle(theta * RRTHelpers.ToDegree + i * angleStep) * RRTHelpers.ToRadians) * radius); double NewOrientation = RRTHelpers.SanatizeAngle(node1.Orientation + i * angleStep); if (InternalMap.IsOccupied(NewX, NewY)) { success = false; break; } RRTNode newNode = null; if (lastNode == null) { newNode = new RRTNode(new System.Drawing.Point(NewX, NewY), NewOrientation, start); newNode.Inverted = start.Inverted; start.Successors.Add(newNode); } else { newNode = new RRTNode(new System.Drawing.Point(NewX, NewY), NewOrientation, lastNode); lastNode.Successors.Add(newNode); newNode.Inverted = lastNode.Inverted; //RRTHelpers.DrawImportantNode (newNode, InternalMap, 5, Color.DarkOrange); } lastNode = newNode; } if (lastNode == null) { success = false; } //We successfully connected them if (success) { double afterLength = 0; temp = lastNode; while (temp.Position != end.Position) { if (temp == null || temp.Predecessor == null) { break; } afterLength += RRTHelpers.CalculateDistance(temp, temp.Predecessor); temp = temp.Predecessor; } if (afterLength > beforeLength) { return(false); } end.Predecessor = lastNode; lastNode.AddSucessor(end); if (node1.Predecessor != null) { node1.Predecessor.Successors.Clear(); node1.Predecessor.AddSucessor(start); start.Predecessor = node1.Predecessor; } else { Console.WriteLine("Node1.Predecessor was null"); } if (node2.Successors.Count > 0) { end.AddSucessor(node2.Successors[0]); node2.Successors[0].Predecessor = end; node2.Predecessor = null; node2.Successors.Clear(); } else { Console.WriteLine("Node2.Successor[0] was null"); } node1.Successors.Clear(); node2.Successors.Clear(); node2.Predecessor = null; node1.Predecessor = null; Path.CalculateLength(); return(true); } return(false); }
private bool StepStraight(RRTNode node1, RRTNode node2, double Distance, double Angle) { RRTNode start = node1.Clone(); RRTNode end = node2.Clone(); RRTNode lastNode = null; bool success = true; //Connect them for (double i = 0; i <= Distance; i += StepWidthStraight) { int NewX = (int)(start.Position.X + i * Math.Cos(Angle)); int NewY = (int)(start.Position.Y + i * Math.Sin(Angle)); if (InternalMap.IsOccupied(NewX, NewY)) { success = false; break; } RRTNode newNode = null; if (lastNode == null) { newNode = new RRTNode(new System.Drawing.Point(NewX, NewY), node1.Orientation, start); newNode.Inverted = start.Inverted; start.Successors.Add(newNode); } else { newNode = new RRTNode(new System.Drawing.Point(NewX, NewY), node1.Orientation, lastNode); lastNode.Successors.Add(newNode); newNode.Inverted = lastNode.Inverted; } lastNode = newNode; } if (lastNode == null) { success = false; } //We successfully connected them if (success) { end.Predecessor = lastNode; lastNode.AddSucessor(end); if (node1.Predecessor != null) { node1.Predecessor.Successors.Clear(); node1.Predecessor.AddSucessor(start); start.Predecessor = node1.Predecessor; } else { Console.WriteLine("Node1.Predecessor was null"); } if (node2.Successors.Count > 0) { end.AddSucessor(node2.Successors[0]); node2.Successors[0].Predecessor = end; node2.Predecessor = null; node2.Successors.Clear(); } else { Console.WriteLine("Node2.Successor[0] was null"); } node1.Successors.Clear(); node2.Successors.Clear(); node2.Predecessor = null; node1.Predecessor = null; Path.CalculateLength(); return(true); } return(false); }
/// <summary> /// Optimize our path so we hit the endpoint /// </summary> public void OptimizeForEndPoint() { //Go along from then nearest point to the endpoint RRTNode previous = Path.Start; Console.WriteLine("Path length before optimization for endpoint: " + Path.Length + " Count: " + Path.CountNodes + " Cost: " + Path.Cost()); Console.WriteLine(); while (previous != null) { if (previous == null) { break; } //Calculate angle delta (angle between orientations) (in degrees) double delta = previous.Orientation - EndPoint.Orientation; double angle = RRTHelpers.CalculateAngle(previous, EndPoint); //Check if the orientation of the selected point is nearly the same as the orientation of the endpoint //if (Math.Abs(previous.Orientation - EndPoint.Orientation) < AllowedOrientationDeviation * 5 || 180 -Math.Abs(previous.Orientation - EndPoint.Orientation) < AllowedOrientationDeviation * 5 ) if (AnglesAreClose(delta, 0, AllowedOrientationDeviation) && (AnglesAreClose(EndPoint.Orientation, angle * RRTHelpers.ToDegree, MaximumDriftAngle))) { //Okey connect them RRTNode selectedNode = previous; RRTNode lastNode = null; //Create a clone we can work on RRTNode start = selectedNode.Clone(); double Distance = RRTHelpers.CalculateDistance(selectedNode, EndPoint); if (Math.Abs(RRTHelpers.SanatizeAngle(angle * RRTHelpers.ToDegree)) > this.MaximumDriftAngle) { previous = previous.Predecessor; continue; } bool success = true; //Connect them for (double i = 0; i <= Distance; i += StepWidthEnd) { //Create new point int NewX = (int)(selectedNode.Position.X + i * Math.Cos(angle)); int NewY = (int)(selectedNode.Position.Y + i * Math.Sin(angle)); //Check if this point is occupied if (InternalMap.IsOccupied(NewX, NewY)) { success = false; break; } RRTNode newNode = null; if (lastNode == null) { newNode = new RRTNode(new System.Drawing.Point(NewX, NewY), start.Orientation, start); start.Successors.Add(newNode); } else { newNode = new RRTNode(new System.Drawing.Point(NewX, NewY), start.Orientation, lastNode); lastNode.Successors.Add(newNode); } lastNode = newNode; } if (lastNode == null) { success = false; } if (success) { Path.Start = lastNode; //Replace the selectNode with our start node. start.Predecessor = selectedNode.Predecessor; selectedNode.Predecessor.Successors.Clear(); selectedNode.Predecessor.AddSucessor(start); selectedNode.Predecessor = null; selectedNode.Successors.Clear(); previous = start; } } previous = previous.Predecessor; } Path.CalculateLength(); Console.WriteLine("Path length after optimization for endpoint: " + Path.Length + " Count: " + Path.CountNodes + " Cost: " + Path.Cost()); }