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