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)));
}
}
}