diff --git a/src/fd_grad.cpp b/src/fd_grad.cpp index 9206181..3f1a6f4 100644 --- a/src/fd_grad.cpp +++ b/src/fd_grad.cpp @@ -1,13 +1,40 @@ #include "fd_grad.h" +#include "fd_partial_derivative.h" +#include void fd_grad( const int nx, const int ny, const int nz, const double h, - Eigen::SparseMatrix & G) -{ + Eigen::SparseMatrix &G) { //////////////////////////////////////////////////////////////////////////// // Add your code here + Eigen::SparseMatrix dx, dy, dz; + + fd_partial_derivative(nx, ny, nz, h, 0, dx); + fd_partial_derivative(nx, ny, nz, h, 1, dy); + fd_partial_derivative(nx, ny, nz, h, 2, dz); + + G.resize(dx.rows() + dy.rows() + dz.rows(), dx.cols()); + G.setZero(); + + std::vector> tripletList; + for (int k = 0; k < dx.outerSize(); ++k) { + for (Eigen::SparseMatrix::InnerIterator it(dx, k); it; ++it) { + tripletList.push_back(Eigen::Triplet(it.row(), it.col(), it.value())); + } + } + for (int k = 0; k < dy.outerSize(); ++k) { + for (Eigen::SparseMatrix::InnerIterator it(dy, k); it; ++it) { + tripletList.push_back(Eigen::Triplet(it.row() + dx.rows(), it.col(), it.value())); + } + } + for (int k = 0; k < dz.outerSize(); ++k) { + for (Eigen::SparseMatrix::InnerIterator it(dz, k); it; ++it) { + tripletList.push_back(Eigen::Triplet(it.row() + dx.rows() + dy.rows(), it.col(), it.value())); + } + } + G.setFromTriplets(tripletList.begin(), tripletList.end()); //////////////////////////////////////////////////////////////////////////// } diff --git a/src/fd_interpolate.cpp b/src/fd_interpolate.cpp index 2a32675..fc39251 100644 --- a/src/fd_interpolate.cpp +++ b/src/fd_interpolate.cpp @@ -5,11 +5,43 @@ void fd_interpolate( const int ny, const int nz, const double h, - const Eigen::RowVector3d & corner, - const Eigen::MatrixXd & P, - Eigen::SparseMatrix & W) -{ + const Eigen::RowVector3d &corner, + const Eigen::MatrixXd &P, + Eigen::SparseMatrix &W) { //////////////////////////////////////////////////////////////////////////// // Add your code here + + W.resize(P.rows(), nx * ny * nz); + + auto ind = [&](int x, int y, int z) { + return x + nx * (y + z * ny); + }; + + std::vector> tripletList; + + // Loop through rows of P (loop through points) + for (int i = 0; i < P.rows(); i++) { + // Find the nearest points + int x = (int) ((P(i, 0) - corner[0]) / h); + int y = (int) ((P(i, 1) - corner[1]) / h); + int z = (int) ((P(i, 2) - corner[2]) / h); + + // Find percent + double px = (P(i, 0) - corner[0]) / h - x; + double py = (P(i, 1) - corner[1]) / h - y; + double pz = (P(i, 2) - corner[2]) / h - z; + + tripletList.push_back(Eigen::Triplet (i, ind(x, y, z), (1 - px) * (1 - py) * (1 - pz))); + tripletList.push_back(Eigen::Triplet (i, ind(x + 1, y, z), px * (1 - py) * (1 - pz))); + tripletList.push_back(Eigen::Triplet (i, ind(x, y + 1, z), (1 - px) * py * (1 - pz))); + tripletList.push_back(Eigen::Triplet (i, ind(x, y, z + 1), (1 - px) * (1 - py) * pz)); + tripletList.push_back(Eigen::Triplet (i, ind(x + 1, y + 1, z), px * py * (1 - pz))); + tripletList.push_back(Eigen::Triplet (i, ind(x + 1, y, z + 1), px * (1 - py) * pz)); + tripletList.push_back(Eigen::Triplet (i, ind(x, y + 1, z + 1), (1 - px) * py * pz)); + tripletList.push_back(Eigen::Triplet (i, ind(x + 1, y + 1, z + 1), px * py * pz)); + } + + W.setFromTriplets(tripletList.begin(), tripletList.end()); + //////////////////////////////////////////////////////////////////////////// } diff --git a/src/fd_partial_derivative.cpp b/src/fd_partial_derivative.cpp index c3c4deb..bd80702 100644 --- a/src/fd_partial_derivative.cpp +++ b/src/fd_partial_derivative.cpp @@ -10,5 +10,51 @@ void fd_partial_derivative( { //////////////////////////////////////////////////////////////////////////// // Add your code here + + int nxm = nx; + int nym = ny; + int nzm = nz; + + int m; + switch (dir) { + case 0: + nxm = nx - 1; + break; + case 1: + nym = ny - 1; + break; + default: + nzm = nz - 1; + } + m = nxm * nym * nzm; + + auto ind = [&](int x, int y, int z) { + return x + nx * (y + z * ny); + }; + auto mind = [&](int x, int y, int z) { + return x + nxm * (y + z * nym); + }; + + D.resize(m, nx * ny * nz); + + // Calculate D + for (int i = 0; i < nxm; i++) { + for (int j = 0; j < nym; j++) { + for (int k = 0; k < nzm; k++) { + D.insert(mind(i, j, k), ind(i, j, k)) = - 1 / h; + switch(dir) { + case 0: + D.insert(mind(i, j, k), ind(i + 1, j, k)) = 1 / h; + break; + case 1: + D.insert(mind(i, j, k), ind(i, j + 1, k)) = 1 / h; + break; + default: + D.insert(mind(i, j, k), ind(i, j, k + 1)) = 1 / h; + } + } + } + } + //////////////////////////////////////////////////////////////////////////// } diff --git a/src/poisson_surface_reconstruction.cpp b/src/poisson_surface_reconstruction.cpp index 4fe7c1e..154fd19 100644 --- a/src/poisson_surface_reconstruction.cpp +++ b/src/poisson_surface_reconstruction.cpp @@ -1,6 +1,10 @@ #include "poisson_surface_reconstruction.h" #include #include +#include "fd_grad.h" +#include "fd_interpolate.h" +#include +#include void poisson_surface_reconstruction( const Eigen::MatrixXd & P, @@ -46,6 +50,40 @@ void poisson_surface_reconstruction( //////////////////////////////////////////////////////////////////////////// // Add your code here + Eigen::SparseMatrix W, Wx, Wy, Wz, G; + fd_interpolate(nx, ny, nz, h, corner, P, W); + fd_grad(nx, ny, nz, h, G); + + fd_interpolate(nx - 1, ny, nz, h, corner + Eigen::RowVector3d(h * 0.5, 0, 0), P, Wx); + fd_interpolate(nx, ny - 1, nz, h, corner + Eigen::RowVector3d(0, h * 0.5, 0), P, Wy); + fd_interpolate(nx, ny, nz - 1, h, corner + Eigen::RowVector3d(0, 0, h * 0.5), P, Wz); + + Eigen::VectorXd vector, vx, vy, vz; + vx = Wx.transpose() * N.col(0); + vy = Wy.transpose() * N.col(1); + vz = Wz.transpose() * N.col(2); + + vector.resize((nx - 1) * ny * nz + nx * (ny - 1) * nz + nx * ny * (nz - 1)); + vector << vx, vy, vz; + + // BiCGSTAB solver always fail for me so use ConjugateGradient + Eigen::ConjugateGradient> solver; + Eigen::SparseMatrix A = G.transpose() * G; + Eigen::VectorXd b = G.transpose() * vector; + solver.compute(A); + if(solver.info()!=Eigen::Success) { + std::cout << "Decomposition failed" << std::endl; + return; + } + g = solver.solve(b); + if(solver.info()!=Eigen::Success) { + std::cout << "Solver failed" << std::endl; + return; + } + + double sigma = (1.0 / n) * Eigen::RowVectorXd::Ones(n) * W * g; + g -= sigma * Eigen::VectorXd::Ones(nx*ny*nz); + //////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////