forked from wpilibsuite/allwpilib
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
13 changed files
with
708 additions
and
51 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,55 @@ | ||
| // 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 "frc/geometry/Ellipse2d.h" | ||
|
|
||
| #include <sleipnir/optimization/OptimizationProblem.hpp> | ||
|
|
||
| using namespace frc; | ||
|
|
||
| units::meter_t Ellipse2d::Distance(const Translation2d& point) const { | ||
| return FindNearestPoint(point).Distance(point); | ||
| } | ||
|
|
||
| Translation2d Ellipse2d::FindNearestPoint(const Translation2d& point) const { | ||
| // Check if already in ellipse | ||
| if (Contains(point)) { | ||
| return point; | ||
| } | ||
|
|
||
| // Rotate the point by the inverse of the ellipse's rotation | ||
| auto rotPoint = | ||
| point.RotateAround(m_center.Translation(), -m_center.Rotation()); | ||
|
|
||
| // Find nearest point | ||
| { | ||
| namespace slp = sleipnir; | ||
|
|
||
| sleipnir::OptimizationProblem problem; | ||
|
|
||
| // Point on ellipse | ||
| auto x = problem.DecisionVariable(); | ||
| x.SetValue(rotPoint.X().value()); | ||
| auto y = problem.DecisionVariable(); | ||
| y.SetValue(rotPoint.Y().value()); | ||
|
|
||
| problem.Minimize(slp::pow(x - rotPoint.X().value(), 2) + | ||
| slp::pow(y - rotPoint.Y().value(), 2)); | ||
|
|
||
| // (x − x_c)²/a² + (y − y_c)²/b² = 1 | ||
| problem.SubjectTo(slp::pow(x - m_center.X().value(), 2) / | ||
| (m_xSemiAxis.value() * m_xSemiAxis.value()) + | ||
| slp::pow(y - m_center.Y().value(), 2) / | ||
| (m_ySemiAxis.value() * m_ySemiAxis.value()) == | ||
| 1); | ||
|
|
||
| problem.Solve(); | ||
|
|
||
| rotPoint = frc::Translation2d{units::meter_t{x.Value()}, | ||
| units::meter_t{y.Value()}}; | ||
| } | ||
|
|
||
| // Undo rotation | ||
| return rotPoint.RotateAround(m_center.Translation(), m_center.Rotation()); | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
199 changes: 199 additions & 0 deletions
199
wpimath/src/main/native/include/frc/geometry/Ellipse2d.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,199 @@ | ||
| // 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 <array> | ||
| #include <cmath> | ||
| #include <stdexcept> | ||
|
|
||
| #include <gcem.hpp> | ||
|
|
||
| #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 ellipse space containing translational, rotational, and | ||
| * scaling components. | ||
| */ | ||
| class Ellipse2d { | ||
| public: | ||
| /** | ||
| * Constructs an ellipse around a center point and two semi-axes, a horizontal | ||
| * and vertical one. | ||
| * | ||
| * @param center The center of the ellipse. | ||
| * @param xSemiAxis The x semi-axis. | ||
| * @param ySemiAxis The y semi-axis. | ||
| */ | ||
| constexpr Ellipse2d(const Pose2d& center, units::meter_t xSemiAxis, | ||
| units::meter_t ySemiAxis) | ||
| : m_center{center}, m_xSemiAxis{xSemiAxis}, m_ySemiAxis{ySemiAxis} { | ||
| if (xSemiAxis <= 0_m || ySemiAxis <= 0_m) { | ||
| throw std::invalid_argument("Ellipse2d semi-axes must be positive"); | ||
| } | ||
| } | ||
|
|
||
| /** | ||
| * Constructs a perfectly circular ellipse with the specified radius. | ||
| * | ||
| * @param center The center of the circle. | ||
| * @param radius The radius of the circle. | ||
| */ | ||
| constexpr Ellipse2d(const Translation2d& center, double radius) | ||
| : m_center{center, Rotation2d{}}, | ||
| m_xSemiAxis{radius}, | ||
| m_ySemiAxis{radius} {} | ||
|
|
||
| /** | ||
| * Returns the center of the ellipse. | ||
| * | ||
| * @return The center of the ellipse. | ||
| */ | ||
| constexpr Pose2d Center() const { return m_center; } | ||
|
|
||
| /** | ||
| * Returns the x semi-axis. | ||
| * | ||
| * @return The x semi-axis. | ||
| */ | ||
| constexpr units::meter_t XSemiAxis() const { return m_xSemiAxis; } | ||
|
|
||
| /** | ||
| * Returns the y semi-axis. | ||
| * | ||
| * @return The y semi-axis. | ||
| */ | ||
| constexpr units::meter_t YSemiAxis() const { return m_ySemiAxis; } | ||
|
|
||
| /** | ||
| * Returns the focal points of the ellipse. In a perfect circle, this will | ||
| * always return the center. | ||
| * | ||
| * @return The focal points. | ||
| */ | ||
| constexpr std::array<Translation2d, 2> FocalPoints() const { | ||
| auto a = units::math::max(m_xSemiAxis, m_ySemiAxis); // Major semi-axis | ||
| auto b = units::math::min(m_xSemiAxis, m_ySemiAxis); // Minor semi-axis | ||
| auto c = units::math::sqrt(a * a - b * b); | ||
|
|
||
| if (m_xSemiAxis > m_ySemiAxis) { | ||
| return std::array{ | ||
| (m_center + Transform2d{-c, 0_m, Rotation2d{}}).Translation(), | ||
| (m_center + Transform2d{c, 0_m, Rotation2d{}}).Translation()}; | ||
| } else { | ||
| return std::array{ | ||
| (m_center + Transform2d{0_m, -c, Rotation2d{}}).Translation(), | ||
| (m_center + Transform2d{0_m, c, Rotation2d{}}).Translation()}; | ||
| } | ||
| } | ||
|
|
||
| /** | ||
| * Transforms the center of the ellipse and returns the new ellipse. | ||
| * | ||
| * @param other The transform to transform by. | ||
| * @return The transformed ellipse. | ||
| */ | ||
| constexpr Ellipse2d TransformBy(const Transform2d& other) const { | ||
| return Ellipse2d{m_center.TransformBy(other), m_xSemiAxis, m_ySemiAxis}; | ||
| } | ||
|
|
||
| /** | ||
| * Rotates the center of the ellipse and returns the new ellipse. | ||
| * | ||
| * @param other The rotation to transform by. | ||
| * @return The rotated ellipse. | ||
| */ | ||
| constexpr Ellipse2d RotateBy(const Rotation2d& other) const { | ||
| return Ellipse2d{m_center.RotateBy(other), m_xSemiAxis, m_ySemiAxis}; | ||
| } | ||
|
|
||
| /** | ||
| * Checks if a point is intersected by this ellipse's circumference. | ||
| * | ||
| * @param point The point to check. | ||
| * @return True, if this ellipse's circumference intersects the point. | ||
| */ | ||
| constexpr bool Intersects(const Translation2d& point) const { | ||
| return gcem::abs(1.0 - SolveEllipseEquation(point)) <= 1E-9; | ||
| } | ||
|
|
||
| /** | ||
| * Checks if a point is contained within this ellipse. This is inclusive, if | ||
| * the point lies on the circumference this will return {@code true}. | ||
| * | ||
| * @param point The point to check. | ||
| * @return True, if the point is within or on the ellipse. | ||
| */ | ||
| constexpr bool Contains(const Translation2d& point) const { | ||
| return SolveEllipseEquation(point) <= 1.0; | ||
| } | ||
|
|
||
| /** | ||
| * Returns the distance between the perimeter of the ellipse and the point. | ||
| * | ||
| * @param point The point to check. | ||
| * @return The distance (0, if the point is contained by the ellipse) | ||
| */ | ||
| units::meter_t Distance(const Translation2d& point) const; | ||
|
|
||
| /** | ||
| * Returns the nearest point that is contained within the ellipse. | ||
| * | ||
| * @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 | ||
| * ellipse. | ||
| */ | ||
| Translation2d FindNearestPoint(const Translation2d& point) const; | ||
|
|
||
| /** | ||
| * Checks equality between this Ellipse2d and another object. | ||
| * | ||
| * @param other The other object. | ||
| * @return Whether the two objects are equal. | ||
| */ | ||
| constexpr bool operator==(const Ellipse2d& other) const { | ||
| return m_center == other.m_center && | ||
| units::math::abs(m_xSemiAxis - other.m_xSemiAxis) < 1E-9_m && | ||
| units::math::abs(m_ySemiAxis - other.m_ySemiAxis) < 1E-9_m; | ||
| } | ||
|
|
||
| private: | ||
| Pose2d m_center; | ||
| units::meter_t m_xSemiAxis; | ||
| units::meter_t m_ySemiAxis; | ||
|
|
||
| /** | ||
| * Solves the equation of an ellipse from the given point. This is a helper | ||
| * function used to determine if that point lies inside of or on an ellipse. | ||
| * | ||
| * <pre> | ||
| * (x - h)²/a² + (y - k)²/b² = 1 | ||
| * </pre> | ||
| * | ||
| * @param point The point to solve for. | ||
| * @return < 1.0 if the point lies inside the ellipse, == 1.0 if a point lies | ||
| * on the ellipse, and > 1.0 if the point lies outsides the ellipse. | ||
| */ | ||
| constexpr double SolveEllipseEquation(const Translation2d& point) const { | ||
| // Rotate the point by the inverse of the ellipse's rotation | ||
| auto rotPoint = | ||
| point.RotateAround(m_center.Translation(), -m_center.Rotation()); | ||
|
|
||
| auto x = rotPoint.X() - m_center.X(); | ||
| auto y = rotPoint.Y() - m_center.Y(); | ||
|
|
||
| return (x * x) / (m_xSemiAxis * m_xSemiAxis) + | ||
| (y * y) / (m_ySemiAxis * m_ySemiAxis); | ||
| } | ||
| }; | ||
|
|
||
| } // namespace frc |
Oops, something went wrong.