Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02:51:42 +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);
}
}

21
wpimath/src/main/native/cpp/geometry/Pose2d.cpp
View File

@@ -9,7 +9,6 @@
#include <wpi/json.h>
#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<Translation2d>(),
json.at("rotation").get<Rotation2d>()};
}
google::protobuf::Message* wpi::Protobuf<frc::Pose2d>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufPose2d>(
arena);
}
frc::Pose2d wpi::Protobuf<frc::Pose2d>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufPose2d*>(&msg);
return Pose2d{wpi::UnpackProtobuf<frc::Translation2d>(m->translation()),
wpi::UnpackProtobuf<frc::Rotation2d>(m->rotation())};
}
void wpi::Protobuf<frc::Pose2d>::Pack(google::protobuf::Message* msg,
const frc::Pose2d& value) {
auto m = static_cast<wpi::proto::ProtobufPose2d*>(msg);
wpi::PackProtobuf(m->mutable_translation(), value.Translation());
wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
}

20
wpimath/src/main/native/cpp/geometry/Pose3d.cpp
View File

@@ -189,23 +189,3 @@ void frc::from_json(const wpi::json& json, Pose3d& pose) {
pose = Pose3d{json.at("translation").get<Translation3d>(),
json.at("rotation").get<Rotation3d>()};
}
google::protobuf::Message* wpi::Protobuf<frc::Pose3d>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufPose3d>(
arena);
}
frc::Pose3d wpi::Protobuf<frc::Pose3d>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufPose3d*>(&msg);
return Pose3d{wpi::UnpackProtobuf<frc::Translation3d>(m->translation()),
wpi::UnpackProtobuf<frc::Rotation3d>(m->rotation())};
}
void wpi::Protobuf<frc::Pose3d>::Pack(google::protobuf::Message* msg,
const frc::Pose3d& value) {
auto m = static_cast<wpi::proto::ProtobufPose3d*>(msg);
wpi::PackProtobuf(m->mutable_translation(), value.Translation());
wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
}

21
wpimath/src/main/native/cpp/geometry/Quaternion.cpp
View File

@@ -232,24 +232,3 @@ void frc::from_json(const wpi::json& json, Quaternion& quaternion) {
Quaternion{json.at("W").get<double>(), json.at("X").get<double>(),
json.at("Y").get<double>(), json.at("Z").get<double>()};
}
google::protobuf::Message* wpi::Protobuf<frc::Quaternion>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufQuaternion>(
arena);
}
frc::Quaternion wpi::Protobuf<frc::Quaternion>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufQuaternion*>(&msg);
return frc::Quaternion{m->w(), m->x(), m->y(), m->z()};
}
void wpi::Protobuf<frc::Quaternion>::Pack(google::protobuf::Message* msg,
const frc::Quaternion& value) {
auto m = static_cast<wpi::proto::ProtobufQuaternion*>(msg);
m->set_w(value.W());
m->set_x(value.X());
m->set_y(value.Y());
m->set_z(value.Z());
}

18
wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
View File

@@ -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<double>()}};
}
google::protobuf::Message* wpi::Protobuf<frc::Rotation2d>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufRotation2d>(
arena);
}
frc::Rotation2d wpi::Protobuf<frc::Rotation2d>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufRotation2d*>(&msg);
return frc::Rotation2d{units::radian_t{m->value()}};
}
void wpi::Protobuf<frc::Rotation2d>::Pack(google::protobuf::Message* msg,
const frc::Rotation2d& value) {
auto m = static_cast<wpi::proto::ProtobufRotation2d*>(msg);
m->set_value(value.Radians().value());
}

18
wpimath/src/main/native/cpp/geometry/Rotation3d.cpp
View File

@@ -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<Quaternion>()};
}
google::protobuf::Message* wpi::Protobuf<frc::Rotation3d>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufRotation3d>(
arena);
}
frc::Rotation3d wpi::Protobuf<frc::Rotation3d>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufRotation3d*>(&msg);
return Rotation3d{wpi::UnpackProtobuf<frc::Quaternion>(m->q())};
}
void wpi::Protobuf<frc::Rotation3d>::Pack(google::protobuf::Message* msg,
const frc::Rotation3d& value) {
auto m = static_cast<wpi::proto::ProtobufRotation3d*>(msg);
wpi::PackProtobuf(m->mutable_q(), value.GetQuaternion());
}

20
wpimath/src/main/native/cpp/geometry/Transform2d.cpp
View File

@@ -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<frc::Transform2d>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<
wpi::proto::ProtobufTransform2d>(arena);
}
frc::Transform2d wpi::Protobuf<frc::Transform2d>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufTransform2d*>(&msg);
return Transform2d{wpi::UnpackProtobuf<frc::Translation2d>(m->translation()),
wpi::UnpackProtobuf<frc::Rotation2d>(m->rotation())};
}
void wpi::Protobuf<frc::Transform2d>::Pack(google::protobuf::Message* msg,
const frc::Transform2d& value) {
auto m = static_cast<wpi::proto::ProtobufTransform2d*>(msg);
wpi::PackProtobuf(m->mutable_translation(), value.Translation());
wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
}

21
wpimath/src/main/native/cpp/geometry/Transform3d.cpp
View File

