\astarpro
/
[AddComponentMenu(“Local Avoidance/RVOGridGraph”)]
public class RVOGridGraph : GraphModifier
{
/* Height of the walls added for each obstacle edge.
- If a graph contains overlapping you should set this low enough so
- that edges on different levels do not interfere, but high enough so that
- agents cannot move over them by mistake.
*/
public float wallHeight = 5;
/** Obstacles currently added to the simulator */
private List obstacles = new List();
/** Last simulator used */
private Simulator lastSim = null;
public override void OnPostCacheLoad()
{
OnLatePostScan();
}
public override void OnLatePostScan()
{
if (!Application.isPlaying)
return;
RemoveObstacles();
NavGraph[] graphs = AstarPath.active.graphs;
RVOSimulator rvosim = FindObjectOfType(typeof(RVOSimulator)) as RVOSimulator;
if (rvosim == null)
throw new System.NullReferenceException("No RVOSimulator could be found in the scene. Please add one to any GameObject");
Pathfinding.RVO.Simulator sim = rvosim.GetSimulator();
for (int i = 0; i < graphs.Length; i++)
{
AddGraphObstacles(sim, graphs[i]);
}
sim.UpdateObstacles();
}
/** Removes obstacles which were added with AddGraphObstacles */
public void RemoveObstacles()
{
if (lastSim == null)
return;
Pathfinding.RVO.Simulator sim = lastSim;
lastSim = null;
for (int i = 0; i < obstacles.Count; i++)
sim.RemoveObstacle(obstacles[i]);
obstacles.Clear();
}
/** Adds RVO obstacles for a Grid Graph */
public void AddGraphObstacles(Pathfinding.RVO.Simulator sim, NavGraph graph)
{
if (obstacles.Count > 0 && lastSim != null && lastSim != sim)
{
Debug.LogError(“Simulator has changed but some old obstacles are still added for the previous simulator. Deleting previous obstacles.”);
RemoveObstacles();
}
//Remember which simulator these obstacles were added to
lastSim = sim;
GridGraph gg = graph as GridGraph;
if (gg == null)
{
return;
}
Node[] nodes = graph.nodes;
for (int w = 0; w < gg.width; w++)
{
for (int d = 0; d < gg.depth; d++)
{
int iA = d * gg.width + w;
if (nodes[iA].walkable == false)
{
/*
* Quarters order ("A" is the tested node) :
* 3 | 4
* --A--B
* 2 | 1
* C D
*/
// Quarter 1
if ((w < (gg.width - 1)) && (d > 0))
{
ComputeQuarter(sim, gg, w, d, 1, 0, 0, -1, 1, -1);
}
// Quarter 2
if ((w > 0) && (d > 0))
{
ComputeQuarter(sim, gg, w, d, 0, -1, -1, 0, -1, -1);
}
// Quarter 3
if ((w > 0) && (d < (gg.depth - 1)))
{
ComputeQuarter(sim, gg, w, d, -1, 0, 0, 1, -1, 1);
}
// Quarter 4
if ((w < (gg.width - 1)) && (d < (gg.depth - 1)))
{
ComputeQuarter(sim, gg, w, d, 0, 1, 1, 0, 1, 1);
}
}
}
}
Debug.Log(string.Format("RVOGridGraph.AddGraphObstacles() : {0} obstacles", obstacles.Count));
}
/** Compute a quarter of 4 nodes (A, B, C, D) where A is unwalkable and add AB or AD ObstacleVertex if needed */
private void ComputeQuarter(Pathfinding.RVO.Simulator sim, GridGraph gg, int w, int d, int wB, int dB, int wC, int dC, int wD, int dD)
{
Node[] nodes = gg.nodes;
int iA = (d + 0) * gg.width + w + 0;
int iB = (d + dB) * gg.width + w + wB;
int iC = (d + dC) * gg.width + w + wC;
int iD = (d + dD) * gg.width + w + wD;
bool nodeB = nodes[iB].walkable;
bool nodeC = nodes[iC].walkable;
bool nodeD = nodes[iD].walkable;
if ((nodeB == false) && (nodeC == true) && (nodeD == true))
{
obstacles.Add(sim.AddObstacle((Vector3)nodes[iA].position, (Vector3)nodes[iB].position, wallHeight));
}
if ((nodeB == false) && (nodeC == true) && (nodeD == false))
{
obstacles.Add(sim.AddObstacle((Vector3)nodes[iA].position, (Vector3)nodes[iD].position, wallHeight));
}
}
/** Draws Gizmos */
public void OnDrawGizmos()
{
OnDrawGizmos(false);
}
/** Draws Gizmos */
public void OnDrawGizmosSelected()
{
OnDrawGizmos(true);
}
/** Draws Gizmos */
public void OnDrawGizmos(bool selected)
{
Gizmos.color = new Color(0.615f, 1, 0.06f, selected ? 1.0f : 0.7f);
foreach (ObstacleVertex ov in obstacles)
{
if (ov.next == null)
throw new System.InvalidOperationException("RVOGridGraph.OnDrawGizmos() : obstacles[...].next == null");
Vector3 a = ov.position;
Vector3 b = ov.next.position;
Gizmos.DrawLine(a, b);
if (selected)
{
// Draw the little arrow to show the direction of the obstacle
Vector3 avg = (a + b) * 0.5f;
Vector3 tang = (b - a).normalized;
if (tang != Vector3.zero)
{
Vector3 normal = Vector3.Cross(Vector3.up, tang);
Gizmos.DrawLine(avg, avg + normal);
Gizmos.DrawLine(avg + normal, avg + normal * 0.5f + tang * 0.5f);
Gizmos.DrawLine(avg + normal, avg + normal * 0.5f - tang * 0.5f);
}
}
}
}
}
`