/// <summary>
/// Return Axis Aligned Bounding Box (AABB) in given coordinate system.
/// </summary>
public Box3d BoundingBox(Coord3d coord = null)
{
coord = (coord == null) ? Coord3d.GlobalCS : coord;
Point3d c = _center.ConvertTo(coord);
double mx = c.X;
double my = c.Y;
double mz = c.Z;
foreach (Point3d p in this.ListOfPoints)
{
Point3d t = p.ConvertTo(coord);
if (t.X < mx)
{
mx = t.X;
}
if (t.Y < my)
{
my = t.Y;
}
if (t.Z < mz)
{
mz = t.Z;
}
}
return(new Box3d(c, 2.0 * (c.X - mx), 2.0 * (c.Y - my), 2.0 * (c.Z - mz), coord));
}
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>
/// Initializes circle passing through three points.
/// </summary>
public Circle3d(Point3d p1, Point3d p2, Point3d p3)
{
Vector3d v1 = new Vector3d(p1, p2);
Vector3d v2 = new Vector3d(p1, p3);
if (v1.Cross(v2).Norm < GeometRi3D.Tolerance)
{
throw new Exception("Collinear points");
}
Coord3d CS = new Coord3d(p1, v1, v2);
Point3d a1 = p1.ConvertTo(CS);
Point3d a2 = p2.ConvertTo(CS);
Point3d a3 = p3.ConvertTo(CS);
double d1 = Math.Pow(a1.X, 2) + Math.Pow(a1.Y, 2);
double d2 = Math.Pow(a2.X, 2) + Math.Pow(a2.Y, 2);
double d3 = Math.Pow(a3.X, 2) + Math.Pow(a3.Y, 2);
double f = 2.0 * (a1.X * (a2.Y - a3.Y) - a1.Y * (a2.X - a3.X) + a2.X * a3.Y - a3.X * a2.Y);
double X = (d1 * (a2.Y - a3.Y) + d2 * (a3.Y - a1.Y) + d3 * (a1.Y - a2.Y)) / f;
double Y = (d1 * (a3.X - a2.X) + d2 * (a1.X - a3.X) + d3 * (a2.X - a1.X)) / f;
//_point = (new Point3d(X, Y, 0, CS)).ConvertTo(p1.Coord);
_point = new Point3d(X, Y, 0, CS);
_point = _point.ConvertTo(p1.Coord);
_r = Sqrt((X - a1.X) * (X - a1.X) + (Y - a1.Y) * (Y - a1.Y));
_normal = v1.Cross(v2);
}
internal override int _PointLocation(Point3d p)
{
Coord3d coord = new Coord3d(this.Center, this.V1, this.V2);
p = p.ConvertTo(coord);
if (GeometRi3D.UseAbsoluteTolerance)
{
if ((Abs(p.X) - L1 / 2) <= GeometRi3D.Tolerance && (Abs(p.Y) - L2 / 2) <= GeometRi3D.Tolerance && (Abs(p.Z) - L3 / 2) <= GeometRi3D.Tolerance)
{
if ((Abs(p.X) - L1 / 2) < -GeometRi3D.Tolerance && (Abs(p.Y) - L2 / 2) < -GeometRi3D.Tolerance && (Abs(p.Z) - L3 / 2) < -GeometRi3D.Tolerance)
{
return(1); // Point is strictly inside box
}
else
{
return(0); // Point is on boundary
}
}
else
{
return(-1); // Point is outside
}
}
else
{
double tol = GeometRi3D.Tolerance;
GeometRi3D.Tolerance = tol * this.Diagonal;
GeometRi3D.UseAbsoluteTolerance = true;
int result = this._PointLocation(p);
GeometRi3D.UseAbsoluteTolerance = false;
GeometRi3D.Tolerance = tol;
return(result);
}
}
public Point3d Rotate(Matrix3d m, Point3d p)
{
if (this._coord != p.Coord)
{
p = p.ConvertTo(this._coord);
}
return(m * (this - p) + p);
}
/// <summary>
/// Returns distance between two points
/// </summary>
public double DistanceTo(Point3d p)
{
if ((this._coord != p._coord))
{
p = p.ConvertTo(this._coord);
}
return(new Vector3d(this, p).Norm);
}
/// <summary>
/// Returns squared distance between two points
/// </summary>
public double DistanceSquared(Point3d p)
{
if ((this._coord != p._coord))
{
p = p.ConvertTo(this._coord);
}
return((this._x - p._x) * (this._x - p._x) + (this._y - p._y) * (this._y - p._y) + (this._z - p._z) * (this._z - p._z));
}
/// <summary>
/// Initializes vector object as radius vector of a point in reference coordinate system.
/// </summary>
public Vector3d(Point3d p, Coord3d coord = null)
{
p = p.ConvertTo(coord);
this.val = new double[3];
this.val[0] = p.X;
this.val[1] = p.Y;
this.val[2] = p.Z;
_coord = (coord == null) ? Coord3d.GlobalCS : coord;
}
/// <summary>
/// Reflect point in given point
/// </summary>
public Point3d ReflectIn(Point3d p)
{
if ((this._coord != p.Coord))
{
p = p.ConvertTo(this._coord);
}
Vector3d v = new Vector3d(this, p);
return(this.Translate(2 * v));
}
/// <summary>
/// Point on box (including interior points) closest to target point "p".
/// </summary>
public Point3d ClosestPoint(Point3d p)
{
Coord3d local_coord = this.LocalCoord();
p = p.ConvertTo(local_coord);
double x = GeometRi3D.Clamp(p.X, -_lx / 2, _lx / 2);
double y = GeometRi3D.Clamp(p.Y, -_ly / 2, _ly / 2);
double z = GeometRi3D.Clamp(p.Z, -_lz / 2, _lz / 2);
return(new Point3d(x, y, z, local_coord));
}
/// <summary>
/// Initializes vector object using two points in reference coordinate system of the first point.
/// </summary>
/// <param name="p1">Start point.</param>
/// <param name="p2">End point.</param>
public Vector3d(Point3d p1, Point3d p2)
{
if (p1.Coord != p2.Coord)
{
p2 = p2.ConvertTo(p1.Coord);
}
this.val = new double[3];
this.val[0] = p2.X - p1.X;
this.val[1] = p2.Y - p1.Y;
this.val[2] = p2.Z - p1.Z;
_coord = p1.Coord;
}
/// <summary>
/// Rotate point around point 'p' as a rotation center.
/// </summary>
public Point3d Rotate(Rotation r, Point3d p)
{
if (this._coord != r.Coord)
{
r = r.ConvertTo(this._coord);
}
if (this._coord != p.Coord)
{
p = p.ConvertTo(this._coord);
}
return(r.ToRotationMatrix * (this - p) + p);
}
public Point3d Add(Point3d p)
{
if ((this._coord != p._coord))
{
p = p.ConvertTo(this._coord);
}
Point3d tmp = this.Copy();
tmp.X += p.X;
tmp.Y += p.Y;
tmp.Z += p.Z;
return(tmp);
}
public Point3d Subtract(Point3d p)
{
if ((this._coord != p._coord))
{
p = p.ConvertTo(this._coord);
}
Point3d tmp = this.Copy();
tmp.X -= p.X;
tmp.Y -= p.Y;
tmp.Z -= p.Z;
return(tmp);
}
/// <summary>
/// Check if sphere is located inside box with tolerance defined by global tolerance property (GeometRi3D.Tolerance).
/// </summary>
public bool IsInside(Box3d box)
{
// Relative tolerance ================================
if (!GeometRi3D.UseAbsoluteTolerance)
{
double tol = GeometRi3D.Tolerance;
GeometRi3D.Tolerance = tol * this.R;
GeometRi3D.UseAbsoluteTolerance = true;
bool result = this.IsInside(box);
GeometRi3D.UseAbsoluteTolerance = false;
GeometRi3D.Tolerance = tol;
return(result);
}
//====================================================
if (!this._point.IsInside(box))
{
return(false);
}
Coord3d local_coord = box.LocalCoord();
Point3d p = _point.ConvertTo(local_coord);
if (box.L1 / 2 - (Abs(p.X) + this._r) < GeometRi3D.Tolerance)
{
return(false);
}
if (box.L2 / 2 - (Abs(p.Y) + this._r) < GeometRi3D.Tolerance)
{
return(false);
}
if (box.L3 / 2 - (Abs(p.Z) + this._r) < GeometRi3D.Tolerance)
{
return(false);
}
return(true);
}
/// <summary>
/// String representation of an object in reference coordinate system.
/// </summary>
public String ToString(Coord3d coord)
{
string nl = System.Environment.NewLine;
if (coord == null) { coord = Coord3d.GlobalCS; }
Point3d P = _point.ConvertTo(coord);
Vector3d normal = _normal.ConvertTo(coord);
string str = string.Format("Circle: ") + nl;
str += string.Format(" Center -> ({0,10:g5}, {1,10:g5}, {2,10:g5})", P.X, P.Y, P.Z) + nl;
str += string.Format(" Radius -> {0,10:g5}", _r) + nl;
str += string.Format(" Normal -> ({0,10:g5}, {1,10:g5}, {2,10:g5})", normal.X, normal.Y, normal.Z);
return str;
}
/// <summary>
/// String representation of an object in reference coordinate system.
/// </summary>
public String ToString(Coord3d coord)
{
string nl = System.Environment.NewLine;
if (coord == null)
{
coord = Coord3d.GlobalCS;
}
Point3d p = _point.ConvertTo(coord);
string str = string.Format("Sphere: ") + nl;
str += string.Format(" Center -> ({0,10:g5}, {1,10:g5}, {2,10:g5})", p.X, p.Y, p.Z) + nl;
str += string.Format(" Radius -> {0,10:g5}", _r);
return(str);
}
/// <summary>
/// String representation of an object in reference coordinate system.
/// </summary>
public String ToString(Coord3d coord)
{
System.Text.StringBuilder str = new System.Text.StringBuilder();
string nl = System.Environment.NewLine;
if (coord == null)
{
coord = Coord3d.GlobalCS;
}
Point3d P = _point.ConvertTo(coord);
Vector3d normal = _normal.ConvertTo(coord);
str.Append("Plane3d:" + nl);
str.Append(string.Format("Point -> ({0,10:g5}, {1,10:g5}, {2,10:g5})", P.X, P.Y, P.Z) + nl);
str.Append(string.Format("Normal -> ({0,10:g5}, {1,10:g5}, {2,10:g5})", normal.X, normal.Y, normal.Z));
return(str.ToString());
}
/// <summary>
/// String representation of an object in reference coordinate system.
/// </summary>
public String ToString(Coord3d coord)
{
System.Text.StringBuilder str = new System.Text.StringBuilder();
string nl = System.Environment.NewLine;
if (coord == null)
{
coord = Coord3d.GlobalCS;
}
Point3d P = _point.ConvertTo(coord);
Vector3d dir = _dir.ConvertTo(coord);
str.Append("Ray:" + nl);
str.Append(string.Format("Point -> ({0,10:g5}, {1,10:g5}, {2,10:g5})", P.X, P.Y, P.Z) + nl);
str.Append(string.Format("Direction -> ({0,10:g5}, {1,10:g5}, {2,10:g5})", dir.X, dir.Y, dir.Z));
return(str.ToString());
}
/// <summary>
/// String representation of an object in reference coordinate system.
/// </summary>
public String ToString(Coord3d coord)
{
System.Text.StringBuilder str = new System.Text.StringBuilder();
string nl = System.Environment.NewLine;
if (coord == null)
{
coord = Coord3d.GlobalCS;
}
Point3d p1 = _p1.ConvertTo(coord);
Point3d p2 = _p2.ConvertTo(coord);
str.Append("Segment:" + nl);
str.Append(string.Format("Point 1 -> ({0,10:g5}, {1,10:g5}, {2,10:g5})", p1.X, p1.Y, p1.Z) + nl);
str.Append(string.Format("Point 2 -> ({0,10:g5}, {1,10:g5}, {2,10:g5})", p2.X, p2.Y, p2.Z));
return(str.ToString());
}
/// <summary>
/// String representation of an object in reference coordinate system.
/// </summary>
public String ToString(Coord3d coord)
{
System.Text.StringBuilder str = new System.Text.StringBuilder();
string nl = System.Environment.NewLine;
if (coord == null)
{
coord = Coord3d.GlobalCS;
}
Point3d A = _a.ConvertTo(coord);
Point3d B = _b.ConvertTo(coord);
Point3d C = _c.ConvertTo(coord);
str.Append("Triangle:" + nl);
str.Append(string.Format("A -> ({0,10:g5}, {1,10:g5}, {2,10:g5})", A.X, A.Y, A.Z) + nl);
str.Append(string.Format("B -> ({0,10:g5}, {1,10:g5}, {2,10:g5})", B.X, B.Y, B.Z) + nl);
str.Append(string.Format("C -> ({0,10:g5}, {1,10:g5}, {2,10:g5})", C.X, C.Y, C.Z));
return(str.ToString());
}
/// <summary>
/// String representation of an object in reference coordinate system.
/// </summary>
public String ToString(Coord3d coord)
{
string nl = System.Environment.NewLine;
if (coord == null)
{
coord = Coord3d.GlobalCS;
}
Point3d P = _point.ConvertTo(coord);
Vector3d v1 = _v1.ConvertTo(coord);
Vector3d v2 = _v2.ConvertTo(coord);
string str = string.Format("Ellipse: ") + nl;
str += string.Format(" Center -> ({0,10:g5}, {1,10:g5}, {2,10:g5})", P.X, P.Y, P.Z) + nl;
str += string.Format(" Semiaxis A -> ({0,10:g5}, {1,10:g5}, {2,10:g5})", v1.X, v1.Y, v1.Z) + nl;
str += string.Format(" Semiaxis B -> ({0,10:g5}, {1,10:g5}, {2,10:g5})", v2.X, v2.Y, v2.Z) + nl;
return(str);
}
/// <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);
}
/// <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>
/// Initializes line segment using two points.
/// </summary>
public Segment3d(Point3d p1, Point3d p2)
{
_p1 = p1.Copy();
_p2 = p2.ConvertTo(p1.Coord);
}
