From 517d8cce9c2896c82634a8f4fa90deed42a2a14e Mon Sep 17 00:00:00 2001 From: rms Date: Thu, 15 Dec 2016 15:22:58 -0500 Subject: [PATCH] added a bunch of distance functions, ray/triangle intersection --- math/MathUtil.cs | 7 + math/Triangle3.cs | 71 ++++++++++ math/Vector3d.cs | 63 +++++++++ queries/DistLine3Ray3.cs | 12 +- queries/DistLine3Segment3.cs | 126 +++++++++++++++++ queries/DistLine3Triangle3.cs | 126 +++++++++++++++++ queries/DistPoint3Triangle3.cs | 228 ++++++++++++++++++++++++++++++ queries/DistRay3Ray3.cs | 8 ++ queries/DistRay3Segment3.cs | 7 +- queries/DistSegment3Triangle3.cs | 87 ++++++++++++ queries/DistTriangle3Triangle3.cs | 113 +++++++++++++++ queries/Distance.cs | 5 +- queries/Intersection.cs | 81 +++++++++++ queries/IntrRay3Triangle3.cs | 103 ++++++++++++++ 14 files changed, 1029 insertions(+), 8 deletions(-) create mode 100644 math/Triangle3.cs create mode 100644 queries/DistLine3Segment3.cs create mode 100644 queries/DistLine3Triangle3.cs create mode 100644 queries/DistPoint3Triangle3.cs create mode 100644 queries/DistSegment3Triangle3.cs create mode 100644 queries/DistTriangle3Triangle3.cs create mode 100644 queries/Intersection.cs create mode 100644 queries/IntrRay3Triangle3.cs diff --git a/math/MathUtil.cs b/math/MathUtil.cs index 409fa8be..5fa9bda3 100644 --- a/math/MathUtil.cs +++ b/math/MathUtil.cs @@ -67,6 +67,13 @@ public static double SignedClamp(double f, double fMin, double fMax) { } + // there are fast approximations to this... + public static double InvSqrt(double f) + { + return f / Math.Sqrt(f); + } + + public static float PlaneAngleD(Vector3f a, Vector3f b, int nPlaneNormalIdx = 1) { a[nPlaneNormalIdx] = b[nPlaneNormalIdx] = 0.0f; diff --git a/math/Triangle3.cs b/math/Triangle3.cs new file mode 100644 index 00000000..f6c10c61 --- /dev/null +++ b/math/Triangle3.cs @@ -0,0 +1,71 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace g3 +{ + public struct Triangle3d + { + public Vector3d V0, V1, V2; + + public Triangle3d(Vector3d v0, Vector3d v1, Vector3d v2) + { + V0 = v0; V1 = v1; V2 = v2; + } + + public Vector3d this[int key] + { + get { return (key == 0) ? V0 : (key == 1) ? V1 : V2; } + set { if (key == 0) V0 = value; else if (key == 1) V1 = value; else V2 = value; } + } + + public Vector3d PointAt(double bary0, double bary1, double bary2) + { + return bary0 * V0 + bary1 * V1 + bary2 * V2; + } + public Vector3d PointAt(Vector3d bary) + { + return bary.x* V0 + bary.y* V1 + bary.z* V2; + } + + // conversion operators + public static implicit operator Triangle3d(Triangle3f v) + { + return new Triangle3d(v.V0, v.V1, v.V2); + } + public static explicit operator Triangle3f(Triangle3d v) + { + return new Triangle3f((Vector3f)v.V0, (Vector3f)v.V1, (Vector3f)v.V2); + } + } + + + + public struct Triangle3f + { + public Vector3f V0, V1, V2; + + public Triangle3f(Vector3f v0, Vector3f v1, Vector3f v2) + { + V0 = v0; V1 = v1; V2 = v2; + } + + public Vector3f this[int key] + { + get { return (key == 0) ? V0 : (key == 1) ? V1 : V2; } + set { if (key == 0) V0 = value; else if (key == 1) V1 = value; else V2 = value; } + } + + + public Vector3f PointAt(float bary0, float bary1, float bary2) + { + return bary0 * V0 + bary1 * V1 + bary2 * V2; + } + public Vector3f PointAt(Vector3f bary) + { + return bary.x * V0 + bary.y * V1 + bary.z * V2; + } + } + +} diff --git a/math/Vector3d.cs b/math/Vector3d.cs index 450f5dbc..98e8597e 100644 --- a/math/Vector3d.cs +++ b/math/Vector3d.cs @@ -250,5 +250,68 @@ public static explicit operator Vector3(Vector3d v) #endif + + + // complicated functions go down here... + + + public static void Orthonormalize(ref Vector3d u, ref Vector3d v, ref Vector3d w) + { + // If the input vectors are v0, v1, and v2, then the Gram-Schmidt + // orthonormalization produces vectors u0, u1, and u2 as follows, + // + // u0 = v0/|v0| + // u1 = (v1-(u0*v1)u0)/|v1-(u0*v1)u0| + // u2 = (v2-(u0*v2)u0-(u1*v2)u1)/|v2-(u0*v2)u0-(u1*v2)u1| + // + // where |A| indicates length of vector A and A*B indicates dot + // product of vectors A and B. + + // compute u0 + u.Normalize(); + + // compute u1 + double dot0 = u.Dot(v); + v -= dot0 * u; + v.Normalize(); + + // compute u2 + double dot1 = v.Dot(w); + dot0 = u.Dot(w); + w -= dot0 * u + dot1 * v; + w.Normalize(); + } + + + // Input W must be a unit-length vector. The output vectors {U,V} are + // unit length and mutually perpendicular, and {U,V,W} is an orthonormal basis. + // ported from WildMagic5 + public static void GenerateComplementBasis(ref Vector3d u, ref Vector3d v, Vector3d w) + { + double invLength; + + if (Math.Abs(w.x) >= Math.Abs(w.y)) { + // W.x or W.z is the largest magnitude component, swap them + invLength = MathUtil.InvSqrt(w.x * w.x + w.z * w.z); + u.x = -w.z * invLength; + u.y = 0; + u.z = +w.x * invLength; + v.x = w.y * u.z; + v.y = w.z * u.x - w.x * u.z; + v.z = -w.y * u.x; + } else { + // W.y or W.z is the largest magnitude component, swap them + invLength = MathUtil.InvSqrt(w.y * w.y + w.z * w.z); + u.x = 0; + u.y = +w.z * invLength; + u.z = -w.y * invLength; + v.x = w.y * u.z - w.z * u.y; + v.y = -w.x * u.z; + v.z = w.x * u.y; + } + } + + + } } diff --git a/queries/DistLine3Ray3.cs b/queries/DistLine3Ray3.cs index 2561ec42..01cab404 100644 --- a/queries/DistLine3Ray3.cs +++ b/queries/DistLine3Ray3.cs @@ -5,6 +5,9 @@ namespace g3 { + // ported from WildMagic 5 + // https://www.geometrictools.com/Downloads/Downloads.html + public class DistLine3Ray3 { Line3d line; @@ -29,9 +32,9 @@ public Ray3d Ray public double RayParameter; - public DistLine3Ray3(Ray3d ray, Line3d Line) + public DistLine3Ray3(Ray3d rayIn, Line3d LineIn) { - this.ray = ray; this.line = Line; + this.ray = rayIn; this.line = LineIn; } static public double MinDistance(Ray3d r, Line3d s) @@ -58,6 +61,9 @@ public double Get() public double GetSquared () { + if (DistanceSquared >= 0) + return DistanceSquared; + Vector3d kDiff = line.Origin - ray.Origin; double a01 = -line.Direction.Dot(ray.Direction); double b0 = kDiff.Dot(line.Direction); @@ -98,6 +104,8 @@ public double GetSquared () if (sqrDist < (double)0) { sqrDist = (double)0; } + DistanceSquared = sqrDist; + return sqrDist; } diff --git a/queries/DistLine3Segment3.cs b/queries/DistLine3Segment3.cs new file mode 100644 index 00000000..39e21200 --- /dev/null +++ b/queries/DistLine3Segment3.cs @@ -0,0 +1,126 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace g3 +{ + // ported from WildMagic 5 + // https://www.geometrictools.com/Downloads/Downloads.html + + public class DistLine3Segment3 + { + Line3d line; + public Line3d Line + { + get { return line; } + set { line = value; DistanceSquared = -1.0; } + } + + Segment3d segment; + public Segment3d Segment + { + get { return segment; } + set { segment = value; DistanceSquared = -1.0; } + } + + public double DistanceSquared = -1.0; + + public Vector3d LineClosest; + public double LineParameter; + public Vector3d SegmentClosest; + public double SegmentParameter; + + + public DistLine3Segment3( Line3d LineIn, Segment3d SegmentIn) + { + this.segment = SegmentIn; this.line = LineIn; + } + + static public double MinDistance(Line3d line, Segment3d segment) + { + return new DistLine3Segment3( line, segment ).Get(); + } + static public double MinDistanceLineParam( Line3d line, Segment3d segment ) + { + return new DistLine3Segment3( line, segment ).Compute().LineParameter; + } + + + public DistLine3Segment3 Compute() + { + GetSquared(); + return this; + } + + public double Get() + { + return Math.Sqrt( GetSquared() ); + } + + + public double GetSquared() + { + if (DistanceSquared >= 0) + return DistanceSquared; + + Vector3d diff = line.Origin - segment.Center; + double a01 = -line.Direction.Dot(segment.Direction); + double b0 = diff.Dot(line.Direction); + double c = diff.LengthSquared; + double det = Math.Abs(1 - a01 * a01); + double b1, s0, s1, sqrDist, extDet; + + if (det >= MathUtil.ZeroTolerance) { + // The line and segment are not parallel. + b1 = -diff.Dot(segment.Direction); + s1 = a01 * b0 - b1; + extDet = segment.Extent * det; + + if (s1 >= -extDet) { + if (s1 <= extDet) { + // Two interior points are closest, one on the line and one + // on the segment. + double invDet = (1) / det; + s0 = (a01 * b1 - b0) * invDet; + s1 *= invDet; + sqrDist = s0 * (s0 + a01 * s1 + (2) * b0) + + s1 * (a01 * s0 + s1 + (2) * b1) + c; + } else { + // The endpoint e1 of the segment and an interior point of + // the line are closest. + s1 = segment.Extent; + s0 = -(a01 * s1 + b0); + sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c; + } + } else { + // The end point e0 of the segment and an interior point of the + // line are closest. + s1 = -segment.Extent; + s0 = -(a01 * s1 + b0); + sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c; + } + } else { + // The line and segment are parallel. Choose the closest pair so that + // one point is at segment center. + s1 = 0; + s0 = -b0; + sqrDist = b0 * s0 + c; + } + + LineClosest = line.Origin + s0 * line.Direction; + SegmentClosest = segment.Center + s1 * segment.Direction; + LineParameter = s0; + SegmentParameter = s1; + + // Account for numerical round-off errors. + if (sqrDist < 0) { + sqrDist = 0; + } + + DistanceSquared = sqrDist; + return sqrDist; + } + } + +} diff --git a/queries/DistLine3Triangle3.cs b/queries/DistLine3Triangle3.cs new file mode 100644 index 00000000..6d22a7e0 --- /dev/null +++ b/queries/DistLine3Triangle3.cs @@ -0,0 +1,126 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace g3 +{ + // ported from WildMagic 5 + // https://www.geometrictools.com/Downloads/Downloads.html + + public class DistLine3Triangle3 + { + Line3d line; + public Line3d Line + { + get { return line; } + set { line = value; DistanceSquared = -1.0; } + } + + Triangle3d triangle; + public Triangle3d Triangle + { + get { return triangle; } + set { triangle = value; DistanceSquared = -1.0; } + } + + public double DistanceSquared = -1.0; + + public Vector3d LineClosest; + public double LineParam; + public Vector3d TriangleClosest; + public Vector3d TriangleBaryCoords; + + + public DistLine3Triangle3(Line3d LineIn, Triangle3d TriangleIn) + { + this.triangle = TriangleIn; this.line = LineIn; + } + + public DistLine3Triangle3 Compute() + { + GetSquared(); + return this; + } + + public double Get() + { + return Math.Sqrt(GetSquared()); + } + + + public double GetSquared() + { + if (DistanceSquared >= 0) + return DistanceSquared; + + // Test if line intersects triangle. If so, the squared distance is zero. + Vector3d edge0 = triangle.V1 - triangle.V0; + Vector3d edge1 = triangle.V2 - triangle.V0; + Vector3d normal = edge0.UnitCross(edge1); + double NdD = normal.Dot(line.Direction); + if (Math.Abs(NdD) > MathUtil.ZeroTolerance) { + // The line and triangle are not parallel, so the line intersects + // the plane of the triangle. + Vector3d diff = line.Origin - triangle.V0; + Vector3d U = Vector3d.Zero, V = Vector3d.Zero; + Vector3d.GenerateComplementBasis(ref U, ref V, line.Direction); + double UdE0 = U.Dot(edge0); + double UdE1 = U.Dot(edge1); + double UdDiff = U.Dot(diff); + double VdE0 = V.Dot(edge0); + double VdE1 = V.Dot(edge1); + double VdDiff = V.Dot(diff); + double invDet = (1) / (UdE0 * VdE1 - UdE1 * VdE0); + + // Barycentric coordinates for the point of intersection. + double b1 = (VdE1 * UdDiff - UdE1 * VdDiff) * invDet; + double b2 = (UdE0 * VdDiff - VdE0 * UdDiff) * invDet; + double b0 = 1 - b1 - b2; + + if (b0 >= 0 && b1 >= 0 && b2 >= 0) { + // Line parameter for the point of intersection. + double DdE0 = line.Direction.Dot(edge0); + double DdE1 = line.Direction.Dot(edge1); + double DdDiff = line.Direction.Dot(diff); + LineParam = b1 * DdE0 + b2 * DdE1 - DdDiff; + + // Barycentric coordinates for the point of intersection. + TriangleBaryCoords = new Vector3d(b0, b1, b2); + + // The intersection point is inside or on the triangle. + LineClosest = line.Origin + LineParam * line.Direction; + TriangleClosest = triangle.V0 + b1 * edge0 + b2 * edge1; + DistanceSquared = 0; + return 0; + } + } + + // Either (1) the line is not parallel to the triangle and the point of + // intersection of the line and the plane of the triangle is outside the + // triangle or (2) the line and triangle are parallel. Regardless, the + // closest point on the triangle is on an edge of the triangle. Compare + // the line to all three edges of the triangle. + double sqrDist = double.MaxValue; + for (int i0 = 2, i1 = 0; i1 < 3; i0 = i1++) { + Segment3d segment = new Segment3d(triangle[i0], triangle[i1]); + DistLine3Segment3 queryLS = new DistLine3Segment3(line, segment); + double sqrDistTmp = queryLS.GetSquared(); + if (sqrDistTmp < sqrDist) { + LineClosest = queryLS.LineClosest; + TriangleClosest = queryLS.SegmentClosest; + sqrDist = sqrDistTmp; + LineParam = queryLS.LineParameter; + double ratio = queryLS.SegmentParameter / segment.Extent; + TriangleBaryCoords = Vector3d.Zero; + TriangleBaryCoords[i0] = (0.5) * (1 - ratio); + TriangleBaryCoords[i1] = 1 - TriangleBaryCoords[i0]; + TriangleBaryCoords[3 - i0 - i1] = 0; + } + } + + DistanceSquared = sqrDist; + return DistanceSquared; + } + } +} diff --git a/queries/DistPoint3Triangle3.cs b/queries/DistPoint3Triangle3.cs new file mode 100644 index 00000000..f54725d5 --- /dev/null +++ b/queries/DistPoint3Triangle3.cs @@ -0,0 +1,228 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace g3 +{ + // ported from WildMagic 5 + // https://www.geometrictools.com/Downloads/Downloads.html + + public class DistPoint3Triangle3 + { + Vector3d point; + public Vector3d Point + { + get { return point; } + set { point = value; DistanceSquared = -1.0; } + } + + Triangle3d triangle; + public Triangle3d Triangle + { + get { return triangle; } + set { triangle = value; DistanceSquared = -1.0; } + } + + public double DistanceSquared = -1.0; + + public Vector3d TriangleClosest; + public Vector3d TriangleBaryCoords; + + + public DistPoint3Triangle3(Vector3d PointIn, Triangle3d TriangleIn ) + { + point = PointIn; triangle = TriangleIn; + } + + public DistPoint3Triangle3 Compute() + { + GetSquared(); + return this; + } + + public double Get() + { + return Math.Sqrt(GetSquared()); + } + + + public double GetSquared() + { + if (DistanceSquared >= 0) + return DistanceSquared; + + Vector3d diff = triangle.V0 - point; + Vector3d edge0 = triangle.V1 - triangle.V0; + Vector3d edge1 = triangle.V2 - triangle.V0; + double a00 = edge0.LengthSquared; + double a01 = edge0.Dot(edge1); + double a11 = edge1.LengthSquared; + double b0 = diff.Dot(edge0); + double b1 = diff.Dot(edge1); + double c = diff.LengthSquared; + double det = Math.Abs(a00 * a11 - a01 * a01); + double s = a01 * b1 - a11 * b0; + double t = a01 * b0 - a00 * b1; + double sqrDistance; + + if (s + t <= det) { + if (s < 0) { + if (t < 0) // region 4 + { + if (b0 < 0) { + t = 0; + if (-b0 >= a00) { + s = 1; + sqrDistance = a00 + (2) * b0 + c; + } else { + s = -b0 / a00; + sqrDistance = b0 * s + c; + } + } else { + s = 0; + if (b1 >= 0) { + t = 0; + sqrDistance = c; + } else if (-b1 >= a11) { + t = 1; + sqrDistance = a11 + (2) * b1 + c; + } else { + t = -b1 / a11; + sqrDistance = b1 * t + c; + } + } + } else // region 3 + { + s = 0; + if (b1 >= 0) { + t = 0; + sqrDistance = c; + } else if (-b1 >= a11) { + t = 1; + sqrDistance = a11 + (2) * b1 + c; + } else { + t = -b1 / a11; + sqrDistance = b1 * t + c; + } + } + } else if (t < 0) // region 5 + { + t = 0; + if (b0 >= 0) { + s = 0; + sqrDistance = c; + } else if (-b0 >= a00) { + s = 1; + sqrDistance = a00 + (2) * b0 + c; + } else { + s = -b0 / a00; + sqrDistance = b0 * s + c; + } + } else // region 0 + { + // minimum at interior point + double invDet = (1) / det; + s *= invDet; + t *= invDet; + sqrDistance = s * (a00 * s + a01 * t + (2) * b0) + + t * (a01 * s + a11 * t + (2) * b1) + c; + } + } else { + double tmp0, tmp1, numer, denom; + + if (s < 0) // region 2 + { + tmp0 = a01 + b0; + tmp1 = a11 + b1; + if (tmp1 > tmp0) { + numer = tmp1 - tmp0; + denom = a00 - (2) * a01 + a11; + if (numer >= denom) { + s = 1; + t = 0; + sqrDistance = a00 + (2) * b0 + c; + } else { + s = numer / denom; + t = 1 - s; + sqrDistance = s * (a00 * s + a01 * t + (2) * b0) + + t * (a01 * s + a11 * t + (2) * b1) + c; + } + } else { + s = 0; + if (tmp1 <= 0) { + t = 1; + sqrDistance = a11 + (2) * b1 + c; + } else if (b1 >= 0) { + t = 0; + sqrDistance = c; + } else { + t = -b1 / a11; + sqrDistance = b1 * t + c; + } + } + } else if (t < 0) // region 6 + { + tmp0 = a01 + b1; + tmp1 = a00 + b0; + if (tmp1 > tmp0) { + numer = tmp1 - tmp0; + denom = a00 - (2) * a01 + a11; + if (numer >= denom) { + t = 1; + s = 0; + sqrDistance = a11 + (2) * b1 + c; + } else { + t = numer / denom; + s = 1 - t; + sqrDistance = s * (a00 * s + a01 * t + (2) * b0) + + t * (a01 * s + a11 * t + (2) * b1) + c; + } + } else { + t = 0; + if (tmp1 <= 0) { + s = 1; + sqrDistance = a00 + (2) * b0 + c; + } else if (b0 >= 0) { + s = 0; + sqrDistance = c; + } else { + s = -b0 / a00; + sqrDistance = b0 * s + c; + } + } + } else // region 1 + { + numer = a11 + b1 - a01 - b0; + if (numer <= 0) { + s = 0; + t = 1; + sqrDistance = a11 + (2) * b1 + c; + } else { + denom = a00 - (2) * a01 + a11; + if (numer >= denom) { + s = 1; + t = 0; + sqrDistance = a00 + (2) * b0 + c; + } else { + s = numer / denom; + t = 1 - s; + sqrDistance = s * (a00 * s + a01 * t + (2) * b0) + + t * (a01 * s + a11 * t + (2) * b1) + c; + } + } + } + } + + // Account for numerical round-off error. + if (sqrDistance < 0) { + sqrDistance = 0; + } + DistanceSquared = sqrDistance; + + TriangleClosest = triangle.V0 + s * edge0 + t * edge1; + TriangleBaryCoords = new Vector3d(1 - s - t, s, t); + return sqrDistance; + } + } +} diff --git a/queries/DistRay3Ray3.cs b/queries/DistRay3Ray3.cs index c33f0751..0adf4866 100644 --- a/queries/DistRay3Ray3.cs +++ b/queries/DistRay3Ray3.cs @@ -5,6 +5,9 @@ namespace g3 { + // ported from WildMagic 5 + // https://www.geometrictools.com/Downloads/Downloads.html + public class DistRay3Ray3 { Ray3d ray1; @@ -55,6 +58,9 @@ public double Get() public double GetSquared() { + if (DistanceSquared >= 0) + return DistanceSquared; + Vector3d diff = ray1.Origin - ray2.Origin; double a01 = -ray1.Direction.Dot(ray2.Direction); double b0 = diff.Dot(ray1.Direction); @@ -149,6 +155,8 @@ public double GetSquared() if (sqrDist < 0) { sqrDist = 0; } + DistanceSquared = sqrDist; + return sqrDist; } diff --git a/queries/DistRay3Segment3.cs b/queries/DistRay3Segment3.cs index 2d1dc276..b5a0697c 100644 --- a/queries/DistRay3Segment3.cs +++ b/queries/DistRay3Segment3.cs @@ -5,6 +5,9 @@ namespace g3 { + // ported from WildMagic 5 + // https://www.geometrictools.com/Downloads/Downloads.html + class DistRay3Segment3 { Ray3d ray; @@ -29,9 +32,9 @@ public Segment3d Segment public double SegmentParameter; - public DistRay3Segment3(Ray3d ray, Segment3d segment) + public DistRay3Segment3(Ray3d rayIn, Segment3d segmentIn) { - this.ray = ray; this.segment = segment; + this.ray = rayIn; this.segment = segmentIn; } diff --git a/queries/DistSegment3Triangle3.cs b/queries/DistSegment3Triangle3.cs new file mode 100644 index 00000000..120866a7 --- /dev/null +++ b/queries/DistSegment3Triangle3.cs @@ -0,0 +1,87 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace g3 +{ + // ported from WildMagic 5 + // https://www.geometrictools.com/Downloads/Downloads.html + + public class DistSegment3Triangle3 + { + Segment3d segment; + public Segment3d Segment + { + get { return segment; } + set { segment = value; DistanceSquared = -1.0; } + } + + Triangle3d triangle; + public Triangle3d Triangle + { + get { return triangle; } + set { triangle = value; DistanceSquared = -1.0; } + } + + public double DistanceSquared = -1.0; + + public Vector3d SegmentClosest; + public double SegmentParam; + public Vector3d TriangleClosest; + public Vector3d TriangleBaryCoords; + + public DistSegment3Triangle3(Segment3d SegmentIn, Triangle3d TriangleIn) + { + this.triangle = TriangleIn; this.segment = SegmentIn; + } + + + public DistSegment3Triangle3 Compute() + { + GetSquared(); + return this; + } + + public double Get() + { + return Math.Sqrt(GetSquared()); + } + + + public double GetSquared() + { + if (DistanceSquared >= 0) + return DistanceSquared; + Line3d line = new Line3d(segment.Center, segment.Direction); + DistLine3Triangle3 queryLT = new DistLine3Triangle3(line, triangle); + double sqrDist = queryLT.GetSquared(); + SegmentParam = queryLT.LineParam; + + if (SegmentParam >= -segment.Extent) { + if (SegmentParam <= segment.Extent) { + SegmentClosest = queryLT.LineClosest; + TriangleClosest = queryLT.TriangleClosest; + TriangleBaryCoords = queryLT.TriangleBaryCoords; + } else { + SegmentClosest = segment.P1; + DistPoint3Triangle3 queryPT = new DistPoint3Triangle3(SegmentClosest, triangle); + sqrDist = queryPT.GetSquared(); + TriangleClosest = queryPT.TriangleClosest; + SegmentParam = segment.Extent; + TriangleBaryCoords = queryPT.TriangleBaryCoords; + } + } else { + SegmentClosest = segment.P0; + DistPoint3Triangle3 queryPT = new DistPoint3Triangle3(SegmentClosest, triangle); + sqrDist = queryPT.GetSquared(); + TriangleClosest = queryPT.TriangleClosest; + SegmentParam = -segment.Extent; + TriangleBaryCoords = queryPT.TriangleBaryCoords; + } + + DistanceSquared = sqrDist; + return DistanceSquared; + } + } +} diff --git a/queries/DistTriangle3Triangle3.cs b/queries/DistTriangle3Triangle3.cs new file mode 100644 index 00000000..cea54ac1 --- /dev/null +++ b/queries/DistTriangle3Triangle3.cs @@ -0,0 +1,113 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace g3 +{ + public class DistTriangle3Triangle3 + { + Triangle3d triangle0; + public Triangle3d Triangle0 + { + get { return triangle0; } + set { triangle0 = value; DistanceSquared = -1.0; } + } + + Triangle3d triangle1; + public Triangle3d Triangle1 + { + get { return triangle1; } + set { triangle1 = value; DistanceSquared = -1.0; } + } + + public double DistanceSquared = -1.0; + + public Vector3d Triangle0Closest; + public Vector3d Triangle0BaryCoords; + public Vector3d Triangle1Closest; + public Vector3d Triangle1BaryCoords; + + + public DistTriangle3Triangle3(Triangle3d Triangle0in, Triangle3d Triangle1in) + { + this.triangle0 = Triangle0in; this.triangle1 = Triangle1in; + } + + public DistTriangle3Triangle3 Compute() + { + GetSquared(); + return this; + } + + public double Get() + { + return Math.Sqrt(GetSquared()); + } + + + public double GetSquared() + { + if (DistanceSquared >= 0) + return DistanceSquared; + + // Compare edges of triangle0 to the interior of triangle1. + double sqrDist = Double.MaxValue, sqrDistTmp; + Segment3d edge = new Segment3d(); + double ratio; + int i0, i1; + for (i0 = 2, i1 = 0; i1 < 3; i0 = i1++) { + edge.SetEndpoints(triangle0[i0], triangle0[i1]); + + DistSegment3Triangle3 queryST = new DistSegment3Triangle3(edge, triangle1); + sqrDistTmp = queryST.GetSquared(); + if (sqrDistTmp < sqrDist) { + Triangle0Closest = queryST.SegmentClosest; + Triangle1Closest = queryST.TriangleClosest; + sqrDist = sqrDistTmp; + + ratio = queryST.SegmentParam / edge.Extent; + Triangle0BaryCoords = Vector3d.Zero; + Triangle0BaryCoords[i0] = (0.5) * (1 - ratio); + Triangle0BaryCoords[i1] = 1 - Triangle0BaryCoords[i0]; + Triangle0BaryCoords[3 - i0 - i1] = 0; + Triangle1BaryCoords = queryST.TriangleBaryCoords; + + if (sqrDist <= MathUtil.ZeroTolerance) { + DistanceSquared = 0; + return 0; + } + } + } + + // Compare edges of triangle1 to the interior of triangle0. + for (i0 = 2, i1 = 0; i1 < 3; i0 = i1++) { + edge.SetEndpoints(triangle1[i0], triangle1[i1]); + + DistSegment3Triangle3 queryST = new DistSegment3Triangle3(edge, triangle0); + sqrDistTmp = queryST.GetSquared(); + if (sqrDistTmp < sqrDist) { + Triangle0Closest = queryST.SegmentClosest; + Triangle1Closest = queryST.TriangleClosest; + sqrDist = sqrDistTmp; + + ratio = queryST.SegmentParam / edge.Extent; + Triangle1BaryCoords = Vector3d.Zero; + Triangle1BaryCoords[i0] = (0.5) * (1 - ratio); + Triangle1BaryCoords[i1] = 1 - Triangle1BaryCoords[i0]; + Triangle1BaryCoords[3 - i0 - i1] = 0; + Triangle0BaryCoords = queryST.TriangleBaryCoords; + + if (sqrDist <= MathUtil.ZeroTolerance) { + DistanceSquared = 0; + return 0; + } + } + } + + DistanceSquared = sqrDist; + return DistanceSquared; + } + } + +} \ No newline at end of file diff --git a/queries/Distance.cs b/queries/Distance.cs index 160224b6..87a047c1 100644 --- a/queries/Distance.cs +++ b/queries/Distance.cs @@ -5,11 +5,8 @@ namespace g3 { - public class Distance + public static class Distance { - private Distance() - { - } public static Vector3f ClosestPointOnLine(Vector3f p0, Vector3f dir, Vector3f pt) { diff --git a/queries/Intersection.cs b/queries/Intersection.cs new file mode 100644 index 00000000..5d5811de --- /dev/null +++ b/queries/Intersection.cs @@ -0,0 +1,81 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace g3 +{ + public enum IntersectionResult + { + NotComputed, + Intersects, + NoIntersection + } + + public enum IntersectionType + { + Point, Line + } + + + + public static class Intersection + { + + + + // same code as IntrRay3Triangle3, but can be called w/o constructing additional data structures + public static bool Intersects(Vector3d rayOrigin, Vector3d rayDirection, Vector3d V0, Vector3d V1, Vector3d V2) + { + // Compute the offset origin, edges, and normal. + Vector3d diff = rayOrigin - V0; + Vector3d edge1 = V1 - V0; + Vector3d edge2 = V2 - V0; + Vector3d normal = edge1.Cross(edge2); + + // Solve Q + t*D = b1*E1 + b2*E2 (Q = kDiff, D = ray direction, + // E1 = kEdge1, E2 = kEdge2, N = Cross(E1,E2)) by + // |Dot(D,N)|*b1 = sign(Dot(D,N))*Dot(D,Cross(Q,E2)) + // |Dot(D,N)|*b2 = sign(Dot(D,N))*Dot(D,Cross(E1,Q)) + // |Dot(D,N)|*t = -sign(Dot(D,N))*Dot(Q,N) + double DdN = rayDirection.Dot(normal); + double sign; + if (DdN > MathUtil.ZeroTolerance) { + sign = 1; + } else if (DdN < -MathUtil.ZeroTolerance) { + sign = -1; + DdN = -DdN; + } else { + // Ray and triangle are parallel, call it a "no intersection" + // even if the ray does intersect. + return false; + } + + double DdQxE2 = sign * rayDirection.Dot(diff.Cross(edge2)); + if (DdQxE2 >= 0) { + double DdE1xQ = sign * rayDirection.Dot(edge1.Cross(diff)); + if (DdE1xQ >= 0) { + if (DdQxE2 + DdE1xQ <= DdN) { + // Line intersects triangle, check if ray does. + double QdN = -sign * diff.Dot(normal); + if (QdN >= 0) { + // Ray intersects triangle. + return true; + } + // else: t < 0, no intersection + } + // else: b1+b2 > 1, no intersection + } + // else: b2 < 0, no intersection + } + // else: b1 < 0, no intersection + return false; + + } + + + } + + + +} diff --git a/queries/IntrRay3Triangle3.cs b/queries/IntrRay3Triangle3.cs new file mode 100644 index 00000000..dcd4d033 --- /dev/null +++ b/queries/IntrRay3Triangle3.cs @@ -0,0 +1,103 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace g3 +{ + public class IntrRay3Triangle3 + { + Ray3d ray; + public Ray3d Ray + { + get { return ray; } + set { ray = value; Result = IntersectionResult.NotComputed; } + } + + Triangle3d triangle; + public Triangle3d Triangle + { + get { return triangle; } + set { triangle = value; Result = IntersectionResult.NotComputed; } + } + + public IntersectionResult Result; + public IntersectionType Type; + + public bool IsSimpleIntersection { + get { return Result == IntersectionResult.Intersects && Type == IntersectionType.Point; } + } + + + public double RayParameter; + public Vector3d TriangleBaryCoords; + + + public IntrRay3Triangle3 Compute() + { + Find(); + return this; + } + + + public bool Find() + { + if (Result != IntersectionResult.NotComputed) + return (Result != g3.IntersectionResult.NoIntersection); + + // Compute the offset origin, edges, and normal. + Vector3d diff = ray.Origin - triangle.V0; + Vector3d edge1 = triangle.V1 - triangle.V0; + Vector3d edge2 = triangle.V2 - triangle.V0; + Vector3d normal = edge1.Cross(edge2); + + // Solve Q + t*D = b1*E1 + b2*E2 (Q = kDiff, D = ray direction, + // E1 = kEdge1, E2 = kEdge2, N = Cross(E1,E2)) by + // |Dot(D,N)|*b1 = sign(Dot(D,N))*Dot(D,Cross(Q,E2)) + // |Dot(D,N)|*b2 = sign(Dot(D,N))*Dot(D,Cross(E1,Q)) + // |Dot(D,N)|*t = -sign(Dot(D,N))*Dot(Q,N) + double DdN = ray.Direction.Dot(normal); + double sign; + if (DdN > MathUtil.ZeroTolerance) { + sign = 1; + } else if (DdN < -MathUtil.ZeroTolerance) { + sign = -1; + DdN = -DdN; + } else { + // Ray and triangle are parallel, call it a "no intersection" + // even if the ray does intersect. + Result = IntersectionResult.NoIntersection; + return false; + } + + double DdQxE2 = sign * ray.Direction.Dot(diff.Cross(edge2)); + if (DdQxE2 >= 0) { + double DdE1xQ = sign * ray.Direction.Dot(edge1.Cross(diff)); + if (DdE1xQ >= 0) { + if (DdQxE2 + DdE1xQ <= DdN) { + // Line intersects triangle, check if ray does. + double QdN = -sign * diff.Dot(normal); + if (QdN >= 0) { + // Ray intersects triangle. + double inv = (1) / DdN; + RayParameter = QdN * inv; + double mTriBary1 = DdQxE2 * inv; + double mTriBary2 = DdE1xQ * inv; + TriangleBaryCoords = new Vector3d(1 - mTriBary1 - mTriBary2, mTriBary1, mTriBary2); + Type = IntersectionType.Point; + Result = IntersectionResult.Intersects; + return true; + } + // else: t < 0, no intersection + } + // else: b1+b2 > 1, no intersection + } + // else: b2 < 0, no intersection + } + // else: b1 < 0, no intersection + + Result = IntersectionResult.NoIntersection; + return false; + } + } +}