rrt.c

Go to the documentation of this file.

#include "rrt.h"

#include "defines.h"
#include "algebra.h"
#include "random.h"
#include "samples.h"
#include "filename.h"

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

void NewRRTBranch(TRRTStatistics *rst);

void NewRRTNoEmptyBranch(TRRTStatistics *rst);

void NewRRTTreeConnection(TRRTStatistics *rst);

void NewRRTNoEmptyTreeConnection(TRRTStatistics *rst);

void NewRRTStep(TRRTStatistics *rst);

void NewRRTDistanceQrand(double dqr,TRRTStatistics *rst);

void NewRRTQrandReached(TRRTStatistics *rst);

void NewRRTConvergenceError(TRRTStatistics *rst);

void NewRRTHighCost(TRRTStatistics *rst);

void NewRRTOutOfDomain(TRRTStatistics *rst);

void NewRRTDistanceIncreases(TRRTStatistics *rst);

void NewRRTCollision(TRRTStatistics *rst);

void NewRRTTooFar(TRRTStatistics *rst);

void NewRRTStalled(TRRTStatistics *rst);

void NewRRTSample(TRRTStatistics *rst);

void NewRRTRandomSample(TRRTStatistics *rst);

void NewRRTSampleRejection(TRRTStatistics *rst);

void NewRRTCollisionCheck(TRRTStatistics *rst);


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

void InitRRTStatistics(TRRTStatistics *rst)
{
  if (rst!=NULL)
    {
      rst->n=0;

      rst->nBranch=0;
      rst->nNoEmptyBranch=0;

      rst->nTreeConnection=0;
      rst->nNoEmptyTreeConnection=0;

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

      rst->nQrandReached=0;
      rst->nConvergenceError=0;
      rst->nHighCost=0;
      rst->nOutOfDomain=0;
      rst->nDistanceIncreases=0;
      rst->nCollision=0;
      rst->nTooFar=0;
      rst->nStalled=0;

      rst->nSample=0;

      rst->nRandom=0;
      rst->nRejections=0;

      rst->nCollisionChecks=0;
    }
}

void NewRRTBranch(TRRTStatistics *rst)
{
  if (rst!=NULL) rst->nBranch++;
}

void NewRRTNoEmptyBranch(TRRTStatistics *rst)
{
  if (rst!=NULL) rst->nNoEmptyBranch++;
}

void NewRRTTreeConnection(TRRTStatistics *rst)
{
  if (rst!=NULL) rst->nTreeConnection++;
}

void NewRRTNoEmptyTreeConnection(TRRTStatistics *rst)
{
  if (rst!=NULL) rst->nNoEmptyTreeConnection++;
}

void NewRRTDistanceQrand(double dqr,TRRTStatistics *rst)
{
  if (rst!=NULL) rst->dQrand+=dqr;
}

void NewRRTStep(TRRTStatistics *rst)
{
  if (rst!=NULL) rst->nStep++;
}

void NewRRTQrandReached(TRRTStatistics *rst)
{
  if (rst!=NULL) rst->nQrandReached++;
}

void NewRRTConvergenceError(TRRTStatistics *rst)
{
  if (rst!=NULL) rst->nConvergenceError++;
}

void NewRRTHighCost(TRRTStatistics *rst)
{
  if (rst!=NULL) rst->nHighCost++;
}

void NewRRTOutOfDomain(TRRTStatistics *rst)
{
  if (rst!=NULL) rst->nOutOfDomain++;
}

void NewRRTDistanceIncreases(TRRTStatistics *rst)
{
  if (rst!=NULL) rst->nDistanceIncreases++;
}

void NewRRTCollision(TRRTStatistics *rst)
{
  if (rst!=NULL) rst->nCollision++;
}

void NewRRTTooFar(TRRTStatistics *rst)
{
  if (rst!=NULL) rst->nTooFar++;
}

void NewRRTStalled(TRRTStatistics *rst)
{
  if (rst!=NULL) rst->nStalled++;
}

void NewRRTSample(TRRTStatistics *rst)
{
  if (rst!=NULL) rst->nSample++;
}

void NewRRTRandomSample(TRRTStatistics *rst)
{
  if (rst!=NULL) rst->nRandom++;
}

void NewRRTSampleRejection(TRRTStatistics *rst)
{
  if (rst!=NULL) rst->nRejections++;
}

void NewRRTCollisionCheck(TRRTStatistics *rst)
{
  if (rst!=NULL) rst->nCollisionChecks++;
}

void AccumulateRRTStatistics(TRRTStatistics *rst1,TRRTStatistics *rst2)
{
  if ((rst1!=NULL)&&(rst2!=NULL))
    {
      if (rst1->n==0)
        rst2->n++;
      else
        rst2->n+=rst1->n;

      rst2->nBranch+=rst1->nBranch;
      rst2->nNoEmptyBranch+=rst1->nNoEmptyBranch;

      rst2->nTreeConnection+=rst1->nTreeConnection;
      rst2->nNoEmptyTreeConnection+=rst1->nNoEmptyTreeConnection;

      rst2->nStep+=rst1->nStep;
      rst2->dQrand+=rst1->dQrand;

      rst2->nQrandReached+=rst1->nQrandReached;
      rst2->nOutOfDomain+=rst1->nOutOfDomain;
      rst2->nDistanceIncreases+=rst1->nDistanceIncreases;
      rst2->nCollision+=rst1->nCollision;
      rst2->nTooFar+=rst1->nTooFar;
      rst2->nStalled+=rst1->nStalled;

      rst2->nSample+=rst1->nSample;

      rst2->nRandom+=rst1->nRandom;
      rst2->nRejections+=rst1->nRejections;

      rst2->nCollisionChecks+=rst1->nCollisionChecks;
    }
}

void PrintRRTStatistics(Trrt *rrt,TRRTStatistics *rst)
{
  if (rst!=NULL) 
    {
      unsigned int m,nExt;

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

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

      fprintf(stderr,"%% **************************************************\n");
      if (rst->n>1)
        fprintf(stderr,"RRT Statistics (averaged over %u repetitions)\n",rst->n);
      else
        fprintf(stderr,"RRT Statistics\n\n");
      fprintf(stderr,"  Num. random samples             : %.2f\n",
              (double)rst->nRandom/(double)rst->n);
      fprintf(stderr,"  Num. rejected random samples    : %.2f (%.2f)\n",
              (double)rst->nRejections/(double)rst->n,
              (double)rst->nRejections/(double)rst->nRandom);
      fprintf(stderr,"  Num. accepted random samples    : %.2f (%.2f)\n",
              (double)(rst->nRandom-rst->nRejections)/(double)rst->n,
              (double)(rst->nRandom-rst->nRejections)/(double)rst->nRandom);
  
      if ((rst->n==1)&&(rrt!=NULL)&&(DynamicDomainRRT(rrt)))
        {
          unsigned int i,n;
          double miDD,maDD,avDD,r;

          miDD=+INF;
          maDD=-INF;
          avDD=0.0;
          n=0;
          for(i=0;i<rrt->ns;i++)
            {
              r=GetDynamicDomainRadius(i,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,rrt->ns);
        }
      fprintf(stderr,"  Average  distance to Qrand      : %.2f\n",
              rst->dQrand/(double)rst->nBranch);

      if (rst->nTreeConnection>0)
        {
          fprintf(stderr,"  Num. direct RRT extensions      : %.2f\n",
                  (double)rst->nBranch/(double)rst->n);

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

      m=rst->nConvergenceError;
      fprintf(stderr,"    Convergence errors            : %.2f (%.2f)\n",
              (double)m/(double)rst->n,
              (double)m/(double)nExt);

      m=rst->nHighCost;
      fprintf(stderr,"    High cost configurations      : %.2f (%.2f)\n",
              (double)m/(double)rst->n,
              (double)m/(double)nExt);

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

      m=rst->nDistanceIncreases;
      fprintf(stderr,"    Distance to q_rand increases  : %.2f (%.2f)\n",
              (double)m/(double)rst->n,
              (double)m/(double)nExt);

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

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

      m=rst->nStalled;
      fprintf(stderr,"    Stalled branch                : %.2f (%.2f)\n",
              (double)m/(double)rst->n,
              (double)m/(double)nExt);


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

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


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

void DeleteRRTStatistics(TRRTStatistics *rst)
{
}


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


void SetRRTTopology(Tparameters *pr,Trrt *rrt);

boolean AddBranchToRRT(Tparameters *pr,boolean ccRRT,
                       unsigned int tree,unsigned int i_near,
                       double *q_rand,double *goal,
                       unsigned int *lastSample, 
                       double (*costF)(Tparameters*, boolean , double*, void*),
                       void *costData,
                       TRRTStatistics *rst,Trrt *rrt);

unsigned int ReconstructRRTPath(Tparameters *pr,unsigned int sID,
                                double *pl,double* pc,unsigned int *ns,
                                double ***path,Trrt *rrt);

unsigned int Steps2PathinRRT(Tparameters *pr,Tvector *steps,double *pl,double* pc,
                             unsigned int *ns,double ***path,Trrt *rrt);

double RRTPathLength(Tparameters *pr,unsigned int sID,Trrt *rrt);

unsigned int AddStepToRRTstar(Tparameters *pr,unsigned int i_near,
                              double *q_rand,double *goal,
                              unsigned int *i_goal,
                              TRRTStatistics *rst,Trrt *rrt);
void WireRRTstar(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,
                 TRRTStatistics *rst,Trrt *rrt);

void ReWireRRTstar(Tparameters *pr,
                   unsigned int id_new,double gamma,
                   unsigned int nn,unsigned int *n,double *c,
                   Tvector *steps,double *l,
                   Trrt *rrt);

void RRTstarCloseIteration(unsigned int it,unsigned int id_goal,
                           double time,double gamma,
                           double *times,double *costs,
                           Trrt *rrt);

void BiRRTstarCloseIteration(unsigned int it,double l,
                             double time,double gamma,
                             double *times,double *costs,
                             Trrt *rrt);

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

void CopyRRTStep(void *a,void *b)
{
  ((TRRTStep *)a)->id=((TRRTStep *)b)->id;
  ((TRRTStep *)a)->cost=((TRRTStep *)b)->cost;
}

void DeleteRRTStep(void *a)
{
}

void SetRRTTopology(Tparameters *pr,Trrt *rrt)
{
  boolean hasS;
  unsigned int i;

  if (CS_WD_GET_SIMP_TOPOLOGY(pr,&(rrt->tp),rrt->w)!=rrt->m)
    Error("Missmatch number of variables in SetRRTTopology");

  /* Search for a "circular" variable */
  hasS=FALSE;
  i=0;
  while((i<rrt->m)&&(!hasS))
    {
      hasS=(rrt->tp[i]==TOPOLOGY_S);
      i++;
    }
  if (!hasS)
    {
      /* if all variables are real no need to consider topology */
      free(rrt->tp);
      rrt->tp=NULL;
    }
}

void AddEdgeToRRT(unsigned int i,unsigned int j,double c,Trrt *rrt)
{
  if (rrt->graph)
    {
      TRRTSampleInfo *si;
      unsigned int k;

      if ((i>=rrt->ns)||(j>=rrt->ns))
        Error("Nodes out of range in AddEdgeToRRT");

      for(k=0;k<2;k++)
        {
          si=(k==0?rrt->si[i]:rrt->si[j]);
          if (si->nn==si->m)
            {
              MEM_DUP(si->n,si->m,unsigned int);
              MEM_EXPAND(si->cn,si->m,double);
            }
          si->n[si->nn]=(k==0?j:i);
          si->cn[si->nn]=c;
          si->nn++;
        }
    }
}

boolean TransitionTestRRT(Tparameters *pr,unsigned int parent,double* q_next,double deltaStep,double *cost,
                          double (*costF)(Tparameters*,boolean,double*,void*),void *costData,Trrt *rrt) 
{
  double prevCost;
  double threshold;
  boolean success;
  double epsilon;
  double coefTemp;
  double nFailMax;

  *cost=costF(pr,TRUE,q_next,costData);
  prevCost=rrt->si[parent]->cost;
  
  epsilon=GetParameter(CT_EPSILON,pr);
  
  if ((prevCost+epsilon)>(*cost))
    return(TRUE);
  else
    {
      threshold=exp((prevCost-(*cost))/(rrt->temperature*deltaStep));
      coefTemp=GetParameter(CT_COEF_TEMP,pr);
      success=(randomDouble()<threshold);
      if(success)
        {
          rrt->nFail=0;     
          rrt->temperature=rrt->temperature/pow(coefTemp,((*cost)-prevCost)/10);
        }
      else 
        {
          rrt->nFail=rrt->nFail+1;
          nFailMax=GetParameter(CT_NFAIL_MAX,pr);
          if (rrt->nFail>nFailMax) 
            {
              rrt->temperature=rrt->temperature*coefTemp;
              rrt->nFail=0;             
            }
        }
      return(success);
    }
}

boolean AddBranchToRRT(Tparameters *pr,boolean ccRRT,
                       unsigned int tree,unsigned int i_near,
                       double *q_rand,double *goal,
                       unsigned int *lastSample,
                       double (*costF)(Tparameters*,boolean,double*,void*),
                       void *costData,
                       TRRTStatistics *rst,Trrt *rrt)
{
  boolean validSample,inDomain,inCollision,goalReached,distanceDecrease,tooFar,
          qRandReached,tooClose;
  unsigned int it;
  double d,d1,dp;
  #if (!EXPLORATION_RRT)
    double dg;
  #endif
    double *v,*q_near,*q_next,*q_tmp;
  unsigned int parent;
  double eps;
  boolean trans;
  double cost;

  if (ccRRT)
    eps=0;
  else
    eps=rrt->delta/50.0; /* for delta = 0.05 -> eps= 0.001 (value used in original cbirrt2) */

  q_near=rrt->si[i_near]->samples;

  NEW(v,rrt->m,double);
  NEW(q_next,rrt->m,double);
  NEW(q_tmp,rrt->m,double);
  memcpy(q_next,q_near,rrt->m*sizeof(double)); /* start from q_near */
  parent=i_near; /* inear will be parent for 1st sample if any */
  *lastSample=i_near;

  DifferenceVectorTopology(rrt->m,rrt->tp,q_rand,q_next,v); /* q_next=q_near */
  d1=Norm(rrt->m,v);
  if (d1>rrt->delta)
    ScaleVector(rrt->delta/d1,rrt->m,v);

  #if (_DEBUG>1)
    printf("  Distance q_rand-tree: %f (%.12f)\n",d1,rrt->delta);
  #endif

  goalReached=FALSE;
  validSample=TRUE;
  inDomain=TRUE;
  inCollision=FALSE;
  distanceDecrease=TRUE;
  tooFar=FALSE;
  tooClose=FALSE;
  qRandReached=FALSE;
  trans=TRUE;
  
  it=1;
  while((!goalReached)&&(validSample)&&(!qRandReached)&&(inDomain)&&(trans)&&
        (!inCollision)&&(distanceDecrease)&&(!tooFar)&&(!tooClose))
    {
      if (ccRRT)
        SumVectorScale(rrt->m,q_near,(double)it,v,q_next);
      else
        AccumulateVector(rrt->m,v,q_next);      

      if (rrt->n>0)
        validSample=(CS_WD_NEWTON_IN_SIMP(pr,q_next,rrt->w)!=DIVERGED);

      if (validSample)
        {
          /* Compute the distance to previous sample */
          dp=DistanceTopology(rrt->m,rrt->tp,rrt->si[parent]->samples,q_next);

          /* Distance to previous sample should not be too small */
          tooClose=(dp<eps); 
          if (!tooClose)
            {
              /* Distance to previous sample should not too large */
              tooFar=(dp>2*rrt->delta);
              if (!tooFar)
                {
                  /* Distance to q_rand should not increase */
                  d=d1;
                  d1=DistanceTopology(rrt->m,rrt->tp,q_rand,q_next);
                  distanceDecrease=(d1<d); /* new < previous */
          
                  if (distanceDecrease)
                    {
                      /* before checking the inDomain condition, we have to
                         move the ranges to [-pi,pi], if necessary*/
                      inDomain=((PointInBoxTopology(NULL,FALSE,rrt->m,q_next,0,rrt->tp,rrt->ambient))&&
                                (CS_WD_SIMP_INEQUALITIES_HOLD(pr,q_next,rrt->w)));
                      if (inDomain)
                        {
                          if(costF!=NULL) 
                            trans=TransitionTestRRT(pr,parent,q_next,dp,&cost,costF,costData,rrt);

                          if(trans)
                            {
                              CS_WD_IN_COLLISION(inCollision,pr,q_next,rrt->si[parent]->samples,rrt->w);
                              #if (GET_RRT_STATISTICS)
                                NewRRTCollisionCheck(rst);
                              #endif 
                              if (!inCollision)
                                {
                                  // fprintf(stderr," exp Step:  %f \n",dp);
                                  qRandReached=AddNodeToRRT(pr,tree,parent,q_next,q_rand,
                                                            lastSample,NULL,dp,cost,rst,rrt);
                                  #if (GET_RRT_STATISTICS)
                                    if (qRandReached)
                                      NewRRTQrandReached(rst);
                                  #endif 
                                
                                  /* currentNode will be parent for next iteration,
                                     if any */
                                  parent=*lastSample; 
                                
                                  #if (!EXPLORATION_RRT)
                                    dg=DistanceTopology(rrt->m,rrt->tp,q_next,goal);
                                    goalReached=(dg<rrt->delta);
                                  #endif

                                  if ((!ccRRT)&&(!goalReached))
                                    {
                                      DifferenceVectorTopology(rrt->m,rrt->tp,
                                                               q_rand,q_next,v);
                                      /* d1 computed above = 
                                         distance(q_rand_q_next)*/
                                      if (d1>rrt->delta)
                                        ScaleVector(rrt->delta/d1,rrt->m,v);
                                    }
                                }
                              #if ((RRT_VERBOSE)||(GET_RRT_STATISTICS))
                              else
                                {
                                  #if (GET_RRT_STATISTICS)
                                    NewRRTCollision(rst);
                                  #endif
                                  #if (RRT_VERBOSE)
                                    fprintf(stderr,"  Configuration in collision.\n");
                                  #endif
                                }
                              #endif
                            }
                          #if ((RRT_VERBOSE)||(GET_RRT_STATISTICS))
                          else
                            {
                              #if (GET_RRT_STATISTICS)
                                NewRRTHighCost(rst);
                              #endif
                              #if (RRT_VERBOSE)
                                fprintf(stderr,"  Configuration cost too high.\n");
                              #endif
                            }
                          #endif
                        }
                      #if ((RRT_VERBOSE)||(GET_RRT_STATISTICS))
                      else
                        {
                          #if (GET_RRT_STATISTICS)
                            NewRRTOutOfDomain(rst);
                          #endif
                          #if (RRT_VERBOSE)
                            fprintf(stderr,"  Configuration not in domain.\n");
                          #endif
                        }
                      #endif
                    }
                  #if ((RRT_VERBOSE)||(GET_RRT_STATISTICS))
                  else
                    {
                      #if (GET_RRT_STATISTICS)
                        NewRRTDistanceIncreases(rst);
                      #endif
                      #if (RRT_VERBOSE)
                        fprintf(stderr,"  Distance to q_rand increases (or stalled)\n");
                      #endif
                    }
                  #endif
                }
              #if ((RRT_VERBOSE)||(GET_RRT_STATISTICS))
              else
                {
                  #if (GET_RRT_STATISTICS)
                    NewRRTTooFar(rst);
                  #endif
                  #if (RRT_VERBOSE)
                    fprintf(stderr,"  Previous configuration is too far (too large advance step)\n");
                  #endif
                }
              #endif
            }
          #if ((RRT_VERBOSE)||(GET_RRT_STATISTICS))
          else
            {
              #if (GET_RRT_STATISTICS)
                NewRRTStalled(rst);
              #endif
              #if (RRT_VERBOSE)
                fprintf(stderr,"  Previous configuration is too close (small advance step)\n");
              #endif
            }
          #endif
        }
      #if ((RRT_VERBOSE)||(GET_RRT_STATISTICS))
      else
        {
          #if (GET_RRT_STATISTICS)
            NewRRTConvergenceError(rst);
          #endif
          #if (RRT_VERBOSE)
            fprintf(stderr,"  Sample does not converge to manifold.\n");
          #endif
        }
      #endif
      it++;
    }

  if (rrt->dd>0)
    AdjustDynamicDomain(i_near,inCollision,rrt);

  free(v);
  free(q_next);
  free(q_tmp);

  return(goalReached);
}


unsigned int ReconstructRRTPath(Tparameters *pr,unsigned int sID,double *pl,double *pc,
                                unsigned int *ns,double ***path,Trrt *rrt)
{
  Tvector steps;
  unsigned int nvs;

  RRTPathSteps(sID,&steps,rrt);

  nvs=Steps2PathinRRT(pr,&steps,pl,pc,ns,path,rrt);
  ReverseSamples(*ns,*path);

  DeleteVector(&steps);

  return(nvs);
}


unsigned int Steps2PathinRRT(Tparameters *pr,Tvector *steps,double *pl,double *pc,
                             unsigned int *ns,double ***path,Trrt *rrt)
{
  TRRTStep *st;
  unsigned int nv,nvs,ms,n,i,k,current,next;
  signed int j;
  double *o,c;
  boolean *systemVars;
  unsigned int lns;
  double **lpath;
  boolean reached,collision;
  double epsilon,dc;

  *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,rrt->w);
  nvs=0;
  for(i=0;i<nv;i++)
    {
      if (systemVars[i])
        nvs++;
    }

  InitSamples(&ms,ns,path);
      
  st=(TRRTStep *)GetVectorElement(0,steps);
  current=st->id;
  n=VectorSize(steps);
  k=1;
  while (k<n)
    {
      st=(TRRTStep *)GetVectorElement(k,steps);
      next=st->id;
      
      CS_WD_REGENERATE_ORIGINAL_POINT(pr,rrt->si[current]->samples,&o,rrt->w);
      AddSample2Samples(nv,o,nvs,systemVars,&ms,ns,path);
      free(o);
  
      c=ConnectSamples(pr,rrt->tp,rrt->ambient,rrt->m,rrt->n,
                       rrt->si[current]->samples,rrt->si[next]->samples,INF,
                       FALSE,&reached,&collision,NULL,&lns,&lpath,rrt->w);
      #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=rrt->si[current]->cost-rrt->si[next]->cost;
          (*pc)+=(dc>0?dc:0)+epsilon*c;
        }
            
      if (!reached) 
        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,rrt->w);
              AddSample2Samples(nv,o,nvs,systemVars,&ms,ns,path);
              free(o);
            }
          DeleteSamples(lns,lpath);
        }
        
      current=next;
      k++;
    }
  if (n>1)
    {
      /* Ensure that the last sample is also added (ConnectSamples stops close to the
         goal but not exactly on it) */
      CS_WD_REGENERATE_ORIGINAL_POINT(pr,rrt->si[current]->samples,&o,rrt->w);
      AddSample2Samples(nv,o,nvs,systemVars,&ms,ns,path);
      free(o);
    }
  
  free(systemVars);

  return(nvs);
}

