FastMarching.cpp

#include<stdio.h>
#include<stdlib.h>
#include<cassert>
#include<iostream>
#include<set>
#include<math.h>
#include<algorithm>
#include "DataEikonal.hpp"
#include "Mesh.hpp"
#include "FastMarching.hpp"
#include "Chrono.hpp"
#include "ProxyFunctions.hpp"

#ifndef FM_TOLL
#define FM_TOLL 1.e-8
#endif



FastMarching::FastMarching(const Mesh * theMesh, const DataEikonal * theData)
:Eikonal(theMesh,theData)
{
     _changed.clear();
    initializeAuxVar();
}

FastMarching::~FastMarching()
{
    _changed.clear();
    for(size_t iPt=0; iPt<npt; iPt++)
    {
        _changedPtr[iPt] =_changed.end();
    }
}


void  FastMarching::iterate(const double & integration_time)
{
      double tn=0.0;
      QueueTypeIterator itm;
      while(!(_trial.empty()  &&_changed.empty() ) && (tn<= integration_time) )
      {
            size_t pn=0;
            // first: the changed list
            if(!_changed.empty())
            {
                  itm=_changed.begin();
                  pn = itm->second;
                  tn=itm->first;
                  if(tn> integration_time)
                  {
                        break;
                  }
                  else
                  {
                        //pop from queue
                        _changed.erase(itm);
                        _ischanged[pn]=false;
                        _changedPtr[pn] = _changed.end();
                  }
            }
            else
            {
                  itm=_trial.begin();
                  pn = itm->second;
                  tn=itm->first;
                  if(tn> integration_time)
                  {
                        break;
                  }
                  else
                  {
                        //pop from queue
                        _trial.erase(itm);
                        _istrial[pn]=false;
                        _trialPtr[pn] = _trial.end();
                        _known[pn] = true;
                  }
            }
            if(tn> integration_time)
            {
                  break;
            }
            else
            {
                  _tactivOutput[pn]=_tactiv[pn];
                  //now evaluate the neighborhood of the popped point
                  PointConnect neighborhood = _mesh->pointConnectivity(pn);
                  PointConnect known_neighborhood,trial_unknown_neighborhood;
                  known_neighborhood.clear();
                  trial_unknown_neighborhood.clear();

                  //fill the containers of subsets
                  for(PointConnectIterator itneigh =neighborhood.begin(); itneigh!=neighborhood.end(); ++itneigh )
                  {
                        if(_known[*itneigh])
                        {
                              known_neighborhood.insert(*itneigh);
                        }
                        else
                        {
                              if(!_ischanged[*itneigh])
                              {
                                    trial_unknown_neighborhood.insert(*itneigh);
                              }
                        }
                  }
                  //Update each subset
                  updateSubset(neighborhood, known_neighborhood,pn);
                  updateSubset(neighborhood, trial_unknown_neighborhood,pn);
            }// end if tn<= IT
      }// End while
}


void FastMarching::evaluateActivationTimes()
{
      QueueTypeIterator itm;
      //while(!(_trial.empty()  &&_changed.empty() )  )
      while(!_trial.empty() )
      {
            //std::cout<<"_trial.size() "<<_trial.size()<<std::endl;
            size_t pn=0;
            // first: the changed list
            /*if(!_changed.empty())
            {
                  itm=_changed.begin();
                  pn = itm->second;
                  //pop from queue
                  _changed.erase(itm);
                  _ischanged[pn]=false;
                  _changedPtr[pn] = _changed.end();
            }
            else*/
            {
                  itm=_trial.begin();
                  pn = itm->second;
                  //pop from queue
                  _trial.erase(itm);
                  _istrial[pn]=false;
                  _trialPtr[pn] = _trial.end();
                  _known[pn] = true;
            }
            _tactivOutput[pn]=_tactiv[pn];
            //now evaluate the neighborhood of the popped point
            PointConnect neighborhood = _mesh->pointConnectivity(pn);
            PointConnect known_neighborhood,trial_unknown_neighborhood;
            known_neighborhood.clear();
            trial_unknown_neighborhood.clear();

            //fill the containers of subsets
            for(PointConnectIterator itneigh =neighborhood.begin(); itneigh!=neighborhood.end(); ++itneigh )
            {
                  if(_known[*itneigh])
                  {
                        known_neighborhood.insert(*itneigh);
                  }
                  else
                  {
                        //if(!_ischanged[*itneigh])
                        //{
                              trial_unknown_neighborhood.insert(*itneigh);
                        //}
                  }
            }
            //Update each subset
            //updateSubset(neighborhood, known_neighborhood,pn);
            updateSubset(neighborhood, trial_unknown_neighborhood,pn);
      }// End while
}


