diff --git a/wpimath/CMakeLists.txt b/wpimath/CMakeLists.txt index 45badc6fc3..d0984731b5 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/generated/main/java/edu/wpi/first/math/proto/Geometry2D.java b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Geometry2D.java index 331ccd1c8a..c0dc337008 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.ProtoUtil; import us.hebi.quickbuf.RepeatedByte; public final class Geometry2D { - private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(1256, + private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(1889, "ChBnZW9tZXRyeTJkLnByb3RvEgl3cGkucHJvdG8iMwoVUHJvdG9idWZUcmFuc2xhdGlvbjJkEgwKAXgY" + "ASABKAFSAXgSDAoBeRgCIAEoAVIBeSIqChJQcm90b2J1ZlJvdGF0aW9uMmQSFAoFdmFsdWUYASABKAFS" + "BXZhbHVlIo8BCg5Qcm90b2J1ZlBvc2UyZBJCCgt0cmFuc2xhdGlvbhgBIAEoCzIgLndwaS5wcm90by5Q" + @@ -27,19 +27,30 @@ public final class Geometry2D { "dHJhbnNsYXRpb24YASABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUgt0cmFuc2xh" + "dGlvbhI5Cghyb3RhdGlvbhgCIAEoCzIdLndwaS5wcm90by5Qcm90b2J1ZlJvdGF0aW9uMmRSCHJvdGF0" + "aW9uIkkKD1Byb3RvYnVmVHdpc3QyZBIOCgJkeBgBIAEoAVICZHgSDgoCZHkYAiABKAFSAmR5EhYKBmR0" + - "aGV0YRgDIAEoAVIGZHRoZXRhQhoKGGVkdS53cGkuZmlyc3QubWF0aC5wcm90b0rPBQoGEgQAAB0BCggK" + - "AQwSAwAAEgoICgECEgMCABIKCAoBCBIDBAAxCgkKAggBEgMEADEKCgoCBAASBAYACQEKCgoDBAABEgMG" + - "CB0KCwoEBAACABIDBwIPCgwKBQQAAgAFEgMHAggKDAoFBAACAAESAwcJCgoMCgUEAAIAAxIDBw0OCgsK" + - "BAQAAgESAwgCDwoMCgUEAAIBBRIDCAIICgwKBQQAAgEBEgMICQoKDAoFBAACAQMSAwgNDgoKCgIEARIE" + - "CwANAQoKCgMEAQESAwsIGgoLCgQEAQIAEgMMAhMKDAoFBAECAAUSAwwCCAoMCgUEAQIAARIDDAkOCgwK" + - "BQQBAgADEgMMERIKCgoCBAISBA8AEgEKCgoDBAIBEgMPCBYKCwoEBAICABIDEAIoCgwKBQQCAgAGEgMQ" + - "AhcKDAoFBAICAAESAxAYIwoMCgUEAgIAAxIDECYnCgsKBAQCAgESAxECIgoMCgUEAgIBBhIDEQIUCgwK" + - "BQQCAgEBEgMRFR0KDAoFBAICAQMSAxEgIQoKCgIEAxIEFAAXAQoKCgMEAwESAxQIGwoLCgQEAwIAEgMV" + - "AigKDAoFBAMCAAYSAxUCFwoMCgUEAwIAARIDFRgjCgwKBQQDAgADEgMVJicKCwoEBAMCARIDFgIiCgwK" + - "BQQDAgEGEgMWAhQKDAoFBAMCAQESAxYVHQoMCgUEAwIBAxIDFiAhCgoKAgQEEgQZAB0BCgoKAwQEARID" + - "GQgXCgsKBAQEAgASAxoCEAoMCgUEBAIABRIDGgIICgwKBQQEAgABEgMaCQsKDAoFBAQCAAMSAxoODwoL" + - "CgQEBAIBEgMbAhAKDAoFBAQCAQUSAxsCCAoMCgUEBAIBARIDGwkLCgwKBQQEAgEDEgMbDg8KCwoEBAQC", - "AhIDHAIUCgwKBQQEAgIFEgMcAggKDAoFBAQCAgESAxwJDwoMCgUEBAICAxIDHBITYgZwcm90bzM="); + "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); @@ -53,6 +64,10 @@ 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, 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. */ @@ -1803,4 +1818,870 @@ public final class Geometry2D { 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 xWidth = 2; + */ + private double xWidth; + + /** + * optional double yWidth = 3; + */ + private double yWidth; + + /** + * 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 xWidth = 2; + * @return whether the xWidth field is set + */ + public boolean hasXWidth() { + return (bitField0_ & 0x00000001) != 0; + } + + /** + * optional double xWidth = 2; + * @return this + */ + public ProtobufRectangle2d clearXWidth() { + bitField0_ &= ~0x00000001; + xWidth = 0D; + return this; + } + + /** + * optional double xWidth = 2; + * @return the xWidth + */ + public double getXWidth() { + return xWidth; + } + + /** + * optional double xWidth = 2; + * @param value the xWidth to set + * @return this + */ + public ProtobufRectangle2d setXWidth(final double value) { + bitField0_ |= 0x00000001; + xWidth = value; + return this; + } + + /** + * optional double yWidth = 3; + * @return whether the yWidth field is set + */ + public boolean hasYWidth() { + return (bitField0_ & 0x00000002) != 0; + } + + /** + * optional double yWidth = 3; + * @return this + */ + public ProtobufRectangle2d clearYWidth() { + bitField0_ &= ~0x00000002; + yWidth = 0D; + return this; + } + + /** + * optional double yWidth = 3; + * @return the yWidth + */ + public double getYWidth() { + return yWidth; + } + + /** + * optional double yWidth = 3; + * @param value the yWidth to set + * @return this + */ + public ProtobufRectangle2d setYWidth(final double value) { + bitField0_ |= 0x00000002; + yWidth = 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_; + xWidth = other.xWidth; + yWidth = other.yWidth; + center.copyFrom(other.center); + } + return this; + } + + @Override + public ProtobufRectangle2d mergeFrom(final ProtobufRectangle2d other) { + if (other.isEmpty()) { + return this; + } + cachedSize = -1; + if (other.hasXWidth()) { + setXWidth(other.xWidth); + } + if (other.hasYWidth()) { + setYWidth(other.yWidth); + } + if (other.hasCenter()) { + getMutableCenter().mergeFrom(other.center); + } + return this; + } + + @Override + public ProtobufRectangle2d clear() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + xWidth = 0D; + yWidth = 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_ + && (!hasXWidth() || ProtoUtil.isEqual(xWidth, other.xWidth)) + && (!hasYWidth() || ProtoUtil.isEqual(yWidth, other.yWidth)) + && (!hasCenter() || center.equals(other.center)); + } + + @Override + public void writeTo(final ProtoSink output) throws IOException { + if ((bitField0_ & 0x00000001) != 0) { + output.writeRawByte((byte) 17); + output.writeDoubleNoTag(xWidth); + } + if ((bitField0_ & 0x00000002) != 0) { + output.writeRawByte((byte) 25); + output.writeDoubleNoTag(yWidth); + } + 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: { + // xWidth + xWidth = input.readDouble(); + bitField0_ |= 0x00000001; + tag = input.readTag(); + if (tag != 25) { + break; + } + } + case 25: { + // yWidth + yWidth = 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.xWidth, xWidth); + } + if ((bitField0_ & 0x00000002) != 0) { + output.writeDouble(FieldNames.yWidth, yWidth); + } + 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 -775894994: { + if (input.isAtField(FieldNames.xWidth)) { + if (!input.trySkipNullValue()) { + xWidth = input.readDouble(); + bitField0_ |= 0x00000001; + } + } else { + input.skipUnknownField(); + } + break; + } + case -747265843: { + if (input.isAtField(FieldNames.yWidth)) { + if (!input.trySkipNullValue()) { + yWidth = 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 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 ySemiAxis = FieldName.forField("ySemiAxis"); + + static final FieldName center = FieldName.forField("center"); + } + } } 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 517c13297b..10a9147c7d 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 final class WPIMathJNI { 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 new file mode 100644 index 0000000000..e52464a30e --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java @@ -0,0 +1,241 @@ +// 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 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; +import edu.wpi.first.util.struct.StructSerializable; +import java.util.Objects; + +/** 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; + private final double 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. + */ + public Ellipse2d(Pose2d center, double xSemiAxis, double ySemiAxis) { + if (xSemiAxis <= 0 || ySemiAxis <= 0) { + throw new IllegalArgumentException("Ellipse2d semi-axes must be positive"); + } + + m_center = center; + m_xSemiAxis = xSemiAxis; + m_ySemiAxis = ySemiAxis; + } + + /** + * 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(Translation2d center, double radius) { + this(new Pose2d(center, Rotation2d.kZero), radius, radius); + } + + /** + * Returns the center of the ellipse. + * + * @return The center of the ellipse. + */ + 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. + * + * @return The x semi-axis. + */ + public double getXSemiAxis() { + return m_xSemiAxis; + } + + /** + * Returns the y semi-axis. + * + * @return The y semi-axis. + */ + public double getYSemiAxis() { + return m_ySemiAxis; + } + + /** + * Returns the focal points of the ellipse. In a perfect circle, this will always return the + * center. + * + * @return The focal points. + */ + public Pair getFocalPoints() { + // 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) { + 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 { + 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()); + } + } + + /** + * 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_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); + } + + /** + * 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 intersects(Translation2d point) { + return Math.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. + */ + 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; + } + + // Find nearest point + 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 + 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); + } + + /** + * 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(); + + /** 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 new file mode 100644 index 0000000000..9541021ffe --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java @@ -0,0 +1,217 @@ +// 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 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. + */ +public class Rectangle2d implements ProtobufSerializable, StructSerializable { + private final Pose2d m_center; + private final double m_xWidth; + private final double m_yWidth; + + /** + * Constructs a rectangle at the specified position with the specified width and height. + * + * @param center The position (translation and rotation) of the rectangle. + * @param xWidth The x size component of the rectangle, in unrotated coordinate frame. + * @param yWidth The y size component of the rectangle, in unrotated coordinate frame. + */ + public Rectangle2d(Pose2d center, double xWidth, double yWidth) { + if (xWidth < 0 || yWidth < 0) { + throw new IllegalArgumentException("Rectangle2d dimensions cannot be less than 0!"); + } + + m_center = center; + m_xWidth = xWidth; + m_yWidth = yWidth; + } + + /** + * 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( + new Pose2d(cornerA.plus(cornerB).div(2.0), Rotation2d.kZero), + Math.abs(cornerA.getX() - cornerB.getX()), + Math.abs(cornerA.getY() - cornerB.getY())); + } + + /** + * 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 x size component of the rectangle. + * + * @return The x size component of the rectangle. + */ + public double getXWidth() { + return m_xWidth; + } + + /** + * Returns the y size component of the rectangle. + * + * @return The y size component of the rectangle. + */ + public double getYWidth() { + 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 + */ + public Rectangle2d transformBy(Transform2d other) { + return new 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. + */ + 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 the rectangle's perimeter intersects the 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(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(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; + } + + 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. + */ + public boolean contains(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); + } + + /** + * 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) + */ + public double getDistance(Translation2d point) { + return findNearestPoint(point).getDistance(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. + */ + public Translation2d findNearestPoint(Translation2d point) { + // Check if already in rectangle + if (contains(point)) { + return 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)); + + // Undo rotation + return point.rotateAround(m_center.getTranslation(), m_center.getRotation()); + } + + @Override + public String toString() { + return String.format("Rectangle2d(center: %s, x: %.2f, y: %.2f)", m_center, m_xWidth, m_yWidth); + } + + /** + * 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).getXWidth() == m_xWidth + && ((Rectangle2d) obj).getYWidth() == m_yWidth; + } + return false; + } + + @Override + public int hashCode() { + return Objects.hash(m_center, m_xWidth, m_yWidth); + } + + /** 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 d0357e80ae..9899b97ef4 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 @@ -179,6 +179,24 @@ public class Translation2d 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/Ellipse2dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Ellipse2dProto.java new file mode 100644 index 0000000000..587f0e65d0 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Ellipse2dProto.java @@ -0,0 +1,46 @@ +// 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 new file mode 100644 index 0000000000..4876d1e26a --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rectangle2dProto.java @@ -0,0 +1,45 @@ +// 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.getXWidth(), msg.getYWidth()); + } + + @Override + public void pack(ProtobufRectangle2d msg, Rectangle2d value) { + Pose2d.proto.pack(msg.getMutableCenter(), value.getCenter()); + 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 0000000000..870cfaddb2 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Ellipse2dStruct.java @@ -0,0 +1,52 @@ +// 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 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 + 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/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 0000000000..62c6eff6bb --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rectangle2dStruct.java @@ -0,0 +1,52 @@ +// 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 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 + 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 xWidth; double yWidth"; + } + + @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.getXWidth()); + bb.putDouble(value.getYWidth()); + } +} 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 0000000000..4288b8b44b --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp @@ -0,0 +1,57 @@ +// 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 + +#include "geometry2d.pb.h" + +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/Rectangle2d.cpp b/wpimath/src/main/native/cpp/geometry/Rectangle2d.cpp new file mode 100644 index 0000000000..0d1f77bc64 --- /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/Translation2d.cpp b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp index 90162018ed..c0a349e7aa 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/cpp/geometry/proto/Ellipse2dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Ellipse2dProto.cpp new file mode 100644 index 0000000000..adfc28d3c2 --- /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 0000000000..98d4430218 --- /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 0000000000..4e7cadaf73 --- /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 + 8; +} // 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 0000000000..77f46bd35f --- /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 + 8; +} // 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/cpp/jni/WPIMathJNI_Ellipse2d.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI_Ellipse2d.cpp new file mode 100644 index 0000000000..ae6f88a21e --- /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 new file mode 100644 index 0000000000..f57303d1e8 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h @@ -0,0 +1,212 @@ +// 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 "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 WPILIB_DLLEXPORT 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 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. + * + * @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 wpi::array FocalPoints() const { + // 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) { + return wpi::array{ + (m_center + Transform2d{-c, 0_m, Rotation2d{}}).Translation(), + (m_center + Transform2d{c, 0_m, Rotation2d{}}).Translation()}; + } else { + return wpi::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 + +#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 new file mode 100644 index 0000000000..f03ca98c44 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/Rectangle2d.h @@ -0,0 +1,210 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#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 WPILIB_DLLEXPORT Rectangle2d { + public: + /** + * Constructs a rectangle at the specified position with the specified width + * and height. + * + * @param center The position (translation and rotation) of the rectangle. + * @param xWidth The x size component of the rectangle, in unrotated + * coordinate frame. + * @param yWidth The y size component of the rectangle, in unrotated + * coordinate frame. + */ + constexpr Rectangle2d(const Pose2d& center, units::meter_t xWidth, + units::meter_t yWidth) + : m_center{center}, m_xWidth{xWidth}, m_yWidth{yWidth} { + if (xWidth < 0_m || yWidth < 0_m) { + throw std::invalid_argument( + "Rectangle2d dimensions cannot be less than 0!"); + } + } + + /** + * Creates an unrotated rectangle from the given corners. The corners should + * be diagonally opposite of each other. + * + * @param cornerA The first corner of the rectangle. + * @param cornerB The second corner of the rectangle. + */ + constexpr Rectangle2d(const Translation2d& cornerA, + const Translation2d& cornerB) + : m_center{(cornerA + cornerB) / 2.0, Rotation2d{}}, + m_xWidth{units::math::abs(cornerA.X() - cornerB.X())}, + m_yWidth{units::math::abs(cornerA.Y() - cornerB.Y())} {} + + /** + * Returns the center of the rectangle. + * + * @return The center of the rectangle. + */ + constexpr const Pose2d& Center() const { return m_center; } + + /** + * Returns the rotational component of the rectangle. + * + * @return The rotational component of the rectangle. + */ + constexpr const Rotation2d& Rotation() const { return m_center.Rotation(); } + + /** + * Returns the x size component of the rectangle. + * + * @return The x size component of the rectangle. + */ + constexpr units::meter_t XWidth() const { return m_xWidth; } + + /** + * Returns the y size component of the rectangle. + * + * @return The y size component of the rectangle. + */ + constexpr units::meter_t YWidth() const { return m_yWidth; } + + /** + * Transforms the center of the rectangle and returns the new rectangle. + * + * @param other The transform to transform by. + * @return The transformed rectangle + */ + constexpr Rectangle2d TransformBy(const Transform2d& other) const { + return Rectangle2d{m_center.TransformBy(other), m_xWidth, m_yWidth}; + } + + /** + * Rotates the center of the rectangle and returns the new rectangle. + * + * @param other The rotation to transform by. + * @return The rotated rectangle. + */ + constexpr Rectangle2d RotateBy(const Rotation2d& other) const { + return Rectangle2d{m_center.RotateBy(other), m_xWidth, m_yWidth}; + } + + /** + * Checks if a point is intersected by the rectangle's perimeter. + * + * @param point The point to check. + * @return True, if the rectangle's perimeter intersects the point. + */ + constexpr bool Intersects(const Translation2d& point) const { + // Move the point into the rectangle's coordinate frame + auto pointInRect = point - m_center.Translation(); + pointInRect = pointInRect.RotateBy(-m_center.Rotation()); + + if (units::math::abs(units::math::abs(pointInRect.X()) - m_xWidth / 2.0) <= + 1E-9_m) { + // Point rests on left/right perimeter + return units::math::abs(pointInRect.Y()) <= m_yWidth / 2.0; + } else if (units::math::abs(units::math::abs(pointInRect.Y()) - + m_yWidth / 2.0) <= 1E-9_m) { + // Point rests on top/bottom perimeter + return units::math::abs(pointInRect.X()) <= m_xWidth / 2.0; + } + + return false; + } + + /** + * Checks if a point is contained within the rectangle. This is inclusive, if + * the point lies on the perimeter it will return true. + * + * @param point The point to check. + * @return True, if the rectangle contains the point or the perimeter + * intersects the point. + */ + constexpr bool Contains(const Translation2d& point) const { + // Rotate the point into the rectangle's coordinate frame + auto rotPoint = + point.RotateAround(m_center.Translation(), -m_center.Rotation()); + + // Check if within bounding box + return rotPoint.X() >= (m_center.X() - m_xWidth / 2.0) && + rotPoint.X() <= (m_center.X() + m_xWidth / 2.0) && + rotPoint.Y() >= (m_center.Y() - m_yWidth / 2.0) && + rotPoint.Y() <= (m_center.Y() + m_yWidth / 2.0); + } + + /** + * Returns the distance between the perimeter of the rectangle and the point. + * + * @param point The point to check. + * @return The distance (0, if the point is contained by the rectangle) + */ + constexpr units::meter_t Distance(const Translation2d& point) const { + return FindNearestPoint(point).Distance(point); + } + + /** + * Returns the nearest point that is contained within the rectangle. + * + * @param point The point that this will find the nearest point to. + * @return A new point that is nearest to {@code point} and contained in the + * rectangle. + */ + constexpr Translation2d FindNearestPoint(const Translation2d& point) const { + // Check if already in rectangle + if (Contains(point)) { + return point; + } + + // Rotate the point by the inverse of the rectangle's rotation + auto rotPoint = + point.RotateAround(m_center.Translation(), -m_center.Rotation()); + + // Find nearest point + rotPoint = + Translation2d{std::clamp(rotPoint.X(), m_center.X() - m_xWidth / 2.0, + m_center.X() + m_xWidth / 2.0), + std::clamp(rotPoint.Y(), m_center.Y() - m_yWidth / 2.0, + m_center.Y() + m_yWidth / 2.0)}; + + // Undo rotation + return rotPoint.RotateAround(m_center.Translation(), m_center.Rotation()); + } + + /** + * Checks equality between this Rectangle2d and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + constexpr bool operator==(const Rectangle2d& other) const { + return m_center == other.m_center && + units::math::abs(m_xWidth - other.m_xWidth) < 1E-9_m && + units::math::abs(m_yWidth - other.m_yWidth) < 1E-9_m; + } + + private: + Pose2d m_center; + units::meter_t m_xWidth; + units::meter_t m_yWidth; +}; + +} // namespace frc + +#include "frc/geometry/proto/Rectangle2dProto.h" +#include "frc/geometry/struct/Rectangle2dStruct.h" diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h index 9d9a77c3bf..674bd9efe1 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 6b291e1f74..2d1160abb4 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/frc/geometry/proto/Ellipse2dProto.h b/wpimath/src/main/native/include/frc/geometry/proto/Ellipse2dProto.h new file mode 100644 index 0000000000..550e783e1c --- /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 0000000000..e6f20da005 --- /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 0000000000..ac7ea3c535 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/Ellipse2dStruct.h @@ -0,0 +1,33 @@ +// 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); + } +}; + +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 0000000000..86bda44a4c --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/Rectangle2dStruct.h @@ -0,0 +1,33 @@ +// 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); + } +}; + +static_assert(wpi::StructSerializable); +static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/proto/geometry2d.proto b/wpimath/src/main/proto/geometry2d.proto index d52da4585a..1749c63bc2 100644 --- a/wpimath/src/main/proto/geometry2d.proto +++ b/wpimath/src/main/proto/geometry2d.proto @@ -28,3 +28,15 @@ message ProtobufTwist2d { double dy = 2; double dtheta = 3; } + +message ProtobufRectangle2d { + ProtobufPose2d center = 1; + double xWidth = 2; + double yWidth = 3; +} + +message ProtobufEllipse2d { + ProtobufPose2d center = 1; + double xSemiAxis = 2; + double ySemiAxis = 3; +} 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 0000000000..28c9d41d01 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Ellipse2dTest.java @@ -0,0 +1,121 @@ +// 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.assertNotEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +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()); + var ellipse = new Ellipse2d(center, 5.0, 4.0); + + var pair = ellipse.getFocalPoints(); + var a = pair.getFirst(); + var b = pair.getSecond(); + + 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.intersects(pointA)), + () -> assertFalse(ellipse.intersects(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.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); + 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); + assertEquals(0.0, ellipse.getDistance(point3), 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, 2.0, Rotation2d.fromDegrees(270.0)); + var ellipse = new Ellipse2d(center, 1.0, 2.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(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(-0.8512799937611617, nearestPoint4.getX(), kEpsilon), + () -> assertEquals(2.378405333174535, nearestPoint4.getY(), kEpsilon)); + } + + @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(center2, 2.0, 3.0); + + var center3 = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(90.0)); + var ellipse3 = new Ellipse2d(center3, 3.0, 2.0); + + assertAll( + () -> 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 new file mode 100644 index 0000000000..7ee86d6d7d --- /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.assertNotEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import org.junit.jupiter.api.Test; + +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.getXWidth()), + () -> assertEquals(4.0, rect.getYWidth()), + () -> 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.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 + 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.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 + 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); + 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); + assertEquals(0.5, rect.getDistance(point3), kEpsilon); + + var point4 = new Translation2d(-1.0, 2.5); + assertEquals(1.0, rect.getDistance(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); + 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(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(() -> assertEquals(rect1, rect2), () -> assertNotEquals(rect2, 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 cd7d021afa..82ea7ab866 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 @@ class Translation2dTest { () -> 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/Ellipse2dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Ellipse2dProtoTest.java new file mode 100644 index 0000000000..92c5614d29 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Ellipse2dProtoTest.java @@ -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. + +package edu.wpi.first.math.geometry.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +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; + +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/proto/Rectangle2dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rectangle2dProtoTest.java new file mode 100644 index 0000000000..e270c75cab --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rectangle2dProtoTest.java @@ -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. + +package edu.wpi.first.math.geometry.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +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; + +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.getXWidth(), data.getXWidth()); + assertEquals(DATA.getYWidth(), data.getYWidth()); + } +} 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 0000000000..1bb4323423 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Ellipse2dStructTest.java @@ -0,0 +1,32 @@ +// 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 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; + +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()); + } +} 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 0000000000..506209bdc4 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rectangle2dStructTest.java @@ -0,0 +1,32 @@ +// 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 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; + +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(Rectangle2d.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.getXWidth(), data.getXWidth()); + assertEquals(DATA.getYWidth(), data.getYWidth()); + } +} 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 0000000000..c51e50e4b6 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/Ellipse2dTest.cpp @@ -0,0 +1,100 @@ +// 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}; + 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, 2_m, 270_deg}; + constexpr frc::Ellipse2d ellipse{center, 1_m, 2_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); + + 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); + + 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) { + 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 6d689db68b..a9490fbdcc 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 0000000000..f4d2d4350e --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/Rectangle2dTest.cpp @@ -0,0 +1,88 @@ +// 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}; + 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); +} + +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); + 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); +} + +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); +} 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 0000000000..47819002b5 --- /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 0000000000..4949c0bb14 --- /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 0000000000..7603b961e2 --- /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 0000000000..8743c96e1b --- /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()); +}