public static double SqrDistXY(GeoPoint a, GeoPoint b) => Sqr(a.coords[0] - b.coords[0]) + Sqr(a.coords[1] - b.coords[1]);
public static double SqrDist(GeoPoint a, GeoPoint b) => a.coords.Zip(b.coords, (ca, cb) => Sqr(ca - cb)).Sum();
public static (GeoPoint[] points, string errMsg) FromPipeCoords(this byte[] raw, double Length, double Z0, double Z1)
{
if (raw == null)
{
return(points : Array.Empty <GeoPoint>(), errMsg : "coords NULL");
}
if (raw.Length < 4)
{
return(points : Array.Empty <GeoPoint>(), errMsg : "coords blobSize < 4");
}
var maxL = Length * 3;
var minL = Length * 0.8;
using (var ms = new MemoryStream(raw))
{
var br = new BinaryReader(ms);
int n = br.ReadInt32();
string errMsg = double.IsNaN((double)Length) ? "MaxLength is NaN" : null;
int calcSize = 4 + n * 16;
if (calcSize < raw.Length)
{
n = (raw.Length - 4) / 16;
errMsg = errMsg.AppendLog("coords calcSize < blobSize");
}
else if (calcSize > raw.Length)
{
n = (raw.Length - 4) / 16;
errMsg = errMsg.AppendLog("coords calcSize > blobSize");
}
if (n < 2)
{
errMsg = errMsg.AppendLog("coords points count < 2");
}
var res = new GeoPoint[n];
var prevX = double.NaN;
var prevY = double.NaN;
int j = 0;
bool withDups = false;
bool withZero = false;
bool withBurst = false;
var SumL = 0d;
for (int i = 0; i < n; i++)
{
var x = br.ReadDouble();
var y = br.ReadDouble();
//if (prevX == x && prevY == y)
//{ withDups = true; continue; }
if (x == 0 && y == 0)
{
withZero = true; continue;
}
var p = new double[] { x, y, 0 };
if (j > 0)
{
if (Enumerable.Range(0, j).Any(k => EqualCoords(res[k].coords[0], x) && EqualCoords(res[k].coords[1], y)))
{
withDups = true; continue;
}
var L = Math.Sqrt(Pow2(x - prevX) + Pow2(y - prevY));
if (SumL + L > maxL)
{
withBurst = true;
continue;
}
SumL += L;
}
prevX = x; prevY = y;
res[j].coords = p;
j++;
}
if (withBurst)
{
if (j < 2)
{
return(points : Array.Empty <GeoPoint>(), errMsg.AppendLog("!no points without bursts"));
}
errMsg = errMsg.AppendLog("possible burst(s) found");
}
if (SumL < minL)
{
errMsg = errMsg.AppendLog("Lcalc < Ldecl");
}
else if (SumL > maxL)
{
errMsg = errMsg.AppendLog("Lcalc > Ldecl");
}
if (j < n)
{
res = res.Take(j).ToArray();
}
if (withDups)
{
errMsg = errMsg.AppendLog("duplicate point(s) found");
}
if (withZero)
{
errMsg = errMsg.AppendLog("null point(s) found");
}
if (res.Length > 0)
{
var dL = 1 / SumL;
var L = 0d;
bool wrongZ = false;
if (Z0 < -12000 || Z0 > 12000)
{
wrongZ = true;
errMsg = errMsg.AppendLog("wrong Z of start node");
}
else
{
res[0].coords[2] = Z0;
}
if (Z1 < -12000 || Z1 > 12000)
{
wrongZ = true;
errMsg = errMsg.AppendLog("wrong Z of end node");
}
else
{
res[res.Length - 1].coords[2] = Z1;
}
if (!wrongZ && Z0 != 0 && Z1 != 0)
{
// interpolate Z between begin and end
for (int i = 1; i < res.Length - 1; i++)
{
L += Math.Sqrt(GeoPoint.SqrDistXY(res[i], res[i - 1]));
var w = L * dL;
var Z = (1 - w) * Z0 + w * Z1;
if (Z == 0)
{
Z = 1e-6;
}
res[i].coords[2] = Z;
}
}
}
return(points : res, errMsg);
}
}