/// <summary>
/// Projects points onto plane (shortest distance).
/// </summary>
public static V3d[] Project(this Plane3d plane, V3d[] pointArray, int startIndex = 0, int count = 0)
{
if (pointArray == null)
{
throw new ArgumentNullException();
}
if (startIndex < 0 || startIndex >= pointArray.Length)
{
throw new ArgumentOutOfRangeException();
}
if (count < 0 || startIndex + count >= pointArray.Length)
{
throw new ArgumentOutOfRangeException();
}
if (count == 0)
{
count = pointArray.Length - startIndex;
}
var normal = plane.Normal;
var result = new V3d[count];
for (int i = startIndex, j = 0; j < count; i++, j++)
{
var p = pointArray[i];
var height = plane.Height(p);
result[j] = p - height * normal;
}
return(result);
}
/// <summary>
/// All points NOT within maxDistance of given plane.
/// </summary>
public static IEnumerable <Chunk> QueryPointsNotNearPlane(
this IPointCloudNode node, Plane3d plane, double maxDistance, int minCellExponent = int.MinValue
)
=> QueryPoints(node,
n => !node.BoundingBoxExactGlobal.Intersects(plane, maxDistance),
n => plane.Contains(maxDistance, node.BoundingBoxExactGlobal),
p => Math.Abs(plane.Height(p)) > maxDistance,
minCellExponent
);
/// <summary>
/// Count points within maxDistance of given plane.
/// </summary>
public static long CountPointsNearPlane(
this IPointCloudNode node, Plane3d plane, double maxDistance, int minCellExponent = int.MinValue
)
=> CountPoints(node,
n => plane.Contains(maxDistance, node.BoundingBoxExactGlobal),
n => !node.BoundingBoxExactGlobal.Intersects(plane, maxDistance),
p => Math.Abs(plane.Height(p)) <= maxDistance,
minCellExponent
);
/// <summary>
/// All points within maxDistance of given plane.
/// </summary>
public static IEnumerable <Chunk> QueryPointsNearPlane(
this PointSetNode node, Plane3d plane, double maxDistance, int minCellExponent = int.MinValue
)
=> QueryPoints(node,
n => plane.Contains(maxDistance, node.BoundingBox),
n => !node.BoundingBox.Intersects(plane, maxDistance),
p => Math.Abs(plane.Height(p)) <= maxDistance,
minCellExponent
);
/// <summary>
/// Count points NOT within maxDistance of given plane.
/// </summary>
public static long CountPointsNotNearPlane(
this PointSetNode node, Plane3d plane, double maxDistance, int minCellExponent = int.MinValue
)
=> CountPoints(node,
n => !node.BoundingBox.Intersects(plane, maxDistance),
n => plane.Contains(maxDistance, node.BoundingBox),
p => Math.Abs(plane.Height(p)) > maxDistance,
minCellExponent
);
/// <summary>
/// Perform weighted least squares plane fitting
/// </summary>
/// <param name="points">Data points</param>
/// <param name="maxIter">Maximum number of iterations</param>
/// <param name="thres">The algorithm terminates if the change in mean squared error
/// is below the given threshold</param>
/// <param name="mse">Mean squared error</param>
/// <returns>The found plane</returns>
public static Plane3d FitPlane3dWeightedLeastSquares(this V3d[] points, int maxIter, double thres, out double mse)
{
if (points == null)
{
throw new ArgumentNullException(nameof(points));
}
if (points.Length <= 0)
{
throw new InvalidOperationException();
}
if (maxIter <= 0)
{
throw new InvalidOperationException();
}
Plane3d wlsPlane = new Plane3d();
var weights = new double[points.Length].Set(1.0);
var sqDists = new double[weights.Length];
double meanSquaredErrorNew = 0.0;
double ch = double.MaxValue;
for (int iter = 0; iter < maxIter && ch > thres; iter++)
{
double meanSquaredErrorCurrent = meanSquaredErrorNew;
wlsPlane = PerformOneStep(points, weights);
sqDists.SetByIndex(i => Fun.Square(wlsPlane.Height(points[i])));
meanSquaredErrorNew = sqDists.InnerProduct(weights,
(s, w) => s * w,
0.0, (s, m) => s + m) / weights.Aggregate((sum, x) => sum += x);
weights.SetByIndex(i => 1.0 / (sqDists[i] + 1.0));
ch = Fun.Abs(meanSquaredErrorNew - meanSquaredErrorCurrent);
}
;
mse = meanSquaredErrorNew;
return(wlsPlane);
}
/// <summary>
/// Perform least median of squares plane fitting.
/// </summary>
/// <param name="points">Data points</param>
/// <returns>The found plane</returns>
public static Plane3d FitPlane3dLeastMedianOfSquares(this V3d[] points)
{
if (points.Length < 3)
{
return(Plane3d.Invalid);
}
if (points.Length == 3)
{
return(new Plane3d(points.First(), points.ElementAt(1), points.Last()));
}
var plane = new Plane3d();
var rnd = new Random();
var relativeOutlierRatio = 0.4;
var probabilityOfSuccess = 0.999;
var numOfIterations = (int)(Fun.Log(1.0 - probabilityOfSuccess) / Fun.Log(1.0 - (1.0 - relativeOutlierRatio).Pow(3.0)));
var leastMedianOfSquaredResiduals = double.PositiveInfinity;
for (int i = 0; i < numOfIterations; i++)
{
int p0Index, p1Index, p2Index;
bool randomPointsSelected;
do
{
randomPointsSelected = false;
p0Index = rnd.Next(points.Length);
p1Index = rnd.Next(points.Length);
p2Index = rnd.Next(points.Length);
if (p0Index != p1Index && p0Index != p2Index && p1Index != p2Index)
{
randomPointsSelected = true;
}
} while (!randomPointsSelected);
var p0 = points[p0Index];
var p1 = points[p1Index];
var p2 = points[p2Index];
// check if points are degenerated -> same position or all along a line -> continue then
var candidatePlane = new Plane3d(p0, p1, p2);
if (candidatePlane.Normal.LengthSquared.IsTiny())
{
continue;
}
var medianOfSquaredResiduals = points
.Where((p, k) => k != p0Index && k != p1Index && k != p2Index)
.Select(p => candidatePlane.Height(p).Square())
.Median();
if (medianOfSquaredResiduals < leastMedianOfSquaredResiduals)
{
leastMedianOfSquaredResiduals = medianOfSquaredResiduals;
plane = candidatePlane;
}
}
return(plane);
}
/// <summary>
/// Clips the mesh with the given plane.
/// The part on the positive hals will remain.
/// Will return null when everything is clipped.
/// Cap-faces will be built on everything that is cut open by the plane (non-convex cap faces are not handled properly).
/// Only works for meshes without Face-/FaceVertexAttributes -> attributes will be invalid for generated faces.
/// </summary>
public PolyMesh ClipByPlane(Plane3d plane, double epsilon = 1e-7)
{
var clippedMesh = SplitOnPlane(plane, epsilon).Item2;
// in case everything is clipped away
if (clippedMesh == null)
{
return(null);
}
// 1. go trough all edges
// 2. if edge on plane -> test if there is an open face along the plane (border edges)
var vertexOnPlane = clippedMesh.PositionArray.Map(clippedMesh.VertexCount, p => plane.Height(p).Abs() <= epsilon);
clippedMesh.BuildTopology();
// use indices so an edge can be removed no matter what "ref"
var edges = new IntSet(clippedMesh.EdgeCount);
foreach (var e in clippedMesh.Edges)
{
if (e.IsValid)
{
edges.Add(e.Index);
}
}
var capFaceEdges = new List <PolyMesh.Edge>();
var capFaceEdgeSet = new IntSet();
while (edges.Count > 0)
{
var e = clippedMesh.GetEdge(edges.First());
edges.Remove(e.Index);
if (e.IsAnyBorder &&
vertexOnPlane[e.FromVertexIndex] &&
vertexOnPlane[e.ToVertexIndex])
{
// try to find other edges along the plane
// keep set of all edges so in case there the loop runs into a degenerated case an exit is possible (will generate degenerated face)
capFaceEdges.Clear();
capFaceEdgeSet.Clear();
var currEdge = e;
do
{
if (currEdge.IsValid)
{
capFaceEdges.Add(currEdge);
capFaceEdgeSet.Add(currEdge.Index);
}
// find next edge at start-vertex along plane
// the new cap face should have winding order start<-to becaues it is on the opposite of the current face-edge
currEdge = currEdge.FromVertex.Edges
.Select(x => x.FromVertexIndex == currEdge.FromVertexIndex ? x.Opposite : x)
.FirstOrDefault(x => x.IsAnyBorder && x.Index != currEdge.Index && vertexOnPlane[x.FromVertexIndex], PolyMesh.Edge.Invalid);
} while (currEdge.IsValid &&
currEdge.Index != e.Index &&
!capFaceEdgeSet.Contains(currEdge.Index));
if (capFaceEdges.Count > 2)
{
// add cap-face
foreach (var fe in capFaceEdges.Skip(1))
{
edges.Remove(fe.Index);
}
clippedMesh.AddFace(capFaceEdges.Select(fe => fe.ToVertexIndex).ToArray());
}
}
}
// clear topology (is invalid if face has been added)
clippedMesh.ClearTopology();
return(clippedMesh);
}
/// <summary>
/// Splits the mesh on the specified plane in a pair containing the negative
/// (element 0) and the positive side (element 1) of the plane.
/// Note that the normal vector of the plane need not be normalized.
/// </summary>
public (PolyMesh, PolyMesh) SplitOnPlane(
Plane3d plane, double epsilon, SplitterOptions options)
{
var heightArray = m_positionArray.Map(
m_vertexCount, p => plane.Height(p));
var splitter = new PolygonSplitter(
m_firstIndexArray, m_faceCount,
m_vertexIndexArray, m_faceVertexCountRange.Max,
heightArray, epsilon, options);
var result = (default(PolyMesh), default(PolyMesh));
for (int side = 0; side < 2; side++)
{
var fia = splitter.FirstIndexArray(side);
if (fia != null)
{
if (splitter.IsEqualToInput(side))
{
//result[side] = this;
switch (side)
{
case 0:
result = (this, result.Item2);
break;
case 1:
result = (result.Item1, this);
break;
default:
throw new IndexOutOfRangeException();
}
}
else
{
var pm = new PolyMesh()
{
FirstIndexArray = fia,
VertexIndexArray = splitter.VertexIndexArray(side),
InstanceAttributes = InstanceAttributes,
FaceAttributes = FaceIAttributes.Select(
a => splitter.FaceAttribute(side, a)).ToSymbolDict(),
VertexAttributes = VertexIAttributes.Select(
a => splitter.VertexAttribute(side, a)).ToSymbolDict(),
FaceVertexAttributes = FaceVertexIAttributes.Select(
a => splitter.FaceVertexAttribute(side, a)).ToSymbolDict(),
};
//result[side] = pm;
switch (side)
{
case 0:
result = (pm, result.Item2);
break;
case 1:
result = (result.Item1, pm);
break;
default:
throw new IndexOutOfRangeException();
}
}
}
}
return(result);
}
/// <summary>
/// Returns true if this node is fully inside the negative halfspace defined by given plane.
/// </summary>
public static bool InsideNegativeHalfSpace(this PointSetNode self, Plane3d plane)
{
self.BoundingBox.GetMinMaxInDirection(-plane.Normal, out V3d min, out V3d max);
return(plane.Height(min) < 0);
}
/// <summary> /// Returns true if this node intersects the negative halfspace defined by given plane. /// </summary> public static bool IntersectsNegativeHalfSpace(this PointSetNode self, Plane3d plane) => self.Corners.Any(p => plane.Height(p) < 0);
/// <summary> /// Projects a point onto the plane (shortest distance). /// </summary> public static V3d Project(this Plane3d plane, V3d p) => p - plane.Height(p) * plane.Normal;