///////////////////////
// Private functions //
///////////////////////

void FastMarching::initializeAuxVar()
{
      _ischanged.resize(npt);
      _changedPtr.resize(npt);    

      for(size_t iPt=0; iPt<npt; iPt++)
      {
            _ischanged[iPt] = false;
            _changedPtr[iPt] =_changed.end();
      }
}


void FastMarching::updateSubset(const PointConnect & neighborhood, const PointConnect & subset, const size_t & pn)
{
      for(PointConnectIterator itneigh =subset.begin(); itneigh!=subset.end(); ++itneigh )
      {
      
            //1 Extract intersection of neighbours of pn (point i) and inteigh (point j)
            PointConnect neighborhoodj=_mesh->pointConnectivity(*itneigh);
            NeighbType neighborhoodAB;
            neighborhoodAB.clear();
            std::set_intersection(neighborhood.begin(),neighborhood.end(),               // neigh of i
                                  neighborhoodj.begin(),neighborhoodj.end(),             // neigh of j
                            std::inserter(neighborhoodAB, neighborhoodAB.begin())); 
                  
            IndexVector pointList((_mesh->spacedim_GeoEle()+1),0);
            pointList[0]=pn;
            pointList[_mesh->spacedim_GeoEle()]=*itneigh;
            switch (_mesh->spacedim_GeoEle())
            {
                  case 2:
                  {
                        //Here a for loop where I set pointList[1] from neighborhoodAB 
                        for(NeighbType::iterator ita=neighborhoodAB.begin();ita!=neighborhoodAB.end();++ita)
                        {
                              pointList[1]=*ita;
                              updateNeighPoint(pointList);
                        }                                                     
                        break;
                  }
                  case 3:
                  {
                        //Here two nested for loop where I set pointList[1] from neighborhoodAB a=1..N
                        // and pointList[2] from neighborhoodAB b=a+1..N
                        for(NeighbType::iterator ita=neighborhoodAB.begin();ita!=neighborhoodAB.end();++ita)
                        {
                              pointList[1]=*ita;
                              NeighbType::iterator itb=ita;
                              for(++itb;itb!=neighborhoodAB.end();++itb)
                              {
                                    pointList[2]=*itb;
                                    updateNeighPoint(pointList); 
                              }
                        }         
                        break;
                  }
                  default:
                  {
                        std::cerr<<"UNKNOWN GEO DIM!"<<std::endl;
                        exit(1);
                        break;
                  }
            }
      }//end on neighb
}


void FastMarching::updateNeighPoint(const IndexVector & pointList)
{
      //pointList[_mesh->spacedim_GeoEle()]=*itneigh;   candidate, point in the neighbourhood
      size_t neigh=pointList[_mesh->spacedim_GeoEle()];
      double tij=timeij( pointList); 
      if(_known[neigh]) // known points
      {
            if(tij<_tactiv[neigh])
            {
                  std::cout<<neigh<<" "<<_tactiv[neigh]<<" NEW: "<<tij<<std::endl;
                  /*_tactiv[neigh]=tij;
                  if(_ischanged[neigh]==false)
                  {
                        _ischanged[neigh] = true;
                  }
                  else
                  {
                        QueueTypeIterator itQueue = _changedPtr[neigh];
                        _changed.erase(itQueue);
                  }
                  _changedPtr[neigh]=_changed.insert( timePair(tij,neigh)  ); */           
            } // the point changed
      }
      else  // trial and unknown points
      {
            if(_istrial[neigh]  ) //the new tact is earlier
            {
                  if(tij<_tactiv[neigh])
                  {
                      _tactiv[neigh] = tij;
                      QueueTypeIterator itQueue = _trialPtr[neigh];
                      _trial.erase(itQueue);
                      _trialPtr[neigh]=_trial.insert( timePair(tij,neigh)  );            
                  }
            }
            else //Point is unknown: put it to trial 
            {
                  _tactiv[neigh] = tij;
                  _istrial[neigh] = true;
                  _trialPtr[neigh]=_trial.insert( timePair(tij,neigh)  );            
            }
      }
}


