/**
* <summary>Recursive method for computing the agent neighbors of the
* specified agent.</summary>
*
* <param name="agent">The agent for which agent neighbors are to be
* computed.</param>
* <param name="rangeSq">The squared range around the agent.</param>
* <param name="node">The current agent k-D tree node index.</param>
*/
private void QueryAgentTreeRecursive(Agent agent, ref float rangeSq, int node)
{
if (_agentTree[node].End - _agentTree[node].Begin <= MaxLeafSize)
{
for (int i = _agentTree[node].Begin; i < _agentTree[node].End; ++i)
{
agent.InsertAgentNeighbor(_agents[i], ref rangeSq);
}
}
else
{
float distSqLeft = RVOMath.Sqr(Math.Max(0.0f, _agentTree[_agentTree[node].Left].MinX - agent.Position.X)) + RVOMath.Sqr(Math.Max(0.0f, agent.Position.X - _agentTree[_agentTree[node].Left].MaxX)) + RVOMath.Sqr(Math.Max(0.0f, _agentTree[_agentTree[node].Left].MinY - agent.Position.Y)) + RVOMath.Sqr(Math.Max(0.0f, agent.Position.Y - _agentTree[_agentTree[node].Left].MaxY));
float distSqRight = RVOMath.Sqr(Math.Max(0.0f, _agentTree[_agentTree[node].Right].MinX - agent.Position.X)) + RVOMath.Sqr(Math.Max(0.0f, agent.Position.X - _agentTree[_agentTree[node].Right].MaxX)) + RVOMath.Sqr(Math.Max(0.0f, _agentTree[_agentTree[node].Right].MinY - agent.Position.Y)) + RVOMath.Sqr(Math.Max(0.0f, agent.Position.Y - _agentTree[_agentTree[node].Right].MaxY));
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);
}
}
}
}
}
/**
* <summary>Recursive method for querying the visibility between two
* points within a specified radius.</summary>
*
* <returns>True if q1 and q2 are mutually visible within the radius;
* false otherwise.</returns>
*
* <param name="q1">The first point between which visibility is to be
* tested.</param>
* <param name="q2">The second point between which visibility is to be
* tested.</param>
* <param name="radius">The radius within which visibility is to be
* tested.</param>
* <param name="node">The current obstacle k-D node.</param>
*/
private static bool QueryVisibilityRecursive(Vector2 q1, Vector2 q2, float radius, ObstacleTreeNode node)
{
if (node == null)
{
return(true);
}
Obstacle obstacle1 = node.Obstacle;
Obstacle obstacle2 = obstacle1.Next;
float q1LeftOfI = RVOMath.LeftOf(obstacle1.Point, obstacle2.Point, q1);
float q2LeftOfI = RVOMath.LeftOf(obstacle1.Point, obstacle2.Point, q2);
float invLengthI = 1.0f / RVOMath.AbsSq(obstacle2.Point - obstacle1.Point);
if (q1LeftOfI >= 0.0f && q2LeftOfI >= 0.0f)
{
return(QueryVisibilityRecursive(q1, q2, radius, node.Left) && ((RVOMath.Sqr(q1LeftOfI) * invLengthI >= RVOMath.Sqr(radius) && RVOMath.Sqr(q2LeftOfI) * invLengthI >= RVOMath.Sqr(radius)) || QueryVisibilityRecursive(q1, q2, radius, node.Right)));
}
if (q1LeftOfI <= 0.0f && q2LeftOfI <= 0.0f)
{
return(QueryVisibilityRecursive(q1, q2, radius, node.Right) && ((RVOMath.Sqr(q1LeftOfI) * invLengthI >= RVOMath.Sqr(radius) && RVOMath.Sqr(q2LeftOfI) * invLengthI >= RVOMath.Sqr(radius)) || QueryVisibilityRecursive(q1, q2, radius, node.Left)));
}
if (q1LeftOfI >= 0.0f && q2LeftOfI <= 0.0f)
{
/* One can see through obstacle from left to right. */
return(QueryVisibilityRecursive(q1, q2, radius, node.Left) && QueryVisibilityRecursive(q1, q2, radius, node.Right));
}
float point1LeftOfQ = RVOMath.LeftOf(q1, q2, obstacle1.Point);
float point2LeftOfQ = RVOMath.LeftOf(q1, q2, obstacle2.Point);
float invLengthQ = 1.0f / RVOMath.AbsSq(q2 - q1);
return(point1LeftOfQ * point2LeftOfQ >= 0.0f && RVOMath.Sqr(point1LeftOfQ) * invLengthQ > RVOMath.Sqr(radius) && RVOMath.Sqr(point2LeftOfQ) * invLengthQ > RVOMath.Sqr(radius) && QueryVisibilityRecursive(q1, q2, radius, node.Left) && QueryVisibilityRecursive(q1, q2, radius, node.Right));
}
/**
* <summary>Recursive method for building an obstacle k-D tree.
* </summary>
*
* <returns>An obstacle k-D tree node.</returns>
*
* <param name="obstacles">A list of obstacles.</param>
*/
private static ObstacleTreeNode BuildObstacleTreeRecursive(IList <Obstacle> obstacles)
{
if (obstacles.Count == 0)
{
return(null);
}
ObstacleTreeNode node = new ObstacleTreeNode();
int optimalSplit = 0;
int minLeft = obstacles.Count;
int minRight = obstacles.Count;
for (int i = 0; i < obstacles.Count; ++i)
{
int leftSize = 0;
int rightSize = 0;
Obstacle obstacleI1 = obstacles[i];
Obstacle obstacleI2 = obstacleI1.Next;
/* Compute optimal split node. */
for (int j = 0; j < obstacles.Count; ++j)
{
if (i == j)
{
continue;
}
Obstacle obstacleJ1 = obstacles[j];
Obstacle obstacleJ2 = obstacleJ1.Next;
float j1LeftOfI = RVOMath.LeftOf(obstacleI1.Point, obstacleI2.Point, obstacleJ1.Point);
float j2LeftOfI = RVOMath.LeftOf(obstacleI1.Point, obstacleI2.Point, obstacleJ2.Point);
if (j1LeftOfI >= -RVOMath.RvoEpsilon && j2LeftOfI >= -RVOMath.RvoEpsilon)
{
++leftSize;
}
else if (j1LeftOfI <= RVOMath.RvoEpsilon && j2LeftOfI <= RVOMath.RvoEpsilon)
{
++rightSize;
}
else
{
++leftSize;
++rightSize;
}
if (new FloatPair(Math.Max(leftSize, rightSize), Math.Min(leftSize, rightSize)) >= new FloatPair(Math.Max(minLeft, minRight), Math.Min(minLeft, minRight)))
{
break;
}
}
if (new FloatPair(Math.Max(leftSize, rightSize), Math.Min(leftSize, rightSize)) < new FloatPair(Math.Max(minLeft, minRight), Math.Min(minLeft, minRight)))
{
minLeft = leftSize;
minRight = rightSize;
optimalSplit = i;
}
}
{
/* Build split node. */
IList <Obstacle> leftObstacles = new List <Obstacle>(minLeft);
for (int n = 0; n < minLeft; ++n)
{
leftObstacles.Add(null);
}
IList <Obstacle> rightObstacles = new List <Obstacle>(minRight);
for (int n = 0; n < minRight; ++n)
{
rightObstacles.Add(null);
}
int leftCounter = 0;
int rightCounter = 0;
int i = optimalSplit;
Obstacle obstacleI1 = obstacles[i];
Obstacle obstacleI2 = obstacleI1.Next;
for (int j = 0; j < obstacles.Count; ++j)
{
if (i == j)
{
continue;
}
Obstacle obstacleJ1 = obstacles[j];
Obstacle obstacleJ2 = obstacleJ1.Next;
float j1LeftOfI = RVOMath.LeftOf(obstacleI1.Point, obstacleI2.Point, obstacleJ1.Point);
float j2LeftOfI = RVOMath.LeftOf(obstacleI1.Point, obstacleI2.Point, obstacleJ2.Point);
if (j1LeftOfI >= -RVOMath.RvoEpsilon && j2LeftOfI >= -RVOMath.RvoEpsilon)
{
leftObstacles[leftCounter++] = obstacles[j];
}
else if (j1LeftOfI <= RVOMath.RvoEpsilon && j2LeftOfI <= RVOMath.RvoEpsilon)
{
rightObstacles[rightCounter++] = obstacles[j];
}
else
{
/* Split obstacle j. */
float t = RVOMath.Det(obstacleI2.Point - obstacleI1.Point, obstacleJ1.Point - obstacleI1.Point) / RVOMath.Det(obstacleI2.Point - obstacleI1.Point, obstacleJ1.Point - obstacleJ2.Point);
Vector2 splitPoint = obstacleJ1.Point + t * (obstacleJ2.Point - obstacleJ1.Point);
Obstacle newObstacle = new Obstacle();
newObstacle.Point = splitPoint;
newObstacle.Previous = obstacleJ1;
newObstacle.Next = obstacleJ2;
newObstacle.Convex = true;
newObstacle.Direction = obstacleJ1.Direction;
newObstacle.Id = Simulator.Instance.Obstacles.Count;
Simulator.Instance.Obstacles.Add(newObstacle);
obstacleJ1.Next = newObstacle;
obstacleJ2.Previous = newObstacle;
if (j1LeftOfI > 0.0f)
{
leftObstacles[leftCounter++] = obstacleJ1;
rightObstacles[rightCounter++] = newObstacle;
}
else
{
rightObstacles[rightCounter++] = obstacleJ1;
leftObstacles[leftCounter++] = newObstacle;
}
}
}
node.Obstacle = obstacleI1;
node.Left = BuildObstacleTreeRecursive(leftObstacles);
node.Right = BuildObstacleTreeRecursive(rightObstacles);
return(node);
}
}
