/**
* <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 Fix64 rangeSq, int node)
{
if (agentTree_[node].end_ - agentTree_[node].begin_ <= MAX_LEAF_SIZE)
{
for (int i = agentTree_[node].begin_; i < agentTree_[node].end_; ++i)
{
agent.insertAgentNeighbor(agents_[i], ref rangeSq);
}
}
else
{
Fix64 distSqLeft = RVOMath.sqr(RVOMath.Max(Fix64.Zero, agentTree_[agentTree_[node].left_].minX_ - agent.Position.X)) + RVOMath.sqr(RVOMath.Max(Fix64.Zero, agent.Position.X - agentTree_[agentTree_[node].left_].maxX_)) + RVOMath.sqr(RVOMath.Max(Fix64.Zero, agentTree_[agentTree_[node].left_].minY_ - agent.Position.Y)) + RVOMath.sqr(RVOMath.Max(Fix64.Zero, agent.Position.Y - agentTree_[agentTree_[node].left_].maxY_));
Fix64 distSqRight = RVOMath.sqr(RVOMath.Max(Fix64.Zero, agentTree_[agentTree_[node].right_].minX_ - agent.Position.X)) + RVOMath.sqr(RVOMath.Max(Fix64.Zero, agent.Position.X - agentTree_[agentTree_[node].right_].maxX_)) + RVOMath.sqr(RVOMath.Max(Fix64.Zero, agentTree_[agentTree_[node].right_].minY_ - agent.Position.Y)) + RVOMath.sqr(RVOMath.Max(Fix64.Zero, 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>Solves a one-dimensional linear program on a specified line
* subject to linear constraints defined by lines and a circular
* constraint.</summary>
*
* <returns>True if successful.</returns>
*
* <param name="lines">Lines defining the linear constraints.</param>
* <param name="lineNo">The specified line constraint.</param>
* <param name="radius">The radius of the circular constraint.</param>
* <param name="optVelocity">The optimization velocity.</param>
* <param name="directionOpt">True if the direction should be optimized.
* </param>
* <param name="result">A reference to the result of the linear program.
* </param>
*/
private bool linearProgram1(IList <Line> lines, int lineNo, Fix64 radius, Vector2 optVelocity, bool directionOpt, ref Vector2 result)
{
Fix64 dotProduct = Vector2.Dot(lines[lineNo].point, lines[lineNo].direction);
Fix64 discriminant = RVOMath.sqr(dotProduct) + RVOMath.sqr(radius) - lines[lineNo].point.LengthSquared();
if (discriminant < Fix64.Zero)
{
/* Max speed circle fully invalidates line lineNo. */
return(false);
}
Fix64 sqrtDiscriminant = Fix64.Sqrt(discriminant);
Fix64 tLeft = -dotProduct - sqrtDiscriminant;
Fix64 tRight = -dotProduct + sqrtDiscriminant;
for (int i = 0; i < lineNo; ++i)
{
Fix64 denominator = RVOMath.det(lines[lineNo].direction, lines[i].direction);
Fix64 numerator = RVOMath.det(lines[i].direction, lines[lineNo].point - lines[i].point);
if (Fix64.Abs(denominator) <= RVOMath.RVO_EPSILON)
{
/* Lines lineNo and i are (almost) parallel. */
if (numerator < Fix64.Zero)
{
return(false);
}
continue;
}
Fix64 t = numerator / denominator;
if (denominator >= Fix64.Zero)
{
/* Line i bounds line lineNo on the right. */
tRight = RVOMath.Min(tRight, t);
}
else
{
/* Line i bounds line lineNo on the left. */
tLeft = RVOMath.Max(tLeft, t);
}
if (tLeft > tRight)
{
return(false);
}
}
if (directionOpt)
{
/* Optimize direction. */
if (Vector2.Dot(optVelocity, lines[lineNo].direction) > Fix64.Zero)
{
/* Take right extreme. */
result = lines[lineNo].point + tRight * lines[lineNo].direction;
}
else
{
/* Take left extreme. */
result = lines[lineNo].point + tLeft * lines[lineNo].direction;
}
}
else
{
/* Optimize closest point. */
Fix64 t = Vector2.Dot(lines[lineNo].direction, optVelocity - lines[lineNo].point);
if (t < tLeft)
{
result = lines[lineNo].point + tLeft * lines[lineNo].direction;
}
else if (t > tRight)
{
result = lines[lineNo].point + tRight * lines[lineNo].direction;
}
else
{
result = lines[lineNo].point + t * lines[lineNo].direction;
}
}
return(true);
}
/**
* <summary>Recursive method for building an agent k-D tree.</summary>
*
* <param name="begin">The beginning agent k-D tree node node index.
* </param>
* <param name="end">The ending agent k-D tree node index.</param>
* <param name="node">The current agent k-D tree node index.</param>
*/
private void buildAgentTreeRecursive(int begin, int end, int node)
{
agentTree_[node].begin_ = begin;
agentTree_[node].end_ = end;
agentTree_[node].minX_ = agentTree_[node].maxX_ = agents_[begin].Position.X;
agentTree_[node].minY_ = agentTree_[node].maxY_ = agents_[begin].Position.Y;
for (int i = begin + 1; i < end; ++i)
{
agentTree_[node].maxX_ = RVOMath.Max(agentTree_[node].maxX_, agents_[i].Position.X);
agentTree_[node].minX_ = RVOMath.Min(agentTree_[node].minX_, agents_[i].Position.X);
agentTree_[node].maxY_ = RVOMath.Max(agentTree_[node].maxY_, agents_[i].Position.Y);
agentTree_[node].minY_ = RVOMath.Min(agentTree_[node].minY_, agents_[i].Position.Y);
}
if (end - begin > MAX_LEAF_SIZE)
{
/* No leaf node. */
bool isVertical = agentTree_[node].maxX_ - agentTree_[node].minX_ > agentTree_[node].maxY_ - agentTree_[node].minY_;
Fix64 splitValue = 0.5m * (isVertical ? agentTree_[node].maxX_ + agentTree_[node].minX_ : agentTree_[node].maxY_ + agentTree_[node].minY_);
int left = begin;
int right = end;
while (left < right)
{
while (left < right && (isVertical ? agents_[left].Position.X : agents_[left].Position.Y) < splitValue)
{
++left;
}
while (right > left && (isVertical ? agents_[right - 1].Position.X : agents_[right - 1].Position.Y) >= splitValue)
{
--right;
}
if (left < right)
{
Agent tempAgent = agents_[left];
agents_[left] = agents_[right - 1];
agents_[right - 1] = tempAgent;
++left;
--right;
}
}
int leftSize = left - begin;
if (leftSize == 0)
{
++leftSize;
++left;
++right;
}
agentTree_[node].left_ = node + 1;
agentTree_[node].right_ = node + 2 * leftSize;
buildAgentTreeRecursive(begin, left, agentTree_[node].left_);
buildAgentTreeRecursive(left, end, agentTree_[node].right_);
}
}
