/// <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 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>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));
}
/// <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));
}
/// <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));
}
//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>
/// 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>
/// 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;
}
/// <summary>
/// Gets a value indicating whether or not the elements of <paramref name="left"/> are equal to <paramref name="right"/> within the given absolute 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)
{
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>
/// 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)
{
//TODO (CR February 2011) - Medium (SDK release): same determinant computed in 3 places; should make it a method (ComputeDeterminant?)
// 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);
}