@@ -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<frc::Transform3d>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<
wpi::proto::ProtobufTransform3d>(arena);
}
frc::Transform3d wpi::Protobuf<frc::Transform3d>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufTransform3d*>(&msg);
return Transform3d{wpi::UnpackProtobuf<frc::Translation3d>(m->translation()),
wpi::UnpackProtobuf<frc::Rotation3d>(m->rotation())};
}
void wpi::Protobuf<frc::Transform3d>::Pack(google::protobuf::Message* msg,
const frc::Transform3d& value) {
auto m = static_cast<wpi::proto::ProtobufTransform3d*>(msg);
wpi::PackProtobuf(m->mutable_translation(), value.Translation());
wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
}

19
wpimath/src/main/native/cpp/geometry/Translation2d.cpp
View File

@@ -49,22 +49,3 @@ void frc::from_json(const wpi::json& json, Translation2d& translation) {
translation = Translation2d{units::meter_t{json.at("x").get<double>()},
units::meter_t{json.at("y").get<double>()}};
}
google::protobuf::Message* wpi::Protobuf<frc::Translation2d>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<
wpi::proto::ProtobufTranslation2d>(arena);
}
frc::Translation2d wpi::Protobuf<frc::Translation2d>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufTranslation2d*>(&msg);
return frc::Translation2d{units::meter_t{m->x()}, units::meter_t{m->y()}};
}
void wpi::Protobuf<frc::Translation2d>::Pack(google::protobuf::Message* msg,
const frc::Translation2d& value) {
auto m = static_cast<wpi::proto::ProtobufTranslation2d*>(msg);
m->set_x(value.X().value());
m->set_y(value.Y().value());
}

21
wpimath/src/main/native/cpp/geometry/Translation3d.cpp
View File

@@ -53,24 +53,3 @@ void frc::from_json(const wpi::json& json, Translation3d& translation) {
units::meter_t{json.at("y").get<double>()},
units::meter_t{json.at("z").get<double>()}};
}
google::protobuf::Message* wpi::Protobuf<frc::Translation3d>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<
wpi::proto::ProtobufTranslation3d>(arena);
}
frc::Translation3d wpi::Protobuf<frc::Translation3d>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufTranslation3d*>(&msg);
return frc::Translation3d{units::meter_t{m->x()}, units::meter_t{m->y()},
units::meter_t{m->z()}};
}
void wpi::Protobuf<frc::Translation3d>::Pack(google::protobuf::Message* msg,
const frc::Translation3d& value) {
auto m = static_cast<wpi::proto::ProtobufTranslation3d*>(msg);
m->set_x(value.X().value());
m->set_y(value.Y().value());
m->set_z(value.Z().value());
}

29
wpimath/src/main/native/cpp/geometry/proto/Pose2dProto.cpp Normal file
View File

@@ -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<frc::Pose2d>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufPose2d>(
arena);
}
frc::Pose2d wpi::Protobuf<frc::Pose2d>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufPose2d*>(&msg);
return frc::Pose2d{
wpi::UnpackProtobuf<frc::Translation2d>(m->translation()),
wpi::UnpackProtobuf<frc::Rotation2d>(m->rotation()),
};
}
void wpi::Protobuf<frc::Pose2d>::Pack(google::protobuf::Message* msg,
const frc::Pose2d& value) {
auto m = static_cast<wpi::proto::ProtobufPose2d*>(msg);
wpi::PackProtobuf(m->mutable_translation(), value.Translation());
wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
}

29
wpimath/src/main/native/cpp/geometry/proto/Pose3dProto.cpp Normal file
View File

@@ -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<frc::Pose3d>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufPose3d>(
arena);
}
frc::Pose3d wpi::Protobuf<frc::Pose3d>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufPose3d*>(&msg);
return frc::Pose3d{
wpi::UnpackProtobuf<frc::Translation3d>(m->translation()),
wpi::UnpackProtobuf<frc::Rotation3d>(m->rotation()),
};
}
void wpi::Protobuf<frc::Pose3d>::Pack(google::protobuf::Message* msg,
const frc::Pose3d& value) {
auto m = static_cast<wpi::proto::ProtobufPose3d*>(msg);
wpi::PackProtobuf(m->mutable_translation(), value.Translation());
wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
}

33
wpimath/src/main/native/cpp/geometry/proto/QuaternionProto.cpp Normal file
View File

@@ -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<frc::Quaternion>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufQuaternion>(
arena);
}
frc::Quaternion wpi::Protobuf<frc::Quaternion>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufQuaternion*>(&msg);
return frc::Quaternion{
m->w(),
m->x(),
m->y(),
m->z(),
};
}
void wpi::Protobuf<frc::Quaternion>::Pack(google::protobuf::Message* msg,
const frc::Quaternion& value) {
auto m = static_cast<wpi::proto::ProtobufQuaternion*>(msg);
m->set_w(value.W());
m->set_x(value.X());
m->set_y(value.Y());
m->set_z(value.Z());
}

27
wpimath/src/main/native/cpp/geometry/proto/Rotation2dProto.cpp Normal file
View File

@@ -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<frc::Rotation2d>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufRotation2d>(
arena);
}
frc::Rotation2d wpi::Protobuf<frc::Rotation2d>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufRotation2d*>(&msg);
return frc::Rotation2d{
units::radian_t{m->value()},
};
}
void wpi::Protobuf<frc::Rotation2d>::Pack(google::protobuf::Message* msg,
const frc::Rotation2d& value) {
auto m = static_cast<wpi::proto::ProtobufRotation2d*>(msg);
m->set_value(value.Radians().value());
}

