// Gnurbs - A curve and surface library
// Copyright (C) 2008-2026 Eric Bechet
//
// See the LICENSE file for contributions and license information.
// Please report all bugs and problems to <bechet@cadxfem.org>.
//

#include <iostream>
#include <vector>
#include <deque>

#include "ndisplay.h"
#include "nbsplinesurface.h"
#include "nbeziercurve.h"

class mysurf : public nparametricsurface
{
  
public:
  virtual double min_u() const {return -7.07;} // minimal allowed value for u
  virtual double max_u() const {return 7.07;} // maximal allowed value for u
  virtual double min_v() const {return -7.07;} // minimal allowed value for v
  virtual double max_v() const {return 7.07;} // maximal allowed value for v
  virtual void P(double u,double v, npoint& ret) const     // point on the surface S(u,v)
  {
    double radius=5;
    double prof=4;
    ret[0]=u;
    ret[1]=v;
#if 0 
    if (u*u+v*v>(radius*radius-prof*prof))
      ret[2]=1*(atan((u*u+v*v-(radius*radius-prof*prof)))/3.1415926536+0.5);
    else
      ret[2]=(1+(sqrt(radius*radius-(u*u+v*v))-prof))*(atan((u*u+v*v-(radius*radius-prof*prof)))/3.1415926536+0.5) ;
#else
    ret[2]=(sin(sqrt(u*u+v*v)*1.5)/sqrt(u*u+v*v))*1;
#endif
    ret[3]=1.0;
  }
  virtual nsurface* clone() const // clones the surface (allocated on the heap)
  {
    return new mysurf(*this);
  } 
};


class mycurve : public nparametriccurve
{
  
public:
  virtual double min_u() const {return -2;} // minimal allowed value for u
  virtual double max_u() const {return 2;} // maximal allowed value for u
  virtual void P(double u, npoint& ret) const     // point on the surface S(u,v)
  {
    ret[0]=u/2+3;
    ret[1]=3-sqrt(1*1-(u*u)/4);
    ret[2]=0;
    ret[3]=1.0;
  }
  virtual ncurve* clone() const // clones the surface (allocated on the heap)
  {
    return new mycurve(*this);
  } 
};

void draw_geodesic(nparametricsurface &surf,npoint3 pt_depart,npoint3 dir,double delta,std::vector<npoint3> &pts,std::vector<npoint3> &tang,int limit);
void compute_geodesics(nparametricsurface &surf,npoint3 pt,double delta,int nb,std::vector<npoint3> &pts,int step, int limit,int limit2);
void draw_curvature_line(nparametricsurface &surf,npoint3 &pt_depart,double delta,std::vector<npoint3> &pts,std::vector<npoint3> &liste1,std::vector<npoint3> &liste2,int direction,int step, int limit=100, int limit2=100);
void compute_curvature_lines(nparametricsurface &surf,npoint3 pt,double delta,int nb,std::vector<npoint3> &pts,int step, int limit=100,int limit2=100);
void initsurface(nbsplinesurface &surf,double DX,double DY,double DZ); //fonction d'initalisation de la surface
void initsurface2(nbsplinesurface &surf,double DX,double DY,double DZ); //fonction d'initalisation de la surface
double cos_angle(double V1[2],double M1[2][2],double V2[2]);

struct info_s
{
  npoint3 pt;
  npoint3 dir1;
  npoint3 dir2;
  int info;
};


