path: root/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GraphTransform.cs

using Unity.Mathematics;
using UnityEngine;

namespace Pathfinding.Util {
	/// <summary>
	/// Transforms to and from world space to a 2D movement plane.
	/// The transformation is guaranteed to be purely a rotation
	/// so no scale or offset is used. This interface is primarily
	/// used to make it easier to write movement scripts which can
	/// handle movement both in the XZ plane and in the XY plane.
	///
	/// See: <see cref="Pathfinding.Util.GraphTransform"/>
	/// </summary>
	public interface IMovementPlane {
		Vector2 ToPlane(Vector3 p);
		Vector2 ToPlane(Vector3 p, out float elevation);
		Vector3 ToWorld(Vector2 p, float elevation = 0);
		SimpleMovementPlane ToSimpleMovementPlane();
	}

	/// <summary>
	/// A matrix wrapper which can be used to project points from world space to a movement plane.
	///
	/// In contrast to <see cref="NativeMovementPlane"/>, this is represented by a matrix instead of a quaternion.
	/// This means it is less space efficient (36 bytes instead of 16 bytes) but it is more performant when
	/// you need to do a lot of ToPlane conversions.
	/// </summary>
	public readonly struct ToPlaneMatrix {
		public readonly float3x3 matrix;

		public ToPlaneMatrix (NativeMovementPlane plane) => this.matrix = new float3x3(math.conjugate(plane.rotation));

		/// <summary>
		/// Transforms from world space to the 'ground' plane of the graph.
		/// The transformation is purely a rotation so no scale or offset is used.
		///
		/// See: <see cref="NativeMovementPlane.ToPlane(float3)"/>
		/// </summary>
		public float2 ToPlane(float3 p) => math.mul(matrix, p).xz;

		/// <summary>
		/// Transforms from world space to the 'ground' plane of the graph.
		/// The transformation is purely a rotation so no scale or offset is used.
		///
		/// The elevation coordinate will be returned as the y coordinate of the returned vector.
		///
		/// See: <see cref="NativeMovementPlane.ToPlane(float3)"/>
		/// </summary>
		public float3 ToXZPlane(float3 p) => math.mul(matrix, p);

		/// <summary>
		/// Transforms from world space to the 'ground' plane of the graph.
		/// The transformation is purely a rotation so no scale or offset is used.
		///
		/// See: <see cref="NativeMovementPlane.ToPlane(float3)"/>
		/// </summary>
		public float2 ToPlane (float3 p, out float elevation) {
			var v = math.mul(matrix, p);
			elevation = v.y;
			return v.xz;
		}
	}

	/// <summary>
	/// A matrix wrapper which can be used to project points from a movement plane to world space.
	///
	/// In contrast to <see cref="NativeMovementPlane"/>, this is represented by a matrix instead of a quaternion.
	/// This means it is less space efficient (36 bytes instead of 16 bytes) but it is more performant when
	/// you need to do a lot of ToWorld conversions.
	/// </summary>
	public readonly struct ToWorldMatrix {
		public readonly float3x3 matrix;

		public ToWorldMatrix (NativeMovementPlane plane) => this.matrix = new float3x3(plane.rotation);

		public ToWorldMatrix (float3x3 matrix) => this.matrix = matrix;

		public float3 ToWorld(float2 p, float elevation = 0) => math.mul(matrix, new float3(p.x, elevation, p.y));

		/// <summary>
		/// Transforms a bounding box from local space to world space.
		///
		/// The Y coordinate of the bounding box is the elevation coordinate.
		///
		/// See: https://zeux.io/2010/10/17/aabb-from-obb-with-component-wise-abs/
		/// </summary>
		public Bounds ToWorld (Bounds bounds) {
			Bounds result = default;
			result.center = math.mul(matrix, (float3)bounds.center);
			result.extents = math.mul(new float3x3(
				math.abs(matrix.c0),
				math.abs(matrix.c1),
				math.abs(matrix.c2)
				), (float3)bounds.extents);
			return result;
		}
	}