27
wpimath/src/main/native/cpp/geometry/proto/Rotation3dProto.cpp Normal file
View File

@@ -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<frc::Rotation3d>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufRotation3d>(
arena);
}
frc::Rotation3d wpi::Protobuf<frc::Rotation3d>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufRotation3d*>(&msg);
return frc::Rotation3d{
wpi::UnpackProtobuf<frc::Quaternion>(m->q()),
};
}
void wpi::Protobuf<frc::Rotation3d>::Pack(google::protobuf::Message* msg,
const frc::Rotation3d& value) {
auto m = static_cast<wpi::proto::ProtobufRotation3d*>(msg);
wpi::PackProtobuf(m->mutable_q(), value.GetQuaternion());
}

29
wpimath/src/main/native/cpp/geometry/proto/Transform2dProto.cpp Normal file
View File

@@ -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<frc::Transform2d>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<
wpi::proto::ProtobufTransform2d>(arena);
}
frc::Transform2d wpi::Protobuf<frc::Transform2d>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufTransform2d*>(&msg);
return frc::Transform2d{
wpi::UnpackProtobuf<frc::Translation2d>(m->translation()),
wpi::UnpackProtobuf<frc::Rotation2d>(m->rotation()),
};
}
void wpi::Protobuf<frc::Transform2d>::Pack(google::protobuf::Message* msg,
const frc::Transform2d& value) {
auto m = static_cast<wpi::proto::ProtobufTransform2d*>(msg);
wpi::PackProtobuf(m->mutable_translation(), value.Translation());
wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
}

29
wpimath/src/main/native/cpp/geometry/proto/Transform3dProto.cpp Normal file
View File

@@ -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<frc::Transform3d>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<
wpi::proto::ProtobufTransform3d>(arena);
}
frc::Transform3d wpi::Protobuf<frc::Transform3d>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufTransform3d*>(&msg);
return frc::Transform3d{
wpi::UnpackProtobuf<frc::Translation3d>(m->translation()),
wpi::UnpackProtobuf<frc::Rotation3d>(m->rotation()),
};
}
void wpi::Protobuf<frc::Transform3d>::Pack(google::protobuf::Message* msg,
const frc::Transform3d& value) {
auto m = static_cast<wpi::proto::ProtobufTransform3d*>(msg);
wpi::PackProtobuf(m->mutable_translation(), value.Translation());
wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
}

29
wpimath/src/main/native/cpp/geometry/proto/Translation2dProto.cpp Normal file
View File

@@ -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<frc::Translation2d>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<
wpi::proto::ProtobufTranslation2d>(arena);
}
frc::Translation2d wpi::Protobuf<frc::Translation2d>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufTranslation2d*>(&msg);
return frc::Translation2d{
units::meter_t{m->x()},
units::meter_t{m->y()},
};
}
void wpi::Protobuf<frc::Translation2d>::Pack(google::protobuf::Message* msg,
const frc::Translation2d& value) {
auto m = static_cast<wpi::proto::ProtobufTranslation2d*>(msg);
m->set_x(value.X().value());
m->set_y(value.Y().value());
}

31
wpimath/src/main/native/cpp/geometry/proto/Translation3dProto.cpp Normal file
View File

@@ -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<frc::Translation3d>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<
wpi::proto::ProtobufTranslation3d>(arena);
}
frc::Translation3d wpi::Protobuf<frc::Translation3d>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufTranslation3d*>(&msg);
return frc::Translation3d{
units::meter_t{m->x()},
units::meter_t{m->y()},
units::meter_t{m->z()},
};
}
void wpi::Protobuf<frc::Translation3d>::Pack(google::protobuf::Message* msg,
const frc::Translation3d& value) {
auto m = static_cast<wpi::proto::ProtobufTranslation3d*>(msg);
m->set_x(value.X().value());
m->set_y(value.Y().value());
m->set_z(value.Z().value());
}

11
wpimath/src/main/native/cpp/geometry/Twist2d.cpp → wpimath/src/main/native/cpp/geometry/proto/Twist2dProto.cpp
View File

