public ClosestPointOnNode ( |
||
p | ||
return |
/** This performs a linear search through all polygons returning the closest one. * This will fill the NNInfo with .node for the closest node not necessarily complying with the NNConstraint, and .constrainedNode with the closest node * complying with the NNConstraint. * \see GetNearestForce(Node[],Int3[],Vector3,NNConstraint,bool) */ public static NNInfo GetNearestForceBoth(NavGraph graph, INavmeshHolder navmesh, Vector3 position, NNConstraint constraint, bool accurateNearestNode) { Int3 pos = (Int3)position; float minDist = -1; GraphNode minNode = null; float minConstDist = -1; GraphNode minConstNode = null; float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity; GraphNodeDelegateCancelable del = delegate(GraphNode _node) { TriangleMeshNode node = _node as TriangleMeshNode; if (accurateNearestNode) { Vector3 closest = node.ClosestPointOnNode(position); float dist = ((Vector3)pos - closest).sqrMagnitude; if (minNode == null || dist < minDist) { minDist = dist; minNode = node; } if (dist < maxDistSqr && constraint.Suitable(node)) { if (minConstNode == null || dist < minConstDist) { minConstDist = dist; minConstNode = node; } } } else { if (!node.ContainsPoint((Int3)position)) { float dist = (node.position - pos).sqrMagnitude; if (minNode == null || dist < minDist) { minDist = dist; minNode = node; } if (dist < maxDistSqr && constraint.Suitable(node)) { if (minConstNode == null || dist < minConstDist) { minConstDist = dist; minConstNode = node; } } } else { int dist = AstarMath.Abs(node.position.y - pos.y); if (minNode == null || dist < minDist) { minDist = dist; minNode = node; } if (dist < maxDistSqr && constraint.Suitable(node)) { if (minConstNode == null || dist < minConstDist) { minConstDist = dist; minConstNode = node; } } } } return(true); }; graph.GetNodes(del); NNInfo nninfo = new NNInfo(minNode); //Find the point closest to the nearest triangle if (nninfo.node != null) { TriangleMeshNode node = nninfo.node as TriangleMeshNode; //minNode2 as MeshNode; Vector3 clP = node.ClosestPointOnNode(position); nninfo.clampedPosition = clP; } nninfo.constrainedNode = minConstNode; if (nninfo.constrainedNode != null) { TriangleMeshNode node = nninfo.constrainedNode as TriangleMeshNode; //minNode2 as MeshNode; Vector3 clP = node.ClosestPointOnNode(position); nninfo.constClampedPosition = clP; } return(nninfo); }
public static NNInfo GetNearestForceBoth(NavGraph graph, INavmeshHolder navmesh, Vector3 position, NNConstraint constraint, bool accurateNearestNode) { Int3 pos = (Int3)position; float minDist = -1f; GraphNode minNode = null; float minConstDist = -1f; GraphNode minConstNode = null; float maxDistSqr = (!constraint.constrainDistance) ? float.PositiveInfinity : AstarPath.active.maxNearestNodeDistanceSqr; GraphNodeDelegateCancelable del = delegate(GraphNode _node) { TriangleMeshNode triangleMeshNode3 = _node as TriangleMeshNode; if (accurateNearestNode) { Vector3 b = triangleMeshNode3.ClosestPointOnNode(position); float sqrMagnitude = ((Vector3)pos - b).sqrMagnitude; if (minNode == null || sqrMagnitude < minDist) { minDist = sqrMagnitude; minNode = triangleMeshNode3; } if (sqrMagnitude < maxDistSqr && constraint.Suitable(triangleMeshNode3) && (minConstNode == null || sqrMagnitude < minConstDist)) { minConstDist = sqrMagnitude; minConstNode = triangleMeshNode3; } } else if (!triangleMeshNode3.ContainsPoint((Int3)position)) { float sqrMagnitude2 = (triangleMeshNode3.position - pos).sqrMagnitude; if (minNode == null || sqrMagnitude2 < minDist) { minDist = sqrMagnitude2; minNode = triangleMeshNode3; } if (sqrMagnitude2 < maxDistSqr && constraint.Suitable(triangleMeshNode3) && (minConstNode == null || sqrMagnitude2 < minConstDist)) { minConstDist = sqrMagnitude2; minConstNode = triangleMeshNode3; } } else { int num = AstarMath.Abs(triangleMeshNode3.position.y - pos.y); if (minNode == null || (float)num < minDist) { minDist = (float)num; minNode = triangleMeshNode3; } if ((float)num < maxDistSqr && constraint.Suitable(triangleMeshNode3) && (minConstNode == null || (float)num < minConstDist)) { minConstDist = (float)num; minConstNode = triangleMeshNode3; } } return(true); }; graph.GetNodes(del); NNInfo result = new NNInfo(minNode); if (result.node != null) { TriangleMeshNode triangleMeshNode = result.node as TriangleMeshNode; Vector3 clampedPosition = triangleMeshNode.ClosestPointOnNode(position); result.clampedPosition = clampedPosition; } result.constrainedNode = minConstNode; if (result.constrainedNode != null) { TriangleMeshNode triangleMeshNode2 = result.constrainedNode as TriangleMeshNode; Vector3 constClampedPosition = triangleMeshNode2.ClosestPointOnNode(position); result.constClampedPosition = constClampedPosition; } return(result); }
// Token: 0x060004BE RID: 1214 RVA: 0x00029030 File Offset: 0x00027430 public static bool Linecast(INavmesh graph, Vector3 tmp_origin, Vector3 tmp_end, GraphNode hint, out GraphHitInfo hit, List <GraphNode> trace) { Int3 @int = (Int3)tmp_end; Int3 int2 = (Int3)tmp_origin; hit = default(GraphHitInfo); if (float.IsNaN(tmp_origin.x + tmp_origin.y + tmp_origin.z)) { throw new ArgumentException("origin is NaN"); } if (float.IsNaN(tmp_end.x + tmp_end.y + tmp_end.z)) { throw new ArgumentException("end is NaN"); } TriangleMeshNode triangleMeshNode = hint as TriangleMeshNode; if (triangleMeshNode == null) { triangleMeshNode = ((graph as NavGraph).GetNearest(tmp_origin, NNConstraint.None).node as TriangleMeshNode); if (triangleMeshNode == null) { Debug.LogError("Could not find a valid node to start from"); hit.point = tmp_origin; return(true); } } if (int2 == @int) { hit.node = triangleMeshNode; return(false); } int2 = (Int3)triangleMeshNode.ClosestPointOnNode((Vector3)int2); hit.origin = (Vector3)int2; if (!triangleMeshNode.Walkable) { hit.point = (Vector3)int2; hit.tangentOrigin = (Vector3)int2; return(true); } List <Vector3> list = ListPool <Vector3> .Claim(); List <Vector3> list2 = ListPool <Vector3> .Claim(); int num = 0; for (;;) { num++; if (num > 2000) { break; } TriangleMeshNode triangleMeshNode2 = null; if (trace != null) { trace.Add(triangleMeshNode); } if (triangleMeshNode.ContainsPoint(@int)) { goto Block_9; } for (int i = 0; i < triangleMeshNode.connections.Length; i++) { if (triangleMeshNode.connections[i].GraphIndex == triangleMeshNode.GraphIndex) { list.Clear(); list2.Clear(); if (triangleMeshNode.GetPortal(triangleMeshNode.connections[i], list, list2, false)) { Vector3 vector = list[0]; Vector3 vector2 = list2[0]; if (Polygon.LeftNotColinear(vector, vector2, hit.origin) || !Polygon.LeftNotColinear(vector, vector2, tmp_end)) { float num2; float num3; if (Polygon.IntersectionFactor(vector, vector2, hit.origin, tmp_end, out num2, out num3)) { if (num3 >= 0f) { if (num2 >= 0f && num2 <= 1f) { triangleMeshNode2 = (triangleMeshNode.connections[i] as TriangleMeshNode); break; } } } } } } } if (triangleMeshNode2 == null) { goto Block_18; } triangleMeshNode = triangleMeshNode2; } Debug.LogError("Linecast was stuck in infinite loop. Breaking."); ListPool <Vector3> .Release(list); ListPool <Vector3> .Release(list2); return(true); Block_9: ListPool <Vector3> .Release(list); ListPool <Vector3> .Release(list2); return(false); Block_18: int vertexCount = triangleMeshNode.GetVertexCount(); for (int j = 0; j < vertexCount; j++) { Vector3 vector3 = (Vector3)triangleMeshNode.GetVertex(j); Vector3 vector4 = (Vector3)triangleMeshNode.GetVertex((j + 1) % vertexCount); if (Polygon.LeftNotColinear(vector3, vector4, hit.origin) || !Polygon.LeftNotColinear(vector3, vector4, tmp_end)) { float num4; float num5; if (Polygon.IntersectionFactor(vector3, vector4, hit.origin, tmp_end, out num4, out num5)) { if (num5 >= 0f) { if (num4 >= 0f && num4 <= 1f) { Vector3 point = vector3 + (vector4 - vector3) * num4; hit.point = point; hit.node = triangleMeshNode; hit.tangent = vector4 - vector3; hit.tangentOrigin = vector3; ListPool <Vector3> .Release(list); ListPool <Vector3> .Release(list2); return(true); } } } } } Debug.LogWarning("Linecast failing because point not inside node, and line does not hit any edges of it"); ListPool <Vector3> .Release(list); ListPool <Vector3> .Release(list2); return(false); }
public Vector3 ClosestPointOnNode(TriangleMeshNode node, Vector3 pos) { return(node.ClosestPointOnNode(pos)); }
private bool ClampToNavmeshInternalFull(ref Vector3 position) { int index = 0; float closestDist = float.PositiveInfinity; bool closestIsInPath = false; TriangleMeshNode closestNode = null; this.checkForDestroyedNodesCounter = 0; int i = 0; int count = this.nodes.Count; while (i < count) { if (this.nodes[i].Destroyed) { return(true); } Vector3 a = this.nodes[i].ClosestPointOnNode(position); float sqrMagnitude = (a - position).sqrMagnitude; if (sqrMagnitude < closestDist) { closestDist = sqrMagnitude; index = i; closestNode = this.nodes[i]; closestIsInPath = true; } i++; } Vector3 posCopy = position; int containingIndex = this.nodes.Count - 1; int closestIsNeighbourOf = 0; Action <GraphNode> action = delegate(GraphNode node) { if ((containingIndex <= 0 || node != this.nodes[containingIndex - 1]) && (containingIndex >= this.nodes.Count - 1 || node != this.nodes[containingIndex + 1])) { TriangleMeshNode triangleMeshNode = node as TriangleMeshNode; if (triangleMeshNode != null) { Vector3 a2 = triangleMeshNode.ClosestPointOnNode(posCopy); float sqrMagnitude2 = (a2 - posCopy).sqrMagnitude; if (sqrMagnitude2 < closestDist) { closestDist = sqrMagnitude2; closestIsNeighbourOf = containingIndex; closestNode = triangleMeshNode; closestIsInPath = false; } } } }; while (containingIndex >= 0) { this.nodes[containingIndex].GetConnections(action); containingIndex--; } if (closestIsInPath) { this.currentNode = index; position = this.nodes[index].ClosestPointOnNodeXZ(position); } else { position = closestNode.ClosestPointOnNodeXZ(position); this.exactStart = position; this.UpdateFunnelCorridor(closestIsNeighbourOf, closestNode); this.currentNode = 0; } return(false); }
public static bool Linecast(INavmesh graph, VInt3 tmp_origin, VInt3 tmp_end, GraphNode hint, out GraphHitInfo hit, List<GraphNode> trace) { VInt3 vInt = tmp_end; VInt3 vInt2 = tmp_origin; hit = default(GraphHitInfo); if (float.IsNaN((float)(tmp_origin.x + tmp_origin.y + tmp_origin.z))) { throw new ArgumentException("origin is NaN"); } if (float.IsNaN((float)(tmp_end.x + tmp_end.y + tmp_end.z))) { throw new ArgumentException("end is NaN"); } TriangleMeshNode triangleMeshNode = hint as TriangleMeshNode; if (triangleMeshNode == null) { triangleMeshNode = ((graph as NavGraph).GetNearest(tmp_origin, NNConstraint.None).node as TriangleMeshNode); if (triangleMeshNode == null) { Debug.LogError("Could not find a valid node to start from"); hit.point = tmp_origin; return true; } } if (vInt2 == vInt) { hit.node = triangleMeshNode; return false; } vInt2 = (VInt3)triangleMeshNode.ClosestPointOnNode((Vector3)vInt2); hit.origin = vInt2; if (!triangleMeshNode.Walkable) { hit.point = vInt2; hit.tangentOrigin = vInt2; return true; } List<VInt3> list = ListPool<VInt3>.Claim(); List<VInt3> list2 = ListPool<VInt3>.Claim(); int num = 0; while (true) { num++; if (num > 2000) { break; } TriangleMeshNode triangleMeshNode2 = null; if (trace != null) { trace.Add(triangleMeshNode); } if (triangleMeshNode.ContainsPoint(vInt)) { goto Block_9; } for (int i = 0; i < triangleMeshNode.connections.Length; i++) { if (triangleMeshNode.connections[i].GraphIndex == triangleMeshNode.GraphIndex) { list.Clear(); list2.Clear(); if (triangleMeshNode.GetPortal(triangleMeshNode.connections[i], list, list2, false)) { VInt3 vInt3 = list.get_Item(0); VInt3 vInt4 = list2.get_Item(0); if (Polygon.LeftNotColinear(vInt3, vInt4, hit.origin) || !Polygon.LeftNotColinear(vInt3, vInt4, tmp_end)) { float num2; float num3; if (Polygon.IntersectionFactor(vInt3, vInt4, hit.origin, tmp_end, out num2, out num3)) { if (num3 >= 0f) { if (num2 >= 0f && num2 <= 1f) { triangleMeshNode2 = (triangleMeshNode.connections[i] as TriangleMeshNode); break; } } } } } } } if (triangleMeshNode2 == null) { goto Block_18; } triangleMeshNode = triangleMeshNode2; } Debug.LogError("Linecast was stuck in infinite loop. Breaking."); ListPool<VInt3>.Release(list); ListPool<VInt3>.Release(list2); return true; Block_9: ListPool<VInt3>.Release(list); ListPool<VInt3>.Release(list2); return false; Block_18: int vertexCount = triangleMeshNode.GetVertexCount(); for (int j = 0; j < vertexCount; j++) { VInt3 vertex = triangleMeshNode.GetVertex(j); VInt3 vertex2 = triangleMeshNode.GetVertex((j + 1) % vertexCount); if (Polygon.LeftNotColinear(vertex, vertex2, hit.origin) || !Polygon.LeftNotColinear(vertex, vertex2, tmp_end)) { VFactor vFactor; VFactor vFactor2; if (Polygon.IntersectionFactor(vertex, vertex2, hit.origin, tmp_end, out vFactor, out vFactor2)) { if (!vFactor2.IsNegative) { if (!vFactor.IsNegative && vFactor.nom / vFactor.den <= 1L) { VInt3 vInt5 = (vertex2 - vertex) * (float)vFactor.nom; vInt5 = IntMath.Divide(vInt5, vFactor.den); vInt5 += vertex; hit.point = vInt5; hit.node = triangleMeshNode; hit.tangent = vertex2 - vertex; hit.tangentOrigin = vertex; ListPool<VInt3>.Release(list); ListPool<VInt3>.Release(list2); return true; } } } } } Debug.LogWarning("Linecast failing because point not inside node, and line does not hit any edges of it"); ListPool<VInt3>.Release(list); ListPool<VInt3>.Release(list2); return false; }
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); } }
public Vector3 Update(Vector3 position, List <Vector3> buffer, int numCorners, out bool lastCorner, out bool requiresRepath) { lastCorner = false; requiresRepath = false; Int3 p = (Int3)position; if (this.nodes[this.currentNode].Destroyed) { requiresRepath = true; lastCorner = false; buffer.Add(position); return(position); } if (this.nodes[this.currentNode].ContainsPoint(p)) { if (this.checkForDestroyedNodesCounter >= 10) { this.checkForDestroyedNodesCounter = 0; int i = 0; int count = this.nodes.Count; while (i < count) { if (this.nodes[i].Destroyed) { requiresRepath = true; break; } i++; } } else { this.checkForDestroyedNodesCounter++; } } else { bool flag = false; int num = this.currentNode + 1; int num2 = Math.Min(this.currentNode + 3, this.nodes.Count); while (num < num2 && !flag) { if (this.nodes[num].Destroyed) { requiresRepath = true; lastCorner = false; buffer.Add(position); return(position); } if (this.nodes[num].ContainsPoint(p)) { this.currentNode = num; flag = true; } num++; } int num3 = this.currentNode - 1; int num4 = Math.Max(this.currentNode - 3, 0); while (num3 > num4 && !flag) { if (this.nodes[num3].Destroyed) { requiresRepath = true; lastCorner = false; buffer.Add(position); return(position); } if (this.nodes[num3].ContainsPoint(p)) { this.currentNode = num3; flag = true; } num3--; } if (!flag) { int index = 0; int closestIsNeighbourOf = 0; float closestDist = float.PositiveInfinity; bool closestIsInPath = false; TriangleMeshNode closestNode = null; int containingIndex = this.nodes.Count - 1; this.checkForDestroyedNodesCounter = 0; int j = 0; int count2 = this.nodes.Count; while (j < count2) { if (this.nodes[j].Destroyed) { requiresRepath = true; lastCorner = false; buffer.Add(position); return(position); } Vector3 a = this.nodes[j].ClosestPointOnNode(position); float sqrMagnitude = (a - position).sqrMagnitude; if (sqrMagnitude < closestDist) { closestDist = sqrMagnitude; index = j; closestNode = this.nodes[j]; closestIsInPath = true; } j++; } Vector3 posCopy = position; GraphNodeDelegate del = delegate(GraphNode node) { if ((containingIndex <= 0 || node != this.nodes[containingIndex - 1]) && (containingIndex >= this.nodes.Count - 1 || node != this.nodes[containingIndex + 1])) { TriangleMeshNode triangleMeshNode = node as TriangleMeshNode; if (triangleMeshNode != null) { Vector3 a2 = triangleMeshNode.ClosestPointOnNode(posCopy); float sqrMagnitude2 = (a2 - posCopy).sqrMagnitude; if (sqrMagnitude2 < closestDist) { closestDist = sqrMagnitude2; closestIsNeighbourOf = containingIndex; closestNode = triangleMeshNode; closestIsInPath = false; } } } }; while (containingIndex >= 0) { this.nodes[containingIndex].GetConnections(del); containingIndex--; } if (closestIsInPath) { this.currentNode = index; position = this.nodes[index].ClosestPointOnNodeXZ(position); } else { position = closestNode.ClosestPointOnNodeXZ(position); this.exactStart = position; this.UpdateFunnelCorridor(closestIsNeighbourOf, closestNode); this.currentNode = 0; } } } this.currentPosition = position; if (!this.FindNextCorners(position, this.currentNode, buffer, numCorners, out lastCorner)) { Debug.LogError("Oh oh"); buffer.Add(position); return(position); } return(position); }
// Token: 0x060025FC RID: 9724 RVA: 0x001A3984 File Offset: 0x001A1B84 private static bool NodeIntersectsCircle(TriangleMeshNode node, Vector3 p, float radius) { return(float.IsPositiveInfinity(radius) || (p - node.ClosestPointOnNode(p)).sqrMagnitude < radius * radius); }
private bool ClampToNavmeshInternal(ref Vector3 position) { if (this.nodes[this.currentNode].Destroyed) { return(true); } Int3 p = (Int3)position; if (this.nodes[this.currentNode].ContainsPoint(p)) { return(false); } int i = this.currentNode + 1; int num = Math.Min(this.currentNode + 3, this.nodes.Count); while (i < num) { if (this.nodes[i].Destroyed) { return(true); } if (this.nodes[i].ContainsPoint(p)) { this.currentNode = i; return(false); } i++; } int j = this.currentNode - 1; int num2 = Math.Max(this.currentNode - 3, 0); while (j > num2) { if (this.nodes[j].Destroyed) { return(true); } if (this.nodes[j].ContainsPoint(p)) { this.currentNode = j; return(false); } j--; } int index = 0; int closestIsNeighbourOf = 0; float closestDist = float.PositiveInfinity; bool closestIsInPath = false; TriangleMeshNode closestNode = null; int containingIndex = this.nodes.Count - 1; this.checkForDestroyedNodesCounter = 0; int k = 0; int count = this.nodes.Count; while (k < count) { if (this.nodes[k].Destroyed) { return(true); } Vector3 a = this.nodes[k].ClosestPointOnNode(position); float sqrMagnitude = (a - position).sqrMagnitude; if (sqrMagnitude < closestDist) { closestDist = sqrMagnitude; index = k; closestNode = this.nodes[k]; closestIsInPath = true; } k++; } Vector3 posCopy = position; GraphNodeDelegate del = delegate(GraphNode node) { if ((containingIndex <= 0 || node != this.nodes[containingIndex - 1]) && (containingIndex >= this.nodes.Count - 1 || node != this.nodes[containingIndex + 1])) { TriangleMeshNode triangleMeshNode = node as TriangleMeshNode; if (triangleMeshNode != null) { Vector3 a2 = triangleMeshNode.ClosestPointOnNode(posCopy); float sqrMagnitude2 = (a2 - posCopy).sqrMagnitude; if (sqrMagnitude2 < closestDist) { closestDist = sqrMagnitude2; closestIsNeighbourOf = containingIndex; closestNode = triangleMeshNode; closestIsInPath = false; } } } }; while (containingIndex >= 0) { this.nodes[containingIndex].GetConnections(del); containingIndex--; } if (closestIsInPath) { this.currentNode = index; position = this.nodes[index].ClosestPointOnNodeXZ(position); } else { position = closestNode.ClosestPointOnNodeXZ(position); this.exactStart = position; this.UpdateFunnelCorridor(closestIsNeighbourOf, closestNode); this.currentNode = 0; } return(false); }
//public Vector3 ClosestPointOnNode (TriangleMeshNode node, Vector3 pos) { public VInt3 ClosestPointOnNode(TriangleMeshNode node, VInt3 pos) { return(node.ClosestPointOnNode(pos)); }