	/// <summary>A variant of <see cref="SimpleMovementPlane"/> that can be passed to burst functions</summary>
	public readonly struct NativeMovementPlane {
		/// <summary>
		/// The rotation of the plane.
		/// The plane is defined by the XZ-plane rotated by this quaternion.
		///
		/// Should always be normalized.
		/// </summary>
		public readonly quaternion rotation;

		/// <summary>Normal of the plane</summary>
		// TODO: Check constructor for float3x3(quaternion), seems smarter, at least in burst
		public float3 up => 2 * new float3(rotation.value.x * rotation.value.y - rotation.value.w * rotation.value.z, 0.5f - rotation.value.x * rotation.value.x - rotation.value.z * rotation.value.z, rotation.value.w * rotation.value.x + rotation.value.y * rotation.value.z); // math.mul(rotation, Vector3.up);

		public NativeMovementPlane(quaternion rotation) {
			// We need to normalize to make sure that math.inverse(rotation) == math.conjugate(rotation).
			// We want to use conjugate because it's faster.
			this.rotation = math.normalizesafe(rotation);
		}

		public NativeMovementPlane(SimpleMovementPlane plane) : this(plane.rotation) {}

		public ToPlaneMatrix AsWorldToPlaneMatrix() => new ToPlaneMatrix(this);
		public ToWorldMatrix AsPlaneToWorldMatrix() => new ToWorldMatrix(this);

		public float ProjectedLength(float3 v) => math.length(ToPlane(v));

		/// <summary>
		/// Transforms from world space to the 'ground' plane of the graph.
		/// The transformation is purely a rotation so no scale or offset is used.
		///
		/// For a graph rotated with the rotation (-90, 0, 0) this will transform
		/// a coordinate (x,y,z) to (x,y). For a graph with the rotation (0,0,0)
		/// this will tranform a coordinate (x,y,z) to (x,z). More generally for
		/// a graph with a quaternion rotation R this will transform a vector V
		/// to inverse(R) * V (i.e rotate the vector V using the inverse of rotation R).
		/// </summary>
		public float2 ToPlane (float3 p) {
			return math.mul(math.conjugate(rotation), p).xz;
		}

		/// <summary>Transforms from world space to the 'ground' plane of the graph</summary>
		public float2 ToPlane (float3 p, out float elevation) {
			p = math.mul(math.conjugate(rotation), p);
			elevation = p.y;
			return p.xz;
		}

		/// <summary>
		/// Transforms from the 'ground' plane of the graph to world space.
		/// The transformation is purely a rotation so no scale or offset is used.
		/// </summary>
		public float3 ToWorld (float2 p, float elevation = 0f) {
			return math.mul(rotation, new float3(p.x, elevation, p.y));
		}

		/// <summary>
		/// Projects a rotation onto the plane.
		///
		/// The returned angle is such that
		///
		/// <code>
		/// var angle = ...;
		/// var q = math.mul(plane.rotation, quaternion.RotateY(angle));
		/// AstarMath.DeltaAngle(plane.ToPlane(q), -angle) == 0; // or at least approximately equal
		/// </code>
		///
		/// See: <see cref="ToWorldRotation"/>
		/// See: <see cref="ToWorldRotationDelta"/>
		/// </summary>
		/// <param name="rotation">the rotation to project</param>
		public float ToPlane (quaternion rotation) {
			var inPlaneRotation = math.mul(math.conjugate(this.rotation), rotation);
			// Ensure the rotation axis is always along +Y
			if (inPlaneRotation.value.y < 0) inPlaneRotation.value = -inPlaneRotation.value;
			var twist = math.normalizesafe(new quaternion(0, inPlaneRotation.value.y, 0, inPlaneRotation.value.w));
			return -VectorMath.QuaternionAngle(twist);
		}

		public quaternion ToWorldRotation (float angle) {
			return math.mul(rotation, quaternion.RotateY(-angle));
		}

		public quaternion ToWorldRotationDelta (float deltaAngle) {
			return quaternion.AxisAngle(ToWorld(float2.zero, 1), -deltaAngle);
		}

		/// <summary>
		/// Transforms a bounding box from local space to world space.
		///
		/// The Y coordinate of the bounding box is the elevation coordinate.
		/// </summary>
		public Bounds ToWorld(Bounds bounds) => AsPlaneToWorldMatrix().ToWorld(bounds);
	}

