Skip to content

Commit

Permalink
cpp header file created, java spotlessapply ran
Browse files Browse the repository at this point in the history
  • Loading branch information
@Braykoff
Braykoff committed Apr 14, 2024
1 parent 6d87027 commit 767a11c
Show file tree
Hide file tree
Showing 15 changed files with 134 additions and 145 deletions.
53 changes: 25 additions & 28 deletions wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,23 +4,20 @@

package edu.wpi.first.math.geometry;

import java.util.Objects;

import edu.wpi.first.math.geometry.proto.Ellipse2dProto;
import edu.wpi.first.math.geometry.struct.Ellipse2dStruct;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
import java.util.Objects;

/**
* Represents a 2d ellipse space containing translational, rotational, and scaling components
*/
/** Represents a 2d ellipse space containing translational, rotational, and scaling components */
public class Ellipse2d implements ProtobufSerializable, StructSerializable {
private final Pose2d m_center;
private final double m_xSemiAxis, m_ySemiAxis;

/**
* 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.
Expand All @@ -38,7 +35,7 @@ public Ellipse2d(Pose2d center, double xSemiAxis, double ySemiAxis) {

/**
* Constructs a perfectly circular ellipse with the specified radius.
*
*
* @param center The center of the circle.
* @param radius The radius of the circle.
*/
Expand All @@ -48,7 +45,7 @@ public Ellipse2d(Pose2d center, double radius) {

/**
* Returns the center of the ellipse.
*
*
* @return The center of the ellipse.
*/
public Pose2d getCenter() {
Expand All @@ -57,7 +54,7 @@ public Pose2d getCenter() {

/**
* Returns the x semi-axis.
*
*
* @return The x semi-axis.
*/
public double getXSemiAxis() {
Expand All @@ -66,24 +63,24 @@ public double getXSemiAxis() {

/**
* Returns the y semi-axis.
*
*
* @return The y semi-axis.
*/
public double getYSemiAxis() {
return m_ySemiAxis;
}

/**
* Returns either of the focal points of the ellipse.
* In a perfect circle, this will always return the center.
*
* Returns either of the focal points of the ellipse. In a perfect circle, this will always return
* the center.
*
* @param first Whether to return the first (-c) or second (+c) focal point.
* @return A focal point.
*/
public Translation2d getFocalPoint(boolean first) {
double a = Math.max(m_xSemiAxis, m_ySemiAxis); // Major semi-axis
double b = Math.min(m_xSemiAxis, m_ySemiAxis); // Minor semi-axis
double c = Math.sqrt(a*a - b*b);
double c = Math.sqrt(a * a - b * b);

c = (first ? -c : c);

Expand All @@ -98,7 +95,7 @@ public Translation2d getFocalPoint(boolean first) {

/**
* Transforms the center of the ellipse and returns the new ellipse.
*
*
* @param other The transform to transform by.
* @return The transformed ellipse.
*/
Expand All @@ -108,7 +105,7 @@ public Ellipse2d transformBy(Transform2d other) {

/**
* Rotates the center of the ellipse and returns the new ellipse.
*
*
* @param other The rotation to transform by.
* @return The rotated ellipse.
*/
Expand All @@ -119,13 +116,14 @@ public Ellipse2d rotateBy(Rotation2d other) {
/**
* 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)^2 / a^2 + (y-k)^2 / b^2 = 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.
* @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.
*/
private double solveEllipseEquation(Translation2d point) {
// Rotate the point by the inverse of the ellipse's rotation
Expand All @@ -134,13 +132,12 @@ private double solveEllipseEquation(Translation2d point) {
double x = point.getX() - m_center.getX();
double y = point.getY() - m_center.getY();

return (x * x) / (m_xSemiAxis * m_xSemiAxis) +
(y * y) / (m_ySemiAxis * m_ySemiAxis);
return (x * x) / (m_xSemiAxis * m_xSemiAxis) + (y * y) / (m_ySemiAxis * 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.
*/
Expand All @@ -149,9 +146,9 @@ public boolean intersectsPoint(Translation2d point) {
}

/**
* Checks if a point is contained within this ellipse.
* This is inclusive, if the point lies on the circumference this will return {@code true}.
*
* 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.
*/
Expand All @@ -161,13 +158,13 @@ public boolean containsPoint(Translation2d point) {

@Override
public String toString() {
return String.format("Ellipse2d(center: %s, x: %.2f, y:%.2f)",
m_center, m_xSemiAxis, m_ySemiAxis);
return String.format(
"Ellipse2d(center: %s, x: %.2f, y:%.2f)", m_center, m_xSemiAxis, m_ySemiAxis);
}

/**
* Checks equality between this Ellipse2d and another object
*
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
Expand Down
80 changes: 39 additions & 41 deletions wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,24 +4,23 @@

package edu.wpi.first.math.geometry;

import java.util.Objects;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.proto.Rectangle2dProto;
import edu.wpi.first.math.geometry.struct.Rectangle2dStruct;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
import java.util.Objects;

/**
* Represents a 2d rectangular space containing translational, rotational, and scaling components
/**
* Represents a 2d rectangular space containing translational, rotational, and scaling components.
*/
public class Rectangle2d implements ProtobufSerializable, StructSerializable {
private final Pose2d m_center;
private final double m_xWidth, m_yWidth;

/**
* 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.
Expand All @@ -40,7 +39,7 @@ public Rectangle2d(Pose2d center, double xWidth, double yWidth) {
/**
* Constructs a rectangle at the specified position and rotation with the specified width and
* height.
*
*
* @param center The center of the rectangle.
* @param width The x size component of the rectangle, in unrotated coordinate frame.
* @param height The y size component of the rectangle, in unrotated coordinate frame.
Expand All @@ -51,24 +50,23 @@ public Rectangle2d(Translation2d center, double xWidth, double yWidth, Rotation2
}

/**
* Creates an unrotated rectangle from the given corners.
* The corners should be diagonally opposite of each other.
*
* 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.
*/
public Rectangle2d(Translation2d cornerA, Translation2d cornerB) {
this(
cornerA.plus(cornerB).div(2.0),
Math.abs(cornerA.getX() - cornerB.getX()),
Math.abs(cornerA.getY() - cornerB.getY()),
new Rotation2d()
);
cornerA.plus(cornerB).div(2.0),
Math.abs(cornerA.getX() - cornerB.getX()),
Math.abs(cornerA.getY() - cornerB.getY()),
new Rotation2d());
}

/**
* Returns the center of the rectangle.
*
*
* @return The center of the rectangle.
*/
public Pose2d getCenter() {
Expand All @@ -77,7 +75,7 @@ public Pose2d getCenter() {

/**
* Returns the rotational component of the rectangle.
*
*
* @return The rotational component of the rectangle.
*/
public Rotation2d getRotation() {
Expand All @@ -86,7 +84,7 @@ public Rotation2d getRotation() {

/**
* Returns the x size component of the rectangle.
*
*
* @return The x size component of the rectangle.
*/
public double getXWidth() {
Expand All @@ -95,7 +93,7 @@ public double getXWidth() {

/**
* Returns the y size component of the rectangle.
*
*
* @return The y size component of the rectangle.
*/
public double getYWidth() {
Expand All @@ -104,7 +102,7 @@ public double getYWidth() {

/**
* Transforms the center of the rectangle and returns the new rectangle.
*
*
* @param other The transform to transform by.
* @return The transformed rectangle
*/
Expand All @@ -114,7 +112,7 @@ public Rectangle2d transformBy(Transform2d other) {

/**
* Rotates the center of the rectangle and returns the new rectangle.
*
*
* @param other The rotation to transform by.
* @return The rotated rectangle.
*/
Expand All @@ -124,7 +122,7 @@ public Rectangle2d rotateBy(Rotation2d other) {

/**
* 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.
*/
Expand All @@ -145,27 +143,26 @@ public boolean intersectsPoint(Translation2d point) {
}

/**
* Checks if a point is contained within the rectangle.
* This is inclusive, if the point lies on the perimeter it will return true.
*
* 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.
*/
public boolean containsPoint(Translation2d point) {
// Rotate the point into the rectangle's coordinate frame
point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus());

// Check if within bounding box
return (
point.getX() >= (m_center.getX() - m_xWidth/2.0) &&
point.getX() <= (m_center.getX() + m_xWidth/2.0) &&
point.getY() >= (m_center.getY() - m_yWidth/2.0) &&
point.getY() <= (m_center.getY() + m_yWidth/2.0));
return (point.getX() >= (m_center.getX() - m_xWidth / 2.0)
&& point.getX() <= (m_center.getX() + m_xWidth / 2.0)
&& point.getY() >= (m_center.getY() - m_yWidth / 2.0)
&& point.getY() <= (m_center.getY() + 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)
*/
Expand All @@ -177,16 +174,16 @@ public double distanceToPoint(Translation2d point) {
point = point.rotateBy(m_center.getRotation().unaryMinus());

// Find x and y distances
double dx = Math.max(0.0, Math.abs(point.getX()) - m_xWidth/2.0);
double dy = Math.max(0.0, Math.abs(point.getY()) - m_yWidth/2.0);
double dx = Math.max(0.0, Math.abs(point.getX()) - m_xWidth / 2.0);
double dy = Math.max(0.0, Math.abs(point.getY()) - m_yWidth / 2.0);

// Distance formula
return Math.hypot(dx, dy);
}

/**
* 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.
*/
Expand All @@ -196,21 +193,22 @@ public Translation2d findNearestPoint(Translation2d point) {

// Rotate the point by the inverse of the rectangle's rotation
point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus());

// Find nearest point
point = new Translation2d(
MathUtil.clamp(point.getX(), m_center.getX() - m_xWidth/2.0, m_center.getX() + m_xWidth/2.0),
MathUtil.clamp(point.getY(), m_center.getY() - m_yWidth/2.0, m_center.getY() + m_yWidth/2.0)
);
point =
new Translation2d(
MathUtil.clamp(
point.getX(), m_center.getX() - m_xWidth / 2.0, m_center.getX() + m_xWidth / 2.0),
MathUtil.clamp(
point.getY(), m_center.getY() - m_yWidth / 2.0, m_center.getY() + m_yWidth / 2.0));

// Undo rotation
return point.rotateAround(m_center.getTranslation(), m_center.getRotation());
}

@Override
public String toString() {
return String.format("Rectangle2d(center: %s, x: %.2f, y: %.2f)",
m_center, m_xWidth, m_yWidth);
return String.format("Rectangle2d(center: %s, x: %.2f, y: %.2f)", m_center, m_xWidth, m_yWidth);
}

/**
Expand Down
Loading

0 comments on commit 767a11c

Please sign in to comment.