41 lines
1.8 KiB
C#
41 lines
1.8 KiB
C#
using System.Numerics;
|
|
|
|
namespace Demo
|
|
{
|
|
/// <summary>
|
|
/// <para>
|
|
/// Represents a rigid transformation from one coordinate system to another. <b>NOTE: By default, unless otherwise
|
|
/// stated, the <c>Pose</c> of an object transforms its local space to the ambient world space. </b>
|
|
/// </para>
|
|
/// <para>
|
|
/// Given two <c>Pose</c>s <c>p1</c> and <c>p2</c> which transform from space A to B and from B to C respectively,
|
|
/// the pose <c>p2 * p1</c> transforms from A to C, and <c>p1 * v</c> transforms a vector <c>v</c> from space A to
|
|
/// B. A <c>Pose</c> has a rotation and a translation. The rotation is always applied first, followed by the
|
|
/// translation.
|
|
/// </para>
|
|
/// </summary>
|
|
/// <param name="rotation">The rotation of this pose.</param>
|
|
/// <param name="translation">The translation of this pose.</param>
|
|
public struct Pose(Quaternion rotation, Vector3 translation)
|
|
{
|
|
public Vector3 Translation { readonly get; set; } = translation;
|
|
public Quaternion Rotation { readonly get; set; } = rotation;
|
|
|
|
public static Pose Identity => new(Quaternion.Identity, Vector3.Zero);
|
|
|
|
public Pose(Vector3 translation) : this(Quaternion.Identity, translation) { }
|
|
public Pose(Quaternion rotation) : this(rotation, Vector3.Zero) { }
|
|
|
|
public static Vector3 operator *(Pose p, Vector3 v) => Vector3.Transform(v, p.Rotation) + p.Translation;
|
|
public static Pose operator *(Pose a, Pose b) => new(a.Rotation * b.Rotation, a * b.Translation);
|
|
public readonly Pose Inverse
|
|
{
|
|
get
|
|
{
|
|
var rotInv = Quaternion.Conjugate(Rotation);
|
|
return new(rotInv, -Vector3.Transform(Translation, rotInv));
|
|
}
|
|
}
|
|
}
|
|
}
|