atlasrrt.c

Go to the documentation of this file.

#include "atlasrrt.h"

#include "chart.h"
#include "filename.h"
#include "random.h"
#include "algebra.h"
#include "samples.h"
#include "vector.h"
#include "heap.h"

#include "boolean.h"

#include <string.h>
#include <math.h>

#define ADD_ALL  0

#define ADD_LAST 1

#define ADD_LAST_NO_REP 2

#define ADD_NONE 3

/**********************************************************************************************************/
/**********************************************************************************************************/
/**********************************************************************************************************/

#define EXTEND_RRT      0

#define CONNECT_RRTs    1 

#define CONNECT_SAMPLES 2

#define CONNECT_SAFE    3


typedef struct {
  unsigned int addMode;      
  boolean revisitCharts;     
  boolean checkCollisions;   
  boolean onManifold;        
  boolean explorationSample; 
  double maxLength;          
  double (*costF)(Tparameters*,boolean,double*,void*); 
  void *costData;            
} TAtlasRRTBranchConfig;

/**********************************************************************************************************/
/**********************************************************************************************************/

typedef struct {
  boolean reachedGoal;    
  boolean reachedQrand;   
  boolean reachedTmpQrand;
  boolean blocked;        
  boolean tooFar;         
  boolean collision;      
  boolean validChart;     
  unsigned int lastID;    
  unsigned int nCreatedCharts; 
  unsigned int tree;      
  double *deltaParam;     
  Tvector traversedCharts;
  double desiredLength;   
  double maxLength;       
  double length;          
  double stepLength;      
  double distQrand;       
  double distGoal;        
  double distOrigin;      
  double maxDistOrigin;   
  double delta;           
  double cost;            
} TAtlasRRTBranchStatus;

/**********************************************************************************************************/
/**********************************************************************************************************/

void InitBranchConfig(unsigned int mode,TAtlasRRTBranchConfig *config);


/**********************************************************************************************************/
/**********************************************************************************************************/

void InitBranchStatus(double epsilon,double delta,TAtlasRRTBranchConfig *config,
                      TSampleInfo *near,double *randSample,double *goalSample,
                      Tatlasrrt *ar,TAtlasRRTBranchStatus *status);

boolean canContinueBranch(TAtlasRRTBranchConfig *config,TAtlasRRTBranchStatus *status);

void DeleteBranchStatus(TAtlasRRTBranchStatus *status);

/**********************************************************************************************************/
/**********************************************************************************************************/

void InitSampleInfo(unsigned int m,unsigned int k,TSampleInfo *si);

void SetSampleInfo(unsigned int m,unsigned int k,TSampleInfo *si_dst,TSampleInfo *si_src);

void SetSampleInfoFromNode(unsigned int n,Tatlasrrt *ar,TSampleInfo *si);

void DeleteSampleInfo(TSampleInfo *si);

/**********************************************************************************************************/
/**********************************************************************************************************/
/**********************************************************************************************************/

void NewAtlasRRTBranch(TAtlasRRTStatistics *arst);

void NewAtlasRRTNoEmptyBranch(TAtlasRRTStatistics *arst);

void NewAtlasRRTTreeConnection(TAtlasRRTStatistics *arst);

void NewAtlasRRTNoEmptyTreeConnection(TAtlasRRTStatistics *arst);

void NewAtlasRRTDistanceQrand(double dqr,TAtlasRRTStatistics *arst);

void NewAtlasRRTStep(TAtlasRRTStatistics *arst);

void NewAtlasRRTNoConvergentStep(TAtlasRRTStatistics *arst);

void NewAtlasRRTQrandReached(TAtlasRRTStatistics *arst);

void NewAtlasRRTNotInDomain(TAtlasRRTStatistics *arst);

void NewAtlasRRTCollision(TAtlasRRTStatistics *arst);

void NewAtlasRRTTooLong(TAtlasRRTStatistics *arst);

void NewAtlasRRTTooFar(TAtlasRRTStatistics *arst);

void NewAtlasRRTOverlap(TAtlasRRTStatistics *arst);

void NewAtlasRRTInitChartError(TAtlasRRTStatistics *arst);

void NewAtlasRRTOutOfChart(TAtlasRRTStatistics *arst);

void NewAtlasRRTLargeCurvature(TAtlasRRTStatistics *arst);

void NewAtlasRRTDirLargeCurvature(TAtlasRRTStatistics *arst);

void NewAtlasRRTStepReduction(TAtlasRRTStatistics *arst);

void NewAtlasRRTSample(TAtlasRRTStatistics *arst);

void NewAtlasRRTChart(TAtlasRRTStatistics *arst);

void NewAtlasRRTNoConnectToParent(TAtlasRRTStatistics *arst);
  
void NewAtlasRRTBlockBySingularity(TAtlasRRTStatistics *arst);

void NewAtlasRRTRandomSample(TAtlasRRTStatistics *arst);

void NewAtlasRRTSampleRejection(TAtlasRRTStatistics *arst);

void NewAtlasRRTCollisionCheck(TAtlasRRTStatistics *arst);


/******************************************************************************/
/******************************************************************************/
/******************************************************************************/

void InitAtlasRRTStatistics(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL)
    {
      arst->n=0;

      arst->nBranch=0;
      arst->nNoEmptyBranch=0;

      arst->nTreeConnection=0;
      arst->nNoEmptyTreeConnection=0;

      arst->nStep=0;
      arst->dQrand=0.0;

      arst->nQrandReached=0;
      arst->nNotInDomain=0;
      arst->nCollision=0;
      arst->nTooFar=0;
      arst->nTooLong=0;
      arst->nOverlap=0;

      arst->nErrorNewChart=0;
      arst->nNoConvergent=0;
      arst->nOutOfChart=0;
      arst->nLargeCurvature=0;
      arst->nDirLargeCurvature=0;
  
      arst->nStepReduction=0;

      arst->nSample=0;
      arst->nChart=0;
      arst->nNoConnect=0;
      arst->nSingular=0;

      arst->nRandom=0;
      arst->nRejections=0;

      arst->nCollisionChecks=0;
    }
}

void NewAtlasRRTBranch(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->nBranch++;
}

void NewAtlasRRTNoEmptyBranch(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->nNoEmptyBranch++;
}

void NewAtlasRRTTreeConnection(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->nTreeConnection++;
}

void NewAtlasRRTNoEmptyTreeConnection(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->nNoEmptyTreeConnection++;
}

void NewAtlasRRTDistanceQrand(double dqr,TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->dQrand+=dqr;
}

void NewAtlasRRTStep(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->nStep++;
}

void NewAtlasRRTNoConvergentStep(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->nNoConvergent++;
}

void NewAtlasRRTQrandReached(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->nQrandReached++;
}

void NewAtlasRRTNotInDomain(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->nNotInDomain++;
}

void NewAtlasRRTCollision(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->nCollision++;
}

void NewAtlasRRTTooLong(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->nTooLong++;
}

void NewAtlasRRTTooFar(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->nTooFar++;
}

void NewAtlasRRTOverlap(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->nOverlap++;
}

void NewAtlasRRTOutOfChart(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->nOutOfChart++;
}

void NewAtlasRRTInitChartError(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->nErrorNewChart++;
}

void NewAtlasRRTLargeCurvature(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->nLargeCurvature++;
}

void NewAtlasRRTDirLargeCurvature(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->nDirLargeCurvature++;
}

void NewAtlasRRTStepReduction(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->nStepReduction++;
}

void NewAtlasRRTSample(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->nSample++;
}

void NewAtlasRRTChart(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->nChart++;
}

void NewAtlasRRTNoConnectToParent(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->nNoConnect++;
}

void NewAtlasRRTBlockBySingularity(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->nSingular++;
}

void NewAtlasRRTRandomSample(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->nRandom++;
}

void NewAtlasRRTSampleRejection(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->nRejections++;
}

void NewAtlasRRTCollisionCheck(TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) arst->nCollisionChecks++;
}

void AccumulateAtlasRRTStatistics(TAtlasRRTStatistics *arst1,TAtlasRRTStatistics *arst2)
{
  if ((arst1!=NULL)&&(arst2!=NULL))
    {
      if (arst1->n==0)
        arst2->n++;
      else
        arst2->n+=arst1->n;

      arst2->nBranch+=arst1->nBranch;
      arst2->nNoEmptyBranch+=arst1->nNoEmptyBranch;

      arst2->nTreeConnection+=arst1->nTreeConnection;
      arst2->nNoEmptyTreeConnection+=arst1->nNoEmptyTreeConnection;

      arst2->nStep+=arst1->nStep;
      arst2->dQrand+=arst1->dQrand;

      arst2->nQrandReached+=arst1->nQrandReached;
      arst2->nNotInDomain+=arst1->nNotInDomain;
      arst2->nCollision+=arst1->nCollision;
      arst2->nTooLong+=arst1->nTooLong;
      arst2->nTooFar+=arst1->nTooFar;
      arst2->nOverlap+=arst1->nOverlap;

      arst2->nErrorNewChart+=arst1->nErrorNewChart;
      arst2->nNoConvergent+=arst1->nNoConvergent;
      arst2->nOutOfChart+=arst1->nOutOfChart;
      arst2->nLargeCurvature+=arst1->nLargeCurvature;
      arst2->nDirLargeCurvature+=arst1->nDirLargeCurvature;
  
      arst2->nStepReduction+=arst1->nStepReduction;

      arst2->nSample+=arst1->nSample;
      arst2->nChart+=arst1->nChart;

      arst2->nNoConnect+=arst1->nNoConnect;
      arst2->nSingular+=arst1->nSingular;

      arst2->nRandom+=arst1->nRandom;
      arst2->nRejections+=arst1->nRejections;

      arst2->nCollisionChecks+=arst1->nCollisionChecks;
    }
}

void PrintAtlasRRTStatistics(Tatlasrrt *ar,TAtlasRRTStatistics *arst)
{
  if (arst!=NULL) 
    {
      unsigned int m,nExt;

      if (arst->n==0)
        arst->n=1;

      nExt=arst->nBranch+arst->nTreeConnection;

      fprintf(stderr,"%% **************************************************\n");
      if (arst->n>1)
        fprintf(stderr,"AtlasRRT Statistics (averaged over %u repetitions)\n",arst->n);
      else
        fprintf(stderr,"AtlasRRT Statistics\n\n");
      fprintf(stderr,"  Num. random samples             : %.2f\n",
              (double)arst->nRandom/(double)arst->n);
      fprintf(stderr,"  Num. rejected random samples    : %.2f (%.2f)\n",
              (double)arst->nRejections/(double)arst->n,
              (double)arst->nRejections/(double)arst->nRandom);
      fprintf(stderr,"  Num. accepted random samples    : %.2f (%.2f)\n",
              (double)(arst->nRandom-arst->nRejections)/(double)arst->n,
              (double)(arst->nRandom-arst->nRejections)/(double)arst->nRandom);
  
      /* if the chart sampling radius is potentially different for each chart
         we print information about the extreme/average chart sampling radius. */
      #if (ADJUST_SA==1)
        if ((arst->n==1)&&(ar!=NULL))
          {
            unsigned int i;
            double miSr,maSr,avSr,sr;

            miSr=+INF;
            maSr=-INF;
            avSr=0.0;
            for(i=0;i<ar->nc;i++)
              {
                sr=GetChartSamplingRadius(GetAtlasChart(i,&(ar->atlas)));
                if (sr<miSr)
                  miSr=sr;
                if (sr>maSr)
                  maSr=sr;
                avSr+=sr;
              }
            fprintf(stderr,"  Sampling radius info            : %.2f <-- %.2f --> %.2f\n",
                    miSr,avSr/(double)ar->nc,maSr);
          }
      #endif

      if ((arst->n==1)&&(ar!=NULL)&&(DynamicDomainRRT(&(ar->rrt))))
        {
          unsigned int i,n;
          double miDD,maDD,avDD,r;

          miDD=+INF;
          maDD=-INF;
          avDD=0.0;
          n=0;
          for(i=0;i<ar->ns;i++)
            {
              r=GetDynamicDomainRadius(i,&(ar->rrt));
              if (r>0)
                {
                  if (r<miDD)
                    miDD=r;
                  if (r>maDD)
                    maDD=r;
                  avDD+=r;
                  n++;
                }
            }
          if (n==0)
            fprintf(stderr,"  No dynamic domain effect.");
          else
            fprintf(stderr,"  Dynamic domain radius           : %.2f <-- %.2f --> %.2f (%u/%u)\n",
                    miDD,avDD/(double)n,maDD,n,ar->ns);
        }
      fprintf(stderr,"  Average  distance to Qrand      : %.2f\n",
              arst->dQrand/(double)arst->nBranch);

      if (arst->nTreeConnection>0)
        {
          fprintf(stderr,"  Num. direct RRT extensions      : %.2f\n",
                  (double)arst->nBranch/(double)arst->n);
          m=arst->nNoEmptyBranch;
          fprintf(stderr,"  Num. no-empty direct extensions : %.2f (%.2f)\n",
                  (double)m/(double)arst->n,
                  (double)m/(double)arst->nBranch);  
      
          fprintf(stderr,"  Num. RRT connections            : %.2f\n",
                  (double)arst->nTreeConnection/(double)arst->n);
          m=arst->nNoEmptyTreeConnection;
          fprintf(stderr,"  Num. no-empty RRT connections   : %.2f (%.2f)\n",
                  (double)m/(double)arst->n,
                  (double)m/(double)arst->nTreeConnection);
      
          fprintf(stderr,"  Total branches                  : %.2f\n",
                  (double)nExt/(double)arst->n);
          m=(arst->nNoEmptyBranch+arst->nNoEmptyTreeConnection);
          fprintf(stderr,"  Total no-empty branches         : %.2f (%.2f)\n",
                  (double)m/(double)arst->n,
                  (double)m/(double)nExt);
        }
      else
        {
          fprintf(stderr,"  Total branches                  : %.2f\n",
                  (double)arst->nBranch/(double)arst->n);
          m=arst->nNoEmptyBranch;
          fprintf(stderr,"  Total no-empty branches         : %.2f (%.2f)\n",
                  (double)m/(double)arst->n,
                  (double)m/(double)arst->nBranch);  
        }
      fprintf(stderr,"  Reasons to stop the branch/connection\n");
      m=arst->nQrandReached;
      fprintf(stderr,"    q_rand reached                : %.2f (%.2f)\n",
              (double)m/(double)arst->n,
              (double)m/(double)nExt);

      m=arst->nNotInDomain;
      fprintf(stderr,"    Out of domain                 : %.2f (%.2f)\n",
              (double)m/(double)arst->n,
              (double)m/(double)nExt);

      m=arst->nCollision;
      fprintf(stderr,"    Collision                     : %.2f (%.2f)\n",
              (double)m/(double)arst->n,
              (double)m/(double)nExt);

      m=arst->nTooLong;
      fprintf(stderr,"    Long branch                   : %.2f (%.2f)\n",
              (double)m/(double)arst->n,
              (double)m/(double)nExt);

      m=arst->nTooFar;
      fprintf(stderr,"    Too far from q_near           : %.2f (%.2f)\n",
              (double)m/(double)arst->n,
              (double)m/(double)nExt);

      m=arst->nOverlap;
      fprintf(stderr,"    Avoid overlap                 : %.2f (%.2f)\n",
              (double)m/(double)arst->n,
              (double)m/(double)nExt);

      m=arst->nSingular;
      fprintf(stderr,"    Singularity                   : %.2f (%.2f)\n",
              (double)m/(double)arst->n,
              (double)m/(double)nExt);

      fprintf(stderr,"  Num. extension steps            : %.2f\n",
              (double)arst->nStep/(double)arst->n);  
      fprintf(stderr,"    Num. steps per branch         : %.2f\n",
              (double)arst->nStep/(double)nExt);
      fprintf(stderr,"    Num. step reductions          : %.2f\n",
              (double)arst->nStepReduction/(double)arst->n);

      fprintf(stderr,"  Num. collision checks           : %.2f\n",
              (double)arst->nCollisionChecks/(double)arst->n);


      fprintf(stderr,"  Num. samples in (bi)RRT         : %.2f\n",
              (double)arst->nSample/(double)arst->n);
      fprintf(stderr,"    Samples per non-empty extens. : %.2f\n",
              (double)arst->nSample/(double)(arst->nNoEmptyBranch+arst->nNoEmptyTreeConnection));

      fprintf(stderr,"  Num. charts                     : %.2f\n",
              (double)arst->nChart/(double)arst->n);

      fprintf(stderr,"  Reasons to create charts\n");

      m=arst->nErrorNewChart;
      fprintf(stderr,"    Error defining chart          : %.2f (%.2f)\n",
              (double)m/(double)arst->n,
              (double)m/(double)arst->nChart);

      m=arst->nOutOfChart; 
      fprintf(stderr,"    Out of radius                 : %.2f (%.2f)\n",
              (double)m/(double)arst->n,
              (double)m/(double)arst->nChart);

      m=arst->nNoConvergent;
      fprintf(stderr,"    Large or convergence error    : %.2f (%.2f)\n",
              (double)m/(double)arst->n,
              (double)m/(double)arst->nChart);

      #if (GET_ATLASRRT_GLOBAL_CURV_CHECK)
        m=arst->nLargeCurvature;
        fprintf(stderr,"    Curvature error             : %.2f (%.2f)\n",
                (double)m/(double)arst->n,
                (double)m/(double)arst->nChart);
      #endif

      m=arst->nDirLargeCurvature;
      fprintf(stderr,"    Directional curvature error   : %.2f (%.2f)\n",
              (double)m/(double)arst->n,
              (double)m/(double)arst->nChart);

      m=arst->nNoConnect;
      fprintf(stderr,"  Num. no-intersect with parent   : %.2f (%.2f)\n",
              (double)m/(double)arst->n,
              (double)m/(double)arst->nChart);

      fprintf(stderr,"  Samples per chart               : %.2f\n",
              (double)arst->nSample/(double)arst->nChart);
    }
}

void DeleteAtlasRRTStatistics(TAtlasRRTStatistics *arst)
{
}


/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
double New_AddBranchToAtlasRRT(Tparameters *pr,unsigned int it,TAtlasRRTBranchConfig *config,
                               unsigned int i_near,double *randSample,double *goalSample,
                               void *st,TAtlasRRTBranchStatus *status,
                               unsigned int *ns,double ***path,
                               Tatlasrrt *ar);

double AddBranchToAtlasRRT(Tparameters *pr,unsigned int it,
                           unsigned int addMode,boolean revisitCharts,
                           boolean checkCollisions,boolean onManifold,
                           boolean explorationSample,
                           double maxLength,unsigned int i_near,
                           double *origRandSample,
                           double *goalSample,void *st,
                           unsigned int *lastSampleID,
                           boolean *reachedQRand,boolean *reachedGoal,
                           unsigned int *ns,double ***path,
                           double (*costF)(Tparameters*,boolean,double*,void*),
                           void *costData,
                           Tatlasrrt *ar);


void New_NewTemptativeSample(Tparameters *pr,unsigned int it,
                             TAtlasRRTBranchConfig *config,
                             TAtlasRRTBranchStatus *status,
                             TSampleInfo *current,TSampleInfo *next,
                             TSampleInfo *rand,double *randSample,
                             void * st,Tatlasrrt *ar);

void NewTemptativeSample(Tparameters *pr,unsigned int it,unsigned int *nChanges,
                         boolean revisitCharts,boolean chartCreated,
                         boolean checkCollisions,boolean onManifold,
                         TSampleInfo *currentSampleInfo,double *currentParam,
                         double *origRandSample,
                         unsigned int *randChart,double *randParam,double *randSample,
                         double currentDelta,double d,double *deltaParam,
                         unsigned int *nextChart,double *nextParam,double *nextSample,
                         void *st,double (*costF)(Tparameters*,boolean,double*,void*),
                         void *costData,double *cost,boolean *blocked,
                         boolean *inCollision,boolean *valid,
                         Tatlasrrt *ar);

double AddSample2AtlasRRT(Tparameters *pr,unsigned int tree,
                          unsigned int *currentID,
                          unsigned int nextChart,double *nextParam,double *nextSample,
                          double dp,double cost,Tatlasrrt *ar);
boolean AddChart2AtlasRRT(Tparameters *pr,unsigned int tree,unsigned int it,
                          TSampleInfo *currentSampleInfo,
                          boolean* intersectParent,Tatlasrrt *ar);

void PopulateWithSamples(Tparameters *pr,unsigned int id,Tatlasrrt *ar);

void AddChart2Tree(unsigned int tree,unsigned int chartId,Tatlasrrt* ar);


boolean New_PointTowardRandSample(TAtlasRRTBranchConfig *config,
                                  unsigned int samplingMode,double *randSample,
                                  TAtlasRRTBranchStatus *status,
                                  TSampleInfo *current,TSampleInfo *rand,Tatlasrrt *ar);

boolean PointTowardRandSample(unsigned int cId,double d,double *t,
                              unsigned int samplingMode,
                              boolean onManifold,double *origRandSample,
                              unsigned int *randChart,
                              double *randParam,double *randSample,
                              double *deltaParam,Tatlasrrt* ar);

unsigned int ReconstructAtlasRRTPath(Tparameters *pr,unsigned int sID,
                                     double *pl,unsigned int *ns,
                                     double ***path,Tatlasrrt *ar);

unsigned int Steps2PathinAtlasRRT(Tparameters *pr,Tvector *steps,double *pl,double* pc,
                                  unsigned int *ns,double ***path,Tatlasrrt *ar);

void SmoothPathInAtlasRRT(Tparameters *pr,unsigned int sID,Tatlasrrt* ar);

unsigned int AddStepToAtlasRRTstar(Tparameters* pr,
                                   unsigned int it,boolean expand2Goal,
                                   unsigned int i_near,double *q_rand,
                                   unsigned int *id_goal,double *goal,
                                   TAtlasRRTStatistics *arst,
                                   Tatlasrrt *ar);

void WireAtlasRRTstar(Tparameters *pr,
                      unsigned int id_new,unsigned int i_near,double gamma,
                      unsigned int nn,unsigned int *n,double **c,
                      double h,Theap *q,unsigned int *t,
                      TAtlasRRTStatistics *arst,Tatlasrrt *ar);

void ReWireAtlasRRTstar(Tparameters *pr,
                        unsigned int id_new,double gamma,
                        unsigned int nn,unsigned int *n,double *c,
                        Tvector *steps,double *l,
                        TAtlasRRTStatistics *arst,Tatlasrrt *ar);

void AtlasRRTstarCloseIteration(unsigned int it,unsigned int id_goal,
                                double time,double gamma,
                                double *times,double *costs,
                                Tatlasrrt *ar);

void AtlasBiRRTstarCloseIteration(unsigned int it,double l,
                                  double time,double gamma,
                                  double *times,double *costs,
                                  Tatlasrrt *ar);

void SaveAtlasRRTSampleInfo(FILE *f,TSampleInfo *si,Tatlasrrt *ar);

void SaveAtlasRRTChartInfo(FILE *f,TChartInfo *ci,Tatlasrrt *ar);


void LoadAtlasRRTSampleInfo(FILE *f,TSampleInfo *si,Tatlasrrt *ar);

void LoadAtlasRRTChartInfo(FILE *f,TChartInfo *ci,Tatlasrrt *ar);

void PlotQrand(Tparameters *pr,char *prefix,unsigned int inear,
               unsigned int c_rand,double *t_rand,double *q_rand,
               unsigned int xID,unsigned int yID,unsigned int zID,
               Tatlasrrt *ar);

void PlotConnection(Tparameters *pr,char *prefix,
                    unsigned int target,unsigned int near,unsigned int end,
                    unsigned int xID,unsigned int yID,unsigned int zID,
                    Tatlasrrt *ar);

/******************************************************************************/
/******************************************************************************/
/******************************************************************************/

