bool KdTree::queryVisibilityRecursive(const Vector2& q1, const Vector2& q2, float radius, const ObstacleTreeNode* node) const
{
if (node == 0) {
return true;
} else {
const Obstacle* const obstacle1 = node->obstacle;
const Obstacle* const obstacle2 = obstacle1->nextObstacle;
const float q1LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q1);
const float q2LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q2);
const float invLengthI = 1.0f / absSq(obstacle2->point_ - obstacle1->point_);
if (q1LeftOfI >= 0.0f && q2LeftOfI >= 0.0f) {
return queryVisibilityRecursive(q1, q2, radius, node->left) && ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node->right));
} else if (q1LeftOfI <= 0.0f && q2LeftOfI <= 0.0f) {
return queryVisibilityRecursive(q1, q2, radius, node->right) && ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node->left));
} else 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);
} else {
const float point1LeftOfQ = leftOf(q1, q2, obstacle1->point_);
const float point2LeftOfQ = leftOf(q1, q2, obstacle2->point_);
const float invLengthQ = 1.0f / absSq(q2 - q1);
return (point1LeftOfQ * point2LeftOfQ >= 0.0f && sqr(point1LeftOfQ) * invLengthQ > sqr(radius) && sqr(point2LeftOfQ) * invLengthQ > sqr(radius) && queryVisibilityRecursive(q1, q2, radius, node->left) && queryVisibilityRecursive(q1, q2, radius, node->right));
}
}
}
bool KdTree::queryVisibility(const Vector2& q1, const Vector2& q2, float radius) const
{
return queryVisibilityRecursive(q1, q2, radius, obstacleTree_);
}
bool ObstacleKDTree::queryVisibility(const Vector2& q1, const Vector2& q2, float radius) const {
return queryVisibilityRecursive(q1, q2, radius, _tree);
}
