forked from bartvdbraak/blender
511 lines
17 KiB
C++
511 lines
17 KiB
C++
// Begin License:
|
|
// Copyright (C) 2006-2011 Tobias Sargeant (tobias.sargeant@gmail.com).
|
|
// All rights reserved.
|
|
//
|
|
// This file is part of the Carve CSG Library (http://carve-csg.com/)
|
|
//
|
|
// This file may be used under the terms of the GNU General Public
|
|
// License version 2.0 as published by the Free Software Foundation
|
|
// and appearing in the file LICENSE.GPL2 included in the packaging of
|
|
// this file.
|
|
//
|
|
// This file is provided "AS IS" with NO WARRANTY OF ANY KIND,
|
|
// INCLUDING THE WARRANTIES OF DESIGN, MERCHANTABILITY AND FITNESS FOR
|
|
// A PARTICULAR PURPOSE.
|
|
// End:
|
|
|
|
|
|
#pragma once
|
|
|
|
#include <carve/carve.hpp>
|
|
|
|
#include <carve/geom.hpp>
|
|
#include <carve/aabb.hpp>
|
|
|
|
#include <iostream>
|
|
|
|
#include <cmath>
|
|
#include <limits>
|
|
|
|
#if defined(HAVE_STDINT_H)
|
|
# include <stdint.h>
|
|
#endif
|
|
|
|
namespace carve {
|
|
namespace geom {
|
|
|
|
template<unsigned ndim,
|
|
typename data_t,
|
|
typename aabb_calc_t = carve::geom::get_aabb<ndim, data_t> >
|
|
struct RTreeNode {
|
|
typedef aabb<ndim> aabb_t;
|
|
typedef vector<ndim> vector_t;
|
|
typedef RTreeNode<ndim, data_t, aabb_calc_t> node_t;
|
|
|
|
aabb_t bbox;
|
|
node_t *child;
|
|
node_t *sibling;
|
|
std::vector<data_t> data;
|
|
|
|
aabb_t getAABB() const { return bbox; }
|
|
|
|
struct data_aabb_t {
|
|
aabb_t bbox;
|
|
data_t data;
|
|
|
|
data_aabb_t() { }
|
|
data_aabb_t(const data_t &_data) : bbox(aabb_calc_t()(_data)), data(_data) {
|
|
}
|
|
|
|
aabb_t getAABB() const { return bbox; }
|
|
|
|
struct cmp {
|
|
size_t dim;
|
|
cmp(size_t _dim) : dim(_dim) { }
|
|
bool operator()(const data_aabb_t &a, const data_aabb_t &b) {
|
|
return a.bbox.pos.v[dim] < b.bbox.pos.v[dim];
|
|
}
|
|
};
|
|
};
|
|
|
|
// Fill an rtree node with a set of (data, aabb) pairs.
|
|
template<typename iter_t>
|
|
void _fill(iter_t begin, iter_t end, data_aabb_t) {
|
|
data.reserve(std::distance(begin, end));
|
|
for (iter_t i = begin; i != end; ++i) {
|
|
data.push_back((*i).data);
|
|
}
|
|
bbox.fit(begin, end);
|
|
}
|
|
|
|
// Fill an rtree node with a set of data.
|
|
template<typename iter_t>
|
|
void _fill(iter_t begin, iter_t end, data_t) {
|
|
data.reserve(std::distance(begin, end));
|
|
std::copy(begin, end, std::back_inserter(data));
|
|
bbox.fit(begin, end, aabb_calc_t());
|
|
}
|
|
|
|
// Fill an rtree node with a set of child nodes.
|
|
template<typename iter_t>
|
|
void _fill(iter_t begin, iter_t end, node_t *) {
|
|
iter_t i = begin;
|
|
node_t *curr = child = *i;
|
|
while (++i != end) {
|
|
curr->sibling = *i;
|
|
curr = curr->sibling;
|
|
}
|
|
bbox.fit(begin, end);
|
|
}
|
|
|
|
// Search the rtree for objects that intersect obj (generally an aabb).
|
|
// The aabb class must provide a method intersects(obj_t).
|
|
template<typename obj_t, typename out_iter_t>
|
|
void search(const obj_t &obj, out_iter_t out) const {
|
|
if (!bbox.intersects(obj)) return;
|
|
if (child) {
|
|
for (node_t *node = child; node; node = node->sibling) {
|
|
node->search(obj, out);
|
|
}
|
|
} else {
|
|
std::copy(data.begin(), data.end(), out);
|
|
}
|
|
}
|
|
|
|
// update the bounding box extents of nodes that intersect obj (generally an aabb).
|
|
// The aabb class must provide a method intersects(obj_t).
|
|
template<typename obj_t>
|
|
void updateExtents(const obj_t &obj) {
|
|
if (!bbox.intersects(obj)) return;
|
|
|
|
if (child) {
|
|
node_t *node = child;
|
|
node->updateExtents(obj);
|
|
bbox = node->bbox;
|
|
for (node = node->sibling; node; node = node->sibling) {
|
|
node->updateExtents(obj);
|
|
bbox.unionAABB(node->bbox);
|
|
}
|
|
} else {
|
|
bbox.fit(data.begin(), data.end());
|
|
}
|
|
}
|
|
|
|
// update the bounding box extents of nodes that intersect obj (generally an aabb).
|
|
// The aabb class must provide a method intersects(obj_t).
|
|
bool remove(const data_t &val, const aabb_t &val_aabb) {
|
|
if (!bbox.intersects(val_aabb)) return false;
|
|
|
|
if (child) {
|
|
node_t *node = child;
|
|
node->remove(val, val_aabb);
|
|
bbox = node->bbox;
|
|
bool removed = false;
|
|
for (node = node->sibling; node; node = node->sibling) {
|
|
if (!removed) removed = node->remove(val, val_aabb);
|
|
bbox.unionAABB(node->bbox);
|
|
}
|
|
return removed;
|
|
} else {
|
|
typename std::vector<data_t>::iterator i = std::remove(data.begin(), data.end(), val);
|
|
if (i == data.end()) {
|
|
return false;
|
|
}
|
|
data.erase(i, data.end());
|
|
bbox.fit(data.begin(), data.end());
|
|
return true;
|
|
}
|
|
}
|
|
|
|
template<typename iter_t>
|
|
RTreeNode(iter_t begin, iter_t end) : bbox(), child(NULL), sibling(NULL), data() {
|
|
_fill(begin, end, typename std::iterator_traits<iter_t>::value_type());
|
|
}
|
|
|
|
~RTreeNode() {
|
|
if (child) {
|
|
RTreeNode *next = child;
|
|
while (next) {
|
|
RTreeNode *curr = next;
|
|
next = next->sibling;
|
|
delete curr;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// functor for ordering nodes by increasing aabb midpoint, along a specified axis.
|
|
struct aabb_cmp_mid {
|
|
size_t dim;
|
|
aabb_cmp_mid(size_t _dim) : dim(_dim) { }
|
|
|
|
bool operator()(const node_t *a, const node_t *b) {
|
|
return a->bbox.mid(dim) < b->bbox.mid(dim);
|
|
}
|
|
bool operator()(const data_aabb_t &a, const data_aabb_t &b) {
|
|
return a.bbox.mid(dim) < b.bbox.mid(dim);
|
|
}
|
|
};
|
|
|
|
// functor for ordering nodes by increasing aabb minimum, along a specified axis.
|
|
struct aabb_cmp_min {
|
|
size_t dim;
|
|
aabb_cmp_min(size_t _dim) : dim(_dim) { }
|
|
|
|
bool operator()(const node_t *a, const node_t *b) {
|
|
return a->bbox.min(dim) < b->bbox.min(dim);
|
|
}
|
|
bool operator()(const data_aabb_t &a, const data_aabb_t &b) {
|
|
return a.bbox.min(dim) < b.bbox.min(dim);
|
|
}
|
|
};
|
|
|
|
// functor for ordering nodes by increasing aabb maximum, along a specified axis.
|
|
struct aabb_cmp_max {
|
|
size_t dim;
|
|
aabb_cmp_max(size_t _dim) : dim(_dim) { }
|
|
|
|
bool operator()(const node_t *a, const node_t *b) {
|
|
return a->bbox.max(dim) < b->bbox.max(dim);
|
|
}
|
|
bool operator()(const data_aabb_t &a, const data_aabb_t &b) {
|
|
return a.bbox.max(dim) < b.bbox.max(dim);
|
|
}
|
|
};
|
|
|
|
// facade for projecting node bounding box onto an axis.
|
|
struct aabb_extent {
|
|
size_t dim;
|
|
aabb_extent(size_t _dim) : dim(_dim) { }
|
|
|
|
double min(const node_t *a) { return a->bbox.pos.v[dim] - a->bbox.extent.v[dim]; }
|
|
double max(const node_t *a) { return a->bbox.pos.v[dim] + a->bbox.extent.v[dim]; }
|
|
double len(const node_t *a) { return 2.0 * a->bbox.extent.v[dim]; }
|
|
double min(const data_aabb_t &a) { return a.bbox.pos.v[dim] - a.bbox.extent.v[dim]; }
|
|
double max(const data_aabb_t &a) { return a.bbox.pos.v[dim] + a.bbox.extent.v[dim]; }
|
|
double len(const data_aabb_t &a) { return 2.0 * a.bbox.extent.v[dim]; }
|
|
};
|
|
|
|
template<typename iter_t>
|
|
static void makeNodes(const iter_t begin,
|
|
const iter_t end,
|
|
size_t dim_num,
|
|
uint32_t dim_mask,
|
|
size_t child_size,
|
|
std::vector<node_t *> &out) {
|
|
const size_t N = std::distance(begin, end);
|
|
|
|
size_t dim = ndim;
|
|
double r_best = N+1;
|
|
|
|
// find the sparsest remaining dimension to partition by.
|
|
for (size_t i = 0; i < ndim; ++i) {
|
|
if (dim_mask & (1U << i)) continue;
|
|
aabb_extent extent(i);
|
|
double dmin, dmax, dsum;
|
|
|
|
dmin = extent.min(*begin);
|
|
dmax = extent.max(*begin);
|
|
dsum = 0.0;
|
|
for (iter_t j = begin; j != end; ++j) {
|
|
dmin = std::min(dmin, extent.min(*j));
|
|
dmax = std::max(dmax, extent.max(*j));
|
|
dsum += extent.len(*j);
|
|
}
|
|
double r = dsum ? dsum / (dmax - dmin) : 0.0;
|
|
if (r_best > r) {
|
|
dim = i;
|
|
r_best = r;
|
|
}
|
|
}
|
|
|
|
CARVE_ASSERT(dim < ndim);
|
|
|
|
// dim = dim_num;
|
|
|
|
const size_t P = (N + child_size - 1) / child_size;
|
|
const size_t n_parts = (size_t)std::ceil(std::pow((double)P, 1.0 / (ndim - dim_num)));
|
|
|
|
std::sort(begin, end, aabb_cmp_mid(dim));
|
|
|
|
if (dim_num == ndim - 1 || n_parts == 1) {
|
|
for (size_t i = 0, s = 0, e = 0; i < P; ++i, s = e) {
|
|
e = N * (i+1) / P;
|
|
CARVE_ASSERT(e - s <= child_size);
|
|
out.push_back(new node_t(begin + s, begin + e));
|
|
}
|
|
} else {
|
|
for (size_t i = 0, s = 0, e = 0; i < n_parts; ++i, s = e) {
|
|
e = N * (i+1) / n_parts;
|
|
makeNodes(begin + s, begin + e, dim_num + 1, dim_mask | (1U << dim), child_size, out);
|
|
}
|
|
}
|
|
}
|
|
|
|
static node_t *construct_STR(std::vector<data_aabb_t> &data, size_t leaf_size, size_t internal_size) {
|
|
std::vector<node_t *> out;
|
|
makeNodes(data.begin(), data.end(), 0, 0, leaf_size, out);
|
|
|
|
while (out.size() > 1) {
|
|
std::vector<node_t *> next;
|
|
makeNodes(out.begin(), out.end(), 0, 0, internal_size, next);
|
|
std::swap(out, next);
|
|
}
|
|
|
|
CARVE_ASSERT(out.size() == 1);
|
|
return out[0];
|
|
}
|
|
|
|
template<typename iter_t>
|
|
static node_t *construct_STR(const iter_t &begin,
|
|
const iter_t &end,
|
|
size_t leaf_size,
|
|
size_t internal_size) {
|
|
std::vector<data_aabb_t> data;
|
|
data.reserve(std::distance(begin, end));
|
|
for (iter_t i = begin; i != end; ++i) {
|
|
data.push_back(*i);
|
|
}
|
|
return construct_STR(data, leaf_size, internal_size);
|
|
}
|
|
|
|
|
|
template<typename iter_t>
|
|
static node_t *construct_STR(const iter_t &begin1,
|
|
const iter_t &end1,
|
|
const iter_t &begin2,
|
|
const iter_t &end2,
|
|
size_t leaf_size,
|
|
size_t internal_size) {
|
|
std::vector<data_aabb_t> data;
|
|
data.reserve(std::distance(begin1, end1) + std::distance(begin2, end2));
|
|
for (iter_t i = begin1; i != end1; ++i) {
|
|
data.push_back(*i);
|
|
}
|
|
for (iter_t i = begin2; i != end2; ++i) {
|
|
data.push_back(*i);
|
|
}
|
|
return construct_STR(data, leaf_size, internal_size);
|
|
}
|
|
|
|
|
|
struct partition_info {
|
|
double score;
|
|
size_t partition_pos;
|
|
|
|
partition_info() : score(std::numeric_limits<double>::max()), partition_pos(0) {
|
|
}
|
|
partition_info(double _score, size_t _partition_pos) :
|
|
score(_score),
|
|
partition_pos(_partition_pos) {
|
|
}
|
|
};
|
|
|
|
static partition_info findPartition(typename std::vector<data_aabb_t>::iterator base,
|
|
std::vector<size_t>::iterator begin,
|
|
std::vector<size_t>::iterator end,
|
|
size_t part_size) {
|
|
partition_info best(std::numeric_limits<double>::max(), 0);
|
|
const size_t N = std::distance(begin, end);
|
|
|
|
std::vector<double> rhs_vol(N, 0.0);
|
|
|
|
aabb_t rhs = base[begin[N-1]].aabb;
|
|
rhs_vol[N-1] = rhs.volume();
|
|
for (size_t i = N - 1; i > 0; ) {
|
|
rhs.unionAABB(base[begin[--i]].aabb);
|
|
rhs_vol[i] = rhs.volume();
|
|
}
|
|
|
|
aabb_t lhs = base[begin[0]].aabb;
|
|
for (size_t i = 1; i < N; ++i) {
|
|
lhs.unionAABB(base[begin[i]].aabb);
|
|
if (i % part_size == 0 || (N - i) % part_size == 0) {
|
|
partition_info curr(lhs.volume() + rhs_vol[i], i);
|
|
if (best.score > curr.score) best = curr;
|
|
}
|
|
}
|
|
return best;
|
|
}
|
|
|
|
static void partition(typename std::vector<data_aabb_t>::iterator base,
|
|
std::vector<size_t>::iterator begin,
|
|
std::vector<size_t>::iterator end,
|
|
size_t part_size,
|
|
std::vector<size_t> &part_num,
|
|
size_t &part_next) {
|
|
const size_t N = std::distance(begin, end);
|
|
|
|
partition_info best;
|
|
partition_info curr;
|
|
size_t part_curr = part_num[*begin];
|
|
|
|
std::vector<size_t> tmp(begin, end);
|
|
|
|
for (size_t dim = 0; dim < ndim; ++dim) {
|
|
std::sort(tmp.begin(), tmp.end(), make_index_sort(base, aabb_cmp_min(dim)));
|
|
curr = findPartition(base, tmp.begin(), tmp.end(), part_size);
|
|
if (best.score > curr.score) {
|
|
best = curr;
|
|
std::copy(tmp.begin(), tmp.end(), begin);
|
|
}
|
|
|
|
std::sort(tmp.begin(), tmp.end(), make_index_sort(base, aabb_cmp_mid(dim)));
|
|
curr = findPartition(base, tmp.begin(), tmp.end(), part_size);
|
|
if (best.score > curr.score) {
|
|
best = curr;
|
|
std::copy(tmp.begin(), tmp.end(), begin);
|
|
}
|
|
|
|
std::sort(tmp.begin(), tmp.end(), make_index_sort(base, aabb_cmp_max(dim)));
|
|
curr = findPartition(base, tmp.begin(), tmp.end(), part_size);
|
|
if (best.score > curr.score) {
|
|
best = curr;
|
|
std::copy(tmp.begin(), tmp.end(), begin);
|
|
}
|
|
}
|
|
|
|
for (size_t j = 0; j < best.partition_pos; ++j) part_num[begin[j]] = part_curr;
|
|
for (size_t j = best.partition_pos; j < N; ++j) part_num[begin[j]] = part_next;
|
|
++part_next;
|
|
|
|
if (best.partition_pos > part_size) {
|
|
partition(base, begin, begin + best.partition_pos, part_size, part_num, part_next);
|
|
}
|
|
if (N - best.partition_pos > part_size) {
|
|
partition(base, begin + best.partition_pos, end, part_size, part_num, part_next);
|
|
}
|
|
}
|
|
|
|
static size_t makePartitions(typename std::vector<data_aabb_t>::iterator begin,
|
|
typename std::vector<data_aabb_t>::iterator end,
|
|
size_t part_size,
|
|
std::vector<size_t> &part_num) {
|
|
const size_t N = std::distance(begin, end);
|
|
std::vector<size_t> idx;
|
|
idx.reserve(N);
|
|
for (size_t i = 0; i < N; ++i) { idx.push_back(i); }
|
|
size_t part_next = 1;
|
|
|
|
partition(begin, idx.begin(), idx.end(), part_size, part_num, part_next);
|
|
return part_next;
|
|
}
|
|
|
|
static node_t *construct_TGS(typename std::vector<data_aabb_t>::iterator begin,
|
|
typename std::vector<data_aabb_t>::iterator end,
|
|
size_t leaf_size,
|
|
size_t internal_size) {
|
|
size_t N = std::distance(begin, end);
|
|
|
|
if (N <= leaf_size) {
|
|
return new node_t(begin, end);
|
|
} else {
|
|
size_t P = (N + internal_size - 1) / internal_size;
|
|
std::vector<size_t> part_num(N, 0);
|
|
P = makePartitions(begin, end, P, part_num);
|
|
|
|
size_t S = 0, E = 0;
|
|
std::vector<node_t *> children;
|
|
for (size_t i = 0; i < P; ++i) {
|
|
size_t j = S, k = N;
|
|
while (true) {
|
|
while (true) {
|
|
if (j == k) goto done;
|
|
else if (part_num[j] == i) ++j;
|
|
else break;
|
|
}
|
|
--k;
|
|
while (true) {
|
|
if (j == k) goto done;
|
|
else if (part_num[k] != i) --k;
|
|
else break;
|
|
}
|
|
std::swap(*(begin+j), *(begin+k));
|
|
std::swap(part_num[j], part_num[k]);
|
|
++j;
|
|
}
|
|
done:
|
|
E = j;
|
|
children.push_back(construct_TGS(begin + S, begin + E, leaf_size, internal_size));
|
|
S = E;
|
|
}
|
|
return new node_t(children.begin(), children.end());
|
|
}
|
|
}
|
|
|
|
template<typename iter_t>
|
|
static node_t *construct_TGS(const iter_t &begin,
|
|
const iter_t &end,
|
|
size_t leaf_size,
|
|
size_t internal_size) {
|
|
std::vector<data_aabb_t> data;
|
|
data.reserve(std::distance(begin, end));
|
|
for (iter_t i = begin; i != end; ++i) {
|
|
data.push_back(*i);
|
|
}
|
|
return construct_TGS(data.begin(), data.end(), leaf_size, internal_size);
|
|
}
|
|
|
|
template<typename iter_t>
|
|
static node_t *construct_TGS(const iter_t &begin1,
|
|
const iter_t &end1,
|
|
const iter_t &begin2,
|
|
const iter_t &end2,
|
|
size_t leaf_size,
|
|
size_t internal_size) {
|
|
std::vector<data_aabb_t> data;
|
|
data.reserve(std::distance(begin1, end1) + std::distance(begin2, end2));
|
|
for (iter_t i = begin1; i != end1; ++i) {
|
|
data.push_back(*i);
|
|
}
|
|
for (iter_t i = begin2; i != end2; ++i) {
|
|
data.push_back(*i);
|
|
}
|
|
return construct_TGS(data.begin(), data.end(), leaf_size, internal_size);
|
|
}
|
|
};
|
|
|
|
}
|
|
}
|