140 lines
3.1 KiB
C++
140 lines
3.1 KiB
C++
#ifndef OBSTACLETREE_H
|
|
#define OBSTACLETREE_H
|
|
|
|
#include "../../../geo/Sphere3.h"
|
|
#include "Obstacle3.h"
|
|
#include <algorithm>
|
|
|
|
struct ObstacleNode {
|
|
bool isLeaf = true;
|
|
Sphere3 boundSphere;
|
|
std::vector<ObstacleNode*> sub;
|
|
ObstacleNode(bool isLeaf = false) : isLeaf(isLeaf) {;}
|
|
};
|
|
|
|
struct ObstacleLeaf : public ObstacleNode {
|
|
Obstacle3D obs;
|
|
ObstacleLeaf() : ObstacleNode(true) {;}
|
|
};
|
|
|
|
|
|
|
|
class ObstacleTree {
|
|
|
|
ObstacleNode root;
|
|
|
|
public:
|
|
|
|
/** append a new leaf */
|
|
void add(ObstacleLeaf* leaf) {
|
|
root.sub.push_back(leaf);
|
|
}
|
|
|
|
void optimize() {
|
|
while(true) {
|
|
bool did = concat();
|
|
if (!did) {break;}
|
|
}
|
|
}
|
|
|
|
std::vector<Obstacle3D*> getHits(const Ray3 ray) const {
|
|
std::vector<Obstacle3D*> obs;
|
|
getHits(ray, &root, obs);
|
|
return obs;
|
|
}
|
|
|
|
void getHits(const Ray3 ray, const ObstacleNode* node, std::vector<Obstacle3D*>& hits) const {
|
|
for (const ObstacleNode* sub : node->sub) {
|
|
if (sub->boundSphere.intersects(ray)) {
|
|
if (sub->isLeaf) {
|
|
ObstacleLeaf* leaf = (ObstacleLeaf*)(sub);
|
|
hits.push_back(&leaf->obs);
|
|
} else {
|
|
if (sub->boundSphere.intersects(ray)) {getHits(ray, sub, hits);}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
bool concat() {
|
|
|
|
bool concated = false;
|
|
|
|
// first, sort all elements by radius (smallest first)
|
|
auto compRadius = [] (const ObstacleNode* l1, const ObstacleNode* l2) {
|
|
return l1->boundSphere.radius < l2->boundSphere.radius;
|
|
};
|
|
std::sort(root.sub.begin(), root.sub.end(), compRadius);
|
|
|
|
ObstacleNode newRoot;
|
|
|
|
// combine nearby elements
|
|
//for (size_t i = 0; i < root.sub.size(); ++i) {
|
|
while(true) {
|
|
|
|
// get [and remove] the next element
|
|
ObstacleLeaf* l0 = (ObstacleLeaf*) root.sub[0];
|
|
root.sub.erase(root.sub.begin()+0);
|
|
|
|
// find another element that yields minimal increase in volume
|
|
auto compNear = [l0] (const ObstacleNode* l1, const ObstacleNode* l2) {
|
|
const float d1 = Sphere3::join(l0->boundSphere, l1->boundSphere).radius;
|
|
const float d2 = Sphere3::join(l0->boundSphere, l2->boundSphere).radius;
|
|
return d1 < d2;
|
|
};
|
|
auto it = std::min_element(root.sub.begin(), root.sub.end(), compNear);
|
|
ObstacleNode* l1 = *it;
|
|
|
|
float increment = Sphere3::join(l0->boundSphere, l1->boundSphere).radius / l0->boundSphere.radius;
|
|
|
|
const bool combine = (root.sub.size() > 1) && (it != root.sub.end()) && (increment < 1.75);
|
|
|
|
if (combine) {
|
|
|
|
// combine both into a new node
|
|
ObstacleNode* node = new ObstacleNode();
|
|
node->sub.push_back(l0);
|
|
node->sub.push_back(*it);
|
|
node->boundSphere = Sphere3::join(l0->boundSphere, (*it)->boundSphere);
|
|
root.sub.erase(it);
|
|
newRoot.sub.push_back(node);
|
|
|
|
concated = true;
|
|
|
|
} else {
|
|
|
|
ObstacleNode* node = new ObstacleNode();
|
|
node->sub.push_back(l0);
|
|
node->boundSphere = l0->boundSphere;
|
|
newRoot.sub.push_back(node);
|
|
|
|
}
|
|
|
|
// done?
|
|
if (root.sub.size() == 1) {
|
|
|
|
ObstacleNode* node = new ObstacleNode();
|
|
node->sub.push_back(root.sub.front());
|
|
node->boundSphere = root.sub.front()->boundSphere;
|
|
newRoot.sub.push_back(node);
|
|
break;
|
|
|
|
} else if (root.sub.size() == 0) {
|
|
break;
|
|
}
|
|
|
|
//--i;
|
|
|
|
}
|
|
|
|
root = newRoot;
|
|
|
|
return concated;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // OBSTACLETREE_H
|