double FastMarching::timeij(const IndexVector & pointList)
{
      //pointList[0]=pn;  this is the point where I start (known; popped from the queue)
      double tj =_tactiv[pointList[0]];
      //pointList[_mesh->spacedim_GeoEle()]=*itneigh;   candidate, point in the neighbourhood
      double ti =_tactiv[pointList[_mesh->spacedim_GeoEle()]];
      //Cost to go from j to i
      double cij=ceij(pointList);
      // if it is cheaper to reach ni from nj, so cost is
      // tj + cij; ti otherwise
      double tij=std::min(ti, (tj+cij)  );
      return(tij);
}


double FastMarching::ceij(const IndexVector & pointList)
{
      //This is phi4-phi3; phi3 is phi(pointList[0]) and is the point popped from the queue
      double ceij=tMax;
      if(_activateEiko)
      {	
            double alpha=0.0;
            for(unsigned char jpt=0; jpt<(1+_mesh->spacedim_GeoEle()); jpt++ )
            {
                  double alpha1=(_data->alpha())[pointList[jpt]];
                  if(alpha1>0.0)
                  {
                        alpha=std::max(alpha, alpha1  );
                  }
                  else
                  {
                        alpha=0.0;
                        break;
                  }
            }
	      
            if(alpha>0.0)
            {
                  //First evaluate the tensor for the resistence
                  std::vector<double> Mij=tensorResistance(pointList);
                  NeighbType known_vertices;
                  known_vertices.clear();
                  for(unsigned char jpt=1; jpt<(_mesh->spacedim_GeoEle()); jpt++ )                  
                  {
                        if(_known[pointList[jpt]])
                        {
                              known_vertices.insert(pointList[jpt]);
                        }
                  }
                  if(known_vertices.empty()) // only the source point is known: propagate as a grapsh
                  {
                        std::vector<double> vij(3,0.0);
                        const Point & coordi=_mesh->Pts(pointList[0]);
                        const Point & coordj=_mesh->Pts(pointList[_mesh->spacedim_GeoEle()]);
                        //ceij
	                      ceij=0.0;
                        for(unsigned char iDim = 0; iDim<3; iDim++)
                        {
                              vij[iDim] = coordj.coord[iDim] - coordi.coord[iDim];
                        }  
                        for(unsigned char iDim=0; iDim<3; iDim++)
                        {
                              for(unsigned char jDim=0; jDim<3; jDim++)
                              {
                                    ceij=ceij+vij[iDim]*Mij[RMIndex(iDim,jDim,3)]*vij[jDim];
                              }
                        }
                        ceij=sqrt(ceij);
                  }
                  else
                  {
                        //need to evaluate phi4-phi3 here !!!! 
                        //// TO BE COMPLETED ///
                        // for the update if pointList has 4 points follow page 6 onwards of:
                        //
                        //    https://www.ncbi.nlm.nih.gov/pmc/articles/PMC4162315/
                        //
                        // otherwise, adapt to the 2D case for triangles; e.g.:
                        //
                        //    https://www.ncbi.nlm.nih.gov/pmc/articles/PMC3360588/ 
                  
                        switch(known_vertices.size())
                        {
                              case 1:  // Propagate on a triangle/face of a tetra
                              {
                              
                                    const Point & coord0=_mesh->Pts(pointList[0]);
                                    const Point & coord1=_mesh->Pts(*(known_vertices.begin()));
                                    const Point & coord2=_mesh->Pts(pointList[_mesh->spacedim_GeoEle()]);
                                    
                                    double DT01=_tactiv[*(known_vertices.begin())]-_tactiv[pointList[0]];
                                    std::vector<double> v02(3,0.0), v10(3,0.0);
                                    
                                    for(unsigned char iDim = 0; iDim<3; iDim++)
                                    {
                                          v02[iDim] = coord2.coord[iDim] - coord0.coord[iDim];
                                          v10[iDim] = coord0.coord[iDim] - coord1.coord[iDim];
                                    }
                                    
                                    double a=0.0,b=0.0,c=0.0;
                                    for(unsigned char iDim=0; iDim<3; iDim++)
                                    {
                                          for(unsigned char jDim=0; jDim<3; jDim++)
                                          {
                                                a=a+v10[iDim]*Mij[RMIndex(iDim,jDim,3)]*v10[jDim];
                                                b=b+v02[iDim]*Mij[RMIndex(iDim,jDim,3)]*v10[jDim];
                                                c=c+v02[iDim]*Mij[RMIndex(iDim,jDim,3)]*v02[jDim];
                                          }
                                    }
                                    
                                    // DT02=T2-T0=p*DT01+sqrt(a*p*p+2*b*p+c)
                                    // p is the barycentric coordinate that minimises T2 (or DT02)
                                    ///Here goes the minimising function with checks that coordinates are barycentrics and so on....
                                    
                                    double deno_01=DT01*DT01-a;
                                    if(  (fabs(deno_01)-FM_TOLL)>0.0 )
                                    {
                                        double Delta=  b*b-a*(DT01*DT01*c-b*b) /deno_01;
                                        if((Delta+FM_TOLL)>0.0)
                                        {
                                            // p1 and p2 minimising DT02
                                            double p1=-b/a+sqrt(fabs(Delta))/a;
                                            double p2=-b/a-sqrt(fabs(Delta))/a;
                                            //argument of sqrt(a*p*p+2*b*p+c)
                                            double r1=a*p1*p1+2.0*b*p1+c;
                                            double r2=a*p2*p2+2.0*b*p2+c;
                                            double optim1=tMax;
                                            double optim2=tMax;
                                            if(  ((r1+FM_TOLL)>0.0) && ( (p1*(1.0-p1)+FM_TOLL)>0.0    )     )
                                            {
                                                optim1=DT01*p1+sqrt(fabs(r1));
                                            }
                                            if(  ((r2+FM_TOLL)>0.0) && ( (p2*(1.0-p2)+FM_TOLL)>0.0    )     )
                                            {
                                                optim2=DT01*p2+sqrt(fabs(r2));
                                            
                                            }
                                            ceij=std::min(optim1,optim2);
                                            
                                        }
                                    }
                                    break;
                              }
                              case 2:   //Propagate on a tetra (3D only)
                              {
                                    NeighbType::iterator itknown=known_vertices.begin();
                                    const Point & coord0=_mesh->Pts(pointList[0]);
                                    const Point & coord1=_mesh->Pts(*itknown); //HERE!!!!
                                    double DT01=_tactiv[*itknown]-_tactiv[pointList[0]];
                                    ++itknown;
                                    const Point & coord2=_mesh->Pts(*itknown);
                                    double DT02=_tactiv[*itknown]-_tactiv[pointList[0]];
                                    const Point & coord3=_mesh->Pts(pointList[_mesh->spacedim_GeoEle()]);
                                    std::vector<double> v03(3,0.0), v10(3,0.0),v20(3,0.0);
                                    
                                    for(unsigned char iDim = 0; iDim<3; iDim++)
                                    {
                                          v03[iDim] = coord3.coord[iDim] - coord0.coord[iDim];
                                          v10[iDim] = coord0.coord[iDim] - coord1.coord[iDim];
                                          v20[iDim] = coord0.coord[iDim] - coord2.coord[iDim];
                                    }

                                    double a=0.0,b=0.0,c=0.0,d=0.0,e=0.0,f=0.0;
                                    for(unsigned char iDim=0; iDim<3; iDim++)
                                    {
                                          for(unsigned char jDim=0; jDim<3; jDim++)
                                          {
                                                a=a+v10[iDim]*Mij[RMIndex(iDim,jDim,3)]*v10[jDim];
                                                b=b+v20[iDim]*Mij[RMIndex(iDim,jDim,3)]*v10[jDim];
                                                c=c+v20[iDim]*Mij[RMIndex(iDim,jDim,3)]*v20[jDim];
                                                
                                                d=d+v03[iDim]*Mij[RMIndex(iDim,jDim,3)]*v10[jDim];
                                                e=e+v03[iDim]*Mij[RMIndex(iDim,jDim,3)]*v20[jDim];
                                                
                                                f=f+v03[iDim]*Mij[RMIndex(iDim,jDim,3)]*v03[jDim];
                                          }
                                    }
                                        
                                    // For solution, see:  
                                    // https://www.geometrictools.com/Documentation/PolynomialSystems.pdf
                                    // (page 6) produce a 4th degree polynomia in y only; recover the roots
                                    // DT03=T3-T0=p*DT01  +q*DT02     +  sqrt( a*p*p+2*b*p*q+c*q+q   +2*d*p +2*e*q +f  )
                                    // p,q are the barycentric coordinates that minimises T3 (or DT03)
                                    ///Here goes the minimising function with checks that coordinates are barycentrics and so on....

                                    RealVector Q_sol;
                                    Q_sol.clear();
                                    RealVector P_sol;
                                    P_sol.clear();
                                    double deno_01=DT01*DT01-a;
                                    if(  (fabs(deno_01)-FM_TOLL)>0.0 )
                                    {
                                        //evaluate the roots of the 4th degree associated equation    q=y
                                        Q_sol=y_roots(a,b,c,d,e,f,DT01, DT02);
                                    }
                                    else
                                    {
                                        double A01=(DT01*DT01*c-b*b),
                                               B01=(DT01*DT01*e-b*d),
                                               C01=(DT01*DT01*f-d*d);
                                         
                                          double Delta01=B01*B01-A01*C01;
                                          if((Delta01-FM_TOLL)>0.0)
                                          {
                                                Q_sol.resize(2,0.0);
                                                double coef1=-1.0*B01/A01,
                                                       coef2=Delta01/A01;
                                                Q_sol[0] = (coef1+coef2);
                                                Q_sol[1] = (coef1-coef2);
                                          }
                                    }
                                    unsigned char nroots_q=static_cast<unsigned char>(Q_sol.size());
                                    //double deno_02=DT02*DT02-c;
                                    for(unsigned char irq=0; irq<nroots_q; irq++)
                                    {
                                        P_sol.clear();
                                        double q=Q_sol[irq];
                                        if( (q*(1.0-q)+FM_TOLL)>0.0    )
                                        {
                                              P_sol=f1_x_roots_y(a, b, c, d, e, f, DT01, DT02, q);
                                              
                                              if(P_sol.empty())
                                              {
                                                  P_sol=f2_x_roots_y(a, b, c, d, e, f, DT01, DT02, q);
                                              }
                                              unsigned char nroots_p=static_cast<unsigned char>(P_sol.size());
                                              for(unsigned char irp=0; irp<nroots_p; irp++)
                                              {
                                                  double p=P_sol[irp],
                                                         t=1.0-p-q,
                                                         r= a*p*p+2.0*b*p*q+c*q+q   +2.0*d*p +2.0*e*q +f ;
                                                  if( ((p*(1.0-p)+FM_TOLL)>0.0) and ((t*(1.0-t)+FM_TOLL)>0.0)  and ((r+FM_TOLL)>0.0)   )  //1-p-q
                                                  {
                                                       
                                                       ///here goes the rest...
                                                       double optim=p*DT01+q*DT02+sqrt(fabs(r) );
                                                       ceij=std::min(ceij,optim);
                                                  }
                                              }
                                        }
                                    }                                    
                                    
                                    break;
                              }
                              default:
                              {
                                    std::cerr<<"known_vertices:WRONG NUMBER OF POINTS"<<std::endl;
                                    exit(1);
                                    break;                              
                              }
                        }
                  }
                if (!_data->isComputingDistanceOnly())
                {
                  ceij=ceij/alpha;
                }
            }//end if alpha>0
      }//end if activate eiko
      return(ceij);
}



