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));
}
