Skip to content

Commit

Permalink
Merge pull request #413 from GSharker/dev/mibi/angle-vector
Browse files Browse the repository at this point in the history
Angle vector
  • Loading branch information
sonomirco authored May 4, 2023
2 parents 9e6056f + 3d0c8ab commit 14848cf
Show file tree
Hide file tree
Showing 5 changed files with 148 additions and 13 deletions.
55 changes: 55 additions & 0 deletions src/GShark.Test.XUnit/Core/TrigonometryTests.cs
Original file line number Diff line number Diff line change
Expand Up @@ -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)]
Expand Down
29 changes: 22 additions & 7 deletions src/GShark.Test.XUnit/Geometry/Vector3Tests.cs
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down
64 changes: 64 additions & 0 deletions src/GShark/Core/Trigonometry.cs
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
using System;
using System.Collections.Generic;
using System.Linq;
using GShark.Geometry;

namespace GShark.Core
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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 />
Expand Down
2 changes: 1 addition & 1 deletion src/GShark/Geometry/Point3.cs
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
11 changes: 6 additions & 5 deletions src/GShark/Geometry/Vector3.cs
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down

0 comments on commit 14848cf

Please sign in to comment.