Dijkstra.cpp

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

Dijkstra::Dijkstra(const Mesh * theMesh, const DataEikonal * theData)
:Eikonal(theMesh,theData)
{
      _costVector.clear();    
      evaluateCosts();
}


Dijkstra::~Dijkstra()
{
      _costVector.clear();
}


void  Dijkstra::iterate(const double & integration_time)
{
      double tn=0.0;
      while(!(_trial.empty()) && (tn<= integration_time) )
      {
            // first: bring the smaller element
            QueueTypeIterator itm=_trial.begin(); // bring smaller element
            tn=itm->first;
            // if passed the time, exit and do not pop the element
            if(tn> integration_time)
            {
                  break;
            }
            else
            {
                  size_t pn = itm->second;
                  _known[pn] = true;
                  _tactivOutput[pn]=_tactiv[pn];
                  //pop from queue
                  (_trial).erase(itm);
                  _istrial[pn]=false;
                  _trialPtr[pn] = _trial.end();
                  PointConnect neighborhood = _mesh->pointConnectivity(pn);
                  PointConnectIterator itneigh;
                  for(itneigh =neighborhood.begin(); itneigh!=neighborhood.end(); ++itneigh )
                  {
                        double tij = timeij(pn,*itneigh);
                        if(!(_known[*itneigh]))
                        {
                              _tactiv[*itneigh] = tij;
                              if(_istrial[*itneigh]==false)
                              {
                                    _istrial[*itneigh] = true;
                              }
                              else
                              {
                                  QueueTypeIterator itQueue = _trialPtr[*itneigh];
                                  _trial.erase(itQueue);
                              }
                              _trialPtr[*itneigh]=_trial.insert( timePair(tij,*itneigh)  );
                        }
                        else
                        {
                              if(_tactiv[*itneigh]>tij)
                              {
                                    std::cerr<<integration_time<<" ------------------> "<<_tactiv[*itneigh]<<" "<<tij<<std::endl;
                                    std::cerr<<"ERROR: modifing a visited point"<<std::endl;
                                    exit(1);
                              }
                        }
                  }//end on neighb
            }// end if tn<= IT
      }// End while
}


void Dijkstra::evaluateActivationTimes()
{
      while(!_trial.empty() )
      {
		// first: bring the smaller element
    		QueueTypeIterator itm=_trial.begin(); // bring "cheaper" element
    		size_t pn = itm->second;
    		_known[pn] = true;
    		_tactivOutput[pn]=_tactiv[pn];
    
    		//pop from queue
    		_trial.erase(itm);
		_istrial[pn]=false;
		_trialPtr[pn] = _trial.end();
    
		//now evaluate the neighborhood of the popped point
    		PointConnect neighborhood = _mesh->pointConnectivity(pn);
		PointConnectIterator itneigh;
    		for(itneigh =neighborhood.begin(); itneigh!=neighborhood.end(); ++itneigh )
    		{
			double tij = timeij(pn,*itneigh);
			if(!(_known[*itneigh]))
			{
          			_tactiv[*itneigh] = tij;
			      if(_istrial[*itneigh]==false)
				{
					_istrial[*itneigh] = true;
				}
				else
				{
					QueueTypeIterator itQueue = _trialPtr[*itneigh];
					_trial.erase(itQueue);
				}
				_trialPtr[*itneigh]=_trial.insert( timePair(tij,*itneigh)  );            
			}
        		else
			{
				if(_tactiv[*itneigh]>tij)
          			{
			            std::cerr<<" ------------------> "<<_tactiv[*itneigh]<<" > "<<tij<<std::endl;
            			std::cerr<<"ERROR: modifing a visited point"<<std::endl;
            			exit(1);
          			}
			}
    		}//end on neighb
	}// End while
}


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

void Dijkstra::evaluateCosts()
{
	_costVector.resize(_mesh->num_Points());
	Mesh::PointConnectIterator itcon;
	Chrono chrono;
	chrono.start();
	for (size_t iPt=0; iPt<_mesh->num_Points(); iPt++)
	{
      	Mesh::PointConnect neighb = _mesh->pointConnectivity( iPt);
      	for(itcon=neighb.begin(); itcon!=neighb.end(); ++itcon)
      	{
        		double cost= costij(iPt,*itcon );
			_costVector[iPt].insert(costPtType(*itcon,cost));
      	}
    }
    chrono.stop();
    std::cout<<"cost evaluated in "<<chrono<<std::endl;
}


double Dijkstra::timeij(const size_t & nj, const size_t & ni)
{
	//ni: not visited; nj: visited
	// if it is cheaper to reach ni from nj, so cost is
	// tj + cij; ti otherwise
      double cij=ceij(nj,ni);
	double ti =_tactiv[ni];
	double tj =_tactiv[nj];
	double tij=std::min(ti, (tj+cij)  );
	return(tij);
}


double Dijkstra::ceij(const size_t & nj, const size_t & ni)
{
	double alpha=0.0;
	if((_data->alpha())[ni]>0 && (_data->alpha())[nj] >0)
	{
		alpha=std::max( (_data->alpha())[ni] , (_data->alpha())[nj]  );
	}

	double cij=tMax;
	if(_activateEiko)
	{
		if(alpha<=0)
		{
            	cij=tMax;
		}
        	else
            {
                if(!_data->isComputingDistanceOnly())
                {
                    cij=((_costVector[ni])[nj])/alpha;
                }
                else
                {
                    cij=(_costVector[ni])[nj];
                }
            }
	}
	return(cij);
}


