void GetNearestConnectionInternal(int index, Int3 point, NNConstraint constraint, ref GraphNode best, ref long bestSqrDist, long distanceThresholdOffset) { var data = tree[index].data; if (data != null) { var pointv3 = (UnityEngine.Vector3)point; for (int i = tree[index].count - 1; i >= 0; i--) { var dist = (data[i].position - point).sqrMagnitudeLong; // Note: the subtraction is important. If we used an addition on the RHS instead the result might overflow as bestSqrDist starts as long.MaxValue if (dist - distanceThresholdOffset < bestSqrDist && (constraint == null || constraint.Suitable(data[i]))) { // This node may contains the closest connection // Check all connections var conns = (data[i] as PointNode).connections; if (conns != null) { var nodePos = (UnityEngine.Vector3)data[i].position; for (int j = 0; j < conns.Length; j++) { // Find the closest point on the connection, but only on this node's side of the connection // This ensures that we will find the closest node with the closest connection. var connectionMidpoint = ((UnityEngine.Vector3)conns[j].node.position + nodePos) * 0.5f; float sqrConnectionDistance = VectorMath.SqrDistancePointSegment(nodePos, connectionMidpoint, pointv3); // Convert to Int3 space long sqrConnectionDistanceInt = (long)(sqrConnectionDistance * Int3.FloatPrecision * Int3.FloatPrecision); if (sqrConnectionDistanceInt < bestSqrDist) { bestSqrDist = sqrConnectionDistanceInt; best = data[i]; } } } // Also check if the node itself is close enough. // This is important if the node has no connections at all. if (dist < bestSqrDist) { bestSqrDist = dist; best = data[i]; } } } } else { var dist = (long)(point[tree[index].splitAxis] - tree[index].split); var childIndex = 2 * index + (dist < 0 ? 0 : 1); GetNearestConnectionInternal(childIndex, point, constraint, ref best, ref bestSqrDist, distanceThresholdOffset); // Try the other one if it is possible to find a valid node on the other side // Note: the subtraction is important. If we used an addition on the RHS instead the result might overflow as bestSqrDist starts as long.MaxValue if (dist * dist - distanceThresholdOffset < bestSqrDist) { // childIndex ^ 1 will flip the last bit, so if childIndex is odd, then childIndex ^ 1 will be even GetNearestConnectionInternal(childIndex ^ 0x1, point, constraint, ref best, ref bestSqrDist, distanceThresholdOffset); } } }
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 = VectorMath.SqrDistancePointSegment(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); } }
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]); } else { GraphHitInfo info; if (rcg.Linecast(startPoint, endPoint, nodes[start], out info, result) || (result[result.Count - 1] != nodes[end])) { result.RemoveRange(count, result.Count - count); int num2 = -1; float positiveInfinity = float.PositiveInfinity; for (int i = start + 1; i < end; i++) { float num5 = VectorMath.SqrDistancePointSegment(startPoint, endPoint, (Vector3)nodes[i].position); if ((num2 == -1) || (num5 < positiveInfinity)) { num2 = i; positiveInfinity = num5; } } SimplifyPath2(rcg, nodes, start, num2, result, startPoint, (Vector3)nodes[num2].position); result.RemoveAt(result.Count - 1); SimplifyPath2(rcg, nodes, num2, end, result, (Vector3)nodes[num2].position, endPoint); } } }
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 = VectorMath.SqrDistancePointSegment(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); } } }
public static void SimplifyPath3(IRaycastableGraph rcg, List <GraphNode> nodes, int start, int end, List <GraphNode> result, Vector3 startPoint, Vector3 endPoint, [Optional, DefaultParameterValue(0)] int depth) { if (start == end) { result.Add(nodes[start]); } else if ((start + 1) == end) { result.Add(nodes[start]); result.Add(nodes[end]); } else { GraphHitInfo info; int count = result.Count; if (rcg.Linecast(startPoint, endPoint, nodes[start], out info, result) || (result[result.Count - 1] != nodes[end])) { result.RemoveRange(count, result.Count - count); int num2 = 0; float num3 = 0f; for (int i = start + 1; i < (end - 1); i++) { float num5 = VectorMath.SqrDistancePointSegment(startPoint, endPoint, (Vector3)nodes[i].position); if (num5 > num3) { num2 = i; num3 = num5; } } int num6 = (num2 + start) / 2; int num7 = (num2 + end) / 2; if (num6 == num7) { SimplifyPath3(rcg, nodes, start, num6, result, startPoint, (Vector3)nodes[num6].position, 0); result.RemoveAt(result.Count - 1); SimplifyPath3(rcg, nodes, num6, end, result, (Vector3)nodes[num6].position, endPoint, depth + 1); } else { SimplifyPath3(rcg, nodes, start, num6, result, startPoint, (Vector3)nodes[num6].position, depth + 1); result.RemoveAt(result.Count - 1); SimplifyPath3(rcg, nodes, num6, num7, result, (Vector3)nodes[num6].position, (Vector3)nodes[num7].position, depth + 1); result.RemoveAt(result.Count - 1); SimplifyPath3(rcg, nodes, num7, end, result, (Vector3)nodes[num7].position, endPoint, depth + 1); } } } }
public static void SimplifyPath2(IRaycastableGraph rcg, List <GraphNode> nodes, int start, int end, List <GraphNode> result, Vector3 startPoint, Vector3 endPoint) { int resCount = result.Count; if (end <= start + 1) { result.Add(nodes[start]); result.Add(nodes[end]); //t--; return; } GraphHitInfo hit; if ((rcg.Linecast(startPoint, endPoint, nodes[start], out hit, result) || result[result.Count - 1] != nodes[end])) { //Obstacle //Refine further result.RemoveRange(resCount, result.Count - resCount); int minDistNode = -1; float minDist = float.PositiveInfinity; for (int i = start + 1; i < end; i++) { float dist = VectorMath.SqrDistancePointSegment(startPoint, endPoint, (Vector3)nodes[i].position); if (minDistNode == -1 || dist < minDist) { minDistNode = i; minDist = dist; } } SimplifyPath2(rcg, nodes, start, minDistNode, result, startPoint, (Vector3)nodes[minDistNode].position); //Remove start node of next part so that it is not added twice result.RemoveAt(result.Count - 1); SimplifyPath2(rcg, nodes, minDistNode, end, result, (Vector3)nodes[minDistNode].position, endPoint); } }
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 resCount = result.Count; GraphHitInfo hit; bool linecast = rcg.Linecast(startPoint, endPoint, nodes[start], out hit, result); if (linecast || result[result.Count - 1] != nodes[end]) { //Debug.DrawLine (startPoint, endPoint, Color.black); //Obstacle //Refine further result.RemoveRange(resCount, result.Count - resCount); int maxDistNode = 0; float maxDist = 0; for (int i = start + 1; i < end - 1; i++) { float dist = VectorMath.SqrDistancePointSegment(startPoint, endPoint, (Vector3)nodes[i].position); if (dist > maxDist) { maxDistNode = i; maxDist = dist; } } int mid1 = (maxDistNode + start) / 2; int mid2 = (maxDistNode + end) / 2; if (mid1 == mid2) { SimplifyPath3(rcg, nodes, start, mid1, result, startPoint, (Vector3)nodes[mid1].position); //Remove start node of next part so that it is not added twice result.RemoveAt(result.Count - 1); SimplifyPath3(rcg, nodes, mid1, end, result, (Vector3)nodes[mid1].position, endPoint, depth + 1); } else { SimplifyPath3(rcg, nodes, start, mid1, result, startPoint, (Vector3)nodes[mid1].position, depth + 1); //Remove start node of next part so that it is not added twice result.RemoveAt(result.Count - 1); SimplifyPath3(rcg, nodes, mid1, mid2, result, (Vector3)nodes[mid1].position, (Vector3)nodes[mid2].position, depth + 1); //Remove start node of next part so that it is not added twice result.RemoveAt(result.Count - 1); SimplifyPath3(rcg, nodes, mid2, end, result, (Vector3)nodes[mid2].position, endPoint, depth + 1); } } }
public static float DistancePointSegmentStrict(Vector3 a, Vector3 b, Vector3 p) { return(VectorMath.SqrDistancePointSegment(a, b, p)); }