123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215 |
- using UnityEngine;
- namespace Pathfinding.Util {
-
-
-
-
-
-
-
-
-
- public interface IMovementPlane {
- Vector2 ToPlane(Vector3 p);
- Vector2 ToPlane(Vector3 p, out float elevation);
- Vector3 ToWorld(Vector2 p, float elevation = 0);
- }
-
- public interface ITransform {
- Vector3 Transform(Vector3 position);
- Vector3 InverseTransform(Vector3 position);
- }
-
-
-
-
- public class GraphTransform : IMovementPlane, ITransform {
-
- public readonly bool identity;
-
- public readonly bool onlyTranslational;
- readonly bool isXY;
- readonly bool isXZ;
- readonly Matrix4x4 matrix;
- readonly Matrix4x4 inverseMatrix;
- readonly Vector3 up;
- readonly Vector3 translation;
- readonly Int3 i3translation;
- readonly Quaternion rotation;
- readonly Quaternion inverseRotation;
- public static readonly GraphTransform identityTransform = new GraphTransform(Matrix4x4.identity);
- public GraphTransform (Matrix4x4 matrix) {
- this.matrix = matrix;
- inverseMatrix = matrix.inverse;
- identity = matrix.isIdentity;
- onlyTranslational = MatrixIsTranslational(matrix);
- up = matrix.MultiplyVector(Vector3.up).normalized;
- translation = matrix.MultiplyPoint3x4(Vector3.zero);
- i3translation = (Int3)translation;
-
-
-
-
-
- rotation = Quaternion.LookRotation(TransformVector(Vector3.forward), TransformVector(Vector3.up));
- inverseRotation = Quaternion.Inverse(rotation);
-
- 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 point) {
- if (onlyTranslational) return point;
- return matrix.MultiplyVector(point);
- }
- 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 (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 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 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
-
-
-
-
-
-
-
-
-
-
- Vector2 IMovementPlane.ToPlane (Vector3 point) {
-
-
-
- if (isXY) return new Vector2(point.x, point.y);
- if (!isXZ) point = inverseRotation * point;
- return new Vector2(point.x, point.z);
- }
-
-
-
-
- Vector2 IMovementPlane.ToPlane (Vector3 point, out float elevation) {
- if (!isXZ) point = inverseRotation * point;
- elevation = point.y;
- return new Vector2(point.x, point.z);
- }
-
-
-
-
- Vector3 IMovementPlane.ToWorld (Vector2 point, float elevation) {
- return rotation * new Vector3(point.x, elevation, point.y);
- }
- #endregion
- }
- }
|