Unstylish versions of my fast winding number point cloud code

Former-commit-id: 0990ad1200dd5495b1edef3a6a4c1eb84d28f8fd

include/igl/build_octree.cpp

@@ -0,0 +1,136 @@
+#include "build_octree.h"
+#include <vector>
+#include <queue>
+
+const int MAX_DEPTH = 1000;
+
+int get_octant(const Eigen::RowVector3d & location, const Eigen::RowVector3d & center){
+    //Binary numbering of children:
+    //if we treat location as the origin,
+    //first bit is 1 if x is positive, 0 if not
+    //second bit is 1 if y is positive, 0 if not
+    //third bit is 1 if z is positive, 0 if not
+    //for example, the quadrant with negative x, positive y, positive z is:
+    //110 binary = 6 decimal
+    int index = 0;
+    if( location(0) >= center(0)){
+        index = index + 1;
+    }
+    if( location(1) >= center(1)){
+        index = index + 2;
+    }
+    if( location(2) >= center(2)){
+        index = index + 4;
+    }
+    return index;
+}
+
+
+Eigen::RowVector3d translate_center(const Eigen::RowVector3d & parent_center, double h, int child_index){
+    Eigen::RowVector3d change_vector;
+    change_vector << h,h,h;
+    if(child_index % 2){ //positive x chilren are 1,3,4,7
+        change_vector(0) = -h;
+    }
+    if(child_index == 2 || child_index == 3 ||
+       child_index == 6 || child_index == 7){ //positive y children are 2,3,6,7
+        change_vector(1) = -h;
+    }
+    if(child_index > 3){ //positive z children are 4,5,6,7
+        change_vector(2) = -h;
+    }
+    return parent_center - change_vector;
+}
+
+void build_octree(const Eigen::MatrixXd & P,
+                  std::vector<std::vector<int> > & point_indices,
+                  std::vector<Eigen::Matrix<int,8,1>, Eigen::aligned_allocator<Eigen::Matrix<int,8,1>>> & children,
+                  std::vector<Eigen::RowVector3d, Eigen::aligned_allocator<Eigen::RowVector3d>> & centers,
+                  std::vector<double> & widths
+                  ){
+    typedef Eigen::Matrix<int,8,1> Vector8i;
+    typedef Eigen::Matrix<double,8,1> Vector8d;
+
+    // How many cells do we have so far?
+    int m = 0;
+    
+    // Useful list of number 0..7
+    const Vector8i zero_to_seven = (Vector8i()<<0,1,2,3,4,5,6,7).finished();
+    const Vector8i neg_ones = (Vector8i()<<-1,-1,-1,-1,-1,-1,-1,-1).finished();
+    
+    // This function variable needs to be declared before it is defined so that
+    // you can capture it and call it recursively. Annoyingly this means you need
+    // to fill in the function prototype here, too.
+    std::function< void(const int, const int) > helper;
+    helper = [
+              // These variables from the parent scope are "captured" you can read and
+              // write to them
+              &helper,
+              &zero_to_seven,&neg_ones,&P,&point_indices,&children,&centers,&widths,&m]
+    // The "-> bool" means that this function will return bool (I don't think
+    // you need this, but in case you do.)
+    (const int index, const int depth)-> void
+    {
+        if(point_indices.at(index).size() > 1 && depth < MAX_DEPTH){
+            //give the parent access to the children
+            children.at(index) = zero_to_seven.array() + m;
+            //make the children's data in our arrays
+            
+            //Add the children to the lists, as default children
+            double h = widths.at(index)/2;
+            Eigen::RowVector3d curr_center = centers.at(index);
+            for(int i = 0; i < 8; i++){
+                children.emplace_back(-1 * Eigen::MatrixXi::Ones(8,1));
+                point_indices.emplace_back(std::vector<int>());
+                centers.emplace_back(translate_center(curr_center,h,i));
+                widths.emplace_back(h);
+            }
+            
+            //Split up the points into the corresponding children
+            for(int j = 0; j < point_indices.at(index).size(); j++){
+                int curr_point_index = point_indices.at(index).at(j);
+                int cell_of_curr_point = get_octant(P.row(curr_point_index),curr_center)+m;
+                point_indices.at(cell_of_curr_point).emplace_back(curr_point_index);
+            }
+            
+            //Now increase m
+            m += 8;
+            
+            // Look ma, I'm calling myself.
+            for(int i = 0; i < 8; i++){
+                helper(children.at(index)(i),depth+1);
+            }
+        }
+    };
+    
+    {
+        std::vector<int> all(P.rows());
+        for(int i = 0;i<all.size();i++) all[i]=i;
+        point_indices.emplace_back(all);
+    }
+    children.emplace_back(-1 * Eigen::MatrixXi::Ones(8,1));
+    
+    //Get the minimum AABB for the points
+    Eigen::RowVector3d backleftbottom(P.col(0).minCoeff(),P.col(1).minCoeff(),P.col(2).minCoeff());
+    Eigen::RowVector3d frontrighttop(P.col(0).maxCoeff(),P.col(1).maxCoeff(),P.col(2).maxCoeff());
+    Eigen::RowVector3d aabb_center = (backleftbottom+frontrighttop)/2.0;
+    double aabb_width = std::max(std::max(frontrighttop(0) - backleftbottom(0),
+                                          frontrighttop(1) - backleftbottom(1)),
+                                 frontrighttop(2) - backleftbottom(2));
+    centers.emplace_back( aabb_center );
+    widths.emplace_back( aabb_width ); //Widths are the side length of the cube, (not half the side length)
+    m++;
+    // then you have to actually call the function
+    helper(0,0);
+    
+    assert(point_indices.size() == m);
+    assert(children.size() == m);
+    assert(centers.size() == m);
+    assert(widths.size() == m);
+}
+//}
+//#ifndef IGL_STATIC_LIBRARY
+//#  include "point_areas_and_normals.cpp"
+//#endif
+//#endif
+