double Dijkstra::costij(const size_t & nj, const size_t & ni)
{

	IndexVector EdgePoints (2,0);
	EdgePoints[0]=ni;
	EdgePoints[1]=nj;
	std::vector<double> tensFib=tensorResistance(EdgePoints);

	//Determine vector vij
	const Point & coordi=_mesh->Pts(ni);
	const Point & coordj=_mesh->Pts(nj);
	std::vector<double> vij(3,0.0);
	for(unsigned char iDim = 0; iDim<3; iDim++)
	{
		vij[iDim] = coordj.coord[iDim] - coordi.coord[iDim];
	}  
	double cij=0.0;
  
	for(unsigned char iDim=0; iDim<3; iDim++)
	{
		for(unsigned char jDim=0; jDim<3; jDim++)
		{
			cij=cij+vij[iDim]*tensFib[RMIndex(iDim,jDim,3)]*vij[jDim];
		}
	}
	cij=sqrt(cij);
	return(cij);
}





/*
double Dijkstra::costij(const size_t & nj, const size_t & ni)
{
  
  //ni: visited; nj: not visited
  double cij=0.0;
  //1) eval fib direction on edge
  fiberDirection & fibni = (_data->fibres(ni));
  fiberDirection & fibnj = (_data->fibres(nj));
  fiberDirection fib;
  double normFib=0.0;
  for(unsigned char icoord=0; icoord<3; icoord++)
  {
    fib.direction[icoord]=0.5*(fibni.direction[icoord]+fibnj.direction[icoord]);
    normFib=normFib+(fib.direction[icoord]*fib.direction[icoord]);
  }
  normFib=sqrt(normFib);
  for(unsigned char icoord=0; icoord<3; icoord++)
  {
    fib.direction[icoord] = fib.direction[icoord]/normFib;
  }
  
  // Determine common regions
  regionLab & region_ni = _mesh->regions(ni);
  regionLab & region_nj = _mesh->regions(nj);
  
  regionLab commonRegions;
  regionLabIterator it_ni, it_nj;
  for(it_ni=region_ni.begin(); (it_ni != region_ni.end());  it_ni++)
  {
    for(it_nj=region_nj.begin(); (it_nj != region_nj.end());  it_nj++)
    {
      if( (*it_ni) == (*it_nj) )
      {
        commonRegions.insert(*it_ni);
      }
    }
  }
  regionLabIterator it_common;
  conductivity edgeConductivity;
  edgeConductivity.l=0.0;
  edgeConductivity.t=0.0;
  edgeConductivity.lambda=0.0;
  edgeConductivity.delta=0.0;
  if(commonRegions.empty())
  {
    std::cerr<<"error: points with no common regions"<<std::endl;
    exit(1);
  }
  for(it_common=commonRegions.begin(); (it_common != commonRegions.end());  it_common++)
  {
    edgeConductivity.l = edgeConductivity.l + (_data->conductivities(*it_common)).l;
    edgeConductivity.t = edgeConductivity.t + (_data->conductivities(*it_common)).t;
    edgeConductivity.lambda = edgeConductivity.lambda +(_data->conductivities(*it_common)).lambda;
    edgeConductivity.delta = edgeConductivity.delta +(_data->conductivities(*it_common)).delta;
  }
  
  double nbreg = static_cast<double>(commonRegions.size());
  edgeConductivity.l = edgeConductivity.l /nbreg;
  edgeConductivity.t = edgeConductivity.t /nbreg;
  edgeConductivity.lambda = edgeConductivity.lambda/nbreg;
  edgeConductivity.delta = edgeConductivity.delta/nbreg;
  
  double ratio_tl = edgeConductivity.t/edgeConductivity.l;
  double coefEq   = edgeConductivity.lambda/(1.0+edgeConductivity.lambda);
  
  double sigma_l = 1.0;
  double sigma_t = (ratio_tl); //(ratio_tl)*(ratio_tl);
  double deltaCond = sigma_l - sigma_t;
  double sigmaEq = coefEq*edgeConductivity.l;
  
  //double alpha=0.5*( (_data->alpha())[ni] + (_data->alpha())[nj]  );
  double tau=0.5*((_data->tau())[ni]+(_data->tau())[nj]);

  //double Ftilda = (alpha/edgeConductivity.delta)*sqrt(2.0*sigmaEq/tau);
  double Ftilda = (1.0/edgeConductivity.delta)*sqrt(2.0*sigmaEq/tau);
  std::vector<double> tensD(9,0.0);
  //Determine conductivity tensor
  for(unsigned char iDim=0; iDim<3; iDim++)
  {
    for(unsigned char jDim=iDim+1; jDim<3; jDim++)
    {
      tensD[RMIndex(iDim,jDim,3)]=deltaCond*(fib.direction[iDim]*fib.direction[jDim]);
      tensD[RMIndex(jDim,iDim,3)]=tensD[RMIndex(iDim,jDim,3)];
    }
    tensD[RMIndex(iDim,iDim,3)]= 1.0*sigma_t + deltaCond*(fib.direction[iDim]*fib.direction[iDim]);
  }
  
  //Evaluating inverse of tensD
  std::vector<double>tensFib=InvertA3X3(tensD);
  //Determine vector vij
  std::vector<double> vij;
  vij.resize(3);
  
  Point & coordi=_mesh->Pts(ni);
  Point & coordj=_mesh->Pts(nj);
  
  for(unsigned char iDim = 0; iDim<3; iDim++)
  {
    vij[iDim] = coordj.coord[iDim] - coordi.coord[iDim];
  }
  
  cij=0.0;
  for(unsigned char iDim=0; iDim<3; iDim++)
  {
    for(unsigned char jDim=0; jDim<3; jDim++)
    {
      cij=cij+vij[iDim]*tensFib[RMIndex(iDim,jDim,3)]*vij[jDim];
    }
  }
  cij=sqrt(cij)/Ftilda;
  
  return(cij);
}
*/