private object _line_intersection(Line3d l, double t0, double t1) { // Smith's algorithm: // "An Efficient and Robust Ray–Box Intersection Algorithm" // Amy Williams, Steve Barrus, R. Keith Morley, Peter Shirley // http://www.cs.utah.edu/~awilliam/box/box.pdf // Modified to allow tolerance based checks // Relative tolerance ================================ if (!GeometRi3D.UseAbsoluteTolerance) { double tol = GeometRi3D.Tolerance; GeometRi3D.Tolerance = tol * this.Diagonal; GeometRi3D.UseAbsoluteTolerance = true; object result = _line_intersection(l, t0, t1); GeometRi3D.UseAbsoluteTolerance = false; GeometRi3D.Tolerance = tol; return(result); } //==================================================== // Define local CS aligned with box Coord3d local_CS = new Coord3d(this.Center, this.Orientation.ConvertToGlobal().ToRotationMatrix, "local_CS"); Point3d Pmin = this.P1.ConvertTo(local_CS); Point3d Pmax = this.P7.ConvertTo(local_CS); l = new Line3d(l.Point.ConvertTo(local_CS), l.Direction.ConvertTo(local_CS).Normalized); double tmin, tmax, tymin, tymax, tzmin, tzmax; double divx = 1 / l.Direction.X; if (divx >= 0) { tmin = (Pmin.X - l.Point.X) * divx; tmax = (Pmax.X - l.Point.X) * divx; } else { tmin = (Pmax.X - l.Point.X) * divx; tmax = (Pmin.X - l.Point.X) * divx; } double divy = 1 / l.Direction.Y; if (divy >= 0) { tymin = (Pmin.Y - l.Point.Y) * divy; tymax = (Pmax.Y - l.Point.Y) * divy; } else { tymin = (Pmax.Y - l.Point.Y) * divy; tymax = (Pmin.Y - l.Point.Y) * divy; } if (GeometRi3D.Greater(tmin, tymax) || GeometRi3D.Greater(tymin, tmax)) { return(null); } if (GeometRi3D.Greater(tymin, tmin)) { tmin = tymin; } if (GeometRi3D.Smaller(tymax, tmax)) { tmax = tymax; } double divz = 1 / l.Direction.Z; if (divz >= 0) { tzmin = (Pmin.Z - l.Point.Z) * divz; tzmax = (Pmax.Z - l.Point.Z) * divz; } else { tzmin = (Pmax.Z - l.Point.Z) * divz; tzmax = (Pmin.Z - l.Point.Z) * divz; } if (GeometRi3D.Greater(tmin, tzmax) || GeometRi3D.Greater(tzmin, tmax)) { return(null); } if (GeometRi3D.Greater(tzmin, tmin)) { tmin = tzmin; } if (GeometRi3D.Smaller(tzmax, tmax)) { tmax = tzmax; } // Now check the overlapping portion of the segments // This part is missing in the original algorithm if (GeometRi3D.Greater(tmin, t1)) { return(null); } if (GeometRi3D.Smaller(tmax, t0)) { return(null); } if (GeometRi3D.Smaller(tmin, t0)) { tmin = t0; } if (GeometRi3D.Greater(tmax, t1)) { tmax = t1; } if (GeometRi3D.AlmostEqual(tmin, tmax)) { return(l.Point.Translate(tmin * l.Direction)); } else { return(new Segment3d(l.Point.Translate(tmin * l.Direction), l.Point.Translate(tmax * l.Direction))); } }
/// <summary> /// Reflect circle in given line /// </summary> public Circle3d ReflectIn(Line3d l) { return(new Circle3d(this.Center.ReflectIn(l), this.R, this.Normal.ReflectIn(l))); }
/// <summary> /// Reflect plane in given line /// </summary> public Plane3d ReflectIn(Line3d l) { return(new Plane3d(this.Point.ReflectIn(l), this.Normal.ReflectIn(l))); }
/// <summary> /// Get intersection of line with box. /// Returns 'null' (no intersection) or object of type 'Point3d' or 'Segment3d'. /// </summary> public object IntersectionWith(Line3d l) { return(_line_intersection(l, double.NegativeInfinity, double.PositiveInfinity)); }
/// <summary> /// Reflect segment in given line /// </summary> public virtual Segment3d ReflectIn(Line3d l) { return(new Segment3d(P1.ReflectIn(l), P2.ReflectIn(l))); }
/// <summary> /// Reflect ray in given line /// </summary> public Ray3d ReflectIn(Line3d l) { return(new Ray3d(this.Point.ReflectIn(l), this.Direction.ReflectIn(l))); }
/// <summary> /// Get intersection of line with triangle. /// Returns 'null' (no intersection) or object of type 'Point3d' or 'Segment3d'. /// </summary> public object IntersectionWith(Line3d l) { // Relative tolerance ================================ if (!GeometRi3D.UseAbsoluteTolerance) { double tol = GeometRi3D.Tolerance; GeometRi3D.Tolerance = tol * Max(AB, Max(BC, AC)); GeometRi3D.UseAbsoluteTolerance = true; object result = this.IntersectionWith(l); GeometRi3D.UseAbsoluteTolerance = false; GeometRi3D.Tolerance = tol; return(result); } //==================================================== Plane3d s = new Plane3d(this.A, this.Normal); object obj = l.IntersectionWith(s); if (obj == null) { return(null); } else { if (obj.GetType() == typeof(Line3d)) { // Coplanar line and triangle // Check intersection in one corner // or in corner and opposite side if (_a.BelongsTo(l)) { object obj2 = new Segment3d(_b, _c).IntersectionWith(l); if (obj2 != null && obj2.GetType() == typeof(Point3d)) { return(new Segment3d(_a, (Point3d)obj2)); } else { return(A); } } if (_b.BelongsTo(l)) { object obj2 = new Segment3d(_a, _c).IntersectionWith(l); if (obj2 != null && obj2.GetType() == typeof(Point3d)) { return(new Segment3d(_b, (Point3d)obj2)); } else { return(B); } } if (_c.BelongsTo(l)) { object obj2 = new Segment3d(_a, _b).IntersectionWith(l); if (obj2 != null && obj2.GetType() == typeof(Point3d)) { return(new Segment3d(_c, (Point3d)obj2)); } else { return(C); } } // Check intersection with two sides object objAB = new Segment3d(_a, _b).IntersectionWith(l); object objBC = new Segment3d(_b, _c).IntersectionWith(l); if (objAB != null && objAB.GetType() == typeof(Point3d)) { if (objBC != null && objBC.GetType() == typeof(Point3d)) { return(new Segment3d((Point3d)objAB, (Point3d)objBC)); } else { object objAC = new Segment3d(_a, _c).IntersectionWith(l); if (objAC != null && objAC.GetType() == typeof(Point3d)) { return(new Segment3d((Point3d)objAB, (Point3d)objAC)); } else { return((Point3d)objAB); } } } if (objBC != null && objBC.GetType() == typeof(Point3d)) { object objAC = new Segment3d(_a, _c).IntersectionWith(l); if (objAC != null && objAC.GetType() == typeof(Point3d)) { return(new Segment3d((Point3d)objBC, (Point3d)objAC)); } else { return((Point3d)objBC); } } object objAC2 = new Segment3d(_a, _c).IntersectionWith(l); if (objAC2 != null && objAC2.GetType() == typeof(Point3d)) { return((Point3d)objAC2); } else { return(null); } } else { // result of intersection is point Point3d p = (Point3d)obj; if (p.BelongsTo(this)) { return(p); } else { return(null); } } } }
/// <summary> /// Intersection of ellipsoid with line. /// Returns 'null' (no intersection) or object of type 'Point3d' or 'Segment3d'. /// </summary> public object IntersectionWith(Line3d s) { // Analytical solution from: // https://johannesbuchner.github.io/intersection/intersection_line_ellipsoid.html // Define local cordinate system for ellipsoid // and present line in parametric form in local coordinate system // x: t + x0 // y: k * t + y0 // z: l * t + z0 // For numerical stability choose local X axis such that k<=1 and l<=1 !!! Coord3d lc = new Coord3d(_point, _v1, _v2); Vector3d v0 = s.Direction.ConvertTo(lc); if (Abs(v0.Y) > Abs(v0.X) || Abs(v0.Z) > Abs(v0.X)) { // Bad choice of X axis, try again lc = new Coord3d(_point, _v2, _v3); v0 = s.Direction.ConvertTo(lc); if (Abs(v0.Y) > Abs(v0.X) || Abs(v0.Z) > Abs(v0.X)) { lc = new Coord3d(_point, _v3, _v1); v0 = s.Direction.ConvertTo(lc); } } // Normalize direction vector double k = v0.Y / v0.X; double l = v0.Z / v0.X; Point3d p0 = s.Point.ConvertTo(lc); double x0 = p0.X; double y0 = p0.Y; double z0 = p0.Z; double a2b2 = A * A * B * B; double a2c2 = A * A * C * C; double b2c2 = B * B * C * C; double det = a2b2 * C * C * (a2b2 * l * l + a2c2 * k * k - A * A * k * k * z0 * z0 + 2 * A * A * k * l * y0 * z0 - A * A * l * l * y0 * y0 + b2c2 - B * B * l * l * x0 * x0 + 2 * B * B * l * x0 * z0 - B * B * z0 * z0 - C * C * k * k * x0 * x0 + 2 * C * C * k * x0 * y0 - C * C * y0 * y0); if (det < -GeometRi3D.Tolerance) { return(null); } double sum1 = a2b2 * l * z0 + a2c2 * k * y0 + b2c2 * x0; double sum2 = a2b2 * l * l + a2c2 * k * k + b2c2; if (Abs(det) <= GeometRi3D.Tolerance) { // Intersection is point double t = -sum1 / sum2; return(new Point3d(t + x0, k * t + y0, l * t + z0, lc)); } else { double t = -(sum1 + Sqrt(det)) / sum2; Point3d p1 = new Point3d(t + x0, k * t + y0, l * t + z0, lc); t = -(sum1 - Sqrt(det)) / sum2; Point3d p2 = new Point3d(t + x0, k * t + y0, l * t + z0, lc); return(new Segment3d(p1, p2)); } }
/// <summary> /// Get intersection of line with triangle. /// Returns 'null' (no intersection) or object of type 'Point3d' or 'Segment3d'. /// </summary> public object IntersectionWith(Line3d l) { // Relative tolerance ================================ if (!GeometRi3D.UseAbsoluteTolerance) { double tol = GeometRi3D.Tolerance; GeometRi3D.Tolerance = tol * Max(AB, Max(BC, AC)); GeometRi3D.UseAbsoluteTolerance = true; object result = this.IntersectionWith(l); GeometRi3D.UseAbsoluteTolerance = false; GeometRi3D.Tolerance = tol; return(result); } //==================================================== Plane3d s = new Plane3d(this.A, this.Normal); object obj = l.IntersectionWith(s); if (obj == null) { return(null); } else { if (obj.GetType() == typeof(Line3d)) { Segment3d sAB = new Segment3d(A, B); Segment3d sBC = new Segment3d(B, C); Segment3d sAC = new Segment3d(A, C); // Line coincides with one side, return segment if (sAB.BelongsTo(l)) { return(sAB); } if (sBC.BelongsTo(l)) { return(sBC); } if (sAC.BelongsTo(l)) { return(sAC); } Point3d pAB = (Point3d)sAB.IntersectionWith(l); Point3d pBC = (Point3d)sBC.IntersectionWith(l); Point3d pAC = (Point3d)sAC.IntersectionWith(l); bool bAB = (object.ReferenceEquals(null, pAB)) ? false : pAB.BelongsTo(sAB); bool bAC = (object.ReferenceEquals(null, pAC)) ? false : pAC.BelongsTo(sAC); bool bBC = (object.ReferenceEquals(null, pBC)) ? false : pBC.BelongsTo(sBC); // Line crosses one corner, return point if (bAB && bBC && pAB == pBC && !bAC) { return(pAB); } if (bAB && bAC && pAB == pAC && !bBC) { return(pAB); } if (bAC && bBC && pAC == pBC && !bAB) { return(pAC); } // Line crosses two sides, return segment if (bAB && bBC && !bAC) { return(new Segment3d(pAB, pBC)); } if (bAB && bAC && !bBC) { return(new Segment3d(pAB, pAC)); } if (bAC && bBC && !bAB) { return(new Segment3d(pAC, pBC)); } // Line crosses one corner and one side, return segment if (pAB == pBC && bAC) { return(new Segment3d(pAB, pAC)); } if (pAB == pAC && bBC) { return(new Segment3d(pAB, pBC)); } if (pAC == pBC && bAB) { return(new Segment3d(pAB, pAC)); } //else return(null); } else { // result of intersection is point Point3d p = (Point3d)obj; if (p.BelongsTo(this)) { return(p); } else { return(null); } } } }
/// <summary> /// Reflect triangle in given line /// </summary> public Triangle ReflectIn(Line3d l) { return(new Triangle(_a.ReflectIn(l), _b.ReflectIn(l), _c.ReflectIn(l))); }
/// <summary> /// Intersection of circle with line. /// Returns 'null' (no intersection) or object of type 'Point3d' or 'Segment3d'. /// </summary> public object IntersectionWith(Line3d l) { Ellipse e = this.ToEllipse; return e.IntersectionWith(l); }
/// <summary> /// Reflect sphere in given line /// </summary> public Sphere ReflectIn(Line3d l) { return(new Sphere(this.Center.ReflectIn(l), this.R)); }
/// <summary> /// Orthogonal projection of the sphere to the line /// </summary> public Segment3d ProjectionTo(Line3d l) { Point3d p = this.Center.ProjectionTo(l); return(new Segment3d(p.Translate(this.R * l.Direction.Normalized), p.Translate(-this.R * l.Direction.Normalized))); }
/// <summary> /// Intersection of ellipse with line. /// Returns 'null' (no intersection) or object of type 'Point3d' or 'Segment3d'. /// </summary> public object IntersectionWith(Line3d l) { // Relative tolerance ================================ if (!GeometRi3D.UseAbsoluteTolerance) { double tol = GeometRi3D.Tolerance; GeometRi3D.Tolerance = tol * this.A; GeometRi3D.UseAbsoluteTolerance = true; object result = this.IntersectionWith(l); GeometRi3D.UseAbsoluteTolerance = false; GeometRi3D.Tolerance = tol; return(result); } //==================================================== if (l.Direction.IsOrthogonalTo(this.Normal)) { if (l.Point.BelongsTo(new Plane3d(this.Center, this.Normal))) { // coplanar objects // Find intersection of line and ellipse (2D) // Solution from: http://www.ambrsoft.com/TrigoCalc/Circles2/Ellipse/EllipseLine.htm Coord3d local_coord = new Coord3d(this.Center, this._v1, this._v2); Point3d p = l.Point.ConvertTo(local_coord); Vector3d v = l.Direction.ConvertTo(local_coord); double a = this.A; double b = this.B; if (Abs(v.Y / v.X) > 100) { // line is almost vertical, rotate local coord local_coord = new Coord3d(this.Center, this._v2, this._v1); p = l.Point.ConvertTo(local_coord); v = l.Direction.ConvertTo(local_coord); a = this.B; b = this.A; } // Line equation in form: y = mx + c double m = v.Y / v.X; double c = p.Y - m * p.X; double amb = Math.Pow(a, 2) * Math.Pow(m, 2) + Math.Pow(b, 2); double det = amb - Math.Pow(c, 2); if (det < -GeometRi3D.Tolerance) { return(null); } else if (GeometRi3D.AlmostEqual(det, 0)) { double x = -Math.Pow(a, 2) * m * c / amb; double y = Math.Pow(b, 2) * c / amb; return(new Point3d(x, y, 0, local_coord)); } else { double x1 = (-Math.Pow(a, 2) * m * c + a * b * Sqrt(det)) / amb; double x2 = (-Math.Pow(a, 2) * m * c - a * b * Sqrt(det)) / amb; double y1 = (Math.Pow(b, 2) * c + a * b * m * Sqrt(det)) / amb; double y2 = (Math.Pow(b, 2) * c - a * b * m * Sqrt(det)) / amb; return(new Segment3d(new Point3d(x1, y1, 0, local_coord), new Point3d(x2, y2, 0, local_coord))); } } else { // parallel objects return(null); } } else { // Line intersects ellipse' plane Point3d p = (Point3d)l.IntersectionWith(new Plane3d(this.Center, this.Normal)); if (p.BelongsTo(this)) { return(p); } else { return(null); } } }
/// <summary> /// Returns shortest distance to the line /// </summary> /// <param name="l"></param> /// <returns></returns> public double DistanceTo(Line3d l) { Vector3d v = new Vector3d(this, l.Point); return(v.Cross(l.Direction).Norm / l.Direction.Norm); }
/// <summary> /// Reflect line in given line /// </summary> public virtual Line3d ReflectIn(Line3d l) { return(new Line3d(this.Point.ReflectIn(l), this.Direction.ReflectIn(l))); }
/// <summary> /// Reflect point in given line /// </summary> public Point3d ReflectIn(Line3d l) { Vector3d v = new Vector3d(this, this.ProjectionTo(l)); return(this.Translate(2 * v)); }
/// <summary> /// Reflect ellipsoid in given line /// </summary> public Ellipsoid ReflectIn(Line3d l) { return(new Ellipsoid(this.Center.ReflectIn(l), _v1.ReflectIn(l), _v2.ReflectIn(l), _v3.ReflectIn(l))); }
/// <summary> /// Intersection of ellipse with plane. /// Returns 'null' (no intersection) or object of type 'Ellipse', 'Point3d' or 'Segment3d'. /// </summary> public object IntersectionWith(Plane3d s) { if (this.Normal.IsParallelTo(s.Normal)) { if (this.Center.BelongsTo(s)) { // coplanar objects return(this.Copy()); } else { // parallel objects return(null); } } else { Line3d l = (Line3d)s.IntersectionWith(new Plane3d(this.Center, this.Normal)); Coord3d local_coord = new Coord3d(this.Center, this._v1, this._v2); Point3d p = l.Point.ConvertTo(local_coord); Vector3d v = l.Direction.ConvertTo(local_coord); double a = this.A; double b = this.B; if (Abs(v.Y / v.X) > 100) { // line is almost vertical, rotate local coord local_coord = new Coord3d(this.Center, this._v2, this._v1); p = l.Point.ConvertTo(local_coord); v = l.Direction.ConvertTo(local_coord); a = this.B; b = this.A; } // Find intersection of line and ellipse (2D) // Solution from: http://www.ambrsoft.com/TrigoCalc/Circles2/Ellipse/EllipseLine.htm // Line equation in form: y = mx + c double m = v.Y / v.X; double c = p.Y - m * p.X; double amb = Math.Pow(a, 2) * Math.Pow(m, 2) + Math.Pow(b, 2); double det = amb - Math.Pow(c, 2); if (det < -GeometRi3D.Tolerance) { return(null); } else if (GeometRi3D.AlmostEqual(det, 0)) { double x = -Math.Pow(a, 2) * m * c / amb; double y = Math.Pow(b, 2) * c / amb; return(new Point3d(x, y, 0, local_coord)); } else { double x1 = (-Math.Pow(a, 2) * m * c + a * b * Sqrt(det)) / amb; double x2 = (-Math.Pow(a, 2) * m * c - a * b * Sqrt(det)) / amb; double y1 = (Math.Pow(b, 2) * c + a * b * m * Sqrt(det)) / amb; double y2 = (Math.Pow(b, 2) * c - a * b * m * Sqrt(det)) / amb; return(new Segment3d(new Point3d(x1, y1, 0, local_coord), new Point3d(x2, y2, 0, local_coord))); } } }