double RRTPathLength(Tparameters *pr,unsigned int sID,Trrt *rrt)
{
  unsigned int i;
  double pl;

  pl=0;
  i=sID;
  while (i!=NO_UINT)
    {   
      if (rrt->si[i]->parent!=NO_UINT)
        pl+=DistanceTopology(rrt->m,rrt->tp,
                             rrt->si[i]->samples,rrt->si[rrt->si[i]->parent]->samples);
      i=rrt->si[i]->parent;
    }

  return(pl);
}

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

void InitRRT(Tparameters *pr,boolean parallel,boolean simp,double *ps,
             unsigned int mode,boolean graph,double *pg,
             unsigned int m,TAtlasBase *w,Trrt *rrt)
{
  double epsilon,er,*pWithDummies;
  unsigned int i;
  boolean collision=FALSE;
  unsigned int samplingMode;
  double dr;
  
  #if (EXPLORATION_RRT)
    if (mode!=ONE_TREE)
      Error("An EXPLORATION_RRT must be one directional");
  #endif

  /* Init the RRT structure */
  epsilon=GetParameter(CT_EPSILON,pr);
  samplingMode=(unsigned int)GetParameter(CT_SAMPLING,pr);
  dr=GetParameter(CT_DYNAMIC_DOMAIN,pr);

  /* Set up the possible parallel execution */
  rrt->parallel=FALSE; /* The default is not to execute in parallel */
  #ifdef _OPENMP
  if (parallel)
    {
      rrt->nCores=omp_get_max_threads();
      /* even if having many cores ans OpenMP we only execute in parallel
         large problems. */
      rrt->parallel=(rrt->nCores>1);
    }
  #endif
  //rrt->parallel=FALSE; 
  if (!rrt->parallel)
    rrt->nCores=1;

  fprintf(stderr,"Number of computing cores (rrt)   : %u\n",rrt->nCores);

  /* The base information with the equations, collions,... */
  rrt->w=w;

  /* one_tree two_trees, two_trees_with_swap */
  rrt->mode=mode;
  rrt->graph=graph;
  /* in kdtree sampling mode the dynamic domain is fixed
     by the kdtree, and not in the samples. */
  if (samplingMode==KDTREE_SAMPLING)
    rrt->dd=0.0;
  else
    rrt->dd=dr;

  /* only used for t-rrt */
  rrt->temperature=TEMPERATURE_INIT;
  rrt->nFail=0;

  /* Information for each sample */
  rrt->ms=INIT_NUM_SAMPLES_RRT;
  rrt->ns=0;
  NEW(rrt->si,rrt->ms,TRRTSampleInfo*);
  for(i=0;i<rrt->ms;i++)
    rrt->si[i]=NULL;

  /* Initialize the collision detection, just in case */
  CS_WD_INIT_CD(pr,rrt->nCores,rrt->w);

  /* Box enclosing the part of the manifold to explore */
  NEW(rrt->ambient,1,Tbox);
  CS_WD_GENERATE_SIMP_INITIAL_BOX(pr,rrt->ambient,rrt->w);

  NEW(rrt->si[0],1,TRRTSampleInfo);
  if (simp)
    {
      /* If the input point is already in simplified form, use
         it directly. This is not the usual case. */
      rrt->m=m;
      NEW(rrt->si[0]->samples,rrt->m,double);
      memcpy(rrt->si[0]->samples,ps,sizeof(double)*rrt->m);
      if (rrt->mode!=ONE_TREE)
        {
          /* In bi-directional trees the goal is also added */
          NEW(rrt->si[1],1,TRRTSampleInfo);
          NEW(rrt->si[1]->samples,rrt->m,double);
          memcpy(rrt->si[1]->samples,pg,sizeof(double)*rrt->m); 
        }
    }
  else
    {
      /* Simplify the input point and add it to the tree */
      CS_WD_REGENERATE_SOLUTION_POINT(pr,ps,&pWithDummies,rrt->w);
      rrt->m=CS_WD_GENERATE_SIMPLIFIED_POINT(pr,pWithDummies,
                                             &(rrt->si[0]->samples),rrt->w);
      free(pWithDummies);

      if (rrt->mode!=ONE_TREE)
        {
          /* In bi-directional trees the goal is also added */
          NEW(rrt->si[1],1,TRRTSampleInfo);
          CS_WD_REGENERATE_SOLUTION_POINT(pr,pg,&pWithDummies,rrt->w);
          CS_WD_GENERATE_SIMPLIFIED_POINT(pr,pWithDummies,
                                          &(rrt->si[1]->samples),rrt->w);
          free(pWithDummies);
        }
    }

  // rrt->m dimension of the ambient space
  rrt->k=(unsigned int)GetParameter(CT_N_DOF,pr); //dimension of the manifold
  rrt->n=rrt->m-rrt->k; //num of independent equations
  rrt->delta=GetParameter(CT_DELTA,pr);

  SetRRTTopology(pr,rrt);

  if ((!PointInBoxTopology(NULL,TRUE,rrt->m,rrt->si[0]->samples,0,rrt->tp,rrt->ambient))||
      (!CS_WD_SIMP_INEQUALITIES_HOLD(pr,rrt->si[0]->samples,rrt->w)))
    Error("The start point is not in domain");

  /* Verify the start/goal samples, now that they are already simplified
     and added to the RRT */
  if (rrt->n==0)
    er=0;
  else
    er=CS_WD_ERROR_IN_SIMP_EQUATIONS(pr,rrt->si[0]->samples,rrt->w);
  if (er>epsilon)
    Error("The start point is not on the manifold");

  CS_WD_IN_COLLISION(collision,pr,rrt->si[0]->samples,NULL,rrt->w);
  if (collision)
    Error("Start point for the RRT is in collision");

  rrt->si[0]->costp=0.0;
  rrt->si[0]->cost=0.0;
  rrt->si[0]->ddr=0.0;
  rrt->si[0]->tree=START2GOAL;
  rrt->si[0]->parent=NO_UINT;
  if (rrt->graph)
    {
      rrt->si[0]->g=0;
      rrt->si[0]->m=10; /* initial space for 10 neighbours (extended when necessary) */
      rrt->si[0]->nn=0;
      NEW(rrt->si[0]->n,rrt->si[0]->m,unsigned int);
      NEW(rrt->si[0]->cn,rrt->si[0]->m,double);
    }
  rrt->ns=1;
  
  if (rrt->mode!=ONE_TREE)
    {
      rrt->si[1]->costp=0.0;
      rrt->si[1]->cost=0.0;
      rrt->si[1]->ddr=0.0;
      rrt->si[1]->tree=GOAL2START;
      rrt->si[1]->parent=NO_UINT;
      if (rrt->graph)
        {
          rrt->si[1]->g=0;
          rrt->si[1]->m=10; /* initial space for 10 neighbours (extended when necessary) */
          rrt->si[1]->nn=0;
          NEW(rrt->si[1]->n,rrt->si[1]->m,unsigned int);
          NEW(rrt->si[1]->cn,rrt->si[1]->m,double);
        }
      rrt->ns=2;

      if ((!PointInBoxTopology(NULL,TRUE,rrt->m,rrt->si[1]->samples,0,rrt->tp,rrt->ambient))||
          (!CS_WD_SIMP_INEQUALITIES_HOLD(pr,rrt->si[1]->samples,rrt->w)))
        Error("The goal point is not in domain"); 

      if (rrt->n==0)
        er=0;
      else
        er=CS_WD_ERROR_IN_SIMP_EQUATIONS(pr,rrt->si[1]->samples,rrt->w);
      if (er>epsilon)
        Error("The goal point is not on the manifold");

      CS_WD_IN_COLLISION(collision,pr,rrt->si[1]->samples,NULL,rrt->w);
      if (collision)
        Error("Goal point for the RRT is in collision");
    }

  #if (_KDTREE==1)
  { 
    unsigned int nd,i;
    Trectangle rec;
    Tinterval *range;

    nd=GetBoxNIntervals(rrt->ambient);
    InitRectangle(nd,NULL,NULL,&rec);
    for(i=0;i<nd;i++)
      {
        range=GetBoxInterval(i,rrt->ambient);
        SetRectangleLimits(i,LowerLimit(range),UpperLimit(range),&rec);
      }
    i=0;
    rrt->treeS2G=InitKDTree(&rec,rrt->tp,25,dr,1,&(rrt->si[0]->samples),&i);

    if (rrt->mode!=ONE_TREE)
      {
        i=1;
        if (rrt->mode==TWO_TREES)
          rrt->treeG2S=InitKDTree(&rec,rrt->tp,25,dr,1,&(rrt->si[1]->samples),&i);
        else
          /* TWO_TREES_WITH_SWAP -> only one tree is used */
          AddPoint2KDtree(rrt->si[1]->samples,i,&(rrt->treeS2G));
      }
    else
      rrt->treeG2S=NULL;

    DeleteRectangle(&rec);
  }
  #endif

  #if (_KDTREE==2)
    /* Create the start 2 goal tree */
    if ((!RRT_NN_TOPOLOGY)||(rrt->tp==NULL))
      rrt->treeS2G=CreateKDTree(rrt->m); 
    else
      rrt->treeS2G=CreateKDTreeT(rrt->m,(int *)rrt->tp);
      
    AddPoint2KDTree(rrt->si[0]->samples,rrt->treeS2G);
    if (rrt->mode!=ONE_TREE)
      {
        /* and the goal to start, if necessary */
        if (rrt->mode==TWO_TREES)
          {
            if ((!RRT_NN_TOPOLOGY)||(rrt->tp==NULL))
              rrt->treeG2S=CreateKDTree(rrt->m); 
            else
              rrt->treeG2S=CreateKDTreeT(rrt->m,(int *)rrt->tp);

            AddPoint2KDTree(rrt->si[1]->samples,rrt->treeG2S);

            rrt->maxNodesT1=rrt->ms;
            rrt->maxNodesT2=rrt->ms;
            
            NEW(rrt->t1ToId,rrt->maxNodesT1,unsigned int);
            NEW(rrt->t2ToId,rrt->maxNodesT2,unsigned int);
            
            rrt->t1ToId[0]=0;
            rrt->t2ToId[0]=1;
            
            rrt->nodesT1=1;
            rrt->nodesT2=1;
          }
        else
          {
            /* TWO_TREES_WITH_SWAP -> only one kd-tree is used
               to speed up the search for nearest neighbours, 
               independent of the tree. Searches in a particular
               three, though, will be serial */
            AddPoint2KDTree(rrt->si[1]->samples,rrt->treeS2G);
          }
      }
    else
      {
        rrt->treeG2S=NULL;
        rrt->t1ToId=NULL;
        rrt->t2ToId=NULL;
      }
  #endif
}

