mg_fast_marching2D.cpp

Aller à la documentation de ce fichier.

//####//------------------------------------------------------------
//####//------------------------------------------------------------
//####//  MAGiC
//####//  Jean Christophe Cuilliere et Vincent FRANCOIS
//####//  Departement de Genie Mecanique - UQTR
//####//------------------------------------------------------------
//####//  MAGIC est un projet de recherche de l equipe ERICCA
//####//  du departement de genie mecanique de l Universite du Quebec a Trois Rivieres
//####//  http://www.uqtr.ca/ericca
//####//  http://www.uqtr.ca/
//####//------------------------------------------------------------
//####//------------------------------------------------------------
//####//
//####//       mg_fast_marching2D.cpp
//####//
//####//------------------------------------------------------------
//####//------------------------------------------------------------
//####//    COPYRIGHT 2000-2024
//####//  jeu 13 jun 2024 11:58:56 EDT
//####//------------------------------------------------------------
//####//------------------------------------------------------------
 
 
#include "gestionversion.h"
#include "mg_fast_marching2D.h"
#include "mg_gestionnaire.h"
#include "fem_maillage.h"
#include "fem_triangle3.h"
#include "fem_solution.h"
#include "tpl_map_entite.h"
#include "tpl_liste_unique_entite.h"
#include "ot_mathematique.h"
#include <math.h>
#include "pars_argument.h"
#include "parse.h"
#include "fem_maillage_outils.h"
#include "ot_boite_3d.h"
 
 
namespace MAGIC
{
#define INFINI 1e300
 
 
 
MG_FAST_MARCHING2D::MG_FAST_MARCHING2D(FEM_SOLUTION* solu,int numchamp):MG_FAST_MARCHING(solu,numchamp)
{
}
 
MG_FAST_MARCHING2D::MG_FAST_MARCHING2D(MG_FAST_MARCHING &mdd):MG_FAST_MARCHING(mdd)
{
}
  
  
MG_FAST_MARCHING2D::~MG_FAST_MARCHING2D()
{
}
 
 
 
 
 
 
 
void MG_FAST_MARCHING2D::init_coord(double x,double y,double z)
{
TPL_LISTE_ENTITE<FEM_NOEUD*> lst;
LISTE_FEM_NOEUD::iterator it;
BOITE_3D boite;
FEM_NOEUD* no=fem->get_premier_noeud(it);
boite=no->get_boite_3D();
for (FEM_NOEUD* no=fem->get_premier_noeud(it);no!=NULL;no=fem->get_suivant_noeud(it))
     {
     BOITE_3D boitetmp=no->get_boite_3D();
      boite=boite+boitetmp;
      lst.ajouter(no);
     }
 
boite.change_grosseur(1.05);
oc.initialiser(&lst,1,boite.get_xmin(),boite.get_ymin(),boite.get_zmin(),boite.get_xmax(),boite.get_ymax(),boite.get_zmax());
LISTE_FEM_ELEMENT2::iterator it2;
for (FEM_ELEMENT2* ele=fem->get_premier_element2(it2);ele!=NULL;ele=fem->get_suivant_element2(it2))
  oc.inserer(ele);
TPL_MAP_ENTITE<FEM_ELEMENT2*> lst2;
oc.rechercher(x,y,z,0.000000001,lst2);
int nb=lst2.get_nb();
TPL_MAP_ENTITE<FEM_ELEMENT2*>::ITERATEUR ite;
bool trouve=false;
for (FEM_ELEMENT2* ele=lst2.get_premier(ite);ele!=NULL;ele=lst2.get_suivant(ite))
  {
   FEM_MAILLAGE_OUTILS ot;
   //int num=ot.estdansletriangle(ele,x,y,z);
   int num=ot.projeteestdansletriangle(ele,x,y,z);
   int res=OPERATEUR::compare_etat_triangle(num,OPERATEUR::INTERIEUR);
   if (res)
        {
        trouve=true;
    double xyz[3]={x,y,z};
        for (int i=0;i<3;i++)
            {
            double *xyz1=ele->get_fem_noeud(i)->get_coord();
            OT_VECTEUR_3D vec(xyz,xyz1);
            ele->get_fem_noeud(i)->change_solution(vec.get_longueur());
            }
        }
  }
if (!trouve)
    {
    char mess[500];
    sprintf(mess,"    Le point %lf, %lf, %lf n'a pas pu être initialisé",x,y,z) ;
    affiche(mess);    
    }
}
 
 
double MG_FAST_MARCHING2D::get_valeur(double x,double y,double z)
{
TPL_MAP_ENTITE<FEM_ELEMENT2*> lst2;
oc.rechercher(x,y,z,0.000000001,lst2);
TPL_MAP_ENTITE<FEM_ELEMENT2*>::ITERATEUR ite;
for (FEM_ELEMENT2* ele=lst2.get_premier(ite);ele!=NULL;ele=lst2.get_suivant(ite))
  {
   FEM_MAILLAGE_OUTILS ot;
   //int num=ot.estdansletriangle(ele,x,y,z);
   int num=ot.projeteestdansletriangle(ele,x,y,z);
   int res=OPERATEUR::compare_etat_triangle(num,OPERATEUR::INTERIEUR);
   if (res)
        {
        double xyz[3]={x,y,z};
        double uvw[3];
        ele->get_param_element_fini_2D(xyz,uvw);
        double val=0;
        for (int i=0;i<ele->get_nb_fem_noeud();i++)
            val=val+ele->get_fonction_interpolation(i+1,uvw)*ele->get_fem_noeud(i)->get_solution();
        return val;
        }
  } 
char mess[500];
sprintf(mess,"    Le point %lf, %lf, %lf n'a pas pu être lu",x,y,z) ;
affiche(mess);    
return 0.;
}
 
 
 
 
 
void MG_FAST_MARCHING2D::propage(double vitesse)
{
if (nbnoeudinit==1)
    {
    int nb;
    nb=last->get_lien_element2()->get_nb();
    TPL_MAP_ENTITE<FEM_NOEUD*> noproche;
    for (int i=0;i<nb;i++)
        {
          FEM_ELEMENT2* ele=last->get_lien_element2()->get(i);
          for (int j=0;j<ele->get_nb_fem_noeud();j++)
            noproche.ajouter(ele->get_fem_noeud(j));
        }
    TPL_MAP_ENTITE<FEM_NOEUD*>::ITERATEUR it;
    for (FEM_NOEUD* no=noproche.get_premier(it);no!=NULL;no=noproche.get_suivant(it))
        if (no!=last)  
          {  
          OT_VECTEUR_3D vec(no->get_coord(),last->get_coord());
          no->change_solution(vec.get_longueur()*vitesse);
          }
      
    }
 
 
TPL_LISTE_ENTITE<FEM_ELEMENT2*> know,trial,far;
LISTE_FEM_ELEMENT2::iterator it;
for (FEM_ELEMENT2* ele=fem->get_premier_element2(it);ele!=NULL;ele=fem->get_suivant_element2(it))
        {
        int nb=ele->get_nb_fem_noeud();
        int nbsol=0;
        for (int i=0;i<nb;i++)
          if (ele->get_fem_noeud(i)->get_solution()<INFINI) 
              {
                nbsol++;
                ele->get_fem_noeud(i)->change_numero(-1);
              }
          else ele->get_fem_noeud(i)->change_numero(1);
        if ((nbsol==0)||(nbsol==1)) far.ajouter(ele);
        if (nbsol==2) trial.ajouter(ele);
        if (nbsol==3) know.ajouter(ele);
        ele->change_numero(nbsol);
        }
std::multimap<double,FEM_NOEUD*> trialnoeud;
std::map<FEM_NOEUD*,std::multimap<double,FEM_NOEUD*>::iterator> trialnoeud2;
 
for (int i=0;i<trial.get_nb();i++)
    {
    FEM_ELEMENT2* ele=trial.get(i);
    FEM_NOEUD* no;
    double res;
    int numno;
    int erreur=resoudgradTtriangle(ele,vitesse,&no,&res);
    no->change_solution(std::min(res,no->get_solution()));
    std::map<FEM_NOEUD*,std::multimap<double,FEM_NOEUD*>::iterator>::iterator itf=trialnoeud2.find(no);
    if (itf!=trialnoeud2.end())
                    {
                    trialnoeud.erase(itf->second);
                    trialnoeud2.erase(itf);
                    }
    std::pair<double,FEM_NOEUD*> p(res,no);
    std::multimap<double,FEM_NOEUD*>::iterator it=trialnoeud.insert(p);
    std::pair<FEM_NOEUD*,std::multimap<double,FEM_NOEUD*>::iterator > p2(no,it);
    trialnoeud2.insert(p2);
    }
int compteur=0;
FEM_NOEUD* next=NULL;
while (trialnoeud.size()!=0)
    {
    compteur++;
    if (compteur>10000000) return;
    bool globalerreur=false;
    if (next==NULL) next=trialnoeud.begin()->second;
    FEM_NOEUD* no=next;
    next=NULL;
    int nbele=no->get_lien_element2()->get_nb();
    no->change_numero(0);
    std::vector<FEM_ELEMENT2*> lst;
    /*if (no->get_numero_opt()==32)
      next=NULL;
    if (no->get_numero_opt()==26)
      next=NULL;*/
    for (int i=0;i<nbele;i++)
        {
        FEM_ELEMENT2* ele=no->get_lien_element2()->get(i);
        int nbnum=ele->get_numero();
        //int n1=ele->get_fem_noeud(0)->get_numero_opt();
        //int n2=ele->get_fem_noeud(1)->get_numero_opt();
        //int n3=ele->get_fem_noeud(2)->get_numero_opt();
        nbnum++;
        if (nbnum>3) nbnum=3;
        if (nbnum==2) 
              {
              FEM_NOEUD* nvno;
              double res;
              int numno;
              int erreur=resoudgradTtriangle(ele,vitesse,&nvno,&res);
              double T=std::min(res,nvno->get_solution());
              nvno->change_solution(T);
              std::map<FEM_NOEUD*,std::multimap<double,FEM_NOEUD*>::iterator>::iterator it=trialnoeud2.find(nvno);
              if (it!=trialnoeud2.end())
                    {
                    trialnoeud.erase(it->second);
                    trialnoeud2.erase(it);
                    }
              std::pair<double,FEM_NOEUD*> p(T,nvno);
              std::multimap<double,FEM_NOEUD*>::iterator it2=trialnoeud.insert(p);
              std::pair<FEM_NOEUD*,std::multimap<double,FEM_NOEUD*>::iterator > p2(nvno,it2);
              trialnoeud2.insert(p2);
              }
        ele->change_numero(nbnum);
        lst.push_back(ele);
        }
    if (globalerreur==false)
      {
      std::map<FEM_NOEUD*,std::multimap<double,FEM_NOEUD*>::iterator>::iterator it=trialnoeud2.find(no);
      if (it!=trialnoeud2.end())
        {
        trialnoeud.erase(it->second);
        trialnoeud2.erase(it);
        }
      }
    }
finalise();
}
 
 
 
  
int MG_FAST_MARCHING2D::resoudgradTtriangle(FEM_ELEMENT2* ele,double v,FEM_NOEUD** noupdate,double *res)
{
int erreur;
/*switch (versiongradTtriangle)
  {
  case VERSION_GRADT_TRI_SETHIAN:
    erreur=resoudgradTtriangle_sethian(ele,v,noupdate,res);
    break;
  case VERSION_GRADT_TRI_PERSO:
    erreur=resoudgradTtriangle_perso(ele,v,noupdate,res);
    break;
  }  */
 
FEM_NOEUD *noa,*nob,*noc;
int numc;
if (ele->get_fem_noeud(0)->get_numero()==1) {noc=ele->get_fem_noeud(0);numc=0;}
if (ele->get_fem_noeud(1)->get_numero()==1) {noc=ele->get_fem_noeud(1);numc=1;}
if (ele->get_fem_noeud(2)->get_numero()==1) {noc=ele->get_fem_noeud(2);numc=2;}
*noupdate=noc;  
int nummin;
double solmin=1e300;
for (int i=0;i<3;i++)
if (i!=numc)
   {
   double sol=ele->get_fem_noeud(i)->get_solution();
   if (sol<solmin) {solmin=sol;nummin=i;}  
   }
noa=ele->get_fem_noeud(nummin);
if ((nummin!=0) && (numc!=0)) nob=ele->get_fem_noeud(0);
if ((nummin!=1) && (numc!=1)) nob=ele->get_fem_noeud(1);
if ((nummin!=2) && (numc!=2)) nob=ele->get_fem_noeud(2);
double ax=noa->get_x();
double ay=noa->get_y();
double az=noa->get_z();
double bx=nob->get_x();
double by=nob->get_y();
double bz=nob->get_z();
double dx=noc->get_x();
double dy=noc->get_y();
double dz=noc->get_z();
double at=noa->get_solution();
double bt=nob->get_solution();
double alpha;
erreur=val2d(ax,ay,az,bx,by,bz,dx,dy,dz,at,bt,v,(*res),alpha);
return erreur;
  
}
 
/*
int MG_FAST_MARCHING::resoudgradTtriangle_sethian(FEM_ELEMENT2* ele,double v,FEM_NOEUD** noupdate,double *res)
{
FEM_NOEUD *noa,*nob,*noc;
int numc;
if (ele->get_fem_noeud(0)->get_numero()==1) {noc=ele->get_fem_noeud(0);numc=0;}
if (ele->get_fem_noeud(1)->get_numero()==1) {noc=ele->get_fem_noeud(1);numc=1;}
if (ele->get_fem_noeud(2)->get_numero()==1) {noc=ele->get_fem_noeud(2);numc=2;}
*noupdate=noc;  
int nummin;
double solmin=1e300;
for (int i=0;i<3;i++)
 if (i!=numc)
   {
   double sol=ele->get_fem_noeud(i)->get_solution();
   if (sol<solmin) {solmin=sol;nummin=i;}  
   }
noa=ele->get_fem_noeud(nummin);
if ((nummin!=0) && (numc!=0)) nob=ele->get_fem_noeud(0);
if ((nummin!=1) && (numc!=1)) nob=ele->get_fem_noeud(1);
if ((nummin!=2) && (numc!=2)) nob=ele->get_fem_noeud(2);
OT_VECTEUR_3D veca(nob->get_coord(),noc->get_coord());
OT_VECTEUR_3D vecb(noa->get_coord(),noc->get_coord());
double a=veca.get_longueur();
double b=vecb.get_longueur();
OT_VECTEUR_3D normal=veca&vecb;
normal.norme();
double costeta=veca*vecb/a/b;
OT_VECTEUR_3D basex=(-vecb);
OT_VECTEUR_3D basey=normal&basex;
basex.norme();
basey.norme();
OT_MATRICE_3D base(basex,basey,normal);
OT_MATRICE_3D baseinverse=base.transpose();
OT_VECTEUR_3D ptb=(-veca);
OT_VECTEUR_3D baseb=baseinverse*ptb;
if (baseb.get_x()<0)  
       return 1;
double u=nob->get_solution()-noa->get_solution();
if (fabs(u)<1e-10) 
      {
      double nume=a*b*v*sqrt(1-costeta*costeta);
      double deno=a*a+b*b-2*a*b*costeta;
      deno=sqrt(deno);
      *res=noa->get_solution()+nume/deno;
      return 0; 
      }
double aa=a*a+b*b-2.*a*b*costeta;
double bb=2*b*u*(a*costeta-b);
double cc=b*b*(u*u-v*v*a*a*(1.-costeta*costeta));
double delta=bb*bb-4.*aa*cc;
double t1=-bb+sqrt(delta);
double t2=-bb-sqrt(delta);
t1=t1/2./aa;
t2=t2/2./aa;
double t=t1;
if (t<u) t=t2;
double valide=b*(t-u)/t;
if (t<u) 
  valide=-100; 
if ((valide>a*costeta) && (valide<a/costeta))
    *res=std::min(noc->get_solution(),t+noa->get_solution());
  else
    {
    *res=std::min(noc->get_solution(),b*v+noa->get_solution());
    *res=std::min(*res,a*v+nob->get_solution());
    } 
return 0;   
}
  
  
int MG_FAST_MARCHING::resoudgradTtriangle_perso(FEM_ELEMENT2* ele,double v,FEM_NOEUD** noupdate,double *res)
{
double j[9];
double J[9];int li,col;
double N[9]={0.,0.,0.,0.,0.,0.,0.,0.,0.};
double jN[9]={0.,0.,0.,0.,0.,0.,0.,0.,0.};
double T[3];
double gTcst[3]={0.,0.,0.};
double gT[3]={0.,0.,0.};
double uv[2]={0.25,0.25};
 
 
ele->get_inverse_jacob(j,uv);
 
int numno;
for (int i=0;i<2;i++)
        for (int k=0;k<3;k++)
                N[i*3+k]=ele->get_fonction_derive_interpolation(k+1,i+1,uv);
for (int i=0;i<3;i++)
        {
        T[i]=ele->get_fem_noeud(i)->get_solution();
        if (ele->get_fem_noeud(i)->get_numero()==1) 
                {
                *noupdate=ele->get_fem_noeud(i);
                numno=i;
                }
          
        }
 
        
for (int i=0;i<3;i++)
        for (int k=0;k<3;k++)
                for (int l=0;l<3;l++)
                        jN[i*3+k]=jN[i*3+k]+j[i*3+l]*N[l*3+k];
for (int i=0;i<3;i++)
        for (int j=0;j<3;j++)
        {
        if (numno==j)  gT[i]=gT[i]+jN[i*3+j];
          else gTcst[i]=gTcst[i]+jN[i*3+j]*T[j];
        }
double a=gT[0]*gT[0]+gT[1]*gT[1]+gT[2]*gT[2];   
double b=2.*(gTcst[0]*gT[0]+gTcst[1]*gT[1]+gTcst[2]*gT[2]);     
double c=-v*v+gTcst[0]*gTcst[0]+gTcst[1]*gTcst[1]+gTcst[2]*gTcst[2];    
double det=b*b-4.*a*c;
if (det<-1e-8) 
  return 2; 
if (det<0)
  det=0;
else 
  det=sqrt(det);
double sol1=(-b-det)/2./a;
double sol2=(-b+det)/2./a;
double sol=sol1;
if (sol2>sol1) sol=sol2;
 
for (int i=0;i<3;i++)
   if (numno!=i)
     if (sol<ele->get_fem_noeud(i)->get_solution()*0.99)
         return 1;
 
*res=sol;
return 0;
}  */
 
 
 
};