diff options
Diffstat (limited to 'Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GraphTransform.cs')
| -rw-r--r-- | Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GraphTransform.cs | 575 |
1 files changed, 575 insertions, 0 deletions
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GraphTransform.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GraphTransform.cs new file mode 100644 index 0000000..3e25b20 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GraphTransform.cs @@ -0,0 +1,575 @@ +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; + } + } +} |