/// <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); }