@@ -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<frc::Twist2d>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufTwist2d>(
@@ -17,8 +15,11 @@ google::protobuf::Message* wpi::Protobuf<frc::Twist2d>::New(
frc::Twist2d wpi::Protobuf<frc::Twist2d>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufTwist2d*>(&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<frc::Twist2d>::Pack(google::protobuf::Message* msg,

12
wpimath/src/main/native/cpp/geometry/Twist3d.cpp → wpimath/src/main/native/cpp/geometry/proto/Twist3dProto.cpp
View File

@@ -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<frc::Twist3d>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufTwist3d>(
@@ -17,9 +15,11 @@ google::protobuf::Message* wpi::Protobuf<frc::Twist3d>::New(
frc::Twist3d wpi::Protobuf<frc::Twist3d>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufTwist3d*>(&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<frc::Twist3d>::Pack(google::protobuf::Message* msg,

32
wpimath/src/main/native/cpp/geometry/struct/Pose2dStruct.cpp Normal file
View File

@@ -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<frc::Translation2d>::kSize;
} // namespace
using StructType = wpi::Struct<frc::Pose2d>;
frc::Pose2d StructType::Unpack(std::span<const uint8_t, kSize> data) {
return frc::Pose2d{
wpi::UnpackStruct<frc::Translation2d, kTranslationOff>(data),
wpi::UnpackStruct<frc::Rotation2d, kRotationOff>(data),
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
const frc::Pose2d& value) {
wpi::PackStruct<kTranslationOff>(data, value.Translation());
wpi::PackStruct<kRotationOff>(data, value.Rotation());
}
void StructType::ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Translation2d>(fn);
wpi::ForEachStructSchema<frc::Rotation2d>(fn);
}

32
wpimath/src/main/native/cpp/geometry/struct/Pose3dStruct.cpp Normal file
View File

@@ -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<frc::Translation3d>::kSize;
} // namespace
using StructType = wpi::Struct<frc::Pose3d>;
frc::Pose3d StructType::Unpack(std::span<const uint8_t, kSize> data) {
return frc::Pose3d{
wpi::UnpackStruct<frc::Translation3d, kTranslationOff>(data),
wpi::UnpackStruct<frc::Rotation3d, kRotationOff>(data),
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
const frc::Pose3d& value) {
wpi::PackStruct<kTranslationOff>(data, value.Translation());
wpi::PackStruct<kRotationOff>(data, value.Rotation());
}
void StructType::ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Translation3d>(fn);
wpi::ForEachStructSchema<frc::Rotation3d>(fn);
}

31
wpimath/src/main/native/cpp/geometry/struct/QuaternionStruct.cpp Normal file
View File

@@ -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>;
frc::Quaternion StructType::Unpack(std::span<const uint8_t, kSize> data) {
return frc::Quaternion{
wpi::UnpackStruct<double, kWOff>(data),
wpi::UnpackStruct<double, kXOff>(data),
wpi::UnpackStruct<double, kYOff>(data),
wpi::UnpackStruct<double, kZOff>(data),
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
const frc::Quaternion& value) {
wpi::PackStruct<kWOff>(data, value.W());
wpi::PackStruct<kXOff>(data, value.X());
wpi::PackStruct<kYOff>(data, value.Y());
wpi::PackStruct<kZOff>(data, value.Z());
}

22
wpimath/src/main/native/cpp/geometry/struct/Rotation2dStruct.cpp Normal file
View File

@@ -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>;
frc::Rotation2d StructType::Unpack(std::span<const uint8_t, kSize> data) {
return frc::Rotation2d{
units::radian_t{wpi::UnpackStruct<double, kValueOff>(data)},
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
const frc::Rotation2d& value) {
wpi::PackStruct<kValueOff>(data, value.Radians().value());
}

27
wpimath/src/main/native/cpp/geometry/struct/Rotation3dStruct.cpp Normal file
View File

@@ -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>;
frc::Rotation3d StructType::Unpack(std::span<const uint8_t, kSize> data) {
return frc::Rotation3d{
wpi::UnpackStruct<frc::Quaternion, kQOff>(data),
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
const frc::Rotation3d& value) {
wpi::PackStruct<kQOff>(data, value.GetQuaternion());
}
void StructType::ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Quaternion>(fn);
}

32
wpimath/src/main/native/cpp/geometry/struct/Transform2dStruct.cpp Normal file
View File

@@ -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<frc::Translation2d>::kSize;
} // namespace
using StructType = wpi::Struct<frc::Transform2d>;
frc::Transform2d StructType::Unpack(std::span<const uint8_t, kSize> data) {
return frc::Transform2d{
wpi::UnpackStruct<frc::Translation2d, kTranslationOff>(data),
wpi::UnpackStruct<frc::Rotation2d, kRotationOff>(data),
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
const frc::Transform2d& value) {
wpi::PackStruct<kTranslationOff>(data, value.Translation());
wpi::PackStruct<kRotationOff>(data, value.Rotation());
}
void StructType::ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Translation2d>(fn);
wpi::ForEachStructSchema<frc::Rotation2d>(fn);
}

32
wpimath/src/main/native/cpp/geometry/struct/Transform3dStruct.cpp Normal file
View File

@@ -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<frc::Translation3d>::kSize;
} // namespace
using StructType = wpi::Struct<frc::Transform3d>;
frc::Transform3d StructType::Unpack(std::span<const uint8_t, kSize> data) {
return frc::Transform3d{
wpi::UnpackStruct<frc::Translation3d, kTranslationOff>(data),
wpi::UnpackStruct<frc::Rotation3d, kRotationOff>(data),
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
const frc::Transform3d& value) {
wpi::PackStruct<kTranslationOff>(data, value.Translation());
wpi::PackStruct<kRotationOff>(data, value.Rotation());
}
void StructType::ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Translation3d>(fn);
wpi::ForEachStructSchema<frc::Rotation3d>(fn);
}

25
wpimath/src/main/native/cpp/geometry/struct/Translation2dStruct.cpp Normal file
View File

@@ -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>;
frc::Translation2d StructType::Unpack(std::span<const uint8_t, kSize> data) {
return frc::Translation2d{
units::meter_t{wpi::UnpackStruct<double, kXOff>(data)},
units::meter_t{wpi::UnpackStruct<double, kYOff>(data)},
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
const frc::Translation2d& value) {
wpi::PackStruct<kXOff>(data, value.X().value());
wpi::PackStruct<kYOff>(data, value.Y().value());
}

28
wpimath/src/main/native/cpp/geometry/struct/Translation3dStruct.cpp Normal file
View File

@@ -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>;
frc::Translation3d StructType::Unpack(std::span<const uint8_t, kSize> data) {
return frc::Translation3d{
units::meter_t{wpi::UnpackStruct<double, kXOff>(data)},
units::meter_t{wpi::UnpackStruct<double, kYOff>(data)},
units::meter_t{wpi::UnpackStruct<double, kZOff>(data)},
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
const frc::Translation3d& value) {
wpi::PackStruct<kXOff>(data, value.X().value());
wpi::PackStruct<kYOff>(data, value.Y().value());
wpi::PackStruct<kZOff>(data, value.Z().value());
}

28
wpimath/src/main/native/cpp/geometry/struct/Twist2dStruct.cpp Normal file
View File

@@ -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>;
frc::Twist2d StructType::Unpack(std::span<const uint8_t, kSize> data) {
return frc::Twist2d{
units::meter_t{wpi::UnpackStruct<double, kDxOff>(data)},
units::meter_t{wpi::UnpackStruct<double, kDyOff>(data)},
units::radian_t{wpi::UnpackStruct<double, kDthetaOff>(data)},
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
const frc::Twist2d& value) {
wpi::PackStruct<kDxOff>(data, value.dx.value());
wpi::PackStruct<kDyOff>(data, value.dy.value());
wpi::PackStruct<kDthetaOff>(data, value.dtheta.value());
}

37
wpimath/src/main/native/cpp/geometry/struct/Twist3dStruct.cpp 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.
#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>;
frc::Twist3d StructType::Unpack(std::span<const uint8_t, kSize> data) {
return frc::Twist3d{
units::meter_t{wpi::UnpackStruct<double, kDxOff>(data)},
units::meter_t{wpi::UnpackStruct<double, kDyOff>(data)},
units::meter_t{wpi::UnpackStruct<double, kDzOff>(data)},
units::radian_t{wpi::UnpackStruct<double, kRxOff>(data)},
units::radian_t{wpi::UnpackStruct<double, kRyOff>(data)},
units::radian_t{wpi::UnpackStruct<double, kRzOff>(data)},
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
const frc::Twist3d& value) {
wpi::PackStruct<kDxOff>(data, value.dx.value());
wpi::PackStruct<kDyOff>(data, value.dy.value());
wpi::PackStruct<kDzOff>(data, value.dz.value());
wpi::PackStruct<kRxOff>(data, value.rx.value());
wpi::PackStruct<kRyOff>(data, value.ry.value());
wpi::PackStruct<kRzOff>(data, value.rz.value());
}

38
wpimath/src/main/native/include/frc/geometry/Pose2d.h
View File

@@ -9,8 +9,6 @@
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
#include <wpi/protobuf/Protobuf.h>
#include <wpi/struct/Struct.h>
#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<frc::Pose2d> {
static constexpr std::string_view kTypeString = "struct:Pose2d";
static constexpr size_t kSize = wpi::Struct<frc::Translation2d>::kSize +
wpi::Struct<frc::Rotation2d>::kSize;
static constexpr std::string_view kSchema =
"Translation2d translation;Rotation2d rotation";
static frc::Pose2d Unpack(std::span<const uint8_t, kSize> data) {
return {wpi::UnpackStruct<frc::Translation2d, 0>(data),
wpi::UnpackStruct<frc::Rotation2d, kRotationOff>(data)};
}
static void Pack(std::span<uint8_t, kSize> data, const frc::Pose2d& value) {
wpi::PackStruct<0>(data, value.Translation());
wpi::PackStruct<kRotationOff>(data, value.Rotation());
}
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Translation2d>(fn);
wpi::ForEachStructSchema<frc::Rotation2d>(fn);
}
private:
static constexpr size_t kRotationOff = wpi::Struct<frc::Translation2d>::kSize;
};
static_assert(wpi::HasNestedStruct<frc::Pose2d>);
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Pose2d> {
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"

37
wpimath/src/main/native/include/frc/geometry/Pose3d.h
View File

@@ -6,8 +6,6 @@
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
#include <wpi/protobuf/Protobuf.h>
#include <wpi/struct/Struct.h>
#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<frc::Pose3d> {
static constexpr std::string_view kTypeString = "struct:Pose3d";
static constexpr size_t kSize = wpi::Struct<frc::Translation3d>::kSize +
wpi::Struct<frc::Rotation3d>::kSize;
static constexpr std::string_view kSchema =
"Translation3d translation;Rotation3d rotation";
static frc::Pose3d Unpack(std::span<const uint8_t, kSize> data) {
return {wpi::UnpackStruct<frc::Translation3d, 0>(data),
wpi::UnpackStruct<frc::Rotation3d, kRotationOff>(data)};
}
static void Pack(std::span<uint8_t, kSize> data, const frc::Pose3d& value) {
wpi::PackStruct<0>(data, value.Translation());
wpi::PackStruct<kRotationOff>(data, value.Rotation());
}
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Translation3d>(fn);
wpi::ForEachStructSchema<frc::Rotation3d>(fn);
}
private:
static constexpr size_t kRotationOff = wpi::Struct<frc::Translation3d>::kSize;
};
static_assert(wpi::HasNestedStruct<frc::Pose3d>);
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Pose3d> {
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"

32
wpimath/src/main/native/include/frc/geometry/Quaternion.h
View File

@@ -7,8 +7,6 @@
#include <Eigen/Core>
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
#include <wpi/protobuf/Protobuf.h>
#include <wpi/struct/Struct.h>
namespace frc {
@@ -190,31 +188,5 @@ void from_json(const wpi::json& json, Quaternion& quaternion);
} // namespace frc
template <>
struct wpi::Struct<frc::Quaternion> {
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<const uint8_t, 32> data) {
return {wpi::UnpackStruct<double, 0>(data),
wpi::UnpackStruct<double, 8>(data),
wpi::UnpackStruct<double, 16>(data),
wpi::UnpackStruct<double, 24>(data)};
}
static void Pack(std::span<uint8_t, 32> 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<frc::Quaternion> {
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"

25
wpimath/src/main/native/include/frc/geometry/Rotation2d.h
View File

@@ -6,8 +6,6 @@
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
#include <wpi/protobuf/Protobuf.h>
#include <wpi/struct/Struct.h>
#include "units/angle.h"
@@ -178,25 +176,6 @@ void from_json(const wpi::json& json, Rotation2d& rotation);
} // namespace frc
template <>
struct wpi::Struct<frc::Rotation2d> {
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<const uint8_t, 8> data) {
return units::radian_t{wpi::UnpackStruct<double>(data)};
}
static void Pack(std::span<uint8_t, 8> data, const frc::Rotation2d& value) {
wpi::PackStruct(data, value.Radians().value());
}
};
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Rotation2d> {
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"

31
wpimath/src/main/native/include/frc/geometry/Rotation3d.h
View File

@@ -7,8 +7,6 @@
#include <Eigen/Core>
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
#include <wpi/protobuf/Protobuf.h>
#include <wpi/struct/Struct.h>
#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<frc::Rotation3d> {
static constexpr std::string_view kTypeString = "struct:Rotation3d";
static constexpr size_t kSize = wpi::Struct<frc::Quaternion>::kSize;
static constexpr std::string_view kSchema = "Quaternion q";
static frc::Rotation3d Unpack(std::span<const uint8_t, kSize> data) {
return frc::Rotation3d{wpi::UnpackStruct<frc::Quaternion, 0>(data)};
}
static void Pack(std::span<uint8_t, kSize> data,
const frc::Rotation3d& value) {
wpi::PackStruct<0>(data, value.GetQuaternion());
}
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Quaternion>(fn);
}
};
static_assert(wpi::HasNestedStruct<frc::Rotation3d>);
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Rotation3d> {
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"

40
wpimath/src/main/native/include/frc/geometry/Transform2d.h
View File

@@ -5,8 +5,6 @@
#pragma once
#include <wpi/SymbolExports.h>
#include <wpi/protobuf/Protobuf.h>
#include <wpi/struct/Struct.h>
#include "frc/geometry/Translation2d.h"
@@ -126,40 +124,6 @@ class WPILIB_DLLEXPORT Transform2d {
};
} // namespace frc
template <>
struct wpi::Struct<frc::Transform2d> {
static constexpr std::string_view kTypeString = "struct:Transform2d";
static constexpr size_t kSize = wpi::Struct<frc::Translation2d>::kSize +
wpi::Struct<frc::Rotation2d>::kSize;
static constexpr std::string_view kSchema =
"Translation2d translation;Rotation2d rotation";
static frc::Transform2d Unpack(std::span<const uint8_t, kSize> data) {
return {wpi::UnpackStruct<frc::Translation2d, 0>(data),
wpi::UnpackStruct<frc::Rotation2d, kRotationOff>(data)};
}
static void Pack(std::span<uint8_t, kSize> data,
const frc::Transform2d& value) {
wpi::PackStruct<0>(data, value.Translation());
wpi::PackStruct<kRotationOff>(data, value.Rotation());
}
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Translation2d>(fn);
wpi::ForEachStructSchema<frc::Rotation2d>(fn);
}
private:
static constexpr size_t kRotationOff = wpi::Struct<frc::Translation2d>::kSize;
};
static_assert(wpi::HasNestedStruct<frc::Transform2d>);
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Transform2d> {
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"

39
wpimath/src/main/native/include/frc/geometry/Transform3d.h
View File

@@ -5,8 +5,6 @@
#pragma once
#include <wpi/SymbolExports.h>
#include <wpi/protobuf/Protobuf.h>
#include <wpi/struct/Struct.h>
#include "frc/geometry/Translation3d.h"
@@ -132,38 +130,5 @@ class WPILIB_DLLEXPORT Transform3d {
};
} // namespace frc
template <>
struct wpi::Struct<frc::Transform3d> {
static constexpr std::string_view kTypeString = "struct:Transform3d";
static constexpr size_t kSize = wpi::Struct<frc::Translation3d>::kSize +
wpi::Struct<frc::Rotation3d>::kSize;
static constexpr std::string_view kSchema =
"Translation3d translation;Rotation3d rotation";
static frc::Transform3d Unpack(std::span<const uint8_t, kSize> data) {
return {wpi::UnpackStruct<frc::Translation3d, 0>(data),
wpi::UnpackStruct<frc::Rotation3d, kRotationOff>(data)};
}
static void Pack(std::span<uint8_t, kSize> data,
const frc::Transform3d& value) {
wpi::PackStruct<0>(data, value.Translation());
wpi::PackStruct<kRotationOff>(data, value.Rotation());
}
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Translation3d>(fn);
wpi::ForEachStructSchema<frc::Rotation3d>(fn);
}
private:
static constexpr size_t kRotationOff = wpi::Struct<frc::Translation3d>::kSize;
};
static_assert(wpi::HasNestedStruct<frc::Transform3d>);
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Transform3d> {
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"

28
wpimath/src/main/native/include/frc/geometry/Translation2d.h
View File

@@ -9,8 +9,6 @@
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
#include <wpi/protobuf/Protobuf.h>
#include <wpi/struct/Struct.h>
#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<frc::Translation2d> {
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<const uint8_t, 16> data) {
return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
units::meter_t{wpi::UnpackStruct<double, 8>(data)}};
}
static void Pack(std::span<uint8_t, 16> 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<frc::Translation2d> {
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"

30
wpimath/src/main/native/include/frc/geometry/Translation3d.h
View File

@@ -6,8 +6,6 @@
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
#include <wpi/protobuf/Protobuf.h>
#include <wpi/struct/Struct.h>
#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<frc::Translation3d> {
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<const uint8_t, 24> data) {
return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
units::meter_t{wpi::UnpackStruct<double, 8>(data)},
units::meter_t{wpi::UnpackStruct<double, 16>(data)}};
}
static void Pack(std::span<uint8_t, 24> 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<frc::Translation3d> {
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"

28
wpimath/src/main/native/include/frc/geometry/Twist2d.h
View File

@@ -5,8 +5,6 @@
#pragma once
#include <wpi/SymbolExports.h>
#include <wpi/protobuf/Protobuf.h>
#include <wpi/struct/Struct.h>
#include "units/angle.h"
#include "units/length.h"
@@ -60,27 +58,5 @@ struct WPILIB_DLLEXPORT Twist2d {
};
} // namespace frc
template <>
struct wpi::Struct<frc::Twist2d> {
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<const uint8_t, 24> data) {
return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
units::meter_t{wpi::UnpackStruct<double, 8>(data)},
units::radian_t{wpi::UnpackStruct<double, 16>(data)}};
}
static void Pack(std::span<uint8_t, 24> 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<frc::Twist2d> {
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"

35
wpimath/src/main/native/include/frc/geometry/Twist3d.h
View File

@@ -5,8 +5,6 @@
#pragma once
#include <wpi/SymbolExports.h>
#include <wpi/protobuf/Protobuf.h>
#include <wpi/struct/Struct.h>
#include "frc/geometry/Rotation3d.h"
#include "units/angle.h"
@@ -80,34 +78,5 @@ struct WPILIB_DLLEXPORT Twist3d {
};
} // namespace frc
template <>
struct wpi::Struct<frc::Twist3d> {
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<const uint8_t, 48> data) {
return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
units::meter_t{wpi::UnpackStruct<double, 8>(data)},
units::meter_t{wpi::UnpackStruct<double, 16>(data)},
units::radian_t{wpi::UnpackStruct<double, 24>(data)},
units::radian_t{wpi::UnpackStruct<double, 32>(data)},
units::radian_t{wpi::UnpackStruct<double, 40>(data)}};
}
static void Pack(std::span<uint8_t, 48> 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<frc::Twist3d> {
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"

17
wpimath/src/main/native/include/frc/geometry/proto/Pose2dProto.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/protobuf/Protobuf.h>
#include "frc/geometry/Pose2d.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Pose2d> {
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);
};

17
wpimath/src/main/native/include/frc/geometry/proto/Pose3dProto.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/protobuf/Protobuf.h>
#include "frc/geometry/Pose3d.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Pose3d> {
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);
};

18
wpimath/src/main/native/include/frc/geometry/proto/QuaternionProto.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/protobuf/Protobuf.h>
#include "frc/geometry/Quaternion.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::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);
};

18
wpimath/src/main/native/include/frc/geometry/proto/Rotation2dProto.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/protobuf/Protobuf.h>
#include "frc/geometry/Rotation2d.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Rotation2d> {
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);
};