double GetTRRTTemperature(Trrt *rrt)
{
  return(rrt->temperature);
}

boolean IsRRTGraph(Trrt *rrt)
{
  return(rrt->graph);
}

boolean RRTSample(unsigned int samplingMode,unsigned int tree,
                  double *goal,double *q_rand,
                  TRRTStatistics *rst,Trrt *rrt)
{
  boolean expand2Goal;

  #if (GET_RRT_STATISTICS)
    if (rst!=NULL)
      NewRRTRandomSample(rst);
  #endif

  expand2Goal=((goal!=NULL)&&(randomDouble()<0.01));
  if (expand2Goal)
    {
      memcpy(q_rand,goal,rrt->m*sizeof(double));
      #if (RRT_VERBOSE)
        fprintf(stderr,"  Expanding to goal----------------------------------\n");
      #endif
    }
  else
    {
      #if (_KDTREE==1)
      if (samplingMode==KDTREE_SAMPLING)
        {
          if ((rrt->mode==ONE_TREE)||(tree==START2GOAL)||
              ((tree==BOTHTREES)&&(randomDouble()<0.5)))
            SampleInKDtree(q_rand,rrt->treeS2G);
          else
            SampleInKDtree(q_rand,rrt->treeG2S);
        }
      else
      #endif
        /* samplingMode != KDTREE_SAMPLING  (AMBIENT or TANGENT) */
        RandomPointInBox(NULL,q_rand,rrt->ambient);
    }
  
  return(expand2Goal);
}

boolean RRTValidateSample(Tparameters *pr,double *q_rand,unsigned int tree,boolean expand2goal,
                          double *goal,double l,double *h,unsigned int *i_near,
                          TRRTStatistics *rst,Trrt *rrt)
{
  boolean validSample=TRUE;
 
  /* First check the optimality heuristics, if any */
  if ((expand2goal)||(goal==NULL))
    *h=0.0;
  else
    {
      *h=DistanceTopology(rrt->m,rrt->tp,q_rand,goal);
      if ((l<INF)&&((HEURISTIC_RRT_STAR)&(1)))
        validSample=((DistanceTopology(rrt->m,rrt->tp,rrt->si[0]->samples,q_rand)+*h)<=l);
    }

  /* Now check the domain */ 
  if (validSample)
    validSample=((expand2goal)||(CS_WD_SIMP_INEQUALITIES_HOLD(pr,q_rand,rrt->w)));

  /* Now the NN conditions (dynamic domain) */
  if (validSample)
    {
      *i_near=GetRRTNN(tree,q_rand,rrt);

      if (rrt->dd>0)
        validSample=InDynamicDomain(*i_near,q_rand,rrt);
    }

  #if (GET_RRT_STATISTICS)
    if (!validSample)
      NewRRTSampleRejection(rst);
  #endif
  
  return(validSample);
}

unsigned int GetRRTNN(unsigned int tree,double *q_rand,Trrt *rrt)
{  
  #if (_KDTREE==1)
  if ((rrt->mode!=TWO_TREES_WITH_SWAP)||(tree==BOTHTREES))
    {
      unsigned int inear;

      if (rrt->mode==TWO_TREES)
        {
          if (tree==START2GOAL)
            inear=NearestNeighbour(q_rand,rrt->treeS2G);
          else
            {
              if (tree==GOAL2START)
                inear=NearestNeighbour(q_rand,rrt->treeG2S);
              else
                {
                  /* BOTHTREES */
                  unsigned int inear1,inear2;
                  double d1,d2;

                  inear1=NearestNeighbour(q_rand,rrt->treeS2G);
                  inear2=NearestNeighbour(q_rand,rrt->treeG2S);

                  d1=SquaredDistanceTopology(rrt->m,rrt->tp,rrt->si[inear1]->samples,q_rand);
                  d2=SquaredDistanceTopology(rrt->m,rrt->tp,rrt->si[inear2]->samples,q_rand);
                  
                  inear=(d1<d2?inear1:inear2);
                }
            }
        }
      else
        /* rrt->mode==ONE_TREE or (rrt->mode==TWO_TREES_WITH_SWAP and tree==BOTHTREES) */
        inear=NearestNeighbour(q_rand,rrt->treeS2G);
        
      return(inear);
    }
  else
  #endif

  #if (_KDTREE==2)
  if ((rrt->mode!=TWO_TREES_WITH_SWAP)||(tree==BOTHTREES))
    {
      double d;
      signed int inear;

      if (rrt->mode==TWO_TREES)
        {
          if (tree==START2GOAL)
            {
              QueryKDTree(q_rand,&inear,&d,rrt->treeS2G);
              inear=rrt->t1ToId[inear];
            }
          else
            {
              if (tree==GOAL2START)
                {
                  QueryKDTree(q_rand,&inear,&d,rrt->treeG2S);
                  inear=rrt->t2ToId[inear];
                }
              else
                {
                  /* NN in any of the two trees */
                  double d1,d2;
                  signed int inear1,inear2;

                  QueryKDTree(q_rand,&inear1,&d1,rrt->treeG2S);
                  QueryKDTree(q_rand,&inear2,&d2,rrt->treeS2G);

                  if (d1<d2)
                    inear=rrt->t1ToId[inear1];
                  else
                    inear=rrt->t2ToId[inear2];
                }
            }
        }
      else
        /* rrt->mode==ONE_TREE or (rrt->mode==TWO_TREES_WITH_SWAP and tree==BOTHTREES) */
        QueryKDTree(q_rand,&inear,&d,rrt->treeS2G);
        
      return((unsigned int)inear);
    }
  else
  #endif
    {
      unsigned int j,inear;
      double d,d1;
 
      /* _KDTREE==0 or (BOTH_TREES_WITH_SWAP and tree!=BOTH_TREES) */ 
      d=INF;
      inear=NO_UINT;
      for(j=0;j<rrt->ns;j++)
        {
          if ((rrt->mode==ONE_TREE)||(rrt->si[j]->tree&tree))
            {
              d1=DistanceTopologyMin(d,rrt->m,rrt->tp,q_rand,rrt->si[j]->samples);
              if (d1<d)
                {
                  d=d1;
                  inear=j;
                }
            }
        }
      return(inear);
    }
}

void GetRRTNNInBall(unsigned int tree,double *q_rand,double r,
                    unsigned int *nn,unsigned int **n,Trrt *rrt)
{
  #if (_KDTREE==1)
  if ((rrt->mode!=TWO_TREES_WITH_SWAP)||(tree==BOTHTREES))
    {
      if (rrt->mode==TWO_TREES)
        {
          if (tree==START2GOAL)
            NeighboursInBall(q_rand,r,nn,n,rrt->treeS2G);
          else
            {
              if (tree==GOAL2START)
                NeighboursInBall(q_rand,r,nn,n,rrt->treeG2S);
              else
                {
                  unsigned int i;
                  unsigned int nn1,nn2;
                  unsigned int *n1,*n2;

                  NeighboursInBall(q_rand,r,&nn1,&n1,rrt->treeS2G);
                  NeighboursInBall(q_rand,r,&nn2,&n2,rrt->treeG2S);

                  *nn=nn1+nn2;
                  NEW(*n,*nn,unsigned int);
                  for(i=0;i<nn1;i++)
                    (*n)[i]=n1[i];

                  for(i=0;i<nn2;i++)
                    (*n)[nn1+i]=n2[i];
                }
            }
        }
      else
        /* rrt->mode==ONE_TREE or (rrt->mode==TWO_TREES_WITH_SWAP and tree==BOTHTREES) */
        NeighboursInBall(q_rand,r,nn,n,rrt->treeS2G);
    }
  else
  #endif
  #if (_KDTREE==2)
  if ((rrt->mode!=TWO_TREES_WITH_SWAP)||(tree==BOTHTREES))
    {
      double *dst;

      if (rrt->mode==TWO_TREES)
        {
          unsigned int i;

          if (tree==START2GOAL)
            {
              QueryKDTreeR(q_rand,r,(int *)(nn),(int **)(n),&dst,rrt->treeS2G);
              free(dst);

              for(i=0;i<*nn;i++)
                (*n)[i]=rrt->t1ToId[(*n)[i]];
            }
          else
            {
              if (tree==GOAL2START)
                {
                  QueryKDTreeR(q_rand,r,(int *)(nn),(int **)(n),&dst,rrt->treeG2S);
                  free(dst);

                  for(i=0;i<*nn;i++)
                    (*n)[i]=rrt->t2ToId[(*n)[i]];
                }
              else
                {
                  /* Both trees */
                  unsigned int nn1,nn2;
                  unsigned int *n1,*n2;
                
                  QueryKDTreeR(q_rand,r,(int *)(&nn1),(int **)(&n1),&dst,rrt->treeS2G);
                  free(dst);
                  QueryKDTreeR(q_rand,r,(int *)(&nn2),(int **)(&n2),&dst,rrt->treeG2S);
                  free(dst);
                
                  *nn=nn1+nn2;
                  NEW(*n,*nn,unsigned int);
                  for(i=0;i<nn1;i++)
                    (*n)[i]=rrt->t1ToId[n1[i]];

                  for(i=0;i<nn2;i++)
                    (*n)[nn1+i]=rrt->t2ToId[n2[i]];
                }
            }    
        }
      else
        {
          /* rrt->mode==ONE_TREE or (rrt->mode==TWO_TREES_WITH_SWAP and tree==BOTHTREES) */
          QueryKDTreeR(q_rand,r,(int *)(nn),(int **)(n),&dst,rrt->treeS2G);
          free(dst);
        }
    }
  else
  #endif
    {
      unsigned int m,j;
      double d1;
            
      /* _KDTREE==0 or BOTH_TREES_WITH_SWAP and tree!=BOTH_TREES */ 
      m=10;
      *nn=0;
      NEW(*n,m,unsigned int); 
      for(j=0;j<rrt->ns;j++)
        {
          if ((rrt->mode==ONE_TREE)||(rrt->si[j]->tree&tree))
            {
              d1=DistanceTopologyMin(r,rrt->m,rrt->tp,q_rand,rrt->si[j]->samples);
              if (d1<r)
                {
                  if (*nn==m)
                    MEM_DUP(*n,m,unsigned int);
                  (*n)[*nn]=j;
                  (*nn)++;
                }
            }
        }  
    }
}

