| static private leftOf ( Vector2 a, Vector2 b, Vector2 c ) : float | ||
| a | Vector2 | The first point on the line. |
| b | Vector2 | The second point on the line. |
| c | Vector2 | The point to which the signed distance is to be * calculated. |
| return | float | |
void queryObstacleTreeRecursive(Agent agent, float rangeSq, ObstacleTreeNode node)
{
if (node == null)
{
return;
}
else
{
Obstacle obstacle1 = node.obstacle;
Obstacle obstacle2 = obstacle1.nextObstacle;
float agentLeftOfLine = RVOMath.leftOf(obstacle1.point_, obstacle2.point_, agent.position_);
queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node.left : node.right));
float distSqLine = RVOMath.sqr(agentLeftOfLine) / RVOMath.absSq(obstacle2.point_ - obstacle1.point_);
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.insertObstacleNeighbor(node.obstacle, rangeSq);
}
/* Try other side of line. */
queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node.right : node.left));
}
}
}
/**
* <summary>Recursive method for computing the obstacle neighbors of the
* specified agent.</summary>
*
* <param name="agent">The agent for which obstacle neighbors are to be
* computed.</param>
* <param name="rangeSq">The squared range around the agent.</param>
* <param name="node">The current obstacle k-D node.</param>
*/
private void queryObstacleTreeRecursive(Agent agent, KInt rangeSq, ObstacleTreeNode node)
{
if (node != null)
{
Obstacle obstacle1 = node.obstacle_;
Obstacle obstacle2 = obstacle1.next_;
KInt agentLeftOfLine = RVOMath.leftOf(obstacle1.point_, obstacle2.point_, agent.position_);
queryObstacleTreeRecursive(agent, rangeSq, agentLeftOfLine >= 0 ? node.left_ : node.right_);
if (RVOMath.absSq(obstacle2.point_ - obstacle1.point_) == 0)
{
return;
}
KInt distSqLine = RVOMath.sqr(agentLeftOfLine) / RVOMath.absSq(obstacle2.point_ - obstacle1.point_);
if (distSqLine < rangeSq)
{
if (agentLeftOfLine < 0)
{
/*
* Try obstacle at this node only if agent is on right side of
* obstacle (and can see obstacle).
*/
agent.insertObstacleNeighbor(node.obstacle_, rangeSq);
}
/* Try other side of line. */
queryObstacleTreeRecursive(agent, rangeSq, agentLeftOfLine >= 0 ? node.right_ : node.left_);
}
}
}
/**
* <summary>Adds a new obstacle to the simulation.</summary>
*
* <returns>The number of the first vertex of the obstacle, or -1 when
* the number of vertices is less than two.</returns>
*
* <param name="vertices">List of the vertices of the polygonal obstacle
* in counterclockwise order.</param>
*
* <remarks>To add a "negative" obstacle, e.g. a bounding polygon around
* the environment, the vertices should be listed in clockwise order.
* </remarks>
*/
public int addObstacle(IList <Vector3> vertices3, float height)
{
List <Vector2> vertices = new List <Vector2>();
List <float> yPos = new List <float>();
foreach (Vector3 avertex3 in vertices3)
{
vertices.Add(new Vector2(avertex3.x, avertex3.z));
yPos.Add(avertex3.y);
}
if (vertices.Count < 2)
{
return(-1);
}
int obstacleNo = obstacles_.Count;
for (int i = 0; i < vertices.Count; ++i)
{
Obstacle obstacle = new Obstacle();
obstacle.point_ = vertices[i];
obstacle.curHeight_ = yPos[i];
if (i != 0)
{
obstacle.previous_ = obstacles_[obstacles_.Count - 1];
obstacle.previous_.next_ = obstacle;
}
if (i == vertices.Count - 1)
{
obstacle.next_ = obstacles_[obstacleNo];
obstacle.next_.previous_ = obstacle;
}
obstacle.direction_ = RVOMath.normalize(vertices[(i == vertices.Count - 1 ? 0 : i + 1)] - vertices[i]);
if (vertices.Count == 2)
{
obstacle.convex_ = true;
}
else
{
obstacle.convex_ = (RVOMath.leftOf(vertices[(i == 0 ? vertices.Count - 1 : i - 1)], vertices[i], vertices[(i == vertices.Count - 1 ? 0 : i + 1)]) >= 0.0f);
}
obstacle.ObsHeight_ = height;
obstacle.id_ = obstacles_.Count;
obstacles_.Add(obstacle);
}
RefreshList();
return(obstacleNo);
}
/**
* <summary>Adds a new obstacle to the simulation.</summary>
*
* <returns>The number of the first vertex of the obstacle, or -1 when
* the number of vertices is less than two.</returns>
*
* <param name="vertices">List of the vertices of the polygonal obstacle
* in counterclockwise order.</param>
*
* <remarks>To add a "negative" obstacle, e.g. a bounding polygon around
* the environment, the vertices should be listed in clockwise order.
* </remarks>
*/
public Obstacle addObstacle(IList <Vector2> vertices, bool _isMapBound)
{
if (vertices.Count < 2)
{
return(null);
}
int obstacleNo = obstacles_.Count;
Obstacle result = null;
for (int i = 0; i < vertices.Count; ++i)
{
Obstacle obstacle = new Obstacle();
obstacle.isMapBound = _isMapBound;
if (result == null)
{
result = obstacle;
}
obstacle.point_ = vertices[i];
if (i != 0)
{
obstacle.previous_ = obstacles_[obstacles_.Count - 1];
obstacle.previous_.next_ = obstacle;
}
if (i == vertices.Count - 1)
{
obstacle.next_ = obstacles_[obstacleNo];
obstacle.next_.previous_ = obstacle;
}
obstacle.direction_ = RVOMath.normalize(vertices[(i == vertices.Count - 1 ? 0 : i + 1)] - vertices[i]);
if (vertices.Count == 2)
{
obstacle.convex_ = true;
}
else
{
obstacle.convex_ = (RVOMath.leftOf(vertices[(i == 0 ? vertices.Count - 1 : i - 1)], vertices[i], vertices[(i == vertices.Count - 1 ? 0 : i + 1)]) >= 0.0);
}
obstacles_.Add(obstacle);
}
return(result);
}
/**
* <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, 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 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(Vec2 q1, Vec2 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 q12 = RVOMath.absSq(obstacle2.point_ - obstacle1.point_);
if (q12 == Fix64.Zero)
{
return(true);
}
Fix64 invLengthI = Fix64.One / RVOMath.absSq(obstacle2.point_ - obstacle1.point_);
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 sqp12 = RVOMath.absSq(q2 - q1);
if (sqp12 == Fix64.Zero)
{
return(true);
}
Fix64 invLengthQ = Fix64.One / sqp12;
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>Adds a new obstacle to the simulation.</summary>
*
* <returns>The number of the first vertex of the obstacle, or -1 when
* the number of vertices is less than two.</returns>
*
* <param name="vertices">List of the vertices of the polygonal obstacle
* in counterclockwise order.</param>
*
* <remarks>To add a "negative" obstacle, e.g. a bounding polygon around
* the environment, the vertices should be listed in clockwise order.
* </remarks>
*/
public int addObstacle(IList <Vector2> vertices)
{
if (vertices.Count < 2)
{
return(-1);
}
int obstacleNo = obstacles_.Count;
for (int i = 0; i < vertices.Count; ++i)
{
Obstacle obstacle = new Obstacle();
obstacle.point_ = vertices[i];
if (i != 0)
{
obstacle.previous_ = obstacles_[obstacles_.Count - 1];
obstacle.previous_.next_ = obstacle;
}
if (i == vertices.Count - 1)
{
obstacle.next_ = obstacles_[obstacleNo];
obstacle.next_.previous_ = obstacle;
}
obstacle.direction_ = RVOMath.normalize(vertices[(i == vertices.Count - 1 ? 0 : i + 1)] - vertices[i]);
if (vertices.Count == 2)
{
obstacle.convex_ = true;
}
else
{
obstacle.convex_ = RVOMath.leftOf(vertices[(i == 0 ? vertices.Count - 1 : i - 1)],
vertices[i],
vertices[(i == vertices.Count - 1 ? 0 : i + 1)]) >= 0.0f;
}
obstacle.id_ = obstacles_.Count;
obstacles_.Add(obstacle);
}
return(obstacleNo);
}
public int addObstacle(IList <Vector2> vertices)
{
if (vertices.Count < 2)
{
return(RVO_ERROR);
}
int obstacleNo = obstacles_.Count;
for (int i = 0; i < vertices.Count; ++i)
{
Obstacle obstacle = new Obstacle();
obstacle.point_ = vertices[i];
if (i > 0)
{
obstacle.previous_ = obstacles_[obstacleNo - 1];
obstacle.previous_.next_ = obstacle;
}
if (i == vertices.Count - 1)
{
obstacle.next_ = obstacles_[obstacleNo - 1];
obstacle.next_.previous_ = obstacle;
}
obstacle.direction_ = Vector2.normalize(vertices[(i == vertices.Count - 1 ? 0 : i + 1)] - vertices[i]);
if (vertices.Count == 2)
{
obstacle.convex_ = true;
}
else
{
obstacle.convex_ = (RVOMath.leftOf(vertices[(i == 0 ? vertices.Count - 1 : i - 1)], vertices[(i == vertices.Count - 1 ? 0 : i + 1)], vertices[i]) >= 0);
} // TO DO - Fix leftOf(dunno what is do)
obstacle.id_ = obstacles_.Count;
obstacles_.Add(obstacle);
obstacleNo = obstacles_.Count;
}
return(obstacleNo);
}
/**
* <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_;
KInt j1LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ1.point_);
KInt j2LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ2.point_);
if (j1LeftOfI >= 0 && j2LeftOfI >= 0)
{
++leftSize;
}
else if (j1LeftOfI <= 0 && j2LeftOfI <= 0)
{
++rightSize;
}
else
{
++leftSize;
++rightSize;
}
if (!Less(Math.Max(leftSize, rightSize), Math.Min(leftSize, rightSize), Math.Max(minLeft, minRight), Math.Min(minLeft, minRight)))
{
break;
}
}
if (Less(Math.Max(leftSize, rightSize), Math.Min(leftSize, rightSize), 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_;
KInt j1LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ1.point_);
KInt j2LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ2.point_);
if (j1LeftOfI >= 0 && j2LeftOfI >= 0)
{
leftObstacles[leftCounter++] = obstacles[j];
}
else if (j1LeftOfI <= 0 && j2LeftOfI <= 0)
{
rightObstacles[rightCounter++] = obstacles[j];
}
else
{
/* Split obstacle j. */
//KInt t = RVOMath.det(obstacleI2.point_ - obstacleI1.point_, obstacleJ1.point_ - obstacleI1.point_) / RVOMath.det(obstacleI2.point_ - obstacleI1.point_, obstacleJ1.point_ - obstacleJ2.point_);
KInt2 splitPoint = obstacleJ1.point_ + RVOMath.det(obstacleI2.point_ - obstacleI1.point_, obstacleJ1.point_ - obstacleI1.point_) * (obstacleJ2.point_ - obstacleJ1.point_) / RVOMath.det(obstacleI2.point_ - obstacleI1.point_, obstacleJ1.point_ - obstacleJ2.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)
{
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);
}
}
ObstacleTreeNode buildObstacleTreeRecursive(IList <Obstacle> obstacles)
{
if (obstacles.Count == 0)
{
return(null);
}
else
{
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.nextObstacle;
/* Compute optimal split node. */
for (int j = 0; j < obstacles.Count; ++j)
{
if (i == j)
{
continue;
}
Obstacle obstacleJ1 = obstacles[j];
Obstacle obstacleJ2 = obstacleJ1.nextObstacle;
float j1LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ1.point_);
float 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 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.nextObstacle;
for (int j = 0; j < obstacles.Count; ++j)
{
if (i == j)
{
continue;
}
Obstacle obstacleJ1 = obstacles[j];
Obstacle obstacleJ2 = obstacleJ1.nextObstacle;
float j1LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ1.point_);
float 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. */
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.prevObstacle = obstacleJ1;
newObstacle.nextObstacle = obstacleJ2;
newObstacle.isConvex_ = true;
newObstacle.unitDir_ = obstacleJ1.unitDir_;
newObstacle.id_ = Simulator.Instance.obstacles_.Count;
Simulator.Instance.obstacles_.Add(newObstacle);
obstacleJ1.nextObstacle = newObstacle;
obstacleJ2.prevObstacle = 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);
}
}
}
/**
* <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)
{
//遍历多边形obstacle的每一条边
//以这条边拆分二维空间
Obstacle obstacleI1 = obstacles[i];
Obstacle obstacleI2 = obstacleI1.next_;
//统计在这条边左边和右边的所有其他边的数据
//以此来选择最优拆分边
int leftSize = 0;
int rightSize = 0;
/* 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.RVO_EPSILON && j2LeftOfI >= -RVOMath.RVO_EPSILON)
{
++leftSize;
}
else if (j1LeftOfI <= RVOMath.RVO_EPSILON && j2LeftOfI <= RVOMath.RVO_EPSILON)
{
++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.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. */
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);
}
}