From 2c406e61e09cc9dbff6657eb6f2a398f7ec024b2 Mon Sep 17 00:00:00 2001 From: braykoff <99614905+Braykoff@users.noreply.github.com> Date: Wed, 10 Apr 2024 13:20:00 -0400 Subject: [PATCH 01/30] Java Rectangle2d classes --- .../edu/wpi/first/math/proto/Geometry2D.java | 451 +++++++++++++++++- .../wpi/first/math/geometry/Rectangle2d.java | 258 ++++++++++ .../first/math/geometry/Translation2d.java | 19 + .../math/geometry/proto/Rectangle2dProto.java | 49 ++ .../geometry/struct/Rectangle2dStruct.java | 49 ++ wpimath/src/main/proto/geometry2d.proto | 6 + 6 files changed, 827 insertions(+), 5 deletions(-) create mode 100644 wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java create mode 100644 wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rectangle2dProto.java create mode 100644 wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rectangle2dStruct.java diff --git a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Geometry2D.java b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Geometry2D.java index 331ccd1c8ad..a33d3767841 100644 --- a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Geometry2D.java +++ b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Geometry2D.java @@ -18,7 +18,7 @@ import us.hebi.quickbuf.RepeatedByte; public final class Geometry2D { - private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(1256, + private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(1565, "ChBnZW9tZXRyeTJkLnByb3RvEgl3cGkucHJvdG8iMwoVUHJvdG9idWZUcmFuc2xhdGlvbjJkEgwKAXgY" + "ASABKAFSAXgSDAoBeRgCIAEoAVIBeSIqChJQcm90b2J1ZlJvdGF0aW9uMmQSFAoFdmFsdWUYASABKAFS" + "BXZhbHVlIo8BCg5Qcm90b2J1ZlBvc2UyZBJCCgt0cmFuc2xhdGlvbhgBIAEoCzIgLndwaS5wcm90by5Q" + @@ -27,7 +27,9 @@ public final class Geometry2D { "dHJhbnNsYXRpb24YASABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUgt0cmFuc2xh" + "dGlvbhI5Cghyb3RhdGlvbhgCIAEoCzIdLndwaS5wcm90by5Qcm90b2J1ZlJvdGF0aW9uMmRSCHJvdGF0" + "aW9uIkkKD1Byb3RvYnVmVHdpc3QyZBIOCgJkeBgBIAEoAVICZHgSDgoCZHkYAiABKAFSAmR5EhYKBmR0" + - "aGV0YRgDIAEoAVIGZHRoZXRhQhoKGGVkdS53cGkuZmlyc3QubWF0aC5wcm90b0rPBQoGEgQAAB0BCggK" + + "aGV0YRgDIAEoAVIGZHRoZXRhInYKE1Byb3RvYnVmUmVjdGFuZ2xlMmQSMQoGY2VudGVyGAEgASgLMhku" + + "d3BpLnByb3RvLlByb3RvYnVmUG9zZTJkUgZjZW50ZXISFAoFd2lkdGgYAiABKAFSBXdpZHRoEhYKBmhl" + + "aWdodBgDIAEoAVIGaGVpZ2h0QhoKGGVkdS53cGkuZmlyc3QubWF0aC5wcm90b0qMBwoGEgQAACMBCggK" + "AQwSAwAAEgoICgECEgMCABIKCAoBCBIDBAAxCgkKAggBEgMEADEKCgoCBAASBAYACQEKCgoDBAABEgMG" + "CB0KCwoEBAACABIDBwIPCgwKBQQAAgAFEgMHAggKDAoFBAACAAESAwcJCgoMCgUEAAIAAxIDBw0OCgsK" + "BAQAAgESAwgCDwoMCgUEAAIBBRIDCAIICgwKBQQAAgEBEgMICQoKDAoFBAACAQMSAwgNDgoKCgIEARIE" + @@ -36,10 +38,14 @@ public final class Geometry2D { "AhcKDAoFBAICAAESAxAYIwoMCgUEAgIAAxIDECYnCgsKBAQCAgESAxECIgoMCgUEAgIBBhIDEQIUCgwK" + "BQQCAgEBEgMRFR0KDAoFBAICAQMSAxEgIQoKCgIEAxIEFAAXAQoKCgMEAwESAxQIGwoLCgQEAwIAEgMV" + "AigKDAoFBAMCAAYSAxUCFwoMCgUEAwIAARIDFRgjCgwKBQQDAgADEgMVJicKCwoEBAMCARIDFgIiCgwK" + - "BQQDAgEGEgMWAhQKDAoFBAMCAQESAxYVHQoMCgUEAwIBAxIDFiAhCgoKAgQEEgQZAB0BCgoKAwQEARID" + + "BQQDAgEGEgMWAhQKDAoFBAMCAQESAxYVHQoMCgUEAwIBAxIDFiAhCgoKAgQEEgQZAB0BCgoKAwQEARID", "GQgXCgsKBAQEAgASAxoCEAoMCgUEBAIABRIDGgIICgwKBQQEAgABEgMaCQsKDAoFBAQCAAMSAxoODwoL" + - "CgQEBAIBEgMbAhAKDAoFBAQCAQUSAxsCCAoMCgUEBAIBARIDGwkLCgwKBQQEAgEDEgMbDg8KCwoEBAQC", - "AhIDHAIUCgwKBQQEAgIFEgMcAggKDAoFBAQCAgESAxwJDwoMCgUEBAICAxIDHBITYgZwcm90bzM="); + "CgQEBAIBEgMbAhAKDAoFBAQCAQUSAxsCCAoMCgUEBAIBARIDGwkLCgwKBQQEAgEDEgMbDg8KCwoEBAQC" + + "AhIDHAIUCgwKBQQEAgIFEgMcAggKDAoFBAQCAgESAxwJDwoMCgUEBAICAxIDHBITCgoKAgQFEgQfACMB" + + "CgoKAwQFARIDHwgbCgsKBAQFAgASAyACHAoMCgUEBQIABhIDIAIQCgwKBQQFAgABEgMgERcKDAoFBAUC" + + "AAMSAyAaGwoLCgQEBQIBEgMhAhMKDAoFBAUCAQUSAyECCAoMCgUEBQIBARIDIQkOCgwKBQQFAgEDEgMh" + + "ERIKCwoEBAUCAhIDIgIUCgwKBQQFAgIFEgMiAggKDAoFBAUCAgESAyIJDwoMCgUEBQICAxIDIhITYgZw" + + "cm90bzM="); static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("geometry2d.proto", "wpi.proto", descriptorData); @@ -53,6 +59,8 @@ public final class Geometry2D { static final Descriptors.Descriptor wpi_proto_ProtobufTwist2d_descriptor = descriptor.internalContainedType(425, 73, "ProtobufTwist2d", "wpi.proto.ProtobufTwist2d"); + static final Descriptors.Descriptor wpi_proto_ProtobufRectangle2d_descriptor = descriptor.internalContainedType(500, 118, "ProtobufRectangle2d", "wpi.proto.ProtobufRectangle2d"); + /** * @return this proto file's descriptor. */ @@ -1803,4 +1811,437 @@ static class FieldNames { static final FieldName dtheta = FieldName.forField("dtheta"); } } + + /** + * Protobuf type {@code ProtobufRectangle2d} + */ + public static final class ProtobufRectangle2d extends ProtoMessage implements Cloneable { + private static final long serialVersionUID = 0L; + + /** + * optional double width = 2; + */ + private double width; + + /** + * optional double height = 3; + */ + private double height; + + /** + * optional .wpi.proto.ProtobufPose2d center = 1; + */ + private final ProtobufPose2d center = ProtobufPose2d.newInstance(); + + private ProtobufRectangle2d() { + } + + /** + * @return a new empty instance of {@code ProtobufRectangle2d} + */ + public static ProtobufRectangle2d newInstance() { + return new ProtobufRectangle2d(); + } + + /** + * optional double width = 2; + * @return whether the width field is set + */ + public boolean hasWidth() { + return (bitField0_ & 0x00000001) != 0; + } + + /** + * optional double width = 2; + * @return this + */ + public ProtobufRectangle2d clearWidth() { + bitField0_ &= ~0x00000001; + width = 0D; + return this; + } + + /** + * optional double width = 2; + * @return the width + */ + public double getWidth() { + return width; + } + + /** + * optional double width = 2; + * @param value the width to set + * @return this + */ + public ProtobufRectangle2d setWidth(final double value) { + bitField0_ |= 0x00000001; + width = value; + return this; + } + + /** + * optional double height = 3; + * @return whether the height field is set + */ + public boolean hasHeight() { + return (bitField0_ & 0x00000002) != 0; + } + + /** + * optional double height = 3; + * @return this + */ + public ProtobufRectangle2d clearHeight() { + bitField0_ &= ~0x00000002; + height = 0D; + return this; + } + + /** + * optional double height = 3; + * @return the height + */ + public double getHeight() { + return height; + } + + /** + * optional double height = 3; + * @param value the height to set + * @return this + */ + public ProtobufRectangle2d setHeight(final double value) { + bitField0_ |= 0x00000002; + height = value; + return this; + } + + /** + * optional .wpi.proto.ProtobufPose2d center = 1; + * @return whether the center field is set + */ + public boolean hasCenter() { + return (bitField0_ & 0x00000004) != 0; + } + + /** + * optional .wpi.proto.ProtobufPose2d center = 1; + * @return this + */ + public ProtobufRectangle2d clearCenter() { + bitField0_ &= ~0x00000004; + center.clear(); + return this; + } + + /** + * optional .wpi.proto.ProtobufPose2d center = 1; + * + * This method returns the internal storage object without modifying any has state. + * The returned object should not be modified and be treated as read-only. + * + * Use {@link #getMutableCenter()} if you want to modify it. + * + * @return internal storage object for reading + */ + public ProtobufPose2d getCenter() { + return center; + } + + /** + * optional .wpi.proto.ProtobufPose2d center = 1; + * + * This method returns the internal storage object and sets the corresponding + * has state. The returned object will become part of this message and its + * contents may be modified as long as the has state is not cleared. + * + * @return internal storage object for modifications + */ + public ProtobufPose2d getMutableCenter() { + bitField0_ |= 0x00000004; + return center; + } + + /** + * optional .wpi.proto.ProtobufPose2d center = 1; + * @param value the center to set + * @return this + */ + public ProtobufRectangle2d setCenter(final ProtobufPose2d value) { + bitField0_ |= 0x00000004; + center.copyFrom(value); + return this; + } + + @Override + public ProtobufRectangle2d copyFrom(final ProtobufRectangle2d other) { + cachedSize = other.cachedSize; + if ((bitField0_ | other.bitField0_) != 0) { + bitField0_ = other.bitField0_; + width = other.width; + height = other.height; + center.copyFrom(other.center); + } + return this; + } + + @Override + public ProtobufRectangle2d mergeFrom(final ProtobufRectangle2d other) { + if (other.isEmpty()) { + return this; + } + cachedSize = -1; + if (other.hasWidth()) { + setWidth(other.width); + } + if (other.hasHeight()) { + setHeight(other.height); + } + if (other.hasCenter()) { + getMutableCenter().mergeFrom(other.center); + } + return this; + } + + @Override + public ProtobufRectangle2d clear() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + width = 0D; + height = 0D; + center.clear(); + return this; + } + + @Override + public ProtobufRectangle2d clearQuick() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + center.clearQuick(); + return this; + } + + @Override + public boolean equals(Object o) { + if (o == this) { + return true; + } + if (!(o instanceof ProtobufRectangle2d)) { + return false; + } + ProtobufRectangle2d other = (ProtobufRectangle2d) o; + return bitField0_ == other.bitField0_ + && (!hasWidth() || ProtoUtil.isEqual(width, other.width)) + && (!hasHeight() || ProtoUtil.isEqual(height, other.height)) + && (!hasCenter() || center.equals(other.center)); + } + + @Override + public void writeTo(final ProtoSink output) throws IOException { + if ((bitField0_ & 0x00000001) != 0) { + output.writeRawByte((byte) 17); + output.writeDoubleNoTag(width); + } + if ((bitField0_ & 0x00000002) != 0) { + output.writeRawByte((byte) 25); + output.writeDoubleNoTag(height); + } + if ((bitField0_ & 0x00000004) != 0) { + output.writeRawByte((byte) 10); + output.writeMessageNoTag(center); + } + } + + @Override + protected int computeSerializedSize() { + int size = 0; + if ((bitField0_ & 0x00000001) != 0) { + size += 9; + } + if ((bitField0_ & 0x00000002) != 0) { + size += 9; + } + if ((bitField0_ & 0x00000004) != 0) { + size += 1 + ProtoSink.computeMessageSizeNoTag(center); + } + return size; + } + + @Override + @SuppressWarnings("fallthrough") + public ProtobufRectangle2d mergeFrom(final ProtoSource input) throws IOException { + // Enabled Fall-Through Optimization (QuickBuffers) + int tag = input.readTag(); + while (true) { + switch (tag) { + case 17: { + // width + width = input.readDouble(); + bitField0_ |= 0x00000001; + tag = input.readTag(); + if (tag != 25) { + break; + } + } + case 25: { + // height + height = input.readDouble(); + bitField0_ |= 0x00000002; + tag = input.readTag(); + if (tag != 10) { + break; + } + } + case 10: { + // center + input.readMessage(center); + bitField0_ |= 0x00000004; + tag = input.readTag(); + if (tag != 0) { + break; + } + } + case 0: { + return this; + } + default: { + if (!input.skipField(tag)) { + return this; + } + tag = input.readTag(); + break; + } + } + } + } + + @Override + public void writeTo(final JsonSink output) throws IOException { + output.beginObject(); + if ((bitField0_ & 0x00000001) != 0) { + output.writeDouble(FieldNames.width, width); + } + if ((bitField0_ & 0x00000002) != 0) { + output.writeDouble(FieldNames.height, height); + } + if ((bitField0_ & 0x00000004) != 0) { + output.writeMessage(FieldNames.center, center); + } + output.endObject(); + } + + @Override + public ProtobufRectangle2d mergeFrom(final JsonSource input) throws IOException { + if (!input.beginObject()) { + return this; + } + while (!input.isAtEnd()) { + switch (input.readFieldHash()) { + case 113126854: { + if (input.isAtField(FieldNames.width)) { + if (!input.trySkipNullValue()) { + width = input.readDouble(); + bitField0_ |= 0x00000001; + } + } else { + input.skipUnknownField(); + } + break; + } + case -1221029593: { + if (input.isAtField(FieldNames.height)) { + if (!input.trySkipNullValue()) { + height = input.readDouble(); + bitField0_ |= 0x00000002; + } + } else { + input.skipUnknownField(); + } + break; + } + case -1364013995: { + if (input.isAtField(FieldNames.center)) { + if (!input.trySkipNullValue()) { + input.readMessage(center); + bitField0_ |= 0x00000004; + } + } else { + input.skipUnknownField(); + } + break; + } + default: { + input.skipUnknownField(); + break; + } + } + } + input.endObject(); + return this; + } + + @Override + public ProtobufRectangle2d clone() { + return new ProtobufRectangle2d().copyFrom(this); + } + + @Override + public boolean isEmpty() { + return ((bitField0_) == 0); + } + + public static ProtobufRectangle2d parseFrom(final byte[] data) throws + InvalidProtocolBufferException { + return ProtoMessage.mergeFrom(new ProtobufRectangle2d(), data).checkInitialized(); + } + + public static ProtobufRectangle2d parseFrom(final ProtoSource input) throws IOException { + return ProtoMessage.mergeFrom(new ProtobufRectangle2d(), input).checkInitialized(); + } + + public static ProtobufRectangle2d parseFrom(final JsonSource input) throws IOException { + return ProtoMessage.mergeFrom(new ProtobufRectangle2d(), input).checkInitialized(); + } + + /** + * @return factory for creating ProtobufRectangle2d messages + */ + public static MessageFactory getFactory() { + return ProtobufRectangle2dFactory.INSTANCE; + } + + /** + * @return this type's descriptor. + */ + public static Descriptors.Descriptor getDescriptor() { + return Geometry2D.wpi_proto_ProtobufRectangle2d_descriptor; + } + + private enum ProtobufRectangle2dFactory implements MessageFactory { + INSTANCE; + + @Override + public ProtobufRectangle2d create() { + return ProtobufRectangle2d.newInstance(); + } + } + + /** + * Contains name constants used for serializing JSON + */ + static class FieldNames { + static final FieldName width = FieldName.forField("width"); + + static final FieldName height = FieldName.forField("height"); + + static final FieldName center = FieldName.forField("center"); + } + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java new file mode 100644 index 00000000000..de0ab791c1e --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java @@ -0,0 +1,258 @@ +package edu.wpi.first.math.geometry; + +import java.util.Objects; + +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; + +/** + * 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_width, m_height; + + /** + * Constructs a rectangle at the specified position with the specified width and height. + * + * @param center The position (translation and rotation) of the rectangle. + * @param width The width (x size component) of the rectangle. + * @param height The height (y size component) of the rectangle. + */ + public Rectangle2d(Pose2d center, double width, double height) { + // Safety check size + if (width < 0 || height < 0) { + throw new IllegalArgumentException("Rectangle2d dimensions cannot be less than 0!"); + } + + m_center = center; + m_width = width; + m_height = height; + } + + /** + * 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 width (x size component) of the rectangle, in unrotated coordinate frame. + * @param height The height (y size component) of the rectangle, in unrotated coordinate frame. + * @param rotation The rotation of the rectangle. + */ + public Rectangle2d(Translation2d center, double width, double height, Rotation2d rotation) { + this(new Pose2d(center, rotation), width, height); + } + + /** + * 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() + ); + } + + /** + * Transforms the center of the rectangle and returns the new rectangle. + * + * @param other The transform to transform by. + * @return The transformed rectangle + */ + public Rectangle2d transformBy(Transform2d other) { + return new Rectangle2d(m_center.transformBy(other), m_width, m_height); + } + + /** + * Returns the center of the rectangle. + * + * @return The center of the rectangle. + */ + public Pose2d getCenter() { + return m_center; + } + + /** + * Returns the rotational component of the rectangle. + * + * @return The rotational component of the rectangle. + */ + public Rotation2d getRotation() { + return m_center.getRotation(); + } + + /** + * Returns the width (x size component) of the rectangle. + * + * @return The width (x size component) of the rectangle. + */ + public double getWidth() { + return m_width; + } + + /** + * Returns the height (y size component) of the rectangle. + * + * @return The height (y size component) of the rectangle. + */ + public double getHeight() { + return m_height; + } + + /** + * Checks if a point is intersected by this rectangle's perimeter. + * + * @param point The point to check. + * @return True, if this rectangle's perimeter intersects the point. + */ + public boolean intersectsPoint(Translation2d point) { + // Rotate the point around the inverse of the rectangle's rotation + point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus()); + + // Half of width and height + double w = m_width/2.0; + double h = m_height/2.0; + + if (point.getX() == (m_center.getX() - w) || point.getX() == (m_center.getX() + w)) { + // Rests on left/right perimeter + return (point.getY() >= (m_center.getY() - h) && point.getY() <= (m_center.getY() - h)); + } else if (point.getY() == (m_center.getY() - h) || point.getY() == (m_center.getY() + h)) { + // Rest on top/bottom perimeter + return (point.getX() >= (m_center.getX() - w) && point.getX() <= (m_center.getX() + h)); + } + + return false; + } + + /** + * Checks if a point is contained within this rectangle. + * This is exclusive, if the point lies on the perimeter it will return {@code false}. + * + * @param point The point to check. + * @return True, if this rectangle contains the point. + */ + public boolean containsPointExclusive(Translation2d point) { + // Rotate the point around the inverse of the rectangle's rotation + point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus()); + + // Check if within bounding box + return ( + point.getX() > (m_center.getX() - m_width/2.0) && + point.getX() < (m_center.getX() + m_width/2.0) && + point.getY() > (m_center.getY() - m_height/2.0) && + point.getY() < (m_center.getY() + m_height/2.0) + ); + } + + /** + * Checks if a point is contained within this rectangle. + * This is inclusive, if the point lies on the perimeter it will return {@code true}. + * + * @param point The point to check. + * @return True, if this rectangle contains the point or the perimeter intersects the point. + */ + public boolean containsPointInclusive(Translation2d point) { + // Rotate the point around the inverse of the rectangle's rotation + point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus()); + + // Check if within bounding box + return ( + point.getX() >= (m_center.getX() - m_width/2.0) && + point.getX() <= (m_center.getX() + m_width/2.0) && + point.getY() >= (m_center.getY() - m_height/2.0) && + point.getY() <= (m_center.getY() + m_height/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 on the perimeter or contained by the rectangle) + */ + public double distanceToPoint(Translation2d point) { + if (containsPointInclusive(point)) return 0.0; + + // Rotate the point around the inverse of the rectangle's rotation + point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus()); + + // Find x and y distances + double dx = + Math.max(m_center.getX() - m_width - point.getX(), + Math.max(0.0, point.getX() - m_center.getX() + m_width)); + + double dy = + Math.max(m_center.getY() - m_height - point.getY(), + Math.max(0.0, point.getY() - m_center.getY() + m_height)); + + // Distance formula + return Math.sqrt(dx*dx + dy*dy); + } + + /** + * Returns the nearest point that is contained within this 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. + */ + public Translation2d findNearestPoint(Translation2d point) { + // Check if already in rectangle + if (containsPointInclusive(point)) return new Translation2d(point.getX(), point.getY()); + + // Rotate the point around the inverse of the rectangle's rotation + point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus()); + + // Find nearest point + point = new Translation2d( + Math.max(m_center.getX()-m_width/2.0, Math.min(point.getX(), m_center.getX()+m_width/2.0)), + Math.max(m_center.getY()-m_height/2.0, Math.min(point.getY(), m_center.getY()+m_height/2.0)) + ); + + // Undo rotation + return point.rotateAround(m_center.getTranslation(), m_center.getRotation()); + } + + @Override + public String toString() { + return String.format("Rectangle2d(Center: %s, W: %.2f, H: %.2f)", + m_center.toString(), + m_width, + m_height + ); + } + + /** + * Checks equality between this Rectangle2d and another object. + * + * @param obj The other object. + * @return Whether the two objects are equal or not. + */ + @Override + public boolean equals(Object obj) { + if (obj instanceof Rectangle2d) { + return ((Rectangle2d) obj).getCenter().equals(m_center) + && ((Rectangle2d) obj).getWidth() == m_width + && ((Rectangle2d) obj).getHeight() == m_height; + } + return false; + } + + @Override + public int hashCode() { + return Objects.hash(m_center, m_width, m_height); + } + + /** Rectangle2d protobuf for serialization. */ + public static final Rectangle2dProto proto = new Rectangle2dProto(); + + /** Rectangle2d struct for serialization. */ + public static final Rectangle2dStruct struct = new Rectangle2dStruct(); +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java index 4d04df354e3..59afcc14fe0 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java @@ -172,6 +172,25 @@ public Translation2d rotateBy(Rotation2d other) { m_x * other.getCos() - m_y * other.getSin(), m_x * other.getSin() + m_y * other.getCos()); } + /** + * Rotates this translation around another translation in 2D space. + * + * 
+   * [x_new]   [rot.cos, -rot.sin][x - other.x]   [other.x]
+   * [y_new] = [rot.sin, rot.cos][y - other.y]  + [other.y]
+   *
+ * + * @param other The other translation to rotate around. + * @param rot The rotation to rotate the translation by. + * @return The new rotated translation. + */ + public Translation2d rotateAround(Translation2d other, Rotation2d rot) { + return new Translation2d( + (m_x - other.getX()) * rot.getCos() - (m_y - other.getY()) * rot.getSin() + other.getX(), + (m_x - other.getX()) * rot.getSin() + (m_y - other.getY()) * rot.getCos() + other.getY() + ); + } + /** * Returns the sum of two translations in 2D space. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rectangle2dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rectangle2dProto.java new file mode 100644 index 00000000000..18c481cf5d8 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rectangle2dProto.java @@ -0,0 +1,49 @@ +// 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. + +package edu.wpi.first.math.geometry.proto; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rectangle2d; +import edu.wpi.first.math.proto.Geometry2D.ProtobufRectangle2d; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class Rectangle2dProto implements Protobuf { + @Override + public Class getTypeClass() { + return Rectangle2d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufRectangle2d.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Pose2d.proto}; + } + + @Override + public ProtobufRectangle2d createMessage() { + return ProtobufRectangle2d.newInstance(); + } + + @Override + public Rectangle2d unpack(ProtobufRectangle2d msg) { + return new Rectangle2d( + Pose2d.proto.unpack(msg.getCenter()), + msg.getWidth(), + msg.getHeight() + ); + } + + @Override + public void pack(ProtobufRectangle2d msg, Rectangle2d value) { + Pose2d.proto.pack(msg.getMutableCenter(), value.getCenter()); + msg.setWidth(value.getWidth()); + msg.setHeight(value.getHeight()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rectangle2dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rectangle2dStruct.java new file mode 100644 index 00000000000..d8ab2108bcf --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rectangle2dStruct.java @@ -0,0 +1,49 @@ +package edu.wpi.first.math.geometry.struct; + +import java.nio.ByteBuffer; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rectangle2d; +import edu.wpi.first.util.struct.Struct; + +public class Rectangle2dStruct implements Struct { + @Override + public Class getTypeClass() { + return Rectangle2d.class; + } + + @Override + public String getTypeString() { + return "struct:Rectangle2d"; + } + + @Override + public int getSize() { + return Pose2d.struct.getSize() + kSizeDouble + kSizeDouble; + } + + @Override + public String getSchema() { + return "Pose2d center;double width; double height"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Pose2d.struct}; + } + + @Override + public Rectangle2d unpack(ByteBuffer bb) { + Pose2d center = Pose2d.struct.unpack(bb); + double width = bb.getDouble(); + double height = bb.getDouble(); + return new Rectangle2d(center, width, height); + } + + @Override + public void pack(ByteBuffer bb, Rectangle2d value) { + Pose2d.struct.pack(bb, value.getCenter()); + bb.putDouble(value.getWidth()); + bb.putDouble(value.getHeight()); + } +} diff --git a/wpimath/src/main/proto/geometry2d.proto b/wpimath/src/main/proto/geometry2d.proto index d52da4585a7..6fd14d0ac7f 100644 --- a/wpimath/src/main/proto/geometry2d.proto +++ b/wpimath/src/main/proto/geometry2d.proto @@ -28,3 +28,9 @@ message ProtobufTwist2d { double dy = 2; double dtheta = 3; } + +message ProtobufRectangle2d { + ProtobufPose2d center = 1; + double width = 2; + double height = 3; +} \ No newline at end of file From 065accee02bdbd467683918e4ce34d4ca3d2c62e Mon Sep 17 00:00:00 2001 From: braykoff <99614905+Braykoff@users.noreply.github.com> Date: Wed, 10 Apr 2024 21:57:41 -0400 Subject: [PATCH 02/30] Java Rectangle2d cleanup, unit tests, and headers --- .../wpi/first/math/geometry/Rectangle2d.java | 38 ++----- .../geometry/struct/Rectangle2dStruct.java | 4 + .../first/math/geometry/Rectangle2dTest.java | 104 ++++++++++++++++++ .../math/geometry/Translation2dTest.java | 11 ++ .../geometry/proto/Rectangle2dProtoTest.java | 30 +++++ .../struct/Rectangle2dStructTest.java | 36 ++++++ 6 files changed, 196 insertions(+), 27 deletions(-) create mode 100644 wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java create mode 100644 wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rectangle2dProtoTest.java create mode 100644 wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rectangle2dStructTest.java diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java index de0ab791c1e..319c2c7a81c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java @@ -1,3 +1,7 @@ +// 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. + package edu.wpi.first.math.geometry; import java.util.Objects; @@ -132,26 +136,6 @@ public boolean intersectsPoint(Translation2d point) { return false; } - /** - * Checks if a point is contained within this rectangle. - * This is exclusive, if the point lies on the perimeter it will return {@code false}. - * - * @param point The point to check. - * @return True, if this rectangle contains the point. - */ - public boolean containsPointExclusive(Translation2d point) { - // Rotate the point around the inverse of the rectangle's rotation - point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus()); - - // Check if within bounding box - return ( - point.getX() > (m_center.getX() - m_width/2.0) && - point.getX() < (m_center.getX() + m_width/2.0) && - point.getY() > (m_center.getY() - m_height/2.0) && - point.getY() < (m_center.getY() + m_height/2.0) - ); - } - /** * Checks if a point is contained within this rectangle. * This is inclusive, if the point lies on the perimeter it will return {@code true}. @@ -159,7 +143,7 @@ public boolean containsPointExclusive(Translation2d point) { * @param point The point to check. * @return True, if this rectangle contains the point or the perimeter intersects the point. */ - public boolean containsPointInclusive(Translation2d point) { + public boolean containsPoint(Translation2d point) { // Rotate the point around the inverse of the rectangle's rotation point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus()); @@ -179,19 +163,19 @@ public boolean containsPointInclusive(Translation2d point) { * @return The distance (0, if the point is on the perimeter or contained by the rectangle) */ public double distanceToPoint(Translation2d point) { - if (containsPointInclusive(point)) return 0.0; + if (containsPoint(point)) return 0.0; // Rotate the point around the inverse of the rectangle's rotation point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus()); // Find x and y distances double dx = - Math.max(m_center.getX() - m_width - point.getX(), - Math.max(0.0, point.getX() - m_center.getX() + m_width)); + Math.max(m_center.getX() - (m_width/2.0) - point.getX(), + Math.max(0.0, point.getX() - m_center.getX() - (m_width/2.0))); double dy = - Math.max(m_center.getY() - m_height - point.getY(), - Math.max(0.0, point.getY() - m_center.getY() + m_height)); + Math.max(m_center.getY() - (m_height/2.0) - point.getY(), + Math.max(0.0, point.getY() - m_center.getY() - (m_height/2.0))); // Distance formula return Math.sqrt(dx*dx + dy*dy); @@ -205,7 +189,7 @@ public double distanceToPoint(Translation2d point) { */ public Translation2d findNearestPoint(Translation2d point) { // Check if already in rectangle - if (containsPointInclusive(point)) return new Translation2d(point.getX(), point.getY()); + if (containsPoint(point)) return new Translation2d(point.getX(), point.getY()); // Rotate the point around the inverse of the rectangle's rotation point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus()); diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rectangle2dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rectangle2dStruct.java index d8ab2108bcf..76a61cc1edb 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rectangle2dStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rectangle2dStruct.java @@ -1,3 +1,7 @@ +// 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. + package edu.wpi.first.math.geometry.struct; import java.nio.ByteBuffer; diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java new file mode 100644 index 00000000000..6a9a7904658 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java @@ -0,0 +1,104 @@ +// 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. + +package edu.wpi.first.math.geometry; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import org.junit.jupiter.api.Test; + +public class Rectangle2dTest { + private static final double kEpsilon = 1E-9; + + @Test + void testNewWithCorners() { + var cornerA = new Translation2d(1.0, 2.0); + var cornerB = new Translation2d(4.0, 6.0); + + var rect = new Rectangle2d(cornerA, cornerB); + + assertAll( + () -> assertEquals(3.0, rect.getWidth()), + () -> assertEquals(4.0, rect.getHeight()), + () -> assertEquals(2.5, rect.getCenter().getX()), + () -> assertEquals(4.0, rect.getCenter().getY())); + } + + @Test + void testIntersectsPoint() { + var center = new Pose2d(4.0, 3.0, Rotation2d.fromDegrees(90.0)); + var rect = new Rectangle2d(center, 2.0, 3.0); + + assertAll( + () -> assertTrue(rect.intersectsPoint(new Translation2d(5.5, 4.0))), + () -> assertTrue(rect.containsPoint(new Translation2d(3.0, 2.0))), + () -> assertFalse(rect.containsPoint(new Translation2d(4.0, 1.5))), + () -> assertFalse(rect.intersectsPoint(new Translation2d(4.0, 3.5)))); + } + + @Test + void testContainsPoint() { + var center = new Pose2d(2.0, 3.0, Rotation2d.fromDegrees(45.0)); + var rect = new Rectangle2d(center, 3.0, 1.0); + + assertAll( + () -> assertTrue(rect.containsPoint(new Translation2d(2.0, 3.0))), + () -> assertTrue(rect.containsPoint(new Translation2d(3.0, 4.0))), + () -> assertFalse(rect.containsPoint(new Translation2d(3.0, 3.0)))); + } + + @Test + void testDistanceToPoint() { + var center = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(270.0)); + var rect = new Rectangle2d(center, 1.0, 2.0); + + var point1 = new Translation2d(2.5, 2.0); + var point2 = new Translation2d(1.0, 2.0); + var point3 = new Translation2d(1.0, 1.0); + var point4 = new Translation2d(-1.0, 2.5); + + assertAll( + () -> assertEquals(0.5, rect.distanceToPoint(point1), kEpsilon), + () -> assertEquals(0.0, rect.distanceToPoint(point2), kEpsilon), + () -> assertEquals(0.5, rect.distanceToPoint(point3), kEpsilon), + () -> assertEquals(1.0, rect.distanceToPoint(point4), kEpsilon)); + } + + @Test + void testFindNearestPoint() { + var center = new Pose2d(1.0, 1.0, Rotation2d.fromDegrees(90.0)); + var rect = new Rectangle2d(center, 3.0, 4.0); + + var point1 = new Translation2d(1.0, 3.0); + var nearestPoint1 = rect.findNearestPoint(point1); + + var point2 = new Translation2d(0.0, 0.0); + var nearestPoint2 = rect.findNearestPoint(point2); + + assertAll( + () -> assertEquals(1.0, nearestPoint1.getX(), kEpsilon), + () -> assertEquals(2.5, nearestPoint1.getY(), kEpsilon), + () -> assertEquals(0.0, nearestPoint2.getX(), kEpsilon), + () -> assertEquals(0.0, nearestPoint2.getY(), kEpsilon)); + } + + @Test + void testEquals() { + var center1 = new Pose2d(2.0, 3.0, new Rotation2d()); + var rect1 = new Rectangle2d(center1, 5.0, 3.0); + + var center2 = new Pose2d(2.0, 3.0, new Rotation2d()); + var rect2 = new Rectangle2d(center2, 5.0, 3.0); + + var center3 = new Pose2d(2.0, 3.0, new Rotation2d()); + var rect3 = new Rectangle2d(center3, 3.0, 3.0); + + assertAll( + () -> assertTrue(rect1.equals(rect2)), + () -> assertFalse(rect2.equals(rect3))); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java index 39e2073b638..70c2c336012 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java @@ -49,6 +49,17 @@ void testRotateBy() { () -> assertEquals(3.0, rotated.getY(), kEpsilon)); } + @Test + void testRotateAround() { + var original = new Translation2d(2.0, 1.0); + var other = new Translation2d(3.0, 2.0); + var rotated = original.rotateAround(other, Rotation2d.fromDegrees(180.0)); + + assertAll( + () -> assertEquals(4.0, rotated.getX(), kEpsilon), + () -> assertEquals(3.0, rotated.getY(), kEpsilon)); + } + @Test void testMultiplication() { var original = new Translation2d(3.0, 5.0); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rectangle2dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rectangle2dProtoTest.java new file mode 100644 index 00000000000..efd3440df4d --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rectangle2dProtoTest.java @@ -0,0 +1,30 @@ +// 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. + +package edu.wpi.first.math.geometry.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rectangle2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.proto.Geometry2D.ProtobufRectangle2d; + +public class Rectangle2dProtoTest { + private static final Rectangle2d DATA = + new Rectangle2d(new Pose2d(0.1, 0.2, new Rotation2d()), 4.0, 3.0); + + @Test + void testRoundtrip() { + ProtobufRectangle2d proto = Rectangle2d.proto.createMessage(); + Rectangle2d.proto.pack(proto, DATA); + + Rectangle2d data = Rectangle2d.proto.unpack(proto); + assertEquals(DATA.getCenter(), data.getCenter()); + assertEquals(DATA.getWidth(), data.getWidth()); + assertEquals(DATA.getHeight(), data.getHeight()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rectangle2dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rectangle2dStructTest.java new file mode 100644 index 00000000000..c3e09c62ac1 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rectangle2dStructTest.java @@ -0,0 +1,36 @@ +// 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. + +package edu.wpi.first.math.geometry.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import java.nio.ByteBuffer; +import java.nio.ByteOrder; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rectangle2d; +import edu.wpi.first.math.geometry.Rotation2d; + +public class Rectangle2dStructTest { + private static final Rectangle2d DATA = + new Rectangle2d( + new Pose2d(1.0, 2.0, new Rotation2d(3.1)), + 5.0, 3.0); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(Rotation2d.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + Rectangle2d.struct.pack(buffer, DATA); + buffer.rewind(); + + Rectangle2d data = Rectangle2d.struct.unpack(buffer); + assertEquals(DATA.getCenter(), data.getCenter()); + assertEquals(DATA.getWidth(), data.getWidth()); + assertEquals(DATA.getHeight(), data.getHeight()); + } +} From ce0f9d9131c6da9d34dd7c8dd62b04c1b09d0145 Mon Sep 17 00:00:00 2001 From: braykoff <99614905+Braykoff@users.noreply.github.com> Date: Wed, 10 Apr 2024 21:58:10 -0400 Subject: [PATCH 03/30] Fixed unit test error --- .../wpi/first/math/geometry/struct/Rectangle2dStructTest.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rectangle2dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rectangle2dStructTest.java index c3e09c62ac1..ad325ab5463 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rectangle2dStructTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rectangle2dStructTest.java @@ -23,7 +23,7 @@ public class Rectangle2dStructTest { @Test void testRoundtrip() { - ByteBuffer buffer = ByteBuffer.allocate(Rotation2d.struct.getSize()); + ByteBuffer buffer = ByteBuffer.allocate(Rectangle2d.struct.getSize()); buffer.order(ByteOrder.LITTLE_ENDIAN); Rectangle2d.struct.pack(buffer, DATA); buffer.rewind(); From 157b6aeb3251d100f3b6bc816f756b63e7ec3f67 Mon Sep 17 00:00:00 2001 From: braykoff <99614905+Braykoff@users.noreply.github.com> Date: Thu, 11 Apr 2024 19:37:00 -0400 Subject: [PATCH 04/30] Rectangle2d grammar fixes + initial Ellipse2d math --- .../wpi/first/math/geometry/Ellipse2d.java | 157 ++++++++++++++++++ .../wpi/first/math/geometry/Rectangle2d.java | 28 ++-- 2 files changed, 171 insertions(+), 14 deletions(-) create mode 100644 wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java new file mode 100644 index 00000000000..b651c9c4290 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java @@ -0,0 +1,157 @@ +package edu.wpi.first.math.geometry; + +/** + * Represents a 2d ellipse space containing translational, rotational, and scaling components + */ +public class Ellipse2d { + private final Pose2d m_center; + private final double m_horizontalSemiAxis, m_verticalSemiAxis; + + /** + * Constructs an ellipse from two semi-axes, a horizontal and vertical one. + * + *

The orientation of these radii may be changed via the rotational component of the center. + * + * @param center The center of the ellipse. + * @param horizontalSemiAxis The horizontal (x-axis parallel) semi-axis. + * @param semiMajorAxis The vertical (y-axis parallel) semi-axis. + */ + public Ellipse2d(Pose2d center, double horizontalSemiAxis, double verticalSemiAxis) { + // Safety check + if (horizontalSemiAxis <= 0 || verticalSemiAxis <= 0) { + throw new IllegalArgumentException("Ellipse2d semi-axes must be positive"); + } + + m_center = center; + m_horizontalSemiAxis = horizontalSemiAxis; + m_verticalSemiAxis = verticalSemiAxis; + } + + /** + * Constructs a perfectly circular ellipse with the specified radius. + * + * @param center The center of the circle. + * @param radius The radius of the circle. + */ + public Ellipse2d(Pose2d center, double radius) { + this(center, radius, radius); + } + + /** + * Returns the center of the ellipse. + * + * @return The center of the ellipse. + */ + public Pose2d getCenter() { + return m_center; + } + + /** + * Returns the horizontal semi-axis. + * + * @return The horizontal semi-axis. + */ + public double getHorizontalSemiAxis() { + return m_horizontalSemiAxis; + } + + /** + * Returns the vertical semi-axis. + * + * @return The vertical semi-axis. + */ + public double getVerticalSemiAxis() { + return m_verticalSemiAxis; + } + + /** + * 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_horizontalSemiAxis, m_verticalSemiAxis); // Major semi-axis + double b = Math.min(m_horizontalSemiAxis, m_verticalSemiAxis); // Minor semi-axis + double c = Math.sqrt(a*a - b*b); + + c = (first ? -c : c); + + if (m_horizontalSemiAxis > m_verticalSemiAxis) { + Transform2d diff = new Transform2d(c, 0.0, new Rotation2d()); + return m_center.plus(diff).getTranslation(); + } else { + Transform2d diff = new Transform2d(0.0, c, new Rotation2d()); + return m_center.plus(diff).getTranslation(); + } + } + + /** + * Transforms the center of the ellipse and returns the new ellipse. + * + * @param other The transform to transform by. + * @return The transformed ellipse. + */ + public Ellipse2d transformBy(Transform2d other) { + return new Ellipse2d( + m_center.transformBy(other), + m_horizontalSemiAxis, + m_verticalSemiAxis); + } + + + /** + * 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. + * + * 

+   * (x-h)^2 / a^2 + (y-k)^2 / b^2 = 1
+   *
+ * @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. + */ + private double solveEllipseEquation(Translation2d point) { + // Rotate the point by the inverse of the ellipse's rotation + point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus()); + + double x = point.getX() - m_center.getX(); + double y = point.getY() - m_center.getY(); + + return (x * x) / (m_horizontalSemiAxis * m_horizontalSemiAxis) + + (y * y) / (m_verticalSemiAxis * m_verticalSemiAxis); + } + + /** + * 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. + */ + public boolean intersectsPoint(Translation2d point) { + return solveEllipseEquation(point) == 1.0; + } + + /** + * Checks if a point is contained within this ellipse. + * This is inclusive, if the point lies on the circumference it will return {@code true}. + * + * @param point The point to check. + * @return True, if the point is within or on the ellipse. + */ + public boolean containsPoint(Translation2d point) { + return solveEllipseEquation(point) <= 1.0; + } + + /** + * Finds the minimum distance between the ellipse and the point. + * If the point lies within the ellipse, it returns 0.0. + * + * @param point The point to check. + * @return The distance. + */ + public double distanceToPoint(Translation2d point) { + + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java index 319c2c7a81c..06688da3221 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java @@ -65,16 +65,6 @@ public Rectangle2d(Translation2d cornerA, Translation2d cornerB) { ); } - /** - * Transforms the center of the rectangle and returns the new rectangle. - * - * @param other The transform to transform by. - * @return The transformed rectangle - */ - public Rectangle2d transformBy(Transform2d other) { - return new Rectangle2d(m_center.transformBy(other), m_width, m_height); - } - /** * Returns the center of the rectangle. * @@ -111,6 +101,16 @@ public double getHeight() { return m_height; } + /** + * Transforms the center of the rectangle and returns the new rectangle. + * + * @param other The transform to transform by. + * @return The transformed rectangle + */ + public Rectangle2d transformBy(Transform2d other) { + return new Rectangle2d(m_center.transformBy(other), m_width, m_height); + } + /** * Checks if a point is intersected by this rectangle's perimeter. * @@ -118,7 +118,7 @@ public double getHeight() { * @return True, if this rectangle's perimeter intersects the point. */ public boolean intersectsPoint(Translation2d point) { - // Rotate the point around the inverse of the rectangle's rotation + // Rotate the point by the inverse of the rectangle's rotation point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus()); // Half of width and height @@ -144,7 +144,7 @@ public boolean intersectsPoint(Translation2d point) { * @return True, if this rectangle contains the point or the perimeter intersects the point. */ public boolean containsPoint(Translation2d point) { - // Rotate the point around the inverse of the rectangle's rotation + // Rotate the point by the inverse of the rectangle's rotation point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus()); // Check if within bounding box @@ -165,7 +165,7 @@ public boolean containsPoint(Translation2d point) { public double distanceToPoint(Translation2d point) { if (containsPoint(point)) return 0.0; - // Rotate the point around the inverse of the rectangle's rotation + // Rotate the point by the inverse of the rectangle's rotation point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus()); // Find x and y distances @@ -191,7 +191,7 @@ public Translation2d findNearestPoint(Translation2d point) { // Check if already in rectangle if (containsPoint(point)) return new Translation2d(point.getX(), point.getY()); - // Rotate the point around the inverse of the rectangle's rotation + // Rotate the point by the inverse of the rectangle's rotation point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus()); // Find nearest point From c7a5190b609f37da356d2c028ea254209b394b7d Mon Sep 17 00:00:00 2001 From: braykoff <99614905+Braykoff@users.noreply.github.com> Date: Sat, 13 Apr 2024 15:41:38 -0400 Subject: [PATCH 05/30] Removed getDistance() from ellipse --- .../edu/wpi/first/math/geometry/Ellipse2d.java | 18 +++++------------- 1 file changed, 5 insertions(+), 13 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java index b651c9c4290..c12f7c38c4c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java @@ -1,9 +1,12 @@ package edu.wpi.first.math.geometry; +import edu.wpi.first.util.protobuf.ProtobufSerializable; +import edu.wpi.first.util.struct.StructSerializable; + /** * Represents a 2d ellipse space containing translational, rotational, and scaling components */ -public class Ellipse2d { +public class Ellipse2d implements ProtobufSerializable, StructSerializable { private final Pose2d m_center; private final double m_horizontalSemiAxis, m_verticalSemiAxis; @@ -135,7 +138,7 @@ public boolean intersectsPoint(Translation2d point) { /** * Checks if a point is contained within this ellipse. - * This is inclusive, if the point lies on the circumference it will return {@code true}. + * 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. @@ -143,15 +146,4 @@ public boolean intersectsPoint(Translation2d point) { public boolean containsPoint(Translation2d point) { return solveEllipseEquation(point) <= 1.0; } - - /** - * Finds the minimum distance between the ellipse and the point. - * If the point lies within the ellipse, it returns 0.0. - * - * @param point The point to check. - * @return The distance. - */ - public double distanceToPoint(Translation2d point) { - - } } From 2f64e1ef9f99bde2d72c364035fe9f89c0c2803f Mon Sep 17 00:00:00 2001 From: braykoff <99614905+Braykoff@users.noreply.github.com> Date: Sat, 13 Apr 2024 16:17:53 -0400 Subject: [PATCH 06/30] Rectangle2d reformats --- .../wpi/first/math/geometry/Rectangle2d.java | 133 +++++++++--------- .../math/geometry/proto/Rectangle2dProto.java | 4 +- .../geometry/struct/Rectangle2dStruct.java | 6 +- .../first/math/geometry/Rectangle2dTest.java | 4 +- .../geometry/proto/Rectangle2dProtoTest.java | 4 +- .../struct/Rectangle2dStructTest.java | 4 +- 6 files changed, 79 insertions(+), 76 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java index 06688da3221..9c1bc576e5a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java @@ -6,6 +6,7 @@ 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; @@ -16,24 +17,24 @@ */ public class Rectangle2d implements ProtobufSerializable, StructSerializable { private final Pose2d m_center; - private final double m_width, m_height; + 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 width The width (x size component) of the rectangle. - * @param height The height (y size component) 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. */ - public Rectangle2d(Pose2d center, double width, double height) { + public Rectangle2d(Pose2d center, double xWidth, double yWidth) { // Safety check size - if (width < 0 || height < 0) { + if (xWidth < 0 || yWidth < 0) { throw new IllegalArgumentException("Rectangle2d dimensions cannot be less than 0!"); } m_center = center; - m_width = width; - m_height = height; + m_xWidth = xWidth; + m_yWidth = yWidth; } /** @@ -41,12 +42,12 @@ public Rectangle2d(Pose2d center, double width, double height) { * height. * * @param center The center of the rectangle. - * @param width The width (x size component) of the rectangle, in unrotated coordinate frame. - * @param height The height (y size component) of the rectangle, in unrotated coordinate frame. + * @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. * @param rotation The rotation of the rectangle. */ - public Rectangle2d(Translation2d center, double width, double height, Rotation2d rotation) { - this(new Pose2d(center, rotation), width, height); + public Rectangle2d(Translation2d center, double xWidth, double yWidth, Rotation2d rotation) { + this(new Pose2d(center, rotation), xWidth, yWidth); } /** @@ -84,21 +85,21 @@ public Rotation2d getRotation() { } /** - * Returns the width (x size component) of the rectangle. + * Returns the x size component of the rectangle. * - * @return The width (x size component) of the rectangle. + * @return The x size component of the rectangle. */ - public double getWidth() { - return m_width; + public double getXWidth() { + return m_xWidth; } /** - * Returns the height (y size component) of the rectangle. + * Returns the y size component of the rectangle. * - * @return The height (y size component) of the rectangle. + * @return The y size component of the rectangle. */ - public double getHeight() { - return m_height; + public double getYWidth() { + return m_yWidth; } /** @@ -108,81 +109,83 @@ public double getHeight() { * @return The transformed rectangle */ public Rectangle2d transformBy(Transform2d other) { - return new Rectangle2d(m_center.transformBy(other), m_width, m_height); + return new Rectangle2d(m_center.transformBy(other), m_xWidth, m_yWidth); } /** - * Checks if a point is intersected by this rectangle's perimeter. + * Rotates the center of the rectangle and returns the new rectangle. + * + * @param other The rotation to transform by. + * @return The rotated rectangle. + */ + public Rectangle2d rotateBy(Rotation2d other) { + return new 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 this rectangle's perimeter intersects the point. + * @return True, if the rectangle's perimeter intersects the point. */ public boolean intersectsPoint(Translation2d point) { - // Rotate the point by the inverse of the rectangle's rotation - point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus()); - - // Half of width and height - double w = m_width/2.0; - double h = m_height/2.0; - - if (point.getX() == (m_center.getX() - w) || point.getX() == (m_center.getX() + w)) { - // Rests on left/right perimeter - return (point.getY() >= (m_center.getY() - h) && point.getY() <= (m_center.getY() - h)); - } else if (point.getY() == (m_center.getY() - h) || point.getY() == (m_center.getY() + h)) { - // Rest on top/bottom perimeter - return (point.getX() >= (m_center.getX() - w) && point.getX() <= (m_center.getX() + h)); + // Move the point into the rectangle's coordinate frame + point = point.minus(m_center.getTranslation()); + point = point.rotateBy(m_center.getRotation().unaryMinus()); + + if (Math.abs(point.getX()) == m_xWidth / 2.0) { + // Point rests on left/right perimeter + return (Math.abs(point.getY()) <= m_yWidth / 2.0); + } else if (Math.abs(point.getY()) == m_yWidth / 2.0) { + // Point rests on top/bottom perimeter + return (Math.abs(point.getX()) <= m_xWidth / 2.0); } return false; } /** - * Checks if a point is contained within this rectangle. - * This is inclusive, if the point lies on the perimeter it will return {@code 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 this rectangle contains the point or the perimeter intersects the point. + * @return True, if the rectangle contains the point or the perimeter intersects the point. */ public boolean containsPoint(Translation2d point) { - // Rotate the point by the inverse of the rectangle's rotation + // 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_width/2.0) && - point.getX() <= (m_center.getX() + m_width/2.0) && - point.getY() >= (m_center.getY() - m_height/2.0) && - point.getY() <= (m_center.getY() + m_height/2.0) - ); + 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 on the perimeter or contained by the rectangle) + * @return The distance (0, if the point is contained by the rectangle) */ public double distanceToPoint(Translation2d point) { if (containsPoint(point)) return 0.0; - // Rotate the point by the inverse of the rectangle's rotation - point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus()); + // Move the point into the rectangle's coordinate frame + point = point.minus(m_center.getTranslation()); + point = point.rotateBy(m_center.getRotation().unaryMinus()); // Find x and y distances - double dx = - Math.max(m_center.getX() - (m_width/2.0) - point.getX(), - Math.max(0.0, point.getX() - m_center.getX() - (m_width/2.0))); - - double dy = - Math.max(m_center.getY() - (m_height/2.0) - point.getY(), - Math.max(0.0, point.getY() - m_center.getY() - (m_height/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.sqrt(dx*dx + dy*dy); + return Math.hypot(dx, dy); } /** - * Returns the nearest point that is contained within this rectangle. + * 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. @@ -196,8 +199,8 @@ public Translation2d findNearestPoint(Translation2d point) { // Find nearest point point = new Translation2d( - Math.max(m_center.getX()-m_width/2.0, Math.min(point.getX(), m_center.getX()+m_width/2.0)), - Math.max(m_center.getY()-m_height/2.0, Math.min(point.getY(), m_center.getY()+m_height/2.0)) + 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 @@ -206,10 +209,10 @@ public Translation2d findNearestPoint(Translation2d point) { @Override public String toString() { - return String.format("Rectangle2d(Center: %s, W: %.2f, H: %.2f)", - m_center.toString(), - m_width, - m_height + return String.format("Rectangle2d(center: %s, x: %.2f, y: %.2f)", + m_center, + m_xWidth, + m_yWidth ); } @@ -223,15 +226,15 @@ public String toString() { public boolean equals(Object obj) { if (obj instanceof Rectangle2d) { return ((Rectangle2d) obj).getCenter().equals(m_center) - && ((Rectangle2d) obj).getWidth() == m_width - && ((Rectangle2d) obj).getHeight() == m_height; + && ((Rectangle2d) obj).getXWidth() == m_xWidth + && ((Rectangle2d) obj).getYWidth() == m_yWidth; } return false; } @Override public int hashCode() { - return Objects.hash(m_center, m_width, m_height); + return Objects.hash(m_center, m_xWidth, m_yWidth); } /** Rectangle2d protobuf for serialization. */ diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rectangle2dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rectangle2dProto.java index 18c481cf5d8..395c50ce16a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rectangle2dProto.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rectangle2dProto.java @@ -43,7 +43,7 @@ public Rectangle2d unpack(ProtobufRectangle2d msg) { @Override public void pack(ProtobufRectangle2d msg, Rectangle2d value) { Pose2d.proto.pack(msg.getMutableCenter(), value.getCenter()); - msg.setWidth(value.getWidth()); - msg.setHeight(value.getHeight()); + msg.setWidth(value.getXWidth()); + msg.setHeight(value.getYWidth()); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rectangle2dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rectangle2dStruct.java index 76a61cc1edb..58c7f33bf94 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rectangle2dStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rectangle2dStruct.java @@ -28,7 +28,7 @@ public int getSize() { @Override public String getSchema() { - return "Pose2d center;double width; double height"; + return "Pose2d center;double xWidth; double yWidth"; } @Override @@ -47,7 +47,7 @@ public Rectangle2d unpack(ByteBuffer bb) { @Override public void pack(ByteBuffer bb, Rectangle2d value) { Pose2d.struct.pack(bb, value.getCenter()); - bb.putDouble(value.getWidth()); - bb.putDouble(value.getHeight()); + bb.putDouble(value.getXWidth()); + bb.putDouble(value.getYWidth()); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java index 6a9a7904658..1c8c5ca2009 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java @@ -22,8 +22,8 @@ void testNewWithCorners() { var rect = new Rectangle2d(cornerA, cornerB); assertAll( - () -> assertEquals(3.0, rect.getWidth()), - () -> assertEquals(4.0, rect.getHeight()), + () -> assertEquals(3.0, rect.getXWidth()), + () -> assertEquals(4.0, rect.getYWidth()), () -> assertEquals(2.5, rect.getCenter().getX()), () -> assertEquals(4.0, rect.getCenter().getY())); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rectangle2dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rectangle2dProtoTest.java index efd3440df4d..a22120d3170 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rectangle2dProtoTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rectangle2dProtoTest.java @@ -24,7 +24,7 @@ void testRoundtrip() { Rectangle2d data = Rectangle2d.proto.unpack(proto); assertEquals(DATA.getCenter(), data.getCenter()); - assertEquals(DATA.getWidth(), data.getWidth()); - assertEquals(DATA.getHeight(), data.getHeight()); + assertEquals(DATA.getXWidth(), data.getXWidth()); + assertEquals(DATA.getYWidth(), data.getYWidth()); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rectangle2dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rectangle2dStructTest.java index ad325ab5463..27e4bb844f5 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rectangle2dStructTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rectangle2dStructTest.java @@ -30,7 +30,7 @@ void testRoundtrip() { Rectangle2d data = Rectangle2d.struct.unpack(buffer); assertEquals(DATA.getCenter(), data.getCenter()); - assertEquals(DATA.getWidth(), data.getWidth()); - assertEquals(DATA.getHeight(), data.getHeight()); + assertEquals(DATA.getXWidth(), data.getXWidth()); + assertEquals(DATA.getYWidth(), data.getYWidth()); } } From 42603c411c7f990af1b8692a5553d32981dfa844 Mon Sep 17 00:00:00 2001 From: braykoff <99614905+Braykoff@users.noreply.github.com> Date: Sat, 13 Apr 2024 16:32:20 -0400 Subject: [PATCH 07/30] Reformats and serialization --- .../edu/wpi/first/math/proto/Geometry2D.java | 598 +++++++++++++++--- .../wpi/first/math/geometry/Ellipse2d.java | 96 ++- .../wpi/first/math/geometry/Rectangle2d.java | 5 +- .../math/geometry/proto/Ellipse2dProto.java | 49 ++ .../math/geometry/proto/Rectangle2dProto.java | 8 +- .../math/geometry/struct/Ellipse2dStruct.java | 53 ++ wpimath/src/main/proto/geometry2d.proto | 10 +- 7 files changed, 704 insertions(+), 115 deletions(-) create mode 100644 wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Ellipse2dProto.java create mode 100644 wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Ellipse2dStruct.java diff --git a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Geometry2D.java b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Geometry2D.java index a33d3767841..c0dc337008f 100644 --- a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Geometry2D.java +++ b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Geometry2D.java @@ -18,7 +18,7 @@ import us.hebi.quickbuf.RepeatedByte; public final class Geometry2D { - private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(1565, + private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(1889, "ChBnZW9tZXRyeTJkLnByb3RvEgl3cGkucHJvdG8iMwoVUHJvdG9idWZUcmFuc2xhdGlvbjJkEgwKAXgY" + "ASABKAFSAXgSDAoBeRgCIAEoAVIBeSIqChJQcm90b2J1ZlJvdGF0aW9uMmQSFAoFdmFsdWUYASABKAFS" + "BXZhbHVlIo8BCg5Qcm90b2J1ZlBvc2UyZBJCCgt0cmFuc2xhdGlvbhgBIAEoCzIgLndwaS5wcm90by5Q" + @@ -27,25 +27,30 @@ public final class Geometry2D { "dHJhbnNsYXRpb24YASABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUgt0cmFuc2xh" + "dGlvbhI5Cghyb3RhdGlvbhgCIAEoCzIdLndwaS5wcm90by5Qcm90b2J1ZlJvdGF0aW9uMmRSCHJvdGF0" + "aW9uIkkKD1Byb3RvYnVmVHdpc3QyZBIOCgJkeBgBIAEoAVICZHgSDgoCZHkYAiABKAFSAmR5EhYKBmR0" + - "aGV0YRgDIAEoAVIGZHRoZXRhInYKE1Byb3RvYnVmUmVjdGFuZ2xlMmQSMQoGY2VudGVyGAEgASgLMhku" + - "d3BpLnByb3RvLlByb3RvYnVmUG9zZTJkUgZjZW50ZXISFAoFd2lkdGgYAiABKAFSBXdpZHRoEhYKBmhl" + - "aWdodBgDIAEoAVIGaGVpZ2h0QhoKGGVkdS53cGkuZmlyc3QubWF0aC5wcm90b0qMBwoGEgQAACMBCggK" + - "AQwSAwAAEgoICgECEgMCABIKCAoBCBIDBAAxCgkKAggBEgMEADEKCgoCBAASBAYACQEKCgoDBAABEgMG" + - "CB0KCwoEBAACABIDBwIPCgwKBQQAAgAFEgMHAggKDAoFBAACAAESAwcJCgoMCgUEAAIAAxIDBw0OCgsK" + - "BAQAAgESAwgCDwoMCgUEAAIBBRIDCAIICgwKBQQAAgEBEgMICQoKDAoFBAACAQMSAwgNDgoKCgIEARIE" + - "CwANAQoKCgMEAQESAwsIGgoLCgQEAQIAEgMMAhMKDAoFBAECAAUSAwwCCAoMCgUEAQIAARIDDAkOCgwK" + - "BQQBAgADEgMMERIKCgoCBAISBA8AEgEKCgoDBAIBEgMPCBYKCwoEBAICABIDEAIoCgwKBQQCAgAGEgMQ" + - "AhcKDAoFBAICAAESAxAYIwoMCgUEAgIAAxIDECYnCgsKBAQCAgESAxECIgoMCgUEAgIBBhIDEQIUCgwK" + - "BQQCAgEBEgMRFR0KDAoFBAICAQMSAxEgIQoKCgIEAxIEFAAXAQoKCgMEAwESAxQIGwoLCgQEAwIAEgMV" + - "AigKDAoFBAMCAAYSAxUCFwoMCgUEAwIAARIDFRgjCgwKBQQDAgADEgMVJicKCwoEBAMCARIDFgIiCgwK" + - "BQQDAgEGEgMWAhQKDAoFBAMCAQESAxYVHQoMCgUEAwIBAxIDFiAhCgoKAgQEEgQZAB0BCgoKAwQEARID", - "GQgXCgsKBAQEAgASAxoCEAoMCgUEBAIABRIDGgIICgwKBQQEAgABEgMaCQsKDAoFBAQCAAMSAxoODwoL" + - "CgQEBAIBEgMbAhAKDAoFBAQCAQUSAxsCCAoMCgUEBAIBARIDGwkLCgwKBQQEAgEDEgMbDg8KCwoEBAQC" + - "AhIDHAIUCgwKBQQEAgIFEgMcAggKDAoFBAQCAgESAxwJDwoMCgUEBAICAxIDHBITCgoKAgQFEgQfACMB" + - "CgoKAwQFARIDHwgbCgsKBAQFAgASAyACHAoMCgUEBQIABhIDIAIQCgwKBQQFAgABEgMgERcKDAoFBAUC" + - "AAMSAyAaGwoLCgQEBQIBEgMhAhMKDAoFBAUCAQUSAyECCAoMCgUEBQIBARIDIQkOCgwKBQQFAgEDEgMh" + - "ERIKCwoEBAUCAhIDIgIUCgwKBQQFAgIFEgMiAggKDAoFBAUCAgESAyIJDwoMCgUEBQICAxIDIhITYgZw" + - "cm90bzM="); + "aGV0YRgDIAEoAVIGZHRoZXRhIngKE1Byb3RvYnVmUmVjdGFuZ2xlMmQSMQoGY2VudGVyGAEgASgLMhku" + + "d3BpLnByb3RvLlByb3RvYnVmUG9zZTJkUgZjZW50ZXISFgoGeFdpZHRoGAIgASgBUgZ4V2lkdGgSFgoG" + + "eVdpZHRoGAMgASgBUgZ5V2lkdGgiggEKEVByb3RvYnVmRWxsaXBzZTJkEjEKBmNlbnRlchgBIAEoCzIZ" + + "LndwaS5wcm90by5Qcm90b2J1ZlBvc2UyZFIGY2VudGVyEhwKCXhTZW1pQXhpcxgCIAEoAVIJeFNlbWlB" + + "eGlzEhwKCXlTZW1pQXhpcxgDIAEoAVIJeVNlbWlBeGlzQhoKGGVkdS53cGkuZmlyc3QubWF0aC5wcm90" + + "b0rJCAoGEgQAACkBCggKAQwSAwAAEgoICgECEgMCABIKCAoBCBIDBAAxCgkKAggBEgMEADEKCgoCBAAS" + + "BAYACQEKCgoDBAABEgMGCB0KCwoEBAACABIDBwIPCgwKBQQAAgAFEgMHAggKDAoFBAACAAESAwcJCgoM" + + "CgUEAAIAAxIDBw0OCgsKBAQAAgESAwgCDwoMCgUEAAIBBRIDCAIICgwKBQQAAgEBEgMICQoKDAoFBAAC" + + "AQMSAwgNDgoKCgIEARIECwANAQoKCgMEAQESAwsIGgoLCgQEAQIAEgMMAhMKDAoFBAECAAUSAwwCCAoM" + + "CgUEAQIAARIDDAkOCgwKBQQBAgADEgMMERIKCgoCBAISBA8AEgEKCgoDBAIBEgMPCBYKCwoEBAICABID" + + "EAIoCgwKBQQCAgAGEgMQAhcKDAoFBAICAAESAxAYIwoMCgUEAgIAAxIDECYnCgsKBAQCAgESAxECIgoM" + + "CgUEAgIBBhIDEQIUCgwKBQQCAgEBEgMRFR0KDAoFBAICAQMSAxEgIQoKCgIEAxIEFAAXAQoKCgMEAwES", + "AxQIGwoLCgQEAwIAEgMVAigKDAoFBAMCAAYSAxUCFwoMCgUEAwIAARIDFRgjCgwKBQQDAgADEgMVJicK" + + "CwoEBAMCARIDFgIiCgwKBQQDAgEGEgMWAhQKDAoFBAMCAQESAxYVHQoMCgUEAwIBAxIDFiAhCgoKAgQE" + + "EgQZAB0BCgoKAwQEARIDGQgXCgsKBAQEAgASAxoCEAoMCgUEBAIABRIDGgIICgwKBQQEAgABEgMaCQsK" + + "DAoFBAQCAAMSAxoODwoLCgQEBAIBEgMbAhAKDAoFBAQCAQUSAxsCCAoMCgUEBAIBARIDGwkLCgwKBQQE" + + "AgEDEgMbDg8KCwoEBAQCAhIDHAIUCgwKBQQEAgIFEgMcAggKDAoFBAQCAgESAxwJDwoMCgUEBAICAxID" + + "HBITCgoKAgQFEgQfACMBCgoKAwQFARIDHwgbCgsKBAQFAgASAyACHAoMCgUEBQIABhIDIAIQCgwKBQQF" + + "AgABEgMgERcKDAoFBAUCAAMSAyAaGwoLCgQEBQIBEgMhAhQKDAoFBAUCAQUSAyECCAoMCgUEBQIBARID" + + "IQkPCgwKBQQFAgEDEgMhEhMKCwoEBAUCAhIDIgIUCgwKBQQFAgIFEgMiAggKDAoFBAUCAgESAyIJDwoM" + + "CgUEBQICAxIDIhITCgoKAgQGEgQlACkBCgoKAwQGARIDJQgZCgsKBAQGAgASAyYCHAoMCgUEBgIABhID" + + "JgIQCgwKBQQGAgABEgMmERcKDAoFBAYCAAMSAyYaGwoLCgQEBgIBEgMnAhcKDAoFBAYCAQUSAycCCAoM" + + "CgUEBgIBARIDJwkSCgwKBQQGAgEDEgMnFRYKCwoEBAYCAhIDKAIXCgwKBQQGAgIFEgMoAggKDAoFBAYC" + + "AgESAygJEgoMCgUEBgICAxIDKBUWYgZwcm90bzM="); static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("geometry2d.proto", "wpi.proto", descriptorData); @@ -59,7 +64,9 @@ public final class Geometry2D { static final Descriptors.Descriptor wpi_proto_ProtobufTwist2d_descriptor = descriptor.internalContainedType(425, 73, "ProtobufTwist2d", "wpi.proto.ProtobufTwist2d"); - static final Descriptors.Descriptor wpi_proto_ProtobufRectangle2d_descriptor = descriptor.internalContainedType(500, 118, "ProtobufRectangle2d", "wpi.proto.ProtobufRectangle2d"); + static final Descriptors.Descriptor wpi_proto_ProtobufRectangle2d_descriptor = descriptor.internalContainedType(500, 120, "ProtobufRectangle2d", "wpi.proto.ProtobufRectangle2d"); + + static final Descriptors.Descriptor wpi_proto_ProtobufEllipse2d_descriptor = descriptor.internalContainedType(623, 130, "ProtobufEllipse2d", "wpi.proto.ProtobufEllipse2d"); /** * @return this proto file's descriptor. @@ -1819,14 +1826,14 @@ public static final class ProtobufRectangle2d extends ProtoMessageoptional double width = 2; + * optional double xWidth = 2; */ - private double width; + private double xWidth; /** - * optional double height = 3; + * optional double yWidth = 3; */ - private double height; + private double yWidth; /** * optional .wpi.proto.ProtobufPose2d center = 1; @@ -1844,76 +1851,76 @@ public static ProtobufRectangle2d newInstance() { } /** - * optional double width = 2; - * @return whether the width field is set + * optional double xWidth = 2; + * @return whether the xWidth field is set */ - public boolean hasWidth() { + public boolean hasXWidth() { return (bitField0_ & 0x00000001) != 0; } /** - * optional double width = 2; + * optional double xWidth = 2; * @return this */ - public ProtobufRectangle2d clearWidth() { + public ProtobufRectangle2d clearXWidth() { bitField0_ &= ~0x00000001; - width = 0D; + xWidth = 0D; return this; } /** - * optional double width = 2; - * @return the width + * optional double xWidth = 2; + * @return the xWidth */ - public double getWidth() { - return width; + public double getXWidth() { + return xWidth; } /** - * optional double width = 2; - * @param value the width to set + * optional double xWidth = 2; + * @param value the xWidth to set * @return this */ - public ProtobufRectangle2d setWidth(final double value) { + public ProtobufRectangle2d setXWidth(final double value) { bitField0_ |= 0x00000001; - width = value; + xWidth = value; return this; } /** - * optional double height = 3; - * @return whether the height field is set + * optional double yWidth = 3; + * @return whether the yWidth field is set */ - public boolean hasHeight() { + public boolean hasYWidth() { return (bitField0_ & 0x00000002) != 0; } /** - * optional double height = 3; + * optional double yWidth = 3; * @return this */ - public ProtobufRectangle2d clearHeight() { + public ProtobufRectangle2d clearYWidth() { bitField0_ &= ~0x00000002; - height = 0D; + yWidth = 0D; return this; } /** - * optional double height = 3; - * @return the height + * optional double yWidth = 3; + * @return the yWidth */ - public double getHeight() { - return height; + public double getYWidth() { + return yWidth; } /** - * optional double height = 3; - * @param value the height to set + * optional double yWidth = 3; + * @param value the yWidth to set * @return this */ - public ProtobufRectangle2d setHeight(final double value) { + public ProtobufRectangle2d setYWidth(final double value) { bitField0_ |= 0x00000002; - height = value; + yWidth = value; return this; } @@ -1979,8 +1986,8 @@ public ProtobufRectangle2d copyFrom(final ProtobufRectangle2d other) { cachedSize = other.cachedSize; if ((bitField0_ | other.bitField0_) != 0) { bitField0_ = other.bitField0_; - width = other.width; - height = other.height; + xWidth = other.xWidth; + yWidth = other.yWidth; center.copyFrom(other.center); } return this; @@ -1992,11 +1999,11 @@ public ProtobufRectangle2d mergeFrom(final ProtobufRectangle2d other) { return this; } cachedSize = -1; - if (other.hasWidth()) { - setWidth(other.width); + if (other.hasXWidth()) { + setXWidth(other.xWidth); } - if (other.hasHeight()) { - setHeight(other.height); + if (other.hasYWidth()) { + setYWidth(other.yWidth); } if (other.hasCenter()) { getMutableCenter().mergeFrom(other.center); @@ -2011,8 +2018,8 @@ public ProtobufRectangle2d clear() { } cachedSize = -1; bitField0_ = 0; - width = 0D; - height = 0D; + xWidth = 0D; + yWidth = 0D; center.clear(); return this; } @@ -2038,8 +2045,8 @@ public boolean equals(Object o) { } ProtobufRectangle2d other = (ProtobufRectangle2d) o; return bitField0_ == other.bitField0_ - && (!hasWidth() || ProtoUtil.isEqual(width, other.width)) - && (!hasHeight() || ProtoUtil.isEqual(height, other.height)) + && (!hasXWidth() || ProtoUtil.isEqual(xWidth, other.xWidth)) + && (!hasYWidth() || ProtoUtil.isEqual(yWidth, other.yWidth)) && (!hasCenter() || center.equals(other.center)); } @@ -2047,11 +2054,11 @@ public boolean equals(Object o) { public void writeTo(final ProtoSink output) throws IOException { if ((bitField0_ & 0x00000001) != 0) { output.writeRawByte((byte) 17); - output.writeDoubleNoTag(width); + output.writeDoubleNoTag(xWidth); } if ((bitField0_ & 0x00000002) != 0) { output.writeRawByte((byte) 25); - output.writeDoubleNoTag(height); + output.writeDoubleNoTag(yWidth); } if ((bitField0_ & 0x00000004) != 0) { output.writeRawByte((byte) 10); @@ -2082,8 +2089,8 @@ public ProtobufRectangle2d mergeFrom(final ProtoSource input) throws IOException while (true) { switch (tag) { case 17: { - // width - width = input.readDouble(); + // xWidth + xWidth = input.readDouble(); bitField0_ |= 0x00000001; tag = input.readTag(); if (tag != 25) { @@ -2091,8 +2098,8 @@ public ProtobufRectangle2d mergeFrom(final ProtoSource input) throws IOException } } case 25: { - // height - height = input.readDouble(); + // yWidth + yWidth = input.readDouble(); bitField0_ |= 0x00000002; tag = input.readTag(); if (tag != 10) { @@ -2126,10 +2133,10 @@ public ProtobufRectangle2d mergeFrom(final ProtoSource input) throws IOException public void writeTo(final JsonSink output) throws IOException { output.beginObject(); if ((bitField0_ & 0x00000001) != 0) { - output.writeDouble(FieldNames.width, width); + output.writeDouble(FieldNames.xWidth, xWidth); } if ((bitField0_ & 0x00000002) != 0) { - output.writeDouble(FieldNames.height, height); + output.writeDouble(FieldNames.yWidth, yWidth); } if ((bitField0_ & 0x00000004) != 0) { output.writeMessage(FieldNames.center, center); @@ -2144,10 +2151,10 @@ public ProtobufRectangle2d mergeFrom(final JsonSource input) throws IOException } while (!input.isAtEnd()) { switch (input.readFieldHash()) { - case 113126854: { - if (input.isAtField(FieldNames.width)) { + case -775894994: { + if (input.isAtField(FieldNames.xWidth)) { if (!input.trySkipNullValue()) { - width = input.readDouble(); + xWidth = input.readDouble(); bitField0_ |= 0x00000001; } } else { @@ -2155,10 +2162,10 @@ public ProtobufRectangle2d mergeFrom(final JsonSource input) throws IOException } break; } - case -1221029593: { - if (input.isAtField(FieldNames.height)) { + case -747265843: { + if (input.isAtField(FieldNames.yWidth)) { if (!input.trySkipNullValue()) { - height = input.readDouble(); + yWidth = input.readDouble(); bitField0_ |= 0x00000002; } } else { @@ -2237,9 +2244,442 @@ public ProtobufRectangle2d create() { * Contains name constants used for serializing JSON */ static class FieldNames { - static final FieldName width = FieldName.forField("width"); + static final FieldName xWidth = FieldName.forField("xWidth"); + + static final FieldName yWidth = FieldName.forField("yWidth"); + + static final FieldName center = FieldName.forField("center"); + } + } + + /** + * Protobuf type {@code ProtobufEllipse2d} + */ + public static final class ProtobufEllipse2d extends ProtoMessage implements Cloneable { + private static final long serialVersionUID = 0L; + + /** + * optional double xSemiAxis = 2; + */ + private double xSemiAxis; + + /** + * optional double ySemiAxis = 3; + */ + private double ySemiAxis; + + /** + * optional .wpi.proto.ProtobufPose2d center = 1; + */ + private final ProtobufPose2d center = ProtobufPose2d.newInstance(); + + private ProtobufEllipse2d() { + } + + /** + * @return a new empty instance of {@code ProtobufEllipse2d} + */ + public static ProtobufEllipse2d newInstance() { + return new ProtobufEllipse2d(); + } + + /** + * optional double xSemiAxis = 2; + * @return whether the xSemiAxis field is set + */ + public boolean hasXSemiAxis() { + return (bitField0_ & 0x00000001) != 0; + } + + /** + * optional double xSemiAxis = 2; + * @return this + */ + public ProtobufEllipse2d clearXSemiAxis() { + bitField0_ &= ~0x00000001; + xSemiAxis = 0D; + return this; + } + + /** + * optional double xSemiAxis = 2; + * @return the xSemiAxis + */ + public double getXSemiAxis() { + return xSemiAxis; + } + + /** + * optional double xSemiAxis = 2; + * @param value the xSemiAxis to set + * @return this + */ + public ProtobufEllipse2d setXSemiAxis(final double value) { + bitField0_ |= 0x00000001; + xSemiAxis = value; + return this; + } + + /** + * optional double ySemiAxis = 3; + * @return whether the ySemiAxis field is set + */ + public boolean hasYSemiAxis() { + return (bitField0_ & 0x00000002) != 0; + } + + /** + * optional double ySemiAxis = 3; + * @return this + */ + public ProtobufEllipse2d clearYSemiAxis() { + bitField0_ &= ~0x00000002; + ySemiAxis = 0D; + return this; + } + + /** + * optional double ySemiAxis = 3; + * @return the ySemiAxis + */ + public double getYSemiAxis() { + return ySemiAxis; + } + + /** + * optional double ySemiAxis = 3; + * @param value the ySemiAxis to set + * @return this + */ + public ProtobufEllipse2d setYSemiAxis(final double value) { + bitField0_ |= 0x00000002; + ySemiAxis = value; + return this; + } + + /** + * optional .wpi.proto.ProtobufPose2d center = 1; + * @return whether the center field is set + */ + public boolean hasCenter() { + return (bitField0_ & 0x00000004) != 0; + } + + /** + * optional .wpi.proto.ProtobufPose2d center = 1; + * @return this + */ + public ProtobufEllipse2d clearCenter() { + bitField0_ &= ~0x00000004; + center.clear(); + return this; + } + + /** + * optional .wpi.proto.ProtobufPose2d center = 1; + * + * This method returns the internal storage object without modifying any has state. + * The returned object should not be modified and be treated as read-only. + * + * Use {@link #getMutableCenter()} if you want to modify it. + * + * @return internal storage object for reading + */ + public ProtobufPose2d getCenter() { + return center; + } + + /** + * optional .wpi.proto.ProtobufPose2d center = 1; + * + * This method returns the internal storage object and sets the corresponding + * has state. The returned object will become part of this message and its + * contents may be modified as long as the has state is not cleared. + * + * @return internal storage object for modifications + */ + public ProtobufPose2d getMutableCenter() { + bitField0_ |= 0x00000004; + return center; + } + + /** + * optional .wpi.proto.ProtobufPose2d center = 1; + * @param value the center to set + * @return this + */ + public ProtobufEllipse2d setCenter(final ProtobufPose2d value) { + bitField0_ |= 0x00000004; + center.copyFrom(value); + return this; + } + + @Override + public ProtobufEllipse2d copyFrom(final ProtobufEllipse2d other) { + cachedSize = other.cachedSize; + if ((bitField0_ | other.bitField0_) != 0) { + bitField0_ = other.bitField0_; + xSemiAxis = other.xSemiAxis; + ySemiAxis = other.ySemiAxis; + center.copyFrom(other.center); + } + return this; + } + + @Override + public ProtobufEllipse2d mergeFrom(final ProtobufEllipse2d other) { + if (other.isEmpty()) { + return this; + } + cachedSize = -1; + if (other.hasXSemiAxis()) { + setXSemiAxis(other.xSemiAxis); + } + if (other.hasYSemiAxis()) { + setYSemiAxis(other.ySemiAxis); + } + if (other.hasCenter()) { + getMutableCenter().mergeFrom(other.center); + } + return this; + } + + @Override + public ProtobufEllipse2d clear() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + xSemiAxis = 0D; + ySemiAxis = 0D; + center.clear(); + return this; + } + + @Override + public ProtobufEllipse2d clearQuick() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + center.clearQuick(); + return this; + } + + @Override + public boolean equals(Object o) { + if (o == this) { + return true; + } + if (!(o instanceof ProtobufEllipse2d)) { + return false; + } + ProtobufEllipse2d other = (ProtobufEllipse2d) o; + return bitField0_ == other.bitField0_ + && (!hasXSemiAxis() || ProtoUtil.isEqual(xSemiAxis, other.xSemiAxis)) + && (!hasYSemiAxis() || ProtoUtil.isEqual(ySemiAxis, other.ySemiAxis)) + && (!hasCenter() || center.equals(other.center)); + } + + @Override + public void writeTo(final ProtoSink output) throws IOException { + if ((bitField0_ & 0x00000001) != 0) { + output.writeRawByte((byte) 17); + output.writeDoubleNoTag(xSemiAxis); + } + if ((bitField0_ & 0x00000002) != 0) { + output.writeRawByte((byte) 25); + output.writeDoubleNoTag(ySemiAxis); + } + if ((bitField0_ & 0x00000004) != 0) { + output.writeRawByte((byte) 10); + output.writeMessageNoTag(center); + } + } + + @Override + protected int computeSerializedSize() { + int size = 0; + if ((bitField0_ & 0x00000001) != 0) { + size += 9; + } + if ((bitField0_ & 0x00000002) != 0) { + size += 9; + } + if ((bitField0_ & 0x00000004) != 0) { + size += 1 + ProtoSink.computeMessageSizeNoTag(center); + } + return size; + } + + @Override + @SuppressWarnings("fallthrough") + public ProtobufEllipse2d mergeFrom(final ProtoSource input) throws IOException { + // Enabled Fall-Through Optimization (QuickBuffers) + int tag = input.readTag(); + while (true) { + switch (tag) { + case 17: { + // xSemiAxis + xSemiAxis = input.readDouble(); + bitField0_ |= 0x00000001; + tag = input.readTag(); + if (tag != 25) { + break; + } + } + case 25: { + // ySemiAxis + ySemiAxis = input.readDouble(); + bitField0_ |= 0x00000002; + tag = input.readTag(); + if (tag != 10) { + break; + } + } + case 10: { + // center + input.readMessage(center); + bitField0_ |= 0x00000004; + tag = input.readTag(); + if (tag != 0) { + break; + } + } + case 0: { + return this; + } + default: { + if (!input.skipField(tag)) { + return this; + } + tag = input.readTag(); + break; + } + } + } + } + + @Override + public void writeTo(final JsonSink output) throws IOException { + output.beginObject(); + if ((bitField0_ & 0x00000001) != 0) { + output.writeDouble(FieldNames.xSemiAxis, xSemiAxis); + } + if ((bitField0_ & 0x00000002) != 0) { + output.writeDouble(FieldNames.ySemiAxis, ySemiAxis); + } + if ((bitField0_ & 0x00000004) != 0) { + output.writeMessage(FieldNames.center, center); + } + output.endObject(); + } + + @Override + public ProtobufEllipse2d mergeFrom(final JsonSource input) throws IOException { + if (!input.beginObject()) { + return this; + } + while (!input.isAtEnd()) { + switch (input.readFieldHash()) { + case -858640185: { + if (input.isAtField(FieldNames.xSemiAxis)) { + if (!input.trySkipNullValue()) { + xSemiAxis = input.readDouble(); + bitField0_ |= 0x00000001; + } + } else { + input.skipUnknownField(); + } + break; + } + case 1628872648: { + if (input.isAtField(FieldNames.ySemiAxis)) { + if (!input.trySkipNullValue()) { + ySemiAxis = input.readDouble(); + bitField0_ |= 0x00000002; + } + } else { + input.skipUnknownField(); + } + break; + } + case -1364013995: { + if (input.isAtField(FieldNames.center)) { + if (!input.trySkipNullValue()) { + input.readMessage(center); + bitField0_ |= 0x00000004; + } + } else { + input.skipUnknownField(); + } + break; + } + default: { + input.skipUnknownField(); + break; + } + } + } + input.endObject(); + return this; + } + + @Override + public ProtobufEllipse2d clone() { + return new ProtobufEllipse2d().copyFrom(this); + } + + @Override + public boolean isEmpty() { + return ((bitField0_) == 0); + } + + public static ProtobufEllipse2d parseFrom(final byte[] data) throws + InvalidProtocolBufferException { + return ProtoMessage.mergeFrom(new ProtobufEllipse2d(), data).checkInitialized(); + } + + public static ProtobufEllipse2d parseFrom(final ProtoSource input) throws IOException { + return ProtoMessage.mergeFrom(new ProtobufEllipse2d(), input).checkInitialized(); + } + + public static ProtobufEllipse2d parseFrom(final JsonSource input) throws IOException { + return ProtoMessage.mergeFrom(new ProtobufEllipse2d(), input).checkInitialized(); + } + + /** + * @return factory for creating ProtobufEllipse2d messages + */ + public static MessageFactory getFactory() { + return ProtobufEllipse2dFactory.INSTANCE; + } + + /** + * @return this type's descriptor. + */ + public static Descriptors.Descriptor getDescriptor() { + return Geometry2D.wpi_proto_ProtobufEllipse2d_descriptor; + } + + private enum ProtobufEllipse2dFactory implements MessageFactory { + INSTANCE; + + @Override + public ProtobufEllipse2d create() { + return ProtobufEllipse2d.newInstance(); + } + } + + /** + * Contains name constants used for serializing JSON + */ + static class FieldNames { + static final FieldName xSemiAxis = FieldName.forField("xSemiAxis"); - static final FieldName height = FieldName.forField("height"); + static final FieldName ySemiAxis = FieldName.forField("ySemiAxis"); static final FieldName center = FieldName.forField("center"); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java index c12f7c38c4c..d53088cffab 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java @@ -1,5 +1,9 @@ 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; @@ -8,26 +12,24 @@ */ public class Ellipse2d implements ProtobufSerializable, StructSerializable { private final Pose2d m_center; - private final double m_horizontalSemiAxis, m_verticalSemiAxis; + private final double m_xSemiAxis, m_ySemiAxis; /** - * Constructs an ellipse from two semi-axes, a horizontal and vertical one. - * - *

The orientation of these radii may be changed via the rotational component of the center. + * Constructs an ellipse around a center point and two semi-axes, a horizontal and vertical one. * * @param center The center of the ellipse. - * @param horizontalSemiAxis The horizontal (x-axis parallel) semi-axis. - * @param semiMajorAxis The vertical (y-axis parallel) semi-axis. + * @param xSemiAxis The x semi-axis. + * @param ySemiAxis The y semi-axis. */ - public Ellipse2d(Pose2d center, double horizontalSemiAxis, double verticalSemiAxis) { + public Ellipse2d(Pose2d center, double xSemiAxis, double ySemiAxis) { // Safety check - if (horizontalSemiAxis <= 0 || verticalSemiAxis <= 0) { + if (xSemiAxis <= 0 || ySemiAxis <= 0) { throw new IllegalArgumentException("Ellipse2d semi-axes must be positive"); } m_center = center; - m_horizontalSemiAxis = horizontalSemiAxis; - m_verticalSemiAxis = verticalSemiAxis; + m_xSemiAxis = xSemiAxis; + m_ySemiAxis = ySemiAxis; } /** @@ -50,21 +52,21 @@ public Pose2d getCenter() { } /** - * Returns the horizontal semi-axis. + * Returns the x semi-axis. * - * @return The horizontal semi-axis. + * @return The x semi-axis. */ - public double getHorizontalSemiAxis() { - return m_horizontalSemiAxis; + public double getXSemiAxis() { + return m_xSemiAxis; } /** - * Returns the vertical semi-axis. + * Returns the y semi-axis. * - * @return The vertical semi-axis. + * @return The y semi-axis. */ - public double getVerticalSemiAxis() { - return m_verticalSemiAxis; + public double getYSemiAxis() { + return m_ySemiAxis; } /** @@ -75,13 +77,13 @@ public double getVerticalSemiAxis() { * @return A focal point. */ public Translation2d getFocalPoint(boolean first) { - double a = Math.max(m_horizontalSemiAxis, m_verticalSemiAxis); // Major semi-axis - double b = Math.min(m_horizontalSemiAxis, m_verticalSemiAxis); // Minor semi-axis - double c = Math.sqrt(a*a - b*b); + 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.hypot(a, b); c = (first ? -c : c); - if (m_horizontalSemiAxis > m_verticalSemiAxis) { + if (m_xSemiAxis > m_ySemiAxis) { Transform2d diff = new Transform2d(c, 0.0, new Rotation2d()); return m_center.plus(diff).getTranslation(); } else { @@ -99,10 +101,19 @@ public Translation2d getFocalPoint(boolean first) { public Ellipse2d transformBy(Transform2d other) { return new Ellipse2d( m_center.transformBy(other), - m_horizontalSemiAxis, - m_verticalSemiAxis); + 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. + */ + public Ellipse2d rotateBy(Rotation2d other) { + return new Ellipse2d(m_center.rotateBy(other), m_xSemiAxis, m_ySemiAxis); + } /** * Solves the equation of an ellipse from the given point. This is a helper function used to @@ -122,8 +133,8 @@ private double solveEllipseEquation(Translation2d point) { double x = point.getX() - m_center.getX(); double y = point.getY() - m_center.getY(); - return (x * x) / (m_horizontalSemiAxis * m_horizontalSemiAxis) + - (y * y) / (m_verticalSemiAxis * m_verticalSemiAxis); + return (x * x) / (m_xSemiAxis * m_ySemiAxis) + + (y * y) / (m_xSemiAxis * m_ySemiAxis); } /** @@ -146,4 +157,37 @@ public boolean intersectsPoint(Translation2d point) { public boolean containsPoint(Translation2d point) { return solveEllipseEquation(point) <= 1.0; } + + @Override + public String toString() { + 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. + */ + @Override + public boolean equals(Object obj) { + if (obj instanceof Ellipse2d) { + return ((Ellipse2d) obj).getCenter().equals(m_center) + && ((Ellipse2d) obj).getXSemiAxis() == m_xSemiAxis + && ((Ellipse2d) obj).getYSemiAxis() == m_ySemiAxis; + } + return false; + } + + @Override + public int hashCode() { + return Objects.hash(m_center, m_xSemiAxis, m_ySemiAxis); + } + + /** Ellipse2d protobuf for serialization. */ + public static final Ellipse2dProto proto = new Ellipse2dProto(); + + /** Ellipse2d struct for serialization. */ + public static final Ellipse2dStruct struct = new Ellipse2dStruct(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java index 9c1bc576e5a..86046fdf27d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java @@ -210,10 +210,7 @@ public Translation2d findNearestPoint(Translation2d point) { @Override public String toString() { return String.format("Rectangle2d(center: %s, x: %.2f, y: %.2f)", - m_center, - m_xWidth, - m_yWidth - ); + m_center, m_xWidth, m_yWidth); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Ellipse2dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Ellipse2dProto.java new file mode 100644 index 00000000000..abd8a9ba0ba --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Ellipse2dProto.java @@ -0,0 +1,49 @@ +// 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. + +package edu.wpi.first.math.geometry.proto; + +import edu.wpi.first.math.geometry.Ellipse2d; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.proto.Geometry2D.ProtobufEllipse2d; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class Ellipse2dProto implements Protobuf { + @Override + public Class getTypeClass() { + return Ellipse2d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufEllipse2d.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Pose2d.proto}; + } + + @Override + public ProtobufEllipse2d createMessage() { + return ProtobufEllipse2d.newInstance(); + } + + @Override + public Ellipse2d unpack(ProtobufEllipse2d msg) { + return new Ellipse2d( + Pose2d.proto.unpack(msg.getCenter()), + msg.getXSemiAxis(), + msg.getYSemiAxis() + ); + } + + @Override + public void pack(ProtobufEllipse2d msg, Ellipse2d value) { + Pose2d.proto.pack(msg.getMutableCenter(), value.getCenter()); + msg.setXSemiAxis(value.getXSemiAxis()); + msg.setYSemiAxis(value.getYSemiAxis()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rectangle2dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rectangle2dProto.java index 395c50ce16a..3264448174f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rectangle2dProto.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rectangle2dProto.java @@ -35,15 +35,15 @@ public ProtobufRectangle2d createMessage() { public Rectangle2d unpack(ProtobufRectangle2d msg) { return new Rectangle2d( Pose2d.proto.unpack(msg.getCenter()), - msg.getWidth(), - msg.getHeight() + msg.getXWidth(), + msg.getYWidth() ); } @Override public void pack(ProtobufRectangle2d msg, Rectangle2d value) { Pose2d.proto.pack(msg.getMutableCenter(), value.getCenter()); - msg.setWidth(value.getXWidth()); - msg.setHeight(value.getYWidth()); + msg.setXWidth(value.getXWidth()); + msg.setYWidth(value.getYWidth()); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Ellipse2dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Ellipse2dStruct.java new file mode 100644 index 00000000000..c8d3a312f8e --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Ellipse2dStruct.java @@ -0,0 +1,53 @@ +// 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. + +package edu.wpi.first.math.geometry.struct; + +import java.nio.ByteBuffer; + +import edu.wpi.first.math.geometry.Ellipse2d; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.util.struct.Struct; + +public class Ellipse2dStruct implements Struct { + @Override + public Class getTypeClass() { + return Ellipse2d.class; + } + + @Override + public String getTypeString() { + return "struct:Ellipse2d"; + } + + @Override + public int getSize() { + return Pose2d.struct.getSize() + kSizeDouble + kSizeDouble; + } + + @Override + public String getSchema() { + return "Pose2d center;double xSemiAxis; double ySemiAxis"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Pose2d.struct}; + } + + @Override + public Ellipse2d unpack(ByteBuffer bb) { + Pose2d center = Pose2d.struct.unpack(bb); + double xSemiAxis = bb.getDouble(); + double ySemiAxis = bb.getDouble(); + return new Ellipse2d(center, xSemiAxis, ySemiAxis); + } + + @Override + public void pack(ByteBuffer bb, Ellipse2d value) { + Pose2d.struct.pack(bb, value.getCenter()); + bb.putDouble(value.getXSemiAxis()); + bb.putDouble(value.getYSemiAxis()); + } +} diff --git a/wpimath/src/main/proto/geometry2d.proto b/wpimath/src/main/proto/geometry2d.proto index 6fd14d0ac7f..4f6438d2ca9 100644 --- a/wpimath/src/main/proto/geometry2d.proto +++ b/wpimath/src/main/proto/geometry2d.proto @@ -31,6 +31,12 @@ message ProtobufTwist2d { message ProtobufRectangle2d { ProtobufPose2d center = 1; - double width = 2; - double height = 3; + double xWidth = 2; + double yWidth = 3; +} + +message ProtobufEllipse2d { + ProtobufPose2d center = 1; + double xSemiAxis = 2; + double ySemiAxis = 3; } \ No newline at end of file From e932a9c3f05576f44d240bb9c2f263e1c0c8446a Mon Sep 17 00:00:00 2001 From: braykoff <99614905+Braykoff@users.noreply.github.com> Date: Sat, 13 Apr 2024 17:13:04 -0400 Subject: [PATCH 08/30] Unit tests and file header --- .../wpi/first/math/geometry/Ellipse2d.java | 15 +++-- .../first/math/geometry/Ellipse2dTest.java | 66 +++++++++++++++++++ .../geometry/proto/Ellipse2dProtoTest.java | 30 +++++++++ .../geometry/struct/Ellipse2dStructTest.java | 36 ++++++++++ 4 files changed, 140 insertions(+), 7 deletions(-) create mode 100644 wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java create mode 100644 wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Ellipse2dProtoTest.java create mode 100644 wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Ellipse2dStructTest.java diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java index d53088cffab..b2864d7f503 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java @@ -1,3 +1,7 @@ +// 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. + package edu.wpi.first.math.geometry; import java.util.Objects; @@ -79,7 +83,7 @@ public double getYSemiAxis() { 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.hypot(a, b); + double c = Math.sqrt(a*a - b*b); c = (first ? -c : c); @@ -99,10 +103,7 @@ public Translation2d getFocalPoint(boolean first) { * @return The transformed ellipse. */ public Ellipse2d transformBy(Transform2d other) { - return new Ellipse2d( - m_center.transformBy(other), - m_xSemiAxis, - m_ySemiAxis); + return new Ellipse2d(m_center.transformBy(other), m_xSemiAxis, m_ySemiAxis); } /** @@ -133,8 +134,8 @@ 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_ySemiAxis) + - (y * y) / (m_xSemiAxis * m_ySemiAxis); + return (x * x) / (m_xSemiAxis * m_xSemiAxis) + + (y * y) / (m_ySemiAxis * m_ySemiAxis); } /** diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java new file mode 100644 index 00000000000..647567836e8 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java @@ -0,0 +1,66 @@ +package edu.wpi.first.math.geometry; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import org.junit.jupiter.api.Test; + +public class Ellipse2dTest { + @Test + void testGetFocalPoints() { + var center = new Pose2d(1, 2, new Rotation2d()); + var ellipse = new Ellipse2d(center, 5.0, 4.0); + + var a = ellipse.getFocalPoint(true); + var b = ellipse.getFocalPoint(false); + + assertAll( + () -> assertEquals(new Translation2d(-2.0, 2.0), a), + () -> assertEquals(new Translation2d(4.0, 2.0), b)); + } + + @Test + void testIntersectsPoint() { + var center = new Pose2d(1.0, 2.0, new Rotation2d()); + var ellipse = new Ellipse2d(center, 2.0, 1.0); + + var pointA = new Translation2d(1.0, 3.0); + var pointB = new Translation2d(0.0, 3.0); + + assertAll( + () -> assertTrue(ellipse.intersectsPoint(pointA)), + () -> assertFalse(ellipse.intersectsPoint(pointB))); + } + + @Test + void testContainsPoint() { + var center = new Pose2d(-1.0, -2.0, Rotation2d.fromDegrees(45.0)); + var ellipse = new Ellipse2d(center, 2.0, 1.0); + + var pointA = new Translation2d(0.0, -1.0); + var pointB = new Translation2d(0.5, -2.0); + + assertAll( + () -> assertTrue(ellipse.containsPoint(pointA)), + () -> assertFalse(ellipse.containsPoint(pointB))); + } + + @Test + void testEquals() { + var center1 = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(90.0)); + var ellipse1 = new Ellipse2d(center1, 2.0, 3.0); + + var center2 = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(90.0)); + var ellipse2 = new Ellipse2d(center1, 2.0, 3.0); + + var center3 = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(90.0)); + var ellipse3 = new Ellipse2d(center1, 3.0, 2.0); + + assertAll( + () -> assertTrue(ellipse1.equals(ellipse2)), + () -> assertFalse(ellipse1.equals(ellipse3)), + () -> assertFalse(ellipse3.equals(ellipse2))); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Ellipse2dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Ellipse2dProtoTest.java new file mode 100644 index 00000000000..6d9f7fcfe25 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Ellipse2dProtoTest.java @@ -0,0 +1,30 @@ +// 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. + +package edu.wpi.first.math.geometry.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.math.geometry.Ellipse2d; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.proto.Geometry2D.ProtobufEllipse2d; + +public class Ellipse2dProtoTest { + private static final Ellipse2d DATA = + new Ellipse2d(new Pose2d(1.0, 3.6, new Rotation2d(4.1)), 2.0, 1.0); + + @Test + void testRoundtrip() { + ProtobufEllipse2d proto = Ellipse2d.proto.createMessage(); + Ellipse2d.proto.pack(proto, DATA); + + Ellipse2d data = Ellipse2d.proto.unpack(proto); + assertEquals(DATA.getCenter(), data.getCenter()); + assertEquals(DATA.getXSemiAxis(), data.getXSemiAxis()); + assertEquals(DATA.getYSemiAxis(), data.getYSemiAxis()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Ellipse2dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Ellipse2dStructTest.java new file mode 100644 index 00000000000..1ae0f516c55 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Ellipse2dStructTest.java @@ -0,0 +1,36 @@ +// 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. + +package edu.wpi.first.math.geometry.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import java.nio.ByteBuffer; +import java.nio.ByteOrder; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.math.geometry.Ellipse2d; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + +public class Ellipse2dStructTest { + private static final Ellipse2d DATA = + new Ellipse2d( + new Pose2d(0.0, 1.0, new Rotation2d(2.4)), + 3.0, 4.0); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(Ellipse2d.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + Ellipse2d.struct.pack(buffer, DATA); + buffer.rewind(); + + Ellipse2d data = Ellipse2d.struct.unpack(buffer); + assertEquals(DATA.getCenter(), data.getCenter()); + assertEquals(DATA.getXSemiAxis(), data.getXSemiAxis()); + assertEquals(DATA.getYSemiAxis(), data.getYSemiAxis()); + } +} From 339cc04cda2d8737efd99212f573ecbf459bad9e Mon Sep 17 00:00:00 2001 From: braykoff <99614905+Braykoff@users.noreply.github.com> Date: Sat, 13 Apr 2024 17:16:27 -0400 Subject: [PATCH 09/30] Added missing file header --- .../test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java index 647567836e8..68c45f08a34 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java @@ -1,3 +1,7 @@ +// 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. + package edu.wpi.first.math.geometry; import static org.junit.jupiter.api.Assertions.assertAll; From 6d8702722c6a6ae05da244c0e954c9b33a1aea6c Mon Sep 17 00:00:00 2001 From: braykoff <99614905+Braykoff@users.noreply.github.com> Date: Sat, 13 Apr 2024 23:28:19 -0400 Subject: [PATCH 10/30] Fixed testing error --- .../test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java index 68c45f08a34..5841558f7af 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java @@ -57,10 +57,10 @@ void testEquals() { var ellipse1 = new Ellipse2d(center1, 2.0, 3.0); var center2 = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(90.0)); - var ellipse2 = new Ellipse2d(center1, 2.0, 3.0); + var ellipse2 = new Ellipse2d(center2, 2.0, 3.0); var center3 = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(90.0)); - var ellipse3 = new Ellipse2d(center1, 3.0, 2.0); + var ellipse3 = new Ellipse2d(center3, 3.0, 2.0); assertAll( () -> assertTrue(ellipse1.equals(ellipse2)), From 767a11c00e5a43c85b078f48275cce13ca692828 Mon Sep 17 00:00:00 2001 From: braykoff <99614905+Braykoff@users.noreply.github.com> Date: Sat, 13 Apr 2024 23:41:06 -0400 Subject: [PATCH 11/30] cpp header file created, java spotlessapply ran --- .../wpi/first/math/geometry/Ellipse2d.java | 53 ++++++------ .../wpi/first/math/geometry/Rectangle2d.java | 80 +++++++++---------- .../first/math/geometry/Translation2d.java | 9 +-- .../math/geometry/proto/Ellipse2dProto.java | 5 +- .../math/geometry/proto/Rectangle2dProto.java | 6 +- .../math/geometry/struct/Ellipse2dStruct.java | 5 +- .../geometry/struct/Rectangle2dStruct.java | 5 +- .../native/include/frc/geometry/Rectangle2d.h | 16 ++++ .../first/math/geometry/Ellipse2dTest.java | 18 ++--- .../first/math/geometry/Rectangle2dTest.java | 44 +++++----- .../math/geometry/Translation2dTest.java | 4 +- .../geometry/proto/Ellipse2dProtoTest.java | 5 +- .../geometry/proto/Rectangle2dProtoTest.java | 5 +- .../geometry/struct/Ellipse2dStructTest.java | 12 +-- .../struct/Rectangle2dStructTest.java | 12 +-- 15 files changed, 134 insertions(+), 145 deletions(-) create mode 100644 wpimath/src/main/native/include/frc/geometry/Rectangle2d.h diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java index b2864d7f503..f3b16acbc80 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java @@ -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. @@ -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. */ @@ -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() { @@ -57,7 +54,7 @@ public Pose2d getCenter() { /** * Returns the x semi-axis. - * + * * @return The x semi-axis. */ public double getXSemiAxis() { @@ -66,7 +63,7 @@ public double getXSemiAxis() { /** * Returns the y semi-axis. - * + * * @return The y semi-axis. */ public double getYSemiAxis() { @@ -74,16 +71,16 @@ public double getYSemiAxis() { } /** - * 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); @@ -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. */ @@ -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. */ @@ -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. - * + * * 

    * (x-h)^2 / a^2 + (y-k)^2 / b^2 = 1
    *
+ * * @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 @@ -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. */ @@ -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. */ @@ -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. */ diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java index 86046fdf27d..0f0d69df1e8 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java @@ -4,16 +4,15 @@ 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; @@ -21,7 +20,7 @@ public class Rectangle2d implements ProtobufSerializable, StructSerializable { /** * 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. @@ -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. @@ -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() { @@ -77,7 +75,7 @@ public Pose2d getCenter() { /** * Returns the rotational component of the rectangle. - * + * * @return The rotational component of the rectangle. */ public Rotation2d getRotation() { @@ -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() { @@ -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() { @@ -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 */ @@ -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. */ @@ -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. */ @@ -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) */ @@ -177,8 +174,8 @@ 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); @@ -186,7 +183,7 @@ public double distanceToPoint(Translation2d 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. */ @@ -196,12 +193,14 @@ 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()); @@ -209,8 +208,7 @@ public Translation2d findNearestPoint(Translation2d point) { @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); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java index 59afcc14fe0..94dbc960d0c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java @@ -174,21 +174,20 @@ public Translation2d rotateBy(Rotation2d other) { /** * Rotates this translation around another translation in 2D space. - * + * * 
    * [x_new]   [rot.cos, -rot.sin][x - other.x]   [other.x]
    * [y_new] = [rot.sin, rot.cos][y - other.y]  + [other.y]
    *
- * + * * @param other The other translation to rotate around. * @param rot The rotation to rotate the translation by. * @return The new rotated translation. */ public Translation2d rotateAround(Translation2d other, Rotation2d rot) { return new Translation2d( - (m_x - other.getX()) * rot.getCos() - (m_y - other.getY()) * rot.getSin() + other.getX(), - (m_x - other.getX()) * rot.getSin() + (m_y - other.getY()) * rot.getCos() + other.getY() - ); + (m_x - other.getX()) * rot.getCos() - (m_y - other.getY()) * rot.getSin() + other.getX(), + (m_x - other.getX()) * rot.getSin() + (m_y - other.getY()) * rot.getCos() + other.getY()); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Ellipse2dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Ellipse2dProto.java index abd8a9ba0ba..587f0e65d0a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Ellipse2dProto.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Ellipse2dProto.java @@ -34,10 +34,7 @@ public ProtobufEllipse2d createMessage() { @Override public Ellipse2d unpack(ProtobufEllipse2d msg) { return new Ellipse2d( - Pose2d.proto.unpack(msg.getCenter()), - msg.getXSemiAxis(), - msg.getYSemiAxis() - ); + Pose2d.proto.unpack(msg.getCenter()), msg.getXSemiAxis(), msg.getYSemiAxis()); } @Override diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rectangle2dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rectangle2dProto.java index 3264448174f..4876d1e26a2 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rectangle2dProto.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rectangle2dProto.java @@ -33,11 +33,7 @@ public ProtobufRectangle2d createMessage() { @Override public Rectangle2d unpack(ProtobufRectangle2d msg) { - return new Rectangle2d( - Pose2d.proto.unpack(msg.getCenter()), - msg.getXWidth(), - msg.getYWidth() - ); + return new Rectangle2d(Pose2d.proto.unpack(msg.getCenter()), msg.getXWidth(), msg.getYWidth()); } @Override diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Ellipse2dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Ellipse2dStruct.java index c8d3a312f8e..870cfaddb23 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Ellipse2dStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Ellipse2dStruct.java @@ -4,11 +4,10 @@ package edu.wpi.first.math.geometry.struct; -import java.nio.ByteBuffer; - import edu.wpi.first.math.geometry.Ellipse2d; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; public class Ellipse2dStruct implements Struct { @Override @@ -49,5 +48,5 @@ public void pack(ByteBuffer bb, Ellipse2d value) { Pose2d.struct.pack(bb, value.getCenter()); bb.putDouble(value.getXSemiAxis()); bb.putDouble(value.getYSemiAxis()); - } + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rectangle2dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rectangle2dStruct.java index 58c7f33bf94..62c6eff6bbe 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rectangle2dStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rectangle2dStruct.java @@ -4,11 +4,10 @@ package edu.wpi.first.math.geometry.struct; -import java.nio.ByteBuffer; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rectangle2d; import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; public class Rectangle2dStruct implements Struct { @Override @@ -49,5 +48,5 @@ public void pack(ByteBuffer bb, Rectangle2d value) { Pose2d.struct.pack(bb, value.getCenter()); bb.putDouble(value.getXWidth()); bb.putDouble(value.getYWidth()); - } + } } diff --git a/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h b/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h new file mode 100644 index 00000000000..58b4f017b8d --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h @@ -0,0 +1,16 @@ +// 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 + +namespace frc { + +/** + * Represents a 2d rectangular space containing translational, rotational, and scaling components. + */ +class WPILIB_DLLEXPORT Rectangle2d { + +} + +} // namespace frc \ No newline at end of file diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java index 5841558f7af..02e1398a541 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java @@ -21,8 +21,8 @@ void testGetFocalPoints() { var b = ellipse.getFocalPoint(false); assertAll( - () -> assertEquals(new Translation2d(-2.0, 2.0), a), - () -> assertEquals(new Translation2d(4.0, 2.0), b)); + () -> assertEquals(new Translation2d(-2.0, 2.0), a), + () -> assertEquals(new Translation2d(4.0, 2.0), b)); } @Test @@ -34,8 +34,8 @@ void testIntersectsPoint() { var pointB = new Translation2d(0.0, 3.0); assertAll( - () -> assertTrue(ellipse.intersectsPoint(pointA)), - () -> assertFalse(ellipse.intersectsPoint(pointB))); + () -> assertTrue(ellipse.intersectsPoint(pointA)), + () -> assertFalse(ellipse.intersectsPoint(pointB))); } @Test @@ -47,8 +47,8 @@ void testContainsPoint() { var pointB = new Translation2d(0.5, -2.0); assertAll( - () -> assertTrue(ellipse.containsPoint(pointA)), - () -> assertFalse(ellipse.containsPoint(pointB))); + () -> assertTrue(ellipse.containsPoint(pointA)), + () -> assertFalse(ellipse.containsPoint(pointB))); } @Test @@ -63,8 +63,8 @@ void testEquals() { var ellipse3 = new Ellipse2d(center3, 3.0, 2.0); assertAll( - () -> assertTrue(ellipse1.equals(ellipse2)), - () -> assertFalse(ellipse1.equals(ellipse3)), - () -> assertFalse(ellipse3.equals(ellipse2))); + () -> assertTrue(ellipse1.equals(ellipse2)), + () -> assertFalse(ellipse1.equals(ellipse3)), + () -> assertFalse(ellipse3.equals(ellipse2))); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java index 1c8c5ca2009..2dceea92f63 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java @@ -13,7 +13,7 @@ public class Rectangle2dTest { private static final double kEpsilon = 1E-9; - + @Test void testNewWithCorners() { var cornerA = new Translation2d(1.0, 2.0); @@ -22,10 +22,10 @@ void testNewWithCorners() { var rect = new Rectangle2d(cornerA, cornerB); assertAll( - () -> assertEquals(3.0, rect.getXWidth()), - () -> assertEquals(4.0, rect.getYWidth()), - () -> assertEquals(2.5, rect.getCenter().getX()), - () -> assertEquals(4.0, rect.getCenter().getY())); + () -> assertEquals(3.0, rect.getXWidth()), + () -> assertEquals(4.0, rect.getYWidth()), + () -> assertEquals(2.5, rect.getCenter().getX()), + () -> assertEquals(4.0, rect.getCenter().getY())); } @Test @@ -34,10 +34,10 @@ void testIntersectsPoint() { var rect = new Rectangle2d(center, 2.0, 3.0); assertAll( - () -> assertTrue(rect.intersectsPoint(new Translation2d(5.5, 4.0))), - () -> assertTrue(rect.containsPoint(new Translation2d(3.0, 2.0))), - () -> assertFalse(rect.containsPoint(new Translation2d(4.0, 1.5))), - () -> assertFalse(rect.intersectsPoint(new Translation2d(4.0, 3.5)))); + () -> assertTrue(rect.intersectsPoint(new Translation2d(5.5, 4.0))), + () -> assertTrue(rect.containsPoint(new Translation2d(3.0, 2.0))), + () -> assertFalse(rect.containsPoint(new Translation2d(4.0, 1.5))), + () -> assertFalse(rect.intersectsPoint(new Translation2d(4.0, 3.5)))); } @Test @@ -46,9 +46,9 @@ void testContainsPoint() { var rect = new Rectangle2d(center, 3.0, 1.0); assertAll( - () -> assertTrue(rect.containsPoint(new Translation2d(2.0, 3.0))), - () -> assertTrue(rect.containsPoint(new Translation2d(3.0, 4.0))), - () -> assertFalse(rect.containsPoint(new Translation2d(3.0, 3.0)))); + () -> assertTrue(rect.containsPoint(new Translation2d(2.0, 3.0))), + () -> assertTrue(rect.containsPoint(new Translation2d(3.0, 4.0))), + () -> assertFalse(rect.containsPoint(new Translation2d(3.0, 3.0)))); } @Test @@ -62,10 +62,10 @@ void testDistanceToPoint() { var point4 = new Translation2d(-1.0, 2.5); assertAll( - () -> assertEquals(0.5, rect.distanceToPoint(point1), kEpsilon), - () -> assertEquals(0.0, rect.distanceToPoint(point2), kEpsilon), - () -> assertEquals(0.5, rect.distanceToPoint(point3), kEpsilon), - () -> assertEquals(1.0, rect.distanceToPoint(point4), kEpsilon)); + () -> assertEquals(0.5, rect.distanceToPoint(point1), kEpsilon), + () -> assertEquals(0.0, rect.distanceToPoint(point2), kEpsilon), + () -> assertEquals(0.5, rect.distanceToPoint(point3), kEpsilon), + () -> assertEquals(1.0, rect.distanceToPoint(point4), kEpsilon)); } @Test @@ -80,10 +80,10 @@ void testFindNearestPoint() { var nearestPoint2 = rect.findNearestPoint(point2); assertAll( - () -> assertEquals(1.0, nearestPoint1.getX(), kEpsilon), - () -> assertEquals(2.5, nearestPoint1.getY(), kEpsilon), - () -> assertEquals(0.0, nearestPoint2.getX(), kEpsilon), - () -> assertEquals(0.0, nearestPoint2.getY(), kEpsilon)); + () -> assertEquals(1.0, nearestPoint1.getX(), kEpsilon), + () -> assertEquals(2.5, nearestPoint1.getY(), kEpsilon), + () -> assertEquals(0.0, nearestPoint2.getX(), kEpsilon), + () -> assertEquals(0.0, nearestPoint2.getY(), kEpsilon)); } @Test @@ -97,8 +97,6 @@ void testEquals() { var center3 = new Pose2d(2.0, 3.0, new Rotation2d()); var rect3 = new Rectangle2d(center3, 3.0, 3.0); - assertAll( - () -> assertTrue(rect1.equals(rect2)), - () -> assertFalse(rect2.equals(rect3))); + assertAll(() -> assertTrue(rect1.equals(rect2)), () -> assertFalse(rect2.equals(rect3))); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java index 70c2c336012..e6caee61111 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java @@ -56,8 +56,8 @@ void testRotateAround() { var rotated = original.rotateAround(other, Rotation2d.fromDegrees(180.0)); assertAll( - () -> assertEquals(4.0, rotated.getX(), kEpsilon), - () -> assertEquals(3.0, rotated.getY(), kEpsilon)); + () -> assertEquals(4.0, rotated.getX(), kEpsilon), + () -> assertEquals(3.0, rotated.getY(), kEpsilon)); } @Test diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Ellipse2dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Ellipse2dProtoTest.java index 6d9f7fcfe25..98e856e2346 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Ellipse2dProtoTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Ellipse2dProtoTest.java @@ -6,16 +6,15 @@ import static org.junit.jupiter.api.Assertions.assertEquals; -import org.junit.jupiter.api.Test; - import edu.wpi.first.math.geometry.Ellipse2d; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.proto.Geometry2D.ProtobufEllipse2d; +import org.junit.jupiter.api.Test; public class Ellipse2dProtoTest { private static final Ellipse2d DATA = - new Ellipse2d(new Pose2d(1.0, 3.6, new Rotation2d(4.1)), 2.0, 1.0); + new Ellipse2d(new Pose2d(1.0, 3.6, new Rotation2d(4.1)), 2.0, 1.0); @Test void testRoundtrip() { diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rectangle2dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rectangle2dProtoTest.java index a22120d3170..f405fd56604 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rectangle2dProtoTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rectangle2dProtoTest.java @@ -6,16 +6,15 @@ import static org.junit.jupiter.api.Assertions.assertEquals; -import org.junit.jupiter.api.Test; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rectangle2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.proto.Geometry2D.ProtobufRectangle2d; +import org.junit.jupiter.api.Test; public class Rectangle2dProtoTest { private static final Rectangle2d DATA = - new Rectangle2d(new Pose2d(0.1, 0.2, new Rotation2d()), 4.0, 3.0); + new Rectangle2d(new Pose2d(0.1, 0.2, new Rotation2d()), 4.0, 3.0); @Test void testRoundtrip() { diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Ellipse2dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Ellipse2dStructTest.java index 1ae0f516c55..026675d99b1 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Ellipse2dStructTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Ellipse2dStructTest.java @@ -6,20 +6,16 @@ import static org.junit.jupiter.api.Assertions.assertEquals; -import java.nio.ByteBuffer; -import java.nio.ByteOrder; - -import org.junit.jupiter.api.Test; - import edu.wpi.first.math.geometry.Ellipse2d; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; public class Ellipse2dStructTest { private static final Ellipse2d DATA = - new Ellipse2d( - new Pose2d(0.0, 1.0, new Rotation2d(2.4)), - 3.0, 4.0); + new Ellipse2d(new Pose2d(0.0, 1.0, new Rotation2d(2.4)), 3.0, 4.0); @Test void testRoundtrip() { diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rectangle2dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rectangle2dStructTest.java index 27e4bb844f5..187fa06d8e4 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rectangle2dStructTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rectangle2dStructTest.java @@ -6,20 +6,16 @@ import static org.junit.jupiter.api.Assertions.assertEquals; -import java.nio.ByteBuffer; -import java.nio.ByteOrder; - -import org.junit.jupiter.api.Test; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rectangle2d; import edu.wpi.first.math.geometry.Rotation2d; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; public class Rectangle2dStructTest { private static final Rectangle2d DATA = - new Rectangle2d( - new Pose2d(1.0, 2.0, new Rotation2d(3.1)), - 5.0, 3.0); + new Rectangle2d(new Pose2d(1.0, 2.0, new Rotation2d(3.1)), 5.0, 3.0); @Test void testRoundtrip() { From 097d3e55b6863b1eb261d91e3ba389c17a63739e Mon Sep 17 00:00:00 2001 From: braykoff <99614905+Braykoff@users.noreply.github.com> Date: Sun, 28 Apr 2024 16:55:19 -0400 Subject: [PATCH 12/30] Removed c++ header file --- .../native/include/frc/geometry/Rectangle2d.h | 16 ---------------- 1 file changed, 16 deletions(-) delete mode 100644 wpimath/src/main/native/include/frc/geometry/Rectangle2d.h diff --git a/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h b/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h deleted file mode 100644 index 58b4f017b8d..00000000000 --- a/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h +++ /dev/null @@ -1,16 +0,0 @@ -// 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 - -namespace frc { - -/** - * Represents a 2d rectangular space containing translational, rotational, and scaling components. - */ -class WPILIB_DLLEXPORT Rectangle2d { - -} - -} // namespace frc \ No newline at end of file From fd1b7f6d24f260f5cad58f5e84e6f1ea700313e7 Mon Sep 17 00:00:00 2001 From: braykoff <99614905+Braykoff@users.noreply.github.com> Date: Sun, 28 Apr 2024 17:28:35 -0400 Subject: [PATCH 13/30] Fixed failing git tests (formatting). --- .../wpi/first/math/geometry/Ellipse2d.java | 9 ++++--- .../wpi/first/math/geometry/Rectangle2d.java | 27 +++++++++++-------- wpimath/src/main/proto/geometry2d.proto | 2 +- .../first/math/geometry/Ellipse2dTest.java | 9 ++++--- .../first/math/geometry/Rectangle2dTest.java | 5 ++-- .../geometry/proto/Ellipse2dProtoTest.java | 2 +- .../geometry/proto/Rectangle2dProtoTest.java | 2 +- .../geometry/struct/Ellipse2dStructTest.java | 2 +- .../struct/Rectangle2dStructTest.java | 2 +- 9 files changed, 34 insertions(+), 26 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java index f3b16acbc80..14c28d1e2b9 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java @@ -10,10 +10,11 @@ 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; + private final double m_xSemiAxis; + private final double m_ySemiAxis; /** * Constructs an ellipse around a center point and two semi-axes, a horizontal and vertical one. @@ -82,7 +83,7 @@ public Translation2d getFocalPoint(boolean first) { double b = Math.min(m_xSemiAxis, m_ySemiAxis); // Minor semi-axis double c = Math.sqrt(a * a - b * b); - c = (first ? -c : c); + c = first ? -c : c; if (m_xSemiAxis > m_ySemiAxis) { Transform2d diff = new Transform2d(c, 0.0, new Rotation2d()); @@ -163,7 +164,7 @@ public String toString() { } /** - * Checks equality between this Ellipse2d and another object + * Checks equality between this Ellipse2d and another object. * * @param obj The other object. * @return Whether the two objects are equal or not. diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java index 0f0d69df1e8..f5acdadeee1 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java @@ -16,7 +16,8 @@ */ public class Rectangle2d implements ProtobufSerializable, StructSerializable { private final Pose2d m_center; - private final double m_xWidth, m_yWidth; + private final double m_xWidth; + private final double m_yWidth; /** * Constructs a rectangle at the specified position with the specified width and height. @@ -37,12 +38,12 @@ public Rectangle2d(Pose2d center, double xWidth, double yWidth) { } /** - * Constructs a rectangle at the specified position and rotation with the specified width and - * height. + * Constructs a rectangle at the specified position and rotation with the specified x width and y + * width. * * @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. + * @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. * @param rotation The rotation of the rectangle. */ public Rectangle2d(Translation2d center, double xWidth, double yWidth, Rotation2d rotation) { @@ -133,10 +134,10 @@ public boolean intersectsPoint(Translation2d point) { if (Math.abs(point.getX()) == m_xWidth / 2.0) { // Point rests on left/right perimeter - return (Math.abs(point.getY()) <= m_yWidth / 2.0); + return Math.abs(point.getY()) <= m_yWidth / 2.0; } else if (Math.abs(point.getY()) == m_yWidth / 2.0) { // Point rests on top/bottom perimeter - return (Math.abs(point.getX()) <= m_xWidth / 2.0); + return Math.abs(point.getX()) <= m_xWidth / 2.0; } return false; @@ -154,10 +155,10 @@ public boolean containsPoint(Translation2d point) { 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) + 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)); + && point.getY() <= (m_center.getY() + m_yWidth / 2.0); } /** @@ -167,7 +168,9 @@ public boolean containsPoint(Translation2d point) { * @return The distance (0, if the point is contained by the rectangle) */ public double distanceToPoint(Translation2d point) { - if (containsPoint(point)) return 0.0; + if (containsPoint(point)) { + return 0.0; + } // Move the point into the rectangle's coordinate frame point = point.minus(m_center.getTranslation()); @@ -189,7 +192,9 @@ public double distanceToPoint(Translation2d point) { */ public Translation2d findNearestPoint(Translation2d point) { // Check if already in rectangle - if (containsPoint(point)) return new Translation2d(point.getX(), point.getY()); + if (containsPoint(point)) { + return new Translation2d(point.getX(), point.getY()); + } // Rotate the point by the inverse of the rectangle's rotation point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus()); diff --git a/wpimath/src/main/proto/geometry2d.proto b/wpimath/src/main/proto/geometry2d.proto index 4f6438d2ca9..1749c63bc2a 100644 --- a/wpimath/src/main/proto/geometry2d.proto +++ b/wpimath/src/main/proto/geometry2d.proto @@ -39,4 +39,4 @@ message ProtobufEllipse2d { ProtobufPose2d center = 1; double xSemiAxis = 2; double ySemiAxis = 3; -} \ No newline at end of file +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java index 02e1398a541..6e3422976e7 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java @@ -7,11 +7,12 @@ import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertNotEquals; import static org.junit.jupiter.api.Assertions.assertTrue; import org.junit.jupiter.api.Test; -public class Ellipse2dTest { +class Ellipse2dTest { @Test void testGetFocalPoints() { var center = new Pose2d(1, 2, new Rotation2d()); @@ -63,8 +64,8 @@ void testEquals() { var ellipse3 = new Ellipse2d(center3, 3.0, 2.0); assertAll( - () -> assertTrue(ellipse1.equals(ellipse2)), - () -> assertFalse(ellipse1.equals(ellipse3)), - () -> assertFalse(ellipse3.equals(ellipse2))); + () -> assertEquals(ellipse1, ellipse2), + () -> assertNotEquals(ellipse1, ellipse3), + () -> assertNotEquals(ellipse3, ellipse2)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java index 2dceea92f63..a578872f9a8 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java @@ -7,11 +7,12 @@ import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertNotEquals; import static org.junit.jupiter.api.Assertions.assertTrue; import org.junit.jupiter.api.Test; -public class Rectangle2dTest { +class Rectangle2dTest { private static final double kEpsilon = 1E-9; @Test @@ -97,6 +98,6 @@ void testEquals() { var center3 = new Pose2d(2.0, 3.0, new Rotation2d()); var rect3 = new Rectangle2d(center3, 3.0, 3.0); - assertAll(() -> assertTrue(rect1.equals(rect2)), () -> assertFalse(rect2.equals(rect3))); + assertAll(() -> assertEquals(rect1, rect2), () -> assertNotEquals(rect2, rect3)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Ellipse2dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Ellipse2dProtoTest.java index 98e856e2346..92c5614d291 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Ellipse2dProtoTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Ellipse2dProtoTest.java @@ -12,7 +12,7 @@ import edu.wpi.first.math.proto.Geometry2D.ProtobufEllipse2d; import org.junit.jupiter.api.Test; -public class Ellipse2dProtoTest { +class Ellipse2dProtoTest { private static final Ellipse2d DATA = new Ellipse2d(new Pose2d(1.0, 3.6, new Rotation2d(4.1)), 2.0, 1.0); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rectangle2dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rectangle2dProtoTest.java index f405fd56604..e270c75cabc 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rectangle2dProtoTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rectangle2dProtoTest.java @@ -12,7 +12,7 @@ import edu.wpi.first.math.proto.Geometry2D.ProtobufRectangle2d; import org.junit.jupiter.api.Test; -public class Rectangle2dProtoTest { +class Rectangle2dProtoTest { private static final Rectangle2d DATA = new Rectangle2d(new Pose2d(0.1, 0.2, new Rotation2d()), 4.0, 3.0); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Ellipse2dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Ellipse2dStructTest.java index 026675d99b1..1bb43234237 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Ellipse2dStructTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Ellipse2dStructTest.java @@ -13,7 +13,7 @@ import java.nio.ByteOrder; import org.junit.jupiter.api.Test; -public class Ellipse2dStructTest { +class Ellipse2dStructTest { private static final Ellipse2d DATA = new Ellipse2d(new Pose2d(0.0, 1.0, new Rotation2d(2.4)), 3.0, 4.0); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rectangle2dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rectangle2dStructTest.java index 187fa06d8e4..506209bdc48 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rectangle2dStructTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rectangle2dStructTest.java @@ -13,7 +13,7 @@ import java.nio.ByteOrder; import org.junit.jupiter.api.Test; -public class Rectangle2dStructTest { +class Rectangle2dStructTest { private static final Rectangle2d DATA = new Rectangle2d(new Pose2d(1.0, 2.0, new Rotation2d(3.1)), 5.0, 3.0); From 2bc4c31ed13e6d1d04cdbeffaac2ca0a01633662 Mon Sep 17 00:00:00 2001 From: braykoff <99614905+Braykoff@users.noreply.github.com> Date: Mon, 29 Apr 2024 18:12:58 -0400 Subject: [PATCH 14/30] Shortened names, added tolerances. --- .../wpi/first/math/geometry/Ellipse2d.java | 6 ++--- .../wpi/first/math/geometry/Rectangle2d.java | 16 +++++++------- .../first/math/geometry/Ellipse2dTest.java | 7 +++--- .../first/math/geometry/Rectangle2dTest.java | 22 +++++++++---------- 4 files changed, 25 insertions(+), 26 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java index 14c28d1e2b9..c90ec077429 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java @@ -142,8 +142,8 @@ private double solveEllipseEquation(Translation2d point) { * @param point The point to check. * @return True, if this ellipse's circumference intersects the point. */ - public boolean intersectsPoint(Translation2d point) { - return solveEllipseEquation(point) == 1.0; + public boolean intersects(Translation2d point) { + return Math.abs(1.0 - solveEllipseEquation(point)) <= 1E-9; } /** @@ -153,7 +153,7 @@ public boolean intersectsPoint(Translation2d point) { * @param point The point to check. * @return True, if the point is within or on the ellipse. */ - public boolean containsPoint(Translation2d point) { + public boolean contains(Translation2d point) { return solveEllipseEquation(point) <= 1.0; } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java index f5acdadeee1..617764f6d6d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java @@ -127,15 +127,15 @@ public Rectangle2d rotateBy(Rotation2d other) { * @param point The point to check. * @return True, if the rectangle's perimeter intersects the point. */ - public boolean intersectsPoint(Translation2d point) { + public boolean intersects(Translation2d point) { // Move the point into the rectangle's coordinate frame point = point.minus(m_center.getTranslation()); point = point.rotateBy(m_center.getRotation().unaryMinus()); - if (Math.abs(point.getX()) == m_xWidth / 2.0) { + if (Math.abs(Math.abs(point.getX()) - m_xWidth / 2.0) <= 1E-9) { // Point rests on left/right perimeter return Math.abs(point.getY()) <= m_yWidth / 2.0; - } else if (Math.abs(point.getY()) == m_yWidth / 2.0) { + } else if (Math.abs(Math.abs(point.getY()) - m_yWidth / 2.0) <= 1E-9) { // Point rests on top/bottom perimeter return Math.abs(point.getX()) <= m_xWidth / 2.0; } @@ -150,7 +150,7 @@ public boolean intersectsPoint(Translation2d point) { * @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) { + public boolean contains(Translation2d point) { // Rotate the point into the rectangle's coordinate frame point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus()); @@ -167,8 +167,8 @@ public boolean containsPoint(Translation2d point) { * @param point The point to check. * @return The distance (0, if the point is contained by the rectangle) */ - public double distanceToPoint(Translation2d point) { - if (containsPoint(point)) { + public double distance(Translation2d point) { + if (contains(point)) { return 0.0; } @@ -192,8 +192,8 @@ public double distanceToPoint(Translation2d point) { */ public Translation2d findNearestPoint(Translation2d point) { // Check if already in rectangle - if (containsPoint(point)) { - return new Translation2d(point.getX(), point.getY()); + if (contains(point)) { + return point; } // Rotate the point by the inverse of the rectangle's rotation diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java index 6e3422976e7..16a2def2ebb 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java @@ -35,8 +35,8 @@ void testIntersectsPoint() { var pointB = new Translation2d(0.0, 3.0); assertAll( - () -> assertTrue(ellipse.intersectsPoint(pointA)), - () -> assertFalse(ellipse.intersectsPoint(pointB))); + () -> assertTrue(ellipse.intersects(pointA)), + () -> assertFalse(ellipse.intersects(pointB))); } @Test @@ -48,8 +48,7 @@ void testContainsPoint() { var pointB = new Translation2d(0.5, -2.0); assertAll( - () -> assertTrue(ellipse.containsPoint(pointA)), - () -> assertFalse(ellipse.containsPoint(pointB))); + () -> assertTrue(ellipse.contains(pointA)), () -> assertFalse(ellipse.contains(pointB))); } @Test diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java index a578872f9a8..5a4e389854d 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java @@ -35,10 +35,10 @@ void testIntersectsPoint() { var rect = new Rectangle2d(center, 2.0, 3.0); assertAll( - () -> assertTrue(rect.intersectsPoint(new Translation2d(5.5, 4.0))), - () -> assertTrue(rect.containsPoint(new Translation2d(3.0, 2.0))), - () -> assertFalse(rect.containsPoint(new Translation2d(4.0, 1.5))), - () -> assertFalse(rect.intersectsPoint(new Translation2d(4.0, 3.5)))); + () -> assertTrue(rect.intersects(new Translation2d(5.5, 4.0))), + () -> assertTrue(rect.intersects(new Translation2d(3.0, 2.0))), + () -> assertFalse(rect.intersects(new Translation2d(4.0, 1.5))), + () -> assertFalse(rect.intersects(new Translation2d(4.0, 3.5)))); } @Test @@ -47,9 +47,9 @@ void testContainsPoint() { var rect = new Rectangle2d(center, 3.0, 1.0); assertAll( - () -> assertTrue(rect.containsPoint(new Translation2d(2.0, 3.0))), - () -> assertTrue(rect.containsPoint(new Translation2d(3.0, 4.0))), - () -> assertFalse(rect.containsPoint(new Translation2d(3.0, 3.0)))); + () -> assertTrue(rect.contains(new Translation2d(2.0, 3.0))), + () -> assertTrue(rect.contains(new Translation2d(3.0, 4.0))), + () -> assertFalse(rect.contains(new Translation2d(3.0, 3.0)))); } @Test @@ -63,10 +63,10 @@ void testDistanceToPoint() { var point4 = new Translation2d(-1.0, 2.5); assertAll( - () -> assertEquals(0.5, rect.distanceToPoint(point1), kEpsilon), - () -> assertEquals(0.0, rect.distanceToPoint(point2), kEpsilon), - () -> assertEquals(0.5, rect.distanceToPoint(point3), kEpsilon), - () -> assertEquals(1.0, rect.distanceToPoint(point4), kEpsilon)); + () -> assertEquals(0.5, rect.distance(point1), kEpsilon), + () -> assertEquals(0.0, rect.distance(point2), kEpsilon), + () -> assertEquals(0.5, rect.distance(point3), kEpsilon), + () -> assertEquals(1.0, rect.distance(point4), kEpsilon)); } @Test From afd24b26b95a5326e01adaded10bf64a0106fcdc Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Fri, 24 May 2024 18:04:57 -0700 Subject: [PATCH 15/30] Address review comments --- .../edu/wpi/first/math/geometry/Ellipse2d.java | 8 ++++---- .../wpi/first/math/geometry/Rectangle2d.java | 18 ++---------------- 2 files changed, 6 insertions(+), 20 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java index c90ec077429..0aff74007fc 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java @@ -40,8 +40,8 @@ public Ellipse2d(Pose2d center, double xSemiAxis, double ySemiAxis) { * @param center The center of the circle. * @param radius The radius of the circle. */ - public Ellipse2d(Pose2d center, double radius) { - this(center, radius, radius); + public Ellipse2d(Translation2d center, double radius) { + this(new Pose2d(center, Rotation2d.kZero), radius, radius); } /** @@ -86,10 +86,10 @@ public Translation2d getFocalPoint(boolean first) { c = first ? -c : c; if (m_xSemiAxis > m_ySemiAxis) { - Transform2d diff = new Transform2d(c, 0.0, new Rotation2d()); + Transform2d diff = new Transform2d(c, 0.0, Rotation2d.kZero); return m_center.plus(diff).getTranslation(); } else { - Transform2d diff = new Transform2d(0.0, c, new Rotation2d()); + Transform2d diff = new Transform2d(0.0, c, Rotation2d.kZero); return m_center.plus(diff).getTranslation(); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java index 617764f6d6d..18f73e8631f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java @@ -37,19 +37,6 @@ public Rectangle2d(Pose2d center, double xWidth, double yWidth) { m_yWidth = yWidth; } - /** - * Constructs a rectangle at the specified position and rotation with the specified x width and y - * width. - * - * @param center The center 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. - * @param rotation The rotation of the rectangle. - */ - public Rectangle2d(Translation2d center, double xWidth, double yWidth, Rotation2d rotation) { - this(new Pose2d(center, rotation), xWidth, yWidth); - } - /** * Creates an unrotated rectangle from the given corners. The corners should be diagonally * opposite of each other. @@ -59,10 +46,9 @@ public Rectangle2d(Translation2d center, double xWidth, double yWidth, Rotation2 */ public Rectangle2d(Translation2d cornerA, Translation2d cornerB) { this( - cornerA.plus(cornerB).div(2.0), + new Pose2d(cornerA.plus(cornerB).div(2.0), Rotation2d.kZero), Math.abs(cornerA.getX() - cornerB.getX()), - Math.abs(cornerA.getY() - cornerB.getY()), - new Rotation2d()); + Math.abs(cornerA.getY() - cornerB.getY())); } /** From ad53348cc647998377b37c8d5a33d24c61a6124d Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 28 May 2024 11:30:47 -0700 Subject: [PATCH 16/30] Rename getDistance() and refactor its internals --- .../wpi/first/math/geometry/Ellipse2d.java | 32 +++++++++++++++++++ .../wpi/first/math/geometry/Rectangle2d.java | 17 ++-------- .../first/math/geometry/Rectangle2dTest.java | 8 ++--- 3 files changed, 38 insertions(+), 19 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java index 0aff74007fc..24d21b76247 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java @@ -157,6 +157,38 @@ public boolean contains(Translation2d point) { 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) + */ + public double getDistance(Translation2d point) { + return findNearestPoint(point).getDistance(point); + } + + /** + * 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. + */ + public Translation2d findNearestPoint(Translation2d point) { + // Check if already in ellipse + if (contains(point)) { + return point; + } + + // Rotate the point by the inverse of the ellipse's rotation + point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus()); + + // Find nearest point + // TODO + + // Undo rotation + return point.rotateAround(m_center.getTranslation(), m_center.getRotation()); + } + @Override public String toString() { return String.format( diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java index 18f73e8631f..576a9614a54 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java @@ -153,21 +153,8 @@ public boolean contains(Translation2d point) { * @param point The point to check. * @return The distance (0, if the point is contained by the rectangle) */ - public double distance(Translation2d point) { - if (contains(point)) { - return 0.0; - } - - // Move the point into the rectangle's coordinate frame - point = point.minus(m_center.getTranslation()); - 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); - - // Distance formula - return Math.hypot(dx, dy); + public double getDistance(Translation2d point) { + return findNearestPoint(point).getDistance(point); } /** diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java index 5a4e389854d..0f6f92b156e 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java @@ -63,10 +63,10 @@ void testDistanceToPoint() { var point4 = new Translation2d(-1.0, 2.5); assertAll( - () -> assertEquals(0.5, rect.distance(point1), kEpsilon), - () -> assertEquals(0.0, rect.distance(point2), kEpsilon), - () -> assertEquals(0.5, rect.distance(point3), kEpsilon), - () -> assertEquals(1.0, rect.distance(point4), kEpsilon)); + () -> assertEquals(0.5, rect.getDistance(point1), kEpsilon), + () -> assertEquals(0.0, rect.getDistance(point2), kEpsilon), + () -> assertEquals(0.5, rect.getDistance(point3), kEpsilon), + () -> assertEquals(1.0, rect.getDistance(point4), kEpsilon)); } @Test From 5cf49fbb7947b8acef9046dae720306cd90f755d Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 1 Jun 2024 16:41:07 -0700 Subject: [PATCH 17/30] Write C++ version --- .../wpi/first/math/geometry/Ellipse2d.java | 67 +++--- .../wpi/first/math/geometry/Rectangle2d.java | 1 - .../main/native/cpp/geometry/Ellipse2d.cpp | 55 +++++ .../native/cpp/geometry/Translation2d.cpp | 9 - .../native/include/frc/geometry/Ellipse2d.h | 199 +++++++++++++++++ .../native/include/frc/geometry/Rectangle2d.h | 205 ++++++++++++++++++ .../include/frc/geometry/Translation2d.h | 22 +- .../include/frc/geometry/Translation2d.inc | 14 ++ wpimath/src/main/native/include/units/base.h | 4 +- .../first/math/geometry/Ellipse2dTest.java | 5 +- .../native/cpp/geometry/Ellipse2dTest.cpp | 89 ++++++++ .../test/native/cpp/geometry/Pose3dTest.cpp | 2 +- .../native/cpp/geometry/Rectangle2dTest.cpp | 87 ++++++++ 13 files changed, 708 insertions(+), 51 deletions(-) create mode 100644 wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp create mode 100644 wpimath/src/main/native/include/frc/geometry/Ellipse2d.h create mode 100644 wpimath/src/main/native/include/frc/geometry/Rectangle2d.h create mode 100644 wpimath/src/test/native/cpp/geometry/Ellipse2dTest.cpp create mode 100644 wpimath/src/test/native/cpp/geometry/Rectangle2dTest.cpp diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java index 24d21b76247..9b28e020d6e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java @@ -4,6 +4,7 @@ package edu.wpi.first.math.geometry; +import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.proto.Ellipse2dProto; import edu.wpi.first.math.geometry.struct.Ellipse2dStruct; import edu.wpi.first.util.protobuf.ProtobufSerializable; @@ -24,7 +25,6 @@ public class Ellipse2d implements ProtobufSerializable, StructSerializable { * @param ySemiAxis The y semi-axis. */ public Ellipse2d(Pose2d center, double xSemiAxis, double ySemiAxis) { - // Safety check if (xSemiAxis <= 0 || ySemiAxis <= 0) { throw new IllegalArgumentException("Ellipse2d semi-axes must be positive"); } @@ -72,25 +72,24 @@ public double getYSemiAxis() { } /** - * Returns either of the focal points of the ellipse. In a perfect circle, this will always return - * the center. + * Returns 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. + * @return The focal points. */ - public Translation2d getFocalPoint(boolean first) { + public Pair getFocalPoints() { 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); - c = first ? -c : c; - if (m_xSemiAxis > m_ySemiAxis) { - Transform2d diff = new Transform2d(c, 0.0, Rotation2d.kZero); - return m_center.plus(diff).getTranslation(); + return new Pair<>( + m_center.plus(new Transform2d(-c, 0.0, Rotation2d.kZero)).getTranslation(), + m_center.plus(new Transform2d(c, 0.0, Rotation2d.kZero)).getTranslation()); } else { - Transform2d diff = new Transform2d(0.0, c, Rotation2d.kZero); - return m_center.plus(diff).getTranslation(); + return new Pair<>( + m_center.plus(new Transform2d(0.0, -c, Rotation2d.kZero)).getTranslation(), + m_center.plus(new Transform2d(0.0, c, Rotation2d.kZero)).getTranslation()); } } @@ -114,28 +113,6 @@ public Ellipse2d rotateBy(Rotation2d other) { return new Ellipse2d(m_center.rotateBy(other), m_xSemiAxis, 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. - * - * 
-   * (x-h)^2 / a^2 + (y-k)^2 / b^2 = 1
-   *
- * - * @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. - */ - private double solveEllipseEquation(Translation2d point) { - // Rotate the point by the inverse of the ellipse's rotation - point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus()); - - 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); - } - /** * Checks if a point is intersected by this ellipse's circumference. * @@ -216,6 +193,28 @@ public int hashCode() { return Objects.hash(m_center, m_xSemiAxis, 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. + * + * 
+   * (x - h)²/a² + (y - k)²/b² = 1
+   *
+ * + * @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. + */ + private double solveEllipseEquation(Translation2d point) { + // Rotate the point by the inverse of the ellipse's rotation + point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus()); + + 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); + } + /** Ellipse2d protobuf for serialization. */ public static final Ellipse2dProto proto = new Ellipse2dProto(); diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java index 576a9614a54..9541021ffe6 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java @@ -27,7 +27,6 @@ public class Rectangle2d implements ProtobufSerializable, StructSerializable { * @param yWidth The y size component of the rectangle, in unrotated coordinate frame. */ public Rectangle2d(Pose2d center, double xWidth, double yWidth) { - // Safety check size if (xWidth < 0 || yWidth < 0) { throw new IllegalArgumentException("Rectangle2d dimensions cannot be less than 0!"); } diff --git a/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp b/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp new file mode 100644 index 00000000000..6f8afa636a2 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp @@ -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 + +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()); +} diff --git a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp index 90162018ed7..c0a349e7aac 100644 --- a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp @@ -14,19 +14,10 @@ using namespace frc; Translation2d::Translation2d(const Eigen::Vector2d& vector) : m_x{units::meter_t{vector.x()}}, m_y{units::meter_t{vector.y()}} {} -units::meter_t Translation2d::Distance(const Translation2d& other) const { - return units::math::hypot(other.m_x - m_x, other.m_y - m_y); -} - units::meter_t Translation2d::Norm() const { return units::math::hypot(m_x, m_y); } -bool Translation2d::operator==(const Translation2d& other) const { - return units::math::abs(m_x - other.m_x) < 1E-9_m && - units::math::abs(m_y - other.m_y) < 1E-9_m; -} - Translation2d Translation2d::Nearest( std::span translations) const { return *std::min_element(translations.begin(), translations.end(), diff --git a/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h b/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h new file mode 100644 index 00000000000..e95d6a02c9a --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h @@ -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 +#include +#include +#include + +#include + +#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 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. + * + * 
+   * (x - h)²/a² + (y - k)²/b² = 1
+   *
+ * + * @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 diff --git a/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h b/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h new file mode 100644 index 00000000000..20edf95b245 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h @@ -0,0 +1,205 @@ +// 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 +#include + +#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 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 Pose2d Center() const { return m_center; } + + /** + * Returns the rotational component of the rectangle. + * + * @return The rotational component of the rectangle. + */ + constexpr 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(point.X()) - m_xWidth / 2.0) <= + 1E-9_m) { + // Point rests on left/right perimeter + return units::math::abs(point.Y()) <= m_yWidth / 2.0; + } else if (units::math::abs(units::math::abs(point.Y()) - m_yWidth / 2.0) <= + 1E-9_m) { + // Point rests on top/bottom perimeter + return units::math::abs(point.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(point.X(), m_center.X() - m_xWidth / 2.0, + m_center.X() + m_xWidth / 2.0), + std::clamp(point.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 diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h index 9d9a77c3bf2..cde09fa894a 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h @@ -13,6 +13,7 @@ #include "frc/geometry/Rotation2d.h" #include "units/length.h" +#include "units/math.h" namespace frc { @@ -66,7 +67,9 @@ class WPILIB_DLLEXPORT Translation2d { * * @return The distance between the two translations. */ - units::meter_t Distance(const Translation2d& other) const; + constexpr units::meter_t Distance(const Translation2d& other) const { + return units::math::hypot(other.m_x - m_x, other.m_y - m_y); + } /** * Returns the X component of the translation. @@ -123,6 +126,21 @@ class WPILIB_DLLEXPORT Translation2d { */ constexpr Translation2d RotateBy(const Rotation2d& other) const; + /** + * Rotates this translation around another translation in 2D space. + * + * 
+   * [x_new]   [rot.cos, -rot.sin][x - other.x]   [other.x]
+   * [y_new] = [rot.sin, rot.cos][y - other.y]  + [other.y]
+   *
+ * + * @param other The other translation to rotate around. + * @param rot The rotation to rotate the translation by. + * @return The new rotated translation. + */ + constexpr Translation2d RotateAround(const Translation2d& other, + const Rotation2d& rot) const; + /** * Returns the sum of two translations in 2D space. * @@ -184,7 +202,7 @@ class WPILIB_DLLEXPORT Translation2d { * @param other The other object. * @return Whether the two objects are equal. */ - bool operator==(const Translation2d& other) const; + constexpr bool operator==(const Translation2d& other) const; /** * Returns the nearest Translation2d from a collection of translations diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.inc b/wpimath/src/main/native/include/frc/geometry/Translation2d.inc index 6b291e1f74c..2d1160abb4b 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation2d.inc +++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.inc @@ -6,6 +6,7 @@ #include "frc/geometry/Translation2d.h" #include "units/length.h" +#include "units/math.h" namespace frc { @@ -29,6 +30,14 @@ constexpr Translation2d Translation2d::RotateBy(const Rotation2d& other) const { m_x * other.Sin() + m_y * other.Cos()}; } +constexpr Translation2d Translation2d::RotateAround( + const Translation2d& other, const Rotation2d& rot) const { + return { + (m_x - other.X()) * rot.Cos() - (m_y - other.Y()) * rot.Sin() + other.X(), + (m_x - other.X()) * rot.Sin() + (m_y - other.Y()) * rot.Cos() + + other.Y()}; +} + constexpr Translation2d Translation2d::operator+( const Translation2d& other) const { return {X() + other.X(), Y() + other.Y()}; @@ -51,4 +60,9 @@ constexpr Translation2d Translation2d::operator/(double scalar) const { return operator*(1.0 / scalar); } +constexpr bool Translation2d::operator==(const Translation2d& other) const { + return units::math::abs(m_x - other.m_x) < 1E-9_m && + units::math::abs(m_y - other.m_y) < 1E-9_m; +} + } // namespace frc diff --git a/wpimath/src/main/native/include/units/base.h b/wpimath/src/main/native/include/units/base.h index e7333a91ee4..0e9ae23768e 100644 --- a/wpimath/src/main/native/include/units/base.h +++ b/wpimath/src/main/native/include/units/base.h @@ -3408,7 +3408,7 @@ namespace units { // the "_min" user-defined literal in time.h. template - UnitTypeLhs (min)(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) + constexpr UnitTypeLhs (min)(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) { static_assert(traits::is_convertible_unit_t::value, "Unit types are not compatible."); UnitTypeLhs r(rhs); @@ -3416,7 +3416,7 @@ namespace units { } template - UnitTypeLhs (max)(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) + constexpr UnitTypeLhs (max)(const UnitTypeLhs& lhs, const UnitTypeRhs& rhs) { static_assert(traits::is_convertible_unit_t::value, "Unit types are not compatible."); UnitTypeLhs r(rhs); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java index 16a2def2ebb..566926852e3 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java @@ -18,8 +18,9 @@ void testGetFocalPoints() { var center = new Pose2d(1, 2, new Rotation2d()); var ellipse = new Ellipse2d(center, 5.0, 4.0); - var a = ellipse.getFocalPoint(true); - var b = ellipse.getFocalPoint(false); + var pair = ellipse.getFocalPoints(); + var a = pair.getFirst(); + var b = pair.getSecond(); assertAll( () -> assertEquals(new Translation2d(-2.0, 2.0), a), diff --git a/wpimath/src/test/native/cpp/geometry/Ellipse2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Ellipse2dTest.cpp new file mode 100644 index 00000000000..4c0a7684f1f --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/Ellipse2dTest.cpp @@ -0,0 +1,89 @@ +// 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 + +#include "frc/geometry/Ellipse2d.h" + +TEST(Ellipse2dTest, FocalPoints) { + constexpr frc::Pose2d center{1_m, 2_m, 0_deg}; + constexpr frc::Ellipse2d ellipse{center, 5_m, 4_m}; + + const auto [a, b] = ellipse.FocalPoints(); + + EXPECT_EQ(frc::Translation2d(-2_m, 2_m), a); + EXPECT_EQ(frc::Translation2d(4_m, 2_m), b); +} + +TEST(Ellipse2dTest, IntersectsPoint) { + constexpr frc::Pose2d center{1_m, 2_m, 0_deg}; + constexpr frc::Ellipse2d ellipse{center, 2_m, 1_m}; + + constexpr frc::Translation2d pointA{1_m, 3_m}; + constexpr frc::Translation2d pointB{0_m, 3_m}; + + EXPECT_TRUE(ellipse.Intersects(pointA)); + EXPECT_FALSE(ellipse.Intersects(pointB)); +} + +TEST(Ellipse2dTest, ContainsPoint) { + constexpr frc::Pose2d center{-1_m, -2_m, 45_deg}; + constexpr frc::Ellipse2d ellipse{center, 2_m, 1_m}; + + constexpr frc::Translation2d pointA{0_m, -1_m}; + constexpr frc::Translation2d pointB{0.5_m, -2_m}; + + EXPECT_TRUE(ellipse.Contains(pointA)); + EXPECT_FALSE(ellipse.Contains(pointB)); +} + +TEST(Ellipse2dTest, DistanceToPoint) { + constexpr double kEpsilon = 1E-9; + + constexpr frc::Pose2d center{1_m, 2_m, 270_deg}; + constexpr frc::Ellipse2d ellipse{center, 1_m, 2_m}; + + constexpr frc::Translation2d point1{2.5_m, 2_m}; + constexpr frc::Translation2d point2{1_m, 2_m}; + constexpr frc::Translation2d point3{1_m, 1_m}; + constexpr frc::Translation2d point4{-1_m, 2.5_m}; + + EXPECT_NEAR(0.5, ellipse.Distance(point1).value(), kEpsilon); + EXPECT_NEAR(0, ellipse.Distance(point2).value(), kEpsilon); + EXPECT_NEAR(0.5, ellipse.Distance(point3).value(), kEpsilon); + EXPECT_NEAR(1, ellipse.Distance(point4).value(), kEpsilon); +} + +TEST(Ellipse2dTest, FindNearestPoint) { + constexpr double kEpsilon = 1E-9; + + constexpr frc::Pose2d center{1_m, 1_m, 90_deg}; + constexpr frc::Ellipse2d ellipse{center, 3_m, 4_m}; + + constexpr frc::Translation2d point1{1_m, 3_m}; + auto nearestPoint1 = ellipse.FindNearestPoint(point1); + + frc::Translation2d point2{0_m, 0_m}; + auto nearestPoint2 = ellipse.FindNearestPoint(point2); + + EXPECT_NEAR(1.0, nearestPoint1.X().value(), kEpsilon); + EXPECT_NEAR(2.5, nearestPoint1.Y().value(), kEpsilon); + EXPECT_NEAR(0.0, nearestPoint2.X().value(), kEpsilon); + EXPECT_NEAR(0.0, nearestPoint2.Y().value(), kEpsilon); +} + +TEST(Ellipse2dTest, Equals) { + constexpr frc::Pose2d center1{1_m, 2_m, 90_deg}; + constexpr frc::Ellipse2d ellipse1{center1, 2_m, 3_m}; + + constexpr frc::Pose2d center2{1_m, 2_m, 90_deg}; + constexpr frc::Ellipse2d ellipse2{center2, 2_m, 3_m}; + + constexpr frc::Pose2d center3{1_m, 2_m, 90_deg}; + constexpr frc::Ellipse2d ellipse3{center3, 3_m, 2_m}; + + EXPECT_EQ(ellipse1, ellipse2); + EXPECT_NE(ellipse1, ellipse3); + EXPECT_NE(ellipse3, ellipse2); +} diff --git a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp index 6d689db68b7..a9490fbdcc9 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp @@ -33,7 +33,7 @@ TEST(Pose3dTest, RotateBy) { } TEST(Pose3dTest, TestTransformByRotations) { - const double kEpsilon = 1E-9; + constexpr double kEpsilon = 1E-9; const Pose3d initialPose{0_m, 0_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}}; const Transform3d transform1{Translation3d{0_m, 0_m, 0_m}, diff --git a/wpimath/src/test/native/cpp/geometry/Rectangle2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rectangle2dTest.cpp new file mode 100644 index 00000000000..ceeecd53657 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/Rectangle2dTest.cpp @@ -0,0 +1,87 @@ +// 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 + +#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}; + constexpr frc::Translation2d point2{1_m, 2_m}; + constexpr frc::Translation2d point3{1_m, 1_m}; + constexpr frc::Translation2d point4{-1_m, 2.5_m}; + + EXPECT_NEAR(0.5, rect.Distance(point1).value(), kEpsilon); + EXPECT_NEAR(0, rect.Distance(point2).value(), kEpsilon); + EXPECT_NEAR(0.5, rect.Distance(point3).value(), kEpsilon); + 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); + + frc::Translation2d point2{0_m, 0_m}; + auto nearestPoint2 = rect.FindNearestPoint(point2); + + EXPECT_NEAR(1.0, nearestPoint1.X().value(), kEpsilon); + EXPECT_NEAR(2.5, nearestPoint1.Y().value(), kEpsilon); + 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); +} From 57861b288718cb35f6b01fe5e60db5e207fec959 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 1 Jun 2024 21:42:41 -0700 Subject: [PATCH 18/30] Add Java tests for ellipse distance and nearest-point --- .../first/math/geometry/Ellipse2dTest.java | 37 +++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java index 566926852e3..194abfe9279 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java @@ -13,6 +13,8 @@ import org.junit.jupiter.api.Test; class Ellipse2dTest { + private static final double kEpsilon = 1E-9; + @Test void testGetFocalPoints() { var center = new Pose2d(1, 2, new Rotation2d()); @@ -52,6 +54,41 @@ void testContainsPoint() { () -> assertTrue(ellipse.contains(pointA)), () -> assertFalse(ellipse.contains(pointB))); } + @Test + void testDistanceToPoint() { + var center = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(270.0)); + var ellipse = new Ellipse2d(center, 1.0, 2.0); + + var point1 = new Translation2d(2.5, 2.0); + var point2 = new Translation2d(1.0, 2.0); + var point3 = new Translation2d(1.0, 1.0); + var point4 = new Translation2d(-1.0, 2.5); + + assertAll( + () -> assertEquals(0.5, ellipse.getDistance(point1), kEpsilon), + () -> assertEquals(0.0, ellipse.getDistance(point2), kEpsilon), + () -> assertEquals(0.5, ellipse.getDistance(point3), kEpsilon), + () -> assertEquals(1.0, ellipse.getDistance(point4), kEpsilon)); + } + + @Test + void testFindNearestPoint() { + var center = new Pose2d(1.0, 1.0, Rotation2d.fromDegrees(90.0)); + var ellipse = new Ellipse2d(center, 3.0, 4.0); + + var point1 = new Translation2d(1.0, 3.0); + var nearestPoint1 = ellipse.findNearestPoint(point1); + + var point2 = new Translation2d(0.0, 0.0); + var nearestPoint2 = ellipse.findNearestPoint(point2); + + assertAll( + () -> assertEquals(1.0, nearestPoint1.getX(), kEpsilon), + () -> assertEquals(2.5, nearestPoint1.getY(), kEpsilon), + () -> assertEquals(0.0, nearestPoint2.getX(), kEpsilon), + () -> assertEquals(0.0, nearestPoint2.getY(), kEpsilon)); + } + @Test void testEquals() { var center1 = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(90.0)); From 6236983282cd1116f150c9d350f259f7dc794211 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 2 Jun 2024 10:35:05 -0700 Subject: [PATCH 19/30] Use wpi::array instead of std::array --- .../src/main/native/include/frc/geometry/Ellipse2d.h | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h b/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h index e95d6a02c9a..7573593bcb0 100644 --- a/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h @@ -4,12 +4,10 @@ #pragma once -#include -#include -#include #include #include +#include #include "frc/geometry/Pose2d.h" #include "frc/geometry/Rotation2d.h" @@ -80,17 +78,17 @@ class Ellipse2d { * * @return The focal points. */ - constexpr std::array FocalPoints() const { + constexpr wpi::array 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{ + return wpi::array{ (m_center + Transform2d{-c, 0_m, Rotation2d{}}).Translation(), (m_center + Transform2d{c, 0_m, Rotation2d{}}).Translation()}; } else { - return std::array{ + return wpi::array{ (m_center + Transform2d{0_m, -c, Rotation2d{}}).Translation(), (m_center + Transform2d{0_m, c, Rotation2d{}}).Translation()}; } From 67a17845dda1a3af5ea2ae0ea6e41902059f72aa Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 2 Jun 2024 11:03:52 -0700 Subject: [PATCH 20/30] Fix Rectangle2d::Intersects() --- .../edu/wpi/first/math/geometry/Ellipse2d.java | 17 +++++++++++++++-- .../native/include/frc/geometry/Ellipse2d.h | 17 ++++++++++++++--- .../native/include/frc/geometry/Rectangle2d.h | 14 +++++++------- 3 files changed, 36 insertions(+), 12 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java index 9b28e020d6e..08d4973154a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java @@ -53,6 +53,15 @@ public Pose2d getCenter() { return m_center; } + /** + * Returns the rotational component of the ellipse. + * + * @return The rotational component of the ellipse. + */ + public Rotation2d getRotation() { + return m_center.getRotation(); + } + /** * Returns the x semi-axis. * @@ -78,8 +87,12 @@ public double getYSemiAxis() { * @return The focal points. */ public Pair getFocalPoints() { - double a = Math.max(m_xSemiAxis, m_ySemiAxis); // Major semi-axis - double b = Math.min(m_xSemiAxis, m_ySemiAxis); // Minor semi-axis + // Major semi-axis + double a = Math.max(m_xSemiAxis, m_ySemiAxis); + + // Minor semi-axis + double b = Math.min(m_xSemiAxis, m_ySemiAxis); + double c = Math.sqrt(a * a - b * b); if (m_xSemiAxis > m_ySemiAxis) { diff --git a/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h b/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h index 7573593bcb0..0d87d8dd431 100644 --- a/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h @@ -56,7 +56,14 @@ class Ellipse2d { * * @return The center of the ellipse. */ - constexpr Pose2d Center() const { return m_center; } + constexpr const Pose2d& Center() const { return m_center; } + + /** + * Returns the rotational component of the ellipse. + * + * @return The rotational component of the ellipse. + */ + constexpr const Rotation2d& Rotation() const { return m_center.Rotation(); } /** * Returns the x semi-axis. @@ -79,8 +86,12 @@ class Ellipse2d { * @return The focal points. */ constexpr wpi::array 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 + // Major semi-axis + auto a = units::math::max(m_xSemiAxis, m_ySemiAxis); + + // Minor semi-axis + auto b = units::math::min(m_xSemiAxis, m_ySemiAxis); // NOLINT + auto c = units::math::sqrt(a * a - b * b); if (m_xSemiAxis > m_ySemiAxis) { diff --git a/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h b/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h index 20edf95b245..f5b3d6c2b7e 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h @@ -59,14 +59,14 @@ class Rectangle2d { * * @return The center of the rectangle. */ - constexpr Pose2d Center() const { return m_center; } + constexpr const Pose2d& Center() const { return m_center; } /** * Returns the rotational component of the rectangle. * * @return The rotational component of the rectangle. */ - constexpr Rotation2d Rotation() const { return m_center.Rotation(); } + constexpr const Rotation2d& Rotation() const { return m_center.Rotation(); } /** * Returns the x size component of the rectangle. @@ -113,14 +113,14 @@ class Rectangle2d { auto pointInRect = point - m_center.Translation(); pointInRect = pointInRect.RotateBy(-m_center.Rotation()); - if (units::math::abs(units::math::abs(point.X()) - m_xWidth / 2.0) <= + 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(point.Y()) <= m_yWidth / 2.0; - } else if (units::math::abs(units::math::abs(point.Y()) - m_yWidth / 2.0) <= - 1E-9_m) { + 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(point.X()) <= m_xWidth / 2.0; + return units::math::abs(pointInRect.X()) <= m_xWidth / 2.0; } return false; From 2dfbfc6d83164c50843f5d35a7b05675e8a93fc1 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 2 Jun 2024 11:05:35 -0700 Subject: [PATCH 21/30] Fix Rectangle2d::FindNearestPoint() --- wpimath/src/main/native/include/frc/geometry/Rectangle2d.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h b/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h index f5b3d6c2b7e..a019455960d 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h @@ -175,9 +175,9 @@ class Rectangle2d { // Find nearest point rotPoint = - Translation2d{std::clamp(point.X(), m_center.X() - m_xWidth / 2.0, + Translation2d{std::clamp(rotPoint.X(), m_center.X() - m_xWidth / 2.0, m_center.X() + m_xWidth / 2.0), - std::clamp(point.Y(), m_center.Y() - m_yWidth / 2.0, + std::clamp(rotPoint.Y(), m_center.Y() - m_yWidth / 2.0, m_center.Y() + m_yWidth / 2.0)}; // Undo rotation From 645a69e389092ff9feab0e3427cb6a94f456e726 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 2 Jun 2024 11:54:58 -0700 Subject: [PATCH 22/30] Fix Ellipse2d.findNearestPoint() --- .../java/edu/wpi/first/math/WPIMathJNI.java | 26 +++++++++++++ .../wpi/first/math/geometry/Ellipse2d.java | 19 +++++---- .../native/cpp/jni/WPIMathJNI_Ellipse2d.cpp | 39 +++++++++++++++++++ .../native/include/frc/geometry/Ellipse2d.h | 2 +- .../first/math/geometry/Ellipse2dTest.java | 8 ++-- .../native/cpp/geometry/Ellipse2dTest.cpp | 8 ++-- 6 files changed, 86 insertions(+), 16 deletions(-) create mode 100644 wpimath/src/main/native/cpp/jni/WPIMathJNI_Ellipse2d.cpp diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java index 517c13297bf..c723f29a68e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java +++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java @@ -279,6 +279,32 @@ public static native void rankUpdate( public static native void solveFullPivHouseholderQr( double[] A, int Arows, int Acols, double[] B, int Brows, int Bcols, double[] dst); + // Ellipse2d wrappers + + /** + * Returns the nearest point that is contained within the ellipse. + * + *

Constructs an Ellipse2d object and runs its FindNearestPoint() method. + * + * @param centerX The x coordinate of the center of the ellipse in meters. + * @param centerY The y coordinate of the center of the ellipse in meters. + * @param centerHeading The ellipse's rotation in radians. + * @param xSemiAxis The x semi-axis in meters. + * @param ySemiAxis The y semi-axis in meters. + * @param pointX The x coordinate of the point that this will find the nearest point to. + * @param pointY The y coordinate of the point that this will find the nearest point to. + * @param nearestPoint Array to store nearest point into. + */ + public static native void Ellipse2dFindNearestPoint( + double centerX, + double centerY, + double centerHeading, + double xSemiAxis, + double ySemiAxis, + double pointX, + double pointY, + double[] nearestPoint); + // Pose3d wrappers /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java index 08d4973154a..1d3bae77fb9 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java @@ -5,6 +5,7 @@ package edu.wpi.first.math.geometry; import edu.wpi.first.math.Pair; +import edu.wpi.first.math.WPIMathJNI; import edu.wpi.first.math.geometry.proto.Ellipse2dProto; import edu.wpi.first.math.geometry.struct.Ellipse2dStruct; import edu.wpi.first.util.protobuf.ProtobufSerializable; @@ -169,14 +170,18 @@ public Translation2d findNearestPoint(Translation2d point) { return point; } - // Rotate the point by the inverse of the ellipse's rotation - point = point.rotateAround(m_center.getTranslation(), m_center.getRotation().unaryMinus()); - // Find nearest point - // TODO - - // Undo rotation - return point.rotateAround(m_center.getTranslation(), m_center.getRotation()); + var nearestPoint = new double[2]; + WPIMathJNI.Ellipse2dFindNearestPoint( + m_center.getX(), + m_center.getY(), + m_center.getRotation().getRadians(), + m_xSemiAxis, + m_ySemiAxis, + point.getX(), + point.getY(), + nearestPoint); + return new Translation2d(nearestPoint[0], nearestPoint[1]); } @Override diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Ellipse2d.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Ellipse2d.cpp new file mode 100644 index 00000000000..1c9f5395f9c --- /dev/null +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Ellipse2d.cpp @@ -0,0 +1,39 @@ +// 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 + +#include +#include + +#include "edu_wpi_first_math_WPIMathJNI.h" +#include "frc/geometry/Ellipse2d.h" + +using namespace wpi::java; + +extern "C" { + +/* + * Class: edu_wpi_first_math_WPIMathJNI + * Method: Ellipse2dFindNearestPoint + * Signature: (DDDDDDD[D)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_math_WPIMathJNI_Ellipse2dFindNearestPoint + (JNIEnv* env, jclass, jdouble centerX, jdouble centerY, jdouble centerHeading, + jdouble xSemiAxis, jdouble ySemiAxis, jdouble pointX, jdouble pointY, + jdoubleArray nearestPoint) +{ + auto point = + frc::Ellipse2d{ + frc::Pose2d{units::meter_t{centerX}, units::meter_t{centerY}, + units::radian_t{centerHeading}}, + units::meter_t{xSemiAxis}, units::meter_t{ySemiAxis}} + .FindNearestPoint({units::meter_t{pointX}, units::meter_t{pointY}}); + + wpi::array buf{point.X().value(), point.Y().value()}; + env->SetDoubleArrayRegion(nearestPoint, 0, 2, buf.data()); +} + +} // extern "C" diff --git a/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h b/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h index 0d87d8dd431..641138f5f50 100644 --- a/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h @@ -159,7 +159,7 @@ class Ellipse2d { * * @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. + * ellipse. */ Translation2d FindNearestPoint(const Translation2d& point) const; diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java index 194abfe9279..47b20167826 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java @@ -65,10 +65,10 @@ void testDistanceToPoint() { var point4 = new Translation2d(-1.0, 2.5); assertAll( - () -> assertEquals(0.5, ellipse.getDistance(point1), kEpsilon), + () -> assertEquals(0.0, ellipse.getDistance(point1), kEpsilon), () -> assertEquals(0.0, ellipse.getDistance(point2), kEpsilon), - () -> assertEquals(0.5, ellipse.getDistance(point3), kEpsilon), - () -> assertEquals(1.0, ellipse.getDistance(point4), kEpsilon)); + () -> assertEquals(0.0, ellipse.getDistance(point3), kEpsilon), + () -> assertEquals(0.19210128384806818, ellipse.getDistance(point4), kEpsilon)); } @Test @@ -84,7 +84,7 @@ void testFindNearestPoint() { assertAll( () -> assertEquals(1.0, nearestPoint1.getX(), kEpsilon), - () -> assertEquals(2.5, nearestPoint1.getY(), kEpsilon), + () -> assertEquals(3.0, nearestPoint1.getY(), kEpsilon), () -> assertEquals(0.0, nearestPoint2.getX(), kEpsilon), () -> assertEquals(0.0, nearestPoint2.getY(), kEpsilon)); } diff --git a/wpimath/src/test/native/cpp/geometry/Ellipse2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Ellipse2dTest.cpp index 4c0a7684f1f..12668a4de9b 100644 --- a/wpimath/src/test/native/cpp/geometry/Ellipse2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Ellipse2dTest.cpp @@ -49,10 +49,10 @@ TEST(Ellipse2dTest, DistanceToPoint) { constexpr frc::Translation2d point3{1_m, 1_m}; constexpr frc::Translation2d point4{-1_m, 2.5_m}; - EXPECT_NEAR(0.5, ellipse.Distance(point1).value(), kEpsilon); + EXPECT_NEAR(0, ellipse.Distance(point1).value(), kEpsilon); EXPECT_NEAR(0, ellipse.Distance(point2).value(), kEpsilon); - EXPECT_NEAR(0.5, ellipse.Distance(point3).value(), kEpsilon); - EXPECT_NEAR(1, ellipse.Distance(point4).value(), kEpsilon); + EXPECT_NEAR(0, ellipse.Distance(point3).value(), kEpsilon); + EXPECT_NEAR(0.19210128384806818, ellipse.Distance(point4).value(), kEpsilon); } TEST(Ellipse2dTest, FindNearestPoint) { @@ -68,7 +68,7 @@ TEST(Ellipse2dTest, FindNearestPoint) { auto nearestPoint2 = ellipse.FindNearestPoint(point2); EXPECT_NEAR(1.0, nearestPoint1.X().value(), kEpsilon); - EXPECT_NEAR(2.5, nearestPoint1.Y().value(), kEpsilon); + EXPECT_NEAR(3.0, nearestPoint1.Y().value(), kEpsilon); EXPECT_NEAR(0.0, nearestPoint2.X().value(), kEpsilon); EXPECT_NEAR(0.0, nearestPoint2.Y().value(), kEpsilon); } From 10462d918b7ce5a348d49561266aac8b318523b4 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 2 Jun 2024 12:17:48 -0700 Subject: [PATCH 23/30] Build Ellipse2d JNI in CMake --- wpimath/CMakeLists.txt | 1 + wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java | 2 +- .../src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java | 2 +- wpimath/src/main/native/cpp/jni/WPIMathJNI_Ellipse2d.cpp | 4 ++-- 4 files changed, 5 insertions(+), 4 deletions(-) diff --git a/wpimath/CMakeLists.txt b/wpimath/CMakeLists.txt index 45badc6fc3d..d0984731b58 100644 --- a/wpimath/CMakeLists.txt +++ b/wpimath/CMakeLists.txt @@ -22,6 +22,7 @@ file( src/main/native/cpp/jni/WPIMathJNI_ArmFeedforward.cpp src/main/native/cpp/jni/WPIMathJNI_DARE.cpp src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp + src/main/native/cpp/jni/WPIMathJNI_Ellipse2d.cpp src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp src/main/native/cpp/jni/WPIMathJNI_Pose3d.cpp src/main/native/cpp/jni/WPIMathJNI_StateSpaceUtil.cpp diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java index c723f29a68e..10a9147c7d2 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java +++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java @@ -295,7 +295,7 @@ public static native void solveFullPivHouseholderQr( * @param pointY The y coordinate of the point that this will find the nearest point to. * @param nearestPoint Array to store nearest point into. */ - public static native void Ellipse2dFindNearestPoint( + public static native void ellipse2dFindNearestPoint( double centerX, double centerY, double centerHeading, diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java index 1d3bae77fb9..e52464a30e6 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java @@ -172,7 +172,7 @@ public Translation2d findNearestPoint(Translation2d point) { // Find nearest point var nearestPoint = new double[2]; - WPIMathJNI.Ellipse2dFindNearestPoint( + WPIMathJNI.ellipse2dFindNearestPoint( m_center.getX(), m_center.getY(), m_center.getRotation().getRadians(), diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Ellipse2d.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Ellipse2d.cpp index 1c9f5395f9c..ae6f88a21ee 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI_Ellipse2d.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Ellipse2d.cpp @@ -16,11 +16,11 @@ extern "C" { /* * Class: edu_wpi_first_math_WPIMathJNI - * Method: Ellipse2dFindNearestPoint + * Method: ellipse2dFindNearestPoint * Signature: (DDDDDDD[D)V */ JNIEXPORT void JNICALL -Java_edu_wpi_first_math_WPIMathJNI_Ellipse2dFindNearestPoint +Java_edu_wpi_first_math_WPIMathJNI_ellipse2dFindNearestPoint (JNIEnv* env, jclass, jdouble centerX, jdouble centerY, jdouble centerHeading, jdouble xSemiAxis, jdouble ySemiAxis, jdouble pointX, jdouble pointY, jdoubleArray nearestPoint) From 9e916192c5f98a7418bfd64a2dd7d963dc405d3e Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 2 Jun 2024 12:36:05 -0700 Subject: [PATCH 24/30] Refactor tests --- .../first/math/geometry/Ellipse2dTest.java | 41 ++++++++++++------- .../first/math/geometry/Rectangle2dTest.java | 19 +++++---- .../native/cpp/geometry/Ellipse2dTest.cpp | 35 ++++++++++------ .../native/cpp/geometry/Rectangle2dTest.cpp | 17 ++++---- 4 files changed, 69 insertions(+), 43 deletions(-) diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java index 47b20167826..28c9d41d015 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java @@ -60,33 +60,46 @@ void testDistanceToPoint() { var ellipse = new Ellipse2d(center, 1.0, 2.0); var point1 = new Translation2d(2.5, 2.0); + assertEquals(0.0, ellipse.getDistance(point1), kEpsilon); + var point2 = new Translation2d(1.0, 2.0); + assertEquals(0.0, ellipse.getDistance(point2), kEpsilon); + var point3 = new Translation2d(1.0, 1.0); - var point4 = new Translation2d(-1.0, 2.5); + assertEquals(0.0, ellipse.getDistance(point3), kEpsilon); - assertAll( - () -> assertEquals(0.0, ellipse.getDistance(point1), kEpsilon), - () -> assertEquals(0.0, ellipse.getDistance(point2), kEpsilon), - () -> assertEquals(0.0, ellipse.getDistance(point3), kEpsilon), - () -> assertEquals(0.19210128384806818, ellipse.getDistance(point4), kEpsilon)); + var point4 = new Translation2d(-1.0, 2.5); + assertEquals(0.19210128384806818, ellipse.getDistance(point4), kEpsilon); } @Test void testFindNearestPoint() { - var center = new Pose2d(1.0, 1.0, Rotation2d.fromDegrees(90.0)); - var ellipse = new Ellipse2d(center, 3.0, 4.0); + var center = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(270.0)); + var ellipse = new Ellipse2d(center, 1.0, 2.0); - var point1 = new Translation2d(1.0, 3.0); + var point1 = new Translation2d(2.5, 2.0); var nearestPoint1 = ellipse.findNearestPoint(point1); + assertAll( + () -> assertEquals(2.5, nearestPoint1.getX(), kEpsilon), + () -> assertEquals(2.0, nearestPoint1.getY(), kEpsilon)); - var point2 = new Translation2d(0.0, 0.0); + var point2 = new Translation2d(1.0, 2.0); var nearestPoint2 = ellipse.findNearestPoint(point2); + assertAll( + () -> assertEquals(1.0, nearestPoint2.getX(), kEpsilon), + () -> assertEquals(2.0, nearestPoint2.getY(), kEpsilon)); + var point3 = new Translation2d(1.0, 1.0); + var nearestPoint3 = ellipse.findNearestPoint(point3); + assertAll( + () -> assertEquals(1.0, nearestPoint3.getX(), kEpsilon), + () -> assertEquals(1.0, nearestPoint3.getY(), kEpsilon)); + + var point4 = new Translation2d(-1.0, 2.5); + var nearestPoint4 = ellipse.findNearestPoint(point4); assertAll( - () -> assertEquals(1.0, nearestPoint1.getX(), kEpsilon), - () -> assertEquals(3.0, nearestPoint1.getY(), kEpsilon), - () -> assertEquals(0.0, nearestPoint2.getX(), kEpsilon), - () -> assertEquals(0.0, nearestPoint2.getY(), kEpsilon)); + () -> assertEquals(-0.8512799937611617, nearestPoint4.getX(), kEpsilon), + () -> assertEquals(2.378405333174535, nearestPoint4.getY(), kEpsilon)); } @Test diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java index 0f6f92b156e..7ee86d6d7db 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rectangle2dTest.java @@ -58,15 +58,16 @@ void testDistanceToPoint() { var rect = new Rectangle2d(center, 1.0, 2.0); var point1 = new Translation2d(2.5, 2.0); + assertEquals(0.5, rect.getDistance(point1), kEpsilon); + var point2 = new Translation2d(1.0, 2.0); + assertEquals(0.0, rect.getDistance(point2), kEpsilon); + var point3 = new Translation2d(1.0, 1.0); - var point4 = new Translation2d(-1.0, 2.5); + assertEquals(0.5, rect.getDistance(point3), kEpsilon); - assertAll( - () -> assertEquals(0.5, rect.getDistance(point1), kEpsilon), - () -> assertEquals(0.0, rect.getDistance(point2), kEpsilon), - () -> assertEquals(0.5, rect.getDistance(point3), kEpsilon), - () -> assertEquals(1.0, rect.getDistance(point4), kEpsilon)); + var point4 = new Translation2d(-1.0, 2.5); + assertEquals(1.0, rect.getDistance(point4), kEpsilon); } @Test @@ -76,13 +77,13 @@ void testFindNearestPoint() { var point1 = new Translation2d(1.0, 3.0); var nearestPoint1 = rect.findNearestPoint(point1); + assertAll( + () -> assertEquals(1.0, nearestPoint1.getX(), kEpsilon), + () -> assertEquals(2.5, nearestPoint1.getY(), kEpsilon)); var point2 = new Translation2d(0.0, 0.0); var nearestPoint2 = rect.findNearestPoint(point2); - assertAll( - () -> assertEquals(1.0, nearestPoint1.getX(), kEpsilon), - () -> assertEquals(2.5, nearestPoint1.getY(), kEpsilon), () -> assertEquals(0.0, nearestPoint2.getX(), kEpsilon), () -> assertEquals(0.0, nearestPoint2.getY(), kEpsilon)); } diff --git a/wpimath/src/test/native/cpp/geometry/Ellipse2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Ellipse2dTest.cpp index 12668a4de9b..c51e50e4b61 100644 --- a/wpimath/src/test/native/cpp/geometry/Ellipse2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Ellipse2dTest.cpp @@ -45,32 +45,43 @@ TEST(Ellipse2dTest, DistanceToPoint) { constexpr frc::Ellipse2d ellipse{center, 1_m, 2_m}; constexpr frc::Translation2d point1{2.5_m, 2_m}; - constexpr frc::Translation2d point2{1_m, 2_m}; - constexpr frc::Translation2d point3{1_m, 1_m}; - constexpr frc::Translation2d point4{-1_m, 2.5_m}; - EXPECT_NEAR(0, ellipse.Distance(point1).value(), kEpsilon); + + constexpr frc::Translation2d point2{1_m, 2_m}; EXPECT_NEAR(0, ellipse.Distance(point2).value(), kEpsilon); + + constexpr frc::Translation2d point3{1_m, 1_m}; EXPECT_NEAR(0, ellipse.Distance(point3).value(), kEpsilon); + + constexpr frc::Translation2d point4{-1_m, 2.5_m}; EXPECT_NEAR(0.19210128384806818, ellipse.Distance(point4).value(), kEpsilon); } TEST(Ellipse2dTest, FindNearestPoint) { constexpr double kEpsilon = 1E-9; - constexpr frc::Pose2d center{1_m, 1_m, 90_deg}; - constexpr frc::Ellipse2d ellipse{center, 3_m, 4_m}; + constexpr frc::Pose2d center{1_m, 2_m, 270_deg}; + constexpr frc::Ellipse2d ellipse{center, 1_m, 2_m}; - constexpr frc::Translation2d point1{1_m, 3_m}; + constexpr frc::Translation2d point1{2.5_m, 2_m}; auto nearestPoint1 = ellipse.FindNearestPoint(point1); + EXPECT_NEAR(2.5, nearestPoint1.X().value(), kEpsilon); + EXPECT_NEAR(2.0, nearestPoint1.Y().value(), kEpsilon); - frc::Translation2d point2{0_m, 0_m}; + constexpr frc::Translation2d point2{1_m, 2_m}; auto nearestPoint2 = ellipse.FindNearestPoint(point2); + EXPECT_NEAR(1.0, nearestPoint2.X().value(), kEpsilon); + EXPECT_NEAR(2.0, nearestPoint2.Y().value(), kEpsilon); + + constexpr frc::Translation2d point3{1_m, 1_m}; + auto nearestPoint3 = ellipse.FindNearestPoint(point3); + EXPECT_NEAR(1.0, nearestPoint3.X().value(), kEpsilon); + EXPECT_NEAR(1.0, nearestPoint3.Y().value(), kEpsilon); - EXPECT_NEAR(1.0, nearestPoint1.X().value(), kEpsilon); - EXPECT_NEAR(3.0, nearestPoint1.Y().value(), kEpsilon); - EXPECT_NEAR(0.0, nearestPoint2.X().value(), kEpsilon); - EXPECT_NEAR(0.0, nearestPoint2.Y().value(), kEpsilon); + constexpr frc::Translation2d point4{-1_m, 2.5_m}; + auto nearestPoint4 = ellipse.FindNearestPoint(point4); + EXPECT_NEAR(-0.8512799937611617, nearestPoint4.X().value(), kEpsilon); + EXPECT_NEAR(2.378405333174535, nearestPoint4.Y().value(), kEpsilon); } TEST(Ellipse2dTest, Equals) { diff --git a/wpimath/src/test/native/cpp/geometry/Rectangle2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rectangle2dTest.cpp index ceeecd53657..f4d2d4350e2 100644 --- a/wpimath/src/test/native/cpp/geometry/Rectangle2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rectangle2dTest.cpp @@ -44,13 +44,15 @@ TEST(Rectangle2dTest, DistanceToPoint) { constexpr frc::Rectangle2d rect{center, 1_m, 2_m}; constexpr frc::Translation2d point1{2.5_m, 2_m}; - constexpr frc::Translation2d point2{1_m, 2_m}; - constexpr frc::Translation2d point3{1_m, 1_m}; - constexpr frc::Translation2d point4{-1_m, 2.5_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); } @@ -62,12 +64,11 @@ TEST(Rectangle2dTest, FindNearestPoint) { constexpr frc::Translation2d point1{1_m, 3_m}; auto nearestPoint1 = rect.FindNearestPoint(point1); - - frc::Translation2d point2{0_m, 0_m}; - auto nearestPoint2 = rect.FindNearestPoint(point2); - 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); } From 6d18ef0af6afcc8073b0a379ff34e98bb5939bbc Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 2 Jun 2024 12:39:26 -0700 Subject: [PATCH 25/30] Fix gcem min/max --- ...001-Call-std-functions-if-not-constant-evaluated.patch | 8 ++++---- .../main/native/thirdparty/gcem/include/gcem_incl/max.hpp | 2 +- .../main/native/thirdparty/gcem/include/gcem_incl/min.hpp | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/upstream_utils/gcem_patches/0001-Call-std-functions-if-not-constant-evaluated.patch b/upstream_utils/gcem_patches/0001-Call-std-functions-if-not-constant-evaluated.patch index b7742f0e2b8..207359f6771 100644 --- a/upstream_utils/gcem_patches/0001-Call-std-functions-if-not-constant-evaluated.patch +++ b/upstream_utils/gcem_patches/0001-Call-std-functions-if-not-constant-evaluated.patch @@ -1454,14 +1454,14 @@ index af23ea21d247327fa224370544e5f4410eac214b..4c95110d627568577440e5e662e72fde + #endif diff --git a/include/gcem_incl/max.hpp b/include/gcem_incl/max.hpp -index ddc3e4e6caff1a781e662a3ded8909cb703729ab..9f3901b4b19eb35331cd22be16c5b3a1ab5f65d6 100644 +index ddc3e4e6caff1a781e662a3ded8909cb703729ab..258031508ec490b49c71dbf60f3c5669f4c7c380 100644 --- a/include/gcem_incl/max.hpp +++ b/include/gcem_incl/max.hpp @@ -21,6 +21,12 @@ #ifndef _gcem_max_HPP #define _gcem_max_HPP -+#include ++#include +#include + +namespace gcem @@ -1485,14 +1485,14 @@ index ddc3e4e6caff1a781e662a3ded8909cb703729ab..9f3901b4b19eb35331cd22be16c5b3a1 #endif diff --git a/include/gcem_incl/min.hpp b/include/gcem_incl/min.hpp -index 5ce70b38e6d243267a053ec33fae31e59f6a359f..a35bcf6e2cb65f8712b873d3ef2827aca2d4d0f0 100644 +index 5ce70b38e6d243267a053ec33fae31e59f6a359f..af1be618776413f8de8bcce5d56fc838ee5c968a 100644 --- a/include/gcem_incl/min.hpp +++ b/include/gcem_incl/min.hpp @@ -21,6 +21,12 @@ #ifndef _gcem_min_HPP #define _gcem_min_HPP -+#include ++#include +#include + +namespace gcem diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp index 9f3901b4b19..258031508ec 100644 --- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/max.hpp @@ -21,7 +21,7 @@ #ifndef _gcem_max_HPP #define _gcem_max_HPP -#include +#include #include namespace gcem diff --git a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp index a35bcf6e2cb..af1be618776 100644 --- a/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp +++ b/wpimath/src/main/native/thirdparty/gcem/include/gcem_incl/min.hpp @@ -21,7 +21,7 @@ #ifndef _gcem_min_HPP #define _gcem_min_HPP -#include +#include #include namespace gcem From 255ff677fcc5800256694bcd31a759273e4c96bb Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 2 Jun 2024 13:00:55 -0700 Subject: [PATCH 26/30] Export Ellipse2d and Rectangle2d --- wpimath/src/main/native/include/frc/geometry/Ellipse2d.h | 3 ++- wpimath/src/main/native/include/frc/geometry/Rectangle2d.h | 4 +++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h b/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h index 641138f5f50..c14464f51e0 100644 --- a/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h @@ -7,6 +7,7 @@ #include #include +#include #include #include "frc/geometry/Pose2d.h" @@ -22,7 +23,7 @@ namespace frc { * Represents a 2d ellipse space containing translational, rotational, and * scaling components. */ -class Ellipse2d { +class WPILIB_DLLEXPORT Ellipse2d { public: /** * Constructs an ellipse around a center point and two semi-axes, a horizontal diff --git a/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h b/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h index a019455960d..2f742f9f4ae 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h @@ -7,6 +7,8 @@ #include #include +#include + #include "frc/geometry/Pose2d.h" #include "frc/geometry/Rotation2d.h" #include "frc/geometry/Transform2d.h" @@ -20,7 +22,7 @@ namespace frc { * Represents a 2d rectangular space containing translational, rotational, and * scaling components. */ -class Rectangle2d { +class WPILIB_DLLEXPORT Rectangle2d { public: /** * Constructs a rectangle at the specified position with the specified width From 89688914f136d46f1d11c13a5e3f93d21718bc7e Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 2 Jun 2024 13:12:15 -0700 Subject: [PATCH 27/30] Fix comment alignment --- .../main/java/edu/wpi/first/math/geometry/Translation2d.java | 2 +- wpimath/src/main/native/include/frc/geometry/Translation2d.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java index c4f1242b7ca..9899b97ef46 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java @@ -184,7 +184,7 @@ public Translation2d rotateBy(Rotation2d other) { * * 

    * [x_new]   [rot.cos, -rot.sin][x - other.x]   [other.x]
-   * [y_new] = [rot.sin, rot.cos][y - other.y]  + [other.y]
+   * [y_new] = [rot.sin,  rot.cos][y - other.y] + [other.y]
    *
* * @param other The other translation to rotate around. diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h index cde09fa894a..674bd9efe16 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h @@ -131,7 +131,7 @@ class WPILIB_DLLEXPORT Translation2d { * * 
    * [x_new]   [rot.cos, -rot.sin][x - other.x]   [other.x]
-   * [y_new] = [rot.sin, rot.cos][y - other.y]  + [other.y]
+   * [y_new] = [rot.sin,  rot.cos][y - other.y] + [other.y]
    *
* * @param other The other translation to rotate around. From 623601e99b922ea472e98770caf1c35f94554716 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 2 Jun 2024 14:27:02 -0700 Subject: [PATCH 28/30] Implement Rectangle2d and Ellipse2d serialization --- .../main/native/cpp/geometry/Ellipse2d.cpp | 2 ++ .../main/native/cpp/geometry/Rectangle2d.cpp | 7 ++++ .../cpp/geometry/proto/Ellipse2dProto.cpp | 31 ++++++++++++++++ .../cpp/geometry/proto/Rectangle2dProto.cpp | 31 ++++++++++++++++ .../cpp/geometry/struct/Ellipse2dStruct.cpp | 27 ++++++++++++++ .../cpp/geometry/struct/Rectangle2dStruct.cpp | 27 ++++++++++++++ .../native/include/frc/geometry/Ellipse2d.h | 3 ++ .../native/include/frc/geometry/Rectangle2d.h | 3 ++ .../frc/geometry/proto/Ellipse2dProto.h | 17 +++++++++ .../frc/geometry/proto/Rectangle2dProto.h | 18 ++++++++++ .../frc/geometry/struct/Ellipse2dStruct.h | 35 +++++++++++++++++++ .../frc/geometry/struct/Rectangle2dStruct.h | 35 +++++++++++++++++++ .../cpp/geometry/proto/Ellipse2dProtoTest.cpp | 29 +++++++++++++++ .../geometry/proto/Rectangle2dProtoTest.cpp | 29 +++++++++++++++ .../geometry/struct/Ellipse2dStructTest.cpp | 28 +++++++++++++++ .../geometry/struct/Rectangle2dStructTest.cpp | 28 +++++++++++++++ 16 files changed, 350 insertions(+) create mode 100644 wpimath/src/main/native/cpp/geometry/Rectangle2d.cpp create mode 100644 wpimath/src/main/native/cpp/geometry/proto/Ellipse2dProto.cpp create mode 100644 wpimath/src/main/native/cpp/geometry/proto/Rectangle2dProto.cpp create mode 100644 wpimath/src/main/native/cpp/geometry/struct/Ellipse2dStruct.cpp create mode 100644 wpimath/src/main/native/cpp/geometry/struct/Rectangle2dStruct.cpp create mode 100644 wpimath/src/main/native/include/frc/geometry/proto/Ellipse2dProto.h create mode 100644 wpimath/src/main/native/include/frc/geometry/proto/Rectangle2dProto.h create mode 100644 wpimath/src/main/native/include/frc/geometry/struct/Ellipse2dStruct.h create mode 100644 wpimath/src/main/native/include/frc/geometry/struct/Rectangle2dStruct.h create mode 100644 wpimath/src/test/native/cpp/geometry/proto/Ellipse2dProtoTest.cpp create mode 100644 wpimath/src/test/native/cpp/geometry/proto/Rectangle2dProtoTest.cpp create mode 100644 wpimath/src/test/native/cpp/geometry/struct/Ellipse2dStructTest.cpp create mode 100644 wpimath/src/test/native/cpp/geometry/struct/Rectangle2dStructTest.cpp diff --git a/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp b/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp index 6f8afa636a2..4288b8b44bf 100644 --- a/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp @@ -6,6 +6,8 @@ #include +#include "geometry2d.pb.h" + using namespace frc; units::meter_t Ellipse2d::Distance(const Translation2d& point) const { diff --git a/wpimath/src/main/native/cpp/geometry/Rectangle2d.cpp b/wpimath/src/main/native/cpp/geometry/Rectangle2d.cpp new file mode 100644 index 00000000000..0d1f77bc64f --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/Rectangle2d.cpp @@ -0,0 +1,7 @@ +// 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/Rectangle2d.h" + +#include "geometry2d.pb.h" diff --git a/wpimath/src/main/native/cpp/geometry/proto/Ellipse2dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Ellipse2dProto.cpp new file mode 100644 index 00000000000..adfc28d3c24 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/proto/Ellipse2dProto.cpp @@ -0,0 +1,31 @@ +// 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/proto/Ellipse2dProto.h" + +#include "geometry2d.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage( + arena); +} + +frc::Ellipse2d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Ellipse2d{ + wpi::UnpackProtobuf(m->center()), + units::meter_t{m->xsemiaxis()}, + units::meter_t{m->ysemiaxis()}, + }; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Ellipse2d& value) { + auto m = static_cast(msg); + wpi::PackProtobuf(m->mutable_center(), value.Center()); + m->set_xsemiaxis(value.XSemiAxis().value()); + m->set_ysemiaxis(value.YSemiAxis().value()); +} diff --git a/wpimath/src/main/native/cpp/geometry/proto/Rectangle2dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Rectangle2dProto.cpp new file mode 100644 index 00000000000..98d44302187 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/proto/Rectangle2dProto.cpp @@ -0,0 +1,31 @@ +// 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/proto/Rectangle2dProto.h" + +#include "geometry2d.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufRectangle2d>(arena); +} + +frc::Rectangle2d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Rectangle2d{ + wpi::UnpackProtobuf(m->center()), + units::meter_t{m->xwidth()}, + units::meter_t{m->ywidth()}, + }; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Rectangle2d& value) { + auto m = static_cast(msg); + wpi::PackProtobuf(m->mutable_center(), value.Center()); + m->set_xwidth(value.XWidth().value()); + m->set_ywidth(value.YWidth().value()); +} diff --git a/wpimath/src/main/native/cpp/geometry/struct/Ellipse2dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Ellipse2dStruct.cpp new file mode 100644 index 00000000000..b646823b5a8 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/struct/Ellipse2dStruct.cpp @@ -0,0 +1,27 @@ +// 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/struct/Ellipse2dStruct.h" + +namespace { +constexpr size_t kCenterOff = 0; +constexpr size_t kXSemiAxisOff = kCenterOff + wpi::GetStructSize(); +constexpr size_t kYSemiAxisOff = kXSemiAxisOff + sizeof(units::meter_t); +} // namespace + +using StructType = wpi::Struct; + +frc::Ellipse2d StructType::Unpack(std::span data) { + return frc::Ellipse2d{ + wpi::UnpackStruct(data), + units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, const frc::Ellipse2d& value) { + wpi::PackStruct(data, value.Center()); + wpi::PackStruct(data, value.XSemiAxis().value()); + wpi::PackStruct(data, value.YSemiAxis().value()); +} diff --git a/wpimath/src/main/native/cpp/geometry/struct/Rectangle2dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Rectangle2dStruct.cpp new file mode 100644 index 00000000000..988428083cc --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/struct/Rectangle2dStruct.cpp @@ -0,0 +1,27 @@ +// 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/struct/Rectangle2dStruct.h" + +namespace { +constexpr size_t kCenterOff = 0; +constexpr size_t kXWidthOff = kCenterOff + wpi::GetStructSize(); +constexpr size_t kYWidthOff = kXWidthOff + sizeof(units::meter_t); +} // namespace + +using StructType = wpi::Struct; + +frc::Rectangle2d StructType::Unpack(std::span data) { + return frc::Rectangle2d{ + wpi::UnpackStruct(data), + units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, const frc::Rectangle2d& value) { + wpi::PackStruct(data, value.Center()); + wpi::PackStruct(data, value.XWidth().value()); + wpi::PackStruct(data, value.YWidth().value()); +} diff --git a/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h b/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h index c14464f51e0..f57303d1e87 100644 --- a/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h @@ -207,3 +207,6 @@ class WPILIB_DLLEXPORT Ellipse2d { }; } // namespace frc + +#include "frc/geometry/proto/Ellipse2dProto.h" +#include "frc/geometry/struct/Ellipse2dStruct.h" diff --git a/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h b/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h index 2f742f9f4ae..f03ca98c448 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h @@ -205,3 +205,6 @@ class WPILIB_DLLEXPORT Rectangle2d { }; } // namespace frc + +#include "frc/geometry/proto/Rectangle2dProto.h" +#include "frc/geometry/struct/Rectangle2dStruct.h" diff --git a/wpimath/src/main/native/include/frc/geometry/proto/Ellipse2dProto.h b/wpimath/src/main/native/include/frc/geometry/proto/Ellipse2dProto.h new file mode 100644 index 00000000000..550e783e1c4 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/proto/Ellipse2dProto.h @@ -0,0 +1,17 @@ +// 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 +#include + +#include "frc/geometry/Ellipse2d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Ellipse2d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, const frc::Ellipse2d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/proto/Rectangle2dProto.h b/wpimath/src/main/native/include/frc/geometry/proto/Rectangle2dProto.h new file mode 100644 index 00000000000..e6f20da005c --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/proto/Rectangle2dProto.h @@ -0,0 +1,18 @@ +// 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 +#include + +#include "frc/geometry/Rectangle2d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Rectangle2d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Rectangle2d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Ellipse2dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Ellipse2dStruct.h new file mode 100644 index 00000000000..9831a177328 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/Ellipse2dStruct.h @@ -0,0 +1,35 @@ +// 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 +#include + +#include "frc/geometry/Ellipse2d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view GetTypeString() { + return "struct:Ellipse2d"; + } + static constexpr size_t GetSize() { + return wpi::GetStructSize() + 16; + } + static constexpr std::string_view GetSchema() { + return "Pose2d center;double xSemiAxis;double ySemiAxis"; + } + + static frc::Ellipse2d Unpack(std::span data); + static void Pack(std::span data, const frc::Ellipse2d& value); + static void ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + wpi::ForEachStructSchema(fn); + wpi::ForEachStructSchema(fn); + } +}; + +static_assert(wpi::StructSerializable); +static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Rectangle2dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Rectangle2dStruct.h new file mode 100644 index 00000000000..40083368f8f --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/Rectangle2dStruct.h @@ -0,0 +1,35 @@ +// 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 +#include + +#include "frc/geometry/Rectangle2d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view GetTypeString() { + return "struct:Rectangle2d"; + } + static constexpr size_t GetSize() { + return wpi::GetStructSize() + 16; + } + static constexpr std::string_view GetSchema() { + return "Pose2d center;double xWidth;double yWidth"; + } + + static frc::Rectangle2d Unpack(std::span data); + static void Pack(std::span data, const frc::Rectangle2d& value); + static void ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + wpi::ForEachStructSchema(fn); + wpi::ForEachStructSchema(fn); + } +}; + +static_assert(wpi::StructSerializable); +static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/test/native/cpp/geometry/proto/Ellipse2dProtoTest.cpp b/wpimath/src/test/native/cpp/geometry/proto/Ellipse2dProtoTest.cpp new file mode 100644 index 00000000000..47819002b53 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/proto/Ellipse2dProtoTest.cpp @@ -0,0 +1,29 @@ +// 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 + +#include "frc/geometry/Ellipse2d.h" +#include "geometry2d.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Ellipse2d kExpectedData{ + Pose2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{22.9_rad}}, 1.2_m, 2.3_m}; +} // namespace + +TEST(Ellipse2dProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Ellipse2d unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.Center(), unpacked_data.Center()); + EXPECT_EQ(kExpectedData.XSemiAxis(), unpacked_data.XSemiAxis()); + EXPECT_EQ(kExpectedData.YSemiAxis(), unpacked_data.YSemiAxis()); +} diff --git a/wpimath/src/test/native/cpp/geometry/proto/Rectangle2dProtoTest.cpp b/wpimath/src/test/native/cpp/geometry/proto/Rectangle2dProtoTest.cpp new file mode 100644 index 00000000000..4949c0bb14d --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/proto/Rectangle2dProtoTest.cpp @@ -0,0 +1,29 @@ +// 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 + +#include "frc/geometry/Rectangle2d.h" +#include "geometry2d.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Rectangle2d kExpectedData{ + Pose2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{22.9_rad}}, 1.2_m, 2.3_m}; +} // namespace + +TEST(Rectangle2dProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Rectangle2d unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.Center(), unpacked_data.Center()); + EXPECT_EQ(kExpectedData.XWidth(), unpacked_data.XWidth()); + EXPECT_EQ(kExpectedData.YWidth(), unpacked_data.YWidth()); +} diff --git a/wpimath/src/test/native/cpp/geometry/struct/Ellipse2dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Ellipse2dStructTest.cpp new file mode 100644 index 00000000000..7603b961e27 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/struct/Ellipse2dStructTest.cpp @@ -0,0 +1,28 @@ +// 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 + +#include "frc/geometry/Ellipse2d.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const Ellipse2d kExpectedData{ + Pose2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{22.9_rad}}, 1.2_m, 2.3_m}; +} // namespace + +TEST(Ellipse2dStructTest, Roundtrip) { + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); + StructType::Pack(buffer, kExpectedData); + + Ellipse2d unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.Center(), unpacked_data.Center()); + EXPECT_EQ(kExpectedData.XSemiAxis(), unpacked_data.XSemiAxis()); + EXPECT_EQ(kExpectedData.YSemiAxis(), unpacked_data.YSemiAxis()); +} diff --git a/wpimath/src/test/native/cpp/geometry/struct/Rectangle2dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Rectangle2dStructTest.cpp new file mode 100644 index 00000000000..8743c96e1b2 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/struct/Rectangle2dStructTest.cpp @@ -0,0 +1,28 @@ +// 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 + +#include "frc/geometry/Rectangle2d.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const Rectangle2d kExpectedData{ + Pose2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{22.9_rad}}, 1.2_m, 2.3_m}; +} // namespace + +TEST(Rectangle2dStructTest, Roundtrip) { + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); + StructType::Pack(buffer, kExpectedData); + + Rectangle2d unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.Center(), unpacked_data.Center()); + EXPECT_EQ(kExpectedData.XWidth(), unpacked_data.XWidth()); + EXPECT_EQ(kExpectedData.YWidth(), unpacked_data.YWidth()); +} From ed8cae83ee46b1ef87088075d15a415c9d3eb23f Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Mon, 3 Jun 2024 14:02:06 -0700 Subject: [PATCH 29/30] Use numeric literal instead of sizeof --- wpimath/src/main/native/cpp/geometry/struct/Ellipse2dStruct.cpp | 2 +- .../src/main/native/cpp/geometry/struct/Rectangle2dStruct.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/wpimath/src/main/native/cpp/geometry/struct/Ellipse2dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Ellipse2dStruct.cpp index b646823b5a8..4e7cadaf734 100644 --- a/wpimath/src/main/native/cpp/geometry/struct/Ellipse2dStruct.cpp +++ b/wpimath/src/main/native/cpp/geometry/struct/Ellipse2dStruct.cpp @@ -7,7 +7,7 @@ namespace { constexpr size_t kCenterOff = 0; constexpr size_t kXSemiAxisOff = kCenterOff + wpi::GetStructSize(); -constexpr size_t kYSemiAxisOff = kXSemiAxisOff + sizeof(units::meter_t); +constexpr size_t kYSemiAxisOff = kXSemiAxisOff + 8; } // namespace using StructType = wpi::Struct; diff --git a/wpimath/src/main/native/cpp/geometry/struct/Rectangle2dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Rectangle2dStruct.cpp index 988428083cc..77f46bd35f3 100644 --- a/wpimath/src/main/native/cpp/geometry/struct/Rectangle2dStruct.cpp +++ b/wpimath/src/main/native/cpp/geometry/struct/Rectangle2dStruct.cpp @@ -7,7 +7,7 @@ namespace { constexpr size_t kCenterOff = 0; constexpr size_t kXWidthOff = kCenterOff + wpi::GetStructSize(); -constexpr size_t kYWidthOff = kXWidthOff + sizeof(units::meter_t); +constexpr size_t kYWidthOff = kXWidthOff + 8; } // namespace using StructType = wpi::Struct; From 2371c1eeb09072a99ac57ef573d10ea17b9c73cc Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Mon, 3 Jun 2024 16:00:18 -0700 Subject: [PATCH 30/30] Remove ForEachStructSchema lines --- .../main/native/include/frc/geometry/struct/Ellipse2dStruct.h | 2 -- .../main/native/include/frc/geometry/struct/Rectangle2dStruct.h | 2 -- 2 files changed, 4 deletions(-) diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Ellipse2dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Ellipse2dStruct.h index 9831a177328..ac7ea3c5351 100644 --- a/wpimath/src/main/native/include/frc/geometry/struct/Ellipse2dStruct.h +++ b/wpimath/src/main/native/include/frc/geometry/struct/Ellipse2dStruct.h @@ -26,8 +26,6 @@ struct WPILIB_DLLEXPORT wpi::Struct { static void ForEachNested( std::invocable auto fn) { wpi::ForEachStructSchema(fn); - wpi::ForEachStructSchema(fn); - wpi::ForEachStructSchema(fn); } }; diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Rectangle2dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Rectangle2dStruct.h index 40083368f8f..86bda44a4cc 100644 --- a/wpimath/src/main/native/include/frc/geometry/struct/Rectangle2dStruct.h +++ b/wpimath/src/main/native/include/frc/geometry/struct/Rectangle2dStruct.h @@ -26,8 +26,6 @@ struct WPILIB_DLLEXPORT wpi::Struct { static void ForEachNested( std::invocable auto fn) { wpi::ForEachStructSchema(fn); - wpi::ForEachStructSchema(fn); - wpi::ForEachStructSchema(fn); } };