Merge pull request #413 from GSharker/dev/mibi/angle-vector

Angle vector

src/GShark.Test.XUnit/Core/TrigonometryTests.cs

@@ -46,6 +46,61 @@ public void It_Returns_False_If_Three_Points_Are_Not_Collinear()
         Trigonometry.ArePointsCollinear(pt1, pt2, pt3).Should().BeFalse();
     }
 
+    [Fact]
+    public void Returns_A_List_Of_Points_Without_Collinear()
+    {
+        // Arrange
+        Point3[] pts =
+        {
+            new Point3(0.0, 0.0, 0.0),
+            new Point3(1.343973, 0.318025, 1.148505),
+            new Point3(2.392638, 0.56617, 2.044653),
+            new Point3(3.423607, 0.810129, 2.683595)
+        };
+
+        Point3[] expectedPts =
+        {
+            new Point3(0.0, 0.0, 0.0),
+            new Point3(2.392638, 0.56617, 2.044653),
+            new Point3(3.423607, 0.810129, 2.683595)
+        };
+
+
+        // Act
+        var result = Trigonometry.RemoveCollinear(pts);
+
+        // Assert
+        result.Should().BeEquivalentTo(expectedPts);
+    }
+
+    [Fact]
+    public void It_Returns_True_If_Points_Are_Clockwise()
+    {
+        // Arrange
+        Point3 pt1 = new Point3(10.0, 0.0, 0.0);
+        Point3 pt2 = new Point3(5.0, 0.0, 0.0);
+        Point3 pt3 = new Point3(5.0, 5.0, 0.0);
+        Point3 pt4 = new Point3(7.0, 7.0, 0.0);
+        List<Point3> points = new List<Point3> { pt1, pt2, pt3, pt4 };
+
+        // Arrange
+        Trigonometry.ArePointsClockwise(points).Should().BeTrue();
+    }
+
+    [Fact]
+    public void It_Returns_True_If_Points_Are_Clockwise_On_A_Plane()
+    {
+        // Arrange
+        Point3 pt1 = new Point3(10.0, 0.0, -5.0);
+        Point3 pt2 = new Point3(5.0, 0.0, 3.0);
+        Point3 pt3 = new Point3(5.0, 5.0, 10.0);
+        Point3 pt4 = new Point3(7.0, 7.0, -2.0);
+        List<Point3> points = new List<Point3> { pt1, pt2, pt3, pt4 };
+
+        // Arrange
+        Trigonometry.ArePointsClockwise(points, Plane.PlaneXY).Should().BeTrue();
+    }
+
     [Theory]
     [InlineData(new double[] {5, 7, 0}, new double[] {6, 6, 0}, 0.2)]
     [InlineData(new double[] {7, 6, 0}, new[] {6.5, 6.5, 0}, 0.3)]

src/GShark.Test.XUnit/Geometry/Vector3Tests.cs

@@ -33,17 +33,32 @@ public void It_Returns_The_Radian_Angle_Between_Two_Vectors()
         public void It_Returns_The_Radian_Angle_Between_Two_Vectors_On_A_Plane()
         {
             // Arrange
-            Vector3 v1 = new Vector3(1, 0, 0);
-            Vector3 v2 = new Vector3(3, 0, 4);
-            var expectedAngle = System.Math.Atan2(4, 3);
+            Vector3 v1 = new Vector3(54.534634, 70.106922, 28.165069);
+            Vector3 v2 = new Vector3(-65.601466, 59.878665, -45.937199);
+
+            const double expectedAngleWithPlaneXY = 1.492079;
+            const double expectedAngleReflectionWithPlaneXY = 4.791106;
+
+            const double expectedAngleWithPlaneYZ = 5.246775;
+            const double expectedAngleReflectionWithPlaneYZ = 1.03641;
+
+            const double expectedAngleWithPlaneZX = 3.007432;
+            const double expectedAngleReflectionWithPlaneZX = 3.275754;
 
             // Act
-            double reflexAngle;
-            double angle = Vector3.VectorAngleOnPlane(v1, v2, Plane.PlaneZX, out reflexAngle);
+            double anglePlaneXY = Vector3.VectorAngleOnPlane(v1, v2, Plane.PlaneXY, out var reflexAngleWithPlaneXY);
+            double anglePlaneYZ = Vector3.VectorAngleOnPlane(v1, v2, Plane.PlaneYZ, out var reflexAngleWithPlaneYZ);
+            double anglePlaneZX = Vector3.VectorAngleOnPlane(v1, v2, Plane.PlaneZX, out var reflexAngleWithPlaneZX);
 
             // Assert
-            angle.Should().BeApproximately(expectedAngle, GSharkMath.Epsilon);
-            reflexAngle.Should().BeApproximately(2 * Math.PI - expectedAngle, GSharkMath.Epsilon);
+            anglePlaneXY.Should().BeApproximately(expectedAngleWithPlaneXY, GSharkMath.MinTolerance);
+            reflexAngleWithPlaneXY.Should().BeApproximately(expectedAngleReflectionWithPlaneXY, GSharkMath.MinTolerance);
+
+            anglePlaneYZ.Should().BeApproximately(expectedAngleWithPlaneYZ, GSharkMath.MinTolerance);
+            reflexAngleWithPlaneYZ.Should().BeApproximately(expectedAngleReflectionWithPlaneYZ, GSharkMath.MinTolerance);
+
+            anglePlaneZX.Should().BeApproximately(expectedAngleWithPlaneZX, GSharkMath.MinTolerance);
+            reflexAngleWithPlaneZX.Should().BeApproximately(expectedAngleReflectionWithPlaneZX, GSharkMath.MinTolerance);
         }
 
         [Fact]

