/// <summary>
/// Test a linesegment is completely inside a bounded face. For it to be true, it must fulfill:
/// 1. Both endpoints are inside the face
/// 2. the segment does not intersect the face
/// </summary>
/// <param name="F1">The Face</param>
/// <param name="LS">The Line Segment</param>
/// <returns>true=inside; otherwise false</returns>
public static bool inside(Face3D F1, LineSegment3D LS)
{
// Line segment must completely lie on the face
LineSegment3D lineS1 = new LineSegment3D(F1.basePlane.point, LS.startPoint);
LineSegment3D lineS2 = new LineSegment3D(F1.basePlane.point, LS.endPoint);
double d1 = Vector3D.DotProduct(F1.basePlane.normalVector, lineS1.baseLine.direction);
double d2 = Vector3D.DotProduct(F1.basePlane.normalVector, lineS2.baseLine.direction);
if (!MathUtils.equalTol(d1, 0.0, MathUtils.defaultTol) || !MathUtils.equalTol(d2, 0.0, MathUtils.defaultTol))
{
return(false); // The segment does not lie on the face/plane
}
// To optimize the algorithm a little bit, we will change the way we test:
// First we do intersection test between the face and the linesegment
// Second we test at least one point must be inside the face
List <Point3D> iPoints = new List <Point3D>();
bool ret = intersect(F1, LS, out iPoints);
if (ret)
{
return(false); // The segment intersect the face
}
// Test whether at least one point is inside the face
ret = inside(F1, LS.startPoint);
if (ret)
{
return(true);
}
return(false);
}
/// <summary>
/// Testing intersection between a line segment and a polyhedron and return ALL intersection points
/// </summary>
/// <param name="polyH">Polyhedron</param>
/// <param name="lineS">Line segment</param>
/// <param name="intPoints">List of intersection points</param>
/// <returns></returns>
public static bool intersect(Polyhedron polyH, LineSegment3D lineS, out List <Point3D> intPoints)
{
List <Point3D> iPoints = new List <Point3D>();
intPoints = iPoints;
List <Point3D> corners = new List <Point3D>();
corners.Add(lineS.startPoint);
corners.Add(lineS.endPoint);
BoundingBox3D bound = new BoundingBox3D(corners);
List <Face3D> reducedList = Face3D.exclFacesOutsideOfBound(polyH.Faces, bound, 0x111);
if (reducedList.Count == 0)
{
return(false); // no faces left, either they are all on the left or they are all on the right
}
// Now test whether there is any intersection. It needs to complete the test with entire faces to collect the intersection points
for (int i = 0; i < reducedList.Count; i++)
{
List <Point3D> iPts = new List <Point3D>();
if (Face3D.intersect(reducedList[i], lineS, out iPts))
{
for (int j = 0; j < iPts.Count; j++)
{
iPoints.Add(iPts[j]);
}
}
}
if (iPoints.Count > 0)
{
return(true);
}
return(false);
}
public static bool Overlaps(Plane3D P1, LineSegment3D LS)
{
double res = Vector3D.DotProduct(P1.normalVector, LS.baseLine.direction);
bool pOnPlane = pointOnPlane(P1, LS.startPoint);
// if the normals are the same and point in P2 is also point in P1, the two planes are overlapped
return(MathUtils.equalTol(res, 0.0, MathUtils.defaultTol) && pOnPlane);
}
public static CellID64 getSmallestContainingCell(LineSegment3D line)
{
CellID64 llbSmallestCell = CellID64.cellAtMaxDepth(line.startPoint);
CellID64 urtSmallestCell = CellID64.cellAtMaxDepth(line.endPoint);
CellID64 theCell = getCombinedSmallestContainingCell(llbSmallestCell, urtSmallestCell);
return(theCell);
}
public static bool Parallels(Plane3D P1, LineSegment3D LS)
{
double res = Vector3D.DotProduct(P1.normalVector, LS.baseLine.direction);
if (MathUtils.equalTol(res, 0.0, MathUtils.defaultTol))
{
return(true);
}
return(false);
}
/// <summary>
/// Equality test. Use EqualTol instead of the exact comparison
/// </summary>
/// <param name="ob"></param>
/// <returns></returns>
public override bool Equals(object ob)
{
if (ob is LineSegment3D)
{
LineSegment3D li = ob as LineSegment3D;
return(MathUtils.equalTol(_pStart.X, li._pStart.X) && MathUtils.equalTol(_pStart.Y, li._pStart.Y) && MathUtils.equalTol(_pStart.Z, li._pStart.Z) &&
MathUtils.equalTol(_pEnd.X, li._pEnd.X) && MathUtils.equalTol(_pEnd.Y, li._pEnd.Y) && MathUtils.equalTol(_pEnd.Z, li._pEnd.Z));
}
else
{
return(false);
}
}
/// <summary>
/// Calculating an intersection between a Face and a LineSegment
/// </summary>
/// <param name="F1">the Face3D</param>
/// <param name="LS">the Line Segment</param>
/// <param name="intPoint">the intersection point</param>
/// <returns></returns>
public static bool intersect(Face3D F1, LineSegment3D LS, out List <Point3D> intPoints)
{
intPoints = new List <Point3D>();
Point3D intPt = new Point3D();
// There are 2 possible cases: 1. Line punching the face, 2. Line lies on the same plane as the face
// Test whether the line is on the same plane
if (MathUtils.equalTol(Vector3D.DotProduct(F1.basePlane.normalVector, LS.baseLine.direction), 0.0, MathUtils.defaultTol))
{
// test whether at least one point of the segment is on the plane
if (!Plane3D.pointOnPlane(F1.basePlane, LS.startPoint))
{
return(false); // line is parallel with the plane, no intersection
}
LineSegmentIntersectEnum mode = LineSegmentIntersectEnum.Undefined;
for (int i = 0; i < F1.boundaries.Count; i++)
{
bool st = LineSegment3D.intersect(F1.boundaries[i], LS, out intPt, out mode);
if (st)
{
intPoints.Add(intPt);
}
}
if (intPoints.Count > 0)
{
return(true);
}
return(false);
}
else
{
bool res = Plane3D.PLintersect(F1.basePlane, LS, out intPt);
if (res == false)
{
return(false); // intersection occurs beyond the line segment
}
// There is intersection point, test whether the point in within (inside the boundary of the face boundaries
res = inside(F1, intPt);
if (res)
{
intPoints.Add(intPt);
return(true);
}
}
return(false);
}
/// <summary>
/// Testing a point inside a polyhedron is similar with the 2D version of a point inside a polygon by testing intersection between a ray starting from the point.
/// If the number of intersection is odd the point is inside. In 3D the intersection is against the Faces
/// </summary>
/// <param name="polyH"></param>
/// <param name="aPoint"></param>
/// <returns></returns>
public static bool inside(Polyhedron polyH, Point3D aPoint)
{
double extent = 0;
if (Octree.WorldBB == null)
{
extent = Point3D.distance(polyH.boundingBox.LLB, polyH.boundingBox.URT);
}
else
{
extent = Octree.WorldBB.extent;
}
// define a ray using linesegment from the point toward and along +X-axis with 2*the extent of the World BB to ensure ray is always long enough
LineSegment3D ray = new LineSegment3D(aPoint, new Point3D(aPoint.X + 2 * extent, aPoint.Y, aPoint.Z));
List <Face3D> reducedList = Face3D.inclFacesBeyondAxis(polyH.Faces, new Plane3D(aPoint, new Vector3D(1.0, 0.0, 0.0))); // beyond YZ plane
List <Point3D> corners = new List <Point3D>();
corners.Add(aPoint);
corners.Add(aPoint);
BoundingBox3D bound = new BoundingBox3D(corners);
if (reducedList.Count > 0)
{
reducedList = Face3D.exclFacesOutsideOfBound(reducedList, bound, 0x011); // reduce list on Y and Z both direction
}
if (reducedList.Count == 0)
{
return(false); // no faces left, either they are all on the left or they are all on the right
}
int iCount = 0;
for (int i = 0; i < reducedList.Count; i++)
{
List <Point3D> intPts = new List <Point3D>();
if (Face3D.intersect(reducedList[i], ray, out intPts))
{
iCount++;
}
}
if ((iCount % 2) == 1)
{
return(true);
}
return(false);
}
/// <summary>
/// To determine that a line segment is inside (completely inside) of a polyhedron, it must satisfy the following:
/// 1. both end points are inside the polyhedron
/// 2. There no intersection between the segment and the polyhedron
/// </summary>
/// <param name="polyH"></param>
/// <param name="lineS"></param>
/// <returns></returns>
public static bool inside(Polyhedron polyH, LineSegment3D lineS)
{
// reducing the face candidate list is less expensive than inside test, do it first
Point3D leftX = new Point3D();
Point3D rightX = new Point3D();
leftX.X = lineS.startPoint.X < lineS.endPoint.X ? lineS.startPoint.X : lineS.endPoint.X;
rightX.X = lineS.startPoint.X < lineS.endPoint.X ? lineS.endPoint.X : lineS.startPoint.X;
List <Face3D> reducedList = Face3D.inclFacesBeyondAxis(polyH.Faces, new Plane3D(leftX, new Vector3D(1.0, 0.0, 0.0)));
// reducedList = Face3D.exclFacesBeyondAxis(reducedList, rightX); // cannot remove this otherwise inside test for StartPoint may not be correct!!!
List <Point3D> corners = new List <Point3D>();
corners.Add(lineS.startPoint);
corners.Add(lineS.endPoint);
BoundingBox3D bound = new BoundingBox3D(corners);
if (reducedList.Count > 0)
{
reducedList = Face3D.exclFacesOutsideOfBound(reducedList, bound, 0x011); // reduce list on Y and Z both direction
}
if (reducedList.Count == 0)
{
return(false); // no faces left, either they are all on the left or they are all on the right
}
// inside test for both segment ends. Test one by one so that we can exit when any one of them are not inside
if (!inside(polyH, lineS.startPoint))
{
return(false);
}
if (!inside(polyH, lineS.endPoint))
{
return(false);
}
// Now test whether there is any intersection. If there is, the segment is not completely inside
for (int i = 0; i < reducedList.Count; i++)
{
List <Point3D> iPoints = new List <Point3D>();
if (Face3D.intersect(reducedList[i], lineS, out iPoints))
{
return(false);
}
}
return(true);
}
/// <summary>
/// Plane and a linesegment intersection
/// </summary>
/// <param name="P1">Plane</param>
/// <param name="L1">Linesegment</param>
/// <param name="intersectingPoint">Intersection Point</param>
/// <returns></returns>
public static bool PLintersect(Plane3D P1, LineSegment3D L1, out Point3D intersectingPoint)
{
intersectingPoint = new Point3D();
double denom = Vector3D.DotProduct(P1.normalVector, L1.unNormalizedVector);
if (MathUtils.equalTol(denom, 0.0, MathUtils.defaultTol))
{
return(false); // Normal and the lines are perpendicular to each other: line is parallel to the plane, no intersection
}
double r = Vector3D.DotProduct(P1.normalVector, new Vector3D(P1.point.X - L1.startPoint.X, P1.point.Y - L1.startPoint.Y, P1.point.Z - L1.startPoint.Z))
/ denom;
if (r < 0.0 || r > 1.0)
{
return(false); // intersection occurs outside of the segment
}
intersectingPoint.X = L1.startPoint.X + r * L1.unNormalizedVector.X;
intersectingPoint.Y = L1.startPoint.Y + r * L1.unNormalizedVector.Y;
intersectingPoint.Z = L1.startPoint.Z + r * L1.unNormalizedVector.Z;
return(true);
}
/// <summary>
/// Test whether a Face (bounded plane) intersects with a plane
/// </summary>
/// <param name="P1">Plane</param>
/// <param name="F1">Face</param>
/// <param name="intersectingLine">Resulting intercting linesegment (bounded)</param>
/// <returns></returns>
public static bool PPintersect(Plane3D P1, Face3D F1, out LineSegment3D intersectingLine)
{
Line3D intLine = new Line3D();
LineSegment3D ls = new LineSegment3D(new Point3D(0.0, 0.0, 0.0), new Point3D(1.0, 1.0, 1.0));
intersectingLine = ls;
// Intersect operation: get the intersection (unbounded) line
if (!PPintersect(P1, F1.basePlane, out intLine))
{
return(false);
}
// Now needs to check get the segment by getting intersecting points between the line and the Face boundaries
List <Point3D> intPts = new List <Point3D>();
// Use line segment that is large enough to ensure it covers the extent of the face
//double extent = Point3D.distance(F1.boundingBox.LLB, F1.boundingBox.URT) * 10; // Bug: NOT big enough for some cases!, use worldBB extent
double extent;
if (Octree.WorldBB == null)
{
extent = 1000000000;
}
else
{
extent = Octree.WorldBB.extent * 10;
}
LineSegment3D intLS = new LineSegment3D(new Point3D(intLine.point - (intLine.direction * extent)), new Point3D(intLine.point + (intLine.direction * extent)));
bool res = Face3D.intersect(F1, intLS, out intPts);
if (res)
{
intersectingLine.startPoint = intPts[0];
intersectingLine.endPoint = intPts[intPts.Count - 1];
return(true);
}
return(false);
}
static void generateEdges(List <Point3D> vertices, out List <LineSegment3D> edgeLIst, out List <int> nonColinearEdgesIdx)
{
edgeLIst = new List <LineSegment3D>();
nonColinearEdgesIdx = new List <int>();
for (int i = 0; i < vertices.Count; i++)
{
LineSegment3D edge = null;
if (i == vertices.Count - 1)
{
if (vertices[i] != vertices[0]) // add only the last edge with the first vertex if they are not explicitly closed
{
edge = new LineSegment3D(vertices[i], vertices[0]);
}
}
else
{
edge = new LineSegment3D(vertices[i], vertices[i + 1]);
}
if (edge != null)
{
edgeLIst.Add(edge);
if (i > 0 && i < vertices.Count)
{
if (edgeLIst[i].baseLine.direction != edgeLIst[i - 1].baseLine.direction)
{
nonColinearEdgesIdx.Add(i - 1);
}
}
if (i == vertices.Count - 1)
{
if (edgeLIst[i].baseLine.direction != edgeLIst[0].baseLine.direction)
{
nonColinearEdgesIdx.Add(i);
}
}
}
}
}
public void Reverse()
{
// First reverse the vertex list, and then regenerate the rest
_vertices.Reverse();
_boundaryLines.Clear();
_nonColinearEdgesIdx.Clear();
for (int i = 0; i < vertices.Count; i++)
{
LineSegment3D edge;
if (i == vertices.Count - 1)
{
edge = new LineSegment3D(vertices[i], vertices[0]);
}
else
{
edge = new LineSegment3D(vertices[i], vertices[i + 1]);
}
_boundaryLines.Add(edge);
if (i > 0 && i < vertices.Count)
{
if (_boundaryLines[i].baseLine.direction != _boundaryLines[i - 1].baseLine.direction)
{
_nonColinearEdgesIdx.Add(i - 1);
}
}
if (i == vertices.Count - 1)
{
if (_boundaryLines[i].baseLine.direction != _boundaryLines[0].baseLine.direction)
{
_nonColinearEdgesIdx.Add(i);
}
}
}
_basePlane = new Plane3D(vertices[0], _boundaryLines[_nonColinearEdgesIdx[0]].baseLine.direction, _boundaryLines[_nonColinearEdgesIdx[1]].baseLine.direction);
containingBB = new BoundingBox3D(vertices);
}
/// <summary>
/// Test a face is completely inside another face. For it to be true, it must fulfill:
/// 1. All the vertices are inside the other face
/// 2. There is no intersection between the edges with the other face
/// </summary>
/// <param name="F1">The Face</param>
/// <param name="F2">The other face to test whether it is inside the first face</param>
/// <returns>true=inside, otherwise false</returns>
public static bool inside(Face3D F1, Face3D F2)
{
// Do intersection first
for (int i = 0; i < F1.boundaries.Count; i++)
{
for (int j = 0; j < F2.boundaries.Count; j++)
{
Point3D iPoint = new Point3D();
LineSegmentIntersectEnum mode;
if (LineSegment3D.intersect(F1.boundaries[i], F2.boundaries[j], out iPoint, out mode))
{
return(false); // there is intersection
}
}
}
// If there is no intersection, at least one vertex of F2 must be inside F1
if (inside(F1, F2.vertices[0]))
{
return(true); // one vertex of F2 is inside F1 and there is no intersection
}
return(false);
}
/// <summary>
/// Test intersection between a polyhedron and a face. There is optmization applied for axis-aligned face (useful for Octree cells as they are all axis aligned)
/// </summary>
/// <param name="polyH">The Polyhedron</param>
/// <param name="face">The face to test the intersection</param>
/// <returns>true=intersected; false otherwise</returns>
public static bool intersect(Polyhedron polyH, Face3D face)
{
List <Face3D> faceList = new List <Face3D>();
BoundingBox3D bound = new BoundingBox3D(face.vertices);
faceList = Face3D.exclFacesOutsideOfBound(polyH.Faces, bound, 0x111);
if (faceList.Count == 0)
{
return(false); // There is no face remaining to test, return false
}
for (int i = 0; i < faceList.Count; i++)
{
FaceIntersectEnum mode;
LineSegment3D intL = new LineSegment3D(new Point3D(), new Point3D());
bool status = Face3D.intersect(face, faceList[i], out intL, out mode);
if (status == true)
{
return(true); // return true as soon as an intersection is detected
}
}
return(false);
}
/// <summary>
/// Compute Octree for a Line Segment
/// </summary>
/// <param name="elementID"></param>
/// <param name="lineS"></param>
/// <param name="forUserDict"></param>
public void ComputeOctree(string elementID, LineSegment3D lineS, bool forUserDict)
{
// Make sure ElementID string is 22 character long for correct encoding/decoding
if (elementID.Length < 22)
{
elementID = elementID.PadLeft(22, '0');
}
ElementID eidNo = new ElementID(elementID);
Tuple <UInt64, UInt64> elementIDNo = eidNo.ElementIDNo;
OctreeNode theTree = new OctreeNode();
// Add a step:
// 1. Find the smallest containing cell based on the Face BB, it it to quickly eliminate the irrelevant cells very quickly
theTree.nodeCellID = OctreeNodeProcess.getSmallestContainingCell(lineS);
theTree._depth = theTree.nodeCellID.Level;
OctreeNodeProcess.Process(theTree, lineS);
List <CellID64> collCellID;
List <int> collBorderFlag;
OctreeNodeProcess.collectCellIDs(theTree, out collCellID, out collBorderFlag);
for (int i = 0; i < collCellID.Count; ++i)
{
if (forUserDict)
{
insertDataToUserDict(elementIDNo, collCellID[i], collBorderFlag[i], false);
}
else
{
//insertDataToDictDB(elementID, collCellID[i]);
insertDataToDict(elementIDNo, collCellID[i]);
}
}
}
/// <summary>
/// Testing intersection between a line segment and a polyhedron only. It stops at the first intersection
/// </summary>
/// <param name="polyH">polyhedron</param>
/// <param name="lineS">Line segment</param>
/// <returns></returns>
public static bool intersect(Polyhedron polyH, LineSegment3D lineS)
{
List <Point3D> corners = new List <Point3D>();
corners.Add(lineS.startPoint);
corners.Add(lineS.endPoint);
BoundingBox3D bound = new BoundingBox3D(corners);
List <Face3D> reducedList = Face3D.exclFacesOutsideOfBound(polyH.Faces, bound, 0x111);
if (reducedList.Count == 0)
{
return(false); // no faces left, either they are all on the left or they are all on the right
}
// Now test whether there is any intersection.
for (int i = 0; i < reducedList.Count; i++)
{
List <Point3D> iPoints = new List <Point3D>();
if (Face3D.intersect(reducedList[i], lineS, out iPoints))
{
return(true);
}
}
return(false);
}
public LineSegment3DLW(LineSegment3D l3d)
{
startpoint = new Point3DLW(l3d.startPoint);
endpoint = new Point3DLW(l3d.endPoint);
}
/// <summary>
/// Testing whether 2 line segments intersect
/// </summary>
/// <param name="L1">Line 1</param>
/// <param name="L2">Line 2</param>
/// <param name="intersectionPoint">Intersection point</param>
/// <param name="mode">enumeration that identifies how the two lines intersect</param>
/// <returns></returns>
public static bool intersect(LineSegment3D L1, LineSegment3D L2, out Point3D intersectionPoint, out LineSegmentIntersectEnum mode)
{
mode = LineSegmentIntersectEnum.Undefined;
Point3D ip = new Point3D();
intersectionPoint = ip;
if (Line3D.parallel(L1.baseLine, L2.baseLine))
{
return(false); // Lines are parallel, no intersection
}
// With segments AB and CD, we get Pab = (1-s)A + sB, and Qab = (1-t)C + D
// For intersection Pab = Qcd and therefore (1-s)A + sB = (1-t)C + tD. We get s(B-A) - t(D-C) = C-A for some s,t
// using matrix operation we can get s and t using X and Y, and also using X and Z. If they intersect s and t have to be equal for both
double a = L1.endPoint.X - L1.startPoint.X;
double b = L2.startPoint.X - L2.endPoint.X;
double c = L1.endPoint.Y - L1.startPoint.Y;
double d = L2.startPoint.Y - L2.endPoint.Y;
double e = L1.endPoint.Z - L1.startPoint.Z;
double f = L2.startPoint.Z - L2.endPoint.Z;
double g = L2.startPoint.X - L1.startPoint.X;
double h = L2.startPoint.Y - L1.startPoint.Y;
double i = L2.startPoint.Z - L1.startPoint.Z;
// Work on special case when the lines are 2D
if ((MathUtils.equalTol(a, 0.0) && MathUtils.equalTol(b, 0.0) && MathUtils.equalTol(g, 0.0)) ||
(MathUtils.equalTol(e, 0.0) && MathUtils.equalTol(f, 0.0) && MathUtils.equalTol(i, 0.0)) ||
(MathUtils.equalTol(c, 0.0) && MathUtils.equalTol(d, 0.0) && MathUtils.equalTol(h, 0.0)))
{
double s = -1.0;
double t = -1.0;
// 2D line segments on Y-Z plane (X = 0)
if (MathUtils.equalTol(a, 0.0) && MathUtils.equalTol(b, 0.0) && MathUtils.equalTol(g, 0.0))
{
s = (d * i - h * f) / (d * e - c * f);
t = (e * h - i * c) / (d * e - c * f);
}
// 2D line segments on X-Y plane (Z = 0)
else if (MathUtils.equalTol(e, 0.0) && MathUtils.equalTol(f, 0.0) && MathUtils.equalTol(i, 0.0))
{
s = (b * h - g * d) / (b * c - a * d);
t = (g * c - h * a) / (b * c - a * d);
}
// 2D line segment on X-Z plane (Y = 0)
else if (MathUtils.equalTol(c, 0.0) && MathUtils.equalTol(d, 0.0) && MathUtils.equalTol(h, 0.0))
{
s = (b * i - g * f) / (b * e - a * f);
t = (g * e - i * a) / (b * e - a * f);
}
// calculate intersection point
ip.X = (1 - s) * L1.startPoint.X + s * L1.endPoint.X;
ip.Y = (1 - s) * L1.startPoint.Y + s * L1.endPoint.Y;
ip.Z = (1 - s) * L1.startPoint.Z + s * L1.endPoint.Z;
intersectionPoint = ip;
if ((0 <= s && s <= 1) && (0 <= t && t <= 1))
{
// If the segments intersect s and t have to be between 0 and 1
mode = LineSegmentIntersectEnum.IntersectedWithinSegments;
return(true);
}
else
{
// Lines intersect but intersection occurs outside of the segments
mode = LineSegmentIntersectEnum.IntersectedOutsideSegments;
return(false);
}
}
// Line segments are real 3D lines
{
// on X-Y
double s1 = (b * h - g * d) / (b * c - a * d);
double t1 = (g * c - h * a) / (b * c - a * d);
// on X-Z
double s2 = (b * i - g * f) / (b * e - a * f);
double t2 = (g * e - i * a) / (b * e - a * f);
// on Y-Z
double s3 = (d * i - h * f) / (d * e - c * f);
double t3 = (e * h - i * c) / (d * e - c * f);
// When the result of calculation gives infinity or NaN, the line is somewhat aligned, so it can hav any value. Set it here to be the same with one other
if (double.IsInfinity(s1) || double.IsNaN(s1))
{
if (double.IsInfinity(s2) || double.IsNaN(s2))
{
if (double.IsInfinity(s3) || double.IsNaN(s3))
{
s1 = 0.0;
}
else
{
s1 = s3;
}
}
else
{
s1 = s2;
}
}
if (double.IsInfinity(s2) || double.IsNaN(s2))
{
if (double.IsInfinity(s1) || double.IsNaN(s1))
{
if (double.IsInfinity(s3) || double.IsNaN(s3))
{
s2 = 0.0;
}
else
{
s2 = s3;
}
}
else
{
s2 = s1;
}
}
if (double.IsInfinity(s3) || double.IsNaN(s3))
{
if (double.IsInfinity(s1) || double.IsNaN(s1))
{
if (double.IsInfinity(s2) || double.IsNaN(s2))
{
s3 = 0.0;
}
else
{
s3 = s2;
}
}
else
{
s3 = s1;
}
}
// When the result of calculation gives infinity or NaN, the line is somewhat aligned, so it can hav any value. Set it here to be the same with one other
if (double.IsInfinity(t1) || double.IsNaN(t1))
{
if (double.IsInfinity(t2) || double.IsNaN(t2))
{
if (double.IsInfinity(t3) || double.IsNaN(t3))
{
t1 = 0.0;
}
else
{
t1 = t3;
}
}
else
{
t1 = t2;
}
}
if (double.IsInfinity(t2) || double.IsNaN(t2))
{
if (double.IsInfinity(t1) || double.IsNaN(t1))
{
if (double.IsInfinity(t3) || double.IsNaN(t3))
{
t2 = 0.0;
}
else
{
t2 = t3;
}
}
else
{
t2 = t1;
}
}
if (double.IsInfinity(t3) || double.IsNaN(t3))
{
if (double.IsInfinity(t1) || double.IsNaN(t1))
{
if (double.IsInfinity(t2) || double.IsNaN(t2))
{
t3 = 0.0;
}
else
{
t3 = t2;
}
}
else
{
t3 = t1;
}
}
// Lines intersect when s1=s2 and t1=t2 and als s1=s3 and t1=t3
if (MathUtils.equalTol(s1, s2, MathUtils.defaultTol) && MathUtils.equalTol(t1, t2, MathUtils.defaultTol) && MathUtils.equalTol(s1, s3, MathUtils.defaultTol) && MathUtils.equalTol(t1, t3, MathUtils.defaultTol))
{
// calculate intersection point
ip.X = (1 - s1) * L1.startPoint.X + s1 * L1.endPoint.X;
ip.Y = (1 - s1) * L1.startPoint.Y + s1 * L1.endPoint.Y;
ip.Z = (1 - s1) * L1.startPoint.Z + s1 * L1.endPoint.Z;
intersectionPoint = ip;
if ((0 <= s1 && s1 <= 1) && (0 <= t1 && t1 <= 1))
{
// If the segments intersect s and t have to be between 0 and 1
mode = LineSegmentIntersectEnum.IntersectedWithinSegments;
return(true);
}
else
{
// Lines intersect but intersection occurs outside of the segments
mode = LineSegmentIntersectEnum.IntersectedOutsideSegments;
return(false);
}
}
}
return(false);
}
/// <summary>
/// Testing whether 2 line segments overlapped
/// </summary>
/// <param name="lineSegment1">First line segment</param>
/// <param name="lineSegment2">Second line segment</param>
/// <param name="overlappedSegment">The resulting overlapped line segment</param>
/// <param name="mode">The enumeration of the result</param>
/// <returns></returns>
public static bool overlap(LineSegment3D lineSegment1, LineSegment3D lineSegment2, out LineSegment3D overlappedSegment, out LineSegmentOverlapEnum mode)
{
mode = LineSegmentOverlapEnum.Undefined;
overlappedSegment = lineSegment1;
// First check whether they have the same lineDirection (absolute: -v = +v for overlap test)
if (!Line3D.parallel(lineSegment1.baseLine, lineSegment2.baseLine))
{
return(false);
}
// The lines are parallel, now check whether there is at least one point falls in the segment
// Test points in line 2 on line 1
LineSegmentOnSegmentEnum p1l1_s, p2l1_s;
LineSegmentOnSegmentEnum p1l2_s, p2l2_s;
bool p1l1 = isInSegment(lineSegment1, lineSegment2.startPoint, out p1l1_s);
bool p2l1 = isInSegment(lineSegment1, lineSegment2.endPoint, out p2l1_s);
if (p1l1 && p2l1)
{
if ((p1l1_s == p2l1_s && p2l1_s == LineSegmentOnSegmentEnum.InsideSegment) ||
(p1l1_s == LineSegmentOnSegmentEnum.CoincideEndSegment && p2l1_s == LineSegmentOnSegmentEnum.InsideSegment) ||
(p1l1_s == LineSegmentOnSegmentEnum.InsideSegment && p2l1_s == LineSegmentOnSegmentEnum.CoincideEndSegment))
{
mode = LineSegmentOverlapEnum.SubSegment;
}
if (p1l1_s == p2l1_s && p1l1_s == LineSegmentOnSegmentEnum.CoincideEndSegment)
{
mode = LineSegmentOverlapEnum.ExactlyOverlap;
}
overlappedSegment = lineSegment2;
return(true);
}
else if (p1l1 || p2l1)
{
if (p1l1_s == LineSegmentOnSegmentEnum.CoincideEndSegment || p2l1_s == LineSegmentOnSegmentEnum.CoincideEndSegment)
{
mode = LineSegmentOverlapEnum.Touch;
// Zero lenth line segments = a point, a valid data
if (p1l1)
{
overlappedSegment = new LineSegment3D(lineSegment2.startPoint, lineSegment2.startPoint);
}
if (p2l1)
{
overlappedSegment = new LineSegment3D(lineSegment2.endPoint, lineSegment2.endPoint);
}
}
if (p1l1_s == LineSegmentOnSegmentEnum.InsideSegment || p2l1_s == LineSegmentOnSegmentEnum.InsideSegment)
{
bool p1l2 = isInSegment(lineSegment2, lineSegment1.startPoint, out p1l2_s);
bool p2l2 = isInSegment(lineSegment2, lineSegment1.endPoint, out p2l2_s);
if (p1l1_s == LineSegmentOnSegmentEnum.InsideSegment && p1l2_s == LineSegmentOnSegmentEnum.InsideSegment)
{
overlappedSegment = new LineSegment3D(lineSegment2.startPoint, lineSegment1.startPoint);
}
if (p2l1_s == LineSegmentOnSegmentEnum.InsideSegment && p1l2_s == LineSegmentOnSegmentEnum.InsideSegment)
{
overlappedSegment = new LineSegment3D(lineSegment2.endPoint, lineSegment1.startPoint);
}
if (p1l1_s == LineSegmentOnSegmentEnum.InsideSegment && p2l2_s == LineSegmentOnSegmentEnum.InsideSegment)
{
overlappedSegment = new LineSegment3D(lineSegment2.startPoint, lineSegment1.endPoint);
}
if (p2l1_s == LineSegmentOnSegmentEnum.InsideSegment && p2l2_s == LineSegmentOnSegmentEnum.InsideSegment)
{
overlappedSegment = new LineSegment3D(lineSegment2.endPoint, lineSegment1.endPoint);
}
mode = LineSegmentOverlapEnum.PartiallyOverlap;
}
return(true);
}
else
{
// if both p1l1 and p2l1 are false, there is probability that the l1 are inside the l2 instead. Only this case needs to be checked since other
// cases should be covered by the previous conditional checks
bool p1l2 = isInSegment(lineSegment2, lineSegment1.startPoint, out p1l2_s);
bool p2l2 = isInSegment(lineSegment2, lineSegment1.endPoint, out p2l2_s);
if ((p1l2 && p2l2) &&
(p1l2_s == p2l2_s && p1l2_s == LineSegmentOnSegmentEnum.InsideSegment))
{
mode = LineSegmentOverlapEnum.SuperSegment;
overlappedSegment = lineSegment1;
return(true);
}
}
return(false);
}
/// <summary>
/// Testing whether a point lies on the segment
/// </summary>
/// <param name="lineSegment">Line segment</param>
/// <param name="thePoint">Point</param>
/// <param name="status">Returnign an enumeration to identify where the point is relative to the line segment</param>
/// <returns></returns>
public static bool isInSegment(LineSegment3D lineSegment, Point3D thePoint, out LineSegmentOnSegmentEnum status)
{
status = LineSegmentOnSegmentEnum.Undefined;
double tx = (thePoint.X - lineSegment.startPoint.X) / (lineSegment.unNormalizedVector.X);
double ty = (thePoint.Y - lineSegment.startPoint.Y) / (lineSegment.unNormalizedVector.Y);
double tz = (thePoint.Z - lineSegment.startPoint.Z) / (lineSegment.unNormalizedVector.Z);
// Handle in case the vector is parallel to an axis (hence the value is 0), in this case any value is acceptable and therefore should be set the same as at least one other
// that has a valid value
if (MathUtils.equalTol(lineSegment.unNormalizedVector.X, 0.0))
{
if (MathUtils.equalTol(lineSegment.unNormalizedVector.Y, 0.0))
{
if (MathUtils.equalTol(lineSegment.unNormalizedVector.Z, 0.0))
{
tx = 0.0;
}
else
{
tx = tz;
}
}
else
{
tx = ty;
}
}
if (MathUtils.equalTol(lineSegment.unNormalizedVector.Y, 0.0))
{
if (MathUtils.equalTol(lineSegment.unNormalizedVector.X, 0.0))
{
if (MathUtils.equalTol(lineSegment.unNormalizedVector.Z, 0.0))
{
ty = 0.0;
}
else
{
ty = tz;
}
}
else
{
ty = tx;
}
}
if (MathUtils.equalTol(lineSegment.unNormalizedVector.Z, 0.0))
{
if (MathUtils.equalTol(lineSegment.unNormalizedVector.Y, 0.0))
{
if (MathUtils.equalTol(lineSegment.unNormalizedVector.X, 0.0))
{
tz = 0.0;
}
else
{
tz = tx;
}
}
else
{
tz = ty;
}
}
if ((MathUtils.equalTol(tx, 0.0, MathUtils.defaultTol) && MathUtils.equalTol(ty, 0.0, MathUtils.defaultTol) && MathUtils.equalTol(tz, 0.0, MathUtils.defaultTol)) ||
(MathUtils.equalTol(tx, 1.0, MathUtils.defaultTol) && MathUtils.equalTol(ty, 1.0, MathUtils.defaultTol) && MathUtils.equalTol(tz, 1.0, MathUtils.defaultTol)))
{
status = LineSegmentOnSegmentEnum.CoincideEndSegment;
return(true);
}
if (MathUtils.equalTolSign(tx, ty, MathUtils.defaultTol) && MathUtils.equalTolSign(ty, tz, MathUtils.defaultTol) && MathUtils.equalTolSign(tx, tz, MathUtils.defaultTol))
{
if (tx > 1.0 || tx < 0.0)
{
status = LineSegmentOnSegmentEnum.OutsideSegment;
return(false);
}
else
{
status = LineSegmentOnSegmentEnum.InsideSegment;
return(true);
}
}
return(false);
}
/// <summary>
/// Testing whether a point lies on the segment
/// </summary>
/// <param name="lineSegment">Line segment</param>
/// <param name="thePoint">Point</param>
/// <returns></returns>
public static bool isInSegment(LineSegment3D lineSegment, Point3D thePoint)
{
LineSegmentOnSegmentEnum stat;
return(isInSegment(lineSegment, thePoint, out stat));
}
/// <summary>
/// Process Octree for a line segment
/// </summary>
/// <param name="_polyH"></param>
/// <param name="polyHF"></param>
public static void Process(OctreeNode node, LineSegment3D lineSegment)
{
if (node._depth < Octree.MaxDepth)
{
int disjointCount = 0;
OctreeNodeProcess.Split(node);
List <int> childToRemove = new List <int>();
List <int> childToTraverse = new List <int>();
for (int i = 0; i < node._children.Count; i++)
{
OctreeNode childNode = node._children[i];
if (Polyhedron.intersect(childNode.nodeCellCuboid.cuboidPolyhedron, lineSegment))
{
childToTraverse.Add(i);
childNode._flag = PolyhedronIntersectEnum.Intersect;
childNode.nodeCellID.setBorderCell();
continue;
}
// If doesn't intersect (passes the check above), either it is fully contained, full contains or disjoint
// To optimize the operation, we will use a single sampling point instead of checking the entire polyhedron since a single point can tell if a polyhedron is inside the other one
// Fully contains check only valid if the parent is fully contains, if intersect, it should never be full contains
if (node._flag == PolyhedronIntersectEnum.FullyContains)
{
if (Polyhedron.insideCuboid(childNode.nodeCellCuboid.cuboidPolyhedron, lineSegment.startPoint))
{
// if polyH is entirely inside the cuboid, we will set this for further split (the same as intersection
childToTraverse.Add(i); // We will remove the node if it is disjoint, otherwise it will continue splitting until the condition met
childNode._flag = PolyhedronIntersectEnum.FullyContains;
childNode.nodeCellID.setBorderCell();
continue;
}
}
// If the Line does not intersect the cuboid, or the cuboid does not fully contain the Line, it must be disjoint
childNode._flag = PolyhedronIntersectEnum.Disjoint;
disjointCount++;
continue;
}
if (disjointCount == 8)
{
// All children are disjoint. Remove all children and set the node to Disjoint
node._children.Clear();
node._flag = PolyhedronIntersectEnum.Disjoint;
return;
}
if (childToTraverse.Count == 1)
{
OctreeNodeProcess.Process(node._children[childToTraverse[0]], lineSegment);
}
else if (childToTraverse.Count > 1)
{
Parallel.ForEach(childToTraverse, i => OctreeNodeProcess.Process(node._children[i], lineSegment));
}
// If there is any disjoint, we need to keep this node as it is. This should be done after we processed all the children to be traversed!!
if (disjointCount > 0 && disjointCount < 8)
{
return;
}
int countGrandChildren = 0;
// If there is no disjoint, we need to check whether all children are terminal (i.e. child._children.Count == 0)
foreach (OctreeNode child in node._children)
{
countGrandChildren += child._children.Count;
}
// All children are terminal and no disjoint (by implication of previous steps). Remove children
if (countGrandChildren == 0)
{
node._children.Clear();
node._flag = PolyhedronIntersectEnum.IntersectOrInside;
return;
}
}
else
{
// at _depth == Octree.MaxDepth there is nothing else to do since the test has been done at the parent level and when entering this stage, the test has determined
// that the cell is intersected with the polyH
}
return;
}
public bool intersectWith(LineSegment3D LS)
{
return(Polyhedron.intersect(this._polyHRep, LS));
}
public bool isInside(LineSegment3D LS)
{
return(Polyhedron.inside(this._polyHRep, LS));
}
/// <summary>
/// Test a point is inside a face
/// </summary>
/// <param name="F1"></param>
/// <param name="P1"></param>
/// <returns></returns>
public static bool inside(Face3D F1, Point3D P1)
{
if (!Plane3D.pointOnPlane(F1.basePlane, P1))
{
return(false); // Point is not on a plane
}
// test Point inside a Face
// Need to project the plane to 2D (XY-, YZ-, or XZ- plane). It should work well with convex face (esp. triangles and rectangles we will deal with mostly)
double maxDim = Double.MinValue;
int DimttoZero = 0;
// First test whether the plane is already axis-aligned (in this case we can just remove the appropriate axis)
if (MathUtils.equalTol(F1.basePlane.normalVector.Y, 0.0, MathUtils.defaultTol) && MathUtils.equalTol(F1.basePlane.normalVector.Z, 0.0, MathUtils.defaultTol))
{
DimttoZero = 0; // Ignore X, project to Y-Z plane
}
else if (MathUtils.equalTol(F1.basePlane.normalVector.X, 0.0, MathUtils.defaultTol) && MathUtils.equalTol(F1.basePlane.normalVector.Z, 0.0, MathUtils.defaultTol))
{
DimttoZero = 1; // Ignore Y, project to X-Z plane
}
else if (MathUtils.equalTol(F1.basePlane.normalVector.X, 0.0, MathUtils.defaultTol) && MathUtils.equalTol(F1.basePlane.normalVector.Y, 0.0, MathUtils.defaultTol))
{
DimttoZero = 2; // Ignore Z, project to X-Y plane
}
else
{
if (maxDim < Math.Abs(F1.basePlane.normalVector.X))
{
maxDim = Math.Abs(F1.basePlane.normalVector.X);
DimttoZero = 0;
}
if (maxDim < Math.Abs(F1.basePlane.normalVector.Y))
{
maxDim = Math.Abs(F1.basePlane.normalVector.Y);
DimttoZero = 1;
}
if (maxDim < Math.Abs(F1.basePlane.normalVector.Z))
{
maxDim = Math.Abs(F1.basePlane.normalVector.Z);
DimttoZero = 2;
}
}
// We ignore the largest component, which means the least impact to the projection plane
List <Point3D> projVert = new List <Point3D>();
Point3D projIntP = new Point3D(P1.X, P1.Y, P1.Z);
Point3D rayEndP = new Point3D(P1.X, P1.Y, P1.Z);
if (DimttoZero == 0)
{
for (int i = 0; i < F1.vertices.Count; i++)
{
projVert.Add(new Point3D(0.0, F1.vertices[i].Y, F1.vertices[i].Z));
}
projIntP.X = 0.0;
rayEndP.X = 0.0;
if (Octree.WorldBB == null)
{
rayEndP.Y += Point3D.distance(F1.containingBB.URT, F1.containingBB.LLB) * 1000;
}
else
{
rayEndP.Y += Octree.WorldBB.extent * 2;
}
}
else if (DimttoZero == 1)
{
for (int i = 0; i < F1.vertices.Count; i++)
{
projVert.Add(new Point3D(F1.vertices[i].X, 0.0, F1.vertices[i].Z));
}
projIntP.Y = 0.0;
rayEndP.Y = 0.0;
//rayEndP.Z += Point3D.distance(F1.containingBB.URT, F1.containingBB.LLB) * 2;
if (Octree.WorldBB == null)
{
rayEndP.X += Point3D.distance(F1.containingBB.URT, F1.containingBB.LLB) * 2; // Use X axis for the ray
}
else
{
rayEndP.X += Octree.WorldBB.extent * 2;
}
}
else if (DimttoZero == 2)
{
for (int i = 0; i < F1.vertices.Count; i++)
{
projVert.Add(new Point3D(F1.vertices[i].X, F1.vertices[i].Y, 0.0));
}
projIntP.Z = 0.0;
rayEndP.Z = 0.0;
if (Octree.WorldBB == null)
{
rayEndP.X += Point3D.distance(F1.containingBB.URT, F1.containingBB.LLB) * 2;
}
else
{
rayEndP.X += Octree.WorldBB.extent * 2;
}
}
Face3D projFace = new Face3D(projVert);
// define a ray from the intersection point along the X-axis of the projected plane by using long enough line segment beginning from the point, using
// max extent of the face containingBB *2
LineSegment3D ray = new LineSegment3D(projIntP, rayEndP);
// Now do intersection between the ray and all the segments of the face. Odd number indicates the point is inside
// 4 rules to follow:
// 1. If the segment is upward, exclude the endpoint for intersection (only consider the startpoint)
// 2. If the segment is downward, exclude the startpoint for intersection (only considers the end point)
// 3. Ignore the segment that is horizontal (parallel to the ray)
// 4. ray is always strictly to the right of the Point
int intCount = 0;
Point3D iP = new Point3D();
LineSegmentIntersectEnum mod = LineSegmentIntersectEnum.Undefined;
for (int i = 0; i < projFace.boundaries.Count; i++)
{
if (ray.baseLine.direction == projFace.boundaries[i].baseLine.direction)
{
continue; //ignore segment that is parallel to the ray (rule #3)
}
Point3D pointToExclude = new Point3D();
if (DimttoZero == 0 || DimttoZero == 1)
{
// for both X-Z and Y-Z plane (Z as vertical axis)
if (projFace.boundaries[i].startPoint.Z <= ray.startPoint.Z && projFace.boundaries[i].endPoint.Z > ray.startPoint.Z)
{
pointToExclude = projFace.boundaries[i].endPoint; // Rule #1
}
else if (projFace.boundaries[i].startPoint.Z > ray.startPoint.Z && projFace.boundaries[i].endPoint.Z <= ray.startPoint.Z)
{
pointToExclude = projFace.boundaries[i].startPoint; // Rule #2
}
}
else
{
// for X-Y plane (Y as vertical axis)
if (projFace.boundaries[i].startPoint.Y <= ray.startPoint.Y && projFace.boundaries[i].endPoint.Y > ray.startPoint.Y)
{
pointToExclude = projFace.boundaries[i].endPoint; // Rule #1
}
else if (projFace.boundaries[i].startPoint.Y > ray.startPoint.Y && projFace.boundaries[i].endPoint.Y <= ray.startPoint.Y)
{
pointToExclude = projFace.boundaries[i].startPoint; // Rule #2
}
}
// In the evaluation of the number of intersection between a ray and a face, we will ignore the intersection point that is equal to the rule #2 or #3
if (LineSegment3D.intersect(ray, projFace.boundaries[i], out iP, out mod) && (iP != pointToExclude))
{
intCount++;
}
}
if (intCount % 2 == 1)
{
return(true);
}
return(false);
}
/// <summary>
/// Calculating intersection beween 2 Faces. The outcome of the intersection should be a LineSegment
/// </summary>
/// <param name="F1">First Face</param>
/// <param name="F2">Second Face</param>
/// <param name="intersectionLine">The resulting intersection line. Zero if no intersection</param>
/// <param name="mode">The mode of the intersection: No intersection/parallel, partially intersect, no actual intersection/undefined</param>
/// <returns></returns>
public static bool intersect(Face3D F1, Face3D F2, out LineSegment3D intersectionLine, out FaceIntersectEnum mode)
{
intersectionLine = new LineSegment3D(new Point3D(0, 0, 0), new Point3D(0, 0, 0));
mode = FaceIntersectEnum.Undefined;
if (F1._basePlane.normalVector == F2._basePlane.normalVector)
{
// test points inside another face
for (int i = 0; i < F2._vertices.Count; i++)
{
if (!Face3D.inside(F1, F2._vertices[i]))
{
continue;
}
mode = FaceIntersectEnum.Overlap;
return(true);
}
mode = FaceIntersectEnum.NoIntersectionParallel;
return(false);
}
LineSegment3D ls1;
LineSegment3D ls2;
bool res1 = Plane3D.PPintersect(F1._basePlane, F2, out ls1);
bool res2 = Plane3D.PPintersect(F2._basePlane, F1, out ls2);
if (!res1 || !res2)
{
return(false); // the faces do not intersect
}
// Special case if the intersection occurs only at a single point
if (ls1.startPoint == ls1.endPoint && ls2.startPoint == ls2.endPoint)
{
if (ls1.startPoint.Equals(ls2.startPoint))
{
mode = FaceIntersectEnum.IntersectPartial;
return(true);
}
return(false);
}
else if (ls1.startPoint.Equals(ls1.endPoint))
{
// a single point intersection: ls1 is 0 length linesegnment = point
if (LineSegment3D.isInSegment(ls2, ls1.startPoint))
{
mode = FaceIntersectEnum.IntersectPartial;
return(true);
}
return(false);
}
else if (ls2.startPoint.Equals(ls2.endPoint))
{
// a single point intersection: ls1 is 0 length linesegnment = point
if (LineSegment3D.isInSegment(ls1, ls2.startPoint))
{
mode = FaceIntersectEnum.IntersectPartial;
return(true);
}
return(false);
}
LineSegment3D ovSegment;
LineSegmentOverlapEnum ovstat = LineSegmentOverlapEnum.Undefined;
bool lint = LineSegment3D.overlap(ls1, ls2, out ovSegment, out ovstat);
if (lint)
{
intersectionLine = ovSegment;
mode = FaceIntersectEnum.IntersectPartial;
return(true);
}
return(false);
}