public bool GetPortal(NavMeshNode other, List <Int3> left, List <Int3> right)
{
int first = -1;
int acount = 3;
int bcount = 3;
// find the shared edge
for (int a = 0; a < acount; a++)
{
var va = GetVertex(a);
var va2 = GetVertex((a + 1) % acount);
for (int b = 0; b < bcount; b++)
{
if (va == other.GetVertex((b + 1) % bcount) && va2 == other.GetVertex(b))
{
first = a;
a = acount; // to stop the outer loop
break;
}
}
}
if (first != -1)
{
//All triangles should be clockwise so second is the rightmost vertex (seen from this node)
left.Add(GetVertex(first));
right.Add(GetVertex((first + 1) % acount));
return(true);
}
return(false);
}
protected override int CalCostH(ProcessingNode node, AStarContext context)
{
float heuristicScale = 1.0f;
NavMeshNode calNode = node.astarNode as NavMeshNode;
NavMeshNode targetNode = context.GetTargetNode().astarNode as NavMeshNode;
int dist = (calNode.position - targetNode.position).costMagnitude;
return(UnityEngine.Mathf.RoundToInt(dist * heuristicScale));
}
static bool NodeIntersectsCircle(NavMeshNode node, Int3 p, int radius)
{
if (float.IsPositiveInfinity(radius))
{
return(true);
}
/** \bug Is not correct on the Y axis */
return((p - node.ClosestPointOnNode(p)).sqrMagnitude < radius * radius);
}
int GetBox(NavMeshNode node, IntRect bounds)
{
if (count >= arr.Length)
{
EnsureCapacity(count + 1);
}
arr[count] = new BBTreeBox(node, bounds);
count++;
return(count - 1);
}
public override void Init(INavData data)
{
navData = data as NavMeshData;
for (int i = 0; i < navData.nodes.Length; i++)
{
NavMeshNode node = navData.nodes[i] as NavMeshNode;
AddNode(node);
}
bbTree.RebuildFrom(navData.nodes);
}
public static void Optimize(ref List <AStarNode> path, ref List <Int3> vectorPath)
{
if (path == null || path.Count == 0 || vectorPath == null || vectorPath.Count != path.Count)
{
return;
}
funnelPath.Clear();
left.Clear();
right.Clear();
// Add start point
left.Add(vectorPath[0]);
right.Add(vectorPath[0]);
for (int i = 0; i < path.Count - 1; i++)
{
NavMeshNode node0 = path[i] as NavMeshNode;
NavMeshNode node1 = path[i + 1] as NavMeshNode;
// Get the portal between path[i] and path[i+1] and add it to the left and right lists
bool portalWasAdded = node0.GetPortal(node1, left, right);
if (!portalWasAdded)
{
// Fallback, just use the positions of the nodes
left.Add(node0.position);
right.Add(node0.position);
left.Add(node1.position);
right.Add(node1.position);
}
}
// Add end point
left.Add(vectorPath[vectorPath.Count - 1]);
right.Add(vectorPath[vectorPath.Count - 1]);
if (!RunFunnel(left, right, funnelPath))
{
// If funnel algorithm failed, degrade to simple line
funnelPath.Add(vectorPath[0]);
funnelPath.Add(vectorPath[vectorPath.Count - 1]);
}
vectorPath.Clear();
vectorPath.AddRange(funnelPath);
left.Clear();
right.Clear();
}
private void FindPath(ProcessingNode start, ProcessingNode end, AStarContext context)
{
ProcessingNode endNode = DoAStar(context);
context.rawPathNodeCache.Clear();
context.rawPathPoints.Clear();
ProcessingNode pathNode = endNode;
while (pathNode != null)
{
NavMeshNode navNode = pathNode.astarNode as NavMeshNode;
context.rawPathNodeCache.Add(navNode);
context.rawPathPoints.Add(navNode.position);
pathNode = pathNode.prev;
}
}
/// <summary>
/// calculate tactical G
/// </summary>
private int TacticalCost(NavMeshNode prevNode, NavMeshNode currentNode, AStarContext context)
{
int distCost = prevNode.GetConnectionCost(currentNode.id);
int tacCost = 0;
PathFindingRequest request = context.Request as PathFindingRequest;
// doer's team
TwGame.Team team = (TwGame.Team)request.extend1;
switch (team)
{
case TwGame.Team.Neutral:
tacCost = 0;
break;
case TwGame.Team.Team_1:
case TwGame.Team.Team_2:
{
int MaxInfluence = TwGame.ComInfluenceMap.MaxTeamStrengthValue;
int cur = TwGame.AIUtil.GetTeamStrength(currentNode.position, team);
int pre = TwGame.AIUtil.GetTeamStrength(prevNode.position, team);
// avarage influence between current node's position and previous node's position.
int infl = (cur + pre) >> 1;
if (infl > 0)
{
tacCost = System.Math.Max(-distCost + 1, -distCost * infl / MaxInfluence);
}
else if (infl < 0)
{
tacCost = -distCost * infl / MaxInfluence * 2;
}
}
break;
}
return(distCost + tacCost);
}
public NavMeshNode GetNeighborByEdge(int edge, out int otherEdge)
{
otherEdge = -1;
if (((edge < 0) || (edge > 2)) || (this.connections == null))
{
return(null);
}
Int3 vertex0 = this.GetVertex(edge % 3);
Int3 vertex1 = this.GetVertex((edge + 1) % 3);
for (int i = 0; i < connections.Length; i++)
{
NavMeshNode node2 = graph.GetNode(connections[i]) as NavMeshNode;
if (node2 != null)
{
if ((node2.v1 == vertex0) && (node2.v0 == vertex1))
{
otherEdge = 0;
}
else if ((node2.v2 == vertex0) && (node2.v1 == vertex1))
{
otherEdge = 1;
}
else if ((node2.v0 == vertex0) && (node2.v2 == vertex1))
{
otherEdge = 2;
}
if (otherEdge != -1)
{
return(node2);
}
}
}
return(null);
}
public BBTreeBox(NavMeshNode node, IntRect rect)
{
this.node = node;
this.rect = rect;
left = right = -1;
}
public BBTreeBox(IntRect rect)
{
node = null;
this.rect = rect;
left = right = -1;
}
public override void OnDrawGizmosSelected(Transform transform)
{
#if UNITY_EDITOR
if (navmeshMaterial == null)
{
navmeshMaterial = UnityEditor.AssetDatabase.LoadAssetAtPath <Material>(editorAssets + "/Materials/Navmesh.mat");
if (navmeshMaterial == null)
{
editorAssets = "Assets/Scripts/PathFinding/Editor/EditorAssets";
navmeshMaterial = UnityEditor.AssetDatabase.LoadAssetAtPath <Material>(editorAssets + "/Materials/Navmesh.mat");
}
if (navmeshMaterial == null)
{
editorAssets = "Assets/Editor/PathFinding/EditorAssets";
navmeshMaterial = UnityEditor.AssetDatabase.LoadAssetAtPath <Material>(editorAssets + "/Materials/Navmesh.mat");
}
//navmeshOutlineMaterial = UnityEditor.AssetDatabase.LoadAssetAtPath<Material>(editorAssets + "/Materials/NavmeshOutline.mat");
}
#endif
Matrix4x4 defaultMatrix = Gizmos.matrix;
Gizmos.matrix = transform.localToWorldMatrix;
Color defaultColor = Gizmos.color;
// begin draw
if (ShowOutline)
{
NavMeshNode[] nodes = this.nodes;
for (int i = 0; i < nodes.Length; i++)
{
NavMeshNode node = nodes[i] as NavMeshNode;
float a1 = VectorMath.SignedTriangleAreaTimes2XZ((Vector3)node.v0, (Vector3)node.v1, (Vector3)node.v2);
long a2 = VectorMath.SignedTriangleAreaTimes2XZ(node.v0, node.v1, node.v2);
if (a1 * a2 < 0)
{
Debug.LogError(a1 + " " + a2);
}
Vector3 up = Vector3.up * 0.01f;
if (VectorMath.IsClockwiseXZ(node.v0, node.v1, node.v2))
{
Debug.DrawLine((Vector3)node.v0, up + (Vector3)node.v1, lineColor);
Debug.DrawLine((Vector3)node.v1, up + (Vector3)node.v2, lineColor);
Debug.DrawLine((Vector3)node.v2, up + (Vector3)node.v0, lineColor);
}
else
{
Debug.DrawLine((Vector3)node.v0, up + (Vector3)node.v1, Color.red);
Debug.DrawLine((Vector3)node.v1, up + (Vector3)node.v2, Color.red);
Debug.DrawLine((Vector3)node.v2, up + (Vector3)node.v0, Color.red);
}
}
}
UpdateGizmoMeshes();
if (ShowMesh)
{
for (int pass = 0; pass <= 2; pass++)
{
if (navmeshMaterial != null)
{
navmeshMaterial.SetPass(pass);
}
for (int i = 0; i < gizmoMeshes.Count; i++)
{
Graphics.DrawMeshNow(gizmoMeshes[i].surfaceMesh, Matrix4x4.identity);
}
}
}
/*for (int i = 0; i < insertPoints.Count; i+=3)
* {
* Debug.DrawLine(Vector3.up + (Vector3)insertPoints[i], Vector3.up + (Vector3)insertPoints[i + 1], Color.green);
* Debug.DrawLine(Vector3.up + (Vector3)insertPoints[i + 1], Vector3.up + (Vector3)insertPoints[i + 2], Color.green);
* Debug.DrawLine(Vector3.up + (Vector3)insertPoints[i + 2], Vector3.up + (Vector3)insertPoints[i], Color.green);
* }*/
// end draw
Gizmos.color = defaultColor;
Gizmos.matrix = defaultMatrix;
}
void GenerateNodes(int[] triangles, Int3[] vertices)
{
UnityEngine.Profiling.Profiler.BeginSample("Constructing Nodes");
nodes = new NavMeshNode[triangles.Length / 3];
for (int i = 0; i < nodes.Length; i++)
{
nodes[i] = new NavMeshNode(i);
NavMeshNode node = nodes[i];
node.Penalty = initialPenalty;
node.Walkable = true;
node.v0 = vertices[triangles[i * 3]];
node.v1 = vertices[triangles[i * 3 + 1]];
node.v2 = vertices[triangles[i * 3 + 2]];
if (!VectorMath.IsClockwiseXZ(node.v0, node.v1, node.v2))
{
Int3 tmp = node.v0;
node.v0 = node.v2;
node.v2 = tmp;
}
if (VectorMath.IsColinearXZ(node.v0, node.v1, node.v2))
{
Debug.DrawLine((Vector3)node.v0, (Vector3)node.v1, Color.red);
Debug.DrawLine((Vector3)node.v1, (Vector3)node.v2, Color.red);
Debug.DrawLine((Vector3)node.v2, (Vector3)node.v0, Color.red);
}
// Make sure position is correctly set
node.UpdatePositionFromVertices();
}
UnityEngine.Profiling.Profiler.EndSample();
var sides = new Dictionary <Int2, NavMeshNode>();
for (int i = 0, j = 0; i < triangles.Length; j += 1, i += 3)
{
sides[new Int2(triangles[i + 0], triangles[i + 1])] = nodes[j];
sides[new Int2(triangles[i + 1], triangles[i + 2])] = nodes[j];
sides[new Int2(triangles[i + 2], triangles[i + 0])] = nodes[j];
}
UnityEngine.Profiling.Profiler.BeginSample("Connecting Nodes");
var connections = new List <int>();
var connectionCosts = new List <int>();
for (int i = 0, j = 0; i < triangles.Length; j += 1, i += 3)
{
connections.Clear();
connectionCosts.Clear();
NavMeshNode node = nodes[j];
for (int q = 0; q < 3; q++)
{
NavMeshNode other;
if (sides.TryGetValue(new Int2(triangles[i + ((q + 1) % 3)], triangles[i + q]), out other))
{
connections.Add(other.id);
connectionCosts.Add((node.position - other.position).costMagnitude);
}
}
node.connections = connections.ToArray();
node.connectionCosts = connectionCosts.ToArray();
}
UnityEngine.Profiling.Profiler.EndSample();
UnityEngine.Profiling.Profiler.BeginSample("Rebuilding BBTree");
//RebuildBBTree(this);
UnityEngine.Profiling.Profiler.EndSample();
//Debug.Log("Node Count " + nodes.Length);
}
static Int3 ClosestPointOnNode(NavMeshNode node, Int3 pos)
{
return(Polygon.ClosestPointOnTriangle(node.v0, node.v1, node.v2, pos));
}
public override bool LineCastForMoving(ref HitInfo hit, MoveType mov)
{
Int3 from = hit.from;
Int3 to = hit.to;
hit.hitPosition = from;
if (trace != null)
{
trace.Clear();
}
Int3 end = to;
Int3 origin = from;
if (origin == end)
{
hit.hitPosition = from;
return(false);
}
NavMeshNode node = GetNearest(from, NNConstraint.None).node;
if (node == null)
{
Debug.LogError("Could not find a valid node to start from");
hit.hitPosition = from;
return(true);
}
origin = node.ClosestPointOnNode(origin);
List <Int3> left = Util.ListPool <Int3> .Claim();
List <Int3> right = Util.ListPool <Int3> .Claim();
int counter = 0;
while (true)
{
counter++;
if (counter > 2000)
{
Debug.LogError("Linecast was stuck in infinite loop. Breaking.");
Util.ListPool <Int3> .Release(left);
Util.ListPool <Int3> .Release(right);
return(true);
}
NavMeshNode newNode = null;
if (trace != null)
{
trace.Add(node);
}
if (node.ContainsPoint(end))
{
hit.hitPosition = to;
Util.ListPool <Int3> .Release(left);
Util.ListPool <Int3> .Release(right);
return(false);
}
for (int i = 0; i < node.connections.Length; i++)
{
left.Clear();
right.Clear();
NavMeshNode other = GetNode(node.connections[i]) as NavMeshNode;
if (!node.GetPortal(other, left, right))
{
continue;
}
Int3 a = left[0];
Int3 b = right[0];
//i.e Left or colinear
if (!VectorMath.RightXZ(a, b, origin))
{
if (VectorMath.RightXZ(a, b, end))
{
//Since polygons are laid out in clockwise order, the ray would intersect (if intersecting) this edge going in to the node, not going out from it
continue;
}
}
float factor1, factor2;
if (VectorMath.LineIntersectionFactorXZ(a, b, origin, end, out factor1, out factor2))
{
//Intersection behind the start
if (factor2 < 0)
{
continue;
}
if (factor1 >= 0 && factor1 <= 1)
{
newNode = other;
break;
}
}
}
if (newNode == null)
{
//Possible edge hit
int vs = node.GetVertexCount();
for (int i = 0; i < vs; i++)
{
var a = node.GetVertex(i);
var b = node.GetVertex((i + 1) % vs);
//i.e left or colinear
if (!VectorMath.RightXZ(a, b, origin))
{
//Since polygons are laid out in clockwise order, the ray would intersect (if intersecting) this edge going in to the node, not going out from it
if (VectorMath.RightXZ(a, b, end))
{
//Since polygons are laid out in clockwise order, the ray would intersect (if intersecting) this edge going in to the node, not going out from it
continue;
}
}
float factor1, factor2;
if (VectorMath.LineIntersectionFactorXZ(a, b, origin, end, out factor1, out factor2))
{
if (factor2 < 0)
{
continue;
}
if (factor1 >= 0 && factor1 <= 1)
{
Vector3 intersectionPoint = (Vector3)a + (Vector3)(b - a) * factor1;
hit.hitPosition = new Int3(intersectionPoint);
Util.ListPool <Int3> .Release(left);
Util.ListPool <Int3> .Release(right);
return(true);
}
}
}
//Ok, this is wrong...
Debug.LogWarning("Linecast failing because point not inside node, and line does not hit any edges of it");
Util.ListPool <Int3> .Release(left);
Util.ListPool <Int3> .Release(right);
return(false);
}
node = newNode;
}
}