Eikonal::RealVector FastMarching::y_roots(const double & a, const double &b, const double &c ,const double &d, const double &e, const double & f,const double & DT01, const double & DT02)
{

    //https://www.geometrictools.com/Documentation/PolynomialSystems.pdf
    // (page 6) produce a 4th degree polynomia in y only; recover the roots
                                            
    double alpha1=DT01*DT01-a,
        beta1 =(c*DT01*DT01-b*b),
        gamma1=(e*DT01*DT01-b*d),
        k1=f*DT01*DT01-d*d;

    double alpha2=DT02*DT02-c,
        beta2=(a*DT02*DT02-b*b),
        gamma2=(d*DT02*DT02-b*e),
        k2=f*DT02*DT02-e*e;
        // k1 + 2*alpha1*d*p +   2*gamma1*q + alpha1*a*p*p + 2*alpha1*b*q*p +    beta1*q*q
        // k2 +   2*gamma2*p + 2*alpha2*e*q +    beta2*p*p + 2*alpha2*b*p*q + alpha2*c*q*q
        double a00= k1,
               a10=2.0*alpha1*d,
               a01=2.0*gamma1,
               a11=2.0*alpha1*b,
               a20=alpha1*a,
               a02=beta1;
        
        double b00=k2,
               b10=2.0*gamma2,
               b01=2.0*alpha2*e,
               b11=2.0*alpha2*b,
               b20=beta2,
               b02=alpha2*c;
        
        double d00 = a20*b10-b20*a10,
               d01 = a20*b11-b20*a11,
               d10 = a10*b00-b10*a00,        
               d11 = a11*b00+a10*b01-b11*a00-b10*a01,
               d12 = a11*b01+a10*b02-b11*a01-b10*a02,
               d13 = a11*b02-b11*a02,
               d20 = a20*b00-b20*a00,
               d21 = a20*b01-b20*a01,
               d22 = a20*b02-b20*a02;

        double h0 = d00*d10-d20*d20,
               h1 = d01*d10+d00*d11-2.0*d20*d21,
               h2 = d01*d11+d00*d12-d21*d21-2.0*d20*d22, 
               h3 = d01*d12+d00*d13-2.0*d21*d22,
               h4 = d01*d13-d22*d22;

      //find root of  sum_i=0^4 h_i y^i = 0
      
      //roots of 4th order polynomia
      //https://en.wikipedia.org/wiki/Quartic_function               
      
      double Delta0 = compute_delta_0_4ord(h4,h3,h2,h1,h0);
      double Delta1 = compute_delta_1_4ord(h4,h3,h2,h1,h0);
      //a=h4,b=h3,c=h2,d=h1,e=h0
      double p=(8.0*h4*h2-3.0*h3*h3)/(8.0*h4*h4);
      double q=(h3*h3*h3-4.0*h4*h3*h2+8.0*h4*h4*h1)/(8.0*h4*h4*h4);
      double Q=cbrt( 0.5*(Delta1+sqrt(Delta1*Delta1-4.0*Delta0*Delta0*Delta0)) );
      double S=0.5*sqrt(  (1.0/h4*(Q+Delta0/Q) -2.0*p)/3.0 );
      
      double mb4a=-0.25*h3/h4;
      
      
      //double Delta = compute_delta_4ord(h4,h3,h2,h1,h0);
      //double D4ord=compute_D_4ord(h4,h3,h2,h1,h0);
      //double P4ord=8.0*h4*h2-3.0*h3*h3;
      
      Eikonal::RealVector roots;
      roots.clear();
      double r1 = mb4a - S + 0.5*sqrt(-4.0*S*S-2.0*p+q/S);
      double r2 = mb4a - S - 0.5*sqrt(-4.0*S*S-2.0*p+q/S);
      double r3 = mb4a + S + 0.5*sqrt(-4.0*S*S-2.0*p-q/S);
      double r4 = mb4a + S - 0.5*sqrt(-4.0*S*S-2.0*p-q/S);
      
      if(not isnan(r1))
      {
          roots.push_back(r1);
      }
      
      if(not isnan(r2))
      {
          roots.push_back(r2);
      }

      if(not isnan(r3))
      {
          roots.push_back(r3);
      }

      if(not isnan(r4))
      {
          roots.push_back(r4);
      }
      return(roots);
}