src/GShark/Core/Trigonometry.cs

@@ -1,5 +1,6 @@
 using System;
 using System.Collections.Generic;
+using System.Linq;
 using GShark.Geometry;
 
 namespace GShark.Core
@@ -53,6 +54,34 @@ public static bool ArePointsCollinear(Point3 pt1, Point3 pt2, Point3 pt3, double
             return area < tol;
         }
 
+        /// <summary>
+        ///     Remove the collinear points into a ordered collection of points.
+        /// </summary>
+        /// <param name="pts">Collection of point to clean up.</param>
+        /// <param name="tol">Tolerance from the straight line between collinear points. Set by default at 1e-3.</param>
+        /// <returns>Cleaned collection of points.</returns>
+        public static IEnumerable<Point3> RemoveCollinear(IEnumerable<Point3> pts, double tol = 1e-3)
+        {
+            Point3[] ptsCollection = pts.ToArray();
+
+            List<Point3> result = new List<Point3> { ptsCollection[0] };
+
+            for (int i = 0; i < ptsCollection.Length - 2; i++)
+            {
+                if (ArePointsCollinear(ptsCollection[i], ptsCollection[i + 1], ptsCollection[i + 2], tol))
+                    continue;
+                result.Add(ptsCollection[i + 1]);
+            }
+
+            if (ptsCollection.Last() != ptsCollection.First())
+                if (ArePointsCollinear(result[result.Count - 1], ptsCollection[ptsCollection.Length - 1], result[0],
+                        tol))
+                    return result;
+
+            result.Add(ptsCollection.Last());
+            return result;
+        }
+
         /// <summary>
         ///     Calculates the point at the equal distance from the three points, it can be also described as the center of a
         ///     circle.
@@ -99,6 +128,41 @@ public static int PointOrder(Point3 pt1, Point3 pt2, Point3 pt3)
             return result < 0 ? 1 : 2;
         }
 
+        /// <summary>
+        ///     Determines if the points direction is clockwise in its plane.
+        ///     It'll even work with a self-intersecting polygon like a figure-eight, telling you whether it's mostly clockwise.
+        ///     https://stackoverflow.com/questions/1165647/how-to-determine-if-a-list-of-polygon-points-are-in-clockwise-order.
+        /// </summary>
+        /// <param name="points">Collection of points.</param>
+        /// <returns>True if clockwise.</returns>
+        public static bool ArePointsClockwise(IList<Point3> points)
+        {
+            double sum = 0;
+            for (int i = 0; i < points.Count - 1; i++)
+            {
+                sum += (points[i + 1].X - points[i].X) * (points[i + 1].Y + points[i].Y);
+            }
+
+            if (sum == 0)
+            {
+                throw new ArgumentException("The points are on a straight line or are creating a closed loop.");
+            }
+
+            return sum > 0;
+        }
+
+        /// <summary>
+        ///     Determines if the points direction is clockwise on a passed plane.
+        /// </summary>
+        /// <param name="points">Collection of points that make up the polygon.</param>
+        /// <param name="pl">The</param>
+        /// <returns>True if clockwise.</returns>
+        public static bool ArePointsClockwise(IList<Point3> points, Plane pl)
+        {
+            var projectedPts = points.Select(pt => pl.ClosestPoint(pt, out _)).ToList();
+            return ArePointsClockwise(projectedPts);
+        }
+
         /// <summary>
         ///     Finds the closest point on a segment.<br />
         ///     It is not a segment like a line or ray but the part that compose the segment.<br />

src/GShark/Geometry/Point3.cs

@@ -632,7 +632,7 @@ public bool IsOnPlane(Plane plane, double tolerance = GSharkMath.MaxTolerance)
         /// </summary>
         /// <param name="line">The line to test against.</param>
         /// <param name="tolerance">Default is use 1e-6</param>
-        /// <returns>Returns true if point is on plane.</returns>
+        /// <returns>Returns true if point is on line.</returns>
         public bool IsOnLine(Line line, double tolerance = GSharkMath.MaxTolerance)
         {
             return line.ClosestPoint(this).DistanceTo(this) < tolerance;

src/GShark/Geometry/Vector3.cs

@@ -282,14 +282,15 @@ public static double VectorAngleOnPlane(Vector3 a, Vector3 b, Plane plane, out d
                 return Math.PI;
             }
 
-            if (angle > Math.PI)
+            Vector3 other = Vector3.CrossProduct(a, b);
+            if (plane.ZAxis.IsParallelTo(other) == 1)
             {
-                reflexAngle = angle;
-                return 2 * Math.PI - angle;
+                reflexAngle = 2 * Math.PI - angle;
+                return angle;
             }
 
-            reflexAngle = 2 * Math.PI - angle;
-            return angle;
+            reflexAngle = angle;
+            return 2 * Math.PI - angle;
         }
 
         /// <summary>