/// <summary> /// Build route based on project any points to GLONASS arcs /// </summary> /// <param name="point1">First point (will be not included in path)</param> /// <param name="point2">Destinations point</param> /// <param name="mdist1">max distance from start point to GLONASS route</param> /// <param name="mdist2">max distance from destitation to GLONASS route</param> /// <param name="radius">radius of interception (projective) points</param> /// <returns>List of path points, if route fail list with only one destination point (for direct run)</returns> public List<Point3D> GetArcSplitRoute(Point3D point1, Point3D point2, double mdist1, double mdist2, double radius) { bool pass = true; var rdist1 = mdist1; var rdist2 = mdist2; var rndist1 = mdist1; var rndist2 = mdist2; var node1 = this.G.ClosestNode(point1, out rndist1, pass); var node2 = this.G.ClosestNode(point2, out rndist2, pass); var arc1 = this.G.ClosestArc(point1, out rdist1, pass); // TODO optimize var arc2 = this.G.ClosestArc(point2, out rdist2, pass); // if (node1 == node2) { // lasterror = GLONASSLastError.DirectRoute; // return new List<Point3D> { point2 }; // } // if (arc1 == arc2) { // lasterror = GLONASSLastError.DirectRoute; // return new List<Point3D> { point2 }; // } if (rdist1 == -1 || rdist2 == -1 || rndist1 == -1 || rndist2 == -1) { lasterror = GLONASSLastError.GraphIsEmpty; return new List<Point3D> { point2 }; } if (rdist1 > rndist1) rdist1 = rndist1; // node is closest if (rdist2 > rndist2) rdist2 = rndist2; // node is closest if (rdist1 > point1.distZ(point2) && rdist2 > point1.distZ(point2)) { Dbg("Direct route selected"); return new List<Point3D> { point2 }; } Point3D poststartpoint = null; Point3D preendpoint = null; if (rdist1 < rndist1) { poststartpoint = Point3D.ProjectOnLine(point1, arc1.StartNode.Position, arc1.EndNode.Position); poststartpoint.radius = radius; node1 = arc1.StartNode; } if (rdist2 < rndist2) { preendpoint = Point3D.ProjectOnLine(point2, arc2.StartNode.Position, arc2.EndNode.Position); preendpoint.radius = radius; node2 = arc2.StartNode; } if (poststartpoint != null) { if (point1.distZ(poststartpoint) > mdist1) { Dbg("point1.distZ(poststartpoint) =" + point1.distZ(poststartpoint)); lasterror = GLONASSLastError.StartPointTooFar; return new List<Point3D> { point2 }; } } else if (point1.distZ(node1.Position) > mdist1) { Dbg("point1.distZ(node1.Position) =" + point1.distZ(node1.Position)); lasterror = GLONASSLastError.StartPointTooFar; return new List<Point3D> { point2 }; } if (preendpoint != null) { if (point2.distZ(preendpoint) > mdist2) { Dbg("point2.distZ(preendpoint) =" + point2.distZ(preendpoint)); lasterror = GLONASSLastError.DestinationPointTooFar; return new List<Point3D> { point2 }; } } else if (point2.distZ(node2.Position) > mdist2) { Dbg("point2.distZ(node2.Position) =" + point2.distZ(node2.Position)); lasterror = GLONASSLastError.DestinationPointTooFar; return new List<Point3D> { point2 }; } if (!AS.SearchPath(node1, node2)) { lasterror = GLONASSLastError.PathNotFound; Dbg("SearchPath " + lasterror + " node1=" + node1 + " node2=" + node2); return new List<Point3D> { point2 }; } //Dbg("poststartpoint=" + poststartpoint + " dist=" + me.distZ(poststartpoint)); List<Point3D> npath = AS.PathByCoordinates.ToList(); List<Point3D> path2 = new List<Point3D> { }; var mustpoints = 2; if (rdist1 < rndist1) mustpoints++; if (rdist2 < rndist2) mustpoints++; if (npath.Count < mustpoints) { foreach (var np in npath) Dbg("p=" + np); // Dbg("route to " + point2.name + " npath.Count=" + npath.Count + " mustpoints=" + mustpoints); // lasterror = GLONASSLastError.DestinationTooClose; // return new List<Point3D> { point2 }; } // block of removing unneed endpoints if (poststartpoint != null && npath.Count > 2 && ((npath[0] == arc1.StartNode.Position && npath[1] == arc1.EndNode.Position) || (npath[1] == arc1.StartNode.Position && npath[0] == arc1.EndNode.Position))) { Dbg("start node by arc split"); npath.RemoveAt(0); } if (preendpoint != null && npath.Count > 2 && ((npath[npath.Count() - 1] == arc2.StartNode.Position && npath[npath.Count() - 2] == arc2.EndNode.Position) || (npath[npath.Count() - 2] == arc2.StartNode.Position && npath[npath.Count() - 1] == arc2.EndNode.Position))) { Dbg("end node by arc split"); npath.RemoveAt(npath.Count - 1); } // one madness off // adding split points and target if (poststartpoint != null) path2.Add(poststartpoint); path2.AddRange(npath); if (preendpoint != null) path2.Add(preendpoint); path2.Add(point2); Dbg("dists: " + rdist1 + " " + rdist2 + " " + rndist1 + " " + rndist2 + " Path count=" + path2.Count + " lpn=" + point2.name); return path2; }
private List<Point3D> GeneratePath(Point3D destination, Point3D startpoint = null, double dist1 = -1, double dist2 = -1) { if (destination == null) { lasterror = GLONASSLastError.DestinationIsEmpty; return new List<Point3D> { }; } if (dist1 == -1) dist1 = lazydist / 2; if (dist2 == -1) dist2 = lazydist / 2; if (startpoint == null) { startpoint = this.path.LastOrDefault(); if (startpoint == null) { SetMe(); startpoint = me; } } Dbg("GeneratePath from " + startpoint + " to " + destination + " current path:" + this.path.Count); if (startpoint.distNoZ(destination) < (startpoint.radius + destination.radius + 1)) { lasterror = GLONASSLastError.DestinationTooClose; return new List<Point3D> { }; } if (G.Arcs.Count < 1) { lasterror = GLONASSLastError.GraphIsEmpty; return new List<Point3D> { }; } // Point3D c = _core.me; //double dist; //var startnode = this.G.ClosestNode(startpoint, out dist, true); //Dbg("ClosestNode dist=" + dist); //if (startnode == null) { // lasterror = GLONASSLastError.GraphIsEmpty; // return new List<Point3D> { }; //} //if (dist > this.lazydist) { // lasterror = GLONASSLastError.StartPointTooFar; // return new List<Point3D> { }; //} Dbg("destination = " + destination + " startpoint = " + startpoint + " endpoint = " + destination); var newpath = GetArcSplitRoute(startpoint, destination, dist1, dist2, me.radius * 2); //Dbg("newpath.Count=" + newpath.Count); if (newpath.Count == 1 && startpoint.distZ(newpath[0]) > (dist1 + dist2)) { lasterror = GLONASSLastError.DestinationPointTooFar; return new List<Point3D> { }; } if (newpath.Count == 1 && startpoint.distZ(newpath[newpath.Count - 1]) < (startpoint.radius + newpath[newpath.Count - 1].radius + 1)) { lasterror = GLONASSLastError.DestinationTooClose; return new List<Point3D> { }; } //if (newpath.Count == 1) { // return SplitPath(new List<Point3D> { destination }); // direct route //} //Dbg("AS.PathByCoordinates " + AS.PathByCoordinates.Count()); return SplitPath(newpath); }