Your code works fine.
The only thing you were doing wrong is interpreting the results.
AiComponent.cs
using System.Collections.Generic;
using UnityEngine;
[ExecuteAlways]
public class AiComponent : MonoBehaviour
{
public Transform Target = null;
public NodeComponent[] AllNodes = null;
public List<NodeComponent> result = null;
void Awake ()
{
AllNodes = FindObjectsOfType<NodeComponent>();
}
void Update ()
{
Breadthwise( GetClosestNodeTo(transform) , GetClosestNodeTo(Target) );
}
#if UNITY_EDITOR
void OnDrawGizmos ()
{
if( result!=null && result.Count>1 )
{
Gizmos.color = new Color( 1 , 0 , 0 , 0.8f );
for( int i=0 ; i<result.Count-1 ; i++ )
{
Gizmos.DrawLine( result[i].transform.position , result[i+1].transform.position );
UnityEditor.Handles.Label( result[i].transform.position , $"waypoint #{i}" );
}
UnityEditor.Handles.Label( result[ result.Count-1 ].transform.position , $"waypoint #{result.Count-1}" );
Gizmos.DrawSphere( transform.position , 0.1f );
}
}
#endif
List<NodeComponent> Breadthwise ( NodeComponent start , NodeComponent end )
{
// Debug.Log($"Breadthwise( {start} , {end} )");
result = new List<NodeComponent>();
var visited = new List<NodeComponent>();
var work = new Queue<NodeComponent>();
start.history = new List<NodeComponent>();
visited.Add( start );
work.Enqueue( start );
// Debug.Log($"work.Count: {work.Count}");
while( work.Count>0 )
{
NodeComponent current = work.Dequeue();
if( current==end )
{
//Found Node
// Debug.Log("Found Node");
result = current.history;
result.Add(current);
return result;
}
else
{
//Didn't find Node
// Debug.Log("Didn't find Node");
for( int i=0 ; i<current.neighbors.Length ; i++ )
{
// Debug.Log("\t7");
NodeComponent currentNeighbor = current.neighbors[i];
if( !visited.Contains(currentNeighbor) )
{
// Debug.Log("\t\t8");
currentNeighbor.history = new List<NodeComponent>( current.history );
currentNeighbor.history.Add(current);
visited.Add(currentNeighbor);
work.Enqueue(currentNeighbor);
}
}
}
}
//Route not found, loop ends
return null;
}
NodeComponent GetClosestNodeTo ( Transform t )
{
// Debug.Log($"GetClosestNodeTo( {t} )",t);
Vector3 destination = t.position;
NodeComponent fNode = null;
float minDistSq = float.PositiveInfinity;
foreach( var node in AllNodes )
{
float distSq = ( node.transform.position - destination ).sqrMagnitude;
if( distSq<minDistSq )
{
minDistSq = distSq;
fNode = node;
}
}
return fNode;
}
}
NodeComponent.cs
using System.Collections.Generic;
using System.Linq;
using UnityEngine;
public class NodeComponent : MonoBehaviour
{
public NodeComponent[] neighbors = new NodeComponent[0];
public List<NodeComponent> history = new List<NodeComponent>();
#if UNITY_EDITOR
void OnDrawGizmos ()
{
Gizmos.DrawIcon( this.transform.position , "blendsampler" );
Gizmos.color = new Color( 1 , 1 , 1 , 0.1f );
foreach( var neighbor in neighbors )
Gizmos.DrawLine( this.transform.position , neighbor.transform.position );
}
void OnDrawGizmosSelected ()
{
Gizmos.color = new Color( 1 , 0.92f , 0.016f , 1f );
for( int i=0 ; i<history.Count-1 ; i++ )
Gizmos.DrawLine( history[i].transform.position , history[i+1].transform.position );
}
[UnityEditor.MenuItem( "CONTEXT/"+nameof(NodeComponent)+"/"+nameof(ConnectSelectedNodes) )]
[UnityEditor.MenuItem( "GameObject/"+nameof(ConnectSelectedNodes)+" %q" )]
static void ConnectSelectedNodes ()
{
if( Application.isPlaying ) return;
var selected = UnityEditor.Selection.GetFiltered<NodeComponent>( UnityEditor.SelectionMode.TopLevel );
if( selected.Length<2 ) return;
UnityEditor.Undo.RecordObjects( selected , nameof(ConnectSelectedNodes) );
foreach( var A in selected )
foreach( var B in selected )
if( A!=B )
{
if( !A.neighbors.Any( (other) => other==B ) )
{
System.Array.Resize( ref A.neighbors , A.neighbors.Length+1 );
A.neighbors[ A.neighbors.Length-1 ] = B;
}
UnityEditor.EditorUtility.SetDirty( A );
}
}
#endif
}