a_shape_sphere.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_sphere.h"
#include <math.h>
#include <time.h>
#include <vnl/vnl_least_squares_function.h>
#include "a_mat_sq.h"
 
//***************************************************************************
class a_shape_sphere_function : public vnl_least_squares_function
{
        public:
                a_shape_sphere_function( a_shape_sphere& 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_sphere &               shape_;
                const std::vector<a_point> &    cloud_;
};
//---------------------------------------------------------------------------
void a_shape_sphere_function::f(const vnl_vector<double>& x, vnl_vector<double>& fx)
{
        a_point center(x[0],x[1],x[2]);
        shape_.radius(x[3]);
        for (int i = 0; i< fx.size(); i++)
                fx[i] = shape_.dist_point(cloud_[i]);
}
//***************************************************************************
//---------------------------------------------------------------------------
const std::string a_shape_sphere::help()
{    
  std::ostringstream o;
  o << "***************" << std::endl;
  o << "a_shape_sphere:" << std::endl;
  o << "***************" << std::endl;
  o << "This is a 'sphere' fitting class" << std::endl;
  o << std::endl;                       
  o << a_shape::help();                                                        
  return o.str();                 
}
//---------------------------------------------------------------------------
void a_shape_sphere::p4pts(const a_point p1, const a_point p2, const a_point p3, const a_point p4)
{
        //see http://paulbourke.net/geometry/circlesphere/ 
        a_mat_sq m(4);
        m(0,0)=p1.x();  m(0,1)=p1.y();  m(0,2)=p1.z();  m(0,3)=1.;
        m(1,0)=p2.x();  m(1,1)=p2.y();  m(1,2)=p2.z();  m(1,3)=1.;
        m(2,0)=p3.x();  m(2,1)=p3.y();  m(2,2)=p3.z();  m(2,3)=1.;
        m(3,0)=p4.x();  m(3,1)=p4.y();  m(3,2)=p4.z();  m(3,3)=1.;
        double M11 = m.determinant();
        double t1 = p1.x()*p1.x()+p1.y()*p1.y()+p1.z()*p1.z();  
        double t2 = p2.x()*p2.x()+p2.y()*p2.y()+p2.z()*p2.z();  
        double t3 = p3.x()*p3.x()+p3.y()*p3.y()+p3.z()*p3.z();  
        double t4 = p4.x()*p4.x()+p4.y()*p4.y()+p4.z()*p4.z();
        m(0,0)= t1; m(1,0)= t2; m(2,0)= t3; m(3,0)= t4;
        double M12 = m.determinant();
        m(0,1)= p1.x(); m(1,1)= p2.x(); m(2,1)= p3.x(); m(3,1)= p4.x();
        double M13 = m.determinant();
        m(0,2)= p1.y(); m(1,2)= p2.y(); m(2,2)= p3.y(); m(3,2)= p4.y();
        double M14 = m.determinant();
        m(0,3)= p1.z(); m(1,3)= p2.z(); m(2,3)= p3.z(); m(3,3)= p4.z();
        double M15 = m.determinant();
        a_point centre(.5*M12/M11,-.5*M13/M11,.5*M14/M11);
        this->center(centre);
        this->radius(sqrt(centre.sumsq()-M15/M11));     
}
//---------------------------------------------------------------------------
a_point a_shape_sphere::closest_point(const a_point pt) const
{
        a_point d = (pt-this->center()).normalise();
        return this->center()+d*this->radius();
}
//---------------------------------------------------------------------------
double a_shape_sphere::dist_point(const a_point pt) const
{
        a_point cp = this->closest_point(pt);
        return pt.dist(cp);
}
//---------------------------------------------------------------------------
void a_shape_sphere::random_hint(const std::vector<a_point>& pts)
{
        int n = pts.size();
        int i1 = rand()%n;
        int i2,i3,i4;
        do
        {
                i2 = rand()%n;
        } while (i1==i2);
        do
        {
                i3 = rand()%n;
        } while ((i1==i3)||(i2==i3));
        do
        {
                i4 = rand()%n;
        } while ((i1==i4)||(i2==i4)||(i3==i4));
        a_point p1 = pts[i1];
        a_point p2 = pts[i2];
        a_point p3 = pts[i3];
        a_point p4 = pts[i4];
        //      std::cerr << p1 << " * " << p2 << " * " << p3 << " * " << p4 << std::endl; 
        this->p4pts(p1,p2,p3,p4);
}
//---------------------------------------------------------------------------
void a_shape_sphere::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_sphere_function f(*this,pts2);
                a_shape::fit_cloud(pts2,f);
        }
        pts = pts2;
}
//---------------------------------------------------------------------------
void a_shape_sphere::export_points(const unsigned int nseg, const std::vector<a_point>& pts) const
{
        //see http://www.math.niu.edu/~rusin/known-math/95/sphere.faq
        const double pi = 3.14159265358979;
        a_point ce = this->center();
        double r = this->radius();
        std::vector<a_point> points;
        points.push_back(ce+a_point(0.,0.,r));
        points.push_back(ce+a_point(0.,0.,-r));
        int K = int((sqrt(pi/4.*nseg))+.5);
        for (int i=0; i<K; i++)
        {
                double ui=pi/2-i*(pi/K);
                double li = 2*pi*cos(ui);
                int Ki = int(li/(pi/K)+.5);
                for (int j=0; j<Ki; j++)
                {
                        double vi = 2*pi*j/Ki;
                        a_point dir(cos(ui)*cos(vi),cos(ui)*sin(vi),sin(ui));
                        points.push_back(ce+r*dir);
                }
        }
        std::cout << points.size() << std::endl;
        for (int i=0; i< points.size(); i++)
                std::cout << points[i] << std::endl;
        points.clear();
}
//---------------------------------------------------------------------------
int right(int ref, int max)
{
        if (ref==max-1)
                return 0;
        else
                return ref+1;
}
//---------------------------------------------------------------------------
void a_shape_sphere::export_triangles(const unsigned int nseg, const std::vector<a_point>& pts) const
{
        const double pi = 3.14159265358979;
        a_point ce = this->center();
        double r = this->radius();
        std::vector<std::vector<a_point> *> points;
        auto pt_line = new std::vector<a_point>;
        pt_line->push_back(ce+a_point(0.,0.,r));
        points.push_back(pt_line);
        auto pt_line2 = new std::vector<a_point>;
        pt_line2->push_back(ce+a_point(0.,0.,-r));
        points.push_back(pt_line2);
        int K = int(sqrt(pi/8.*nseg)+.5);
        K += 1-K%2;
        std::cerr << "K = " << K << std::endl;
        int npt_tot = 1;
        int npt_i;
        for (int i=1; i<=K/2; i++)
        {
                npt_i = 0;
                pt_line = new std::vector<a_point>;
                pt_line2 = new std::vector<a_point>;
                double ui=pi/2-i*(pi/K);
                double li = 2*pi*cos(ui);
                int Ki = int(li/(pi/K)+.5);
                for (int j=0; j<Ki; j++)
                {
                        double vi = 2*pi*j/Ki;
                        a_point dir(cos(ui)*sin(vi),cos(ui)*cos(vi),sin(ui));
                        pt_line->push_back(ce+r*dir);
                        a_point dir2(cos(ui)*sin(vi),cos(ui)*cos(vi),-sin(ui));
                        pt_line2->push_back(ce+r*dir2);
                        npt_i++;
                }
                npt_tot += npt_i;
                points.push_back(pt_line);
                points.push_back(pt_line2);
        }
        std::cout << npt_tot*2 << std::endl;
        for (int i=0; i<points.size();i++)
        {
                pt_line = points[i];
                for (int j=0; j<pt_line->size();j++)
                        std::cout << (*pt_line)[j] << std::endl;
        }
        int ref1 = npt_tot*2;
        int ref2 = ref1-npt_i*2;
        std::vector<int> refs;
        for (int i=points.size()-1; i>1; i--)
        {
                std::vector<a_point> * pt1, * pt2;
                pt1 = points[i];
                pt2 = points[i-2];
                int l1 = pt1->size();
                int l2 = pt2->size();
                ref1 -= l1;
                ref2 -= l2;
                int j2 = 0;
                for (int j=0; j<l1; j++)
                {
                        //iterate along the segments of the long ring
                        double x1a, x1b, x2a, x2b;
                        x1a = j/double(l1);
                        x1b = right(j,l1)/double(l1);
                        x2a = j2/double(l2);
                        x2b = right(j2,l2)/double(l2);
                        double d1 = dist((*pt1)[j],(*pt2)[right(j2,l2)]);
                        double d2 = dist((*pt1)[right(j,l1)],(*pt2)[j2]);
                        int r[3];
                        if (d1<d2)
                        {
                                refs.push_back(ref1+j);
                                if (i%2==0)
                                {
                                        refs.push_back(ref1+right(j,l1));
                                        refs.push_back(ref2+right(j2,l2));
                                } else
                                {
                                        refs.push_back(ref2+right(j2,l2));
                                        refs.push_back(ref1+right(j,l1));
                                }
                                refs.push_back(ref1+j);
                                if (i%2==0)
                                {
                                        refs.push_back(ref2+right(j2,l2));
                                        refs.push_back(ref2+j2);
                                } else
                                {
                                        refs.push_back(ref2+j2);
                                        refs.push_back(ref2+right(j2,l2));
                                }
                                j2 = right(j2,l2);
                        } else
                        {
                                refs.push_back(ref1+j);
                                if (i%2==0)
                                {
                                        refs.push_back(ref1+right(j,l1));
                                        refs.push_back(ref2+j2);
                                } else
                                {
                                        refs.push_back(ref2+j2);
                                        refs.push_back(ref1+right(j,l1));
                                }
                        }
                }
        }
        std::vector<a_point> * pt1 = points[points.size()-1];
        std::vector<a_point> * pt2 = points[points.size()-2];
        int l = pt1->size();
        ref1 = npt_tot*2-l;
        ref2 = ref1-l;
        for (int i=0; i<l; i++)
        {
                refs.push_back(ref1+i);
                refs.push_back(ref1+right(i,l));
                refs.push_back(ref2+i);
                refs.push_back(ref1+right(i,l));
                refs.push_back(ref2+right(i,l));
                refs.push_back(ref2+i);
        }
        std::cout << refs.size()/3 <<std::endl;
        for (int i=0; i< refs.size(); i+=3)
                std::cout << refs[i] << " " << refs[i+1] << " " << refs[i+2] << std::endl;
}