Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Move struct/proto classes to separate files (#5918)

Browse Source 
Also add unit tests.
This commit is contained in:
PJ Reiniger
2023-11-21 13:11:57 -05:00
committed by GitHub
parent 80d7ad58ea
commit 35744a036e
141 changed files with 3360 additions and 1379 deletions
Download Patch File Download Diff File Expand all files Collapse all files

87
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
View File

@@ -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<Pose2d> {
}
}
public static final class AStruct implements Struct<Pose2d> {
@Override
public Class<Pose2d> 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<Pose2d, ProtobufPose2d> {
@Override
public Class<Pose2d> 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();
}

87
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
View File

@@ -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<Pose3d> {
}
}
public static final class AStruct implements Struct<Pose3d> {
@Override
public Class<Pose3d> 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<Pose3d, ProtobufPose3d> {
@Override
public Class<Pose3d> 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();
}

78
wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java
View File

@@ -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<Quaternion> {
@Override
public Class<Quaternion> 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<Quaternion, ProtobufQuaternion> {
@Override
public Class<Quaternion> 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();
}

71
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
View File

@@ -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<Rotation2d> {
return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
}
public static final class AStruct implements Struct<Rotation2d> {
@Override
public Class<Rotation2d> 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<Rotation2d, ProtobufRotation2d> {
@Override
public Class<Rotation2d> 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();
}

81
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
View File

@@ -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<Rotation3d> {
return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
}
public static final class AStruct implements Struct<Rotation3d> {
@Override
public Class<Rotation3d> 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<Rotation3d, ProtobufRotation3d> {
@Override
public Class<Rotation3d> 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();
}

87
wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
View File

@@ -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<Transform2d> {
@Override
public Class<Transform2d> 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<Transform2d, ProtobufTransform2d> {
@Override
public Class<Transform2d> 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();
}

87
wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java
View File

@@ -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<Transform3d> {
@Override
public Class<Transform3d> 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<Transform3d, ProtobufTransform3d> {
@Override
public Class<Transform3d> 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();
}

74
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
View File

@@ -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<Translation2d> {
MathUtil.interpolate(this.getY(), endValue.getY(), t));
}
public static final class AStruct implements Struct<Translation2d> {
@Override
public Class<Translation2d> 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<Translation2d, ProtobufTranslation2d> {
@Override
public Class<Translation2d> 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();
}

76
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
View File

@@ -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<Translation3d> {
MathUtil.interpolate(this.getZ(), endValue.getZ(), t));
}
public static final class AStruct implements Struct<Translation3d> {
@Override
public Class<Translation3d> 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<Translation3d, ProtobufTranslation3d> {
@Override
public Class<Translation3d> 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();
}

76
wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java
View File

@@ -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<Twist2d> {
@Override
public Class<Twist2d> 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<Twist2d, ProtobufTwist2d> {
@Override
public Class<Twist2d> 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();
}

88
wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java
View File

@@ -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<Twist3d> {
@Override
public Class<Twist3d> 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<Twist3d, ProtobufTwist3d> {
@Override
public Class<Twist3d> 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();
}

47
wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Pose2dProto.java Normal file
View File

@@ -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<Pose2d, ProtobufPose2d> {
@Override
public Class<Pose2d> 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());
}
}

47
wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Pose3dProto.java Normal file
View File

@@ -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<Pose3d, ProtobufPose3d> {
@Override
public Class<Pose3d> 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());
}
}

40
wpimath/src/main/java/edu/wpi/first/math/geometry/proto/QuaternionProto.java Normal file
View File

@@ -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<Quaternion, ProtobufQuaternion> {
@Override
public Class<Quaternion> 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());
}
}

37
wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rotation2dProto.java Normal file
View File

@@ -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<Rotation2d, ProtobufRotation2d> {
@Override
public Class<Rotation2d> 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());
}
}

43
wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rotation3dProto.java Normal file
View File

@@ -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<Rotation3d, ProtobufRotation3d> {
@Override
public Class<Rotation3d> 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());
}
}

47
wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Transform2dProto.java Normal file
View File

@@ -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<Transform2d, ProtobufTransform2d> {
@Override
public Class<Transform2d> 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());
}
}

47
wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Transform3dProto.java Normal file
View File

@@ -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<Transform3d, ProtobufTransform3d> {
@Override
public Class<Transform3d> 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());
}
}

38
wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Translation2dProto.java Normal file
View File

@@ -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<Translation2d, ProtobufTranslation2d> {
@Override
public Class<Translation2d> 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());
}
}

39
wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Translation3dProto.java Normal file
View File

@@ -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<Translation3d, ProtobufTranslation3d> {
@Override
public Class<Translation3d> 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());
}
}

39
wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Twist2dProto.java Normal file
View File

@@ -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<Twist2d, ProtobufTwist2d> {
@Override
public Class<Twist2d> 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);
}
}

43
wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Twist3dProto.java Normal file
View File

@@ -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<Twist3d, ProtobufTwist3d> {
@Override
public Class<Twist3d> 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);
}
}

51
wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Pose2dStruct.java Normal file
View File

@@ -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<Pose2d> {
@Override
public Class<Pose2d> 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());
}
}

51
wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Pose3dStruct.java Normal file
View File

@@ -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<Pose3d> {
@Override
public Class<Pose3d> 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());
}
}

48
wpimath/src/main/java/edu/wpi/first/math/geometry/struct/QuaternionStruct.java Normal file
View File

@@ -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<Quaternion> {
@Override
public Class<Quaternion> 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());
}
}

42
wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rotation2dStruct.java Normal file
View File

@@ -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<Rotation2d> {
@Override
public Class<Rotation2d> 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());
}
}

48
wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rotation3dStruct.java Normal file
View File

@@ -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<Rotation3d> {
@Override
public Class<Rotation3d> 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());
}
}

51
wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Transform2dStruct.java Normal file
View File

@@ -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<Transform2d> {
@Override
public Class<Transform2d> 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());
}
}

51
wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Transform3dStruct.java Normal file
View File

@@ -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<Transform3d> {
@Override
public Class<Transform3d> 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());
}
}

44
wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Translation2dStruct.java Normal file
View File

@@ -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<Translation2d> {
@Override
public Class<Translation2d> 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());
}
}

46
wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Translation3dStruct.java Normal file
View File

@@ -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<Translation3d> {
@Override
public Class<Translation3d> 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());
}
}

46
wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Twist2dStruct.java Normal file
View File

@@ -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<Twist2d> {
@Override
public Class<Twist2d> 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);
}
}

52
wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Twist3dStruct.java Normal file
View File

@@ -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<Twist3d> {
@Override
public Class<Twist3d> 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);
}
}