/// <summary> /// Steps to node. /// Takes a start and an end node and the argument wether to go straight or in a circle. /// Steps into this direction /// </summary> /// <param name="Start">Start.</param> /// <param name="End">End.</param> private void StepToNodeStraight(RRTNode Start, RRTNode End) { double distance = RRTHelpers.CalculateDistance(Start, End); double angle = RRTHelpers.CalculateAngle(Start, End); double cosArgument = Math.Cos(angle); double sinArgument = Math.Sin(angle); RRTNode lastFoundNode = null; //Lambda function that calculates a new point from a given x value //Checks if the node is valid //Adds it into the list of nodes. //Returns false if point not valid Func <double, bool> CalculateNewPoint = (double x) => { int NewX = Start.Position.X + (int)((double)x * cosArgument); int NewY = Start.Position.Y + (int)((double)x * sinArgument); if (!PointValid(new Point(NewX, NewY))) { if (lastFoundNode == null) { RRTNode BetweenNode = new RRTNode(new Point(NewX, NewY), Start.Orientation, Start); Start.AddSucessor(BetweenNode); lastFoundNode = BetweenNode; BetweenNode.Inverted = End.Inverted; this.AllNodes.Add(BetweenNode); } else { RRTNode BetweenNode = new RRTNode(new Point(NewX, NewY), lastFoundNode.Orientation, lastFoundNode); lastFoundNode.AddSucessor(BetweenNode); lastFoundNode = BetweenNode; BetweenNode.Inverted = End.Inverted; this.AllNodes.Add(BetweenNode); } return(true); } else { return(false); } }; for (double x = 0; x < distance; x += StepWidth) { if (!CalculateNewPoint(x)) { break; } } }
/// <summary> /// Optimize two points taken from the path by trying to connect them via a curve. /// </summary> public bool OptimizeCurves(RRTNode start, RRTNode end) { //Calculate distance double distance = RRTHelpers.CalculateDistance(start, end); //Distance is too small that it makes sense to optimize it if (distance < 10) { return(false); } //Calculate angle delta (angle between orientations) (in degrees) double delta = end.Orientation - start.Orientation; //Calculate angle between points (in radians) double angle = RRTHelpers.CalculateAngle(start, end); //We can't go from inverted to not inverted (not from forward to backwards or the other way round) //NOTE Mit Kurvenfahrt ist das kein Problem, kann aber vermehrt zu Wendeschleifen führen if (start.Inverted != end.Inverted) { return(false); } //Now decide if going straight is way to go //NOTE delta ist entweder sehr klein oder sehr groß (fast 360°, siehe Hilfsfunktion "anglesAreClose" in pseudocode) //NOTE Im zweiten Check prüfe, ob Orientierung und Fahrtwinkel näher zusammenliegen als MaximumDriftAngle if (AnglesAreClose(delta, 0, AllowedOrientationDeviation) && (AnglesAreClose(start.Orientation, angle * RRTHelpers.ToDegree, MaximumDriftAngle))) //if (Math.Abs (delta) < AllowedOrientationDeviation*5 && Math.Abs(angle*RRTHelpers.ToDegree) < MaximumDriftAngle) { //The deviation in the orientation is small enough we can accept going straight (or drift) //And the angle between the points is smaller than the maximum drift we can do //Step straight or in a drift. This function may manipulate the path bool ret = StepStraight(start, end, distance, angle); return(ret); } else { //We try a curve double theta = RRTHelpers.SanatizeAngle(angle * RRTHelpers.ToDegree + Math.Sign(delta) * (180 - Math.Abs(delta)) / 2) * RRTHelpers.ToRadians; double radius = Math.Abs(distance / (2 * Math.Sin(delta * RRTHelpers.ToRadians / 2))); //Check if the radius is > minimum radius if (radius < MinimumRadius) { return(false); } //Calculate middle point (theta is in radians) double midX = start.Position.X + Math.Cos(theta) * radius; double midY = start.Position.Y + Math.Sin(theta) * radius; //RRTHelpers.DrawImportantNode (new RRTNode (new System.Drawing.Point ((int)midX, (int)midY), theta, null), InternalMap, 5, System.Drawing.Color.DarkMagenta); //Theta in radians, delta in degrees -> gamma in degrees double gamma = start.Orientation - RRTHelpers.SanatizeAngle(theta * RRTHelpers.ToDegree - Math.Sign(delta) * 90); double driftAngle = gamma; //In degree if (Math.Abs(driftAngle) / MaximumDriftAngle + MinimumRadius / radius >= 1) { return(false); } bool ret = StepCurve(start, end, delta, new System.Drawing.Point((int)midX, (int)midY), radius, angle, theta); return(ret); } }
/// <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()); }