static void Test_NavEdge_Compare()
{
var cmp = new NavEdgeEqualityComparer();
var edge = new NavEdge(new Vector3(10f, 20f, 30f), new Vector3(20f, 20f, 20f));
var edge_identical = new NavEdge(edge.m_StartPos, edge.m_EndPos);
var edge_reverse = new NavEdge(edge.m_EndPos, edge.m_StartPos);
Debug.Assert(cmp.Equals(edge, edge), "compare to self.");
Debug.Assert(cmp.Equals(edge, edge_identical), "compare to identical.");
Debug.Assert(cmp.Equals(edge, edge_reverse), "compare to mirrored.");
var edge_list = new HashSet <NavEdge>(cmp);
edge_list.Add(edge);
Debug.Assert(edge_list.Add(edge) == false, "Add failed to find duplicate");
Debug.Assert(edge_list.Remove(edge) == true, "Remove failed to find edge");
Debug.Assert(edge_list.Count == 0, "Must be empty now.");
AddIfUniqueAndRemoveIfNot(edge_list, edge);
Debug.Assert(edge_list.Count == 1, "AddIfUniqueAndRemoveIfNot should add edge to empty set.");
AddIfUniqueAndRemoveIfNot(edge_list, edge_identical);
Debug.Assert(edge_list.Count == 0, "AddIfUniqueAndRemoveIfNot should remove identical edge.");
AddIfUniqueAndRemoveIfNot(edge_list, edge);
Debug.Assert(edge_list.Count == 1, "AddIfUniqueAndRemoveIfNot failed to add edge");
AddIfUniqueAndRemoveIfNot(edge_list, edge_reverse);
Debug.Assert(edge_list.Count == 0, "AddIfUniqueAndRemoveIfNot failed to find edge");
Debug.Log("Test complete: NavEdge");
}
// Don't want inner edges (which match another existing edge).
static void AddIfUniqueAndRemoveIfNot(HashSet <NavEdge> set, NavEdge edge)
{
bool had_edge = set.Remove(edge);
if (!had_edge)
{
set.Add(edge);
}
}
public bool addEdge(int para_node1ID, int para_node2ID, NavEdge para_edgeData)
{
bool successFlag = edges.addEdge(para_node1ID,para_node2ID,para_edgeData);
if(successFlag)
{
// Automaticaly place internal neighbour references.
vertices.addNeighbourReferences(para_node1ID,para_node2ID);
}
return successFlag;
}
public bool addEdge(int para_node1ID, int para_node2ID, NavEdge para_edgeData)
{
bool successFlag = false;
producePotentialEdgeKeys(para_node1ID,para_node2ID);
if(( ! edges.ContainsKey(tmpEdgeKeys[0]))
&&( ! edges.ContainsKey(tmpEdgeKeys[1])))
{
edges.Add(createEdgeKey(para_node1ID,para_node2ID),para_edgeData);
successFlag = true;
}
return successFlag;
}
protected override void OnStartRunning()
{
NavMeshTriangulation triangulation = NavMesh.CalculateTriangulation();
// Build Graph from NavMesh
for (var i = 0; i < triangulation.indices.Length - 1; i++)
{
Vector3 from;
Vector3 to;
// If we are on the 3rd indice we close the tri instead of continuing to the next indice
if ((i + 1) % 3 == 0 && i != 0)
{
from = triangulation.vertices[triangulation.indices[i]];
to = triangulation.vertices[triangulation.indices[i - 2]];
}
else
{
from = triangulation.vertices[triangulation.indices[i]];
to = triangulation.vertices[triangulation.indices[i + 1]];
}
if (!_graph.ContainsKey(from))
{
_graph.Add(from, new NavNode {
Index = triangulation.indices[i],
Location = from,
Edges = new HashSet <NavEdge> {
new NavEdge(from, to, Vector3.Distance(from, to), Vector3.Distance(from, to), i)
}
});
}
else
{
var node = _graph[from];
var navEdge = new NavEdge(from, to, Vector3.Distance(from, to), Vector3.Distance(from, to), i);
if (!node.Edges.Contains(navEdge))
{
node.Edges.Add(navEdge);
}
}
if (!_graph.ContainsKey(to))
{
_graph.Add(to, new NavNode {
Index = triangulation.indices[i + 1],
Location = to,
Edges = new HashSet <NavEdge> {
new NavEdge(to, from, Vector3.Distance(from, to), Vector3.Distance(from, to), i)
}
});
}
else
{
var node = _graph[to];
var navEdge = new NavEdge(to, from, Vector3.Distance(from, to), Vector3.Distance(from, to), i);
if (!node.Edges.Contains(navEdge))
{
node.Edges.Add(navEdge);
}
}
}
}
NavMeshLink CreateNavLink(Transform parent, NavLinkGenerator gen, NavEdge edge, Vector3 mid, Vector3 fwd)
{
RaycastHit phys_hit;
RaycastHit ignored;
NavMeshHit nav_hit;
var ground_found = Color.Lerp(Color.red, Color.white, 0.75f);
var ground_missing = Color.Lerp(Color.red, Color.white, 0.35f);
var navmesh_found = Color.Lerp(Color.cyan, Color.white, 0.75f);
var navmesh_missing = Color.Lerp(Color.red, Color.white, 0.65f);
var traverse_clear = Color.green;
var traverse_hit = Color.red;
for (int i = 0; i < gen.m_Steps; ++i)
{
float scale = (float)i / (float)gen.m_Steps;
var top = mid + (fwd * gen.m_MaxHorizontalJump * scale);
var down = top + (Vector3.down * gen.m_MaxVerticalFall);
bool hit = Physics.Linecast(top, down, out phys_hit, gen.m_PhysicsMask.value, QueryTriggerInteraction.Ignore);
//~ Debug.DrawLine(mid, top, hit ? ground_found : ground_missing, k_DrawDuration);
//~ Debug.DrawLine(top, down, hit ? ground_found : ground_missing, k_DrawDuration);
if (hit)
{
var max_distance = gen.m_MaxVerticalFall - phys_hit.distance;
hit = NavMesh.SamplePosition(phys_hit.point, out nav_hit, max_distance, (int)gen.m_NavMask);
// Only place downward links (to avoid back and forth double placement).
hit = hit && (nav_hit.position.y <= mid.y);
// Only accept 90 wedge in front of normal (prevent links
// that other edges are already handling).
hit = hit && Vector3.Dot(nav_hit.position - mid, edge.m_Normal) > Mathf.Cos(gen.m_MaxAngleFromEdgeNormal);
bool is_original_edge = edge.IsPointOnEdge(nav_hit.position);
hit &= !is_original_edge; // don't count self
//~ Debug.DrawLine(phys_hit.point, nav_hit.position, hit ? navmesh_found : navmesh_missing, k_DrawDuration);
if (hit)
{
var height_offset = Vector3.up * gen.m_AgentHeight;
var transit_start = mid + height_offset;
var transit_end = nav_hit.position + height_offset;
// Raycast both ways to ensure we're not inside a collider.
hit = Physics.Linecast(transit_start, transit_end, out ignored, gen.m_PhysicsMask.value, QueryTriggerInteraction.Ignore) ||
Physics.Linecast(transit_end, transit_start, out ignored, gen.m_PhysicsMask.value, QueryTriggerInteraction.Ignore);
//~ Debug.DrawLine(transit_start, transit_end, hit ? traverse_clear : traverse_hit, k_DrawDuration);
if (hit)
{
// Agent can't jump through here.
continue;
}
var height_delta = mid.y - nav_hit.position.y;
Debug.Assert(height_delta >= 0, "Not handling negative delta.");
var prefab = gen.m_JumpLinkPrefab;
if (height_delta > gen.m_MaxVerticalJump)
{
prefab = gen.m_FallLinkPrefab;
}
var t = PrefabUtility.InstantiatePrefab(prefab, parent.gameObject.scene) as Transform;
Debug.Assert(t != null, $"Failed to instantiate {prefab}");
t.SetParent(parent);
t.SetPositionAndRotation(mid, edge.m_Away);
var link = t.GetComponent <NavMeshLink>();
// Push endpoint out into the navmesh to ensure good
// connection. Necessary to prevent invalid links.
var inset = 0.05f;
link.startPoint = link.transform.InverseTransformPoint(mid - fwd * inset);
link.endPoint = link.transform.InverseTransformPoint(nav_hit.position) + (Vector3.forward * inset);
link.width = edge.m_Length;
link.UpdateLink();
Debug.Log("Created NavLink", link);
Undo.RegisterCompleteObjectUndo(link.gameObject, "Create NavMeshLink");
if (m_AttachDebugToLinks)
{
// Attach a component that has the information we
// used to decide how to create this navlink. Much
// easier to go back and inspect it like this than
// to try to examine the output as you generate
// navlinks. Mostly useful for debugging
// NavLinkGenerator.
var reason = link.gameObject.AddComponent <NavLinkCreationReason>();
reason.gen = gen;
reason.fwd = fwd;
reason.mid = mid;
reason.top = top;
reason.down = down;
reason.transit_start = transit_start;
reason.transit_end = transit_end;
reason.nav_hit_position = nav_hit.position;
reason.phys_hit_point = phys_hit.point;
}
return(link);
}
}
}
return(null);
}
