Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add geometry classes for Rectangle2d and Ellipse2d (#6555)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Brendan Raykoff
2024-06-04 21:27:32 -04:00
committed by GitHub
parent afaf7e2c3f
commit 8def7b2222
41 changed files with 3042 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

88
wpimath/src/test/native/cpp/geometry/Rectangle2dTest.cpp Normal file
View File

@@ -0,0 +1,88 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <gtest/gtest.h>
#include "frc/geometry/Rectangle2d.h"
TEST(Rectangle2dTest, NewWithCorners) {
constexpr frc::Translation2d cornerA{1_m, 2_m};
constexpr frc::Translation2d cornerB{4_m, 6_m};
frc::Rectangle2d rect{cornerA, cornerB};
EXPECT_EQ(3.0, rect.XWidth().value());
EXPECT_EQ(4.0, rect.YWidth().value());
EXPECT_EQ(2.5, rect.Center().X().value());
EXPECT_EQ(4.0, rect.Center().Y().value());
}
TEST(Rectangle2dTest, IntersectsPoint) {
constexpr frc::Pose2d center{4_m, 3_m, 90_deg};
constexpr frc::Rectangle2d rect{center, 2_m, 3_m};
EXPECT_TRUE(rect.Intersects(frc::Translation2d{5.5_m, 4_m}));
EXPECT_TRUE(rect.Intersects(frc::Translation2d{3_m, 2_m}));
EXPECT_FALSE(rect.Intersects(frc::Translation2d{4_m, 1.5_m}));
EXPECT_FALSE(rect.Intersects(frc::Translation2d{4_m, 3.5_m}));
}
TEST(Rectangle2dTest, ContainsPoint) {
constexpr frc::Pose2d center{2_m, 3_m, 45_deg};
constexpr frc::Rectangle2d rect{center, 3_m, 1_m};
EXPECT_TRUE(rect.Contains(frc::Translation2d{2_m, 3_m}));
EXPECT_TRUE(rect.Contains(frc::Translation2d{3_m, 4_m}));
EXPECT_FALSE(rect.Contains(frc::Translation2d{3_m, 3_m}));
}
TEST(Rectangle2dTest, DistanceToPoint) {
constexpr double kEpsilon = 1E-9;
constexpr frc::Pose2d center{1_m, 2_m, 270_deg};
constexpr frc::Rectangle2d rect{center, 1_m, 2_m};
constexpr frc::Translation2d point1{2.5_m, 2_m};
EXPECT_NEAR(0.5, rect.Distance(point1).value(), kEpsilon);
constexpr frc::Translation2d point2{1_m, 2_m};
EXPECT_NEAR(0, rect.Distance(point2).value(), kEpsilon);
constexpr frc::Translation2d point3{1_m, 1_m};
EXPECT_NEAR(0.5, rect.Distance(point3).value(), kEpsilon);
constexpr frc::Translation2d point4{-1_m, 2.5_m};
EXPECT_NEAR(1, rect.Distance(point4).value(), kEpsilon);
}
TEST(Rectangle2dTest, FindNearestPoint) {
constexpr double kEpsilon = 1E-9;
constexpr frc::Pose2d center{1_m, 1_m, 90_deg};
constexpr frc::Rectangle2d rect{center, 3_m, 4_m};
constexpr frc::Translation2d point1{1_m, 3_m};
auto nearestPoint1 = rect.FindNearestPoint(point1);
EXPECT_NEAR(1.0, nearestPoint1.X().value(), kEpsilon);
EXPECT_NEAR(2.5, nearestPoint1.Y().value(), kEpsilon);
constexpr frc::Translation2d point2{0_m, 0_m};
auto nearestPoint2 = rect.FindNearestPoint(point2);
EXPECT_NEAR(0.0, nearestPoint2.X().value(), kEpsilon);
EXPECT_NEAR(0.0, nearestPoint2.Y().value(), kEpsilon);
}
TEST(Rectangle2dTest, Equals) {
constexpr frc::Pose2d center1{2_m, 3_m, 0_deg};
constexpr frc::Rectangle2d rect1{center1, 5_m, 3_m};
constexpr frc::Pose2d center2{2_m, 3_m, 0_deg};
constexpr frc::Rectangle2d rect2{center2, 5_m, 3_m};
constexpr frc::Pose2d center3{2_m, 3_m, 0_deg};
constexpr frc::Rectangle2d rect3{center3, 3_m, 3_m};
EXPECT_EQ(rect1, rect2);
EXPECT_NE(rect2, rect3);
}