提交

JNFrame/Assets/Game/Plugins/AstarPathfindingProject/Legacy/LegacyRichAI.cs

@@ -0,0 +1,295 @@
+using UnityEngine;
+using System.Collections;
+using System.Collections.Generic;
+using Pathfinding.RVO;
+
+namespace Pathfinding.Legacy {
+	[RequireComponent(typeof(Seeker))]
+	[AddComponentMenu("Pathfinding/Legacy/AI/Legacy RichAI (3D, for navmesh)")]
+	/// <summary>
+	/// Advanced AI for navmesh based graphs.
+	///
+	/// Deprecated: Use the RichAI class instead. This class only exists for compatibility reasons.
+	/// </summary>
+	[HelpURL("https://arongranberg.com/astar/documentation/stable/class_pathfinding_1_1_legacy_1_1_legacy_rich_a_i.php")]
+	public class LegacyRichAI : RichAI {
+		/// <summary>
+		/// Use a 3rd degree equation for calculating slowdown acceleration instead of a 2nd degree.
+		/// A 3rd degree equation can also make sure that the velocity when reaching the target is roughly zero and therefore
+		/// it will have a more direct stop. In contrast solving a 2nd degree equation which will just make sure the target is reached but
+		/// will usually have a larger velocity when reaching the target and therefore look more "bouncy".
+		/// </summary>
+		public bool preciseSlowdown = true;
+
+		public bool raycastingForGroundPlacement = false;
+
+		/// <summary>
+		/// Current velocity of the agent.
+		/// Includes eventual velocity due to gravity
+		/// </summary>
+		new Vector3 velocity;
+
+		Vector3 lastTargetPoint;
+		Vector3 currentTargetDirection;
+
+		public override void OnSyncLoad()
+		{
+			base.OnSyncLoad();
+			if (rvoController != null) {
+				if (rvoController is LegacyRVOController) (rvoController as LegacyRVOController).enableRotation = false;
+				else Debug.LogError("The LegacyRichAI component only works with the legacy RVOController, not the latest one. Please upgrade this component", this);
+			}
+		}
+
+		/// <summary>Smooth delta time to avoid getting overly affected by e.g GC</summary>
+		static float deltaTime;
+
+		public override void OnSyncUpdate(int dt, JNFrameInfo frame, Object input)
+		{
+			base.OnSyncUpdate(dt, frame, input);
+			deltaTime = Mathf.Min(Time.smoothDeltaTime*2, Time.deltaTime);
+
+			if (richPath != null) {
+				//System.Diagnostics.Stopwatch w = new System.Diagnostics.Stopwatch();
+				//w.Start();
+				RichPathPart pt = richPath.GetCurrentPart();
+				var fn = pt as RichFunnel;
+				if (fn != null) {
+					//Clear buffers for reuse
+					Vector3 position = UpdateTarget(fn);
+
+					//tr.position = ps;
+
+					//Only get walls every 5th frame to save on performance
+					if (Time.frameCount % 5 == 0 && wallForce > 0 && wallDist > 0) {
+						wallBuffer.Clear();
+						fn.FindWalls(wallBuffer, wallDist);
+					}
+
+					/*for (int i=0;i<wallBuffer.Count;i+=2) {
+					 *  Debug.DrawLine (wallBuffer[i],wallBuffer[i+1],Color.magenta);
+					 * }*/
+
+					//Pick next waypoint if current is reached
+					int tgIndex = 0;
+					/*if (buffer.Count > 1) {
+					 *  if ((buffer[tgIndex]-tr.position).sqrMagnitude < pickNextWaypointDist*pickNextWaypointDist) {
+					 *      tgIndex++;
+					 *  }
+					 * }*/
+
+
+					//Target point
+					Vector3 tg = nextCorners[tgIndex];
+					Vector3 dir = tg-position;
+					dir.y = 0;
+
+					bool passedTarget = Vector3.Dot(dir, currentTargetDirection) < 0;
+					//Check if passed target in another way
+					if (passedTarget && nextCorners.Count-tgIndex > 1) {
+						tgIndex++;
+						tg = nextCorners[tgIndex];
+					}
+
+					if (tg != lastTargetPoint) {
+						currentTargetDirection = (tg - position);
+						currentTargetDirection.y = 0;
+						currentTargetDirection.Normalize();
+						lastTargetPoint = tg;
+						//Debug.DrawRay (tr.position, Vector3.down*2,Color.blue,0.2f);
+					}
+
+					//Direction to target
+					dir = (tg-position);
+					dir.y = 0;
+					float magn = dir.magnitude;
+
+					//Write out for other scripts to read
+					distanceToSteeringTarget = magn;
+
+					//Normalize
+					dir = magn == 0 ? Vector3.zero : dir/magn;
+					Vector3 normdir = dir;
+
+					Vector3 force = Vector3.zero;
+
+					if (wallForce > 0 && wallDist > 0) {
+						float wLeft = 0;
+						float wRight = 0;
+
+						for (int i = 0; i < wallBuffer.Count; i += 2) {
+							Vector3 closest = VectorMath.ClosestPointOnSegment(wallBuffer[i], wallBuffer[i+1], tr.position);
+							float dist = (closest-position).sqrMagnitude;
+
+							if (dist > wallDist*wallDist) continue;
+
+							Vector3 tang = (wallBuffer[i+1]-wallBuffer[i]).normalized;
+
+							//Using the fact that all walls are laid out clockwise (seeing from inside)
+							//Then left and right (ish) can be figured out like this
+							float dot = Vector3.Dot(dir, tang) * (1 - System.Math.Max(0, (2*(dist / (wallDist*wallDist))-1)));
+							if (dot > 0) wRight = System.Math.Max(wRight, dot);
+							else wLeft = System.Math.Max(wLeft, -dot);
+						}
+
+						Vector3 norm = Vector3.Cross(Vector3.up, dir);
+						force = norm*(wRight-wLeft);
+
+						//Debug.DrawRay (tr.position, force, Color.cyan);
+					}
+
+					//Is the endpoint of the path (part) the current target point
+					bool endPointIsTarget = lastCorner && nextCorners.Count-tgIndex == 1;
+
+					if (endPointIsTarget) {
+						//Use 2nd or 3rd degree motion equation to figure out acceleration to reach target in "exact" [slowdownTime] seconds
+
+						//Clamp to avoid divide by zero
+						if (slowdownTime < 0.001f) {
+							slowdownTime = 0.001f;
+						}
+
+						Vector3 diff = tg - position;
+						diff.y = 0;
+
+						if (preciseSlowdown) {
+							//{ t = slowdownTime
+							//{ diff = vt + at^2/2 + qt^3/6
+							//{ 0 = at + qt^2/2
+							//{ solve for a
+							dir = (6*diff - 4*slowdownTime*velocity)/(slowdownTime*slowdownTime);
+						} else {
+							dir = 2*(diff -   slowdownTime*velocity)/(slowdownTime*slowdownTime);
+						}
+						dir = Vector3.ClampMagnitude(dir, acceleration);
+
+						force *= System.Math.Min(magn/0.5f, 1);
+
+						if (magn < endReachedDistance) {
+							//END REACHED
+							NextPart();
+						}
+					} else {
+						dir *= acceleration;
+					}
+
+					//Debug.DrawRay (tr.position+Vector3.up, dir*3, Color.blue);
+
+					velocity += (dir + force*wallForce)*deltaTime;
+
+					if (slowWhenNotFacingTarget) {
+						float dot = (Vector3.Dot(normdir, tr.forward)+0.5f)*(1.0f/1.5f);
+						//velocity = Vector3.ClampMagnitude (velocity, maxSpeed * Mathf.Max (dot, 0.2f) );
+						float xzmagn = Mathf.Sqrt(velocity.x*velocity.x + velocity.z*velocity.z);
+						float prevy = velocity.y;
+						velocity.y = 0;
+						float mg = Mathf.Min(xzmagn, maxSpeed * Mathf.Max(dot, 0.2f));
+						velocity = Vector3.Lerp(tr.forward * mg, velocity.normalized * mg, Mathf.Clamp(endPointIsTarget ? (magn*2) : 0, 0.5f, 1.0f));
+
+						velocity.y = prevy;
+					} else {
+						// Clamp magnitude on the XZ axes
+						float xzmagn = Mathf.Sqrt(velocity.x*velocity.x + velocity.z*velocity.z);
+						xzmagn = maxSpeed/xzmagn;
+						if (xzmagn < 1) {
+							velocity.x *= xzmagn;
+							velocity.z *= xzmagn;
+							//Vector3.ClampMagnitude (velocity, maxSpeed);
+						}
+					}
+
+					//Debug.DrawLine (tr.position, tg, lastCorner ? Color.red : Color.green);
+
+
+					if (endPointIsTarget) {
+						Vector3 trotdir = Vector3.Lerp(velocity, currentTargetDirection, System.Math.Max(1 - magn*2, 0));
+						RotateTowards(trotdir);
+					} else {
+						RotateTowards(velocity);
+					}
+
+					//Applied after rotation to enable proper checks on if velocity is zero
+					velocity += deltaTime * gravity;
+
+					if (rvoController != null && rvoController.enabled) {
+						//Use RVOController
+						tr.position = position;
+						rvoController.Move(velocity);
+					} else
+					if (controller != null && controller.enabled) {
+						//Use CharacterController
+						tr.position = position;
+						controller.Move(velocity * deltaTime);
+					} else {
+						//Use Transform
+						float lasty = position.y;
+						position += velocity*deltaTime;
+
+						position = RaycastPosition(position, lasty);
+
+						tr.position = position;
+					}
+				} else {
+					if (rvoController != null && rvoController.enabled) {
+						//Use RVOController
+						rvoController.Move(Vector3.zero);
+					}
+				}
+
+				if (pt is RichSpecial) {
+					if (!traversingOffMeshLink) {
+						StartCoroutine(TraverseSpecial(pt as RichSpecial));
+					}
+				}
+				//w.Stop();
+				//Debug.Log ((w.Elapsed.TotalMilliseconds*1000));
+			} else {
+				if (rvoController != null && rvoController.enabled) {
+					//Use RVOController
+					rvoController.Move(Vector3.zero);
+				} else
+				if (controller != null && controller.enabled) {
+				} else {
+					tr.position = RaycastPosition(tr.position, tr.position.y);
+				}
+			}
+
+			UpdateVelocity();
+			lastDeltaTime = Time.deltaTime;
+		}
+
+		/// <summary>Update is called once per frame</summary>
+
+		new Vector3 RaycastPosition (Vector3 position, float lasty) {
+			if (raycastingForGroundPlacement) {
+				RaycastHit hit;
+				float up = Mathf.Max(height*0.5f, lasty-position.y+height*0.5f);
+
+				if (Physics.Raycast(position+Vector3.up*up, Vector3.down, out hit, up, groundMask)) {
+					if (hit.distance < up) {
+						//grounded
+						position = hit.point;//.up * -(hit.distance-centerOffset);
+						velocity.y = 0;
+					}
+				}
+			}
+			return position;
+		}
+
+		/// <summary>Rotates along the Y-axis the transform towards trotdir</summary>
+		bool RotateTowards (Vector3 trotdir) {
+			trotdir.y = 0;
+			if (trotdir != Vector3.zero) {
+				Quaternion rot = tr.rotation;
+
+				Vector3 trot = Quaternion.LookRotation(trotdir).eulerAngles;
+				Vector3 eul = rot.eulerAngles;
+				eul.y = Mathf.MoveTowardsAngle(eul.y, trot.y, rotationSpeed*deltaTime);
+				tr.rotation = Quaternion.Euler(eul);
+				//Magic number, should expose as variable
+				return Mathf.Abs(eul.y-trot.y) < 5f;
+			}
+			return false;
+		}
+	}
+}