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>
/// Determines whether two objects are equal.
/// </summary>
public override bool Equals(object obj)
{
if (obj == null || (!object.ReferenceEquals(this.GetType(), obj.GetType())))
{
return(false);
}
Box3d b = (Box3d)obj;
if (GeometRi3D.UseAbsoluteTolerance)
{
return(this.Center == b.Center && _r == b.Orientation &&
GeometRi3D.AlmostEqual(L1, b.L1) &&
GeometRi3D.AlmostEqual(L2, b.L2) &&
GeometRi3D.AlmostEqual(L3, b.L3));
}
else
{
double tol = GeometRi3D.Tolerance;
GeometRi3D.Tolerance = tol * this.Diagonal;
GeometRi3D.UseAbsoluteTolerance = true;
bool result = this.Equals(b);
GeometRi3D.UseAbsoluteTolerance = false;
GeometRi3D.Tolerance = tol;
return(result);
}
}
/// <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);
}
}
}
internal override int _PointLocation(Point3d p)
{
if (GeometRi3D.UseAbsoluteTolerance)
{
Plane3d s = new Plane3d(this.A, this.Normal);
Point3d proj = p.ProjectionTo(s);
if (GeometRi3D.AlmostEqual(p.DistanceTo(proj), 0))
{
if (p.BelongsTo(new Segment3d(_a, _b)) || p.BelongsTo(new Segment3d(_a, _b)) || p.BelongsTo(new Segment3d(_a, _b)))
{
return(0); // Point is on boundary
}
else
{
double area = this.Area;
double alpha = new Vector3d(proj, _b).Cross(new Vector3d(proj, _c)).Norm / (2 * area);
double beta = new Vector3d(proj, _c).Cross(new Vector3d(proj, _a)).Norm / (2 * area);
double gamma = 1 - alpha - beta;
if (0 < alpha && alpha < 1 && 0 < beta && beta < 1 && 0 < gamma && gamma < 1)
{
return(1); // Point is strictly inside
}
else
{
return(-1);
}
}
}
else
{
return(-1); // Point is outside
}
}
else
{
double tol = GeometRi3D.Tolerance;
GeometRi3D.Tolerance = tol * (AB + BC + AC) / 3;
GeometRi3D.UseAbsoluteTolerance = true;
int result = this._PointLocation(p);
GeometRi3D.UseAbsoluteTolerance = false;
GeometRi3D.Tolerance = tol;
return(result);
}
}
/// <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);
}
}
/// <summary>
/// Calculates the point on the ellipse's boundary closest to given point.
/// </summary>
public Point3d ClosestPoint(Point3d p)
{
// Algorithm by Dr. Robert Nurnberg
// http://wwwf.imperial.ac.uk/~rn/distance2ellipse.pdf
// Does not work for interior points
Coord3d local_coord = new Coord3d(this.Center, this._v1, this._v2);
p = p.ConvertTo(local_coord);
if (GeometRi3D.AlmostEqual(p.X, 0) && GeometRi3D.AlmostEqual(p.Y, 0))
{
// Center point, choose any minor-axis
return(new Point3d(0, this.B, 0, local_coord));
}
double theta = Atan2(this.A * p.Y, this.B * p.X);
int iter = 0;
int max_iter = 100;
Point3d n0 = p.Copy();
while (iter < max_iter)
{
iter += 1;
double f = (A * A - B * B) * Cos(theta) * Sin(theta) - p.X * A * Sin(theta) + p.Y * B * Cos(theta);
double f_prim = (A * A - B * B) * (Cos(theta) * Cos(theta) - Sin(theta) * Sin(theta))
- p.X * A * Cos(theta) - p.Y * B * Sin(theta);
theta = theta - f / f_prim;
Point3d n = new Point3d(A * Cos(theta), B * Sin(theta), 0, local_coord);
if (n0.DistanceTo(n) < GeometRi3D.Tolerance)
{
return(n);
}
n0 = n.Copy();
}
return(n0);
}
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);
}
}
internal override int _PointLocation(Point3d p)
{
if (GeometRi3D.UseAbsoluteTolerance)
{
Point3d proj = p.ProjectionTo(this.ToLine);
if (GeometRi3D.AlmostEqual(p.DistanceTo(proj), 0))
{
if (GeometRi3D.AlmostEqual(p.DistanceTo(this.P1), 0) || GeometRi3D.AlmostEqual(p.DistanceTo(this.P2), 0))
{
return(0); // Point is on boundary
}
else if (Abs(new Vector3d(proj, this.P1).AngleTo(new Vector3d(proj, this.P2))) < 2e-8)
// Need to decrease accuracy due to Acos calculations
{
return(-1); // Point is outside
}
else
{
return(1); // // Point is strictly inside
}
}
else
{
return(-1); // Point is outside
}
}
else
{
double tol = GeometRi3D.Tolerance;
GeometRi3D.Tolerance = tol * this.Length;
GeometRi3D.UseAbsoluteTolerance = true;
int result = this._PointLocation(p);
GeometRi3D.UseAbsoluteTolerance = false;
GeometRi3D.Tolerance = tol;
return(result);
}
}
/// <summary>
/// Returns the hashcode for the object.
/// </summary>
public override int GetHashCode()
{
return(GeometRi3D.HashFunction(Row1.GetHashCode(), Row2.GetHashCode(), Row3.GetHashCode()));
}
/// <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>
/// Returns the hashcode for the object.
/// </summary>
public override int GetHashCode()
{
return(GeometRi3D.HashFunction(_a.GetHashCode(), _b.GetHashCode(), _c.GetHashCode()));
}
/// <summary>
/// Returns the hashcode for the object.
/// </summary>
public override int GetHashCode()
{
int hash_code = GeometRi3D.HashFunction(_lx.GetHashCode(), _ly.GetHashCode(), _lz.GetHashCode());
return(GeometRi3D.HashFunction(_center.GetHashCode(), _r.GetHashCode(), hash_code));
}
/// <summary>
/// Intersection of ellipsoid with plane.
/// Returns 'null' (no intersection) or object of type 'Point3d' or 'Ellipse'.
/// </summary>
public object IntersectionWith(Plane3d plane)
{
// Solution 1:
// Peter Paul Klein
// On the Ellipsoid and Plane Intersection Equation
// Applied Mathematics, 2012, 3, 1634-1640 (DOI:10.4236/am.2012.311226)
// Solution 2:
// Sebahattin Bektas
// Intersection of an Ellipsoid and a Plane
// International Journal of Research in Engineering and Applied Sciences, VOLUME 6, ISSUE 6 (June, 2016)
Coord3d lc = new Coord3d(_point, _v1, _v2, "LC1");
plane.SetCoord(lc);
double Ax, Ay, Az, Ad;
double a, b, c;
if (Abs(plane.C) >= Abs(plane.A) && Abs(plane.C) >= Abs(plane.B))
{
a = this.A; b = this.B; c = this.C;
}
else
{
lc = new Coord3d(_point, _v2, _v3, "LC2");
plane.SetCoord(lc);
if (Abs(plane.C) >= Abs(plane.A) && Abs(plane.C) >= Abs(plane.B))
{
a = this.B; b = this.C; c = this.A;
}
else
{
lc = new Coord3d(_point, _v3, _v1, "LC3");
plane.SetCoord(lc);
a = this.C; b = this.A; c = this.B;
}
}
Ax = plane.A; Ay = plane.B; Az = plane.C; Ad = plane.D;
double tmp = (Az * Az * c * c);
double AA = 1.0 / (a * a) + Ax * Ax / tmp;
double BB = 2.0 * Ax * Ay / tmp;
double CC = 1.0 / (b * b) + Ay * Ay / tmp;
double DD = 2.0 * Ax * Ad / tmp;
double EE = 2.0 * Ay * Ad / tmp;
double FF = Ad * Ad / tmp - 1.0;
double det = 4.0 * AA * CC - BB * BB;
if (GeometRi3D.AlmostEqual(det, 0))
{
return(null);
}
double X0 = (BB * EE - 2 * CC * DD) / det;
double Y0 = (BB * DD - 2 * AA * EE) / det;
double Z0 = -(Ax * X0 + Ay * Y0 + Ad) / Az;
Point3d P0 = new Point3d(X0, Y0, Z0, lc);
if (P0.IsOnBoundary(this))
{
// the plane is tangent to ellipsoid
return(P0);
}
else if (P0.IsInside(this))
{
Vector3d q = P0.ToVector.ConvertTo(lc);
Matrix3d D1 = Matrix3d.DiagonalMatrix(1 / a, 1 / b, 1 / c);
Vector3d r = plane.Normal.ConvertTo(lc).OrthogonalVector.Normalized;
Vector3d s = plane.Normal.ConvertTo(lc).Cross(r).Normalized;
double omega = 0;
double qq, qr, qs, rr, ss, rs;
if (!GeometRi3D.AlmostEqual((D1 * r) * (D1 * s), 0))
{
rr = (D1 * r) * (D1 * r);
rs = (D1 * r) * (D1 * s);
ss = (D1 * s) * (D1 * s);
if (GeometRi3D.AlmostEqual(rr - ss, 0))
{
omega = PI / 4;
}
else
{
omega = 0.5 * Atan(2.0 * rs / (rr - ss));
}
Vector3d rprim = Cos(omega) * r + Sin(omega) * s;
Vector3d sprim = -Sin(omega) * r + Cos(omega) * s;
r = rprim;
s = sprim;
}
qq = (D1 * q) * (D1 * q);
qr = (D1 * q) * (D1 * r);
qs = (D1 * q) * (D1 * s);
rr = (D1 * r) * (D1 * r);
ss = (D1 * s) * (D1 * s);
double d = qq - qr * qr / rr - qs * qs / ss;
AA = Sqrt((1 - d) / rr);
BB = Sqrt((1 - d) / ss);
return(new Ellipse(P0, AA * r, BB * s));
}
else
{
return(null);
}
}
/// <summary>
/// Returns the hashcode for the object.
/// </summary>
public override int GetHashCode()
{
return(GeometRi3D.HashFunction(_p1.GetHashCode(), _p2.GetHashCode()));
}
/// <summary>
/// Returns the hashcode for the object.
/// </summary>
public override int GetHashCode()
{
return(GeometRi3D.HashFunction(val[0].GetHashCode(), val[1].GetHashCode(), val[2].GetHashCode(), _coord.GetHashCode()));
}
/// <summary>
/// Calculates the point on the ellipsoid's boundary closest to given point.
/// </summary>
public Point3d ClosestPoint(Point3d p)
{
// Algorithm by Dr. Robert Nurnberg
// http://wwwf.imperial.ac.uk/~rn/distance2ellipse.pdf
Coord3d local_coord = new Coord3d(this.Center, this._v1, this._v2);
p = p.ConvertTo(local_coord);
if (GeometRi3D.AlmostEqual(p.X, 0) && GeometRi3D.AlmostEqual(p.Y, 0))
{
// Center point, choose any minor-axis
return(new Point3d(0, C, 0, local_coord));
}
double theta = Atan2(A * p.Y, B * p.X);
double phi = Atan2(p.Z, C * Sqrt((p.X * p.X) / (A * A) + (p.Y * p.Y) / (B * B)));
int iter = 0;
int max_iter = 100;
Point3d n0 = p.Copy();
while (iter < max_iter)
{
iter += 1;
double ct = Cos(theta);
double st = Sin(theta);
double cp = Cos(phi);
double sp = Sin(phi);
double F1 = (A * A - B * B) * ct * st * cp - p.X * A * st + p.Y * B * ct;
double F2 = (A * A * ct * ct + B * B * st * st - C * C) * sp * cp - p.X * A * sp * ct - p.Y * B * sp * st + p.Z * C * cp;
double a11 = (A * A - B * B) * (ct * ct - st * st) * cp - p.X * A * ct - p.Y * B * st;
double a12 = -(A * A - B * B) * ct * st * sp;
double a21 = -2.0 * (A * A - B * B) * ct * st * cp * sp + p.X * A * sp * st - p.Y * B * sp * ct;
double a22 = (A * A * ct * ct + B * B * st * st - C * C) * (cp * cp - sp * sp) - p.X * A * cp * ct - p.Y * B * cp * st - p.Z * C * sp;
double det = a11 * a22 - a12 * a21;
if (det == 0)
{
throw new Exception("Zero determinant");
}
// Calc reverse matrix B[ij] = A[ij]^-1
double b11 = a22 / det;
double b12 = -a12 / det;
double b21 = -a21 / det;
double b22 = a11 / det;
theta = theta - (b11 * F1 + b12 * F2);
phi = phi - (b21 * F1 + b22 * F2);
Point3d n = new Point3d(A * Cos(phi) * Cos(theta), B * Cos(phi) * Sin(theta), C * Sin(phi), local_coord);
if (n0.DistanceTo(n) < GeometRi3D.Tolerance)
{
return(n);
}
n0 = n.Copy();
}
return(n0);
}
/// <summary>
/// Angle between two objects in radians (0 < angle < Pi)
/// </summary>
public double AngleTo(IPlanarObject obj)
{
return(GeometRi3D.GetAngle(this, obj));
}
/// <summary>
/// Determines whether two objects are equal.
/// </summary>
public override bool Equals(object obj)
{
if (obj == null || (!object.ReferenceEquals(this.GetType(), obj.GetType())))
{
return(false);
}
Ellipse e = (Ellipse)obj;
if (GeometRi3D.UseAbsoluteTolerance)
{
if (GeometRi3D.AlmostEqual(this.A, this.B))
{
// Ellipse is circle
if (GeometRi3D.AlmostEqual(e.A, e.B))
{
// Second ellipse also circle
return(this.Center == e.Center && GeometRi3D.AlmostEqual(this.A, e.A) && e.Normal.IsParallelTo(this.Normal));
}
else
{
return(false);
}
}
else
{
return(this.Center == e.Center && GeometRi3D.AlmostEqual(this.A, e.A) && GeometRi3D.AlmostEqual(this.B, e.B) &&
e.MajorSemiaxis.IsParallelTo(this.MajorSemiaxis) && e.MinorSemiaxis.IsParallelTo(this.MinorSemiaxis));
}
}
else
{
double tol = GeometRi3D.Tolerance;
GeometRi3D.Tolerance = tol * e.MajorSemiaxis.Norm;
GeometRi3D.UseAbsoluteTolerance = true;
if (GeometRi3D.AlmostEqual(this.A, this.B))
{
// Ellipse is circle
if (GeometRi3D.AlmostEqual(e.A, e.B))
{
// Second ellipse also circle
bool res1 = this.Center == e.Center && GeometRi3D.AlmostEqual(this.A, e.A);
GeometRi3D.UseAbsoluteTolerance = false;
GeometRi3D.Tolerance = tol;
bool res2 = e.Normal.IsParallelTo(this.Normal);
return(res1 && res2);
}
else
{
GeometRi3D.UseAbsoluteTolerance = false;
GeometRi3D.Tolerance = tol;
return(false);
}
}
else
{
bool res1 = this.Center == e.Center && GeometRi3D.AlmostEqual(this.A, e.A) && GeometRi3D.AlmostEqual(this.B, e.B);
GeometRi3D.UseAbsoluteTolerance = false;
GeometRi3D.Tolerance = tol;
bool res2 = e.MajorSemiaxis.IsParallelTo(this.MajorSemiaxis) && e.MinorSemiaxis.IsParallelTo(this.MinorSemiaxis);
return(res1 && res2);
}
}
}
/// <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)));
}
}
}
/// <summary>
/// Determines whether two objects are equal.
/// </summary>
public override bool Equals(object obj)
{
if (obj == null || (!object.ReferenceEquals(this.GetType(), obj.GetType())))
{
return(false);
}
Ellipsoid e = (Ellipsoid)obj;
if (GeometRi3D.UseAbsoluteTolerance)
{
if (this.Center != e.Center)
{
return(false);
}
if (GeometRi3D.AlmostEqual(this.A, this.B) && GeometRi3D.AlmostEqual(this.A, this.C))
{
// Ellipsoid is sphere
if (GeometRi3D.AlmostEqual(e.A, e.B) && GeometRi3D.AlmostEqual(e.A, e.C))
{
// Second ellipsoid also sphere
return(GeometRi3D.AlmostEqual(this.A, e.A));
}
else
{
return(false);
}
}
else if (GeometRi3D.AlmostEqual(this.A, this.B) && GeometRi3D.AlmostEqual(e.A, e.B))
{
return(GeometRi3D.AlmostEqual(this.A, e.A) && GeometRi3D.AlmostEqual(this.C, e.C) &&
e.SemiaxisC.IsParallelTo(this.SemiaxisC));
}
else if (GeometRi3D.AlmostEqual(this.A, this.C) && GeometRi3D.AlmostEqual(e.A, e.C))
{
return(GeometRi3D.AlmostEqual(this.A, e.A) && GeometRi3D.AlmostEqual(this.B, e.B) &&
e.SemiaxisB.IsParallelTo(this.SemiaxisB));
}
else if (GeometRi3D.AlmostEqual(this.C, this.B) && GeometRi3D.AlmostEqual(e.C, e.B))
{
return(GeometRi3D.AlmostEqual(this.A, e.A) && GeometRi3D.AlmostEqual(this.C, e.C) &&
e.SemiaxisA.IsParallelTo(this.SemiaxisA));
}
else
{
return(GeometRi3D.AlmostEqual(this.A, e.A) && e.SemiaxisA.IsParallelTo(this.SemiaxisA) &&
GeometRi3D.AlmostEqual(this.B, e.B) && e.SemiaxisB.IsParallelTo(this.SemiaxisB) &&
GeometRi3D.AlmostEqual(this.C, e.C) && e.SemiaxisC.IsParallelTo(this.SemiaxisC));
}
}
else
{
double tol = GeometRi3D.Tolerance;
GeometRi3D.Tolerance = tol * e.SemiaxisA.Norm;
GeometRi3D.UseAbsoluteTolerance = true;
if (this.Center != e.Center)
{
GeometRi3D.UseAbsoluteTolerance = false;
GeometRi3D.Tolerance = tol;
return(false);
}
if (GeometRi3D.AlmostEqual(this.A, this.B) && GeometRi3D.AlmostEqual(this.A, this.C))
{
// Ellipsoid is sphere
if (GeometRi3D.AlmostEqual(e.A, e.B) && GeometRi3D.AlmostEqual(e.A, e.C))
{
// Second ellipsoid also sphere
bool res = GeometRi3D.AlmostEqual(this.A, e.A);
GeometRi3D.UseAbsoluteTolerance = false;
GeometRi3D.Tolerance = tol;
return(res);
}
else
{
GeometRi3D.UseAbsoluteTolerance = false;
GeometRi3D.Tolerance = tol;
return(false);
}
}
else if (GeometRi3D.AlmostEqual(this.A, this.B) && GeometRi3D.AlmostEqual(e.A, e.B))
{
bool res1 = GeometRi3D.AlmostEqual(this.A, e.A) && GeometRi3D.AlmostEqual(this.C, e.C);
GeometRi3D.UseAbsoluteTolerance = false;
GeometRi3D.Tolerance = tol;
bool res2 = e.SemiaxisC.IsParallelTo(this.SemiaxisC);
return(res1 && res2);
}
else if (GeometRi3D.AlmostEqual(this.A, this.C) && GeometRi3D.AlmostEqual(e.A, e.C))
{
bool res1 = GeometRi3D.AlmostEqual(this.A, e.A) && GeometRi3D.AlmostEqual(this.B, e.B);
GeometRi3D.UseAbsoluteTolerance = false;
GeometRi3D.Tolerance = tol;
bool res2 = e.SemiaxisB.IsParallelTo(this.SemiaxisB);
return(res1 && res2);
}
else if (GeometRi3D.AlmostEqual(this.C, this.B) && GeometRi3D.AlmostEqual(e.C, e.B))
{
bool res1 = GeometRi3D.AlmostEqual(this.A, e.A) && GeometRi3D.AlmostEqual(this.C, e.C);
GeometRi3D.UseAbsoluteTolerance = false;
GeometRi3D.Tolerance = tol;
bool res2 = e.SemiaxisA.IsParallelTo(this.SemiaxisA);
return(res1 && res2);
}
else
{
bool res1 = GeometRi3D.AlmostEqual(this.A, e.A) && GeometRi3D.AlmostEqual(this.B, e.B) && GeometRi3D.AlmostEqual(this.C, e.C);
GeometRi3D.UseAbsoluteTolerance = false;
GeometRi3D.Tolerance = tol;
bool res2 = e.SemiaxisA.IsParallelTo(this.SemiaxisA) && e.SemiaxisB.IsParallelTo(this.SemiaxisB) && e.SemiaxisC.IsParallelTo(this.SemiaxisC);
return(res1 && res2);
}
}
}
/// <summary>
/// Returns the hashcode for the object.
/// </summary>
public override int GetHashCode()
{
return(GeometRi3D.HashFunction(_point.GetHashCode(), _dir.GetHashCode()));
}
/// <summary>
/// Returns the hashcode for the object.
/// </summary>
public override int GetHashCode()
{
return(GeometRi3D.HashFunction(_x.GetHashCode(), _y.GetHashCode(), _z.GetHashCode(), _coord.GetHashCode()));
}
