From 2bc4c31ed13e6d1d04cdbeffaac2ca0a01633662 Mon Sep 17 00:00:00 2001 From: braykoff <99614905+Braykoff@users.noreply.github.com> Date: Mon, 29 Apr 2024 18:12:58 -0400 Subject: [PATCH] Shortened names, added tolerances. --- .../wpi/first/math/geometry/Ellipse2d.java | 6 ++--- .../wpi/first/math/geometry/Rectangle2d.java | 16 +++++++------- .../first/math/geometry/Ellipse2dTest.java | 7 +++--- .../first/math/geometry/Rectangle2dTest.java | 22 +++++++++---------- 4 files changed, 25 insertions(+), 26 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java index 14c28d1e2b9..c90ec077429 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java @@ -142,8 +142,8 @@ private double solveEllipseEquation(Translation2d point) { * @param point The point to check. * @return True, if this ellipse's circumference intersects the point. */ - public boolean intersectsPoint(Translation2d point) { - return solveEllipseEquation(point) == 1.0; + public boolean intersects(Translation2d point) { + return Math.abs(1.0 - solveEllipseEquation(point)) <= 1E-9; } /** @@ -153,7 +153,7 @@ public boolean intersectsPoint(Translation2d point) { * @param point The point to check. * @return True, if the point is within or on the ellipse. */ - public boolean containsPoint(Translation2d point) { + public boolean contains(Translation2d point) { return solveEllipseEquation(point) <= 1.0; } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java index f5acdadeee1..617764f6d6d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java @@ -127,15 +127,15 @@ public Rectangle2d rotateBy(Rotation2d other) { * @param point The point to check. * @return True, if the rectangle's perimeter intersects the point. */ - public boolean intersectsPoint(Translation2d point) { + public boolean intersects(Translation2d point) { // Move the point into the rectangle's coordinate frame point = point.minus(m_center.getTranslation()); point = point.rotateBy(m_center.getRotation().unaryMinus()); - if (Math.abs(point.getX()) == m_xWidth / 2.0) { + if (Math.abs(Math.abs(point.getX()) - m_xWidth / 2.0) <= 1E-9) { // Point rests on left/right perimeter return Math.abs(point.getY()) <= m_yWidth / 2.0; - } else if (Math.abs(point.getY()) == m_yWidth / 2.0) { + } else if (Math.abs(Math.abs(point.getY()) - m_yWidth / 2.0) <= 1E-9) { // Point rests on top/bottom perimeter return Math.abs(point.getX()) <= m_xWidth / 2.0; } @@ -150,7 +150,7 @@ public boolean intersectsPoint(Translation2d point) { * @param point The point to check. * @return True, if the rectangle contains the point or the perimeter intersects the point. */ - public boolean containsPoint(Translation2d point) { + public boolean contains(Translation2d point) { // Rotate the point into the rectangle's coordinate frame point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus()); @@ -167,8 +167,8 @@ public boolean containsPoint(Translation2d point) { * @param point The point to check. * @return The distance (0, if the point is contained by the rectangle) */ - public double distanceToPoint(Translation2d point) { - if (containsPoint(point)) { + public double distance(Translation2d point) { + if (contains(point)) { return 0.0; } @@ -192,8 +192,8 @@ public double distanceToPoint(Translation2d point) { */ public Translation2d findNearestPoint(Translation2d point) { // Check if already in rectangle - if (containsPoint(point)) { - return new Translation2d(point.getX(), point.getY()); + if (contains(point)) { + return point; } // Rotate the point by the inverse of the rectangle's rotation diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java index 6e3422976e7..16a2def2ebb 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java @@ -35,8 +35,8 @@ void testIntersectsPoint() { var pointB = new Translation2d(0.0, 3.0); assertAll( - () -> assertTrue(ellipse.intersectsPoint(pointA)), - () -> assertFalse(ellipse.intersectsPoint(pointB))); + () -> assertTrue(ellipse.intersects(pointA)), + () -> assertFalse(ellipse.intersects(pointB))); } @Test @@ -48,8 +48,7 @@ void testContainsPoint() { var pointB = new Translation2d(0.5, -2.0); assertAll( - () -> assertTrue(ellipse.containsPoint(pointA)), - () -> assertFalse(ellipse.containsPoint(pointB))); + () -> assertTrue(ellipse.contains(pointA)), () -> assertFalse(ellipse.contains(pointB))); } @Test diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java index a578872f9a8..5a4e389854d 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java @@ -35,10 +35,10 @@ void testIntersectsPoint() { var rect = new Rectangle2d(center, 2.0, 3.0); assertAll( - () -> assertTrue(rect.intersectsPoint(new Translation2d(5.5, 4.0))), - () -> assertTrue(rect.containsPoint(new Translation2d(3.0, 2.0))), - () -> assertFalse(rect.containsPoint(new Translation2d(4.0, 1.5))), - () -> assertFalse(rect.intersectsPoint(new Translation2d(4.0, 3.5)))); + () -> assertTrue(rect.intersects(new Translation2d(5.5, 4.0))), + () -> assertTrue(rect.intersects(new Translation2d(3.0, 2.0))), + () -> assertFalse(rect.intersects(new Translation2d(4.0, 1.5))), + () -> assertFalse(rect.intersects(new Translation2d(4.0, 3.5)))); } @Test @@ -47,9 +47,9 @@ void testContainsPoint() { var rect = new Rectangle2d(center, 3.0, 1.0); assertAll( - () -> assertTrue(rect.containsPoint(new Translation2d(2.0, 3.0))), - () -> assertTrue(rect.containsPoint(new Translation2d(3.0, 4.0))), - () -> assertFalse(rect.containsPoint(new Translation2d(3.0, 3.0)))); + () -> assertTrue(rect.contains(new Translation2d(2.0, 3.0))), + () -> assertTrue(rect.contains(new Translation2d(3.0, 4.0))), + () -> assertFalse(rect.contains(new Translation2d(3.0, 3.0)))); } @Test @@ -63,10 +63,10 @@ void testDistanceToPoint() { var point4 = new Translation2d(-1.0, 2.5); assertAll( - () -> assertEquals(0.5, rect.distanceToPoint(point1), kEpsilon), - () -> assertEquals(0.0, rect.distanceToPoint(point2), kEpsilon), - () -> assertEquals(0.5, rect.distanceToPoint(point3), kEpsilon), - () -> assertEquals(1.0, rect.distanceToPoint(point4), kEpsilon)); + () -> assertEquals(0.5, rect.distance(point1), kEpsilon), + () -> assertEquals(0.0, rect.distance(point2), kEpsilon), + () -> assertEquals(0.5, rect.distance(point3), kEpsilon), + () -> assertEquals(1.0, rect.distance(point4), kEpsilon)); } @Test