NNInfoInternal FindClosestConnectionPoint(PointNode node, Vector3 position)
{
var closestConnectionPoint = (Vector3)node.position;
var conns = node.connections;
var nodePos = (Vector3)node.position;
var bestDist = float.PositiveInfinity;
if (conns != null)
{
for (int i = 0; i < conns.Length; i++)
{
var connectionMidpoint = ((UnityEngine.Vector3)conns[i].node.position + nodePos) * 0.5f;
var closestPoint = VectorMath.ClosestPointOnSegment(nodePos, connectionMidpoint, position);
var dist = (closestPoint - position).sqrMagnitude;
if (dist < bestDist)
{
bestDist = dist;
closestConnectionPoint = closestPoint;
}
}
}
var result = new NNInfoInternal();
result.node = node;
result.clampedPosition = closestConnectionPoint;
return(result);
}
//Movement stuff
/** Returns in which direction to move from a point on the path.
* A simple and quite slow (well, compared to more optimized algorithms) algorithm first finds the closest path segment (from #vectorPath) and then returns
* the direction to the next point from there. The direction is not normalized.
* \returns Direction to move from a \a point, returns Vector3.zero if #vectorPath is null or has a length of 0 */
public Vector3 GetMovementVector(Vector3 point)
{
if (vectorPath == null || vectorPath.Count == 0)
{
return(Vector3.zero);
}
if (vectorPath.Count == 1)
{
return(vectorPath[0] - point);
}
float minDist = float.PositiveInfinity; //Mathf.Infinity;
int minSegment = 0;
for (int i = 0; i < vectorPath.Count - 1; i++)
{
Vector3 closest = VectorMath.ClosestPointOnSegment(vectorPath[i], vectorPath[i + 1], point);
float dist = (closest - point).sqrMagnitude;
if (dist < minDist)
{
minDist = dist;
minSegment = i;
}
}
return(vectorPath[minSegment + 1] - point);
}
private Vector3 CalculateWallForce(Vector3 position, Vector3 directionToTarget)
{
if (this.wallForce > 0f && this.wallDist > 0f)
{
float num = 0f;
float num2 = 0f;
for (int i = 0; i < this.wallBuffer.Count; i += 2)
{
Vector3 a = VectorMath.ClosestPointOnSegment(this.wallBuffer[i], this.wallBuffer[i + 1], this.tr.position);
float sqrMagnitude = (a - position).sqrMagnitude;
if (sqrMagnitude <= this.wallDist * this.wallDist)
{
Vector3 normalized = (this.wallBuffer[i + 1] - this.wallBuffer[i]).normalized;
float num3 = Vector3.Dot(directionToTarget, normalized) * (1f - Math.Max(0f, 2f * (sqrMagnitude / (this.wallDist * this.wallDist)) - 1f));
if (num3 > 0f)
{
num2 = Math.Max(num2, num3);
}
else
{
num = Math.Max(num, -num3);
}
}
}
Vector3 a2 = Vector3.Cross(Vector3.up, directionToTarget);
return(a2 * (num2 - num));
}
return(Vector3.zero);
}
// Token: 0x0600217F RID: 8575 RVA: 0x0018E8E8 File Offset: 0x0018CAE8
private Vector2 CalculateWallForce(Vector2 position, float elevation, Vector2 directionToTarget)
{
if (this.wallForce <= 0f || this.wallDist <= 0f)
{
return(Vector2.zero);
}
float num = 0f;
float num2 = 0f;
Vector3 vector = this.movementPlane.ToWorld(position, elevation);
for (int i = 0; i < this.wallBuffer.Count; i += 2)
{
float sqrMagnitude = (VectorMath.ClosestPointOnSegment(this.wallBuffer[i], this.wallBuffer[i + 1], vector) - vector).sqrMagnitude;
if (sqrMagnitude <= this.wallDist * this.wallDist)
{
Vector2 normalized = this.movementPlane.ToPlane(this.wallBuffer[i + 1] - this.wallBuffer[i]).normalized;
float num3 = Vector2.Dot(directionToTarget, normalized);
float num4 = 1f - Math.Max(0f, 2f * (sqrMagnitude / (this.wallDist * this.wallDist)) - 1f);
if (num3 > 0f)
{
num2 = Math.Max(num2, num3 * num4);
}
else
{
num = Math.Max(num, -num3 * num4);
}
}
}
return(new Vector2(directionToTarget.y, -directionToTarget.x) * (num2 - num));
}
public Vector3 GetMovementVector(Vector3 point)
{
if ((base.vectorPath == null) || (base.vectorPath.Count == 0))
{
return(Vector3.zero);
}
if (base.vectorPath.Count == 1)
{
return(base.vectorPath[0] - point);
}
float positiveInfinity = float.PositiveInfinity;
int num2 = 0;
for (int i = 0; i < (base.vectorPath.Count - 1); i++)
{
Vector3 vector2 = VectorMath.ClosestPointOnSegment(base.vectorPath[i], base.vectorPath[i + 1], point) - point;
float sqrMagnitude = vector2.sqrMagnitude;
if (sqrMagnitude < positiveInfinity)
{
positiveInfinity = sqrMagnitude;
num2 = i;
}
}
return(base.vectorPath[num2 + 1] - point);
}
public Vector3 GetMovementVector(Vector3 point)
{
if (this.vectorPath == null || this.vectorPath.Count == 0)
{
return(Vector3.zero);
}
if (this.vectorPath.Count == 1)
{
return(this.vectorPath[0] - point);
}
float num = float.PositiveInfinity;
int num2 = 0;
for (int i = 0; i < this.vectorPath.Count - 1; i++)
{
Vector3 a = VectorMath.ClosestPointOnSegment(this.vectorPath[i], this.vectorPath[i + 1], point);
float sqrMagnitude = (a - point).sqrMagnitude;
if (sqrMagnitude < num)
{
num = sqrMagnitude;
num2 = i;
}
}
return(this.vectorPath[num2 + 1] - point);
}
Vector2 CalculateWallForce(Vector2 position, float elevation, Vector2 directionToTarget)
{
if (wallForce <= 0 || wallDist <= 0)
{
return(Vector2.zero);
}
float wLeft = 0;
float wRight = 0;
var position3D = movementPlane.ToWorld(position, elevation);
for (int i = 0; i < wallBuffer.Count; i += 2)
{
Vector3 closest = VectorMath.ClosestPointOnSegment(wallBuffer[i], wallBuffer[i + 1], position3D);
float dist = (closest - position3D).sqrMagnitude;
if (dist > wallDist * wallDist)
{
continue;
}
Vector2 tang = movementPlane.ToPlane(wallBuffer[i + 1] - wallBuffer[i]).normalized;
// Using the fact that all walls are laid out clockwise (looking from inside the obstacle)
// Then left and right (ish) can be figured out like this
float dot = Vector2.Dot(directionToTarget, tang);
float weight = 1 - System.Math.Max(0, (2 * (dist / (wallDist * wallDist)) - 1));
if (dot > 0)
{
wRight = System.Math.Max(wRight, dot * weight);
}
else
{
wLeft = System.Math.Max(wLeft, -dot * weight);
}
}
Vector2 normal = new Vector2(directionToTarget.y, -directionToTarget.x);
return(normal * (wRight - wLeft));
}
Vector3 CalculateWallForce(Vector3 position, Vector3 directionToTarget)
{
if (wallForce > 0 && wallDist > 0)
{
float wLeft = 0;
float wRight = 0;
for (int i = 0; i < wallBuffer.Count; i += 2)
{
Vector3 closest = VectorMath.ClosestPointOnSegment(wallBuffer[i], wallBuffer[i + 1], tr.position);
float dist = (closest - position).sqrMagnitude;
if (dist > wallDist * wallDist)
{
continue;
}
Vector3 tang = (wallBuffer[i + 1] - wallBuffer[i]).normalized;
// Using the fact that all walls are laid out clockwise (seeing from inside)
// Then left and right (ish) can be figured out like this
float dot = Vector3.Dot(directionToTarget, 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, directionToTarget);
return(norm * (wRight - wLeft));
}
return(Vector3.zero);
}
/** 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();
var fn = pt as RichFunnel;
if (fn != null)
{
//Clear buffers for reuse
Vector3 position = UpdateTarget(fn);
//tr.position = ps;
//Only get walls every 5th frame to save on performance
if (Time.frameCount % 5 == 0 && wallForce > 0 && wallDist > 0)
{
wallBuffer.Clear();
fn.FindWalls(wallBuffer, wallDist);
}
/*for (int i=0;i<wallBuffer.Count;i+=2) {
* Debug.DrawLine (wallBuffer[i],wallBuffer[i+1],Color.magenta);
* }*/
//Pick next waypoint if current is reached
int tgIndex = 0;
/*if (buffer.Count > 1) {
* if ((buffer[tgIndex]-tr.position).sqrMagnitude < pickNextWaypointDist*pickNextWaypointDist) {
* tgIndex++;
* }
* }*/
//Target point
Vector3 tg = 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 = VectorMath.ClosestPointOnSegment(wallBuffer[i], wallBuffer[i + 1], tr.position);
float dist = (closest - position).sqrMagnitude;
if (dist > wallDist * wallDist)
{
continue;
}
Vector3 tang = (wallBuffer[i + 1] - wallBuffer[i]).normalized;
//Using the fact that all walls are laid out clockwise (seeing from inside)
//Then left and right (ish) can be figured out like this
float dot = Vector3.Dot(dir, tang) * (1 - System.Math.Max(0, (2 * (dist / (wallDist * wallDist)) - 1)));
if (dot > 0)
{
wRight = System.Math.Max(wRight, dot);
}
else
{
wLeft = System.Math.Max(wLeft, -dot);
}
}
Vector3 norm = Vector3.Cross(Vector3.up, dir);
force = norm * (wRight - wLeft);
//Debug.DrawRay (tr.position, force, Color.cyan);
}
//Is the endpoint of the path (part) the current target point
bool endPointIsTarget = lastCorner && 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)
{
if (!traversingSpecialPath)
{
StartCoroutine(TraverseSpecial(pt as RichSpecial));
}
}
//w.Stop();
//Debug.Log ((w.Elapsed.TotalMilliseconds*1000));
}
else
{
if (rvoController != null && rvoController.enabled)
{
//Use RVOController
rvoController.Move(Vector3.zero);
}
else
if (controller != null && controller.enabled)
{
}
else
{
tr.position = RaycastPosition(tr.position, tr.position.y);
}
}
}
Vector3 Snap(ABPath path, Exactness mode, bool start, out bool forceAddPoint)
{
var index = start ? 0 : path.path.Count - 1;
var node = path.path[index];
var nodePos = (Vector3)node.position;
forceAddPoint = false;
switch (mode)
{
case Exactness.ClosestOnNode:
return(GetClampedPoint(nodePos, start ? path.startPoint : path.endPoint, node));
case Exactness.SnapToNode:
return(nodePos);
case Exactness.Original:
case Exactness.Interpolate:
case Exactness.NodeConnection:
Vector3 relevantPoint;
if (start)
{
relevantPoint = adjustStartPoint != null?adjustStartPoint() : path.originalStartPoint;
}
else
{
relevantPoint = path.originalEndPoint;
}
switch (mode)
{
case Exactness.Original:
return(GetClampedPoint(nodePos, relevantPoint, node));
case Exactness.Interpolate:
var clamped = GetClampedPoint(nodePos, relevantPoint, node);
// Adjacent node to either the start node or the end node in the path
var adjacentNode = path.path[Mathf.Clamp(index + (start ? 1 : -1), 0, path.path.Count - 1)];
return(VectorMath.ClosestPointOnSegment(nodePos, (Vector3)adjacentNode.position, clamped));
case Exactness.NodeConnection:
// This code uses some tricks to avoid allocations
// even though it uses delegates heavily
// The connectionBufferAddDelegate delegate simply adds whatever node
// it is called with to the connectionBuffer
connectionBuffer = connectionBuffer ?? new List <GraphNode>();
connectionBufferAddDelegate = connectionBufferAddDelegate ?? (GraphNodeDelegate)connectionBuffer.Add;
// Adjacent node to either the start node or the end node in the path
adjacentNode = path.path[Mathf.Clamp(index + (start ? 1 : -1), 0, path.path.Count - 1)];
// Add all neighbours of #node to the connectionBuffer
node.GetConnections(connectionBufferAddDelegate);
var bestPos = nodePos;
var bestDist = float.PositiveInfinity;
// Loop through all neighbours
// Do it in reverse order because the length of the connectionBuffer
// will change during iteration
for (int i = connectionBuffer.Count - 1; i >= 0; i--)
{
var neighbour = connectionBuffer[i];
// Find the closest point on the connection between the nodes
// and check if the distance to that point is lower than the previous best
var closest = VectorMath.ClosestPointOnSegment(nodePos, (Vector3)neighbour.position, relevantPoint);
var dist = (closest - relevantPoint).sqrMagnitude;
if (dist < bestDist)
{
bestPos = closest;
bestDist = dist;
// If this node is not the adjacent node
// then the path should go through the start node as well
forceAddPoint = neighbour != adjacentNode;
}
}
connectionBuffer.Clear();
return(bestPos);
default:
throw new System.ArgumentException("Cannot reach this point, but the compiler is not smart enough to realize that.");
}
#if BNICKSON_UPDATED
case Exactness.VisibilityCheck:
if (start)
{
return(GetClampedPoint((Vector3)path.path[0].position, path.originalStartPoint, path.path[0], true));
}
else
{
return(GetClampedPoint((Vector3)path.path[path.path.Count - 1].position, path.originalEndPoint, path.path[path.path.Count - 1], true));
}
#endif
default:
throw new System.ArgumentException("Invalid mode");
}
}
public static float SqrDistancePointSegment(Vector3 a, Vector3 b, Vector3 p)
{
Vector3 a2 = VectorMath.ClosestPointOnSegment(a, b, p);
return((a2 - p).sqrMagnitude);
}
public static Vector3 NearestPointStrict(Vector3 lineStart, Vector3 lineEnd, Vector3 point)
{
return(VectorMath.ClosestPointOnSegment(lineStart, lineEnd, point));
}
private Vector3 Snap(ABPath path, StartEndModifier.Exactness mode, bool start, out bool forceAddPoint)
{
int num = start ? 0 : (path.path.Count - 1);
GraphNode graphNode = path.path[num];
Vector3 vector = (Vector3)graphNode.position;
forceAddPoint = false;
switch (mode)
{
case StartEndModifier.Exactness.SnapToNode:
return(vector);
case StartEndModifier.Exactness.Original:
case StartEndModifier.Exactness.Interpolate:
case StartEndModifier.Exactness.NodeConnection:
{
Vector3 vector2;
if (start)
{
vector2 = ((this.adjustStartPoint != null) ? this.adjustStartPoint() : path.originalStartPoint);
}
else
{
vector2 = path.originalEndPoint;
}
switch (mode)
{
case StartEndModifier.Exactness.Original:
return(this.GetClampedPoint(vector, vector2, graphNode));
case StartEndModifier.Exactness.Interpolate:
{
Vector3 clampedPoint = this.GetClampedPoint(vector, vector2, graphNode);
GraphNode graphNode2 = path.path[Mathf.Clamp(num + (start ? 1 : -1), 0, path.path.Count - 1)];
return(VectorMath.ClosestPointOnSegment(vector, (Vector3)graphNode2.position, clampedPoint));
}
case StartEndModifier.Exactness.NodeConnection:
{
this.connectionBuffer = (this.connectionBuffer ?? new List <GraphNode>());
Action <GraphNode> action;
if ((action = this.connectionBufferAddDelegate) == null)
{
action = new Action <GraphNode>(this.connectionBuffer.Add);
}
this.connectionBufferAddDelegate = action;
GraphNode graphNode2 = path.path[Mathf.Clamp(num + (start ? 1 : -1), 0, path.path.Count - 1)];
graphNode.GetConnections(this.connectionBufferAddDelegate);
Vector3 result = vector;
float num2 = float.PositiveInfinity;
for (int i = this.connectionBuffer.Count - 1; i >= 0; i--)
{
GraphNode graphNode3 = this.connectionBuffer[i];
Vector3 vector3 = VectorMath.ClosestPointOnSegment(vector, (Vector3)graphNode3.position, vector2);
float sqrMagnitude = (vector3 - vector2).sqrMagnitude;
if (sqrMagnitude < num2)
{
result = vector3;
num2 = sqrMagnitude;
forceAddPoint = (graphNode3 != graphNode2);
}
}
this.connectionBuffer.Clear();
return(result);
}
}
throw new ArgumentException("Cannot reach this point, but the compiler is not smart enough to realize that.");
}
case StartEndModifier.Exactness.ClosestOnNode:
return(this.GetClampedPoint(vector, start ? path.startPoint : path.endPoint, graphNode));
default:
throw new ArgumentException("Invalid mode");
}
}
private Vector3 Snap(ABPath path, Exactness mode, bool start, out bool forceAddPoint)
{
Vector3 originalEndPoint;
GraphNode node2;
int num = !start ? (path.path.Count - 1) : 0;
GraphNode hint = path.path[num];
Vector3 position = (Vector3)hint.position;
forceAddPoint = false;
switch (mode)
{
case Exactness.SnapToNode:
return(position);
case Exactness.Original:
case Exactness.Interpolate:
case Exactness.NodeConnection:
if (!start)
{
originalEndPoint = path.originalEndPoint;
break;
}
originalEndPoint = (this.adjustStartPoint == null) ? path.originalStartPoint : this.adjustStartPoint();
break;
case Exactness.ClosestOnNode:
return(this.GetClampedPoint(position, !start ? path.endPoint : path.startPoint, hint));
default:
throw new ArgumentException("Invalid mode");
}
switch (mode)
{
case Exactness.Original:
return(this.GetClampedPoint(position, originalEndPoint, hint));
case Exactness.Interpolate:
{
Vector3 point = this.GetClampedPoint(position, originalEndPoint, hint);
node2 = path.path[Mathf.Clamp(num + (!start ? -1 : 1), 0, path.path.Count - 1)];
return(VectorMath.ClosestPointOnSegment(position, (Vector3)node2.position, point));
}
case Exactness.NodeConnection:
{
if (this.connectionBuffer == null)
{
}
this.connectionBuffer = new List <GraphNode>();
if (this.connectionBufferAddDelegate == null)
{
}
this.connectionBufferAddDelegate = new GraphNodeDelegate(this.connectionBuffer.Add);
node2 = path.path[Mathf.Clamp(num + (!start ? -1 : 1), 0, path.path.Count - 1)];
hint.GetConnections(this.connectionBufferAddDelegate);
Vector3 vector4 = position;
float positiveInfinity = float.PositiveInfinity;
for (int i = this.connectionBuffer.Count - 1; i >= 0; i--)
{
GraphNode node3 = this.connectionBuffer[i];
Vector3 vector5 = VectorMath.ClosestPointOnSegment(position, (Vector3)node3.position, originalEndPoint);
Vector3 vector6 = vector5 - originalEndPoint;
float sqrMagnitude = vector6.sqrMagnitude;
if (sqrMagnitude < positiveInfinity)
{
vector4 = vector5;
positiveInfinity = sqrMagnitude;
forceAddPoint = node3 != node2;
}
}
this.connectionBuffer.Clear();
return(vector4);
}
}
throw new ArgumentException("Cannot reach this point, but the compiler is not smart enough to realize that.");
}
