/// <summary> /// cos of angle in current point /// </summary> /// <param name="b">prevois point</param> /// <param name="c">next point</param> /// <returns>cos</returns> public double cos_azimut(Point3D b, Point3D c) { if (b == null) { return(1); } if (c == null) { return(0); } if (b == c || this.distNoZ(b) == 0 || this.distNoZ(c) == 0) { return(1); } return((Sqr(this.distNoZ(b)) + Sqr(this.distNoZ(c)) - Sqr(b.distNoZ(c))) / (2 * this.distNoZ(b) * this.distNoZ(c))); }
/// <summary> /// Рубит маршрут на короткие отрезки и прописывает длины /// </summary> /// <param name="path2">original path</param> /// <returns>splitted path</returns> public List<Point3D> SplitPath(List<Point3D> path2) { if (path2.Count < 2) return path2; Point3D c = path2[0]; List<Point3D> rpath = new List<Point3D> { c }; for (var i = 1; i < path2.Count; i++) { Point3D n = path2[i]; if (c.distNoZ(n) < 0.1) continue; // paranoid mode off - skip duplicate //Log("@" + c.ToString() + " => " + n.ToString() + " dist=" + c.dist(n)); double nd = c.distNoZ(n); while (nd > dmax) { nd = c.distNoZ(n); double d = Math.Round(nd / dmax, 0, MidpointRounding.AwayFromZero) - 1; if (d <= n.radius) break; double nx = Math.Round((c.X * d + n.X) / (d + 1), 2); double ny = Math.Round((c.Y * d + n.Y) / (d + 1), 2); c = new Point3D(nx, ny, _core.getZFromHeightMap(nx, ny), ""); c.ndist = nd / d; //this.totaldist += c.ndist; //Err("@@" + c + " => " + n + " dist=" + c.distZ(n) + " d=" + d); rpath.Add(c); //Dbg("@" + c); } c.ndist = c.distNoZ(n); //this.totaldist += c.ndist; c = n; rpath.Add(c); } //this.totaldist = path.Sum(x=> x.ndist); return rpath; }
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); }
/// <summary> /// cos of angle in current point /// </summary> /// <param name="b">prevois point</param> /// <param name="c">next point</param> /// <returns>cos</returns> public double cos_azimut(Point3D b, Point3D c) { if (b == null) return 1; if (c == null) return 0; if (b == c || this.distNoZ(b) == 0 || this.distNoZ(c) == 0) return 1; return ((Sqr(this.distNoZ(b)) + Sqr(this.distNoZ(c)) - Sqr(b.distNoZ(c))) / (2 * this.distNoZ(b) * this.distNoZ(c))); }