// Reconstruction - A tool for building cad models from meshes
// Copyright (C) 2010-2026 Eric Bechet, Borhen Louhichi
//
// See the LICENSE file for license information and contributions.
// Please report all bugs and problems to <bechet@cadxfem.org>.


#include "RecPrimitive.h"
#include "OCCIncludes.h"
#include "gp_Vec.hxx"
#include "math_Vector.hxx"
#include "math_Matrix.hxx"
#include "math_Gauss.hxx"

std::vector<double> RecPrimitive::Plan()
{

  std::vector<double> temp;

  int numPoints =  pts.size();
  std::vector<gp_Vec> points;

  for (int i0 = 0; i0 < numPoints; i0++)
  {
    points.push_back(gp_Vec(pts[i0].x(),pts[i0].y(),pts[i0].z()));

  }



  int maxIteration = 30;

  math_Vector P0(1,6);

  P0(1) = 0.0;
  P0(2) = 50.0;
  P0(3) = 0.0;
  P0(4) = 1.0;
  P0(5) = 1.0;
  P0(6) = 1.0;

  gp_Vec Center(P0(1),P0(2),P0(3));
  gp_Vec Axe(P0(4),P0(5),P0(6));

  double Lamda = 0.0001;

  for (int ii0 = 0; ii0 < maxIteration; ++ii0)
  {

    Lamda = Lamda*0.04;

    Axe.Normalize();

    math_Matrix F0(1, numPoints, 1, 6, 0.0);

    for (int ii1 = 1; ii1 <= numPoints; ++ii1)
    {

      F0(ii1,1) = -Axe.X();
      F0(ii1,2) = -Axe.Y();
      F0(ii1,3) = -Axe.Z();
      F0(ii1,4) = points[ii1-1].X()-Center.X();
      F0(ii1,5) = points[ii1-1].Y()-Center.Y();
      F0(ii1,6) = points[ii1-1].Z()-Center.Z();


    }

    math_Matrix U(1, 6, 1, 6, 0.0);

    math_Matrix F0T = F0.Transposed();

    U=F0T*F0;

    math_Vector dP0(1,numPoints);

    for (int ii1 = 1; ii1 <= numPoints; ++ii1)
    {
      dP0(ii1) = ((points[ii1-1]-Center) *Axe);

    }

    math_Vector V(1,6,0.0);

    V = F0T*dP0;

    double J0 = 0.0;

    for (int ii1 = 1; ii1 <= numPoints; ++ii1)
    {

      J0 = J0 + pow(dP0(ii1),2);
    }

    double Jnew = 0.0;

    math_Vector Pnew(1,6);
    gp_Vec Center_new;
    gp_Vec Axe_new;
    double Rayon_new= 0.0;

    int iteration = 0;

    do
    {

      Lamda = Lamda*10;
      math_Matrix H(1, 6, 1, 6, 0.0);
      math_Matrix U0(1,6,1,6,0.0);

      for (int ii1 = 1; ii1 <= 6; ++ii1)
      {
        U0(ii1,ii1) = Lamda* (1+U(ii1,ii1));
      }

      H = U+U0;

      math_Vector X(1,6,0.0);
      math_Gauss M(H, 1.0e-20);

      M.Solve((-1.0*V),X);
      Pnew = P0+X;

      Center_new = gp_Vec(Pnew(1),Pnew(2),Pnew(3));
      Axe_new = gp_Vec(Pnew(4),Pnew(5),Pnew(6));

      for (int ii1 = 1; ii1 <= numPoints; ++ii1)
      {
        Jnew = Jnew + pow(((points[ii1-1]-Center_new) *Axe_new),2);
      }


      iteration = iteration + 1;

    }
    while ((Jnew > J0) /*&& (iteration !=6)*/);

    if (Jnew<J0)
    {

      P0 = Pnew;

      Center = Center_new;
      Axe = Axe_new;
      Axe.Normalize();

    }



  }

  temp.push_back(Center.X());
  temp.push_back(Center.Y());
  temp.push_back(Center.Z());
  temp.push_back(Axe.X());
  temp.push_back(Axe.Y());
  temp.push_back(Axe.Z());


  return temp;



}