	/// <summary>
	/// Represents the orientation of a plane.
	///
	/// When a character walks around in the world, it may not necessarily walk on the XZ-plane.
	/// It may be the case that the character is on a spherical world, or maybe it walks on a wall or upside down on the ceiling.
	///
	/// A movement plane is used to handle this. It contains functions for converting a 3D point into a 2D point on that plane, and functions for converting back to 3D.
	///
	/// See: NativeMovementPlane
	/// </summary>
#if MODULE_COLLECTIONS_2_0_0_OR_NEWER && UNITY_2022_2_OR_NEWER
	[Unity.Collections.GenerateTestsForBurstCompatibility]
#endif
	public readonly struct SimpleMovementPlane : IMovementPlane {
		public readonly Quaternion rotation;
		public readonly Quaternion inverseRotation;
		readonly byte plane;
		public bool isXY => plane == 1;
		public bool isXZ => plane == 2;

		/// <summary>A plane that spans the X and Y axes</summary>
		public static readonly SimpleMovementPlane XYPlane = new SimpleMovementPlane(Quaternion.Euler(-90, 0, 0));

		/// <summary>A plane that spans the X and Z axes</summary>
		public static readonly SimpleMovementPlane XZPlane = new SimpleMovementPlane(Quaternion.identity);

		public SimpleMovementPlane (Quaternion rotation) {
			this.rotation = rotation;
			// TODO: Normalize #rotation and compute inverse every time instead (less memory)
			inverseRotation = Quaternion.Inverse(rotation);
			// Some short circuiting code for the movement plane calculations
			if (rotation == XYPlane.rotation) plane = 1;
			else if (rotation == Quaternion.identity) plane = 2;
			else plane = 0;
		}

		/// <summary>
		/// Transforms from world space to the 'ground' plane of the graph.
		/// The transformation is purely a rotation so no scale or offset is used.
		///
		/// For a graph rotated with the rotation (-90, 0, 0) this will transform
		/// a coordinate (x,y,z) to (x,y). For a graph with the rotation (0,0,0)
		/// this will tranform a coordinate (x,y,z) to (x,z). More generally for
		/// a graph with a quaternion rotation R this will transform a vector V
		/// to inverse(R) * V (i.e rotate the vector V using the inverse of rotation R).
		/// </summary>
		public Vector2 ToPlane (Vector3 point) {
			// These special cases cover most graph orientations used in practice.
			// Having them here improves performance in those cases by a factor of
			// 2.5 without impacting the generic case in any significant way.
			if (isXY) return new Vector2(point.x, point.y);
			if (!isXZ) point = inverseRotation * point;
			return new Vector2(point.x, point.z);
		}

		/// <summary>
		/// Transforms from world space to the 'ground' plane of the graph.
		/// The transformation is purely a rotation so no scale or offset is used.
		///
		/// For a graph rotated with the rotation (-90, 0, 0) this will transform
		/// a coordinate (x,y,z) to (x,y). For a graph with the rotation (0,0,0)
		/// this will tranform a coordinate (x,y,z) to (x,z). More generally for
		/// a graph with a quaternion rotation R this will transform a vector V
		/// to inverse(R) * V (i.e rotate the vector V using the inverse of rotation R).
		/// </summary>
		public float2 ToPlane (float3 point) {
			return ((float3)(inverseRotation * (Vector3)point)).xz;
		}

		/// <summary>
		/// Transforms from world space to the 'ground' plane of the graph.
		/// The transformation is purely a rotation so no scale or offset is used.
		/// </summary>
		public Vector2 ToPlane (Vector3 point, out float elevation) {
			if (!isXZ) point = inverseRotation * point;
			elevation = point.y;
			return new Vector2(point.x, point.z);
		}

		/// <summary>
		/// Transforms from world space to the 'ground' plane of the graph.
		/// The transformation is purely a rotation so no scale or offset is used.
		/// </summary>
		public float2 ToPlane (float3 point, out float elevation) {
			point = math.mul(inverseRotation, point);
			elevation = point.y;
			return point.xz;
		}

		/// <summary>
		/// Transforms from the 'ground' plane of the graph to world space.
		/// The transformation is purely a rotation so no scale or offset is used.
		/// </summary>
		public Vector3 ToWorld (Vector2 point, float elevation = 0) {
			return rotation * new Vector3(point.x, elevation, point.y);
		}

		/// <summary>
		/// Transforms from the 'ground' plane of the graph to world space.
		/// The transformation is purely a rotation so no scale or offset is used.
		/// </summary>
		public float3 ToWorld (float2 point, float elevation = 0) {
			return rotation * new Vector3(point.x, elevation, point.y);
		}

		public SimpleMovementPlane ToSimpleMovementPlane () {
			return this;
		}

		public static bool operator== (SimpleMovementPlane lhs, SimpleMovementPlane rhs) {
			return lhs.rotation == rhs.rotation;
		}

		public static bool operator!= (SimpleMovementPlane lhs, SimpleMovementPlane rhs) {
			return lhs.rotation != rhs.rotation;
		}

		public override bool Equals (System.Object other) {
			if (!(other is SimpleMovementPlane)) return false;
			return rotation == ((SimpleMovementPlane)other).rotation;
		}

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

	/// <summary>Generic 3D coordinate transformation</summary>
	public interface ITransform {
		Vector3 Transform(Vector3 position);
		Vector3 InverseTransform(Vector3 position);
	}

	/// <summary>Like <see cref="Pathfinding.Util.GraphTransform"/>, but mutable</summary>
	public class MutableGraphTransform : GraphTransform {
		public MutableGraphTransform (Matrix4x4 matrix) : base(matrix) {}

		/// <summary>Replace this transform with the given matrix transformation</summary>
		public void SetMatrix (Matrix4x4 matrix) {
			Set(matrix);
		}
	}

	/// <summary>
	/// Defines a transformation from graph space to world space.
	/// This is essentially just a simple wrapper around a matrix, but it has several utilities that are useful.
	/// </summary>
	public class GraphTransform : IMovementPlane, ITransform {
		/// <summary>True if this transform is the identity transform (i.e it does not do anything)</summary>
		public bool identity { get { return isIdentity; } }

		/// <summary>True if this transform is a pure translation without any scaling or rotation</summary>
		public bool onlyTranslational { get { return isOnlyTranslational; } }

		bool isXY;
		bool isXZ;
		bool isOnlyTranslational;
		bool isIdentity;

		public Matrix4x4 matrix { get; private set; }
		public Matrix4x4 inverseMatrix { get; private set; }
		Vector3 up;
		Vector3 translation;
		Int3 i3translation;
		public Quaternion rotation { get; private set; }
		Quaternion inverseRotation;

		public static readonly GraphTransform identityTransform = new GraphTransform(Matrix4x4.identity);

		/// <summary>Transforms from the XZ plane to the XY plane</summary>
		public static readonly GraphTransform xyPlane = new GraphTransform(Matrix4x4.TRS(Vector3.zero, Quaternion.Euler(-90, 0, 0), Vector3.one));

		/// <summary>Transforms from the XZ plane to the XZ plane (i.e. an identity transformation)</summary>
		public static readonly GraphTransform xzPlane = new GraphTransform(Matrix4x4.identity);

		public GraphTransform (Matrix4x4 matrix) {
			Set(matrix);
		}

		protected void Set (Matrix4x4 matrix) {
			this.matrix = matrix;
			inverseMatrix = matrix.inverse;
			isIdentity = matrix.isIdentity;
			isOnlyTranslational = MatrixIsTranslational(matrix);
			up = matrix.MultiplyVector(Vector3.up).normalized;
			translation = matrix.MultiplyPoint3x4(Vector3.zero);
			i3translation = (Int3)translation;

			// Extract the rotation from the matrix. This is only correct if the matrix has no skew, but we only
			// want to use it for the movement plane so as long as the Up axis is parpendicular to the Forward
			// axis everything should be ok. In fact the only case in the project when all three axes are not
			// perpendicular is when hexagon or isometric grid graphs are used, but in those cases only the
			// X and Z axes are not perpendicular.
			rotation = Quaternion.LookRotation(TransformVector(Vector3.forward), TransformVector(Vector3.up));
			inverseRotation = Quaternion.Inverse(rotation);
			// Some short circuiting code for the movement plane calculations
			isXY = rotation == Quaternion.Euler(-90, 0, 0);
			isXZ = rotation == Quaternion.Euler(0, 0, 0);
		}

		public Vector3 WorldUpAtGraphPosition (Vector3 point) {
			return up;
		}

		static bool MatrixIsTranslational (Matrix4x4 matrix) {
			return matrix.GetColumn(0) == new Vector4(1, 0, 0, 0) && matrix.GetColumn(1) == new Vector4(0, 1, 0, 0) && matrix.GetColumn(2) == new Vector4(0, 0, 1, 0) && matrix.m33 == 1;
		}

		public Vector3 Transform (Vector3 point) {
			if (onlyTranslational) return point + translation;
			return matrix.MultiplyPoint3x4(point);
		}

		public Vector3 TransformVector (Vector3 dir) {
			if (onlyTranslational) return dir;
			return matrix.MultiplyVector(dir);
		}

		public void Transform (Int3[] arr) {
			if (onlyTranslational) {
				for (int i = arr.Length - 1; i >= 0; i--) arr[i] += i3translation;
			} else {
				for (int i = arr.Length - 1; i >= 0; i--) arr[i] = (Int3)matrix.MultiplyPoint3x4((Vector3)arr[i]);
			}
		}

		public void Transform (UnsafeSpan<Int3> arr) {
			if (onlyTranslational) {
				for (int i = arr.Length - 1; i >= 0; i--) arr[i] += i3translation;
			} else {
				for (int i = arr.Length - 1; i >= 0; i--) arr[i] = (Int3)matrix.MultiplyPoint3x4((Vector3)arr[i]);
			}
		}

		public void Transform (Vector3[] arr) {
			if (onlyTranslational) {
				for (int i = arr.Length - 1; i >= 0; i--) arr[i] += translation;
			} else {
				for (int i = arr.Length - 1; i >= 0; i--) arr[i] = matrix.MultiplyPoint3x4(arr[i]);
			}
		}

		public Vector3 InverseTransform (Vector3 point) {
			if (onlyTranslational) return point - translation;
			return inverseMatrix.MultiplyPoint3x4(point);
		}

		public Vector3 InverseTransformVector (Vector3 dir) {
			if (onlyTranslational) return dir;
			return inverseMatrix.MultiplyVector(dir);
		}

		public Int3 InverseTransform (Int3 point) {
			if (onlyTranslational) return point - i3translation;
			return (Int3)inverseMatrix.MultiplyPoint3x4((Vector3)point);
		}

		public void InverseTransform (Int3[] arr) {
			for (int i = arr.Length - 1; i >= 0; i--) arr[i] = (Int3)inverseMatrix.MultiplyPoint3x4((Vector3)arr[i]);
		}

		public void InverseTransform (UnsafeSpan<Int3> arr) {
			for (int i = arr.Length - 1; i >= 0; i--) arr[i] = (Int3)inverseMatrix.MultiplyPoint3x4((Vector3)arr[i]);
		}

		public static GraphTransform operator * (GraphTransform lhs, Matrix4x4 rhs) {
			return new GraphTransform(lhs.matrix * rhs);
		}

		public static GraphTransform operator * (Matrix4x4 lhs, GraphTransform rhs) {
			return new GraphTransform(lhs * rhs.matrix);
		}

		public Bounds Transform (Bounds bounds) {
			if (onlyTranslational) return new Bounds(bounds.center + translation, bounds.size);

			var corners = ArrayPool<Vector3>.Claim(8);
			var extents = bounds.extents;
			corners[0] = Transform(bounds.center + new Vector3(extents.x, extents.y, extents.z));
			corners[1] = Transform(bounds.center + new Vector3(extents.x, extents.y, -extents.z));
			corners[2] = Transform(bounds.center + new Vector3(extents.x, -extents.y, extents.z));
			corners[3] = Transform(bounds.center + new Vector3(extents.x, -extents.y, -extents.z));
			corners[4] = Transform(bounds.center + new Vector3(-extents.x, extents.y, extents.z));
			corners[5] = Transform(bounds.center + new Vector3(-extents.x, extents.y, -extents.z));
			corners[6] = Transform(bounds.center + new Vector3(-extents.x, -extents.y, extents.z));
			corners[7] = Transform(bounds.center + new Vector3(-extents.x, -extents.y, -extents.z));

			var min = corners[0];
			var max = corners[0];
			for (int i = 1; i < 8; i++) {
				min = Vector3.Min(min, corners[i]);
				max = Vector3.Max(max, corners[i]);
			}
			ArrayPool<Vector3>.Release(ref corners);
			return new Bounds((min+max)*0.5f, max - min);
		}

		public Bounds InverseTransform (Bounds bounds) {
			if (onlyTranslational) return new Bounds(bounds.center - translation, bounds.size);

			var corners = ArrayPool<Vector3>.Claim(8);
			var extents = bounds.extents;
			corners[0] = InverseTransform(bounds.center + new Vector3(extents.x, extents.y, extents.z));
			corners[1] = InverseTransform(bounds.center + new Vector3(extents.x, extents.y, -extents.z));
			corners[2] = InverseTransform(bounds.center + new Vector3(extents.x, -extents.y, extents.z));
			corners[3] = InverseTransform(bounds.center + new Vector3(extents.x, -extents.y, -extents.z));
			corners[4] = InverseTransform(bounds.center + new Vector3(-extents.x, extents.y, extents.z));
			corners[5] = InverseTransform(bounds.center + new Vector3(-extents.x, extents.y, -extents.z));
			corners[6] = InverseTransform(bounds.center + new Vector3(-extents.x, -extents.y, extents.z));
			corners[7] = InverseTransform(bounds.center + new Vector3(-extents.x, -extents.y, -extents.z));

			var min = corners[0];
			var max = corners[0];
			for (int i = 1; i < 8; i++) {
				min = Vector3.Min(min, corners[i]);
				max = Vector3.Max(max, corners[i]);
			}
			ArrayPool<Vector3>.Release(ref corners);
			return new Bounds((min+max)*0.5f, max - min);
		}

		#region IMovementPlane implementation

		/// <summary>
		/// Transforms from world space to the 'ground' plane of the graph.
		/// The transformation is purely a rotation so no scale or offset is used.
		///
		/// For a graph rotated with the rotation (-90, 0, 0) this will transform
		/// a coordinate (x,y,z) to (x,y). For a graph with the rotation (0,0,0)
		/// this will tranform a coordinate (x,y,z) to (x,z). More generally for
		/// a graph with a quaternion rotation R this will transform a vector V
		/// to R * V (i.e rotate the vector V using the rotation R).
		/// </summary>
		Vector2 IMovementPlane.ToPlane (Vector3 point) {
			// These special cases cover most graph orientations used in practice.
			// Having them here improves performance in those cases by a factor of
			// 2.5 without impacting the generic case in any significant way.
			if (isXY) return new Vector2(point.x, point.y);
			if (!isXZ) point = inverseRotation * point;
			return new Vector2(point.x, point.z);
		}

		/// <summary>
		/// Transforms from world space to the 'ground' plane of the graph.
		/// The transformation is purely a rotation so no scale or offset is used.
		/// </summary>
		Vector2 IMovementPlane.ToPlane (Vector3 point, out float elevation) {
			if (!isXZ) point = inverseRotation * point;
			elevation = point.y;
			return new Vector2(point.x, point.z);
		}

		/// <summary>
		/// Transforms from the 'ground' plane of the graph to world space.
		/// The transformation is purely a rotation so no scale or offset is used.
		/// </summary>
		Vector3 IMovementPlane.ToWorld (Vector2 point, float elevation) {
			return rotation * new Vector3(point.x, elevation, point.y);
		}

		public SimpleMovementPlane ToSimpleMovementPlane () {
			return new SimpleMovementPlane(rotation);
		}

		#endregion

		/// <summary>Copies the data in this transform to another mutable graph transform</summary>
		public void CopyTo (MutableGraphTransform graphTransform) {
			graphTransform.isXY = isXY;
			graphTransform.isXZ = isXZ;
			graphTransform.isOnlyTranslational = isOnlyTranslational;
			graphTransform.isIdentity = isIdentity;
			graphTransform.matrix = matrix;
			graphTransform.inverseMatrix = inverseMatrix;
			graphTransform.up = up;
			graphTransform.translation = translation;
			graphTransform.i3translation = i3translation;
			graphTransform.rotation = rotation;
			graphTransform.inverseRotation = inverseRotation;
		}
	}
}