/// <summary>
/// Initializes plane using three points.
/// </summary>
/// <param name="p1">First point.</param>
/// <param name="p2">Second point.</param>
/// <param name="p3">Third point.</param>
public Plane3d(Point3d p1, Point3d p2, Point3d p3)
{
Vector3d v1 = new Vector3d(p1, p2);
Vector3d v2 = new Vector3d(p1, p3);
_normal = v1.Cross(v2);
_point = p1.Copy();
}
/// <summary>
/// Initializes box with specified dimensions and orientation defined by rotation object.
/// </summary>
/// <param name="center">Center point of the box.</param>
/// <param name="lx">First dimension.</param>
/// <param name="ly">Second dimension.</param>
/// <param name="lz">Third dimension.</param>
/// <param name="r">Orientation of the box, defined as rotation from axis aligned position (in global CS) to final position.</param>
public Box3d(Point3d center, double lx, double ly, double lz, Rotation r)
{
_center = center.Copy();
_lx = lx;
_ly = ly;
_lz = lz;
_r = r.Copy();
}
/// <summary>
/// Initializes axis aligned box in local coordinate system.
/// </summary>
/// <param name="center">Center point of the box.</param>
/// <param name="lx">First dimension.</param>
/// <param name="ly">Second dimension.</param>
/// <param name="lz">Third dimension.</param>
/// <param name="coord">Local coordinate system.</param>
public Box3d(Point3d center, double lx, double ly, double lz, Coord3d coord)
{
_center = center.Copy();
_lx = lx;
_ly = ly;
_lz = lz;
_r = new Rotation(coord);
}
/// <summary>
/// Initializes axis aligned box in local coordinate system.
/// </summary>
/// <param name="center">Center point of the box.</param>
/// <param name="lx">First dimension.</param>
/// <param name="ly">Second dimension.</param>
/// <param name="lz">Third dimension.</param>
/// <param name="coord">Local coordinate system.</param>
public Box3d(Point3d center, double lx, double ly, double lz, Coord3d coord)
{
_center = center.Copy();
_lx = lx;
_ly = ly;
_lz = lz;
_r = new Rotation(coord);
_local_coord = new Coord3d(_center, _r.ConvertToGlobal().ToRotationMatrix.Transpose());
}
/// <summary>
/// Initializes triangle object using three points.
/// </summary>
public Triangle(Point3d A, Point3d B, Point3d C)
{
if (Point3d.CollinearPoints(A, B, C))
{
throw new Exception("Collinear points");
}
_a = A.Copy();
_b = B.Copy();
_c = C.Copy();
}
/// <summary>
/// Initializes ellipsoid instance using center point and three orthogonal vectors.
/// </summary>
/// <param name="Center">Center point.</param>
/// <param name="v1">First semiaxis.</param>
/// <param name="v2">Second semiaxis.</param>
/// <param name="v3">Third semiaxis.</param>
public Ellipsoid(Point3d Center, Vector3d v1, Vector3d v2, Vector3d v3)
{
if (!(v1.IsOrthogonalTo(v2) && v1.IsOrthogonalTo(v3) && v3.IsOrthogonalTo(v2)))
{
throw new Exception("Semiaxes are not orthogonal");
}
_point = Center.Copy();
if (v1.Norm >= v2.Norm && v1.Norm >= v3.Norm)
{
_v1 = v1.Copy();
if (v2.Norm >= v3.Norm)
{
_v2 = v2.Copy();
_v3 = v3.Copy();
}
else
{
_v2 = v3.Copy();
_v3 = v2.Copy();
}
}
else if (v2.Norm >= v1.Norm && v2.Norm >= v3.Norm)
{
_v1 = v2.Copy();
if (v1.Norm >= v3.Norm)
{
_v2 = v1.Copy();
_v3 = v3.Copy();
}
else
{
_v2 = v3.Copy();
_v3 = v1.Copy();
}
}
else
{
_v1 = v3.Copy();
if (v1.Norm >= v2.Norm)
{
_v2 = v1.Copy();
_v3 = v2.Copy();
}
else
{
_v2 = v2.Copy();
_v3 = v1.Copy();
}
}
}
/// <summary>
/// Initializes ellipse instance using center point and two orthogonal vectors.
/// </summary>
/// <param name="Center">Center point.</param>
/// <param name="v1">First semiaxis.</param>
/// <param name="v2">Second semiaxis.</param>
public Ellipse(Point3d Center, Vector3d v1, Vector3d v2)
{
if (!v1.IsOrthogonalTo(v2))
{
throw new Exception("Semiaxes are not orthogonal");
}
_point = Center.Copy();
if (v1.Norm >= v2.Norm)
{
_v1 = v1.Copy();
_v2 = v2.Copy();
}
else
{
_v1 = v2.Copy();
_v2 = v1.Copy();
}
}
/// <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 circle instance using center point, radius and normal vector.
/// </summary>
public Circle3d(Point3d Center, double Radius, Vector3d Normal)
{
_point = Center.Copy();
_r = Radius;
_normal = Normal.Copy();
}
/// <summary>
/// Creates copy of the object
/// </summary>
public Ellipsoid Copy()
{
return(new Ellipsoid(_point.Copy(), _v1.Copy(), _v2.Copy(), _v3.Copy()));
}
/// <summary>
/// Initializes line segment using two points.
/// </summary>
public Segment3d(Point3d p1, Point3d p2)
{
_p1 = p1.Copy();
_p2 = p2.ConvertTo(p1.Coord);
}
/// <summary>
/// Initializes plane using point and normal vector.
/// </summary>
/// <param name="p1"></param>
/// <param name="v1"></param>
public Plane3d(Point3d p1, Vector3d v1)
{
_normal = v1.Copy();
_point = p1.Copy();
}
/// <summary>
/// Initializes ray using starting point and direction.
/// </summary>
public Ray3d(Point3d p, Vector3d v)
{
_point = p.Copy();
_dir = v.ConvertTo(p.Coord).Normalized;
}
/// <summary>
/// Initializes sphere using center point and radius.
/// </summary>
public Sphere(Point3d P, double R)
{
_point = P.Copy();
_r = R;
}
/// <summary>
/// Creates copy of the object
/// </summary>
public Ellipse Copy()
{
return(new Ellipse(_point.Copy(), _v1.Copy(), _v2.Copy()));
}
/// <summary>
/// Initializes line using point and direction.
/// </summary>
/// <param name="p">Point on the line.</param>
/// <param name="v">Direction vector.</param>
public Line3d(Point3d p, Vector3d v)
{
_point = p.Copy();
_dir = v.Copy();
}
/// <summary>
/// Initializes line using two points.
/// </summary>
/// <param name="p1">First point.</param>
/// <param name="p2">Second point.</param>
public Line3d(Point3d p1, Point3d p2)
{
_point = p1.Copy();
_dir = new Vector3d(p1, p2);
}
