public static DoubleVector3 FindNearestPointXYZ(DoubleVector3 p1, DoubleVector3 p2, DoubleVector3 query)
{
var p1p2 = p2 - p1;
var p1Query = query - p1;
var p1QueryProjP1P2Component = p1Query.ProjectOntoComponentD(p1p2);
if (p1QueryProjP1P2Component <= 0)
{
return(p1);
}
else if (p1QueryProjP1P2Component >= 1)
{
return(p2);
}
else
{
return(p1 + p1QueryProjP1P2Component * p1p2);
}
}
public bool Equals(DoubleVector3 other) => X == other.X && Y == other.Y && Z == other.Z;
public DoubleVector3 MaxWith(DoubleVector3 o) => new DoubleVector3(Math.Max(X, o.X), Math.Max(Y, o.Y), Math.Max(Z, o.Z));
public DoubleVector3 MinWith(DoubleVector3 o) => new DoubleVector3(Math.Min(X, o.X), Math.Min(Y, o.Y), Math.Min(Z, o.Z));
/// <summary>
/// result * other ~= Proj(this onto other)
/// </summary>
/// <param name="other"></param>
/// <returns></returns>
public double ProjectOntoComponentD(DoubleVector3 other)
{
return(other.Dot(this) / other.SquaredNorm2D());
}
public DoubleVector3 To(DoubleVector3 other) => other - this;
public double Dot(DoubleVector3 other) => X * other.X + Y * other.Y + Z * other.Z;
public static ContourNearestPointResult3 FindNearestPointXYZOnContour(List <IntVector3> contour, DoubleVector3 query)
{
var result = new ContourNearestPointResult3 {
Distance = double.PositiveInfinity,
Query = query
};
var pointCount = contour.First().Equals(contour.Last()) ? contour.Count - 1 : contour.Count;
for (int i = 0; i < pointCount; i++)
{
var p1 = contour[i].ToDoubleVector3();
var p2 = contour[(i + 1) % pointCount].ToDoubleVector3();
var nearestPoint = FindNearestPointXYZ(p1, p2, query);
var distance = (query - nearestPoint).Norm2D();
if (distance < result.Distance)
{
result.Distance = distance;
result.SegmentFirstPointContourIndex = i;
result.NearestPoint = nearestPoint;
}
}
return(result);
}
public static Vector3 ToDotNetVector(this DoubleVector3 v) => new Vector3((float)v.X, (float)v.Y, (float)v.Z);
public static bool IsReal(DoubleVector3 v) => IsReal(v.X) && IsReal(v.Y) && IsReal(v.Z);