int main(void)
{
  data_container data;
  ndisplay display;
  double delta=0.0005;
  int step=50;
  int limit=500;
  std::vector<npoint3> liste; // la liste des points de la geodesique
  std::vector<npoint3> liste2; // la liste des points de la geodesique
  nbsplinesurface surface(11,3,11,3); // la surface
  initsurface2(surface,0,0,0);
//  mysurf surface;
  //npoint3 point_depart(2.5,2.5,0);// point de depart (u,v)
  mycurve crv;
  nembeddedcurve crv2(crv,surface);
  npoint dep;
  double Tdep=1.0;
  crv.P(Tdep,dep);


  npoint3 point_depart(dep);
  point_depart.print(std::cout);
//    npoint3 point_depart(5,5,0);
//    npoint3 point_depart(-7,-7,0);
//  compute_curvature_lines(surface,point_depart,delta,1,liste,step,limit,4*limit);
#if 0
    data.setcolorpoints(color(0,0,255));
  for (int i=0;i<liste.size();++i)
  {
    if (!(i%10))
    {
      npoint p;
      surface.P(liste[i].x(),liste[i].y(),p);
      data.add_point(p);
//      data.add_point(liste[i]);
    }
  }

//  compute_geodesics(surface,point_depart,delta,1,liste2,step,limit,4*limit);
    data.setcolorpoints(color(0,255,0));
  for (int i=0;i<liste2.size();++i)
  {
    if (!(i%10))
    {
      npoint p;
      surface.P(liste2[i].x(),liste2[i].y(),p);
      data.add_point(p);
  //    data.add_point(liste2[i]);
    }
  }

  properties pr=data.getproppoints();
  pr.pointsize=3.0;
  data.setproppoints(pr);

  
  std::deque<std::pair<int,int> > cells;
  const int MAXU=70;
  const int MAXV=70;
  const double cellsize = 0.025;
  const int steps = 3;
  info_s *tab[MAXU][MAXV];
  for (int u=0;u<MAXU;++u)
    for (int v=0;v<MAXV;++v)
      tab[u][v]=0;
  npoint3 dir1(1,0,0);
  npoint3 dir2(1,0,0);
  npoint3 newp1,newdir1,newdirr1;
  npoint3 newp2,newdir2,newdirr2;
  npoint3 newp01,newdir01;
  npoint3 newp02,newdir02;
  
  npoint p;
  double angle=3.1415926536/2;
  double angle1=angle;
  double angle2=angle;
  info_s* nc= new info_s;
  
  nc->pt=point_depart;
  nc->dir1=dir1;
  surface.rotate(point_depart[0],point_depart[1],dir1,angle,dir2);
  nc->dir2=dir2;
  nc->info=3;
  tab[0][0]=nc;
  newp1=point_depart;
  newdir1=dir1;
  for (int u=1;u<MAXU;++u)
  {
    surface.walk(newp1[0],newp1[1],newdir1,cellsize,newp1,newdir1,steps);
    nc= new info_s;
    nc->pt=newp1;
    nc->dir1=newdir1;
    nc->info=1;
    tab[u][0]=nc;
  }
  newp2=point_depart;
  newdir2=dir2;
  
  for (int v=1;v<MAXV;++v)
  {
    surface.walk(newp2[0],newp2[1],newdir2,cellsize,newp2,newdir2,steps);
    nc= new info_s;
    nc->pt=newp2;
    nc->dir2=newdir2;
    nc->info=2;
    tab[0][v]=nc;
  }
  cells.push_back(std::pair<int,int>(1,1));
//  std::cout << " init done " << std::endl;
  while (!cells.empty())
  {
    int u=cells[0].first;
    int v=cells[0].second;
//    std::cout << u << " " << v  << std::endl;
    cells.pop_front();
    if (v>=MAXV||u>=MAXU) continue;
    int info;
    point_depart=tab[u-1][v-1]->pt;
    newp1=tab[u][v-1]->pt;
    newp2=tab[u-1][v]->pt;
    newdir1=tab[u][v-1]->dir1;
    newdir2=tab[u-1][v]->dir2;
    info=tab[u-1][v-1]->info;
//    std: cout << " loop " << info <<  std::endl;
    for(int i=0;i<100;++i)
    {

      if (! surface.walk(newp1[0],newp1[1],newdir2,cellsize,newp01,newdir02,steps)) break;

      if (! surface.walk(newp2[0],newp2[1],newdir1,cellsize,newp02,newdir01,steps)) break;
  
      double M[2][2];

      npoint3 newdirr10b=newp02-newp1;
      npoint3 newdirr10a=newp01-newp1;
      surface.fundamental_forms(newp1[0],newp1[1],M);
      int sign1 = ((newdirr10a[0]*newdirr10b[1]-newdirr10a[1]*newdirr10b[0])>=0)?1:-1;
      double dangle1=acos(cos_angle(newdirr10a.array(),M,newdirr10b.array()))*sign1;

      
//      std::cout << dangle1*180/3.1415926536 << std::endl;
      
      npoint3 newdirr20b=newp01-newp2;
      npoint3 newdirr20a=newp02-newp2;
      surface.fundamental_forms(newp2[0],newp2[1],M);
      int sign2 = ((newdirr20a[0]*newdirr20b[1]-newdirr20a[1]*newdirr20b[0])>=0)?1:-1;
      double dangle2=acos(cos_angle(newdirr20a.array(),M,newdirr20b.array()))*sign2;

//      std::cout << dangle2*180/3.1415926536 << std::endl;

      if (fabs(dangle1)+fabs(dangle2)<=2e-6) break;
      surface.rotate(newp2[0],newp2[1],newdir1,dangle2,newdir1);
      surface.rotate(newp1[0],newp1[1],newdir2,dangle1,newdir2);      
    }
    nc= new info_s;
    nc->pt=newp01;
    nc->dir1=newdir01;
    nc->dir2=newdir02;
    nc->info=0;
    tab[u][v]=nc;
    tab[u-1][v]->info+=1;
    if (tab[u-1][v]->info==3) cells.push_back(std::pair<int,int>(u,v+1));
    tab[u-1][v]->dir1=newdir1;
    tab[u][v-1]->info+=2;
    if (tab[u][v-1]->info==3) cells.push_back(std::pair<int,int>(u+1,v));
    tab[u][v-1]->dir2=newdir2;
  }
  
  for (int u=0;u<MAXU;++u)
    for (int v=0;v<MAXV;++v)
    {
      double M[2][2];
      npoint p;
      npoint3 p0,v1,v2;
      if (tab[u][v])
      {
        p0=tab[u][v]->pt;
        v1=tab[u][v]->dir1;
        v2=tab[u][v]->dir2;
        surface.P(p0[0],p0[1],p);
        surface.fundamental_forms(p0[0],p0[1],M);
        double cangle = cos_angle(v1.array(),M,v2.array());
        if (cangle>=0)
          data.setcolorpoints(color(255*cangle,0,255-255*cangle));
        if (cangle<=0)
          data.setcolorpoints(color(0,-255*cangle,255+255*cangle));
        data.add_point(p);
      }
    }
  
#endif
#if 1
  npoint3 arrivee(point_depart);
  npoint pp1,pp2;
  line l;
  properties pr;
  data.setcolorpoints(color(255,0,0));
  pr=data.getproppoints();
  pr.pointsize=3.0;
  data.setproppoints(pr);
  surface.P(point_depart[0],point_depart[1],pp1);
  pp1.print(std::cout);
  data.add_point(pp1);
  double kappa[2];
  double V[2][2];
  double M1[2][2];
  double M2[2][2];
  surface.principal_curvatures(point_depart[0],point_depart[1],kappa,V,M1,M2);
  std::cout << "K1=" << kappa[0] << " K2=" << kappa[1] << std::endl;
  arrivee[0]+=V[0][0]*delta*step;
  arrivee[1]+=V[0][1]*delta*step;
  surface.P(arrivee[0],arrivee[1],pp2);
  l.pts[0]=pp1;
  l.pts[1]=pp2;
  double len=0;
  for (int i=0;i<3;++i) len+=(pp2[i]-pp1[i])*(pp2[i]-pp1[i]);
  len=sqrt(len);
  std::cout << "L1 = " <<  len/(delta*step) << std::endl;

  data.setcolorlines(color(255,0,0));
  pr=data.getproplines();
  pr.thickness=1.0;
  data.setproplines(pr);
  data.add_line(l);
  data.add_point(pp2);
  arrivee=point_depart;
  arrivee[0]+=V[1][0]*delta*step;
  arrivee[1]+=V[1][1]*delta*step;
  surface.P(arrivee[0],arrivee[1],pp2);
  l.pts[1]=pp2;
  len=0;
  for (int i=0;i<3;++i) len+=(pp2[i]-pp1[i])*(pp2[i]-pp1[i]);
  len=sqrt(len);
  std::cout << "L2 = " << len/(delta*step) << std::endl;
  data.add_line(l);
  data.add_point(pp2);
  pr.pointsize=1.0;
  data.setproppoints(pr);
  
 
  double kappa0;
  crv.principal_curvatures(Tdep,kappa0);
  std::cout << "K_uv = " << kappa0 << std::endl;
  
  crv2.principal_curvatures(Tdep,kappa0);
  std::cout << "K = " << kappa0 << std::endl;
  crv2.Display(data);
  crv2.relative_curvatures(Tdep,kappa);
  std::cout << "K_n=" << kappa[0] << " K_g=" << kappa[1] << std::endl;
  npoint P;
  crv.P(Tdep,P);
  npoint3 Pt(P);
  crv.dPdu(Tdep,dep);
  npoint3 dPdt(dep);
  npoint3 d2Pdt2;
  surface.find_geodesic(Pt[0],Pt[1],dPdt,d2Pdt2);

  std::cout << "expected second derivatives in the parametric space for a geodesic: " << d2Pdt2[0] << " " << d2Pdt2[1] << std::endl;
  
  std::vector<npoint3> tang;
  draw_geodesic(surface,Pt,dPdt,delta*5,liste2,tang,500);
  npoint PP2;
  surface.P(liste2[0][0],liste2[0][1],PP2);
  for (int i=1;i<liste2.size();++i)
  {
    npoint PP1=PP2;
    surface.P(liste2[i][0],liste2[i][1],PP2);
    data.add_line(line(PP1,PP2));
    data.add_line(line(liste2[i-1],liste2[i]));
  }
  
  
 /* double da=3.1415926536/36;
  for (double angle=0;angle<(2*3.14156536);angle+=da)
  {
    npoint3 ini(1,0,0);
    npoint3 dpdt;
    surface.rotate(Pt[0],Pt[1],ini,angle,dpdt);
    std::vector<npoint3> tang;
    draw_geodesic(surface,Pt,dpdt,delta*5,liste2,tang,5000);
  }
  */
  
  
 npoint3 Puv,dPdu,dPdv,d2Pdu2,d2Pdudv,d2Pdv2;
  double kappa2[2];
 
  surface.P(Pt[0],Pt[1],P);P[3]=1.0;Puv=P;
  surface.dPdu(Pt[0],Pt[1],P);P[3]=1.0;dPdu=P;
  surface.dPdv(Pt[0],Pt[1],P);P[3]=1.0;dPdv=P;
  surface.d2Pdu2(Pt[0],Pt[1],P);P[3]=1.0;d2Pdu2=P;
  surface.d2Pdv2(Pt[0],Pt[1],P);P[3]=1.0;d2Pdv2=P;
  surface.d2Pdudv(Pt[0],Pt[1],P);P[3]=1.0;d2Pdudv=P;
  surface.d2Pdudv(Pt[0],Pt[1],P);P[3]=1.0;d2Pdudv=P;
  surface.fundamental_forms(Pt[0],Pt[1],M1);
  double VV[2]={dPdt[0],dPdt[1]};
  double ds2=multi(VV,M1,VV);
  double ds=sqrt(ds2);

  
  
  npoint3 dgdt(dPdt[0]*dPdu+dPdt[1]*dPdv);
  npoint3 d2gdt2(d2Pdt2[0]*dPdu+d2Pdt2[1]*dPdv+dPdt[0]*dPdt[0]*d2Pdu2+2*dPdt[0]*dPdt[1]*d2Pdudv+dPdt[1]*dPdt[1]*d2Pdv2);
  npoint3 n;
  n.crossprod(dPdu,dPdv);n.normalize();
  npoint3 ndgdt;
  ndgdt.crossprod(n,dgdt);
  kappa2[0]=d2gdt2.dotprod(n)/ds2;
  kappa2[1]=d2gdt2.dotprod(ndgdt)/(ds*ds2);
  std::cout << "expected relative curvatures for the geodesic : " << kappa2[0] << " " << kappa2[1] << std::endl;
  
  crv.Display(data);
#endif
 
  data.setcolorpoints(color(255,0,0));
  data.setcolorquads(color(127,127,127,192));

  surface.Display(data);
  display.init_data(data);
  display.display();
  return 0;
}




