samples.c

Go to the documentation of this file.

#include "samples.h"

#include "filename.h"
#include "algebra.h"
#include "parameters.h"
#include "list.h"
#include "random.h"

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

int cmpUInt(const void *a,const void *b);

double PathLength(unsigned int *tp,unsigned int m,
                  unsigned int np,double **point);


void ShortcutSmooth(Tparameters *pr,Tstatistics *stime,
                    unsigned int *np,double ***point,TAtlasBase *w);

void RandomSmooth(Tparameters *pr,unsigned int nCores,unsigned int maxIterations,
                  Tstatistics *stime,
                  unsigned int *np,double ***point,TAtlasBase *w);

void GradientSmooth(Tparameters *pr,unsigned int nCores,unsigned int maxIterations,
                    Tstatistics *stime,
                    unsigned int *np,double ***point,TAtlasBase *w);



void SaveSamplesInt(Tfilename *fpath,unsigned int nvs,
                    unsigned int ns,double **path);

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

void InitSamples(unsigned int *ms,unsigned int *ns,double ***path)
{
  *ms=INIT_NUM_SAMPLES;
  *ns=0;
  NEW((*path),*ms,double *);
}

void AddSample2Samples(unsigned int nv,double *sample,
                       unsigned int nvs,boolean *systemVars,
                       unsigned int *ms,unsigned int *ns,double ***path)
{
  unsigned int i,k;
  
  if (*ns==*ms)
    MEM_DUP(*path,*ms,double *);
  
  NEW((*path)[*ns],nvs,double);
  k=0;
  for(i=0;i<nv;i++)
    {
      if ((systemVars==NULL)||(systemVars[i]))
        {
          (*path)[*ns][k]=sample[i];
          k++;
        }
    }
  (*ns)++;
}

