diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java index 032522b864..9844198462 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java @@ -10,18 +10,15 @@ import com.fasterxml.jackson.annotation.JsonAutoDetect; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.math.geometry.proto.Pose2dProto; +import edu.wpi.first.math.geometry.struct.Pose2dStruct; import edu.wpi.first.math.interpolation.Interpolatable; -import edu.wpi.first.math.proto.Geometry2D.ProtobufPose2d; import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; -import edu.wpi.first.util.protobuf.Protobuf; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; import java.util.Collections; import java.util.Comparator; import java.util.List; import java.util.Objects; -import us.hebi.quickbuf.Descriptors.Descriptor; /** Represents a 2D pose containing translational and rotational elements. */ @JsonIgnoreProperties(ignoreUnknown = true) @@ -327,82 +324,6 @@ public class Pose2d implements Interpolatable { } } - public static final class AStruct implements Struct { - @Override - public Class getTypeClass() { - return Pose2d.class; - } - - @Override - public String getTypeString() { - return "struct:Pose2d"; - } - - @Override - public int getSize() { - return Translation2d.struct.getSize() + Rotation2d.struct.getSize(); - } - - @Override - public String getSchema() { - return "Translation2d translation;Rotation2d rotation"; - } - - @Override - public Struct[] getNested() { - return new Struct[] {Translation2d.struct, Rotation2d.struct}; - } - - @Override - public Pose2d unpack(ByteBuffer bb) { - Translation2d translation = Translation2d.struct.unpack(bb); - Rotation2d rotation = Rotation2d.struct.unpack(bb); - return new Pose2d(translation, rotation); - } - - @Override - public void pack(ByteBuffer bb, Pose2d value) { - Translation2d.struct.pack(bb, value.m_translation); - Rotation2d.struct.pack(bb, value.m_rotation); - } - } - - public static final AStruct struct = new AStruct(); - - public static final class AProto implements Protobuf { - @Override - public Class getTypeClass() { - return Pose2d.class; - } - - @Override - public Descriptor getDescriptor() { - return ProtobufPose2d.getDescriptor(); - } - - @Override - public Protobuf[] getNested() { - return new Protobuf[] {Translation2d.proto, Rotation2d.proto}; - } - - @Override - public ProtobufPose2d createMessage() { - return ProtobufPose2d.newInstance(); - } - - @Override - public Pose2d unpack(ProtobufPose2d msg) { - return new Pose2d( - Translation2d.proto.unpack(msg.getTranslation()), - Rotation2d.proto.unpack(msg.getRotation())); - } - - @Override - public void pack(ProtobufPose2d msg, Pose2d value) { - Translation2d.proto.pack(msg.getMutableTranslation(), value.m_translation); - Rotation2d.proto.pack(msg.getMutableRotation(), value.m_rotation); - } - } - - public static final AProto proto = new AProto(); + public static final Pose2dStruct struct = new Pose2dStruct(); + public static final Pose2dProto proto = new Pose2dProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java index 2ea709439a..3fd5b653b8 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java @@ -9,13 +9,10 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.WPIMathJNI; +import edu.wpi.first.math.geometry.proto.Pose3dProto; +import edu.wpi.first.math.geometry.struct.Pose3dStruct; import edu.wpi.first.math.interpolation.Interpolatable; -import edu.wpi.first.math.proto.Geometry3D.ProtobufPose3d; -import edu.wpi.first.util.protobuf.Protobuf; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; import java.util.Objects; -import us.hebi.quickbuf.Descriptors.Descriptor; /** Represents a 3D pose containing translational and rotational elements. */ @JsonIgnoreProperties(ignoreUnknown = true) @@ -324,82 +321,6 @@ public class Pose3d implements Interpolatable { } } - public static final class AStruct implements Struct { - @Override - public Class getTypeClass() { - return Pose3d.class; - } - - @Override - public String getTypeString() { - return "struct:Pose3d"; - } - - @Override - public int getSize() { - return Translation3d.struct.getSize() + Rotation3d.struct.getSize(); - } - - @Override - public String getSchema() { - return "Translation3d translation;Rotation3d rotation"; - } - - @Override - public Struct[] getNested() { - return new Struct[] {Translation3d.struct, Rotation3d.struct}; - } - - @Override - public Pose3d unpack(ByteBuffer bb) { - Translation3d translation = Translation3d.struct.unpack(bb); - Rotation3d rotation = Rotation3d.struct.unpack(bb); - return new Pose3d(translation, rotation); - } - - @Override - public void pack(ByteBuffer bb, Pose3d value) { - Translation3d.struct.pack(bb, value.m_translation); - Rotation3d.struct.pack(bb, value.m_rotation); - } - } - - public static final AStruct struct = new AStruct(); - - public static final class AProto implements Protobuf { - @Override - public Class getTypeClass() { - return Pose3d.class; - } - - @Override - public Descriptor getDescriptor() { - return ProtobufPose3d.getDescriptor(); - } - - @Override - public Protobuf[] getNested() { - return new Protobuf[] {Translation3d.proto, Rotation3d.proto}; - } - - @Override - public ProtobufPose3d createMessage() { - return ProtobufPose3d.newInstance(); - } - - @Override - public Pose3d unpack(ProtobufPose3d msg) { - return new Pose3d( - Translation3d.proto.unpack(msg.getTranslation()), - Rotation3d.proto.unpack(msg.getRotation())); - } - - @Override - public void pack(ProtobufPose3d msg, Pose3d value) { - Translation3d.proto.pack(msg.getMutableTranslation(), value.m_translation); - Rotation3d.proto.pack(msg.getMutableRotation(), value.m_rotation); - } - } - - public static final AProto proto = new AProto(); + public static final Pose3dStruct struct = new Pose3dStruct(); + public static final Pose3dProto proto = new Pose3dProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java index 23fd26b6c0..0d9ef88145 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java @@ -10,13 +10,10 @@ import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.proto.QuaternionProto; +import edu.wpi.first.math.geometry.struct.QuaternionStruct; import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.proto.Geometry3D.ProtobufQuaternion; -import edu.wpi.first.util.protobuf.Protobuf; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; import java.util.Objects; -import us.hebi.quickbuf.Descriptors.Descriptor; @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) @@ -406,73 +403,6 @@ public class Quaternion { return VecBuilder.fill(coeff * getX(), coeff * getY(), coeff * getZ()); } - public static final class AStruct implements Struct { - @Override - public Class getTypeClass() { - return Quaternion.class; - } - - @Override - public String getTypeString() { - return "struct:Quaternion"; - } - - @Override - public int getSize() { - return kSizeDouble * 4; - } - - @Override - public String getSchema() { - return "double w;double x;double y;double z"; - } - - @Override - public Quaternion unpack(ByteBuffer bb) { - double w = bb.getDouble(); - double x = bb.getDouble(); - double y = bb.getDouble(); - double z = bb.getDouble(); - return new Quaternion(w, x, y, z); - } - - @Override - public void pack(ByteBuffer bb, Quaternion value) { - bb.putDouble(value.getW()); - bb.putDouble(value.getX()); - bb.putDouble(value.getY()); - bb.putDouble(value.getZ()); - } - } - - public static final AStruct struct = new AStruct(); - - public static final class AProto implements Protobuf { - @Override - public Class getTypeClass() { - return Quaternion.class; - } - - @Override - public Descriptor getDescriptor() { - return ProtobufQuaternion.getDescriptor(); - } - - @Override - public ProtobufQuaternion createMessage() { - return ProtobufQuaternion.newInstance(); - } - - @Override - public Quaternion unpack(ProtobufQuaternion msg) { - return new Quaternion(msg.getW(), msg.getX(), msg.getY(), msg.getZ()); - } - - @Override - public void pack(ProtobufQuaternion msg, Quaternion value) { - msg.setW(value.getW()).setX(value.getX()).setY(value.getY()).setZ(value.getZ()); - } - } - - public static final AProto proto = new AProto(); + public static final QuaternionStruct struct = new QuaternionStruct(); + public static final QuaternionProto proto = new QuaternionProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java index 55f42e601d..9da3fa2ea2 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java @@ -11,16 +11,13 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.proto.Rotation2dProto; +import edu.wpi.first.math.geometry.struct.Rotation2dStruct; import edu.wpi.first.math.interpolation.Interpolatable; -import edu.wpi.first.math.proto.Geometry2D.ProtobufRotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.Angle; import edu.wpi.first.units.Measure; -import edu.wpi.first.util.protobuf.Protobuf; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; import java.util.Objects; -import us.hebi.quickbuf.Descriptors.Descriptor; /** * A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine). @@ -275,66 +272,6 @@ public class Rotation2d implements Interpolatable { return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1))); } - public static final class AStruct implements Struct { - @Override - public Class getTypeClass() { - return Rotation2d.class; - } - - @Override - public String getTypeString() { - return "struct:Rotation2d"; - } - - @Override - public int getSize() { - return kSizeDouble; - } - - @Override - public String getSchema() { - return "double value"; - } - - @Override - public Rotation2d unpack(ByteBuffer bb) { - return new Rotation2d(bb.getDouble()); - } - - @Override - public void pack(ByteBuffer bb, Rotation2d value) { - bb.putDouble(value.m_value); - } - } - - public static final AStruct struct = new AStruct(); - - public static final class AProto implements Protobuf { - @Override - public Class getTypeClass() { - return Rotation2d.class; - } - - @Override - public Descriptor getDescriptor() { - return ProtobufRotation2d.getDescriptor(); - } - - @Override - public ProtobufRotation2d createMessage() { - return ProtobufRotation2d.newInstance(); - } - - @Override - public Rotation2d unpack(ProtobufRotation2d msg) { - return new Rotation2d(msg.getValue()); - } - - @Override - public void pack(ProtobufRotation2d msg, Rotation2d value) { - msg.setValue(value.m_value); - } - } - - public static final AProto proto = new AProto(); + public static final Rotation2dStruct struct = new Rotation2dStruct(); + public static final Rotation2dProto proto = new Rotation2dProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java index 6b7ab6efac..b4f93c038f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java @@ -14,15 +14,12 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.proto.Rotation3dProto; +import edu.wpi.first.math.geometry.struct.Rotation3dStruct; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.proto.Geometry3D.ProtobufRotation3d; -import edu.wpi.first.util.protobuf.Protobuf; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; import java.util.Objects; import org.ejml.dense.row.factory.DecompositionFactory_DDRM; -import us.hebi.quickbuf.Descriptors.Descriptor; /** A rotation in a 3D coordinate frame represented by a quaternion. */ @JsonIgnoreProperties(ignoreUnknown = true) @@ -437,76 +434,6 @@ public class Rotation3d implements Interpolatable { return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1))); } - public static final class AStruct implements Struct { - @Override - public Class getTypeClass() { - return Rotation3d.class; - } - - @Override - public String getTypeString() { - return "struct:Rotation3d"; - } - - @Override - public int getSize() { - return Quaternion.struct.getSize(); - } - - @Override - public String getSchema() { - return "Quaternion q"; - } - - @Override - public Struct[] getNested() { - return new Struct[] {Quaternion.struct}; - } - - @Override - public Rotation3d unpack(ByteBuffer bb) { - return new Rotation3d(Quaternion.struct.unpack(bb)); - } - - @Override - public void pack(ByteBuffer bb, Rotation3d value) { - Quaternion.struct.pack(bb, value.m_q); - } - } - - public static final AStruct struct = new AStruct(); - - public static final class AProto implements Protobuf { - @Override - public Class getTypeClass() { - return Rotation3d.class; - } - - @Override - public Descriptor getDescriptor() { - return ProtobufRotation3d.getDescriptor(); - } - - @Override - public Protobuf[] getNested() { - return new Protobuf[] {Quaternion.proto}; - } - - @Override - public ProtobufRotation3d createMessage() { - return ProtobufRotation3d.newInstance(); - } - - @Override - public Rotation3d unpack(ProtobufRotation3d msg) { - return new Rotation3d(Quaternion.proto.unpack(msg.getQ())); - } - - @Override - public void pack(ProtobufRotation3d msg, Rotation3d value) { - Quaternion.proto.pack(msg.getMutableQ(), value.m_q); - } - } - - public static final AProto proto = new AProto(); + public static final Rotation3dStruct struct = new Rotation3dStruct(); + public static final Rotation3dProto proto = new Rotation3dProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java index ea39127b5c..e5a8255f92 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java @@ -6,14 +6,11 @@ package edu.wpi.first.math.geometry; import static edu.wpi.first.units.Units.Meters; -import edu.wpi.first.math.proto.Geometry2D.ProtobufTransform2d; +import edu.wpi.first.math.geometry.proto.Transform2dProto; +import edu.wpi.first.math.geometry.struct.Transform2dStruct; import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; -import edu.wpi.first.util.protobuf.Protobuf; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; import java.util.Objects; -import us.hebi.quickbuf.Descriptors.Descriptor; /** Represents a transformation for a Pose2d in the pose's frame. */ public class Transform2d { @@ -185,82 +182,6 @@ public class Transform2d { return Objects.hash(m_translation, m_rotation); } - public static final class AStruct implements Struct { - @Override - public Class getTypeClass() { - return Transform2d.class; - } - - @Override - public String getTypeString() { - return "struct:Transform2d"; - } - - @Override - public int getSize() { - return Translation2d.struct.getSize() + Rotation2d.struct.getSize(); - } - - @Override - public String getSchema() { - return "Translation2d translation;Rotation2d rotation"; - } - - @Override - public Struct[] getNested() { - return new Struct[] {Translation2d.struct, Rotation2d.struct}; - } - - @Override - public Transform2d unpack(ByteBuffer bb) { - Translation2d translation = Translation2d.struct.unpack(bb); - Rotation2d rotation = Rotation2d.struct.unpack(bb); - return new Transform2d(translation, rotation); - } - - @Override - public void pack(ByteBuffer bb, Transform2d value) { - Translation2d.struct.pack(bb, value.m_translation); - Rotation2d.struct.pack(bb, value.m_rotation); - } - } - - public static final AStruct struct = new AStruct(); - - public static final class AProto implements Protobuf { - @Override - public Class getTypeClass() { - return Transform2d.class; - } - - @Override - public Descriptor getDescriptor() { - return ProtobufTransform2d.getDescriptor(); - } - - @Override - public Protobuf[] getNested() { - return new Protobuf[] {Translation2d.proto, Rotation2d.proto}; - } - - @Override - public ProtobufTransform2d createMessage() { - return ProtobufTransform2d.newInstance(); - } - - @Override - public Transform2d unpack(ProtobufTransform2d msg) { - return new Transform2d( - Translation2d.proto.unpack(msg.getTranslation()), - Rotation2d.proto.unpack(msg.getRotation())); - } - - @Override - public void pack(ProtobufTransform2d msg, Transform2d value) { - Translation2d.proto.pack(msg.getMutableTranslation(), value.m_translation); - Rotation2d.proto.pack(msg.getMutableRotation(), value.m_rotation); - } - } - - public static final AProto proto = new AProto(); + public static final Transform2dStruct struct = new Transform2dStruct(); + public static final Transform2dProto proto = new Transform2dProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java index 223a14b18f..6b4fe1f63c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java @@ -4,12 +4,9 @@ package edu.wpi.first.math.geometry; -import edu.wpi.first.math.proto.Geometry3D.ProtobufTransform3d; -import edu.wpi.first.util.protobuf.Protobuf; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; +import edu.wpi.first.math.geometry.proto.Transform3dProto; +import edu.wpi.first.math.geometry.struct.Transform3dStruct; import java.util.Objects; -import us.hebi.quickbuf.Descriptors.Descriptor; /** Represents a transformation for a Pose3d in the pose's frame. */ public class Transform3d { @@ -179,82 +176,6 @@ public class Transform3d { return Objects.hash(m_translation, m_rotation); } - public static final class AStruct implements Struct { - @Override - public Class getTypeClass() { - return Transform3d.class; - } - - @Override - public String getTypeString() { - return "struct:Transform3d"; - } - - @Override - public int getSize() { - return Translation3d.struct.getSize() + Rotation3d.struct.getSize(); - } - - @Override - public String getSchema() { - return "Translation3d translation;Rotation3d rotation"; - } - - @Override - public Struct[] getNested() { - return new Struct[] {Translation3d.struct, Rotation3d.struct}; - } - - @Override - public Transform3d unpack(ByteBuffer bb) { - Translation3d translation = Translation3d.struct.unpack(bb); - Rotation3d rotation = Rotation3d.struct.unpack(bb); - return new Transform3d(translation, rotation); - } - - @Override - public void pack(ByteBuffer bb, Transform3d value) { - Translation3d.struct.pack(bb, value.m_translation); - Rotation3d.struct.pack(bb, value.m_rotation); - } - } - - public static final AStruct struct = new AStruct(); - - public static final class AProto implements Protobuf { - @Override - public Class getTypeClass() { - return Transform3d.class; - } - - @Override - public Descriptor getDescriptor() { - return ProtobufTransform3d.getDescriptor(); - } - - @Override - public Protobuf[] getNested() { - return new Protobuf[] {Translation3d.proto, Rotation3d.proto}; - } - - @Override - public ProtobufTransform3d createMessage() { - return ProtobufTransform3d.newInstance(); - } - - @Override - public Transform3d unpack(ProtobufTransform3d msg) { - return new Transform3d( - Translation3d.proto.unpack(msg.getTranslation()), - Rotation3d.proto.unpack(msg.getRotation())); - } - - @Override - public void pack(ProtobufTransform3d msg, Transform3d value) { - Translation3d.proto.pack(msg.getMutableTranslation(), value.m_translation); - Rotation3d.proto.pack(msg.getMutableRotation(), value.m_rotation); - } - } - - public static final AProto proto = new AProto(); + public static final Transform3dStruct struct = new Transform3dStruct(); + public static final Transform3dProto proto = new Transform3dProto(); } 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 ebfce32045..4c8d17c8dc 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 @@ -11,18 +11,15 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.proto.Translation2dProto; +import edu.wpi.first.math.geometry.struct.Translation2dStruct; import edu.wpi.first.math.interpolation.Interpolatable; -import edu.wpi.first.math.proto.Geometry2D.ProtobufTranslation2d; import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; -import edu.wpi.first.util.protobuf.Protobuf; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; import java.util.Collections; import java.util.Comparator; import java.util.List; import java.util.Objects; -import us.hebi.quickbuf.Descriptors.Descriptor; /** * Represents a translation in 2D space. This object can be used to represent a point or a vector. @@ -250,69 +247,6 @@ public class Translation2d implements Interpolatable { MathUtil.interpolate(this.getY(), endValue.getY(), t)); } - public static final class AStruct implements Struct { - @Override - public Class getTypeClass() { - return Translation2d.class; - } - - @Override - public String getTypeString() { - return "struct:Translation2d"; - } - - @Override - public int getSize() { - return kSizeDouble * 2; - } - - @Override - public String getSchema() { - return "double x;double y"; - } - - @Override - public Translation2d unpack(ByteBuffer bb) { - double x = bb.getDouble(); - double y = bb.getDouble(); - return new Translation2d(x, y); - } - - @Override - public void pack(ByteBuffer bb, Translation2d value) { - bb.putDouble(value.m_x); - bb.putDouble(value.m_y); - } - } - - public static final AStruct struct = new AStruct(); - - public static final class AProto implements Protobuf { - @Override - public Class getTypeClass() { - return Translation2d.class; - } - - @Override - public Descriptor getDescriptor() { - return ProtobufTranslation2d.getDescriptor(); - } - - @Override - public ProtobufTranslation2d createMessage() { - return ProtobufTranslation2d.newInstance(); - } - - @Override - public Translation2d unpack(ProtobufTranslation2d msg) { - return new Translation2d(msg.getX(), msg.getY()); - } - - @Override - public void pack(ProtobufTranslation2d msg, Translation2d value) { - msg.setX(value.m_x).setY(value.m_y); - } - } - - public static final AProto proto = new AProto(); + public static final Translation2dStruct struct = new Translation2dStruct(); + public static final Translation2dProto proto = new Translation2dProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java index 7b690dfedf..8d12d56512 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java @@ -11,15 +11,12 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.proto.Translation3dProto; +import edu.wpi.first.math.geometry.struct.Translation3dStruct; import edu.wpi.first.math.interpolation.Interpolatable; -import edu.wpi.first.math.proto.Geometry3D.ProtobufTranslation3d; import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; -import edu.wpi.first.util.protobuf.Protobuf; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; import java.util.Objects; -import us.hebi.quickbuf.Descriptors.Descriptor; /** * Represents a translation in 3D space. This object can be used to represent a point or a vector. @@ -253,71 +250,6 @@ public class Translation3d implements Interpolatable { MathUtil.interpolate(this.getZ(), endValue.getZ(), t)); } - public static final class AStruct implements Struct { - @Override - public Class getTypeClass() { - return Translation3d.class; - } - - @Override - public String getTypeString() { - return "struct:Translation3d"; - } - - @Override - public int getSize() { - return kSizeDouble * 3; - } - - @Override - public String getSchema() { - return "double x;double y;double z"; - } - - @Override - public Translation3d unpack(ByteBuffer bb) { - double x = bb.getDouble(); - double y = bb.getDouble(); - double z = bb.getDouble(); - return new Translation3d(x, y, z); - } - - @Override - public void pack(ByteBuffer bb, Translation3d value) { - bb.putDouble(value.m_x); - bb.putDouble(value.m_y); - bb.putDouble(value.m_z); - } - } - - public static final AStruct struct = new AStruct(); - - public static final class AProto implements Protobuf { - @Override - public Class getTypeClass() { - return Translation3d.class; - } - - @Override - public Descriptor getDescriptor() { - return ProtobufTranslation3d.getDescriptor(); - } - - @Override - public ProtobufTranslation3d createMessage() { - return ProtobufTranslation3d.newInstance(); - } - - @Override - public Translation3d unpack(ProtobufTranslation3d msg) { - return new Translation3d(msg.getX(), msg.getY(), msg.getZ()); - } - - @Override - public void pack(ProtobufTranslation3d msg, Translation3d value) { - msg.setX(value.m_x).setY(value.m_y).setZ(value.m_z); - } - } - - public static final AProto proto = new AProto(); + public static final Translation3dStruct struct = new Translation3dStruct(); + public static final Translation3dProto proto = new Translation3dProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java index a4ae9f83ac..1a0b678664 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java @@ -4,12 +4,9 @@ package edu.wpi.first.math.geometry; -import edu.wpi.first.math.proto.Geometry2D.ProtobufTwist2d; -import edu.wpi.first.util.protobuf.Protobuf; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; +import edu.wpi.first.math.geometry.proto.Twist2dProto; +import edu.wpi.first.math.geometry.struct.Twist2dStruct; import java.util.Objects; -import us.hebi.quickbuf.Descriptors.Descriptor; /** * A change in distance along a 2D arc since the last pose update. We can use ideas from @@ -68,71 +65,6 @@ public class Twist2d { return Objects.hash(dx, dy, dtheta); } - public static final class AStruct implements Struct { - @Override - public Class getTypeClass() { - return Twist2d.class; - } - - @Override - public String getTypeString() { - return "struct:Twist2d"; - } - - @Override - public int getSize() { - return kSizeDouble * 3; - } - - @Override - public String getSchema() { - return "double dx;double dy;double dtheta"; - } - - @Override - public Twist2d unpack(ByteBuffer bb) { - double dx = bb.getDouble(); - double dy = bb.getDouble(); - double dtheta = bb.getDouble(); - return new Twist2d(dx, dy, dtheta); - } - - @Override - public void pack(ByteBuffer bb, Twist2d value) { - bb.putDouble(value.dx); - bb.putDouble(value.dy); - bb.putDouble(value.dtheta); - } - } - - public static final AStruct struct = new AStruct(); - - public static final class AProto implements Protobuf { - @Override - public Class getTypeClass() { - return Twist2d.class; - } - - @Override - public Descriptor getDescriptor() { - return ProtobufTwist2d.getDescriptor(); - } - - @Override - public ProtobufTwist2d createMessage() { - return ProtobufTwist2d.newInstance(); - } - - @Override - public Twist2d unpack(ProtobufTwist2d msg) { - return new Twist2d(msg.getDx(), msg.getDy(), msg.getDtheta()); - } - - @Override - public void pack(ProtobufTwist2d msg, Twist2d value) { - msg.setDx(value.dx).setDy(value.dy).setDtheta(value.dtheta); - } - } - - public static final AProto proto = new AProto(); + public static final Twist2dStruct struct = new Twist2dStruct(); + public static final Twist2dProto proto = new Twist2dProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java index d08d5cf158..8b2470f727 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java @@ -4,12 +4,9 @@ package edu.wpi.first.math.geometry; -import edu.wpi.first.math.proto.Geometry3D.ProtobufTwist3d; -import edu.wpi.first.util.protobuf.Protobuf; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; +import edu.wpi.first.math.geometry.proto.Twist3dProto; +import edu.wpi.first.math.geometry.struct.Twist3dStruct; import java.util.Objects; -import us.hebi.quickbuf.Descriptors.Descriptor; /** * A change in distance along a 3D arc since the last pose update. We can use ideas from @@ -88,83 +85,6 @@ public class Twist3d { return Objects.hash(dx, dy, dz, rx, ry, rz); } - public static final class AStruct implements Struct { - @Override - public Class getTypeClass() { - return Twist3d.class; - } - - @Override - public String getTypeString() { - return "struct:Twist3d"; - } - - @Override - public int getSize() { - return kSizeDouble * 6; - } - - @Override - public String getSchema() { - return "double dx;double dy;double dz;double rx;double ry;double rz"; - } - - @Override - public Twist3d unpack(ByteBuffer bb) { - double dx = bb.getDouble(); - double dy = bb.getDouble(); - double dz = bb.getDouble(); - double rx = bb.getDouble(); - double ry = bb.getDouble(); - double rz = bb.getDouble(); - return new Twist3d(dx, dy, dz, rx, ry, rz); - } - - @Override - public void pack(ByteBuffer bb, Twist3d value) { - bb.putDouble(value.dx); - bb.putDouble(value.dy); - bb.putDouble(value.dz); - bb.putDouble(value.rx); - bb.putDouble(value.ry); - bb.putDouble(value.rz); - } - } - - public static final AStruct struct = new AStruct(); - - public static final class AProto implements Protobuf { - @Override - public Class getTypeClass() { - return Twist3d.class; - } - - @Override - public Descriptor getDescriptor() { - return ProtobufTwist3d.getDescriptor(); - } - - @Override - public ProtobufTwist3d createMessage() { - return ProtobufTwist3d.newInstance(); - } - - @Override - public Twist3d unpack(ProtobufTwist3d msg) { - return new Twist3d( - msg.getDx(), msg.getDy(), msg.getDz(), msg.getRx(), msg.getRy(), msg.getRz()); - } - - @Override - public void pack(ProtobufTwist3d msg, Twist3d value) { - msg.setDx(value.dx) - .setDy(value.dy) - .setDz(value.dz) - .setRx(value.rx) - .setRy(value.ry) - .setRz(value.rz); - } - } - - public static final AProto proto = new AProto(); + public static final Twist3dStruct struct = new Twist3dStruct(); + public static final Twist3dProto proto = new Twist3dProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Pose2dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Pose2dProto.java new file mode 100644 index 0000000000..7921100d6e --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Pose2dProto.java @@ -0,0 +1,47 @@ +// 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.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.proto.Geometry2D.ProtobufPose2d; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class Pose2dProto implements Protobuf { + @Override + public Class getTypeClass() { + return Pose2d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufPose2d.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Translation2d.proto, Rotation2d.proto}; + } + + @Override + public ProtobufPose2d createMessage() { + return ProtobufPose2d.newInstance(); + } + + @Override + public Pose2d unpack(ProtobufPose2d msg) { + return new Pose2d( + Translation2d.proto.unpack(msg.getTranslation()), + Rotation2d.proto.unpack(msg.getRotation())); + } + + @Override + public void pack(ProtobufPose2d msg, Pose2d value) { + Translation2d.proto.pack(msg.getMutableTranslation(), value.getTranslation()); + Rotation2d.proto.pack(msg.getMutableRotation(), value.getRotation()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Pose3dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Pose3dProto.java new file mode 100644 index 0000000000..69d6ba2926 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Pose3dProto.java @@ -0,0 +1,47 @@ +// 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.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.proto.Geometry3D.ProtobufPose3d; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class Pose3dProto implements Protobuf { + @Override + public Class getTypeClass() { + return Pose3d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufPose3d.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Translation3d.proto, Rotation3d.proto}; + } + + @Override + public ProtobufPose3d createMessage() { + return ProtobufPose3d.newInstance(); + } + + @Override + public Pose3d unpack(ProtobufPose3d msg) { + return new Pose3d( + Translation3d.proto.unpack(msg.getTranslation()), + Rotation3d.proto.unpack(msg.getRotation())); + } + + @Override + public void pack(ProtobufPose3d msg, Pose3d value) { + Translation3d.proto.pack(msg.getMutableTranslation(), value.getTranslation()); + Rotation3d.proto.pack(msg.getMutableRotation(), value.getRotation()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/QuaternionProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/QuaternionProto.java new file mode 100644 index 0000000000..c464ba270c --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/QuaternionProto.java @@ -0,0 +1,40 @@ +// 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.Quaternion; +import edu.wpi.first.math.proto.Geometry3D.ProtobufQuaternion; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class QuaternionProto implements Protobuf { + @Override + public Class getTypeClass() { + return Quaternion.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufQuaternion.getDescriptor(); + } + + @Override + public ProtobufQuaternion createMessage() { + return ProtobufQuaternion.newInstance(); + } + + @Override + public Quaternion unpack(ProtobufQuaternion msg) { + return new Quaternion(msg.getW(), msg.getX(), msg.getY(), msg.getZ()); + } + + @Override + public void pack(ProtobufQuaternion msg, Quaternion value) { + msg.setW(value.getW()); + msg.setX(value.getX()); + msg.setY(value.getY()); + msg.setZ(value.getZ()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rotation2dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rotation2dProto.java new file mode 100644 index 0000000000..98ad5c7bcf --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rotation2dProto.java @@ -0,0 +1,37 @@ +// 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.Rotation2d; +import edu.wpi.first.math.proto.Geometry2D.ProtobufRotation2d; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class Rotation2dProto implements Protobuf { + @Override + public Class getTypeClass() { + return Rotation2d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufRotation2d.getDescriptor(); + } + + @Override + public ProtobufRotation2d createMessage() { + return ProtobufRotation2d.newInstance(); + } + + @Override + public Rotation2d unpack(ProtobufRotation2d msg) { + return new Rotation2d(msg.getValue()); + } + + @Override + public void pack(ProtobufRotation2d msg, Rotation2d value) { + msg.setValue(value.getRadians()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rotation3dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rotation3dProto.java new file mode 100644 index 0000000000..9c9b01a93b --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rotation3dProto.java @@ -0,0 +1,43 @@ +// 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.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.proto.Geometry3D.ProtobufRotation3d; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class Rotation3dProto implements Protobuf { + @Override + public Class getTypeClass() { + return Rotation3d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufRotation3d.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Quaternion.proto}; + } + + @Override + public ProtobufRotation3d createMessage() { + return ProtobufRotation3d.newInstance(); + } + + @Override + public Rotation3d unpack(ProtobufRotation3d msg) { + return new Rotation3d(Quaternion.proto.unpack(msg.getQ())); + } + + @Override + public void pack(ProtobufRotation3d msg, Rotation3d value) { + Quaternion.proto.pack(msg.getMutableQ(), value.getQuaternion()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Transform2dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Transform2dProto.java new file mode 100644 index 0000000000..ecc05287cf --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Transform2dProto.java @@ -0,0 +1,47 @@ +// 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.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.proto.Geometry2D.ProtobufTransform2d; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class Transform2dProto implements Protobuf { + @Override + public Class getTypeClass() { + return Transform2d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTransform2d.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Translation2d.proto, Rotation2d.proto}; + } + + @Override + public ProtobufTransform2d createMessage() { + return ProtobufTransform2d.newInstance(); + } + + @Override + public Transform2d unpack(ProtobufTransform2d msg) { + return new Transform2d( + Translation2d.proto.unpack(msg.getTranslation()), + Rotation2d.proto.unpack(msg.getRotation())); + } + + @Override + public void pack(ProtobufTransform2d msg, Transform2d value) { + Translation2d.proto.pack(msg.getMutableTranslation(), value.getTranslation()); + Rotation2d.proto.pack(msg.getMutableRotation(), value.getRotation()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Transform3dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Transform3dProto.java new file mode 100644 index 0000000000..29ea793e60 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Transform3dProto.java @@ -0,0 +1,47 @@ +// 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.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.proto.Geometry3D.ProtobufTransform3d; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class Transform3dProto implements Protobuf { + @Override + public Class getTypeClass() { + return Transform3d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTransform3d.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Translation3d.proto, Rotation3d.proto}; + } + + @Override + public ProtobufTransform3d createMessage() { + return ProtobufTransform3d.newInstance(); + } + + @Override + public Transform3d unpack(ProtobufTransform3d msg) { + return new Transform3d( + Translation3d.proto.unpack(msg.getTranslation()), + Rotation3d.proto.unpack(msg.getRotation())); + } + + @Override + public void pack(ProtobufTransform3d msg, Transform3d value) { + Translation3d.proto.pack(msg.getMutableTranslation(), value.getTranslation()); + Rotation3d.proto.pack(msg.getMutableRotation(), value.getRotation()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Translation2dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Translation2dProto.java new file mode 100644 index 0000000000..a63fec35bd --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Translation2dProto.java @@ -0,0 +1,38 @@ +// 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.Translation2d; +import edu.wpi.first.math.proto.Geometry2D.ProtobufTranslation2d; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class Translation2dProto implements Protobuf { + @Override + public Class getTypeClass() { + return Translation2d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTranslation2d.getDescriptor(); + } + + @Override + public ProtobufTranslation2d createMessage() { + return ProtobufTranslation2d.newInstance(); + } + + @Override + public Translation2d unpack(ProtobufTranslation2d msg) { + return new Translation2d(msg.getX(), msg.getY()); + } + + @Override + public void pack(ProtobufTranslation2d msg, Translation2d value) { + msg.setX(value.getX()); + msg.setY(value.getY()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Translation3dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Translation3dProto.java new file mode 100644 index 0000000000..b854a343f5 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Translation3dProto.java @@ -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. + +package edu.wpi.first.math.geometry.proto; + +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.proto.Geometry3D.ProtobufTranslation3d; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class Translation3dProto implements Protobuf { + @Override + public Class getTypeClass() { + return Translation3d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTranslation3d.getDescriptor(); + } + + @Override + public ProtobufTranslation3d createMessage() { + return ProtobufTranslation3d.newInstance(); + } + + @Override + public Translation3d unpack(ProtobufTranslation3d msg) { + return new Translation3d(msg.getX(), msg.getY(), msg.getZ()); + } + + @Override + public void pack(ProtobufTranslation3d msg, Translation3d value) { + msg.setX(value.getX()); + msg.setY(value.getY()); + msg.setZ(value.getZ()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Twist2dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Twist2dProto.java new file mode 100644 index 0000000000..bc9da6b341 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Twist2dProto.java @@ -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. + +package edu.wpi.first.math.geometry.proto; + +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.proto.Geometry2D.ProtobufTwist2d; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class Twist2dProto implements Protobuf { + @Override + public Class getTypeClass() { + return Twist2d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTwist2d.getDescriptor(); + } + + @Override + public ProtobufTwist2d createMessage() { + return ProtobufTwist2d.newInstance(); + } + + @Override + public Twist2d unpack(ProtobufTwist2d msg) { + return new Twist2d(msg.getDx(), msg.getDy(), msg.getDtheta()); + } + + @Override + public void pack(ProtobufTwist2d msg, Twist2d value) { + msg.setDx(value.dx); + msg.setDy(value.dy); + msg.setDtheta(value.dtheta); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Twist3dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Twist3dProto.java new file mode 100644 index 0000000000..356c18f7a1 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Twist3dProto.java @@ -0,0 +1,43 @@ +// 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.Twist3d; +import edu.wpi.first.math.proto.Geometry3D.ProtobufTwist3d; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class Twist3dProto implements Protobuf { + @Override + public Class getTypeClass() { + return Twist3d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTwist3d.getDescriptor(); + } + + @Override + public ProtobufTwist3d createMessage() { + return ProtobufTwist3d.newInstance(); + } + + @Override + public Twist3d unpack(ProtobufTwist3d msg) { + return new Twist3d( + msg.getDx(), msg.getDy(), msg.getDz(), msg.getRx(), msg.getRy(), msg.getRz()); + } + + @Override + public void pack(ProtobufTwist3d msg, Twist3d value) { + msg.setDx(value.dx); + msg.setDy(value.dy); + msg.setDz(value.dz); + msg.setRx(value.rx); + msg.setRy(value.ry); + msg.setRz(value.rz); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Pose2dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Pose2dStruct.java new file mode 100644 index 0000000000..8435354db5 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Pose2dStruct.java @@ -0,0 +1,51 @@ +// 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.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class Pose2dStruct implements Struct { + @Override + public Class getTypeClass() { + return Pose2d.class; + } + + @Override + public String getTypeString() { + return "struct:Pose2d"; + } + + @Override + public int getSize() { + return Translation2d.struct.getSize() + Rotation2d.struct.getSize(); + } + + @Override + public String getSchema() { + return "Translation2d translation;Rotation2d rotation"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Translation2d.struct, Rotation2d.struct}; + } + + @Override + public Pose2d unpack(ByteBuffer bb) { + Translation2d translation = Translation2d.struct.unpack(bb); + Rotation2d rotation = Rotation2d.struct.unpack(bb); + return new Pose2d(translation, rotation); + } + + @Override + public void pack(ByteBuffer bb, Pose2d value) { + Translation2d.struct.pack(bb, value.getTranslation()); + Rotation2d.struct.pack(bb, value.getRotation()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Pose3dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Pose3dStruct.java new file mode 100644 index 0000000000..db380f6f90 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Pose3dStruct.java @@ -0,0 +1,51 @@ +// 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.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class Pose3dStruct implements Struct { + @Override + public Class getTypeClass() { + return Pose3d.class; + } + + @Override + public String getTypeString() { + return "struct:Pose3d"; + } + + @Override + public int getSize() { + return Translation3d.struct.getSize() + Rotation3d.struct.getSize(); + } + + @Override + public String getSchema() { + return "Translation3d translation;Rotation3d rotation"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Translation3d.struct, Rotation3d.struct}; + } + + @Override + public Pose3d unpack(ByteBuffer bb) { + Translation3d translation = Translation3d.struct.unpack(bb); + Rotation3d rotation = Rotation3d.struct.unpack(bb); + return new Pose3d(translation, rotation); + } + + @Override + public void pack(ByteBuffer bb, Pose3d value) { + Translation3d.struct.pack(bb, value.getTranslation()); + Rotation3d.struct.pack(bb, value.getRotation()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/QuaternionStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/QuaternionStruct.java new file mode 100644 index 0000000000..fec8452b0e --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/QuaternionStruct.java @@ -0,0 +1,48 @@ +// 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.Quaternion; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class QuaternionStruct implements Struct { + @Override + public Class getTypeClass() { + return Quaternion.class; + } + + @Override + public String getTypeString() { + return "struct:Quaternion"; + } + + @Override + public int getSize() { + return kSizeDouble * 4; + } + + @Override + public String getSchema() { + return "double w;double x;double y;double z"; + } + + @Override + public Quaternion unpack(ByteBuffer bb) { + double w = bb.getDouble(); + double x = bb.getDouble(); + double y = bb.getDouble(); + double z = bb.getDouble(); + return new Quaternion(w, x, y, z); + } + + @Override + public void pack(ByteBuffer bb, Quaternion value) { + bb.putDouble(value.getW()); + bb.putDouble(value.getX()); + bb.putDouble(value.getY()); + bb.putDouble(value.getZ()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rotation2dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rotation2dStruct.java new file mode 100644 index 0000000000..b22e3d15eb --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rotation2dStruct.java @@ -0,0 +1,42 @@ +// 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.Rotation2d; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class Rotation2dStruct implements Struct { + @Override + public Class getTypeClass() { + return Rotation2d.class; + } + + @Override + public String getTypeString() { + return "struct:Rotation2d"; + } + + @Override + public int getSize() { + return kSizeDouble; + } + + @Override + public String getSchema() { + return "double value"; + } + + @Override + public Rotation2d unpack(ByteBuffer bb) { + double value = bb.getDouble(); + return new Rotation2d(value); + } + + @Override + public void pack(ByteBuffer bb, Rotation2d value) { + bb.putDouble(value.getRadians()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rotation3dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rotation3dStruct.java new file mode 100644 index 0000000000..4184990a22 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rotation3dStruct.java @@ -0,0 +1,48 @@ +// 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.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class Rotation3dStruct implements Struct { + @Override + public Class getTypeClass() { + return Rotation3d.class; + } + + @Override + public String getTypeString() { + return "struct:Rotation3d"; + } + + @Override + public int getSize() { + return Quaternion.struct.getSize(); + } + + @Override + public String getSchema() { + return "Quaternion q"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Quaternion.struct}; + } + + @Override + public Rotation3d unpack(ByteBuffer bb) { + Quaternion q = Quaternion.struct.unpack(bb); + return new Rotation3d(q); + } + + @Override + public void pack(ByteBuffer bb, Rotation3d value) { + Quaternion.struct.pack(bb, value.getQuaternion()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Transform2dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Transform2dStruct.java new file mode 100644 index 0000000000..298d6db4e2 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Transform2dStruct.java @@ -0,0 +1,51 @@ +// 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.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class Transform2dStruct implements Struct { + @Override + public Class getTypeClass() { + return Transform2d.class; + } + + @Override + public String getTypeString() { + return "struct:Transform2d"; + } + + @Override + public int getSize() { + return Translation2d.struct.getSize() + Rotation2d.struct.getSize(); + } + + @Override + public String getSchema() { + return "Translation2d translation;Rotation2d rotation"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Translation2d.struct, Rotation2d.struct}; + } + + @Override + public Transform2d unpack(ByteBuffer bb) { + Translation2d translation = Translation2d.struct.unpack(bb); + Rotation2d rotation = Rotation2d.struct.unpack(bb); + return new Transform2d(translation, rotation); + } + + @Override + public void pack(ByteBuffer bb, Transform2d value) { + Translation2d.struct.pack(bb, value.getTranslation()); + Rotation2d.struct.pack(bb, value.getRotation()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Transform3dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Transform3dStruct.java new file mode 100644 index 0000000000..2ad5bc153b --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Transform3dStruct.java @@ -0,0 +1,51 @@ +// 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.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class Transform3dStruct implements Struct { + @Override + public Class getTypeClass() { + return Transform3d.class; + } + + @Override + public String getTypeString() { + return "struct:Transform3d"; + } + + @Override + public int getSize() { + return Translation3d.struct.getSize() + Rotation3d.struct.getSize(); + } + + @Override + public String getSchema() { + return "Translation3d translation;Rotation3d rotation"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Translation3d.struct, Rotation3d.struct}; + } + + @Override + public Transform3d unpack(ByteBuffer bb) { + Translation3d translation = Translation3d.struct.unpack(bb); + Rotation3d rotation = Rotation3d.struct.unpack(bb); + return new Transform3d(translation, rotation); + } + + @Override + public void pack(ByteBuffer bb, Transform3d value) { + Translation3d.struct.pack(bb, value.getTranslation()); + Rotation3d.struct.pack(bb, value.getRotation()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Translation2dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Translation2dStruct.java new file mode 100644 index 0000000000..aac179aacb --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Translation2dStruct.java @@ -0,0 +1,44 @@ +// 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.Translation2d; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class Translation2dStruct implements Struct { + @Override + public Class getTypeClass() { + return Translation2d.class; + } + + @Override + public String getTypeString() { + return "struct:Translation2d"; + } + + @Override + public int getSize() { + return kSizeDouble * 2; + } + + @Override + public String getSchema() { + return "double x;double y"; + } + + @Override + public Translation2d unpack(ByteBuffer bb) { + double x = bb.getDouble(); + double y = bb.getDouble(); + return new Translation2d(x, y); + } + + @Override + public void pack(ByteBuffer bb, Translation2d value) { + bb.putDouble(value.getX()); + bb.putDouble(value.getY()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Translation3dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Translation3dStruct.java new file mode 100644 index 0000000000..f4e10299fd --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Translation3dStruct.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.struct; + +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class Translation3dStruct implements Struct { + @Override + public Class getTypeClass() { + return Translation3d.class; + } + + @Override + public String getTypeString() { + return "struct:Translation3d"; + } + + @Override + public int getSize() { + return kSizeDouble * 3; + } + + @Override + public String getSchema() { + return "double x;double y;double z"; + } + + @Override + public Translation3d unpack(ByteBuffer bb) { + double x = bb.getDouble(); + double y = bb.getDouble(); + double z = bb.getDouble(); + return new Translation3d(x, y, z); + } + + @Override + public void pack(ByteBuffer bb, Translation3d value) { + bb.putDouble(value.getX()); + bb.putDouble(value.getY()); + bb.putDouble(value.getZ()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Twist2dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Twist2dStruct.java new file mode 100644 index 0000000000..7797f3ac9d --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Twist2dStruct.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.struct; + +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class Twist2dStruct implements Struct { + @Override + public Class getTypeClass() { + return Twist2d.class; + } + + @Override + public String getTypeString() { + return "struct:Twist2d"; + } + + @Override + public int getSize() { + return kSizeDouble * 3; + } + + @Override + public String getSchema() { + return "double dx;double dy;double dtheta"; + } + + @Override + public Twist2d unpack(ByteBuffer bb) { + double dx = bb.getDouble(); + double dy = bb.getDouble(); + double dtheta = bb.getDouble(); + return new Twist2d(dx, dy, dtheta); + } + + @Override + public void pack(ByteBuffer bb, Twist2d value) { + bb.putDouble(value.dx); + bb.putDouble(value.dy); + bb.putDouble(value.dtheta); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Twist3dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Twist3dStruct.java new file mode 100644 index 0000000000..68926a4c41 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Twist3dStruct.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.Twist3d; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class Twist3dStruct implements Struct { + @Override + public Class getTypeClass() { + return Twist3d.class; + } + + @Override + public String getTypeString() { + return "struct:Twist3d"; + } + + @Override + public int getSize() { + return kSizeDouble * 6; + } + + @Override + public String getSchema() { + return "double dx;double dy;double dz;double rx;double ry;double rz"; + } + + @Override + public Twist3d unpack(ByteBuffer bb) { + double dx = bb.getDouble(); + double dy = bb.getDouble(); + double dz = bb.getDouble(); + double rx = bb.getDouble(); + double ry = bb.getDouble(); + double rz = bb.getDouble(); + return new Twist3d(dx, dy, dz, rx, ry, rz); + } + + @Override + public void pack(ByteBuffer bb, Twist3d value) { + bb.putDouble(value.dx); + bb.putDouble(value.dy); + bb.putDouble(value.dz); + bb.putDouble(value.rx); + bb.putDouble(value.ry); + bb.putDouble(value.rz); + } +} diff --git a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp index 2e15216527..53779d05c8 100644 --- a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp @@ -9,7 +9,6 @@ #include #include "frc/MathUtil.h" -#include "geometry2d.pb.h" using namespace frc; @@ -109,23 +108,3 @@ void frc::from_json(const wpi::json& json, Pose2d& pose) { pose = Pose2d{json.at("translation").get(), json.at("rotation").get()}; } - -google::protobuf::Message* wpi::Protobuf::New( - google::protobuf::Arena* arena) { - return google::protobuf::Arena::CreateMessage( - arena); -} - -frc::Pose2d wpi::Protobuf::Unpack( - const google::protobuf::Message& msg) { - auto m = static_cast(&msg); - return Pose2d{wpi::UnpackProtobuf(m->translation()), - wpi::UnpackProtobuf(m->rotation())}; -} - -void wpi::Protobuf::Pack(google::protobuf::Message* msg, - const frc::Pose2d& value) { - auto m = static_cast(msg); - wpi::PackProtobuf(m->mutable_translation(), value.Translation()); - wpi::PackProtobuf(m->mutable_rotation(), value.Rotation()); -} diff --git a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp index ffbaecbff4..90f7b491f5 100644 --- a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp @@ -189,23 +189,3 @@ void frc::from_json(const wpi::json& json, Pose3d& pose) { pose = Pose3d{json.at("translation").get(), json.at("rotation").get()}; } - -google::protobuf::Message* wpi::Protobuf::New( - google::protobuf::Arena* arena) { - return google::protobuf::Arena::CreateMessage( - arena); -} - -frc::Pose3d wpi::Protobuf::Unpack( - const google::protobuf::Message& msg) { - auto m = static_cast(&msg); - return Pose3d{wpi::UnpackProtobuf(m->translation()), - wpi::UnpackProtobuf(m->rotation())}; -} - -void wpi::Protobuf::Pack(google::protobuf::Message* msg, - const frc::Pose3d& value) { - auto m = static_cast(msg); - wpi::PackProtobuf(m->mutable_translation(), value.Translation()); - wpi::PackProtobuf(m->mutable_rotation(), value.Rotation()); -} diff --git a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp index 37afbb88a3..3aad64c377 100644 --- a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp +++ b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp @@ -232,24 +232,3 @@ void frc::from_json(const wpi::json& json, Quaternion& quaternion) { Quaternion{json.at("W").get(), json.at("X").get(), json.at("Y").get(), json.at("Z").get()}; } - -google::protobuf::Message* wpi::Protobuf::New( - google::protobuf::Arena* arena) { - return google::protobuf::Arena::CreateMessage( - arena); -} - -frc::Quaternion wpi::Protobuf::Unpack( - const google::protobuf::Message& msg) { - auto m = static_cast(&msg); - return frc::Quaternion{m->w(), m->x(), m->y(), m->z()}; -} - -void wpi::Protobuf::Pack(google::protobuf::Message* msg, - const frc::Quaternion& value) { - auto m = static_cast(msg); - m->set_w(value.W()); - m->set_x(value.X()); - m->set_y(value.Y()); - m->set_z(value.Z()); -} diff --git a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp index 05a644ea2b..69193028f7 100644 --- a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp @@ -20,21 +20,3 @@ void frc::to_json(wpi::json& json, const Rotation2d& rotation) { void frc::from_json(const wpi::json& json, Rotation2d& rotation) { rotation = Rotation2d{units::radian_t{json.at("radians").get()}}; } - -google::protobuf::Message* wpi::Protobuf::New( - google::protobuf::Arena* arena) { - return google::protobuf::Arena::CreateMessage( - arena); -} - -frc::Rotation2d wpi::Protobuf::Unpack( - const google::protobuf::Message& msg) { - auto m = static_cast(&msg); - return frc::Rotation2d{units::radian_t{m->value()}}; -} - -void wpi::Protobuf::Pack(google::protobuf::Message* msg, - const frc::Rotation2d& value) { - auto m = static_cast(msg); - m->set_value(value.Radians().value()); -} diff --git a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp index 3a68870045..072d023cb2 100644 --- a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp @@ -262,21 +262,3 @@ void frc::to_json(wpi::json& json, const Rotation3d& rotation) { void frc::from_json(const wpi::json& json, Rotation3d& rotation) { rotation = Rotation3d{json.at("quaternion").get()}; } - -google::protobuf::Message* wpi::Protobuf::New( - google::protobuf::Arena* arena) { - return google::protobuf::Arena::CreateMessage( - arena); -} - -frc::Rotation3d wpi::Protobuf::Unpack( - const google::protobuf::Message& msg) { - auto m = static_cast(&msg); - return Rotation3d{wpi::UnpackProtobuf(m->q())}; -} - -void wpi::Protobuf::Pack(google::protobuf::Message* msg, - const frc::Rotation3d& value) { - auto m = static_cast(msg); - wpi::PackProtobuf(m->mutable_q(), value.GetQuaternion()); -} diff --git a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp index 77f3ceed2d..157359bfbc 100644 --- a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp @@ -22,23 +22,3 @@ Transform2d::Transform2d(Pose2d initial, Pose2d final) { Transform2d Transform2d::operator+(const Transform2d& other) const { return Transform2d{Pose2d{}, Pose2d{}.TransformBy(*this).TransformBy(other)}; } - -google::protobuf::Message* wpi::Protobuf::New( - google::protobuf::Arena* arena) { - return google::protobuf::Arena::CreateMessage< - wpi::proto::ProtobufTransform2d>(arena); -} - -frc::Transform2d wpi::Protobuf::Unpack( - const google::protobuf::Message& msg) { - auto m = static_cast(&msg); - return Transform2d{wpi::UnpackProtobuf(m->translation()), - wpi::UnpackProtobuf(m->rotation())}; -} - -void wpi::Protobuf::Pack(google::protobuf::Message* msg, - const frc::Transform2d& value) { - auto m = static_cast(msg); - wpi::PackProtobuf(m->mutable_translation(), value.Translation()); - wpi::PackProtobuf(m->mutable_rotation(), value.Rotation()); -} diff --git a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp index de6c2533c9..556a3027d0 100644 --- a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp @@ -5,7 +5,6 @@ #include "frc/geometry/Transform3d.h" #include "frc/geometry/Pose3d.h" -#include "geometry3d.pb.h" using namespace frc; @@ -36,23 +35,3 @@ Transform3d Transform3d::Inverse() const { Transform3d Transform3d::operator+(const Transform3d& other) const { return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)}; } - -google::protobuf::Message* wpi::Protobuf::New( - google::protobuf::Arena* arena) { - return google::protobuf::Arena::CreateMessage< - wpi::proto::ProtobufTransform3d>(arena); -} - -frc::Transform3d wpi::Protobuf::Unpack( - const google::protobuf::Message& msg) { - auto m = static_cast(&msg); - return Transform3d{wpi::UnpackProtobuf(m->translation()), - wpi::UnpackProtobuf(m->rotation())}; -} - -void wpi::Protobuf::Pack(google::protobuf::Message* msg, - const frc::Transform3d& value) { - auto m = static_cast(msg); - wpi::PackProtobuf(m->mutable_translation(), value.Translation()); - wpi::PackProtobuf(m->mutable_rotation(), value.Rotation()); -} diff --git a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp index 6d5f3153ae..c875582820 100644 --- a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp @@ -49,22 +49,3 @@ void frc::from_json(const wpi::json& json, Translation2d& translation) { translation = Translation2d{units::meter_t{json.at("x").get()}, units::meter_t{json.at("y").get()}}; } - -google::protobuf::Message* wpi::Protobuf::New( - google::protobuf::Arena* arena) { - return google::protobuf::Arena::CreateMessage< - wpi::proto::ProtobufTranslation2d>(arena); -} - -frc::Translation2d wpi::Protobuf::Unpack( - const google::protobuf::Message& msg) { - auto m = static_cast(&msg); - return frc::Translation2d{units::meter_t{m->x()}, units::meter_t{m->y()}}; -} - -void wpi::Protobuf::Pack(google::protobuf::Message* msg, - const frc::Translation2d& value) { - auto m = static_cast(msg); - m->set_x(value.X().value()); - m->set_y(value.Y().value()); -} diff --git a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp index 90e94ae825..ecfee1ca3a 100644 --- a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp @@ -53,24 +53,3 @@ void frc::from_json(const wpi::json& json, Translation3d& translation) { units::meter_t{json.at("y").get()}, units::meter_t{json.at("z").get()}}; } - -google::protobuf::Message* wpi::Protobuf::New( - google::protobuf::Arena* arena) { - return google::protobuf::Arena::CreateMessage< - wpi::proto::ProtobufTranslation3d>(arena); -} - -frc::Translation3d wpi::Protobuf::Unpack( - const google::protobuf::Message& msg) { - auto m = static_cast(&msg); - return frc::Translation3d{units::meter_t{m->x()}, units::meter_t{m->y()}, - units::meter_t{m->z()}}; -} - -void wpi::Protobuf::Pack(google::protobuf::Message* msg, - const frc::Translation3d& value) { - auto m = static_cast(msg); - m->set_x(value.X().value()); - m->set_y(value.Y().value()); - m->set_z(value.Z().value()); -} diff --git a/wpimath/src/main/native/cpp/geometry/proto/Pose2dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Pose2dProto.cpp new file mode 100644 index 0000000000..2b8bf5ff66 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/proto/Pose2dProto.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 "frc/geometry/proto/Pose2dProto.h" + +#include "geometry2d.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage( + arena); +} + +frc::Pose2d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Pose2d{ + wpi::UnpackProtobuf(m->translation()), + wpi::UnpackProtobuf(m->rotation()), + }; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Pose2d& value) { + auto m = static_cast(msg); + wpi::PackProtobuf(m->mutable_translation(), value.Translation()); + wpi::PackProtobuf(m->mutable_rotation(), value.Rotation()); +} diff --git a/wpimath/src/main/native/cpp/geometry/proto/Pose3dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Pose3dProto.cpp new file mode 100644 index 0000000000..581cafb0f4 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/proto/Pose3dProto.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 "frc/geometry/proto/Pose3dProto.h" + +#include "geometry3d.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage( + arena); +} + +frc::Pose3d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Pose3d{ + wpi::UnpackProtobuf(m->translation()), + wpi::UnpackProtobuf(m->rotation()), + }; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Pose3d& value) { + auto m = static_cast(msg); + wpi::PackProtobuf(m->mutable_translation(), value.Translation()); + wpi::PackProtobuf(m->mutable_rotation(), value.Rotation()); +} diff --git a/wpimath/src/main/native/cpp/geometry/proto/QuaternionProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/QuaternionProto.cpp new file mode 100644 index 0000000000..aadefa0391 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/proto/QuaternionProto.cpp @@ -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. + +#include "frc/geometry/proto/QuaternionProto.h" + +#include "geometry3d.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage( + arena); +} + +frc::Quaternion wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Quaternion{ + m->w(), + m->x(), + m->y(), + m->z(), + }; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Quaternion& value) { + auto m = static_cast(msg); + m->set_w(value.W()); + m->set_x(value.X()); + m->set_y(value.Y()); + m->set_z(value.Z()); +} diff --git a/wpimath/src/main/native/cpp/geometry/proto/Rotation2dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Rotation2dProto.cpp new file mode 100644 index 0000000000..c9005f2bf8 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/proto/Rotation2dProto.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/proto/Rotation2dProto.h" + +#include "geometry2d.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage( + arena); +} + +frc::Rotation2d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Rotation2d{ + units::radian_t{m->value()}, + }; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Rotation2d& value) { + auto m = static_cast(msg); + m->set_value(value.Radians().value()); +} diff --git a/wpimath/src/main/native/cpp/geometry/proto/Rotation3dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Rotation3dProto.cpp new file mode 100644 index 0000000000..72645e1952 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/proto/Rotation3dProto.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/proto/Rotation3dProto.h" + +#include "geometry3d.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage( + arena); +} + +frc::Rotation3d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Rotation3d{ + wpi::UnpackProtobuf(m->q()), + }; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Rotation3d& value) { + auto m = static_cast(msg); + wpi::PackProtobuf(m->mutable_q(), value.GetQuaternion()); +} diff --git a/wpimath/src/main/native/cpp/geometry/proto/Transform2dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Transform2dProto.cpp new file mode 100644 index 0000000000..4d38b81278 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/proto/Transform2dProto.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 "frc/geometry/proto/Transform2dProto.h" + +#include "geometry2d.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufTransform2d>(arena); +} + +frc::Transform2d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Transform2d{ + wpi::UnpackProtobuf(m->translation()), + wpi::UnpackProtobuf(m->rotation()), + }; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Transform2d& value) { + auto m = static_cast(msg); + wpi::PackProtobuf(m->mutable_translation(), value.Translation()); + wpi::PackProtobuf(m->mutable_rotation(), value.Rotation()); +} diff --git a/wpimath/src/main/native/cpp/geometry/proto/Transform3dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Transform3dProto.cpp new file mode 100644 index 0000000000..4a8fbfd326 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/proto/Transform3dProto.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 "frc/geometry/proto/Transform3dProto.h" + +#include "geometry3d.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufTransform3d>(arena); +} + +frc::Transform3d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Transform3d{ + wpi::UnpackProtobuf(m->translation()), + wpi::UnpackProtobuf(m->rotation()), + }; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Transform3d& value) { + auto m = static_cast(msg); + wpi::PackProtobuf(m->mutable_translation(), value.Translation()); + wpi::PackProtobuf(m->mutable_rotation(), value.Rotation()); +} diff --git a/wpimath/src/main/native/cpp/geometry/proto/Translation2dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Translation2dProto.cpp new file mode 100644 index 0000000000..739dc99f34 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/proto/Translation2dProto.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 "frc/geometry/proto/Translation2dProto.h" + +#include "geometry2d.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufTranslation2d>(arena); +} + +frc::Translation2d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Translation2d{ + units::meter_t{m->x()}, + units::meter_t{m->y()}, + }; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Translation2d& value) { + auto m = static_cast(msg); + m->set_x(value.X().value()); + m->set_y(value.Y().value()); +} diff --git a/wpimath/src/main/native/cpp/geometry/proto/Translation3dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Translation3dProto.cpp new file mode 100644 index 0000000000..6285b2b852 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/proto/Translation3dProto.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/Translation3dProto.h" + +#include "geometry3d.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufTranslation3d>(arena); +} + +frc::Translation3d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Translation3d{ + units::meter_t{m->x()}, + units::meter_t{m->y()}, + units::meter_t{m->z()}, + }; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Translation3d& value) { + auto m = static_cast(msg); + m->set_x(value.X().value()); + m->set_y(value.Y().value()); + m->set_z(value.Z().value()); +} diff --git a/wpimath/src/main/native/cpp/geometry/Twist2d.cpp b/wpimath/src/main/native/cpp/geometry/proto/Twist2dProto.cpp similarity index 83% rename from wpimath/src/main/native/cpp/geometry/Twist2d.cpp rename to wpimath/src/main/native/cpp/geometry/proto/Twist2dProto.cpp index 6c106eb436..2590fc96a3 100644 --- a/wpimath/src/main/native/cpp/geometry/Twist2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/proto/Twist2dProto.cpp @@ -2,12 +2,10 @@ // 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/Twist2d.h" +#include "frc/geometry/proto/Twist2dProto.h" #include "geometry2d.pb.h" -using namespace frc; - google::protobuf::Message* wpi::Protobuf::New( google::protobuf::Arena* arena) { return google::protobuf::Arena::CreateMessage( @@ -17,8 +15,11 @@ google::protobuf::Message* wpi::Protobuf::New( frc::Twist2d wpi::Protobuf::Unpack( const google::protobuf::Message& msg) { auto m = static_cast(&msg); - return frc::Twist2d{units::meter_t{m->dx()}, units::meter_t{m->dy()}, - units::radian_t{m->dtheta()}}; + return frc::Twist2d{ + units::meter_t{m->dx()}, + units::meter_t{m->dy()}, + units::radian_t{m->dtheta()}, + }; } void wpi::Protobuf::Pack(google::protobuf::Message* msg, diff --git a/wpimath/src/main/native/cpp/geometry/Twist3d.cpp b/wpimath/src/main/native/cpp/geometry/proto/Twist3dProto.cpp similarity index 77% rename from wpimath/src/main/native/cpp/geometry/Twist3d.cpp rename to wpimath/src/main/native/cpp/geometry/proto/Twist3dProto.cpp index 4f4ce86b4b..e2d91e4e16 100644 --- a/wpimath/src/main/native/cpp/geometry/Twist3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/proto/Twist3dProto.cpp @@ -2,12 +2,10 @@ // 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/Twist3d.h" +#include "frc/geometry/proto/Twist3dProto.h" #include "geometry3d.pb.h" -using namespace frc; - google::protobuf::Message* wpi::Protobuf::New( google::protobuf::Arena* arena) { return google::protobuf::Arena::CreateMessage( @@ -17,9 +15,11 @@ google::protobuf::Message* wpi::Protobuf::New( frc::Twist3d wpi::Protobuf::Unpack( const google::protobuf::Message& msg) { auto m = static_cast(&msg); - return frc::Twist3d{units::meter_t{m->dx()}, units::meter_t{m->dy()}, - units::meter_t{m->dz()}, units::radian_t{m->rx()}, - units::radian_t{m->ry()}, units::radian_t{m->rz()}}; + return frc::Twist3d{ + units::meter_t{m->dx()}, units::meter_t{m->dy()}, + units::meter_t{m->dz()}, units::radian_t{m->rx()}, + units::radian_t{m->ry()}, units::radian_t{m->rz()}, + }; } void wpi::Protobuf::Pack(google::protobuf::Message* msg, diff --git a/wpimath/src/main/native/cpp/geometry/struct/Pose2dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Pose2dStruct.cpp new file mode 100644 index 0000000000..68e63476ac --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/struct/Pose2dStruct.cpp @@ -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. + +#include "frc/geometry/struct/Pose2dStruct.h" + +namespace { +constexpr size_t kTranslationOff = 0; +constexpr size_t kRotationOff = + kTranslationOff + wpi::Struct::kSize; +} // namespace + +using StructType = wpi::Struct; + +frc::Pose2d StructType::Unpack(std::span data) { + return frc::Pose2d{ + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + }; +} + +void StructType::Pack(std::span data, + const frc::Pose2d& value) { + wpi::PackStruct(data, value.Translation()); + wpi::PackStruct(data, value.Rotation()); +} + +void StructType::ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + wpi::ForEachStructSchema(fn); +} diff --git a/wpimath/src/main/native/cpp/geometry/struct/Pose3dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Pose3dStruct.cpp new file mode 100644 index 0000000000..37995781e1 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/struct/Pose3dStruct.cpp @@ -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. + +#include "frc/geometry/struct/Pose3dStruct.h" + +namespace { +constexpr size_t kTranslationOff = 0; +constexpr size_t kRotationOff = + kTranslationOff + wpi::Struct::kSize; +} // namespace + +using StructType = wpi::Struct; + +frc::Pose3d StructType::Unpack(std::span data) { + return frc::Pose3d{ + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + }; +} + +void StructType::Pack(std::span data, + const frc::Pose3d& value) { + wpi::PackStruct(data, value.Translation()); + wpi::PackStruct(data, value.Rotation()); +} + +void StructType::ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + wpi::ForEachStructSchema(fn); +} diff --git a/wpimath/src/main/native/cpp/geometry/struct/QuaternionStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/QuaternionStruct.cpp new file mode 100644 index 0000000000..140b6faca6 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/struct/QuaternionStruct.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/struct/QuaternionStruct.h" + +namespace { +constexpr size_t kWOff = 0; +constexpr size_t kXOff = kWOff + 8; +constexpr size_t kYOff = kXOff + 8; +constexpr size_t kZOff = kYOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::Quaternion StructType::Unpack(std::span data) { + return frc::Quaternion{ + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + }; +} + +void StructType::Pack(std::span data, + const frc::Quaternion& value) { + wpi::PackStruct(data, value.W()); + wpi::PackStruct(data, value.X()); + wpi::PackStruct(data, value.Y()); + wpi::PackStruct(data, value.Z()); +} diff --git a/wpimath/src/main/native/cpp/geometry/struct/Rotation2dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Rotation2dStruct.cpp new file mode 100644 index 0000000000..8027858cb2 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/struct/Rotation2dStruct.cpp @@ -0,0 +1,22 @@ +// 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/Rotation2dStruct.h" + +namespace { +constexpr size_t kValueOff = 0; +} // namespace + +using StructType = wpi::Struct; + +frc::Rotation2d StructType::Unpack(std::span data) { + return frc::Rotation2d{ + units::radian_t{wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::Rotation2d& value) { + wpi::PackStruct(data, value.Radians().value()); +} diff --git a/wpimath/src/main/native/cpp/geometry/struct/Rotation3dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Rotation3dStruct.cpp new file mode 100644 index 0000000000..e2a122d561 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/struct/Rotation3dStruct.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/Rotation3dStruct.h" + +namespace { +constexpr size_t kQOff = 0; +} // namespace + +using StructType = wpi::Struct; + +frc::Rotation3d StructType::Unpack(std::span data) { + return frc::Rotation3d{ + wpi::UnpackStruct(data), + }; +} + +void StructType::Pack(std::span data, + const frc::Rotation3d& value) { + wpi::PackStruct(data, value.GetQuaternion()); +} + +void StructType::ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); +} diff --git a/wpimath/src/main/native/cpp/geometry/struct/Transform2dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Transform2dStruct.cpp new file mode 100644 index 0000000000..25ef5119a0 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/struct/Transform2dStruct.cpp @@ -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. + +#include "frc/geometry/struct/Transform2dStruct.h" + +namespace { +constexpr size_t kTranslationOff = 0; +constexpr size_t kRotationOff = + kTranslationOff + wpi::Struct::kSize; +} // namespace + +using StructType = wpi::Struct; + +frc::Transform2d StructType::Unpack(std::span data) { + return frc::Transform2d{ + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + }; +} + +void StructType::Pack(std::span data, + const frc::Transform2d& value) { + wpi::PackStruct(data, value.Translation()); + wpi::PackStruct(data, value.Rotation()); +} + +void StructType::ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + wpi::ForEachStructSchema(fn); +} diff --git a/wpimath/src/main/native/cpp/geometry/struct/Transform3dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Transform3dStruct.cpp new file mode 100644 index 0000000000..5f169c5b2a --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/struct/Transform3dStruct.cpp @@ -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. + +#include "frc/geometry/struct/Transform3dStruct.h" + +namespace { +constexpr size_t kTranslationOff = 0; +constexpr size_t kRotationOff = + kTranslationOff + wpi::Struct::kSize; +} // namespace + +using StructType = wpi::Struct; + +frc::Transform3d StructType::Unpack(std::span data) { + return frc::Transform3d{ + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + }; +} + +void StructType::Pack(std::span data, + const frc::Transform3d& value) { + wpi::PackStruct(data, value.Translation()); + wpi::PackStruct(data, value.Rotation()); +} + +void StructType::ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + wpi::ForEachStructSchema(fn); +} diff --git a/wpimath/src/main/native/cpp/geometry/struct/Translation2dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Translation2dStruct.cpp new file mode 100644 index 0000000000..e9deb8ea29 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/struct/Translation2dStruct.cpp @@ -0,0 +1,25 @@ +// 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/Translation2dStruct.h" + +namespace { +constexpr size_t kXOff = 0; +constexpr size_t kYOff = kXOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::Translation2d StructType::Unpack(std::span data) { + return frc::Translation2d{ + units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::Translation2d& value) { + wpi::PackStruct(data, value.X().value()); + wpi::PackStruct(data, value.Y().value()); +} diff --git a/wpimath/src/main/native/cpp/geometry/struct/Translation3dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Translation3dStruct.cpp new file mode 100644 index 0000000000..d747cb3c22 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/struct/Translation3dStruct.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 "frc/geometry/struct/Translation3dStruct.h" + +namespace { +constexpr size_t kXOff = 0; +constexpr size_t kYOff = kXOff + 8; +constexpr size_t kZOff = kYOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::Translation3d StructType::Unpack(std::span data) { + return frc::Translation3d{ + units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::Translation3d& value) { + wpi::PackStruct(data, value.X().value()); + wpi::PackStruct(data, value.Y().value()); + wpi::PackStruct(data, value.Z().value()); +} diff --git a/wpimath/src/main/native/cpp/geometry/struct/Twist2dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Twist2dStruct.cpp new file mode 100644 index 0000000000..9ad58199e4 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/struct/Twist2dStruct.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 "frc/geometry/struct/Twist2dStruct.h" + +namespace { +constexpr size_t kDxOff = 0; +constexpr size_t kDyOff = kDxOff + 8; +constexpr size_t kDthetaOff = kDyOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::Twist2d StructType::Unpack(std::span data) { + return frc::Twist2d{ + units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + units::radian_t{wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::Twist2d& value) { + wpi::PackStruct(data, value.dx.value()); + wpi::PackStruct(data, value.dy.value()); + wpi::PackStruct(data, value.dtheta.value()); +} diff --git a/wpimath/src/main/native/cpp/geometry/struct/Twist3dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Twist3dStruct.cpp new file mode 100644 index 0000000000..f434a8f1a9 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/struct/Twist3dStruct.cpp @@ -0,0 +1,37 @@ +// 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/Twist3dStruct.h" + +namespace { +constexpr size_t kDxOff = 0; +constexpr size_t kDyOff = kDxOff + 8; +constexpr size_t kDzOff = kDyOff + 8; +constexpr size_t kRxOff = kDzOff + 8; +constexpr size_t kRyOff = kRxOff + 8; +constexpr size_t kRzOff = kRyOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::Twist3d StructType::Unpack(std::span data) { + return frc::Twist3d{ + units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + units::radian_t{wpi::UnpackStruct(data)}, + units::radian_t{wpi::UnpackStruct(data)}, + units::radian_t{wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::Twist3d& value) { + wpi::PackStruct(data, value.dx.value()); + wpi::PackStruct(data, value.dy.value()); + wpi::PackStruct(data, value.dz.value()); + wpi::PackStruct(data, value.rx.value()); + wpi::PackStruct(data, value.ry.value()); + wpi::PackStruct(data, value.rz.value()); +} diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h index 970f792961..65a146c3dd 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.h @@ -9,8 +9,6 @@ #include #include -#include -#include #include "frc/geometry/Rotation2d.h" #include "frc/geometry/Transform2d.h" @@ -215,38 +213,6 @@ void from_json(const wpi::json& json, Pose2d& pose); } // namespace frc -template <> -struct wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Pose2d"; - static constexpr size_t kSize = wpi::Struct::kSize + - wpi::Struct::kSize; - static constexpr std::string_view kSchema = - "Translation2d translation;Rotation2d rotation"; - static frc::Pose2d Unpack(std::span data) { - return {wpi::UnpackStruct(data), - wpi::UnpackStruct(data)}; - } - static void Pack(std::span data, const frc::Pose2d& value) { - wpi::PackStruct<0>(data, value.Translation()); - wpi::PackStruct(data, value.Rotation()); - } - static void ForEachNested( - std::invocable auto fn) { - wpi::ForEachStructSchema(fn); - wpi::ForEachStructSchema(fn); - } - - private: - static constexpr size_t kRotationOff = wpi::Struct::kSize; -}; - -static_assert(wpi::HasNestedStruct); - -template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { - static google::protobuf::Message* New(google::protobuf::Arena* arena); - static frc::Pose2d Unpack(const google::protobuf::Message& msg); - static void Pack(google::protobuf::Message* msg, const frc::Pose2d& value); -}; - +#include "frc/geometry/proto/Pose2dProto.h" +#include "frc/geometry/struct/Pose2dStruct.h" #include "frc/geometry/Pose2d.inc" diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h index b5a0e031ba..526dced399 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose3d.h @@ -6,8 +6,6 @@ #include #include -#include -#include #include "frc/geometry/Pose2d.h" #include "frc/geometry/Rotation3d.h" @@ -217,36 +215,5 @@ void from_json(const wpi::json& json, Pose3d& pose); } // namespace frc -template <> -struct wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Pose3d"; - static constexpr size_t kSize = wpi::Struct::kSize + - wpi::Struct::kSize; - static constexpr std::string_view kSchema = - "Translation3d translation;Rotation3d rotation"; - static frc::Pose3d Unpack(std::span data) { - return {wpi::UnpackStruct(data), - wpi::UnpackStruct(data)}; - } - static void Pack(std::span data, const frc::Pose3d& value) { - wpi::PackStruct<0>(data, value.Translation()); - wpi::PackStruct(data, value.Rotation()); - } - static void ForEachNested( - std::invocable auto fn) { - wpi::ForEachStructSchema(fn); - wpi::ForEachStructSchema(fn); - } - - private: - static constexpr size_t kRotationOff = wpi::Struct::kSize; -}; - -static_assert(wpi::HasNestedStruct); - -template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { - static google::protobuf::Message* New(google::protobuf::Arena* arena); - static frc::Pose3d Unpack(const google::protobuf::Message& msg); - static void Pack(google::protobuf::Message* msg, const frc::Pose3d& value); -}; +#include "frc/geometry/proto/Pose3dProto.h" +#include "frc/geometry/struct/Pose3dStruct.h" diff --git a/wpimath/src/main/native/include/frc/geometry/Quaternion.h b/wpimath/src/main/native/include/frc/geometry/Quaternion.h index 63a3af2784..a9526bc379 100644 --- a/wpimath/src/main/native/include/frc/geometry/Quaternion.h +++ b/wpimath/src/main/native/include/frc/geometry/Quaternion.h @@ -7,8 +7,6 @@ #include #include #include -#include -#include namespace frc { @@ -190,31 +188,5 @@ void from_json(const wpi::json& json, Quaternion& quaternion); } // namespace frc -template <> -struct wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Quaternion"; - static constexpr size_t kSize = 32; - static constexpr std::string_view kSchema = - "double w;double x;double y;double z"; - static frc::Quaternion Unpack(std::span data) { - return {wpi::UnpackStruct(data), - wpi::UnpackStruct(data), - wpi::UnpackStruct(data), - wpi::UnpackStruct(data)}; - } - static void Pack(std::span data, const frc::Quaternion& value) { - wpi::PackStruct<0>(data, value.W()); - wpi::PackStruct<8>(data, value.X()); - wpi::PackStruct<16>(data, value.Y()); - wpi::PackStruct<24>(data, value.Z()); - } -}; - -template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { - static constexpr std::string_view kTypeString = "proto:Quaternion"; - static google::protobuf::Message* New(google::protobuf::Arena* arena); - static frc::Quaternion Unpack(const google::protobuf::Message& msg); - static void Pack(google::protobuf::Message* msg, - const frc::Quaternion& value); -}; +#include "frc/geometry/proto/QuaternionProto.h" +#include "frc/geometry/struct/QuaternionStruct.h" diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h index 96041a4979..8f89556dd8 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h @@ -6,8 +6,6 @@ #include #include -#include -#include #include "units/angle.h" @@ -178,25 +176,6 @@ void from_json(const wpi::json& json, Rotation2d& rotation); } // namespace frc -template <> -struct wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Rotation2d"; - static constexpr size_t kSize = 8; - static constexpr std::string_view kSchema = "double value"; - static frc::Rotation2d Unpack(std::span data) { - return units::radian_t{wpi::UnpackStruct(data)}; - } - static void Pack(std::span data, const frc::Rotation2d& value) { - wpi::PackStruct(data, value.Radians().value()); - } -}; - -template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { - static google::protobuf::Message* New(google::protobuf::Arena* arena); - static frc::Rotation2d Unpack(const google::protobuf::Message& msg); - static void Pack(google::protobuf::Message* msg, - const frc::Rotation2d& value); -}; - +#include "frc/geometry/proto/Rotation2dProto.h" +#include "frc/geometry/struct/Rotation2dStruct.h" #include "frc/geometry/Rotation2d.inc" diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h index 6d04715316..98abe007a8 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h @@ -7,8 +7,6 @@ #include #include #include -#include -#include #include "frc/geometry/Quaternion.h" #include "frc/geometry/Rotation2d.h" @@ -197,30 +195,5 @@ void from_json(const wpi::json& json, Rotation3d& rotation); } // namespace frc -template <> -struct wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Rotation3d"; - static constexpr size_t kSize = wpi::Struct::kSize; - static constexpr std::string_view kSchema = "Quaternion q"; - static frc::Rotation3d Unpack(std::span data) { - return frc::Rotation3d{wpi::UnpackStruct(data)}; - } - static void Pack(std::span data, - const frc::Rotation3d& value) { - wpi::PackStruct<0>(data, value.GetQuaternion()); - } - static void ForEachNested( - std::invocable auto fn) { - wpi::ForEachStructSchema(fn); - } -}; - -static_assert(wpi::HasNestedStruct); - -template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { - static google::protobuf::Message* New(google::protobuf::Arena* arena); - static frc::Rotation3d Unpack(const google::protobuf::Message& msg); - static void Pack(google::protobuf::Message* msg, - const frc::Rotation3d& value); -}; +#include "frc/geometry/proto/Rotation3dProto.h" +#include "frc/geometry/struct/Rotation3dStruct.h" diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.h b/wpimath/src/main/native/include/frc/geometry/Transform2d.h index 593dce03ab..dccc0e1eee 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.h @@ -5,8 +5,6 @@ #pragma once #include -#include -#include #include "frc/geometry/Translation2d.h" @@ -126,40 +124,6 @@ class WPILIB_DLLEXPORT Transform2d { }; } // namespace frc -template <> -struct wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Transform2d"; - static constexpr size_t kSize = wpi::Struct::kSize + - wpi::Struct::kSize; - static constexpr std::string_view kSchema = - "Translation2d translation;Rotation2d rotation"; - static frc::Transform2d Unpack(std::span data) { - return {wpi::UnpackStruct(data), - wpi::UnpackStruct(data)}; - } - static void Pack(std::span data, - const frc::Transform2d& value) { - wpi::PackStruct<0>(data, value.Translation()); - wpi::PackStruct(data, value.Rotation()); - } - static void ForEachNested( - std::invocable auto fn) { - wpi::ForEachStructSchema(fn); - wpi::ForEachStructSchema(fn); - } - - private: - static constexpr size_t kRotationOff = wpi::Struct::kSize; -}; - -static_assert(wpi::HasNestedStruct); - -template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { - static google::protobuf::Message* New(google::protobuf::Arena* arena); - static frc::Transform2d Unpack(const google::protobuf::Message& msg); - static void Pack(google::protobuf::Message* msg, - const frc::Transform2d& value); -}; - +#include "frc/geometry/proto/Transform2dProto.h" +#include "frc/geometry/struct/Transform2dStruct.h" #include "frc/geometry/Transform2d.inc" diff --git a/wpimath/src/main/native/include/frc/geometry/Transform3d.h b/wpimath/src/main/native/include/frc/geometry/Transform3d.h index d5af97d473..94ecfe37ed 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform3d.h @@ -5,8 +5,6 @@ #pragma once #include -#include -#include #include "frc/geometry/Translation3d.h" @@ -132,38 +130,5 @@ class WPILIB_DLLEXPORT Transform3d { }; } // namespace frc -template <> -struct wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Transform3d"; - static constexpr size_t kSize = wpi::Struct::kSize + - wpi::Struct::kSize; - static constexpr std::string_view kSchema = - "Translation3d translation;Rotation3d rotation"; - static frc::Transform3d Unpack(std::span data) { - return {wpi::UnpackStruct(data), - wpi::UnpackStruct(data)}; - } - static void Pack(std::span data, - const frc::Transform3d& value) { - wpi::PackStruct<0>(data, value.Translation()); - wpi::PackStruct(data, value.Rotation()); - } - static void ForEachNested( - std::invocable auto fn) { - wpi::ForEachStructSchema(fn); - wpi::ForEachStructSchema(fn); - } - - private: - static constexpr size_t kRotationOff = wpi::Struct::kSize; -}; - -static_assert(wpi::HasNestedStruct); - -template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { - static google::protobuf::Message* New(google::protobuf::Arena* arena); - static frc::Transform3d Unpack(const google::protobuf::Message& msg); - static void Pack(google::protobuf::Message* msg, - const frc::Transform3d& value); -}; +#include "frc/geometry/proto/Transform3dProto.h" +#include "frc/geometry/struct/Transform3dStruct.h" diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h index 1568586163..a99492dceb 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h @@ -9,8 +9,6 @@ #include #include -#include -#include #include "frc/geometry/Rotation2d.h" #include "units/length.h" @@ -200,28 +198,6 @@ void from_json(const wpi::json& json, Translation2d& state); } // namespace frc -template <> -struct wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Translation2d"; - static constexpr size_t kSize = 16; - static constexpr std::string_view kSchema = "double x;double y"; - static frc::Translation2d Unpack(std::span data) { - return {units::meter_t{wpi::UnpackStruct(data)}, - units::meter_t{wpi::UnpackStruct(data)}}; - } - static void Pack(std::span data, - const frc::Translation2d& value) { - wpi::PackStruct<0>(data, value.X().value()); - wpi::PackStruct<8>(data, value.Y().value()); - } -}; - -template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { - static google::protobuf::Message* New(google::protobuf::Arena* arena); - static frc::Translation2d Unpack(const google::protobuf::Message& msg); - static void Pack(google::protobuf::Message* msg, - const frc::Translation2d& value); -}; - +#include "frc/geometry/proto/Translation2dProto.h" +#include "frc/geometry/struct/Translation2dStruct.h" #include "frc/geometry/Translation2d.inc" diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h index 47ba1f6bad..b0b3359d96 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h @@ -6,8 +6,6 @@ #include #include -#include -#include #include "frc/geometry/Rotation3d.h" #include "frc/geometry/Translation2d.h" @@ -185,30 +183,6 @@ void from_json(const wpi::json& json, Translation3d& state); } // namespace frc -template <> -struct wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Translation3d"; - static constexpr size_t kSize = 24; - static constexpr std::string_view kSchema = "double x;double y;double z"; - static frc::Translation3d Unpack(std::span data) { - return {units::meter_t{wpi::UnpackStruct(data)}, - units::meter_t{wpi::UnpackStruct(data)}, - units::meter_t{wpi::UnpackStruct(data)}}; - } - static void Pack(std::span data, - const frc::Translation3d& value) { - wpi::PackStruct<0>(data, value.X().value()); - wpi::PackStruct<8>(data, value.Y().value()); - wpi::PackStruct<16>(data, value.Z().value()); - } -}; - -template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { - static google::protobuf::Message* New(google::protobuf::Arena* arena); - static frc::Translation3d Unpack(const google::protobuf::Message& msg); - static void Pack(google::protobuf::Message* msg, - const frc::Translation3d& value); -}; - +#include "frc/geometry/proto/Translation3dProto.h" +#include "frc/geometry/struct/Translation3dStruct.h" #include "frc/geometry/Translation3d.inc" diff --git a/wpimath/src/main/native/include/frc/geometry/Twist2d.h b/wpimath/src/main/native/include/frc/geometry/Twist2d.h index 257f1b65e7..d13cc4ec80 100644 --- a/wpimath/src/main/native/include/frc/geometry/Twist2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Twist2d.h @@ -5,8 +5,6 @@ #pragma once #include -#include -#include #include "units/angle.h" #include "units/length.h" @@ -60,27 +58,5 @@ struct WPILIB_DLLEXPORT Twist2d { }; } // namespace frc -template <> -struct wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Twist2d"; - static constexpr size_t kSize = 24; - static constexpr std::string_view kSchema = - "double dx;double dy;double dtheta"; - static frc::Twist2d Unpack(std::span data) { - return {units::meter_t{wpi::UnpackStruct(data)}, - units::meter_t{wpi::UnpackStruct(data)}, - units::radian_t{wpi::UnpackStruct(data)}}; - } - static void Pack(std::span data, const frc::Twist2d& value) { - wpi::PackStruct<0>(data, value.dx.value()); - wpi::PackStruct<8>(data, value.dy.value()); - wpi::PackStruct<16>(data, value.dtheta.value()); - } -}; - -template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { - static google::protobuf::Message* New(google::protobuf::Arena* arena); - static frc::Twist2d Unpack(const google::protobuf::Message& msg); - static void Pack(google::protobuf::Message* msg, const frc::Twist2d& value); -}; +#include "frc/geometry/proto/Twist2dProto.h" +#include "frc/geometry/struct/Twist2dStruct.h" diff --git a/wpimath/src/main/native/include/frc/geometry/Twist3d.h b/wpimath/src/main/native/include/frc/geometry/Twist3d.h index 4d902dfcbb..3262367dc0 100644 --- a/wpimath/src/main/native/include/frc/geometry/Twist3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Twist3d.h @@ -5,8 +5,6 @@ #pragma once #include -#include -#include #include "frc/geometry/Rotation3d.h" #include "units/angle.h" @@ -80,34 +78,5 @@ struct WPILIB_DLLEXPORT Twist3d { }; } // namespace frc -template <> -struct wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Twist3d"; - static constexpr size_t kSize = 48; - static constexpr std::string_view kSchema = - "double dx;double dy;double dz;double rx;double ry;double rz"; - static frc::Twist3d Unpack(std::span data) { - return {units::meter_t{wpi::UnpackStruct(data)}, - units::meter_t{wpi::UnpackStruct(data)}, - units::meter_t{wpi::UnpackStruct(data)}, - units::radian_t{wpi::UnpackStruct(data)}, - units::radian_t{wpi::UnpackStruct(data)}, - units::radian_t{wpi::UnpackStruct(data)}}; - } - static void Pack(std::span data, const frc::Twist3d& value) { - wpi::PackStruct<0>(data, value.dx.value()); - wpi::PackStruct<8>(data, value.dy.value()); - wpi::PackStruct<16>(data, value.dz.value()); - wpi::PackStruct<24>(data, value.rx.value()); - wpi::PackStruct<32>(data, value.ry.value()); - wpi::PackStruct<40>(data, value.rz.value()); - } -}; - -template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { - static constexpr std::string_view kTypeString = "proto:Twist3d"; - static google::protobuf::Message* New(google::protobuf::Arena* arena); - static frc::Twist3d Unpack(const google::protobuf::Message& msg); - static void Pack(google::protobuf::Message* msg, const frc::Twist3d& value); -}; +#include "frc/geometry/proto/Twist3dProto.h" +#include "frc/geometry/struct/Twist3dStruct.h" diff --git a/wpimath/src/main/native/include/frc/geometry/proto/Pose2dProto.h b/wpimath/src/main/native/include/frc/geometry/proto/Pose2dProto.h new file mode 100644 index 0000000000..9934b4ae83 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/proto/Pose2dProto.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/Pose2d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Pose2d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, const frc::Pose2d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/proto/Pose3dProto.h b/wpimath/src/main/native/include/frc/geometry/proto/Pose3dProto.h new file mode 100644 index 0000000000..9941568d25 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/proto/Pose3dProto.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/Pose3d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Pose3d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, const frc::Pose3d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/proto/QuaternionProto.h b/wpimath/src/main/native/include/frc/geometry/proto/QuaternionProto.h new file mode 100644 index 0000000000..77f371d67e --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/proto/QuaternionProto.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/Quaternion.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Quaternion Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Quaternion& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/proto/Rotation2dProto.h b/wpimath/src/main/native/include/frc/geometry/proto/Rotation2dProto.h new file mode 100644 index 0000000000..33c348e6fc --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/proto/Rotation2dProto.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/Rotation2d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Rotation2d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Rotation2d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/proto/Rotation3dProto.h b/wpimath/src/main/native/include/frc/geometry/proto/Rotation3dProto.h new file mode 100644 index 0000000000..f6f5662357 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/proto/Rotation3dProto.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/Rotation3d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Rotation3d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Rotation3d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/proto/Transform2dProto.h b/wpimath/src/main/native/include/frc/geometry/proto/Transform2dProto.h new file mode 100644 index 0000000000..5ff19bd226 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/proto/Transform2dProto.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/Transform2d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Transform2d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Transform2d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/proto/Transform3dProto.h b/wpimath/src/main/native/include/frc/geometry/proto/Transform3dProto.h new file mode 100644 index 0000000000..d27fb9a0b8 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/proto/Transform3dProto.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/Transform3d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Transform3d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Transform3d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/proto/Translation2dProto.h b/wpimath/src/main/native/include/frc/geometry/proto/Translation2dProto.h new file mode 100644 index 0000000000..6516fc8d88 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/proto/Translation2dProto.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/Translation2d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Translation2d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Translation2d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/proto/Translation3dProto.h b/wpimath/src/main/native/include/frc/geometry/proto/Translation3dProto.h new file mode 100644 index 0000000000..2d317fc5af --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/proto/Translation3dProto.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/Translation3d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Translation3d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Translation3d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/proto/Twist2dProto.h b/wpimath/src/main/native/include/frc/geometry/proto/Twist2dProto.h new file mode 100644 index 0000000000..328798e14c --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/proto/Twist2dProto.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/Twist2d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Twist2d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, const frc::Twist2d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/proto/Twist3dProto.h b/wpimath/src/main/native/include/frc/geometry/proto/Twist3dProto.h new file mode 100644 index 0000000000..24f496935e --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/proto/Twist3dProto.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/Twist3d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Twist3d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, const frc::Twist3d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Pose2dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Pose2dStruct.h new file mode 100644 index 0000000000..ee517fb491 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/Pose2dStruct.h @@ -0,0 +1,26 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include "frc/geometry/Pose2d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Pose2d"; + static constexpr size_t kSize = wpi::Struct::kSize + + wpi::Struct::kSize; + static constexpr std::string_view kSchema = + "Translation2d translation;Rotation2d rotation"; + + static frc::Pose2d Unpack(std::span data); + static void Pack(std::span data, const frc::Pose2d& value); + static void ForEachNested( + std::invocable auto fn); +}; + +static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Pose3dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Pose3dStruct.h new file mode 100644 index 0000000000..2f899491a7 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/Pose3dStruct.h @@ -0,0 +1,26 @@ +// 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/Pose3d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Pose3d"; + static constexpr size_t kSize = wpi::Struct::kSize + + wpi::Struct::kSize; + static constexpr std::string_view kSchema = + "Translation3d translation;Rotation3d rotation"; + + static frc::Pose3d Unpack(std::span data); + static void Pack(std::span data, const frc::Pose3d& value); + static void ForEachNested( + std::invocable auto fn); +}; + +static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/native/include/frc/geometry/struct/QuaternionStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/QuaternionStruct.h new file mode 100644 index 0000000000..35acd4ba35 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/QuaternionStruct.h @@ -0,0 +1,22 @@ +// 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/Quaternion.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Quaternion"; + static constexpr size_t kSize = 32; + static constexpr std::string_view kSchema = + "double w;double x;double y;double z"; + + static frc::Quaternion Unpack(std::span data); + static void Pack(std::span data, + const frc::Quaternion& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Rotation2dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Rotation2dStruct.h new file mode 100644 index 0000000000..ce4f7a6f02 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/Rotation2dStruct.h @@ -0,0 +1,21 @@ +// 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/Rotation2d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Rotation2d"; + static constexpr size_t kSize = 8; + static constexpr std::string_view kSchema = "double value"; + + static frc::Rotation2d Unpack(std::span data); + static void Pack(std::span data, + const frc::Rotation2d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Rotation3dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Rotation3dStruct.h new file mode 100644 index 0000000000..9d73f72d40 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/Rotation3dStruct.h @@ -0,0 +1,25 @@ +// 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/Rotation3d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Rotation3d"; + static constexpr size_t kSize = wpi::Struct::kSize; + static constexpr std::string_view kSchema = "Quaternion q"; + + static frc::Rotation3d Unpack(std::span data); + static void Pack(std::span data, + const frc::Rotation3d& value); + static void ForEachNested( + std::invocable auto fn); +}; + +static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Transform2dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Transform2dStruct.h new file mode 100644 index 0000000000..55f0ef8604 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/Transform2dStruct.h @@ -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. + +#pragma once + +#include +#include + +#include "frc/geometry/Transform2d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Transform2d"; + static constexpr size_t kSize = wpi::Struct::kSize + + wpi::Struct::kSize; + static constexpr std::string_view kSchema = + "Translation2d translation;Rotation2d rotation"; + + static frc::Transform2d Unpack(std::span data); + static void Pack(std::span data, + const frc::Transform2d& value); + static void ForEachNested( + std::invocable auto fn); +}; + +static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Transform3dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Transform3dStruct.h new file mode 100644 index 0000000000..4fb592016c --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/Transform3dStruct.h @@ -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. + +#pragma once + +#include +#include + +#include "frc/geometry/Transform3d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Transform3d"; + static constexpr size_t kSize = wpi::Struct::kSize + + wpi::Struct::kSize; + static constexpr std::string_view kSchema = + "Translation3d translation;Rotation3d rotation"; + + static frc::Transform3d Unpack(std::span data); + static void Pack(std::span data, + const frc::Transform3d& value); + static void ForEachNested( + std::invocable auto fn); +}; + +static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Translation2dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Translation2dStruct.h new file mode 100644 index 0000000000..1f6ea39772 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/Translation2dStruct.h @@ -0,0 +1,21 @@ +// 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/Translation2d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Translation2d"; + static constexpr size_t kSize = 16; + static constexpr std::string_view kSchema = "double x;double y"; + + static frc::Translation2d Unpack(std::span data); + static void Pack(std::span data, + const frc::Translation2d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Translation3dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Translation3dStruct.h new file mode 100644 index 0000000000..34604c6e1f --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/Translation3dStruct.h @@ -0,0 +1,21 @@ +// 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/Translation3d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Translation3d"; + static constexpr size_t kSize = 24; + static constexpr std::string_view kSchema = "double x;double y;double z"; + + static frc::Translation3d Unpack(std::span data); + static void Pack(std::span data, + const frc::Translation3d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Twist2dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Twist2dStruct.h new file mode 100644 index 0000000000..bda746c6da --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/Twist2dStruct.h @@ -0,0 +1,21 @@ +// 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/Twist2d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Twist2d"; + static constexpr size_t kSize = 24; + static constexpr std::string_view kSchema = + "double dx;double dy;double dtheta"; + + static frc::Twist2d Unpack(std::span data); + static void Pack(std::span data, const frc::Twist2d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Twist3dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Twist3dStruct.h new file mode 100644 index 0000000000..7cbe8a5d20 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/Twist3dStruct.h @@ -0,0 +1,21 @@ +// 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/Twist3d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Twist3d"; + static constexpr size_t kSize = 48; + static constexpr std::string_view kSchema = + "double dx;double dy;double dz;double rx;double ry;double rz"; + + static frc::Twist3d Unpack(std::span data); + static void Pack(std::span data, const frc::Twist3d& value); +}; diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Pose2dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Pose2dProtoTest.java new file mode 100644 index 0000000000..05ba185cf7 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Pose2dProtoTest.java @@ -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. + +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.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.proto.Geometry2D.ProtobufPose2d; +import org.junit.jupiter.api.Test; + +class Pose2dProtoTest { + private static final Pose2d DATA = + new Pose2d(new Translation2d(0.191, 2.2), new Rotation2d(22.9)); + + @Test + void testRoundtrip() { + ProtobufPose2d proto = Pose2d.proto.createMessage(); + Pose2d.proto.pack(proto, DATA); + + Pose2d data = Pose2d.proto.unpack(proto); + assertEquals(DATA.getTranslation(), data.getTranslation()); + assertEquals(DATA.getRotation(), data.getRotation()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Pose3dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Pose3dProtoTest.java new file mode 100644 index 0000000000..1043408c6c --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Pose3dProtoTest.java @@ -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. + +package edu.wpi.first.math.geometry.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.proto.Geometry3D.ProtobufPose3d; +import org.junit.jupiter.api.Test; + +class Pose3dProtoTest { + private static final Pose3d DATA = + new Pose3d( + new Translation3d(1.1, 2.2, 1.1), + new Rotation3d(new Quaternion(1.91, 0.3504, 3.3, 1.74))); + + @Test + void testRoundtrip() { + ProtobufPose3d proto = Pose3d.proto.createMessage(); + Pose3d.proto.pack(proto, DATA); + + Pose3d data = Pose3d.proto.unpack(proto); + assertEquals(DATA.getTranslation(), data.getTranslation()); + assertEquals(DATA.getRotation(), data.getRotation()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/QuaternionProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/QuaternionProtoTest.java new file mode 100644 index 0000000000..9129c19e33 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/QuaternionProtoTest.java @@ -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. + +package edu.wpi.first.math.geometry.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.proto.Geometry3D.ProtobufQuaternion; +import org.junit.jupiter.api.Test; + +class QuaternionProtoTest { + private static final Quaternion DATA = new Quaternion(1.1, 0.191, 35.04, 19.1); + + @Test + void testRoundtrip() { + ProtobufQuaternion proto = Quaternion.proto.createMessage(); + Quaternion.proto.pack(proto, DATA); + + Quaternion data = Quaternion.proto.unpack(proto); + assertEquals(DATA.getW(), data.getW()); + assertEquals(DATA.getX(), data.getX()); + assertEquals(DATA.getY(), data.getY()); + assertEquals(DATA.getZ(), data.getZ()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rotation2dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rotation2dProtoTest.java new file mode 100644 index 0000000000..6021ec0838 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rotation2dProtoTest.java @@ -0,0 +1,24 @@ +// 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.Rotation2d; +import edu.wpi.first.math.proto.Geometry2D.ProtobufRotation2d; +import org.junit.jupiter.api.Test; + +class Rotation2dProtoTest { + private static final Rotation2d DATA = new Rotation2d(1.91); + + @Test + void testRoundtrip() { + ProtobufRotation2d proto = Rotation2d.proto.createMessage(); + Rotation2d.proto.pack(proto, DATA); + + Rotation2d data = Rotation2d.proto.unpack(proto); + assertEquals(DATA.getRadians(), data.getRadians()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rotation3dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rotation3dProtoTest.java new file mode 100644 index 0000000000..5860114bf3 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rotation3dProtoTest.java @@ -0,0 +1,25 @@ +// 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.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.proto.Geometry3D.ProtobufRotation3d; +import org.junit.jupiter.api.Test; + +class Rotation3dProtoTest { + private static final Rotation3d DATA = new Rotation3d(new Quaternion(2.29, 0.191, 0.191, 17.4)); + + @Test + void testRoundtrip() { + ProtobufRotation3d proto = Rotation3d.proto.createMessage(); + Rotation3d.proto.pack(proto, DATA); + + Rotation3d data = Rotation3d.proto.unpack(proto); + assertEquals(DATA.getQuaternion(), data.getQuaternion()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Transform2dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Transform2dProtoTest.java new file mode 100644 index 0000000000..78294a105f --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Transform2dProtoTest.java @@ -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. + +package edu.wpi.first.math.geometry.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.proto.Geometry2D.ProtobufTransform2d; +import org.junit.jupiter.api.Test; + +class Transform2dProtoTest { + private static final Transform2d DATA = + new Transform2d(new Translation2d(0.191, 2.2), new Rotation2d(4.4)); + + @Test + void testRoundtrip() { + ProtobufTransform2d proto = Transform2d.proto.createMessage(); + Transform2d.proto.pack(proto, DATA); + + Transform2d data = Transform2d.proto.unpack(proto); + assertEquals(DATA.getTranslation(), data.getTranslation()); + assertEquals(DATA.getRotation(), data.getRotation()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Transform3dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Transform3dProtoTest.java new file mode 100644 index 0000000000..fd72334520 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Transform3dProtoTest.java @@ -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. + +package edu.wpi.first.math.geometry.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.proto.Geometry3D.ProtobufTransform3d; +import org.junit.jupiter.api.Test; + +class Transform3dProtoTest { + private static final Transform3d DATA = + new Transform3d( + new Translation3d(0.3504, 22.9, 3.504), + new Rotation3d(new Quaternion(0.3504, 35.04, 2.29, 0.3504))); + + @Test + void testRoundtrip() { + ProtobufTransform3d proto = Transform3d.proto.createMessage(); + Transform3d.proto.pack(proto, DATA); + + Transform3d data = Transform3d.proto.unpack(proto); + assertEquals(DATA.getTranslation(), data.getTranslation()); + assertEquals(DATA.getRotation(), data.getRotation()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Translation2dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Translation2dProtoTest.java new file mode 100644 index 0000000000..07b9080194 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Translation2dProtoTest.java @@ -0,0 +1,25 @@ +// 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.Translation2d; +import edu.wpi.first.math.proto.Geometry2D.ProtobufTranslation2d; +import org.junit.jupiter.api.Test; + +class Translation2dProtoTest { + private static final Translation2d DATA = new Translation2d(3.504, 22.9); + + @Test + void testRoundtrip() { + ProtobufTranslation2d proto = Translation2d.proto.createMessage(); + Translation2d.proto.pack(proto, DATA); + + Translation2d data = Translation2d.proto.unpack(proto); + assertEquals(DATA.getX(), data.getX()); + assertEquals(DATA.getY(), data.getY()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Translation3dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Translation3dProtoTest.java new file mode 100644 index 0000000000..d94e177009 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Translation3dProtoTest.java @@ -0,0 +1,26 @@ +// 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.Translation3d; +import edu.wpi.first.math.proto.Geometry3D.ProtobufTranslation3d; +import org.junit.jupiter.api.Test; + +class Translation3dProtoTest { + private static final Translation3d DATA = new Translation3d(35.04, 22.9, 3.504); + + @Test + void testRoundtrip() { + ProtobufTranslation3d proto = Translation3d.proto.createMessage(); + Translation3d.proto.pack(proto, DATA); + + Translation3d data = Translation3d.proto.unpack(proto); + assertEquals(DATA.getX(), data.getX()); + assertEquals(DATA.getY(), data.getY()); + assertEquals(DATA.getZ(), data.getZ()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Twist2dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Twist2dProtoTest.java new file mode 100644 index 0000000000..e7a0631add --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Twist2dProtoTest.java @@ -0,0 +1,26 @@ +// 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.Twist2d; +import edu.wpi.first.math.proto.Geometry2D.ProtobufTwist2d; +import org.junit.jupiter.api.Test; + +class Twist2dProtoTest { + private static final Twist2d DATA = new Twist2d(2.29, 35.04, 35.04); + + @Test + void testRoundtrip() { + ProtobufTwist2d proto = Twist2d.proto.createMessage(); + Twist2d.proto.pack(proto, DATA); + + Twist2d data = Twist2d.proto.unpack(proto); + assertEquals(DATA.dx, data.dx); + assertEquals(DATA.dy, data.dy); + assertEquals(DATA.dtheta, data.dtheta); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Twist3dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Twist3dProtoTest.java new file mode 100644 index 0000000000..70e0f07957 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Twist3dProtoTest.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.Twist3d; +import edu.wpi.first.math.proto.Geometry3D.ProtobufTwist3d; +import org.junit.jupiter.api.Test; + +class Twist3dProtoTest { + private static final Twist3d DATA = new Twist3d(1.1, 2.29, 35.04, 0.174, 19.1, 4.4); + + @Test + void testRoundtrip() { + ProtobufTwist3d proto = Twist3d.proto.createMessage(); + Twist3d.proto.pack(proto, DATA); + + Twist3d data = Twist3d.proto.unpack(proto); + assertEquals(DATA.dx, data.dx); + assertEquals(DATA.dy, data.dy); + assertEquals(DATA.dz, data.dz); + assertEquals(DATA.rx, data.rx); + assertEquals(DATA.ry, data.ry); + assertEquals(DATA.rz, data.rz); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Pose2dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Pose2dStructTest.java new file mode 100644 index 0000000000..0a8c62ab28 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Pose2dStructTest.java @@ -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. + +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.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class Pose2dStructTest { + private static final Pose2d DATA = + new Pose2d(new Translation2d(0.191, 2.2), new Rotation2d(22.9)); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(Pose2d.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + Pose2d.struct.pack(buffer, DATA); + buffer.rewind(); + + Pose2d data = Pose2d.struct.unpack(buffer); + assertEquals(DATA.getTranslation(), data.getTranslation()); + assertEquals(DATA.getRotation(), data.getRotation()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Pose3dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Pose3dStructTest.java new file mode 100644 index 0000000000..e2aac4631c --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Pose3dStructTest.java @@ -0,0 +1,34 @@ +// 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.Pose3d; +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class Pose3dStructTest { + private static final Pose3d DATA = + new Pose3d( + new Translation3d(1.1, 2.2, 1.1), + new Rotation3d(new Quaternion(1.91, 0.3504, 3.3, 1.74))); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(Pose3d.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + Pose3d.struct.pack(buffer, DATA); + buffer.rewind(); + + Pose3d data = Pose3d.struct.unpack(buffer); + assertEquals(DATA.getTranslation(), data.getTranslation()); + assertEquals(DATA.getRotation(), data.getRotation()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/QuaternionStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/QuaternionStructTest.java new file mode 100644 index 0000000000..41d4604673 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/QuaternionStructTest.java @@ -0,0 +1,30 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.geometry.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Quaternion; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class QuaternionStructTest { + private static final Quaternion DATA = new Quaternion(1.1, 0.191, 35.04, 19.1); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(Quaternion.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + Quaternion.struct.pack(buffer, DATA); + buffer.rewind(); + + Quaternion data = Quaternion.struct.unpack(buffer); + assertEquals(DATA.getW(), data.getW()); + assertEquals(DATA.getX(), data.getX()); + assertEquals(DATA.getY(), data.getY()); + assertEquals(DATA.getZ(), data.getZ()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rotation2dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rotation2dStructTest.java new file mode 100644 index 0000000000..62569b57e1 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rotation2dStructTest.java @@ -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. + +package edu.wpi.first.math.geometry.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation2d; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class Rotation2dStructTest { + private static final Rotation2d DATA = new Rotation2d(1.91); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(Rotation2d.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + Rotation2d.struct.pack(buffer, DATA); + buffer.rewind(); + + Rotation2d data = Rotation2d.struct.unpack(buffer); + assertEquals(DATA.getRadians(), data.getRadians()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rotation3dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rotation3dStructTest.java new file mode 100644 index 0000000000..7f275e74e4 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rotation3dStructTest.java @@ -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. + +package edu.wpi.first.math.geometry.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class Rotation3dStructTest { + private static final Rotation3d DATA = new Rotation3d(new Quaternion(2.29, 0.191, 0.191, 17.4)); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(Rotation3d.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + Rotation3d.struct.pack(buffer, DATA); + buffer.rewind(); + + Rotation3d data = Rotation3d.struct.unpack(buffer); + assertEquals(DATA.getQuaternion(), data.getQuaternion()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Transform2dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Transform2dStructTest.java new file mode 100644 index 0000000000..cf583d4ce5 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Transform2dStructTest.java @@ -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. + +package edu.wpi.first.math.geometry.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class Transform2dStructTest { + private static final Transform2d DATA = + new Transform2d(new Translation2d(0.191, 2.2), new Rotation2d(4.4)); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(Transform2d.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + Transform2d.struct.pack(buffer, DATA); + buffer.rewind(); + + Transform2d data = Transform2d.struct.unpack(buffer); + assertEquals(DATA.getTranslation(), data.getTranslation()); + assertEquals(DATA.getRotation(), data.getRotation()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Transform3dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Transform3dStructTest.java new file mode 100644 index 0000000000..10a5100d9c --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Transform3dStructTest.java @@ -0,0 +1,34 @@ +// 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.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class Transform3dStructTest { + private static final Transform3d DATA = + new Transform3d( + new Translation3d(0.3504, 22.9, 3.504), + new Rotation3d(new Quaternion(0.3504, 35.04, 2.29, 0.3504))); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(Transform3d.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + Transform3d.struct.pack(buffer, DATA); + buffer.rewind(); + + Transform3d data = Transform3d.struct.unpack(buffer); + assertEquals(DATA.getTranslation(), data.getTranslation()); + assertEquals(DATA.getRotation(), data.getRotation()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Translation2dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Translation2dStructTest.java new file mode 100644 index 0000000000..366df17657 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Translation2dStructTest.java @@ -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. + +package edu.wpi.first.math.geometry.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Translation2d; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class Translation2dStructTest { + private static final Translation2d DATA = new Translation2d(3.504, 22.9); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(Translation2d.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + Translation2d.struct.pack(buffer, DATA); + buffer.rewind(); + + Translation2d data = Translation2d.struct.unpack(buffer); + assertEquals(DATA.getX(), data.getX()); + assertEquals(DATA.getY(), data.getY()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Translation3dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Translation3dStructTest.java new file mode 100644 index 0000000000..fc48981917 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Translation3dStructTest.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.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Translation3d; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class Translation3dStructTest { + private static final Translation3d DATA = new Translation3d(35.04, 22.9, 3.504); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(Translation3d.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + Translation3d.struct.pack(buffer, DATA); + buffer.rewind(); + + Translation3d data = Translation3d.struct.unpack(buffer); + assertEquals(DATA.getX(), data.getX()); + assertEquals(DATA.getY(), data.getY()); + assertEquals(DATA.getZ(), data.getZ()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Twist2dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Twist2dStructTest.java new file mode 100644 index 0000000000..6d4d174c50 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Twist2dStructTest.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.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Twist2d; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class Twist2dStructTest { + private static final Twist2d DATA = new Twist2d(2.29, 35.04, 35.04); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(Twist2d.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + Twist2d.struct.pack(buffer, DATA); + buffer.rewind(); + + Twist2d data = Twist2d.struct.unpack(buffer); + assertEquals(DATA.dx, data.dx); + assertEquals(DATA.dy, data.dy); + assertEquals(DATA.dtheta, data.dtheta); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Twist3dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Twist3dStructTest.java new file mode 100644 index 0000000000..131eb2f6ac --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Twist3dStructTest.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.Twist3d; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class Twist3dStructTest { + private static final Twist3d DATA = new Twist3d(1.1, 2.29, 35.04, 0.174, 19.1, 4.4); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(Twist3d.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + Twist3d.struct.pack(buffer, DATA); + buffer.rewind(); + + Twist3d data = Twist3d.struct.unpack(buffer); + assertEquals(DATA.dx, data.dx); + assertEquals(DATA.dy, data.dy); + assertEquals(DATA.dz, data.dz); + assertEquals(DATA.rx, data.rx); + assertEquals(DATA.ry, data.ry); + assertEquals(DATA.rz, data.rz); + } +} diff --git a/wpimath/src/test/native/cpp/geometry/proto/Pose2dProtoTest.cpp b/wpimath/src/test/native/cpp/geometry/proto/Pose2dProtoTest.cpp new file mode 100644 index 0000000000..c6e9d49ec5 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/proto/Pose2dProtoTest.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/Pose2d.h" +#include "geometry2d.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Pose2d kExpectedData = + Pose2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{22.9_rad}}; +} // namespace + +TEST(Pose2dProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Pose2d unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation()); + EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation()); +} diff --git a/wpimath/src/test/native/cpp/geometry/proto/Pose3dProtoTest.cpp b/wpimath/src/test/native/cpp/geometry/proto/Pose3dProtoTest.cpp new file mode 100644 index 0000000000..d8847cab88 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/proto/Pose3dProtoTest.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/Pose3d.h" +#include "geometry3d.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Pose3d kExpectedData = + Pose3d{Translation3d{1.1_m, 2.2_m, 1.1_m}, + Rotation3d{Quaternion{1.91, 0.3504, 3.3, 1.74}}}; +} // namespace + +TEST(Pose3dProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Pose3d unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation()); + EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation()); +} diff --git a/wpimath/src/test/native/cpp/geometry/proto/QuaternionProtoTest.cpp b/wpimath/src/test/native/cpp/geometry/proto/QuaternionProtoTest.cpp new file mode 100644 index 0000000000..92338df832 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/proto/QuaternionProtoTest.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/Quaternion.h" +#include "geometry3d.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Quaternion kExpectedData = Quaternion{1.1, 0.191, 35.04, 19.1}; +} // namespace + +TEST(QuaternionProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Quaternion unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.W(), unpacked_data.W()); + EXPECT_EQ(kExpectedData.X(), unpacked_data.X()); + EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y()); + EXPECT_EQ(kExpectedData.Z(), unpacked_data.Z()); +} diff --git a/wpimath/src/test/native/cpp/geometry/proto/Rotation2dProtoTest.cpp b/wpimath/src/test/native/cpp/geometry/proto/Rotation2dProtoTest.cpp new file mode 100644 index 0000000000..3ab9e1bb26 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/proto/Rotation2dProtoTest.cpp @@ -0,0 +1,26 @@ +// 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/Rotation2d.h" +#include "geometry2d.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Rotation2d kExpectedData = Rotation2d{1.91_rad}; +} // namespace + +TEST(Rotation2dProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Rotation2d unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.Radians().value(), unpacked_data.Radians().value()); +} diff --git a/wpimath/src/test/native/cpp/geometry/proto/Rotation3dProtoTest.cpp b/wpimath/src/test/native/cpp/geometry/proto/Rotation3dProtoTest.cpp new file mode 100644 index 0000000000..a83b78c2a0 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/proto/Rotation3dProtoTest.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 + +#include "frc/geometry/Rotation3d.h" +#include "geometry3d.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Rotation3d kExpectedData = + Rotation3d{Quaternion{2.29, 0.191, 0.191, 17.4}}; +} // namespace + +TEST(Rotation3dProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Rotation3d unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.GetQuaternion(), unpacked_data.GetQuaternion()); +} diff --git a/wpimath/src/test/native/cpp/geometry/proto/Transform2dProtoTest.cpp b/wpimath/src/test/native/cpp/geometry/proto/Transform2dProtoTest.cpp new file mode 100644 index 0000000000..4b975f3e14 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/proto/Transform2dProtoTest.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/Transform2d.h" +#include "geometry2d.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Transform2d kExpectedData = + Transform2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{4.4_rad}}; +} // namespace + +TEST(Transform2dProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Transform2d unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation()); + EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation()); +} diff --git a/wpimath/src/test/native/cpp/geometry/proto/Transform3dProtoTest.cpp b/wpimath/src/test/native/cpp/geometry/proto/Transform3dProtoTest.cpp new file mode 100644 index 0000000000..3a86421cfa --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/proto/Transform3dProtoTest.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/Transform3d.h" +#include "geometry3d.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Transform3d kExpectedData = + Transform3d{Translation3d{0.3504_m, 22.9_m, 3.504_m}, + Rotation3d{Quaternion{0.3504, 35.04, 2.29, 0.3504}}}; +} // namespace + +TEST(Transform3dProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Transform3d unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation()); + EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation()); +} diff --git a/wpimath/src/test/native/cpp/geometry/proto/Translation2dProtoTest.cpp b/wpimath/src/test/native/cpp/geometry/proto/Translation2dProtoTest.cpp new file mode 100644 index 0000000000..e6a895969d --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/proto/Translation2dProtoTest.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 + +#include "frc/geometry/Translation2d.h" +#include "geometry2d.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Translation2d kExpectedData = Translation2d{3.504_m, 22.9_m}; +} // namespace + +TEST(Translation2dProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Translation2d unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.X().value(), unpacked_data.X().value()); + EXPECT_EQ(kExpectedData.Y().value(), unpacked_data.Y().value()); +} diff --git a/wpimath/src/test/native/cpp/geometry/proto/Translation3dProtoTest.cpp b/wpimath/src/test/native/cpp/geometry/proto/Translation3dProtoTest.cpp new file mode 100644 index 0000000000..3f6d59f9cf --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/proto/Translation3dProtoTest.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/Translation3d.h" +#include "geometry3d.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Translation3d kExpectedData = Translation3d{35.04_m, 22.9_m, 3.504_m}; +} // namespace + +TEST(Translation3dProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Translation3d unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.X(), unpacked_data.X()); + EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y()); + EXPECT_EQ(kExpectedData.Z(), unpacked_data.Z()); +} diff --git a/wpimath/src/test/native/cpp/geometry/proto/Twist2dProtoTest.cpp b/wpimath/src/test/native/cpp/geometry/proto/Twist2dProtoTest.cpp new file mode 100644 index 0000000000..d9f4faeb21 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/proto/Twist2dProtoTest.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/Twist2d.h" +#include "geometry2d.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Twist2d kExpectedData = Twist2d{2.29_m, 35.04_m, 35.04_rad}; +} // namespace + +TEST(Twist2dProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Twist2d unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.dx.value(), unpacked_data.dx.value()); + EXPECT_EQ(kExpectedData.dy.value(), unpacked_data.dy.value()); + EXPECT_EQ(kExpectedData.dtheta.value(), unpacked_data.dtheta.value()); +} diff --git a/wpimath/src/test/native/cpp/geometry/proto/Twist3dProtoTest.cpp b/wpimath/src/test/native/cpp/geometry/proto/Twist3dProtoTest.cpp new file mode 100644 index 0000000000..b4e7bf0730 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/proto/Twist3dProtoTest.cpp @@ -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. + +#include + +#include "frc/geometry/Twist3d.h" +#include "geometry3d.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Twist3d kExpectedData = + Twist3d{1.1_m, 2.29_m, 35.04_m, 0.174_rad, 19.1_rad, 4.4_rad}; +} // namespace + +TEST(Twist3dProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Twist3d unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.dx.value(), unpacked_data.dx.value()); + EXPECT_EQ(kExpectedData.dy.value(), unpacked_data.dy.value()); + EXPECT_EQ(kExpectedData.dz.value(), unpacked_data.dz.value()); + EXPECT_EQ(kExpectedData.rx.value(), unpacked_data.rx.value()); + EXPECT_EQ(kExpectedData.ry.value(), unpacked_data.ry.value()); + EXPECT_EQ(kExpectedData.rz.value(), unpacked_data.rz.value()); +} diff --git a/wpimath/src/test/native/cpp/geometry/struct/Pose2dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Pose2dStructTest.cpp new file mode 100644 index 0000000000..369a6aae07 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/struct/Pose2dStructTest.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 + +#include "frc/geometry/Pose2d.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const Pose2d kExpectedData{ + Pose2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{22.9_rad}}}; +} // namespace + +TEST(Pose2dStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + Pose2d unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation()); + EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation()); +} diff --git a/wpimath/src/test/native/cpp/geometry/struct/Pose3dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Pose3dStructTest.cpp new file mode 100644 index 0000000000..5eb1bc2d4e --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/struct/Pose3dStructTest.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/Pose3d.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const Pose3d kExpectedData{ + Pose3d{Translation3d{1.1_m, 2.2_m, 1.1_m}, + Rotation3d{Quaternion{1.91, 0.3504, 3.3, 1.74}}}}; +} // namespace + +TEST(Pose3dStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + Pose3d unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation()); + EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation()); +} diff --git a/wpimath/src/test/native/cpp/geometry/struct/QuaternionStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/QuaternionStructTest.cpp new file mode 100644 index 0000000000..6eede3c6a6 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/struct/QuaternionStructTest.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/Quaternion.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const Quaternion kExpectedData{Quaternion{1.1, 0.191, 35.04, 19.1}}; +} // namespace + +TEST(QuaternionStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + Quaternion unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.W(), unpacked_data.W()); + EXPECT_EQ(kExpectedData.X(), unpacked_data.X()); + EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y()); + EXPECT_EQ(kExpectedData.Z(), unpacked_data.Z()); +} diff --git a/wpimath/src/test/native/cpp/geometry/struct/Rotation2dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Rotation2dStructTest.cpp new file mode 100644 index 0000000000..9007463209 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/struct/Rotation2dStructTest.cpp @@ -0,0 +1,25 @@ +// 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/Rotation2d.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const Rotation2d kExpectedData{Rotation2d{1.91_rad}}; +} // namespace + +TEST(Rotation2dStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + Rotation2d unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.Radians(), unpacked_data.Radians()); +} diff --git a/wpimath/src/test/native/cpp/geometry/struct/Rotation3dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Rotation3dStructTest.cpp new file mode 100644 index 0000000000..992eb45e6c --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/struct/Rotation3dStructTest.cpp @@ -0,0 +1,26 @@ +// 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/Rotation3d.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const Rotation3d kExpectedData{ + Rotation3d{Quaternion{2.29, 0.191, 0.191, 17.4}}}; +} // namespace + +TEST(Rotation3dStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + Rotation3d unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.GetQuaternion(), unpacked_data.GetQuaternion()); +} diff --git a/wpimath/src/test/native/cpp/geometry/struct/Transform2dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Transform2dStructTest.cpp new file mode 100644 index 0000000000..b28aeaabf6 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/struct/Transform2dStructTest.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 + +#include "frc/geometry/Transform2d.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const Transform2d kExpectedData{ + Transform2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{4.4_rad}}}; +} // namespace + +TEST(Transform2dStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + Transform2d unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation()); + EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation()); +} diff --git a/wpimath/src/test/native/cpp/geometry/struct/Transform3dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Transform3dStructTest.cpp new file mode 100644 index 0000000000..d32433183b --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/struct/Transform3dStructTest.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/Transform3d.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const Transform3d kExpectedData{ + Transform3d{Translation3d{0.3504_m, 22.9_m, 3.504_m}, + Rotation3d{Quaternion{0.3504, 35.04, 2.29, 0.3504}}}}; +} // namespace + +TEST(Transform3dStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + Transform3d unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation()); + EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation()); +} diff --git a/wpimath/src/test/native/cpp/geometry/struct/Translation2dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Translation2dStructTest.cpp new file mode 100644 index 0000000000..6f87420cde --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/struct/Translation2dStructTest.cpp @@ -0,0 +1,26 @@ +// 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/Translation2d.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const Translation2d kExpectedData{Translation2d{3.504_m, 22.9_m}}; +} // namespace + +TEST(Translation2dStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + Translation2d unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.X(), unpacked_data.X()); + EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y()); +} diff --git a/wpimath/src/test/native/cpp/geometry/struct/Translation3dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Translation3dStructTest.cpp new file mode 100644 index 0000000000..1caa8a327f --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/struct/Translation3dStructTest.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 + +#include "frc/geometry/Translation3d.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const Translation3d kExpectedData{Translation3d{35.04_m, 22.9_m, 3.504_m}}; +} // namespace + +TEST(Translation3dStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + Translation3d unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.X(), unpacked_data.X()); + EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y()); + EXPECT_EQ(kExpectedData.Z(), unpacked_data.Z()); +} diff --git a/wpimath/src/test/native/cpp/geometry/struct/Twist2dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Twist2dStructTest.cpp new file mode 100644 index 0000000000..a3d1370d4b --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/struct/Twist2dStructTest.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 + +#include "frc/geometry/Twist2d.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const Twist2d kExpectedData{Twist2d{2.29_m, 35.04_m, 35.04_rad}}; +} // namespace + +TEST(Twist2dStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + Twist2d unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.dx.value(), unpacked_data.dx.value()); + EXPECT_EQ(kExpectedData.dy.value(), unpacked_data.dy.value()); + EXPECT_EQ(kExpectedData.dtheta.value(), unpacked_data.dtheta.value()); +} diff --git a/wpimath/src/test/native/cpp/geometry/struct/Twist3dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Twist3dStructTest.cpp new file mode 100644 index 0000000000..b753a3b263 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/struct/Twist3dStructTest.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 + +#include "frc/geometry/Twist3d.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const Twist3d kExpectedData{ + Twist3d{1.1_m, 2.29_m, 35.04_m, 0.174_rad, 19.1_rad, 4.4_rad}}; +} // namespace + +TEST(Twist3dStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + Twist3d unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.dx.value(), unpacked_data.dx.value()); + EXPECT_EQ(kExpectedData.dy.value(), unpacked_data.dy.value()); + EXPECT_EQ(kExpectedData.dz.value(), unpacked_data.dz.value()); + EXPECT_EQ(kExpectedData.rx.value(), unpacked_data.rx.value()); + EXPECT_EQ(kExpectedData.ry.value(), unpacked_data.ry.value()); + EXPECT_EQ(kExpectedData.rz.value(), unpacked_data.rz.value()); +}