// Token: 0x0600217B RID: 8571 RVA: 0x0018E3AC File Offset: 0x0018C5AC
protected override void MovementUpdateInternal(float deltaTime, out Vector3 nextPosition, out Quaternion nextRotation)
{
if (this.updatePosition)
{
this.simulatedPosition = this.tr.position;
}
if (this.updateRotation)
{
this.simulatedRotation = this.tr.rotation;
}
RichPathPart currentPart = this.richPath.GetCurrentPart();
if (currentPart is RichSpecial)
{
if (!this.traversingOffMeshLink)
{
base.StartCoroutine(this.TraverseSpecial(currentPart as RichSpecial));
}
nextPosition = (this.steeringTarget = this.simulatedPosition);
nextRotation = base.rotation;
return;
}
RichFunnel richFunnel = currentPart as RichFunnel;
if (richFunnel != null && !base.isStopped)
{
this.TraverseFunnel(richFunnel, deltaTime, out nextPosition, out nextRotation);
return;
}
this.velocity2D -= Vector2.ClampMagnitude(this.velocity2D, this.acceleration * deltaTime);
this.FinalMovement(this.simulatedPosition, deltaTime, float.PositiveInfinity, 1f, out nextPosition, out nextRotation);
this.steeringTarget = this.simulatedPosition;
}
private void OnPathComplete(Path p)
{
this.waitingForPathCalc = false;
p.Claim(this);
if (p.error)
{
p.Release(this, false);
return;
}
if (this.traversingSpecialPath)
{
this.delayUpdatePath = true;
}
else
{
this.richPath.Initialize(this.seeker, p, true, this.funnelSimplification);
RichFunnel richFunnel = this.richPath.GetCurrentPart() as RichFunnel;
if (richFunnel != null)
{
Vector2 b = this.movementPlane.ToPlane(this.UpdateTarget(richFunnel));
if (this.lastCorner && this.nextCorners.Count == 1)
{
Vector2 a = this.waypoint = this.movementPlane.ToPlane(this.nextCorners[0]);
this.distanceToWaypoint = (a - b).magnitude;
if (this.distanceToWaypoint <= this.endReachedDistance)
{
this.NextPart();
}
}
}
}
p.Release(this, false);
}
protected override void MovementUpdate(float deltaTime)
{
RichPathPart currentPart = this.richPath.GetCurrentPart();
RichFunnel richFunnel = currentPart as RichFunnel;
if (richFunnel != null)
{
this.TraverseFunnel(richFunnel, deltaTime);
}
else if (currentPart is RichSpecial)
{
if (!this.traversingSpecialPath)
{
base.StartCoroutine(this.TraverseSpecial(currentPart as RichSpecial));
}
}
else
{
if (this.rvoController != null && this.rvoController.enabled)
{
this.rvoController.Move(Vector3.zero);
}
base.Move(this.tr.position, Vector3.zero);
}
Vector3 position = this.tr.position;
this.realVelocity = ((deltaTime > 0f) ? ((position - this.prevPosition) / deltaTime) : Vector3.zero);
this.prevPosition = position;
}
// Token: 0x0600217E RID: 8574 RVA: 0x0018E818 File Offset: 0x0018CA18
protected override Vector3 ClampToNavmesh(Vector3 position, out bool positionChanged)
{
if (this.richPath != null)
{
RichFunnel richFunnel = this.richPath.GetCurrentPart() as RichFunnel;
if (richFunnel != null)
{
Vector3 a = richFunnel.ClampToNavmesh(position);
Vector2 vector = this.movementPlane.ToPlane(a - position);
float sqrMagnitude = vector.sqrMagnitude;
if (sqrMagnitude > 1.00000011E-06f)
{
this.velocity2D -= vector * Vector2.Dot(vector, this.velocity2D) / sqrMagnitude;
if (this.rvoController != null && this.rvoController.enabled)
{
this.rvoController.SetCollisionNormal(vector);
}
positionChanged = true;
return(position + this.movementPlane.ToWorld(vector, 0f));
}
}
}
positionChanged = false;
return(position);
}
protected override Vector3 UpdateTarget(RichFunnel fn)
{
Matrix4x4 matrix = this.graph.GetMatrix();
Matrix4x4 inverse = matrix.inverse;
Debug.DrawRay(matrix.MultiplyPoint3x4(this.tr.position), Vector3.up * 2f, Color.red);
Debug.DrawRay(inverse.MultiplyPoint3x4(this.tr.position), Vector3.up * 2f, Color.green);
this.nextCorners.Clear();
Vector3 vector = this.tr.position;
Vector3 vector2 = matrix.MultiplyPoint3x4(vector);
bool flag;
vector2 = fn.Update(vector2, this.nextCorners, 2, out this.lastCorner, out flag);
vector = inverse.MultiplyPoint3x4(vector2);
Debug.DrawRay(vector, Vector3.up * 3f, Color.black);
for (int i = 0; i < this.nextCorners.Count; i++)
{
this.nextCorners[i] = inverse.MultiplyPoint3x4(this.nextCorners[i]);
Debug.DrawRay(this.nextCorners[i], Vector3.up * 3f, Color.yellow);
}
if (flag && !this.waitingForPathCalc)
{
this.UpdatePath();
}
return(vector);
}
protected override Vector3 UpdateTarget (RichFunnel fn) {
Matrix4x4 m = graph.GetMatrix();
Matrix4x4 mi = m.inverse;
Debug.DrawRay(m.MultiplyPoint3x4(tr.position), Vector3.up*2, Color.red);
Debug.DrawRay(mi.MultiplyPoint3x4(tr.position), Vector3.up*2, Color.green);
buffer.Clear();
/* Current position. We read and write to tr.position as few times as possible since doing so
* is much slower than to read and write from/to a local variable
*/
Vector3 position = tr.position;
bool requiresRepath;
// Update, but first convert our position to graph space, then convert the result back to world space
var positionInGraphSpace = m.MultiplyPoint3x4(position);
positionInGraphSpace = fn.Update(positionInGraphSpace, buffer, 2, out lastCorner, out requiresRepath);
position = mi.MultiplyPoint3x4(positionInGraphSpace);
Debug.DrawRay(position, Vector3.up*3, Color.black);
// convert the result to world space from graph space
for (int i = 0; i < buffer.Count; i++) {
buffer[i] = mi.MultiplyPoint3x4(buffer[i]);
Debug.DrawRay(buffer[i], Vector3.up*3, Color.yellow);
}
if (requiresRepath && !waitingForPathCalc) {
UpdatePath();
}
return position;
}
public static void SimplifyPath2(IRaycastableGraph rcg, List <GraphNode> nodes, int start, int end, List <GraphNode> result, Vector3 startPoint, Vector3 endPoint)
{
int count = result.Count;
if (end <= start + 1)
{
result.Add(nodes[start]);
result.Add(nodes[end]);
return;
}
GraphHitInfo graphHitInfo;
if (rcg.Linecast(startPoint, endPoint, nodes[start], out graphHitInfo, result) || result[result.Count - 1] != nodes[end])
{
result.RemoveRange(count, result.Count - count);
int num = -1;
float num2 = float.PositiveInfinity;
for (int i = start + 1; i < end; i++)
{
float num3 = AstarMath.DistancePointSegmentStrict(startPoint, endPoint, (Vector3)nodes[i].position);
if (num == -1 || num3 < num2)
{
num = i;
num2 = num3;
}
}
RichFunnel.SimplifyPath2(rcg, nodes, start, num, result, startPoint, (Vector3)nodes[num].position);
result.RemoveAt(result.Count - 1);
RichFunnel.SimplifyPath2(rcg, nodes, num, end, result, (Vector3)nodes[num].position, endPoint);
}
}
protected override Vector3 UpdateTarget(RichFunnel fn)
{
bool flag;
Matrix4x4 matrix = this.graph.GetMatrix();
Matrix4x4 inverse = matrix.inverse;
Debug.DrawRay(matrix.MultiplyPoint3x4(base.tr.position), (Vector3)(Vector3.up * 2f), Color.red);
Debug.DrawRay(inverse.MultiplyPoint3x4(base.tr.position), (Vector3)(Vector3.up * 2f), Color.green);
base.buffer.Clear();
Vector3 position = base.tr.position;
Vector3 vector2 = matrix.MultiplyPoint3x4(position);
vector2 = fn.Update(vector2, base.buffer, 2, out this.lastCorner, out flag);
position = inverse.MultiplyPoint3x4(vector2);
Debug.DrawRay(position, (Vector3)(Vector3.up * 3f), Color.black);
for (int i = 0; i < base.buffer.Count; i++)
{
base.buffer[i] = inverse.MultiplyPoint3x4(base.buffer[i]);
Debug.DrawRay(base.buffer[i], (Vector3)(Vector3.up * 3f), Color.yellow);
}
if (flag && !base.waitingForPathCalc)
{
this.UpdatePath();
}
return(position);
}
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);
}
protected virtual Vector3 UpdateTarget(RichFunnel fn)
{
this.nextCorners.Clear();
Vector3 vector = this.tr.position;
bool flag;
vector = fn.Update(vector, this.nextCorners, 2, out this.lastCorner, out flag);
if (flag && !this.waitingForPathCalc)
{
this.UpdatePath();
}
return vector;
}
protected override Vector3 ClampToNavmesh(Vector3 position)
{
if (this.richPath != null)
{
RichFunnel richFunnel = this.richPath.GetCurrentPart() as RichFunnel;
if (richFunnel != null)
{
return(richFunnel.ClampToNavmesh(position));
}
}
return(position);
}
// Token: 0x0600217A RID: 8570 RVA: 0x0018E360 File Offset: 0x0018C560
protected virtual Vector3 UpdateTarget(RichFunnel fn)
{
this.nextCorners.Clear();
bool flag;
Vector3 result = fn.Update(this.simulatedPosition, this.nextCorners, 2, out this.lastCorner, out flag);
if (flag && !this.waitingForPathCalculation && this.canSearch)
{
this.SearchPath();
}
return(result);
}
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 void SimplifyPath3(IRaycastableGraph rcg, List <GraphNode> nodes, int start, int end, List <GraphNode> result, Vector3 startPoint, Vector3 endPoint, int depth = 0)
{
if (start == end)
{
result.Add(nodes[start]);
return;
}
if (start + 1 == end)
{
result.Add(nodes[start]);
result.Add(nodes[end]);
return;
}
int count = result.Count;
GraphHitInfo graphHitInfo;
if (rcg.Linecast(startPoint, endPoint, nodes[start], out graphHitInfo, result) || result[result.Count - 1] != nodes[end])
{
result.RemoveRange(count, result.Count - count);
int num = 0;
float num2 = 0f;
for (int i = start + 1; i < end - 1; i++)
{
float num3 = AstarMath.DistancePointSegmentStrict(startPoint, endPoint, (Vector3)nodes[i].position);
if (num3 > num2)
{
num = i;
num2 = num3;
}
}
int num4 = (num + start) / 2;
int num5 = (num + end) / 2;
if (num4 == num5)
{
RichFunnel.SimplifyPath3(rcg, nodes, start, num4, result, startPoint, (Vector3)nodes[num4].position, 0);
result.RemoveAt(result.Count - 1);
RichFunnel.SimplifyPath3(rcg, nodes, num4, end, result, (Vector3)nodes[num4].position, endPoint, depth + 1);
}
else
{
RichFunnel.SimplifyPath3(rcg, nodes, start, num4, result, startPoint, (Vector3)nodes[num4].position, depth + 1);
result.RemoveAt(result.Count - 1);
RichFunnel.SimplifyPath3(rcg, nodes, num4, num5, result, (Vector3)nodes[num4].position, (Vector3)nodes[num5].position, depth + 1);
result.RemoveAt(result.Count - 1);
RichFunnel.SimplifyPath3(rcg, nodes, num5, end, result, (Vector3)nodes[num5].position, endPoint, depth + 1);
}
}
}
protected virtual Vector3 UpdateTarget(RichFunnel fn)
{
nextCorners.Clear();
// Current position. We read and write to tr.position as few times as possible since doing so
// is much slower than to read and write from/to a local variable
bool requiresRepath;
Vector3 position = fn.Update(tr.position, nextCorners, 2, out lastCorner, out requiresRepath);
if (requiresRepath && !waitingForPathCalc)
{
UpdatePath();
}
return(position);
}
protected virtual Vector3 UpdateTarget(RichFunnel fn)
{
nextCorners.Clear();
// This method assumes simulatedPosition is up to date as our current position.
// We read and write to tr.position as few times as possible since doing so
// is much slower than to read and write from/to a local/member variable.
bool requiresRepath;
Vector3 position = fn.Update(simulatedPosition, nextCorners, 2, out lastCorner, out requiresRepath);
if (requiresRepath && !waitingForPathCalculation && canSearch)
{
// TODO: What if canSearch is false? How do we notify other scripts that might be handling the path calculation that a new path needs to be calculated?
SearchPath();
}
return(position);
}
// 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);
}
// Token: 0x06002177 RID: 8567 RVA: 0x0018E20C File Offset: 0x0018C40C
protected override void OnPathComplete(Path p)
{
this.waitingForPathCalculation = false;
p.Claim(this);
if (p.error)
{
p.Release(this, false);
return;
}
if (this.traversingOffMeshLink)
{
this.delayUpdatePath = true;
}
else
{
this.richPath.Initialize(this.seeker, p, true, this.funnelSimplification);
RichFunnel richFunnel = this.richPath.GetCurrentPart() as RichFunnel;
if (richFunnel != null)
{
if (this.updatePosition)
{
this.simulatedPosition = this.tr.position;
}
Vector2 b = this.movementPlane.ToPlane(this.UpdateTarget(richFunnel));
if (this.lastCorner && this.nextCorners.Count == 1)
{
this.steeringTarget = this.nextCorners[0];
Vector2 a = this.movementPlane.ToPlane(this.steeringTarget);
this.distanceToSteeringTarget = (a - b).magnitude;
if (this.distanceToSteeringTarget <= this.endReachedDistance)
{
this.NextPart();
}
}
}
}
p.Release(this, false);
}
// Token: 0x06002140 RID: 8512 RVA: 0x001890A0 File Offset: 0x001872A0
public void Initialize(Seeker seeker, Path path, bool mergePartEndpoints, bool simplificationMode)
{
if (path.error)
{
throw new ArgumentException("Path has an error");
}
List <GraphNode> path2 = path.path;
if (path2.Count == 0)
{
throw new ArgumentException("Path traverses no nodes");
}
this.seeker = seeker;
for (int i = 0; i < this.parts.Count; i++)
{
RichFunnel richFunnel = this.parts[i] as RichFunnel;
RichSpecial richSpecial = this.parts[i] as RichSpecial;
if (richFunnel != null)
{
ObjectPool <RichFunnel> .Release(ref richFunnel);
}
else if (richSpecial != null)
{
ObjectPool <RichSpecial> .Release(ref richSpecial);
}
}
this.Clear();
this.Endpoint = path.vectorPath[path.vectorPath.Count - 1];
for (int j = 0; j < path2.Count; j++)
{
if (path2[j] is TriangleMeshNode)
{
NavmeshBase navmeshBase = AstarData.GetGraph(path2[j]) as NavmeshBase;
if (navmeshBase == null)
{
throw new Exception("Found a TriangleMeshNode that was not in a NavmeshBase graph");
}
RichFunnel richFunnel2 = ObjectPool <RichFunnel> .Claim().Initialize(this, navmeshBase);
richFunnel2.funnelSimplification = simplificationMode;
int num = j;
uint graphIndex = path2[num].GraphIndex;
while (j < path2.Count && (path2[j].GraphIndex == graphIndex || path2[j] is NodeLink3Node))
{
j++;
}
j--;
if (num == 0)
{
richFunnel2.exactStart = path.vectorPath[0];
}
else
{
richFunnel2.exactStart = (Vector3)path2[mergePartEndpoints ? (num - 1) : num].position;
}
if (j == path2.Count - 1)
{
richFunnel2.exactEnd = path.vectorPath[path.vectorPath.Count - 1];
}
else
{
richFunnel2.exactEnd = (Vector3)path2[mergePartEndpoints ? (j + 1) : j].position;
}
richFunnel2.BuildFunnelCorridor(path2, num, j);
this.parts.Add(richFunnel2);
}
else if (NodeLink2.GetNodeLink(path2[j]) != null)
{
NodeLink2 nodeLink = NodeLink2.GetNodeLink(path2[j]);
int num2 = j;
uint graphIndex2 = path2[num2].GraphIndex;
j++;
while (j < path2.Count && path2[j].GraphIndex == graphIndex2)
{
j++;
}
j--;
if (j - num2 > 1)
{
throw new Exception("NodeLink2 path length greater than two (2) nodes. " + (j - num2));
}
if (j - num2 != 0)
{
RichSpecial item = ObjectPool <RichSpecial> .Claim().Initialize(nodeLink, path2[num2]);
this.parts.Add(item);
}
}
}
}
// Token: 0x06000033 RID: 51 RVA: 0x0000417C File Offset: 0x0000257C
public void Initialize(Seeker s, Path p, bool mergePartEndpoints, RichFunnel.FunnelSimplification simplificationMode)
{
if (p.error)
{
throw new ArgumentException("Path has an error");
}
List <GraphNode> path = p.path;
if (path.Count == 0)
{
throw new ArgumentException("Path traverses no nodes");
}
this.seeker = s;
for (int i = 0; i < this.parts.Count; i++)
{
if (this.parts[i] is RichFunnel)
{
ObjectPool <RichFunnel> .Release(this.parts[i] as RichFunnel);
}
else if (this.parts[i] is RichSpecial)
{
ObjectPool <RichSpecial> .Release(this.parts[i] as RichSpecial);
}
}
this.parts.Clear();
this.currentPart = 0;
for (int j = 0; j < path.Count; j++)
{
if (path[j] is TriangleMeshNode)
{
IFunnelGraph graph = AstarData.GetGraph(path[j]) as IFunnelGraph;
RichFunnel richFunnel = ObjectPool <RichFunnel> .Claim().Initialize(this, graph);
richFunnel.funnelSimplificationMode = simplificationMode;
int num = j;
uint graphIndex = path[num].GraphIndex;
while (j < path.Count)
{
if (path[j].GraphIndex != graphIndex && !(path[j] is NodeLink3Node))
{
break;
}
j++;
}
j--;
if (num == 0)
{
richFunnel.exactStart = p.vectorPath[0];
}
else if (mergePartEndpoints)
{
richFunnel.exactStart = (Vector3)path[num - 1].position;
}
else
{
richFunnel.exactStart = (Vector3)path[num].position;
}
if (j == path.Count - 1)
{
richFunnel.exactEnd = p.vectorPath[p.vectorPath.Count - 1];
}
else if (mergePartEndpoints)
{
richFunnel.exactEnd = (Vector3)path[j + 1].position;
}
else
{
richFunnel.exactEnd = (Vector3)path[j].position;
}
richFunnel.BuildFunnelCorridor(path, num, j);
this.parts.Add(richFunnel);
}
else if (path[j] != null && NodeLink2.GetNodeLink(path[j]) != null)
{
NodeLink2 nodeLink = NodeLink2.GetNodeLink(path[j]);
int num2 = j;
uint graphIndex2 = path[num2].GraphIndex;
for (j++; j < path.Count; j++)
{
if (path[j].GraphIndex != graphIndex2)
{
break;
}
}
j--;
if (j - num2 > 1)
{
throw new Exception("NodeLink2 path length greater than two (2) nodes. " + (j - num2));
}
if (j - num2 != 0)
{
RichSpecial item = ObjectPool <RichSpecial> .Claim().Initialize(nodeLink, path[num2]);
this.parts.Add(item);
}
}
}
}
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.wallBuffer.Clear();
richFunnel.FindWalls(this.wallBuffer, this.wallDist);
}
int num = 0;
Vector3 vector2 = this.buffer[num];
Vector3 vector3 = vector2 - vector;
vector3.y = 0f;
bool flag = Vector3.Dot(vector3, this.currentTargetDirection) < 0f;
if (flag && this.buffer.Count - num > 1)
{
num++;
vector2 = this.buffer[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;
float magnitude = vector3.magnitude;
this.distanceToWaypoint = magnitude;
vector3 = ((magnitude != 0f) ? (vector3 / magnitude) : Vector3.zero);
Vector3 vector4 = vector3;
Vector3 vector5 = Vector3.zero;
if (this.wallForce > 0f && this.wallDist > 0f)
{
float num2 = 0f;
float num3 = 0f;
for (int i = 0; i < this.wallBuffer.Count; i += 2)
{
Vector3 vector6 = AstarMath.NearestPointStrict(this.wallBuffer[i], this.wallBuffer[i + 1], this.tr.position);
float sqrMagnitude = (vector6 - vector).sqrMagnitude;
if (sqrMagnitude <= this.wallDist * this.wallDist)
{
Vector3 normalized = (this.wallBuffer[i + 1] - this.wallBuffer[i]).normalized;
float num4 = Vector3.Dot(vector3, normalized) * (1f - Math.Max(0f, 2f * (sqrMagnitude / (this.wallDist * this.wallDist)) - 1f));
if (num4 > 0f)
{
num3 = Math.Max(num3, num4);
}
else
{
num2 = Math.Max(num2, -num4);
}
}
}
Vector3 vector7 = Vector3.Cross(Vector3.up, vector3);
vector5 = vector7 * (num3 - num2);
}
bool flag2 = this.lastCorner && this.buffer.Count - num == 1;
if (flag2)
{
if (this.slowdownTime < 0.001f)
{
this.slowdownTime = 0.001f;
}
Vector3 vector8 = vector2 - vector;
vector8.y = 0f;
if (this.preciseSlowdown)
{
vector3 = (6f * vector8 - 4f * this.slowdownTime * this.velocity) / (this.slowdownTime * this.slowdownTime);
}
else
{
vector3 = 2f * (vector8 - this.slowdownTime * this.velocity) / (this.slowdownTime * this.slowdownTime);
}
vector3 = Vector3.ClampMagnitude(vector3, this.acceleration);
vector5 *= Math.Min(magnitude / 0.5f, 1f);
if (magnitude < this.endReachedDistance)
{
this.NextPart();
}
}
else
{
vector3 *= this.acceleration;
}
this.velocity += (vector3 + vector5 * this.wallForce) * RichAI.deltaTime;
if (this.slowWhenNotFacingTarget)
{
float num5 = (Vector3.Dot(vector4, this.tr.forward) + 0.5f) * 0.6666667f;
float num6 = Mathf.Sqrt(this.velocity.x * this.velocity.x + this.velocity.z * this.velocity.z);
float y = this.velocity.y;
this.velocity.y = 0f;
float num7 = Mathf.Min(num6, this.maxSpeed * Mathf.Max(num5, 0.2f));
this.velocity = Vector3.Lerp(this.tr.forward * num7, this.velocity.normalized * num7, Mathf.Clamp((!flag2) ? 0f : (magnitude * 2f), 0.5f, 1f));
this.velocity.y = y;
}
else
{
float num8 = Mathf.Sqrt(this.velocity.x * this.velocity.x + this.velocity.z * this.velocity.z);
num8 = this.maxSpeed / num8;
if (num8 < 1f)
{
this.velocity.x = this.velocity.x * num8;
this.velocity.z = this.velocity.z * num8;
}
}
if (flag2)
{
Vector3 trotdir = Vector3.Lerp(this.velocity, this.currentTargetDirection, Math.Max(1f - magnitude * 2f, 0f));
this.RotateTowards(trotdir);
}
else
{
this.RotateTowards(this.velocity);
}
this.velocity += RichAI.deltaTime * this.gravity;
if (this.rvoController != null && this.rvoController.enabled)
{
this.tr.position = vector;
this.rvoController.Move(this.velocity);
}
else if (this.controller != null && this.controller.enabled)
{
this.tr.position = vector;
this.controller.Move(this.velocity * RichAI.deltaTime);
}
else
{
float y2 = vector.y;
vector += this.velocity * RichAI.deltaTime;
vector = this.RaycastPosition(vector, y2);
this.tr.position = vector;
}
}
else if (this.rvoController != null && this.rvoController.enabled)
{
this.rvoController.Move(Vector3.zero);
}
if (currentPart is RichSpecial)
{
RichSpecial rs = currentPart as RichSpecial;
if (!this.traversingSpecialPath)
{
base.StartCoroutine(this.TraverseSpecial(rs));
}
}
}
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);
}
}
/** Use this for initialization.
*
* \param s Optionally provide in order to take tag penalties into account. May be null if you do not use a Seeker\
* \param p Path to follow
* \param mergePartEndpoints If true, then adjacent parts that the path is split up in will
* try to use the same start/end points. For example when using a link on a navmesh graph
* Instead of first following the path to the center of the node where the link is and then
* follow the link, the path will be adjusted to go to the exact point where the link starts
* which usually makes more sense.
* \param simplificationMode The path can optionally be simplified. This can be a bit expensive for long paths.
*/
public void Initialize (Seeker s, Path p, bool mergePartEndpoints, RichFunnel.FunnelSimplification simplificationMode) {
if (p.error) throw new System.ArgumentException ("Path has an error");
List<GraphNode> nodes = p.path;
if (nodes.Count == 0) throw new System.ArgumentException ("Path traverses no nodes");
seeker = s;
// Release objects back to object pool
// Yeah, I know, it's casting... but this won't be called much
for (int i=0;i<parts.Count;i++) {
if (parts[i] is RichFunnel) ObjectPool<RichFunnel>.Release (parts[i] as RichFunnel);
else if (parts[i] is RichSpecial) ObjectPool<RichSpecial>.Release (parts[i] as RichSpecial);
}
parts.Clear();
currentPart = 0;
// Initialize new
//Break path into parts
for (int i=0;i<nodes.Count;i++) {
if (nodes[i] is TriangleMeshNode) {
var funnelGraph = AstarData.GetGraph (nodes[i]) as IFunnelGraph;
RichFunnel f = ObjectPool<RichFunnel>.Claim ().Initialize (this, funnelGraph);
f.funnelSimplificationMode = simplificationMode;
int sIndex = i;
uint currentGraphIndex = nodes[sIndex].GraphIndex;
for (; i < nodes.Count; i++) {
if (nodes[i].GraphIndex != currentGraphIndex && !(nodes[i] is NodeLink3Node) ) {
break;
}
}
i--;
if (sIndex == 0) {
f.exactStart = p.vectorPath[0];
} else {
f.exactStart = (Vector3)nodes[mergePartEndpoints ? sIndex-1 : sIndex].position;
}
if (i == nodes.Count-1) {
f.exactEnd = p.vectorPath[p.vectorPath.Count-1];
} else {
f.exactEnd = (Vector3)nodes[mergePartEndpoints ? i+1 : i].position;
}
f.BuildFunnelCorridor (nodes, sIndex, i);
parts.Add (f);
} else if (NodeLink2.GetNodeLink(nodes[i]) != null) {
NodeLink2 nl = NodeLink2.GetNodeLink(nodes[i]);
int sIndex = i;
uint currentGraphIndex = nodes[sIndex].GraphIndex;
for (i++; i < nodes.Count; i++) {
if (nodes[i].GraphIndex != currentGraphIndex) {
break;
}
}
i--;
if (i - sIndex > 1) {
throw new System.Exception("NodeLink2 path length greater than two (2) nodes. " + (i - sIndex));
} else if (i - sIndex == 0) {
//Just continue, it might be the case that a NodeLink was the closest node
continue;
}
RichSpecial rps = ObjectPool<RichSpecial>.Claim().Initialize (nl, nodes[sIndex]);
parts.Add(rps);
}
}
}
protected virtual Vector3 UpdateTarget ( RichFunnel fn ) {
buffer.Clear ();
/* Current position. We read and write to tr.position as few times as possible since doing so
* is much slower than to read and write from/to a local variable
*/
Vector3 position = tr.position;
bool requiresRepath;
position = fn.Update (position, buffer, 2, out lastCorner, out requiresRepath);
if (requiresRepath && !waitingForPathCalc) {
UpdatePath ();
}
return position;
}
/** Update is called once per frame */
protected virtual void Update()
{
deltaTime = Mathf.Min(Time.smoothDeltaTime * 2, Time.deltaTime);
if (rp != null)
{
//System.Diagnostics.Stopwatch w = new System.Diagnostics.Stopwatch();
//w.Start();
RichPathPart pt = rp.GetCurrentPart();
RichFunnel 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)
{
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 = buffer[tgIndex];
Vector3 dir = tg - position;
dir.y = 0;
bool passedTarget = Vector3.Dot(dir, currentTargetDirection) < 0;
//Check if passed target in another way
if (passedTarget && buffer.Count - tgIndex > 1)
{
tgIndex++;
tg = buffer[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
distanceToWaypoint = 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 = AstarMath.NearestPointStrict(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 && buffer.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)
{
RichSpecial rs = pt as RichSpecial;
if (!traversingSpecialPath)
{
StartCoroutine(TraverseSpecial(rs));
}
}
//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);
}
}
}
public void BuildFunnelCorridor(List <GraphNode> nodes, int start, int end)
{
this.exactStart = (nodes[start] as MeshNode).ClosestPointOnNode(this.exactStart);
this.exactEnd = (nodes[end] as MeshNode).ClosestPointOnNode(this.exactEnd);
this.left.Clear();
this.right.Clear();
this.left.Add(this.exactStart);
this.right.Add(this.exactStart);
this.nodes.Clear();
IRaycastableGraph raycastableGraph = this.graph as IRaycastableGraph;
if (raycastableGraph != null && this.funnelSimplificationMode != RichFunnel.FunnelSimplification.None)
{
List <GraphNode> list = ListPool <GraphNode> .Claim(end - start);
switch (this.funnelSimplificationMode)
{
case RichFunnel.FunnelSimplification.Iterative:
this.SimplifyPath(raycastableGraph, nodes, start, end, list, this.exactStart, this.exactEnd);
break;
case RichFunnel.FunnelSimplification.RecursiveBinary:
RichFunnel.SimplifyPath2(raycastableGraph, nodes, start, end, list, this.exactStart, this.exactEnd);
break;
case RichFunnel.FunnelSimplification.RecursiveTrinary:
RichFunnel.SimplifyPath3(raycastableGraph, nodes, start, end, list, this.exactStart, this.exactEnd, 0);
break;
}
if (this.nodes.Capacity < list.Count)
{
this.nodes.Capacity = list.Count;
}
for (int i = 0; i < list.Count; i++)
{
TriangleMeshNode triangleMeshNode = list[i] as TriangleMeshNode;
if (triangleMeshNode != null)
{
this.nodes.Add(triangleMeshNode);
}
}
ListPool <GraphNode> .Release(list);
}
else
{
if (this.nodes.Capacity < end - start)
{
this.nodes.Capacity = end - start;
}
for (int j = start; j <= end; j++)
{
TriangleMeshNode triangleMeshNode2 = nodes[j] as TriangleMeshNode;
if (triangleMeshNode2 != null)
{
this.nodes.Add(triangleMeshNode2);
}
}
}
for (int k = 0; k < this.nodes.Count - 1; k++)
{
this.nodes[k].GetPortal(this.nodes[k + 1], this.left, this.right, false);
}
this.left.Add(this.exactEnd);
this.right.Add(this.exactEnd);
}
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);
}
}
protected virtual Vector3 UpdateTarget(RichFunnel fn)
{
this.buffer.Clear();
Vector3 vector = this.tr.position;
bool flag;
vector = fn.Update(vector, this.buffer, 2, out this.lastCorner, out flag);
if (flag && !this.waitingForPathCalc)
{
this.UpdatePath();
}
return vector;
}
/** Use this for initialization.
*
* \param s Optionally provide in order to take tag penalties into account. May be null if you do not use a Seeker\
* \param p Path to follow
* \param mergePartEndpoints If true, then adjacent parts that the path is split up in will
* try to use the same start/end points. For example when using a link on a navmesh graph
* Instead of first following the path to the center of the node where the link is and then
* follow the link, the path will be adjusted to go to the exact point where the link starts
* which usually makes more sense.
* \param simplificationMode The path can optionally be simplified. This can be a bit expensive for long paths.
*/
public void Initialize(Seeker s, Path p, bool mergePartEndpoints, RichFunnel.FunnelSimplification simplificationMode)
{
if (p.error)
{
throw new System.ArgumentException("Path has an error");
}
List <GraphNode> nodes = p.path;
if (nodes.Count == 0)
{
throw new System.ArgumentException("Path traverses no nodes");
}
seeker = s;
// Release objects back to object pool
// Yeah, I know, it's casting... but this won't be called much
for (int i = 0; i < parts.Count; i++)
{
if (parts[i] is RichFunnel)
{
ObjectPool <RichFunnel> .Release(parts[i] as RichFunnel);
}
else if (parts[i] is RichSpecial)
{
ObjectPool <RichSpecial> .Release(parts[i] as RichSpecial);
}
}
parts.Clear();
currentPart = 0;
// Initialize new
//Break path into parts
for (int i = 0; i < nodes.Count; i++)
{
if (nodes[i] is TriangleMeshNode)
{
var graph = AstarData.GetGraph(nodes[i]);
RichFunnel f = ObjectPool <RichFunnel> .Claim().Initialize(this, graph);
f.funnelSimplificationMode = simplificationMode;
int sIndex = i;
uint currentGraphIndex = nodes[sIndex].GraphIndex;
for (; i < nodes.Count; i++)
{
if (nodes[i].GraphIndex != currentGraphIndex && !(nodes[i] is NodeLink3Node))
{
break;
}
}
i--;
if (sIndex == 0)
{
f.exactStart = p.vectorPath[0];
}
else
{
f.exactStart = (Vector3)nodes[mergePartEndpoints ? sIndex - 1 : sIndex].position;
}
if (i == nodes.Count - 1)
{
f.exactEnd = p.vectorPath[p.vectorPath.Count - 1];
}
else
{
f.exactEnd = (Vector3)nodes[mergePartEndpoints ? i + 1 : i].position;
}
f.BuildFunnelCorridor(nodes, sIndex, i);
parts.Add(f);
}
else if (NodeLink2.GetNodeLink(nodes[i]) != null)
{
NodeLink2 nl = NodeLink2.GetNodeLink(nodes[i]);
int sIndex = i;
uint currentGraphIndex = nodes[sIndex].GraphIndex;
for (i++; i < nodes.Count; i++)
{
if (nodes[i].GraphIndex != currentGraphIndex)
{
break;
}
}
i--;
if (i - sIndex > 1)
{
throw new System.Exception("NodeLink2 path length greater than two (2) nodes. " + (i - sIndex));
}
else if (i - sIndex == 0)
{
//Just continue, it might be the case that a NodeLink was the closest node
continue;
}
RichSpecial rps = ObjectPool <RichSpecial> .Claim().Initialize(nl, nodes[sIndex]);
parts.Add(rps);
}
}
}
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));
}
public void Initialize(Seeker s, Path p, bool mergePartEndpoints, RichFunnel.FunnelSimplification simplificationMode)
{
if (p.error)
{
throw new ArgumentException("Path has an error");
}
List <GraphNode> path = p.path;
if (path.Count == 0)
{
throw new ArgumentException("Path traverses no nodes");
}
this.seeker = s;
for (int i = 0; i < this.parts.Count; i++)
{
RichFunnel funnel = this.parts[i] as RichFunnel;
RichSpecial special = this.parts[i] as RichSpecial;
if (funnel != null)
{
Pathfinding.Util.ObjectPool <RichFunnel> .Release(ref funnel);
}
else if (special != null)
{
Pathfinding.Util.ObjectPool <RichSpecial> .Release(ref special);
}
}
this.parts.Clear();
this.currentPart = 0;
for (int j = 0; j < path.Count; j++)
{
if (path[j] is TriangleMeshNode)
{
NavGraph graph = AstarData.GetGraph(path[j]);
RichFunnel item = Pathfinding.Util.ObjectPool <RichFunnel> .Claim().Initialize(this, graph);
item.funnelSimplificationMode = simplificationMode;
int start = j;
uint graphIndex = path[start].GraphIndex;
while (j < path.Count)
{
if ((path[j].GraphIndex != graphIndex) && !(path[j] is NodeLink3Node))
{
break;
}
j++;
}
j--;
if (start == 0)
{
item.exactStart = p.vectorPath[0];
}
else
{
item.exactStart = (Vector3)path[!mergePartEndpoints ? start : (start - 1)].position;
}
if (j == (path.Count - 1))
{
item.exactEnd = p.vectorPath[p.vectorPath.Count - 1];
}
else
{
item.exactEnd = (Vector3)path[!mergePartEndpoints ? j : (j + 1)].position;
}
item.BuildFunnelCorridor(path, start, j);
this.parts.Add(item);
continue;
}
if (NodeLink2.GetNodeLink(path[j]) != null)
{
NodeLink2 nodeLink = NodeLink2.GetNodeLink(path[j]);
int num5 = j;
uint num6 = path[num5].GraphIndex;
j++;
while (j < path.Count)
{
if (path[j].GraphIndex != num6)
{
break;
}
j++;
}
j--;
if ((j - num5) > 1)
{
throw new Exception("NodeLink2 path length greater than two (2) nodes. " + (j - num5));
}
if ((j - num5) != 0)
{
RichSpecial special2 = Pathfinding.Util.ObjectPool <RichSpecial> .Claim().Initialize(nodeLink, path[num5]);
this.parts.Add(special2);
}
}
}
}
/// <summary>Use this for initialization.</summary>
/// <param name="seeker">Optionally provide in order to take tag penalties into account. May be null if you do not use a Seeker\</param>
/// <param name="path">Path to follow</param>
/// <param name="mergePartEndpoints">If true, then adjacent parts that the path is split up in will
/// try to use the same start/end points. For example when using a link on a navmesh graph
/// Instead of first following the path to the center of the node where the link is and then
/// follow the link, the path will be adjusted to go to the exact point where the link starts
/// which usually makes more sense.</param>
/// <param name="simplificationMode">The path can optionally be simplified. This can be a bit expensive for long paths.</param>
public void Initialize(Seeker seeker, Path path, bool mergePartEndpoints, bool simplificationMode)
{
if (path.error)
{
throw new System.ArgumentException("Path has an error");
}
List <GraphNode> nodes = path.path;
if (nodes.Count == 0)
{
throw new System.ArgumentException("Path traverses no nodes");
}
this.seeker = seeker;
// Release objects back to object pool
// Yeah, I know, it's casting... but this won't be called much
for (int i = 0; i < parts.Count; i++)
{
var funnelPart = parts[i] as RichFunnel;
var specialPart = parts[i] as RichSpecial;
if (funnelPart != null)
{
ObjectPool <RichFunnel> .Release(ref funnelPart);
}
else if (specialPart != null)
{
ObjectPool <RichSpecial> .Release(ref specialPart);
}
}
Clear();
// Initialize new
Endpoint = path.vectorPath[path.vectorPath.Count - 1];
//Break path into parts
for (int i = 0; i < nodes.Count; i++)
{
if (nodes[i] is TriangleMeshNode)
{
var graph = AstarData.GetGraph(nodes[i]) as NavmeshBase;
if (graph == null)
{
throw new System.Exception("Found a TriangleMeshNode that was not in a NavmeshBase graph");
}
RichFunnel f = ObjectPool <RichFunnel> .Claim().Initialize(this, graph);
f.funnelSimplification = simplificationMode;
int sIndex = i;
uint currentGraphIndex = nodes[sIndex].GraphIndex;
for (; i < nodes.Count; i++)
{
if (nodes[i].GraphIndex != currentGraphIndex && !(nodes[i] is NodeLink3Node))
{
break;
}
}
i--;
if (sIndex == 0)
{
f.exactStart = path.vectorPath[0];
}
else
{
f.exactStart = (Vector3)nodes[mergePartEndpoints ? sIndex - 1 : sIndex].position;
}
if (i == nodes.Count - 1)
{
f.exactEnd = path.vectorPath[path.vectorPath.Count - 1];
}
else
{
f.exactEnd = (Vector3)nodes[mergePartEndpoints ? i + 1 : i].position;
}
f.BuildFunnelCorridor(nodes, sIndex, i);
parts.Add(f);
}
else if (NodeLink2.GetNodeLink(nodes[i]) != null)
{
NodeLink2 nl = NodeLink2.GetNodeLink(nodes[i]);
int sIndex = i;
uint currentGraphIndex = nodes[sIndex].GraphIndex;
for (i++; i < nodes.Count; i++)
{
if (nodes[i].GraphIndex != currentGraphIndex)
{
break;
}
}
i--;
if (i - sIndex > 1)
{
throw new System.Exception("NodeLink2 path length greater than two (2) nodes. " + (i - sIndex));
}
else if (i - sIndex == 0)
{
//Just continue, it might be the case that a NodeLink was the closest node
continue;
}
RichSpecial rps = ObjectPool <RichSpecial> .Claim().Initialize(nl, nodes[sIndex]);
parts.Add(rps);
}
else if (!(nodes[i] is PointNode))
{
// Some other graph type which we do not have support for
throw new System.InvalidOperationException("The RichAI movment script can only be used on recast/navmesh graphs. A node of type " + nodes[i].GetType().Name + " was in the path.");
}
}
}
