libigl_Martin/include/igl/fit_plane.cpp

// This file is part of libigl, a simple c++ geometry processing library.
// 
// Copyright (C) 2013 Daniele Panozzo <daniele.panozzo@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/.

#include "fit_plane.h"
#include <iostream>

void igl::fit_plane(
    const Eigen::MatrixXd & V,
    Eigen::RowVector3d & N,
    Eigen::RowVector3d & C)
{
    assert(V.rows()>0);

    Eigen::Vector3d sum = V.colwise().sum();

    Eigen::Vector3d center = sum.array()/(double(V.rows()));

    C = center;

    double sumXX=0.0f,sumXY=0.0f,sumXZ=0.0f;
    double sumYY=0.0f,sumYZ=0.0f;
    double sumZZ=0.0f;

    for(int i=0;i<V.rows();i++){
        double diffX=V(i,0)-center(0);
        double diffY=V(i,1)-center(1);
        double diffZ=V(i,2)-center(2);
        sumXX+=diffX*diffX;
        sumXY+=diffX*diffY;
        sumXZ+=diffX*diffZ;
        sumYY+=diffY*diffY;
        sumYZ+=diffY*diffZ;
        sumZZ+=diffZ*diffZ;
    }

    Eigen::MatrixXd m(3,3);
    m << sumXX,sumXY,sumXZ,
        sumXY,sumYY,sumYZ,
        sumXZ,sumYZ,sumZZ;

	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(m);
  
  N = es.eigenvectors().col(0);
}

#ifndef IGL_HEADER_ONLY
#endif