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_;
}
