/// <summary>
/// Create a circle from three points which lie along its circumference.
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <param name="p3"></param>
public Circle3D(Point3D p1, Point3D p2, Point3D p3)
{
https: //www.physicsforums.com/threads/equation-of-a-circle-through-3-points-in-3d-space.173847/
Vector3D p1p2 = p2 - p1;
Vector3D p2p3 = p3 - p2;
this.Axis = p1p2.CrossProduct(p2p3).Normalize();
Point3D midPointA = p1 + 0.5 * p1p2;
Point3D midPointB = p2 + 0.5 * p2p3;
Vector3D directionA = p1p2.CrossProduct(this.Axis);
Vector3D directionB = p2p3.CrossProduct(this.Axis);
Ray3D bisectorA = new Ray3D(midPointA, directionA);
Plane bisectorB = new Plane(midPointB, midPointB + directionB.Normalize(), midPointB + this.Axis);
var center = bisectorA.IntersectionWith(bisectorB);
if (center == null)
{
throw new ArgumentException("A circle cannot be created from these points, are they colinear?");
}
this.CenterPoint = (Point3D)center;
this.Radius = this.CenterPoint.DistanceTo(p1);
}
/// <summary>
/// Finds the intersection of the two planes, throws if they are parallel
/// http://mathworld.wolfram.com/Plane-PlaneIntersection.html
/// </summary>
/// <param name="intersectingPlane"></param>
/// <param name="tolerance"></param>
/// <returns></returns>
public Ray3D IntersectionWith(Plane intersectingPlane, double tolerance = float.Epsilon)
{
var a = new DenseMatrix(2, 3);
a.SetRow(0, this.Normal.ToVector());
a.SetRow(1, intersectingPlane.Normal.ToVector());
var svd = a.Svd(true);
if (svd.S[1] < tolerance)
{
throw new ArgumentException("Planes are parallel");
}
var y = new DenseMatrix(2, 1);
y[0, 0] = -1 * this.D;
y[1, 0] = -1 * intersectingPlane.D;
Matrix <double> pointOnIntersectionLine = svd.Solve(y);
var throughPoint = new Point3D(pointOnIntersectionLine.Column(0));
var direction = new UnitVector3D(svd.VT.Row(2));
return(new Ray3D(throughPoint, direction));
}
/// <summary>
/// Transforms a vector and returns the transformed vector
/// </summary>
/// <param name="v">a unit vector</param>
/// <returns>A transformed vector</returns>
public Vector3D Transform(UnitVector3D v)
{
var v3 = Vector <double> .Build.Dense(new[] { v.X, v.Y, v.Z });
this.GetRotationSubMatrix().Multiply(v3, v3);
return(new Vector3D(v3[0], v3[1], v3[2]));
}
public bool IsParallelTo(UnitVector3D othervector, double tolerance = 1e-6)
{
var @this = this.Normalize();
var dp = Math.Abs(@this.DotProduct(othervector));
return(Math.Abs(1 - dp) < tolerance);
}
public Plane Rotate(UnitVector3D aboutVector, Angle angle)
{
var rootPoint = this.RootPoint;
var rotatedPoint = rootPoint.Rotate(aboutVector, angle);
var rotatedPlaneVector = this.Normal.Rotate(aboutVector, angle);
return(new Plane(rotatedPlaneVector, rotatedPoint));
}
public void ReadXml(XmlReader reader)
{
reader.MoveToContent();
var e = (XElement)XNode.ReadFrom(reader);
XmlExt.SetReadonlyField(ref this, l => l.ThroughPoint, Point3D.ReadFrom(e.SingleElement("ThroughPoint").CreateReader()));
XmlExt.SetReadonlyField(ref this, l => l.Direction, UnitVector3D.ReadFrom(e.SingleElement("Direction").CreateReader()));
}
/// <summary>
/// Creates a coordinate system that rotates
/// </summary>
/// <param name="angle">Angle to rotate</param>
/// <param name="v">Vector to rotate about</param>
/// <returns>A rotating coordinate system</returns>
public static CoordinateSystem Rotation(Angle angle, UnitVector3D v)
{
var m = Build.Dense(4, 4);
m.SetSubMatrix(0, 3, 0, 3, Matrix3D.RotationAroundArbitraryVector(v, angle));
m[3, 3] = 1;
return(new CoordinateSystem(m));
}
/// <summary>
/// Sets to the matrix of rotation that aligns the 'from' vector with the 'to' vector.
/// The optional Axis argument may be used when the two vectors are perpendicular and in opposite directions to specify a specific solution, but is otherwise ignored.
/// </summary>
/// <param name="fromVector3D">Input Vector object to align from.</param>
/// <param name="toVector3D">Input Vector object to align to.</param>
/// <param name="axis">Input Vector object. </param>
/// <returns>A rotated coordinate system </returns>
public static CoordinateSystem RotateTo(UnitVector3D fromVector3D, UnitVector3D toVector3D, UnitVector3D?axis = null)
{
var r = Matrix3D.RotationTo(fromVector3D, toVector3D, axis);
var coordinateSystem = new CoordinateSystem();
var cs = SetRotationSubMatrix(r, coordinateSystem);
return(cs);
}
public Vector3D CrossProduct(UnitVector3D inVector3D)
{
var x = (this.Y*inVector3D.Z) - (this.Z*inVector3D.Y);
var y = (this.Z*inVector3D.X) - (this.X*inVector3D.Z);
var z = (this.X*inVector3D.Y) - (this.Y*inVector3D.X);
var v = new Vector3D(x, y, z);
return v;
}
public Vector3D CrossProduct(UnitVector3D other)
{
var x = (this.Y * other.Z) - (this.Z * other.Y);
var y = (this.Z * other.X) - (this.X * other.Z);
var z = (this.X * other.Y) - (this.Y * other.X);
var v = new Vector3D(x, y, z);
return(v);
}
public void ReadXml(XmlReader reader)
{
reader.MoveToContent();
var e = (XElement)XNode.ReadFrom(reader);
XmlExt.SetReadonlyField(ref this, l => l.RootPoint, Point3D.ReadFrom(e.SingleElement("RootPoint").CreateReader()));
XmlExt.SetReadonlyField(ref this, l => l.Normal, UnitVector3D.ReadFrom(e.SingleElement("Normal").CreateReader()));
XmlExt.SetReadonlyField(ref this, l => l.D, -this.RootPoint.ToVector3D().DotProduct(this.Normal));
}
/// <inheritdoc/>
void IXmlSerializable.ReadXml(XmlReader reader)
{
reader.MoveToContent();
var e = (XElement)XNode.ReadFrom(reader);
this = new Ray3D(
Point3D.ReadFrom(e.SingleElement("ThroughPoint").CreateReader()),
UnitVector3D.ReadFrom(e.SingleElement("Direction").CreateReader()));
}
public bool Equals(UnitVector3D other, double tolerance)
{
if (tolerance < 0)
{
throw new ArgumentException("epsilon < 0");
}
return Math.Abs(other.X - this.X) < tolerance &&
Math.Abs(other.Y - this.Y) < tolerance &&
Math.Abs(other.Z - this.Z) < tolerance;
}
/// <summary>
/// Throws if StartPoint == EndPoint
/// </summary>
/// <param name="startPoint"></param>
/// <param name="endPoint"></param>
public Line3D(Point3D startPoint, Point3D endPoint)
{
this.StartPoint = startPoint;
this.EndPoint = endPoint;
if (this.StartPoint == this.EndPoint)
{
throw new ArgumentException("StartPoint == EndPoint");
}
this._length = -1.0;
this._direction = new UnitVector3D();
}
public static Matrix <double> RotationAroundArbitraryVector(UnitVector3D aboutVector, Angle angle)
{
// http://en.wikipedia.org/wiki/Rotation_matrix
var unitTensorProduct = aboutVector.GetUnitTensorProduct();
var crossproductMatrix = aboutVector.CrossProductMatrix; // aboutVector.Clone().CrossProduct(aboutVector.Clone());
var r1 = DenseMatrix.CreateIdentity(3).Multiply(Math.Cos(angle.Radians));
var r2 = crossproductMatrix.Multiply(Math.Sin(angle.Radians));
var r3 = unitTensorProduct.Multiply(1 - Math.Cos(angle.Radians));
var totalR = r1.Add(r2).Add(r3);
return(totalR);
}
/// <summary>
/// http://www.had2know.com/academics/equation-plane-through-3-points.html
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <param name="p3"></param>
public Plane(Point3D p1, Point3D p2, Point3D p3)
{
if (p1 == p2 || p1 == p3 || p2 == p3)
{
throw new ArgumentException("Must use three different points");
}
Vector3D v1 = new Vector3D(p2.X - p1.X, p2.Y - p1.Y, p2.Z - p1.Z);
Vector3D v2 = new Vector3D(p3.X - p1.X, p3.Y - p1.Y, p3.Z - p1.Z);
Vector3D cross = v1.CrossProduct(v2);
if (cross.Length <= float.Epsilon)
{
throw new ArgumentException("The 3 points should not be on the same line");
}
this.RootPoint = p1;
this.Normal = cross.Normalize();
this.D = -this.RootPoint.ToVector3D().DotProduct(this.Normal);
}
/// <summary>
/// Sets to the matrix of rotation that would align the 'from' vector with the 'to' vector.
/// The optional Axis argument may be used when the two vectors are perpendicular and in opposite directions to specify a specific solution, but is otherwise ignored.
/// </summary>
/// <param name="fromVector">Input Vector object to align from.</param>
/// <param name="toVector">Input Vector object to align to.</param>
/// <param name="axis">Input Vector object. </param>
public static Matrix <double> RotationTo(UnitVector3D fromVector, UnitVector3D toVector, UnitVector3D?axis = null)
{
if (fromVector == toVector)
{
return(DenseMatrix.CreateIdentity(3));
}
if (fromVector.IsParallelTo(toVector))
{
if (axis == null)
{
axis = fromVector.Orthogonal;
}
}
else
{
axis = fromVector.CrossProduct(toVector);
}
var signedAngleTo = fromVector.SignedAngleTo(toVector, axis.Value);
return(RotationAroundArbitraryVector(axis.Value, signedAngleTo));
}
public Point3D Rotate(UnitVector3D aboutVector, Angle angle)
{
var cs = CoordinateSystem.Rotation(angle, aboutVector);
return(cs.Transform(this));
}
/// <summary>
/// Initializes a new instance of the <see cref="Plane"/> struct.
/// Constructs a Plane from the given normal and distance along the normal from the origin.
/// </summary>
/// <param name="normal">The Plane's normal vector.</param>
/// <param name="rootPoint">A point in the plane.</param>
public Plane(Point3D rootPoint, UnitVector3D normal)
: this(normal, normal.DotProduct(rootPoint))
{
}
/// <summary>
/// Initializes a new instance of the <see cref="Plane"/> struct.
/// Constructs a Plane from the given normal and distance along the normal from the origin.
/// </summary>
/// <param name="normal">The Plane's normal vector.</param>
/// <param name="offset">The Plane's distance from the origin along its normal vector.</param>
public Plane(UnitVector3D normal, double offset = 0)
{
this.Normal = normal;
this.D = -offset;
}
/// <summary>
/// Initializes a new instance of the <see cref="Plane"/> struct.
/// Constructs a Plane from the X, Y, and Z components of its normal, and its distance from the origin on that normal.
/// </summary>
/// <param name="x">The X-component of the normal.</param>
/// <param name="y">The Y-component of the normal.</param>
/// <param name="z">The Z-component of the normal.</param>
/// <param name="d">The distance of the Plane along its normal from the origin.</param>
public Plane(double x, double y, double z, double d)
: this(UnitVector3D.Create(x, y, z), -d)
{
}
public Ray3D Project(UnitVector3D vector3DToProject)
{
return(this.Project(vector3DToProject.ToVector3D()));
}
/// <summary>
/// Returns a vector that is this vector rotated the signed angle around the about vector
/// </summary>
/// <param name="about"></param>
/// <param name="angle"></param>
/// <returns></returns>
public Vector3D Rotate(UnitVector3D about, Angle angle)
{
var cs = CoordinateSystem.Rotation(angle, about);
return cs.Transform(this);
}
/// <summary>
/// Returns a vector that is this vector rotated the signed angle around the about vector
/// </summary>
/// <typeparam name="T">Constraining it like this does not box</typeparam>
/// <param name="about"></param>
/// <param name="angle"></param>
/// <param name="angleUnit"></param>
/// <returns></returns>
public Vector3D Rotate<T>(UnitVector3D about, double angle, T angleUnit)
where T : IAngleUnit
{
return Rotate(about, Angle.From(angle, angleUnit));
}
/// <summary>
/// Compute the angle between this vector and a unit vector using the arccosine of the dot product.
/// </summary>
/// <param name="v">The other vector</param>
/// <returns>The angle between the vectors, with a range between 0° and 180°</returns>
public Angle AngleTo(UnitVector3D v)
{
var uv = this.Normalize();
return uv.AngleTo(v);
}
/// <summary>
/// Returns signed angle
/// </summary>
/// <param name="v">The vector to calculate the signed angle to </param>
/// <param name="about">The vector around which to rotate to get the correct sign</param>
public Angle SignedAngleTo(UnitVector3D v, UnitVector3D about)
{
return this.Normalize().SignedAngleTo(v, about);
}
public double DotProduct(UnitVector3D v)
{
return (this.X*v.X) + (this.Y*v.Y) + (this.Z*v.Z);
}
/// <summary>
/// Computes whether or not this vector is perpendicular to another vector using the dot product method and
/// comparing it to within a specified tolerance
/// </summary>
/// <param name="othervector"></param>
/// <param name="tolerance"></param>
/// <returns>true if the vector dot product is within the given tolerance of zero, false if not</returns>
public bool IsPerpendicularTo(UnitVector3D othervector, double tolerance = 1e-6)
{
var @this = this.Normalize();
return Math.Abs(@this.DotProduct(othervector)) < tolerance;
}
/// <summary>
/// Determine whether or not this vector is parallel to a unit vector within a given angle tolerance.
/// </summary>
/// <param name="othervector"></param>
/// <param name="angleTolerance"></param>
/// <returns>true if the vectors are parallel within the angle tolerance, false if they are not</returns>
public bool IsParallelTo(UnitVector3D othervector, Angle angleTolerance)
{
var @this = this.Normalize();
return @this.IsParallelTo(othervector, angleTolerance);
}
/// <summary>
/// Computes whether or not this vector is parallel to a unit vector using the dot product method and comparing it
/// to within a specified tolerance.
/// </summary>
/// <param name="othervector"></param>
/// <param name="tolerance">A tolerance value for the dot product method. Values below 2*Precision.DoublePrecision may cause issues.</param>
/// <returns>true if the vector dot product is within the given tolerance of unity, false if not</returns>
public bool IsParallelTo(UnitVector3D othervector, double tolerance = 1e-10)
{
var @this = this.Normalize();
return @this.IsParallelTo(othervector, tolerance);
}