18
wpimath/src/main/native/include/frc/geometry/proto/Rotation3dProto.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/protobuf/Protobuf.h>
#include "frc/geometry/Rotation3d.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Rotation3d> {
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);
};

18
wpimath/src/main/native/include/frc/geometry/proto/Transform2dProto.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/protobuf/Protobuf.h>
#include "frc/geometry/Transform2d.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Transform2d> {
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);
};

18
wpimath/src/main/native/include/frc/geometry/proto/Transform3dProto.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/protobuf/Protobuf.h>
#include "frc/geometry/Transform3d.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Transform3d> {
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);
};

18
wpimath/src/main/native/include/frc/geometry/proto/Translation2dProto.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/protobuf/Protobuf.h>
#include "frc/geometry/Translation2d.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Translation2d> {
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);
};

18
wpimath/src/main/native/include/frc/geometry/proto/Translation3dProto.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/protobuf/Protobuf.h>
#include "frc/geometry/Translation3d.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Translation3d> {
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);
};

17
wpimath/src/main/native/include/frc/geometry/proto/Twist2dProto.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/protobuf/Protobuf.h>
#include "frc/geometry/Twist2d.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Twist2d> {
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);
};

17
wpimath/src/main/native/include/frc/geometry/proto/Twist3dProto.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/protobuf/Protobuf.h>
#include "frc/geometry/Twist3d.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::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);
};

