KavTest/KavTest/Utility/Transform3D.cs

using Microsoft.Xna.Framework;

namespace KavTest
{
    public struct Transform3D : System.IEquatable<Transform3D>
    {
        public static Transform3D Identity = new Transform3D(Vector3.Zero, Quaternion.Identity, Vector3.One);

        public Matrix TransformMatrix { get; }
        public Vector3 Position { get; }
        public Quaternion Orientation { get; }
        public Vector3 Scale { get; }

        public Vector3 Forward { get { return TransformMatrix.Forward; } }
        public Vector3 Right { get { return TransformMatrix.Right; } }
        public Vector3 Up { get { return TransformMatrix.Up; } }

        public Transform3D(Vector3 position, Quaternion orientation, Vector3 scale)
        {
            Position = position;
            Orientation = orientation;
            Scale = scale;
            TransformMatrix = CreateTransformMatrix(Position, Orientation, Scale);
        }

        public Transform3D(Vector3 position, Quaternion orientation)
        {
            Position = position;
            Orientation = orientation;
            Scale = Vector3.One;
            TransformMatrix = CreateTransformMatrix(Position, Orientation, Scale);
        }

        public Transform3D(Vector3 position)
        {
            Position = position;
            Orientation = Quaternion.Identity;
            Scale = Vector3.One;
            TransformMatrix = CreateTransformMatrix(Position, Orientation, Scale);
        }

        public Transform3D WithPosition(Vector3 position)
        {
            return Translate(position - Position);
        }

        public Transform3D Translate(Vector3 translation)
        {
            return new Transform3D(Position + translation, Orientation, Scale);
        }

        public Transform3D TranslateLocal(Vector3 translation)
        {
            return Translate(Vector3.Transform(translation, Orientation));
        }

        public Transform3D RotateLocal(Quaternion other)
        {
            return new Transform3D(Position, Orientation * other, Scale);
        }

        public Transform3D RotateLocal(float yaw, float pitch, float roll)
        {
            return RotateLocal(Quaternion.CreateFromYawPitchRoll(
                yaw,
                pitch,
                roll
            ));
        }

        public Transform3D RotateLocal(Vector3 eulerAngles)
        {
            return RotateLocal(Quaternion.CreateFromYawPitchRoll(
                eulerAngles.X,
                eulerAngles.Y,
                eulerAngles.Z
            ));
        }

        public Transform3D Compose(Transform3D other)
        {
            return new Transform3D(Position + other.Position, Orientation * other.Orientation, Scale * other.Scale);
        }

        private static Matrix CreateTransformMatrix(Vector3 position, Quaternion orientation, Vector3 scale)
        {
            return Matrix.CreateScale(scale) *
                   Matrix.CreateFromQuaternion(orientation) *
                   Matrix.CreateTranslation(position);
        }

        public override bool Equals(object other)
        {
            if (other is Transform3D otherTransform)
            {
                return Equals(otherTransform);
            }

            return false;
        }

        public bool Equals(Transform3D other)
        {
            return TransformMatrix == other.TransformMatrix;
        }

        public override int GetHashCode()
        {
            return TransformMatrix.GetHashCode();
        }

        public static bool operator ==(Transform3D a, Transform3D b)
        {
            return a.Equals(b);
        }

        public static bool operator !=(Transform3D a, Transform3D b)
        {
            return !(a == b);
        }
    }
}