diff --git a/src/ntcore/INtSendable.cs b/src/ntcore/INtSendable.cs
index e0bb6187..67fba910 100644
--- a/src/ntcore/INtSendable.cs
+++ b/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);
         }
     }
diff --git a/src/wpimath/Geometry/Pose2d.cs b/src/wpimath/Geometry/Pose2d.cs
index fadc475b..4fe3d76f 100644
--- a/src/wpimath/Geometry/Pose2d.cs
+++ b/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,6 +62,8 @@ 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>,
@@ -68,14 +71,14 @@ public partial class Pose2dJsonContext : JsonSerializerContext
 {
     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)
diff --git a/src/wpimath/Geometry/Quaternion.cs b/src/wpimath/Geometry/Quaternion.cs
new file mode 100644
index 00000000..937f88fc
--- /dev/null
+++ b/src/wpimath/Geometry/Quaternion.cs
@@ -0,0 +1,285 @@
+using System;
+using System.Numerics;
+using System.Text.Json.Serialization;
+using Google.Protobuf.Reflection;
+using WPIMath.Proto;
+using WPIUtil.Serialization.Protobuf;
+using WPIUtil.Serialization.Struct;
+
+namespace WPIMath;
+
+public class QuaternionProto : IProtobuf<Quaternion, ProtobufQuaternion>
+{
+    public MessageDescriptor Descriptor => ProtobufQuaternion.Descriptor;
+
+    public ProtobufQuaternion CreateMessage()
+    {
+        return new ProtobufQuaternion();
+    }
+
+    public void Pack(ProtobufQuaternion msg, Quaternion value)
+    {
+        msg.W = value.W;
+        msg.X = value.X;
+        msg.Y = value.Y;
+        msg.Z = value.Z;
+    }
+
+    public Quaternion Unpack(ProtobufQuaternion msg)
+    {
+        double w = msg.W;
+        double x = msg.X;
+        double y = msg.Y;
+        double z = msg.Z;
+        return new Quaternion(w, x, y, z);
+    }
+}
+
+public class QuaternionStruct : IStruct<Quaternion>
+{
+    public string TypeString => "struct:Quaternion";
+
+    public int Size => IStructBase.SizeDouble * 4;
+
+    public string Schema => "double w;double x;double y;double z";
+
+    public void Pack(ref StructPacker buffer, Quaternion value)
+    {
+        buffer.WriteDouble(value.W);
+        buffer.WriteDouble(value.X);
+        buffer.WriteDouble(value.Y);
+        buffer.WriteDouble(value.Z);
+    }
+
+    public Quaternion Unpack(ref StructUnpacker buffer)
+    {
+        double w = buffer.ReadDouble();
+        double x = buffer.ReadDouble();
+        double y = buffer.ReadDouble();
+        double z = buffer.ReadDouble();
+        return new Quaternion(w, x, y, z);
+    }
+}
+
+[JsonSerializable(typeof(Quaternion))]
+public partial class QuaternionJsonContext : JsonSerializerContext
+{
+}
+
+public readonly struct Quaternion : IEquatable<Quaternion>, IEqualityOperators<Quaternion, Quaternion, bool>,
+                        IAdditionOperators<Quaternion, Quaternion, Quaternion>,
+                        ISubtractionOperators<Quaternion, Quaternion, Quaternion>,
+                        IMultiplyOperators<Quaternion, double, Quaternion>,
+                        IMultiplyOperators<Quaternion, Quaternion, Quaternion>,
+                        IDivisionOperators<Quaternion, double, Quaternion>,
+                        IStructSerializable<Quaternion>,
+                        IProtobufSerializable<Quaternion, ProtobufQuaternion>
+
+{
+    public static IStruct<Quaternion> Struct { get; } = new QuaternionStruct();
+    public static IProtobuf<Quaternion, ProtobufQuaternion> Proto { get; } = new QuaternionProto();
+    static IProtobuf<Quaternion> IProtobufSerializable<Quaternion>.Proto => Proto.UntypedProto;
+
+    [JsonInclude]
+    public double W { get; init; }
+    [JsonInclude]
+    public double X { get; init; }
+    [JsonInclude]
+    public double Y { get; init; }
+    [JsonInclude]
+    public double Z { get; init; }
+
+    public Quaternion()
+    {
+        W = 1.0;
+        X = 0.0;
+        Y = 0.0;
+        Z = 0.0;
+    }
+
+    [JsonConstructor]
+    public Quaternion(double w, double x, double y, double z)
+    {
+        W = w;
+        X = x;
+        Y = y;
+        Z = z;
+    }
+
+    public bool Equals(Quaternion other)
+    {
+        var thisNorm = Norm();
+        var otherNorm = other.Norm();
+        return Math.Abs(Dot(other) - thisNorm * otherNorm) < 1e-9 && Math.Abs(thisNorm - otherNorm) < 1e-9;
+    }
+
+    public static bool operator ==(Quaternion left, Quaternion right)
+    {
+        return left.Equals(right);
+    }
+
+    public static bool operator !=(Quaternion left, Quaternion right)
+    {
+        return !(left == right);
+    }
+
+    public static Quaternion operator +(Quaternion left, Quaternion right)
+    {
+        return new(left.W + right.W, left.X + right.X, left.Y + right.Y, left.Z + right.Z);
+    }
+
+    public static Quaternion operator -(Quaternion left, Quaternion right)
+    {
+        return new(left.W - right.W, left.X - right.X, left.Y - right.Y, left.Z - right.Z);
+    }
+
+    public static Quaternion operator /(Quaternion left, double right)
+    {
+        return new(left.W / right, left.X / right, left.Y / right, left.Z / right);
+    }
+
+    public static Quaternion operator *(Quaternion left, double right)
+    {
+        return new(left.W * right, left.X * right, left.Y * right, left.Z * right);
+    }
+
+    public static Quaternion operator *(Quaternion left, Quaternion right)
+    {
+        // https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
+        var r1 = left.W;
+        var r2 = right.W;
+
+        // v₁ ⋅ v₂
+        double dot = left.X * right.X + left.Y * right.Y + left.Z * right.Z;
+
+        // v₁ x v₂
+        double cross_x = left.Y * right.Z - right.Y * left.Z;
+        double cross_y = right.X * left.Z - left.X * right.Z;
+        double cross_z = left.X * right.Y - right.X * left.Y;
+
+        return new(
+            // r = r₁r₂ − v₁ ⋅ v₂
+            r1 * r2 - dot,
+            // v = r₁v₂ + r₂v₁ + v₁ x v₂
+            r1 * right.X + r2 * left.X + cross_x,
+            r1 * right.Y + r2 * left.Y + cross_y,
+            r1 * right.Z + r2 * left.Z + cross_z);
+    }
+
+    public Quaternion Conjugate()
+    {
+        return new(W, -X, -Y, -Z);
+    }
+
+    public double Dot(Quaternion other)
+    {
+        return W * other.W + X * other.X + Y * other.Y + Z * other.Z;
+    }
+
+    public Quaternion Inverse()
+    {
+        var norm = Norm();
+        return Conjugate() / (norm * norm);
+    }
+
+    public double Norm()
+    {
+        return Math.Sqrt(Dot(this));
+    }
+
+    public Quaternion Normalize()
+    {
+        double norm = Norm();
+        if (norm == 0.0)
+        {
+            return new();
+        }
+        else
+        {
+            return this / norm;
+        }
+    }
+
+    public Quaternion Pow(double t)
+    {
+        // q^t = e^(ln(q^t)) = e^(t * ln(q))
+        return (this.Log() * t).Exp();
+    }
+
+    public Quaternion Exp(Quaternion adjustment)
+    {
+        return adjustment.Exp() * this;
+    }
+
+    public Quaternion Exp()
+    {
+        var scalar = Math.Exp(W);
+
+        var axialMagnitude = Math.Sqrt(X * X + Y * Y + Z * Z);
+        var cosine = Math.Cos(axialMagnitude);
+
+        double axialScalar;
+
+        if (axialMagnitude < 1e-9)
+        {
+            // Taylor series of sin(θ) / θ near θ = 0: 1 − θ²/6 + θ⁴/120 + O(n⁶)
+            var axialMagnitudeSq = axialMagnitude * axialMagnitude;
+            var axialMagnitudeSqSq = axialMagnitudeSq * axialMagnitudeSq;
+            axialScalar = 1.0 - axialMagnitudeSq / 6.0 + axialMagnitudeSqSq / 120.0;
+        }
+        else
+        {
+            axialScalar = Math.Sin(axialMagnitude) / axialMagnitude;
+        }
+
+        return new(
+            cosine * scalar,
+            X * axialScalar * scalar,
+            Y * axialScalar * scalar,
+            Z * axialScalar * scalar);
+    }
+
+    public Quaternion Log(Quaternion end)
+    {
+        return (end * Inverse()).Log();
+    }
+
+    public Quaternion Log()
+    {
+        var norm = Norm();
+        var scalar = Math.Log(norm);
+
+        var vNorm = Math.Sqrt(X * X + Y * Y + Z * Z);
+
+        var sNorm = W / norm;
+
+        if (Math.Abs(sNorm + 1) < 1e-9)
+        {
+            return new Quaternion(scalar, -Math.PI, 0, 0);
+        }
+
+        double vScalar;
+
+        if (vNorm < 1e-9)
+        {
+            // Taylor series expansion of atan2(y / x) / y around y = 0 => 1/x - y²/3*x³ + O(y⁴)
+            vScalar = 1.0 / W - 1.0 / 3.0 * vNorm * vNorm / (W * W * W);
+        }
+        else
+        {
+            vScalar = Math.Atan2(vNorm, W) / vNorm;
+        }
+
+        return new Quaternion(scalar, vScalar * X, vScalar * Y, vScalar * Z);
+    }
+
+    public override bool Equals(object? obj)
+    {
+        return obj is Quaternion quaternion && Equals(quaternion);
+    }
+
+    public override int GetHashCode()
+    {
+        return HashCode.Combine(W, X, Y, Z);
+    }
+}
diff --git a/src/wpimath/Geometry/Rotation2d.cs b/src/wpimath/Geometry/Rotation2d.cs
index 99c018d5..5aadf3e1 100644
--- a/src/wpimath/Geometry/Rotation2d.cs
+++ b/src/wpimath/Geometry/Rotation2d.cs
@@ -69,7 +69,7 @@ public partial class Rotation2dJsonContext : JsonSerializerContext
 {
     public static IStruct<Rotation2d> Struct { get; } = new Rotation2dStruct();
     public static IProtobuf<Rotation2d, ProtobufRotation2d> Proto { get; } = new Rotation2dProto();
-    static IProtobuf<Rotation2d> IProtobufSerializable<Rotation2d>.Proto => Proto;
+    static IProtobuf<Rotation2d> IProtobufSerializable<Rotation2d>.Proto => Proto.UntypedProto;
 
     public Rotation2d(Angle angle)
     {
diff --git a/src/wpimath/Geometry/Rotation3d.cs b/src/wpimath/Geometry/Rotation3d.cs
new file mode 100644
index 00000000..0c7a284f
--- /dev/null
+++ b/src/wpimath/Geometry/Rotation3d.cs
@@ -0,0 +1,268 @@
+using System;
+using System.Numerics;
+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;
+using WPIUtil.Serialization.Struct;
+
+namespace WPIMath.Geometry;
+
+public class Rotation3dProto : IProtobuf<Rotation3d, ProtobufRotation3d>
+{
+    public MessageDescriptor Descriptor => ProtobufRotation3d.Descriptor;
+
+    public ProtobufRotation3d CreateMessage()
+    {
+        return new ProtobufRotation3d();
+    }
+
+    public void Pack(ProtobufRotation3d msg, Rotation3d value)
+    {
+        Quaternion.Proto.Pack(msg.Q, value.Quaternion);
+    }
+
+    public Rotation3d Unpack(ProtobufRotation3d msg)
+    {
+        Quaternion q = Quaternion.Proto.Unpack(msg.Q);
+        return new(q);
+    }
+}
+
+public class Rotation3dStruct : IStruct<Rotation3d>
+{
+    public string TypeString => "struct:Rotation3d";
+
+    public int Size => IStructBase.SizeDouble;
+
+    public string Schema => "Quaternion q";
+
+    public IStructBase[] Nested { get; } = [Quaternion.Struct];
+
+    public void Pack(ref StructPacker buffer, Rotation3d value)
+    {
+        Quaternion.Struct.Pack(ref buffer, value.Quaternion);
+    }
+
+    public Rotation3d Unpack(ref StructUnpacker buffer)
+    {
+        Quaternion q = Quaternion.Struct.Unpack(ref buffer);
+        return new Rotation3d(q);
+    }
+}
+
+[JsonSerializable(typeof(Rotation3d))]
+public partial class Rotation3dJsonContext : JsonSerializerContext
+{
+}
+
+public readonly struct Rotation3d : IStructSerializable<Rotation3d>,
+                                    IProtobufSerializable<Rotation3d, ProtobufRotation3d>,
+                                    IAdditionOperators<Rotation3d, Rotation3d, Rotation3d>,
+                                    ISubtractionOperators<Rotation3d, Rotation3d, Rotation3d>,
+                                    IUnaryNegationOperators<Rotation3d, Rotation3d>,
+                                    IMultiplyOperators<Rotation3d, double, Rotation3d>,
+                                    IDivisionOperators<Rotation3d, double, Rotation3d>,
+                                    IEqualityOperators<Rotation3d, Rotation3d, bool>,
+                                    IAdditiveIdentity<Rotation3d, Rotation3d>,
+                                    IInterpolatable<Rotation3d>,
+                                    IEquatable<Rotation3d>
+{
+    public static IStruct<Rotation3d> Struct { get; } = new Rotation3dStruct();
+    public static IProtobuf<Rotation3d, ProtobufRotation3d> Proto { get; } = new Rotation3dProto();
+    static IProtobuf<Rotation3d> IProtobufSerializable<Rotation3d>.Proto => Proto.UntypedProto;
+
+    [JsonInclude]
+    [JsonPropertyName("quaternion")]
+    public Quaternion Quaternion { get; init; }
+
+    [JsonConstructor]
+    public Rotation3d(Quaternion quaternion)
+    {
+        Quaternion = quaternion;
+    }
+
+    public Rotation3d(Angle roll, Angle pitch, Angle yaw)
+    {
+        // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion
+        double rollRadians = roll.Radians;
+        double pitchRadians = pitch.Radians;
+        double yawRadians = yaw.Radians;
+
+        double cr = Math.Cos(rollRadians * 0.5);
+        double sr = Math.Sin(rollRadians * 0.5);
+
+        double cp = Math.Cos(pitchRadians * 0.5);
+        double sp = Math.Sin(pitchRadians * 0.5);
+
+        double cy = Math.Cos(yawRadians * 0.5);
+        double sy = Math.Sin(yawRadians * 0.5);
+
+        Quaternion =
+            new Quaternion(
+                cr * cp * cy + sr * sp * sy,
+                sr * cp * cy - cr * sp * sy,
+                cr * sp * cy + sr * cp * sy,
+                cr * cp * sy - sr * sp * cy);
+    }
+
+    [JsonIgnore]
+    public double X
+    {
+        get
+        {
+            var w = Quaternion.W;
+            var x = Quaternion.X;
+            var y = Quaternion.Y;
+            var z = Quaternion.Z;
+
+            // wpimath/algorithms.md
+            var cxcy = 1.0 - 2.0 * (x * x + y * y);
+            var sxcy = 2.0 * (w * x + y * z);
+            var cy_sq = cxcy * cxcy + sxcy * sxcy;
+            if (cy_sq > 1e-20)
+            {
+                return Math.Atan2(sxcy, cxcy);
+            }
+            else
+            {
+                return 0.0;
+            }
+        }
+    }
+
+    [JsonIgnore]
+    public double Y
+    {
+        get
+        {
+            var w = Quaternion.W;
+            var x = Quaternion.X;
+            var y = Quaternion.Y;
+            var z = Quaternion.Z;
+
+            // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_(in_3-2-1_sequence)_conversion
+            double ratio = 2.0 * (w * y - z * x);
+            if (Math.Abs(ratio) >= 1.0)
+            {
+                return Math.CopySign(Math.PI / 2.0, ratio);
+            }
+            else
+            {
+                return Math.Asin(ratio);
+            }
+        }
+    }
+
+    [JsonIgnore]
+    public double Z
+    {
+        get
+        {
+            var w = Quaternion.W;
+            var x = Quaternion.X;
+            var y = Quaternion.Y;
+            var z = Quaternion.Z;
+
+            // wpimath/algorithms.md
+            var cycz = 1.0 - 2.0 * (y * y + z * z);
+            var cysz = 2.0 * (w * z + x * y);
+            var cy_sq = cycz * cycz + cysz * cysz;
+            if (cy_sq > 1e-20)
+            {
+                return Math.Atan2(cysz, cycz);
+            }
+            else
+            {
+                return Math.Atan2(2.0 * w * z, w * w - z * z);
+            }
+        }
+    }
+
+    [JsonIgnore]
+    public Angle Angle
+    {
+        get
+        {
+            var w = Quaternion.W;
+            var x = Quaternion.X;
+            var y = Quaternion.Y;
+            var z = Quaternion.Z;
+
+            double norm =
+                Math.Sqrt(x * x + y * y + z * z);
+            return (2.0 * Math.Atan2(norm, w)).Radians();
+        }
+    }
+
+    public Rotation2d ToRotation2d()
+    {
+        return new Rotation2d(Z);
+    }
+
+    public static Rotation3d AdditiveIdentity => new();
+
+    public readonly Rotation3d RotateBy(Rotation3d other)
+    {
+        return new(other.Quaternion * Quaternion);
+    }
+
+    public static Rotation3d operator +(Rotation3d left, Rotation3d right)
+    {
+        return left.RotateBy(right);
+    }
+
+    public static Rotation3d operator -(Rotation3d left, Rotation3d right)
+    {
+        return left + -right;
+    }
+
+    public static Rotation3d operator -(Rotation3d value)
+    {
+        return new(value.Quaternion.Inverse());
+    }
+
+    public static Rotation3d operator *(Rotation3d left, double right)
+    {
+        throw new NotImplementedException();
+    }
+
+    public static Rotation3d operator /(Rotation3d left, double right)
+    {
+        return left * (1.0 / right);
+    }
+
+    public static bool operator ==(Rotation3d left, Rotation3d right)
+    {
+        return Math.Abs(Math.Abs(left.Quaternion.Dot(right.Quaternion)) - left.Quaternion.Norm() * right.Quaternion.Norm()) < 1e-9;
+    }
+
+    public static bool operator !=(Rotation3d left, Rotation3d right)
+    {
+        return !(left == right);
+    }
+
+    public override bool Equals(object? obj)
+    {
+        return obj is Rotation3d rot && Equals(rot);
+    }
+
+    public override int GetHashCode()
+    {
+        return Quaternion.GetHashCode();
+    }
+
+    public bool Equals(Rotation3d other)
+    {
+        return this == other;
+    }
+
+    public Rotation3d Interpolate(Rotation3d endValue, double t)
+    {
+        return MathExtras.Lerp(this, endValue, t);
+    }
+
+}
diff --git a/src/wpimath/Geometry/Transform2d.cs b/src/wpimath/Geometry/Transform2d.cs
new file mode 100644
index 00000000..84d1c9e6
--- /dev/null
+++ b/src/wpimath/Geometry/Transform2d.cs
@@ -0,0 +1,158 @@
+using System;
+using System.Numerics;
+using System.Text.Json.Serialization;
+using Google.Protobuf.Reflection;
+using UnitsNet;
+using UnitsNet.NumberExtensions.NumberToAngle;
+using UnitsNet.NumberExtensions.NumberToLength;
+using WPIMath.Proto;
+using WPIUtil.Serialization.Protobuf;
+using WPIUtil.Serialization.Struct;
+
+namespace WPIMath.Geometry;
+
+public class Transform2dProto : IProtobuf<Transform2d, ProtobufTransform2d>
+{
+    public MessageDescriptor Descriptor => ProtobufTransform2d.Descriptor;
+
+    public ProtobufTransform2d CreateMessage()
+    {
+        return new ProtobufTransform2d();
+    }
+
+    public void Pack(ProtobufTransform2d msg, Transform2d value)
+    {
+        Translation2d.Proto.Pack(msg.Translation, value.Translation);
+        Rotation2d.Proto.Pack(msg.Rotation, value.Rotation);
+    }
+
+    public Transform2d Unpack(ProtobufTransform2d msg)
+    {
+        var translation = Translation2d.Proto.Unpack(msg.Translation);
+        var rotation = Rotation2d.Proto.Unpack(msg.Rotation);
+        return new Transform2d(translation, rotation);
+    }
+}
+
+public class Transform2dStruct : IStruct<Transform2d>
+{
+    public string TypeString => "struct:Transform2d";
+
+    public int Size => Translation2d.Struct.Size + Rotation2d.Struct.Size;
+
+    public IStructBase[] Nested { get; } = [Translation2d.Struct, Rotation2d.Struct];
+
+    public string Schema => "Translation2d translation;Rotation2d rotation";
+
+    public void Pack(ref StructPacker buffer, Transform2d value)
+    {
+        Translation2d.Struct.Pack(ref buffer, value.Translation);
+        Rotation2d.Struct.Pack(ref buffer, value.Rotation);
+    }
+
+    public Transform2d Unpack(ref StructUnpacker buffer)
+    {
+        var translation = Translation2d.Struct.Unpack(ref buffer);
+        var rotation = Rotation2d.Struct.Unpack(ref buffer);
+        return new Transform2d(translation, rotation);
+    }
+}
+
+[JsonSerializable(typeof(Transform2d))]
+public partial class Transform2dJsonContext : JsonSerializerContext
+{
+}
+
+public readonly struct Transform2d : IAdditionOperators<Transform2d, Transform2d, Transform2d>,
+                                    IMultiplyOperators<Transform2d, double, Transform2d>,
+                                    IDivisionOperators<Transform2d, double, Transform2d>,
+                                    IEqualityOperators<Transform2d, Transform2d, bool>,
+                                    IAdditiveIdentity<Transform2d, Transform2d>,
+                                    IEquatable<Transform2d>
+{
+    [JsonInclude]
+    [JsonPropertyName("translation")]
+    public Translation2d Translation { get; init; }
+    [JsonInclude]
+    [JsonPropertyName("rotation")]
+    public Rotation2d Rotation { get; init; }
+
+    public static Transform2d AdditiveIdentity => new();
+
+    [JsonConstructor]
+    public Transform2d(Translation2d translation, Rotation2d rotation)
+    {
+        Translation = translation;
+        Rotation = rotation;
+    }
+
+    public Transform2d(Pose2d initial, Pose2d last)
+    {
+        // We are rotating the difference between the translations
+        // using a clockwise rotation matrix. This transforms the global
+        // delta into a local delta (relative to the initial pose).
+        Translation = (last.Translation - initial.Translation).RotateBy(-initial.Rotation);
+
+        Rotation = last.Rotation - initial.Rotation;
+    }
+
+    public Transform2d(Length x, Length y, Rotation2d rotation)
+    {
+        Translation = new Translation2d(x, y);
+        Rotation = rotation;
+    }
+
+    [JsonIgnore]
+    public Length X => Translation.X;
+    [JsonIgnore]
+    public Length Y => Translation.Y;
+
+    public Transform2d Inverse()
+    {
+        // We are rotating the difference between the translations
+        // using a clockwise rotation matrix. This transforms the global
+        // delta into a local delta (relative to the initial pose).
+        var negRotation = Rotation;
+        return new((-Translation).RotateBy(negRotation), negRotation);
+    }
+
+    public static Transform2d operator +(Transform2d left, Transform2d right)
+    {
+        return new(new Pose2d(), new Pose2d().TransformBy(left).TransformBy(right));
+    }
+
+    public static Transform2d operator *(Transform2d left, double right)
+    {
+        return new(left.Translation * right, left.Rotation * right);
+    }
+
+    public static Transform2d operator /(Transform2d left, double right)
+    {
+        return left * (1.0 / right);
+    }
+
+    public static bool operator ==(Transform2d left, Transform2d right)
+    {
+        return left.Equals(right);
+    }
+
+    public static bool operator !=(Transform2d left, Transform2d right)
+    {
+        return !(left == right);
+    }
+
+    public bool Equals(Transform2d other)
+    {
+        return Translation == other.Translation && Rotation == other.Rotation;
+    }
+
+    public override bool Equals(object? obj)
+    {
+        return obj is Transform2d d && Equals(d);
+    }
+
+    public override int GetHashCode()
+    {
+        return HashCode.Combine(Translation, Rotation);
+    }
+}
diff --git a/src/wpimath/Geometry/Translation2d.cs b/src/wpimath/Geometry/Translation2d.cs
index 4848a1d5..49f2b492 100644
--- a/src/wpimath/Geometry/Translation2d.cs
+++ b/src/wpimath/Geometry/Translation2d.cs
@@ -72,7 +72,7 @@ public partial class Translation2dJsonContext : JsonSerializerContext
 {
     public static IStruct<Translation2d> Struct { get; } = new Translation2dStruct();
     public static IProtobuf<Translation2d, ProtobufTranslation2d> Proto { get; } = new Translation2dProto();
-    static IProtobuf<Translation2d> IProtobufSerializable<Translation2d>.Proto => Proto;
+    static IProtobuf<Translation2d> IProtobufSerializable<Translation2d>.Proto => Proto.UntypedProto;
 
     [JsonIgnore]
     public Length X { get; }
@@ -129,47 +129,47 @@ public Translation2d Nearest(ReadOnlySpan<Translation2d> translations)
 
     public Translation2d Interpolate(Translation2d endValue, double t)
     {
-        throw new NotImplementedException();
+        return new Translation2d(MathExtras.Lerp(X, endValue.X, t), MathExtras.Lerp(Y, endValue.Y, t));
     }
 
     public bool Equals(Translation2d other)
     {
-        throw new NotImplementedException();
+        return X.Equals(other.X, 1e-9.Meters()) && Y.Equals(other.Y, 1e-9.Meters());
     }
 
     public static Translation2d operator +(Translation2d left, Translation2d right)
     {
-        throw new NotImplementedException();
+        return new(left.X + right.X, left.Y + right.Y);
     }
 
     public static Translation2d operator -(Translation2d left, Translation2d right)
     {
-        throw new NotImplementedException();
+        return new Translation2d(left.X - right.X, left.Y - right.Y);
     }
 
     public static Translation2d operator -(Translation2d value)
     {
-        throw new NotImplementedException();
+        return new Translation2d(-value.X, -value.Y);
     }
 
     public static Translation2d operator *(Translation2d left, double right)
     {
-        throw new NotImplementedException();
+        return new(left.X * right, left.Y * right);
     }
 
     public static Translation2d operator /(Translation2d left, double right)
     {
-        throw new NotImplementedException();
+        return new(left.X / right, left.Y / right);
     }
 
     public static bool operator ==(Translation2d left, Translation2d right)
     {
-        throw new NotImplementedException();
+        return left.Equals(right);
     }
 
     public static bool operator !=(Translation2d left, Translation2d right)
     {
-        throw new NotImplementedException();
+        return !(left == right);
     }
 
     public override bool Equals(object? obj)
diff --git a/src/wpimath/Geometry/Twist2d.cs b/src/wpimath/Geometry/Twist2d.cs
new file mode 100644
index 00000000..55b260fc
--- /dev/null
+++ b/src/wpimath/Geometry/Twist2d.cs
@@ -0,0 +1,137 @@
+using System;
+using System.Numerics;
+using System.Text.Json.Serialization;
+using Google.Protobuf.Reflection;
+using UnitsNet;
+using UnitsNet.NumberExtensions.NumberToAngle;
+using UnitsNet.NumberExtensions.NumberToLength;
+using WPIMath.Proto;
+using WPIUtil.Serialization.Protobuf;
+using WPIUtil.Serialization.Struct;
+
+namespace WPIMath.Geometry;
+
+public class Twist2dProto : IProtobuf<Twist2d, ProtobufTwist2d>
+{
+    public MessageDescriptor Descriptor => ProtobufTwist2d.Descriptor;
+
+    public ProtobufTwist2d CreateMessage()
+    {
+        return new ProtobufTwist2d();
+    }
+
+    public void Pack(ProtobufTwist2d msg, Twist2d value)
+    {
+        msg.Dx = value.dxMeters;
+        msg.Dy = value.dyMeters;
+        msg.Dtheta = value.dthetaRadians;
+    }
+
+    public Twist2d Unpack(ProtobufTwist2d msg)
+    {
+        double dxMeters = msg.Dx;
+        double dyMeters = msg.Dy;
+        double dthetaRadians = msg.Dtheta;
+        return new Twist2d(dxMeters, dyMeters, dthetaRadians);
+    }
+}
+
+public class Twist2dStruct : IStruct<Twist2d>
+{
+    public string TypeString => "struct:Twist2d";
+
+    public int Size => IStructBase.SizeDouble * 4;
+
+    public string Schema => "double dx;double dy;double dtheta";
+
+    public void Pack(ref StructPacker buffer, Twist2d value)
+    {
+        buffer.WriteDouble(value.dxMeters);
+        buffer.WriteDouble(value.dyMeters);
+        buffer.WriteDouble(value.dthetaRadians);
+    }
+
+    public Twist2d Unpack(ref StructUnpacker buffer)
+    {
+        double dxMeters = buffer.ReadDouble();
+        double dyMeters = buffer.ReadDouble();
+        double dthetaRadians = buffer.ReadDouble();
+        return new Twist2d(dxMeters, dyMeters, dthetaRadians);
+    }
+}
+
+[JsonSerializable(typeof(Twist2d))]
+public partial class Twist2dJsonContext : JsonSerializerContext
+{
+}
+
+public readonly struct Twist2d : IEquatable<Twist2d>, IEqualityOperators<Twist2d, Twist2d, bool>, IMultiplyOperators<Twist2d, double, Twist2d>,
+                        IStructSerializable<Twist2d>,
+                        IProtobufSerializable<Twist2d, ProtobufTwist2d>
+
+{
+    public static IStruct<Twist2d> Struct { get; } = new Twist2dStruct();
+    public static IProtobuf<Twist2d, ProtobufTwist2d> Proto { get; } = new Twist2dProto();
+    static IProtobuf<Twist2d> IProtobufSerializable<Twist2d>.Proto => Proto.UntypedProto;
+
+    [JsonIgnore]
+    public Length Dx { get; init; }
+    [JsonIgnore]
+    public Length Dy { get; init; }
+    [JsonIgnore]
+    public Angle Dtheta { get; init; }
+
+    [JsonInclude]
+    [JsonPropertyName("dx")]
+    internal double dxMeters => Dx.Meters;
+    [JsonInclude]
+    [JsonPropertyName("dy")]
+    internal double dyMeters => Dy.Meters;
+    [JsonInclude]
+    [JsonPropertyName("dtheta")]
+    internal double dthetaRadians => Dtheta.Radians;
+    [JsonConstructor]
+    internal Twist2d(double dx, double dy, double dtheta)
+    {
+        Dx = dx.Meters();
+        Dy = dy.Meters();
+        Dtheta = dtheta.Radians();
+    }
+
+    public Twist2d(Length dx, Length dy, Angle dtheta)
+    {
+        Dx = dx;
+        Dy = dy;
+        Dtheta = dtheta;
+    }
+
+    public bool Equals(Twist2d other)
+    {
+        return Dx.Equals(other.Dx, 1e-9.Meters()) && Dy.Equals(other.Dy, 1e-9.Meters()) && Dtheta.Equals(other.Dtheta, 1e-9.Radians());
+    }
+
+    public static bool operator ==(Twist2d left, Twist2d right)
+    {
+        return left.Equals(right);
+    }
+
+    public static bool operator !=(Twist2d left, Twist2d right)
+    {
+        return !(left == right);
+    }
+
+    public static Twist2d operator *(Twist2d left, double right)
+    {
+        return new(left.Dx * right, left.Dy * right, left.Dtheta * right);
+    }
+
+    public override bool Equals(object? obj)
+    {
+        return obj is Twist2d d && Equals(d);
+    }
+
+    public override int GetHashCode()
+    {
+        return HashCode.Combine(Dx, Dy, Dtheta);
+    }
+}
diff --git a/src/wpimath/Geometry/Twist3d.cs b/src/wpimath/Geometry/Twist3d.cs
new file mode 100644
index 00000000..d25ed20d
--- /dev/null
+++ b/src/wpimath/Geometry/Twist3d.cs
@@ -0,0 +1,170 @@
+using System;
+using System.Numerics;
+using System.Text.Json.Serialization;
+using Google.Protobuf.Reflection;
+using UnitsNet;
+using UnitsNet.NumberExtensions.NumberToAngle;
+using UnitsNet.NumberExtensions.NumberToLength;
+using WPIMath.Proto;
+using WPIUtil.Serialization.Protobuf;
+using WPIUtil.Serialization.Struct;
+
+namespace WPIMath.Geometry;
+
+public class Twist3dProto : IProtobuf<Twist3d, ProtobufTwist3d>
+{
+    public MessageDescriptor Descriptor => ProtobufTwist3d.Descriptor;
+
+    public ProtobufTwist3d CreateMessage()
+    {
+        return new ProtobufTwist3d();
+    }
+
+    public void Pack(ProtobufTwist3d msg, Twist3d value)
+    {
+        msg.Dx = value.dxMeters;
+        msg.Dy = value.dyMeters;
+        msg.Dz = value.dzMeters;
+        msg.Rx = value.rxRadians;
+        msg.Ry = value.ryRadians;
+        msg.Rz = value.rzRadians;
+    }
+
+    public Twist3d Unpack(ProtobufTwist3d msg)
+    {
+        double dxMeters = msg.Dx;
+        double dyMeters = msg.Dy;
+        double dzMeters = msg.Dz;
+        double rxRadians = msg.Rx;
+        double ryRadians = msg.Ry;
+        double rzRadians = msg.Rz;
+        return new Twist3d(dxMeters, dyMeters, dzMeters, rxRadians, ryRadians, rzRadians);
+    }
+}
+
+public class Twist3dStruct : IStruct<Twist3d>
+{
+    public string TypeString => "struct:Twist3d";
+
+    public int Size => IStructBase.SizeDouble * 4;
+
+    public string Schema => "double dx;double dy;double dtheta";
+
+    public void Pack(ref StructPacker buffer, Twist3d value)
+    {
+        buffer.WriteDouble(value.dxMeters);
+        buffer.WriteDouble(value.dyMeters);
+        buffer.WriteDouble(value.dzMeters);
+        buffer.WriteDouble(value.rxRadians);
+        buffer.WriteDouble(value.ryRadians);
+        buffer.WriteDouble(value.rzRadians);
+    }
+
+    public Twist3d Unpack(ref StructUnpacker buffer)
+    {
+        double dxMeters = buffer.ReadDouble();
+        double dyMeters = buffer.ReadDouble();
+        double dzMeters = buffer.ReadDouble();
+        double rxRadians = buffer.ReadDouble();
+        double ryRadians = buffer.ReadDouble();
+        double rzRadians = buffer.ReadDouble();
+        return new Twist3d(dxMeters, dyMeters, dzMeters, rxRadians, ryRadians, rzRadians);
+    }
+}
+
+[JsonSerializable(typeof(Twist3d))]
+public partial class Twist3dJsonContext : JsonSerializerContext
+{
+}
+
+public readonly struct Twist3d : IEquatable<Twist3d>, IEqualityOperators<Twist3d, Twist3d, bool>, IMultiplyOperators<Twist3d, double, Twist3d>,
+                        IStructSerializable<Twist3d>,
+                        IProtobufSerializable<Twist3d, ProtobufTwist3d>
+
+{
+    public static IStruct<Twist3d> Struct { get; } = new Twist3dStruct();
+    public static IProtobuf<Twist3d, ProtobufTwist3d> Proto { get; } = new Twist3dProto();
+    static IProtobuf<Twist3d> IProtobufSerializable<Twist3d>.Proto => Proto.UntypedProto;
+
+    [JsonIgnore]
+    public Length Dx { get; init; }
+    [JsonIgnore]
+    public Length Dy { get; init; }
+    [JsonIgnore]
+    public Length Dz { get; init; }
+    [JsonIgnore]
+    public Angle Rx { get; init; }
+    [JsonIgnore]
+    public Angle Ry { get; init; }
+    [JsonIgnore]
+    public Angle Rz { get; init; }
+
+    [JsonInclude]
+    [JsonPropertyName("dx")]
+    internal double dxMeters => Dx.Meters;
+    [JsonInclude]
+    [JsonPropertyName("dy")]
+    internal double dyMeters => Dy.Meters;
+    [JsonInclude]
+    [JsonPropertyName("dz")]
+    internal double dzMeters => Dz.Meters;
+    [JsonInclude]
+    [JsonPropertyName("rx")]
+    internal double rxRadians => Rx.Radians;
+    [JsonInclude]
+    [JsonPropertyName("ry")]
+    internal double ryRadians => Ry.Radians;
+    [JsonInclude]
+    [JsonPropertyName("rz")]
+    internal double rzRadians => Rz.Radians;
+    [JsonConstructor]
+    internal Twist3d(double dx, double dy, double dz, double rx, double ry, double rz)
+    {
+        Dx = dx.Meters();
+        Dy = dy.Meters();
+        Dz = dz.Meters();
+        Rx = rx.Radians();
+        Ry = ry.Radians();
+        Rz = rz.Radians();
+    }
+
+    public Twist3d(Length dx, Length dy, Length dz, Angle rx, Angle ry, Angle rz)
+    {
+        Dx = dx;
+        Dy = dy;
+        Dz = dz;
+        Rx = rx;
+        Ry = ry;
+        Rz = rz;
+    }
+
+    public bool Equals(Twist3d other)
+    {
+        return Dx.Equals(other.Dx, 1e-9.Meters()) && Dy.Equals(other.Dy, 1e-9.Meters()) && Dz.Equals(other.Dz, 1e-9.Meters()) && Rx.Equals(other.Rx, 1e-9.Radians()) && Ry.Equals(other.Ry, 1e-9.Radians()) && Rz.Equals(other.Rz, 1e-9.Radians());
+    }
+
+    public static bool operator ==(Twist3d left, Twist3d right)
+    {
+        return left.Equals(right);
+    }
+
+    public static bool operator !=(Twist3d left, Twist3d right)
+    {
+        return !(left == right);
+    }
+
+    public static Twist3d operator *(Twist3d left, double right)
+    {
+        return new(left.Dx * right, left.Dy * right, left.Dz * right, left.Rx * right, left.Ry * right, left.Rz * right);
+    }
+
+    public override bool Equals(object? obj)
+    {
+        return obj is Twist3d d && Equals(d);
+    }
+
+    public override int GetHashCode()
+    {
+        return HashCode.Combine(Dx, Dy, Dz, Rx, Ry, Rz);
+    }
+}
diff --git a/src/wpiutil/Serialization/Protobuf/IProtobuf.cs b/src/wpiutil/Serialization/Protobuf/IProtobuf.cs
index 178251c5..f87a9ab9 100644
--- a/src/wpiutil/Serialization/Protobuf/IProtobuf.cs
+++ b/src/wpiutil/Serialization/Protobuf/IProtobuf.cs
@@ -32,7 +32,7 @@ private static void ForEachDescriptorImpl(FileDescriptor desc, Func<string, bool
 
 public interface IProtobuf<T> : IProtobufBase
 {
-    abstract IMessage CreateMessage();
+    IMessage CreateMessage();
 
     T Unpack(IMessage msg);
 
@@ -41,29 +41,26 @@ public interface IProtobuf<T> : IProtobufBase
     void UnpackInto(ref T value, IMessage msg);
 }
 
-public interface IProtobuf<T, MessageType> : IProtobuf<T> where MessageType : IMessage<MessageType>
+public interface IProtobuf<T, MessageType> : IProtobufBase where MessageType : IMessage<MessageType>
 {
-    IMessage IProtobuf<T>.CreateMessage()
+    internal class ProtobufWrapper(IProtobuf<T, MessageType> proto) : IProtobuf<T>
     {
-        return CreateMessage();
-    }
+        private readonly IProtobuf<T, MessageType> m_proto = proto;
 
-    void IProtobuf<T>.Pack(IMessage msg, T value)
-    {
-        Pack((MessageType)msg, value);
-    }
+        public MessageDescriptor Descriptor => m_proto.Descriptor;
 
-    T IProtobuf<T>.Unpack(IMessage msg)
-    {
-        return Unpack((MessageType)msg);
-    }
+        public IMessage CreateMessage() => m_proto.CreateMessage();
 
-    void IProtobuf<T>.UnpackInto(ref T value, IMessage msg)
-    {
-        UnpackInto(ref value, (MessageType)msg);
+        public void Pack(IMessage msg, T value) => m_proto.Pack((MessageType)msg, value);
+
+        public T Unpack(IMessage msg) => m_proto.Unpack((MessageType)msg);
+
+        public void UnpackInto(ref T value, IMessage msg) => m_proto.UnpackInto(ref value, (MessageType)msg);
     }
 
-    new MessageType CreateMessage();
+    public IProtobuf<T> UntypedProto => new ProtobufWrapper(this);
+
+    MessageType CreateMessage();
 
     T Unpack(MessageType msg);