public void BoxReflectInPlaneTest()
{
Box3d b = new Box3d();
List <Point3d> original_points = b.ListOfPoints;
Plane3d s = new Plane3d(new Point3d(-4.1, 7.876, -8), new Vector3d(1.25, -8, -22));
Box3d reflected_box = b.ReflectIn(s);
List <Point3d> reflected_points = reflected_box.ListOfPoints;
foreach (Point3d op in original_points)
{
Point3d reflected_p = op.ReflectIn(s);
foreach (Point3d rp in reflected_points)
{
if (reflected_p == rp)
{
reflected_points.Remove(rp);
break;
}
}
}
Assert.IsTrue(reflected_points.Count == 0);
}
/// <summary>
/// Transforms points from plane space to world space.
/// </summary>
public static V3d[] Unproject(this Plane3d plane, V2d[] points)
{
var local2global = plane.GetPlaneToWorld();
return(points.Map(p => local2global.TransformPos(p.XYO)));
}
/// <summary>
/// Performs RANSAC consensus plane fitting.
/// Returns a least-squares fitted plane to the inliers.
/// </summary>
/// <param name="points">Point Array - here the most dominant plane will be fitted</param>
/// <param name="iterations">Number of RANSAC iterations: depends on the expected ratio between inliers and outliers. Independent of point array's length.</param>
/// <param name="min_consensus">Minimum number of inlier points to be in the plane</param>
/// <param name="epsilon">The +-distance of the points to the plane to be considered (absolute value)</param>
/// <param name="plane">returns the fitted plane</param>
/// <param name="inliers"> Returns the indices of inlier-points </param>
/// <returns>returns true if a valid plane was found</returns>
public static bool FitPlane3dRansac(this V3d[] points, int iterations, int min_consensus, double epsilon,
out Plane3d plane, out int[] inliers)
{
plane = FitPlane3dRansac(points, iterations, min_consensus, epsilon, out inliers);
return(!plane.IsInvalid);
}
/// <summary>
/// Least squares plane fitting to a set of points.
/// </summary>
public static bool FitPlane3dLeastMedianOfSquares(this V3d[] points, out Plane3d plane)
{
plane = FitPlane3dLeastSquares(points);
return(!plane.IsInvalid);
}
/// <summary>
/// Perform one step of the weighted least squares plane fitting
/// </summary>
/// <param name="points">Data points</param>
/// <param name="weights">Corresponding point weights</param>
/// <returns></returns>
private static Plane3d PerformOneStep(V3d[] points, double[] weights)
{
if (points == null)
{
throw new ArgumentNullException(nameof(points));
}
if (weights == null)
{
throw new ArgumentNullException(nameof(weights));
}
if (points.Length != weights.Length)
{
throw new InvalidOperationException();
}
if (points.Length < 3)
{
return(Plane3d.Invalid);
}
if (points.Length == 3)
{
var t0 = points[1] - points[0];
var t1 = points[2] - points[0];
var n = t0.Cross(t1).Normalized;
return(new Plane3d(n, points[0]));
}
V3d c = V3d.Zero;
for (var i = 0; i < points.Length; ++i)
{
c += weights[i] * points[i];
}
c /= weights.Sum();
var data = new double[points.Length * 3];
for (int i = 0; i < points.Length; ++i)
{
data[3 * i + 0] = Fun.Sqrt(weights[i]) * (points[i].X - c.X);
data[3 * i + 1] = Fun.Sqrt(weights[i]) * (points[i].Y - c.Y);
data[3 * i + 2] = Fun.Sqrt(weights[i]) * (points[i].Z - c.Z);
}
var A = new Matrix <double>(data, new V2i(3, points.Length));
A.ComputeSingularValues(out Matrix <double> UT, out Matrix <double> VT, out Matrix <double> S, true);
var normal = new V3d(VT.SubXVectorWindow(2).CopyWindow().Data);
var plane = new Plane3d(normal.Normalized, c);
//-- set the normal to be on the same side as the origin
if (plane.Sign(V3d.OOO) < 0)
{
plane.Reverse();
}
return(plane);
}
/// <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>
/// <param name="plane">The found plane</param>
/// <returns>True if a valid plane was found</returns>
public static bool FitPlane3dWeightedLeastSquares(this V3d[] points, int maxIter, double thres, out double mse, out Plane3d plane)
{
plane = FitPlane3dWeightedLeastSquares(points, maxIter, thres, out mse);
return(!plane.IsInvalid);
}
public static FloatL PlaneEquation(Vector3L point, Plane3d plane)
{
return(Vector3L.Dot(point, plane.m_planeNormal) - plane.GetDistanceFromOrigin());
}
/// <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);
}
public void CodePlane3d(ref Plane3d v)
{
throw new NotImplementedException();
}
public void CodePlane3d(ref Plane3d v)
{
AddValue(v.ToString());
}
/// <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> /// Returns true if this node intersects the space within a given distance to a plane. /// </summary> public static bool Intersects(this PointSetNode self, Plane3d plane, double distance) => self.BoundingBox.Intersects(plane, distance);
public static Segment3d Unproject(this Plane3d _plane, Segment2d _seg)
{
return(new Segment3d(_plane.Evaluate(_seg.P0), _plane.Evaluate(_seg.P1)));
}
/// <summary>Set the <see cref="Position"/> and <see cref="Direction"/> parameters using the <see cref="Plane3d"/>.</summary>
/// <param name="plane"></param>
public void Setup(Plane3d plane)
{
Direction = plane.Normal; Position = plane.Distance * direction;
}
/// <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>
/// 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)
{
return(SplitOnPlane(plane, epsilon, SplitterOptions.NegativeAndPositive));
}
bool IntersectsSegment(ref Plane3d plane, ref Triangle3d triangle, Vector3D end0, Vector3D end1)
{
// Compute the 2D representations of the triangle vertices and the
// segment endpoints relative to the plane of the triangle. Then
// compute the intersection in the 2D space.
// Project the triangle and segment onto the coordinate plane most
// aligned with the plane normal.
int maxNormal = 0;
double fmax = Math.Abs(plane.Normal.x);
double absMax = Math.Abs(plane.Normal.y);
if (absMax > fmax)
{
maxNormal = 1;
fmax = absMax;
}
absMax = Math.Abs(plane.Normal.z);
if (absMax > fmax)
{
maxNormal = 2;
}
Triangle2d projTri = new Triangle2d();
Vector2D projEnd0 = Vector2D.Zero, projEnd1 = Vector2D.Zero;
int i;
if (maxNormal == 0)
{
// Project onto yz-plane.
for (i = 0; i < 3; ++i)
{
projTri[i] = triangle[i].yz;
projEnd0.x = end0.y;
projEnd0.y = end0.z;
projEnd1.x = end1.y;
projEnd1.y = end1.z;
}
}
else if (maxNormal == 1)
{
// Project onto xz-plane.
for (i = 0; i < 3; ++i)
{
projTri[i] = triangle[i].xz;
projEnd0.x = end0.x;
projEnd0.y = end0.z;
projEnd1.x = end1.x;
projEnd1.y = end1.z;
}
}
else
{
// Project onto xy-plane.
for (i = 0; i < 3; ++i)
{
projTri[i] = triangle[i].xy;
projEnd0.x = end0.x;
projEnd0.y = end0.y;
projEnd1.x = end1.x;
projEnd1.y = end1.y;
}
}
Segment2d projSeg = new Segment2d(projEnd0, projEnd1);
IntrSegment2Triangle2 calc = new IntrSegment2Triangle2(projSeg, projTri);
if (!calc.Find())
{
return(false);
}
Vector2dTuple2 intr = new Vector2dTuple2();
if (calc.Type == IntersectionType.Segment)
{
Result = IntersectionResult.Intersects;
Type = IntersectionType.Segment;
Quantity = 2;
intr.V0 = calc.Point0;
intr.V1 = calc.Point1;
}
else
{
Debug.Assert(calc.Type == IntersectionType.Point);
//"Intersection must be a point\n";
Result = IntersectionResult.Intersects;
Type = IntersectionType.Point;
Quantity = 1;
intr.V0 = calc.Point0;
}
// Unproject the segment of intersection.
if (maxNormal == 0)
{
double invNX = ((double)1) / plane.Normal.x;
for (i = 0; i < Quantity; ++i)
{
double y = intr[i].x;
double z = intr[i].y;
double x = invNX * (plane.Constant - plane.Normal.y * y - plane.Normal.z * z);
Points[i] = new Vector3D(x, y, z);
}
}
else if (maxNormal == 1)
{
double invNY = ((double)1) / plane.Normal.y;
for (i = 0; i < Quantity; ++i)
{
double x = intr[i].x;
double z = intr[i].y;
double y = invNY * (plane.Constant - plane.Normal.x * x - plane.Normal.z * z);
Points[i] = new Vector3D(x, y, z);
}
}
else
{
double invNZ = ((double)1) / plane.Normal.z;
for (i = 0; i < Quantity; ++i)
{
double x = intr[i].x;
double y = intr[i].y;
double z = invNZ * (plane.Constant - plane.Normal.x * x - plane.Normal.y * y);
Points[i] = new Vector3D(x, y, z);
}
}
return(true);
}
/// <summary>
/// Splits the supplied PolyMeshes on the specified plane and
/// returns the desired results (specified by the options parameter)
/// in a pair containing lists of PolyMeshes for 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 static (List <PolyMesh>, List <PolyMesh>) SplitOnPlane(
this IEnumerable <PolyMesh> polyMeshes,
Plane3d plane, double epsilon, SplitterOptions options)
{
return(polyMeshes.SplitOnPlane(plane, epsilon, options, s_polyMeshSplitOnPlane));
}
bool GetCoplanarIntersection(ref Plane3d plane, ref Triangle3d tri0, ref Triangle3d tri1)
{
// Project triangles onto coordinate plane most aligned with plane
// normal.
int maxNormal = 0;
double fmax = Math.Abs(plane.Normal.x);
double absMax = Math.Abs(plane.Normal.y);
if (absMax > fmax)
{
maxNormal = 1;
fmax = absMax;
}
absMax = Math.Abs(plane.Normal.z);
if (absMax > fmax)
{
maxNormal = 2;
}
Triangle2d projTri0 = new Triangle2d(), projTri1 = new Triangle2d();
int i;
if (maxNormal == 0)
{
// Project onto yz-plane.
for (i = 0; i < 3; ++i)
{
projTri0[i] = tri0[i].yz;
projTri1[i] = tri1[i].yz;
}
}
else if (maxNormal == 1)
{
// Project onto xz-plane.
for (i = 0; i < 3; ++i)
{
projTri0[i] = tri0[i].xz;
projTri1[i] = tri1[i].xz;
}
}
else
{
// Project onto xy-plane.
for (i = 0; i < 3; ++i)
{
projTri0[i] = tri0[i].xy;
projTri1[i] = tri1[i].xy;
}
}
// 2D triangle intersection routines require counterclockwise ordering.
Vector2D save;
Vector2D edge0 = projTri0[1] - projTri0[0];
Vector2D edge1 = projTri0[2] - projTri0[0];
if (edge0.DotPerp(edge1) < (double)0)
{
// Triangle is clockwise, reorder it.
save = projTri0[1];
projTri0[1] = projTri0[2];
projTri0[2] = save;
}
edge0 = projTri1[1] - projTri1[0];
edge1 = projTri1[2] - projTri1[0];
if (edge0.DotPerp(edge1) < (double)0)
{
// Triangle is clockwise, reorder it.
save = projTri1[1];
projTri1[1] = projTri1[2];
projTri1[2] = save;
}
IntrTriangle2Triangle2 intr = new IntrTriangle2Triangle2(projTri0, projTri1);
if (!intr.Find())
{
return(false);
}
PolygonPoints = new Vector3D[intr.Quantity];
// Map 2D intersections back to the 3D triangle space.
Quantity = intr.Quantity;
if (maxNormal == 0)
{
double invNX = ((double)1) / plane.Normal.x;
for (i = 0; i < Quantity; i++)
{
double y = intr.Points[i].x;
double z = intr.Points[i].y;
double x = invNX * (plane.Constant - plane.Normal.y * y - plane.Normal.z * z);
PolygonPoints[i] = new Vector3D(x, y, z);
}
}
else if (maxNormal == 1)
{
double invNY = ((double)1) / plane.Normal.y;
for (i = 0; i < Quantity; i++)
{
double x = intr.Points[i].x;
double z = intr.Points[i].y;
double y = invNY * (plane.Constant - plane.Normal.x * x - plane.Normal.z * z);
PolygonPoints[i] = new Vector3D(x, y, z);
}
}
else
{
double invNZ = ((double)1) / plane.Normal.z;
for (i = 0; i < Quantity; i++)
{
double x = intr.Points[i].x;
double y = intr.Points[i].y;
double z = invNZ * (plane.Constant - plane.Normal.x * x - plane.Normal.y * y);
PolygonPoints[i] = new Vector3D(x, y, z);
}
}
Result = IntersectionResult.Intersects;
Type = IntersectionType.Polygon;
return(true);
}
public void CodePlane3d(ref Plane3d v)
{
CodeV3d(ref v.Normal); CodeDouble(ref v.Distance);
}
public bool Find()
{
if (Result != IntersectionResult.NotComputed)
{
return(Result != IntersectionResult.NoIntersection);
}
// in this code the results get initialized in subroutines, so we
// set the defautl value here...
Result = IntersectionResult.NoIntersection;
int i, iM, iP;
// Get the plane of triangle0.
Plane3d plane0 = new Plane3d(triangle0.V0, triangle0.V1, triangle0.V2);
// Compute the signed distances of triangle1 vertices to plane0. Use
// an epsilon-thick plane test.
int pos1, neg1, zero1;
Index3i sign1;
Vector3D dist1;
TrianglePlaneRelations(ref triangle1, ref plane0, out dist1, out sign1, out pos1, out neg1, out zero1);
if (pos1 == 3 || neg1 == 3)
{
// Triangle1 is fully on one side of plane0.
return(false);
}
if (zero1 == 3)
{
// Triangle1 is contained by plane0.
if (ReportCoplanarIntersection)
{
return(GetCoplanarIntersection(ref plane0, ref triangle0, ref triangle1));
}
return(false);
}
// Check for grazing contact between triangle1 and plane0.
if (pos1 == 0 || neg1 == 0)
{
if (zero1 == 2)
{
// An edge of triangle1 is in plane0.
for (i = 0; i < 3; ++i)
{
if (sign1[i] != 0)
{
iM = (i + 2) % 3;
iP = (i + 1) % 3;
return(IntersectsSegment(ref plane0, ref triangle0, triangle1[iM], triangle1[iP]));
}
}
}
else // zero1 == 1
// A vertex of triangle1 is in plane0.
{
for (i = 0; i < 3; ++i)
{
if (sign1[i] == 0)
{
return(ContainsPoint(ref triangle0, ref plane0, triangle1[i]));
}
}
}
}
// At this point, triangle1 tranversely intersects plane 0. Compute the
// line segment of intersection. Then test for intersection between this
// segment and triangle 0.
double t;
Vector3D intr0, intr1;
if (zero1 == 0)
{
int iSign = (pos1 == 1 ? +1 : -1);
for (i = 0; i < 3; ++i)
{
if (sign1[i] == iSign)
{
iM = (i + 2) % 3;
iP = (i + 1) % 3;
t = dist1[i] / (dist1[i] - dist1[iM]);
intr0 = triangle1[i] + t * (triangle1[iM] - triangle1[i]);
t = dist1[i] / (dist1[i] - dist1[iP]);
intr1 = triangle1[i] + t * (triangle1[iP] - triangle1[i]);
return(IntersectsSegment(ref plane0, ref triangle0, intr0, intr1));
}
}
}
// zero1 == 1
for (i = 0; i < 3; ++i)
{
if (sign1[i] == 0)
{
iM = (i + 2) % 3;
iP = (i + 1) % 3;
t = dist1[iM] / (dist1[iM] - dist1[iP]);
intr0 = triangle1[iM] + t * (triangle1[iP] - triangle1[iM]);
return(IntersectsSegment(ref plane0, ref triangle0, triangle1[i], intr0));
}
}
// should never get here...
Debug.Assert(false);
return(false);
}
/// <summary>
/// Perform weighted least squares plane fitting
/// </summary>
/// <param name="points">Data points</param>
/// <param name="weights">Point weights</param>
/// <param name="plane">Found plane (out)</param>
/// <returns>True if a valid plane was found</returns>
public static bool FitPlane3dWeightedLeastSquares(this V3d[] points, double[] weights, out Plane3d plane)
{
plane = FitPlane3dWeightedLeastSquares(points, weights);
return(!plane.IsInvalid);
}
public static Plane3d FromTriangle(Triangle3d t)
{
Plane3d result = new Plane3d(Vector3L.Cross(t.m_point1 - t.m_point0, t.m_point2 - t.m_point0), t.m_point0);
return(result);
}
/// <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);
}
public Rect RectHandle(Transform owner, Rect rect, Color lineColor, Color dotColor)
{
Rect resRect = rect;
Vector2[] cornes =
{
new Vector3(rect.x, rect.y), //corneUpLeft
new Vector3(rect.xMax, rect.y), // corneUpRight
new Vector3(rect.x, rect.yMax), //corneDownLeft
new Vector3(rect.xMax, rect.yMax) // corneDownRight
};
Vector2[] cornesS = new Vector2[4];
for (int i = 0; i < 4; i++)
{
cornesS[i] = HandleUtility.WorldToGUIPoint(owner.TransformPoint(cornes[i]));
}
if (Event.current.type == EventType.MouseDown)
{
float minDis = float.MaxValue;
Vector2 mPos = Event.current.mousePosition;
int closestCorn = -1;
for (int i = 0; i < cornesS.Length; i++)
{
float d = Vector2.Distance(cornesS[i], mPos);
if (d < minDis)
{
minDis = d;
closestCorn = i;
}
}
if (minDis < 10)
{
selCorner = closestCorn;
}
else
{
selCorner = -1;
}
}
if (Event.current.type == EventType.MouseUp)
{
selCorner = -1;
}
if (Event.current.type == EventType.MouseDrag && selCorner != -1)
{
Plane3d plnr = new Plane3d(owner.transform.position, owner.transform.rotation);
Vector3 wp = plnr.rayCast(HandleUtility.GUIPointToWorldRay(Event.current.mousePosition));
Vector2 lm = owner.InverseTransformPoint(wp);
if (selCorner == 0)
{
resRect.xMin = lm.x;
resRect.yMin = lm.y;
}
if (selCorner == 1)
{
resRect.xMax = lm.x;
resRect.yMin = lm.y;
}
if (selCorner == 2)
{
resRect.xMin = lm.x;
resRect.yMax = lm.y;
}
if (selCorner == 3)
{
resRect.xMax = lm.x;
resRect.yMax = lm.y;
}
}
Vector3[] polyRect =
{
owner.TransformPoint(resRect.x, resRect.y, 0),
owner.TransformPoint(resRect.xMax, resRect.y, 0),
owner.TransformPoint(resRect.xMax, resRect.yMax, 0),
owner.TransformPoint(resRect.x, resRect.yMax, 0),
owner.TransformPoint(resRect.x, resRect.y, 0)
};
Handles.color = lineColor;
Handles.DrawPolyLine(polyRect);
Handles.color = dotColor;
foreach (var item in polyRect)
{
Handles.DotCap(0, item, owner.rotation, HandleUtility.GetHandleSize(item) * 0.05f);
}
return(resRect);
}
/// <summary>
/// Performs RANSAC consensus plane fitting.
/// Returns a least-squares fitted plane to the inliers.
/// </summary>
/// <param name="points">Point Array - here the most dominant plane will be fitted</param>
/// <param name="iterations">Number of RANSAC iterations: depends on the expected ratio between inliers and outliers. Independent of point array's length.</param>
/// <param name="minConsensus">Minimum number of inlier points to be in the plane</param>
/// <param name="epsilon">The +-distance of the points to the plane to be considered (absolute value)</param>
/// <param name="plane">The found plane</param>
/// <returns>returns true if a valid plane was found</returns>
public static bool FitPlane3dRansac(this V3d[] points, int iterations, int minConsensus, double epsilon, out Plane3d plane)
{
return(FitPlane3dRansac(points, iterations, minConsensus, epsilon, out plane, out int[] inlier));
}
/// <summary>
/// Count points approximately NOT within maxDistance of given plane.
/// Result is always equal or greater than exact number.
/// Faster than CountPointsNotNearPlane.
/// </summary>
public static long CountPointsApproximatelyNotNearPlane(
this PointSet self, Plane3d plane, double maxDistance, int minCellExponent = int.MinValue
)
=> CountPointsApproximatelyNotNearPlane(self.Root.Value, plane, maxDistance, minCellExponent);
public static void Main()
{
// Examples of basic operations with GeometRi
// Global coordinate system is created automatically and can be accessed as "Coord3d.GlobalCS"
Console.WriteLine("Number of defined coordinate systems: {0}", Coord3d.Counts);
Console.WriteLine();
Console.WriteLine("Default coordinate system: ");
Console.WriteLine(Coord3d.GlobalCS.ToString());
Console.WriteLine();
Console.WriteLine();
Console.WriteLine("!!! Find intersection of plane with line !!!");
// Define point and vector in global CS
Point3d p1 = new Point3d(1, -5, -1);
Vector3d v1 = new Vector3d(-2, 3, 4);
// Define line using point and vector
Line3d l1 = new Line3d(p1, v1);
// Define plane using general equation in 3D space in the form "A*x+B*y+C*z+D=0"
Plane3d s1 = new Plane3d(-2, 2, 3, -29);
// Find the intersection of line with plane.
// The results could be point, line or nothing, therefore get result as general object
// and determine it's type.
object obj = l1.IntersectionWith(s1);
if (obj != null)
{
if (obj.GetType() == typeof(Line3d))
{
Console.WriteLine("Intersection is line");
Line3d l2 = (Line3d)obj;
Console.WriteLine(l2.ToString());
}
else if (obj.GetType() == typeof(Point3d))
{
Console.WriteLine("Intersection is point");
Point3d p2 = (Point3d)obj;
Console.WriteLine(p2.ToString());
}
}
// Short variant
// Will cause "InvalidCastException" if the intersection is not a point
Point3d p3 = (Point3d)l1.IntersectionWith(s1);
Console.WriteLine(p3.ToString());
Line3d ll1 = new Line3d(new Point3d(2, 2, 2), new Vector3d(1, 1, 1));
Line3d ll2 = new Line3d(new Point3d(201, 200, 200), new Vector3d(-10, -10, -10));
GeometRi3D.Tolerance = 0.01;
GeometRi3D.UseAbsoluteTolerance = false;
if (ll1.Equals(ll2))
{
}
;
Console.ReadLine();
}
/// <summary>
/// All points NOT within maxDistance of given plane.
/// </summary>
public static IEnumerable <Chunk> QueryPointsNotNearPlane(
this PointSet self, Plane3d plane, double maxDistance, int minCellExponent = int.MinValue
)
=> QueryPointsNotNearPlane(self.Root.Value, plane, maxDistance, minCellExponent);