/** * <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_); } }