Eikonal::RealVector FastMarching::f1_x_roots_y(const double & a, const double &b, const double &c ,const double &d, const double &e, const double & f,const double & DT01, const double & DT02, const double & yr)
{
    Eikonal::RealVector roots;
    roots.clear();
    double alpha1=DT01*DT01-a,
        beta1 =(c*DT01*DT01-b*b),
        gamma1=(e*DT01*DT01-b*d),
        k1=f*DT01*DT01-d*d;
    
    // k1 + 2*alpha1*d*p +   2*gamma1*q + alpha1*a*p*p + 2*alpha1*b*q*p +    beta1*q*q
    if(  (fabs(alpha1)-FM_TOLL)>0.0 )
    {
        double a00= k1,
            a10=2.0*alpha1*d,
            a01=2.0*gamma1,
            a11=2.0*alpha1*b,
            a20=alpha1*a,
            a02=beta1;

        double A = a20,
               B = a10+yr*a11,
               C = a00+a01*yr+a02*yr*yr;

         double Delta=B*B-4.0*A*C;
         Eikonal::RealVector roots;
         if((Delta-FM_TOLL)>0.0)
         {
              roots.resize(2,0.0);
              double coef1=-0.5*B/A,
                     coef2=0.5*Delta/A;
              roots[0]=(coef1+coef2);
              roots[1]=(coef1-coef2);
         }
    }     
    return(roots);
}


