void KdTree::buildAgentTreeRecursive(size_t begin, size_t end, size_t node)
{
agentTree_[node].begin = begin;
agentTree_[node].end = end;
agentTree_[node].minX = agentTree_[node].maxX = agents_[begin]->position_.x();
agentTree_[node].minY = agentTree_[node].maxY = agents_[begin]->position_.y();
for (size_t i = begin + 1; i < end; ++i) {
agentTree_[node].maxX = std::max(agentTree_[node].maxX, agents_[i]->position_.x());
agentTree_[node].minX = std::min(agentTree_[node].minX, agents_[i]->position_.x());
agentTree_[node].maxY = std::max(agentTree_[node].maxY, agents_[i]->position_.y());
agentTree_[node].minY = std::min(agentTree_[node].minY, agents_[i]->position_.y());
}
if (end - begin > MAX_LEAF_SIZE) {
/* No leaf node. */
const bool isVertical = (agentTree_[node].maxX - agentTree_[node].minX > agentTree_[node].maxY - agentTree_[node].minY);
const float splitValue = (isVertical ? 0.5f * (agentTree_[node].maxX + agentTree_[node].minX) : 0.5f * (agentTree_[node].maxY + agentTree_[node].minY));
size_t left = begin;
size_t right = end;
while (left < right) {
while (left < right && (isVertical ? agents_[left]->position_.x() : agents_[left]->position_.y()) < splitValue) {
++left;
}
while (right > left && (isVertical ? agents_[right-1]->position_.x() : agents_[right-1]->position_.y()) >= splitValue) {
--right;
}
if (left < right) {
std::swap(agents_[left], agents_[right-1]);
++left;
--right;
}
}
size_t leftSize = left - begin;
if (leftSize == 0) {
++leftSize;
++left;
++right;
}
agentTree_[node].left = node + 1;
agentTree_[node].right = node + 1 + (2 * leftSize - 1);
buildAgentTreeRecursive(begin, left, agentTree_[node].left);
buildAgentTreeRecursive(left, end, agentTree_[node].right);
}
}
void KdTree::buildAgentTree()
{
agents_ = sim_->agents_;
if (!agents_.empty()) {
agentTree_.resize(2 * agents_.size() - 1);
buildAgentTreeRecursive(0, agents_.size(), 0);
}
}
void KdTree::buildAgentTree()
{
if (agents_.size() < sim_->agents_.size()) {
for (size_t i = agents_.size(); i < sim_->agents_.size(); ++i) {
agents_.push_back(sim_->agents_[i]);
}
agentTree_.resize(2 * agents_.size() - 1);//TODO
}
if (!agents_.empty()) {
buildAgentTreeRecursive(0, agents_.size(), 0);
}
}
void KdTree::buildAgentTree()
{
agents_.resize(0);
for (size_t i = 0; i < sim_->agents_.size(); ++i) {
if (sim_->agents_[i] != NULL) {
agents_.push_back(sim_->agents_[i]);
}
}
if (!agents_.empty()) {
if (agents_.size() * 2 > agentTree_.size() + 1) {
agentTree_.resize(2 * agents_.size() - 1);
}
buildAgentTreeRecursive(0, agents_.size(), 0);
}
}
void KdTree::buildAgentTreeRecursive(size_t begin, size_t end, size_t node)
{
agentTree_[node].begin = begin;
agentTree_[node].end = end;
agentTree_[node].minCoord = agents_[begin]->position_;
agentTree_[node].maxCoord = agents_[begin]->position_;
for (size_t i = begin + 1; i < end; ++i) {
agentTree_[node].maxCoord[0] = std::max(agentTree_[node].maxCoord[0], agents_[i]->position_.x());
agentTree_[node].minCoord[0] = std::min(agentTree_[node].minCoord[0], agents_[i]->position_.x());
agentTree_[node].maxCoord[1] = std::max(agentTree_[node].maxCoord[1], agents_[i]->position_.y());
agentTree_[node].minCoord[1] = std::min(agentTree_[node].minCoord[1], agents_[i]->position_.y());
agentTree_[node].maxCoord[2] = std::max(agentTree_[node].maxCoord[2], agents_[i]->position_.z());
agentTree_[node].minCoord[2] = std::min(agentTree_[node].minCoord[2], agents_[i]->position_.z());
}
if (end - begin > RVO_MAX_LEAF_SIZE) {
/* No leaf node. */
size_t coord;
if (agentTree_[node].maxCoord[0] - agentTree_[node].minCoord[0] > agentTree_[node].maxCoord[1] - agentTree_[node].minCoord[1] && agentTree_[node].maxCoord[0] - agentTree_[node].minCoord[0] > agentTree_[node].maxCoord[2] - agentTree_[node].minCoord[2]) {
coord = 0;
}
else if (agentTree_[node].maxCoord[1] - agentTree_[node].minCoord[1] > agentTree_[node].maxCoord[2] - agentTree_[node].minCoord[2]) {
coord = 1;
}
else {
coord = 2;
}
const float splitValue = 0.5f * (agentTree_[node].maxCoord[coord] + agentTree_[node].minCoord[coord]);
size_t left = begin;
size_t right = end;
while (left < right) {
while (left < right && agents_[left]->position_[coord] < splitValue) {
++left;
}
while (right > left && agents_[right - 1]->position_[coord] >= splitValue) {
--right;
}
if (left < right) {
std::swap(agents_[left], agents_[right - 1]);
++left;
--right;
}
}
size_t leftSize = left - begin;
if (leftSize == 0) {
++leftSize;
++left;
++right;
}
agentTree_[node].left = node + 1;
agentTree_[node].right = node + 1 + (2 * leftSize - 1);
buildAgentTreeRecursive(begin, left, agentTree_[node].left);
buildAgentTreeRecursive(left, end, agentTree_[node].right);
}
}