void InitBranchStatus(double epsilon,double delta,TAtlasRRTBranchConfig *config,
                      TSampleInfo *near,double *randSample,double *goalSample,
                      Tatlasrrt *ar,TAtlasRRTBranchStatus *status)
{
  /* Check if q_rand is already in the tree almost reached by the tree */
  status->distQrand=DistanceTopology(ar->m,ar->tp,near->s,randSample);
  if (config->onManifold)
    status->reachedQrand=(status->distQrand<epsilon);
  else
    status->reachedQrand=FALSE;
  /* initially the temporay q_rand will be the same as q_rand */
  status->reachedTmpQrand=status->reachedQrand;

  /* Check if goal is already reached */
  #if (EXPLORATION_RRT)
    status->distGoal=INF;
  #else
    if (goalSample!=NULL)
      status->distGoal=DistanceTopology(ar->m,ar->tp,near->s,goalSample);
    else
      status->distGoal=INF;
  #endif
  status->reachedGoal=(status->distGoal<delta);
   
  /* The new brach will belong to the tree of i_near */
  status->tree=GetRRTNodeTree(near->id,&(ar->rrt));

  status->nCreatedCharts=0;

  /* last sample in the branch */
  status->lastID=near->id;
  
  /* Interpolation step */
  status->delta=delta;

  /* Advance direction in tangent space (this is set in
     PointTowardRandSample) */
  NEW(status->deltaParam,ar->k,double);

  /* Vector of visited charts */
  InitVector(sizeof(unsigned int),CopyID,NULL,100,&(status->traversedCharts));
  NewVectorElement((void*)&(near->c),&(status->traversedCharts));
  
  /* Severeral branh advance diagnostics */
  status->blocked=FALSE; /* The branch is stopped for whatever reason */
  status->tooFar=FALSE;  /* The end of the branch is too far */
  status->collision=FALSE; /* The branch collides with an obstacle */
  status->validChart=TRUE; /* The current chart is not valid. */

  /* Cost of the next node (this value is actually not used) */
  status->cost=0;

  /* Set the parameters defining the maximum span of the branch */
  if (config->maxLength<=0.0)
    {
      /* maxLenght not set -> auto-setting of the parameres */
      status->desiredLength=status->distQrand;
      status->maxLength=2.0*status->distQrand;
    }
  else
    {
      /* Use the parameters given by the user.
         Set desired=max to make the branch to grow to its maximum, if needed. */
      status->desiredLength=config->maxLength;
      status->maxLength=config->maxLength;
    }
  
  /* Branch length so far */
  status->length=0.0;

  /* maximum distance to origin */
  status->maxDistOrigin=status->distQrand;
  if (config->onManifold)
    status->maxDistOrigin*=1.1; /* add some margin */
  
  /* distance from the last node of the branch (so far) to the first one */
  status->distOrigin=0;
  
  /* Length of the last step along the branch */
  status->stepLength=0.0;
}


void DeleteBranchStatus(TAtlasRRTBranchStatus *status)
{
  free(status->deltaParam);
  DeleteVector(&(status->traversedCharts));
}

boolean canContinueBranch(TAtlasRRTBranchConfig *config,
                          TAtlasRRTBranchStatus *status)
{
  /* collision -> blocked but we include in anyway */
  return((!status->blocked)&&(!status->collision)&&
         (status->length<status->maxLength)&&
         (!status->reachedTmpQrand)&&(!status->reachedGoal)&&
         (!status->reachedQrand)&&(!status->tooFar));
}

void InitBranchConfig(unsigned int mode,TAtlasRRTBranchConfig *config)
{ 
  switch (mode)
    {
    case EXTEND_RRT:
      config->addMode=ADD_ALL;
      config->revisitCharts=TRUE;
      config->checkCollisions=TRUE;
      config->onManifold=FALSE; /* will vary */
      config->explorationSample=FALSE; /* vary */
      config->maxLength=0.0;
      config->costF=NULL;
      config->costData=NULL;
      break;
    case CONNECT_RRTs:
      config->addMode=ADD_ALL;
      config->revisitCharts=FALSE;
      config->checkCollisions=TRUE;
      config->onManifold=TRUE;
      config->explorationSample=FALSE;
      config->maxLength=0.0;
      config->costF=NULL;
      config->costData=NULL;
      break;
    case CONNECT_SAMPLES:
      config->addMode=ADD_NONE;
      config->revisitCharts=TRUE;
      config->checkCollisions=TRUE;
      config->onManifold=TRUE;
      config->explorationSample=FALSE;
      config->maxLength=0.0;
      config->costF=NULL;
      config->costData=NULL;
      break;
    case CONNECT_SAFE:
      config->addMode=ADD_NONE;
      config->revisitCharts=TRUE;
      config->checkCollisions=FALSE;
      config->onManifold=TRUE;
      config->explorationSample=FALSE;
      config->maxLength=0.0;
      config->costF=NULL;
      config->costData=NULL;
      break;
    default:
      Error("Unknown mode in InitBranchConfig");
    }
}

void InitSampleInfo(unsigned int m,unsigned int k,TSampleInfo *si)
{
  si->id=NO_UINT;
  NEW(si->s,m,double);
  NEW(si->t,k,double);
  si->pc=NO_UINT;
  si->c=NO_UINT;
  si->generateChart=0; /* FALSE */
  si->lsc=NO_UINT;
}

void SetSampleInfo(unsigned int m,unsigned int k,
                   TSampleInfo *si_dst,TSampleInfo *si_src)
{
  si_dst->id=si_src->id;
  memcpy(si_dst->s,si_src->s,m*sizeof(double));
  memcpy(si_dst->t,si_src->t,k*sizeof(double));
  si_dst->pc=si_src->pc;
  si_dst->c=si_src->c;
  si_dst->generateChart=si_src->generateChart;
  si_dst->lsc=si_src->lsc;
}

void SetSampleInfoFromNode(unsigned int n,Tatlasrrt *ar,TSampleInfo *si)
{
  SetSampleInfo(ar->m,ar->k,si,ar->si[n]);
}

void DeleteSampleInfo(TSampleInfo *si)
{
  free(si->s);
  free(si->t);
}

double New_AddBranchToAtlasRRT(Tparameters *pr,unsigned int it,
                               TAtlasRRTBranchConfig *config,
                               unsigned int i_near,
                               double *randSample,double *goalSample,
                               void *st,TAtlasRRTBranchStatus *status,
                               unsigned int *ns,double ***path,
                               Tatlasrrt *ar)
{
  TSampleInfo current,next,rand,*near;
  boolean initData,intersectParent;
  double epsilon,delta;
  unsigned int ms,maxNodes,samplingMode;
  
  /* Sanity check */
  if (i_near==NO_UINT)
    Error("Extending a branch from nowhere");
  
  /* Get the parameters */
  epsilon=GetParameter(CT_EPSILON,pr);
  delta=GetParameter(CT_DELTA,pr);
  maxNodes=(unsigned int)GetParameter(CT_MAX_NODES_RRT,pr);
  samplingMode=(unsigned int)GetParameter(CT_SAMPLING,pr);

  /* Info on the neareast sample */
  near=ar->si[i_near];
  
  /* Initialize the status of the branch */
  InitBranchStatus(epsilon,delta,config,near,randSample,goalSample,ar,status);
  
  /* If we have to store the path, init the storage*/
  if ((ns!=NULL)&&(path!=NULL))
    InitSamples(&ms,ns,path);

  if ((status->reachedQrand)||(status->reachedGoal))
    /* If q_rand/goal are alrady reached, there is not point
       in initializing the structures. */
    initData=FALSE; 
  else
    {
      /* If the goal sample is not already reached, we prepare for the
         branch extension */
      initData=TRUE;
      
      /* q_near is the first 'current' (sample) of the branch */
      InitSampleInfo(ar->m,ar->k,&current);
      SetSampleInfoFromNode(i_near,ar,&current);

      InitSampleInfo(ar->m,ar->k,&next);
      InitSampleInfo(ar->m,ar->k,&rand);

      /* get a copy of the random sample. This will be modified while
         extending the branch. 'rand' is a temporary representation of
         'randSample' in the current chart, which changes along the
         branch */
      memcpy(rand.s,randSample,ar->m*sizeof(double));

      /* Detemine the direction of motion toward 'rand' from the 'current' */
      status->blocked=New_PointTowardRandSample(config,samplingMode,randSample,status,
                                                &current,&rand,ar);
    }

  #if (_DEBUG>1)
    printf("  Adding branch to RRT from sample %u (chart %u)\n",current.id,current.c);
  #endif
      
  /* We stop if blocked, after a maximum dist extansion or 
     if it gets farther than the ambient dist to the randSample. This
     is measured in two ways: the distance traveled over the monifold 
     (status->length) and the distance from the current sample to q_near 
     (flag status->tooFar).
     The new branch should not scape the ball of radious maxR centered
     at q_near. We allow for a larger displacement over the manifold to 
     account for curvature effects.
  */
  while (canContinueBranch(config,status)&&((config->addMode==ADD_NONE)||(ar->ns<maxNodes)))
    {
      /* Generate the new sample from current and rand.  */
      New_NewTemptativeSample(pr,it,config,status,&current,&next,
                              &rand,randSample,st,ar);

      if (!status->blocked)
        {
          /* If we actually managed to generate the temptative sample */
          if (!status->validChart)
            {                         
              /* We need to create a new chart in the current sample
                 (i.e., the new temptative sample is not valid) */
              if (current.generateChart)
                {
                  /* But current samples was already been used to create 
                     a chart. We reduce the step size with the aim of finding 
                     a point close to the current one where to create a chart. */
                  status->delta*=0.5;
                  status->validChart=TRUE; /* re-try with the previous chart */
                  #if (GET_ATLASRRT_STATISTICS)
                    NewAtlasRRTStepReduction((TAtlasRRTStatistics *)st);
                  #endif
                }
              else
                {
                  /* We generate the new chart (this will change 'current') */
                  if (AddChart2AtlasRRT(pr,status->tree,it,&current,
                                        &intersectParent,ar))
                    {
                      /* the chart generation was fine */
                      #if (GET_ATLASRRT_STATISTICS)
                        NewAtlasRRTChart((TAtlasRRTStatistics *)st);
                        if(!intersectParent)
                          NewAtlasRRTNoConnectToParent((TAtlasRRTStatistics *)st);
                      #endif

                      /* Keep track of the new visited chart */
                      NewVectorElement((void*)&(current.c),&(status->traversedCharts));
                      
                      /* The branch created one new more chart */
                      status->nCreatedCharts++;

                      /* We have a new valid chart */
                      status->validChart=TRUE;
                      
                      /* Reset the advance step */
                      status->delta=delta;
                      
                      /* Re-define the advance direction (i.e., rand and 
                         status.deltaParam) in the new chart */
                      status->blocked=New_PointTowardRandSample(config,samplingMode,
                                                                randSample,status,
                                                                &current,&rand,ar);
                      
                      #if (GET_ATLASRRT_STATISTICS)
                        if (status->blocked)
                          NewAtlasRRTQrandReached((TAtlasRRTStatistics *)st);
                      #endif
                    }
                  else 
                    {
                      /* There was an error creating the chart: Stop the 
                         branch extension.
                         TODO: Implement a more sophisticated stratetgy to deal
                         with singular regions.
                       */
                      status->blocked=TRUE; 
                      fprintf(stderr,"  Point in singularity (decrease epsilon?)\n");
                      #if (GET_ATLASRRT_STATISTICS)
                        NewAtlasRRTBlockBySingularity((TAtlasRRTStatistics *)st);
                        NewAtlasRRTInitChartError((TAtlasRRTStatistics *)st);
                      #endif
                    }
                }
            }
          else
            {         
              /* The temptative sample is correct. We can advance along the branch. */
              status->reachedTmpQrand=((rand.c==next.c)&&
                                       (Distance(ar->k,rand.t,next.t)<epsilon));

              status->distQrand=DistanceTopology(ar->m,ar->tp,
                                                 next.s,randSample);
              if (config->onManifold)
                status->reachedQrand=(status->distQrand<epsilon);

              #if (!EXPLORATION_RRT)
                if (goalSample!=NULL)
                  {
                    status->distGoal=DistanceTopology(ar->m,ar->tp,
                                                      next.s,goalSample);
                    status->reachedGoal=(status->distGoal<epsilon);
                  }
              #endif
                
              #if (GET_ATLASRRT_STATISTICS)
                if ((status->reachedQrand)||(status->reachedTmpQrand)||
                    (status->reachedGoal))
                  NewAtlasRRTQrandReached((TAtlasRRTStatistics *)st);
              #endif

              status->distOrigin=DistanceTopology(ar->m,ar->tp,next.s,near->s);
              status->tooFar=(status->distOrigin>status->maxDistOrigin);

              if (!status->tooFar)
                {
                  /* Add the sample to the tree, if required */
                  status->stepLength=DistanceTopology(ar->m,ar->tp,
                                                      current.s,next.s);
                   
                  if (config->addMode==ADD_ALL)
                    {
                      status->length+=AddSample2AtlasRRT(pr,status->tree,
                                                         &(status->lastID), 
                                                         next.c,next.t,next.s,
                                                         status->stepLength,
                                                         status->cost,ar);
                      /* Current is the last node added to the AtlasRRT */
                      SetSampleInfoFromNode(status->lastID,ar,&current);
                    }
                  else
                    {
                      status->length+=status->stepLength;
                       /* current <- next (this sets current.generateChart=0) */
                      SetSampleInfo(ar->m,ar->k,&current,&next);
                    }
                  
                  /* Reset the advance step */
                  status->delta=delta;

                  #if (GET_ATLASRRT_STATISTICS)
                    if (status->length>=status->maxLength)
                      NewAtlasRRTTooLong((TAtlasRRTStatistics *)st);

                    NewAtlasRRTSample((TAtlasRRTStatistics *)st);
                  #endif
                   
                  /* Add the sample the path, if required */
                  if ((ns!=NULL)&&(path!=NULL)&&
                      (!status->reachedTmpQrand)&&(!status->reachedQrand)&&
                      (!status->reachedGoal))
                    AddSample2Samples(ar->m,next.s,ar->m,NULL,&ms,ns,path);
                }
              #if (GET_ATLASRRT_STATISTICS)
              else
                NewAtlasRRTTooFar((TAtlasRRTStatistics *)st);
              #endif
            }
        }
    }
  
  /* The branch is alrady defined. Check what to do with the last sample along
     the branch, release memory, and adjust the final branch lenght. */
  if (initData)
    { 
      if ((status->length>0)&&
          ((config->addMode==ADD_LAST)||((config->addMode==ADD_LAST_NO_REP))))
        {
          boolean noRep;
          
          /* In some branch configurations, the last sample has to be added
             to the RRT, in some cases avoiding repetitions. */
          if (config->addMode==ADD_LAST_NO_REP)
            {
              unsigned int nn;
          
              nn=GetRRTNN(status->tree,current.s,&(ar->rrt));
              noRep=(DistanceTopology(ar->m,ar->tp,
                                      ar->si[nn]->s,current.s)>epsilon);
            }
          else
            noRep=TRUE;

          if (noRep)
            {
              if ((!current.generateChart)&&(Norm(ar->k,current.t)<epsilon))
                Warning("Adding a sample on a chart center");
              
              /* If it was required to advance toward q_rand and we actually 
                 moved in that direction and we have to add the last computed
                 sample -> add it (with distance to parent = branch length) */
              /* Here lastID will be i_near if no sample was added yet */
              AddSample2AtlasRRT(pr,status->tree,&(status->lastID), 
                                 current.c,current.t,current.s,
                                 status->length,status->cost,ar);
              /* In some rare ocassions, the last sample of the branch 
                 (the one we just added to the tree) has been used to
                 generate a chart.  Mark it to avoid the generation of
                 a new chart on this same sample latter on. When adding
                 all samples along the branch to the RRT, none of the
                 added charts can be the center of a chart (charts are
                 only defined on samples already in the tree). When adding
                 only the last sample to the tree, charts are created on
                 temporary samples that might be included in the tree if
                 they are the last sample along a branch. */
              ar->si[status->lastID]->generateChart=current.generateChart;
            }
        }
      
      /* Update the statistic of how far we can get from i_near */
      /* This only has effect if paramter DYNAMIC_DOMAIN is >0 */
      AdjustDynamicDomain(i_near,status->collision,&(ar->rrt));

      DeleteSampleInfo(&current);
      DeleteSampleInfo(&next);
      DeleteSampleInfo(&rand);
    }

  /* Final adjust of the branch lenght: Add the distance from the last 
     sample in the branch to the goal/q_rand (this is epsilon size) */
  if (status->reachedQrand)
    status->length+=status->distQrand;
  else
    {
      if (status->reachedGoal)
        status->length+=status->distGoal;
    }

  /* Release the possible memory allocated by the InitBranchStatus, if any */
  DeleteBranchStatus(status);
  
  return(status->length);
}

void New_NewTemptativeSample(Tparameters *pr,unsigned int it,
                             TAtlasRRTBranchConfig *config,
                             TAtlasRRTBranchStatus *status,
                             TSampleInfo *current,TSampleInfo *next,
                             TSampleInfo *rand,double *randSample,
                             void *st,Tatlasrrt *ar)
{
  double epsilon;
  unsigned int samplingMode;
  double dif;
  Tchart *nextChart;

  #if (GET_ATLASRRT_STATISTICS)
    NewAtlasRRTStep((TAtlasRRTStatistics *)st);
  #endif

  epsilon=GetParameter(CT_EPSILON,pr);
  samplingMode=(unsigned int)GetParameter(CT_SAMPLING,pr);

  /* new samples do not generated a chart (yet) */
  next->generateChart=0;
  
  if (status->delta<epsilon) 
    {
      /* The advance step is too small. This means we are trying to generate 
         a sample (i.e., a chart center) very close to another sample and 
         even being very close we didn't manage to to it. This should never 
         happen for smooth manifolds. */
      Warning("The impossible happened (III)");
      status->blocked=TRUE;
    }
  else
    {     
      /* Move a step of (max) size 'delta' from current towards rand  */
      dif=Distance(ar->k,current->t,rand->t);
      if (dif<status->delta)
        memcpy(next->t,rand->t,ar->k*sizeof(double));
      else
        SumVectorScale(ar->k,current->t,status->delta,
                       status->deltaParam,next->t);
 
      if (Norm(ar->k,next->t)>ar->r)
        {
          /* 'next' is out of the validity area (ball) of the current 
             chart. Its time to generate a new chart (in the previous 
             sample that is still inside the chart). */
          status->validChart=FALSE;
          #if (GET_ATLASRRT_STATISTICS)
            NewAtlasRRTOutOfChart((TAtlasRRTStatistics *)st);
          #endif
        }
      else
        {
          /* In principle, 'next' sampple is in the same chart as the 
             'current' sample */
          next->c=current->c;
          nextChart=GetAtlasChart(next->c,&(ar->atlas));
          
          if (!InsideChartPolytope(next->t,nextChart))
            {
              /* 'next' is outside the polytope of the chart -> it might 
                 be in a neighbouring chart */
              if ((!config->onManifold)&&
                  ((config->explorationSample)||(status->nCreatedCharts>0)))
                {
                  /* A branch aiming a random point in tangent space that is 
                     a exploration branch or that generated new charts while 
                     growing. It is stopped to enter a pre-existing chart.
                     This tries to avoid branch crossing. */
                  status->blocked=TRUE;
                  #if (GET_ATLASRRT_STATISTICS)
                    NewAtlasRRTOverlap((TAtlasRRTStatistics *)st);
                  #endif
                  #if (ATLASRRT_VERBOSE)
                    if (status->nCreatedCharts>0) 
                      fprintf(stderr,"   Branch stopped (created charts>1)\n"); 
                    else
                      fprintf(stderr,"   Branch stopped (exploration)\n"); 
                  #endif
                }
              else
                {
                  /* Determine the neighbouring chart including the 
                     'next' sample */
                  next->c=DetermineChartNeighbour(epsilon,next->t,nextChart);
                  
                  if ((ElementInVector((void*)&(next->c),CmpID,&(status->traversedCharts)))||
                      ((!config->revisitCharts)&&
                       ((!ar->birrt)||
                        ((ar->ci[current->c]->tree)&(ar->ci[next->c]->tree)))))
                    {
                      /* The branch can not re-enter charts already traversed 
                         Moreover, if revisitCharts is FALSE, we have to avoid 
                         branches to enter charts in the same tree. In 
                         oneDirectional trees all charts are in the same tree.
                         This reduces branch crossing.
                         Observe that this does not prevent the connection 
                         between different trees.  */
                      status->blocked=TRUE;
                      #if (GET_ATLASRRT_STATISTICS)
                        NewAtlasRRTOverlap((TAtlasRRTStatistics *)st);
                      #endif
                      #if (ATLASRRT_VERBOSE)
                        fprintf(stderr,"   Branch stopped (non-revisited)\n"); 
                      #endif
                    }
                  else
                    {
                      /* Transition between pre-existing charts -> continue normally. 
                         We basically repeat all the steps for the new chart */
                      #if (ATLASRRT_VERBOSE>1)
                        fprintf(stderr,"   Branch moved to chart: %u\n",next->c); 
                      #endif

                      /* Get the new chart */
                      nextChart=GetAtlasChart(next->c,&(ar->atlas));

                      /* Project the current sample to the new chart */
                      Manifold2Chart(current->s,ar->tp,current->t,nextChart);
                      current->c=next->c;

                      /* Keep track of the visited charts */
                      NewVectorElement((void*)&(current->c),&(status->traversedCharts));
                          
                      /* Ensure some overlap between neighbouring charts: current is in both charts */
                      EnlargeChart(current->t,nextChart);
                            
                      /* Determine the advance direction in the new chart */
                      status->blocked=New_PointTowardRandSample(config,samplingMode,randSample,
                                                                status,current,rand,ar);

                      if (!status->blocked)
                        {
                          /* Moved to a new chart and succesfully projected the random sample to it
                             Now recompute the advance step */
                          dif=Distance(ar->k,current->t,rand->t);
                          if (dif<status->delta)
                            memcpy(next->t,rand->t,ar->k*sizeof(double));
                          else
                            SumVectorScale(ar->k,current->t,status->delta,
                                           status->deltaParam,next->t);

                          /* and just check that it is inside the ball */
                          if (Norm(ar->k,next->t)>ar->r)
                            {
                              /* 'next' is out of the validity area (ball) of the current 
                                 chart. Its time to generate a new chart (in the previous 
                                 sample that is still inside the chart). */
                              status->validChart=FALSE;
                              #if (GET_ATLASRRT_STATISTICS)
                                NewAtlasRRTOutOfChart((TAtlasRRTStatistics *)st);
                              #endif
                            }
                        }
                      #if (GET_ATLASRRT_STATISTICS)
                      else
                        NewAtlasRRTQrandReached((TAtlasRRTStatistics *)st);
                      #endif
                    }
                }
            }
        }
      
      /* Here we have a sample that is correctly parametrized inside 
         the validity area (ball) of a given chart. 
         Now we have to check for the sample validity like in any 
         other path planner. */
      
      if ((status->validChart)&&(!status->blocked)&&
          (Chart2Manifold(pr,&(ar->J),next->t,
                          ar->tp,current->s,
                          next->s,nextChart)<ar->e))
        {
          double dp;
          boolean inDomain,tooCurved;
          boolean trans=TRUE;
              
          /* Check error curvature (only along the expansion direction) */
          dp=DistanceTopology(ar->m,ar->tp,current->s,next->s);

          tooCurved=((status->delta/dp)<(1-ar->ce));
          if (!tooCurved)
            {     
              /* Check the bounds respect to the ambient space */
              inDomain=((PointInBoxTopology(NULL,FALSE,ar->m,next->s,
                                            epsilon,ar->tp,
                                            &(ar->ambient)))&&
                        (CS_WD_SIMP_INEQUALITIES_HOLD(pr,next->s,ar->w)));
              if (inDomain)
                {
                  /* Check if the transition test is passed */                  
                  if (config->costF!=NULL) 
                    trans=TransitionTestRRT(pr,current->id,next->s,dp,
                                            &(status->cost),
                                            config->costF,config->costData,
                                            &(ar->rrt));
                        
                  if (trans)
                    {
                      /* Check the collisions (only if necessary) */
                      if (config->checkCollisions)
                        {
                          CS_WD_IN_COLLISION(status->collision,pr,next->s,
                                             current->s,ar->w);
                          #if (GET_ATLASRRT_STATISTICS)
                            NewAtlasRRTCollisionCheck((TAtlasRRTStatistics *)st);
                          #endif
                        }
                      
                      if (status->collision)
                        {
                          status->blocked=TRUE;
                          #if (GET_ATLASRRT_STATISTICS)
                            NewAtlasRRTCollision((TAtlasRRTStatistics *)st);
                          #endif
                        }
                      #if (GET_ATLASRRT_GLOBAL_CURV_CHECK)
                      else 
                        {
                          Tchart cTmp;
                          boolean closeEnough,canInitChart;
                              
                          /* The curvature check (tooCurved) can be computed 
                             in a more accurate way than the test above. 
                             This is expensive and not commontly used. */
                              
                          /* We further check if we are able to create a 
                             chart at the current sample and if this chart 
                             is not too different from its parent chart. */

                          /* Create a temporary chart at 'next' */
                          canInitChart=(InitChart(pr,TRUE,&(ar->ambient),
                                                  ar->tp,ar->m,ar->k,
                                                  next->s,ar->e,ar->ce,
                                                  ar->r,&(ar->J),
                                                  ar->w,&cTmp)==0);
                          if (canInitChart)
                            {
                              /* Check if new chart respects distance 
                                 and angular tolerances */
                              closeEnough=CloseCharts(pr,ar->tp,
                                                      nextChart,&cTmp);
                              if (!closeEnough)
                                {
                                  /* next sample is out of the domain of the
                                     current chart due to excesive curvature
                                     We generate a new chart closer to the
                                     area of interst. */
                                  status->validChart=FALSE;
                                  #if (GET_ATLASRRT_STATISTICS)
                                    NewAtlasRRTLargeCurvature((TAtlasRRTStatistics *)st);
                                  #endif
                                }
                              DeleteChart(&cTmp); /* Delete only if canInit */
                            }
                          else
                            {
                              /* We did not manage to create the chart:
                                 - Point not on manifold
                                 - error in QR
                                 - singular point 
                                 (typically the last one).
                                 We could implement more complex procedures 
                                 to skip the singular area. */
                              status->blocked=TRUE; 
                              #if (GET_ATLASRRT_STATISTICS)
                                NewAtlasRRTInitChartError((TAtlasRRTStatistics *)st);
                              #endif
                            }
                        }                 
                      #endif
                    }
                  else
                    {
                      /*Transition test failed -> rejected step */
                      status->blocked=TRUE;
                    }
                } 
              else 
                {
                  /* next sample is out of the domain */
                  #if (GET_ATLASRRT_STATISTICS)
                    NewAtlasRRTNotInDomain((TAtlasRRTStatistics *)st);
                  #endif
                  status->blocked=TRUE;
                }
            }
          else
            {
              /* The new sample is out of the validity area of the chart 
                 due to a excesive curvature. */
              #if (GET_ATLASRRT_STATISTICS)
                NewAtlasRRTDirLargeCurvature((TAtlasRRTStatistics *)st);
              #endif
              status->validChart=FALSE;
            }
        }
      else
        {
          /* The projection from the sample in tangent space to the 
             manifold failed */
          #if (GET_ATLASRRT_STATISTICS)
            if ((!status->blocked)&&(status->validChart))
              NewAtlasRRTNoConvergentStep((TAtlasRRTStatistics *)st);
          #endif
          status->validChart=FALSE;
        }
    }
}

