|
public InsertObstacleNeighbour ( |
||
| ob1 | ||
| rangeSq | float | |
| return | void | |
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));
}
}
public void GetStaticAndDynamicObstacleNeighbours(Agent agent, float rangeSq)
{
for (int i = 0; i < obstacles.Count; ++i)
{
if (this.obstacles[i].isActive)
{
// this section is a copy of RVOKDTree.QueryObstacleTreeRecursive() ---------------------------------
ObstacleVertex ob1 = obstacles[i];
float agentLeftOfLine = VectorMath.SignedTriangleAreaTimes2XZ(ob1.position, ob1.next.position, agent.position);
Vector3 dir2D = ob1.position - ob1.next.position;
dir2D.y = 0;
float distSqLine = (agentLeftOfLine * agentLeftOfLine) / dir2D.sqrMagnitude; //Isn't this 4 times too large since TriangleArea is actually 2*triangle area
if (distSqLine < rangeSq)
{
float distSqr = VectorMath.SqrDistancePointSegment(ob1.position, ob1.next.position, agent.position);
if (distSqr < rangeSq)
{
agent.InsertObstacleNeighbour(obstacles[i], rangeSq);
}
}
}
}
for (int dynamic = 0; dynamic < _dynamicObstacles.Count; ++dynamic)
{
if (_dynamicObstacles[dynamic].isActive)
{
// this section is a copy of RVOKDTree.QueryObstacleTreeRecursive() ---------------------------------
ObstacleVertex ob1 = _dynamicObstacles[dynamic];
float agentLeftOfLine = VectorMath.SignedTriangleAreaTimes2XZ(ob1.position, ob1.next.position, agent.position);
Vector3 dir2D = ob1.position - ob1.next.position;
dir2D.y = 0;
float distSqLine = (agentLeftOfLine * agentLeftOfLine) / dir2D.sqrMagnitude; // Isn't this 4 times too large since TriangleArea is actually 2*triangle area
if (distSqLine < rangeSq)
{
agent.InsertObstacleNeighbour(_dynamicObstacles[dynamic], rangeSq);
}
}
}
}
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));
}
}
