KdTree::ObstacleTreeNode* KdTree::buildObstacleTreeRecursive(const std::vector<Obstacle*>& obstacles)
{
if (obstacles.empty()) {
return 0;
} else {
ObstacleTreeNode* const node = new ObstacleTreeNode;
size_t optimalSplit = 0;
size_t minLeft = obstacles.size();
size_t minRight = obstacles.size();
for (size_t i = 0; i < obstacles.size(); ++i) {
size_t leftSize = 0;
size_t rightSize = 0;
const Obstacle* const obstacleI1 = obstacles[i];
const Obstacle* const obstacleI2 = obstacleI1->nextObstacle;
/* Compute optimal split node. */
for (size_t j = 0; j < obstacles.size(); ++j) {
if (i == j) {
continue;
}
const Obstacle* const obstacleJ1 = obstacles[j];
const Obstacle* const obstacleJ2 = obstacleJ1->nextObstacle;
const float j1LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_);
const float j2LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_);
if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) {
++leftSize;
} else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) {
++rightSize;
} else {
++leftSize;
++rightSize;
}
if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) >= std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) {
break;
}
}
if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) < std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) {
minLeft = leftSize;
minRight = rightSize;
optimalSplit = i;
}
}
/* Build split node. */
std::vector<Obstacle*> leftObstacles(minLeft);
std::vector<Obstacle*> rightObstacles(minRight);
size_t leftCounter = 0;
size_t rightCounter = 0;
const size_t i = optimalSplit;
const Obstacle* const obstacleI1 = obstacles[i];
const Obstacle* const obstacleI2 = obstacleI1->nextObstacle;
for (size_t j = 0; j < obstacles.size(); ++j) {
if (i == j) {
continue;
}
Obstacle* const obstacleJ1 = obstacles[j];
Obstacle* const obstacleJ2 = obstacleJ1->nextObstacle;
const float j1LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_);
const float j2LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_);
if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) {
leftObstacles[leftCounter++] = obstacles[j];
} else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) {
rightObstacles[rightCounter++] = obstacles[j];
} else {
/* Split obstacle j. */
const float t = det(obstacleI2->point_ - obstacleI1->point_, obstacleJ1->point_ - obstacleI1->point_) / det(obstacleI2->point_ - obstacleI1->point_, obstacleJ1->point_ - obstacleJ2->point_);
const Vector2 splitpoint = obstacleJ1->point_ + t * (obstacleJ2->point_ - obstacleJ1->point_);
Obstacle* const newObstacle = new Obstacle();
newObstacle->point_ = splitpoint;
newObstacle->prevObstacle = obstacleJ1;
newObstacle->nextObstacle = obstacleJ2;
newObstacle->isConvex_ = true;
newObstacle->unitDir_ = obstacleJ1->unitDir_;
newObstacle->id_ = sim_->obstacles_.size();
sim_->obstacles_.push_back(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;
}
}
ObstacleTreeNode* ObstacleKDTree::buildTreeRecursive( const std::vector<Obstacle*>& obstacles) {
if ( obstacles.empty() ) {
return 0x0;
} else {
ObstacleTreeNode* const node = new ObstacleTreeNode;
size_t optimalSplit = 0;
size_t minLeft = obstacles.size();
size_t minRight = obstacles.size();
for (size_t i = 0; i < obstacles.size(); ++i) {
size_t leftSize = 0;
size_t rightSize = 0;
const Obstacle* const obstacleI = obstacles[i];
const Vector2 I0 = obstacleI->getP0();
const Vector2 I1 = obstacleI->getP1();
/* Compute optimal split node. */
for (size_t j = 0; j < obstacles.size(); ++j) {
if (i == j) {
continue;
}
const Obstacle* const obstacleJ = obstacles[j];
const Vector2 J0 = obstacleJ->getP0();
const Vector2 J1 = obstacleJ->getP1();
const float j1LeftOfI = leftOf( I0, I1, J0 );
const float j2LeftOfI = leftOf( I0, I1, J1 );
if (j1LeftOfI >= -EPS && j2LeftOfI >= -EPS) {
++leftSize;
} else if (j1LeftOfI <= EPS && j2LeftOfI <= EPS ) {
++rightSize;
} else {
++leftSize;
++rightSize;
}
if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) >= std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) {
break;
}
}
if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) < std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) {
minLeft = leftSize;
minRight = rightSize;
optimalSplit = i;
}
}
/* Build split node. */
std::vector<Obstacle*> leftObstacles(minLeft);
std::vector<Obstacle*> rightObstacles(minRight);
size_t leftCounter = 0;
size_t rightCounter = 0;
const size_t i = optimalSplit;
const Obstacle* const obstacleI = obstacles[i];
const Vector2 I0 = obstacleI->getP0();
const Vector2 I1 = obstacleI->getP1();
for (size_t j = 0; j < obstacles.size(); ++j) {
if (i == j) {
continue;
}
Obstacle* const obstacleJ = obstacles[j];
const Vector2 J0 = obstacleJ->getP0();
const Vector2 J1 = obstacleJ->getP1();
const float j1LeftOfI = leftOf( I0, I1, J0 );
const float j2LeftOfI = leftOf( I0, I1, J1 );
if (j1LeftOfI >= -EPS && j2LeftOfI >= -EPS ) {
leftObstacles[leftCounter++] = obstacles[j];
} else if (j1LeftOfI <= EPS && j2LeftOfI <= EPS ) {
rightObstacles[rightCounter++] = obstacles[j];
} else {
/* Split obstacle j. */
const float t = det( I1 - I0, J0 - I0 ) / det( I1 -I0, J0 - J1 );
const Vector2 splitpoint = J0 + t * ( J1 - J0 );
Obstacle* const newObstacle = new Obstacle();
newObstacle->_point = splitpoint;
newObstacle->_prevObstacle = obstacleJ;
newObstacle->_nextObstacle = obstacleJ->_nextObstacle;
if ( newObstacle->_nextObstacle ) {
obstacleJ->_nextObstacle = newObstacle;
}
newObstacle->_isConvex = true;
newObstacle->_unitDir = obstacleJ->_unitDir;
newObstacle->_length = abs( J1 - newObstacle->_point );
newObstacle->_id = _obstacles.size();
newObstacle->_class = obstacleJ->_class;
_obstacles.push_back(newObstacle);
obstacleJ->_nextObstacle = newObstacle;
obstacleJ->_length = abs( J0 - newObstacle->_point );
if (j1LeftOfI > 0.0f) {
leftObstacles[leftCounter++] = obstacleJ;
rightObstacles[rightCounter++] = newObstacle;
} else {
rightObstacles[rightCounter++] = obstacleJ;
leftObstacles[leftCounter++] = newObstacle;
}
}
}
node->_obstacle = obstacleI;
node->_left = buildTreeRecursive(leftObstacles);
node->_right = buildTreeRecursive(rightObstacles);
return node;
}
}
