public static XyzPoint operator -(XyzPoint p1, XyzPoint p2)
{
XyzPoint res = new XyzPoint();
res.X = p1.X - p2.X;
res.Y = p1.Y - p2.Y;
res.Z = p1.Z - p2.Z;
return res;
}
public static XyzPoint operator *(double s, XyzPoint p)
{
XyzPoint res = new XyzPoint();
res.X = s * p.X;
res.Y = s * p.Y;
res.Z = s * p.Z;
return res;
}
public XyzPoint VectorTo(XyzPoint pnt)
{
return pnt - this;
}
public double DistTo(XyzPoint pnt)
{
XyzPoint d = VectorTo(pnt);
return Math.Sqrt(d.X * d.X + d.Y * d.Y + d.Z * d.Z);
}
public static XyzPoint CenterOfGravity(params XyzPoint[] points)
{
double xSum = 0.0;
double ySum = 0.0;
double zSum = 0.0;
double pntCount = (double)points.Length;
foreach (XyzPoint pnt in points)
{
xSum += pnt.X;
ySum += pnt.Y;
zSum += pnt.Z;
}
XyzPoint res = new XyzPoint();
res.X = xSum / pntCount;
res.Y = ySum / pntCount;
res.Z = zSum / pntCount;
return res;
}
public double MultiplyScalar(XyzPoint vector)
{
return this.X * vector.X + this.Y * vector.Y + this.Z * vector.Z;
}
public XyzPoint MultiplyVector(XyzPoint vector)
{
XyzPoint r = new XyzPoint();
r.X = this.Y * vector.Z - this.Z * vector.Y;
r.Y = this.Z * vector.X - this.X * vector.Z;
r.Z = this.X * vector.Y - this.Y * vector.X;
return r;
}
public BlhPoint GetBlh(XyzPoint p)
{
BlhPoint res = new BlhPoint();
double rp = Calc.Dist(p.X, p.Y);
res.Id = p.Id;
double B0 = EstimateB(p.Z, rp, 0.0); //Przybliżona wartość B
res.B = IterateB(p.Z, rp, B0); // B obliczone iteracyjnie - dokładne
res.L = Math.Acos(p.X / rp);
double sinB = Math.Sin(res.B);
double rn = a / Math.Sqrt(1 - e2 * sinB * sinB);
double DeltaR = rp - rn * Math.Cos(res.B);
double DeltaZ = p.Z - rn * Math.Sin(res.B) + Delta(res.B);
res.H = Calc.Dist(DeltaR, DeltaZ);
if ((DeltaR < 0) || (DeltaZ < 0))
res.H = -res.H;
return res;
}
public XyzPoint GetXyz(BlhPoint p)
{
XyzPoint res = new XyzPoint();
double rn = Rn(p.B);
double rp = (rn + p.H) * Math.Cos(p.B);
res.Id = p.Id;
res.X = rp * Math.Cos(p.L);
res.Y = rp * Math.Sin(p.L);
res.Z = (rn * (1 - e2) + p.H) * Math.Sin(p.B);
return res;
}