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, Fix64 rangeSq, ObstacleTreeNode node) { if (node != null) { Obstacle obstacle1 = node.obstacle_; Obstacle obstacle2 = obstacle1.next_; Fix64 agentLeftOfLine = RVOMath.leftOf(obstacle1.point_, obstacle2.point_, agent.Position); queryObstacleTreeRecursive(agent, rangeSq, agentLeftOfLine >= Fix64.Zero ? node.left_ : node.right_); Fix64 distSqLine = RVOMath.sqr(agentLeftOfLine) / (obstacle2.point_ - obstacle1.point_).LengthSquared(); if (distSqLine < rangeSq) { if (agentLeftOfLine < Fix64.Zero) { /* * 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 >= Fix64.Zero ? node.right_ : node.left_); } } }
private void QueryObstacleTreeRecursive(Agent agent, float rangeSq, ObstacleTreeNode node) { if (node == null) { return; } ObstacleVertex ob1 = node.obstacle; ObstacleVertex ob2 = ob1.next; float agentLeftOfLine = Polygon.TriangleArea(ob1.position, ob2.position, agent.position); QueryObstacleTreeRecursive(agent, rangeSq, agentLeftOfLine >= 0.0f ? node.left : node.right); Vector3 dir2D = ob1.position - ob2.position; dir2D.y = 0; //Isn't this 4 times too large since TriangleArea is actually 2*triangle area float distSqLine = Sqr(agentLeftOfLine) / dir2D.sqrMagnitude; 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.InsertObstacleNeighbour(node.obstacle, rangeSq); } 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 static void QueryObstacleTreeRecursive(Agent agent, float rangeSq, ObstacleTreeNode node) { if (node != null) { Obstacle obstacle1 = node.Obstacle; Obstacle obstacle2 = obstacle1.Next; 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); } } }
public void BuildAgentTree () { //Dummy to avoid compiler warnings obstacleRoot = new ObstacleTreeNode(); obstacleRoot.right = obstacleRoot; obstacleRoot.left = obstacleRoot; obstacleRoot = null; List<Agent> ag = simulator.GetAgents (); if (agents == null || agents.Count != ag.Count || rebuildAgents) { rebuildAgents = false; if (agents == null) agents = new List<Agent>(ag.Count); else agents.Clear (); agents.AddRange (simulator.GetAgents()); } if (agentTree.Length != agents.Count*2) { agentTree = new AgentTreeNode[agents.Count*2]; for (int i=0;i<agentTree.Length;i++) agentTree[i] = new AgentTreeNode(); } if (agents.Count != 0) { BuildAgentTreeRecursive (0,agents.Count, 0); } }
/** * <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_); } } }
private int countDepth(ObstacleTreeNode node) { if (node == null) { return(0); } return(1 + countDepth(node.left) + countDepth(node.right)); }
/** * <summary>Builds an obstacle k-D tree.</summary> */ internal void buildObstacleTree() { IList <Obstacle> obstacles = new List <Obstacle>(Simulator.Instance.obstacles_.Count); for (int i = 0; i < Simulator.Instance.obstacles_.Count; ++i) { obstacles.Add(Simulator.Instance.obstacles_[i]); } obstacleTree_ = buildObstacleTreeRecursive(obstacles); }
private static void RecycleOTN(ObstacleTreeNode node) { if (node == null) { return; } OTNPool.Push(node); node.obstacle = null; RecycleOTN(node.left); RecycleOTN(node.right); }
/** * <summary>Builds an obstacle k-D tree.</summary> */ internal void BuildObstacleTree() { _obstacleTree = new ObstacleTreeNode(); IList <Obstacle> obstacles = new List <Obstacle>(Simulator.Instance.Obstacles.Count); for (int i = 0; i < Simulator.Instance.Obstacles.Count; ++i) { obstacles.Add(Simulator.Instance.Obstacles[i]); } _obstacleTree = BuildObstacleTreeRecursive(obstacles); }
/** * <summary>Builds an obstacle k-D tree.</summary> */ internal void buildObstacleTree() { obstacleTree_ = new ObstacleTreeNode(); IList <Obstacle> obstacles = new List <Obstacle>(simulator_.obstacles_.Count); for (int i = 0; i < simulator_.obstacles_.Count; ++i) { obstacles.Add(simulator_.obstacles_[i]); } obstacleTree_ = buildObstacleTreeRecursive(obstacles); }
/** * <summary>Builds an obstacle k-D tree.</summary> */ internal void buildObstacleTree(IList <Obstacle> Obstacles) { obstacleTree_ = new ObstacleTreeNode(); IList <Obstacle> obstacles = new List <Obstacle>(Obstacles.Count); for (int i = 0; i < Obstacles.Count; ++i) { obstacles.Add(Obstacles[i]); } obstacleTree_ = buildObstacleTreeRecursive(obstacles); }
/** * <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>Builds an obstacle k-D tree.</summary> */ public void buildObstacleTree(IList <Obstacle> obstaclesList) { CountObstacles = obstaclesList.Count; obstacleTree_ = new ObstacleTreeNode(); IList <Obstacle> obstacles = new List <Obstacle>(CountObstacles); for (int i = 0; i < CountObstacles; ++i) { obstacles.Add(obstaclesList[i]); } obstacleTree_ = buildObstacleTreeRecursive(obstacles); }
/** * <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_)); }
public void BuildObstacleTree() { List<ObstacleVertex> obstacles = Pathfinding.Util.ListPool<ObstacleVertex>.Claim (); List<ObstacleVertex> src = simulator.GetObstacles (); for (int i=0;i<src.Count;i++) { ObstacleVertex c = src[i]; do { obstacles.Add (c); c = c.next; } while (c != src[i]); } RecycleOTN (obstacleRoot); obstacleRoot = BuildObstacleTreeRecursive (obstacles); }
public void BuildObstacleTree() { List <ObstacleVertex> obstacles = Pathfinding.Util.ListPool <ObstacleVertex> .Claim(); List <ObstacleVertex> src = simulator.GetObstacles(); for (int i = 0; i < src.Count; i++) { ObstacleVertex c = src[i]; do { obstacles.Add(c); c = c.next; } while (c != src[i]); } RecycleOTN(obstacleRoot); obstacleRoot = BuildObstacleTreeRecursive(obstacles); }
private void queryObstacleTreeImpl(Vector3 pos, IList <KeyValuePair <float, Obstacle> > list, float rangeSq) { if (m_ObstacleTree == null) { return; } else { m_QueryStack.Push(m_ObstacleTree); while (m_QueryStack.Count > 0) { ObstacleTreeNode node = m_QueryStack.Pop(); if (null != node) { Obstacle obstacle1 = node.m_Obstacle; Obstacle obstacle2 = obstacle1.m_NextObstacle; float agentLeftOfLine = VectorMath.leftOf(obstacle1.m_Point, obstacle2.m_Point, pos); m_QueryStack.Push(agentLeftOfLine >= 0.0f ? node.m_Left : node.m_Right); float distSqLine = VectorMath.sqr(agentLeftOfLine) / VectorMath.absSq(obstacle2.m_Point - obstacle1.m_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). */ insertObstacleNeighbor(pos, list, node.m_Obstacle, rangeSq); } /* Try other side of line. */ m_QueryStack.Push(agentLeftOfLine >= 0.0f ? node.m_Right : node.m_Left); } } } } }
public void BuildAgentTree() { //Dummy to avoid compiler warnings obstacleRoot = new ObstacleTreeNode(); obstacleRoot.right = obstacleRoot; obstacleRoot.left = obstacleRoot; obstacleRoot = null; List <Agent> ag = simulator.GetAgents(); if (agents == null || agents.Count != ag.Count || rebuildAgents) { rebuildAgents = false; if (agents == null) { agents = new List <Agent>(ag.Count); } else { agents.Clear(); } agents.AddRange(simulator.GetAgents()); } if (agentTree.Length != agents.Count * 2) { agentTree = new AgentTreeNode[agents.Count * 2]; for (int i = 0; i < agentTree.Length; i++) { agentTree[i] = new AgentTreeNode(); } } if (agents.Count != 0) { BuildAgentTreeRecursive(0, agents.Count, 0); } }
private bool CheckVisibilityRecursive(Vector3 q1, Vector3 q2, float radius, ObstacleTreeNode node) { if (node == null) { return(true); } else { Obstacle obstacle1 = node.m_Obstacle; Obstacle obstacle2 = obstacle1.m_NextObstacle; float q1LeftOfI = VectorMath.leftOf(obstacle1.m_Point, obstacle2.m_Point, q1); float q2LeftOfI = VectorMath.leftOf(obstacle1.m_Point, obstacle2.m_Point, q2); float invLengthI = 1.0f / VectorMath.absSq(obstacle2.m_Point - obstacle1.m_Point); if (q1LeftOfI >= 0.0f && q2LeftOfI >= 0.0f) { return(CheckVisibilityRecursive(q1, q2, radius, node.m_Left) && ((VectorMath.sqr(q1LeftOfI) * invLengthI >= VectorMath.sqr(radius) && VectorMath.sqr(q2LeftOfI) * invLengthI >= VectorMath.sqr(radius)) || CheckVisibilityRecursive(q1, q2, radius, node.m_Right))); } else if (q1LeftOfI <= 0.0f && q2LeftOfI <= 0.0f) { return(CheckVisibilityRecursive(q1, q2, radius, node.m_Right) && ((VectorMath.sqr(q1LeftOfI) * invLengthI >= VectorMath.sqr(radius) && VectorMath.sqr(q2LeftOfI) * invLengthI >= VectorMath.sqr(radius)) || CheckVisibilityRecursive(q1, q2, radius, node.m_Left))); } else if (q1LeftOfI >= 0.0f && q2LeftOfI <= 0.0f) { /* One can see through obstacle from left to right. */ return(CheckVisibilityRecursive(q1, q2, radius, node.m_Left) && CheckVisibilityRecursive(q1, q2, radius, node.m_Right)); } else { float point1LeftOfQ = VectorMath.leftOf(q1, q2, obstacle1.m_Point); float point2LeftOfQ = VectorMath.leftOf(q1, q2, obstacle2.m_Point); float invLengthQ = 1.0f / VectorMath.absSq(q2 - q1); return(point1LeftOfQ * point2LeftOfQ >= 0.0f && VectorMath.sqr(point1LeftOfQ) * invLengthQ > VectorMath.sqr(radius) && VectorMath.sqr(point2LeftOfQ) * invLengthQ > VectorMath.sqr(radius) && CheckVisibilityRecursive(q1, q2, radius, node.m_Left) && CheckVisibilityRecursive(q1, q2, radius, node.m_Right)); } } }
/** * <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); } }
/** * <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_; 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; } }
private void queryObstacleTreeRecursiveNew(Agent agent, float rangeSq, ObstacleTreeNode node) { // IList<Obstacle> obstacles = Simulator.Instance.obstacles_; throw new NotImplementedException(); }
private ObstacleTreeNode BuildRecursive(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.m_NextObstacle; /* Compute optimal split node. */ for (int j = 0; j < obstacles.Count; ++j) { if (i == j) { continue; } Obstacle obstacleJ1 = obstacles[j]; Obstacle obstacleJ2 = obstacleJ1.m_NextObstacle; float j1LeftOfI = VectorMath.leftOf(obstacleI1.m_Point, obstacleI2.m_Point, obstacleJ1.m_Point); float j2LeftOfI = VectorMath.leftOf(obstacleI1.m_Point, obstacleI2.m_Point, obstacleJ2.m_Point); if (j1LeftOfI >= -VectorMath.EPSILON && j2LeftOfI >= -VectorMath.EPSILON) { ++leftSize; } else if (j1LeftOfI <= VectorMath.EPSILON && j2LeftOfI <= VectorMath.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.m_NextObstacle; for (int j = 0; j < obstacles.Count; ++j) { if (i == j) { continue; } Obstacle obstacleJ1 = obstacles[j]; Obstacle obstacleJ2 = obstacleJ1.m_NextObstacle; float j1LeftOfI = VectorMath.leftOf(obstacleI1.m_Point, obstacleI2.m_Point, obstacleJ1.m_Point); float j2LeftOfI = VectorMath.leftOf(obstacleI1.m_Point, obstacleI2.m_Point, obstacleJ2.m_Point); if (j1LeftOfI >= -VectorMath.EPSILON && j2LeftOfI >= -VectorMath.EPSILON) { leftObstacles[leftCounter++] = obstacles[j]; } else if (j1LeftOfI <= VectorMath.EPSILON && j2LeftOfI <= VectorMath.EPSILON) { rightObstacles[rightCounter++] = obstacles[j]; } else { /* Split obstacle j. */ float t = VectorMath.det(obstacleI2.m_Point - obstacleI1.m_Point, obstacleJ1.m_Point - obstacleI1.m_Point) / VectorMath.det(obstacleI2.m_Point - obstacleI1.m_Point, obstacleJ1.m_Point - obstacleJ2.m_Point); Vector3 splitpoint = obstacleJ1.m_Point + t * (obstacleJ2.m_Point - obstacleJ1.m_Point); Obstacle newObstacle = new Obstacle(); newObstacle.m_Point = splitpoint; newObstacle.m_PrevObstacle = obstacleJ1; newObstacle.m_NextObstacle = obstacleJ2; newObstacle.m_IsConvex = true; newObstacle.m_UnitDir = obstacleJ1.m_UnitDir; m_Obstacles.Add(newObstacle); obstacleJ1.m_NextObstacle = newObstacle; obstacleJ2.m_PrevObstacle = newObstacle; if (j1LeftOfI > 0.0f) { leftObstacles[leftCounter++] = obstacleJ1; rightObstacles[rightCounter++] = newObstacle; } else { rightObstacles[rightCounter++] = obstacleJ1; leftObstacles[leftCounter++] = newObstacle; } } } node.m_Obstacle = obstacleI1; node.m_Left = BuildRecursive(leftObstacles); node.m_Right = BuildRecursive(rightObstacles); return(node); } } }
/** * <summary>Builds an obstacle k-D tree.</summary> */ internal void buildObstacleTree() { obstacleTree_ = new ObstacleTreeNode(); IList<Obstacle> obstacles = new List<Obstacle>(Simulator.Instance.obstacles_.Count); for (int i = 0; i < Simulator.Instance.obstacles_.Count; ++i) { obstacles.Add(Simulator.Instance.obstacles_[i]); } obstacleTree_ = buildObstacleTreeRecursive(obstacles); }
/** * <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 static 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; float j1LeftOfI = RVOMath.LeftOf(obstacleI1.Point, obstacleI2.Point, obstacleJ1.Point); float j2LeftOfI = RVOMath.LeftOf(obstacleI1.Point, obstacleI2.Point, obstacleJ2.Point); if (j1LeftOfI >= -RVOMath.RvoEpsilon && j2LeftOfI >= -RVOMath.RvoEpsilon) { ++leftSize; } else if (j1LeftOfI <= RVOMath.RvoEpsilon && j2LeftOfI <= RVOMath.RvoEpsilon) { ++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.RvoEpsilon && j2LeftOfI >= -RVOMath.RvoEpsilon) { leftObstacles[leftCounter++] = obstacles[j]; } else if (j1LeftOfI <= RVOMath.RvoEpsilon && j2LeftOfI <= RVOMath.RvoEpsilon) { 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); } }
private void QueryObstacleTreeRecursive(Agent agent, float rangeSq, ObstacleTreeNode node) { }
/** * <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_); }
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); } } }
private void QueryObstacleTreeRecursive (Agent agent, float rangeSq, ObstacleTreeNode node) { }
private int countDepth (ObstacleTreeNode node) { if (node == null) return 0; return 1 + countDepth (node.left) + countDepth (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 static 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)); }
private static void RecycleOTN (ObstacleTreeNode node) { if (node == null) return; OTNPool.Push (node); node.obstacle = null; RecycleOTN (node.left); RecycleOTN (node.right); }
private ObstacleTreeNode BuildObstacleTreeRecursive(List <ObstacleVertex> obstacles) { if (obstacles.Count == 0) { Pathfinding.Util.ListPool <ObstacleVertex> .Release(obstacles); return(null); } ObstacleTreeNode node = GetOTN(); //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; ObstacleVertex obI1 = obstacles[i]; ObstacleVertex obI2 = obI1.next; for (int j = 0; j < obstacles.Count; j++) { if (i == j) { continue; } ObstacleVertex obJ1 = obstacles[j]; ObstacleVertex obJ2 = obJ1.next; float j1LeftOfI = Polygon.TriangleArea(obI1.position, obI2.position, obJ1.position); float j2LeftOfI = Polygon.TriangleArea(obI1.position, obI2.position, obJ2.position); if (j1LeftOfI >= -float.Epsilon && j2LeftOfI >= -float.Epsilon) { leftSize++; } else if (j1LeftOfI <= float.Epsilon && j2LeftOfI <= float.Epsilon) { rightSize++; } else { leftSize++; rightSize++; } int v1a = Math.Max(leftSize, rightSize); int v1b = Math.Min(leftSize, rightSize); int v2a = Math.Max(minLeft, minRight); int v2b = Math.Min(minLeft, minRight); // (v1a, v1b) >= (v2a, v2b) //!(lhs._a < rhs._a || !(rhs._a < lhs._a) && lhs._b < rhs._b); if (!(v1a < v2a || !(v2a < v1a) && v1b < v2b)) { break; } } int x1a = Math.Max(leftSize, rightSize); int x1b = Math.Min(leftSize, rightSize); int x2a = Math.Max(minLeft, minRight); int x2b = Math.Min(minLeft, minRight); if (x1a < x2a || !(x2a < x1a) && x1b < x2b) { minLeft = leftSize; minRight = rightSize; optimalSplit = i; } } //Split node { List <ObstacleVertex> leftObstacles = Pathfinding.Util.ListPool <ObstacleVertex> .Claim(minLeft); //new List<ObstacleVertex>(minLeft); List <ObstacleVertex> rightObstacles = Pathfinding.Util.ListPool <ObstacleVertex> .Claim(minRight); //new List<ObstacleVertex>(minRight); //int leftCounter = 0; //int rightCounter = 0; int i = optimalSplit; ObstacleVertex obI1 = obstacles[i]; ObstacleVertex obI2 = obI1.next; for (int j = 0; j < obstacles.Count; j++) { if (optimalSplit == j) { continue; } ObstacleVertex obJ1 = obstacles[j]; ObstacleVertex obJ2 = obJ1.next; float j1LeftOfI = Polygon.TriangleArea(obI1.position, obI2.position, obJ1.position); float j2LeftOfI = Polygon.TriangleArea(obI1.position, obI2.position, obJ2.position); if (j1LeftOfI >= -float.Epsilon && j2LeftOfI >= -float.Epsilon) { leftObstacles.Add(obJ1); } else if (j1LeftOfI <= float.Epsilon && j2LeftOfI <= float.Epsilon) { rightObstacles.Add(obJ1); } else { //Split Obstacle float t = Polygon.IntersectionFactor(obJ1.position, obJ2.position, obI1.position, obI2.position); Vector3 splitPoint = obJ1.position + (obJ2.position - obJ1.position) * t; ObstacleVertex obst = new ObstacleVertex(); obst.position = splitPoint; obJ1.next = obst; obst.prev = obJ1; obst.next = obJ2; obJ2.prev = obst; obst.dir = obJ1.dir; //Mark as split point so that it can be identified //and removed if an update of this obstacle should be done obst.split = true; //New vertex is added at split, angle will be 180 degrees obst.convex = true; #if ASTARDEBUG Debug.DrawRay(splitPoint, Vector3.up, Color.cyan); #endif if (j1LeftOfI > 0.0f) { leftObstacles.Add(obJ1); rightObstacles.Add(obst); } else { rightObstacles.Add(obJ1); leftObstacles.Add(obst); } } } Pathfinding.Util.ListPool <ObstacleVertex> .Release(obstacles); node.obstacle = obI1; node.left = BuildObstacleTreeRecursive(leftObstacles); node.right = BuildObstacleTreeRecursive(rightObstacles); return(node); } }
/** * <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, float rangeSq, ObstacleTreeNode node) { if (node != null) { Obstacle obstacle1 = node.obstacle_; Obstacle obstacle2 = obstacle1.next_; 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 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); } }
private void QueryObstacleTreeRecursive (Agent agent, float rangeSq, ObstacleTreeNode node) { if (node == null) return; ObstacleVertex ob1 = node.obstacle; ObstacleVertex ob2 = ob1.next; float agentLeftOfLine = Polygon.TriangleArea (ob1.position,ob2.position,agent.position); QueryObstacleTreeRecursive (agent, rangeSq, agentLeftOfLine >= 0.0f ? node.left : node.right); Vector3 dir2D = ob1.position-ob2.position; dir2D.y = 0; //Isn't this 4 times too large since TriangleArea is actually 2*triangle area float distSqLine = Sqr(agentLeftOfLine) / dir2D.sqrMagnitude; 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.InsertObstacleNeighbour (node.obstacle, rangeSq); } QueryObstacleTreeRecursive (agent, rangeSq, (agentLeftOfLine >= 0.0f ? node.right : node.left)); } }
private static void SetObstacleRightTreeNode(AbstractParams p) { ObstacleTreeNode node = p.ReadObject() as ObstacleTreeNode; node.right_ = p.ReadObject() as ObstacleTreeNode; }
private void queryObstacleTreeRecursiveNew(Agent agent, float rangeSq, ObstacleTreeNode node) { IList <Obstacle> obstacles = Simulator.Instance.obstacles_; }