void GetRRTNNInBranch(unsigned int tree,
                      unsigned int n1,unsigned int n2,
                      unsigned int *n,unsigned int *nn,
                      Trrt *rrt)
{
  double md,cd;
  unsigned int cn;
  int cnn;
  boolean done;

  /* Conceptually this operation is used only in bi-directional trees (altough in practice
     it could be implemented in single-trees too */
  if (rrt->mode==ONE_TREE)
    Error("GetRRTNNInBranch is only defined for bidirectional RRTs");

  if (rrt->si[n1]->tree!=rrt->si[n2]->tree)
    Error("The two nodes defining the branch in GetRRTNNInBranch are not in the same tree");

  if (tree==rrt->si[n1]->tree)
    Error("Parameter missmatch in GetRRTNNInBranch (the branch is in the query tree)");

  if (tree==BOTHTREES)
    Error("Can not query both trees in GetRRTNNInBranch");

  cn=n2;
  md=INF;
  *n=NO_UINT;
  *nn=NO_UINT;
  done=FALSE;
  do {
    #if (_KDTREE==1)
    if (rrt->mode!=TWO_TREES_WITH_SWAP)
      {
        if (tree==START2GOAL)
          cnn=NearestNeighbour(rrt->si[cn]->samples,rrt->treeS2G);
        else
          /* tree==GOAL2START */
          cnn=NearestNeighbour(rrt->si[cn]->samples,rrt->treeG2S);
      }
    else
    #endif

    #if (_KDTREE==2)
    if (rrt->mode!=TWO_TREES_WITH_SWAP)
      {
        if (tree==START2GOAL)
          {
            QueryKDTree(rrt->si[cn]->samples,&cnn,&cd,rrt->treeS2G);
            cnn=rrt->t1ToId[cnn];
          }
        else
          {
            /* tree==GOAL2START */
            QueryKDTree(rrt->si[cn]->samples,&cnn,&cd,rrt->treeG2S);
            cnn=rrt->t2ToId[cnn];
          }
      }
    else
    #endif
      {
        unsigned int j;
        double d1;

        cd=INF;
        cnn=NO_UINT;
        for(j=0;j<rrt->ns;j++)
          {
            if (rrt->si[j]->tree==tree)
              {
                d1=DistanceTopologyMin(md,rrt->m,rrt->tp,rrt->si[cn]->samples,rrt->si[j]->samples);
                if (d1<cd)
                  {
                    cd=d1;
                    cnn=j;
                  }
              }
          }
      }

    if (cd<md)
      {
        md=cd;
        *n=cn;
        *nn=cnn;
      }
    done=(cn==n1);
    if (!done)
      cn=rrt->si[cn]->parent;
  } while((!done)||(cn==NO_UINT));
  
  if (cn==NO_UINT)
    Error("Wrongly delimited branch in GetRRTNNInBranch");
}

boolean AddNodeToRRT(Tparameters *pr,unsigned int tree,
                     unsigned int i_near,double *sample,
                     double *goal,unsigned int *lastSample,
                     void *info,double costp,double cost,
                     TRRTStatistics *rst,Trrt *rrt)
{
  TRRTSampleInfo *si;

  #if (GET_RRT_STATISTICS)
    if (rst!=NULL)
      NewRRTSample(rst);
  #endif

  if (tree==BOTHTREES)
    Error("Samples must be assigned to a single tree in AddNodeToRRT");

  if (rrt->ns==rrt->ms)
    MEM_DUP(rrt->si,rrt->ms,TRRTSampleInfo *);

  NEW(rrt->si[rrt->ns],1,TRRTSampleInfo);
  si=rrt->si[rrt->ns];

  NEW(si->samples,rrt->m,double);
  memcpy(si->samples,sample,rrt->m*sizeof(double));
  if (rrt->tp!=NULL)
    ArrayPi2Pi(rrt->m,rrt->tp,si->samples);

  si->parent=i_near;
  si->userInfo=info;
  si->costp=costp;
  si->cost=cost;

  si->ddr=0.0; /* dynamic domain set to 0 -> infty. */

  /* the tree of the node can be different from that of its parent!!
     This can happen when loading a BiRRT* that is not fully updated
     (see \ref UpdateCostAndPath) */
  si->tree=tree;

  #if (_KDTREE==1)
    if (rrt->mode==TWO_TREES)
      {
        if (tree==START2GOAL)
          AddPoint2KDtree(si->samples,rrt->ns,&(rrt->treeS2G));
        else
          AddPoint2KDtree(si->samples,rrt->ns,&(rrt->treeG2S));
      }
    else
      AddPoint2KDtree(si->samples,rrt->ns,&(rrt->treeS2G));
  #endif
  
  #if (_KDTREE==2)
    if (rrt->mode==TWO_TREES)
      {
        if (tree==START2GOAL)
          { 
            if (rrt->nodesT1==rrt->maxNodesT1)
              MEM_DUP(rrt->t1ToId,rrt->maxNodesT1,unsigned int);
            rrt->t1ToId[rrt->nodesT1]=rrt->ns;
            rrt->nodesT1++;
            
            AddPoint2KDTree(si->samples,rrt->treeS2G);
          }
        else
          {
            /* tree is GOAL2START */
            if (rrt->nodesT2==rrt->maxNodesT2)
              MEM_DUP(rrt->t2ToId,rrt->maxNodesT2,unsigned int);
            rrt->t2ToId[rrt->nodesT2]=rrt->ns;
            rrt->nodesT2++;
            
            AddPoint2KDTree(si->samples,rrt->treeG2S);
          }
      }
    else
      AddPoint2KDTree(si->samples,rrt->treeS2G);
  #endif

  *lastSample=rrt->ns;
  rrt->ns++;

  if (rrt->graph)
    {
      si->g=INF;
      si->m=10; /* initial space for 10 neighbours (extended when necessary) */
      si->nn=0;
      NEW(si->n,si->m,unsigned int);
      NEW(si->cn,si->m,double);
      AddEdgeToRRT(*lastSample,i_near,costp,rrt);
    }

  #if (_DEBUG>1)
    fprintf(stderr,"  New sample %u dp:%f\n",rrt->ns,
            DistanceTopology(rrt->m,rrt->tp,rrt->si[i_near]->samples,sample));
  #endif

  if (goal!=NULL)
    return(DistanceTopology(rrt->m,rrt->tp,sample,goal)<GetParameter(CT_EPSILON,pr)); 
  else
    return(FALSE);
}

unsigned int AddStepToRRTstar(Tparameters *pr,unsigned int i_near,
                              double *q_rand,double *goal,
                              unsigned int *i_goal,
                              TRRTStatistics *rst,Trrt *rrt)
{
  double *q_next,d;
  unsigned int i_next;
  boolean goalFound,collision,reached;
  unsigned int tree;

  NEW(q_next,rrt->m,double);

  d=ConnectSamples(pr,rrt->tp,rrt->ambient,
                   rrt->m,rrt->n,rrt->si[i_near]->samples,q_rand,(double)INF,
                   TRUE,&reached,&collision,q_next,NULL,NULL,rrt->w);

  if (rrt->dd>0)
    AdjustDynamicDomain(i_near,collision,rrt);

  i_next=NO_UINT;

  tree=rrt->si[i_near]->tree; /* the sample is assigned to the tree of the nearest-node */

  if (d>0)
    {
      goalFound=AddNodeToRRT(pr,tree,i_near,q_next,goal,&i_next,NULL,
                             d,rrt->si[i_near]->cost+d,rst,rrt);
      
      if ((goal!=NULL)&&(i_goal!=NULL)&&(goalFound)&&(*i_goal==NO_UINT))
        {
          /* The first time we are close enough to the goal we add the goal as a
             node in the RRT */
          double d,epsilon;

          epsilon=GetParameter(CT_EPSILON,pr);
          d=DistanceTopology(rrt->m,rrt->tp,q_next,goal);

          if (d<epsilon)
            *i_goal=i_next;
          else
            AddNodeToRRT(pr,tree,i_next,goal,goal,i_goal,NULL,d,
                         rrt->si[i_next]->cost+d,rst,rrt);
          #if (RRT_VERBOSE)
            fprintf(stderr,"     {%u->%u}[%g] **\n",*i_goal,rrt->si[*i_goal]->parent,
                    rrt->si[*i_goal]->costp);
          #endif
        }
    }
  free(q_next);

  return(i_next);
}

void WireRRTstar(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,
                 TRRTStatistics *rst,Trrt *rrt)
{
  unsigned int i;
  double ic,cn,c_new,epsilon,*q_new;
  boolean *reached;
  TRRTSampleInfo *sNew;
  TDoublePair pair;
  double *cost;
  
  epsilon=GetParameter(CT_EPSILON,pr);

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

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

  #pragma omp parallel for private(i) schedule(dynamic) if (rrt->parallel)
  for(i=0;i<nn;i++)
    { 
      /* do not consider connection to itself */
      if (n[i]==id_new)
        reached[i]=FALSE;
      else
        {
          if (n[i]==i_near)
            { 
              /* we already know the connection to i_near */
              cost[i]=ic;
              reached[i]=TRUE;
            }
          else
            {
              boolean collision;

              /* compute the connection to other samples */
              cost[i]=ConnectSamples(pr,rrt->tp,rrt->ambient,
                                     rrt->m,rrt->n,rrt->si[n[i]]->samples,q_new,gamma,
                                     TRUE,&(reached[i]),&collision,NULL,NULL,NULL,rrt->w);
            }
        }
    }

  for(i=0;i<nn;i++)
    {
      if (n[i]!=id_new)
        {
          #if (GET_RRT_STATISTICS)
            NewRRTBranch(rst);
          #endif
          (*t)|=rrt->si[n[i]]->tree; /* take note of the tree of this neighbour (even if not reached)*/
        }

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

          /* Ensure that cost and tree are correct before using this node */
          /* This might be unnecessary when in graph mode (but it's cheap)  */
          #if (GET_RRT_STATISTICS)
            NewRRTNoEmptyBranch(rst);
          #endif

          #if (!RRTSTAR_UPDATE_COSTS)
          if (rrt->mode==TWO_TREES_WITH_SWAP)
          #endif
            UpdateCostAndTree(n[i],rrt); 

          /* Add a bidirectional edge with the given cost. */
          if (rrt->graph)
            AddEdgeToRRT(id_new,n[i],cn,rrt);
          
          c_new=rrt->si[n[i]]->cost+cn;
          if (c_new<(sNew->cost-epsilon))
            {
              #if (RRT_VERBOSE)
                fprintf(stderr,"     {%u->%u}[%g] ->",id_new,sNew->parent,sNew->costp);
              #endif

              SetRRTCostAndParent(id_new,n[i],cn,c_new,rrt);

              #if (RRT_VERBOSE)
                fprintf(stderr,"{%u->%u}[%g]\n",id_new,sNew->parent,sNew->costp);
              #endif
            }
        }
      else
        cn=INF;

      #if (RRTSTAR_SYMMETRIC_COST)
        if (!rrt->graph) /* if graph, the cost are already stored in the graph edges. */
          (*c)[i]=cn;
      #endif
    }
  free(reached);
  free(cost);

  if (rrt->graph)
    {
      if (!q)
        Error("RRT must be in graph mode to use a heap");
      c_new=rrt->si[id_new]->cost;
      NewDoublePair(c_new+h,c_new,&pair);
      AddElement2Heap(id_new,(void *)&pair,q);
    }
}

