/**
* <summary>Computes the new velocity of this agent.</summary>
*/
internal void computeNewVelocity()
{
OrcaLines.Clear();
Fix64 invTimeHorizonObst = Fix64.One / timeHorizonObst_;
/* Create obstacle ORCA lines. */
for (int i = 0; i < ObstacleNeighbors.Count; ++i)
{
Obstacle obstacle1 = ObstacleNeighbors[i].Value;
Obstacle obstacle2 = obstacle1.next_;
Vector2 relativePosition1 = obstacle1.point_ - Position;
Vector2 relativePosition2 = obstacle2.point_ - Position;
/*
* Check if velocity obstacle of obstacle is already taken care
* of by previously constructed obstacle ORCA lines.
*/
bool alreadyCovered = false;
for (int j = 0; j < OrcaLines.Count; ++j)
{
if (RVOMath.det(invTimeHorizonObst * relativePosition1 - OrcaLines[j].point, OrcaLines[j].direction) - invTimeHorizonObst * radius_ >= -RVOMath.RVO_EPSILON && RVOMath.det(invTimeHorizonObst * relativePosition2 - OrcaLines[j].point, OrcaLines[j].direction) - invTimeHorizonObst * radius_ >= -RVOMath.RVO_EPSILON)
{
alreadyCovered = true;
break;
}
}
if (alreadyCovered)
{
continue;
}
/* Not yet covered. Check for collisions. */
Fix64 distSq1 = relativePosition1.LengthSquared();
Fix64 distSq2 = relativePosition2.LengthSquared();
Fix64 radiusSq = RVOMath.sqr(radius_);
Vector2 obstacleVector = obstacle2.point_ - obstacle1.point_;
Fix64 s = Vector2.Dot(-relativePosition1, obstacleVector) / obstacleVector.LengthSquared();
Fix64 distSqLine = (-relativePosition1 - s * obstacleVector).LengthSquared();
Line line;
if (s < Fix64.Zero && distSq1 <= radiusSq)
{
/* Collision with left vertex. Ignore if non-convex. */
if (obstacle1.convex_)
{
line.point = new Vector2(Fix64.Zero, Fix64.Zero);
line.direction = Vector2.Normalize(new Vector2(-relativePosition1.Y, relativePosition1.X));
OrcaLines.Add(line);
}
continue;
}
else if (s > Fix64.One && distSq2 <= radiusSq)
{
/*
* Collision with right vertex. Ignore if non-convex or if
* it will be taken care of by neighboring obstacle.
*/
if (obstacle2.convex_ && RVOMath.det(relativePosition2, obstacle2.direction_) >= Fix64.Zero)
{
line.point = new Vector2(Fix64.Zero, Fix64.Zero);
line.direction = Vector2.Normalize(new Vector2(-relativePosition2.Y, relativePosition2.X));
OrcaLines.Add(line);
}
continue;
}
else if (s >= Fix64.Zero && s < Fix64.One && distSqLine <= radiusSq)
{
/* Collision with obstacle segment. */
line.point = new Vector2(Fix64.Zero, Fix64.Zero);
line.direction = -obstacle1.direction_;
OrcaLines.Add(line);
continue;
}
/*
* No collision. Compute legs. When obliquely viewed, both legs
* can come from a single vertex. Legs extend cut-off line when
* non-convex vertex.
*/
Vector2 leftLegDirection, rightLegDirection;
if (s < Fix64.Zero && distSqLine <= radiusSq)
{
/*
* Obstacle viewed obliquely so that left vertex
* defines velocity obstacle.
*/
if (!obstacle1.convex_)
{
/* Ignore obstacle. */
continue;
}
obstacle2 = obstacle1;
Fix64 leg1 = Fix64.Sqrt(distSq1 - radiusSq);
leftLegDirection = new Vector2(relativePosition1.X * leg1 - relativePosition1.Y * radius_, relativePosition1.X * radius_ + relativePosition1.Y * leg1) / distSq1;
rightLegDirection = new Vector2(relativePosition1.X * leg1 + relativePosition1.Y * radius_, -relativePosition1.X * radius_ + relativePosition1.Y * leg1) / distSq1;
}
else if (s > Fix64.One && distSqLine <= radiusSq)
{
/*
* Obstacle viewed obliquely so that
* right vertex defines velocity obstacle.
*/
if (!obstacle2.convex_)
{
/* Ignore obstacle. */
continue;
}
obstacle1 = obstacle2;
Fix64 leg2 = Fix64.Sqrt(distSq2 - radiusSq);
leftLegDirection = new Vector2(relativePosition2.X * leg2 - relativePosition2.Y * radius_, relativePosition2.X * radius_ + relativePosition2.Y * leg2) / distSq2;
rightLegDirection = new Vector2(relativePosition2.X * leg2 + relativePosition2.Y * radius_, -relativePosition2.X * radius_ + relativePosition2.Y * leg2) / distSq2;
}
else
{
/* Usual situation. */
if (obstacle1.convex_)
{
Fix64 leg1 = Fix64.Sqrt(distSq1 - radiusSq);
leftLegDirection = new Vector2(relativePosition1.X * leg1 - relativePosition1.Y * radius_, relativePosition1.X * radius_ + relativePosition1.Y * leg1) / distSq1;
}
else
{
/* Left vertex non-convex; left leg extends cut-off line. */
leftLegDirection = -obstacle1.direction_;
}
if (obstacle2.convex_)
{
Fix64 leg2 = Fix64.Sqrt(distSq2 - radiusSq);
rightLegDirection = new Vector2(relativePosition2.X * leg2 + relativePosition2.Y * radius_, -relativePosition2.X * radius_ + relativePosition2.Y * leg2) / distSq2;
}
else
{
/* Right vertex non-convex; right leg extends cut-off line. */
rightLegDirection = obstacle1.direction_;
}
}
/*
* Legs can never point into neighboring edge when convex
* vertex, take cutoff-line of neighboring edge instead. If
* velocity projected on "foreign" leg, no constraint is added.
*/
Obstacle leftNeighbor = obstacle1.previous_;
bool isLeftLegForeign = false;
bool isRightLegForeign = false;
if (obstacle1.convex_ && RVOMath.det(leftLegDirection, -leftNeighbor.direction_) >= Fix64.Zero)
{
/* Left leg points into obstacle. */
leftLegDirection = -leftNeighbor.direction_;
isLeftLegForeign = true;
}
if (obstacle2.convex_ && RVOMath.det(rightLegDirection, obstacle2.direction_) <= Fix64.Zero)
{
/* Right leg points into obstacle. */
rightLegDirection = obstacle2.direction_;
isRightLegForeign = true;
}
/* Compute cut-off centers. */
Vector2 leftCutOff = invTimeHorizonObst * (obstacle1.point_ - Position);
Vector2 rightCutOff = invTimeHorizonObst * (obstacle2.point_ - Position);
Vector2 cutOffVector = rightCutOff - leftCutOff;
/* Project current velocity on velocity obstacle. */
/* Check if current velocity is projected on cutoff circles. */
Fix64 t = obstacle1 == obstacle2 ? 0.5m : Vector2.Dot((Velocity - leftCutOff), cutOffVector) / cutOffVector.LengthSquared();
Fix64 tLeft = Vector2.Dot((Velocity - leftCutOff), leftLegDirection);
Fix64 tRight = Vector2.Dot((Velocity - rightCutOff), rightLegDirection);
if ((t < Fix64.Zero && tLeft < Fix64.Zero) || (obstacle1 == obstacle2 && tLeft < Fix64.Zero && tRight < Fix64.Zero))
{
/* Project on left cut-off circle. */
Vector2 unitW = Vector2.Normalize(Velocity - leftCutOff);
line.direction = new Vector2(unitW.Y, -unitW.X);
line.point = leftCutOff + radius_ * invTimeHorizonObst * unitW;
OrcaLines.Add(line);
continue;
}
else if (t > Fix64.One && tRight < Fix64.Zero)
{
/* Project on right cut-off circle. */
Vector2 unitW = Vector2.Normalize(Velocity - rightCutOff);
line.direction = new Vector2(unitW.Y, -unitW.X);
line.point = rightCutOff + radius_ * invTimeHorizonObst * unitW;
OrcaLines.Add(line);
continue;
}
/*
* Project on left leg, right leg, or cut-off line, whichever is
* closest to velocity.
*/
Fix64 distSqCutoff = (t <Fix64.Zero || t> Fix64.One || obstacle1 == obstacle2) ? Fix64.MaxValue : (Velocity - (leftCutOff + t * cutOffVector)).LengthSquared();
Fix64 distSqLeft = tLeft < Fix64.Zero ? Fix64.MaxValue : (Velocity - (leftCutOff + tLeft * leftLegDirection)).LengthSquared();
Fix64 distSqRight = tRight < Fix64.Zero ? Fix64.MaxValue : (Velocity - (rightCutOff + tRight * rightLegDirection)).LengthSquared();
if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight)
{
/* Project on cut-off line. */
line.direction = -obstacle1.direction_;
line.point = leftCutOff + radius_ * invTimeHorizonObst * new Vector2(-line.direction.Y, line.direction.X);
OrcaLines.Add(line);
continue;
}
if (distSqLeft <= distSqRight)
{
/* Project on left leg. */
if (isLeftLegForeign)
{
continue;
}
line.direction = leftLegDirection;
line.point = leftCutOff + radius_ * invTimeHorizonObst * new Vector2(-line.direction.Y, line.direction.X);
OrcaLines.Add(line);
continue;
}
/* Project on right leg. */
if (isRightLegForeign)
{
continue;
}
line.direction = -rightLegDirection;
line.point = rightCutOff + radius_ * invTimeHorizonObst * new Vector2(-line.direction.Y, line.direction.X);
OrcaLines.Add(line);
}
int numObstLines = OrcaLines.Count;
Fix64 invTimeHorizon = Fix64.One / timeHorizon_;
/* Create agent ORCA lines. */
for (int i = 0; i < AgentNeighbors.Count; ++i)
{
Agent other = AgentNeighbors[i].Value;
Vector2 relativePosition = other.Position - Position;
Vector2 relativeVelocity = Velocity - other.Velocity;
Fix64 distSq = relativePosition.LengthSquared();
Fix64 combinedRadius = radius_ + other.radius_;
Fix64 combinedRadiusSq = RVOMath.sqr(combinedRadius);
Line line;
Vector2 u;
if (distSq > combinedRadiusSq)
{
/* No collision. */
Vector2 w = relativeVelocity - invTimeHorizon * relativePosition;
/* Vector from cutoff center to relative velocity. */
Fix64 wLengthSq = w.LengthSquared();
Fix64 dotProduct1 = Vector2.Dot(w, relativePosition);
if (dotProduct1 < Fix64.Zero && RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq)
{
/* Project on cut-off circle. */
Fix64 wLength = Fix64.Sqrt(wLengthSq);
Vector2 unitW = w / wLength;
line.direction = new Vector2(unitW.Y, -unitW.X);
u = (combinedRadius * invTimeHorizon - wLength) * unitW;
}
else
{
/* Project on legs. */
Fix64 leg = Fix64.Sqrt(distSq - combinedRadiusSq);
if (RVOMath.det(relativePosition, w) > Fix64.Zero)
{
/* Project on left leg. */
line.direction = new Vector2(relativePosition.X * leg - relativePosition.Y * combinedRadius, relativePosition.X * combinedRadius + relativePosition.Y * leg) / distSq;
}
else
{
/* Project on right leg. */
line.direction = -new Vector2(relativePosition.X * leg + relativePosition.Y * combinedRadius, -relativePosition.X * combinedRadius + relativePosition.Y * leg) / distSq;
}
Fix64 dotProduct2 = Vector2.Dot(relativeVelocity, line.direction);
u = dotProduct2 * line.direction - relativeVelocity;
}
}
else
{
/* Collision. Project on cut-off circle of time timeStep. */
Fix64 invTimeStep = Fix64.One / Simulator.Instance.timeStep_;
/* Vector from cutoff center to relative velocity. */
Vector2 w = relativeVelocity - invTimeStep * relativePosition;
Fix64 wLength = w.Length();
Vector2 unitW = w / wLength;
line.direction = new Vector2(unitW.Y, -unitW.X);
u = (combinedRadius * invTimeStep - wLength) * unitW;
}
line.point = Velocity + 0.5m * u;
OrcaLines.Add(line);
}
int lineFail = linearProgram2(OrcaLines, MaxSpeed, PrefVelocity, false, ref newVelocity_);
if (lineFail < OrcaLines.Count)
{
linearProgram3(OrcaLines, numObstLines, lineFail, MaxSpeed, ref newVelocity_);
}
}
/**
* <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 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 bool queryVisibilityRecursive(Vector2 q1, Vector2 q2, Fix64 radius, ObstacleTreeNode node)
{
if (node == null)
{
return(true);
}
Obstacle obstacle1 = node.obstacle_;
Obstacle obstacle2 = obstacle1.next_;
Fix64 q1LeftOfI = RVOMath.leftOf(obstacle1.point_, obstacle2.point_, q1);
Fix64 q2LeftOfI = RVOMath.leftOf(obstacle1.point_, obstacle2.point_, q2);
Fix64 invLengthI = Fix64.One / (obstacle2.point_ - obstacle1.point_).LengthSquared();
if (q1LeftOfI >= Fix64.Zero && q2LeftOfI >= Fix64.Zero)
{
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 <= Fix64.Zero && q2LeftOfI <= Fix64.Zero)
{
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 >= Fix64.Zero && q2LeftOfI <= Fix64.Zero)
{
/* One can see through obstacle from left to right. */
return(queryVisibilityRecursive(q1, q2, radius, node.left_) && queryVisibilityRecursive(q1, q2, radius, node.right_));
}
Fix64 point1LeftOfQ = RVOMath.leftOf(q1, q2, obstacle1.point_);
Fix64 point2LeftOfQ = RVOMath.leftOf(q1, q2, obstacle2.point_);
Fix64 invLengthQ = Fix64.One / (q2 - q1).LengthSquared();
return(point1LeftOfQ * point2LeftOfQ >= Fix64.Zero && 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 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>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 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_;
Fix64 j1LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ1.point_);
Fix64 j2LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ2.point_);
if (j1LeftOfI >= -RVOMath.RVO_EPSILON && j2LeftOfI >= -RVOMath.RVO_EPSILON)
{
++leftSize;
}
else if (j1LeftOfI <= RVOMath.RVO_EPSILON && j2LeftOfI <= RVOMath.RVO_EPSILON)
{
++rightSize;
}
else
{
++leftSize;
++rightSize;
}
if (new Fix64Pair(Math.Max(leftSize, rightSize), Math.Min(leftSize, rightSize)) >= new Fix64Pair(Math.Max(minLeft, minRight), Math.Min(minLeft, minRight)))
{
break;
}
}
if (new Fix64Pair(Math.Max(leftSize, rightSize), Math.Min(leftSize, rightSize)) < new Fix64Pair(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_;
Fix64 j1LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ1.point_);
Fix64 j2LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ2.point_);
if (j1LeftOfI >= -RVOMath.RVO_EPSILON && j2LeftOfI >= -RVOMath.RVO_EPSILON)
{
leftObstacles[leftCounter++] = obstacles[j];
}
else if (j1LeftOfI <= RVOMath.RVO_EPSILON && j2LeftOfI <= RVOMath.RVO_EPSILON)
{
rightObstacles[rightCounter++] = obstacles[j];
}
else
{
/* Split obstacle j. */
Fix64 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 > Fix64.Zero)
{
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);
}
}
/**
* <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_);
}
}