Eikonal::RealVector FastMarching::f1_y_roots_x(const double & a, const double &b, const double &c ,const double &d, const double &e, const double & f,const double & DT01, const double & DT02, const double & xr)
{
    Eikonal::RealVector roots;
    roots.clear();
    double alpha1=DT01*DT01-a,
        beta1 =(c*DT01*DT01-b*b),
        gamma1=(e*DT01*DT01-b*d),
        k1=f*DT01*DT01-d*d;
        // a00     a10*p         a01*q          a20 *p*p         a11 * q*p         a02*q*q
        // k1 + 2*alpha1*d*p +   2*gamma1*q + alpha1*a*p*p + 2*alpha1*b*q*p +    beta1*q*q

    double a00= k1,
        a10=2.0*alpha1*d,
        a01=2.0*gamma1,
        a11=2.0*alpha1*b,
        a20=alpha1*a,
        a02=beta1;

    double A = a02,
           B = a01+xr*a11,
           C = a00+a10*xr+a20*xr*xr;

    double Delta=B*B-4.0*A*C;
    
    if((Delta-FM_TOLL)>0.0)
    {
          double coef1=-0.5*B/A,
                 coef2=0.5*Delta/A;
          roots.resize(2,0.0);
          roots[0]=(coef1+coef2);
          roots[1]=(coef1-coef2);
    }
    return(roots);

}


