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