26
wpimath/src/main/native/include/frc/geometry/struct/Pose2dStruct.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/struct/Struct.h>
#include "frc/geometry/Pose2d.h"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Pose2d> {
static constexpr std::string_view kTypeString = "struct:Pose2d";
static constexpr size_t kSize = wpi::Struct<frc::Translation2d>::kSize +
wpi::Struct<frc::Rotation2d>::kSize;
static constexpr std::string_view kSchema =
"Translation2d translation;Rotation2d rotation";
static frc::Pose2d Unpack(std::span<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data, const frc::Pose2d& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn);
};
static_assert(wpi::HasNestedStruct<frc::Pose2d>);

26
wpimath/src/main/native/include/frc/geometry/struct/Pose3dStruct.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/struct/Struct.h>
#include "frc/geometry/Pose3d.h"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Pose3d> {
static constexpr std::string_view kTypeString = "struct:Pose3d";
static constexpr size_t kSize = wpi::Struct<frc::Translation3d>::kSize +
wpi::Struct<frc::Rotation3d>::kSize;
static constexpr std::string_view kSchema =
"Translation3d translation;Rotation3d rotation";
static frc::Pose3d Unpack(std::span<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data, const frc::Pose3d& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn);
};
static_assert(wpi::HasNestedStruct<frc::Pose3d>);

