mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-23 01:21:42 +00:00
89 lines
2.9 KiB
C++
89 lines
2.9 KiB
C++
|
// 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);
|
||
|
|
}
|