std::vector<double> RecPrimitive::Line()
{



}




std::vector<double> RecPrimitive::Sphere()
{
  std::vector<double> temp;

  int numPoints =  pts.size();
  std::vector<gp_Vec> points;

  for (int i0 = 0; i0 < numPoints; i0++)
  {
    points.push_back(gp_Vec(pts[i0].x(),pts[i0].y(),pts[i0].z()));

  }

  int maxIteration = 30;

  math_Vector P0(1,4);
  P0(1) = 20.0;
  P0(2) = 20.0;
  P0(3) = 20.0;
  P0(4) = 20.0;

  gp_Vec Center(P0(1),P0(2),P0(3));
  double Rayon = P0(4);

  double Lamda = 0.0001;

  for (int ii0 = 0; ii0 < maxIteration; ++ii0)
  {

    Lamda = Lamda*0.04;

    math_Matrix F0(1, numPoints, 1, 4, 0.0);

    for (int ii1 = 1; ii1 <= numPoints; ++ii1)
    {

      F0(ii1,1) = (-1.0 * (points[ii1-1].X()-Center.X())) / ((points[ii1-1]-Center).Magnitude());
      F0(ii1,2) = (-1.0 * (points[ii1-1].Y()-Center.Y())) / ((points[ii1-1]-Center).Magnitude());
      F0(ii1,3) = (-1.0 * (points[ii1-1].Z()-Center.Z())) / ((points[ii1-1]-Center).Magnitude());
      F0(ii1,4) = (-1.0);

    }

    math_Matrix U(1, 4, 1, 4, 0.0);
    math_Matrix F0T = F0.Transposed();
    U=F0T*F0;
    math_Vector dP0(1,numPoints);

    for (int ii1 = 1; ii1 <= numPoints; ++ii1)
    {
      dP0(ii1) = (points[ii1]-Center).Magnitude() - Rayon;
    }

    math_Vector V(1,4,0.0);
    V = F0T*dP0;

    double J0 = 0.0;
    for (int ii1 = 1; ii1 <= numPoints; ++ii1)
    {
      J0 = J0 + ((points[ii1-1]-Center).Magnitude() - Rayon) * ((points[ii1-1]-Center).Magnitude() - Rayon);
    }

    double Jnew = 0.0;
    math_Vector Pnew(1,4);
    gp_Vec Center_new;
    double Rayon_new= 0.0;

    int iteration = 0;

    do
    {
      Lamda = Lamda*10;
      math_Matrix H(1, 4, 1, 4, 0.0);
      math_Matrix U0(1,4,1,4,0.0);
      for (int ii1 = 1; ii1 <= 4; ++ii1)
      {
        U0(ii1,ii1) = Lamda* (1+U(ii1,ii1));
      }

      H = U+U0;
      math_Vector X(1,4,0.0);
      math_Gauss M(H, 1.0e-20);
      M.Solve((-1.0*V),X);
      Pnew = P0+X;
      Center_new = gp_Vec(Pnew(1),Pnew(2),Pnew(3));
      Rayon_new = Pnew(4);

      for (int ii1 = 1; ii1 <= numPoints; ++ii1)
      {
        Jnew = Jnew + ((points[ii1-1]-Center_new).Magnitude()-Rayon_new) * ((points[ii1-1]-Center_new).Magnitude() - Rayon_new);
      }

      iteration = iteration + 1;

    }
    while ((Jnew > J0) && (iteration !=6));

    if (Jnew<J0)
    {
      P0 = Pnew;
      Center = Center_new;
      Rayon = Rayon_new;

    }
  }

  temp.push_back(Center.X());
  temp.push_back(Center.Y());
  temp.push_back(Center.Z());
  temp.push_back(Rayon);


  return temp;
}