double ConnectSamplesChart(Tparameters *pr,unsigned int *tp,
                           Tbox *domain,
                           unsigned int m,unsigned int n,
                           double *s1,double *s2,
                           double md,boolean checkCollisions,
                           boolean *reached,boolean *collision,
                           double *lastSample,unsigned int *ns,double ***path,
                           TAtlasBase *w)
{
  boolean blocked,validChart,first;
  double dist;
  double *s,*s_next,*s_goal,*t,*t_next,*t_goal;
  unsigned int i,k,a,b;
  double delta,step,epsilon,d,dtmp,d12,e,ce,r,nt;
  unsigned int ms;
  TJacobian sJ;
  Tchart c;

  epsilon=GetParameter(CT_EPSILON,pr);
  delta=GetParameter(CT_DELTA,pr);
  k=(unsigned int)GetParameter(CT_N_DOF,pr);
  e=GetParameter(CT_E,pr);
  ce=GetParameter(CT_CE,pr);
  r=GetParameter(CT_R,pr);

  CS_WD_GET_SIMP_JACOBIAN(pr,&sJ,w);  
  /* Assume that the CD is already initialized */

  GetJacobianSize(&a,&b,&sJ);
  if (b!=m)
    Error("Jacobian size missmatch in ConnectSamplesChart");
  
  if ((ns!=NULL)&&(path!=NULL))
    InitSamples(&ms,ns,path);
  else
    ms=0;

  /* we keep track of the current sample in the path
     and the previous one to compute the displacement
     between them */
  if (lastSample==NULL)
    { NEW(s,m,double); }
  else
    s=lastSample;
  memcpy(s,s1,m*sizeof(double)); /* current sample */
  NEW(s_next,m,double); /* space for the next sample */
  NEW(s_goal,m,double); /* goal sample projected to the current chart */
  NEW(t,k,double);      /* parameters for 's' */
  NEW(t_next,k,double); /* parameters for 's_next' */
  NEW(t_goal,k,double); /* parameters for 's_goal' */

  d=DistanceTopology(m,tp,s1,s2);
  d12=d;
  dist=0.0;
  *reached=(d12<delta);
  *collision=FALSE;
  blocked=FALSE;
  
  while ((!*reached)&&(!blocked)&&(!*collision))
    {  
      /* Initialize the chart at the current sample */
      if (tp!=NULL)
        ArrayPi2Pi(m,tp,s);
      
      //fprintf(stderr,"New chart\n");
      if (InitChart(pr,TRUE,domain,tp,m,k,s,e,ce,r,&sJ,w,&c)!=0)
        {
          //fprintf(stderr,"  Blocked (error init chart)\n");
          blocked=TRUE;
        }
      else
        {
          /* 's' is 0 in the new chart */
          for(i=0;i<k;i++)
            t[i]=0.0;

          /* Determine the advance direction */
          DifferenceVectorTopology(m,tp,s2,s,s_goal);
          AccumulateVector(m,s,s_goal);
          Manifold2Chart(s_goal,NULL,t_goal,&c); /* without topology now */
          nt=Norm(k,t_goal);
          if (nt<1e-3)
            {
              //fprintf(stderr,"  Blocked (small projection)\n");
              blocked=TRUE; /* it is not clear which direction to take */
            }
          else
            {
              //fprintf(stderr,"tg:%g\n",Norm(k,t_goal));
              /* when far from the goal, we move the goal far enough to ensure
                 the creation of new charts, if necessary. */
              if (DistanceTopology(m,tp,s2,s)>r)
                ScaleVector(2.0*r/nt,k,t_goal); /* move the goal far enough */

              /* Move in the current chart as far as possible */
              validChart=TRUE;
              step=delta;
              first=TRUE;
              while ((validChart)&&(!*reached)&&(!blocked)&&(!*collision))
                {
                  /* Define t_next from t and t_goal */
                  DifferenceVector(k,t_goal,t,t_next);
                  nt=Norm(k,t_next);
                  if (nt>step)
                    {
                      SumVectorScale(k,t,step/nt,t_next,t_next);
                      nt=step;
                    }
                  else
                    AccumulateVector(k,t,t_next);
                  //fprintf(stderr,"  New step-> dg:%g (%g) st:%g  tn:%g  tg:%g (%g)\n",nt,DistanceTopology(m,tp,s2,s),nt,Norm(k,t_next),Norm(k,t_goal),Distance(k,t_next,t_goal));
                  
                  if (0) //(Norm(k,t_next)>r) /* TO BE REMOVED */
                    {
                      //fprintf(stderr,"  Change chart (r)\n");
                      validChart=FALSE;
                    }
                  else
                    {
                      /* and project to the manifold to obtain q_next */
                      if (Chart2Manifold(pr,&sJ,t_next,NULL,s,s_next,&c)>e)
                        {
                          //fprintf(stderr,"  Change chart (e)\n");
                          validChart=FALSE;
                        }
                      else
                        {
                          if ((nt<epsilon)&&(DistanceTopology(m,tp,s_next,s2)>delta))
                            {
                              /* we reached the targed in tangent space but the projection does not correspond
                                 to the actual goal. Try to advance generating a new chart. */
                              if (first)
                                {
                                  /* Note that it is not worth to reduce the step since we are already taking
                                     steps of size epsilon. */
                                  //fprintf(stderr,"  Blocked (tangent vs ambient)\n");
                                  blocked=TRUE;
                                }
                              else
                                {
                                  //fprintf(stderr,"  Change chart (tangent vs ambient)\n");
                                  validChart=FALSE;
                                }
                            }
                          else
                            {
                              if ((!PointInBoxTopology(NULL,FALSE,m,s_next,0,tp,domain))||
                                  (!CS_WD_SIMP_INEQUALITIES_HOLD(pr,s_next,w)))
                                {
                                  //fprintf(stderr,"  Blocked (out of domain)\n");
                                  blocked=TRUE;
                                }
                              else
                                {
                                  if (checkCollisions)
                                    CS_WD_IN_COLLISION(*collision,pr,s_next,s,w);
                                  if (*collision)
                                    {
                                      //fprintf(stderr,"  Blocked (collision)\n");
                                      blocked=TRUE;
                                    }
                                  else
                                    {
                                      d=DistanceTopology(m,tp,s,s_next);
                                      dtmp=dist+d;

                                      /* Sanity check to avoid infinite connections */
                                      if ((dtmp>md)||(dtmp>2.0*d12)||(DistanceTopology(m,tp,s1,s_next)>1.5*d12))
                                        {
                                          /*
                                          if (dtmp>md)
                                            fprintf(stderr,"  Blocked (max dist reached)\n");
                                          if (dtmp>2.0*d12)
                                            fprintf(stderr,"  Blocked (path too long)\n");
                                          if (DistanceTopology(m,tp,s1,s_next)>1.5*d12)
                                            fprintf(stderr,"  Blocked (too far from origin)\n");
                                          */
                                          blocked=TRUE;
                                        }
                                      else
                                        {
                                          /* Check if it is time to define a new chart along the connection */
                                          /* We do the check before copying s_next to s -> the new chart is
                                             defined in the last good sample */
                                          if (nt/d<(1.0-ce)) /* nt=actual step in tangent space (can be < step at the end) */
                                            {
                                              //fprintf(stderr,"  Change chart (ce)\n");
                                              validChart=FALSE;
                                            }
                                          else
                                            {
                                              dist=dtmp;
                                          
                                              *reached=(DistanceTopology(m,tp,s_next,s2)<delta);
                                              /*
                                              fprintf(stderr,"  New sample (%g)\n",dist);
                                              if (*reached)
                                                fprintf(stderr,"  Reached\n");
                                              */
                                              /* since everything is ok, we will continue from the last point */
                                              memcpy(t,t_next,k*sizeof(double));
                                              memcpy(s,s_next,m*sizeof(double));
                                          
                                              /* t_next and s_next alredy backed up-> we can modify them */
                                              if (tp!=NULL)
                                                ArrayPi2Pi(m,tp,s_next);
                                          
                                              /* Here we have a new valid sample */
                                              if ((ms>0)&&(!(*reached)))
                                                AddSample2Samples(m,s_next,m,NULL,&ms,ns,path);
                                          
                                              first=FALSE; /* we already have one sample */
                                              step=delta; /* the following steps will be normal size */
                                            }
                                        }
                                    }
                                }
                            }
                        }
                    }
                  if ((!validChart)&&(first))
                    { 
                      /* we already have a chart at the last 's' -> we need sample close to 's'
                         where to generate the new chart. This only happens on very curved
                         manifolds or close to singularities. */
                      step*=0.9;
                      if (step<epsilon)
                        {
                          //fprintf(stderr,"  Blocked (impossible)\n");
                          blocked=TRUE;
                        }
                      validChart=TRUE;
                    }
                }
            }
        }
      DeleteChart(&c);
    }

  if (*reached)
    dist+=d; /* epsilon/delta size */
  else
    {
      if (ms>0) 
        {
          DeleteSamples(*ns,*path);
          ns=0;
        }
    }

  if (lastSample==NULL)
    free(s);
  free(s_next);
  free(s_goal);
  free(t);
  free(t_next);
  free(t_goal);
  DeleteJacobian(&sJ);

  return(dist);
}

