/// <summary>
/// Check if two objects are orthogonal
/// </summary>
public bool IsOrthogonalTo(ILinearObject obj)
{
Vector3d v = obj.Direction;
if ((this._coord != v._coord))
{
v = v.ConvertTo(this._coord);
}
double this_norm = this.Norm;
double v_norm = v.Norm;
if (GeometRi3D.UseAbsoluteTolerance)
{
if (GeometRi3D.Greater(this_norm, 0.0) && GeometRi3D.Greater(v_norm, 0.0))
{
return(GeometRi3D.AlmostEqual(Abs(this * v), 0.0));
}
else
{
return(false);
}
}
else
{
if (this_norm > 0.0 && v_norm > 0.0)
{
return(GeometRi3D.AlmostEqual(Abs(this * v) / (this_norm * v_norm), 0.0));
}
else
{
return(false);
}
}
}
/// <summary>
/// Intersection of circle with plane.
/// Returns 'null' (no intersection) or object of type 'Circle3d', 'Point3d' or 'Segment3d'.
/// </summary>
public object IntersectionWith(Plane3d s)
{
// Relative tolerance ================================
if (!GeometRi3D.UseAbsoluteTolerance)
{
double tol = GeometRi3D.Tolerance;
GeometRi3D.Tolerance = tol * this.R;
GeometRi3D.UseAbsoluteTolerance = true;
object result = this.IntersectionWith(s);
GeometRi3D.UseAbsoluteTolerance = false;
GeometRi3D.Tolerance = tol;
return result;
}
//====================================================
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, l.Direction, this.Normal.Cross(l.Direction));
Point3d p = l.Point.ConvertTo(local_coord);
if (GeometRi3D.Greater(Abs(p.Y), this.R))
{
return null;
}
else if (GeometRi3D.AlmostEqual(p.Y, this.R))
{
return new Point3d(0, this.R, 0, local_coord);
}
else if (GeometRi3D.AlmostEqual(p.Y, -this.R))
{
return new Point3d(0, -this.R, 0, local_coord);
}
else
{
double d = Sqrt(Math.Pow(this.R, 2) - Math.Pow(p.Y, 2));
Point3d p1 = new Point3d(-d, p.Y, 0, local_coord);
Point3d p2 = new Point3d(d, p.Y, 0, local_coord);
return new Segment3d(p1, p2);
}
}
}
/// <summary>
/// Intersection of circle with plane.
/// Returns 'null' (no intersection) or object of type 'Circle3d', '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, l.Direction, this.Normal.Cross(l.Direction));
Point3d p = l.Point.ConvertTo(local_coord);
if (GeometRi3D.Greater(Abs(p.Y), this.R))
{
return(null);
}
else if (GeometRi3D.AlmostEqual(p.Y, this.R))
{
return(new Point3d(0, this.R, 0, local_coord));
}
else if (GeometRi3D.AlmostEqual(p.Y, -this.R))
{
return(new Point3d(0, -this.R, 0, local_coord));
}
else
{
double d = Sqrt(Math.Pow(this.R, 2) - Math.Pow(p.Y, 2));
Point3d p1 = new Point3d(-d, p.Y, 0, local_coord);
Point3d p2 = new Point3d(d, p.Y, 0, local_coord);
return(new Segment3d(p1, p2));
}
}
}
//////////////////////////////////////////
#region "ParallelMethods"
/// <summary>
/// Check if two objects are parallel
/// </summary>
public bool IsParallelTo(ILinearObject obj)
{
Vector3d v = obj.Direction;
if ((this._coord != v._coord))
{
v = v.ConvertTo(this._coord);
}
double this_norm = this.Norm;
double v_norm = v.Norm;
if (GeometRi3D.Greater(this_norm, 0.0) && GeometRi3D.Greater(v_norm, 0.0))
{
return(GeometRi3D.AlmostEqual(this.Normalized.Cross(v.Normalized).Norm, 0.0));
}
else
{
return(false);
}
}
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>
/// Intersection of two circles.
/// Returns 'null' (no intersection) or object of type 'Circle3d', 'Point3d' or 'Segment3d'.
/// In 2D (coplanar circles) the segment will define two intersection points.
/// </summary>
public object IntersectionWith(Circle3d c)
{
// Relative tolerance ================================
if (!GeometRi3D.UseAbsoluteTolerance)
{
double tol = GeometRi3D.Tolerance;
GeometRi3D.Tolerance = tol * Max(this.R, c.R);
GeometRi3D.UseAbsoluteTolerance = true;
object result = this.IntersectionWith(c);
GeometRi3D.UseAbsoluteTolerance = false;
GeometRi3D.Tolerance = tol;
return(result);
}
//====================================================
if (this.Normal.IsParallelTo(c.Normal))
{
if (this.Center.BelongsTo(new Plane3d(c.Center, c.Normal)))
{
// Coplanar objects
// Search 2D intersection of two circles
// Equal circles
if (this.Center == c.Center && GeometRi3D.AlmostEqual(this.R, c.R))
{
return(this.Copy());
}
double d = this.Center.DistanceTo(c.Center);
// Separated circles
if (GeometRi3D.Greater(d, this.R + c.R))
{
return(null);
}
// One circle inside the other
if (d < Abs(this.R - c.R) - GeometRi3D.Tolerance)
{
return(null);
}
// Outer tangency
if (GeometRi3D.AlmostEqual(d, this.R + c.R))
{
Vector3d vec = new Vector3d(this.Center, c.Center);
return(this.Center.Translate(this.R * vec.Normalized));
}
// Inner tangency
if (Abs(Abs(this.R - c.R) - d) < GeometRi3D.Tolerance)
{
Vector3d vec = new Vector3d(this.Center, c.Center);
if (this.R > c.R)
{
return(this.Center.Translate(this.R * vec.Normalized));
}
else
{
return(this.Center.Translate(-this.R * vec.Normalized));
}
}
// intersecting circles
// Create local CS with origin in circle's center
Vector3d vec1 = new Vector3d(this.Center, c.Center);
Vector3d vec2 = vec1.Cross(this.Normal);
Coord3d local_cs = new Coord3d(this.Center, vec1, vec2);
double x = 0.5 * (d * d - c.R * c.R + this.R * this.R) / d;
double y = 0.5 * Sqrt((-d + c.R - this.R) * (-d - c.R + this.R) * (-d + c.R + this.R) * (d + c.R + this.R)) / d;
Point3d p1 = new Point3d(x, y, 0, local_cs);
Point3d p2 = new Point3d(x, -y, 0, local_cs);
return(new Segment3d(p1, p2));
}
else
{
// parallel objects
return(null);
}
}
else
{
// Check 3D intersection
Plane3d plane = new Plane3d(this.Center, this.Normal);
object obj = plane.IntersectionWith(c);
if (obj == null)
{
return(null);
}
else if (obj.GetType() == typeof(Point3d))
{
Point3d p = (Point3d)obj;
if (p.BelongsTo(c))
{
return(p);
}
else
{
return(null);
}
}
else
{
Segment3d s = (Segment3d)obj;
return(s.IntersectionWith(this));
}
}
}
/// <summary>
/// Factor a rotation matrix as product of three elemental rotations, i.e. rotations about the axes of a coordinate system.
/// <para>Both proper Euler angles ("xyx", "zxz", etc.) or Tait–Bryan angles ("xyz", "yzx") are allowed.</para>
/// Extrinsic rotations (rotations in fixed frame) should be written in lower case ("xyz", zxz", etc.).
/// <para>Intrinsic rotations (rotations in moving frame) should be written in upper case ("XYZ", "ZXZ", etc.).</para>
/// Note that such factorization generally is not unique!
/// </summary>
/// <param name="RotationOrder">String, representing rotation axes in the form "xyz" (extrinsic rotations, fixed frame) or "XYZ" (intrinsic rotations, moving frame).</param>
/// <param name="coord">Reference coordinate system, default - Coord3d.GlobalCS.</param>
/// <returns>Double array with first, second and third rotation angles</returns>
public double[] ToEulerAngles(string RotationOrder, Coord3d coord = null)
{
if (string.IsNullOrEmpty(RotationOrder) || RotationOrder.Length < 3)
{
throw new ArgumentException("Invalid parameter: RotationOrder");
}
coord = (coord == null) ? Coord3d.GlobalCS : coord;
Rotation r = this.ConvertTo(coord);
// Portions of the code were derived from article
// https://www.geometrictools.com/Documentation/EulerAngles.pdf
// published under Boost license
//============================================================================
// Boost Software License - Version 1.0 - August 17th, 2003
// Permission is hereby granted, free of charge, to any person or organization
//obtaining a copy of the software and accompanying documentation covered by
//this license(the "Software") to use, reproduce, display, distribute,
//execute, and transmit the Software, and to prepare derivative works of the
//Software, and to permit third - parties to whom the Software is furnished to
//do so, all subject to the following:
// The copyright notices in the Software and this entire statement, including
//the above license grant, this restriction and the following disclaimer,
//must be included in all copies of the Software, in whole or in part, and
//all derivative works of the Software, unless such copies or derivative
//works are solely in the form of machine-executable object code generated by
//a source language processor.
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON - INFRINGEMENT.IN NO EVENT
//SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
//FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
//ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
//DEALINGS IN THE SOFTWARE.
//============================================================================
double ax, ay, az;
if (RotationOrder == "XYZ" || RotationOrder == "zyx")
{
if (GeometRi3D.Smaller(r[0, 2], 1))
{
if (GeometRi3D.Greater(r[0, 2], -1))
{
ay = Asin(r[0, 2]);
ax = Atan2(-r[1, 2], r[2, 2]);
az = Atan2(-r[0, 1], r[0, 0]);
}
else
{
ay = -PI / 2.0;
ax = -Atan2(r[1, 0], r[1, 1]);
az = 0;
}
}
else
{
ay = PI / 2.0;
ax = Atan2(r[1, 0], r[1, 1]);
az = 0;
}
if (RotationOrder == "XYZ")
{
return(new[] { ax, ay, az });
}
else
{
return(new[] { az, ay, ax });
}
}
if (RotationOrder == "XZY" || RotationOrder == "yzx")
{
if (GeometRi3D.Smaller(r[0, 1], 1))
{
if (GeometRi3D.Greater(r[0, 1], -1))
{
az = Asin(-r[0, 1]);
ax = Atan2(r[2, 1], r[1, 1]);
ay = Atan2(r[0, 2], r[0, 0]);
}
else
{
az = PI / 2.0;
ax = -Atan2(-r[2, 0], r[2, 2]);
ay = 0;
}
}
else
{
az = -PI / 2.0;
ax = Atan2(-r[2, 0], r[2, 2]);
ay = 0;
}
if (RotationOrder == "XZY")
{
return(new[] { ax, az, ay });
}
else
{
return(new[] { ay, az, ax });
}
}
if (RotationOrder == "YXZ" || RotationOrder == "zxy")
{
if (GeometRi3D.Smaller(r[1, 2], 1))
{
if (GeometRi3D.Greater(r[1, 2], -1))
{
ax = Asin(-r[1, 2]);
ay = Atan2(r[0, 2], r[2, 2]);
az = Atan2(r[1, 0], r[1, 1]);
}
else
{
ax = PI / 2.0;
ay = -Atan2(-r[0, 2], r[0, 0]);
az = 0;
}
}
else
{
ax = -PI / 2.0;
ay = Atan2(-r[0, 1], r[0, 0]);
az = 0;
}
if (RotationOrder == "YXZ")
{
return(new[] { ay, ax, az });
}
else
{
return(new[] { az, ax, ay });
}
}
if (RotationOrder == "YZX" || RotationOrder == "xzy")
{
if (GeometRi3D.Smaller(r[1, 0], 1))
{
if (GeometRi3D.Greater(r[1, 0], -1))
{
az = Asin(r[1, 0]);
ay = Atan2(-r[2, 0], r[0, 0]);
ax = Atan2(-r[1, 2], r[1, 1]);
}
else
{
az = -PI / 2.0;
ay = -Atan2(r[2, 1], r[2, 2]);
ax = 0;
}
}
else
{
az = PI / 2.0;
ay = Atan2(r[2, 1], r[2, 2]);
ax = 0;
}
if (RotationOrder == "YZX")
{
return(new[] { ay, az, ax });
}
else
{
return(new[] { ax, az, ay });
}
}
if (RotationOrder == "ZXY" || RotationOrder == "yxz")
{
if (GeometRi3D.Smaller(r[2, 1], 1))
{
if (GeometRi3D.Greater(r[2, 1], -1))
{
ax = Asin(r[2, 1]);
az = Atan2(-r[0, 1], r[1, 1]);
ay = Atan2(-r[2, 0], r[2, 2]);
}
else
{
ax = -PI / 2.0;
az = -Atan2(r[0, 2], r[0, 0]);
ay = 0;
}
}
else
{
ax = PI / 2.0;
az = Atan2(r[0, 2], r[0, 0]);
ay = 0;
}
if (RotationOrder == "ZXY")
{
return(new[] { az, ax, ay });
}
else
{
return(new[] { ay, ax, az });
}
}
if (RotationOrder == "ZYX" || RotationOrder == "xyz")
{
if (GeometRi3D.Smaller(r[2, 0], 1))
{
if (GeometRi3D.Greater(r[2, 0], -1))
{
ay = Asin(-r[2, 0]);
az = Atan2(r[1, 0], r[0, 0]);
ax = Atan2(r[2, 1], r[2, 2]);
}
else
{
ay = PI / 2.0;
az = -Atan2(-r[1, 2], r[1, 1]);
ax = 0;
}
}
else
{
ay = -PI / 2.0;
az = Atan2(-r[1, 2], r[1, 1]);
ax = 0;
}
if (RotationOrder == "ZYX")
{
return(new[] { az, ay, ax });
}
else
{
return(new[] { ax, ay, az });
}
}
double a1, a2, a3;
if (RotationOrder == "XYX" || RotationOrder == "xyx")
{
if (GeometRi3D.Smaller(r[0, 0], 1))
{
if (GeometRi3D.Greater(r[0, 0], -1))
{
a2 = Acos(r[0, 0]);
a1 = Atan2(r[1, 0], -r[2, 0]);
a3 = Atan2(r[0, 1], r[0, 2]);
}
else
{
a2 = PI;
a1 = -Atan2(-r[1, 2], r[1, 1]);
a3 = 0;
}
}
else
{
a2 = 0;
a1 = Atan2(-r[1, 2], r[1, 1]);
a3 = 0;
}
if (RotationOrder == "XYX")
{
return(new[] { a1, a2, a3 });
}
else
{
return(new[] { a3, a2, a1 });
}
}
if (RotationOrder == "XZX" || RotationOrder == "xzx")
{
if (GeometRi3D.Smaller(r[0, 0], 1))
{
if (GeometRi3D.Greater(r[0, 0], -1))
{
a2 = Acos(r[0, 0]);
a1 = Atan2(r[2, 0], r[1, 0]);
a3 = Atan2(r[0, 2], -r[0, 1]);
}
else
{
a2 = PI;
a1 = -Atan2(-r[2, 1], r[2, 2]);
a3 = 0;
}
}
else
{
a2 = 0;
a1 = Atan2(r[2, 1], r[2, 2]);
a3 = 0;
}
if (RotationOrder == "XZX")
{
return(new[] { a1, a2, a3 });
}
else
{
return(new[] { a3, a2, a1 });
}
}
if (RotationOrder == "YXY" || RotationOrder == "yxy")
{
if (GeometRi3D.Smaller(r[1, 1], 1))
{
if (GeometRi3D.Greater(r[1, 1], -1))
{
a2 = Acos(r[1, 1]);
a1 = Atan2(r[0, 1], r[2, 1]);
a3 = Atan2(r[1, 0], -r[1, 2]);
}
else
{
a2 = PI;
a1 = -Atan2(r[0, 2], r[0, 0]);
a3 = 0;
}
}
else
{
a2 = 0;
a1 = Atan2(r[0, 2], r[0, 0]);
a3 = 0;
}
if (RotationOrder == "YXY")
{
return(new[] { a1, a2, a3 });
}
else
{
return(new[] { a3, a2, a1 });
}
}
if (RotationOrder == "YZY" || RotationOrder == "yzy")
{
if (GeometRi3D.Smaller(r[1, 1], 1))
{
if (GeometRi3D.Greater(r[1, 1], -1))
{
a2 = Acos(r[1, 1]);
a1 = Atan2(r[2, 1], -r[0, 1]);
a3 = Atan2(r[1, 2], r[1, 0]);
}
else
{
a2 = PI;
a1 = -Atan2(-r[2, 0], r[2, 2]);
a3 = 0;
}
}
else
{
a2 = 0;
a1 = Atan2(-r[2, 0], r[2, 2]);
a3 = 0;
}
if (RotationOrder == "YZY")
{
return(new[] { a1, a2, a3 });
}
else
{
return(new[] { a3, a2, a1 });
}
}
if (RotationOrder == "ZXZ" || RotationOrder == "zxz")
{
if (GeometRi3D.Smaller(r[2, 2], 1))
{
if (GeometRi3D.Greater(r[2, 2], -1))
{
a2 = Acos(r[2, 2]);
a1 = Atan2(r[0, 2], -r[1, 2]);
a3 = Atan2(r[2, 0], r[2, 1]);
}
else
{
a2 = PI;
a1 = -Atan2(-r[0, 1], r[0, 0]);
a3 = 0;
}
}
else
{
a2 = 0;
a1 = Atan2(-r[0, 1], r[0, 0]);
a3 = 0;
}
if (RotationOrder == "ZXZ")
{
return(new[] { a1, a2, a3 });
}
else
{
return(new[] { a3, a2, a1 });
}
}
if (RotationOrder == "ZYZ" || RotationOrder == "zyz")
{
if (GeometRi3D.Smaller(r[2, 2], 1))
{
if (GeometRi3D.Greater(r[2, 2], -1))
{
a2 = Acos(r[2, 2]);
a1 = Atan2(r[1, 2], r[0, 2]);
a3 = Atan2(r[2, 1], -r[2, 0]);
}
else
{
a2 = PI;
a1 = -Atan2(r[1, 0], r[1, 1]);
a3 = 0;
}
}
else
{
a2 = 0;
a1 = Atan2(r[1, 0], r[1, 1]);
a3 = 0;
}
if (RotationOrder == "ZYZ")
{
return(new[] { a1, a2, a3 });
}
else
{
return(new[] { a3, a2, a1 });
}
}
throw new ArgumentException("Invalid parameter: RotationOrder");
}
/// <summary>
/// Get intersection of segment with other segment.
/// Returns 'null' (no intersection) or object of type 'Point3d' or 'Segment3d'.
/// </summary>
public object IntersectionWith(Segment3d s)
{
if (this == s)
{
return(this.Copy());
}
object obj = this.ToLine.IntersectionWith(s);
if (obj == null)
{
return(null);
}
if (obj.GetType() == typeof(Point3d))
{
Point3d p = (Point3d)obj;
if (p.BelongsTo(this) && p.BelongsTo(s))
{
return(p);
}
else
{
return(null);
}
}
else if (obj.GetType() == typeof(Segment3d))
{
// Segments are collinear
// Relative tolerance check ================================
double tol = GeometRi3D.Tolerance;
if (!GeometRi3D.UseAbsoluteTolerance)
{
tol = GeometRi3D.Tolerance * Max(this.Length, s.Length);
}
//==========================================================
// Create local CS with X-axis along segment 's'
Vector3d v2 = s.ToVector.OrthogonalVector;
Coord3d cs = new Coord3d(s.P1, s.ToVector, v2);
double x1 = 0.0;
double x2 = s.Length;
double x3 = this.P1.ConvertTo(cs).X;
double x4 = this.P2.ConvertTo(cs).X;
if (GeometRi3D.Smaller(Max(x3, x4), x1, tol) || GeometRi3D.Greater(Min(x3, x4), x2, tol))
{
return(null);
}
if (GeometRi3D.AlmostEqual(Max(x3, x4), x1, tol))
{
return(new Point3d(x1, 0, 0, cs));
}
if (GeometRi3D.AlmostEqual(Min(x3, x4), x2, tol))
{
return(new Point3d(x2, 0, 0, cs));
}
if (GeometRi3D.Smaller(Min(x3, x4), x1, tol) && GeometRi3D.Greater(Max(x3, x4), x2, tol))
{
return(s.Copy());
}
if (GeometRi3D.Greater(Min(x3, x4), x1, tol) && GeometRi3D.Smaller(Max(x3, x4), x2, tol))
{
return(this.Copy());
}
if (GeometRi3D.Smaller(Min(x3, x4), x1, tol))
{
return(new Segment3d(new Point3d(x1, 0, 0, cs), new Point3d(Max(x3, x4), 0, 0, cs)));
}
if (GeometRi3D.Greater(Max(x3, x4), x2, tol))
{
return(new Segment3d(new Point3d(x2, 0, 0, cs), new Point3d(Min(x3, x4), 0, 0, cs)));
}
return(null);
}
else
{
return(null);
}
}
/// <summary>
/// Get intersection of segment with other segment.
/// Returns 'null' (no intersection) or object of type 'Point3d' or 'Segment3d'.
/// </summary>
public object IntersectionWith(Segment3d s)
{
if (this == s)
{
return(this.Copy());
}
Line3d l1 = this.ToLine;
Line3d l2 = s.ToLine;
if (this.BelongsTo(l2) || s.BelongsTo(l1))
{
// Segments are collinear
// Relative tolerance check ================================
double tol = GeometRi3D.Tolerance;
if (!GeometRi3D.UseAbsoluteTolerance)
{
tol = GeometRi3D.Tolerance * Max(this.Length, s.Length);
}
//==========================================================
// Create local CS with X-axis along segment 's'
Vector3d v2 = s.ToVector.OrthogonalVector;
Coord3d cs = new Coord3d(s.P1, s.ToVector, v2);
double x1 = 0.0;
double x2 = s.Length;
double t3 = this.P1.ConvertTo(cs).X;
double t4 = this.P2.ConvertTo(cs).X;
double x3 = Min(t3, t4);
double x4 = Max(t3, t4);
// Segments do not overlap
if (GeometRi3D.Smaller(x4, x1, tol) || GeometRi3D.Greater(x3, x2, tol))
{
return(null);
}
// One common point
if (GeometRi3D.AlmostEqual(Max(x3, x4), x1, tol))
{
return(new Point3d(x1, 0, 0, cs));
}
if (GeometRi3D.AlmostEqual(Min(x3, x4), x2, tol))
{
return(new Point3d(x2, 0, 0, cs));
}
// Overlaping segments
x1 = Max(x1, x3);
x2 = Min(x2, x4);
return(new Segment3d(new Point3d(x1, 0, 0, cs), new Point3d(x2, 0, 0, cs)));
}
else
{
Point3d p = l1.PerpendicularTo(l2);
if (p.BelongsTo(this) && p.BelongsTo(s))
{
return(p);
}
else
{
return(null);
}
}
}