123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333 |
- #include <algorithm>
- #include <iostream>
- #include "common/utils/KDTree.h"
- KDTree::Triangle::Triangle(const Vector3& a, const Vector3& b, const Vector3& c) {
- v[0] = a;
- v[1] = b;
- v[2] = c;
- mid = (a + b + c) * (1.0f / 3.0f);
- }
- const Array<Vector3, 3>& KDTree::Triangle::data() const {
- return v;
- }
- const Vector3& KDTree::Triangle::operator[](int index) const {
- return v[index];
- }
- const Vector3& KDTree::Triangle::getMid() const {
- return mid;
- }
- KDTree::Node::Node() : splitDim(0), splitValue(0.0f), color(0xFFFFFF) {
- childs[0] = nullptr;
- childs[1] = nullptr;
- }
- KDTree::~KDTree() {
- clean(&root);
- }
- void KDTree::clean(Node* n) {
- if(n->childs[0] != nullptr) {
- clean(n->childs[0]);
- }
- if(n->childs[1] != nullptr) {
- clean(n->childs[1]);
- }
- delete n->childs[0];
- delete n->childs[1];
- }
- void KDTree::build(std::vector<KDTree::Triangle>& data) {
- if(data.size() == 0) {
- return;
- }
- min = data[0][0];
- max = data[0][0];
- for(const Triangle& t : data) {
- for(const Vector3& v : t.data()) {
- min.set(std::min(min[0], v[0]), std::min(min[1], v[1]), std::min(min[2], v[2]));
- max.set(std::max(max[0], v[0]), std::max(max[1], v[1]), std::max(max[2], v[2]));
- }
- }
- build(&root, data);
- }
- float KDTree::median(std::vector<KDTree::Triangle>& data, int dim) const {
- auto compare = [dim](const Triangle& a, const Triangle & b) {
- return a.getMid()[dim] < b.getMid()[dim];
- };
- size_t length = data.size();
- if((length & 1) == 0) {
- std::nth_element(data.begin(), data.begin() + (length / 2 - 1), data.end(), compare);
- float tmp = data[length / 2 - 1].getMid()[dim];
- std::nth_element(data.begin(), data.begin() + (length / 2), data.end(), compare);
- return (tmp + data[length / 2].getMid()[dim]) / 2;
- }
- std::nth_element(data.begin(), data.begin() + (length / 2), data.end(), compare);
- return data[length / 2].getMid()[dim];
- }
- void KDTree::build(Node* n, std::vector<KDTree::Triangle>& data) {
- if(data.size() == 0) {
- return;
- } else if(data.size() == 1) {
- n->data.push_back(data[0]);
- return;
- }
- // find min and max coordinates
- Vector3 min = data[0][0];
- Vector3 max = data[0][0];
- for(const Triangle& t : data) {
- for(const Vector3& v : t.data()) {
- min.set(std::min(min[0], v[0]), std::min(min[1], v[1]), std::min(min[2], v[2]));
- max.set(std::max(max[0], v[0]), std::max(max[1], v[1]), std::max(max[2], v[2]));
- }
- }
- // find biggest span and its dimension
- int splitDim = 0;
- float maxSpan = max[0] - min[0];
- for(int i = 1; i < 3; i++) {
- float span = max[i] - min[i];
- if(span > maxSpan) {
- splitDim = i;
- maxSpan = span;
- }
- }
- // assign data to node
- n->splitDim = splitDim;
- n->splitValue = median(data, splitDim);
- // storage for split data
- std::vector<KDTree::Triangle> lessEqualData;
- std::vector<KDTree::Triangle> greaterData;
- // actually split the data
- for(const Triangle& t : data) {
- // count points on each split side
- int lessEqualCounter = 0;
- int greaterCount = 0;
- for(const Vector3& v : t.data()) {
- if(v[n->splitDim] <= n->splitValue) {
- lessEqualCounter++;
- } else {
- greaterCount++;
- }
- }
- // put the data in the correct container
- if(lessEqualCounter == 3) {
- lessEqualData.push_back(t);
- } else if(greaterCount == 3) {
- greaterData.push_back(t);
- } else {
- n->data.push_back(t);
- }
- }
- if(lessEqualData.size() == 0 || greaterData.size() == 0) {
- for(KDTree::Triangle& t : lessEqualData) {
- n->data.push_back(t);
- }
- for(KDTree::Triangle& t : greaterData) {
- n->data.push_back(t);
- }
- return;
- }
- // recursive calls
- if(lessEqualData.size() > 0) {
- n->childs[0] = new Node();
- build(n->childs[0], lessEqualData);
- }
- if(greaterData.size() > 0) {
- n->childs[1] = new Node();
- build(n->childs[1], greaterData);
- }
- }
- void KDTree::fillLines(Lines& lines) const {
- lines.clear();
- lines.add(Vector3(min[0], min[1], min[2]), Vector3(max[0], min[1], min[2]), root.color);
- lines.add(Vector3(min[0], min[1], min[2]), Vector3(min[0], min[1], max[2]), root.color);
- lines.add(Vector3(max[0], min[1], min[2]), Vector3(max[0], min[1], max[2]), root.color);
- lines.add(Vector3(min[0], min[1], max[2]), Vector3(max[0], min[1], max[2]), root.color);
- lines.add(Vector3(min[0], min[1], min[2]), Vector3(min[0], max[1], min[2]), root.color);
- lines.add(Vector3(max[0], min[1], min[2]), Vector3(max[0], max[1], min[2]), root.color);
- lines.add(Vector3(min[0], min[1], max[2]), Vector3(min[0], max[1], max[2]), root.color);
- lines.add(Vector3(max[0], min[1], max[2]), Vector3(max[0], max[1], max[2]), root.color);
- lines.add(Vector3(min[0], max[1], min[2]), Vector3(max[0], max[1], min[2]), root.color);
- lines.add(Vector3(min[0], max[1], min[2]), Vector3(min[0], max[1], max[2]), root.color);
- lines.add(Vector3(max[0], max[1], min[2]), Vector3(max[0], max[1], max[2]), root.color);
- lines.add(Vector3(min[0], max[1], max[2]), Vector3(max[0], max[1], max[2]), root.color);
- fillLines(lines, &root, min, max);
- lines.build();
- }
- void KDTree::fillLines(Lines& lines, const Node* n, const Vector3& min, const Vector3& max) const {
- if(n->childs[0] == nullptr && n->childs[1] == nullptr) {
- return;
- }
- switch(n->splitDim) {
- case 0:
- lines.add(Vector3(n->splitValue, min[1], min[2]), Vector3(n->splitValue, max[1], min[2]), n->color);
- lines.add(Vector3(n->splitValue, max[1], min[2]), Vector3(n->splitValue, max[1], max[2]), n->color);
- lines.add(Vector3(n->splitValue, max[1], max[2]), Vector3(n->splitValue, min[1], max[2]), n->color);
- lines.add(Vector3(n->splitValue, min[1], max[2]), Vector3(n->splitValue, min[1], min[2]), n->color);
- if(n->childs[0] != nullptr) {
- fillLines(lines, n->childs[0], min, Vector3(n->splitValue, max[1], max[2]));
- }
- if(n->childs[1] != nullptr) {
- fillLines(lines, n->childs[1], Vector3(n->splitValue, min[1], min[2]), max);
- }
- break;
- case 1:
- lines.add(Vector3(min[0], n->splitValue, min[2]), Vector3(max[0], n->splitValue, min[2]), n->color);
- lines.add(Vector3(min[0], n->splitValue, min[2]), Vector3(min[0], n->splitValue, max[2]), n->color);
- lines.add(Vector3(max[0], n->splitValue, min[2]), Vector3(max[0], n->splitValue, max[2]), n->color);
- lines.add(Vector3(min[0], n->splitValue, max[2]), Vector3(max[0], n->splitValue, max[2]), n->color);
- if(n->childs[0] != nullptr) {
- fillLines(lines, n->childs[0], min, Vector3(max[0], n->splitValue, max[2]));
- }
- if(n->childs[1] != nullptr) {
- fillLines(lines, n->childs[1], Vector3(min[0], n->splitValue, min[2]), max);
- }
- break;
- case 2:
- lines.add(Vector3(min[0], min[1], n->splitValue), Vector3(min[0], max[1], n->splitValue), n->color);
- lines.add(Vector3(min[0], max[1], n->splitValue), Vector3(max[0], max[1], n->splitValue), n->color);
- lines.add(Vector3(max[0], max[1], n->splitValue), Vector3(max[0], min[1], n->splitValue), n->color);
- lines.add(Vector3(max[0], min[1], n->splitValue), Vector3(min[0], min[1], n->splitValue), n->color);
- if(n->childs[0] != nullptr) {
- fillLines(lines, n->childs[0], min, Vector3(max[0], max[1], n->splitValue));
- }
- if(n->childs[1] != nullptr) {
- fillLines(lines, n->childs[1], Vector3(min[0], min[1], n->splitValue), max);
- }
- break;
- }
- }
- void KDTree::cleanVisited(Node* n) {
- if(n->childs[0] != nullptr) {
- cleanVisited(n->childs[0]);
- }
- if(n->childs[1] != nullptr) {
- cleanVisited(n->childs[1]);
- }
- n->color = 0xFFFFFF;
- }
- bool KDTree::findIntersection(const Vector3& pos, const Vector3& direction) {
- minDistance = 9999999.0f;
- original = pos;
- cleanVisited(&root);
- float t = 0.0f;
- for(int i = 0; i < 3; i++) {
- if(direction[i] == 0.0f) {
- continue;
- }
- t = std::max(t, std::abs((min[i] - pos[i]) / direction[i]));
- t = std::max(t, std::abs((max[i] - pos[i]) / direction[i]));
- }
- return findIntersection(&root, pos, direction, t);
- }
- Vector3 KDTree::getIntersection() const {
- return intersection;
- }
- KDTree::Triangle KDTree::getIntersectedTriangle() const {
- return intersectionTriangle;
- }
- bool KDTree::findIntersection(Node* n, const Vector3& pos, const Vector3& direction, float tMax) {
- if(n == nullptr) {
- return false;
- }
- n->color = 0x00FFFF;
- int dim = n->splitDim;
- // lessEqual = 0
- // greater = 1
- bool r = false;
- bool first = pos[dim] > n->splitValue;
- if(direction[dim] == 0.0f) {
- r = findIntersection(n->childs[first], pos, direction, tMax);
- } else {
- float t = (n->splitValue - pos[dim]) / direction[dim];
- if(t >= 0.0f && t < tMax) {
- r = findIntersection(n->childs[first], pos, direction, t);
- r = findIntersection(n->childs[!first], pos + t * direction, direction, tMax - t) || r;
- } else {
- r = findIntersection(n->childs[first], pos, direction, tMax);
- }
- }
-
- for(KDTree::Triangle& tri : n->data) {
- float t = testIntersection(pos, direction, tri);
- if(t < 0.0f) {
- continue;
- }
- Vector3 i = pos + t * direction;
- float distance = static_cast<Vector3> (original - i).squareLength();
- if(distance > minDistance) {
- continue;
- }
- intersectionTriangle = tri;
- intersection = i;
- minDistance = distance;
- r = true;
- }
- return r;
- }
- float KDTree::orient(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d) const {
- return static_cast<Vector3> (a - d).dot(static_cast<Vector3> (b - d).cross(c - d));
- }
- float KDTree::testIntersection(const Vector3& pos, const Vector3& direction, const KDTree::Triangle& t) const {
- const float eps = 0.0001f;
- Vector3 h1 = t[1] - t[0];
- Vector3 h2 = t[2] - t[0];
- Vector3 abc = h1.cross(h2);
- if(abc.squareLength() < eps) {
- return -1.0f;
- }
- abc.normalize();
- float d = -abc.dot(t[0]);
- float check = abc.dot(direction);
- if(std::abs(check) < eps) {
- return -1.0f;
- }
- float factor = -(abc.dot(pos) + d) / check;
- Vector3 inter = pos + factor * direction;
- int sideA = 0;
- int sideB = 0;
- for(int i = 0; i < 3; i++) {
- const Vector3& a = t[i];
- const Vector3& b = t[(i + 1) % 3];
- const Vector3 c = t[i] + abc;
- float o = orient(a, b, c, inter);
- if(o < -eps) {
- sideA++;
- } else if(o > eps) {
- sideB++;
- } else {
- sideA++;
- sideB++;
- }
- }
- if(sideA >= 3 || sideB >= 3) {
- return factor;
- }
- return -1.0f;
- }
|