boolean New_PointTowardRandSample(TAtlasRRTBranchConfig *config,
                                  unsigned int samplingMode,double *randSample,
                                  TAtlasRRTBranchStatus *status,
                                  TSampleInfo *current,TSampleInfo *rand,
                                  Tatlasrrt *ar)
{
  Tchart *c;
  double n;

  /* We will project 'rand' to the chat of the current sample */
  rand->c=current->c;

  /* Get current chart */
  c=GetAtlasChart(current->c,&(ar->atlas));

  /* The random sample to pursue changes depending on whether it
     is on the manifold of in the tangent space */
  if ((samplingMode!=TANGENT_SAMPLING)||(config->onManifold))
    Manifold2Chart(randSample,ar->tp,rand->t,c);  /* sample not in the tangent space */
  else
    Manifold2Chart(rand->s,ar->tp,rand->t,c); /* sample in the tangent space (of previous chart)
                                                 In the first call, this is the same as randSample */

  /* deltaParam: Vector from 'current' to 'rand' (in tangent space) */
  DifferenceVector(ar->k,rand->t,current->t,status->deltaParam);
  
  /* Ensure that the new rand far from 'current' */
  n=Norm(ar->k,status->deltaParam);
  if (n>1e-3)
    {
      Normalize(ar->k,status->deltaParam);
      /* Displace rand->t far enough, but without modifying the 
         advance direction. */
      if ((samplingMode==TANGENT_SAMPLING)&&(!config->onManifold))
          SumVectorScale(ar->k,current->t,status->maxLength,
                         status->deltaParam,rand->t);
      
      /* Update rand->s so that it correspond to the new rand->t  */
      Local2Global(rand->t,ar->tp,rand->s,c);
      
      #if (ATLASRRT_VERBOSE)
        fprintf(stderr,"      New random sample in chart %u: [%f %f] [%f %f %f]\n",rand->c,rand->t[0],rand->t[1],rand->s[0],rand->s[1],rand->s[2]);
      #endif
      
      return(FALSE);
    }
  else
    /* The projection of 'rand' to the current chart coincides
       with the current sample -> the direction of adavance is
       not clear -> stop the branch extension*/
    return(TRUE);
}

double AddBranchToAtlasRRT(Tparameters *pr,unsigned int it,
                           unsigned int addMode,boolean revisitCharts,
                           boolean checkCollisions,boolean onManifold,
                           boolean explorationSample,
                           double maxLength,unsigned int i_near,
                           double *origRandSample,
                           double *goalSample,void *st,
                           unsigned int *lastSampleID,
                           boolean *reachedQRand,boolean *reachedGoal,
                           unsigned int *ns,double ***path,
                           double (*costF)(Tparameters*,boolean,double*,void*),
                           void *costData,Tatlasrrt *ar)
{
  /* 
    In this function
       current -> refers to the sample already in the altlasRRT that
                  will be extended with a new child
       rand -> is the sample to reach in this particular expansion
       goal -> is the final point to reach in the whole tree.
       next -> is the sample to add just after current.

     for each one of the previous we have
       param -> are the parametes on the corresponding chart
       sample -> is the corresponding point on the manifold.
   */

  boolean blocked,collision,valid,tooFar,init,intersectParent,canInitChart,noRep;
  double epsilon,d,dg,dqr,dnn; /* current branch lenght. */

  /* Next sample */
  unsigned int nextChart; /* Chart to which this sample belongs to. */
  double *currentParam;   /* The parameters for the current sample. Can be different
                             from those stored in the AtlasRRT */
  double *nextParam;      /* Parameters in the chart. */
  double *nextSample;     /* Point on manifolf for next sample. */

  double *deltaParam; /* Normal vector indicating the direction toward
                         q_rand in the current chart. */

  /* Interpolation step */
  double delta;        /* Max step size. */
  double currentDelta; /* Current size. */
  double distQrand;    /* Initial distance to q_rand = max length for the
                          new branch. */
  double *nnSample;    /* Sample from where to extend the tree. */
  TSampleInfo *currentSampleInfo;
  unsigned int randChart;
  double *randParam;
  double dist,maxDist;
  TSampleInfo tmpSI; /* Temporary sample info used when addMode!=ADD_ALL */
  double *randSample;
  boolean tmpQrandReached;
  unsigned int ms,nn;
  double cost;
  unsigned int maxNodes;
  unsigned int samplingMode;
  unsigned int nChanges; /* only one transition between charts is allowed */
  unsigned int tree;

  tree=GetRRTNodeTree(i_near,&(ar->rrt));

  if (i_near==NO_UINT)
    Error("Extending a branch from nowhere");

  epsilon=GetParameter(CT_EPSILON,pr);
  delta=GetParameter(CT_DELTA,pr);
  maxNodes=(unsigned int)GetParameter(CT_MAX_NODES_RRT,pr);
  samplingMode=(unsigned int)GetParameter(CT_SAMPLING,pr);

  nnSample=ar->si[i_near]->s;
  distQrand=DistanceTopology(ar->m,ar->tp,nnSample,origRandSample);
  #if (ATLASRRT_VERBOSE)
    if (distQrand<epsilon)
      Warning("Repeated points in the AtlasRRT?");
  #endif

  nChanges=0;
     
  /* Interpolation parameters */
  currentDelta=delta;

  /* Identifier for current point, used to retive current Chart/Param/Sample 
     from the AtlasRRT structure */
  *lastSampleID=i_near;

  blocked=FALSE;
  tooFar=FALSE;
  collision=FALSE;
  cost=0;
  #if (EXPLORATION_RRT)
    *reachedGoal=FALSE;
    dg=INF;
  #else
    /* The projection on the manifold can be larger than that
       on the tangent space */
    if (goalSample!=NULL)
      {
        dg=DistanceTopology(ar->m,ar->tp,nnSample,goalSample);
        *reachedGoal=(dg<delta);
      }
    else
      *reachedGoal=FALSE;
  #endif
  if (onManifold)
    *reachedQRand=(distQrand<epsilon);
  else
    *reachedQRand=FALSE;
  tmpQrandReached=*reachedQRand;

  if ((ns!=NULL)&&(path!=NULL))
    InitSamples(&ms,ns,path);

  if ((*reachedQRand)||(*reachedGoal))
    {
      init=FALSE; /* not data structre was initialized */
      maxDist=0.0;
      d=0.0;
    }
  else
    {
      /* If the goal sample is not already reached, we prepare for the
         branch extension */
      init=TRUE;

     /* Each sample can have many different representations due to the
         ambient space topology. Select the one that is closer to
         the nearest neighbour. In this wqy we ensure to move towards
         the randSample along the shortest path. */
      if (ar->tp!=NULL)
        {
          DifferenceVectorTopology(ar->m,ar->tp,origRandSample,nnSample,origRandSample);
          AccumulateVector(ar->m,nnSample,origRandSample);
        }
      /* the nn can have different parameters in the same tangent space. We store
         the ones closer to its chart center (obtained using the topology), but
         the direct parameters are better to extend the branch. */
      NEW(currentParam,ar->k,double);
      Manifold2Chart(nnSample,NULL,currentParam,
                     GetAtlasChart(ar->si[i_near]->c,&(ar->atlas)));

      d=0.0;
      /* get a copy of the random sample since we will modify it while the branch 
         is extended */
      NEW(randSample,ar->m,double);
      memcpy(randSample,origRandSample,ar->m*sizeof(double));

      if (maxLength<=0.0)
        {
          dist=distQrand;
          maxDist=2*distQrand;
        }
      else
        {
          dist=maxLength;
          maxDist=maxLength;
        }

      /* if the targed is on the manifold we allow for some tolerance in
         the maximum distance from nn to end of path. */
      if (onManifold)
        distQrand*=1.1;

      /* Detemine the direction of motion toward q_rand from the current sample */
      NEW(deltaParam,ar->k,double);
      
      /* Space for next samples */
      NEW(nextParam,ar->k,double);
      NEW(nextSample,ar->m,double);
      
      /* Space for the parameters of q_rand */
      NEW(randParam,ar->k,double);

      if (addMode==ADD_ALL)
        currentSampleInfo=ar->si[i_near];
      else
        {
          /* Get a copy of the info for sample i_near (many fields are not used
             but copied for completeness) */
          tmpSI.id=NO_UINT;
          NEW(tmpSI.s,ar->m,double);
          memcpy(tmpSI.s,ar->si[i_near]->s,ar->m*sizeof(double));
          NEW(tmpSI.t,ar->k,double);
          memcpy(tmpSI.t,ar->si[i_near]->t,ar->k*sizeof(double));
          tmpSI.pc=ar->si[i_near]->pc;
          tmpSI.c=ar->si[i_near]->c;
          tmpSI.generateChart=ar->si[i_near]->generateChart;
          tmpSI.lsc=ar->si[i_near]->lsc;

          /* And the sample from where to extend is the temporal one that will
             be modified as we grow the branch */
          currentSampleInfo=&tmpSI;
        }

      blocked=PointTowardRandSample(currentSampleInfo->c,dist,currentParam,
                                    samplingMode,onManifold,origRandSample,
                                    &randChart,randParam,randSample,deltaParam,ar);
    }

  #if (_DEBUG>1)
    printf("  Adding branch to RRT from sample %u (chart %u)\n",i_near,ar->si[i_near]->c);
  #endif

  /* We stop if blocked, after a maximum dist extansion or 
     if it gets farther than the ambient dist to the randSample. This
     is measured in two ways: the distance traveled over the monifold (d)
     and the Euclidian distance from the current sample to q_near (tooFar).
     The new branch should not scape the ball of radious maxDist centered
     at q_near. We allow for a larger displacement over the manifold to 
     account for curvature effects.
  */
  while ((!blocked)&&(!collision)&&(d<maxDist)&&(!tmpQrandReached)&&(!(*reachedGoal))&&
         (!(*reachedQRand))&&(!tooFar)&&((addMode==ADD_NONE)||(ar->ns<maxNodes)))
    {
      NewTemptativeSample(pr,it,&nChanges,
                          revisitCharts,explorationSample,checkCollisions,onManifold,
                          currentSampleInfo,currentParam,origRandSample,
                          &randChart,randParam,randSample,
                          currentDelta,dist-d,deltaParam,
                          &nextChart,nextParam,nextSample,
                          st,costF,costData,&cost,&blocked,&collision,&valid,ar);

      if (!blocked)
        {
          if (!valid)
            {
              if (currentSampleInfo->generateChart)
                {
                  currentDelta*=0.5;
                  #if (GET_ATLASRRT_STATISTICS)
                    NewAtlasRRTStepReduction((TAtlasRRTStatistics *)st);
                  #endif
                }
              else
                {
                  canInitChart=AddChart2AtlasRRT(pr,tree,it,currentSampleInfo,
                                                 &intersectParent,ar);
                  if (canInitChart)
                    {
                      #if (GET_ATLASRRT_STATISTICS)
                        NewAtlasRRTChart((TAtlasRRTStatistics *)st);
                        if(!intersectParent)
                          NewAtlasRRTNoConnectToParent((TAtlasRRTStatistics *)st);
                      #endif
                        
                      Manifold2Chart(currentSampleInfo->s,NULL,currentParam,
                                     GetAtlasChart(currentSampleInfo->c,&(ar->atlas)));
                      blocked=PointTowardRandSample(currentSampleInfo->c,dist-d,
                                                    currentParam,samplingMode,
                                                    onManifold,origRandSample,
                                                    &randChart,randParam,randSample,
                                                    deltaParam,ar);
                      #if (GET_ATLASRRT_STATISTICS)
                        if (blocked)
                          NewAtlasRRTQrandReached((TAtlasRRTStatistics *)st);
                      #endif
                      /* Samples that trigger the creation of a chart are considered
                         exploration samples */
                      explorationSample=TRUE;
                    }
                  else 
                    {
                      /* Stop the branch extension.
                         TODO: Implement a more sophisticated stratetgy to deal
                         with singular regions.
                       */
                      blocked=TRUE; 
                      fprintf(stderr,"  Point in singularity (decrease epsilon?)\n");
                      #if (GET_ATLASRRT_STATISTICS)
                        NewAtlasRRTBlockBySingularity((TAtlasRRTStatistics *)st);
                        NewAtlasRRTInitChartError((TAtlasRRTStatistics *)st);
                      #endif
                    }
                }
            }
          else
            {
              tmpQrandReached=((randChart==nextChart)&&
                               (Distance(ar->k,randParam,nextParam)<epsilon));

              if (onManifold)
                {
                  dqr=DistanceTopology(ar->m,ar->tp,nextSample,origRandSample);
                  *reachedQRand=(dqr<epsilon);
                }

              #if (!EXPLORATION_RRT)
                if (goalSample!=NULL)
                  {
                    dg=DistanceTopology(ar->m,ar->tp,nextSample,goalSample);
                    *reachedGoal=(dg<epsilon);
                  }
              #endif
                
              #if (GET_ATLASRRT_STATISTICS)
                if ((*reachedQRand)||(tmpQrandReached)||(*reachedGoal))
                  NewAtlasRRTQrandReached((TAtlasRRTStatistics *)st);
              #endif

              dnn=DistanceTopology(ar->m,ar->tp,nextSample,nnSample);
              tooFar=(dnn>distQrand);

              if (!tooFar)
                {
                  /* Add the sample to the tree, if required */
                  dnn=DistanceTopology(ar->m,ar->tp,currentSampleInfo->s,nextSample);
                  if (addMode==ADD_ALL)
                    {
                      d+=AddSample2AtlasRRT(pr,tree,lastSampleID,
                                            nextChart,nextParam,nextSample,dnn,cost,ar);
     
                      currentSampleInfo=ar->si[*lastSampleID];
                    }
                  else
                    {
                      /* If the sample is not added to the tree, we update the
                         current sample info. */
                      d+=dnn;

                      /* Update the info for the temporary sample */
                      memcpy(currentSampleInfo->s,nextSample,ar->m*sizeof(double));
                      memcpy(currentSampleInfo->t,nextParam,ar->k*sizeof(double));
                      currentSampleInfo->c=nextChart;
                      currentSampleInfo->pc=nextChart;
                      currentSampleInfo->generateChart=0;
                      currentSampleInfo->lsc=NO_UINT;
                    }
                  memcpy(currentParam,nextParam,ar->k*sizeof(double));

                  currentDelta=delta;

                  #if (GET_ATLASRRT_STATISTICS)
                    if (d>=maxDist)
                      NewAtlasRRTTooLong((TAtlasRRTStatistics *)st);

                    NewAtlasRRTSample((TAtlasRRTStatistics *)st);
                  #endif
                   
                  /* Add the sample the path, if required */
                  if ((ns!=NULL)&&(path!=NULL)&&
                      (!tmpQrandReached)&&(!(*reachedGoal))&&(!(*reachedQRand)))
                    AddSample2Samples(ar->m,nextSample,ar->m,NULL,&ms,ns,path);
                }
              #if (GET_ATLASRRT_STATISTICS)
              else
                NewAtlasRRTTooFar((TAtlasRRTStatistics *)st);
              #endif
            }
        }
    }

  /* Add the distance from the last sample in the branch to the
     goal/q_rand (this is epsilon size) */
  if (*reachedQRand)
    d+=dqr;
  else
    {
      if (*reachedGoal)
        d+=dg;
    }

  if ((init)&&(d>0)&&((addMode==ADD_LAST)||((addMode==ADD_LAST_NO_REP))))
    {
      if (addMode==ADD_LAST_NO_REP)
        {
          nn=GetRRTNN(tree,tmpSI.s,&(ar->rrt));
          noRep=(DistanceTopology(ar->m,ar->tp,ar->si[nn]->s,tmpSI.s)>epsilon);
        }
      else
        noRep=TRUE;

      if (noRep)
        {
          /* If it was required to advance toward q_rand and we actually 
             moved in that direction and we have to add the last computed
             sample -> add it */
          AddSample2AtlasRRT(pr,tree,lastSampleID,
                             tmpSI.c,tmpSI.t,tmpSI.s,d,cost,ar);
        }
    }

  if (init)
    {
      /* Update the statistic of how far we can get from i_near */
      /* This only has effect if paramter DYNAMIC_DOMAIN is >0 */
      AdjustDynamicDomain(i_near,collision,&(ar->rrt));

      /* Release allocated space */
      if (addMode!=ADD_ALL)
        {
          free(tmpSI.s);
          free(tmpSI.t);
        }
      free(currentParam);
      free(deltaParam);
      free(nextParam);
      free(nextSample);
      free(randParam);
      free(randSample);
    }

  return(d);
}

