void ObstacleKDTree::queryTreeRecursive( ProximityQuery *filter, Vector2 pt, float& rangeSq, const ObstacleTreeNode* node) const {
if (node == 0) {
return;
} else {
const Obstacle* const obstacle1 = node->_obstacle;
const Vector2 P0 = obstacle1->getP0();
const Vector2 P1 = obstacle1->getP1();
const float agentLeftOfLine = leftOf( P0, P1, pt);
queryTreeRecursive(filter, pt, rangeSq, (agentLeftOfLine >= 0.0f ? node->_left : node->_right));
const float distSqLine = sqr(agentLeftOfLine) / absSq(P1 - P0);
if (distSqLine < rangeSq) {
if ( obstacle1->_doubleSided || agentLeftOfLine < 0.0f) {
/*
* Try obstacle at this node only if agent is on right side of
* obstacle (and can see obstacle).
*/
float distSq = distSqPointLineSegment(node->_obstacle->getP0(), node->_obstacle->getP1(), pt);
filter->filterObstacle(node->_obstacle, distSq);
rangeSq = filter->getMaxObstacleRange();
}
/* Try other side of line. */
queryTreeRecursive(filter, pt, rangeSq, (agentLeftOfLine >= 0.0f ? node->_right : node->_left));
}
}
}
void Agent::insertObstacleNeighbor(const Obstacle* obstacle, float rangeSq)
{
const Obstacle* const nextObstacle = obstacle->nextObstacle;
const float distSq = distSqPointLineSegment(obstacle->point_, nextObstacle->point_, position_);
if (distSq < rangeSq) {
obstacleNeighbors_.push_back(std::make_pair(distSq,obstacle));
size_t i = obstacleNeighbors_.size() - 1;
while (i != 0 && distSq < obstacleNeighbors_[i-1].first) {
obstacleNeighbors_[i] = obstacleNeighbors_[i-1];
--i;
}
obstacleNeighbors_[i] = std::make_pair(distSq, obstacle);
}
}