double ConnectSamples(Tparameters *pr,unsigned int *tp,
                      Tbox *domain,
                      unsigned int m,unsigned int n,
                      double *s1,double *s2,
                      double md,boolean checkCollisions,
                      boolean *reached,boolean *collision,
                      double *lastSample,unsigned int *ns,double ***path,
                      TAtlasBase *w)
{
  boolean blocked;
  double c;
  double *v,*s,*s_next,*s_tmp;
  double delta,epsilon,d,small,d1, d_new;
  unsigned int ms;

  delta=GetParameter(CT_DELTA,pr);
  epsilon=GetParameter(CT_EPSILON,pr);
  small=delta/50; // for delta=0.05 delta/50 gives the value used by Berenson
                  // Use 10 for RRT*, to avoid too irregular samples
  if (tp!=NULL)
    {
      ArrayPi2Pi(m,tp,s1);
      ArrayPi2Pi(m,tp,s2);
    }

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

  /* displacement from current sample to goal */
  NEW(v,m,double);
  /* we keep track of the current sample in the path
     and the previous one to compute the displacement
     between them */
  if (lastSample==NULL)
    { NEW(s,m,double); }
  else
    s=lastSample;
  memcpy(s,s1,sizeof(double)*m);
  NEW(s_next,m,double);
  NEW(s_tmp,m,double);
 
  DifferenceVectorTopology(m,tp,s2,s,v);
  d=Norm(m,v);
  *reached=(d<delta);
  *collision=FALSE;
  blocked=FALSE;

  c=0;

  while ((!*reached)&&(!blocked)&&(!*collision))
    {      
      if (d<delta+small)
        memcpy(s_next,s2,m*sizeof(double));
      else
        {
          SumVectorScale(m,s,delta/d,v,s_next);

          if (n==0)
            blocked=FALSE;
          else
            blocked=(CS_WD_NEWTON_IN_SIMP(pr,s_next,w)==DIVERGED);
        }

      if (!blocked)
        {
          memcpy(s_tmp,s_next,m*sizeof(double));
          if (tp!=NULL)
            ArrayPi2Pi(m,tp,s_tmp);
          if ((!PointInBoxTopology(NULL,FALSE,m,s_tmp,0,tp,domain))||
              (!CS_WD_SIMP_INEQUALITIES_HOLD(pr,s_tmp,w)))
            blocked=TRUE;
          else
            {
              if (checkCollisions)
                CS_WD_IN_COLLISION(*collision,pr,s_next,s,w);
              if (!*collision)
                {
                  d1=DistanceTopology(m,tp,s,s_next);
                  if (d1<small) /* If stalled -> stop */
                    blocked=TRUE;
                  else
                    {
                      if ((c+d1)>md) /* If beyond the max displacement -> stop */
                        blocked=TRUE;
                      else
                        {
                          memcpy(s,s_next,sizeof(double)*m);
                          DifferenceVectorTopology(m,tp,s2,s,v);
                          d_new=Norm(m,v);
                          if(d_new>d) 
                            blocked=TRUE;
                          else 
                            {
                              c+=d1;
                              d=d_new;
                              *reached=(d<epsilon);
                              if ((ms>0)&&(!(*reached)))
                                AddSample2Samples(m,s_next,m,NULL,&ms,ns,path);
                            }
                        }
                    }
                }
            }
        }
    }
  if (*reached)
    c+=d; /* epsilon size */
  else
    {
      if (ms>0) 
        {
          DeleteSamples(*ns,*path);
          ns=0;
        }
    }

  free(v);
  if (lastSample==NULL)
    free(s);
  free(s_next);
  free(s_tmp);

  return(c);
}

double PathLength(unsigned int *tp,unsigned int m,
                  unsigned int np,double **point)
{
  double l;
  unsigned int s;

  l=0.0;
  for(s=1;s<np;s++)
    l+=DistanceTopology(m,tp,point[s-1],point[s]);

  return(l);
}

int cmpUInt(const void *a,const void *b)
{
  int out;

  out=0;

  if ((*(unsigned int *)a)<(*(unsigned int *)b))
    out=-1;
  else
    {
      if ((*(unsigned int *)a)>(*(unsigned int *)b))
        out=+1;
    }
  return(out);
}

