/// <summary>
/// Transforms a vector and returns the transformed vector
/// </summary>
/// <param name="v"></param>
/// <returns></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]));
}
/// <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));
}
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 static Ray3D ParseRay3D(string s)
{
var match = Regex.Match(s, PlanePointVectorPattern);
var p = Point3D.Parse(match.Groups["p"].Value);
var uv = UnitVector3D.Parse(match.Groups["v"].Value);
return(new Ray3D(p, uv));
}
/// <summary>
/// Creates a coordinate system that rotates
/// </summary>
/// <param name="angle">Angle to rotate</param>
/// <param name="v">Vector to rotate about</param>
/// <returns></returns>
public static CoordinateSystem Rotation(Angle angle, UnitVector3D v)
{
var m = Matrix <double> .Build.Dense(4, 4);
m.SetSubMatrix(0, 3, 0, 3, Matrix3D.RotationAroundArbitraryVector(v, angle));
m[3, 3] = 1;
return(new CoordinateSystem(m));
}
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>
/// 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>
public static CoordinateSystem RotateTo(UnitVector3D fromVector3D, UnitVector3D toVector3D, UnitVector3D?axis = null)
{
Matrix <double> r = Matrix3D.RotationTo(fromVector3D, toVector3D, axis);
var coordinateSystem = new CoordinateSystem();
CoordinateSystem cs = SetRotationSubMatrix(r, coordinateSystem);
return(cs);
}
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 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 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));
}
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);
}
public static Plane ParsePlane(string s)
{
var match = Regex.Match(s, PlanePointVectorPattern);
if (match.Success)
{
var p = Point3D.Parse(match.Groups["p"].Value);
var uv = UnitVector3D.Parse(match.Groups["v"].Value);
return(new Plane(p, uv));
}
match = Regex.Match(s, PlaneAbcdPattern);
{
var a = ParseDouble(match.Groups["a"]);
var b = ParseDouble(match.Groups["b"]);
var c = ParseDouble(match.Groups["c"]);
var d = ParseDouble(match.Groups["d"]);
return(new Plane(a, b, c, d));
}
}
/// <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 bool IsPerpendicularTo(UnitVector3D othervector, double tolerance = 1e-6)
{
var @this = this.Normalize();
return(Math.Abs(@this.DotProduct(othervector)) < tolerance);
}
public Circle3D(Point3D centerPoint, UnitVector3D axis, double radius)
{
this.CenterPoint = centerPoint;
this.Axis = axis;
this.Radius = radius;
}
/// <summary>
/// Parses string representation of throughpoint and direction
/// This is mainly meant for tests
/// </summary>
/// <param name="point"></param>
/// <param name="direction"></param>
/// <returns></returns>
public static Ray3D Parse(string point, string direction)
{
return(new Ray3D(Point3D.Parse(point), UnitVector3D.Parse(direction)));
}
public Ray3D(Point3D throughPoint, UnitVector3D direction)
{
this.ThroughPoint = throughPoint;
this.Direction = direction;
}
/// <summary>
/// Creates a coordinate system that rotates
/// </summary>
/// <param name="a">Angle to rotate</param>
/// <param name="unit">The unit of the angle</param>
/// <param name="v">Vector to rotate about</param>
/// <returns></returns>
public static CoordinateSystem Rotation <T>(double a, T unit, UnitVector3D v) where T : IAngleUnit
{
return(Rotation(Angle.From(a, unit), v));
}
public CoordinateSystem(Point3D origin, UnitVector3D xAxis, UnitVector3D yAxis, UnitVector3D zAxis)
: this(origin, xAxis.ToVector3D(), yAxis.ToVector3D(), zAxis.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>
/// The nearest angle between the vectors
/// </summary>
/// <param name="v">The other vector</param>
/// <returns>The angle</returns>
public Angle AngleTo(UnitVector3D v)
{
return(this.Normalize().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 CoordinateSystem OffsetBy(UnitVector3D v)
{
return(new CoordinateSystem(this.Origin + v, this.XAxis, this.YAxis, this.ZAxis));
}
public bool Equals(UnitVector3D other)
{
// ReSharper disable CompareOfFloatsByEqualityOperator
return(this.X == other.X && this.Y == other.Y && this.Z == other.Z);
// ReSharper restore CompareOfFloatsByEqualityOperator
}
public Vector3D ProjectOn(UnitVector3D uv)
{
double pd = DotProduct(uv);
return(pd * this);
}
/// <summary>
///
/// </summary>
/// <param name="aboutVector"></param>
/// <param name="angle">Angle in degrees</param>
/// <param name="angleUnit"></param>
/// <returns></returns>
public static Matrix <double> RotationAroundArbitraryVector <T>(UnitVector3D aboutVector, double angle, T angleUnit) where T : IAngleUnit
{
return(RotationAroundArbitraryVector(aboutVector, Angle.From(angle, angleUnit)));
}
public double DotProduct(UnitVector3D v)
{
return((this.X * v.X) + (this.Y * v.Y) + (this.Z * v.Z));
}
