/// <summary>
/// Get the crowd agent's neighbors.
/// </summary>
/// <param name="pos">Current position</param>
/// <param name="height">The height</param>
/// <param name="range">The range to search within</param>
/// <param name="skip">The current crowd agent</param>
/// <param name="result">The neihbors array</param>
/// <param name="maxResult">The maximum number of neighbors that can be stored</param>
/// <param name="agents">Array of all crowd agents</param>
/// <param name="grid">The ProximityGrid</param>
/// <returns>The number of neighbors</returns>
public int GetNeighbors(Vector3 pos, float height, float range, Agent skip, CrowdNeighbor[] result, int maxResult, Agent[] agents, ProximityGrid<Agent> grid)
{
int n = 0;
const int MAX_NEIS = 32;
Agent[] ids = new Agent[MAX_NEIS];
int nids = grid.QueryItems(pos.X - range, pos.Z - range, pos.X + range, pos.Z + range, ids, MAX_NEIS);
for (int i = 0; i < nids; i++)
{
Agent ag = ids[i];
if (ag == skip)
continue;
//check for overlap
Vector3 diff = pos - ag.Position;
if (Math.Abs(diff.Y) >= (height + ag.Parameters.Height) / 2.0f)
continue;
diff.Y = 0;
float distSqr = diff.LengthSquared();
if (distSqr > range * range)
continue;
n = AddNeighbor(ids[i], distSqr, result, n, maxResult);
}
return n;
}
/// <summary>
/// Add a CrowdNeighbor to the array
/// </summary>
/// <param name="agent">The neighbor</param>
/// <param name="dist">Distance from current agent</param>
/// <param name="neis">The neighbors array</param>
/// <param name="nneis">The number of neighbors</param>
/// <param name="maxNeis">The maximum number of neighbors allowed</param>
/// <returns>An updated neighbor count</returns>
public int AddNeighbor(Agent agent, float dist, CrowdNeighbor[] neis, int nneis, int maxNeis)
{
//insert neighbor based on distance
int neiPos = 0;
if (nneis == 0)
{
neiPos = nneis;
}
else if (dist >= neis[nneis - 1].Distance)
{
if (nneis >= maxNeis)
return nneis;
neiPos = nneis;
}
else
{
int i;
for (i = 0; i < nneis; i++)
if (dist <= neis[i].Distance)
break;
int tgt = i + 1;
int n = Math.Min(nneis - i, maxNeis - tgt);
if (n > 0)
{
for (int j = 0; j < n; j++)
neis[tgt + j] = neis[i + j];
}
neiPos = i;
}
//TODO rework Crowd so that Agents are passed around instead of indices
int index;
for (index = 0; index < agents.Length; index++)
{
if (agent.Equals(agents[index]))
break;
}
if (index == agents.Length)
throw new IndexOutOfRangeException("Agent not in crowd.");
var neighbor = new CrowdNeighbor();
neighbor.Index = index;
neighbor.Distance = dist;
neis[neiPos] = neighbor;
return Math.Min(nneis + 1, maxNeis);
}
