a_shape_plane.cxx

Go to the documentation of this file.

/*
         Copyright 2009-2020 Pierre SMARS (smars@yuntech.edu.tw)
         This program is free software: you can redistribute it and/or modify
         it under the terms of the GNU General Public License as published by
         the Free Software Foundation, either version 2 of the License, or
         (at your option) any later version.
 
         This program is distributed in the hope that it will be useful,
         but WITHOUT ANY WARRANTY; without even the implied warranty of
         MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
         GNU General Public License for more details.
 
         You should have received a copy of the GNU General Public License
         along with this program.  If not, see <http://www.gnu.org/licenses/>.
         */
#include "a_shape_plane.h"
#include <math.h>
#include <vnl/vnl_least_squares_function.h>
#include <vtkPolyData.h>
#include <vtkPoints.h>
#include <vtkCellArray.h>
#include <vtkHull.h>
#include <vtkDelaunay2D.h>
 
//***************************************************************************
class a_shape_plane_function : public vnl_least_squares_function
{
        public:
                a_shape_plane_function( a_shape_plane& shape,
                                const std::vector<a_point> & cloud) :
                        vnl_least_squares_function(4, cloud.size(), no_gradient),
                        shape_(shape),
                        cloud_(cloud) {};
                void f(const vnl_vector<double>& x, vnl_vector<double>& fx);
        protected:
                a_shape_plane &                 shape_;
                const std::vector<a_point> &     cloud_;
};
//---------------------------------------------------------------------------
void a_shape_plane_function::f(const vnl_vector<double>& x, vnl_vector<double>& fx)
{
        a_point norm(x[0],x[1],x[2]);
        double d(x[3]);
        norm.normalise();
        shape_.normal(norm);
        shape_.dist(d);
        for (int i = 0; i< fx.size(); i++)
                fx[i] = shape_.dist_point(cloud_[i]);
}
//***************************************************************************
//---------------------------------------------------------------------------
const std::string a_shape_plane::help()
{    
  std::ostringstream o;
  o << "**************" << std::endl;
  o << "a_shape_plane:" << std::endl;
  o << "**************" << std::endl;
  o << "This is a 'plane' fitting class" << std::endl;
  o << std::endl;                       
  o << a_shape::help();                                                        
  return o.str();                 
}
//---------------------------------------------------------------------------
void a_shape_plane::normal(const a_point p) 
{
        a_point p2=p; 
        p2.normalise(); 
        para_[0] = p2.x();
        para_[1] = p2.y(); 
        para_[2] = p2.z(); 
}
//---------------------------------------------------------------------------
void a_shape_plane::normalise()
{
        a_point v = this->normal();
        v.normalise();
        para_[0] = v.x();
        para_[1] = v.y();
        para_[2] = v.z();
}
//---------------------------------------------------------------------------
a_point a_shape_plane::origin() const
{
        a_point norm = this->normal();
        return norm*this->dist();
}
//---------------------------------------------------------------------------
a_point a_shape_plane::closest_point(const a_point pt) const
{
        //compute the normal
        a_point norm = this->normal();
        //point of the plan closest to the origin
        a_point pt0 = norm*this->dist();
        return pt-norm*(norm*(pt-pt0));
}
//---------------------------------------------------------------------------
double a_shape_plane::dist_point(const a_point pt) const
{
        //compute the normal
        a_point norm = this->normal();
        //point of the plan closest to the origin
        a_point pt0 = norm*this->dist();
        return fabs(norm*(pt-pt0));
}
//---------------------------------------------------------------------------
void a_shape_plane::p3pts(const a_point p1, const a_point p2, const a_point p3)
{
        a_point d1 = p2-p1;
        a_point d2 = p3-p1;
        a_point dn = cross(d1,d2).normalise();
        if (dn.norm()==0) return;
        double d = (p1+p2+p3)*dn/3.;
        this->normal(dn);
        this->dist(d);
        //std::cerr << orig_ << std::endl << dir_ << std::endl;
}
//---------------------------------------------------------------------------
void a_shape_plane::random_hint(const std::vector<a_point>& pts)
{
        int n = pts.size();
        int i1 = rand()%n;
        int i2,i3;
        do
        {
                i2 = rand()%n;
        } while (i1==i2);
        do
        {
                i3 = rand()%n;
        } while ((i1==i3)||(i2==i3));
        a_point p1 = pts[i1];
        a_point p2 = pts[i2];
        a_point p3 = pts[i3];
        this->p3pts(p1,p2,p3);
}
//---------------------------------------------------------------------------
void a_shape_plane::fit_cloud(std::vector<a_point>& pts, short nl)
{
        std::vector<a_point> pts2;
        int cs0 = pts.size();
        this->best_fitting_cloud(pts,pts2);
        if (nl==1)
        {
                a_shape_plane_function f(*this,pts2);
                a_shape::fit_cloud(pts2,f);
        }
        pts = pts2;
}
//---------------------------------------------------------------------------
void a_shape_plane::export_3pts() const
{
        a_point o = this->origin();
        a_point n = this->normal();
        int r;
        if (n[0]<n[1]) r = 0; else r = 1;
        if (n[r]>n[2]) r = 2;
        //find axis farther away from n
        a_point a;
        switch (r)
        {
                case 0 : a.x(1.); break;
                case 1 : a.y(1.); break;
                default : a.z(1.);
        }
        a_point u = cross(n,a).normalise();
        a_point v = cross(n,u).normalise();
        std::cout << o;
        std::cout << o+u;
        std::cout << o+v;
        std::cout << std::endl;
}
//---------------------------------------------------------------------------
void a_shape_plane::export_triangles(const std::vector<a_point>& pts) const
{
        a_point x0 = this->closest_point(pts[0]);
        double vmin, vmax;
        double d0;
        int i0;
        std::vector<a_point> pts_proj;
        pts_proj.push_back(x0);
        for (int i=1; i< pts.size(); i++)
        {
                a_point x = this->closest_point(pts[i]);
                pts_proj.push_back(x);
                double d = (x-x0).norm();
                if (d>d0)
                {
                        i0 = i;
                        d0 = d;
                }
        }
        a_point du = (this->closest_point(pts[i0])-x0).normalise();
        a_point dv = cross(this->normal(),du).normalise();
        vtkPoints * vpts = vtkPoints::New();
        for (int i=0; i< pts.size(); i++)
        {
                a_point x = pts_proj[i];
                double u = x*du;
                double v = x*dv;
                vpts->InsertNextPoint(u,v,0);
        }
        vtkCellArray * vertices = vtkCellArray::New();
        for (vtkIdType k = 0; k < pts.size(); k++)
                vertices->InsertNextCell(1,&k);
        vtkPolyData * data = vtkPolyData::New();
        data->SetPoints(vpts);
        vpts->Delete();
        data->SetVerts(vertices);
        vertices->Delete();
        vtkDelaunay2D * del = vtkDelaunay2D::New();
        del->SetInputData(data);
        data->Delete();
        del->SetTolerance(0.001);
        del->Update();
        int n_pts =  vpts->GetNumberOfPoints();
        std::cout << n_pts << std::endl;
        for (int k = 0; k < n_pts; k++)
                std::cout <<  pts_proj[k] << std::endl;
        vtkCellArray * triangles;
        triangles = del->GetOutput()->GetPolys();
        int n_tri = del->GetOutput()->GetNumberOfPolys();
        std::cout << n_tri << std::endl;
        triangles->InitTraversal();
        for (int k = 0; k < n_tri; k++)
        {
                vtkIdType npt;
                const vtkIdType * ref_pt = nullptr;
                triangles->GetNextCell(npt,ref_pt);
                std::cout << ref_pt[0] << '\t' << ref_pt[1] << '\t' << ref_pt[2] << std::endl;
        }
}