include/igl/build_octree.h

@@ -0,0 +1,18 @@
+#ifndef IGL_BUILD_OCTREE
+#define IGL_BUILD_OCTREE
+#include <Eigen/Core>
+//namespace igl
+//{
+    void build_octree(const Eigen::MatrixXd & P,
+                    std::vector<std::vector<int> > & point_indices,
+                    std::vector<Eigen::Matrix<int,8,1>, Eigen::aligned_allocator<Eigen::Matrix<int,8,1>>> & children,
+                    std::vector<Eigen::RowVector3d, Eigen::aligned_allocator<Eigen::RowVector3d>> & centers,
+                    std::vector<double> & widths
+                    );
+//}
+#ifndef IGL_STATIC_LIBRARY
+#  include "build_octree.cpp"
+#endif
+
+#endif
+

include/igl/fast_winding_number.cpp

@@ -0,0 +1,228 @@
+#include "fast_winding_number.h"
+#include <vector>
+#include <iostream>
+#include <igl/parallel_for.h>
+
+void fast_winding_number_precompute(const Eigen::MatrixXd & P,
+                                        const Eigen::MatrixXd & N,
+                                        const Eigen::VectorXd & A,
+                                        const std::vector<std::vector<int> > & point_indices,
+                                        const std::vector<Eigen::Matrix<int,8,1>, Eigen::aligned_allocator<Eigen::Matrix<int,8,1>>> & children,
+                                        const int expansion_order,
+                                        Eigen::MatrixXd & CM,
+                                        Eigen::VectorXd & R,
+                                        Eigen::MatrixXd & EC
+                                    ){
+    int m = children.size();
+    int num_terms;
+    if(expansion_order == 0){
+        num_terms = 3;
+    } else if(expansion_order ==1){
+        num_terms = 3 + 9;
+    } else if(expansion_order == 2){
+        num_terms = 3 + 9 + 27;
+    } else {
+        assert(false);
+    }
+    
+    R.resize(m);
+    CM.resize(m,3);
+    EC.resize(m,num_terms);
+    EC = Eigen::MatrixXd::Zero(m,num_terms);
+    std::function< void(const int) > helper;
+    helper = [&helper,
+              &P,&N,&A,&expansion_order,&point_indices,&children,&EC,&R,&CM]
+    (const int index)-> void
+    {
+        double sum_area = 0;
+        
+        Eigen::RowVector3d masscenter = Eigen::RowVector3d::Zero();
+        Eigen::RowVector3d zeroth_expansion = Eigen::RowVector3d::Zero();
+        double areatotal = 0.0;
+        for(int j = 0; j < point_indices.at(index).size(); j++){
+            int curr_point_index = point_indices.at(index).at(j);
+            
+            areatotal += A(curr_point_index);
+            masscenter += A(curr_point_index)*P.row(curr_point_index);
+            zeroth_expansion += A(curr_point_index)*N.row(curr_point_index);
+        }
+        
+        masscenter = masscenter/areatotal;
+        CM.row(index) = masscenter;
+        EC.block<1,3>(index,0) = zeroth_expansion;
+        
+        double max_norm = 0;
+        double curr_norm;
+        
+        for(int i = 0; i < point_indices.at(index).size(); i++){
+            //Get max distance from center of mass:
+            int curr_point_index = point_indices.at(index).at(i);
+            Eigen::RowVector3d point = P.row(curr_point_index)-masscenter;
+            curr_norm = point.norm();
+            if(curr_norm > max_norm){
+                max_norm = curr_norm;
+            }
+            
+            //Calculate higher order terms if necessary
+            Eigen::Matrix3d TempCoeffs;
+            if(EC.cols() >= (3+9)){
+                TempCoeffs = A(curr_point_index) * point.transpose() * N.row(curr_point_index);
+                EC.block<1,9>(index,3) += Eigen::Map<Eigen::RowVectorXd>(TempCoeffs.data(), TempCoeffs.size());
+            }
+            
+            if(EC.cols() == (3+9+27)){
+                for(int k = 0; k < 3; k++){
+                    TempCoeffs = 0.5 * point(k) * (A(curr_point_index) * point.transpose() * N.row(curr_point_index)) ;
+                    EC.block<1,9>(index,12+9*k) += Eigen::Map<Eigen::RowVectorXd>(TempCoeffs.data(), TempCoeffs.size());
+                }
+            }
+        }
+        
+        R(index) = max_norm;
+        if(children.at(index)(0) != -1)
+        {
+            for(int i = 0; i < 8; i++){
+                int child = children.at(index)(i);
+                helper(child);
+            }
+        }
+    };
+    helper(0);
+}
+
+double direct_eval(const Eigen::RowVector3d & loc, const Eigen::RowVector3d & anorm){
+    double wn = (loc(0)*anorm(0) + loc(1)*anorm(1) + loc(2)*anorm(2))/(4.0*M_PI*std::pow(loc.norm(),3));
+    if(std::isnan(wn)){
+        return 0.5;
+    }else{
+        return wn;
+    }
+}
+
+double expansion_eval(const Eigen::RowVector3d & loc, const Eigen::RowVectorXd & EC){
+    double wn = direct_eval(loc,EC.head<3>());
+    double r = loc.norm();
+    if(EC.size()>3){
+        Eigen::Matrix3d SecondDerivative = Eigen::Matrix3d::Identity()/(4.0*M_PI*std::pow(r,3));
+        SecondDerivative += -3.0*loc.transpose()*loc/(4.0*M_PI*std::pow(r,5));
+        Eigen::RowVectorXd derivative_vector = Eigen::Map<Eigen::RowVectorXd>(SecondDerivative.data(), SecondDerivative.size());
+        wn += derivative_vector.cwiseProduct(EC.segment<9>(3)).sum();
+    }
+    if(EC.size()>3+9){
+        Eigen::Matrix3d ThirdDerivative;
+        for(int i = 0; i < 3; i++){
+            ThirdDerivative = 15.0*loc(i)*loc.transpose()*loc/(4.0*M_PI*std::pow(r,7));
+            Eigen::Matrix3d Diagonal;
+            Diagonal << loc(i), 0, 0,
+                        0, loc(i), 0,
+                        0, 0, loc(i);
+            Eigen::Matrix3d RowCol = Eigen::Matrix3d::Zero();
+            RowCol.row(i) = loc;
+            RowCol = RowCol + RowCol.transpose();
+            ThirdDerivative += -3.0/(4.0*M_PI*std::pow(r,5)) * (RowCol + Diagonal);
+            Eigen::RowVectorXd derivative_vector = Eigen::Map<Eigen::RowVectorXd>(ThirdDerivative.data(), ThirdDerivative.size());
+            wn += derivative_vector.cwiseProduct(EC.segment<9>(12 + i*9)).sum();
+        }
+    }
+    return wn;
+}
+
+void fast_winding_number(const Eigen::MatrixXd & P,
+                         const Eigen::MatrixXd & N,
+                         const Eigen::VectorXd & A,
+                         const std::vector<std::vector<int> > & point_indices,
+                         const std::vector<Eigen::Matrix<int,8,1>,               Eigen::aligned_allocator<Eigen::Matrix<int,8,1>>> & children,
+                         const Eigen::MatrixXd & CM,
+                         const Eigen::VectorXd & R,
+                         const Eigen::MatrixXd & EC,
+                         const Eigen::MatrixXd & Q,
+                         const double & beta,
+                         Eigen::VectorXd & WN
+                         ){
+    int m = Q.rows();
+    WN.resize(m);
+    
+    std::function< double(const Eigen::RowVector3d, const std::vector<int>) > helper;
+    helper = [&helper,
+              &P,&N,&A,
+              &point_indices,&children,
+              &CM,&R,&EC,&beta]
+    (const Eigen::RowVector3d query, const std::vector<int> near_indices)-> double
+    {
+        std::vector<int> new_near_indices;
+        double wn = 0;
+        for(int i = 0; i < near_indices.size(); i++){
+            int index = near_indices.at(i);
+            //Leaf Case, Brute force
+            if(children.at(index)(0) == -1){
+                for(int j = 0; j < point_indices.at(index).size(); j++){
+                    int curr_row = point_indices.at(index).at(j);
+                    wn += direct_eval(P.row(curr_row)-query,N.row(curr_row)*A(curr_row));
+                }
+            }
+            //Non-Leaf Case
+            else {
+                for(int child = 0; child < 8; child++){
+                    int child_index = children.at(index)(child);
+                    if(point_indices.at(child_index).size() > 0){
+                        if((CM.row(child_index)-query).norm() > beta*R(child_index)){
+                            if(children.at(child_index)(0) == -1){
+                                for(int j = 0; j < point_indices.at(child_index).size(); j++){
+                                    int curr_row = point_indices.at(child_index).at(j);
+                                    wn += direct_eval(P.row(curr_row)-query,N.row(curr_row)*A(curr_row));
+                                }
+                            }else{
+                                wn += expansion_eval(CM.row(child_index)-query,EC.row(child_index));
+                            }
+                        }else {
+                            new_near_indices.emplace_back(child_index);
+                        }
+                    }
+                }
+            }
+        }
+        if(new_near_indices.size() > 0){
+            wn += helper(query,new_near_indices);
+        }
+        return wn;
+    };
+    
+    
+    if(beta >= 0){
+        std::vector<int> near_indices_start = {0};
+        igl::parallel_for(m,[&](int iter){
+            WN(iter) = helper(Q.row(iter),near_indices_start);
+        },1000);
+    } else {
+        igl::parallel_for(m,[&](int iter){
+            double wn = 0;
+            for(int j = 0; j <P.rows(); j++){
+                wn += direct_eval(P.row(j)-Q.row(iter),N.row(j)*A(j));
+            }
+            WN(iter) = wn;
+        },1000);
+    }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

include/igl/fast_winding_number.h

@@ -0,0 +1,33 @@
+#ifndef IGL_FAST_WINDING_NUMBER
+#define IGL_FAST_WINDING_NUMBER
+#include <Eigen/Core>
+namespace igl
+{
+    void fast_winding_number_precompute(const Eigen::MatrixXd & P,
+                                        const Eigen::MatrixXd & N,
+                                        const Eigen::MatrixXd & A,
+                                        const std::vector<std::vector<int> > & point_indices,
+                                        const std::vector<Eigen::Matrix<int,8,1>, Eigen::aligned_allocator<Eigen::Matrix<int,8,1>>> & children,
+                                        Eigen::MatrixXd & CM,
+                                        Eigen::VectorXd & R,
+                                        Eigen::MatrixXd & EC
+                    );
+    
+    void fast_winding_number(const Eigen::MatrixXd & P,
+                             const Eigen::MatrixXd & N,
+                             const Eigen::MatrixXd & A,
+                             const std::vector<std::vector<int> > & point_indices,
+                             const std::vector<Eigen::Matrix<int,8,1>,               Eigen::aligned_allocator<Eigen::Matrix<int,8,1>>> & children,
+                             const Eigen::MatrixXd & CM,
+                             const Eigen::VectorXd & R,
+                             const Eigen::MatrixXd & EC,
+                             const Eigen::MatrixXd & Q,
+                             Eigen::VectorXd & WN
+                             );
+}
+#ifndef IGL_STATIC_LIBRARY
+#  include "fast_winding_number.cpp"
+#endif
+
+#endif
+

include/igl/knn_octree.cpp

@@ -0,0 +1,114 @@
+#include "point_areas_and_normals.h"
+#include <cmath>
+#include <igl/parallel_for.h>
+
+double distance_to_width_one_cube(Eigen::RowVector3d point){
+    return std::sqrt(std::pow(std::max(std::abs(point(0))-1,0.0),2)
+                     + std::pow(std::max(std::abs(point(1))-1,0.0),2)
+                     + std::pow(std::max(std::abs(point(2))-1,0.0),2));
+}
+
+double distance_to_cube(Eigen::RowVector3d point, Eigen::RowVector3d cube_center, double cube_width){
+    Eigen::RowVector3d transformed_point = (point-cube_center)/cube_width;
+    return cube_width*distance_to_width_one_cube(transformed_point);
+}
+
+//    template <typename DerivedP, typename DerivedI, typename DerivedO>
+//    IGL_INLINE void point_areas_and_normals(
+//                               const Eigen::MatrixBase<DerivedP>& P,
+//                               const Eigen::MatrixBase<DerivedI>& I,
+//                               const Eigen::MatrixBase<DerivedO>& O,
+//                               Eigen::MatrixBase<DerivedA> & A,
+//                               Eigen::MatrixBase<DerivedN> & N);
+
+namespace igl {
+    void knn_octree(const Eigen::MatrixXd & P,
+                    const int & k,
+                    const std::vector<std::vector<int> > & point_indices,
+                    const std::vector<Eigen::Matrix<int,8,1>, Eigen::aligned_allocator<Eigen::Matrix<int,8,1>>> & children,
+                    const std::vector<Eigen::RowVector3d, Eigen::aligned_allocator<Eigen::RowVector3d>> & centers,
+                    const std::vector<double> & widths,
+                    Eigen::MatrixXi & I
+                    )
+    {
+        const int n = P.rows();
+        const int real_k = std::min(n,k);
+        I.resize(n,real_k);
+        
+      igl::parallel_for(n,[&](int i)
+      {
+          int points_found = 0;
+          Eigen::RowVector3d point_of_interest = P.row(i);
+          
+          //To make my priority queue take both points and octree cells, I use the indices 0 to n-1
+          //for the n points, and the indices n to n+m-1 for the m octree cells
+          
+          // Using lambda to compare elements.
+          auto cmp = [&point_of_interest, &P, &centers, &widths, &n](int left, int right) {
+              double leftdistance, rightdistance;
+              if(left < n){ //left is a point index
+                  leftdistance = (P.row(left) - point_of_interest).norm();
+              } else { //left is an octree cell
+                  leftdistance = distance_to_cube(point_of_interest,centers.at(left-n),widths.at(left-n));
+              }
+              
+              if(right < n){ //left is a point index
+                  rightdistance = (P.row(right) - point_of_interest).norm();
+              } else { //left is an octree cell
+                  rightdistance = distance_to_cube(point_of_interest,centers.at(right-n),widths.at(right-n));
+              }
+              return leftdistance >= rightdistance;
+          };
+          std::priority_queue<int, std::vector<int>, decltype(cmp)> queue(cmp);
+          queue.push(n); //This is the 0th octree cell (ie the root)
+          while(points_found < real_k){
+              int curr_cell_or_point = queue.top();
+              queue.pop();
+              if(curr_cell_or_point < n){ //current index is for is a point
+                  I(i,points_found) = curr_cell_or_point;
+                  points_found++;
+              } else {
+                  int curr_cell = curr_cell_or_point - n;
+                  if(children.at(curr_cell)(0) == -1){ //In the case of a leaf
+                      if(point_indices.at(curr_cell).size() > 0){ //Assumption: Leaves either have one point, or none
+                          queue.push(point_indices.at(curr_cell).at(0)); //push the point (pardon the pun)
+                      }
+                  } else { //Not a leaf
+                      for(int j = 0; j < 8; j++){
+                          queue.push(children.at(curr_cell)(j)+n); //+n to adjust for the octree cells
+                      }
+                  }
+              }
+          }
+//          points_found = 0;
+//          std::list<int> l = {n}; //This is the 0th octree cell (ie the root)
+//          while(points_found < real_k){
+//              int curr_cell_or_point = l.front();
+//              l.pop_front();
+//              if(curr_cell_or_point < n){ //current index is for is a point
+//                  I(i,points_found) = curr_cell_or_point;
+//                  points_found++;
+//              } else {
+//                  int curr_cell = curr_cell_or_point - n;
+//                  if(children.at(curr_cell)(0) == -1){ //In the case of a leaf
+//                      if(point_indices.at(curr_cell).size() > 0){ //Assumption: Leaves either have one point, or none
+//                          l.emplace_back(point_indices.at(curr_cell).at(0)); //push the point (pardon the pun)
+//                      }
+//                  } else { //Not a leaf
+//                      for(int j = 0; j < 8; j++){
+//                          l.emplace_back(children.at(curr_cell)(j)+n); //+n to adjust for the octree cells
+//                      }
+//                  }
+//                  l.sort(cmp);
+//                  int new_size = std::min<int>(l.size(),real_k-points_found);
+//                  l.resize(new_size);
+//              }
+//          }
+//
+      },1000);
+    }
+}
+
+
+
+

include/igl/knn_octree.h

@@ -0,0 +1,19 @@
+#ifndef IGL_KNN_OCTREE
+#define IGL_KNN_OCTREE
+#include <Eigen/Core>
+namespace igl
+{
+void knn_octree(const Eigen::MatrixXd & P,
+                const int & k,
+                const std::vector<std::vector<int> > & point_indices,
+                const std::vector<Eigen::Matrix<int,8,1>, Eigen::aligned_allocator<Eigen::Matrix<int,8,1>>> & children,
+                const std::vector<Eigen::RowVector3d, Eigen::aligned_allocator<Eigen::RowVector3d>> & centers,
+                const std::vector<double> & widths,
+                Eigen::MatrixXi & I
+                );
+}
+#ifndef IGL_STATIC_LIBRARY
+#  include "knn_octree.cpp"
+#endif
+#endif
+

include/igl/point_areas_and_normals.cpp

@@ -0,0 +1,135 @@
+#include "point_areas_and_normals.h"
+
+#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
+#include <CGAL/Triangulation_vertex_base_with_info_2.h>
+#include <CGAL/Triangulation_data_structure_2.h>
+#include <CGAL/Delaunay_triangulation_2.h>
+
+#include <igl/copyleft/cgal/delaunay_triangulation.h>
+#include <igl/colon.h>
+#include <igl/slice.h>
+#include <igl/slice_mask.h>
+#include <igl/parallel_for.h>
+
+typedef CGAL::Exact_predicates_inexact_constructions_kernel            Kernel;
+typedef CGAL::Triangulation_vertex_base_with_info_2<unsigned int, Kernel> Vb;
+typedef CGAL::Triangulation_data_structure_2<Vb>                       Tds;
+typedef CGAL::Delaunay_triangulation_2<Kernel, Tds>                    Delaunay;
+typedef Kernel::Point_2                                                Point;
+
+template <typename DerivedP, typename DerivedI, typename DerivedO>
+IGL_INLINE void point_areas_and_normals(
+  const Eigen::MatrixBase<DerivedP>& P,
+  const Eigen::MatrixBase<DerivedI>& I,
+  const Eigen::MatrixBase<DerivedO>& O,
+  Eigen::PlainObjectBase<DerivedA> & A,
+  Eigen::PlainObjectBase<DerivedA> & N);
+{
+//namespace igl {
+//    void point_areas_and_normals(const Eigen::MatrixXd & P,
+//                                 const Eigen::MatrixXi & I,
+//                                 const Eigen::MatrixXd & O,
+//                                 Eigen::VectorXd & A,
+//                                 Eigen::MatrixXd & N
+//                                 )
+//    {
+        const int n = P.rows();
+        A.setZero(n,1);
+        N.setZero(n,3);
+        igl::parallel_for(P.rows(),[&](int i)
+      {
+          Eigen::MatrixXi neighbour_index = I.row(i);
+          Eigen::MatrixXd neighbours;
+          igl::slice(P,neighbour_index,1,neighbours);
+          if(O.rows() && neighbours.rows() > 1){
+              Eigen::MatrixXd neighbour_normals;
+              igl::slice(O,neighbour_index,1,neighbour_normals);
+              Eigen::Vector3d poi_normal = neighbour_normals.row(0);
+              Eigen::VectorXd dotprod = poi_normal(0)*neighbour_normals.col(0)
+              + poi_normal(1)*neighbour_normals.col(1)
+              + poi_normal(2)*neighbour_normals.col(2);
+              Eigen::Array<bool,Eigen::Dynamic,1> keep = dotprod.array() > 0;
+              igl::slice_mask(Eigen::MatrixXd(neighbours),keep,1,neighbours);
+          }
+          if(neighbours.rows() <= 2){
+              A(i) = 0;
+              N.row(i) = Eigen::RowVector3d::Zero();
+          } else {
+              //subtract the mean from neighbours, then take svd, the scores will be U*S;
+              //This is our pca plane fitting
+              Eigen::RowVector3d mean = neighbours.colwise().mean();
+              Eigen::MatrixXd mean_centered = neighbours.rowwise() - mean;
+              Eigen::JacobiSVD<Eigen::MatrixXd> svd(mean_centered, Eigen::ComputeThinU | Eigen::ComputeThinV);
+              Eigen::MatrixXd scores = svd.matrixU() * svd.singularValues().asDiagonal();
+              N.row(i) = svd.matrixV().col(2).transpose();
+              if(N.row(i).dot(O.row(i)) < 0){
+                  N.row(i) *= -1;
+              }
+              Eigen::MatrixXd plane;
+              igl::slice(scores,igl::colon<int>(0,scores.rows()-1),igl::colon<int>(0,1),plane);
+              std::vector< std::pair<Point,unsigned> > points;
+              
+              //This is where we obtain a delaunay triangulation of the points
+              for(unsigned iter = 0; iter < plane.rows(); iter++){
+                  points.push_back( std::make_pair( Point(plane(iter,0),plane(iter,1)), iter ) );
+              }
+              Delaunay triangulation;
+              triangulation.insert(points.begin(),points.end());
+              Eigen::MatrixXi F(triangulation.number_of_faces(),3);
+              int f_row = 0;
+              for(Delaunay::Finite_faces_iterator fit = triangulation.finite_faces_begin();
+                  fit != triangulation.finite_faces_end(); ++fit) {
+                  Delaunay::Face_handle face = fit;
+                  F.row(f_row) = Eigen::RowVector3i((int)face->vertex(0)->info(),
+                                                    (int)face->vertex(1)->info(),
+                                                    (int)face->vertex(2)->info());
+                  f_row++;
+              }
+              
+              //Here we calculate the voronoi area of the point
+              double area_accumulator = 0;
+              for(int f = 0; f < F.rows(); f++){
+                  int X = -1;
+                  for(int face_iter = 0; face_iter < 3; face_iter++){
+                      if(F(f,face_iter) == 0){
+                          X = face_iter;
+                      }
+                  }
+                  if(X >= 0){
+                      //Triangle XYZ with X being the point we want the area of
+                      int Y = (X+1)%3;
+                      int Z = (X+2)%3;
+                      double x = (plane.row(F(f,Y))-plane.row(F(f,Z))).norm();
+                      double y = (plane.row(F(f,X))-plane.row(F(f,Z))).norm();
+                      double z = (plane.row(F(f,Y))-plane.row(F(f,X))).norm();
+                      double cosX = (z*z + y*y - x*x)/(2*y*z);
+                      double cosY = (z*z + x*x - y*y)/(2*x*z);
+                      double cosZ = (x*x + y*y - z*z)/(2*y*x);
+                      Eigen::Vector3d barycentric;
+                      barycentric << x*cosX, y*cosY, z*cosZ;
+                      barycentric /= (barycentric(0) + barycentric(1) + barycentric(2));
+                      //TODO: to make numerically stable, reorder so that x≥y≥z:
+                      double full_area = 0.25*std::sqrt((x+(y+z))*(z-(x-y))*(z+(x-y))*(x+(y-z)));
+                      Eigen::Vector3d partial_area = barycentric * full_area;
+                      if(cosX < 0){
+                          area_accumulator += 0.5*full_area;
+                      } else if (cosY < 0 || cosZ < 0){
+                          area_accumulator += 0.25*full_area;
+                      } else {
+                          area_accumulator += (partial_area(1) + partial_area(2))/2;
+                      }
+                  }
+              }
+              if(std::isfinite(area_accumulator)){
+                  A(i) = area_accumulator;
+              } else {
+                  A(i) = 0;
+              }
+          }
+      },1000);
+    }
+}
+
+
+
+