22
wpimath/src/main/native/include/frc/geometry/struct/QuaternionStruct.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/struct/Struct.h>
#include "frc/geometry/Quaternion.h"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Quaternion> {
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<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data,
const frc::Quaternion& value);
};

21
wpimath/src/main/native/include/frc/geometry/struct/Rotation2dStruct.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/struct/Struct.h>
#include "frc/geometry/Rotation2d.h"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Rotation2d> {
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<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data,
const frc::Rotation2d& value);
};

25
wpimath/src/main/native/include/frc/geometry/struct/Rotation3dStruct.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/struct/Struct.h>
#include "frc/geometry/Rotation3d.h"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Rotation3d> {
static constexpr std::string_view kTypeString = "struct:Rotation3d";
static constexpr size_t kSize = wpi::Struct<frc::Quaternion>::kSize;
static constexpr std::string_view kSchema = "Quaternion q";
static frc::Rotation3d Unpack(std::span<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data,
const frc::Rotation3d& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn);
};
static_assert(wpi::HasNestedStruct<frc::Rotation3d>);

27
wpimath/src/main/native/include/frc/geometry/struct/Transform2dStruct.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/struct/Struct.h>
#include "frc/geometry/Transform2d.h"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Transform2d> {
static constexpr std::string_view kTypeString = "struct:Transform2d";
static constexpr size_t kSize = wpi::Struct<frc::Translation2d>::kSize +
wpi::Struct<frc::Rotation2d>::kSize;
static constexpr std::string_view kSchema =
"Translation2d translation;Rotation2d rotation";
static frc::Transform2d Unpack(std::span<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data,
const frc::Transform2d& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn);
};
static_assert(wpi::HasNestedStruct<frc::Transform2d>);

