public float InsertAgentNeighbour (Agent agent, float rangeSq) {
return 0;
}
private void QueryAgentTreeRecursive (Agent agent, ref float rangeSq, int node) {
}
private void QueryObstacleTreeRecursive (Agent agent, float rangeSq, ObstacleTreeNode node) {
}
public void GetAgentNeighbours (Agent agent, float rangeSq) {
QueryAgentTreeRecursive (agent, ref rangeSq, 0);
}
public void GetObstacleNeighbours (Agent agent, float rangeSq) {
QueryObstacleTreeRecursive (agent, rangeSq, obstacleRoot);
}
private void QueryAgentTreeRecursive (Agent agent, ref float rangeSq, int node) {
if (agentTree[node].end - agentTree[node].start <= MAX_LEAF_SIZE) {
for (int i = agentTree[node].start; i < agentTree[node].end; i++) {
rangeSq = agent.InsertAgentNeighbour (agents[i], rangeSq);
}
} else {
//Calculate squared distance to the closest point inside the bounding boxes of the child nodes
float distSqLeft = Sqr (Math.Max (0.0f, agentTree[agentTree[node].left].xmin - agent.position.x))
+ Sqr (Math.Max (0.0f, agent.position.x - agentTree[agentTree[node].left].xmax))
+ Sqr (Math.Max (0.0f, agentTree[agentTree[node].left].ymin - agent.position.z))
+ Sqr (Math.Max (0.0f, agent.position.z - agentTree[agentTree[node].left].ymax));
float distSqRight =Sqr (Math.Max (0.0f, agentTree[agentTree[node].right].xmin - agent.position.x))
+ Sqr (Math.Max (0.0f, agent.position.x - agentTree[agentTree[node].right].xmax))
+ Sqr (Math.Max (0.0f, agentTree[agentTree[node].right].ymin - agent.position.z))
+ Sqr (Math.Max (0.0f, agent.position.z - agentTree[agentTree[node].right].ymax));
if (distSqLeft < distSqRight) {
if (distSqLeft < rangeSq) {
QueryAgentTreeRecursive (agent, ref rangeSq, agentTree[node].left);
if (distSqRight < rangeSq) {
QueryAgentTreeRecursive (agent, ref rangeSq, agentTree[node].right);
}
}
} else {
if (distSqRight < rangeSq) {
QueryAgentTreeRecursive (agent, ref rangeSq, agentTree[node].right);
if (distSqLeft < rangeSq) {
QueryAgentTreeRecursive (agent, ref rangeSq, agentTree[node].left);
}
}
}
}
}
private void QueryObstacleTreeRecursive (Agent agent, float rangeSq, ObstacleTreeNode node) {
if (node == null) return;
ObstacleVertex ob1 = node.obstacle;
ObstacleVertex ob2 = ob1.next;
float agentLeftOfLine = Polygon.TriangleArea (ob1.position,ob2.position,agent.position);
QueryObstacleTreeRecursive (agent, rangeSq, agentLeftOfLine >= 0.0f ? node.left : node.right);
Vector3 dir2D = ob1.position-ob2.position;
dir2D.y = 0;
//Isn't this 4 times too large since TriangleArea is actually 2*triangle area
float distSqLine = Sqr(agentLeftOfLine) / dir2D.sqrMagnitude;
if (distSqLine < rangeSq) {
if (agentLeftOfLine < 0.0f) {
/*
* Try obstacle at this node only if agent is on right side of
* obstacle (and can see obstacle).
*/
agent.InsertObstacleNeighbour (node.obstacle, rangeSq);
}
QueryObstacleTreeRecursive (agent, rangeSq, (agentLeftOfLine >= 0.0f ? node.right : node.left));
}
}
public float InsertAgentNeighbour (Agent agent, float rangeSq) {
if (this == agent) return rangeSq;
//2D Dist
float dist = Sqr(agent.position.x-position.x) + Sqr(agent.position.z - position.z);
if (dist < rangeSq) {
if (neighbours.Count < maxNeighbours) {
neighbours.Add (agent);
neighbourDists.Add (dist);
}
/** \bug This starts to compare with a lower agent if neighbours.Count >= maxNeighbours */
int i = neighbours.Count-1;
while ( i != 0 && dist < neighbourDists[i-1]) {
neighbours[i] = neighbours[i-1];
neighbourDists[i] = neighbourDists[i-1];
i--;
}
neighbours[i] = agent;
neighbourDists[i] = dist;
if (neighbours.Count == maxNeighbours) {
rangeSq = neighbourDists[neighbourDists.Count-1];
}
}
return rangeSq;
}
/** Add an agent at the specified position.
* You can use the returned interface to read several parameters such as position and velocity
* and set for example radius and desired velocity.
*
* \see RemoveAgent
*/
public IAgent AddAgent(Vector3 position)
{
Agent agent = new Agent (position);
//Don't interfere with ongoing calculations
if (Multithreading && doubleBuffering) for (int j=0;j<workers.Length;j++) workers[j].WaitOne();
agents.Add (agent);
kdTree.RebuildAgents ();
agent.simulator = this;
return agent;
}
public void Add(Agent agent)
{
agent.next = linkedList;
linkedList = agent;
}
public void Query(Vector2 p, float speed, float timeHorizon, float agentRadius, Agent agent)
{
new QuadtreeQuery
{
p = p,
speed = speed,
timeHorizon = timeHorizon,
maxRadius = float.PositiveInfinity,
agentRadius = agentRadius,
agent = agent,
nodes = nodes
}.QueryRec(0, bounds);
}
/// <summary>
/// Add a new agent to the tree.
/// Warning: Agents must not be added multiple times to the same tree
/// </summary>
public void Insert(Agent agent)
{
int i = 0;
Rect r = bounds;
Vector2 p = new Vector2(agent.position.x, agent.position.y);
agent.next = null;
maxRadius = System.Math.Max(agent.radius, maxRadius);
int depth = 0;
while (true)
{
depth++;
if (nodes[i].child00 == i)
{
// Leaf node. Break at depth 10 in case lots of agents ( > LeafSize ) are in the same spot
if (nodes[i].count < LeafSize || depth > 10)
{
nodes[i].Add(agent);
nodes[i].count++;
break;
}
else
{
// Split
nodes[i].child00 = GetNodeIndex();
nodes[i].Distribute(nodes, r);
}
}
// Note, no else
if (nodes[i].child00 != i)
{
// Not a leaf node
Vector2 c = r.center;
if (p.x > c.x)
{
if (p.y > c.y)
{
i = nodes[i].child00 + 3;
r = Rect.MinMaxRect(c.x, c.y, r.xMax, r.yMax);
}
else
{
i = nodes[i].child00 + 2;
r = Rect.MinMaxRect(c.x, r.yMin, r.xMax, c.y);
}
}
else
{
if (p.y > c.y)
{
i = nodes[i].child00 + 1;
r = Rect.MinMaxRect(r.xMin, c.y, c.x, r.yMax);
}
else
{
i = nodes[i].child00;
r = Rect.MinMaxRect(r.xMin, r.yMin, c.x, c.y);
}
}
}
}
}
