/// <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);
}