void NewTemptativeSample(Tparameters *pr,unsigned int it,unsigned int *nChanges,
                         boolean revisitCharts,boolean explorationSample,
                         boolean checkCollisions,boolean onManifold,
                         TSampleInfo *currentSampleInfo,double *currentParam,
                         double *origRandSample,
                         unsigned int *randChart,double *randParam,double *randSample,
                         double currentDelta,double d,double *deltaParam,
                         unsigned int *nextChart,double *nextParam,double *nextSample,
                         void *st,double (*costF)(Tparameters*,boolean,double*,void*), 
                         void *costData,double *cost,boolean *blocked,
                         boolean *inCollision,boolean *valid,
                         Tatlasrrt *ar)
{
  double epsilon;
  unsigned int samplingMode;
  double dif;

  #if (GET_ATLASRRT_STATISTICS)
    NewAtlasRRTStep((TAtlasRRTStatistics *)st);
  #endif

  epsilon=GetParameter(CT_EPSILON,pr);
  samplingMode=(unsigned int)GetParameter(CT_SAMPLING,pr);

  if (currentDelta<epsilon) 
    {
      Warning("The impossible happened (III)");
      *blocked=TRUE;
    }
  else
    {
      dif=Distance(ar->k,currentParam,randParam);
      if (dif<currentDelta)
        memcpy(nextParam,randParam,ar->k*sizeof(double));
      else
        SumVectorScale(ar->k,currentParam,currentDelta,deltaParam,nextParam);

      *valid=TRUE;
      *blocked=FALSE;
      *inCollision=FALSE; /* default no collision */
    
      if (Norm(ar->k,nextParam)>ar->r)
        {
          *valid=FALSE;  
          #if (GET_ATLASRRT_STATISTICS)
            NewAtlasRRTOutOfChart((TAtlasRRTStatistics *)st);
          #endif
        }
      else
        {
          Tchart *nChart;
          double *currentSample;
          double dp;
          boolean inDomain,tooCurved;
          boolean trans;

          trans=TRUE; /* default no cost function is considered */

          /* currentSample = sample from where to move forward */
          currentSample=currentSampleInfo->s;

          /* In principle, nextChart is currentChart */
          *nextChart=currentSampleInfo->c;
          nChart=GetAtlasChart(*nextChart,&(ar->atlas));

          if (!InsideChartPolytope(nextParam,nChart))
            {
              if ((!onManifold)&&((explorationSample)||(*nChanges>0)))
                {
                  /* A branch aiming a random point (in tangent space) that potentially generated
                     new charts along its way, is stopped to enter a pre-existing chart.
                     This tries to avoid branch crossing. */
                  *blocked=TRUE;
                  #if (GET_ATLASRRT_STATISTICS)
                    NewAtlasRRTOverlap((TAtlasRRTStatistics *)st);
                  #endif
                  #if (ATLASRRT_VERBOSE)
                    if (*nChanges>0)
                      fprintf(stderr,"   Branch stopped (visited charts>1)\n"); 
                    else
                      fprintf(stderr,"   Branch stopped (exploration)\n"); 
                  #endif
                }
              else
                {
                  double *tc;

                  *nextChart=DetermineChartNeighbour(epsilon,nextParam,nChart);

                  if ((!revisitCharts)&&((!ar->birrt)||((ar->ci[currentSampleInfo->c]->tree)&(ar->ci[*nextChart]->tree))))
                    {
                      /* When trying to connect two trees, the branch should not enter other charts in the
                         same tree. When this happens it means that the sample from where we extend the tree was
                         not the nearest node to the other tree. Extending this branch results in branch crossing.
                         Even the part of the branch already created shoud be removed to improve the quality of the
                         RRT, but this is not implemented yet. 
                         One option would be not to add any node along this "connection" branches. Or implement 
                         a sort of delayed add to the RRT,.... */
                      *blocked=TRUE;
                      #if (GET_ATLASRRT_STATISTICS)
                        NewAtlasRRTOverlap((TAtlasRRTStatistics *)st);
                      #endif
                      #if (ATLASRRT_VERBOSE)
                        fprintf(stderr,"   Branch stopped (non-revisited)\n"); 
                      #endif
                    }
                  else
                    {
                      (*nChanges)++;
                      #if (ATLASRRT_VERBOSE>1)
                        fprintf(stderr,"   Branch moved to chart: %u [num Changes: %u]\n",*nextChart,*nChanges); 
                      #endif
                      /* Transition between pre-existing charts -> continue */
                      nChart=GetAtlasChart(*nextChart,&(ar->atlas));
                  
                      NEW(tc,ar->k,double);
                      Manifold2Chart(currentSample,ar->tp,tc,nChart);
                      *blocked=PointTowardRandSample(*nextChart,d,tc,samplingMode,
                                                     onManifold,origRandSample,
                                                     randChart,randParam,randSample,
                                                     deltaParam,ar);
                      if (!(*blocked))
                        {
                          dif=Distance(ar->k,tc,randParam);
                          if (dif<currentDelta)
                            memcpy(nextParam,randParam,ar->k*sizeof(double));
                          else
                            SumVectorScale(ar->k,tc,currentDelta,deltaParam,nextParam);   
                          free(tc);
                  
                          if (Norm(ar->k,nextParam)>ar->r)
                            {
                              #if (GET_ATLASRRT_STATISTICS)
                                NewAtlasRRTOutOfChart((TAtlasRRTStatistics *)st);
                              #endif
                              *valid=FALSE;
                            }
                          else
                            EnlargeChart(nextParam,nChart);
                        }
                      #if (GET_ATLASRRT_STATISTICS)
                      else
                        NewAtlasRRTQrandReached((TAtlasRRTStatistics *)st);
                      #endif
                    }
                }
            }

          /* Check error projection (distance) */
          if ((*valid)&&(!*blocked)&&
              (Chart2Manifold(pr,&(ar->J),nextParam,
                              ar->tp,currentSample,
                              nextSample,nChart)<ar->e))
            {
              /* Check error curvature (only along the expansion direction) */
              dp=DistanceTopology(ar->m,ar->tp,currentSample,nextSample);

              tooCurved=((currentDelta/dp)<(1-ar->ce));
              if (!tooCurved)
                {
                  /* Check the bounds respect to the ambient space */
                  inDomain=((PointInBoxTopology(NULL,FALSE,ar->m,nextSample,epsilon,
                                                ar->tp,&(ar->ambient)))&&
                            (CS_WD_SIMP_INEQUALITIES_HOLD(pr,nextSample,ar->w)));
                  if (inDomain)
                    {
                      *cost=0;
                        
                      if (costF!=NULL) 
                        trans=TransitionTestRRT(pr,currentSampleInfo->id,nextSample,dp,cost,
                                                costF,costData,&(ar->rrt));
                        
                      if (trans)
                        {
                          /* Check the collisions (only if necessary) */
                          if (checkCollisions)
                            {
                              CS_WD_IN_COLLISION(*inCollision,pr,nextSample,currentSample,ar->w);
                              #if (GET_ATLASRRT_STATISTICS)
                                NewAtlasRRTCollisionCheck((TAtlasRRTStatistics *)st);
                              #endif
                            }
                  
                          if (*inCollision)
                            {
                              *blocked=TRUE;
                              #if (GET_ATLASRRT_STATISTICS)
                                NewAtlasRRTCollision((TAtlasRRTStatistics *)st);
                              #endif
                            }
                          #if (GET_ATLASRRT_GLOBAL_CURV_CHECK)
                          else 
                            {
                              Tchart cTmp;
                              boolean closeEnough,canInitChart;

                              /* We further check if we are able to create a chart
                                 at the current sample and if this chart is not
                                 too different from its parent chart. */

                              /* Check the possibility of a chart creation */
                              canInitChart=(InitChart(pr,TRUE,&(ar->ambient),
                                                      ar->tp,ar->m,ar->k,nextSample,
                                                      ar->e,ar->ce,ar->r,&(ar->J),
                                                      ar->w,&cTmp)==0);
                              if (canInitChart)
                                {
                                  /* Check if new chart respects distance 
                                     and angular tolerances */
                                  closeEnough=CloseCharts(pr,ar->tp,nChart,&cTmp);
                                  if (!closeEnough)
                                    {
                                      *valid=FALSE;
                                      #if (GET_ATLASRRT_STATISTICS)
                                        NewAtlasRRTLargeCurvature((TAtlasRRTStatistics *)st);
                                      #endif
                                    }
                                  DeleteChart(&cTmp); /* Delete only if init was OK */
                                }
                              else
                                {
                                  /* Point not on manifold, error in QR, singular point 
                                     (typically the last one) */
                                  *blocked=TRUE; /* valid=FALSE ?? */
                                  #if (GET_ATLASRRT_STATISTICS)
                                    NewAtlasRRTInitChartError((TAtlasRRTStatistics *)st);
                                  #endif
                                }
                            }             
                          #endif
                        }
                      else
                        {
                          /*Transition test failed -> rejected step */
                          *blocked = TRUE;
                        }
                    } 
                  else 
                    {
                      #if (GET_ATLASRRT_STATISTICS)
                        NewAtlasRRTNotInDomain((TAtlasRRTStatistics *)st);
                      #endif
                      *blocked=TRUE;
                    }
                }
              else
                {       
                  #if (GET_ATLASRRT_STATISTICS)
                    NewAtlasRRTDirLargeCurvature((TAtlasRRTStatistics *)st);
                  #endif
                  *valid=FALSE;
                }
            }
          else
            {
              #if (GET_ATLASRRT_STATISTICS)
                if ((*valid)&&(!*blocked))
                  NewAtlasRRTNoConvergentStep((TAtlasRRTStatistics *)st);
              #endif
              *valid=FALSE;
            }
        }
    }
}

double AddSample2AtlasRRT(Tparameters *pr,unsigned int tree,
                          unsigned int *currentID,
                          unsigned int nextChart,double *nextParam,double *nextSample,
                          double dp,double cost,
                          Tatlasrrt *ar)
{   
  double d;
  double *currentSample;
  unsigned int k;
  TSampleInfo *currentSampleInfo;
  TChartInfo *currentChartInfo;
  
  /* Distance to parent sample */
  currentSample=ar->si[*currentID]->s;
  d=DistanceTopology(ar->m,ar->tp,nextSample,currentSample);
  
  #if (_DEBUG>1)
    printf("  New sample %u (dp:%f)\n",ar->ns,d);
  #endif

  /* Add the sample to the RRT */
  if (ar->ns==ar->ms)
    MEM_DUP(ar->si,ar->ms,TSampleInfo *);

  /* Store the new sample in the RRT */
  AddNodeToRRT(pr,tree,*currentID,nextSample,nextSample,&k,NULL,dp,cost,NULL,&(ar->rrt));
  if (k!=ar->ns)
    Error("Incongruent node identifier in AtlasRRT (vs RRT inside)");  

  /* Set up the information for the new sample */
  NEW(ar->si[ar->ns],1,TSampleInfo);
  currentSampleInfo=ar->si[ar->ns];
  currentChartInfo=ar->ci[nextChart];

  /* Pointer to the just added sample. The sample is stored in the RRT. */
  currentSampleInfo->s=GetRRTNode(ar->ns,&(ar->rrt));
  currentSampleInfo->id=ar->ns;
  
  /* Parent chart: Chart at which the sample is created.
     At initialization this is the same as currentSampleInfo->c
     but currentSampleInfo->c can change as new charts are
     created and currentSampleInfo->pc will remain the same */
  currentSampleInfo->pc=nextChart;

  /* There is no chart at the new sample (yet) */
  currentSampleInfo->generateChart=0;

  /* The chart including the current sample must be associated with
     the tree including this sample. */
  AddChart2Tree(tree,nextChart,ar);
    
  /* Store the projection into the current chart */
  NEW(currentSampleInfo->t,ar->k,double);
  memcpy(currentSampleInfo->t,nextParam,ar->k*sizeof(double));
  /*Manifold2Chart(currentSampleInfo->s,ar->tp,currentSampleInfo->t,
    GetAtlasChart(nextChart,&(ar->atlas)));*/

  /* Assign the sample to the corresponding chart */
  currentSampleInfo->c=nextChart;

  /* Add the sample to the list of samples of the corresponding chart */
  currentSampleInfo->lsc=currentChartInfo->lc;
  currentChartInfo->lc=ar->ns;

  /*Move the pointer to current to the new sample */
  *currentID=ar->ns; 

  ar->ns++;

  return(d);
}

void AddChart2Tree(unsigned int tree,unsigned int chartId,Tatlasrrt* ar) 
{
  if ((ar->birrt)&&(!(ar->ci[chartId]->tree&tree)))
    {
      ar->ci[chartId]->tree|=tree;
      if (tree==START2GOAL)
        { 
          if(ar->nct1==ar->mct1)
            MEM_DUP(ar->chartsAtTree1,ar->mct1,unsigned int);
          ar->chartsAtTree1[ar->nct1]=chartId;
          ar->nct1++;
        }
      else
        { 
          if(ar->nct2==ar->mct2) 
            MEM_DUP(ar->chartsAtTree2,ar->mct2,unsigned int);
          ar->chartsAtTree2[ar->nct2]=chartId;
          ar->nct2++;
        }
    }
}

boolean PointTowardRandSample(unsigned int cId,double d,double *t,
                              unsigned int samplingMode,
                              boolean onManifold,double *origRandSample,
                              unsigned int *randChart,
                              double *randParam,double *randSample,
                              double *deltaParam,
                              Tatlasrrt* ar)
{
  Tchart *c;
  double n;

  *randChart=cId;

  /* Get current chart */
  c=GetAtlasChart(cId,&(ar->atlas));

  /* w: Project randSample on the chart. Do not use topoogy here
     We already used it to fix origRandSample and randSample. If we
     take into account the topology we might end up not moving along
     the shortest path connecting two samples */
  if ((samplingMode!=TANGENT_SAMPLING)||(onManifold))
    Manifold2Chart(origRandSample,NULL,randParam,c);
  else
    Manifold2Chart(randSample,NULL,randParam,c);
  /* deltaParam: Vector from current parameters 't' to 'w' */
  DifferenceVector(ar->k,randParam,t,deltaParam);
  /* Ensure that the new randSample is at least at
     distance 'd' from 't' */
  n=Norm(ar->k,deltaParam);
  if (n>1e-3)
    {
      Normalize(ar->k,deltaParam);
      if ((samplingMode==TANGENT_SAMPLING)&&(!onManifold))
        SumVectorScale(ar->k,t,d,deltaParam,randParam);
      /* Transform randSample to ambient space. Again, skip
         the topology to keep the advance direction.*/
      Local2Global(randParam,NULL,randSample,c);

      return(FALSE);
    }
  else
    return(TRUE);
}

boolean AddChart2AtlasRRT(Tparameters *pr,unsigned int tree,unsigned int it,
                          TSampleInfo *currentSampleInfo,boolean* intersectParent,
                          Tatlasrrt *ar)
{
  unsigned int j,ncp,newChart;
  unsigned  nn,i,id;
  Tchart* ce;
  TChartInfo *currentChartInfo;
  unsigned int currentID;
  boolean sing;
  
  currentID=currentSampleInfo->id;

  /* need to generate a new chart at the current sample */
  /* change cID */
  ncp=ar->nc;

  /* This does not enforce the neighbouring relation with parent chart !! 
     but tries to detect singularities, if any (and enabled) */
  newChart=AddTrustedChart2Atlas(pr,currentSampleInfo->s,
                                 currentSampleInfo->pc,&sing,&(ar->atlas));

  if (newChart!=NO_UINT)
    {
      ar->nc=GetAtlasNumCharts(&(ar->atlas));

      /* Check if the newChart is neighbour with its parent chart */
      if (ncp<(ar->birrt?2:1))
        *intersectParent=TRUE; /* for the chart at the query points -> do not check */
      else 
        {
          (*intersectParent)=FALSE;  
          ce=GetAtlasChart(newChart,&(ar->atlas));
          if (SingularChart(GetAtlasChart(newChart,&(ar->atlas))))
            Warning("Singular chart in AtlasRRT (decrease epsilon?)");
          nn=ChartNumNeighbours(ce);

          for(i=0;((!(*intersectParent))&&(i<nn));i++) 
            {
              id=ChartNeighbourID(i,ce);
              if ((id!=NO_UINT)&&(id==currentSampleInfo->pc))
                (*intersectParent)=TRUE;
            }
          
          if ((!(*intersectParent))&&(currentSampleInfo->pc!=NO_UINT))
            {
              Tchart *parentChart;
              
              parentChart=GetAtlasChart(currentSampleInfo->pc,&(ar->atlas));
              ForceChartCut(pr,ar->tp,&(ar->ambient),newChart,ce,currentSampleInfo->pc,parentChart);
            }
        }

      #if (ATLASRRT_VERBOSE>1)
        fprintf(stderr,"   Sample at chart %u generated a new chart (%u): [%f %f ...] [%.16f %.16f %.16f ...]\n",
                currentSampleInfo->c,newChart,currentSampleInfo->t[0],currentSampleInfo->t[1],
                currentSampleInfo->s[0],currentSampleInfo->s[1],currentSampleInfo->s[2]);
      #endif

      /* expand chart info if necessary */
      if (ar->nc>ar->mc)
        {
          ar->mc=ar->nc+INIT_NUM_CHARTS;
          MEM_EXPAND(ar->ci,ar->mc,TChartInfo *);
        }

      /* and initialize an empty list of samples for the new charts */
      for(j=ncp;j<ar->nc;j++)
        {
          NEW(ar->ci[j],1,TChartInfo);
          ar->ci[j]->lc=NO_UINT;
          ar->ci[j]->it=it;
          ar->ci[j]->tree=0; /* chart in no tree initially */
          AddChart2Tree(tree,j,ar);
        }

      /* Now that the list of samples for the new charts is already initialized,
         we assign samples to the newly created charts */
      for(j=ncp;j<ar->nc;j++)
        PopulateWithSamples(pr,j,ar);

      /* Up to here we accomodated the new chart in the atlas and populated it
         with samples. Now we have to set up the information in the sample. */
      
      if (currentID==NO_UINT)
        {
          /* Sample not in the tree: temporary sample. The new chart is defined on a
             temporary sample. The sample is the center of the chart (just enforce it) */
          currentSampleInfo->c=newChart;
          currentSampleInfo->pc=newChart;
          for(j=0;j<ar->k;j++)
            currentSampleInfo->t[j]=0;
          /* 1+newChart -> we can find out which chart was created at each
             sample just by substracting one (used for debug). */
          currentSampleInfo->generateChart=1+newChart;
        }
      else
        {
          TSampleInfo *sampleInTree;
          
          /* A sample that is in the tree -> operate with the copy in the tree */

          sampleInTree=ar->si[currentID];
          sampleInTree->generateChart=1+newChart; /* just created a chart on the tree sample */
          if (sampleInTree->c!=newChart)
            {
              unsigned int n;
              boolean found;
              Tchart *ce;
              TChartInfo *newChartInfo;
              TSampleInfo *previousSampleInfo,*sampleInfo;

              /* For some reason the sample in the tree is not assigned to the
                 new chart. Correct the situation, stealing the sample from
                 the chart where it is currently assigned */

              n=sampleInTree->c; /*Chart from where to steal the center*/
              currentChartInfo=ar->ci[n];       
              newChartInfo=ar->ci[newChart]; /* The new chart -> this must receive the sample */

              /* iterate over the samples assigned to 'currentChart' until we find
                 'currentSampleID'. Then we re-assign it to 'newChart', properly adjusting
                 the lists of samples. */
              
              previousSampleInfo=NULL; /* previous sample in the list */
              j=currentChartInfo->lc;  /* first sample in the list */
              found=FALSE;
              while((j!=NO_UINT)&&(!found))
                {
                  sampleInfo=ar->si[j];
                  if (j==currentID)
                    {         
                      /* remove the sample from the list of chart c*/
                      if (previousSampleInfo==NULL)
                        currentChartInfo->lc=sampleInfo->lsc;
                      else
                        previousSampleInfo->lsc=sampleInfo->lsc;

                      /* add to the list of the new chart */
                      sampleInfo->lsc=newChartInfo->lc;
                      newChartInfo->lc=j;
                      sampleInfo->c=newChart;

                      /* Project the sample to the chart. This must result
                         in zero but....*/
                      ce=GetAtlasChart(newChart,&(ar->atlas));
                      Manifold2Chart(sampleInfo->s,ar->tp,sampleInfo->t,ce);
                      EnlargeChart(sampleInfo->t,ce);

                      found=TRUE;
                    }
                  else
                    {
                      /* Move to next */          
                      previousSampleInfo=sampleInfo;
                      j=sampleInfo->lsc;
                    }
                }
              if (!found)
                Error("A sample is not where it should be");
            }
          
          if (currentSampleInfo!=sampleInTree)
            {
              /* The input parameter is a copy of the node in the tree
                 We have to update the copy too (basically copy the
                 sample, the chartID, the parent chart, and the 
                 projection to the chart) */
              SetSampleInfo(ar->m,ar->k,currentSampleInfo,sampleInTree);
            }
        }
      return(TRUE); /* The chart could be actually created */
    }
  else
    return(FALSE); /* The chart could not be created (the given point
                      is in a singular region?) */
}


void PopulateWithSamples(Tparameters *pr,unsigned int id,Tatlasrrt *ar)
{
  Tchart *c,*c1,*ce;
  unsigned int i,j,jt,n,nn,ne;
  double *t;
  double epsilon;
  TSampleInfo *currentSampleInfo;
  TSampleInfo *previousSampleInfo;
  TChartInfo *currentChartInfo;
  TChartInfo *newChartInfo;
  
  epsilon=GetParameter(CT_EPSILON,pr);

  NEW(t,ar->k,double);

  c=GetAtlasChart(id,&(ar->atlas));
  nn=ChartNumNeighbours(c);

  for(i=0;i<nn;i++)
    {
      /* The neighbouring chart from where to steal some samples */
      n=ChartNeighbourID(i,c);
      if (n!=NO_UINT)
        {
          currentChartInfo=ar->ci[n];
          c1=GetAtlasChart(n,&(ar->atlas));

          previousSampleInfo=NULL; /* previous sample in the list */
          j=currentChartInfo->lc; /* first sample in the list */
          while(j!=NO_UINT)
            {
              currentSampleInfo=ar->si[j];
              ne=DetermineChartNeighbour(epsilon,currentSampleInfo->t,c1);
              if (ne!=NO_UINT)
                {
                  /* Sample 'j' is not any more in chart 'c1' (chart 'n') but in a
                     neighbouring one, typically the one just created (ne==id),
                     but can be any other neighbour */

                  newChartInfo=ar->ci[ne]; /* This should be the new chart */

                  #if (_DEBUG>2)
                    fprintf(stderr,"    Sample %u moves from chart %u to %u\n",j,n,ne);
                  #endif

                  /* Typically 'ce' will be 'c', but not for sure */
                  ce=GetAtlasChart(ne,&(ar->atlas));
                  Manifold2Chart(currentSampleInfo->s,ar->tp,currentSampleInfo->t,ce);
                  EnlargeChart(currentSampleInfo->t,ce);

                  currentSampleInfo->c=ne;

                  /* we will change lsc[j] in the following -> backup it */
                  jt=currentSampleInfo->lsc;

                  /* remove the sample from the list of chart c*/
                  if (previousSampleInfo==NULL)
                    currentChartInfo->lc=currentSampleInfo->lsc;
                  else
                    previousSampleInfo->lsc=currentSampleInfo->lsc;

                  /* add to the list of chart 'ne' (typically ne==id) */
                  currentSampleInfo->lsc=newChartInfo->lc;
                  newChartInfo->lc=j;

                  /* The chart importing the sample must be associated with
                     the tree including this sample. */
                  AddChart2Tree(GetRRTNodeTree(j,&(ar->rrt)),ne,ar);

                  /* Set 'j' to the next in the list of samples for the current 
                     chart (previously backed up) */
                  j=jt;

                  /* Since we removed one element from the list the
                     previous pointed by 'previousSampleInfo' remains the same. */
                }
              else
                {
                  /* move to next sample in the list of samples assigned to
                     chart 'n' */
                  previousSampleInfo=currentSampleInfo;
                  j=currentSampleInfo->lsc;
                }
            }
        }
    }

  free(t);
}

unsigned int ReconstructAtlasRRTPath(Tparameters *pr,unsigned int sID,
                                     double *pl,unsigned int *ns,double ***path,Tatlasrrt *ar)
{
  unsigned int nv,nvs,i,ms;
  signed int j;
  double *o,c;
  boolean *systemVars;
  unsigned int lns;
  double **lpath;
  unsigned int p;
  #if (NEW_BRANCH_EXTENSION)
    TAtlasRRTBranchConfig configConnect;
    TAtlasRRTBranchStatus statusConnect;

    InitBranchConfig(CONNECT_SAFE,&configConnect);
  #else
    unsigned int ls;
    boolean reachedParent,reachedG;
  #endif
  
  *pl=0;
  
  /* Get the sample in the tree close to the goal */
  nv=CS_WD_GET_SYSTEM_VARS(&systemVars,ar->w);
  nvs=0;
  for(i=0;i<nv;i++)
    {
      if (systemVars[i])
        nvs++;
    }

  InitSamples(&ms,ns,path);
      
  i=sID;
  while (i!=NO_UINT)
    {
      CS_WD_REGENERATE_ORIGINAL_POINT(pr,ar->si[i]->s,&o,ar->w);
      AddSample2Samples(nv,o,nvs,systemVars,&ms,ns,path);
      free(o);
      p=GetRRTParent(i,&(ar->rrt));
      if (p!=NO_UINT)
        {
          /* Check that the two samples can be actually connected and 
             measure the distance between them. */
          #if (NEW_BRANCH_EXTENSION)
            c=New_AddBranchToAtlasRRT(pr,NO_UINT,&configConnect,p,ar->si[i]->s,NULL,NULL,
                                      &statusConnect,&lns,&lpath,ar);
          #else
            c=AddBranchToAtlasRRT(pr,NO_UINT,ADD_NONE,TRUE,FALSE,TRUE,FALSE,
                                  0.0,p,ar->si[i]->s,NULL,NULL,&ls,&reachedParent,&reachedG,
                                  &lns,&lpath,NULL,NULL,ar);
          #endif
            
          #if (NEW_BRANCH_EXTENSION)
            if (!statusConnect.reachedQrand)
          #else
            if (!reachedParent)
          #endif
              Warning("Path segment could not be reconstructed in ReconstructAtlasRRTPath");
            
          (*pl)+=c;
          #if (ATLASRRT_VERBOSE)
            fprintf(stderr,"{%u->%u}[%g]",i,p,c);
          #endif
            
          for(j=lns-1;j>=0;j--)
            {
              CS_WD_REGENERATE_ORIGINAL_POINT(pr,lpath[j],&o,ar->w);
              AddSample2Samples(nv,o,nvs,systemVars,&ms,ns,path);
              free(o);
            }

          DeleteSamples(lns,lpath);
        }
      i=p;
    }
  ReverseSamples(*ns,*path);

  free(systemVars);

  return(nvs);
}