void ShortcutSmooth(Tparameters *pr,Tstatistics *stime,
                    unsigned int *np,double ***point,TAtlasBase *w)
{
  unsigned int c,g,b,i,k,m,s,ne;
  double ll,nl;
  unsigned int newNS,lastNS,lastS;
  boolean pathOk,collision,done;
  double **newPath,**lastPath;
  unsigned int *tp;
  Tbox domain;

  CS_WD_GENERATE_SIMP_INITIAL_BOX(pr,&domain,w);
  m=CS_WD_GET_SIMP_TOPOLOGY(pr,&tp,w);
  ne=m-(unsigned int)GetParameter(CT_N_DOF,pr);
  CS_WD_INIT_CD(pr,1,w);

  //fprintf(stderr,"Initial num steps: %u\n",*np);
  c=0; /* current */
  while(c<*np-1)
    {
      /* dichotomic search of the largest cut possible from l and up to the end of the path*/      
      /* 'g' and 'b' provide a lower/upper bound the index of the futhers step that can
         be reached from 'l' without any collision. This searched index is 's' */
      g=c+1;   /* last known good step (step we can directly connect from 'c') */
      b=*np-1; /* last bad step (step that can not be directly connected from 'c') */
      s=g;
      pathOk=FALSE;
      lastPath=NULL;
      done=FALSE;
      while (!done)
        {
          s=(g+b)/2;
          //fprintf(stderr,"[%u - %u - %u]\n",g,s,b);

          ll=PathLength(tp,m,s-c+1,&((*point)[c]));
          nl=ConnectSamplesChart(pr,tp,&domain,m,ne,(*point)[c],(*point)[s],ll,
                                 TRUE,&pathOk,&collision,NULL,
                                 &newNS,&newPath,w);
          if (pathOk)
            {
              /* keep the last valid path */
              if (lastPath!=NULL)
                {
                  for(i=0;i<lastNS;i++)
                    free(lastPath[i]);
                  free(lastPath);
                }
              lastNS=newNS;
              lastPath=newPath;
              lastS=s;

              //fprintf(stderr,"[%u %u] good\n",c,s);
              if (s==g) /*[g,b] has size 0 or 1*/
                done=TRUE;
              else
                g=s+1; /* the searched index is larger than 's' */
            }
          else
            {
              //fprintf(stderr,"[%u %u] blocked\n",c,s);
              if (s==g) /*[g,b] has size 0 or 1*/
                done=TRUE;
              else
                b=s-1; /* the searched index is lower than 's' */
            }
        }
      
      //fprintf(stderr,"Test %u->%u : steps %u->%u  length %g->%g\n",c,lastS,lastS-c+1,lastNS,ll,nl);
      if ((lastPath!=NULL)&&(c<lastS-1)&&(lastNS<lastS-c+1)&&(nl<ll)) /* replace only if theres is a minimum gap and the num
                                                                of steps of the new path is lower */
        {
          //fprintf(stderr,"Shorcut from %u to %u in %u steps (%u) ",c,lastS,lastNS,lastS-c+1);
                      
          /* replace segment l->s with the new path */
          for(i=c+1;i<lastS;i++)
            free((*point)[i]);

          for(i=0,k=c+1;i<lastNS;i++,k++)
            (*point)[k]=lastPath[i];
          free(lastPath);
              
          for(i=lastS,k=c+1+lastNS;i<*np;i++,k++)
            (*point)[k]=(*point)[i];

          *np=c+1+lastNS+(*np-lastS);
          c=c+1+lastNS;

          fprintf(stderr,"Steps: %u  Path length %g  Time: %g\n",*np,
                  PathLength(tp,m,*np,*point),GetElapsedTime(stime));
        }
      else
        /* continue from 'b' */
        c=lastS;
    }
  free(tp);
  DeleteBox(&domain);
}

