Esempio n. 1
0
		/// <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);
		}
Esempio n. 2
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));
        }
Esempio n. 3
0
        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);
        }
Esempio n. 4
0
        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");
            }
        }
Esempio n. 5
0
        /// <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 &quot;lines&quot; 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));
        }
Esempio n. 6
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;
		}
Esempio n. 7
0
		/// <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);
		}
Esempio n. 8
0
 //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));
 }
Esempio n. 9
0
		/// <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);
		}
Esempio n. 10
0
        /// <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));
        }
Esempio n. 11
0
        /// <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)));
        }
Esempio n. 12
0
        /// <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)));
        }
Esempio n. 13
0
 /// <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)));
 }
Esempio n. 14
0
        /// <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);
        }
Esempio n. 15
0
        /// <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
            }));
        }
Esempio n. 16
0
        /// <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);
        }
Esempio n. 17
0
        /// <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);
        }
Esempio n. 18
0
        /// <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);
        }
Esempio n. 19
0
        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);
        }
Esempio n. 20
0
        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);
        }
Esempio n. 21
0
        /// <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);
        }
Esempio n. 22
0
        /// <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));
        }
Esempio n. 23
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);
        }