unsigned int Steps2PathinAtlasRRT(Tparameters *pr,Tvector *steps,double *pl,double *pc,
                                  unsigned int *ns,double ***path,Tatlasrrt *ar)
{
  unsigned int nv,nvs,ms,n,i,k,current,next;
  signed int j;
  double *o,c;
  boolean *systemVars;
  unsigned int lns;
  double **lpath;
  double epsilon,dc;
  #if (NEW_BRANCH_EXTENSION)
    double delta;
    TAtlasRRTBranchConfig configConnect;
    TAtlasRRTBranchStatus statusConnect;

    InitBranchConfig(CONNECT_SAFE,&configConnect);
    delta=GetParameter(CT_DELTA,pr);
  #else
    unsigned int ls;
    boolean reachedG,reachedNext;
  #endif

  #if (ATLASRRT_VERBOSE)
    fprintf(stderr,"*****************************************************\n");
    fprintf(stderr,"Defining the solution path\n");
    fprintf(stderr,"*****************************************************\n");
  #endif
    
  *pl=0; /* path length */
  if (pc!=NULL) /* path cost (mechanical work) */
    {
      *pc=0;
      epsilon=GetParameter(CT_EPSILON,pr);
    }

  /* Identify system variables */
  nv=CS_WD_GET_SYSTEM_VARS(&systemVars,ar->w);
  nvs=0;
  for(i=0;i<nv;i++)
    {
      if (systemVars[i])
        nvs++;
    }

  InitSamples(&ms,ns,path);
      
  current=*(unsigned int *)GetVectorElement(0,steps);
  n=VectorSize(steps);
  k=1;
  while (k<n)
    {
      next=*(unsigned int *)GetVectorElement(k,steps);
      
      CS_WD_REGENERATE_ORIGINAL_POINT(pr,ar->si[current]->s,&o,ar->w);
      AddSample2Samples(nv,o,nvs,systemVars,&ms,ns,path);
      free(o);
      
      #if (NEW_BRANCH_EXTENSION)
        c=New_AddBranchToAtlasRRT(pr,NO_UINT,&configConnect,current,ar->si[next]->s,NULL,NULL,
                                  &statusConnect,&lns,&lpath,ar);
      #else
        c=AddBranchToAtlasRRT(pr,NO_UINT,ADD_NONE,TRUE,FALSE,TRUE,FALSE,
                              0.0,current,ar->si[next]->s,NULL,NULL,&ls,&reachedNext,&reachedG,
                              &lns,&lpath,NULL,NULL,ar);
      #endif

      #if (ATLASRRT_VERBOSE)
        fprintf(stderr,"{%u->%u}[%g]",current,next,c);
      #endif

      (*pl)+=c; /* Update the path length */
      if(pc!=NULL) /* Update the path cost (mechanical work) */
        {
          dc=GetRRTNodeCost(current,&(ar->rrt))-GetRRTNodeCost(next,&(ar->rrt));
          (*pc)+=(dc>0?dc:0)+epsilon*c;
        }

      #if (NEW_BRANCH_EXTENSION)
        if (statusConnect.distQrand>delta)
      #else
        if (!reachedNext)
      #endif
          Warning("Path segment could not be reconstructed in Steps2PathinAtlasRRT");
      else 
        {
          for(j=0;j<lns;j++)
            {
              CS_WD_REGENERATE_ORIGINAL_POINT(pr,lpath[j],&o,ar->w);
              AddSample2Samples(nv,o,nvs,systemVars,&ms,ns,path);
              free(o);
            }
          DeleteSamples(lns,lpath);
        }
        
      current=next;
      k++;
    }

  free(systemVars);

  #if (ATLASRRT_VERBOSE)
    fprintf(stderr,"*****************************************************\n");
    fprintf(stderr,"*****************************************************\n");
  #endif
    
  return(nvs);
}

void SmoothPathInAtlasRRT(Tparameters *pr,unsigned int sID,Tatlasrrt* ar)
{
  signed int l;
  unsigned int i,j,k,s,n,n1,n2,o;
  unsigned int *path;
  double c12,c2_new,c1,c2;
  #if (NEW_BRANCH_EXTENSION)
    TAtlasRRTBranchConfig configConnect;
    TAtlasRRTBranchStatus statusConnect;

    InitBranchConfig(CONNECT_SAMPLES,&configConnect);
  #else
    boolean reachedQRand,reachedGoal;
    unsigned int lastID;
  #endif
    
  n=StepsToRoot(sID,&(ar->rrt));
  NEW(path,n,unsigned int);
  l=n-1;
  s=sID;
  while (s!=NO_UINT)
    {   
      path[l]=s;
      s=GetRRTParent(s,&(ar->rrt));
      l--;
    }
  s=n; /* initial size of the path */
  for(l=0;l<s;l++)
    {
      /* Select a node at random in the path */
      i=randomMax(n-2);
      j=i+1+randomMax(n-2-i);

      n1=path[i];
      n2=path[j];

      c1=GetRRTNodeCost(n1,&(ar->rrt));
      c2=GetRRTNodeCost(n2,&(ar->rrt));

      /* Try to connect the two samples with a shorcut and measure its length */
      #if (NEW_BRANCH_EXTENSION)
        configConnect.maxLength=c2-c1;
        c12=New_AddBranchToAtlasRRT(pr,NO_UINT,&configConnect,n1,ar->si[n2]->s,NULL,NULL,
                                    &statusConnect,NULL,NULL,ar);
      #else
        c12=AddBranchToAtlasRRT(pr,NO_UINT,ADD_NONE,TRUE,TRUE,TRUE,FALSE,
                                c2-c1,n1,ar->si[n2]->s,NULL,
                                NULL,&lastID,&reachedQRand,&reachedGoal,NULL,NULL,
                                NULL,NULL,ar);
      #endif

      /* If node n2 can be reached from node n1 */
      #if (NEW_BRANCH_EXTENSION)
        if (statusConnect.reachedQrand)
      #else
        if (reachedQRand)
      #endif
          {
            /* and the cost is smaller */
            c2_new=c1+c12;
            if (c2_new<c2)
              {
                /* Set n1 as a parent of n2*/
                SetRRTNodeCost(n2,c12,c2_new,&(ar->rrt));
                SetRRTParent(n2,n1,&(ar->rrt));
              
                /* Shrink the path */
                for(k=j,o=i+1;k<n;k++,o++)
                  path[o]=path[k];
                n=o;
              }
          }
    }
  free(path);
}

boolean RandomPointInAtlasTree(Tparameters *pr,
                               double scale,unsigned int tree,
                               unsigned int *nm,double *t,double *p,
                               Tatlasrrt* ar)
{
  boolean havePoint;

  /* This is the place where to implement different sampling strategies,
     if desired. We can, for instance, we can select the charts taking
     into account the number of samples (more samples, less probability
     of selecting them). This will approximate the AtlasRRT to an EST. */

  if ((ar->birrt)&&(tree!=BOTHTREES))
    {
      /* If the sampling radius is potentially different for each chart, 
         we have to take into account the size of sampling area for each chart to 
         uniformly sample on the part of the C-space explored so far. */
      #if (ADJUST_SA==1)
      {
        double *w,s;
        unsigned int i,nc;
        unsigned int *c;

        if (tree==START2GOAL) 
          {
            nc=ar->nct1;
            c=ar->chartsAtTree1;
          }
        else 
          {
            nc=ar->nct2;
            c=ar->chartsAtTree2;
          }
        NEW(w,nc,double);
        s=0.0;
        for(i=0;i<nc;i++)
          {
            /* The 'scale' factor, if any affects all charts and, thus it can be
               ruled out. */
            w[i]=GetChartSamplingRadius(GetAtlasChart(c[i],&(ar->atlas)));
            w[i]=pow(w[i],(double)ar->k);
            s+=w[i];
          }
        *nm=c[randomWithDistribution(nc,s,w)];
        free(w);
      }
      #else
        /* Uniform distribution on charts of the tree */
        if (tree==START2GOAL) 
          *nm=ar->chartsAtTree1[randomMax(ar->nct1-1)];
        else 
          *nm=ar->chartsAtTree2[randomMax(ar->nct2-1)];
      #endif 

      /* Get a point from the selected chart */
      havePoint=RandomPointInChart(pr,scale,ar->tp,t,p,GetAtlasChart(*nm,&(ar->atlas)));
    }
  else
    {
      double *w=NULL;

      #if (ADJUST_SA==1)
      {
        double s;
        unsigned int i,nc;

        nc=GetAtlasNumCharts(&(ar->atlas));
        NEW(w,nc,double);
        s=0.0;
        for(i=0;i<nc;i++)
          {
            w[i]=GetChartSamplingRadius(GetAtlasChart(i,&(ar->atlas)));
            w[i]=pow(w[i],(double)ar->k);
            s+=w[i];
          }
        *nm=randomWithDistribution(nc,s,w);
      }
      #endif

      havePoint=RandomPointInAtlas(pr,scale,w,nm,t,p,&(ar->atlas));

      #if (ADJUST_SA==1)
        free(w);
      #endif
    }

  return(havePoint);
}


void PlotQrand(Tparameters *pr,char *prefix,unsigned int inear,
               unsigned int c_rand,double *t_rand,double *q_rand,
               unsigned int xID,unsigned int yID,unsigned int zID,
               Tatlasrrt *ar)
{
  Tfilename fplot;
  double *point,*center,*near;
  double *o1,*o2,*o3;
  Tchart *chart;  
  Tcolor color;
  Tplot3d p3d;
  double x[2],y[2],z[2];
  
  CreateFileName(NULL,prefix,"_qrand",PLOT3D_EXT,&fplot);  
  fprintf(stderr,"Plotting Qrand to                 : %s\n",GetFileFullName(&fplot));

  NewColor(1,0,1,&color); /*magenta*/
  InitPlot3d(GetFileFullName(&fplot),FALSE,0,NULL,&p3d);
  DeleteFileName(&fplot);

  NEW(point,ar->m,double);
  chart=GetAtlasChart(c_rand,&(ar->atlas));

  Local2Global(t_rand,ar->tp,point,chart); /* point must be the same as q_rand */
  CS_WD_REGENERATE_ORIGINAL_POINT(pr,point,&o1,ar->w);

  center=GetChartCenter(chart);
  CS_WD_REGENERATE_ORIGINAL_POINT(pr,center,&o2,ar->w);
  
  StartNew3dObject(&color,&p3d);

  /* A large cross at q_rand  */
  x[0]=o1[xID]-0.15;
  y[0]=o1[yID];
  z[0]=o1[zID];

  x[1]=o1[xID]+0.15;
  y[1]=o1[yID];
  z[1]=o1[zID];

  PlotVect3d(2,x,y,z,&p3d);

  x[0]=o1[xID];
  y[0]=o1[yID]-0.15;
  z[0]=o1[zID];

  x[1]=o1[xID];
  y[1]=o1[yID]+0.15;
  z[1]=o1[zID];

  PlotVect3d(2,x,y,z,&p3d);

  x[0]=o1[xID];
  y[0]=o1[yID];
  z[0]=o1[zID]-0.15;

  x[1]=o1[xID];
  y[1]=o1[yID];
  z[1]=o1[zID]+0.15;

  PlotVect3d(2,x,y,z,&p3d);

  /* A line from q_rand to its chart center */
  x[0]=o1[xID];
  y[0]=o1[yID];
  z[0]=o1[zID];

  x[1]=o2[xID];
  y[1]=o2[yID];
  z[1]=o2[zID];

  PlotVect3d(2,x,y,z,&p3d);

  /* A line to q_near */
  near=ar->si[inear]->s;
  CS_WD_REGENERATE_ORIGINAL_POINT(pr,near,&o3,ar->w);
  
  x[0]=o1[xID];
  y[0]=o1[yID];
  z[0]=o1[zID];

  x[1]=o3[xID];
  y[1]=o3[yID];
  z[1]=o3[zID];

  PlotVect3d(2,x,y,z,&p3d);

  DeleteColor(&color);
  Close3dObject(&p3d);
  ClosePlot3d(FALSE,0,0,0,&p3d);

  free(o1);
  free(o2);
  free(o3);
  free(point);
}

void PlotConnection(Tparameters *pr,char *prefix,
                    unsigned int target,unsigned int near,unsigned int end,
                    unsigned int xID,unsigned int yID,unsigned int zID,
                    Tatlasrrt *ar)
{
  Tfilename fplot;
  double *point;
  double *o1,*o2,*o3;
  Tcolor color;
  Tplot3d p3d;
  double x[2],y[2],z[2];

  CreateFileName(NULL,prefix,"_connect",PLOT3D_EXT,&fplot);  
  fprintf(stderr,"Plotting connection to            : %s\n",GetFileFullName(&fplot));

  NewColor(1,0,1,&color); /*magenta*/
  InitPlot3d(GetFileFullName(&fplot),FALSE,0,NULL,&p3d);
  DeleteFileName(&fplot);

  point=ar->si[target]->s;
  CS_WD_REGENERATE_ORIGINAL_POINT(pr,point,&o1,ar->w);

  point=ar->si[near]->s;
  CS_WD_REGENERATE_ORIGINAL_POINT(pr,point,&o2,ar->w);

  point=ar->si[end]->s;
  CS_WD_REGENERATE_ORIGINAL_POINT(pr,point,&o3,ar->w);
  
  StartNew3dObject(&color,&p3d);

  x[0]=o1[xID];
  y[0]=o1[yID];
  z[0]=o1[zID];

  x[1]=o2[xID];
  y[1]=o2[yID];
  z[1]=o2[zID];

  PlotVect3d(2,x,y,z,&p3d);

  x[0]=o1[xID];
  y[0]=o1[yID];
  z[0]=o1[zID];

  x[1]=o3[xID];
  y[1]=o3[yID];
  z[1]=o3[zID];

  PlotVect3d(2,x,y,z,&p3d);

  DeleteColor(&color);
  Close3dObject(&p3d);
  ClosePlot3d(FALSE,0,0,0,&p3d);

  free(o1);
  free(o2);
  free(o3);
}

void SaveAtlasRRTSampleInfo(FILE *f,TSampleInfo *si,Tatlasrrt *ar)
{
  unsigned int i;

  fprintf(f,"%u\n",si->id);
  for(i=0;i<ar->k;i++)
    fprintf(f,"%.16f ",si->t[i]);
  fprintf(f,"\n");
  fprintf(f,"%u\n",si->pc);
  fprintf(f,"%u\n",si->c);
  fprintf(f,"%u\n",si->generateChart);
  fprintf(f,"%u\n",si->lsc);
}

void SaveAtlasRRTChartInfo(FILE *f,TChartInfo *ci,Tatlasrrt *ar)
{
  fprintf(f,"%u\n",ci->lc);
  fprintf(f,"%u\n",ci->tree);
  fprintf(f,"%u\n",ci->it);
}

void LoadAtlasRRTSampleInfo(FILE *f,TSampleInfo *si,Tatlasrrt *ar)
{
  unsigned int i;

  fscanf(f,"%u",&(si->id));
  si->s=GetRRTNode(si->id,&(ar->rrt));
  NEW(si->t,ar->k,double);
  for(i=0;i<ar->k;i++)
    fscanf(f,"%lf",&(si->t[i]));
  fscanf(f,"%u",&(si->pc));
  fscanf(f,"%u",&(si->c));
  fscanf(f,"%u",&(si->generateChart));
  fscanf(f,"%u",&(si->lsc));
}

void LoadAtlasRRTChartInfo(FILE *f,TChartInfo *ci,Tatlasrrt *ar)
{
  fscanf(f,"%u",&(ci->lc));
  fscanf(f,"%u",&(ci->tree));
  fscanf(f,"%u",&(ci->it));
}

/******************************************************************************/
/******************************************************************************/
/******************************************************************************/

void InitAtlasRRT(Tparameters *pr,boolean parallel,double *ps,unsigned int mode,boolean graph,
                  double *pg,TAtlasBase *w,Tatlasrrt *ar)
{
  unsigned int i;
  boolean intersectParent;
  TSampleInfo *currentSampleInfo;
  

  /*************************************************************/
  /* Init the basic AtlasRRT information */
  ar->w=w;
  CS_WD_GENERATE_SIMP_INITIAL_BOX(pr,&(ar->ambient),ar->w);
  ar->m=GetBoxNIntervals(&(ar->ambient));
  ar->e=GetParameter(CT_E,pr);;
  ar->ce=GetParameter(CT_CE,pr);
  ar->r=GetParameter(CT_R,pr);
  /* Exploration trees are forced to be one-directional */
  ar->birrt=((!EXPLORATION_RRT)&&(mode!=ONE_TREE));
  ar->parallel=parallel;

  /*************************************************************/
  /* Init the Atlas */
  /* The atlas is initilized on the initial point only, latter
     on we add a chart for the final point if the AtlasRRT is
     bidirectional. */
  if (!InitAtlasFromPoint(pr,FALSE,TRUE,ps,w,&(ar->atlas)))
    Error("Can not start the atlas at the given point");
  ar->mc=INIT_NUM_CHARTS;

  ar->nc=GetAtlasNumCharts(&(ar->atlas));

  /* Note that InitAtlasFromPoint can change this parameter */
  ar->k=(unsigned int)GetParameter(CT_N_DOF,pr);

  /*************************************************************/
  /* Init the RRT */
  /* This verifies that the initial point are valid (on
     the manifold, collision free, in domain,...). It also
     checks this for the final point on bidirectional RRTs */
  ar->ms=INIT_NUM_SAMPLES_RRT;
  InitRRT(pr,ar->parallel,FALSE,ps,mode,graph,pg,ar->m,w,&(ar->rrt));
  ar->ns=GetRRTNumNodes(&(ar->rrt));

  /*************************************************************/
  /* Init the mapping from samples to charts */
  NEW(ar->ci,ar->mc,TChartInfo*);
  for(i=0;i<ar->nc;i++)
    {
      NEW(ar->ci[i],1,TChartInfo);
      ar->ci[i]->lc=NO_UINT; /* No samples in any chart */
      ar->ci[i]->tree=START2GOAL;
      ar->ci[i]->it=0;
    }

  NEW(ar->si,ar->ms,TSampleInfo*);

  NEW(ar->si[0],1,TSampleInfo);
  /* This sets t[0] to zero (sample[0] is the center of the first chart) */
  currentSampleInfo=ar->si[0];

  NEW(currentSampleInfo->t,ar->k,double);
  for(i=0;i<ar->k;i++)
    currentSampleInfo->t[i]=0.0;

  /* Chart 0 is not generated from any other chart */
  currentSampleInfo->pc=NO_UINT;

  /* Pointer to the first sample in the RRT */
  currentSampleInfo->s=GetRRTNode(0,&(ar->rrt));
  currentSampleInfo->id=0;

  /* assign the first sample to the first chart */
  currentSampleInfo->c=0; /* fist sample is in chart 0 */
  ar->ci[0]->lc=0; /* list of samples for chart 0 start at sample 0 */
  currentSampleInfo->lsc=NO_UINT; /* and there is no other sample next */

  /* we have a chart on sample 0 */
  currentSampleInfo->generateChart=1;

  /*************************************************************/
  /* Init the mapping betwen charts and trees */
  if (ar->birrt)
    {
      NEW(ar->si[1],1,TSampleInfo);
      currentSampleInfo=ar->si[1];

      /* The goal samples has param 0 in its own chart */
      NEW(currentSampleInfo->t,ar->k,double);

      for(i=0;i<ar->k;i++)
        currentSampleInfo->t[i]=0.0;

      /* Allocate the samples 2 tree mapping  */
      ar->mct1=ar->mc;
      NEW(ar->chartsAtTree1,ar->mct1,unsigned int);

      ar->mct2=ar->mc;
      NEW(ar->chartsAtTree2,ar->mct2,unsigned int);

      /* Assign char 0 to the first tree */
      ar->chartsAtTree1[0]=0;
      ar->nct1=1;

      ar->nct2=0;
      /* The chart for the goal sample is added to the tree
         using AddChart2AtlasRRT (see below) and this automatically
         updates ar->chartsAtTree2, ar->nct2,...*/
 
      /* Chart 1 is not generated from any other chart */
      currentSampleInfo->pc=NO_UINT;

      /* Pointer to the sample in the RRT */
      currentSampleInfo->s=GetRRTNode(1,&(ar->rrt));
      currentSampleInfo->id=1;
      
      /* By setting c[1]=1, AddChart2AtlasRRT does not
         re-assigns the new sample. We do it manually
         after the chart creation. */
      currentSampleInfo->c=1;
         
      if (!AddChart2AtlasRRT(pr,GOAL2START,0,currentSampleInfo,&intersectParent,ar))
        Error("Could not init the AtlasRRT on the given point (is it singular? decrease epsilon?)");
      
      /* Manually assign sample 1 to chart 1 */
      ar->ci[1]->lc=1;
      currentSampleInfo->lsc=NO_UINT;
    }
  
  /*************************************************************/
  /* Get the topology for all variables */
  ar->tp=GetRRTTopology(&(ar->rrt));

  /*************************************************************/
  /* Since atlas are defined on the simplified system we get the
     corresponding Jacobian. */
  CS_WD_GET_SIMP_JACOBIAN(pr,&(ar->J),ar->w);
}

boolean AtlasRRTSample(Tparameters *pr,unsigned int samplingMode,
                       unsigned int it,unsigned int tree,
                       double *goal,double scale,boolean *exploration,
                       unsigned int *c_rand,double *t_rand,double *q_rand,
                       TAtlasRRTStatistics *arst,Tatlasrrt *ar)
{
  boolean expand2Goal,validSample;
  #if (ATLASRRT_VERBOSE)
    unsigned int s;
  #endif
  
  expand2Goal=((goal!=NULL)&&((it==0)||((!ar->birrt)&&(randomDouble()<0.01))));

  if (expand2Goal)
    {
      memcpy(q_rand,goal,ar->m*sizeof(double));
      #if (GET_ATLASRRT_STATISTICS)
        NewAtlasRRTRandomSample(arst);
      #endif
      #if (ATLASRRT_VERBOSE)
        fprintf(stderr,"  Expanding to goal-----------------------------------------------------------------\n");
      #endif
      *exploration=FALSE;
      *c_rand=NO_UINT;
    }
  else
    { 
      #if (ATLASRRT_VERBOSE)
        s=0;
      #endif
      switch(samplingMode)
        {
        case AMBIENT_SAMPLING:
        case KDTREE_SAMPLING:
          do {
            RRTSample(samplingMode,tree,NULL,q_rand,NULL,&(ar->rrt));
            validSample=CS_WD_SIMP_INEQUALITIES_HOLD(pr,q_rand,ar->w);
            #if (ATLASRRT_VERBOSE)
              s++;
            #endif
            #if (GET_ATLASRRT_STATISTICS)
              NewAtlasRRTRandomSample(arst);
              if (!validSample)
                NewAtlasRRTSampleRejection(arst);
            #endif
          } while (!validSample);
          *exploration=FALSE;
          *c_rand=NO_UINT;
          #if (ATLASRRT_VERBOSE)
            fprintf(stderr,"  Random point in ambient. Rej: %u\n",s);
          #endif
          break;

        case TANGENT_SAMPLING:
          do {
            validSample=RandomPointInAtlasTree(pr,scale,tree,c_rand,t_rand,q_rand,ar);
            #if (ATLASRRT_VERBOSE)
              s++;
            #endif
            #if (ADJUST_SA==1)
              if (!validSample)
                DecreaseChartSamplingRadius(GetAtlasChart(*c_rand,&(ar->atlas)));
            #endif
            #if (GET_ATLASRRT_STATISTICS)
              NewAtlasRRTRandomSample(arst);
              if (!validSample)
                NewAtlasRRTSampleRejection(arst);
            #endif
          } while (!validSample);

          #if (ADJUST_SA==1)
            IncreaseChartSamplingRadius(GetAtlasChart(*c_rand,&(ar->atlas)));
          #endif
          *exploration=(Norm(ar->k,t_rand)>ar->r);
          #if (ATLASRRT_VERBOSE)
            fprintf(stderr,"  Random point on chart %u (rej: %u) [%g:%s]\n",*c_rand,s,
                    Norm(ar->k,t_rand),(*exploration?"exp":"non-exp"));
          #endif
          break;
        default:
          Error("Unknow sampling mode in AtlasRRTSample");
          break;
        }
    }
  
  return(expand2Goal);
}