void RecursiveReWireRRTstar(Tparameters *pr,Theap *q,double *g,
                            Tvector *steps,double *l,
                            Trrt *rrt)
{
  unsigned int id_new,i;
  double c_new,epsilon,ct;
  TRRTSampleInfo *sNew,*sNear;
  TDoublePair pair;

  if (!rrt->graph)
    Error("RecursiveReWireRRTstar can only be used if RRT in graph mode");

  epsilon=GetParameter(CT_EPSILON,pr);

  #if (RRT_VERBOSE)
    fprintf(stderr,"  Rewire\n");
  #endif
  while ((!HeapEmpty(q))&&(FirstInPair((TDoublePair *)GetHeapElement(0,q))<=(*l)))
    {
      id_new=ExtractMinElement(&pair,q);
      if (id_new==NO_UINT)
        Error("Reading an inconsistent node from the priority queue in RecursiveReWireRRTstar");

      sNew=rrt->si[id_new];
      sNew->g=sNew->cost; /* convert the node to stationary */

      for(i=0;i<sNew->nn;i++)
        {
          sNear=rrt->si[sNew->n[i]];

          /* ensure that cost and tree are correct before using this node. This is already updated
             in the wire but might change during the re-wire */
          if (rrt->mode==TWO_TREES_WITH_SWAP)
            UpdateCostAndTree(sNew->n[i],rrt);
          
          /* cost to n[i] via id_new */
          c_new=sNew->cost+sNew->cn[i];
          if (c_new<(sNear->cost-epsilon)) /* recall that here, cost = rhs */
            {
              #if (RRT_VERBOSE)
                fprintf(stderr,"     {%u->%u}[%g %g] ->",sNew->n[i],sNear->parent,sNear->costp,sNear->cost);
              #endif

              /* sets 'rhs' (cost) but not 'g' -> inconsistent node */
              SetRRTCostAndParent(sNew->n[i],id_new,sNew->cn[i],c_new,rrt);
              
              ct=sNear->cost+DistanceTopology(rrt->m,rrt->tp,sNear->samples,g);
              NewDoublePair(ct,sNear->cost,&pair);
              UpdateHeapElement(sNew->n[i],(void *)&pair,q); /* updates/adds */

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

void ReWireRRTstar(Tparameters *pr,
                   unsigned int id_new,double gamma,
                   unsigned int nn,unsigned int *n,double *c,
                   Tvector *steps,double *l,
                   Trrt *rrt)
{
  unsigned int i,parent;
  boolean reached;
  double cn,c_new,epsilon;
  TRRTSampleInfo *sNew,*sNear;
  #if (!RRTSTAR_SYMMETRIC_COST)
    double *q_new;
    boolean collision;
  #endif

  if (rrt->graph)
    Error("ReWireRRTstar must not be used if the RRT is in graph  mode");

  epsilon=GetParameter(CT_EPSILON,pr);

  #if (RRT_VERBOSE)
    fprintf(stderr,"  Rewire\n");
  #endif
  sNew=rrt->si[id_new];
  #if (!RRTSTAR_SYMMETRIC_COST)
    /* if symmetric costs -> cost to neighbours given as parameters */
    q_new=sNew->samples;
  #endif
  parent=sNew->parent;
  for(i=0;i<nn;i++)
    {
      sNear=rrt->si[n[i]];

      /* There is no need to re-wire the best parent of the new node
         (it will produce an inf. loop) or the new node itself. */
      if ((n[i]==id_new)||(n[i]==parent))
        reached=FALSE;
      else
        {
          #if (RRTSTAR_SYMMETRIC_COST)
            cn=c[i];
            reached=(cn<INF);
          #else
            cn=ConnectSamples(pr,rrt->tp,rrt->ambient,
                              rrt->m,rrt->n,q_new,sNear->samples,gamma,
                              TRUE,&reached,&collision,NULL,NULL,NULL,rrt->w);
          #endif
        }

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

          c_new=sNew->cost+cn;    
          if (c_new<sNear->cost-epsilon)
            {
              #if (RRT_VERBOSE)
                fprintf(stderr,"     {%u->%u}[%g] ->",n[i],sNear->parent,sNear->costp);
              #endif

              SetRRTCostAndParent(n[i],id_new,cn,c_new,rrt);

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

void RRTstarCloseIteration(unsigned int it,unsigned int id_goal,
                           double time,double gamma,
                           double *times,double *costs,
                           Trrt *rrt)
{
  if ((RRT_VERBOSE)||((it%1000)==0))
    {
      if (id_goal!=NO_UINT)
        {
          fprintf(stderr,"Iteration: %u (s:%u t:%g g:%g c:%g [%g])\n",it,rrt->ns,time,gamma,
                  rrt->si[id_goal]->cost,CostToRoot(id_goal,rrt));
        }
      else
        fprintf(stderr,"Iteration: %u (s:%u t:%g)\n",it,rrt->ns,time);
      fflush(stdout);
    }

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

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

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

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

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

boolean RRTstar(Tparameters *pr,double *pg,
                unsigned int *it,double *times,double *costs,
                double *planningTime,double *pl,unsigned int *ns,double ***path,
                TRRTStatistics *grst,Trrt *rrt)
{
  if (EXPLORATION_RRT)
    Error("RRTstar is not designed for exploration RRTs");

  if (rrt->mode!=ONE_TREE)
    return(BiRRTstar(pr,pg,it,times,costs,planningTime,pl,ns,path,grst,rrt));
  else
    {
      double *pWithDummies,*pgs,*q_rand;
      unsigned int samplingMode;
      unsigned int i_near;
      double epsilon;
      boolean expand2Goal;
      double er;
      Tstatistics st; /* used just to measure execution time */
      boolean collision;
      double time;
      double *c; /* if the cost function is symmetrical, we cache
                    the evaluation of the cost to neighbour. */
      /* neighbours for each new node */
      unsigned int nn;
      unsigned int *n;
      /* distance q_rand,tree */
      double gamma,ct_gamma;
      unsigned int i,id_new,id_goal;
      double *q_new;
      boolean done,optimal,validSample;
      double maxTime;
      unsigned int maxIterations;
      double h,l;
      Theap q;
      unsigned int vt; /* trees in  set of neighbours, only relevant in bi-RRTs */
      unsigned int numSamples;
      TRRTStatistics *rst;
    
      if (rrt->ns>1)
        Error("RRTstart must be used to a just initilized RRT");

      #if (GET_RRT_STATISTICS)
      {
        unsigned int i;
    
        NEW(rst,1,TRRTStatistics);
        InitRRTStatistics(rst);
    
        for(i=0;i<rrt->ns;i++)
          NewRRTSample(rst);
      }
      #endif

      /* Init the RRT structure */
      epsilon=GetParameter(CT_EPSILON,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);
      

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

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

      if (rrt->n==0)
        er=0;
      else
        er=CS_WD_ERROR_IN_SIMP_EQUATIONS(pr,pgs,rrt->w);
      if (er>epsilon)
        Error("The target point for the RRTstar is not on the manifold");

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

      /* Room for the randomly generated samples (and its correspoinding point
         on the manifold) */
      NEW(q_rand,rrt->m,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 (rrt->graph)
        InitHeap(sizeof(TDoublePair),CopyDoublePair,DeleteDoublePair,
                 LessThanDoublePair,NULL,TRUE,rrt->ms,&q);

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

      id_goal=NO_UINT;
      *it=0;

      InitStatistics(rrt->nCores,0,&st);
      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,rrt);
          #endif
          /*******************************************************************/
          /* best estimation of the cost to goal (up to now) */
          if (id_goal==NO_UINT)
            l=INF;
          else
            l=rrt->si[id_goal]->cost;

          /*******************************************************************/
          /* Random sample */
          do {
            /* Generate a random sample */
            expand2Goal=RRTSample(samplingMode,START2GOAL,(id_goal==NO_UINT?pgs:NULL),
                                  q_rand,rst,rrt);
      
            /* Check if the sample is valid */
            validSample=RRTValidateSample(pr,q_rand,START2GOAL,expand2Goal,pgs,l,
                                          &h,&i_near,rst,rrt);
            time=GetElapsedTime(&st);
          } while((!validSample)&&(time<maxTime));

          if (time<maxTime)
            {
              /*******************************************************************/
              /* Extend the RRT from q_near to q_rand using the cbiRRT extend method */
              /* Note that here we try to reach q_rand but we do not add samples along the path */
              id_new=AddStepToRRTstar(pr,i_near,q_rand,pgs,&id_goal,rst,rrt);

              /*******************************************************************/
              if (!optimal)
                {
                  if (id_goal!=NO_UINT)
                    done=TRUE;
                }
              else
                {
                  /* If we actually added something */
                  if ((id_new!=NO_UINT)&&((((HEURISTIC_RRT_STAR)&(2))==0)||(id_goal!=NO_UINT)))
                    { 
                      /* q_new is the last sample added to the tree */
                      q_new=rrt->si[id_new]->samples;
                      h=DistanceTopology(rrt->m,rrt->tp,q_new,pgs);
          
                      /*******************************************************************/
                      /* Search for the set of potential neighbours */
                      numSamples=(rrt->ns<3?3:rrt->ns);
                      gamma=ct_gamma*pow(log(numSamples)/numSamples,1/(double)rrt->k);
                      gamma=(gamma<rrt->delta?rrt->delta:gamma);

                      GetRRTNNInBall(START2GOAL,q_new,gamma,&nn,&n,rrt);
       
                      /*******************************************************************/
                      /* Wire: Search for the lowest cost path to the new node */
                      WireRRTstar(pr,id_new,i_near,gamma,nn,n,&c,h,(rrt->graph?&q:NULL),&vt,rst,rrt);
                  
                      /*******************************************************************/
                      /* Rewire the tree around the new point */
                      if (rrt->graph)
                        RecursiveReWireRRTstar(pr,&q,pgs,NULL,&l,rrt);
                      else
                        ReWireRRTstar(pr,id_new,gamma,nn,n,c,NULL,&l,rrt);
                  
                      /*******************************************************************/
                      /* release the data (cost (if actually used) and neighbours) */   
                      if (c!=NULL)
                        free(c);
                      free(n);
                    }
                  #if (RRT_VERBOSE)
                  else
                    {
                      if (id_new==NO_UINT)
                        fprintf(stderr,"  Blocked [%u]\n",i_near);
                    }
                  #endif
                }

              /*******************************************************************/
              /* Conclude the iteration */
              time=GetElapsedTime(&st);
      
              RRTstarCloseIteration(*it,id_goal,time,gamma,times,costs,rrt);
      
              (*it)++;
            }
        }

      DeleteStatistics(&st);
      #if (GET_RRT_STATISTICS)
        if (grst==NULL)
          PrintRRTStatistics(rrt,rst);
        else
          AccumulateRRTStatistics(rst,grst);
        DeleteRRTStatistics(rst);
        free(rst);
      #endif

      if (rrt->graph)
        DeleteHeap(&q);

      *planningTime=GetElapsedTime(&st);

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

      /* Reconstruct the path (if any) */
      if (id_goal!=NO_UINT) 
        ReconstructRRTPath(pr,id_goal,pl,NULL,ns,path,rrt);

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

      return(id_goal!=NO_UINT);
    }
}

boolean BiRRTstar(Tparameters *pr,double *pg,
                  unsigned int *it,double *times,double *costs,
                  double *planningTime,double *pl,unsigned int *ns,double ***path,
                  TRRTStatistics *grst,Trrt *rrt)
{
  if (EXPLORATION_RRT)
    Error("RRTstar is not designed for exploration RRTs");

  if (rrt->mode!=TWO_TREES_WITH_SWAP)
    {
      if (rrt->mode!=ONE_TREE)
        Error("RRTstar operates on two-directional RRTs");
      
      return(RRTstar(pr,pg,it,times,costs,planningTime,pl,ns,path,grst,rrt));
    }
  else
    {
      double *q_rand,*g;
      unsigned int i_near;
      Tstatistics st; /* used just to measure execution time */
      double time;
      double *c; /* if the cost function is symmetrical, we cache
                    the evaluation of the cost to neighbour. */
      /* neighbours for each new node */
      unsigned int nn;
      unsigned int *n;
      /* distance q_rand,tree */
      double gamma,ct_gamma;
      unsigned int i,id_new;
      double *q_new;
      boolean done,optimal,validSample;
      double l;           /* length of the shortest path so far */
      Tvector steps;
      double maxTime;
      unsigned int maxIterations,samplingMode;
      Theap q;
      double h;
      unsigned int vt; /* trees in the set of neighbours. */
      unsigned int numSamples;
      double l1,c12;
      boolean connected,collision;
      TRRTStatistics *rst;

      if (rrt->ns>2)
        Error("RRTstart must be used to a just initilized RRT");
      
      #if (GET_RRT_STATISTICS)
      {
        unsigned int i;
        
        NEW(rst,1,TRRTStatistics);
        InitRRTStatistics(rst);
        
        for(i=0;i<rrt->ns;i++)
          NewRRTSample(rst);
      }
      #endif

      /* Init the RRT structure */
      ct_gamma=GetParameter(CT_GAMMA,pr);
      maxTime=GetParameter(CT_MAX_PLANNING_TIME,pr);    
      maxIterations=(unsigned int)GetParameter(CT_MAX_PLANNING_ITERATIONS,pr);
      /* for BiRRTStart the sampling mode should be "ambient_sampling" but we accept
         other modes just for the sake of completeness. */
      samplingMode=(unsigned int)GetParameter(CT_SAMPLING,pr);

      /* Room for the randomly generated samples (and its correspoinding point
         on the manifold) */
      NEW(q_rand,rrt->m,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 (rrt->graph)
        InitHeap(sizeof(TDoublePair),CopyDoublePair,DeleteDoublePair,
                 LessThanDoublePair,NULL,TRUE,rrt->ms,&q);

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

      *it=0;

      InitStatistics(rrt->nCores,0,&st);
      time=0.0;
      done=FALSE;
  
      InitVector(sizeof(TRRTStep),CopyRRTStep,DeleteRRTStep,100,&steps);
      l=INF;
      c=NULL; /* default not to use cache of cost to neighbours */

      while ((!done)&&(time<maxTime)&&(*it<maxIterations))
        {
          /* Keep the current path up to date (if any) */
          if (l<INF)
            l=UpdateBiRRTSteps(&steps,rrt);

          /*******************************************************************/
          do {
            /* Generate a random sample */
            RRTSample(samplingMode,BOTHTREES,NULL,q_rand,rst,rrt);
        
            /* Check if the sample is valid */  
            validSample=RRTValidateSample(pr,q_rand,BOTHTREES,FALSE,rrt->si[1]->samples,l,
                                          &h,&i_near,rst,rrt);
            time=GetElapsedTime(&st);
          } while((!validSample)&&(time<maxTime));

          if (time<maxTime)
            {
              /*******************************************************************/
              /* heuristic w.r.t distance from q_rand to goal */
              if (rrt->si[i_near]->tree==START2GOAL)
                g=rrt->si[1]->samples;
              else
                g=rrt->si[0]->samples;
              h=DistanceTopology(rrt->m,rrt->tp,q_rand,g);

              /*******************************************************************/
              /* Extend the RRT 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=AddStepToRRTstar(pr,i_near,q_rand,NULL,NULL,rst,rrt);

              /* If we actually added something */
              if ((id_new!=NO_UINT)&&((((HEURISTIC_RRT_STAR)&(2))==0)||(l<INF)))
                { 
                  /* q_new is the last samples added to the tree */
                  q_new=rrt->si[id_new]->samples;
                  h=DistanceTopology(rrt->m,rrt->tp,q_new,g);

                  /*******************************************************************/
                  /* Search for the set of potential neighbours */
                  numSamples=(rrt->ns<3?3:rrt->ns);
                  gamma=ct_gamma*pow(log(numSamples)/numSamples,1/(double)rrt->k);
                  gamma=(gamma<rrt->delta?rrt->delta:gamma);

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

                  /*******************************************************************/
                  /* Wire: Search for the lowest cost path to the new node */
                  WireRRTstar(pr,id_new,i_near,gamma,nn,n,&c,h,(rrt->graph?&q:NULL),&vt,rst,rrt);
                  
                  /*******************************************************************/
                  /* Rewire the tree around the new point */
                  if (rrt->graph)
                    RecursiveReWireRRTstar(pr,&q,g,&steps,&l,rrt);
                  else
                    ReWireRRTstar(pr,id_new,gamma,nn,n,c,&steps,&l,rrt);
                  
                  /*******************************************************************/
                  /* release the data (cost (if actually used) 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 (rrt->si[id_new]->tree==START2GOAL)
                        i_near=GetRRTNN(GOAL2START,q_new,rrt);
                      else
                        i_near=GetRRTNN(START2GOAL,q_new,rrt);

                      l1=rrt->si[id_new]->cost+rrt->si[i_near]->cost;
                      c12=ConnectSamples(pr,rrt->tp,rrt->ambient,
                                         rrt->m,rrt->n,q_new,rrt->si[i_near]->samples,l-l1,
                                         TRUE,&connected,&collision,NULL,NULL,NULL,rrt->w);
                      if (connected)
                        {
                          if (rrt->graph)
                            AddEdgeToRRT(id_new,i_near,c12,rrt);
                          l1+=c12;
                          if (l1<l)
                            l=ChangeBiRRTSteps(id_new,i_near,c12,&steps,rrt);
                        }
                    }
                }
              #if (RRT_VERBOSE)
              else
                {
                  if (id_new==NO_UINT)
                    fprintf(stderr,"  Blocked [%u]\n",i_near);
                }
              #endif

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

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

              BiRRTstarCloseIteration(*it,l,time,gamma,times,costs,rrt);
      
              (*it)++;
            }
        }
  
      DeleteStatistics(&st);
      #if (GET_RRT_STATISTICS)
        if (grst==NULL)
          PrintRRTStatistics(rrt,rst);
        else
          AccumulateRRTStatistics(rst,grst);
        DeleteRRTStatistics(rst);
        free(rst);
      #endif

      if (rrt->graph)
        DeleteHeap(&q);

      *planningTime=GetElapsedTime(&st);

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

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

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

      return(l<INF);
    }
}

boolean ccTRRT(Tparameters *pr,double *pg,double *time,
               double *pl, double* pc, unsigned int *ns,double ***path,
               double (*costF)(Tparameters*,boolean,double*,void*),
               void *costData,
               TRRTStatistics *grst,Trrt *rrt)
{
  double *pWithDummies,*pgs,*q_rand;
  unsigned int it,i_near;
  double epsilon;
  boolean pathFound,done,expand2Goal;
  unsigned int lastSample;
  double er,h;
  Tstatistics st; /* used just to measure execution time */
  boolean collision;
  double maxTime;
  unsigned int maxNodes;
  unsigned int samplingMode;
  boolean validSample;
  TRRTStatistics *rst;

  if(rrt->ns >1)
    Error("ccTRRT should only be initialized with the start config");

  if (rrt->mode!=ONE_TREE)
    Error("ccTRRT operates on one-directional RRTs");
 
  #if (GET_RRT_STATISTICS)
  {
    unsigned int i;
    
    NEW(rst,1,TRRTStatistics);
    InitRRTStatistics(rst);
    
    for(i=0;i<rrt->ns;i++)
      NewRRTSample(rst);
  }
  #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);
  samplingMode=(unsigned int)GetParameter(CT_SAMPLING,pr);

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

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

  if (rrt->n==0)
    er=0;
  else
    er=CS_WD_ERROR_IN_SIMP_EQUATIONS(pr,pgs,rrt->w);
  if (er>epsilon)
    Error("The target point for the ccRRT is not on the manifold");

  collision=CS_WD_ORIGINAL_IN_COLLISION(pr,pWithDummies,NULL,rrt->w);
  if (collision)
    Warning("The target point for the ccRRT is in collision");
  free(pWithDummies);

  /*init the cost of the first node*/
  rrt->si[0]->cost=costF(pr,TRUE,GetRRTNode(0,rrt),costData);

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

  done=FALSE;
  it=0;

  InitStatistics(rrt->nCores,0,&st);
  *time=0.0;

  while ((!done)&&(*time<maxTime)&&(rrt->ns<maxNodes))
    {
      if ((RRT_VERBOSE)||((it%1000)==0))
        {
          fprintf(stderr,"Iteration: %u (s:%u t:%g)\n",it,rrt->ns,*time);
          fflush(stdout);
        }

      /*******************************************************************/
      /* Sample in ambient space (with bias toward goal) */
      do {
        /* Generate a random sample */
        expand2Goal=RRTSample(samplingMode,START2GOAL,(EXPLORATION_RRT?NULL:pgs),
                              q_rand,rst,rrt);
        
        /* Check if the sample is valid */      
        validSample=RRTValidateSample(pr,q_rand,START2GOAL,expand2Goal,NULL,INF,
                                      &h,&i_near,rst,rrt);

        *time=GetElapsedTime(&st);
      } while((!validSample)&&(*time<maxTime));

      if (*time<maxTime)
        {
          /*******************************************************************/
          /* Extend the RRT from q_near to q_rand using the ccRRT extend method */
      
          #if (GET_RRT_STATISTICS)
            NewRRTBranch(rst);
            NewRRTDistanceQrand(DistanceTopology(rrt->m,rrt->tp,rrt->si[i_near]->samples,q_rand),rst);
          #endif
          done=AddBranchToRRT(pr,TRUE,START2GOAL,i_near,q_rand,pgs,&lastSample,
                              costF,costData,rst,rrt);
          #if (GET_RRT_STATISTICS)
            if (lastSample!=i_near)
              NewRRTNoEmptyBranch(rst);
          #endif

          *time=GetElapsedTime(&st);
          it++;
        }
    }

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

  /* Reconstruct the path (if any) */
  pathFound=PathStart2GoalInRRT(pr,pgs,NO_UINT,NO_UINT,pl,pc,ns,path,rrt);
  if((pathFound)&&(!EXPLORATION_RRT))
    rrt->si[rrt->ns-1]->cost=costF(pr,TRUE,GetRRTNode(rrt->ns-1,rrt),costData);

  /* Release memory */
  DeleteStatistics(&st);
  #if (GET_RRT_STATISTICS)
    if (grst==NULL)
      PrintRRTStatistics(rrt,rst);
    else
      AccumulateRRTStatistics(rst,grst);
    DeleteRRTStatistics(rst);
    free(rst);
  #endif

  free(q_rand);
  free(pgs);

  return(pathFound);
}

boolean ccRRT(Tparameters *pr,double *pg,
              double *time,
              double *pl,unsigned int *ns,double ***path,
              TRRTStatistics *grst,Trrt *rrt)
{
  double *pWithDummies,*pgs,*q_rand;
  unsigned int it,i_near;
  double epsilon;
  boolean pathFound,done,expand2Goal;
  unsigned int lastSample;
  double er,h;
  Tstatistics st; /* used just to measure execution time */
  boolean collision;
  double maxTime;
  unsigned int maxNodes;
  unsigned int samplingMode;
  boolean validSample;
  TRRTStatistics *rst;

  if (rrt->mode!=ONE_TREE)
    Error("ccRRT operates on one-directional RRTs");
 
  #if (GET_RRT_STATISTICS)
  {
    unsigned int i;
    
    NEW(rst,1,TRRTStatistics);
    InitRRTStatistics(rst);
    
    for(i=0;i<rrt->ns;i++)
      NewRRTSample(rst);
  }
  #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);
  samplingMode=(unsigned int)GetParameter(CT_SAMPLING,pr);

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

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

  if (rrt->n==0)
    er=0;
  else
    er=CS_WD_ERROR_IN_SIMP_EQUATIONS(pr,pgs,rrt->w);
  if (er>epsilon)
    Error("The target point for the ccRRT is not on the manifold");

  collision=CS_WD_ORIGINAL_IN_COLLISION(pr,pWithDummies,NULL,rrt->w);
  if (collision)
    Warning("The target point for the ccRRT is in collision");
  free(pWithDummies);

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

  done=FALSE;
  it=0;

  InitStatistics(rrt->nCores,0,&st);
  *time=0.0;

  while ((!done)&&(*time<maxTime)&&(rrt->ns<maxNodes))
    {
      if ((RRT_VERBOSE)||((it%1000)==0))
        {
          fprintf(stderr,"Iteration: %u (s:%u t:%g)\n",it,rrt->ns,*time);
          fflush(stdout);
        }

      /*******************************************************************/
      /* Sample in ambient space (with bias toward goal) */
      do {
        /* Generate a random sample */
        expand2Goal=RRTSample(samplingMode,START2GOAL,(EXPLORATION_RRT?NULL:pgs),
                              q_rand,rst,rrt);
        
        /* Check the sample is valid */
        validSample=RRTValidateSample(pr,q_rand,START2GOAL,expand2Goal,NULL,INF,
                                      &h,&i_near,rst,rrt);

        *time=GetElapsedTime(&st);
      } while((!validSample)&&(*time<maxTime));

      if (*time<maxTime)
        {
          /*******************************************************************/
          /* Extend the RRT from q_near to q_rand using the ccRRT extend method */
          #if (GET_RRT_STATISTICS)
            NewRRTBranch(rst);
            NewRRTDistanceQrand(DistanceTopology(rrt->m,rrt->tp,rrt->si[i_near]->samples,q_rand),rst);
          #endif
          done=AddBranchToRRT(pr,TRUE,START2GOAL,i_near,q_rand,pgs,&lastSample,
                              NULL,NULL,rst,rrt);
          #if (GET_RRT_STATISTICS)
            if (lastSample!=i_near)
              NewRRTNoEmptyBranch(rst);
          #endif

          *time=GetElapsedTime(&st);
          it++;
        }
    }

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

  /* Reconstruct the path (if any) */
  pathFound=PathStart2GoalInRRT(pr,pgs,NO_UINT,NO_UINT,pl,NULL,ns,path,rrt);

  /* Release memory */
  DeleteStatistics(&st);
  #if (GET_RRT_STATISTICS)
    if (grst==NULL)
      PrintRRTStatistics(rrt,rst);
    else
      AccumulateRRTStatistics(rst,grst);
    DeleteRRTStatistics(rst);
    free(rst);
  #endif

  free(q_rand);
  free(pgs);

  return(pathFound);
}

boolean cBiRRT(Tparameters *pr,double *pg,
               double *time,
               double *pl,unsigned int *ns,double ***path,
               TRRTStatistics *grst,Trrt *rrt)
{
  double *pWithDummies,*pgs,*q_rand;
  unsigned int it,i_near;
  double epsilon;
  boolean pathFound;
  double er,h;
  unsigned int t1,t2;
  unsigned int lastSample1;
  unsigned int samplingMode;
  Tstatistics st; /* used just to measure execution time */
  boolean collision;
  unsigned int lastSample2;
  double maxTime;
  boolean validSample;
  TRRTStatistics *rst;

  #if (EXPLORATION_RRT)
    Error("cBiRRT can not be used in exploration mode.");
  #endif

  if (rrt->mode!=TWO_TREES)
    Error("cBiRRT operates on bidirectional RRTs");

  #if (GET_RRT_STATISTICS)
  {
    unsigned int i;
    
    NEW(rst,1,TRRTStatistics);
    InitRRTStatistics(rst);
    
    for(i=0;i<rrt->ns;i++)
      NewRRTSample(rst);
  }
  #endif

  /* Init the RRT structure */
  epsilon=GetParameter(CT_EPSILON,pr);
  maxTime=GetParameter(CT_MAX_PLANNING_TIME,pr);
  samplingMode=(unsigned int)GetParameter(CT_SAMPLING,pr);

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

  if ((!PointInBoxTopology(NULL,TRUE,rrt->m,pgs,0,rrt->tp,rrt->ambient))||
      (!CS_WD_SIMP_INEQUALITIES_HOLD(pr,pgs,rrt->w)))
    Error("The targed point is not in domain");

  if (rrt->n==0)
    er=0;
  else
    er=CS_WD_ERROR_IN_SIMP_EQUATIONS(pr,pgs,rrt->w);
  if (er>epsilon)
    Error("The target point for the RRT is not on the manifold");

  collision=CS_WD_ORIGINAL_IN_COLLISION(pr,pWithDummies,NULL,rrt->w);
  if (collision)
    Warning("The target point for the RRT is in collision");
  free(pWithDummies);

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

  pathFound=FALSE;
  it=0;

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

  InitStatistics(rrt->nCores,0,&st);
  *time=0.0;

  while ((!pathFound)&&(*time<maxTime))
    {
      if ((RRT_VERBOSE)||((it%1000)==0))
        {
          fprintf(stderr,"Iteration: %u (s:%u t:%g)\n",it,rrt->ns,*time);
          fflush(stdout);
        }

      /*******************************************************************/
      /* Sample */
      do {
        /* Generate a random sample */
        RRTSample(samplingMode,t1,NULL,q_rand,rst,rrt);
        
        /* Check the sample is valid */
        validSample=RRTValidateSample(pr,q_rand,t1,FALSE,NULL,INF,
                                      &h,&i_near,rst,rrt);

        *time=GetElapsedTime(&st);
      } while((!validSample)&&(*time<maxTime));

      if (*time<maxTime)
        {        
          /*******************************************************************/
          /* Extend the first tree from q_near toward q_rand */
          #if (GET_RRT_STATISTICS)
            NewRRTBranch(rst);
            NewRRTDistanceQrand(DistanceTopology(rrt->m,rrt->tp,rrt->si[i_near]->samples,q_rand),rst);
          #endif
          AddBranchToRRT(pr,FALSE,t1,i_near,q_rand,q_rand,&lastSample1,
                         NULL,NULL,rst,rrt);
          #if (GET_RRT_STATISTICS)
            if (lastSample1!=i_near)
              NewRRTNoEmptyBranch(rst);
          #endif

          /*******************************************************************/
          /* The last node added to t1 is used as random sample */
          memcpy(q_rand,rrt->si[lastSample1]->samples,rrt->m*sizeof(double));

          /*******************************************************************/
          /* Search for q_near in the second tree */
          i_near=GetRRTNN(t2,q_rand,rrt);

          /*******************************************************************/
          /* Extend t2 toward the last sample added to t1 */
          #if (GET_RRT_STATISTICS)
            NewRRTTreeConnection(rst);
          #endif
          pathFound=AddBranchToRRT(pr,FALSE,t2,i_near,q_rand,q_rand,&lastSample2,
                                   NULL,NULL,rst,rrt);
          #if (GET_RRT_STATISTICS)
            if (lastSample2!=i_near)
              NewRRTNoEmptyTreeConnection(rst);
          #endif
        
          /*******************************************************************/
          /* Swap the trees (t1 and t2) */
          if ((it%2)==0)
            {
              t1=GOAL2START;
              t2=START2GOAL;
            }
          else
            {
              t1=START2GOAL;
              t2=GOAL2START;
            }

          *time=GetElapsedTime(&st);
          it++;
        }
    }

  fprintf(stderr,"Final number of samples: %u\n",rrt->ns);
  
  /* Reconstruct the path, if possible */
  if (pathFound)
    {
      if (!PathStart2GoalInRRT(pr,pgs,lastSample1,lastSample2,pl,NULL,ns,path,rrt))
        Error("Inconsitancy in cBiRRT pathFound");
    }

  /* Release memory */
  DeleteStatistics(&st);
  #if (GET_RRT_STATISTICS)
    if (grst==NULL)
      PrintRRTStatistics(rrt,rst);
    else
      AccumulateRRTStatistics(rst,grst);
    DeleteRRTStatistics(rst);
    free(rst);
  #endif

  free(q_rand);
  free(pgs);

  return(pathFound);
}

double RRTPathSteps(unsigned int sID,Tvector *path,Trrt *rrt)
{
  TRRTStep st;
  unsigned int s;
  double l;

  InitVector(sizeof(unsigned int),CopyRRTStep,DeleteRRTStep,100,path);

  s=sID;
  l=0;
  while (s!=NO_UINT)
    {   
      st.id=s;
      st.cost=rrt->si[s]->costp;/* this is 0 for the root */
      NewVectorElement((void *)(&st),path);
      l+=st.cost; 
      s=rrt->si[s]->parent; /* this is NO_UINT for the root */
    }
  return(l);
}

double ChangeBiRRTSteps(unsigned int l1,unsigned int l2,double c12,Tvector *steps,Trrt *rrt)
{
  Tvector v1,v2;
  double lp1,lp2;
  unsigned int n;
  TRRTStep st,*st1,*st2;
  int i;
  double c;

  lp1=RRTPathSteps(l1,&v1,rrt); 
  lp2=RRTPathSteps(l2,&v2,rrt); 
  if (rrt->si[l1]->tree==START2GOAL)
    {
      /* reverse(v1) -> v2 */
      ResetVector(steps);
      n=VectorSize(&v1);
      for(i=n-1;i>=0;i--)
        {
          st1=(TRRTStep *)GetVectorElement(i,&v1);
          if (i==0)
            c=c12;
          else
            {
              st2=(TRRTStep *)GetVectorElement(i-1,&v1);
              c=st2->cost;
            }
          st.id=st1->id;
          st.cost=c;
          NewVectorElement((void*)&st,steps);
        }
      ConcatVectors(&v2,steps);
    }
  else
    {
      /* reverse(v2) -> v1 */
      ResetVector(steps);
      n=VectorSize(&v2);
      for(i=n-1;i>=0;i--)
        {
          st1=(TRRTStep *)GetVectorElement(i,&v2);
          if (i==0)
            c=c12;
          else
            {
              st2=(TRRTStep *)GetVectorElement(i-1,&v2);
              c=st2->cost;
            }
          st.id=st1->id;
          st.cost=c;
          NewVectorElement((void*)&st,steps);
        }
      ConcatVectors(&v1,steps);
    }
  DeleteVector(&v1);
  DeleteVector(&v2);

  return(lp1+c12+lp2);
}

double UpdateBiRRTSteps(Tvector *steps,Trrt *rrt)
{
  TRRTStep *st;
  unsigned int i,n,current,next,id1,id2;
  double c12,l,ct;

  /* Ensure that all nodes in the path are correctly assigned
     to the trees (and that theirs costs are up to date) */
  n=VectorSize(steps);
  for(i=0;i<n;i++)
    {
      st=(TRRTStep *)GetVectorElement(i,steps);
      UpdateCostAndTree(st->id,rrt);
    }

  /* Search for the best transition between trees along the path */
  st=(TRRTStep *)GetVectorElement(0,steps);
  current=st->id;
  c12=st->cost;
  l=INF;
  id1=NO_UINT;
  id2=NO_UINT;
  for(i=1;i<n;i++)
    {
      st=(TRRTStep *)GetVectorElement(i,steps);
      next=st->id;
      if ((current==NO_UINT)||(next==NO_UINT))
        Error("Undefined node in path in BiRRT");
      if ((rrt->si[current]->tree!=rrt->si[next]->tree)&&
          ((rrt->si[current]->cost+c12+rrt->si[next]->cost)<l))
        {
          id1=current;
          id2=next;
          ct=c12;
          l=rrt->si[id1]->cost+ct+rrt->si[id2]->cost;
        }
      current=next;
      c12=st->cost;
    }
  if (id1==NO_UINT) /* (id2==NO_UINT) (l==INF) */
    Error("A global path in a BiRRT without transition between trees?");

  return(ChangeBiRRTSteps(id1,id2,ct,steps,rrt));
}

boolean PathStart2GoalInRRT(Tparameters *pr,double *pgs,
                            unsigned int l1,unsigned int l2,
                            double *pl,double* pc,unsigned int *ns,double ***path,
                            Trrt *rrt)
{
  boolean pathFound;

  #if (EXPLORATION_RRT)
    /* We reconstruct the path from root to last sample in the tree */
    pathFound=TRUE;
    ReconstructRRTPath(pr,rrt->ns-1,pl,pc,ns,path,rrt);
  #else 
    double pl1,pl2,d12;
 
    if (rrt->mode!=ONE_TREE)
      {
        d12=DistanceTopology(rrt->m,rrt->tp,rrt->si[l1]->samples,rrt->si[l2]->samples);
        pathFound=(d12<2.0*rrt->delta);

        if (pathFound)
          {         
            unsigned int ns1,ns2;
            double **p1,**p2;
            unsigned int nvs;

            nvs=ReconstructRRTPath(pr,l1,&pl1,pc,&ns1,&p1,rrt);
            ReconstructRRTPath(pr,l2,&pl2,pc,&ns2,&p2,rrt);
            
            if (rrt->si[l1]->tree==START2GOAL)
              ReverseConcatSamples(nvs,ns1,p1,ns2,p2,ns,path);
            else
              ReverseConcatSamples(nvs,ns2,p2,ns1,p1,ns,path);
            
            DeleteSamples(ns1,p1);
            DeleteSamples(ns2,p2);

            *pl=pl1+d12+pl2;
          }
      }
    else
      {
        unsigned int nn; 
        double d;

        nn=GetRRTNN(START2GOAL,pgs,rrt);
        d=DistanceTopology(rrt->m,rrt->tp,rrt->si[nn]->samples,pgs);
        pathFound=(d<2.0*rrt->delta);
        if (pathFound)
          {
            unsigned int g;

            /* Todo : compute the cost of the goal if required*/
            AddNodeToRRT(pr,START2GOAL,nn,pgs,pgs,&g,NULL,d,0,NULL,rrt); 
            ReconstructRRTPath(pr,g,pl,pc,ns,path,rrt);
          }
      }
  #endif

  return(pathFound);
}

boolean Bidirectional(Trrt *rrt)
{
  return(rrt->mode!=ONE_TREE);
}


unsigned int GetRRTMode(Trrt *rrt)
{
  return(rrt->mode);
}

boolean InDynamicDomain(unsigned int i,double *q,Trrt *rrt)
{
  if ((rrt->dd>0)&&(rrt->si[i]->ddr>0.0))
    return(DistanceTopology(rrt->m,rrt->tp,rrt->si[i]->samples,q)<rrt->si[i]->ddr);
  else
    return(TRUE);
}

void AdjustDynamicDomain(unsigned int i,boolean collision,Trrt *rrt)
{
  if (rrt->dd>0) /*  If the dynamic domain is active */
    {
      if (rrt->si[i]->ddr==0.0)
        {
          /* The dynamic domain radius is not set yet. Set it if we
             detected a collision during branch extension. */
          if (collision)
            rrt->si[i]->ddr=rrt->dd; /* Set to the initial radius. */
        }
#if (0)
      else
        {
          /* Adjust the radious according to the collision status.
             Branches interrupted by a collision -> decrease the radius
             Branches fully extended -> increase the radius */
          if (collision)
            rrt->si[i]->ddr*=(1.0-MOV_AVG_DOWN);
          else
            rrt->si[i]->ddr*=(1.0+MOV_AVG_UP);
        }
#endif 
    }
}

double GetDynamicDomainRadius(unsigned int i,Trrt *rrt)
{
  if (rrt->dd>0)
    return(rrt->si[i]->ddr);
  else
    return(0.0);
}

boolean DynamicDomainRRT(Trrt *rrt)
{
  return(rrt->dd>0);
}

unsigned int GetRRTNumNodes(Trrt *rrt)
{
  return(rrt->ns);
}

double *GetRRTNode(unsigned int i,Trrt *rrt)
{
  if (i>=rrt->ns)
    return(NULL);
  else
    return(rrt->si[i]->samples);
}

unsigned int GetRRTNodeTree(unsigned int i,Trrt *rrt)
{
  if (i>=rrt->ns)
    return(NO_UINT);
  else
    {
      if (rrt->mode==ONE_TREE)
        return(START2GOAL);
      else
        return(rrt->si[i]->tree);
    }
}

unsigned int GetRRTParent(unsigned int i,Trrt *rrt)
{
  if (i>=rrt->ns)
    return(NO_UINT);
  else
    return(rrt->si[i]->parent);
}

void SetRRTParent(unsigned int i,unsigned int p,Trrt *rrt)
{
  if ((i>=rrt->ns)||(p>=rrt->ns))
    Error("Wrong node/parent in SetRRTParent");

  if (rrt->si[i]->parent==NO_UINT)
    Error("Can not change the parent of a root node");
  
  rrt->si[i]->parent=p;
  if (rrt->si[i]->tree!=rrt->si[p]->tree)
    {
      if (rrt->mode!=TWO_TREES_WITH_SWAP)
        Error("Nodes can only swap trees in TWO_TREES_WITH_SWAP RRTs");
      rrt->si[i]->tree=rrt->si[p]->tree;
    }
}

void *GetRRTNodeInfo(unsigned int i,Trrt *rrt)
{
  if (i<rrt->ns)
    return(rrt->si[i]->userInfo);
  else
    return(NULL);
}

void SetRRTNodeInfo(unsigned int i,void *info,Trrt *rrt)
{
  if (i<rrt->ns)
    rrt->si[i]->userInfo=info;
  else
    Error("RRT node out of range in SetRRTNodeInfo");
}

double GetRRTNodeCost(unsigned int i,Trrt *rrt)
{
  if (i<rrt->ns)
    return(rrt->si[i]->cost);
  else
    return(0.0);
}

double GetRRTNodeCostFromParent(unsigned int i,Trrt *rrt)
{
  if (i<rrt->ns)
    return(rrt->si[i]->costp);
  else
    return(0.0);
}

void SetRRTNodeCost(unsigned int i,double costp,double cost,Trrt *rrt)
{
  if (i<rrt->ns)
    {
      rrt->si[i]->costp=costp;
      rrt->si[i]->cost=cost;
    }
  else
    Error("RRT node out of range in SetRRTNodeCost");
}

void SetRRTCostAndParent(unsigned int i,unsigned int p,double costp,double cost,Trrt *rrt)
{
  if (i<rrt->ns)
    {
      rrt->si[i]->costp=costp;
      rrt->si[i]->cost=cost;
      rrt->si[i]->parent=p;
      if (rrt->si[i]->tree!=rrt->si[p]->tree)
        {
          if (rrt->mode!=TWO_TREES_WITH_SWAP)
            Error("A RRT node moved from one tree to another in SetRRTCostAndParent");
          rrt->si[i]->tree=rrt->si[p]->tree;
        }      
    }
  else
    Error("RRT node out of range in SetRRTCostAndParent");
}

unsigned int *GetRRTTopology(Trrt *rrt)
{
  return(rrt->tp);
}

double CostToRoot(unsigned int sID,Trrt* rrt)
{
  double l;
  unsigned int s;

  l=0.0;
  s=sID;
  while (s!=NO_UINT)
    {   
      l+=rrt->si[s]->costp;
      s=rrt->si[s]->parent;
    }
  return(l);
}

void UpdateCostToRoot(unsigned int sID,Trrt* rrt)
{
  rrt->si[sID]->cost=CostToRoot(sID,rrt);
}

void UpdateTree(unsigned int sID,Trrt* rrt)
{
  unsigned int s,p;

  if (rrt->mode!=TWO_TREES_WITH_SWAP)
    Error("Nodes can only change trees in TWO_TREES_WITH_SWAP mode");

  s=sID;
  p=NO_UINT; /* parent node */
  while (s!=NO_UINT)
    {   
      p=s;
      s=rrt->si[s]->parent;
    }
  if (p==NO_UINT)
    Error("Undefined root node in UpdateTree");

  /* here p is the root of the tree including sample sID (0 or 1) */
  if (p>2)
    Error("Wrong root in UpdateTree");

  /* update the tree assigment for this node */
  /* alternatively we could update the tree assigment all along the branch */
  if (p==0)
    rrt->si[sID]->tree=START2GOAL;
  else
    rrt->si[sID]->tree=GOAL2START;
}

void UpdateCostAndTree(unsigned int sID,Trrt* rrt)
{
  double l;
  unsigned int s,p;

  /* This is just cut+paste+joint of CostToRoot, UpdateCost, and UpdateTree 
     defined to gain some efficiency. */

  l=0.0;
  s=sID;
  p=NO_UINT; /* previous node */
  while (s!=NO_UINT)
    {   
      l+=rrt->si[s]->costp;
      p=s;
      s=rrt->si[s]->parent;
    }

  /* update the cost and the tree of the given node */
  rrt->si[sID]->cost=l;
  /* the update of the tree only has effect in TWO_TREES_WITH_SWAP mode */
  if (p==0)
    rrt->si[sID]->tree=START2GOAL;
  else
    rrt->si[sID]->tree=GOAL2START;
}

unsigned int StepsToRoot(unsigned int sID,Trrt* rrt)
{
  unsigned int l;
  unsigned int s;

  l=0;
  s=sID;
  while (s!=NO_UINT)
    {   
      l++;
      s=rrt->si[s]->parent;
    }
  return(l);
}

void PlotRRT(char *fname,int argc, char **arg,Tparameters *pr,
             unsigned int xID,unsigned int yID,unsigned int zID,Trrt *rrt)
{
  Tplot3d p3d;
  unsigned int i;
  Tcolor color;
  double *o;
  double *px,*py,*pz;
  double x[2],y[2],z[2];
  double cx,cy,cz;
 
  if (rrt->mode==TWO_TREES_WITH_SWAP)
    {
      /* To obtain nicer plots we update the tree assigment for all nodes */
      for(i=0;i<rrt->ns;i++)
        UpdateTree(i,rrt);
    }
 
  cx=GetParameter(CT_CUT_X,pr);
  cy=GetParameter(CT_CUT_Y,pr);
  cz=GetParameter(CT_CUT_Z,pr);

  /* Project the RRT nodes */
  NEW(px,rrt->ns,double);
  NEW(py,rrt->ns,double);
  NEW(pz,rrt->ns,double);

  for(i=0;i<rrt->ns;i++)
    {
      CS_WD_REGENERATE_ORIGINAL_POINT(pr,rrt->si[i]->samples,&o,rrt->w);

      px[i]=o[xID];
      py[i]=o[yID];
      pz[i]=o[zID];
      
      if ((cx<0)&&(px[i]<cx))
        px[i]+=M_2PI;
      if ((cx>0)&&(px[i]>cx))
        px[i]-=M_2PI;
      if ((cy<0)&&(py[i]<cy))
        py[i]+=M_2PI;
      if ((cy>0)&&(py[i]>cy))
        py[i]-=M_2PI;
      if ((cz<0)&&(pz[i]<cz))
        pz[i]+=M_2PI;
      if ((cz>0)&&(pz[i]>cz))
        pz[i]-=M_2PI; 
      
      free(o);
    }

  InitPlot3d(fname,FALSE,argc,arg,&p3d);

  NewColor(1,0,0,&color); /*red*/
  StartNew3dObject(&color,&p3d);
  
  for(i=((rrt->mode!=ONE_TREE)?2:1);i<rrt->ns;i++)
    {
      if ((rrt->mode==ONE_TREE)||(rrt->si[i]->tree==START2GOAL))
        { 
          x[0]=px[i];
          y[0]=py[i];
          z[0]=pz[i];

          x[1]=px[rrt->si[i]->parent];
          y[1]=py[rrt->si[i]->parent];
          z[1]=pz[rrt->si[i]->parent];
          
          PlotVect3d(2,x,y,z,&p3d);
        }
    }
  
  DeleteColor(&color);
  Close3dObject(&p3d);

  if (rrt->mode!=ONE_TREE)
    {
      NewColor(0,1,0,&color); /*green*/
      StartNew3dObject(&color,&p3d);
  
      for(i=2;i<rrt->ns;i++)
        {
          if (rrt->si[i]->tree==GOAL2START)
            {
              x[0]=px[i];
              y[0]=py[i];
              z[0]=pz[i];
              
              x[1]=px[rrt->si[i]->parent];
              y[1]=py[rrt->si[i]->parent];
              z[1]=pz[rrt->si[i]->parent];
              
              PlotVect3d(2,x,y,z,&p3d);
            }
        }
      
      DeleteColor(&color);
      Close3dObject(&p3d);
    }
  
  #if (RRT_PLOT_NODES)
    /* Dots, one per node */
    NewColor(0,0,1,&color); /*blue*/
    StartNew3dObject(&color,&p3d);

    for(i=0;i<rrt->ns;i++)
      {
        x[0]=px[i]-0.01;
        y[0]=py[i];
        z[0]=pz[i];

        x[1]=px[i]+0.01;
        y[1]=py[i];
        z[1]=pz[i];

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

        x[0]=px[i];
        y[0]=py[i]-0.01;
        z[0]=pz[i];

        x[1]=px[i];
        y[1]=py[i]+0.01;
        z[1]=pz[i];

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

        x[0]=px[i];
        y[0]=py[i];
        z[0]=pz[i]-0.01;

        x[1]=px[i];
        y[1]=py[i];
        z[1]=pz[i]+0.01;

        PlotVect3d(2,x,y,z,&p3d);
      }
  
    DeleteColor(&color);
    Close3dObject(&p3d);  
  #endif

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

  free(px);
  free(py);
  free(pz);
}

void SaveRRTNodes(Tparameters *pr,char *fname,boolean saveWithDummies,Trrt *rrt)
{   
  double *o;
  unsigned int i,j,nv,nvs;
  boolean *systemVars;
  double *c;
  FILE *fo;
  Tfilename fsol;

  if (saveWithDummies)
    CreateFileName(NULL,fname,"_nodes",SOL_WITH_DUMMIES_EXT,&fsol);
  else
    CreateFileName(NULL,fname,"_nodes",SOL_EXT,&fsol);
  fprintf(stderr,"Writing boxes to                  : %s\n",GetFileFullName(&fsol));
  fo=fopen(GetFileFullName(&fsol),"w");
  if (!fo)
    Error("Could not open the file to store the boxes in SaveRRTNodes");

  nv=CS_WD_GET_SYSTEM_VARS(&systemVars,rrt->w);
  nvs=0;
  for(j=0;j<nv;j++)
    {
      if (systemVars[j])
        nvs++;
    }

  for(i=0;i<rrt->ns;i++)
    {
      c=rrt->si[i]->samples;
      
      CS_WD_REGENERATE_ORIGINAL_POINT(pr,c,&o,rrt->w);
      
      if (saveWithDummies)
        fprintf(fo,"%u { %u ",i,nv);
      else
        fprintf(fo,"%u { %u ",i,nvs);
      for(j=0;j<nv;j++)
        {
          if ((saveWithDummies)||(systemVars[j]))
            fprintf(fo,"[%.12g,%.12g] ",o[j],o[j]);
        }
      fprintf(fo,"}\n");

      free(o);
    }
 
  fclose(fo);
  free(systemVars);
  DeleteFileName(&fsol);
}

void SaveRRTCosts(Tparameters *pr,char *fname,Trrt *rrt)
{   
  unsigned int i;
  FILE *fo;
  Tfilename fcost;
  double ma,mi;
  unsigned int ima,imi;
  double c;

  CreateFileName(NULL,fname,"_nodes",COST_EXT,&fcost);
  fprintf(stderr,"Writing costs to                  : %s\n",GetFileFullName(&fcost));

  fo=fopen(GetFileFullName(&fcost),"w");
  if (!fo)
    Error("Could not open the file to store the costs in SaveRRTCosts");

  ma=-INF;
  mi=INF;
  for(i=0;i<rrt->ns;i++)
    {
      c=rrt->si[i]->cost;
      fprintf(fo,"%f\n",c);
      if (c>ma)
        {
          ma=c;
          ima=i;
        }
      if (c<mi)
        {
          mi=c;
          imi=i;
        }
    }
  fprintf(stderr,"  Extreme costs                   : %f (%u) -- %f (%u)\n",mi,imi,ma,ima);

  fclose(fo);
  DeleteFileName(&fcost);
}

unsigned int RRTMemSize(Trrt *rrt)
{
  unsigned int b,i;

  /* The samples */
  b=sizeof(double)*rrt->m*rrt->ns; 

  if (rrt->graph)
    {
      /* The neighbours */
      for(i=0;i<rrt->ns;i++)
        b+=rrt->si[i]->nn*(sizeof(double)+sizeof(unsigned int)); 
    }

  return(b);
}

void SaveRRT(Tfilename *fname,Trrt *rrt)
{
  FILE *f;
  unsigned int i,j;


  f=fopen(GetFileFullName(fname),"w");
  if (!f)
    Error("Could not open file to store rrt");

  fprintf(f,"%u\n",rrt->graph);
  fprintf(f,"%u\n",rrt->mode);
  fprintf(f,"%f\n",rrt->dd);
  #if (_KDTREE==1)
    fprintf(f,"%f\n",KDtreeSamplingExpansion(rrt->treeS2G));
  #else
    fprintf(f,"0\n");
  #endif

  fprintf(f,"%u\n",rrt->m);
  fprintf(f,"%u\n",rrt->k);
  fprintf(f,"%u\n",rrt->n);

  fprintf(f,"%f\n",rrt->delta);

  fprintf(f,"%f\n",rrt->temperature);
  fprintf(f,"%u\n",rrt->nFail);
 
  fprintf(f,"%u\n",rrt->ms);
  fprintf(f,"%u\n",rrt->ns);
 
  for(i=0;i<rrt->ns;i++)
    {
      for(j=0;j<rrt->m;j++)
        fprintf(f,"%.12f ",rrt->si[i]->samples[j]);
      fprintf(f,"\n");
      fprintf(f,"%u %u %f %f %f\n",rrt->si[i]->parent,rrt->si[i]->tree,
              rrt->si[i]->costp,rrt->si[i]->cost,rrt->si[i]->ddr);

      if (rrt->graph)
        {
          fprintf(f,"%f %u %u",rrt->si[i]->g,rrt->si[i]->m,rrt->si[i]->nn);
          for(j=0;j<rrt->si[i]->nn;j++)
            fprintf(f," %u %f",rrt->si[i]->n[j],rrt->si[i]->cn[j]);
          fprintf(f,"\n");
        }
    }

  fclose(f);
}

void LoadRRT(Tparameters *pr,Tfilename *fname,TAtlasBase *w,Trrt *rrt)
{
  FILE *f;
  unsigned int i,j;  
  double dr;

  f=fopen(GetFileFullName(fname),"r");
  if (!f)
    Error("Could not open file to read the RRT");

  rrt->w=w;

  fscanf(f,"%u",&(rrt->graph));
  fscanf(f,"%u",&(rrt->mode));
  fscanf(f,"%lf",&(rrt->dd));
  fscanf(f,"%lf",&dr);

  fscanf(f,"%u",&(rrt->m));
  fscanf(f,"%u",&(rrt->k));
  fscanf(f,"%u",&(rrt->n));

  fscanf(f,"%lf",&(rrt->delta));
  
  fscanf(f,"%lf",&(rrt->temperature));
  fscanf(f,"%u",&(rrt->nFail));  

  fscanf(f,"%u",&(rrt->ms));
  fscanf(f,"%u",&(rrt->ns));

  NEW(rrt->si,rrt->ms,TRRTSampleInfo*);
  for(i=0;i<rrt->ms;i++)
    rrt->si[i]=NULL;

  SetRRTTopology(pr,rrt);

  NEW(rrt->ambient,1,Tbox);
  CS_WD_GENERATE_SIMP_INITIAL_BOX(pr,rrt->ambient,rrt->w);

  #if (_KDTREE==1)
  {
    unsigned int nd,i;
    Trectangle rec;
    Tinterval *range;

    nd=GetBoxNIntervals(rrt->ambient);
    InitRectangle(nd,NULL,NULL,&rec);
    for(i=0;i<nd;i++)
      {
        range=GetBoxInterval(i,rrt->ambient);
        SetRectangleLimits(i,LowerLimit(range),UpperLimit(range),&rec);
      }
    
    rrt->treeS2G=InitKDTree(&rec,rrt->tp,25,dr,0,NULL,NULL);
    if (rrt->mode==TWO_TREES)
      rrt->treeG2S=InitKDTree(&rec,rrt->tp,25,dr,0,NULL,NULL);

    DeleteRectangle(&rec);
  }
  #endif

  #if (_KDTREE==2)
    if ((!RRT_NN_TOPOLOGY)||(rrt->tp==NULL))
      rrt->treeS2G=CreateKDTree(rrt->m);
    else
      rrt->treeS2G=CreateKDTreeT(rrt->m,(int *)rrt->tp);
      
    if (rrt->mode==TWO_TREES)
      {
        if ((!RRT_NN_TOPOLOGY)||(rrt->tp==NULL))
          rrt->treeG2S=CreateKDTree(rrt->m); 
        else
          rrt->treeG2S=CreateKDTreeT(rrt->m,(int *)rrt->tp);
        
        rrt->maxNodesT1=rrt->ms;
        rrt->maxNodesT2=rrt->ms;
        
        NEW(rrt->t1ToId,rrt->maxNodesT1,unsigned int);
        NEW(rrt->t2ToId,rrt->maxNodesT2,unsigned int);
        
        rrt->nodesT1=0;
        rrt->nodesT2=0;
      }
  #endif

  for(i=0;i<rrt->ns;i++)
    {
      NEW(rrt->si[i],1,TRRTSampleInfo);

      NEW(rrt->si[i]->samples,rrt->m,double);
      for(j=0;j<rrt->m;j++)      /* sample */
        fscanf(f,"%lf",&(rrt->si[i]->samples[j]));

      fscanf(f,"%u",&(rrt->si[i]->parent));
      fscanf(f,"%u",&(rrt->si[i]->tree));
      fscanf(f,"%lf",&(rrt->si[i]->costp));
      fscanf(f,"%lf",&(rrt->si[i]->cost));
      fscanf(f,"%lf",&(rrt->si[i]->ddr));

      rrt->si[i]->userInfo=NULL; /* This information is not stored :( */

      #if (_KDTREE==1)
        if (rrt->mode==TWO_TREES)
          {
            if (rrt->si[i]->tree==START2GOAL)
              AddPoint2KDtree(rrt->si[i]->samples,i,&(rrt->treeS2G));
            else
              AddPoint2KDtree(rrt->si[i]->samples,i,&(rrt->treeG2S));
          }
        else
          AddPoint2KDtree(rrt->si[i]->samples,i,&(rrt->treeS2G));
      #endif
  
      #if (_KDTREE==2)
        if (rrt->mode==TWO_TREES)
          {
            if (rrt->si[i]->tree==START2GOAL)
              {
                rrt->t1ToId[rrt->nodesT1]=i;
                rrt->nodesT1++;
                AddPoint2KDTree(rrt->si[i]->samples,rrt->treeS2G);
              }
            else
              {
                rrt->t2ToId[rrt->nodesT2]=i;
                rrt->nodesT2++;
                AddPoint2KDTree(rrt->si[i]->samples,rrt->treeG2S);
              }
          }
        else
          AddPoint2KDTree(rrt->si[i]->samples,rrt->treeS2G);
      #endif

      if (rrt->graph)
        {
          fscanf(f,"%lf",&(rrt->si[i]->g));
          fscanf(f,"%u",&(rrt->si[i]->m));
          fscanf(f,"%u",&(rrt->si[i]->nn));
          NEW(rrt->si[i]->n,rrt->si[i]->m,unsigned int);
          NEW(rrt->si[i]->cn,rrt->si[i]->m,double);
          for(j=0;j<rrt->si[i]->nn;j++)
            {       
              fscanf(f,"%u",&(rrt->si[i]->n[j]));
              fscanf(f,"%lf",&(rrt->si[i]->cn[j]));
            }
        }
    }

  fclose(f);

  NEW(rrt->ambient,1,Tbox);
  /* The Jacobian is not saved but re-computed */
  CS_WD_GENERATE_SIMP_INITIAL_BOX(pr,rrt->ambient,rrt->w);

  /* The parallelism information is not stored! We de-activate it
     when loading an RRT (load is typically used for visualization
     and not for further processing)*/
  rrt->nCores=1;
  rrt->parallel=FALSE;
}

void DeleteRRT(Trrt *rrt)
{
  unsigned int i;

  for(i=0;i<rrt->ns;i++) 
    {
      free(rrt->si[i]->samples);
      if (rrt->graph)
        {
          free(rrt->si[i]->n);
          free(rrt->si[i]->cn);
        }
      free(rrt->si[i]);
    }

  if (rrt->ambient!=NULL)
    {
      DeleteBox(rrt->ambient);
      free(rrt->ambient);
    }

  free(rrt->si);

  if (rrt->tp!=NULL)
    free(rrt->tp);
  
  #if (_KDTREE==1)
    DeleteKDtree(rrt->treeS2G);
     if (rrt->mode==TWO_TREES)
       DeleteKDtree(rrt->treeG2S);
  #endif
  #if (_KDTREE==2)
    DeleteKDTree(rrt->treeS2G);
    if (rrt->mode==TWO_TREES)
      {
        DeleteKDTree(rrt->treeG2S);

        free(rrt->t1ToId);
        free(rrt->t2ToId);
      }
  #endif
}