void RandomSmooth(Tparameters *pr,unsigned int nCores,unsigned int maxIterations,
                  Tstatistics *stime,
                  unsigned int *np,double ***point,TAtlasBase *w)
{
  unsigned int s,n,i,n1,n2,k,ne;
  double nl,pl;
  unsigned int lns;
  double **lpath;
  unsigned int m;
  Tbox domain;
  unsigned int *tp;
  double **newPoint;
  unsigned int nnp;
  boolean reached,collision;
  unsigned int *step,*newStep,j,dnCores,*localLength,*newLocalLength,*newLocalNS;
  unsigned int *first,*middle,*last;
  boolean *localPathOK;
  double ***newLocalPath;
  unsigned int nCuts;

  CS_WD_GENERATE_SIMP_INITIAL_BOX(pr,&domain,w);
  m=CS_WD_GET_SIMP_TOPOLOGY(pr,&tp,w);
  ne=m-(unsigned int)GetParameter(CT_N_DOF,pr);
  CS_WD_INIT_CD(pr,nCores,w);

  //l=PathLength(tp,m,*np,*point);

  if (nCores>1)
    {
      dnCores=2*nCores;
      NEW(step,dnCores,unsigned int);
      NEW(newStep,nCores,unsigned int);
      NEW(first,nCores,unsigned int);
      NEW(middle,nCores,unsigned int);
      NEW(last,nCores,unsigned int);
      NEW(localLength,nCores,unsigned int);
      NEW(localPathOK,nCores,boolean);
      NEW(newLocalLength,nCores,unsigned int);
      NEW(newLocalNS,nCores,unsigned int);
      NEW(newLocalPath,nCores,double**);
    }

  n=(*np);
  for (s=0;s<maxIterations*n;s++)
    {
      if (nCores>1)
        {
          /* Select the start/end of the tentative shortcuts (note that we can have repeated random numbers!) */
          /* It is not convenient to process in parallel too many shorctus since then all shortcuts are
             small which results in poor path improvement. */
          nCuts=randomMax(nCores-2)+1; 
          for(j=0;j<2*nCuts;j++)
            step[j]=randomMax(*np-1);
          qsort(step,2*nCuts,sizeof(unsigned int),cmpUInt);

          /* Compute the possible shorcuts */
          #pragma omp parallel for private(j,n1,n2,collision) schedule(static)
          for(j=0;j<nCuts;j++)
            {
              n1=step[2*j];
              n2=step[2*j+1];

              /* 
                 The path is split in 'nCuts' different segments. For each
                 segment first[j] is the first element in the segment,
                 middle[j] is the element just after the segment to (potentially)
                 improve and last[j] is the last element included in the segment.
              */
              first[j]=n1+1;
              middle[j]=n2+(n1<n2 ? 0:1); /* if n1==n2, n1 belongs to the previous segment, skip it */
              last[j]=(j==nCuts-1?(*np)-1:step[2*j+2]);

              if (first[j]<middle[j])
                {
                  /* We hava at least one point in the segment to optimize */
                  localLength[j]=PathLength(tp,m,n2-n1+1,&((*point)[n1]));
                  newLocalLength[j]=DEFAULT_CONNECT(pr,tp,&domain,m,ne,(*point)[n1],(*point)[n2],localLength[j],
                                                    TRUE,&(localPathOK[j]),&collision,NULL,
                                                    &(newLocalNS[j]),&(newLocalPath[j]),w);
                }
              else
                {
                  localLength[j]=0;
                  newLocalLength[j]=0;
                  localPathOK[j]=FALSE;
                }
            }
          /* New number of path steps taking into account the possible new shortcuts */
          nnp=first[0];
          for(j=0;j<nCuts;j++)
            {
              newStep[j]=nnp;

              if ((localPathOK[j])&&(newLocalLength[j]<localLength[j]))
                nnp+=newLocalNS[j];
              else
                nnp+=(middle[j]-first[j]);

              /* now the connecting segments in betwen the possibly smoothed ones */
              nnp+=(last[j]-middle[j])+1;
            }

          NEW(newPoint,nnp,double*);

          /* The initial non-smoothed segment */
          for(i=0;i<first[0];i++)
            newPoint[i]=(*point)[i];

          /* now the possibly smoothed segments (in parallel since they store
             different parts of the new path) */
          #pragma omp parallel for private(i,j,k) schedule(static)
          for(j=0;j<nCuts;j++)
            {
              k=newStep[j];
              if ((localPathOK[j])&&(newLocalLength[j]<localLength[j]))
                {
                  /* keep the new path segment */
                  for(i=0;i<newLocalNS[j];i++)
                    { newPoint[k]=newLocalPath[j][i]; k++; }

                  /* delete the previous path segment */
                  for(i=step[2*j]+1;i<step[2*j+1];i++)
                    free((*point)[i]);
                }
              else
                {
                  if (first[j]<middle[j])
                    {
                      /* Delete the new path segment, if any */
                      if (localPathOK[j])
                        DeleteSamples(newLocalNS[j],newLocalPath[j]);
                        
                      /* keep the previous path segment */
                      for(i=first[j];i<middle[j];i++)
                        { newPoint[k]=(*point)[i]; k++; }
                    }
                }

              /* The non-smoothed segment till next step (or the tail
                 non-smoothed segment). */
              for(i=middle[j];i<=last[j];i++)
                { newPoint[k]=(*point)[i]; k++; }
            }

          /* update the path */
          free(*point);
          *point=newPoint;
          *np=nnp;
        }
      else
        {
          /* nCores==1 */
          n1=randomMax(*np-2);
          n2=n1+1+randomMax(*np-2-n1);
            
          pl=PathLength(tp,m,n2-n1+1,&((*point)[n1]));  

          nl=DEFAULT_CONNECT(pr,tp,&domain,m,ne,(*point)[n1],(*point)[n2],pl,
                             TRUE,&reached,&collision,NULL,&lns,&lpath,w);
          /*
          if (reached)
            fprintf(stderr,"Test %u->%u. pl:%g  st:%u  -> nl:%g  nst:%u %s\n",n1,n2,pl,n2-n1+2,nl,lns,(((reached)&&(nl<pl))?"(*)":" "));
          else
            fprintf(stderr,"Test %u->%u. pl:%g  st:%u  failed \n",n1,n2,pl,n2-n1+1);
          */
          if ((reached)&&(nl<pl))
            {
              /* Replace the path segment with the new path */

              /* Concat the sequences in a temporary buffer */
              nnp=n1+1+lns+(*np-n2); /* new number of points */
              NEW(newPoint,nnp,double*);
              k=0;
              for(i=0;i<=n1;i++)
                { newPoint[k]=(*point)[i]; k++; }

              for(i=0;i<lns;i++)
                { newPoint[k]=lpath[i]; k++; }

              for(i=n1+1;i<n2;i++)
                free((*point)[i]);
            
              for(i=n2;i<*np;i++)
                { newPoint[k]=(*point)[i]; k++; }
            
              /* Replace the points by the new points */
              free(*point);
              *point=newPoint;
              *np=nnp;
            }
          else
            {
              if (reached)
                DeleteSamples(lns,lpath);
            }
        }

      /* Print info for debug only */
      if (s%10==0)
        fprintf(stderr,"Shortcut (%u/%u)  Path steps: %u  Path length %g  Time: %g\n",
                s,n*maxIterations,*np,PathLength(tp,m,*np,*point),GetElapsedTime(stime));
    }

  if (nCores>1)
    {
      free(step);
      free(newStep);
      free(first);
      free(middle);
      free(last);
      free(localLength);
      free(localPathOK);
      free(newLocalLength);
      free(newLocalNS);
      free(newLocalPath);
    }

  DeleteBox(&domain);
  free(tp);
}

