149 lines
6.8 KiB
C++
149 lines
6.8 KiB
C++
|
#include "sparse_voxel_grid.h"
|
||
|
|
|
||
|
|
#include <unordered_map>
|
||
|
|
#include <array>
|
||
|
|
#include <vector>
|
||
|
|
|
||
|
|
|
||
|
|
template <typename DerivedP0, typename Func, typename DerivedS, typename DerivedV, typename DerivedI>
|
||
|
|
IGL_INLINE void igl::sparse_voxel_grid(const Eigen::MatrixBase<DerivedP0>& p0,
|
||
|
|
const Func& scalarFunc,
|
||
|
|
const double eps,
|
||
|
|
const int expected_number_of_cubes,
|
||
|
|
Eigen::PlainObjectBase<DerivedS>& CS,
|
||
|
|
Eigen::PlainObjectBase<DerivedV>& CV,
|
||
|
|
Eigen::PlainObjectBase<DerivedI>& CI)
|
||
|
|
{
|
||
|
|
typedef typename DerivedV::Scalar ScalarV;
|
||
|
|
typedef typename DerivedS::Scalar ScalarS;
|
||
|
|
typedef typename DerivedI::Scalar ScalarI;
|
||
|
|
typedef Eigen::Matrix<ScalarV, 1, 3> VertexRowVector;
|
||
|
|
typedef Eigen::Matrix<ScalarI, 1, 8> IndexRowVector;
|
||
|
|
|
||
|
|
|
||
|
|
struct IndexRowVectorHash {
|
||
|
|
std::size_t operator()(const Eigen::RowVector3i& key) const {
|
||
|
|
std::size_t seed = 0;
|
||
|
|
std::hash<int> hasher;
|
||
|
|
for (int i = 0; i < 3; i++) {
|
||
|
|
seed ^= hasher(key[i]) + 0x9e3779b9 + (seed<<6) + (seed>>2); // Copied from boost::hash_combine
|
||
|
|
}
|
||
|
|
return seed;
|
||
|
|
}
|
||
|
|
};
|
||
|
|
|
||
|
|
auto sgn = [](ScalarS val) -> int {
|
||
|
|
return (ScalarS(0) < val) - (val < ScalarS(0));
|
||
|
|
};
|
||
|
|
|
||
|
|
ScalarV half_eps = 0.5 * eps;
|
||
|
|
|
||
|
|
std::vector<IndexRowVector> CI_vector;
|
||
|
|
std::vector<VertexRowVector> CV_vector;
|
||
|
|
std::vector<ScalarS> CS_vector;
|
||
|
|
CI_vector.reserve(expected_number_of_cubes);
|
||
|
|
CV_vector.reserve(8 * expected_number_of_cubes);
|
||
|
|
CS_vector.reserve(8 * expected_number_of_cubes);
|
||
|
|
|
||
|
|
// Track visisted neighbors
|
||
|
|
std::unordered_map<Eigen::RowVector3i, int, IndexRowVectorHash> visited;
|
||
|
|
visited.reserve(6 * expected_number_of_cubes);
|
||
|
|
visited.max_load_factor(0.5);
|
||
|
|
|
||
|
|
// BFS Queue
|
||
|
|
std::vector<Eigen::RowVector3i> queue;
|
||
|
|
queue.reserve(expected_number_of_cubes * 8);
|
||
|
|
queue.push_back(Eigen::RowVector3i(0, 0, 0));
|
||
|
|
|
||
|
|
while (queue.size() > 0)
|
||
|
|
{
|
||
|
|
Eigen::RowVector3i pi = queue.back();
|
||
|
|
queue.pop_back();
|
||
|
|
|
||
|
|
VertexRowVector ctr = p0 + eps*pi.cast<ScalarV>(); // R^3 center of this cube
|
||
|
|
|
||
|
|
// X, Y, Z basis vectors, and array of neighbor offsets used to construct cubes
|
||
|
|
const Eigen::RowVector3i bx(1, 0, 0), by(0, 1, 0), bz(0, 0, -1);
|
||
|
|
const std::array<Eigen::RowVector3i, 6> neighbors = {
|
||
|
|
bx, -bx, by, -by, bz, -bz
|
||
|
|
};
|
||
|
|
|
||
|
|
// Compute the position of the cube corners and the scalar values at those corners
|
||
|
|
std::array<VertexRowVector, 8> cubeCorners = {
|
||
|
|
ctr+half_eps*(bx+by+bz).cast<ScalarV>(), ctr+half_eps*(bx+by-bz).cast<ScalarV>(), ctr+half_eps*(-bx+by-bz).cast<ScalarV>(), ctr+half_eps*(-bx+by+bz).cast<ScalarV>(),
|
||
|
|
ctr+half_eps*(bx-by+bz).cast<ScalarV>(), ctr+half_eps*(bx-by-bz).cast<ScalarV>(), ctr+half_eps*(-bx-by-bz).cast<ScalarV>(), ctr+half_eps*(-bx-by+bz).cast<ScalarV>()
|
||
|
|
};
|
||
|
|
std::array<ScalarS, 8> cubeScalars;
|
||
|
|
for (int i = 0; i < 8; i++) { cubeScalars[i] = scalarFunc(cubeCorners[i]); }
|
||
|
|
|
||
|
|
// If this cube doesn't intersect the surface, disregard it
|
||
|
|
bool validCube = false;
|
||
|
|
int sign = sgn(cubeScalars[0]);
|
||
|
|
for (int i = 1; i < 8; i++) {
|
||
|
|
if (sign != sgn(cubeScalars[i])) {
|
||
|
|
validCube = true;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (!validCube) {
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Add the cube vertices and indices to the output arrays if they are not there already
|
||
|
|
IndexRowVector cube;
|
||
|
|
uint8_t vertexAlreadyAdded = 0; // This is a bimask. If a bit is 1, it has been visited already by the BFS
|
||
|
|
constexpr std::array<uint8_t, 6> zv = {
|
||
|
|
(1 << 0) | (1 << 1) | (1 << 4) | (1 << 5),
|
||
|
|
(1 << 2) | (1 << 3) | (1 << 6) | (1 << 7),
|
||
|
|
(1 << 0) | (1 << 1) | (1 << 2) | (1 << 3),
|
||
|
|
(1 << 4) | (1 << 5) | (1 << 6) | (1 << 7),
|
||
|
|
(1 << 0) | (1 << 3) | (1 << 4) | (1 << 7),
|
||
|
|
(1 << 1) | (1 << 2) | (1 << 5) | (1 << 6), };
|
||
|
|
constexpr std::array<std::array<int, 4>, 6> zvv {{
|
||
|
|
{{0, 1, 4, 5}}, {{3, 2, 7, 6}}, {{0, 1, 2, 3}},
|
||
|
|
{{4, 5, 6, 7}}, {{0, 3, 4, 7}}, {{1, 2, 5, 6}} }};
|
||
|
|
|
||
|
|
for (int n = 0; n < 6; n++) { // For each neighbor, check the hash table to see if its been added before
|
||
|
|
Eigen::RowVector3i nkey = pi + neighbors[n];
|
||
|
|
auto nbr = visited.find(nkey);
|
||
|
|
if (nbr != visited.end()) { // We've already visited this neighbor, use references to its vertices instead of duplicating them
|
||
|
|
vertexAlreadyAdded |= zv[n];
|
||
|
|
for (int i = 0; i < 4; i++) { cube[zvv[n][i]] = CI_vector[nbr->second][zvv[n % 2 == 0 ? n + 1 : n - 1][i]]; }
|
||
|
|
} else {
|
||
|
|
queue.push_back(nkey); // Otherwise, we have not visited the neighbor, put it in the BFS queue
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
for (int i = 0; i < 8; i++) { // Add new, non-visited,2 vertices to the arrays
|
||
|
|
if (0 == ((1 << i) & vertexAlreadyAdded)) {
|
||
|
|
cube[i] = CS_vector.size();
|
||
|
|
CV_vector.push_back(cubeCorners[i]);
|
||
|
|
CS_vector.push_back(cubeScalars[i]);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
visited[pi] = CI_vector.size();
|
||
|
|
CI_vector.push_back(cube);
|
||
|
|
}
|
||
|
|
|
||
|
|
CV.conservativeResize(CV_vector.size(), 3);
|
||
|
|
CS.conservativeResize(CS_vector.size(), 1);
|
||
|
|
CI.conservativeResize(CI_vector.size(), 8);
|
||
|
|
// If you pass in column-major matrices, this is going to be slooooowwwww
|
||
|
|
for (int i = 0; i < CV_vector.size(); i++) {
|
||
|
|
CV.row(i) = CV_vector[i];
|
||
|
|
}
|
||
|
|
for (int i = 0; i < CS_vector.size(); i++) {
|
||
|
|
CS[i] = CS_vector[i];
|
||
|
|
}
|
||
|
|
for (int i = 0; i < CI_vector.size(); i++) {
|
||
|
|
CI.row(i) = CI_vector[i];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
#ifdef IGL_STATIC_LIBRARY
|
||
|
|
template void igl::sparse_voxel_grid<class Eigen::Matrix<double, -1, -1, 0, -1, -1>, class std::function<double(class Eigen::Matrix<double, -1, -1, 0, -1, -1> const &)>, class Eigen::Matrix<double, -1, 1, 0, -1, 1>, class Eigen::Matrix<double, -1, -1, 0, -1, -1>, class Eigen::Matrix<int, -1, -1, 0, -1, -1> >(class Eigen::MatrixBase<class Eigen::Matrix<double, -1, -1, 0, -1, -1> > const &, class std::function<double(class Eigen::Matrix<double, -1, -1, 0, -1, -1> const &)> const &, double, int, class Eigen::PlainObjectBase<class Eigen::Matrix<double, -1, 1, 0, -1, 1> > &, class Eigen::PlainObjectBase<class Eigen::Matrix<double, -1, -1, 0, -1, -1> > &, class Eigen::PlainObjectBase<class Eigen::Matrix<int, -1, -1, 0, -1, -1> > &);
|
||
|
|
template void igl::sparse_voxel_grid<Eigen::Matrix<double, 1, 3, 1, 1, 3>, std::function<double (Eigen::Matrix<double, 1, 3, 1, 1, 3> const&)>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> > const&, std::function<double (Eigen::Matrix<double, 1, 3, 1, 1, 3> const&)> const&, double, int, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&);
|
||
|
|
#endif
|