boolean AtlasRRTValidateSample(Tparameters *pr,double *q_rand,unsigned int tree,boolean expand2goal,
                               unsigned int lastNN2Goal,double *goal,double l,double *h,unsigned int *i_near,
                               TAtlasRRTStatistics *arst,Tatlasrrt *ar)
{
  boolean validSample=TRUE;

  /* check if the sample in invalid due to a heuristic used in optimal path planning */
  if ((expand2goal)||(goal==NULL))
    *h=0;
  else
    {
      *h=DistanceTopology(ar->m,ar->tp,q_rand,goal);
      if ((l<INF)&&((HEURISTIC_RRT_STAR)&(1)))
        validSample=((DistanceTopology(ar->m,ar->tp,ar->si[0]->s,q_rand)+*h)<=l);
    }

  /* Now check if the sample is invalid due to its nearest neighbour (dynamic domain et al.) */
  if (validSample)
    {
      #if (ATLASRRT_GLOBAL_NN)
        /*search in the entire atlas */
        *i_near=GetRRTNN(tree,q_rand,&(ar->rrt));
      #else
        if (expand2Goal)
          *i_near=GetRRTNN(tree,q_rand,&(ar->rrt));
        else
          {
            /* limit the search to q_rand chart and neigbors charts */ 
            *i_near=GetRRTNNInNeighbourChart(tree,c_rand,t_rand,q_rand,ar);
            if (*i_near==NO_UINT)
              *i_near=GetRRTNN(tree,q_rand,&(ar->rrt));
          }
      #endif
      #if (ATLASRRT_VERBOSE)
        fprintf(stderr,"  Nearest point %u on chart %u \n",*i_near,ar->si[*i_near]->c);
        fflush(stdout);
      #endif

      /* Check if the random sample is in the domain of the nearest neighbour */
      /* This only has effect if paramter DYNAMIC_DOMAIN is >0 */
      validSample=(((!expand2goal)||(lastNN2Goal!=*i_near))&&
                   (InDynamicDomain(*i_near,q_rand,&(ar->rrt))));
    }

  #if (GET_ATLASRRT_STATISTICS)
    if (!validSample)
      NewAtlasRRTSampleRejection(arst);
  #endif

  return(validSample);
}

boolean AtlasRRT(Tparameters *pr,double *pg,double *time,
                 double *pl,unsigned int *ns,double ***path,
                 TAtlasRRTStatistics *str,Tatlasrrt *ar)
{
  boolean done;
  double dst;
  unsigned int it,n;
  double er,epsilon,h;
  double *pWithDummies,*initialPoint,*finalPoint,*q_rand,*q_goal,*t_rand;
  boolean expand2Goal,pathFound;
  unsigned int samplingMode;
  unsigned int i_near1,i_near2;
  unsigned int t1,t2;
  unsigned int lastSample;
  unsigned int lastSample2;
  TAtlasRRTStatistics *arst;
  Tstatistics stime; /* used just to measure execution time */
  boolean collision=FALSE;
  unsigned int target;
  unsigned int lastNN2Goal;
  unsigned int db;
  double maxTime;
  unsigned int maxNodes;
  boolean exploration;
  double scale; /* Global scale factor for the sampling areas */
  boolean validSample;
  unsigned int c_rand;
  #if (NEW_BRANCH_EXTENSION)
    TAtlasRRTBranchConfig configExtend,configConnect;
    TAtlasRRTBranchStatus statusExtend,statusConnect;

    InitBranchConfig(EXTEND_RRT,&configExtend);
    InitBranchConfig(CONNECT_RRTs,&configConnect);
  #else
    boolean reachedQRand,reachedGoal;
  #endif
   
  db=(unsigned int)GetParameter(CT_DETECT_BIFURCATIONS,pr);
  if (db>0)
    Error("AtlasRRT can not deal with bifurcations (yet)");
  
  samplingMode=(unsigned int)GetParameter(CT_SAMPLING,pr);

  scale=1.0; /* Not used yet...  */

  /* Initially the AtlasRRT must only include one sample and one chart or
     two sample and two chart for bidirectional trees */
  n=(ar->birrt?2:1);
  if ((ar->ns!=n)||(ar->nc!=n))
    Error("AtlasRRT must be used on a just initialized AtlasRRT");

  #if (GET_ATLASRRT_STATISTICS)
  {
    unsigned int i;

    NEW(arst,1,TAtlasRRTStatistics);
    InitAtlasRRTStatistics(arst);
    for(i=0;i<ar->nc;i++)
      NewAtlasRRTChart(arst);
    for(i=0;i<ar->ns;i++)
      NewAtlasRRTSample(arst);
  } 
  #else 
    arst=NULL;
  #endif

  /* Init the RRT structure */
  epsilon=GetParameter(CT_EPSILON,pr);
  maxTime=GetParameter(CT_MAX_PLANNING_TIME,pr);
  maxNodes=(unsigned int)GetParameter(CT_MAX_NODES_RRT,pr);
  
  /* For bi-directional AtlasRRTs the goal is already in the RRT
     and it is already verified */
  initialPoint=ar->si[0]->s;
  if (ar->birrt)
    finalPoint=ar->si[1]->s;
  else
    {
      if (EXPLORATION_RRT)
        finalPoint=NULL;
      else
        {
          /* For one-directional RRTS, the goal is given as a parameter when
             calling this function and we must ensure that it is valid */
          CS_WD_REGENERATE_SOLUTION_POINT(pr,pg,&pWithDummies,ar->w);
          CS_WD_GENERATE_SIMPLIFIED_POINT(pr,pWithDummies,&finalPoint,ar->w);

          if ((!PointInBoxTopology(NULL,TRUE,ar->m,finalPoint,epsilon,ar->tp,&(ar->ambient)))||
              (!CS_WD_SIMP_INEQUALITIES_HOLD(pr,finalPoint,ar->w)))
            Error("The goal point for the RRT is not in domain");

          if (ar->m==ar->k) /* empty system of equations */
            er=0;
          else
            er=CS_WD_ERROR_IN_SIMP_EQUATIONS(pr,finalPoint,ar->w);
          if (er>epsilon)
            Error("The target point is not on the manifold");
      
          collision=CS_WD_ORIGINAL_IN_COLLISION(pr,pWithDummies,NULL,ar->w);
          if (collision)
            Error("Target point for the RRT is in collision");
          free(pWithDummies);
        }
    }

  NEW(q_rand,ar->m,double);
  if (ar->birrt)
    NEW(q_goal,ar->m,double);
  NEW(t_rand,ar->k,double); 

  done=FALSE;
  lastNN2Goal=NO_UINT;
  it=0;
  InitStatistics(0,0,&stime);
  *time=0.0;

  /* The two trees. This is only used for bidirectiononal RRTs. In norma
     RRTs only the START2GOAL tree is extended*/
  t1=START2GOAL;
  t2=GOAL2START;

  while((!done)&&(*time<maxTime)&&(ar->ns<maxNodes))
    {
      #if (!ATLASRRT_VERBOSE)
        if ((it%1000)==0)
      #endif
          fprintf(stderr,"Iteration: %u (s:%u c:%u t:%u tm:%g sc:%.2f)\n",it,ar->ns,ar->nc,t1,*time,scale);
      
      /*******************************************************************/
      /* Sample */
      exploration=FALSE;
      do {
        /* Generate a random sample */
        expand2Goal=AtlasRRTSample(pr,samplingMode,it,t1,finalPoint,scale,
                                   &exploration,&c_rand,t_rand,q_rand,arst,ar);
  
        /* validate the random sample */
        validSample=AtlasRRTValidateSample(pr,q_rand,t1,expand2Goal,lastNN2Goal,NULL,INF,
                                           &h,&i_near1,arst,ar);
      } while (!validSample);

      if (expand2Goal)
        lastNN2Goal=i_near1;

      /*******************************************************************/
      /* Extend the RRT from q_near toward q_rand */
      #if (GET_ATLASRRT_STATISTICS)
        NewAtlasRRTBranch(arst);
        NewAtlasRRTDistanceQrand(DistanceTopology(ar->m,ar->tp,ar->si[i_near1]->s,q_rand),arst);
      #endif
      #if (NEW_BRANCH_EXTENSION)
        configExtend.onManifold=expand2Goal; /* can eventually be true for oneDirectional RRTs */
        configExtend.explorationSample=exploration;
        dst=New_AddBranchToAtlasRRT(pr,it,&configExtend,i_near1,q_rand,
                                    (((!ar->birrt)||(t1==START2GOAL))?finalPoint:initialPoint),
                                    (void*)arst,&statusExtend,NULL,NULL,ar);
        
        lastSample=statusExtend.lastID;
        done=((!EXPLORATION_RRT)&&(!ar->birrt)&&(statusExtend.reachedGoal));
      #else
        dst=AddBranchToAtlasRRT(pr,it,ADD_ALL,TRUE,TRUE,expand2Goal,exploration,
                                0.0,i_near1,q_rand,
                                (((!ar->birrt)||(t1==START2GOAL))?finalPoint:initialPoint),
                                (void*)arst,&lastSample,&reachedQRand,&reachedGoal,NULL,NULL,
                                NULL,NULL,ar);
        
        done=((!EXPLORATION_RRT)&&(!ar->birrt)&&(reachedGoal));
      #endif
        
      #if (GET_ATLASRRT_STATISTICS)
        if (dst>0)
          NewAtlasRRTNoEmptyBranch(arst);
      #endif
      
      if ((!done)&&(ar->birrt))
        {
          if (dst>0)
            {
              #if (ADJUST_SA)
                if (samplingMode==TANGENT_SAMPLING)
                  {
                    #if (ADJUST_SA==2)
                      /* No trouble extending branches: increase sampling area to focus on exploration */
                      //scale=1.0;
                      scale=scale*(1+MOV_AVG_UP);
                      if (scale>1.0) scale=1.0;
                    #else
                      /* an alternative is to adjust the sampling radius locally (ADJUST_SA==1) */
                      if (!expand2Goal)
                        IncreaseChartSamplingRadius(GetAtlasChart(c_rand,&(ar->atlas)));
                    #endif
                }
              #endif

              /* Try to connect to the other tree */

              /* Get the node of the just added branch closer to the other tree (and the node of
                 the other tree closer to this just added node). */
              GetRRTNNInBranch(t2,i_near1,lastSample,&target,&i_near2,&(ar->rrt));

              lastSample=target;
              memcpy(q_rand,ar->si[target]->s,ar->m*sizeof(double));

              memcpy(q_goal,q_rand,ar->m*sizeof(double)); /* copy of Qrand */

              /* Try to connect the '2nd' tree to the last node added to the '1st' tree */
              #if (GET_ATLASRRT_STATISTICS)
                NewAtlasRRTTreeConnection(arst);
              #endif
              #if (NEW_BRANCH_EXTENSION)
                 dst=New_AddBranchToAtlasRRT(pr,it,&configConnect,i_near2,q_rand,q_goal,
                                          (void*)arst,&statusConnect,NULL,NULL,ar);
                 
                lastSample2=statusConnect.lastID;
                done=((!EXPLORATION_RRT)&&(statusConnect.reachedGoal));
              #else
                dst=AddBranchToAtlasRRT(pr,it,ADD_ALL,FALSE,TRUE,TRUE,FALSE,
                                        0.0,i_near2,q_rand,q_goal,
                                        (void*)arst,&lastSample2,&reachedQRand,&reachedGoal,NULL,NULL,
                                        NULL,NULL,ar);
              
                done=((!EXPLORATION_RRT)&&(reachedGoal));
              #endif
                
              #if (GET_ATLASRRT_STATISTICS)
                if (dst>0)
                  NewAtlasRRTNoEmptyTreeConnection(arst);
              #endif
            }
          #if (ADJUST_SA)
          else
            {
              if (samplingMode==TANGENT_SAMPLING)
                {
                  #if (ADJUST_SA==2)
                    /* Troubles extending branches: reduce the sampling areas to focus on refinement */
                    scale=scale*(1-MOV_AVG_DOWN);
                    if (scale<0.05) scale=0.05;
                  #else
                    /* an alternative is to adjust the sampling radius locally */
                    if (!expand2Goal)
                      DecreaseChartSamplingRadius(GetAtlasChart(c_rand,&(ar->atlas)));
                  #endif
                }
            }
          #endif

          /* Swap the trees */
          if ((it%2)==0)
            {
              t1=START2GOAL;
              t2=GOAL2START;
            }
          else
            {
              t1=GOAL2START;
              t2=START2GOAL;
            }
         }

      it++;
      *time=GetElapsedTime(&stime);
    }
  DeleteStatistics(&stime);

  #if (GET_ATLASRRT_STATISTICS)
    if (str==NULL)
      PrintAtlasRRTStatistics(ar,arst);
    else
      AccumulateAtlasRRTStatistics(arst,str);
    DeleteAtlasRRTStatistics(arst);
    free(arst);
  #endif

  fprintf(stderr,"Final number of samples: %u\n",ar->ns);
  
  /* Reconstruct the path (if any) */
  if (EXPLORATION_RRT)
    pathFound=FALSE;
  else
    pathFound=PathStart2GoalInRRT(pr,finalPoint,lastSample,lastSample2,
                                  pl,NULL,ns,path,&(ar->rrt));

  free(t_rand);
  free(q_rand);
  if (ar->birrt)
    free(q_goal);
  else
    {
      if (finalPoint!=NULL)
        free(finalPoint);
    }

  return(pathFound);
}

boolean AtlasTRRT(Tparameters *pr,double *pg,double *time,
                  double *pl,double *pc,unsigned int *ns,double ***path,
                  double (*costF)(Tparameters*,boolean,double*,void*),
                  void *costData,TAtlasRRTStatistics *str,Tatlasrrt *ar)
{
  boolean done;
  unsigned int it;
  double er,epsilon,h;
  unsigned int samplingMode;
  double *pWithDummies,*finalPoint,*q_rand,*t_rand;
  unsigned int c_rand;
  boolean expand2Goal,pathFound,exploration;
  unsigned int i_near1;
  TAtlasRRTStatistics *arst;
  Tstatistics stime; /* used just to measure execution time */
  boolean collision=FALSE;
  unsigned int lastNN2Goal;
  unsigned int db;
  double maxTime;
  unsigned int maxNodes;
  double scale; /* Global scale factor for sampling areas */
  boolean validSample;
  #if (NEW_BRANCH_EXTENSION)
    TAtlasRRTBranchConfig configExtend;
    TAtlasRRTBranchStatus statusExtend;

    InitBranchConfig(EXTEND_RRT,&configExtend);
    configExtend.costF=costF;
    configExtend.costData=costData;
  #else
    boolean reachedQRand,reachedGoal;
    unsigned int lastSample;
  #endif

  if (ar->birrt)
    Error("AtlasTRRT operates on one-directional RRTs");
  
  db=(unsigned int)GetParameter(CT_DETECT_BIFURCATIONS,pr);
  if (db>0)
    Error("AtlasRRT can not deal with bifurcations (yet)");
  
  samplingMode=(unsigned int)GetParameter(CT_SAMPLING,pr);

  scale=1.0; /* not used yet */
  
  /* Initially the AtlasRRT must only include one sample and one chart or
     two sample and two chart for bidirectional trees */
  if (ar->ns>1)
    Error("AtlasRRTstar must be used to a just initilized RRT");

  #if (GET_ATLASRRT_STATISTICS)
  {
    unsigned int i;

    NEW(arst,1,TAtlasRRTStatistics);
    InitAtlasRRTStatistics(arst);
    for(i=0;i<ar->nc;i++)
      NewAtlasRRTChart(arst);
    for(i=0;i<ar->ns;i++)
      NewAtlasRRTSample(arst);
  } 
  #else 
    arst=NULL;
  #endif

  /* Init the RRT structure */
  epsilon=GetParameter(CT_EPSILON,pr);
  maxTime=GetParameter(CT_MAX_PLANNING_TIME,pr);
  maxNodes=(unsigned int)GetParameter(CT_MAX_NODES_RRT,pr);
  
  if (EXPLORATION_RRT)
    finalPoint=NULL;
  else
    {
      /* One-directional RRTs, the goal is given as a parameter
         and we must ensure that it is valid */
      CS_WD_REGENERATE_SOLUTION_POINT(pr,pg,&pWithDummies,ar->w);
      CS_WD_GENERATE_SIMPLIFIED_POINT(pr,pWithDummies,&finalPoint,ar->w);
  
      if ((!PointInBoxTopology(NULL,TRUE,ar->m,finalPoint,epsilon,ar->tp,&(ar->ambient)))||
          (!CS_WD_SIMP_INEQUALITIES_HOLD(pr,finalPoint,ar->w)))
        Error("The goal point for the RRT is not in domain");

      if (ar->m==ar->k)
        er=0;
      else
        er=CS_WD_ERROR_IN_SIMP_EQUATIONS(pr,finalPoint,ar->w);
      if (er>epsilon)
        Error("The target point is not on the manifold");
  
      collision=CS_WD_ORIGINAL_IN_COLLISION(pr,pWithDummies,NULL,ar->w);;
      if (collision)
        Error("Target point for the RRT is in collision");
      free(pWithDummies);
    }

  /*init the cost of the first node*/
  SetRRTNodeCost(0,0,costF(pr,TRUE,GetRRTNode(0,&(ar->rrt)),costData),&(ar->rrt));

  NEW(q_rand,ar->m,double);
  NEW(t_rand,ar->k,double); 

  done=FALSE;
  lastNN2Goal=NO_UINT;
  it=1;
  InitStatistics(0,0,&stime);
  *time=0.0;

   while((!done)&&(*time<maxTime)&&(ar->ns<maxNodes))
    {
      #if (!ATLASRRT_VERBOSE)
        if ((it%1000)==0)
      #endif
          fprintf(stderr,"Iteration: %u (s:%u c:%u t:%u tm:%g tmp:%g)\n",it,ar->ns,ar->nc,
                  START2GOAL,*time,GetTRRTTemperature(&(ar->rrt)));

      
      /*******************************************************************/
      /* Sample */
      exploration=FALSE;
      do {
        /* generate the random sample */
        expand2Goal=AtlasRRTSample(pr,samplingMode,it,START2GOAL,finalPoint,scale,
                                   &exploration,&c_rand,t_rand,q_rand,arst,ar);

        /* validate the random sample */
        validSample=AtlasRRTValidateSample(pr,q_rand,START2GOAL,expand2Goal,lastNN2Goal,NULL,INF,
                                           &h,&i_near1,arst,ar);

        /* There is no point in expanding the same node towards the goal twice */
      } while (!validSample);

      if (expand2Goal)
        lastNN2Goal=i_near1;

      /*******************************************************************/
      /* Extend the RRT from q_near toward q_rand */
      #if (NEW_BRANCH_EXTENSION)
        configExtend.onManifold=expand2Goal;
        configExtend.explorationSample=exploration;
        New_AddBranchToAtlasRRT(pr,it,&configExtend,i_near1,q_rand,finalPoint,
                                (void*)arst,&statusExtend,NULL,NULL,ar);
        done=((!EXPLORATION_RRT)&&(statusExtend.reachedGoal));
      #else
        AddBranchToAtlasRRT(pr,it,ADD_ALL,TRUE,TRUE,expand2Goal,exploration,
                            0.0,i_near1,q_rand,finalPoint,
                            (void*)arst,&lastSample,&reachedQRand,&reachedGoal,
                            NULL,NULL,costF,costData,ar);
  
        done=((!EXPLORATION_RRT)&&(reachedGoal));
      #endif
      
      it++;
      *time=GetElapsedTime(&stime);
    }
  DeleteStatistics(&stime);

  #if (GET_ATLASRRT_STATISTICS)
    if (str==NULL)
      PrintAtlasRRTStatistics(ar,arst);
    else
      AccumulateAtlasRRTStatistics(arst,str);    
    DeleteAtlasRRTStatistics(arst);
    free(arst);
  #endif

  fprintf(stderr,"Final number of samples: %u\n",ar->ns);
  
  /* Reconstruct the path (if any) */
  #if (NEW_BRANCH_EXTENSION)
    pathFound=PathStart2GoalInRRT(pr,finalPoint,statusExtend.lastID,
                                  NO_UINT,pl,pc,ns,path,&(ar->rrt));
  #else
    pathFound=PathStart2GoalInRRT(pr,finalPoint,lastSample,
                                  NO_UINT,pl,pc,ns,path,&(ar->rrt));
  #endif

  free(t_rand);
  free(q_rand);
  free(finalPoint);

  return(pathFound);
}

unsigned int AddStepToAtlasRRTstar(Tparameters* pr,
                                   unsigned int it,boolean expand2Goal,
                                   unsigned int i_near,double *q_rand,
                                   unsigned int *id_goal,double *goal,
                                   TAtlasRRTStatistics *arst,
                                   Tatlasrrt *ar)
{
  double d,epsilon;
  unsigned int id_new;
  #if (NEW_BRANCH_EXTENSION)
    TAtlasRRTBranchConfig configExtend;
    TAtlasRRTBranchStatus statusExtend;

    InitBranchConfig(CONNECT_SAMPLES,&configExtend);
  #else
    boolean reachedQRand,reachedGoal;
  #endif

  epsilon=GetParameter(CT_EPSILON,pr);

  /* Try to advance as much as possible in the direction of q_rand. Only
     the last node is added to the tree. */
  #if (NEW_BRANCH_EXTENSION)
    configExtend.addMode=(expand2Goal?ADD_LAST_NO_REP:ADD_LAST);
    configExtend.onManifold=expand2Goal;
    d=New_AddBranchToAtlasRRT(pr,it,&configExtend,i_near,q_rand,goal,
                              (void*)arst,&statusExtend,NULL,NULL,ar);
    id_new=statusExtend.lastID;
  #else
    d=AddBranchToAtlasRRT(pr,it,(expand2Goal?ADD_LAST_NO_REP:ADD_LAST),TRUE,TRUE,expand2Goal,FALSE,
                          0.0,i_near,q_rand,goal,
                          (void*)arst,&id_new,&reachedQRand,&reachedGoal,NULL,NULL,NULL,NULL,ar);
  #endif
  
  /* if we actually added a node */
  if (id_new!=i_near)
    {     
      /* Store the cost of the new node. The cost to the parent is already correct (we
         need it if 'graph's is set to TRUE) but the total cost is not set */
      SetRRTNodeCost(id_new,d,GetRRTNodeCost(i_near,&(ar->rrt))+d,&(ar->rrt));

      #if (ATLASRRT_VERBOSE)
        fprintf(stderr,"  {%u->%u}[%g]\n",id_new,GetRRTParent(id_new,&(ar->rrt)),d);
      #endif

      /* if the new point is actually the goal record it */
      #if (NEW_BRANCH_EXTENSION)
      if ((goal!=NULL)&&(*id_goal==NO_UINT)&&
          ((statusExtend.reachedGoal)||((expand2Goal)&&(statusExtend.reachedQrand))))
      #else
      if ((goal!=NULL)&&(*id_goal==NO_UINT)&&
          ((reachedGoal)||((expand2Goal)&&(reachedQRand))))
      #endif
        {
          #if (NEW_BRANCH_EXTENSION)
          if (statusExtend.distGoal<epsilon)
          #else
          double dg;
          
          dg=DistanceTopology(ar->m,ar->tp,ar->si[id_new]->s,goal);
          if (dg<epsilon)
          #endif
            /* The new node is actually the goal */
            *id_goal=id_new; 
          else
            {
              /* If the branch stopped very close to the goal, we add a small
                 branch from the end of the branch to the goal and the goal itself. */
              #if (NEW_BRANCH_EXTENSION)
                configExtend.addMode=ADD_LAST;
                configExtend.onManifold=TRUE;
                d=New_AddBranchToAtlasRRT(pr,it,&configExtend,i_near,goal,NULL,
                                          (void*)arst,&statusExtend,NULL,NULL,ar);
                *id_goal=statusExtend.lastID;
                
                if (!statusExtend.reachedQrand)
              #else
                d=AddBranchToAtlasRRT(pr,it,ADD_LAST,TRUE,TRUE,TRUE,FALSE,
                                      0.0,id_new,goal,NULL,
                                      (void*)arst,id_goal,&reachedQRand,&reachedGoal,NULL,NULL,
                                      NULL,NULL,ar);
                if (!reachedQRand)
              #endif
                  Error("Could not reach goal from near-goal");
                  
              SetRRTNodeCost(*id_goal,d,GetRRTNodeCost(*id_goal,&(ar->rrt))+d,&(ar->rrt));

              #if (ATLASRRT_VERBOSE)
                fprintf(stderr,"     {%u->%u}[%g] **\n",*id_goal,GetRRTParent(*id_goal,&(ar->rrt)),d);
              #endif
            }
        }
    }
  return(id_new);
}