include/igl/point_areas_and_normals.h

@@ -0,0 +1,46 @@
+// This file is part of libigl, a simple c++ geometry processing library.
+//
+// Copyright (C) 2018 Gavin Barill <gavinpcb@gmail.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla Public License
+// v. 2.0. If a copy of the MPL was not distributed with this file, You can
+// obtain one at http://mozilla.org/MPL/2.0/
+#ifndef IGL_POINT_AREAS_AND_NORMALS_H
+#define IGL_POINT_AREAS_AND_NORMALS_H
+#include "igl_inline.h"
+#include <Eigen/Core>
+namespace igl
+{
+    // Given a 3D set of points P, each with a list of k-nearest-neighbours,
+    // estimate the geodesic voronoi area associated with each point.
+    
+    // If given, O represents an input oritentaiton to each point. This could
+    // be an initial guess at the normals, a vector to a camera position (for
+    // scanned data), or ground truth normals if you're only interested in the
+    // area estimation. It's used to ensure area estimation only occurs using
+    // neighbors that are on the same side of the surface (ie for thin
+    // sheets), as well as to solve the orientation ambiguity of normal
+    // estimation.
+    //
+    // Inputs:
+    //   P  #P by 3 list of point locations
+    //   I  #P by k list of k-nearest-neighbor indices into P
+    //   O  #P by 3 list of point orientation vectors (optional)
+    // Outputs:
+    //   A  #P list of estimated areas
+    //   N  #P by 3 list of estimated normals
+    template <typename DerivedP, typename DerivedI, typename DerivedO>
+    IGL_INLINE void point_areas_and_normals(
+                               const Eigen::MatrixBase<DerivedP>& P,
+                               const Eigen::MatrixBase<DerivedI>& I,
+                               const Eigen::MatrixBase<DerivedO>& O,
+                               Eigen::MatrixBase<DerivedA> & A,
+                               Eigen::MatrixBase<DerivedN> & N);
+}
+
+#ifndef IGL_STATIC_LIBRARY
+#  include "point_areas_and_normals.cpp"
+#endif
+
+#endif
+