a_shape_circle.cxx

Go to the documentation of this file.

/*
         Copyright 2009-2019 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_circle.h"
#include <math.h>
#include <time.h>
#include <vnl/vnl_least_squares_function.h>
 
//***************************************************************************
class a_shape_circle_function : public vnl_least_squares_function
{
        public:
                a_shape_circle_function( a_shape_circle& shape,
                                const std::vector<a_point> & cloud) :  
                        vnl_least_squares_function(7, cloud.size(), no_gradient),       
                        shape_(shape),
                        cloud_(cloud) {};
                void f(const vnl_vector<double>& x, vnl_vector<double>& fx);
        protected:
                a_shape_circle &               shape_;
                const std::vector<a_point> &    cloud_;
};
//---------------------------------------------------------------------------
void a_shape_circle_function::f(const vnl_vector<double>& x, vnl_vector<double>& fx)
{
        a_point dir(x[0],x[1],x[2]);
        a_point orig(x[3],x[4],x[5]);
        dir.normalise();
        shape_.dir(dir);
        //      shape_.orig(orig-(dir*orig)*dir);
        shape_.orig(orig);
        shape_.radius(x[6]);
        for (int i = 0; i< fx.size(); i++)
                fx[i] = shape_.dist_point(cloud_[i]);
}
//***************************************************************************
//---------------------------------------------------------------------------
const std::string a_shape_circle::help()
{
  std::ostringstream o;
  o << "***************" << std::endl;
  o << "a_shape_circle:" << std::endl;    
  o << "***************" << std::endl;
  o << "This is a 'circle' fitting class" << std::endl;
        o << std::endl;
  o << a_shape::help();
  return o.str();
}
//---------------------------------------------------------------------------
void a_shape_circle::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 dir = cross(d1,d2);
        if (dir.norm()==0) return;
        this->dir(dir.normalise());
        a_point ce = circle_centre(p1,p2,p3);
        this->orig(ce);
        this->radius(((p1-ce).norm()+(p2-ce).norm()+(p3-ce).norm())/3.);
}
//---------------------------------------------------------------------------
a_point a_shape_circle::closest_point(const a_point pt) const
{
        a_point d = pt-this->orig();
        a_point pv = this->dir()*(this->dir()*d);
        double dv = pv.norm();
        a_point ph = (d-pv).normalise();
        return (this->orig()+ph*this->radius());
}
//---------------------------------------------------------------------------
double a_shape_circle::dist_point(const a_point pt) const
{
        a_point d = pt-this->orig();
        a_point pv = this->dir()*(this->dir()*d);
        double dv = pv.norm();
        a_point ph = d-pv;
        double dh = ph.norm()-this->radius();
        return sqrt(dh*dh+dv*dv);
}
//---------------------------------------------------------------------------
void a_shape_circle::random_hint(const std::vector<a_point>& pts)
{
        //srand(time(NULL));
        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_circle::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 (verbose_)
        {
                std::cerr << "current estimate:" << std::endl;
                std::cerr << "c: " << this->orig() << std::endl;
                std::cerr << "n: " << this->dir() << std::endl;
                std::cerr << "r: " << this->radius() << std::endl;
        }
        if (nl==1)
        {
                a_shape_circle_function f(*this,pts2);
                a_shape::fit_cloud(pts2,f);
        }
        pts = pts2;
}
//---------------------------------------------------------------------------
void a_shape_circle::export_points(const unsigned int nseg, const std::vector<a_point>& pts) const
{
        const double pi2 = 2*3.14159265358979;
        a_point ce = this->orig();
        a_point nz = this->dir();
        a_point p, nx, ny;
        if (fabs(nz.x())>fabs(nz.y()))
                p = a_point(0.,1.,0.);
        else
                p = a_point(1.,0.,0.);
        nx = cross(p,nz).normalise()*this->radius();
        ny = cross(nz,nx).normalise()*this->radius();
        std::cout << nseg << std::endl;
        for (int i = 0; i<nseg; i++)
        {
                double a = i*pi2/nseg;
                double c = cos(a);
                double s = sin(a);
                a_point n = ce+nx*c+ny*s;
                std::cout << n << std::endl;
        }
}
//---------------------------------------------------------------------------
void a_shape_circle::export_lines(const unsigned int nseg, const std::vector<a_point>& pts) const
{
        const double pi2 = 2*3.14159265358979;
        a_point ce = this->orig();
        a_point nz = this->dir();
        a_point p, nx, ny;
        if (fabs(nz.x())>fabs(nz.y()))
                p = a_point(0.,1.,0.);
        else
                p = a_point(1.,0.,0.);
        nx = cross(p,nz).normalise()*this->radius();
        ny = cross(nz,nx).normalise()*this->radius();
        a_point n;
        a_point n0 = ce+nx;
        std::cout << nseg << std::endl;
        for (int i = 1; i<nseg; i++)
        {
                double a = i*pi2/nseg;
                double c = cos(a);
                double s = sin(a);
                n = ce+nx*c+ny*s;
                std::cout << n0 << " " << n << std::endl;
                n0 = n;
        }
        n0 = ce+nx;
        std::cout << n << " " << n0 << std::endl;
}