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] 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