void GradientSmooth(Tparameters *pr,unsigned int nCores,unsigned int maxIterations,
                    Tstatistics *stime,
                    unsigned int *np,double ***point,TAtlasBase *w)
{
  unsigned int i,k,m,n,it;
  double delta,e,ce,r;
  Tbox domain;
  TJacobian sJ;
  unsigned int *tp;
  Tchart *c;
  double **u;
  double **newPoint;
  boolean *stalled;

  k=(unsigned int)GetParameter(CT_N_DOF,pr);
  delta=GetParameter(CT_DELTA,pr);
  e=GetParameter(CT_E,pr);
  ce=GetParameter(CT_CE,pr);
  r=GetParameter(CT_R,pr);

  CS_WD_GENERATE_SIMP_INITIAL_BOX(pr,&domain,w);
  CS_WD_GET_SIMP_JACOBIAN(pr,&sJ,w);  
  CS_WD_GET_SIMP_TOPOLOGY(pr,&tp,w);
  CS_WD_INIT_CD(pr,nCores,w);

  GetJacobianSize(&n,&m,&sJ); /* 'n' not used */

  /* Apply gradient-based length reduction */
  /* Allocate memory for the minimization process */
  NEW(c,*np,Tchart);
  NEW(newPoint,*np,double*);
  NEW(u,*np,double*);
  for(i=0;i<*np;i++)
    {
      NEW(u[i],k,double);
      NEW(newPoint[i],m,double);
    }
  NEW(stalled,*np,boolean);
  for(i=0;i<*np;i++)
    stalled[i]=FALSE;

  for(it=0;it<maxIterations;it++)
    {
      #pragma omp parallel for private(i) schedule(dynamic) if(nCores>1)
      for(i=1;i<*np-1;i++)
        {
          if (!stalled[i])
            {
              /* Define chart */
              if (InitChart(pr,TRUE,&domain,tp,
                            m,k,(*point)[i],e,ce,r,
                            &sJ,w,&(c[i]))==0)
                {
                  double cost,newCost;

                  /* set u=-delta*T'*(2*x_i-x_{i-1}-x_{i+1}) */
                  memcpy(newPoint[i],(*point)[i],m*sizeof(double));
                  ScaleVector(2.0,m,newPoint[i]);
                  SubtractVector(m,newPoint[i],(*point)[i-1]);
                  SubtractVector(m,newPoint[i],(*point)[i+1]);
                  TMatrixVectorProduct(m,k,GetChartTangentSpace(&(c[i])),newPoint[i],u[i]);
                  //ScaleVector(-0.1*delta/Norm(k,u[i]),k,u[i]);
                  ScaleVector(-0.1*delta/Norm(k,u[i]),k,u[i]);

                  cost=(DistanceTopology(m,tp,(*point)[i-1],(*point)[i])+
                        DistanceTopology(m,tp,(*point)[i],(*point)[i+1]));

                  /* a loop to ensurue that the step is not too large */
                  do {
                    /* Chart to manifold with 'u' */
                    if (Chart2Manifold(pr,&sJ,u[i],tp,(*point)[i],newPoint[i],&(c[i]))>e)
                      stalled[i]=TRUE;
                    else
                      {
                        /* Test if collision or out of domain */
                        CS_WD_IN_COLLISION(stalled[i],pr,newPoint[i],NULL,w);
                        stalled[i]=((stalled[i])||
                                    (!PointInBoxTopology(NULL,FALSE,m,newPoint[i],0.0,tp,&domain)));
                        
                        newCost=(DistanceTopology(m,tp,(*point)[i-1],newPoint[i])+
                                 DistanceTopology(m,tp,newPoint[i],(*point)[i+1]));
                      }

                    ScaleVector(0.5,k,u[i]);
                  } while ((!stalled[i])&&(newCost>cost));

                  //fprintf(stderr,"er[%u]: %f\n",i,Distance(m,(*point)[i],newPoint[i]));
              
                  /* Delete the chart */
                  DeleteChart(&(c[i]));
                }
              else
                stalled[i]=TRUE;
            }
        }

      /* Update the path with the possibly new points */
      #pragma omp parallel for private(i)
      for(i=1;i<*np-1;i++)
        {
          if (!stalled[i])
            memcpy((*point)[i],newPoint[i],m*sizeof(double));
        }

      /* Print 'l' */
      fprintf(stderr,"Iteration %u  Path length %g  Time: %g\n",
              it,PathLength(tp,m,*np,*point),GetElapsedTime(stime));
    }

  for(i=0;i<*np;i++)
    {
      free(u[i]);
      free(newPoint[i]);
    }
  free(u);
  free(newPoint);
  free(c);
  free(stalled);

  DeleteBox(&domain);
  free(tp);
  DeleteJacobian(&sJ);
}

void SmoothSamples(Tparameters *pr,boolean parallel,int mode,unsigned int maxIterations,
                   unsigned int ns,double **path,
                   unsigned int *sns,double ***spath,TAtlasBase *w)
{
  unsigned int i,m,np,mp;
  double **point;
  double *o,l,delta;
  unsigned int *tp;
  unsigned int sms;
  unsigned int nv,nvs;
  boolean *systemVars;
  Tstatistics stime;  
  unsigned int nCores;
  
  if (ns<3)
    Error("Too small path in SmoothSamples (only paths with at least 3 samples are considered)");

  if (parallel)
    {
      #ifdef _OPENMP
        nCores=omp_get_max_threads();
        if (ns<nCores*3)
          nCores=1;
      #else
        nCores=1;
      #endif
    }
  else
    nCores=1;

  delta=GetParameter(CT_DELTA,pr);

  InitStatistics(nCores,0,&stime);

  m=CS_WD_GET_SIMP_TOPOLOGY(pr,&tp,w);
  
  mp=ns;
  np=ns;

  NEW(point,mp,double*);
  #pragma omp parallel for private(i) schedule(dynamic)
  for(i=0;i<np;i++)
    {
      double *od;
      
      /* recover the dummy variables from the system ones
         for the point in original system */
      CS_WD_REGENERATE_SOLUTION_POINT(pr,path[i],&od,w);
      /* Get the point in the simplified form */
      CS_WD_GENERATE_SIMPLIFIED_POINT(pr,od,&(point[i]),w);

      free(od);
    }
  /********************************************/
  /* Compute the initial lenght */
  l=PathLength(tp,m,np,point);
  fprintf(stderr,"Initial lenght                    : %g\n",l);
  fprintf(stderr,"Number of processing cores        : %u\n",nCores);

  /* If the path is not densily sampled the Euclidean 
     distance between samples is too far from the Geodesic
     one and then the smoothing is not meaningful */
  if ((l/(double)np)>2*delta)
    Error("The input path is not dense enough");

  switch(mode)
    {
    case SMOOTH_RANDOM:
      fprintf(stderr,"Smooth method                     : RANDOM\n");
      RandomSmooth(pr,nCores,maxIterations,&stime,&np,&point,w);
      break;
    case SMOOTH_GRADIENT:
      fprintf(stderr,"Smooth method                     : GRADIENT\n");
      GradientSmooth(pr,1,maxIterations,&stime,&np,&point,w);
      break;
    case SMOOTH_SHORTCUT:
      fprintf(stderr,"Smooth method                     : SHORTCUT\n");
      ShortcutSmooth(pr,&stime,&np,&point,w);
      break;
    default:
      Error("Unknown smoothing method in ");
    }

  fprintf(stderr,"Final lengtht %g\n",PathLength(tp,m,np,point));

  /********************************************/
  /* The minimization is done, now define the new optimized path */
  nv=CS_WD_GET_SYSTEM_VARS(&systemVars,w);
  nvs=0;
  for(i=0;i<nv;i++)
    {
      if (systemVars[i])
        nvs++;
    }
  sms=np;
  NEW(*spath,sms,double*);
  *sns=0;
  for(i=0;i<np;i++)
    {
      CS_WD_REGENERATE_ORIGINAL_POINT(pr,point[i],&o,w);
      AddSample2Samples(nv,o,nvs,systemVars,&sms,sns,spath);
      free(o);
    }
  free(systemVars);

  /********************************************/
  /* Release the data used to represent the path */
  for(i=0;i<np;i++)
    free(point[i]);
  free(point);

  free(tp);

  fprintf(stderr,"Optimization time: %g\n",GetElapsedTime(&stime));
  DeleteStatistics(&stime);
}

