mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-26 01:51:41 +00:00
[wpimath] Add geometry classes for Rectangle2d and Ellipse2d (#6555)
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Brendan Raykoff
210
wpimath/src/main/native/include/frc/geometry/Rectangle2d.h
Normal file
210
wpimath/src/main/native/include/frc/geometry/Rectangle2d.h
Normal file
@@ -0,0 +1,210 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <algorithm>
|
||||
#include <stdexcept>
|
||||
|
||||
#include <wpi/SymbolExports.h>
|
||||
|
||||
#include "frc/geometry/Pose2d.h"
|
||||
#include "frc/geometry/Rotation2d.h"
|
||||
#include "frc/geometry/Transform2d.h"
|
||||
#include "frc/geometry/Translation2d.h"
|
||||
#include "units/length.h"
|
||||
#include "units/math.h"
|
||||
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
* Represents a 2d rectangular space containing translational, rotational, and
|
||||
* scaling components.
|
||||
*/
|
||||
class WPILIB_DLLEXPORT Rectangle2d {
|
||||
public:
|
||||
/**
|
||||
* Constructs a rectangle at the specified position with the specified width
|
||||
* and height.
|
||||
*
|
||||
* @param center The position (translation and rotation) of the rectangle.
|
||||
* @param xWidth The x size component of the rectangle, in unrotated
|
||||
* coordinate frame.
|
||||
* @param yWidth The y size component of the rectangle, in unrotated
|
||||
* coordinate frame.
|
||||
*/
|
||||
constexpr Rectangle2d(const Pose2d& center, units::meter_t xWidth,
|
||||
units::meter_t yWidth)
|
||||
: m_center{center}, m_xWidth{xWidth}, m_yWidth{yWidth} {
|
||||
if (xWidth < 0_m || yWidth < 0_m) {
|
||||
throw std::invalid_argument(
|
||||
"Rectangle2d dimensions cannot be less than 0!");
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates an unrotated rectangle from the given corners. The corners should
|
||||
* be diagonally opposite of each other.
|
||||
*
|
||||
* @param cornerA The first corner of the rectangle.
|
||||
* @param cornerB The second corner of the rectangle.
|
||||
*/
|
||||
constexpr Rectangle2d(const Translation2d& cornerA,
|
||||
const Translation2d& cornerB)
|
||||
: m_center{(cornerA + cornerB) / 2.0, Rotation2d{}},
|
||||
m_xWidth{units::math::abs(cornerA.X() - cornerB.X())},
|
||||
m_yWidth{units::math::abs(cornerA.Y() - cornerB.Y())} {}
|
||||
|
||||
/**
|
||||
* Returns the center of the rectangle.
|
||||
*
|
||||
* @return The center of the rectangle.
|
||||
*/
|
||||
constexpr const Pose2d& Center() const { return m_center; }
|
||||
|
||||
/**
|
||||
* Returns the rotational component of the rectangle.
|
||||
*
|
||||
* @return The rotational component of the rectangle.
|
||||
*/
|
||||
constexpr const Rotation2d& Rotation() const { return m_center.Rotation(); }
|
||||
|
||||
/**
|
||||
* Returns the x size component of the rectangle.
|
||||
*
|
||||
* @return The x size component of the rectangle.
|
||||
*/
|
||||
constexpr units::meter_t XWidth() const { return m_xWidth; }
|
||||
|
||||
/**
|
||||
* Returns the y size component of the rectangle.
|
||||
*
|
||||
* @return The y size component of the rectangle.
|
||||
*/
|
||||
constexpr units::meter_t YWidth() const { return m_yWidth; }
|
||||
|
||||
/**
|
||||
* Transforms the center of the rectangle and returns the new rectangle.
|
||||
*
|
||||
* @param other The transform to transform by.
|
||||
* @return The transformed rectangle
|
||||
*/
|
||||
constexpr Rectangle2d TransformBy(const Transform2d& other) const {
|
||||
return Rectangle2d{m_center.TransformBy(other), m_xWidth, m_yWidth};
|
||||
}
|
||||
|
||||
/**
|
||||
* Rotates the center of the rectangle and returns the new rectangle.
|
||||
*
|
||||
* @param other The rotation to transform by.
|
||||
* @return The rotated rectangle.
|
||||
*/
|
||||
constexpr Rectangle2d RotateBy(const Rotation2d& other) const {
|
||||
return Rectangle2d{m_center.RotateBy(other), m_xWidth, m_yWidth};
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks if a point is intersected by the rectangle's perimeter.
|
||||
*
|
||||
* @param point The point to check.
|
||||
* @return True, if the rectangle's perimeter intersects the point.
|
||||
*/
|
||||
constexpr bool Intersects(const Translation2d& point) const {
|
||||
// Move the point into the rectangle's coordinate frame
|
||||
auto pointInRect = point - m_center.Translation();
|
||||
pointInRect = pointInRect.RotateBy(-m_center.Rotation());
|
||||
|
||||
if (units::math::abs(units::math::abs(pointInRect.X()) - m_xWidth / 2.0) <=
|
||||
1E-9_m) {
|
||||
// Point rests on left/right perimeter
|
||||
return units::math::abs(pointInRect.Y()) <= m_yWidth / 2.0;
|
||||
} else if (units::math::abs(units::math::abs(pointInRect.Y()) -
|
||||
m_yWidth / 2.0) <= 1E-9_m) {
|
||||
// Point rests on top/bottom perimeter
|
||||
return units::math::abs(pointInRect.X()) <= m_xWidth / 2.0;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks if a point is contained within the rectangle. This is inclusive, if
|
||||
* the point lies on the perimeter it will return true.
|
||||
*
|
||||
* @param point The point to check.
|
||||
* @return True, if the rectangle contains the point or the perimeter
|
||||
* intersects the point.
|
||||
*/
|
||||
constexpr bool Contains(const Translation2d& point) const {
|
||||
// Rotate the point into the rectangle's coordinate frame
|
||||
auto rotPoint =
|
||||
point.RotateAround(m_center.Translation(), -m_center.Rotation());
|
||||
|
||||
// Check if within bounding box
|
||||
return rotPoint.X() >= (m_center.X() - m_xWidth / 2.0) &&
|
||||
rotPoint.X() <= (m_center.X() + m_xWidth / 2.0) &&
|
||||
rotPoint.Y() >= (m_center.Y() - m_yWidth / 2.0) &&
|
||||
rotPoint.Y() <= (m_center.Y() + m_yWidth / 2.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the distance between the perimeter of the rectangle and the point.
|
||||
*
|
||||
* @param point The point to check.
|
||||
* @return The distance (0, if the point is contained by the rectangle)
|
||||
*/
|
||||
constexpr units::meter_t Distance(const Translation2d& point) const {
|
||||
return FindNearestPoint(point).Distance(point);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the nearest point that is contained within the rectangle.
|
||||
*
|
||||
* @param point The point that this will find the nearest point to.
|
||||
* @return A new point that is nearest to {@code point} and contained in the
|
||||
* rectangle.
|
||||
*/
|
||||
constexpr Translation2d FindNearestPoint(const Translation2d& point) const {
|
||||
// Check if already in rectangle
|
||||
if (Contains(point)) {
|
||||
return point;
|
||||
}
|
||||
|
||||
// Rotate the point by the inverse of the rectangle's rotation
|
||||
auto rotPoint =
|
||||
point.RotateAround(m_center.Translation(), -m_center.Rotation());
|
||||
|
||||
// Find nearest point
|
||||
rotPoint =
|
||||
Translation2d{std::clamp(rotPoint.X(), m_center.X() - m_xWidth / 2.0,
|
||||
m_center.X() + m_xWidth / 2.0),
|
||||
std::clamp(rotPoint.Y(), m_center.Y() - m_yWidth / 2.0,
|
||||
m_center.Y() + m_yWidth / 2.0)};
|
||||
|
||||
// Undo rotation
|
||||
return rotPoint.RotateAround(m_center.Translation(), m_center.Rotation());
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks equality between this Rectangle2d and another object.
|
||||
*
|
||||
* @param other The other object.
|
||||
* @return Whether the two objects are equal.
|
||||
*/
|
||||
constexpr bool operator==(const Rectangle2d& other) const {
|
||||
return m_center == other.m_center &&
|
||||
units::math::abs(m_xWidth - other.m_xWidth) < 1E-9_m &&
|
||||
units::math::abs(m_yWidth - other.m_yWidth) < 1E-9_m;
|
||||
}
|
||||
|
||||
private:
|
||||
Pose2d m_center;
|
||||
units::meter_t m_xWidth;
|
||||
units::meter_t m_yWidth;
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
|
||||
#include "frc/geometry/proto/Rectangle2dProto.h"
|
||||
#include "frc/geometry/struct/Rectangle2dStruct.h"
|
||||
Reference in New Issue
Block a user