Eikonal::RealVector FastMarching::f2_x_roots_y(const double & a, const double &b, const double &c ,const double &d, const double &e, const double & f,const double & DT01, const double & DT02, const double & yr)
{
    Eikonal::RealVector roots;
    roots.clear();
    double alpha2=DT02*DT02-c,
           beta2=(a*DT02*DT02-b*b),
           gamma2=(d*DT02*DT02-b*e),
           k2=f*DT02*DT02-e*e;
        //b00 +    b10*p   +   b01*q      +  b20 *p*p +      b11*p*q   +   b02 *q*q
        // k2 + 2*gamma2*p + 2*alpha2*e*q + beta2*p*p + 2*alpha2*b*p*q + alpha2*c*q*q
        
        double b00=k2,
               b10=2.0*gamma2,
               b01=2.0*alpha2*e,
               b11=2.0*alpha2*b,
               b20=beta2,
               b02=alpha2*c;

    double A = b20,
           B = b10+b11*yr,
           C = b00+b01*yr+b02*yr*yr;

    double Delta=B*B-4.0*A*C;
    
    if((Delta-FM_TOLL)>0.0)
    {
          double coef1=-0.5*B/A,
                 coef2=0.5*Delta/A;
          roots.resize(2,0.0);
          roots[0]=(coef1+coef2);
          roots[1]=(coef1-coef2);
    }
    return(roots);



}