void ConcatSamples(unsigned int nvs,
                   unsigned int ns1,double **path1,
                   unsigned int ns2,double **path2,
                   unsigned int *ns,double ***path)
{
  unsigned int i;

  *ns=(ns1+ns2);
  NEW((*path),*ns,double*);
  for(i=0;i<ns1;i++)
    {
      NEW((*path)[i],nvs,double);
      memcpy((*path)[i],path1[i],nvs*sizeof(double));
    }
  for(i=0;i<ns2;i++)
    {
      NEW((*path)[ns1+i],nvs,double);
      memcpy((*path)[ns1+i],path2[i],nvs*sizeof(double));
    }
}

void ReverseConcatSamples(unsigned int nvs,
                          unsigned int ns1,double **path1,
                          unsigned int ns2,double **path2,
                          unsigned int *ns,double ***path)
{
  unsigned int i;
  signed int s;

  *ns=(ns1+ns2);

  NEW((*path),*ns,double*);
  for(i=0;i<ns1;i++)
    {
      NEW((*path)[i],nvs,double);
      memcpy((*path)[i],path1[i],nvs*sizeof(double));
    }
  s=ns2-1;
  for(i=0;i<ns2;i++)
    {
      NEW((*path)[ns1+i],nvs,double);
      memcpy((*path)[ns1+i],path2[s],nvs*sizeof(double));
      s--;
    }
}

void ReverseSamples(unsigned int ns,double **path)
{
  if (ns>0)
    {
      unsigned int s,k;
      double *step;
      
      s=ns-1;
      k=0;
      while(s>k)
        {
          step=path[s];
          path[s]=path[k];
          path[k]=step;
          s--;
          k++;
        }
    }
}

unsigned int ReadOneSample(Tparameters *p,char *fname,unsigned int nvs,double **s)
{
  unsigned int i;
  Tfilename fsamples;
  FILE *f;
  unsigned int r;

  NEW(*s,nvs,double);

  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
  if (r==REP_JOINTS)
    CreateFileName(NULL,fname,NULL,JOINTS_EXT,&fsamples);
  else
    CreateFileName(NULL,fname,NULL,LINKS_EXT,&fsamples);
  f=fopen(GetFileFullName(&fsamples),"r");
  if (!f)
    Error("Could not open sample file");

  fprintf(stderr,"Reading samples from              : %s\n",GetFileFullName(&fsamples));
  for(i=0;i<nvs;i++)
    {
      if (fscanf(f,"%lf",&((*s)[i]))!=1)
        Error("Sample in file with less values than system variables");
    } 
  fclose(f);

  DeleteFileName(&fsamples);

  return(nvs);
}

unsigned int ReadTwoSamples(Tparameters *p,char *fname,unsigned int nvs,double **s1,double **s2)
{
  unsigned int i;
  Tfilename fsamples;
  FILE *f;
  unsigned int r;

  NEW(*s1,nvs,double);
  NEW(*s2,nvs,double);

  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
  if (r==REP_JOINTS)
    CreateFileName(NULL,fname,NULL,JOINTS_EXT,&fsamples);
  else
    CreateFileName(NULL,fname,NULL,LINKS_EXT,&fsamples);
  f=fopen(GetFileFullName(&fsamples),"r");
  if (!f)
    Error("Could not open sample file");
  
  fprintf(stderr,"Reading samples from              : %s\n",GetFileFullName(&fsamples));
  for(i=0;i<nvs;i++)
    {
      if (fscanf(f,"%lf",&((*s1)[i]))!=1)
        Error("Sample in file with less values than system variables");
    } 
  for(i=0;i<nvs;i++)
    {
      if (fscanf(f,"%lf",&((*s2)[i]))!=1)
        Error("Sample in file with less values than system variables");
    }
  fclose(f);

  DeleteFileName(&fsamples);

  return(nvs);
}