//fonction d'initalisation d'une surface
void initsurface(nbsplinesurface &surf,double DX,double DY,double DZ)
{
  for (int i=0;i<surf.nb_CP_u();++i)
  {
    for (int j=0;j<surf.nb_CP_v();++j)
    {
      double R=1.5;
      double r=i*2/((double)surf.nb_CP_u()-1)-1+3;
      double t=j*2/((double)surf.nb_CP_v()-1)-1;
      double x=r*sin(3.14*t);
      double y=r*cos(3.14*t);
      double rr=(4-r)*(4-r)*2;
      double z=1*(sqrt(R*R+rr)-0.9+cos(t*4));
 //     z=z*z*1;
      npoint p(x+DX,y+DY,z+DZ,1.0);
      surf.CP(i,j)=p;
    }
  }
  int endsu=surf.degree_u()+1;
  int du=surf.degree_u();
  int cpsu=surf.nb_CP_u()-1;
  for (int i=0;i<endsu;++i)
  {
    surf.u(i)=0.0;
    surf.u(cpsu+du+1-i)=(cpsu+du+2-2*endsu+1);
  }
  for (int i=1;i<=(cpsu+du+2-2*endsu);++i)
  {
    surf.u(i+endsu-1)=i;
  }
  int endsv=surf.degree_v()+1;
  int dv=surf.degree_v();
  int cpsv=surf.nb_CP_v()-1;

  for (int i=0;i<endsv;++i)
  {
    surf.v(i)=0.0;
    surf.v(cpsv+dv+1-i)=(cpsv+dv+2-2*endsv+1);
  }
  for (int i=1;i<=(cpsv+dv+2-2*endsv);++i)
  {
    surf.v(i+endsv-1)=i;
  }
}