void WireAtlasRRTstar(Tparameters *pr,
                      unsigned int id_new,unsigned int i_near,double gamma,
                      unsigned int nn,unsigned int *n,double **c,
                      double h,Theap *q,unsigned int *t,
                      TAtlasRRTStatistics *arst,Tatlasrrt *ar)
{
  unsigned int i;
  double ic,cn,epsilon,c_new;
  double *cost;
  boolean *reachedQRand;
  double *q_new;
  TDoublePair pair;
  unsigned int rrtMode;
  boolean graph;
  #if (NEW_BRANCH_EXTENSION)
    TAtlasRRTBranchConfig configExtend;
    TAtlasRRTBranchStatus *statusExtend;

    InitBranchConfig(CONNECT_SAMPLES,&configExtend);
    NEW(statusExtend,nn,TAtlasRRTBranchStatus);
  #endif
    
  epsilon=GetParameter(CT_EPSILON,pr);

  /*******************************************************************/
  /* Wire: Search for the lowest cost path to the new node */
  #if (ATLASRRT_VERBOSE)
    fprintf(stderr,"  Wire %u [nn:%u g:%g]\n",id_new,nn,gamma);
  #endif
  rrtMode=GetRRTMode(&(ar->rrt));
  graph=IsRRTGraph(&(ar->rrt));
  #if (RRTSTAR_SYMMETRIC_COST)
    if (!graph)
      NEW(*c,nn,double);
  #endif
  ic=GetRRTNodeCostFromParent(id_new,&(ar->rrt)); /*cost to i_near*/
  q_new=ar->si[id_new]->s;
  *t=0; /* initially no tree in the set of neighbours */

  NEW(cost,nn,double);
  NEW(reachedQRand,nn,boolean);

  #pragma omp parallel for private(i) schedule(dynamic) if (ar->parallel)
  for(i=0;i<nn;i++)
    {
      if (n[i]==id_new)
        reachedQRand[i]=FALSE;
      else
        {
          if (n[i]==i_near)
            {
              cost[i]=ic;
              reachedQRand[i]=TRUE;
            }
          else
            {
              if (ar->parallel)
                {
                  boolean collision;

                  /* When executing in parallel we:
                        - Do not collect statistics about the connection process.
                        - Do not add new charts to the atlas. We only generate temporary charts. */
                  cost[i]=ConnectSamplesChart(pr,ar->tp,NULL,&(ar->ambient),
                                              ar->m,ar->m-ar->k,ar->si[n[i]]->s,q_new,gamma,
                                              TRUE,&(ar->J),&(reachedQRand[i]),&collision,NULL,NULL,NULL,ar->w);;
                }
              else
                {
                  #if (NEW_BRANCH_EXTENSION)
                    configExtend.maxLength=gamma;
                    cost[i]=New_AddBranchToAtlasRRT(pr,NO_UINT,&configExtend,n[i],q_new,NULL,
                                                    (void*)arst,&(statusExtend[i]),NULL,NULL,ar);
                    reachedQRand[i]=statusExtend[i].reachedQrand;
                  #else
                    boolean reachedGoal;
                    unsigned int lastID;

                    cost[i]=AddBranchToAtlasRRT(pr,NO_UINT,ADD_NONE,TRUE,TRUE,TRUE,FALSE,
                                                gamma,n[i],q_new,NULL,
                                                (void*)arst,&lastID,&(reachedQRand[i]),&reachedGoal,NULL,NULL,
                                                NULL,NULL,ar);
                  #endif
                }
            }
        }
    }

  for(i=0;i<nn;i++)
    {
      if (n[i]!=id_new)
        {
          #if (GET_ATLASRRT_STATISTICS)
            NewAtlasRRTBranch(arst);
          #endif
          (*t)|=GetRRTNodeTree(n[i],&(ar->rrt)); /* take note of the tree of this neighbour (even if not reached)*/
        }

      if (reachedQRand[i])
        {
          cn=cost[i];

          #if (GET_ATLASRRT_STATISTICS)
            NewAtlasRRTNoEmptyBranch(arst);
          #endif

          /* update the cost of the reached neighbour */
          #if (!RRTSTAR_UPDATE_COSTS)
          if (rrtMode==TWO_TREES_WITH_SWAP)
          #endif
            UpdateCostAndTree(n[i],&(ar->rrt));

          /* There is an edge connecting id_new and n[i] */
          if (graph)
            AddEdgeToRRT(id_new,n[i],cn,&(ar->rrt));

          c_new=GetRRTNodeCost(n[i],&(ar->rrt))+cn;
          if (c_new<(GetRRTNodeCost(id_new,&(ar->rrt))-epsilon))
            {
              #if (ATLASRRT_VERBOSE)
                fprintf(stderr,"     {%u->%u}[%g %g] ->",id_new,GetRRTParent(id_new,&(ar->rrt)),
                        GetRRTNodeCostFromParent(id_new,&(ar->rrt)),
                        GetRRTNodeCost(id_new,&(ar->rrt)));
              #endif

              SetRRTCostAndParent(id_new,n[i],cn,c_new,&(ar->rrt));

              #if (ATLASRRT_VERBOSE)
                fprintf(stderr,"{%u->%u}[%g %g]\n",id_new,GetRRTParent(id_new,&(ar->rrt)),
                        GetRRTNodeCostFromParent(id_new,&(ar->rrt)),
                        GetRRTNodeCost(id_new,&(ar->rrt)));
              #endif
            }
        }
      else
        cn=INF;

      #if (RRTSTAR_SYMMETRIC_COST)
        if (!graph)
          (*c)[i]=cn;
      #endif
    }

  free(cost);
  free(reachedQRand);
  #if (NEW_BRANCH_EXTENSION)
    free(statusExtend);
  #endif
    
  if (graph)
    {
      if (!q)
        Error("RRT must be in graph mode to use a heap");
      c_new=GetRRTNodeCost(id_new,&(ar->rrt));
      NewDoublePair(c_new+h,c_new,&pair);
      AddElement2Heap(id_new,(void *)&pair,q);
    }           
}

void ReWireAtlasRRTstar(Tparameters *pr,
                        unsigned int id_new,double gamma,
                        unsigned int nn,unsigned int *n,double *c,
                        Tvector *steps,double *l,
                        TAtlasRRTStatistics *arst,Tatlasrrt *ar)
{

  unsigned int i,parent;
  double cn,epsilon,c_new;
  unsigned int rrtMode;
  boolean reachedQRand;
  #if (!RRTSTAR_SYMMETRIC_COST)
    #if (NEW_BRANCH_EXTENSION)
      TAtlasRRTBranchConfig configExtend;
      TAtlasRRTBranchStatus statusExtend;

      InitBranchConfig(CONNECT_SAMPLES,&configExtend);
    #else
      unsigned int lastID;
      boolean reachedGoal;
    #endif
  #endif
    
  epsilon=GetParameter(CT_EPSILON,pr);

  #if (ATLASRRT_VERBOSE)
    fprintf(stderr,"  Rewire\n");
  #endif
  rrtMode=GetRRTMode(&(ar->rrt));
  /* best parent for the new node */
  parent=GetRRTParent(id_new,&(ar->rrt));
  for(i=0;i<nn;i++)
    {
      if ((n[i]==id_new)||(n[i]==parent))
        reachedQRand=FALSE;
      else
        {  
          #if (RRTSTAR_SYMMETRIC_COST)
            cn=c[i];
            reachedQRand=(cn<INF);
          #else
            /* cost to go from id_new to n[i] */
            /* the branch is virtual-> nothing is added to the RRT */
            #if (NEW_BRANCH_EXTENSION)
              configExtend.maxLength=gamma;
              cn=New_AddBranchToAtlasRRT(pr,NO_UINT,&configExtend,id_new,ar->si[n[i]]->s,NULL,
                                         (void*)arst,&statusExtend,NULL,NULL,ar);
              reachedQRand=statusExtend.reachedQrand;
            #else
              cn=AddBranchToAtlasRRT(pr,NO_UINT,ADD_NONE,TRUE,TRUE,TRUE,FALSE,
                                     gamma,id_new,ar->si[n[i]]->s,NULL,
                                     (void*)arst,&lastID,&reachedQRand,&reachedGoal,NULL,NULL,NULL,NULL,ar);
            #endif
          #endif
        }

      if (reachedQRand)
        {
          /* Ensure that any previous-rewire is propagated to the currently tested node */
          if (rrtMode==TWO_TREES_WITH_SWAP)
            UpdateCostAndTree(n[i],&(ar->rrt));

          c_new=GetRRTNodeCost(id_new,&(ar->rrt))+cn;
          if (c_new<(GetRRTNodeCost(n[i],&(ar->rrt))-epsilon))
            {
              #if (ATLASRRT_VERBOSE)
                fprintf(stderr,"     {%u->%u}[%g] ->",n[i],GetRRTParent(n[i],&(ar->rrt)),
                        GetRRTNodeCostFromParent(n[i],&(ar->rrt)));
              #endif    

              SetRRTCostAndParent(n[i],id_new,cn,c_new,&(ar->rrt));

              #if (ATLASRRT_VERBOSE)
                fprintf(stderr,"{%u->%u}[%g]\n",n[i],GetRRTParent(n[i],&(ar->rrt)),
                        GetRRTNodeCostFromParent(n[i],&(ar->rrt)));
              #endif
            }
          else
            {
              /* if the node is connected to a sample in a different tree, update the optimal path */
              if ((rrtMode==TWO_TREES_WITH_SWAP)&&(steps!=NULL)&&(l!=NULL)&&
                  (GetRRTNodeTree(n[i],&(ar->rrt))!=GetRRTNodeTree(id_new,&(ar->rrt))))
                {
                  c_new+=GetRRTNodeCost(n[i],&(ar->rrt)); /* cost of start to goal via this connection
                                                             between trees.*/
                  if (c_new<*l)
                    *l=ChangeBiRRTSteps(n[i],id_new,cn,steps,&(ar->rrt));
                }
            }
        }
    }
}

void AtlasRRTstarCloseIteration(unsigned int it,unsigned int id_goal,
                                double time,double gamma,
                                double *times,double *costs,
                                Tatlasrrt *ar)
{
  /* Print information about the quality of the path so far */

  if ((ATLASRRT_VERBOSE)||((it%1000)==0))
    {
      if (id_goal!=NO_UINT)
        {
          fprintf(stderr,"Iteration: %u (s:%u nc:%u t:%g g:%g c:%g [%g] [%u])\n",it,ar->ns,ar->nc,
                  time,gamma,GetRRTNodeCost(id_goal,&(ar->rrt)),CostToRoot(id_goal,&(ar->rrt)),id_goal);
        }
      else
        fprintf(stderr,"Iteration: %u (s:%u t:%g)\n",it,ar->ns,time);
      fflush(stdout);
    }

  /*******************************************************************/
  /* Store information about the quality of the path so far and 
     conclude the iteration */

  if (times!=NULL)
    times[it]=time;

  if (costs!=NULL)
    {
      if (id_goal==NO_UINT)
        costs[it]=-1.0;
      else
        costs[it]=GetRRTNodeCost(id_goal,&(ar->rrt)); //CostToRoot(id_goal,&(ar->rrt));
    }
}

void AtlasBiRRTstarCloseIteration(unsigned int it,double l,
                                  double time,double gamma,
                                  double *times,double *costs,
                                  Tatlasrrt *ar)
{
  if ((ATLASRRT_VERBOSE)||((it%1000)==0))
    {
      if (l<INF)
        {
          fprintf(stderr,"Iteration: %u (s:%u nc: %u t:%g g:%g c:%g)\n",it,ar->ns,ar->nc,
                  time,gamma,l);
        }
      else
        fprintf(stderr,"Iteration: %u (s:%u t:%g)\n",it,ar->ns,time);
      fflush(stdout);
    }

  if (times!=NULL)
    times[it]=time;

  if (costs!=NULL)
    {
      if (l<INF)
        costs[it]=l;
      else
        costs[it]=-1.0;
    }
}


boolean AtlasRRTstar(Tparameters *pr,double *pg,
                     unsigned int *it,double *times,double *costs,
                     double *planningTime,double *pl,unsigned int *ns,double ***path,
                     TAtlasRRTStatistics *str,Tatlasrrt *ar)
{
  if (EXPLORATION_RRT)
    Error("AtlasRRTstart is not designed for exploration RRTs");
    
  if (ar->birrt)
    return(AtlasBiRRTstar(pr,pg,it,times,costs,planningTime,pl,ns,path,str,ar));
  else
    {
      double *pWithDummies,*pgs,*t_rand,*q_rand,*q_new;
      unsigned int i,id_new,id_goal,c_rand,samplingMode;
      unsigned int i_near;
      double time,epsilon,delta,er,gamma,ct_gamma;
      Tstatistics st; /* used just to measure execution time */
      boolean collision,expand2Goal,done,optimal,validSample,exploration;
      /* cost of paths to neighbouring nodes */
      double *c;
      /* neighbours for each new node */
      unsigned int nn;
      unsigned int *n;
      TAtlasRRTStatistics *arst;
      double maxTime;
      unsigned int maxIterations;
      double h,l;
      Theap q;
      boolean graph;
      unsigned int vt; /* trees in  set of neighbours, only relevant in bi-RRTs */
      double numSamples;
      double scale; /* Global adjustment of sampling areas */

      if (ar->ns>1)
        Error("AtlasRRTstart must be used to a just initilized RRT");

      #if (GET_ATLASRRT_STATISTICS)
      {
        unsigned int i;

        NEW(arst,1,TAtlasRRTStatistics);
        InitAtlasRRTStatistics(arst);
        for(i=0;i<ar->nc;i++)
          NewAtlasRRTChart(arst);
        for(i=0;i<ar->ns;i++)
          NewAtlasRRTSample(arst);
      } 
      #else 
        arst=NULL;
      #endif

      scale=1.0; /* not used yet */
        
      /* Init the AtlasRRT structure */
      epsilon=GetParameter(CT_EPSILON,pr);
      delta=GetParameter(CT_DELTA,pr);
      ct_gamma=GetParameter(CT_GAMMA,pr);
      maxTime=GetParameter(CT_MAX_PLANNING_TIME,pr);
      maxIterations=(unsigned int)GetParameter(CT_MAX_PLANNING_ITERATIONS,pr);
      samplingMode=(unsigned int)GetParameter(CT_SAMPLING,pr);

      graph=IsRRTGraph(&(ar->rrt));

      CS_WD_REGENERATE_SOLUTION_POINT(pr,pg,&pWithDummies,ar->w);
      CS_WD_GENERATE_SIMPLIFIED_POINT(pr,pWithDummies,&pgs,ar->w);

      if ((!PointInBoxTopology(NULL,TRUE,ar->m,pgs,epsilon,ar->tp,&(ar->ambient)))||
          (!CS_WD_SIMP_INEQUALITIES_HOLD(pr,pgs,ar->w)))
        Error("The targed point for the AtlasRRTstar is not in domain");

      if (ar->m==ar->k)
        er=0;
      else
        er=CS_WD_ERROR_IN_SIMP_EQUATIONS(pr,pgs,ar->w);
      if (er>epsilon)
        Error("The target point for the AtlasRRTstar is not on the manifold");

      collision=CS_WD_ORIGINAL_IN_COLLISION(pr,pWithDummies,NULL,ar->w);
      if (collision)
        Error("The target point for the AtlasRRTstar is in collision");
      free(pWithDummies);

      /* Room for the randomly generated samples (and its correspoinding point
         on the manifold) */
      NEW(q_rand,ar->m,double);
      NEW(t_rand,ar->k,double);

      /* Reset the output statistics (just in case maxIterations 
         is not reached) */
      for(i=0;i<maxIterations;i++)
        {
          times[i]=0;
          costs[i]=-1;
        }

      if (graph)
        InitHeap(sizeof(TDoublePair),CopyDoublePair,NULL,
                 LessThanDoublePair,NULL,TRUE,ar->ms,&q);

      gamma=ct_gamma;
      optimal=(ct_gamma>0);

      id_goal=NO_UINT;
      *it=0;

      InitStatistics((ar->parallel?2:1),0,&st); /* 2 just to indicated multi-core process */
      time=0.0;
      done=FALSE;
      l=INF; /* cost to goal (INF while goal not found) */
      c=NULL; /* default -> no cache of costs to neighbours */
  
      while ((!done)&&(time<maxTime)&&(*it<maxIterations))
        {
          #if (HEURISTIC_RRT_STAR&(1))
            /* keep the cost to goal up to date so that the heuristic
               is more effective*/
            if (id_goal!=NO_UINT)
              UpdateCostAndTree(id_goal,&(ar->rrt));
          #endif

          /*******************************************************************/
          /* best estimation of the cost to goal (up to now) */
          if (id_goal==NO_UINT)
            l=INF;
          else
            l=GetRRTNodeCost(id_goal,&(ar->rrt));

          /*******************************************************************/
          /* Generate a random sample */
          do {
            /* Generate the sample */
            expand2Goal=AtlasRRTSample(pr,samplingMode,*it,START2GOAL,(id_goal==NO_UINT?pgs:NULL),
                                       scale,&exploration,&c_rand,t_rand,q_rand,arst,ar);

            
            /* validate the random sample */
            validSample=AtlasRRTValidateSample(pr,q_rand,START2GOAL,expand2Goal,NO_UINT,
                                               pgs,l,&h,&i_near,arst,ar);
          } while (!validSample);

          /*******************************************************************/
          /* Extend the AtlasRRT from q_near to q_rand */
          /* Note that here we use a connect strategy but we only add the last sample */
          id_new=AddStepToAtlasRRTstar(pr,*it,expand2Goal,i_near,q_rand,
                                       &id_goal,pgs,(void*)arst,ar);

          /*******************************************************************/
          if (!optimal)
            {
              if (id_goal!=NO_UINT)
                done=TRUE;
            }
          else
            {
              /* If we actually added something */
              if ((id_new!=i_near)&&((((HEURISTIC_RRT_STAR)&(2))==0)||(id_goal!=NO_UINT)))
                {         
                  /* q_new is the last samples added to the tree */
                  q_new=ar->si[id_new]->s;
                  h=DistanceTopology(ar->m,ar->tp,q_new,pgs);
          
                  /* Wire and re-wire the tree, only we we are interested in an optimal path
                     If heristic #4 is active optimization is delayed until the goal is found */

                  /*******************************************************************/
                  /* Search for the set of potential neighbours */
                  numSamples=(double)(ar->ns<3?3:ar->ns);
                  gamma=ct_gamma*pow(log(numSamples)/numSamples,1.0/(double)ar->k);
                  gamma=(gamma<delta?delta:gamma);
                  
                  GetRRTNNInBall(START2GOAL,q_new,gamma,&nn,&n,&(ar->rrt));
                  
                  /*******************************************************************/
                  /* Wire: Search for the lowest cost path to the new node */
                  WireAtlasRRTstar(pr,id_new,i_near,gamma,nn,n,&c,h,(graph?&q:NULL),&vt,arst,ar);
                  
                  /*******************************************************************/
                  /* Rewire the tree around the new point */
                  if (graph)
                    RecursiveReWireRRTstar(pr,&q,pgs,NULL,&l,&(ar->rrt));
                  else
                    ReWireAtlasRRTstar(pr,id_new,gamma,nn,n,c,NULL,&l,arst,ar);
                  
                  /*******************************************************************/
                  /* release the data for this iteration */     
                  if (c!=NULL)
                    free(c);
                  free(n);
                }
              #if (ATLASRRT_VERBOSE)
              else
                {
                  if (id_new==i_near)
                    fprintf(stderr,"  Blocked [%u]\n",i_near);
                }
              #endif
            }

          /*******************************************************************/
          /* Print a summary about this iteration */
          time=GetElapsedTime(&st);

          AtlasRRTstarCloseIteration(*it,id_goal,time,gamma,times,costs,ar);

          (*it)++;
        }

      if (graph)
        DeleteHeap(&q);

      *planningTime=time;

      DeleteStatistics(&st);

      fprintf(stderr,"Final number of samples: %u\n",ar->ns);

      /* Reconstruct the path (if any) */
      if (id_goal!=NO_UINT)
        ReconstructAtlasRRTPath(pr,id_goal,pl,ns,path,ar);

      /* Release memory */  
      free(t_rand);
      free(q_rand);
      free(pgs);

      #if (GET_ATLASRRT_STATISTICS)
        if (str==NULL)
          PrintAtlasRRTStatistics(ar,arst);
        else
          AccumulateAtlasRRTStatistics(arst,str);  
        DeleteAtlasRRTStatistics(arst);
        free(arst);
      #endif

      return(id_goal!=NO_UINT);
    }
}

