From 8722a9920c1f6119bf6e769cba270e63097f8e25 Mon Sep 17 00:00:00 2001
From: chai <215380520@qq.com>
Date: Thu, 23 May 2024 10:08:29 +0800
Subject: + astar project

---
 .../Core/Control/PIDUtilities.cs                   | 48 ++++++++++++++++++++++
 1 file changed, 48 insertions(+)
 create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Control/PIDUtilities.cs

(limited to 'Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Control/PIDUtilities.cs')

diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Control/PIDUtilities.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Control/PIDUtilities.cs
new file mode 100644
index 0000000..3695b37
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Control/PIDUtilities.cs
@@ -0,0 +1,48 @@
+using UnityEngine;
+using Unity.Mathematics;
+using UnityEngine.Assertions;
+using Pathfinding.Util;
+
+namespace Pathfinding.PID {
+	public struct AnglePIDControlOutput2D {
+		/// <summary>How much to rotate in a single time-step. In radians.</summary>
+		public float rotationDelta;
+		public float targetRotation;
+		/// <summary>How much to move in a single time-step. In world units.</summary>
+		public float2 positionDelta;
+
+		public AnglePIDControlOutput2D(float currentRotation, float targetRotation, float rotationDelta, float moveDistance) {
+			var midpointRotation = currentRotation + rotationDelta * 0.5f;
+			math.sincos(midpointRotation, out float s, out float c);
+			this.rotationDelta = rotationDelta;
+			this.positionDelta = new float2(c, s) * moveDistance;
+			this.targetRotation = targetRotation;
+		}
+
+		public static AnglePIDControlOutput2D WithMovementAtEnd (float currentRotation, float targetRotation, float rotationDelta, float moveDistance) {
+			var finalRotation = currentRotation + rotationDelta;
+			math.sincos(finalRotation, out float s, out float c);
+			return new AnglePIDControlOutput2D {
+					   rotationDelta = rotationDelta,
+					   targetRotation = targetRotation,
+					   positionDelta = new float2(c, s) * moveDistance,
+			};
+		}
+	}
+
+	public struct AnglePIDControlOutput {
+		/// <summary>How much to rotate in a single time-step</summary>
+		public quaternion rotationDelta;
+		/// <summary>How much to move in a single time-step. In world units.</summary>
+		public float3 positionDelta;
+		public float maxDesiredWallDistance;
+
+		public AnglePIDControlOutput(NativeMovementPlane movementPlane, AnglePIDControlOutput2D control2D) {
+			this.rotationDelta = movementPlane.ToWorldRotationDelta(-control2D.rotationDelta);
+			this.positionDelta = movementPlane.ToWorld(control2D.positionDelta, 0);
+			this.maxDesiredWallDistance = 0;
+			Assert.IsTrue(math.all(math.isfinite(rotationDelta.value)));
+			Assert.IsTrue(math.all(math.isfinite(positionDelta)));
+		}
+	}
+}
-- 
cgit v1.1-26-g67d0