//fonction d'initalisation d'une surface
void initsurface2(nbsplinesurface &surf,double DX,double DY,double DZ)
{
  for (int i=0;i<surf.nb_CP_u();++i)
  {
    for (int j=0;j<surf.nb_CP_v();++j)
    {
      double x=i*2/((double)surf.nb_CP_u()-1)-1;
      double y=j*2/((double)surf.nb_CP_v()-1)-1;
      double z=0.;
      if ((i-1)/3==(surf.nb_CP_u()/6)&&((j-1)/3==(surf.nb_CP_v()/6)))
      {
       if ((i==surf.nb_CP_u()/2)&&(j==surf.nb_CP_v()/2)) z=0.0;
           else z=0.4;
      }
      else
        z=0.0;

      npoint p(x+DX,y+DY,z+DZ,1.0);
      surf.CP(i,j)=p;
    }
  }
  int endsu=surf.degree_u()+1;
  int du=surf.degree_u();
  int cpsu=surf.nb_CP_u()-1;
  for (int i=0;i<endsu;++i)
  {
    surf.u(i)=0.0;
    surf.u(cpsu+du+1-i)=(cpsu+du+2-2*endsu+1);
  }
  for (int i=1;i<=(cpsu+du+2-2*endsu);++i)
  {
    surf.u(i+endsu-1)=i;
  }
  int endsv=surf.degree_v()+1;
  int dv=surf.degree_v();
  int cpsv=surf.nb_CP_v()-1;

  for (int i=0;i<endsv;++i)
  {
    surf.v(i)=0.0;
    surf.v(cpsv+dv+1-i)=(cpsv+dv+2-2*endsv+1);
  }
  for (int i=1;i<=(cpsv+dv+2-2*endsv);++i)
  {
    surf.v(i+endsv-1)=i;
  }
}










