/// <summary>
/// Determines whether or not this vector is parallel to <paramref name="other"/> within a certain <paramref name="angleTolerance"/>.
/// </summary>
public bool IsParallelTo(Vector3D other, float angleToleranceRadians)
{
angleToleranceRadians = Math.Abs(angleToleranceRadians);
float angle = GetAngleBetween(other);
return FloatComparer.AreEqual(angle, 0, angleToleranceRadians) ||
FloatComparer.AreEqual(angle, (float)Math.PI, angleToleranceRadians);
}
/// <summary>Used by <see cref="CountWindings(PointF,PointF*,int)"/>.</summary>
/// <remarks>
/// Algorithm as given on <a href="http://softsurfer.com/Archive/algorithm_0103/algorithm_0103.htm">http://softsurfer.com/Archive/algorithm_0103/algorithm_0103.htm</a>.
/// </remarks>
private static int IsLeft(PointF point0, PointF point1, PointF testPoint)
{
// this is a dot product of the vector test to p0, with a normal to the vector p0 to p1.
float result = (point1.X - point0.X) * (testPoint.Y - point0.Y) - (testPoint.X - point0.X) * (point1.Y - point0.Y);
return(FloatComparer.Compare(result, 0, 1));
}
internal static float[,] Invert3(float[,] m)
{
var b0 = (double)m[1, 1] * m[2, 2] - (double)m[1, 2] * m[2, 1];
var b1 = (double)m[1, 0] * m[2, 2] - (double)m[1, 2] * m[2, 0];
var b2 = (double)m[1, 0] * m[2, 1] - (double)m[1, 1] * m[2, 0];
var c0 = (double)m[0, 1] * m[2, 2] - (double)m[0, 2] * m[2, 1];
var s0 = (double)m[0, 1] * m[1, 2] - (double)m[0, 2] * m[1, 1];
var c1 = (double)m[0, 0] * m[2, 2] - (double)m[0, 2] * m[2, 0];
var s1 = (double)m[0, 0] * m[1, 2] - (double)m[0, 2] * m[1, 0];
var c2 = (double)m[0, 0] * m[2, 1] - (double)m[0, 1] * m[2, 0];
var s2 = (double)m[0, 0] * m[1, 1] - (double)m[0, 1] * m[1, 0];
var det = m[0, 0] * b0 - m[0, 1] * b1 + m[0, 2] * b2;
Platform.CheckFalse(FloatComparer.AreEqual(0, det), "Matrix is not invertible!");
var inv = new float[3, 3];
det = 1 / det;
inv[0, 0] = (float)(b0 * det);
inv[0, 1] = (float)(-c0 * det);
inv[0, 2] = (float)(s0 * det);
inv[1, 0] = (float)(-b1 * det);
inv[1, 1] = (float)(c1 * det);
inv[1, 2] = (float)(-s1 * det);
inv[2, 0] = (float)(b2 * det);
inv[2, 1] = (float)(-c2 * det);
inv[2, 2] = (float)(s2 * det);
return(inv);
}
public Matrix Invert()
{
Platform.CheckTrue(_rows == _columns, "Matrix must be square!");
switch (_rows)
{
case 1:
// A*inv(A) = I
// so for matrix A (n=1), with entry A[0,0] = k,
// A*inv(A) = A[0,0]*invA[0,0] = 1 => invA[0,0] = 1/A[0,0]
Platform.CheckFalse(FloatComparer.AreEqual(0, _matrix[0, 0]), "Matrix is not invertible!");
return(new Matrix(new[, ] {
{ 1 / _matrix[0, 0] }
}));
case 2:
return(new Matrix(Invert2(_matrix)));
case 3:
return(new Matrix(Invert3(_matrix)));
case 4:
return(new Matrix(Invert4(_matrix)));
default:
throw new NotImplementedException("Invert is not implemented for matrix size N > 4");
}
}
/// <summary>
/// Determines whether or not two lines are colinear.
/// </summary>
/// <remarks>
/// Colinearity here is defined as whether or not you can draw a single line through all given points.
/// This definition is used because it explicitly provides for classification of degenerate cases
/// where one or both "lines" are actually coincident points.
/// </remarks>
/// <param name="p1">One endpoint of one line.</param>
/// <param name="p2">The other endpoint of one line.</param>
/// <param name="q1">One endpoint of the other line.</param>
/// <param name="q2">The other endpoint of the other line.</param>
/// <returns>True if the lines are colinear; False otherwise.</returns>
public static bool AreColinear(PointF p1, PointF p2, PointF q1, PointF q2)
{
// colinearity test algorithm:
// 1. compute vectors from one endpoint to each of the other three endpoints
// 2. if all three vectors are trivial (i.e. zero) then all endpoints are coincident and thus "colinear"
// 3. otherwise they are colinear iff the non-trivial vector is (anti-)parallel to the other two vectors (as they have a common point)
// to test for parallel vectors while ignoring direction, compute the dot product between one vector and a perpendicular to the other vector.
// compute the vectors from P1 to each of P2, Q1 and Q2
var vector0 = p1 - new SizeF(p2);
var vector1 = p1 - new SizeF(q1);
var vector2 = p1 - new SizeF(q2);
// make sure we have the non-trivial vector in vector0
if (FloatComparer.AreEqual(PointF.Empty, vector0))
{
if (FloatComparer.AreEqual(PointF.Empty, vector1))
{
// if both P1P2 and P1Q1 are trivial, then we have at most two distinct points, through which you can always draw a line!
return(true);
}
else
{
// vector1 is non-trivial and vector0 is trivial, so swap them
var temp = vector0;
vector0 = vector1;
vector1 = temp;
}
}
// lines are colinear iff the other two vectors are parallel to the non-trivial vector
return(FloatComparer.AreEqual(vector0.Y * vector1.X - vector0.X * vector1.Y, 0) && FloatComparer.AreEqual(vector0.Y * vector2.X - vector0.X * vector2.Y, 0));
}
/// <summary>
/// Finds the intersection of the line segment defined by <paramref name="linePoint1"/> and
/// <paramref name="linePoint2"/> with a plane described by it's normal (<paramref name="planeNormal"/>)
/// and an arbitrary point in the plane (<paramref name="pointInPlane"/>).
/// </summary>
/// <param name="planeNormal">The normal vector of an arbitrary plane.</param>
/// <param name="pointInPlane">A point in space that lies on the plane whose normal is <paramref name="planeNormal"/>.</param>
/// <param name="linePoint1">The position vector of the start of the line.</param>
/// <param name="linePoint2">The position vector of the end of the line.</param>
/// <param name="isLineSegment">Specifies whether <paramref name="linePoint1"/> and <paramref name="linePoint2"/>
/// define a line segment, or simply 2 points on an infinite line.</param>
/// <returns>A position vector describing the point of intersection of the line with the plane, or null if the
/// line and plane do not intersect.</returns>
public static Vector3D GetLinePlaneIntersection(
Vector3D planeNormal,
Vector3D pointInPlane,
Vector3D linePoint1,
Vector3D linePoint2,
bool isLineSegment)
{
if (Vector3D.AreEqual(planeNormal, Vector3D.Null))
return null;
Vector3D line = linePoint2 - linePoint1;
Vector3D planeToLineStart = pointInPlane - linePoint1;
float lineDotPlaneNormal = planeNormal.Dot(line);
if (FloatComparer.AreEqual(0F, lineDotPlaneNormal))
return null;
float ratio = planeNormal.Dot(planeToLineStart) / lineDotPlaneNormal;
if (isLineSegment && (ratio < 0F || ratio > 1F))
return null;
return linePoint1 + ratio * line;
}
/// <summary>
/// Determines whether or not this vector is orthogonal to <paramref name="other"/> within a certain <paramref name="angleTolerance"/>.
/// </summary>
public bool IsOrthogonalTo(Vector3D other, float angleToleranceRadians)
{
angleToleranceRadians = Math.Abs(angleToleranceRadians);
float angle = GetAngleBetween(other);
const float halfPi = (float)Math.PI/2;
return FloatComparer.AreEqual(angle, halfPi, angleToleranceRadians);
}
//TODO (CR February 2011) - Low: Not used except in unit tests. Deprecate and recommend using the Vector3D class.
/// <summary>
/// Computes the unit vector of the vector defined by <paramref name="vector"/>.
/// </summary>
/// <param name="vector">The vector given as a point relative to the origin.</param>
/// <returns>The unit vector in the same direction as the given vector.</returns>
/// <exception cref="ArgumentException">If <paramref name="vector"/> is equal to the origin.</exception>
public static PointF CreateUnitVector(PointF vector)
{
if (FloatComparer.AreEqual(PointF.Empty, vector))
{
throw new ArgumentException("Argument must specify a valid vector.", "vector");
}
return(CreateUnitVector(PointF.Empty, vector));
}
/// <summary>
/// Gets whether or not <paramref name="left"/> is equal to <paramref name="right"/>, within a small tolerance (per vector component).
/// </summary>
public static bool AreEqual(Vector3D left, Vector3D right)
{
if (left == null || right == null)
return ReferenceEquals(left, right);
return FloatComparer.AreEqual(left.X, right.X) &&
FloatComparer.AreEqual(left.Y, right.Y) &&
FloatComparer.AreEqual(left.Z, right.Z);
}
/// <summary>
/// Gets whether or not <paramref name="left"/> is equal to <paramref name="right"/>, within a given tolerance (per vector component).
/// </summary>
public static bool AreEqual(Vector3D left, Vector3D right, float tolerance)
{
if (left == null || right == null)
{
return(ReferenceEquals(left, right));
}
return(FloatComparer.AreEqual(left.X, right.X, tolerance) &&
FloatComparer.AreEqual(left.Y, right.Y, tolerance) &&
FloatComparer.AreEqual(left.Z, right.Z, tolerance));
}
/// <summary>
/// Computes the unit vector of the vector defined by <paramref name="vector"/>.
/// </summary>
/// <param name="vector">The vector given as a point relative to the origin.</param>
/// <returns>The unit vector in the same direction as the given vector.</returns>
/// <exception cref="ArgumentException">If <paramref name="vector"/> is equal to the origin.</exception>
public static SizeF GetUnitVector(this SizeF vector)
{
if (FloatComparer.AreEqual(vector, SizeF.Empty))
{
const string msg = "Argument must specify a non-zero vector.";
throw new ArgumentException(msg, "vector");
}
double magnitude = Math.Sqrt(vector.Width * vector.Width + vector.Height * vector.Height);
return(new SizeF((float)(vector.Width / magnitude), (float)(vector.Height / magnitude)));
}
/// <summary>
/// Computes the unit vector of the vector defined by <paramref name="startingPoint"/> to <paramref name="endingPoint"/>.
/// </summary>
/// <param name="startingPoint">The starting point of the vector.</param>
/// <param name="endingPoint">The ending point of the vector.</param>
/// <returns>The unit vector in the same direction as the given vector.</returns>
/// <exception cref="ArgumentException">If <paramref name="startingPoint"/> is equal to <paramref name="endingPoint"/>.</exception>
public static PointF CreateUnitVector(PointF startingPoint, PointF endingPoint)
{
if (FloatComparer.AreEqual(startingPoint, endingPoint))
{
throw new ArgumentException("Arguments must specify a valid vector.", "endingPoint");
}
float deltaX = endingPoint.X - startingPoint.X;
float deltaY = endingPoint.Y - startingPoint.Y;
double magnitude = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
return(new PointF((float)(deltaX / magnitude), (float)(deltaY / magnitude)));
}
/// <summary>
/// Returns a value indicating whether the specified rectangle is normalized.
/// </summary>
/// <param name="rectangle"></param>
/// <returns></returns>
public static bool IsRectangleNormalized(RectangleF rectangle)
{
return(!(FloatComparer.IsLessThan(rectangle.Left, 0.0f) ||
FloatComparer.IsGreaterThan(rectangle.Left, 1.0f) ||
FloatComparer.IsLessThan(rectangle.Right, 0.0f) ||
FloatComparer.IsGreaterThan(rectangle.Right, 1.0f) ||
FloatComparer.IsLessThan(rectangle.Top, 0.0f) ||
FloatComparer.IsGreaterThan(rectangle.Top, 1.0f) ||
FloatComparer.IsLessThan(rectangle.Bottom, 0.0f) ||
FloatComparer.IsGreaterThan(rectangle.Bottom, 1.0f) ||
FloatComparer.IsGreaterThan(rectangle.Left, rectangle.Right) ||
FloatComparer.IsGreaterThan(rectangle.Top, rectangle.Bottom)));
}
/// <summary>
/// Computes the intersection between two line segments, if a solution exists.
/// </summary>
/// <param name="p1">One endpoint of one line segment.</param>
/// <param name="p2">The other endpoint of one line segment.</param>
/// <param name="q1">One endpoint of the other line segment.</param>
/// <param name="q2">The other endpoint of the other line segment.</param>
/// <param name="intersection">The intersection between the two line segments, if a solution exists.</param>
/// <returns>True if the intersection exists; False otherwise.</returns>
//TODO (CR February 2011) - High (SDK release): Name? GetLineSegmentIntersection : Nullable<Point>
public static bool IntersectLineSegments(PointF p1, PointF p2, PointF q1, PointF q2, out PointF intersection)
{
// find the solution to the line equations in matrix form
// P1 + s(P2-P1) = Q1 + t(Q2-Q1)
// => P1 + s(P2-P1) = Q1 - t(Q1-Q2)
// => [P2-P1 Q1-Q2] * [s t]^T = Q1-P1
// => [s t]^T = [P2-P1 Q1-Q2]^-1 * [Q1-P1]
// use double precision floating point variables to minimize precision loss
// compute elements of the matrix M
double m11 = p2.X - p1.X; // M[R1C1]
double m12 = q1.X - q2.X; // M[R1C2]
double m21 = p2.Y - p1.Y; // M[R2C1]
double m22 = q1.Y - q2.Y; // M[R2C2]
// compute determinant of the matrix M
double determinant = m11 * m22 - m12 * m21; // det(M)
if (!FloatComparer.AreEqual(determinant, 0))
{
// compute elements of the inverted matrix M^-1
double v11 = m22 / determinant;
double v12 = -m12 / determinant;
double v21 = -m21 / determinant;
double v22 = m11 / determinant;
// compute elements of the RHS vector
double r1 = q1.X - p1.X;
double r2 = q1.Y - p1.Y;
// left-multiply inverted matrix with RHS to get solution of {s,t}
double s = v11 * r1 + v12 * r2;
double t = v21 * r1 + v22 * r2;
//TODO (CR February 2011) - Medium (SDK release): tolerance seems arbitrary. Should add an overload that accepts the tolerance.
// the solution {s,t} represents the intersection of the lines
// for line segments, we must therefore further restrict the valid range of {s,t} to [0,1]
const int tolerance = 100000; // allow additional tolerance due to amount of floating-point computation
if (FloatComparer.Compare(s, 0, tolerance) >= 0 && FloatComparer.Compare(s, 1, tolerance) <= 0 &&
FloatComparer.Compare(t, 0, tolerance) >= 0 && FloatComparer.Compare(t, 1, tolerance) <= 0)
{
intersection = new PointF((float)(p1.X + s * m11), (float)(p1.Y + s * m21));
return(true);
}
}
intersection = PointF.Empty;
return(false);
}
/// <summary>
/// Computes the unit vector of the offset defined by <paramref name="ptStart"/> to <paramref name="ptEnd"/>.
/// </summary>
/// <param name="ptStart">The starting point of the vector.</param>
/// <param name="ptEnd">The ending point of the vector.</param>
/// <returns>The unit vector in the same direction as the given vector.</returns>
/// <exception cref="ArgumentException">If <paramref name="ptStart"/> is equal to <paramref name="ptEnd"/>.</exception>
public static SizeF GetUnitVector(this PointF ptStart, PointF ptEnd)
{
if (FloatComparer.AreEqual(ptStart, ptEnd))
{
const string msg = "Arguments must specify a valid vector.";
throw new ArgumentException(msg, "ptEnd");
}
float deltaX = ptEnd.X - ptStart.X;
float deltaY = ptEnd.Y - ptStart.Y;
return(GetUnitVector(new SizeF {
Width = deltaX, Height = deltaY
}));
}
/// <summary>
/// Gets a value indicating whether or not the elements of <paramref name="left"/> are equal to <paramref name="right"/> within the given tolerance.
/// </summary>
/// <exception cref="ArgumentException">If the matrices do not have the same dimensions.</exception>
public static bool AreEqual(Matrix3D left, Matrix3D right, float tolerance = 100)
{
for (int row = 0; row < 3; ++row)
{
for (int column = 0; column < 3; ++column)
{
if (!FloatComparer.AreEqual(left[row, column], right[row, column], tolerance))
{
return(false);
}
}
}
return(true);
}
/// <summary>
/// Computes the intersection between two lines, if a solution exists.
/// </summary>
/// <param name="p1">One endpoint of one line.</param>
/// <param name="p2">The other endpoint of one line.</param>
/// <param name="q1">One endpoint of the other line.</param>
/// <param name="q2">The other endpoint of the other line.</param>
/// <param name="intersection">The intersection between the two lines, if a solution exists.</param>
/// <returns>True if the intersection exists and is distinct; False otherwise.</returns>
//TODO (CR February 2011) - High (SDK release): Name? GetLineIntersection : Nullable<Point>
public static bool IntersectLines(PointF p1, PointF p2, PointF q1, PointF q2, out PointF intersection)
{
//TODO (CR February 2011) - Medium (SDK release): I remember talking about it, but looking at these 2 very similar
//methods, I think it makes sense to combine them.
// find the solution to the line equations in matrix form
// P1 + s(P2-P1) = Q1 + t(Q2-Q1)
// => P1 + s(P2-P1) = Q1 - t(Q1-Q2)
// => [P2-P1 Q1-Q2] * [s t]^T = Q1-P1
// => [s t]^T = [P2-P1 Q1-Q2]^-1 * [Q1-P1]
// use double precision floating point variables to minimize precision loss
// compute elements of the matrix M
double m11 = p2.X - p1.X; // M[R1C1]
double m12 = q1.X - q2.X; // M[R1C2]
double m21 = p2.Y - p1.Y; // M[R2C1]
double m22 = q1.Y - q2.Y; // M[R2C2]
// compute determinant of the matrix M
double determinant = m11 * m22 - m12 * m21; // det(M)
if (!FloatComparer.AreEqual(determinant, 0))
{
// compute elements of the inverted matrix M^-1
double v11 = m22 / determinant;
double v12 = -m12 / determinant;
// double v21 = -m21/determinant;
// double v22 = m11/determinant;
// compute elements of the RHS vector
double r1 = q1.X - p1.X;
double r2 = q1.Y - p1.Y;
// left-multiply inverted matrix with RHS to get solution of {s,t}
double s = v11 * r1 + v12 * r2;
// double t = v21*r1 + v22*r2;
// the solution {s,t} represents the intersection of the lines
intersection = new PointF((float)(p1.X + s * m11), (float)(p1.Y + s * m21));
return(true);
}
intersection = PointF.Empty;
return(false);
}
/// <summary>
/// Gets a value indicating whether or not the elements of <paramref name="left"/> are equal to <paramref name="right"/> within a small tolerance.
/// </summary>
/// <exception cref="ArgumentException">If the matrices do not have the same dimensions.</exception>
public static bool AreEqual(Matrix left, Matrix right)
{
Platform.CheckTrue(left.Columns == right.Columns && left.Rows == right.Rows, "Matrix Same Dimensions");
for (int row = 0; row < left.Rows; ++row)
{
for (int column = 0; column < left.Columns; ++column)
{
if (!FloatComparer.AreEqual(left[row, column], right[row, column]))
{
return(false);
}
}
}
return(true);
}
internal static float[,] Invert2(float[,] m)
{
var det = (double)m[0, 0] * m[1, 1] - (double)m[0, 1] * m[1, 0];
Platform.CheckFalse(FloatComparer.AreEqual(0, det), "Matrix is not invertible!");
var inv = new float[2, 2];
det = 1 / det;
inv[0, 0] = (float)(m[1, 1] * det);
inv[0, 1] = (float)(-m[0, 1] * det);
inv[1, 0] = (float)(-m[1, 0] * det);
inv[1, 1] = (float)(m[0, 0] * det);
return(inv);
}
internal static float[,] Invert4(float[,] m)
{
var s0 = (double)m[0, 0] * m[1, 1] - (double)m[1, 0] * m[0, 1];
var s1 = (double)m[0, 0] * m[1, 2] - (double)m[1, 0] * m[0, 2];
var s2 = (double)m[0, 0] * m[1, 3] - (double)m[1, 0] * m[0, 3];
var s3 = (double)m[0, 1] * m[1, 2] - (double)m[1, 1] * m[0, 2];
var s4 = (double)m[0, 1] * m[1, 3] - (double)m[1, 1] * m[0, 3];
var s5 = (double)m[0, 2] * m[1, 3] - (double)m[1, 2] * m[0, 3];
var c5 = (double)m[2, 2] * m[3, 3] - (double)m[3, 2] * m[2, 3];
var c4 = (double)m[2, 1] * m[3, 3] - (double)m[3, 1] * m[2, 3];
var c3 = (double)m[2, 1] * m[3, 2] - (double)m[3, 1] * m[2, 2];
var c2 = (double)m[2, 0] * m[3, 3] - (double)m[3, 0] * m[2, 3];
var c1 = (double)m[2, 0] * m[3, 2] - (double)m[3, 0] * m[2, 2];
var c0 = (double)m[2, 0] * m[3, 1] - (double)m[3, 0] * m[2, 1];
var det = s0 * c5 - s1 * c4 + s2 * c3 + s3 * c2 - s4 * c1 + s5 * c0;
Platform.CheckFalse(FloatComparer.AreEqual(0, det), "Matrix is not invertible!");
var inv = new float[4, 4];
det = 1 / det;
inv[0, 0] = (float)((m[1, 1] * c5 - m[1, 2] * c4 + m[1, 3] * c3) * det);
inv[0, 1] = (float)((-m[0, 1] * c5 + m[0, 2] * c4 - m[0, 3] * c3) * det);
inv[0, 2] = (float)((m[3, 1] * s5 - m[3, 2] * s4 + m[3, 3] * s3) * det);
inv[0, 3] = (float)((-m[2, 1] * s5 + m[2, 2] * s4 - m[2, 3] * s3) * det);
inv[1, 0] = (float)((-m[1, 0] * c5 + m[1, 2] * c2 - m[1, 3] * c1) * det);
inv[1, 1] = (float)((m[0, 0] * c5 - m[0, 2] * c2 + m[0, 3] * c1) * det);
inv[1, 2] = (float)((-m[3, 0] * s5 + m[3, 2] * s2 - m[3, 3] * s1) * det);
inv[1, 3] = (float)((m[2, 0] * s5 - m[2, 2] * s2 + m[2, 3] * s1) * det);
inv[2, 0] = (float)((m[1, 0] * c4 - m[1, 1] * c2 + m[1, 3] * c0) * det);
inv[2, 1] = (float)((-m[0, 0] * c4 + m[0, 1] * c2 - m[0, 3] * c0) * det);
inv[2, 2] = (float)((m[3, 0] * s4 - m[3, 1] * s2 + m[3, 3] * s0) * det);
inv[2, 3] = (float)((-m[2, 0] * s4 + m[2, 1] * s2 - m[2, 3] * s0) * det);
inv[3, 0] = (float)((-m[1, 0] * c3 + m[1, 1] * c1 - m[1, 2] * c0) * det);
inv[3, 1] = (float)((m[0, 0] * c3 - m[0, 1] * c1 + m[0, 2] * c0) * det);
inv[3, 2] = (float)((-m[3, 0] * s3 + m[3, 1] * s1 - m[3, 2] * s0) * det);
inv[3, 3] = (float)((m[2, 0] * s3 - m[2, 1] * s1 + m[2, 2] * s0) * det);
return(inv);
}
/// <summary>
/// Calculates the shortest distance from a point to a line segment.
/// </summary>
/// <param name="ptTest">Point to be tested.</param>
/// <param name="pt1">One end of the line segment.</param>
/// <param name="pt2">The other end of the line segement.</param>
/// <param name="ptNearest">Returns the point on the line segment nearest to the <paramref name="ptTest">test point</paramref>.</param>
/// <returns>The distance from the <paramref name="ptTest">test point</paramref> to the <paramref name="ptNearest">nearest point</paramref> on the line segment.</returns>
public static double DistanceFromPointToLine(PointF ptTest, PointF pt1, PointF pt2, ref PointF ptNearest)
{
// Point on line segment nearest to pt0
float dx = pt2.X - pt1.X;
float dy = pt2.Y - pt1.Y;
// It's a point, not a line
if (FloatComparer.AreEqual(dx, 0) && FloatComparer.AreEqual(dy, 0))
{
ptNearest.X = pt1.X;
ptNearest.Y = pt1.Y;
}
else
{
// Parameter
double t = ((ptTest.X - pt1.X) * dx + (ptTest.Y - pt1.Y) * dy) / (double)(dx * dx + dy * dy);
// Nearest point is pt1
if (t < 0)
{
ptNearest = pt1;
}
// Nearest point is pt2
else if (t > 1)
{
ptNearest = pt2;
}
// Nearest point is on the line segment
else
{
// Parametric equation
ptNearest.X = (float)(pt1.X + t * dx);
ptNearest.Y = (float)(pt1.Y + t * dy);
}
}
float distanceX = ptTest.X - ptNearest.X;
float distanceY = ptTest.Y - ptNearest.Y;
double distance = Math.Sqrt(distanceX * distanceX + distanceY * distanceY);
return(distance);
}
/// <summary>
/// Determines whether or not two lines are parallel.
/// </summary>
/// <param name="p1">One endpoint of one line.</param>
/// <param name="p2">The other endpoint of one line.</param>
/// <param name="q1">One endpoint of the other line.</param>
/// <param name="q2">The other endpoint of the other line.</param>
/// <returns>True if the lines are parallel; False otherwise.</returns>
public static bool AreParallel(PointF p1, PointF p2, PointF q1, PointF q2)
{
// find the solution to the line equations in matrix form
// P1 + s(P2-P1) = Q1 + t(Q2-Q1)
// => P1 + s(P2-P1) = Q1 - t(Q1-Q2)
// => [P2-P1 Q1-Q2] * [s t]^T = Q1-P1
// => [s t]^T = [P2-P1 Q1-Q2]^-1 * [Q1-P1]
// use double precision floating point variables to minimize precision loss
// compute elements of the matrix M
double m11 = p2.X - p1.X; // M[R1C1]
double m12 = q1.X - q2.X; // M[R1C2]
double m21 = p2.Y - p1.Y; // M[R2C1]
double m22 = q1.Y - q2.Y; // M[R2C2]
// compute determinant of the matrix M
double determinant = m11 * m22 - m12 * m21; // det(M)
// if a distinct solution does not exist then the lines must be parallel
return(FloatComparer.AreEqual(determinant, 0));
}
/// <summary>
/// Calculates the angle subtended by two line segments that meet at a vertex.
/// </summary>
/// <param name="start">The end of one of the line segments.</param>
/// <param name="vertex">The vertex of the angle formed by the two line segments.</param>
/// <param name="end">The end of the other line segment.</param>
/// <returns>The angle subtended by the two line segments in degrees.</returns>
public static double SubtendedAngle(PointF start, PointF vertex, PointF end)
{
Vector3D vertexPositionVector = new Vector3D(vertex.X, vertex.Y, 0);
Vector3D a = new Vector3D(start.X, start.Y, 0) - vertexPositionVector;
Vector3D b = new Vector3D(end.X, end.Y, 0) - vertexPositionVector;
float dotProduct = a.Dot(b);
Vector3D crossProduct = a.Cross(b);
float magA = a.Magnitude;
float magB = b.Magnitude;
if (FloatComparer.AreEqual(magA, 0F) || FloatComparer.AreEqual(magB, 0F))
{
return(0);
}
double cosTheta = dotProduct / magA / magB;
// Make sure cosTheta is within bounds so we don't
// get any errors when we take the acos.
if (cosTheta > 1.0f)
{
cosTheta = 1.0f;
}
if (cosTheta < -1.0f)
{
cosTheta = -1.0f;
}
double theta = Math.Acos(cosTheta) * (crossProduct.Z == 0 ? 1 : -Math.Sign(crossProduct.Z));
double thetaInDegrees = theta / Math.PI * 180;
return(thetaInDegrees);
}