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