std::vector<double> RecPrimitive::Cylindre()
{

  std::vector<double> temp;

  int numPoints =  pts.size();
  std::vector<gp_Vec> points;

  for (int i0 = 0; i0 < numPoints; i0++)
  {
    points.push_back(gp_Vec(pts[i0].x(),pts[i0].y(),pts[i0].z()));

  }



  int maxIteration = 50;

  math_Vector P0(1,7);

  P0(1) = 0.0;
  P0(2) = 0.0;
  P0(3) = 0.0;
  P0(4) = 0.20;
  P0(5) = 1.0;
  P0(6) = 0.0;
  P0(7) = 20.0;

  gp_Vec Center(P0(1),P0(2),P0(3));
  gp_Vec Axe(P0(4),P0(5),P0(6));
  double Rayon = P0(7);


  double Lamda = 0.0001;

  for (int ii0 = 0; ii0 < maxIteration; ++ii0)
  {

    Lamda = Lamda*0.04;

    Axe.Normalize();

    math_Matrix F0(1, numPoints, 1, 7, 0.0);

    for (int ii1 = 1; ii1 <= numPoints; ++ii1)
    {
      double fi = sqrt(pow(points[ii1-1].X()-Center.X(),2) +pow(points[ii1-1].Y()-Center.Y(),2) +pow(points[ii1-1].Z()-Center.Z(),2) - pow(Axe.X() * (points[ii1-1].X()-Center.X()) +Axe.Y() * (points[ii1-1].Y()-Center.Y()) +Axe.Z() * (points[ii1-1].Z()-Center.Z()),2));

      F0(ii1,1) = 0.5* (1/fi) * ((-2.0* (points[ii1-1].X()-Center.X())) + 2* Axe.X() * (Axe.X() * (points[ii1-1].X()-Center.X()) + Axe.Y() * (points[ii1-1].Y()-Center.Y()) + Axe.Z() * (points[ii1-1].Z()-Center.Z())));
      F0(ii1,2) = 0.5* (1/fi) * ((-2.0* (points[ii1-1].Y()-Center.Y())) + 2* Axe.Y() * (Axe.X() * (points[ii1-1].X()-Center.X()) + Axe.Y() * (points[ii1-1].Y()-Center.Y()) + Axe.Z() * (points[ii1-1].Z()-Center.Z())));
      F0(ii1,3) = 0.5* (1/fi) * ((-2.0* (points[ii1-1].Z()-Center.Z())) + 2* Axe.Z() * (Axe.X() * (points[ii1-1].X()-Center.X()) + Axe.Y() * (points[ii1-1].Y()-Center.Y()) + Axe.Z() * (points[ii1-1].Z()-Center.Z())));

      F0(ii1,4) = 0.5* (1/fi) * (-2.0* (points[ii1-1].X()-Center.X()) * (Axe.X() * (points[ii1-1].X()-Center.X()) + Axe.Y() * (points[ii1-1].Y()-Center.Y()) + Axe.Z() * (points[ii1-1].Z()-Center.Z())));
      F0(ii1,5) = 0.5* (1/fi) * (-2.0* (points[ii1-1].Y()-Center.Y()) * (Axe.X() * (points[ii1-1].X()-Center.X()) + Axe.Y() * (points[ii1-1].Y()-Center.Y()) + Axe.Z() * (points[ii1-1].Z()-Center.Z())));
      F0(ii1,6) = 0.5* (1/fi) * (-2.0* (points[ii1-1].Z()-Center.Z()) * (Axe.X() * (points[ii1-1].X()-Center.X()) + Axe.Y() * (points[ii1-1].Y()-Center.Y()) + Axe.Z() * (points[ii1-1].Z()-Center.Z())));

      F0(ii1,7) = -1.0;

    }

    math_Matrix U(1, 7, 1, 7, 0.0);

    math_Matrix F0T = F0.Transposed();

    U=F0T*F0;

    math_Vector dP0(1,numPoints);

    for (int ii1 = 1; ii1 <= numPoints; ++ii1)
    {

      double fi = sqrt(pow(points[ii1-1].X()-Center.X(),2) +pow(points[ii1-1].Y()-Center.Y(),2) +pow(points[ii1-1].Z()-Center.Z(),2) - pow(Axe.X() * (points[ii1-1].X()-Center.X()) +Axe.Y() * (points[ii1-1].Y()-Center.Y()) +Axe.Z() * (points[ii1-1].Z()-Center.Z()),2));
      dP0(ii1) = fi - Rayon;

    }

    math_Vector V(1,7,0.0);

    V = F0T*dP0;

    double J0 = 0.0;

    for (int ii1 = 1; ii1 <= numPoints; ++ii1)
    {

      J0 = J0 + pow(dP0(ii1),2);

    }

    double Jnew = 0.0;

    math_Vector Pnew(1,7);
    gp_Vec Center_new;
    gp_Vec Axe_new;
    double Rayon_new= 0.0;

    int iteration = 0;

    do
    {

      Lamda = Lamda*10;
      math_Matrix H(1, 7, 1, 7, 0.0);
      math_Matrix U0(1,7,1,7,0.0);

      for (int ii1 = 1; ii1 <= 7; ++ii1)
      {
        U0(ii1,ii1) = Lamda* (1+U(ii1,ii1));
      }

      H = U+U0;

      math_Vector X(1,7,0.0);
      math_Gauss M(H, 1.0e-20);

      M.Solve((-1.0*V),X);
      Pnew = P0+X;

      Center_new = gp_Vec(Pnew(1),Pnew(2),Pnew(3));
      Axe_new = gp_Vec(Pnew(4),Pnew(5),Pnew(6));
      Rayon_new = Pnew(7);

      for (int ii1 = 1; ii1 <= numPoints; ++ii1)
      {
        double fi_new = sqrt(pow(points[ii1-1].X()-Center_new.X(),2) +pow(points[ii1-1].Y()-Center_new.Y(),2) +pow(points[ii1-1].Z()-Center_new.Z(),2) - pow(Axe_new.X() * (points[ii1-1].X()-Center_new.X()) +Axe_new.Y() * (points[ii1-1].Y()-Center_new.Y()) +Axe_new.Z() * (points[ii1-1].Z()-Center_new.Z()),2));
        Jnew = Jnew + pow((fi_new-Rayon_new),2);
      }


      iteration = iteration + 1;

    }
    while ((Jnew > J0) /*&& (iteration !=6)*/);


    if (Jnew<J0)
    {

      P0 = Pnew;

      Center = Center_new;
      Axe = Axe_new;
      Rayon = Rayon_new;

      Axe.Normalize();

    }

  }

  temp.push_back(Center.X());
  temp.push_back(Center.Y());
  temp.push_back(Center.Z());
  temp.push_back(Axe.X());
  temp.push_back(Axe.Y());
  temp.push_back(Axe.Z());
  temp.push_back(Rayon);


  return temp;
}