void SaveSamplesInt(Tfilename *fpath,unsigned int nvs,
                    unsigned int ns,double **path)
{ 
  Tbox bp;
  unsigned int i,j;
  FILE *f;

  f=fopen(GetFileFullName(fpath),"w");
  if (!f)
    Error("Could not open file to store the solution path on mainfold");

  fprintf(stderr,"Writing solution path to          : %s\n",GetFileFullName(fpath));

  fprintf(f,"%u\n",ns);
  for(i=0;i<ns;i++)
    {
      for(j=0;j<nvs;j++)
        fprintf(f,"%.12f ",path[i][j]);

      fprintf(f,"\n");
    }

  /*We will also write the path in the form of boxes (for animation)*/
  for(i=0;i<ns;i++)
    {
      InitBoxFromPoint(nvs,path[i],&bp);
      PrintBox(f,&bp);
      DeleteBox(&bp);
    }

  fclose(f);
}


void SaveSamples(char *fname,boolean smooth,unsigned int nvs,
                 unsigned int ns,double **path)
{
  Tfilename fpath;

  if (smooth)
    CreateFileName(NULL,fname,"_spath",SOL_EXT,&fpath);
  else
    CreateFileName(NULL,fname,"_path",SOL_EXT,&fpath);

  SaveSamplesInt(&fpath,nvs,ns,path);

  DeleteFileName(&fpath);
}

void SaveSamplesN(char *fname,boolean smooth,unsigned int n,unsigned int nvs,
                  unsigned int ns,double **path)
{
  Tfilename fpath;
  char s[100];

  if (smooth)
    sprintf(s,"_spath_%u",n);
  else
    sprintf(s,"_path_%u",n);
  
  CreateFileName(NULL,fname,s,SOL_EXT,&fpath);

  SaveSamplesInt(&fpath,nvs,ns,path);

  DeleteFileName(&fpath);
}


boolean LoadSamples(Tfilename fname, unsigned int *nvs,
                    unsigned int *ns,double ***path)
{
  FILE *f;
  Tlist lb;
  Tbox *b;
  Titerator i;
  unsigned int k;

  f=fopen(GetFileFullName(&fname),"r");
  if (f)
    {
      /*Load the path from a _path.sol file */
      fprintf(stderr,"Loading solution path from        : %s\n",GetFileFullName(&fname));

      /* We take advantage that the sample list is stored as a box list to read it */
      ReadListOfBoxes(GetFileFullName(&fname),&lb);

      *ns=ListSize(&lb);
      NEW(*path,*ns,double *);
      InitIterator(&i,&lb);
      First(&i);
      k=0;
      while(!EndOfList(&i))
        {
          b=(Tbox *)GetCurrent(&i);
          if (k==0)
            *nvs=GetBoxNIntervals(b); 
          else
            {
              if (*nvs!=GetBoxNIntervals(b))
                Error("Incongruent size of step in path");
            }
          NEW((*path)[k],*nvs,double);
          GetBoxCenter(NULL,(*path)[k],b);
          Advance(&i);
          k++;
        }
      fclose(f);
      DeleteListOfBoxes(&lb);

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

void PlotSamples(Tparameters *p,Tplot3d *p3d,
                 unsigned int xID,unsigned int yID,unsigned int zID,
                 unsigned int ns,double **path)
{
  unsigned int i;
  double *x,*y,*z;
  Tcolor pathColor;
  double cx,cy,cz;
  #if (RRT_PATH_NODES)
    double xc[2],yc[2],zc[2];
    Tcolor blue;
  #endif

  cx=GetParameter(CT_CUT_X,p);
  cy=GetParameter(CT_CUT_Y,p);
  cz=GetParameter(CT_CUT_Z,p);

  NEW(x,ns,double);
  NEW(y,ns,double);
  NEW(z,ns,double);

  for(i=0;i<ns;i++)
    {         
      /* For the paths we only store system variables */
      x[i]=path[i][xID];
      y[i]=path[i][yID];
      z[i]=path[i][zID];

      if ((cx<0)&&(x[i]<cx))
        x[i]+=M_2PI;
      if ((cx>0)&&(x[i]>cx))
        x[i]-=M_2PI;
      if ((cy<0)&&(y[i]<cy))
        y[i]+=M_2PI;
      if ((cy>0)&&(y[i]>cy))
        y[i]-=M_2PI;
      if ((cz<0)&&(z[i]<cz))
        z[i]+=M_2PI;
      if ((cz>0)&&(z[i]>cz))
        z[i]-=M_2PI; 
    }

  NewColor(0,1,0,&pathColor); /* green */
  StartNew3dObject(&pathColor,p3d);
  PlotVect3d(ns,x,y,z,p3d);
  Close3dObject(p3d);
  DeleteColor(&pathColor);

  #if (RRT_PATH_NODES)
  if (ns>2)
    {
      NewColor(0,0,1,&blue);
      StartNew3dObject(&blue,p3d);
      for(i=0;i<ns;i++)
        {
          xc[0]=x[i]-0.01;
          yc[0]=y[i];
          zc[0]=z[i];

          xc[1]=x[i]+0.01;
          yc[1]=y[i];
          zc[1]=z[i];

          PlotVect3d(2,xc,yc,zc,p3d);

          xc[0]=x[i];
          yc[0]=y[i]-0.01;
          zc[0]=z[i];

          xc[1]=x[i];
          yc[1]=y[i]+0.01;
          zc[1]=z[i];

          PlotVect3d(2,xc,yc,zc,p3d);

          xc[0]=x[i];
          yc[0]=y[i];
          zc[0]=z[i]-0.01;

          xc[1]=x[i];
          yc[1]=y[i];
          zc[1]=z[i]+0.01;

          PlotVect3d(2,xc,yc,zc,p3d);
        }
  
      DeleteColor(&blue);
      Close3dObject(p3d);  
    }
  #endif

  free(x);
  free(y);
  free(z);
}

void DeleteSamples(unsigned int ns,double **path)
{
  unsigned int i;

  for(i=0;i<ns;i++)
    free(path[i]);
  free(path); 
}