From 27510c66d69f0fde76689f5b6c79eb6255dfc869 Mon Sep 17 00:00:00 2001 From: Thad House Date: Sat, 10 Feb 2024 00:41:41 -0800 Subject: [PATCH] Add a TON of math --- src/ntcore/INtSendable.cs | 6 +- src/wpimath/Geometry/Pose2d.cs | 119 +++++++- src/wpimath/Geometry/Quaternion.cs | 285 ++++++++++++++++++ src/wpimath/Geometry/Rotation2d.cs | 2 +- src/wpimath/Geometry/Rotation3d.cs | 268 ++++++++++++++++ src/wpimath/Geometry/Transform2d.cs | 158 ++++++++++ src/wpimath/Geometry/Translation2d.cs | 20 +- src/wpimath/Geometry/Twist2d.cs | 137 +++++++++ src/wpimath/Geometry/Twist3d.cs | 170 +++++++++++ .../Serialization/Protobuf/IProtobuf.cs | 31 +- 10 files changed, 1151 insertions(+), 45 deletions(-) create mode 100644 src/wpimath/Geometry/Quaternion.cs create mode 100644 src/wpimath/Geometry/Rotation3d.cs create mode 100644 src/wpimath/Geometry/Transform2d.cs create mode 100644 src/wpimath/Geometry/Twist2d.cs create mode 100644 src/wpimath/Geometry/Twist3d.cs 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 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, IProtobufSerializable, IMultiplyOperators, + IAdditionOperators, + ISubtractionOperators, IDivisionOperators, IEqualityOperators, IInterpolatable, @@ -68,14 +71,14 @@ public partial class Pose2dJsonContext : JsonSerializerContext { public static IStruct Struct { get; } = new Pose2dStruct(); public static IProtobuf Proto { get; } = new Pose2dProto(); - static IProtobuf IProtobufSerializable.Proto => Proto; + static IProtobuf IProtobufSerializable.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 poses) @@ -113,32 +178,56 @@ public Pose2d Nearest(ReadOnlySpan 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 +{ + 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 +{ + 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, IEqualityOperators, + IAdditionOperators, + ISubtractionOperators, + IMultiplyOperators, + IMultiplyOperators, + IDivisionOperators, + IStructSerializable, + IProtobufSerializable + +{ + public static IStruct Struct { get; } = new QuaternionStruct(); + public static IProtobuf Proto { get; } = new QuaternionProto(); + static IProtobuf IProtobufSerializable.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 Struct { get; } = new Rotation2dStruct(); public static IProtobuf Proto { get; } = new Rotation2dProto(); - static IProtobuf IProtobufSerializable.Proto => Proto; + static IProtobuf IProtobufSerializable.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 +{ + 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 +{ + 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, + IProtobufSerializable, + IAdditionOperators, + ISubtractionOperators, + IUnaryNegationOperators, + IMultiplyOperators, + IDivisionOperators, + IEqualityOperators, + IAdditiveIdentity, + IInterpolatable, + IEquatable +{ + public static IStruct Struct { get; } = new Rotation3dStruct(); + public static IProtobuf Proto { get; } = new Rotation3dProto(); + static IProtobuf IProtobufSerializable.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 +{ + 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 +{ + 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, + IMultiplyOperators, + IDivisionOperators, + IEqualityOperators, + IAdditiveIdentity, + IEquatable +{ + [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 Struct { get; } = new Translation2dStruct(); public static IProtobuf Proto { get; } = new Translation2dProto(); - static IProtobuf IProtobufSerializable.Proto => Proto; + static IProtobuf IProtobufSerializable.Proto => Proto.UntypedProto; [JsonIgnore] public Length X { get; } @@ -129,47 +129,47 @@ public Translation2d Nearest(ReadOnlySpan 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 +{ + 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 +{ + 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, IEqualityOperators, IMultiplyOperators, + IStructSerializable, + IProtobufSerializable + +{ + public static IStruct Struct { get; } = new Twist2dStruct(); + public static IProtobuf Proto { get; } = new Twist2dProto(); + static IProtobuf IProtobufSerializable.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 +{ + 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 +{ + 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, IEqualityOperators, IMultiplyOperators, + IStructSerializable, + IProtobufSerializable + +{ + public static IStruct Struct { get; } = new Twist3dStruct(); + public static IProtobuf Proto { get; } = new Twist3dProto(); + static IProtobuf IProtobufSerializable.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 : IProtobufBase { - abstract IMessage CreateMessage(); + IMessage CreateMessage(); T Unpack(IMessage msg); @@ -41,29 +41,26 @@ public interface IProtobuf : IProtobufBase void UnpackInto(ref T value, IMessage msg); } -public interface IProtobuf : IProtobuf where MessageType : IMessage +public interface IProtobuf : IProtobufBase where MessageType : IMessage { - IMessage IProtobuf.CreateMessage() + internal class ProtobufWrapper(IProtobuf proto) : IProtobuf { - return CreateMessage(); - } + private readonly IProtobuf m_proto = proto; - void IProtobuf.Pack(IMessage msg, T value) - { - Pack((MessageType)msg, value); - } + public MessageDescriptor Descriptor => m_proto.Descriptor; - T IProtobuf.Unpack(IMessage msg) - { - return Unpack((MessageType)msg); - } + public IMessage CreateMessage() => m_proto.CreateMessage(); - void IProtobuf.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 UntypedProto => new ProtobufWrapper(this); + + MessageType CreateMessage(); T Unpack(MessageType msg);