std::vector<double> RecPrimitive::Cone()
{

  std::vector<double> temp;

  int numPoints =  pts.size();
  std::vector<gp_Vec> points;

  for (int i0 = 0; i0 < numPoints; i0++)
  {
    points.push_back(gp_Vec(pts[i0].x(),pts[i0].y(),pts[i0].z()));

  }

  int maxIteration =10;

  math_Vector P0(1,8);

  P0(1) = 0;
  P0(2) = 10;
  P0(3) = 0;
  P0(4) = 0;
  P0(5) = 1;
  P0(6) = 0;
  P0(7) = 200;
  P0(8) = atan(0.5);

  gp_Vec Center(P0(1),P0(2),P0(3));
  gp_Vec Axe(P0(4),P0(5),P0(6));
  double dist = P0(7);
  double Angle = P0(8);


  double Lamda = 0.0001;

  for (int ii0 = 0; ii0 < maxIteration; ++ii0)
  {

    Lamda = Lamda*0.04;

    if (ii0 == 0)
    {
      Axe.Normalize();
      Angle = fmod(Angle, 2*PI);

      if (Angle>PI)
      {
        Angle = fmod(Angle, PI);
        Axe = -Axe;
      }

      if (Angle> (PI/2)) Angle = PI - Angle;

      if (dist<0.0)
      {
        dist = -dist;
        Axe = -Axe;
      }
    }

    math_Matrix F0(1, numPoints, 1, 8, 0.0);
    math_Vector dP0(1,numPoints);

    for (int ii1 = 1; ii1 <= numPoints; ++ii1)
    {
      double fi = sqrt(pow(points[ii1-1].X()-Center.X(),2) +pow(points[ii1-1].Y()-Center.Y(),2) +pow(points[ii1-1].Z()-Center.Z(),2) - pow(Axe.X() * (points[ii1-1].X()-Center.X()) +Axe.Y() * (points[ii1-1].Y()-Center.Y()) +Axe.Z() * (points[ii1-1].Z()-Center.Z()),2));
      double gi = Axe.X() * (points[ii1-1].X()-Center.X()) +Axe.Y() * (points[ii1-1].Y()-Center.Y()) +Axe.Z() * (points[ii1-1].Z()-Center.Z());

      double fix = 0.5* (1/fi) * ((-2.0* (points[ii1-1].X()-Center.X())) + 2* Axe.X() * (Axe.X() * (points[ii1-1].X()-Center.X()) + Axe.Y() * (points[ii1-1].Y()-Center.Y()) + Axe.Z() * (points[ii1-1].Z()-Center.Z())));
      double fiy = 0.5* (1/fi) * ((-2.0* (points[ii1-1].Y()-Center.Y())) + 2* Axe.Y() * (Axe.X() * (points[ii1-1].X()-Center.X()) + Axe.Y() * (points[ii1-1].Y()-Center.Y()) + Axe.Z() * (points[ii1-1].Z()-Center.Z())));
      double fiz = 0.5* (1/fi) * ((-2.0* (points[ii1-1].Z()-Center.Z())) + 2* Axe.Z() * (Axe.X() * (points[ii1-1].X()-Center.X()) + Axe.Y() * (points[ii1-1].Y()-Center.Y()) + Axe.Z() * (points[ii1-1].Z()-Center.Z())));

      double fia = 0.5* (1/fi) * (-2.0* (points[ii1-1].X()-Center.X()) * (Axe.X() * (points[ii1-1].X()-Center.X()) + Axe.Y() * (points[ii1-1].Y()-Center.Y()) + Axe.Z() * (points[ii1-1].Z()-Center.Z())));
      double fib = 0.5* (1/fi) * (-2.0* (points[ii1-1].Y()-Center.Y()) * (Axe.X() * (points[ii1-1].X()-Center.X()) + Axe.Y() * (points[ii1-1].Y()-Center.Y()) + Axe.Z() * (points[ii1-1].Z()-Center.Z())));
      double fic = 0.5* (1/fi) * (-2.0* (points[ii1-1].Z()-Center.Z()) * (Axe.X() * (points[ii1-1].X()-Center.X()) + Axe.Y() * (points[ii1-1].Y()-Center.Y()) + Axe.Z() * (points[ii1-1].Z()-Center.Z())));

      double gix = - Axe.X();
      double giy = - Axe.Y();
      double giz = - Axe.Z();

      double gia = points[ii1-1].X()-Center.X();
      double gib = points[ii1-1].Y()-Center.Y();
      double gic = points[ii1-1].Z()-Center.Z();

      double di = fi*cos(Angle) + gi*sin(Angle) - dist;

      F0(ii1,1) = fix*cos(Angle) + gix*sin(Angle);
      F0(ii1,2) = fiy*cos(Angle) + giy*sin(Angle);
      F0(ii1,3) = fiz*cos(Angle) + giz*sin(Angle);

      F0(ii1,4) = fia*cos(Angle) + gia*sin(Angle);
      F0(ii1,5) = fib*cos(Angle) + gib*sin(Angle);
      F0(ii1,6) = fic*cos(Angle) + gic*sin(Angle);

      F0(ii1,7) =  -1;
      F0(ii1,8) = -fi*sin(Angle) + gi*cos(Angle);

      dP0(ii1) = di;

    }

    math_Matrix U(1, 8, 1, 8, 0.0);
    math_Matrix F0T = F0.Transposed();
    U=F0T*F0;
    math_Vector V(1,8,0.0);
    V = F0T*dP0;

    double J0 = 0.0;

    for (int ii1 = 1; ii1 <= numPoints; ++ii1)
    {

      J0 = J0 + pow(dP0(ii1),2);

    }

    double Jnew = 0.0;

    math_Vector Pnew(1,8);
    gp_Vec Center_new;
    gp_Vec Axe_new;
    double dist_new= 0.0;
    double Angle_new= 0.0;


    int iteration = 0;

    do
    {

      Lamda = Lamda*10;
      math_Matrix H(1, 8, 1, 8, 0.0);
      math_Matrix U0(1,8,1,8,0.0);

      for (int ii1 = 1; ii1 <= 8; ++ii1)
      {
        U0(ii1,ii1) = Lamda* (1+U(ii1,ii1));

      }

      H = U+U0;

      math_Vector X(1,8,0.0);
      math_Gauss M(H, 1.0e-20);
      M.Solve((-1.0*V),X);

      Pnew = P0+X;

      Center_new = gp_Vec(Pnew(1),Pnew(2),Pnew(3));
      Axe_new = gp_Vec(Pnew(4),Pnew(5),Pnew(6));
      dist_new = Pnew(7);
      Angle_new = Pnew(8);

      for (int ii1 = 1; ii1 <= numPoints; ++ii1)
      {

        double fi_new = sqrt(pow(points[ii1-1].X()-Center_new.X(),2) +pow(points[ii1-1].Y()-Center_new.Y(),2) +pow(points[ii1-1].Z()-Center_new.Z(),2) - pow(Axe_new.X() * (points[ii1-1].X()-Center_new.X()) +Axe_new.Y() * (points[ii1-1].Y()-Center_new.Y()) +Axe_new.Z() * (points[ii1-1].Z()-Center_new.Z()),2));
        double gi_new = Axe_new.X() * (points[ii1-1].X()-Center_new.X()) +Axe_new.Y() * (points[ii1-1].Y()-Center_new.Y()) +Axe_new.Z() * (points[ii1-1].Z()-Center_new.Z());

        double di_new = fi_new*cos(Angle_new) + gi_new*sin(Angle_new) - dist_new;
        Jnew = Jnew + pow(di_new,2);

      }

      iteration = iteration + 1;

    }
    while ((Jnew > J0) /*&& (iteration !=6)*/);


    if (Jnew<J0)
    {

      P0 = Pnew;
      Center = Center_new;
      Axe = Axe_new;
      dist = dist_new;
      Angle = Angle_new;

      Axe.Normalize();

      Angle = fmod(Angle, 2*PI);

      if (Angle>PI)
      {
        Angle = fmod(Angle, PI);
        Axe = -Axe;
      }

      if (Angle> (PI/2)) Angle = PI - Angle;

      if (dist<0.0)
      {

        dist = -dist;
        Axe = -Axe;

      }
    }





  }

  temp.push_back(Center.X());
  temp.push_back(Center.Y());
  temp.push_back(Center.Z());
  temp.push_back(Axe.X());
  temp.push_back(Axe.Y());
  temp.push_back(Axe.Z());
  temp.push_back(dist);
  temp.push_back(Angle);


  return temp;


}