27
wpimath/src/main/native/include/frc/geometry/struct/Transform3dStruct.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/struct/Struct.h>
#include "frc/geometry/Transform3d.h"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Transform3d> {
static constexpr std::string_view kTypeString = "struct:Transform3d";
static constexpr size_t kSize = wpi::Struct<frc::Translation3d>::kSize +
wpi::Struct<frc::Rotation3d>::kSize;
static constexpr std::string_view kSchema =
"Translation3d translation;Rotation3d rotation";
static frc::Transform3d Unpack(std::span<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data,
const frc::Transform3d& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn);
};
static_assert(wpi::HasNestedStruct<frc::Transform3d>);

21
wpimath/src/main/native/include/frc/geometry/struct/Translation2dStruct.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/struct/Struct.h>
#include "frc/geometry/Translation2d.h"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Translation2d> {
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<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data,
const frc::Translation2d& value);
};

21
wpimath/src/main/native/include/frc/geometry/struct/Translation3dStruct.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/struct/Struct.h>
#include "frc/geometry/Translation3d.h"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Translation3d> {
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<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data,
const frc::Translation3d& value);
};

21
wpimath/src/main/native/include/frc/geometry/struct/Twist2dStruct.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/struct/Struct.h>
#include "frc/geometry/Twist2d.h"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Twist2d> {
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<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data, const frc::Twist2d& value);
};

21
wpimath/src/main/native/include/frc/geometry/struct/Twist3dStruct.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/struct/Struct.h>
#include "frc/geometry/Twist3d.h"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Twist3d> {
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<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data, const frc::Twist3d& value);
};

28
wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Pose2dProtoTest.java Normal file
View File

@@ -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());
}
}

31
wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Pose3dProtoTest.java Normal file
View File

@@ -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());
}
}

27
wpimath/src/test/java/edu/wpi/first/math/geometry/proto/QuaternionProtoTest.java Normal file
View File

@@ -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());
}
}

Some files were not shown because too many files have changed in this diff Show More