internal override int _PointLocation(Point3d p)
{
Coord3d lc = new Coord3d(this.Center, this.SemiaxisA, this.SemiaxisB);
p = p.ConvertTo(lc);
if (GeometRi3D.UseAbsoluteTolerance)
{
double dist = this.ClosestPoint(p).DistanceTo(p);
if (GeometRi3D.AlmostEqual(dist, 0))
{
return(0); // Point is on boundary
}
if (GeometRi3D.Smaller(p.X * p.X / (A * A) + p.Y * p.Y / (B * B) + p.Z * p.Z / (C * C), 1.0))
{
return(1); // Point is strictly inside box
}
return(-1); // Point is outside
}
else
{
double tol = GeometRi3D.Tolerance;
GeometRi3D.Tolerance = tol * this.A;
GeometRi3D.UseAbsoluteTolerance = true;
int result = this._PointLocation(p);
GeometRi3D.UseAbsoluteTolerance = false;
GeometRi3D.Tolerance = tol;
return(result);
}
}
/// <summary>
/// Check if point is inside ellipsoid
/// </summary>
public bool IsInside(Ellipsoid e)
{
Coord3d lc = new Coord3d(e.Center, e.SemiaxisA, e.SemiaxisB);
Point3d p = this.ConvertTo(lc);
if (GeometRi3D.UseAbsoluteTolerance)
{
return(GeometRi3D.Smaller(p.X * p.X / e.A / e.A + p.Y * p.Y / e.B / e.B + p.Z * p.Z / e.C / e.C, 1.0));
}
else
{
double tol = GeometRi3D.Tolerance;
GeometRi3D.Tolerance = tol * e.SemiaxisA.Norm;
GeometRi3D.UseAbsoluteTolerance = true;
bool result = this.IsInside(e);
GeometRi3D.UseAbsoluteTolerance = false;
GeometRi3D.Tolerance = tol;
return(result);
}
}
internal override int _PointLocation(Point3d p)
{
if (GeometRi3D.UseAbsoluteTolerance)
{
Plane3d s = new Plane3d(this.Center, this.Normal);
Point3d proj = p.ProjectionTo(s);
if (GeometRi3D.AlmostEqual(p.DistanceTo(proj), 0))
{
if (GeometRi3D.Smaller(p.DistanceTo(this.Center), this.R))
{
return(1); // Point is strictly inside
}
else if (GeometRi3D.AlmostEqual(p.DistanceTo(this.Center), this.R))
{
return(0); // Point is on boundary
}
else
{
return(-1); // Point is outside
}
}
else
{
return(-1); // Point is outside
}
}
else
{
double tol = GeometRi3D.Tolerance;
GeometRi3D.Tolerance = tol * this.R;
GeometRi3D.UseAbsoluteTolerance = true;
int result = this._PointLocation(p);
GeometRi3D.UseAbsoluteTolerance = false;
GeometRi3D.Tolerance = tol;
return(result);
}
}
/// <summary>
/// Check if point is inside ellipse
/// </summary>
/// <returns>True, if the point is inside ellipse</returns>
public bool IsInside(Ellipse e)
{
if (GeometRi3D.UseAbsoluteTolerance)
{
if (this.BelongsTo(new Plane3d(e.Center, e.MajorSemiaxis, e.MinorSemiaxis)))
{
return(GeometRi3D.Smaller(this.DistanceTo(e.F1) + this.DistanceTo(e.F2), 2 * e.A));
}
else
{
return(false);
}
}
else
{
double tol = GeometRi3D.Tolerance;
GeometRi3D.Tolerance = tol * e.MajorSemiaxis.Norm;
GeometRi3D.UseAbsoluteTolerance = true;
bool result = this.IsInside(e);
GeometRi3D.UseAbsoluteTolerance = false;
GeometRi3D.Tolerance = tol;
return(result);
}
}
/// <summary>
/// Check if point is inside circle
/// </summary>
/// <returns>True, if the point is inside circle</returns>
public bool IsInside(Circle3d c)
{
if (GeometRi3D.UseAbsoluteTolerance)
{
if (this.BelongsTo(new Plane3d(c.Center, c.Normal)))
{
return(GeometRi3D.Smaller(this.DistanceTo(c.Center), c.R));
}
else
{
return(false);
}
}
else
{
double tol = GeometRi3D.Tolerance;
GeometRi3D.Tolerance = tol * c.R;
GeometRi3D.UseAbsoluteTolerance = true;
bool result = this.IsInside(c);
GeometRi3D.UseAbsoluteTolerance = false;
GeometRi3D.Tolerance = tol;
return(result);
}
}
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>
/// 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);
}
}
}