void draw_curvature_line(nparametricsurface &surf,npoint3 &pt_depart,double delta,std::vector<npoint3> &pts,std::vector<npoint3> &liste1,std::vector<npoint3> &liste2,int direction,int step, int limit, int limit2)
{
  npoint3 pt;
  double V[2][2];
  double M1[2][2];
  double M2[2][2];
  double kappa[2];
  double oldDir [2]={0.,0.};

  for (int dir=0;dir<2;++dir)
  {
    pt = pt_depart;
    int i = 0;        // Compteur qui permet de calculer le nombre de point ajouté dans liste
    int sw=1; // si il y a une inversion soudaine
    while    ((pt[0]>surf.min_u() && pt[1]>surf.min_v()) 
           && (pt[0]<surf.max_u() && pt[1]<surf.max_v())
           && i < limit)  //Fait en sorte de ne pas sortir de la surface
    {
      if (step && i%step == 0 && i && i<limit2)  //Si modif=1 on va ajouter des points dans liste1 et liste2 qui vont être les nouveau points de départ par la suite
        if (direction)
          liste1.push_back(pt);
        else
          liste2.push_back(pt);

      pts.push_back(pt); // ajoute un pt à afficher
      surf.principal_curvatures(pt[0],pt[1],kappa,V,M1,M2);  // Calcule les directions principales
      if (i)
      {
        double prodSca = cos_angle(oldDir,M1,V[direction]);  // Verifie que la direction principale trouvée est bien dans le même sens que la précédente
        sw=(prodSca<0)?-1:1;
      }
      pt[0] = sw*(1-2*dir)*delta*V[direction][0]+pt[0];  // Calcule le prochain point à afficher
      pt[1] = sw*(1-2*dir)*delta*V[direction][1]+pt[1];
      oldDir[0]=sw*V[direction][0];              //Met en mémoire la direction principale
      oldDir[1]=sw*V[direction][1];
      ++i;
    }
  }
}

