/// <summary>
/// Searches for the node the agent is inside.
/// This will also clamp the position to the navmesh
/// and repair the funnel cooridor if the agent moves slightly outside it.
///
/// Returns: True if nodes along the path have been destroyed so that a path recalculation is required
/// </summary>
bool ClampToNavmeshInternal(ref Vector3 position)
{
var previousNode = nodes[currentNode];
if (previousNode.Destroyed)
{
return(true);
}
// Check if we are in the same node as we were in during the last frame and otherwise do a more extensive search
if (previousNode.ContainsPoint(position))
{
return(false);
}
// This part of the code is relatively seldom called
// Most of the time we are still on the same node as during the previous frame
var que = navmeshClampQueue;
var allVisited = navmeshClampList;
var parent = navmeshClampDict;
previousNode.TemporaryFlag1 = true;
parent[previousNode] = null;
que.Enqueue(previousNode);
allVisited.Add(previousNode);
float bestDistance = float.PositiveInfinity;
Vector3 bestPoint = position;
TriangleMeshNode bestNode = null;
while (que.Count > 0)
{
var node = que.Dequeue();
// Snap to the closest point in XZ space (keep the Y coordinate)
// If we would have snapped to the closest point in 3D space, the agent
// might slow down when traversing slopes
var closest = node.ClosestPointOnNodeXZ(position);
var dist = VectorMath.MagnitudeXZ(closest - position);
// Check if this node is any closer than the previous best node.
// Allow for a small margin to both avoid floating point errors and to allow
// moving past very small local minima.
if (dist <= bestDistance * 1.05f + 0.001f)
{
if (dist < bestDistance)
{
bestDistance = dist;
bestPoint = closest;
bestNode = node;
}
for (int i = 0; i < node.connections.Length; i++)
{
var neighbour = node.connections[i].node as TriangleMeshNode;
if (neighbour != null && !neighbour.TemporaryFlag1)
{
neighbour.TemporaryFlag1 = true;
parent[neighbour] = node;
que.Enqueue(neighbour);
allVisited.Add(neighbour);
}
}
}
}
for (int i = 0; i < allVisited.Count; i++)
{
allVisited[i].TemporaryFlag1 = false;
}
allVisited.ClearFast();
var closestNodeInPath = nodes.IndexOf(bestNode);
// Move the x and z coordinates of the chararacter but not the y coordinate
// because the navmesh surface may not line up with the ground
position.x = bestPoint.x;
position.z = bestPoint.z;
// Check if the closest node
// was on the path already or if we need to adjust it
if (closestNodeInPath == -1)
{
// Reuse this list, because why not.
var prefix = navmeshClampList;
while (closestNodeInPath == -1)
{
prefix.Add(bestNode);
bestNode = parent[bestNode];
closestNodeInPath = nodes.IndexOf(bestNode);
}
// We have found a node containing the position, but it is outside the funnel
// Recalculate the funnel to include this node
exactStart = position;
UpdateFunnelCorridor(closestNodeInPath, prefix);
prefix.ClearFast();
// Restart from the first node in the updated path
currentNode = 0;
}
else
{
currentNode = closestNodeInPath;
}
parent.Clear();
// Do a quick check to see if the next node in the path has been destroyed
// If that is the case then we should plan a new path immediately
return(currentNode + 1 < nodes.Count && nodes[currentNode + 1].Destroyed);
}
// Token: 0x060021AA RID: 8618 RVA: 0x0018F7E4 File Offset: 0x0018D9E4
private bool ClampToNavmeshInternal(ref Vector3 position)
{
TriangleMeshNode triangleMeshNode = this.nodes[this.currentNode];
if (triangleMeshNode.Destroyed)
{
return(true);
}
if (triangleMeshNode.ContainsPoint(position))
{
return(false);
}
Queue <TriangleMeshNode> queue = RichFunnel.navmeshClampQueue;
List <TriangleMeshNode> list = RichFunnel.navmeshClampList;
Dictionary <TriangleMeshNode, TriangleMeshNode> dictionary = RichFunnel.navmeshClampDict;
triangleMeshNode.TemporaryFlag1 = true;
dictionary[triangleMeshNode] = null;
queue.Enqueue(triangleMeshNode);
list.Add(triangleMeshNode);
float num = float.PositiveInfinity;
Vector3 vector = position;
TriangleMeshNode triangleMeshNode2 = null;
while (queue.Count > 0)
{
TriangleMeshNode triangleMeshNode3 = queue.Dequeue();
Vector3 vector2 = triangleMeshNode3.ClosestPointOnNodeXZ(position);
float num2 = VectorMath.MagnitudeXZ(vector2 - position);
if (num2 <= num * 1.05f + 0.001f)
{
if (num2 < num)
{
num = num2;
vector = vector2;
triangleMeshNode2 = triangleMeshNode3;
}
for (int i = 0; i < triangleMeshNode3.connections.Length; i++)
{
TriangleMeshNode triangleMeshNode4 = triangleMeshNode3.connections[i].node as TriangleMeshNode;
if (triangleMeshNode4 != null && !triangleMeshNode4.TemporaryFlag1)
{
triangleMeshNode4.TemporaryFlag1 = true;
dictionary[triangleMeshNode4] = triangleMeshNode3;
queue.Enqueue(triangleMeshNode4);
list.Add(triangleMeshNode4);
}
}
}
}
for (int j = 0; j < list.Count; j++)
{
list[j].TemporaryFlag1 = false;
}
list.ClearFast <TriangleMeshNode>();
int num3 = this.nodes.IndexOf(triangleMeshNode2);
position.x = vector.x;
position.z = vector.z;
if (num3 == -1)
{
List <TriangleMeshNode> list2 = RichFunnel.navmeshClampList;
while (num3 == -1)
{
list2.Add(triangleMeshNode2);
triangleMeshNode2 = dictionary[triangleMeshNode2];
num3 = this.nodes.IndexOf(triangleMeshNode2);
}
this.exactStart = position;
this.UpdateFunnelCorridor(num3, list2);
list2.ClearFast <TriangleMeshNode>();
this.currentNode = 0;
}
else
{
this.currentNode = num3;
}
dictionary.Clear();
return(this.currentNode + 1 < this.nodes.Count && this.nodes[this.currentNode + 1].Destroyed);
}
protected virtual void Update()
{
RichAI.deltaTime = Mathf.Min(Time.smoothDeltaTime * 2f, Time.deltaTime);
if (this.rp != null)
{
RichPathPart currentPart = this.rp.GetCurrentPart();
RichFunnel richFunnel = currentPart as RichFunnel;
if (richFunnel != null)
{
Vector3 vector = this.UpdateTarget(richFunnel);
if (Time.frameCount % 5 == 0 && this.wallForce > 0f && this.wallDist > 0f)
{
this.wallBuffer.Clear();
richFunnel.FindWalls(this.wallBuffer, this.wallDist);
}
int num = 0;
Vector3 vector2 = this.nextCorners[num];
Vector3 vector3 = vector2 - vector;
vector3.y = 0f;
bool flag = Vector3.Dot(vector3, this.currentTargetDirection) < 0f;
if (flag && this.nextCorners.Count - num > 1)
{
num++;
vector2 = this.nextCorners[num];
}
if (vector2 != this.lastTargetPoint)
{
this.currentTargetDirection = vector2 - vector;
this.currentTargetDirection.y = 0f;
this.currentTargetDirection.Normalize();
this.lastTargetPoint = vector2;
}
vector3 = vector2 - vector;
vector3.y = 0f;
Vector3 vector4 = VectorMath.Normalize(vector3, out this.distanceToWaypoint);
bool flag2 = this.lastCorner && this.nextCorners.Count - num == 1;
if (flag2 && this.distanceToWaypoint < 0.01f * this.maxSpeed)
{
this.velocity = (vector2 - vector) * 100f;
}
else
{
Vector3 a = this.CalculateWallForce(vector, vector4);
Vector2 vector5;
if (flag2)
{
vector5 = this.CalculateAccelerationToReachPoint(RichAI.To2D(vector2 - vector), Vector2.zero, RichAI.To2D(this.velocity));
a *= Math.Min(this.distanceToWaypoint / 0.5f, 1f);
if (this.distanceToWaypoint < this.endReachedDistance)
{
this.NextPart();
}
}
else
{
Vector3 a2 = (num >= this.nextCorners.Count - 1) ? ((vector2 - vector) * 2f + vector) : this.nextCorners[num + 1];
Vector3 v = (a2 - vector2).normalized * this.maxSpeed;
vector5 = this.CalculateAccelerationToReachPoint(RichAI.To2D(vector2 - vector), RichAI.To2D(v), RichAI.To2D(this.velocity));
}
this.velocity += (new Vector3(vector5.x, 0f, vector5.y) + a * this.wallForce) * RichAI.deltaTime;
}
TriangleMeshNode currentNode = richFunnel.CurrentNode;
Vector3 b;
if (currentNode != null)
{
b = currentNode.ClosestPointOnNode(vector);
}
else
{
b = vector;
}
float magnitude = (richFunnel.exactEnd - b).magnitude;
float num2 = this.maxSpeed;
num2 *= Mathf.Sqrt(Mathf.Min(1f, magnitude / (this.maxSpeed * this.slowdownTime)));
if (this.slowWhenNotFacingTarget)
{
float num3 = Mathf.Max((Vector3.Dot(vector4, this.tr.forward) + 0.5f) / 1.5f, 0.2f);
num2 *= num3;
float num4 = VectorMath.MagnitudeXZ(this.velocity);
float y = this.velocity.y;
this.velocity.y = 0f;
num4 = Mathf.Min(num4, num2);
this.velocity = Vector3.Lerp(this.velocity.normalized * num4, this.tr.forward * num4, Mathf.Clamp((!flag2) ? 1f : (this.distanceToWaypoint * 2f), 0f, 0.5f));
this.velocity.y = y;
}
else
{
this.velocity = VectorMath.ClampMagnitudeXZ(this.velocity, num2);
}
this.velocity += RichAI.deltaTime * this.gravity;
if (this.rvoController != null && this.rvoController.enabled)
{
Vector3 pos = vector + VectorMath.ClampMagnitudeXZ(this.velocity, magnitude);
this.rvoController.SetTarget(pos, VectorMath.MagnitudeXZ(this.velocity), this.maxSpeed);
}
Vector3 vector6;
if (this.rvoController != null && this.rvoController.enabled)
{
vector6 = this.rvoController.CalculateMovementDelta(vector, RichAI.deltaTime);
vector6.y = this.velocity.y * RichAI.deltaTime;
}
else
{
vector6 = this.velocity * RichAI.deltaTime;
}
if (flag2)
{
Vector3 trotdir = Vector3.Lerp(vector6.normalized, this.currentTargetDirection, Math.Max(1f - this.distanceToWaypoint * 2f, 0f));
this.RotateTowards(trotdir);
}
else
{
this.RotateTowards(vector6);
}
if (this.controller != null && this.controller.enabled)
{
this.tr.position = vector;
this.controller.Move(vector6);
vector = this.tr.position;
}
else
{
float y2 = vector.y;
vector += vector6;
vector = this.RaycastPosition(vector, y2);
}
Vector3 vector7 = richFunnel.ClampToNavmesh(vector);
if (vector != vector7)
{
Vector3 vector8 = vector7 - vector;
this.velocity -= vector8 * Vector3.Dot(vector8, this.velocity) / vector8.sqrMagnitude;
if (this.rvoController != null && this.rvoController.enabled)
{
this.rvoController.SetCollisionNormal(vector8);
}
}
this.tr.position = vector7;
}
else if (this.rvoController != null && this.rvoController.enabled)
{
this.rvoController.Move(Vector3.zero);
}
if (currentPart is RichSpecial && !this.traversingSpecialPath)
{
base.StartCoroutine(this.TraverseSpecial(currentPart as RichSpecial));
}
}
else if (this.rvoController != null && this.rvoController.enabled)
{
this.rvoController.Move(Vector3.zero);
}
else if (!(this.controller != null) || !this.controller.enabled)
{
this.tr.position = this.RaycastPosition(this.tr.position, this.tr.position.y);
}
}
/** Update is called once per frame */
protected virtual void Update()
{
deltaTime = Mathf.Min(Time.smoothDeltaTime * 2, Time.deltaTime);
if (rp != null)
{
RichPathPart currentPart = rp.GetCurrentPart();
var fn = currentPart as RichFunnel;
if (fn != null)
{
// Clamp the current position to the navmesh
// and update the list of upcoming corners in the path
// and store that in the 'nextCorners' variable
Vector3 position = UpdateTarget(fn);
// 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);
}
// Target point
int tgIndex = 0;
Vector3 targetPoint = nextCorners[tgIndex];
Vector3 dir = targetPoint - 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++;
targetPoint = nextCorners[tgIndex];
}
// Check if the target point changed compared to last frame
if (targetPoint != lastTargetPoint)
{
currentTargetDirection = targetPoint - position;
currentTargetDirection.y = 0;
currentTargetDirection.Normalize();
lastTargetPoint = targetPoint;
}
// Direction to target
dir = targetPoint - position;
dir.y = 0;
// Normalized direction
Vector3 normdir = VectorMath.Normalize(dir, out distanceToWaypoint);
// Is the endpoint of the path (part) the current target point
bool targetIsEndPoint = lastCorner && nextCorners.Count - tgIndex == 1;
// When very close to the target point, move directly towards the target
// instead of using accelerations as they tend to be a bit jittery in this case
if (targetIsEndPoint && distanceToWaypoint < 0.01f * maxSpeed)
{
// Velocity will be at most 1 times max speed, it will be further clamped below
velocity = (targetPoint - position) * 100;
}
else
{
// Calculate force from walls
Vector3 wallForceVector = CalculateWallForce(position, normdir);
Vector2 accelerationVector;
if (targetIsEndPoint)
{
accelerationVector = CalculateAccelerationToReachPoint(To2D(targetPoint - position), Vector2.zero, To2D(velocity));
//accelerationVector = Vector3.ClampMagnitude(accelerationVector, acceleration);
// Reduce the wall avoidance force as we get closer to our target
wallForceVector *= System.Math.Min(distanceToWaypoint / 0.5f, 1);
if (distanceToWaypoint < endReachedDistance)
{
// END REACHED
NextPart();
}
}
else
{
var nextNextCorner = tgIndex < nextCorners.Count - 1 ? nextCorners[tgIndex + 1] : (targetPoint - position) * 2 + position;
var targetVelocity = (nextNextCorner - targetPoint).normalized * maxSpeed;
accelerationVector = CalculateAccelerationToReachPoint(To2D(targetPoint - position), To2D(targetVelocity), To2D(velocity));
}
// Update the velocity using the acceleration
velocity += (new Vector3(accelerationVector.x, 0, accelerationVector.y) + wallForceVector * wallForce) * deltaTime;
}
var currentNode = fn.CurrentNode;
Vector3 closestOnNode;
if (currentNode != null)
{
closestOnNode = currentNode.ClosestPointOnNode(position);
}
else
{
closestOnNode = position;
}
// Distance to the end of the path (as the crow flies)
var distToEndOfPath = (fn.exactEnd - closestOnNode).magnitude;
// Max speed to use for this frame
var currentMaxSpeed = maxSpeed;
currentMaxSpeed *= Mathf.Sqrt(Mathf.Min(1, distToEndOfPath / (maxSpeed * slowdownTime)));
// Check if the agent should slow down in case it is not facing the direction it wants to move in
if (slowWhenNotFacingTarget)
{
// 1 when normdir is in the same direction as tr.forward
// 0.2 when they point in the opposite directions
float directionSpeedFactor = Mathf.Max((Vector3.Dot(normdir, tr.forward) + 0.5f) / 1.5f, 0.2f);
currentMaxSpeed *= directionSpeedFactor;
float currentSpeed = VectorMath.MagnitudeXZ(velocity);
float prevy = velocity.y;
velocity.y = 0;
currentSpeed = Mathf.Min(currentSpeed, currentMaxSpeed);
// Make sure the agent always moves in the forward direction
// except when getting close to the end of the path in which case
// the velocity can be in any direction
velocity = Vector3.Lerp(velocity.normalized * currentSpeed, tr.forward * currentSpeed, Mathf.Clamp(targetIsEndPoint ? distanceToWaypoint * 2 : 1, 0.0f, 0.5f));
velocity.y = prevy;
}
else
{
velocity = VectorMath.ClampMagnitudeXZ(velocity, currentMaxSpeed);
}
// Apply gravity
velocity += deltaTime * gravity;
if (rvoController != null && rvoController.enabled)
{
// Send a message to the RVOController that we want to move
// with this velocity. In the next simulation step, this velocity
// will be processed and it will be fed back the rvo controller
// and finally it will be used by this script when calling the
// CalculateMovementDelta method below
// Make sure that we don't move further than to the end point of the path
// If the RVO simulation FPS is low and we did not do this, the agent
// might overshoot the target a lot.
var rvoTarget = position + VectorMath.ClampMagnitudeXZ(velocity, distToEndOfPath);
rvoController.SetTarget(rvoTarget, VectorMath.MagnitudeXZ(velocity), maxSpeed);
}
// Direction and distance to move during this frame
Vector3 deltaPosition;
if (rvoController != null && rvoController.enabled)
{
// Use RVOController to get a processed delta position
// such that collisions will be avoided if possible
deltaPosition = rvoController.CalculateMovementDelta(position, deltaTime);
// The RVOController does not know about gravity
// so we copy it from the normal velocity calculation
deltaPosition.y = velocity.y * deltaTime;
}
else
{
deltaPosition = velocity * deltaTime;
}
if (targetIsEndPoint)
{
// Rotate towards the direction that the agent was in
// when the target point was seen for the first time
// TODO: Some magic constants here, should probably compute them from other variables
// or expose them as separate variables
Vector3 trotdir = Vector3.Lerp(deltaPosition.normalized, currentTargetDirection, System.Math.Max(1 - distanceToWaypoint * 2, 0));
RotateTowards(trotdir);
}
else
{
// Rotate towards the direction we are moving in
RotateTowards(deltaPosition);
}
if (controller != null && controller.enabled)
{
// Use CharacterController
tr.position = position;
controller.Move(deltaPosition);
// Grab the position after the movement to be able to take physics into account
position = tr.position;
}
else
{
// Use Transform
float lastY = position.y;
position += deltaPosition;
// Position the character on the ground
position = RaycastPosition(position, lastY);
}
// Clamp the position to the navmesh after movement is done
var clampedPosition = fn.ClampToNavmesh(position);
if (position != clampedPosition)
{
// The agent was outside the navmesh. Remove that component of the velocity
// so that the velocity only goes along the direction of the wall, not into it
var difference = clampedPosition - position;
velocity -= difference * Vector3.Dot(difference, velocity) / difference.sqrMagnitude;
// Make sure the RVO system knows that there was a collision here
// Otherwise other agents may think this agent continued to move forwards
// and avoidance quality may suffer
if (rvoController != null && rvoController.enabled)
{
rvoController.SetCollisionNormal(difference);
}
}
tr.position = clampedPosition;
}
else
{
if (rvoController != null && rvoController.enabled)
{
//Use RVOController
rvoController.Move(Vector3.zero);
}
}
if (currentPart is RichSpecial)
{
// The current path part is a special part, for example a link
// Movement during this part of the path is handled by the TraverseSpecial coroutine
if (!traversingSpecialPath)
{
StartCoroutine(TraverseSpecial(currentPart as RichSpecial));
}
}
}
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);
}
}
}
