Skip to content

Instantly share code, notes, and snippets.

@nicolechung
Created October 7, 2014 02:54
Show Gist options
  • Save nicolechung/a62e9fc039eca1839e0e to your computer and use it in GitHub Desktop.
Save nicolechung/a62e9fc039eca1839e0e to your computer and use it in GitHub Desktop.
now it follows the path (before it wasn't) but need to keep rotation on the z-axis
//#define ASTARDEBUG
using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using Pathfinding;
using Pathfinding.RVO;
/** AI for following paths.
* This AI is the default movement script which comes with the A* Pathfinding Project.
* It is in no way required by the rest of the system, so feel free to write your own. But I hope this script will make it easier
* to set up movement for the characters in your game. This script is not written for high performance, so I do not recommend using it for large groups of units.
* \n
* \n
* This script will try to follow a target transform, in regular intervals, the path to that target will be recalculated.
* It will on FixedUpdate try to move towards the next point in the path.
* However it will only move in the forward direction, but it will rotate around it's Y-axis
* to make it reach the target.
*
* \section variables Quick overview of the variables
* In the inspector in Unity, you will see a bunch of variables. You can view detailed information further down, but here's a quick overview.\n
* The #repathRate determines how often it will search for new paths, if you have fast moving targets, you might want to set it to a lower value.\n
* The #target variable is where the AI will try to move, it can be a point on the ground where the player has clicked in an RTS for example.
* Or it can be the player object in a zombie game.\n
* The speed is self-explanatory, so is turningSpeed, however #slowdownDistance might require some explanation.
* It is the approximate distance from the target where the AI will start to slow down. Note that this doesn't only affect the end point of the path
* but also any intermediate points, so be sure to set #forwardLook and #pickNextWaypointDist to a higher value than this.\n
* #pickNextWaypointDist is simply determines within what range it will switch to target the next waypoint in the path.\n
* #forwardLook will try to calculate an interpolated target point on the current segment in the path so that it has a distance of #forwardLook from the AI\n
* Below is an image illustrating several variables as well as some internal ones, but which are relevant for understanding how it works.
* Note that the #forwardLook range will not match up exactly with the target point practically, even though that's the goal.
* \shadowimage{aipath_variables.png}
* This script has many movement fallbacks.
* If it finds a NavmeshController, it will use that, otherwise it will look for a character controller, then for a rigidbody and if it hasn't been able to find any
* it will use Transform.Translate which is guaranteed to always work.
*/
[RequireComponent(typeof(Seeker))]
[AddComponentMenu("Pathfinding/AI/AIPath (generic)")]
public class AIPath2D : MonoBehaviour {
/** Determines how often it will search for new paths.
* If you have fast moving targets or AIs, you might want to set it to a lower value.
* The value is in seconds between path requests.
*/
public float repathRate = 0.5F;
/** Target to move towards.
* The AI will try to follow/move towards this target.
* It can be a point on the ground where the player has clicked in an RTS for example, or it can be the player object in a zombie game.
*/
public Transform target;
/** Enables or disables searching for paths.
* Setting this to false does not stop any active path requests from being calculated or stop it from continuing to follow the current path.
* \see #canMove
*/
public bool canSearch = true;
/** Enables or disables movement.
* \see #canSearch */
public bool canMove = true;
/** Maximum velocity.
* This is the maximum speed in world units per second.
*/
public float speed = 3;
/** Rotation speed.
* Rotation is calculated using Quaternion.SLerp. This variable represents the damping, the higher, the faster it will be able to rotate.
*/
public float turningSpeed = 5;
/** Distance from the target point where the AI will start to slow down.
* Note that this doesn't only affect the end point of the path
* but also any intermediate points, so be sure to set #forwardLook and #pickNextWaypointDist to a higher value than this
*/
public float slowdownDistance = 0.6F;
/** Determines within what range it will switch to target the next waypoint in the path */
public float pickNextWaypointDist = 2;
/** Target point is Interpolated on the current segment in the path so that it has a distance of #forwardLook from the AI.
* See the detailed description of AIPath for an illustrative image */
public float forwardLook = 1;
/** Distance to the end point to consider the end of path to be reached.
* When this has been reached, the AI will not move anymore until the target changes and OnTargetReached will be called.
*/
public float endReachedDistance = 0.2F;
/** Do a closest point on path check when receiving path callback.
* Usually the AI has moved a bit between requesting the path, and getting it back, and there is usually a small gap between the AI
* and the closest node.
* If this option is enabled, it will simulate, when the path callback is received, movement between the closest node and the current
* AI position. This helps to reduce the moments when the AI just get a new path back, and thinks it ought to move backwards to the start of the new path
* even though it really should just proceed forward.
*/
public bool closestOnPathCheck = true;
protected float minMoveScale = 0.05F;
/** Cached Seeker component */
protected Seeker seeker;
/** Cached Transform component */
protected Transform tr;
/** Time when the last path request was sent */
private float lastRepath = -9999;
/** Current path which is followed */
protected Path path;
/** Cached CharacterController component */
protected CharacterController controller;
/** Cached NavmeshController component */
protected NavmeshController navController;
/** Cached Rigidbody component */
protected Rigidbody rigid;
/** Current index in the path which is current target */
protected int currentWaypointIndex = 0;
/** Holds if the end-of-path is reached
* \see TargetReached */
protected bool targetReached = false;
/** Only when the previous path has been returned should be search for a new path */
protected bool canSearchAgain = true;
protected Vector3 lastFoundWaypointPosition;
protected float lastFoundWaypointTime = -9999;
/** Returns if the end-of-path has been reached
* \see targetReached */
public bool TargetReached {
get {
return targetReached;
}
}
/** Holds if the Start function has been run.
* Used to test if coroutines should be started in OnEnable to prevent calculating paths
* in the awake stage (or rather before start on frame 0).
*/
private bool startHasRun = false;
/** Initializes reference variables.
* If you override this function you should in most cases call base.Awake () at the start of it.
* */
protected virtual void Awake () {
seeker = GetComponent<Seeker>();
//This is a simple optimization, cache the transform component lookup
tr = transform;
//Cache some other components (not all are necessarily there)
controller = GetComponent<CharacterController>();
navController = GetComponent<NavmeshController>();
rigid = rigidbody;
}
/** Starts searching for paths.
* If you override this function you should in most cases call base.Start () at the start of it.
* \see OnEnable
* \see RepeatTrySearchPath
*/
protected virtual void Start () {
startHasRun = true;
OnEnable ();
}
/** Run at start and when reenabled.
* Starts RepeatTrySearchPath.
*
* \see Start
*/
protected virtual void OnEnable () {
lastRepath = -9999;
canSearchAgain = true;
lastFoundWaypointPosition = GetFeetPosition ();
if (startHasRun) {
//Make sure we receive callbacks when paths complete
seeker.pathCallback += OnPathComplete;
StartCoroutine (RepeatTrySearchPath ());
}
}
public void OnDisable () {
// Abort calculation of path
if (seeker != null && !seeker.IsDone()) seeker.GetCurrentPath().Error();
// Release current path
if (path != null) path.Release (this);
path = null;
//Make sure we receive callbacks when paths complete
seeker.pathCallback -= OnPathComplete;
}
/** Tries to search for a path every #repathRate seconds.
* \see TrySearchPath
*/
protected IEnumerator RepeatTrySearchPath () {
while (true) {
float v = TrySearchPath ();
yield return new WaitForSeconds (v);
}
}
/** Tries to search for a path.
* Will search for a new path if there was a sufficient time since the last repath and both
* #canSearchAgain and #canSearch are true and there is a target.
*
* \returns The time to wait until calling this function again (based on #repathRate)
*/
public float TrySearchPath () {
if (Time.time - lastRepath >= repathRate && canSearchAgain && canSearch && target != null) {
SearchPath ();
return repathRate;
} else {
//StartCoroutine (WaitForRepath ());
float v = repathRate - (Time.time-lastRepath);
return v < 0 ? 0 : v;
}
}
/** Requests a path to the target */
public virtual void SearchPath () {
if (target == null) throw new System.InvalidOperationException ("Target is null");
lastRepath = Time.time;
//This is where we should search to
Vector3 targetPosition = new Vector3(target.position.x, target.position.y, 0);
canSearchAgain = false;
//Alternative way of requesting the path
//ABPath p = ABPath.Construct (GetFeetPosition(),targetPoint,null);
//seeker.StartPath (p);
//We should search from the current position
seeker.StartPath (GetFeetPosition(), targetPosition);
}
public virtual void OnTargetReached () {
//End of path has been reached
//If you want custom logic for when the AI has reached it's destination
//add it here
//You can also create a new script which inherits from this one
//and override the function in that script
}
/** Called when a requested path has finished calculation.
* A path is first requested by #SearchPath, it is then calculated, probably in the same or the next frame.
* Finally it is returned to the seeker which forwards it to this function.\n
*/
public virtual void OnPathComplete (Path _p) {
ABPath p = _p as ABPath;
if (p == null) throw new System.Exception ("This function only handles ABPaths, do not use special path types");
canSearchAgain = true;
//Claim the new path
p.Claim (this);
// Path couldn't be calculated of some reason.
// More info in p.errorLog (debug string)
if (p.error) {
p.Release (this);
return;
}
//Release the previous path
if (path != null) path.Release (this);
//Replace the old path
path = p;
//Reset some variables
currentWaypointIndex = 0;
targetReached = false;
//The next row can be used to find out if the path could be found or not
//If it couldn't (error == true), then a message has probably been logged to the console
//however it can also be got using p.errorLog
//if (p.error)
if (closestOnPathCheck) {
Vector3 p1 = Time.time - lastFoundWaypointTime < 0.3f ? lastFoundWaypointPosition : p.originalStartPoint;
Vector3 p2 = GetFeetPosition ();
Vector3 dir = p2-p1;
float magn = dir.magnitude;
dir /= magn;
int steps = (int)(magn/pickNextWaypointDist);
for (int i=0;i<=steps;i++) {
CalculateVelocity (p1);
p1 += dir;
}
}
}
public virtual Vector3 GetFeetPosition () {
if (controller != null) {
return tr.position - Vector3.up*controller.height*0.5F;
}
return tr.position;
}
public virtual void Update () {
if (!canMove) { return; }
Vector3 dir = CalculateVelocity (GetFeetPosition());
//Rotate towards targetDirection (filled in by CalculateVelocity)
RotateTowards (targetDirection);
if (navController != null) {
} else if (controller != null) {
controller.SimpleMove (dir);
} else if (rigid != null) {
rigid.AddForce (dir);
} else {
transform.Translate (dir*Time.deltaTime, Space.World);
}
}
/** Point to where the AI is heading.
* Filled in by #CalculateVelocity */
protected Vector3 targetPoint;
/** Relative direction to where the AI is heading.
* Filled in by #CalculateVelocity */
protected Vector3 targetDirection;
protected float XYSqrMagnitude (Vector3 a, Vector3 b) {
float dx = b.x-a.x;
float dz = b.y-a.y;
return dx*dx + dz*dz;
}
/** Calculates desired velocity.
* Finds the target path segment and returns the forward direction, scaled with speed.
* A whole bunch of restrictions on the velocity is applied to make sure it doesn't overshoot, does not look too far ahead,
* and slows down when close to the target.
* /see speed
* /see endReachedDistance
* /see slowdownDistance
* /see CalculateTargetPoint
* /see targetPoint
* /see targetDirection
* /see currentWaypointIndex
*/
protected Vector3 CalculateVelocity (Vector3 currentPosition) {
if (path == null || path.vectorPath == null || path.vectorPath.Count == 0) return Vector3.zero;
List<Vector3> vPath = path.vectorPath;
//Vector3 currentPosition = GetFeetPosition();
if (vPath.Count == 1) {
vPath.Insert (0,currentPosition);
}
if (currentWaypointIndex >= vPath.Count) { currentWaypointIndex = vPath.Count-1; }
if (currentWaypointIndex <= 1) currentWaypointIndex = 1;
while (true) {
if (currentWaypointIndex < vPath.Count-1) {
//There is a "next path segment"
float dist = XYSqrMagnitude (vPath[currentWaypointIndex], currentPosition);
//Mathfx.DistancePointSegmentStrict (vPath[currentWaypointIndex+1],vPath[currentWaypointIndex+2],currentPosition);
if (dist < pickNextWaypointDist*pickNextWaypointDist) {
lastFoundWaypointPosition = currentPosition;
lastFoundWaypointTime = Time.time;
currentWaypointIndex++;
} else {
break;
}
} else {
break;
}
}
Vector3 dir = vPath[currentWaypointIndex] - vPath[currentWaypointIndex-1];
Vector3 targetPosition = CalculateTargetPoint (currentPosition,vPath[currentWaypointIndex-1] , vPath[currentWaypointIndex]);
//vPath[currentWaypointIndex] + Vector3.ClampMagnitude (dir,forwardLook);
dir = targetPosition-currentPosition;
dir.z = 0;
float targetDist = dir.magnitude;
float slowdown = Mathf.Clamp01 (targetDist / slowdownDistance);
this.targetDirection = dir;
this.targetPoint = targetPosition;
if (currentWaypointIndex == vPath.Count-1 && targetDist <= endReachedDistance) {
if (!targetReached) { targetReached = true; OnTargetReached (); }
//Send a move request, this ensures gravity is applied
return Vector3.zero;
}
Vector3 forward = tr.forward;
float dot = Vector2.Dot (dir.normalized,forward);
float sp = speed * Mathf.Max (dot,minMoveScale) * slowdown;
if (Time.deltaTime > 0) {
sp = Mathf.Clamp (sp,0,targetDist/(Time.deltaTime*2));
}
return forward*sp;
}
/** Rotates in the specified direction.
* Rotates around the Y-axis.
* \see turningSpeed
*/
protected virtual void RotateTowards (Vector3 dir) {
if (dir == Vector3.zero) return;
Quaternion rot = tr.rotation;
Quaternion toTarget = Quaternion.LookRotation (dir);
rot = Quaternion.Slerp (rot,toTarget,turningSpeed*Time.deltaTime);
//Vector3 euler = rot.eulerAngles;
//euler.y = 0;
//euler.x = 0;
//rot = Quaternion.Euler (euler);
tr.rotation = rot;
}
/** Calculates target point from the current line segment.
* \param p Current position
* \param a Line segment start
* \param b Line segment end
* The returned point will lie somewhere on the line segment.
* \see #forwardLook
* \todo This function uses .magnitude quite a lot, can it be optimized?
*/
protected Vector3 CalculateTargetPoint (Vector3 p, Vector3 a, Vector3 b) {
a.z = p.z;
b.z = p.z;
float magn = (a-b).magnitude;
if (magn == 0) return a;
float closest = AstarMath.Clamp01 (AstarMath.NearestPointFactor (a, b, p));
Vector3 point = (b-a)*closest + a;
float distance = (point-p).magnitude;
float lookAhead = Mathf.Clamp (forwardLook - distance, 0.0F, forwardLook);
float offset = lookAhead / magn;
offset = Mathf.Clamp (offset+closest,0.0F,1.0F);
return (b-a)*offset + a;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment