private Vector2 CalculateAccelerationToReachPoint(Vector2 deltaPosition, Vector2 targetVelocity, Vector2 currentVelocity)
{
if (targetVelocity == Vector2.zero)
{
float num = 0.05f;
float num2 = 10f;
while (num2 - num > 0.01f)
{
float num3 = (num2 + num) * 0.5f;
Vector2 a = (6f * deltaPosition - 4f * num3 * currentVelocity) / (num3 * num3);
Vector2 a2 = 6f * (num3 * currentVelocity - 2f * deltaPosition) / (num3 * num3 * num3);
if (a.sqrMagnitude > this.acceleration * this.acceleration || (a + a2 * num3).sqrMagnitude > this.acceleration * this.acceleration)
{
num = num3;
}
else
{
num2 = num3;
}
}
return((6f * deltaPosition - 4f * num2 * currentVelocity) / (num2 * num2));
}
float magnitude = deltaPosition.magnitude;
float magnitude2 = currentVelocity.magnitude;
float num4;
Vector2 a3 = VectorMath.Normalize(targetVelocity, out num4);
return((deltaPosition - a3 * Math.Min(0.5f * magnitude * num4 / (magnitude2 + num4), this.maxSpeed * 2f)).normalized * this.acceleration);
}
/** Calculate an acceleration to move deltaPosition units and get there with approximately a velocity of targetVelocity.
*
* When preciseSlowdown is false, only the requirement that we should reach the target is used, not that
* our velocity should be zero when we reach the target.
*/
Vector2 CalculateAccelerationToReachPoint(Vector2 deltaPosition, Vector2 targetVelocity, Vector2 currentVelocity)
{
// If the target velocity is zero we can use a more fancy approach
// and calculate a nicer path.
// In particular, this is the case at the end of the path.
if (targetVelocity == Vector2.zero)
{
// Run a binary search over the time to get
// to the target point.
float mn = 0.05f;
float mx = 10;
while (mx - mn > 0.01f)
{
var time = (mx + mn) * 0.5f;
// Given that we want to move \a deltaPosition units from out current position, that our current velocity is given and
// that when we reach the target we want our velocity to be zero. Also assume that our acceleration will
// vary linearly during the slowdown. Then we can calculate what our acceleration should be during this frame.
//{ t = slowdownTime
//{ deltaPosition = vt + at^2/2 + qt^3/6
//{ 0 = v + at + qt^2/2
//{ solve for a
var a2 = (6 * deltaPosition - 4 * time * currentVelocity) / (time * time);
var q2 = 6 * (time * currentVelocity - 2 * deltaPosition) / (time * time * time);
// Make sure the acceleration is not greater than our maximum allowed acceleration.
// If it is we increase the time we want to use to get to the target
// and if it is not, we decrease the time to get there faster.
if (a2.sqrMagnitude > acceleration * acceleration || (a2 + q2 * time).sqrMagnitude > acceleration * acceleration)
{
mn = time;
}
else
{
mx = time;
}
}
var a = (6 * deltaPosition - 4 * mx * currentVelocity) / (mx * mx);
return(a);
}
else
{
var distance = deltaPosition.magnitude;
// How much to strive for making sure we reach the target point with the target velocity
const float TargetVelocityWeight = 0.5f;
// Limit to how much to care about the target velocity. In seconds.
// This prevents the character from moving away from the path too much when the target point is far away
const float TargetVelocityWeightLimit = 2;
float currentSpeed = currentVelocity.magnitude;
float targetSpeed;
var normalizedTargetVelocity = VectorMath.Normalize(targetVelocity, out targetSpeed);
var targetPoint = deltaPosition - normalizedTargetVelocity * System.Math.Min(TargetVelocityWeight * distance * targetSpeed / (currentSpeed + targetSpeed), maxSpeed * TargetVelocityWeightLimit);
return(targetPoint.normalized * acceleration);
}
}
void TraverseFunnel(RichFunnel fn, float deltaTime, out Vector3 nextPosition, out Quaternion nextRotation)
{
// Clamp the current position to the navmesh
// and update the list of upcoming corners in the path
// and store that in the 'nextCorners' field
var position3D = UpdateTarget(fn);
float elevation;
Vector2 position = movementPlane.ToPlane(position3D, out elevation);
// Only find nearby walls every 5th frame to improve performance
if (Time.frameCount % 5 == 0 && wallForce > 0 && wallDist > 0)
{
wallBuffer.Clear();
fn.FindWalls(wallBuffer, wallDist);
}
// Target point
steeringTarget = nextCorners[0];
Vector2 targetPoint = movementPlane.ToPlane(steeringTarget);
// Direction to target
Vector2 dir = targetPoint - position;
// Normalized direction to the target
Vector2 normdir = VectorMath.Normalize(dir, out distanceToSteeringTarget);
// Calculate force from walls
Vector2 wallForceVector = CalculateWallForce(position, elevation, normdir);
Vector2 targetVelocity;
if (approachingPartEndpoint)
{
targetVelocity = slowdownTime > 0 ? Vector2.zero : normdir * maxSpeed;
// Reduce the wall avoidance force as we get closer to our target
wallForceVector *= System.Math.Min(distanceToSteeringTarget / 0.5f, 1);
if (distanceToSteeringTarget <= endReachedDistance)
{
// Reached the end of the path or an off mesh link
NextPart();
}
}
else
{
var nextNextCorner = nextCorners.Count > 1 ? movementPlane.ToPlane(nextCorners[1]) : position + 2 * dir;
targetVelocity = (nextNextCorner - targetPoint).normalized * maxSpeed;
}
var forwards = movementPlane.ToPlane(simulatedRotation * (orientation == OrientationMode.YAxisForward ? Vector3.up : Vector3.forward));
Vector2 accel = MovementUtilities.CalculateAccelerationToReachPoint(targetPoint - position, targetVelocity, velocity2D, acceleration, rotationSpeed, maxSpeed, forwards);
// Update the velocity using the acceleration
velocity2D += (accel + wallForceVector * wallForce) * deltaTime;
// Distance to the end of the path (almost as the crow flies)
var distanceToEndOfPath = distanceToSteeringTarget + Vector3.Distance(steeringTarget, fn.exactEnd);
var slowdownFactor = distanceToEndOfPath < maxSpeed *slowdownTime?Mathf.Sqrt(distanceToEndOfPath / (maxSpeed *slowdownTime)) : 1;
FinalMovement(position3D, deltaTime, distanceToEndOfPath, slowdownFactor, out nextPosition, out nextRotation);
}
private void TraverseFunnel(RichFunnel fn, float deltaTime)
{
float elevation;
Vector2 vector = this.movementPlane.ToPlane(this.UpdateTarget(fn), out elevation);
if (Time.frameCount % 5 == 0 && this.wallForce > 0f && this.wallDist > 0f)
{
this.wallBuffer.Clear();
fn.FindWalls(this.wallBuffer, this.wallDist);
}
Vector2 vector2 = this.waypoint = this.movementPlane.ToPlane(this.nextCorners[0]);
Vector2 vector3 = vector2 - vector;
object obj = this.lastCorner && this.nextCorners.Count == 1;
Vector2 vector4 = VectorMath.Normalize(vector3, out this.distanceToWaypoint);
Vector2 a = this.CalculateWallForce(vector, elevation, vector4);
object obj2 = obj;
Vector2 targetVelocity;
if (obj2 != null)
{
targetVelocity = ((this.slowdownTime > 0f) ? Vector2.zero : (vector4 * this.maxSpeed));
a *= Math.Min(this.distanceToWaypoint / 0.5f, 1f);
if (this.distanceToWaypoint <= this.endReachedDistance)
{
this.NextPart();
}
}
else
{
targetVelocity = (((this.nextCorners.Count > 1) ? this.movementPlane.ToPlane(this.nextCorners[1]) : (vector + 2f * vector3)) - vector2).normalized * this.maxSpeed;
}
Vector2 a2 = MovementUtilities.CalculateAccelerationToReachPoint(vector2 - vector, targetVelocity, this.velocity2D, this.acceleration, this.maxSpeed);
this.velocity2D += (a2 + a * this.wallForce) * deltaTime;
float distanceToEndOfPath = fn.DistanceToEndOfPath;
float num = (this.slowdownTime > 0f) ? (distanceToEndOfPath / (this.maxSpeed * this.slowdownTime)) : 1f;
this.velocity2D = MovementUtilities.ClampVelocity(this.velocity2D, this.maxSpeed, num, this.slowWhenNotFacingTarget, this.movementPlane.ToPlane(this.rotationIn2D ? this.tr.up : this.tr.forward));
base.ApplyGravity(deltaTime);
if (this.rvoController != null && this.rvoController.enabled)
{
Vector3 pos = this.movementPlane.ToWorld(vector + Vector2.ClampMagnitude(this.velocity2D, distanceToEndOfPath), elevation);
this.rvoController.SetTarget(pos, this.velocity2D.magnitude, this.maxSpeed);
}
Vector2 vector5 = base.CalculateDeltaToMoveThisFrame(vector, distanceToEndOfPath, deltaTime);
float num2 = (obj2 != null) ? Mathf.Clamp01(1.1f * num - 0.1f) : 1f;
this.RotateTowards(vector5, this.rotationSpeed * num2 * deltaTime);
base.Move(this.movementPlane.ToWorld(vector, elevation), this.movementPlane.ToWorld(vector5, this.verticalVelocity * deltaTime));
}
public static float LineCircleIntersectionFactor(Vector3 circleCenter, Vector3 linePoint1, Vector3 linePoint2, float radius)
{
float num;
Vector3 rhs = VectorMath.Normalize(linePoint2 - linePoint1, out num);
Vector3 lhs = linePoint1 - circleCenter;
float num2 = Vector3.Dot(lhs, rhs);
float num3 = num2 * num2 - (lhs.sqrMagnitude - radius * radius);
if (num3 < 0f)
{
num3 = 0f;
}
float num4 = -num2 + Mathf.Sqrt(num3);
return((num <= 1E-05f) ? 0f : (num4 / num));
}
// Token: 0x0600217C RID: 8572 RVA: 0x0018E4A4 File Offset: 0x0018C6A4
private void TraverseFunnel(RichFunnel fn, float deltaTime, out Vector3 nextPosition, out Quaternion nextRotation)
{
Vector3 vector = this.UpdateTarget(fn);
float elevation;
Vector2 vector2 = this.movementPlane.ToPlane(vector, out elevation);
if (Time.frameCount % 5 == 0 && this.wallForce > 0f && this.wallDist > 0f)
{
this.wallBuffer.Clear();
fn.FindWalls(this.wallBuffer, this.wallDist);
}
this.steeringTarget = this.nextCorners[0];
Vector2 vector3 = this.movementPlane.ToPlane(this.steeringTarget);
Vector2 vector4 = vector3 - vector2;
Vector2 vector5 = VectorMath.Normalize(vector4, out this.distanceToSteeringTarget);
Vector2 a = this.CalculateWallForce(vector2, elevation, vector5);
Vector2 targetVelocity;
if (this.approachingPartEndpoint)
{
targetVelocity = ((this.slowdownTime > 0f) ? Vector2.zero : (vector5 * this.maxSpeed));
a *= Math.Min(this.distanceToSteeringTarget / 0.5f, 1f);
if (this.distanceToSteeringTarget <= this.endReachedDistance)
{
this.NextPart();
}
}
else
{
targetVelocity = (((this.nextCorners.Count > 1) ? this.movementPlane.ToPlane(this.nextCorners[1]) : (vector2 + 2f * vector4)) - vector3).normalized * this.maxSpeed;
}
Vector2 forwardsVector = this.movementPlane.ToPlane(this.simulatedRotation * (this.rotationIn2D ? Vector3.up : Vector3.forward));
Vector2 a2 = MovementUtilities.CalculateAccelerationToReachPoint(vector3 - vector2, targetVelocity, this.velocity2D, this.acceleration, this.rotationSpeed, this.maxSpeed, forwardsVector);
this.velocity2D += (a2 + a * this.wallForce) * deltaTime;
float num = this.distanceToSteeringTarget + Vector3.Distance(this.steeringTarget, fn.exactEnd);
float slowdownFactor = (num < this.maxSpeed * this.slowdownTime) ? Mathf.Sqrt(num / (this.maxSpeed * this.slowdownTime)) : 1f;
this.FinalMovement(vector, deltaTime, num, slowdownFactor, out nextPosition, out nextRotation);
}
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);
}
}
void TraverseFunnel(RichFunnel fn, float deltaTime)
{
// Clamp the current position to the navmesh
// and update the list of upcoming corners in the path
// and store that in the 'nextCorners' variable
float elevation;
Vector2 position = movementPlane.ToPlane(UpdateTarget(fn), out elevation);
// Only find nearby walls every 5th frame to improve performance
if (Time.frameCount % 5 == 0 && wallForce > 0 && wallDist > 0)
{
wallBuffer.Clear();
fn.FindWalls(wallBuffer, wallDist);
}
// Target point
Vector2 targetPoint = waypoint = movementPlane.ToPlane(nextCorners[0]);
// Direction to target
Vector2 dir = targetPoint - position;
// Is the endpoint of the path (part) the current target point
bool targetIsEndPoint = lastCorner && nextCorners.Count == 1;
// Normalized direction to the target
Vector2 normdir = VectorMath.Normalize(dir, out distanceToWaypoint);
// Calculate force from walls
Vector2 wallForceVector = CalculateWallForce(position, elevation, normdir);
Vector2 targetVelocity;
if (targetIsEndPoint)
{
targetVelocity = slowdownTime > 0 ? Vector2.zero : normdir * maxSpeed;
// 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 = nextCorners.Count > 1 ? movementPlane.ToPlane(nextCorners[1]) : position + 2 * dir;
targetVelocity = (nextNextCorner - targetPoint).normalized * maxSpeed;
}
Vector2 accel = MovementUtilities.CalculateAccelerationToReachPoint(targetPoint - position, targetVelocity, velocity2D, acceleration, maxSpeed);
// Update the velocity using the acceleration
velocity2D += (accel + wallForceVector * wallForce) * deltaTime;
// Distance to the end of the path (as the crow flies)
var distToEndOfPath = fn.DistanceToEndOfPath;
var slowdownFactor = slowdownTime > 0 ? distToEndOfPath / (maxSpeed * slowdownTime) : 1;
velocity2D = MovementUtilities.ClampVelocity(velocity2D, maxSpeed, slowdownFactor, slowWhenNotFacingTarget, movementPlane.ToPlane(tr.forward));
ApplyGravity(deltaTime);
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 to 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 = movementPlane.ToWorld(position + Vector2.ClampMagnitude(velocity2D, distToEndOfPath), elevation);
rvoController.SetTarget(rvoTarget, velocity2D.magnitude, maxSpeed);
}
// Direction and distance to move during this frame
var deltaPosition = CalculateDeltaToMoveThisFrame(position, distToEndOfPath, deltaTime);
// Rotate towards the direction we are moving in
// Slow down the rotation of the character very close to the endpoint of the path to prevent oscillations
var rotationSpeedFactor = targetIsEndPoint ? Mathf.Clamp01(1.1f * slowdownFactor - 0.1f) : 1f;
RotateTowards(deltaPosition, rotationSpeed * rotationSpeedFactor * deltaTime);
Move(movementPlane.ToWorld(position, elevation), movementPlane.ToWorld(deltaPosition, verticalVelocity * deltaTime));
}
/** 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);
}
}
}