boolean AtlasBiRRTstar(Tparameters *pr,double *pg,
                       unsigned int *it,double *times,double *costs,
                       double *planningTime,double *pl,unsigned int *ns,double ***path,
                       TAtlasRRTStatistics *str,Tatlasrrt *ar)
{
  
  if (EXPLORATION_RRT)
    Error("AtlasBiRRTstart is not designed for exploration RRTs");

  if (!ar->birrt)
    return(AtlasRRTstar(pr,pg,it,times,costs,planningTime,pl,ns,path,str,ar));
  else
    {
      double *q_rand,*t_rand,*g;
      unsigned int c_rand;
      unsigned int i_near;
      double delta;
      Tstatistics st; /* used just to measure execution time */
      double time;
      /* if the cost function is symmetrical, we cache
         the evaluation of the cost to neighbour. */
      double *c;
      /* neighbours for each new node */
      unsigned int nn;
      unsigned int *n;
      /* distance q_rand,tree */
      double gamma,ct_gamma;
      unsigned int i,id_new,samplingMode;
      double *q_new;
      boolean done,optimal,validSample,exploration;
      double l;           /* length of the shortest path so far */
      double h;
      Theap q;
      Tvector steps;
      TAtlasRRTStatistics *arst;
      double maxTime;
      unsigned int maxIterations;
      boolean graph;
      unsigned int vt;
      double l1,c12;
      double numSamples;
      double scale; /* Global adjust of sampling areas */
      #if (NEW_BRANCH_EXTENSION)
        TAtlasRRTBranchConfig configExtend;
        TAtlasRRTBranchStatus statusExtend;

        InitBranchConfig(CONNECT_SAMPLES,&configExtend);
      #else
        boolean reachedQRand,reachedGoal;
        unsigned int lastID;
      #endif
      
      if (ar->ns>2)
        Error("AtlasBiRRTstart must be used to a just initilized RRT");

      #if (GET_ATLASRRT_STATISTICS)
      {
        unsigned int i;

        NEW(arst,1,TAtlasRRTStatistics);
        InitAtlasRRTStatistics(arst);
        for(i=0;i<ar->nc;i++)
          NewAtlasRRTChart(arst);
        for(i=0;i<ar->ns;i++)
          NewAtlasRRTSample(arst);
      } 
      #else 
        arst=NULL;
      #endif

      scale=1.0; /* not used yet */

      /* Init the RRT structure */
      delta=GetParameter(CT_DELTA,pr);
      ct_gamma=GetParameter(CT_GAMMA,pr);
      maxTime=GetParameter(CT_MAX_PLANNING_TIME,pr);    
      maxIterations=(unsigned int)GetParameter(CT_MAX_PLANNING_ITERATIONS,pr);
      samplingMode=(unsigned int)GetParameter(CT_SAMPLING,pr);
  
      graph=IsRRTGraph(&(ar->rrt));

      /* Room for the randomly generated samples (and its correspoinding point
         on the manifold) */
      NEW(q_rand,ar->m,double);
      NEW(t_rand,ar->k,double);

      /* Reset the output statistics (just in case maxIterations 
         is not reached) */  
      for(i=0;i<maxIterations;i++)
        {
          times[i]=0;
          costs[i]=-1;
        }

      if (graph)
        InitHeap(sizeof(TDoublePair),CopyDoublePair,NULL,
                 LessThanDoublePair,NULL,TRUE,ar->ms,&q);

      gamma=ct_gamma;
      optimal=(ct_gamma>0);

      *it=0;

      InitStatistics((ar->parallel?2:1),0,&st); /* if parallel 2 indicates multi-core */
      time=0.0;
      done=FALSE;
  
      InitVector(sizeof(TRRTStep),CopyRRTStep,NULL,100,&steps);
      l=INF;
      c=NULL;

      while ((!done)&&(time<maxTime)&&(*it<maxIterations))
        {
          /* keep the optimal path up to date (if any) */
          if (l<INF)
            l=UpdateBiRRTSteps(&steps,&(ar->rrt));
      
          /*******************************************************************/
          do {
            /* Geerate a random sample */           
            AtlasRRTSample(pr,samplingMode,*it,BOTHTREES,NULL,scale,
                           &exploration,&c_rand,t_rand,q_rand,arst,ar);

            /* validate the random sample */
            validSample=AtlasRRTValidateSample(pr,q_rand,BOTHTREES,FALSE,NO_UINT,
                                               ar->si[1]->s,l,&h,&i_near,arst,ar);
          } while (!validSample);
 
          /*******************************************************************/
          /* decide who is the goal (for the heuristic)  */
          if (GetRRTNodeTree(i_near,&(ar->rrt))==START2GOAL)
            g=ar->si[1]->s;
          else
            g=ar->si[0]->s;
          h=DistanceTopology(ar->m,ar->tp,q_rand,g);


          /*******************************************************************/
          /* Extend the AtlasRRT from q_near to q_rand using the cbiRRT extend method */
          /* Note that here we use a extend strategy (we only add one sample) */

          id_new=AddStepToAtlasRRTstar(pr,*it,FALSE,i_near,q_rand,
                                       NULL,NULL,(void*)arst,ar);

          /* If we actually added something */
          if ((id_new!=i_near)&&((((HEURISTIC_RRT_STAR)&(2))==0)||(l<INF)))
            { 
              /* q_new is the last samples added to the tree */
              q_new=ar->si[id_new]->s;
              h=DistanceTopology(ar->m,ar->tp,q_new,g);
          
              /*******************************************************************/
              /* Search for the set of potential neighbours */
              numSamples=(double)(ar->ns<3?3:ar->ns);
              gamma=ct_gamma*pow(log(numSamples)/numSamples,1.0/(double)ar->k);
              gamma=(gamma<delta?delta:gamma);

              GetRRTNNInBall(BOTHTREES,q_new,gamma,&nn,&n,&(ar->rrt));

              /*******************************************************************/
              /* Wire: Search for the lowest cost path to the new node */
              WireAtlasRRTstar(pr,id_new,i_near,gamma,nn,n,&c,h,(graph?&q:NULL),&vt,arst,ar);
                  
              /*******************************************************************/
              /* Rewire the tree around the new point */
              if (graph)
                RecursiveReWireRRTstar(pr,&q,g,&steps,&l,&(ar->rrt));
              else
                ReWireAtlasRRTstar(pr,id_new,gamma,nn,n,c,&steps,&l,arst,ar);
                  
              /*******************************************************************/
              /* release the data for this iteration (cost and neighbours) */   
              if (c!=NULL)
                free(c);
              free(n);

              /*******************************************************************/
              /* If the set of neighbours does not include nodes in the two trees
                 and we do not have a path start<->goal we try to connect the two 
                 trees (this is hardly used since when l=INF, gamma is large and
                 the neighbours always include nodes in the two trees) */
              //if ((vt!=BOTHTREES)&&(l==INF))
              if (vt!=BOTHTREES)
                {
                  /* In the re-wire the current sample might end up in tree 't2'
                     and the connection must be always with the other tree */
                  if (GetRRTNodeTree(id_new,&(ar->rrt))==START2GOAL)
                    i_near=GetRRTNN(GOAL2START,q_new,&(ar->rrt));
                  else
                    i_near=GetRRTNN(START2GOAL,q_new,&(ar->rrt));

                  l1=GetRRTNodeCost(id_new,&(ar->rrt))+GetRRTNodeCost(i_near,&(ar->rrt));

                  #if (NEW_BRANCH_EXTENSION)
                    configExtend.maxLength=(l<INF?l-l1:0.0);
                    c12=New_AddBranchToAtlasRRT(pr,NO_UINT,&configExtend,id_new,ar->si[i_near]->s,NULL,
                                                (void*)arst,&statusExtend,NULL,NULL,ar);
                  
                    if (statusExtend.reachedQrand)
                  #else
                    c12=AddBranchToAtlasRRT(pr,NO_UINT,ADD_NONE,TRUE,TRUE,TRUE,FALSE,
                                            (l<INF?l-l1:0.0),id_new,ar->si[i_near]->s,NULL,
                                            (void*)arst,&lastID,&reachedQRand,&reachedGoal,NULL,NULL,NULL,NULL,ar);
                  
                    if (reachedQRand)
                  #endif
                      {
                        if (graph)
                          AddEdgeToRRT(id_new,i_near,c12,&(ar->rrt));
                        l1+=c12;
                        if (l1<l)
                          l=ChangeBiRRTSteps(id_new,i_near,c12,&steps,&(ar->rrt));
                      }
                }
            }
          #if (ATLASRRT_VERBOSE)
          else
            {
              if (id_new==i_near)
                fprintf(stderr,"  Blocked [%u]\n",i_near);
            }
          #endif

          if (!optimal)
            done=(l<INF);

          /*******************************************************************/
          /* Conclude the iteration */
          time=GetElapsedTime(&st);

          AtlasBiRRTstarCloseIteration(*it,l,time,gamma,times,costs,ar);
      
          (*it)++;
        }
  
      if (graph)
        DeleteHeap(&q);

      *planningTime=GetElapsedTime(&st);

      DeleteStatistics(&st);

      fprintf(stderr,"Final number of samples: %u\n",ar->ns);

      /* Reconstruct the path (if any) */
      if (l<INF) 
        Steps2PathinAtlasRRT(pr,&steps,pl,NULL,ns,path,ar);

      /* Release memory */
      free(q_rand);
      free(t_rand);
      DeleteVector(&steps);

      #if (GET_ATLASRRT_STATISTICS)
        if (str==NULL)
          PrintAtlasRRTStatistics(ar,arst);
        else
          AccumulateAtlasRRTStatistics(arst,str);  
        DeleteAtlasRRTStatistics(arst);
        free(arst);
      #endif

      return(l<INF);
    }
}

unsigned int GetAtlasRRTNumNodes(Tatlasrrt *ar)
{
  return(ar->ns);
}

unsigned int GetAtlasRRTNumCharts(Tatlasrrt *ar)
{
  return(ar->nc); 
}


unsigned int GetRRTNNInNeighbourChart(unsigned int tree,
                                      unsigned int c_rand,double *t_rand,double *q_rand,
                                      Tatlasrrt *ar) 
{
  double d,d1;
  unsigned int inear,inear1; 
  Tchart *chart;
  unsigned int i,nn,nID;

  inear=GetRRTNNInChart(tree,c_rand,q_rand,INF,&d,ar);

  chart=GetAtlasChart(c_rand,&(ar->atlas));
  nn=ChartNumNeighbours(chart);
  for(i=0;i<nn;i++)
    {
      nID=ChartNeighbourID(i,chart);
      if (nID!=NO_UINT)
        {
          //if ((!ar->birrt)||(ar->ci[nID]->tree&tree))
            {
              inear1=GetRRTNNInChart(tree,nID,q_rand,d,&d1,ar);
              if (d1<d) 
                {
                  d=d1;
                  inear=inear1;
                }
            }
        }
    }
  return(inear);
}

unsigned int GetRRTNNInChart(unsigned int tree,
                             unsigned int chartId,double *q_rand,
                             double t,double *d,Tatlasrrt *ar) 
{
  double d1;
  unsigned int j,inear;
  
  *d=t; 
  inear=NO_UINT;
  j=ar->ci[chartId]->lc;
  while(j!=NO_UINT)
    {
      if ((!ar->birrt)||(GetRRTNodeTree(j,&(ar->rrt))==tree))
        {
          d1=DistanceTopologyMin(*d,ar->m,ar->tp,q_rand,ar->si[j]->s);
          if (d1<*d)
            {
              *d=d1;
              inear=j;
            }
        }
      j=ar->si[j]->lsc;
    }

  return(inear);
}

void PlotAtlasRRT(char *prefix,int argc,char **arg,Tparameters *pr,
                  unsigned int xID,unsigned int yID,unsigned int zID,Tatlasrrt *ar)
{
  Tfilename fplot;
  unsigned int i;
  Tchart *chart1,*chart2;
  double *sample1,*sample2;
  unsigned int parent;
  double x[4],y[4],z[4];
  double *point1,*point2;
  double *o1,*o2,*o3,*o4;
  Tcolor color;
  Tplot3d p3d;

  CreateFileName(NULL,prefix,"_rrt",PLOT3D_EXT,&fplot);  
  fprintf(stderr,"Plotting RRT to                   : %s\n",GetFileFullName(&fplot));
  PlotRRT(GetFileFullName(&fplot),argc,arg,pr,xID,yID,zID,&(ar->rrt));
  DeleteFileName(&fplot);
  
  if ((!PLOT_AS_POLYGONS)||(ar->k<4))
    {
      CreateFileName(NULL,prefix,"_atlas",PLOT3D_EXT,&fplot);  
      fprintf(stderr,"Plotting Atlas to                 : %s\n",GetFileFullName(&fplot));
      PlotAtlas(GetFileFullName(&fplot),argc,arg,pr,NULL,xID,yID,zID,&(ar->atlas));
      DeleteFileName(&fplot);
    }
  else
    fprintf(stderr,"Skipping the plot of the atlas (too large dimension)\n");

  /* Now we plot the RRT in the tangent space and a line from tangent space to
     the RRT. This basically replicates the plot of the RRT but in the tangent
     space */

  CreateFileName(NULL,prefix,"_atlasrrt",PLOT3D_EXT,&fplot);
  fprintf(stderr,"Plotting AtlasRRT to              : %s\n",GetFileFullName(&fplot));

  InitPlot3d(GetFileFullName(&fplot),FALSE,argc,arg,&p3d);
  DeleteFileName(&fplot);

  NEW(point1,ar->m,double);
  NEW(point2,ar->m,double);

  NewColor(0.5,0,0,&color); /*red*/
  StartNew3dObject(&color,&p3d);

  for(i=(ar->birrt?2:1);i<ar->ns;i++)
    {
      if ((!ar->birrt)||(GetRRTNodeTree(i,&(ar->rrt))==START2GOAL))
        {
          parent=GetRRTParent(i,&(ar->rrt));

          chart1=GetAtlasChart(ar->si[i]->c,&(ar->atlas));
          chart2=GetAtlasChart(ar->si[parent]->c,&(ar->atlas));

          if (chart1==chart2)
            {
              sample1=ar->si[i]->s;
              sample2=ar->si[parent]->s;

              CS_WD_REGENERATE_ORIGINAL_POINT(pr,sample1,&o1,ar->w);
              CS_WD_REGENERATE_ORIGINAL_POINT(pr,sample2,&o2,ar->w);

              Local2Global(ar->si[i]->t,NULL,point1,chart1);
              Local2Global(ar->si[parent]->t,NULL,point2,chart2);

              CS_WD_REGENERATE_ORIGINAL_POINT(pr,point1,&o3,ar->w);
              CS_WD_REGENERATE_ORIGINAL_POINT(pr,point2,&o4,ar->w);
          
              x[0]=o1[xID];
              y[0]=o1[yID];
              z[0]=o1[zID];
          
              x[1]=o3[xID];
              y[1]=o3[yID];
              z[1]=o3[zID];

              x[2]=o4[xID];
              y[2]=o4[yID];
              z[2]=o4[zID];

              x[3]=o2[xID];
              y[3]=o2[yID];
              z[3]=o2[zID];
          
              PlotVect3d(4,x,y,z,&p3d);

              free(o1);
              free(o2);
              free(o3);
              free(o4);
            }
        }
    }
  DeleteColor(&color);
  Close3dObject(&p3d);

  if (ar->birrt)
    {      
      NewColor(0,0.5,0,&color); /*green*/
      StartNew3dObject(&color,&p3d);

      for(i=2;i<ar->ns;i++)
        {
          if (GetRRTNodeTree(i,&(ar->rrt))==GOAL2START)
            {
              parent=GetRRTParent(i,&(ar->rrt));

              chart1=GetAtlasChart(ar->si[i]->c,&(ar->atlas));
              chart2=GetAtlasChart(ar->si[parent]->c,&(ar->atlas));

              if (chart1==chart2)
                {
                  sample1=ar->si[i]->s;
                  sample2=ar->si[parent]->s;

                  CS_WD_REGENERATE_ORIGINAL_POINT(pr,sample1,&o1,ar->w);
                  CS_WD_REGENERATE_ORIGINAL_POINT(pr,sample2,&o2,ar->w);

                  Local2Global(ar->si[i]->t,NULL,point1,chart1);
                  Local2Global(ar->si[parent]->t,NULL,point2,chart2);

                  CS_WD_REGENERATE_ORIGINAL_POINT(pr,point1,&o3,ar->w);
                  CS_WD_REGENERATE_ORIGINAL_POINT(pr,point2,&o4,ar->w);
          
                  x[0]=o1[xID];
                  y[0]=o1[yID];
                  z[0]=o1[zID];
          
                  x[1]=o3[xID];
                  y[1]=o3[yID];
                  z[1]=o3[zID];

                  x[2]=o4[xID];
                  y[2]=o4[yID];
                  z[2]=o4[zID];

                  x[3]=o2[xID];
                  y[3]=o2[yID];
                  z[3]=o2[zID];
          
                  PlotVect3d(4,x,y,z,&p3d);

                  free(o1);
                  free(o2);
                  free(o3);
                  free(o4);
                }
            }
        }
      DeleteColor(&color);
      Close3dObject(&p3d);
    }
  
  #if (RRT_PLOT_NODES)
    /* Dots, one per node */
    NewColor(0,0,0,&color); /*black*/
    StartNew3dObject(&color,&p3d);

    for(i=0;i<ar->ns;i++)
      {
        chart1=GetAtlasChart(ar->si[i]->c,&(ar->atlas));
        Local2Global(ar->si[i]->t,NULL,point1,chart1);
        CS_WD_REGENERATE_ORIGINAL_POINT(pr,point1,&o1,ar->w);
        
        x[0]=o1[xID]-0.005;
        y[0]=o1[yID];
        z[0]=o1[zID];

        x[1]=o1[xID]+0.005;
        y[1]=o1[yID];
        z[1]=o1[zID];

        PlotVect3d(2,x,y,z,&p3d);

        x[0]=o1[xID];
        y[0]=o1[yID]-0.005;
        z[0]=o1[zID];

        x[1]=o1[xID];
        y[1]=o1[yID]+0.005;
        z[1]=o1[zID];

        PlotVect3d(2,x,y,z,&p3d);

        x[0]=o1[xID];
        y[0]=o1[yID];
        z[0]=o1[zID]-0.005;

        x[1]=o1[xID];
        y[1]=o1[yID];
        z[1]=o1[zID]+0.005;

        PlotVect3d(2,x,y,z,&p3d);

        free(o1);
      }
    
    DeleteColor(&color);
    Close3dObject(&p3d);  
  #endif

  ClosePlot3d(FALSE,0,0,0,&p3d);

  free(point1);
  free(point2);
}

unsigned int AtlasRRTMemSize(Tatlasrrt *ar)
{
  unsigned int b;

  b=RRTMemSize(&(ar->rrt));
  b+=AtlasMemSize(&(ar->atlas));

  return(b);
}

void SaveAtlasRRT(Tparameters *pr,char *prefix,Tatlasrrt *ar)
{
  Tfilename fsave;
  FILE *f;
  unsigned int i;

  CreateFileName(NULL,prefix,NULL,RRT_EXT,&fsave);
  fprintf(stderr,"Writing RRT to                    : %s\n",GetFileFullName(&fsave));
  SaveRRT(&fsave,&(ar->rrt));
  DeleteFileName(&fsave);
  
  CreateFileName(NULL,prefix,NULL,ATLAS_EXT,&fsave);
  fprintf(stderr,"Writing atlas to                  : %s\n",GetFileFullName(&fsave));
  SaveAtlas(pr,&fsave,&(ar->atlas));
  DeleteFileName(&fsave);  

  CreateFileName(NULL,prefix,NULL,ATLAS_RRT_EXT,&fsave);
  fprintf(stderr,"Writing AtlasRRT to               : %s\n",GetFileFullName(&fsave));
  f=fopen(GetFileFullName(&fsave),"w");
  if (!f)
    Error("Could not open file to write the AtlasRRT information");
  DeleteFileName(&fsave);  

  fprintf(f,"%u\n",ar->m);
  fprintf(f,"%u\n",ar->k);
  fprintf(f,"%.12f\n",ar->e);
  fprintf(f,"%.12f\n",ar->ce);
  fprintf(f,"%.12f\n",ar->r);
  fprintf(f,"%u\n",ar->birrt);

  fprintf(f,"%u\n",ar->ms);
  fprintf(f,"%u\n",ar->ns);
  for(i=0;i<ar->ns;i++)
    SaveAtlasRRTSampleInfo(f,ar->si[i],ar);

  fprintf(f,"%u\n",ar->mc);
  fprintf(f,"%u\n",ar->nc);
  for(i=0;i<ar->nc;i++)
    SaveAtlasRRTChartInfo(f,ar->ci[i],ar);

  if (ar->birrt)
    {
      fprintf(f,"%u\n",ar->mct1);
      fprintf(f,"%u\n",ar->nct1);
      for(i=0;i<ar->nct1;i++)
        fprintf(f,"%u ",ar->chartsAtTree1[i]);
      fprintf(f,"\n");

      fprintf(f,"%u\n",ar->mct2);
      fprintf(f,"%u\n",ar->nct2);
      for(i=0;i<ar->nct2;i++)
        fprintf(f,"%u ",ar->chartsAtTree2[i]);
      fprintf(f,"\n");
    }
  fclose(f);
}

void LoadAtlasRRT(Tparameters *pr,char *prefix,TAtlasBase *w,Tatlasrrt *ar)
{
  Tfilename fload;
  FILE *f;
  unsigned int i;

  CreateFileName(NULL,prefix,NULL,RRT_EXT,&fload);
  fprintf(stderr,"Reading RRT from                  : %s\n",GetFileFullName(&fload));
  LoadRRT(pr,&fload,w,&(ar->rrt));
  DeleteFileName(&fload);
  
  CreateFileName(NULL,prefix,NULL,ATLAS_EXT,&fload);
  fprintf(stderr,"Reading atlas from                : %s\n",GetFileFullName(&fload));
  LoadAtlas(pr,&fload,w,&(ar->atlas));
  DeleteFileName(&fload);  

  CreateFileName(NULL,prefix,NULL,ATLAS_RRT_EXT,&fload);
  fprintf(stderr,"Reading AtlasRRT from             : %s\n",GetFileFullName(&fload));
  f=fopen(GetFileFullName(&fload),"r");
  if (!f)
    Error("Could not open file to load the AtlasRRT information");
  DeleteFileName(&fload); 

  ar->w=w;

  fscanf(f,"%u",&(ar->m));
  fscanf(f,"%u",&(ar->k));
  fscanf(f,"%lf",&(ar->e));
  fscanf(f,"%lf",&(ar->ce));
  fscanf(f,"%lf",&(ar->r));
  fscanf(f,"%u",&(ar->birrt));
  ar->parallel=FALSE; /* AtlasRRts loaded from files are not processed in parallel */

  fscanf(f,"%u",&(ar->ms));
  fscanf(f,"%u",&(ar->ns));
  NEW(ar->si,ar->ms,TSampleInfo *);
  for(i=0;i<ar->ns;i++)
    {
      NEW(ar->si[i],1,TSampleInfo);
      LoadAtlasRRTSampleInfo(f,ar->si[i],ar);
    }

  fscanf(f,"%u",&(ar->mc));
  fscanf(f,"%u",&(ar->nc));
  NEW(ar->ci,ar->mc,TChartInfo *);
  for(i=0;i<ar->nc;i++)
    {
      NEW(ar->ci[i],1,TChartInfo);
      LoadAtlasRRTChartInfo(f,ar->ci[i],ar);
    }

  if (ar->birrt)
    {
      fscanf(f,"%u",&(ar->mct1));
      fscanf(f,"%u",&(ar->nct1));
      NEW(ar->chartsAtTree1,ar->mct1,unsigned int);
      for(i=0;i<ar->nct1;i++)
        fscanf(f,"%u",&(ar->chartsAtTree1[i]));

      fscanf(f,"%u",&(ar->mct2));
      fscanf(f,"%u",&(ar->nct2));
      NEW(ar->chartsAtTree2,ar->mct2,unsigned int);
      for(i=0;i<ar->nct2;i++)
        fscanf(f,"%u",&(ar->chartsAtTree2[i]));
    }
  fclose(f);


  CS_WD_GENERATE_SIMP_INITIAL_BOX(pr,&(ar->ambient),ar->w);
  ar->tp=GetRRTTopology(&(ar->rrt));
  CS_WD_GET_SIMP_JACOBIAN(pr,&(ar->J),ar->w);
}

void DeleteAtlasRRT(Tatlasrrt *ar)
{
  unsigned int i;

  DeleteBox(&(ar->ambient));

  DeleteAtlas(&(ar->atlas));
  DeleteRRT(&(ar->rrt));
  
  /* Release sample info */
  for(i=0;i<ar->ns;i++)
    {
      free(ar->si[i]->t);
      free(ar->si[i]);
      /* The sample info is a pointer to an array
         stored in the RRT -> no need to free here */
    }
  free(ar->si);

  /* Release chart info */
  for(i=0;i<ar->nc;i++)
    free(ar->ci[i]);
    
  free(ar->ci);

  /* Release the assignment of charts to trees */
  if (ar->birrt)
    {
      free(ar->chartsAtTree1);
      free(ar->chartsAtTree2);
    }

  DeleteJacobian(&(ar->J));
}