Eikonal::RealVector FastMarching::f2_y_roots_x(const double & a, const double &b, const double &c ,const double &d, const double &e, const double & f,const double & DT01, const double & DT02, const double & xr)
{
    Eikonal::RealVector roots;
    roots.clear();
    double alpha2=DT02*DT02-c,
           beta2=(a*DT02*DT02-b*b),
           gamma2=(d*DT02*DT02-b*e),
           k2=f*DT02*DT02-e*e;
        //b00 +    b10*p   +   b01*q      +  b20 *p*p +      b11*p*q   +   b02 *q*q
        // k2 + 2*gamma2*p + 2*alpha2*e*q + beta2*p*p + 2*alpha2*b*p*q + alpha2*c*q*q
        
        double b00=k2,
               b10=2.0*gamma2,
               b01=2.0*alpha2*e,
               b11=2.0*alpha2*b,
               b20=beta2,
               b02=alpha2*c;

    double A = b02,
           B = b01+b11*xr,
           C = b00+b10*xr+b20*xr*xr;

    double Delta=B*B-4.0*A*C;
    
    if((Delta-FM_TOLL)>0.0)
    {
          double coef1=-0.5*B/A,
                 coef2=0.5*Delta/A;
          roots.resize(2,0.0);
          roots[0]=(coef1+coef2);
          roots[1]=(coef1-coef2);
    }
    return(roots);
}


double FastMarching::compute_delta_4ord(const double & a,const double & b,const double & c,const double & d,const double & e)
{
    double Delta=256.0*(a*e)*(a*e)*(a*e) -192.0*(a*e)*(a*e)*b*d-128.0*(a*c*e)*(a*c*e)+144.0*(a*d)*(a*d)*c*e -27.0*(a*a)*(d*d*d*d)
                +144.0*a*(b*b)*c*(e*e) -6.0*a*(b*d)*(b*d)*e -80*a*b*(c*c)*d*e +18.0*a*b*c*(d*d*d) + 16.0*a*e*(c*c*c*c)
                -4.0*a*(c*c*c)*(d*d) -27.0*(b*b*b*b)*(e*e) +18.0*(b*b*b)*c*d*e -4.0*(b*d)*(b*d)*(b*d) -4.0*(b*b)*(c*c*c)*e+(b*c*d)*(b*c*d);
    
    return(Delta);
}

double FastMarching::compute_D_4ord(const double & a,const double & b,const double & c,const double & d,const double & e)
{
    return( 64.0*(a*a*a)*e-16.0*(a*c)*(a*c)+16.0*a*(b*b)*c -16.0*(a*a)*b*d -3*(b*b*b*b) );

}


double FastMarching::compute_delta_0_4ord(const double & a,const double & b,const double & c,const double & d,const double & e)
{
    return(c*c-3.0*b*d+12.0*a*e);

}
double FastMarching::compute_delta_1_4ord(const double & a,const double & b,const double & c,const double & d,const double & e)
{
    return(2.0*c*c*c-9.0*b*c*d+27.0*(b*b*e+a*d*d)-72.0*a*c*e  );

}