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

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_SR)||(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 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 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 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);

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

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;
  #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)&&(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);
  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 given samples -> do not check */
      else 
        {
          (*intersectParent)=FALSE;  
          ce=GetAtlasChart(newChart,&(ar->atlas));
          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)\n",
                currentSampleInfo->c,newChart);
      #endif

      /* 1+newChart -> we can find out which chart was created at each
         sample just by substracting one (used for debug). */
      currentSampleInfo->generateChart=1+newChart;

      /* 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);

      /* The sample used to create the new chart should be assigned to it.
         Otherwise we have to correct the situation.
         We do not worry in the case of temporary samples (samples not actually
         included in the tree, ID=NO_UINT)
      */
      if (currentID==NO_UINT)
        {
          /* Sample not in the tree */
          currentSampleInfo->c=newChart;
          currentSampleInfo->pc=newChart;
          for(j=0;j<ar->k;j++)
            currentSampleInfo->t[j]=0;
        }
      else
        {
          if (currentSampleInfo->c!=newChart)
            {
              unsigned int n;
              boolean found;
              Tchart *ce;
              TChartInfo *newChartInfo;
              TSampleInfo *previousSampleInfo;
      
              /* This happens for samples that are not on the chart where
                 they were originally created and are at a chart that is
                 not neighbour to the just created chart (mainly due to
                 curvature errors). 
                 In this case, populateWithSamples is not able to steal
                 the sample used to create the chart from its current chart.
                 Here we force the assignment.
              */
              n=currentSampleInfo->c; /*Chart from where to steal the center*/
              currentChartInfo=ar->ci[n];       
              newChartInfo=ar->ci[newChart];

              previousSampleInfo=NULL; /* previous sample in the list */
              j=currentChartInfo->lc;  /* first sample in the list */
              found=FALSE;
              while((j!=NO_UINT)&&(!found))
                {
                  currentSampleInfo=ar->si[j];
                  if (j==currentID)
                    {         
                      /* 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 the new chart */
                      currentSampleInfo->lsc=newChartInfo->lc;
                      newChartInfo->lc=j;
                      currentSampleInfo->c=newChart;

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

                      found=TRUE;
                    }
                  else
                    {
                      /* Move to next */          
                      previousSampleInfo=currentSampleInfo;
                      j=currentSampleInfo->lsc;
                    }
                }
              if (!found)
                Error("A sample is not where it should be");
            } 
        }
      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;
  boolean reachedParent,reachedG;
  unsigned int p,ls;

  *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. */
          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);

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

          (*pl)+=c;
          if (!reachedParent)
            Warning("Path segment could not be reconstructed in ReconstructAtlasRRTPath");

          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;
  boolean reachedG,reachedNext;
  unsigned int ls; /* last added sample, if any */

  *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);
  
      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);

      #if (RRT_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 (!reachedNext) 
        Warning("Path segment could not be reconstructed in Steps2PathinRRT");
      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);

  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;
  boolean reachedQRand,reachedGoal;
  unsigned int lastID;

  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 */
      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);

      /* If node n2 can be reached from node n1 */
      if (reachedQRand)
        {
          /* 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_SR)||(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_SR)||(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_SR)||(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_SR)
              if (!validSample)
                DecreaseChartSamplingRadius(GetAtlasChart(*c_rand,&(ar->atlas)));
            #endif
            #if (GET_ATLASRRT_STATISTICS)
              NewAtlasRRTRandomSample(arst);
              if (!validSample)
                NewAtlasRRTSampleRejection(arst);
            #endif
          } while (!validSample);

          #if (ADJUST_SR)
            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;
  boolean reachedQRand,reachedGoal;
  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;

  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,0,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
      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);
      #if (GET_ATLASRRT_STATISTICS)
        if (dst>0)
          NewAtlasRRTNoEmptyBranch(arst);
      #endif
      done=((!EXPLORATION_RRT)&&(!ar->birrt)&&(reachedGoal));
      
      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 */
                      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));

              /* Try to connect the '2nd' tree to the last node added to the '1st' tree */
              #if (GET_ATLASRRT_STATISTICS)
                NewAtlasRRTTreeConnection(arst);
              #endif
              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);
              #if (GET_ATLASRRT_STATISTICS)
                if (dst>0)
                  NewAtlasRRTNoEmptyTreeConnection(arst);
              #endif
              
              done=((!EXPLORATION_RRT)&&(reachedGoal));
            }
          #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;
  unsigned int lastSample;
  TAtlasRRTStatistics *arst;
  Tstatistics stime; /* used just to measure execution time */
  boolean collision=FALSE;
  boolean reachedQRand,reachedGoal;
  unsigned int lastNN2Goal;
  unsigned int db;
  double maxTime;
  unsigned int maxNodes;
  double scale; /* Global scale factor for sampling areas */
  boolean validSample;
  
  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,0,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 */
      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));

      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) */
  pathFound=PathStart2GoalInRRT(pr,finalPoint,lastSample,NO_UINT,
                                pl,pc,ns,path,&(ar->rrt));

  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)
{
  unsigned int id_new;
  double d,dg,epsilon;
  boolean reachedQRand,reachedGoal;

  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. */
  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);

  /* 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 ((goal!=NULL)&&(*id_goal==NO_UINT)&&
          ((reachedGoal)||((expand2Goal)&&(reachedQRand))))
        {
          dg=DistanceTopology(ar->m,ar->tp,ar->si[id_new]->s,goal);

          if (dg<epsilon)
            /* 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. */
              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)
                Error("Could not reach goal from near-gaol");
                  
              SetRRTNodeCost(*id_goal,d,GetRRTNodeCost(id_new,&(ar->rrt))+d,&(ar->rrt));

              #if (RRT_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,lastID;
  double ic,cn,epsilon,c_new;
  double *cost;
  boolean *reachedQRand;
  double *q_new;
  TDoublePair pair;
  unsigned int rrtMode;
  boolean graph;

  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,&(ar->ambient),
                                              ar->m,ar->m-ar->k,ar->si[n[i]]->s,q_new,gamma,
                                              TRUE,&(reachedQRand[i]),&collision,NULL,NULL,NULL,ar->w);;
                }
              else
                {
                  boolean reachedGoal;

                  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);
                }
            }
        }
    }

  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 (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;
  boolean reachedQRand;
  unsigned int rrtMode;
  #if (!RRTSTAR_SYMMETRIC_COST)
    boolean reachedGoal;
    unsigned int lastID;
  #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 */
            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
        }

      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;
      #if (RRTSTAR_SYMMETRIC_COST)
        /* cost of paths to neighbouring nodes */
        double *c;
      #endif
      /* 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,0,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,DeleteDoublePair,
                 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 (RRTSTAR_SYMMETRIC_COST)
        /* if the cost function is symmetrical, we cache
           the evaluation of the cost to neighbour. */
        double *c;
      #endif
      /* 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;
      boolean reachedQRand,reachedGoal;
      unsigned int lastID;
      double numSamples;
      double scale; /* Global adjust of sampling areas */

      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,DeleteDoublePair,
                 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,DeleteRRTStep,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));
                  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)
                    {
                      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));
}