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. |
리턴 | 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); } }