std::vector<double> RecPrimitive::Tore()
{

  std::vector<double> temp;

  int numPoints =  pts.size();
  std::vector<gp_Vec> points;

  for (int i0 = 0; i0 < numPoints; i0++)
  {
    points.push_back(gp_Vec(pts[i0].x(),pts[i0].y(),pts[i0].z()));

  }

  int maxIteration = 10;

  math_Vector P0(1,8);
  P0(1) = 1;
  P0(2) = 1;
  P0(3) = 1;
  P0(4) = 1;
  P0(5) = 1;
  P0(6) = 1;
  P0(7) = 100;
  P0(8) = 30;

  gp_Vec Center(P0(1),P0(2),P0(3));
  gp_Vec Axe(P0(4),P0(5),P0(6));
  double GRayon = P0(7);
  double PRayon = P0(8);


  double Lamda = 0.0001;

  for (int ii0 = 0; ii0 < maxIteration; ++ii0)
  {

    Lamda = Lamda*0.04;

    Axe.Normalize();

    math_Matrix F0(1, numPoints, 1, 8, 0.0);

    math_Vector dP0(1,numPoints);

    for (int ii1 = 1; ii1 <= numPoints; ++ii1)
    {
      double fi = sqrt(pow(points[ii1-1].X()-Center.X(),2) +pow(points[ii1-1].Y()-Center.Y(),2) +pow(points[ii1-1].Z()-Center.Z(),2) - pow(Axe.X() * (points[ii1-1].X()-Center.X()) +Axe.Y() * (points[ii1-1].Y()-Center.Y()) +Axe.Z() * (points[ii1-1].Z()-Center.Z()),2));
      double gi = Axe.X() * (points[ii1-1].X()-Center.X()) +Axe.Y() * (points[ii1-1].Y()-Center.Y()) +Axe.Z() * (points[ii1-1].Z()-Center.Z());

      double fix = 0.5* (1/fi) * ((-2.0* (points[ii1-1].X()-Center.X())) + 2* Axe.X() * (Axe.X() * (points[ii1-1].X()-Center.X()) + Axe.Y() * (points[ii1-1].Y()-Center.Y()) + Axe.Z() * (points[ii1-1].Z()-Center.Z())));
      double fiy = 0.5* (1/fi) * ((-2.0* (points[ii1-1].Y()-Center.Y())) + 2* Axe.Y() * (Axe.X() * (points[ii1-1].X()-Center.X()) + Axe.Y() * (points[ii1-1].Y()-Center.Y()) + Axe.Z() * (points[ii1-1].Z()-Center.Z())));
      double fiz = 0.5* (1/fi) * ((-2.0* (points[ii1-1].Z()-Center.Z())) + 2* Axe.Z() * (Axe.X() * (points[ii1-1].X()-Center.X()) + Axe.Y() * (points[ii1-1].Y()-Center.Y()) + Axe.Z() * (points[ii1-1].Z()-Center.Z())));

      double fia = 0.5* (1/fi) * (-2.0* (points[ii1-1].X()-Center.X()) * (Axe.X() * (points[ii1-1].X()-Center.X()) + Axe.Y() * (points[ii1-1].Y()-Center.Y()) + Axe.Z() * (points[ii1-1].Z()-Center.Z())));
      double fib = 0.5* (1/fi) * (-2.0* (points[ii1-1].Y()-Center.Y()) * (Axe.X() * (points[ii1-1].X()-Center.X()) + Axe.Y() * (points[ii1-1].Y()-Center.Y()) + Axe.Z() * (points[ii1-1].Z()-Center.Z())));
      double fic = 0.5* (1/fi) * (-2.0* (points[ii1-1].Z()-Center.Z()) * (Axe.X() * (points[ii1-1].X()-Center.X()) + Axe.Y() * (points[ii1-1].Y()-Center.Y()) + Axe.Z() * (points[ii1-1].Z()-Center.Z())));

      double gix = - Axe.X();
      double giy = - Axe.Y();
      double giz = - Axe.Z();

      double gia = points[ii1-1].X()-Center.X();
      double gib = points[ii1-1].Y()-Center.Y();
      double gic = points[ii1-1].Z()-Center.Z();

      double di = sqrt((gi*gi) + ((fi-GRayon) * (fi-GRayon)))-PRayon;

      F0(ii1,1) = (gi*gix + (fi-GRayon) *fix) / (di+PRayon);
      F0(ii1,2) = (gi*giy + (fi-GRayon) *fiy) / (di+PRayon);
      F0(ii1,3) = (gi*giz + (fi-GRayon) *fiz) / (di+PRayon);

      F0(ii1,4) = (gi*gia + (fi-GRayon) *fia) / (di+PRayon);
      F0(ii1,5) = (gi*gib + (fi-GRayon) *fib) / (di+PRayon);
      F0(ii1,6) = (gi*gic + (fi-GRayon) *fic) / (di+PRayon);

      F0(ii1,7) = (- (fi-GRayon)) / (di+PRayon);
      F0(ii1,8) = -1.0;

      dP0(ii1) = di;

    }


    math_Matrix U(1, 8, 1, 8, 0.0);
    math_Matrix F0T = F0.Transposed();
    U=F0T*F0;

    math_Vector V(1,8,0.0);

    V = F0T*dP0;

    double J0 = 0.0;

    for (int ii1 = 1; ii1 <= numPoints; ++ii1)
    {

      J0 = J0 + pow(dP0(ii1),2);

    }

    double Jnew = 0.0;

    math_Vector Pnew(1,8);
    gp_Vec Center_new;
    gp_Vec Axe_new;
    double GRayon_new= 0.0;
    double PRayon_new= 0.0;


    int iteration = 0;

    do
    {

      Lamda = Lamda*10;
      math_Matrix H(1, 8, 1, 8, 0.0);
      math_Matrix U0(1,8,1,8,0.0);

      for (int ii1 = 1; ii1 <= 8; ++ii1)
      {

        U0(ii1,ii1) = Lamda* (1+U(ii1,ii1));

      }

      H = U+U0;

      math_Vector X(1,8,0.0);
      math_Gauss M(H, 1.0e-20);

      M.Solve((-1.0*V),X);

      Pnew = P0+X;

      Center_new = gp_Vec(Pnew(1),Pnew(2),Pnew(3));
      Axe_new = gp_Vec(Pnew(4),Pnew(5),Pnew(6));
      GRayon_new = Pnew(7);
      PRayon_new = Pnew(8);

      for (int ii1 = 1; ii1 <= numPoints; ++ii1)
      {

        double fi_new = sqrt(pow(points[ii1-1].X()-Center_new.X(),2) +pow(points[ii1-1].Y()-Center_new.Y(),2) +pow(points[ii1-1].Z()-Center_new.Z(),2) - pow(Axe_new.X() * (points[ii1-1].X()-Center_new.X()) +Axe_new.Y() * (points[ii1-1].Y()-Center_new.Y()) +Axe_new.Z() * (points[ii1-1].Z()-Center_new.Z()),2));
        double gi_new = Axe_new.X() * (points[ii1-1].X()-Center_new.X()) +Axe_new.Y() * (points[ii1-1].Y()-Center_new.Y()) +Axe_new.Z() * (points[ii1-1].Z()-Center_new.Z());

        double di_new = sqrt((gi_new*gi_new) + ((fi_new-GRayon_new) * (fi_new-GRayon_new)))-PRayon_new;

        Jnew = Jnew + pow(di_new,2);

      }

      iteration = iteration + 1;

    }
    while ((Jnew > J0) && (iteration !=6));

    if (Jnew<J0)
    {

      P0 = Pnew;
      Center = Center_new;
      Axe = Axe_new;
      GRayon = GRayon_new;
      PRayon = PRayon_new;

      Axe.Normalize();

    }

  }

  temp.push_back(Center.X());
  temp.push_back(Center.Y());
  temp.push_back(Center.Z());
  temp.push_back(Axe.X());
  temp.push_back(Axe.Y());
  temp.push_back(Axe.Z());
  temp.push_back(GRayon);
  temp.push_back(PRayon);

  return temp;

}