Add a TON of math

src/ntcore/INtSendable.cs

@@ -6,8 +6,10 @@ public interface INtSendable : ISendable
 {
     void InitSendable(INtSendableBuilder builder);
 
-    void ISendable.InitSendable(ISendableBuilder builder) {
-        if (builder.BackendKind == ISendableBuilder.BackingKind.NetworkTables) {
+    void ISendable.InitSendable(ISendableBuilder builder)
+    {
+        if (builder.BackendKind == ISendableBuilder.BackingKind.NetworkTables)
+        {
             InitSendable((INtSendableBuilder)builder);
         }
     }

src/wpimath/Geometry/Pose2d.cs

@@ -4,6 +4,7 @@
 using System.Text.Json.Serialization;
 using Google.Protobuf.Reflection;
 using UnitsNet;
+using UnitsNet.NumberExtensions.NumberToAngle;
 using WPIMath.Interpolation;
 using WPIMath.Proto;
 using WPIUtil.Serialization.Protobuf;
@@ -19,14 +20,14 @@ public class Pose2dProto : IProtobuf<Pose2d, ProtobufPose2d>
 
     public void Pack(ProtobufPose2d msg, Pose2d value)
     {
-        Translation2d.Proto.Pack(msg, value.Translation);
-        Rotation2d.Proto.Pack(msg, value.Rotation);
+        Translation2d.Proto.Pack(msg.Translation, value.Translation);
+        Rotation2d.Proto.Pack(msg.Rotation, value.Rotation);
     }
 
     public Pose2d Unpack(ProtobufPose2d msg)
     {
-        Translation2d translation = Translation2d.Proto.Unpack(msg);
-        Rotation2d rotation = Rotation2d.Proto.Unpack(msg);
+        Translation2d translation = Translation2d.Proto.Unpack(msg.Translation);
+        Rotation2d rotation = Rotation2d.Proto.Unpack(msg.Rotation);
         return new(translation, rotation);
     }
 }
@@ -61,21 +62,23 @@ public partial class Pose2dJsonContext : JsonSerializerContext
 
 public readonly struct Pose2d : IStructSerializable<Pose2d>, IProtobufSerializable<Pose2d>,
                                     IMultiplyOperators<Pose2d, double, Pose2d>,
+                                    IAdditionOperators<Pose2d, Transform2d, Pose2d>,
+                                    ISubtractionOperators<Pose2d, Pose2d, Transform2d>,
                                     IDivisionOperators<Pose2d, double, Pose2d>,
     IEqualityOperators<Pose2d, Pose2d, bool>,
         IInterpolatable<Pose2d>,
                                     IEquatable<Pose2d>
 {
     public static IStruct<Pose2d> Struct { get; } = new Pose2dStruct();
     public static IProtobuf<Pose2d, ProtobufPose2d> Proto { get; } = new Pose2dProto();
-    static IProtobuf<Pose2d> IProtobufSerializable<Pose2d>.Proto => Proto;
+    static IProtobuf<Pose2d> IProtobufSerializable<Pose2d>.Proto { get; } = Proto.UntypedProto;
 
     [JsonInclude]
     [JsonPropertyName("translation")]
-    public Translation2d Translation { get; }
+    public Translation2d Translation { get; init; }
     [JsonInclude]
     [JsonPropertyName("rotation")]
-    public Rotation2d Rotation { get; }
+    public Rotation2d Rotation { get; init; }
 
     [JsonIgnore]
     public Length X => Translation.X;
@@ -96,14 +99,76 @@ public Pose2d(Length x, Length y, Rotation2d rotation)
         Rotation = rotation;
     }
 
+    public Pose2d TransformBy(Transform2d other)
+    {
+        return new Pose2d(Translation + other.Translation.RotateBy(Rotation), other.Rotation + Rotation);
+    }
+
     public Pose2d RotateBy(Rotation2d other)
     {
-        throw new NotImplementedException();
+        return new Pose2d(Translation.RotateBy(other), Rotation.RotateBy(other));
     }
 
     public Pose2d RelativeTo(Pose2d other)
     {
-        throw new NotImplementedException();
+        var transform = new Transform2d(other, this);
+        return new Pose2d(transform.Translation, transform.Rotation);
+    }
+
+    public Pose2d Exp(Twist2d twist)
+    {
+        double dx = twist.Dx.Meters;
+        double dy = twist.Dy.Meters;
+        double dtheta = twist.Dtheta.Radians;
+
+        double sinTheta = Math.Sin(dtheta);
+        double cosTheta = Math.Cos(dtheta);
+
+        double s;
+        double c;
+        if (Math.Abs(dtheta) < 1E-9)
+        {
+            s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
+            c = 0.5 * dtheta;
+        }
+        else
+        {
+            s = sinTheta / dtheta;
+            c = (1 - cosTheta) / dtheta;
+        }
+        var transform =
+            new Transform2d(
+                new Translation2d(dx * s - dy * c, dx * c + dy * s),
+                new Rotation2d(cosTheta, sinTheta));
+
+        return this + transform;
+    }
+
+    public Twist2d Log(Pose2d end)
+    {
+        var transform = end.RelativeTo(this);
+        var dtheta = transform.Rotation.Angle.Radians;
+        var halfDtheta = dtheta / 2.0;
+
+        var cosMinusOne = transform.Rotation.Cos - 1;
+
+        double halfThetaByTanOfHalfDtheta;
+        if (Math.Abs(cosMinusOne) < 1E-9)
+        {
+            halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
+        }
+        else
+        {
+            halfThetaByTanOfHalfDtheta = -(halfDtheta * transform.Rotation.Sin) / cosMinusOne;
+        }
+
+        Translation2d translationPart =
+            transform
+                .Translation
+                .RotateBy(new Rotation2d(halfThetaByTanOfHalfDtheta, -halfDtheta))
+                 * double.Hypot(halfThetaByTanOfHalfDtheta, halfDtheta);
+
+        return new Twist2d(translationPart.X, translationPart.Y, dtheta.Radians());
     }
 
     public Pose2d Nearest(ReadOnlySpan<Pose2d> poses)
@@ -113,32 +178,56 @@ public Pose2d Nearest(ReadOnlySpan<Pose2d> poses)
 
     public static bool operator ==(Pose2d left, Pose2d right)
     {
-        throw new NotImplementedException();
+        return left.Equals(right);
     }
 
     public static bool operator !=(Pose2d left, Pose2d right)
     {
-        throw new NotImplementedException();
+        return !(left == right);
     }
 
     public static Pose2d operator *(Pose2d left, double right)
     {
-        throw new NotImplementedException();
+        return new Pose2d(left.Translation * right, left.Rotation * right);
     }
 
     public static Pose2d operator /(Pose2d left, double right)
     {
-        throw new NotImplementedException();
+        return left * (1.0 / right);
+    }
+
+    public static Pose2d operator +(Pose2d left, Transform2d right)
+    {
+        return left.TransformBy(right);
+    }
+
+    public static Transform2d operator -(Pose2d left, Pose2d right)
+    {
+        var pose = left.RelativeTo(right);
+        return new(pose.Translation, pose.Rotation);
     }
 
     public Pose2d Interpolate(Pose2d endValue, double t)
     {
-        throw new NotImplementedException();
+        if (t < 0)
+        {
+            return this;
+        }
+        else if (t >= 1)
+        {
+            return endValue;
+        }
+        else
+        {
+            var twist = Log(endValue);
+            var scaledTwist = new Twist2d(twist.Dx * t, twist.Dy * t, twist.Dtheta * t);
+            return Exp(scaledTwist);
+        }
     }
 
     public bool Equals(Pose2d other)
     {
-        throw new NotImplementedException();
+        return Translation == other.Translation && Rotation == other.Rotation;
     }
 
     public override bool Equals(object? obj)