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);
}
}
// Token: 0x060007EF RID: 2031 RVA: 0x0004C030 File Offset: 0x0004A430
public static void GetPointsAroundPointWorld(Vector3 p, IRaycastableGraph g, List <Vector3> previousPoints, float radius, float clearanceRadius)
{
if (previousPoints.Count == 0)
{
return;
}
Vector3 vector = Vector3.zero;
for (int i = 0; i < previousPoints.Count; i++)
{
vector += previousPoints[i];
}
vector /= (float)previousPoints.Count;
for (int j = 0; j < previousPoints.Count; j++)
{
int index;
previousPoints[index = j] = previousPoints[index] - vector;
}
PathUtilities.GetPointsAroundPoint(p, g, previousPoints, radius, clearanceRadius);
}
public bool ValidateLine(GraphNode n1, GraphNode n2, Vector3 v1, Vector3 v2)
{
if (this.useRaycasting)
{
if (this.thickRaycast && (this.thickRaycastRadius > 0f))
{
Vector3 vector = v2 - v1;
if (Physics.SphereCast(new Ray(v1 + this.raycastOffset, v2 - v1), this.thickRaycastRadius, vector.magnitude, (int)this.mask))
{
return(false);
}
}
else if (Physics.Linecast(v1 + this.raycastOffset, v2 + this.raycastOffset, (int)this.mask))
{
return(false);
}
}
if (this.useGraphRaycasting && (n1 == null))
{
n1 = AstarPath.active.GetNearest(v1).node;
n2 = AstarPath.active.GetNearest(v2).node;
}
if ((this.useGraphRaycasting && (n1 != null)) && (n2 != null))
{
NavGraph graph = AstarData.GetGraph(n1);
NavGraph graph2 = AstarData.GetGraph(n2);
if (graph != graph2)
{
return(false);
}
if (graph != null)
{
IRaycastableGraph graph3 = graph as IRaycastableGraph;
if (graph3 != null)
{
return(!graph3.Linecast(v1, v2, n1));
}
}
}
return(true);
}
// Token: 0x06002440 RID: 9280 RVA: 0x00196CB8 File Offset: 0x00194EB8
private void LateUpdate()
{
if (this.prevNode == null)
{
NNInfo nearest = AstarPath.active.GetNearest(base.transform.position);
this.prevNode = nearest.node;
this.prevPos = base.transform.position;
}
if (this.prevNode == null)
{
return;
}
if (this.prevNode != null)
{
IRaycastableGraph raycastableGraph = AstarData.GetGraph(this.prevNode) as IRaycastableGraph;
if (raycastableGraph != null)
{
GraphHitInfo graphHitInfo;
if (raycastableGraph.Linecast(this.prevPos, base.transform.position, this.prevNode, out graphHitInfo))
{
graphHitInfo.point.y = base.transform.position.y;
Vector3 vector = VectorMath.ClosestPointOnLine(graphHitInfo.tangentOrigin, graphHitInfo.tangentOrigin + graphHitInfo.tangent, base.transform.position);
Vector3 vector2 = graphHitInfo.point;
vector2 += Vector3.ClampMagnitude((Vector3)graphHitInfo.node.position - vector2, 0.008f);
if (raycastableGraph.Linecast(vector2, vector, graphHitInfo.node, out graphHitInfo))
{
graphHitInfo.point.y = base.transform.position.y;
base.transform.position = graphHitInfo.point;
}
else
{
vector.y = base.transform.position.y;
base.transform.position = vector;
}
}
this.prevNode = graphHitInfo.node;
}
}
this.prevPos = base.transform.position;
}
public bool ValidateLine(GraphNode n1, GraphNode n2, Vector3 v1, Vector3 v2)
{
if (this.useRaycasting)
{
if (this.thickRaycast && this.thickRaycastRadius > 0f)
{
if (Physics.SphereCast(new Ray(v1 + this.raycastOffset, v2 - v1), this.thickRaycastRadius, (v2 - v1).magnitude, this.mask))
{
return false;
}
}
else if (Physics.Linecast(v1 + this.raycastOffset, v2 + this.raycastOffset, this.mask))
{
return false;
}
}
if (this.useGraphRaycasting && n1 == null)
{
n1 = AstarPath.active.GetNearest(v1).node;
n2 = AstarPath.active.GetNearest(v2).node;
}
if (this.useGraphRaycasting && n1 != null && n2 != null)
{
NavGraph graph = AstarData.GetGraph(n1);
NavGraph graph2 = AstarData.GetGraph(n2);
if (graph != graph2)
{
return false;
}
if (graph != null)
{
IRaycastableGraph raycastableGraph = graph as IRaycastableGraph;
if (raycastableGraph != null)
{
return !raycastableGraph.Linecast(v1, v2, n1);
}
}
}
return true;
}
/** Will calculate a number of points around \a p which are on the graph and are separated by \a clearance from each other.
* This is like GetPointsAroundPoint except that \a previousPoints are treated as being in world space.
* The average of the points will be found and then that will be treated as the group center.
*/
public static void GetPointsAroundPointWorld(Vector3 p, IRaycastableGraph g, List <Vector3> previousPoints, float radius, float clearanceRadius)
{
if (previousPoints.Count == 0)
{
return;
}
Vector3 avg = Vector3.zero;
for (int i = 0; i < previousPoints.Count; i++)
{
avg += previousPoints[i];
}
avg /= previousPoints.Count;
for (int i = 0; i < previousPoints.Count; i++)
{
previousPoints[i] -= avg;
}
GetPointsAroundPoint(p, g, previousPoints, radius, clearanceRadius);
}
public Vector3 GetClampedPoint(Vector3 from, Vector3 to, Node hint)
{
//float minDistance = Mathf.Infinity;
Vector3 minPoint = to;
if (useRaycasting)
{
RaycastHit hit;
if (Physics.Linecast(from, to, out hit, mask))
{
minPoint = hit.point;
//minDistance = hit.distance;
}
}
if (useGraphRaycasting && hint != null)
{
NavGraph graph = AstarData.GetGraph(hint);
if (graph != null)
{
IRaycastableGraph rayGraph = graph as IRaycastableGraph;
if (rayGraph != null)
{
GraphHitInfo hit;
if (rayGraph.Linecast(from, minPoint, hint, out hit))
{
//if ((hit.point-from).magnitude < minDistance) {
minPoint = hit.point;
//}
}
}
}
}
return(minPoint);
}
private void LateUpdate()
{
if (this.prevNode == null)
{
NNInfo nearest = AstarPath.active.GetNearest(base.transform.position);
this.prevNode = nearest.node;
this.prevPos = base.transform.position;
}
if (this.prevNode != null)
{
if (this.prevNode != null)
{
IRaycastableGraph graph = AstarData.GetGraph(this.prevNode) as IRaycastableGraph;
if (graph != null)
{
GraphHitInfo info2;
if (graph.Linecast(this.prevPos, base.transform.position, this.prevNode, out info2))
{
info2.point.y = base.transform.position.y;
Vector3 end = VectorMath.ClosestPointOnLine(info2.tangentOrigin, info2.tangentOrigin + info2.tangent, base.transform.position);
Vector3 point = info2.point;
point += Vector3.ClampMagnitude(((Vector3)info2.node.position) - point, 0.008f);
if (graph.Linecast(point, end, info2.node, out info2))
{
info2.point.y = base.transform.position.y;
base.transform.position = info2.point;
}
else
{
end.y = base.transform.position.y;
base.transform.position = end;
}
}
this.prevNode = info2.node;
}
}
this.prevPos = base.transform.position;
}
}
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);
}
}
// will return false if any part of the line is off the nav mesh
// if 'clampFromInNavMesh' is true, we make sure the from position is on the nav mesh before doing checks
// (done in 2d, does not currently support nav meshes on top of one and other)
public static bool IsVisibleOnRecastGraph(Vector3 from, Vector3 to, bool clampFromInNavMesh = false, NNInfo?nearestInfo = null)
{
if (null == AstarPath.active)
{
return(false);
}
NNInfo fromInfo = nearestInfo ?? AstarPath.active.GetNearest(from);
if (null == fromInfo.node)
{
return(false);
}
if (clampFromInNavMesh)
{
from = CalculatePointOnRecastGraph(from, 0.1f, fromInfo);
}
NavGraph graph = AstarData.GetGraph(fromInfo.node);
if (graph != null)
{
IRaycastableGraph rayGraph = graph as IRaycastableGraph;
if (rayGraph != null)
{
GraphHitInfo hit;
if (rayGraph.Linecast(from, to, fromInfo.node, out hit))
{
return(false); // hit an obstacle
}
return(true); // no nav mesh exit
}
return(false); // no recast graph
}
return(false); // no nav mesh
}
// Token: 0x060006F5 RID: 1781 RVA: 0x000444A0 File Offset: 0x000428A0
public Vector3 GetClampedPoint(Vector3 from, Vector3 to, GraphNode hint)
{
Vector3 vector = to;
RaycastHit raycastHit;
if (this.useRaycasting && Physics.Linecast(from, to, out raycastHit, this.mask))
{
vector = raycastHit.point;
}
if (this.useGraphRaycasting && hint != null)
{
NavGraph graph = AstarData.GetGraph(hint);
if (graph != null)
{
IRaycastableGraph raycastableGraph = graph as IRaycastableGraph;
GraphHitInfo graphHitInfo;
if (raycastableGraph != null && raycastableGraph.Linecast(from, vector, hint, out graphHitInfo))
{
vector = graphHitInfo.point;
}
}
}
return(vector);
}
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);
}
}
}
/** Simplifies a funnel path using linecasting.
* Running time is roughly O(n^2 log n) in the worst case (where n = end-start)
* Actually it depends on how the graph looks, so in theory the actual upper limit on the worst case running time is O(n*m log n) (where n = end-start and m = nodes in the graph)
* but O(n^2 log n) is a much more realistic worst case limit.
*
* Requires #graph to implement IRaycastableGraph
*/
public void SimplifyPath (IRaycastableGraph graph, List<GraphNode> nodes, int start, int end, List<GraphNode> result, Vector3 startPoint, Vector3 endPoint) {
if (graph == null) throw new System.ArgumentNullException ("graph");
if (start > end) {
throw new System.ArgumentException ("start >= end");
}
int ostart = start;
int count = 0;
while (true) {
if (count++ > 1000) {
Debug.LogError("!!!");
break;
}
if (start == end) {
result.Add (nodes[end]);
return;
}
int resCount = result.Count;
int mx = end+1;
int mn = start+1;
bool anySucceded = false;
while (mx > mn+1) {
int mid = (mx+mn)/2;
GraphHitInfo hit;
Vector3 sp = start == ostart ? startPoint : (Vector3)nodes[start].position;
Vector3 ep = mid == end ? endPoint : (Vector3)nodes[mid].position;
if (graph.Linecast (sp, ep, nodes[start], out hit)) {
mx = mid;
} else {
anySucceded = true;
mn = mid;
}
}
if (!anySucceded) {
result.Add(nodes[start]);
//It is guaranteed that mn = start+1
start = mn;
} else {
//Need to redo the linecast to get the trace
GraphHitInfo hit;
Vector3 sp = start == ostart ? startPoint : (Vector3)nodes[start].position;
Vector3 ep = mn == end ? endPoint : (Vector3)nodes[mn].position;
graph.Linecast (sp, ep, nodes[start], out hit, result);
long penaltySum = 0;
long penaltySum2 = 0;
for (int i=start;i<=mn;i++) {
penaltySum += nodes[i].Penalty + (path.seeker != null ? path.seeker.tagPenalties[nodes[i].Tag] : 0);
}
for (int i=resCount;i<result.Count;i++) {
penaltySum2 += result[i].Penalty + (path.seeker != null ? path.seeker.tagPenalties[result[i].Tag] : 0);
}
// Allow 40% more penalty on average per node
if ((penaltySum*1.4*(mn-start+1)) < (penaltySum2*(result.Count-resCount)) || result[result.Count-1] != nodes[mn]) {
//Debug.Log ((penaltySum*1.4*(mn-start+1)) + " < "+ (penaltySum2*(result.Count-resCount)));
//Debug.DrawLine ((Vector3)nodes[start].Position, (Vector3)nodes[mn].Position, Color.red);
//Linecast hit the wrong node
result.RemoveRange (resCount, result.Count-resCount);
result.Add(nodes[start]);
//Debug.Break();
start = start+1;
} else {
//Debug.Log ("!! " + (penaltySum*1.4*(mn-start+1)) + " < "+ (penaltySum2*(result.Count-resCount)));
//Debug.DrawLine ((Vector3)nodes[start].Position, (Vector3)nodes[mn].Position, Color.green);
//Debug.Break ();
//Remove nodes[end]
result.RemoveAt(result.Count-1);
start = mn;
}
}
}
}
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 = AstarMath.DistancePointSegmentStrict (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 = AstarMath.DistancePointSegmentStrict (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);
}
}
}
private void SimplifyPath(IRaycastableGraph graph, List <GraphNode> nodes, int start, int end, List <GraphNode> result, Vector3 startPoint, Vector3 endPoint)
{
if (graph == null)
{
throw new ArgumentNullException("graph");
}
if (start > end)
{
throw new ArgumentException("start >= end");
}
int num = start;
int num2 = 0;
while (num2++ <= 1000)
{
if (start == end)
{
result.Add(nodes[end]);
return;
}
int count = result.Count;
int i = end + 1;
int num3 = start + 1;
bool flag = false;
while (i > num3 + 1)
{
int num4 = (i + num3) / 2;
Vector3 start2 = (start != num) ? ((Vector3)nodes[start].position) : startPoint;
Vector3 end2 = (num4 != end) ? ((Vector3)nodes[num4].position) : endPoint;
GraphHitInfo graphHitInfo;
if (graph.Linecast(start2, end2, nodes[start], out graphHitInfo))
{
i = num4;
}
else
{
flag = true;
num3 = num4;
}
}
if (!flag)
{
result.Add(nodes[start]);
start = num3;
}
else
{
Vector3 start3 = (start != num) ? ((Vector3)nodes[start].position) : startPoint;
Vector3 end3 = (num3 != end) ? ((Vector3)nodes[num3].position) : endPoint;
GraphHitInfo graphHitInfo2;
graph.Linecast(start3, end3, nodes[start], out graphHitInfo2, result);
long num5 = 0L;
long num6 = 0L;
for (int j = start; j <= num3; j++)
{
num5 += (long)((ulong)nodes[j].Penalty + (ulong)((long)((!(this.path.seeker != null)) ? 0 : this.path.seeker.tagPenalties[(int)((UIntPtr)nodes[j].Tag)])));
}
for (int k = count; k < result.Count; k++)
{
num6 += (long)((ulong)result[k].Penalty + (ulong)((long)((!(this.path.seeker != null)) ? 0 : this.path.seeker.tagPenalties[(int)((UIntPtr)result[k].Tag)])));
}
if ((double)num5 * 1.4 * (double)(num3 - start + 1) < (double)(num6 * (long)(result.Count - count)) || result[result.Count - 1] != nodes[num3])
{
result.RemoveRange(count, result.Count - count);
result.Add(nodes[start]);
start++;
}
else
{
result.RemoveAt(result.Count - 1);
start = num3;
}
}
}
Debug.LogError("Was the path really long or have we got cought in an infinite loop?");
}
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 graph = this.graph as IRaycastableGraph;
if ((graph == null) || (this.funnelSimplificationMode == FunnelSimplification.None))
{
if (this.nodes.Capacity < (end - start))
{
this.nodes.Capacity = end - start;
}
for (int j = start; j <= end; j++)
{
TriangleMeshNode item = nodes[j] as TriangleMeshNode;
if (item != null)
{
this.nodes.Add(item);
}
}
}
else
{
List <GraphNode> result = ListPool <GraphNode> .Claim(end - start);
switch (this.funnelSimplificationMode)
{
case FunnelSimplification.Iterative:
this.SimplifyPath(graph, nodes, start, end, result, this.exactStart, this.exactEnd);
break;
case FunnelSimplification.RecursiveBinary:
SimplifyPath2(graph, nodes, start, end, result, this.exactStart, this.exactEnd);
break;
case FunnelSimplification.RecursiveTrinary:
SimplifyPath3(graph, nodes, start, end, result, this.exactStart, this.exactEnd, 0);
break;
}
if (this.nodes.Capacity < result.Count)
{
this.nodes.Capacity = result.Count;
}
for (int k = 0; k < result.Count; k++)
{
TriangleMeshNode node = result[k] as TriangleMeshNode;
if (node != null)
{
this.nodes.Add(node);
}
}
ListPool <GraphNode> .Release(result);
}
for (int i = 0; i < (this.nodes.Count - 1); i++)
{
this.nodes[i].GetPortal(this.nodes[i + 1], this.left, this.right, false);
}
this.left.Add(this.exactEnd);
this.right.Add(this.exactEnd);
}
/** Will calculate a number of points around \a p which are on the graph and are separated by \a clearance from each other.
* This is like GetPointsAroundPoint except that \a previousPoints are treated as being in world space.
* The average of the points will be found and then that will be treated as the group center.
*/
public static void GetPointsAroundPointWorld (Vector3 p, IRaycastableGraph g, List<Vector3> previousPoints, float radius, float clearanceRadius) {
if ( previousPoints.Count == 0 ) return;
var avg = Vector3.zero;
for ( var i = 0; i < previousPoints.Count; i++ ) avg += previousPoints[i];
avg /= previousPoints.Count;
for ( var i = 0; i < previousPoints.Count; i++ ) previousPoints[i] -= avg;
GetPointsAroundPoint ( p, g, previousPoints, radius, clearanceRadius );
}
/// <summary>
/// Simplifies a funnel path using linecasting.
/// Running time is roughly O(n^2 log n) in the worst case (where n = end-start)
/// Actually it depends on how the graph looks, so in theory the actual upper limit on the worst case running time is O(n*m log n) (where n = end-start and m = nodes in the graph)
/// but O(n^2 log n) is a much more realistic worst case limit.
///
/// Requires <see cref="graph"/> to implement IRaycastableGraph
/// </summary>
public static void Simplify(PathPart part, IRaycastableGraph graph, List <GraphNode> nodes, List <GraphNode> result, int[] tagPenalties, int traversableTags)
{
var start = part.startIndex;
var end = part.endIndex;
var startPoint = part.startPoint;
var endPoint = part.endPoint;
if (graph == null)
{
throw new System.ArgumentNullException(nameof(graph));
}
if (start > end)
{
throw new System.ArgumentException("start > end");
}
// Do a straight line of sight check to see if the path can be simplified to a single line
{
GraphHitInfo hit;
if (!graph.Linecast(startPoint, endPoint, nodes[start], out hit) && hit.node == nodes[end])
{
graph.Linecast(startPoint, endPoint, nodes[start], out hit, result);
long penaltySum = 0;
long penaltySum2 = 0;
for (int i = start; i <= end; i++)
{
penaltySum += nodes[i].Penalty + tagPenalties[nodes[i].Tag];
}
bool walkable = true;
for (int i = 0; i < result.Count; i++)
{
penaltySum2 += result[i].Penalty + tagPenalties[result[i].Tag];
walkable &= ((traversableTags >> (int)result[i].Tag) & 1) == 1;
}
// Allow 40% more penalty on average per node
if (!walkable || (penaltySum * 1.4 * result.Count) < (penaltySum2 * (end - start + 1)))
{
// The straight line penalties are much higher than the original path.
// Revert the simplification
result.Clear();
}
else
{
// The straight line simplification looks good.
// We are done here.
return;
}
}
}
int ostart = start;
int count = 0;
while (true)
{
if (count++ > 1000)
{
Debug.LogError("Was the path really long or have we got cought in an infinite loop?");
break;
}
if (start == end)
{
result.Add(nodes[end]);
return;
}
int resCount = result.Count;
// Run a binary search to find the furthest node that we have a clear line of sight to
int mx = end + 1;
int mn = start + 1;
bool anySucceded = false;
while (mx > mn + 1)
{
int mid = (mx + mn) / 2;
GraphHitInfo hit;
Vector3 sp = start == ostart ? startPoint : (Vector3)nodes[start].position;
Vector3 ep = mid == end ? endPoint : (Vector3)nodes[mid].position;
// Check if there is an obstacle between these points, or if there is no obstacle, but we didn't end up at the right node.
// The second case can happen for example in buildings with multiple floors.
if (graph.Linecast(sp, ep, nodes[start], out hit) || hit.node != nodes[mid])
{
mx = mid;
}
else
{
anySucceded = true;
mn = mid;
}
}
if (!anySucceded)
{
result.Add(nodes[start]);
// It is guaranteed that mn = start+1
start = mn;
}
else
{
// Replace a part of the path with the straight path to the furthest node we had line of sight to.
// Need to redo the linecast to get the trace (i.e. list of nodes along the line of sight).
GraphHitInfo hit;
Vector3 sp = start == ostart ? startPoint : (Vector3)nodes[start].position;
Vector3 ep = mn == end ? endPoint : (Vector3)nodes[mn].position;
graph.Linecast(sp, ep, nodes[start], out hit, result);
long penaltySum = 0;
long penaltySum2 = 0;
for (int i = start; i <= mn; i++)
{
penaltySum += nodes[i].Penalty + tagPenalties[nodes[i].Tag];
}
bool walkable = true;
for (int i = resCount; i < result.Count; i++)
{
penaltySum2 += result[i].Penalty + tagPenalties[result[i].Tag];
walkable &= ((traversableTags >> (int)result[i].Tag) & 1) == 1;
}
// Allow 40% more penalty on average per node
if (!walkable || (penaltySum * 1.4 * (result.Count - resCount)) < (penaltySum2 * (mn - start + 1)) || result[result.Count - 1] != nodes[mn])
{
//Debug.DrawLine ((Vector3)nodes[start].Position, (Vector3)nodes[mn].Position, Color.red);
// Linecast hit the wrong node or it is a lot more expensive than the original path
result.RemoveRange(resCount, result.Count - resCount);
result.Add(nodes[start]);
//Debug.Break();
start = start + 1;
}
else
{
//Debug.DrawLine ((Vector3)nodes[start].Position, (Vector3)nodes[mn].Position, Color.green);
//Remove nodes[end]
result.RemoveAt(result.Count - 1);
start = mn;
}
}
}
}
/** Build a funnel corridor from a node list slice.
* The nodes are assumed to be of type TriangleMeshNode.
*
* \param start Start index in the nodes array
* \param end End index in the nodes array, this index is inclusive
*/
public void BuildFunnelCorridor(List <GraphNode> nodes, int start, int end)
{
//Make sure start and end points are on the correct nodes
exactStart = (nodes[start] as MeshNode).ClosestPointOnNode(exactStart);
exactEnd = (nodes[end] as MeshNode).ClosestPointOnNode(exactEnd);
left.Clear();
right.Clear();
left.Add(exactStart);
right.Add(exactStart);
this.nodes.Clear();
IRaycastableGraph rcg = graph as IRaycastableGraph;
if (rcg != null && funnelSimplificationMode != FunnelSimplification.None)
{
List <GraphNode> tmp = Pathfinding.Util.ListPool <GraphNode> .Claim(end - start);
switch (funnelSimplificationMode)
{
case FunnelSimplification.Iterative:
SimplifyPath(rcg, nodes, start, end, tmp, exactStart, exactEnd);
break;
case FunnelSimplification.RecursiveBinary:
SimplifyPath2(rcg, nodes, start, end, tmp, exactStart, exactEnd);
break;
case FunnelSimplification.RecursiveTrinary:
SimplifyPath3(rcg, nodes, start, end, tmp, exactStart, exactEnd);
break;
}
if (this.nodes.Capacity < tmp.Count)
{
this.nodes.Capacity = tmp.Count;
}
for (int i = 0; i < tmp.Count; i++)
{
//Guaranteed to be TriangleMeshNodes since they are all in the same graph
TriangleMeshNode nd = tmp[i] as TriangleMeshNode;
if (nd != null)
{
this.nodes.Add(nd);
}
}
Pathfinding.Util.ListPool <GraphNode> .Release(tmp);
}
else
{
if (this.nodes.Capacity < end - start)
{
this.nodes.Capacity = (end - start);
}
for (int i = start; i <= end; i++)
{
//Guaranteed to be TriangleMeshNodes since they are all in the same graph
TriangleMeshNode nd = nodes[i] as TriangleMeshNode;
if (nd != null)
{
this.nodes.Add(nd);
}
}
}
for (int i = 0; i < this.nodes.Count - 1; i++)
{
/** \todo should use return value in future versions */
this.nodes[i].GetPortal(this.nodes[i + 1], left, right, false);
}
left.Add(exactEnd);
right.Add(exactEnd);
}
public void SimplifyPath(IRaycastableGraph graph, List <GraphNode> nodes, int start, int end, List <GraphNode> result, Vector3 startPoint, Vector3 endPoint)
{
if (graph == null)
{
throw new ArgumentNullException("graph");
}
if (start > end)
{
throw new ArgumentException("start >= end");
}
int num = start;
int num2 = 0;
Label_0028:
if (num2++ > 0x3e8)
{
Debug.LogError("!!!");
}
else if (start == end)
{
result.Add(nodes[end]);
}
else
{
int count = result.Count;
int num4 = end + 1;
int num5 = start + 1;
bool flag = false;
while (num4 > (num5 + 1))
{
GraphHitInfo info;
int num6 = (num4 + num5) / 2;
Vector3 vector = (start != num) ? ((Vector3)nodes[start].position) : startPoint;
Vector3 vector2 = (num6 != end) ? ((Vector3)nodes[num6].position) : endPoint;
if (graph.Linecast(vector, vector2, nodes[start], out info))
{
num4 = num6;
}
else
{
flag = true;
num5 = num6;
}
}
if (!flag)
{
result.Add(nodes[start]);
start = num5;
}
else
{
GraphHitInfo info2;
Vector3 vector3 = (start != num) ? ((Vector3)nodes[start].position) : startPoint;
Vector3 vector4 = (num5 != end) ? ((Vector3)nodes[num5].position) : endPoint;
graph.Linecast(vector3, vector4, nodes[start], out info2, result);
long num7 = 0L;
long num8 = 0L;
for (int i = start; i <= num5; i++)
{
num7 += nodes[i].Penalty + ((this.path.seeker == null) ? ((long)0) : ((long)this.path.seeker.tagPenalties[nodes[i].Tag]));
}
for (int j = count; j < result.Count; j++)
{
num8 += result[j].Penalty + ((this.path.seeker == null) ? ((long)0) : ((long)this.path.seeker.tagPenalties[result[j].Tag]));
}
if ((((num7 * 1.4) * ((num5 - start) + 1)) < (num8 * (result.Count - count))) || (result[result.Count - 1] != nodes[num5]))
{
result.RemoveRange(count, result.Count - count);
result.Add(nodes[start]);
start++;
}
else
{
result.RemoveAt(result.Count - 1);
start = num5;
}
}
goto Label_0028;
}
}
// Token: 0x060027A4 RID: 10148 RVA: 0x001B33F8 File Offset: 0x001B15F8
public static void GetPointsAroundPoint(Vector3 center, IRaycastableGraph g, List <Vector3> previousPoints, float radius, float clearanceRadius)
{
if (g == null)
{
throw new ArgumentNullException("g");
}
NavGraph navGraph = g as NavGraph;
if (navGraph == null)
{
throw new ArgumentException("g is not a NavGraph");
}
NNInfoInternal nearestForce = navGraph.GetNearestForce(center, NNConstraint.Default);
center = nearestForce.clampedPosition;
if (nearestForce.node == null)
{
return;
}
radius = Mathf.Max(radius, 1.4142f * clearanceRadius * Mathf.Sqrt((float)previousPoints.Count));
clearanceRadius *= clearanceRadius;
int i = 0;
while (i < previousPoints.Count)
{
Vector3 vector = previousPoints[i];
float magnitude = vector.magnitude;
if (magnitude > 0f)
{
vector /= magnitude;
}
float num = radius;
vector *= num;
int num2 = 0;
Vector3 vector2;
for (;;)
{
vector2 = center + vector;
GraphHitInfo graphHitInfo;
if (g.Linecast(center, vector2, nearestForce.node, out graphHitInfo))
{
if (graphHitInfo.point == Vector3.zero)
{
num2++;
if (num2 > 8)
{
goto Block_7;
}
}
else
{
vector2 = graphHitInfo.point;
}
}
bool flag = false;
for (float num3 = 0.1f; num3 <= 1f; num3 += 0.05f)
{
Vector3 vector3 = Vector3.Lerp(center, vector2, num3);
flag = true;
for (int j = 0; j < i; j++)
{
if ((previousPoints[j] - vector3).sqrMagnitude < clearanceRadius)
{
flag = false;
break;
}
}
if (flag || num2 > 8)
{
flag = true;
previousPoints[i] = vector3;
break;
}
}
if (flag)
{
break;
}
clearanceRadius *= 0.9f;
vector = UnityEngine.Random.onUnitSphere * Mathf.Lerp(num, radius, (float)(num2 / 5));
vector.y = 0f;
num2++;
}
IL_19D:
i++;
continue;
Block_7:
previousPoints[i] = vector2;
goto IL_19D;
}
}
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);
}
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);
}
}
}
/** Will calculate a number of points around \a center which are on the graph and are separated by \a clearance from each other.
* The maximum distance from \a center to any point will be \a radius.
* Points will first be tried to be laid out as \a previousPoints and if that fails, random points will be selected.
* This is great if you want to pick a number of target points for group movement. If you pass all current agent points from e.g the group's average position
* this method will return target points so that the units move very little within the group, this is often aesthetically pleasing and reduces jitter if using
* some kind of local avoidance.
*
* \param center The point to generate points around
* \param g The graph to use for linecasting. If you are only using one graph, you can get this by AstarPath.active.graphs[0] as IRaycastableGraph.
* Note that not all graphs are raycastable, recast, navmesh and grid graphs are raycastable. On recast and navmesh it works the best.
* \param previousPoints The points to use for reference. Note that these should not be in world space. They are treated as relative to \a center.
* The new points will overwrite the existing points in the list. The result will be in world space, not relative to \a center.
* \param radius The final points will be at most this distance from \a center.
* \param clearanceRadius The points will if possible be at least this distance from each other.
*
* \todo Write unit tests
*/
public static void GetPointsAroundPoint(Vector3 center, IRaycastableGraph g, List <Vector3> previousPoints, float radius, float clearanceRadius)
{
if (g == null)
{
throw new System.ArgumentNullException("g");
}
var graph = g as NavGraph;
if (graph == null)
{
throw new System.ArgumentException("g is not a NavGraph");
}
NNInfoInternal nn = graph.GetNearestForce(center, NNConstraint.Default);
center = nn.clampedPosition;
if (nn.node == null)
{
// No valid point to start from
return;
}
// Make sure the enclosing circle has a radius which can pack circles with packing density 0.5
radius = Mathf.Max(radius, 1.4142f * clearanceRadius * Mathf.Sqrt(previousPoints.Count)); //Mathf.Sqrt(previousPoints.Count*clearanceRadius*2));
clearanceRadius *= clearanceRadius;
for (int i = 0; i < previousPoints.Count; i++)
{
Vector3 dir = previousPoints[i];
float magn = dir.magnitude;
if (magn > 0)
{
dir /= magn;
}
float newMagn = radius; //magn > radius ? radius : magn;
dir *= newMagn;
GraphHitInfo hit;
int tests = 0;
while (true)
{
Vector3 pt = center + dir;
if (g.Linecast(center, pt, nn.node, out hit))
{
if (hit.point == PF.Vector3.zero)
{
// Oops, linecast actually failed completely
// try again unless we have tried lots of times
// then we just continue anyway
tests++;
if (tests > 8)
{
previousPoints[i] = pt;
break;
}
}
else
{
pt = hit.point;
}
}
bool worked = false;
for (float q = 0.1f; q <= 1.0f; q += 0.05f)
{
Vector3 qt = Vector3.Lerp(center, pt, q);
worked = true;
for (int j = 0; j < i; j++)
{
if ((previousPoints[j] - qt).sqrMagnitude < clearanceRadius)
{
worked = false;
break;
}
}
// Abort after 8 tests or when we have found a valid point
if (worked || tests > 8)
{
worked = true;
previousPoints[i] = qt;
break;
}
}
// Break out of nested loop
if (worked)
{
break;
}
// If we could not find a valid point, reduce the clearance radius slightly to improve
// the chances next time
clearanceRadius *= 0.9f;
// This will pick points in 2D closer to the edge of the circle with a higher probability
dir = Random.onUnitSphere * Mathf.Lerp(newMagn, radius, tests / 5);
dir.y = 0;
tests++;
}
}
}
/** Simplifies a funnel path using linecasting.
* Running time is roughly O(n^2 log n) in the worst case (where n = end-start)
* Actually it depends on how the graph looks, so in theory the actual upper limit on the worst case running time is O(n*m log n) (where n = end-start and m = nodes in the graph)
* but O(n^2 log n) is a much more realistic worst case limit.
*
* Requires #graph to implement IRaycastableGraph
*/
public void SimplifyPath(IRaycastableGraph graph, List <GraphNode> nodes, int start, int end, List <GraphNode> result, Vector3 startPoint, Vector3 endPoint)
{
if (graph == null)
{
throw new System.ArgumentNullException("graph");
}
if (start > end)
{
throw new System.ArgumentException("start >= end");
}
int ostart = start;
int count = 0;
while (true)
{
if (count++ > 1000)
{
Debug.LogError("!!!");
break;
}
if (start == end)
{
result.Add(nodes[end]);
return;
}
int resCount = result.Count;
int mx = end + 1;
int mn = start + 1;
bool anySucceded = false;
while (mx > mn + 1)
{
int mid = (mx + mn) / 2;
GraphHitInfo hit;
Vector3 sp = start == ostart ? startPoint : (Vector3)nodes[start].position;
Vector3 ep = mid == end ? endPoint : (Vector3)nodes[mid].position;
if (graph.Linecast(sp, ep, nodes[start], out hit))
{
mx = mid;
}
else
{
anySucceded = true;
mn = mid;
}
}
if (!anySucceded)
{
result.Add(nodes[start]);
//It is guaranteed that mn = start+1
start = mn;
}
else
{
//Need to redo the linecast to get the trace
GraphHitInfo hit;
Vector3 sp = start == ostart ? startPoint : (Vector3)nodes[start].position;
Vector3 ep = mn == end ? endPoint : (Vector3)nodes[mn].position;
graph.Linecast(sp, ep, nodes[start], out hit, result);
long penaltySum = 0;
long penaltySum2 = 0;
for (int i = start; i <= mn; i++)
{
penaltySum += nodes[i].Penalty + (path.seeker != null ? path.seeker.tagPenalties[nodes[i].Tag] : 0);
}
for (int i = resCount; i < result.Count; i++)
{
penaltySum2 += result[i].Penalty + (path.seeker != null ? path.seeker.tagPenalties[result[i].Tag] : 0);
}
// Allow 40% more penalty on average per node
if ((penaltySum * 1.4 * (mn - start + 1)) < (penaltySum2 * (result.Count - resCount)) || result[result.Count - 1] != nodes[mn])
{
//Debug.Log ((penaltySum*1.4*(mn-start+1)) + " < "+ (penaltySum2*(result.Count-resCount)));
//Debug.DrawLine ((Vector3)nodes[start].Position, (Vector3)nodes[mn].Position, Color.red);
//Linecast hit the wrong node
result.RemoveRange(resCount, result.Count - resCount);
result.Add(nodes[start]);
//Debug.Break();
start = start + 1;
}
else
{
//Debug.Log ("!! " + (penaltySum*1.4*(mn-start+1)) + " < "+ (penaltySum2*(result.Count-resCount)));
//Debug.DrawLine ((Vector3)nodes[start].Position, (Vector3)nodes[mn].Position, Color.green);
//Debug.Break ();
//Remove nodes[end]
result.RemoveAt(result.Count - 1);
start = mn;
}
}
}
}
public static void GetPointsAroundPoint(Vector3 p, IRaycastableGraph g, List <Vector3> previousPoints, float radius, float clearanceRadius)
{
if (g == null)
{
throw new ArgumentNullException("g");
}
NavGraph navGraph = g as NavGraph;
if (navGraph == null)
{
throw new ArgumentException("g is not a NavGraph");
}
NNInfoInternal nearestForce = navGraph.GetNearestForce(p, NNConstraint.Default);
p = nearestForce.clampedPosition;
if (nearestForce.node == null)
{
return;
}
radius = Mathf.Max(radius, 1.4142f * clearanceRadius * Mathf.Sqrt((float)previousPoints.Count));
clearanceRadius *= clearanceRadius;
for (int i = 0; i < previousPoints.Count; i++)
{
Vector3 vector = previousPoints[i];
float magnitude = vector.magnitude;
if (magnitude > 0f)
{
vector /= magnitude;
}
float num = radius;
vector *= num;
bool flag = false;
int num2 = 0;
do
{
Vector3 vector2 = p + vector;
GraphHitInfo graphHitInfo;
if (g.Linecast(p, vector2, nearestForce.node, out graphHitInfo))
{
vector2 = graphHitInfo.point;
}
for (float num3 = 0.1f; num3 <= 1f; num3 += 0.05f)
{
Vector3 vector3 = (vector2 - p) * num3 + p;
flag = true;
for (int j = 0; j < i; j++)
{
if ((previousPoints[j] - vector3).sqrMagnitude < clearanceRadius)
{
flag = false;
break;
}
}
if (flag)
{
previousPoints[i] = vector3;
break;
}
}
if (!flag)
{
if (num2 > 8)
{
flag = true;
}
else
{
clearanceRadius *= 0.9f;
vector = UnityEngine.Random.onUnitSphere * Mathf.Lerp(num, radius, (float)(num2 / 5));
vector.y = 0f;
num2++;
}
}
}while (!flag);
}
}
/** Will calculate a number of points around \a p which are on the graph and are separated by \a clearance from each other.
* The maximum distance from \a p to any point will be \a radius.
* Points will first be tried to be laid out as \a previousPoints and if that fails, random points will be selected.
* This is great if you want to pick a number of target points for group movement. If you pass all current agent points from e.g the group's average position
* this method will return target points so that the units move very little within the group, this is often aesthetically pleasing and reduces jitter if using
* some kind of local avoidance.
*
* \param g The graph to use for linecasting. If you are only using one graph, you can get this by AstarPath.active.graphs[0] as IRaycastableGraph.
* Note that not all graphs are raycastable, recast, navmesh and grid graphs are raycastable. On recast and navmesh it works the best.
* \param previousPoints The points to use for reference. Note that these should not be in world space. They are treated as relative to \a p.
*/
public static void GetPointsAroundPoint (Vector3 p, IRaycastableGraph g, List<Vector3> previousPoints, float radius, float clearanceRadius) {
if (g == null) throw new ArgumentNullException ("g");
var graph = g as NavGraph;
if (graph == null) throw new ArgumentException ("g is not a NavGraph");
var nn = graph.GetNearestForce (p, NNConstraint.Default);
p = nn.clampedPosition;
if (nn.node == null) {
// No valid point to start from
return;
}
// Make sure the enclosing circle has a radius which can pack circles with packing density 0.5
radius = Mathf.Max (radius, 1.4142f*clearanceRadius*Mathf.Sqrt(previousPoints.Count));//Mathf.Sqrt(previousPoints.Count*clearanceRadius*2));
clearanceRadius *= clearanceRadius;
for (var i=0;i<previousPoints.Count;i++) {
var dir = previousPoints[i];
var magn = dir.magnitude;
if (magn > 0) dir /= magn;
var newMagn = radius;//magn > radius ? radius : magn;
dir *= newMagn;
var worked = false;
GraphHitInfo hit;
var tests = 0;
do {
var pt = p + dir;
if (g.Linecast (p, pt, nn.node, out hit)) {
pt = hit.point;
}
for (var q = 0.1f; q <= 1.0f; q+= 0.05f) {
var qt = (pt - p)*q + p;
worked = true;
for (var j=0;j<i;j++) {
if ((previousPoints[j] - qt).sqrMagnitude < clearanceRadius) {
worked = false;
break;
}
}
if (worked) {
previousPoints[i] = qt;
break;
}
}
if (!worked) {
// Abort after 8 tries
if (tests > 8) {
worked = true;
} else {
clearanceRadius *= 0.9f;
// This will pick points in 2D closer to the edge of the circle with a higher probability
dir = Random.onUnitSphere * Mathf.Lerp (newMagn, radius, tests / 5);
dir.y = 0;
tests++;
}
}
} while (!worked);
}
}
public void SimplifyPath(IRaycastableGraph graph, List<GraphNode> nodes, int start, int end, List<GraphNode> result, Vector3 startPoint, Vector3 endPoint)
{
if (graph == null)
{
throw new ArgumentNullException("graph");
}
if (start > end)
{
throw new ArgumentException("start >= end");
}
int num = start;
int num2 = 0;
while (num2++ <= 1000)
{
if (start == end)
{
result.Add(nodes[end]);
return;
}
int count = result.Count;
int i = end + 1;
int num3 = start + 1;
bool flag = false;
while (i > num3 + 1)
{
int num4 = (i + num3) / 2;
Vector3 start2 = (start != num) ? ((Vector3)nodes[start].position) : startPoint;
Vector3 end2 = (num4 != end) ? ((Vector3)nodes[num4].position) : endPoint;
GraphHitInfo graphHitInfo;
if (graph.Linecast(start2, end2, nodes[start], out graphHitInfo))
{
i = num4;
}
else
{
flag = true;
num3 = num4;
}
}
if (!flag)
{
result.Add(nodes[start]);
start = num3;
}
else
{
Vector3 start3 = (start != num) ? ((Vector3)nodes[start].position) : startPoint;
Vector3 end3 = (num3 != end) ? ((Vector3)nodes[num3].position) : endPoint;
GraphHitInfo graphHitInfo2;
graph.Linecast(start3, end3, nodes[start], out graphHitInfo2, result);
long num5 = 0L;
long num6 = 0L;
for (int j = start; j <= num3; j++)
{
num5 += (long)((ulong)nodes[j].Penalty + (ulong)((long)((!(this.path.seeker != null)) ? 0 : this.path.seeker.tagPenalties[(int)((UIntPtr)nodes[j].Tag)])));
}
for (int k = count; k < result.Count; k++)
{
num6 += (long)((ulong)result[k].Penalty + (ulong)((long)((!(this.path.seeker != null)) ? 0 : this.path.seeker.tagPenalties[(int)((UIntPtr)result[k].Tag)])));
}
if ((double)num5 * 1.4 * (double)(num3 - start + 1) < (double)(num6 * (long)(result.Count - count)) || result[result.Count - 1] != nodes[num3])
{
result.RemoveRange(count, result.Count - count);
result.Add(nodes[start]);
start++;
}
else
{
result.RemoveAt(result.Count - 1);
start = num3;
}
}
}
Debug.LogError("!!!");
}