void draw_geodesic(nparametricsurface &surf,npoint3 pt_depart,npoint3 dir,double delta,std::vector<npoint3> &pts,std::vector<npoint3> &tang,int limit)
{
  for (int i=0;i<limit;++i)
  {
    if (surf.walk(pt_depart[0],pt_depart[1],dir,delta,pt_depart,dir,1))
    {
      pts.push_back(pt_depart);
      tang.push_back(dir);
    }
    else
      return;
  }
}

void compute_curvature_lines(nparametricsurface &surf,npoint3 pt,double delta,int nb,std::vector<npoint3> &pts,int step, int limit,int limit2)
{
  std::vector<npoint3> liste1;
  std::vector<npoint3> liste2;

  draw_curvature_line(surf,pt,delta,pts,liste1,liste2,0,step,limit2,limit);
  draw_curvature_line(surf,pt,delta,pts,liste1,liste2,1,step,limit2,limit);

  for (int j=0;j<liste1.size();++j)
  {
    pt = liste1[j];
    draw_curvature_line(surf,pt,delta,pts,liste1,liste2,0,0,limit);
  }
  for (int j=0;j<liste2.size();++j)
  {
    pt = liste2[j];
    draw_curvature_line(surf,pt,delta,pts,liste1,liste2,1,0,limit);
  }
}


void compute_geodesics(nparametricsurface &surf,npoint3 pt,double delta,int nb,std::vector<npoint3> &pts,int step, int limit,int limit2)
{
  std::vector<npoint3> points;
  std::vector<npoint3> tangents;
  std::vector<npoint3> subpts;
  std::vector<npoint3> subtang;
    
  npoint3 kn;
  npoint3 tkn;
  npoint3 dir(1,0,0);
  draw_geodesic(surf,pt,dir,delta,points,tangents,limit);

  for (int j=0;j<points.size();++j)
  {
    if (!(j%step))
    {
      subpts.push_back(points[j]);
      subtang.push_back(tangents[j]);
    }
  }
  points.clear();tangents.clear();
  dir=dir*-1;
  draw_geodesic(surf,pt,dir,delta,points,tangents,limit);
  for (int j=0;j<points.size();++j)
  {
    if (!(j%step))
    {
      subpts.push_back(points[j]);
      subtang.push_back(tangents[j]);
    }
  }
  points.clear();tangents.clear();
  surf.rotate(pt[0],pt[1],dir,3.1415926536/2,dir);
  draw_geodesic(surf,pt,dir,delta,points,tangents,limit);
  for (int j=0;j<points.size();++j)
  {
    if (!(j%step))
    {
      subpts.push_back(points[j]);
      subtang.push_back(tangents[j]);
    }
  }
  points.clear();tangents.clear();
  dir=dir*-1;
  draw_geodesic(surf,pt,dir,delta,points,tangents,limit);
  for (int j=0;j<points.size();++j)
  {
    if (!(j%step))
    {
      subpts.push_back(points[j]);
      subtang.push_back(tangents[j]);
    }
  }
  points.clear();tangents.clear();
  for (int j=0;j<subpts.size();++j)
  {
    kn=subpts[j];
    tkn=subtang[j];
    surf.rotate(kn[0],kn[1],tkn,3.1415926536/2,tkn);
    draw_geodesic(surf,kn,tkn,delta,pts,tangents,limit);
    tkn=tkn*-1;
    draw_geodesic(surf,kn,tkn,delta,pts,tangents,limit);
  }
}


double cos_angle(double V1[2],double M1[2][2],double V2[2])
{
  double a=0;
  double b=0;
  double c=0;
  for (int i=0;i<2;++i)
  {
    for (int j=0;j<2;++j)
    {
      a+=V1[i]*M1[i][j]*V2[j];
      b+=V1[i]*M1[i][j]*V1[j];
      c+=V2[i]*M1[i][j]*V2[j];
    }
  }
//  std::cout << a << " " << b << " " << c << std::endl;
  double sqbc=sqrt(fabs(b*c));
  if (sqbc==0) return 0.;
  double r=a/sqbc;
  if (r>1.0) return 1.0;
  if (r<-1.0) return -1.0;
  return r;
}