samples.c
Go to the documentation of this file.
1 #include "samples.h"
2 
3 #include "filename.h"
4 #include "algebra.h"
5 #include "parameters.h"
6 #include "list.h"
7 #include "random.h"
8 
9 #include <stdlib.h>
10 #include <string.h>
11 #include <math.h>
12 
44 #define DISPERSION 1
45 
54 #define REMOVE_POINTS ((DISPERSION)&&(1))
55 
56 
69 #define APPROXIMATED_CHARTS_IN_GRADIENT 1
70 
101 double StepDispersion(Tparameters *p,unsigned int *tp,
102  boolean *sv,unsigned int m,
103  double *xp,double *x,double *xn,
104  unsigned int current,
105  Tchart *cp,Tchart *c,Tchart *cn,
106  Tworld *w);
107 
152 boolean StepDispersionGradient(Tparameters *p,unsigned int *tp,
153  boolean *sv,unsigned int m,
154  double *xpp,double *xp,double *x,double *xn,double *xnn,
155  Tchart *cpp,Tchart *cp,Tchart *c,Tchart *cn,Tchart *cnn,
156  TJacobian *sJ,TStepCost cost,
157  double *u,Tworld *w);
158 
180 double StepLength(Tparameters *p,unsigned int *tp,
181  boolean *sv,unsigned int m,
182  double *xp,double *x,double *xn,
183  unsigned int current,
184  Tchart *cp,Tchart *c,Tchart *cn,
185  Tworld *w);
186 
221 boolean StepLengthGradient(Tparameters *p,unsigned int *tp,
222  boolean *sv,unsigned int m,
223  double *xpp,double *xp,double *x,double *xn,double *xnn,
224  Tchart *cpp,Tchart *cp,Tchart *c,Tchart *cn,Tchart *cnn,
225  TJacobian *sJ,TStepCost cost,
226  double *u,Tworld *w);
259 double StepEffort(Tparameters *p,unsigned int *tp,
260  boolean *sv,unsigned int m,
261  double *xp,double *x,double *xn,
262  unsigned int current,
263  Tchart *cp,Tchart *c,Tchart *cn,Tworld *w);
264 
298 boolean StepEffortGradient(Tparameters *p,unsigned int *tp,
299  boolean *sv,unsigned int m,
300  double *xpp,double *xp,double *x,double *xn,double *xnn,
301  Tchart *cpp,Tchart *cp,Tchart *c,Tchart *cn,Tchart *cnn,
302  TJacobian *sJ,TStepCost cost,
303  double *u,Tworld *w);
304 
336 boolean StepCostNumericalGradient(Tparameters *p,unsigned int *tp,
337  boolean *sv,unsigned int m,
338  double *xpp,double *xp,double *x,double *xn,double *xnn,
339  Tchart *cpp,Tchart *cp,Tchart *c,Tchart *cn,Tchart *cnn,
340  TJacobian *sJ,TStepCost cost,
341  double *u,Tworld *w);
366 double CombineCosts(TStepCost c1,TStepCost c2,
367  Tparameters *p,unsigned int *tp,
368  boolean *sv,unsigned int m,
369  double *xp,double *x,double *xn,
370  unsigned int current,
371  Tchart *cp,Tchart *c,Tchart *cn,Tworld *w);
372 
408  Tparameters *p,unsigned int *tp,
409  boolean *sv,unsigned int m,
410  double *xpp,double *xp,double *x,double *xn,double *xnn,
411  Tchart *cpp,Tchart *cp,Tchart *c,Tchart *cn,Tchart *cnn,
412  TJacobian *sJ,TStepCost cost,
413  double *u,Tworld *w);
414 
438 double LengthAndDispersion(Tparameters *p,unsigned int *tp,
439  boolean *sv,unsigned int m,
440  double *xp,double *x,double *xn,
441  unsigned int current,
442  Tchart *cp,Tchart *c,Tchart *cn,Tworld *w);
443 
476 boolean LengthAndDispersionGradient(Tparameters *p,unsigned int *tp,
477  boolean *sv,unsigned int m,
478  double *xpp,double *xp,double *x,double *xn,double *xnn,
479  Tchart *cpp,Tchart *cp,Tchart *c,Tchart *cn,Tchart *cnn,
480  TJacobian *sJ,TStepCost cost,
481  double *u,Tworld *w);
506 double EffortAndDispersion(Tparameters *p,unsigned int *tp,
507  boolean *sv,unsigned int m,
508  double *xp,double *x,double *xn,
509  unsigned int current,
510  Tchart *cp,Tchart *c,Tchart *cn,Tworld *w);
511 
544 boolean EffortAndDispersionGradient(Tparameters *p,unsigned int *tp,
545  boolean *sv,unsigned int m,
546  double *xpp,double *xp,double *x,double *xn,double *xnn,
547  Tchart *cpp,Tchart *cp,Tchart *c,Tchart *cn,Tchart *cnn,
548  TJacobian *sJ,TStepCost cost,
549  double *u,Tworld *w);
550 
562 int cmpUInt(const void *a,const void *b);
563 
578 void ShortcutSmooth(Tparameters *pr,boolean *sv,Tstatistics *stime,
579  unsigned int *np,double ***point,TAtlasBase *w);
580 
603 void RandomSmooth(Tparameters *pr,unsigned int nCores,unsigned int maxIterations,
604  boolean *sv,Tstatistics *stime,
605  unsigned int *np,double ***point,TAtlasBase *w);
606 
637 void GradientSmooth(Tparameters *pr,
638  unsigned int nCores,unsigned int maxIterations,
639  boolean *sv,Tstatistics *stime,
640  unsigned int *np,double ***point,
641  TStepCost stepCost,
642  TStepCostGradient stepCostGradient,
643  TAtlasBase *w);
644 
645 
646 
658 void SaveSamplesInt(Tfilename *fpath,unsigned int nvs,
659  unsigned int ns,double **path);
660 
661 /********************************************************************/
662 /********************************************************************/
663 /********************************************************************/
664 
665 void InitSamples(unsigned int *ms,unsigned int *ns,double ***path)
666 {
667  *ms=INIT_NUM_SAMPLES;
668  *ns=0;
669  NEW((*path),*ms,double *);
670 }
671 
672 void AddSample2Samples(unsigned int nv,double *sample,
673  unsigned int nvs,boolean *systemVars,
674  unsigned int *ms,unsigned int *ns,double ***path)
675 {
676  unsigned int i,k;
677 
678  if (*ns==*ms)
679  MEM_DUP(*path,*ms,double *);
680 
681  NEW((*path)[*ns],nvs,double);
682  k=0;
683  for(i=0;i<nv;i++)
684  {
685  if ((systemVars==NULL)||(systemVars[i]))
686  {
687  (*path)[*ns][k]=sample[i];
688  k++;
689  }
690  }
691  (*ns)++;
692 }
693 
694 double ConnectSamplesChart(Tparameters *pr,unsigned int *tp,
695  boolean *sv,Tbox *domain,
696  unsigned int m,unsigned int n,
697  double *s1,double *s2,
698  double md,boolean checkCollisions,
699  TJacobian *sJ,
700  boolean *reached,boolean *collision,
701  double *lastSample,unsigned int *ns,double ***path,
702  TAtlasBase *w)
703 {
704  boolean blocked,validChart,first;
705  double dist;
706  double *s,*s_next,*s_goal,*t,*t_next,*t_goal;
707  unsigned int i,k,a,b;
708  double delta,step,epsilon,d,df,dtmp,distFull,d12,e,ce,r,nt;
709  unsigned int ms;
710  Tchart c;
711 
712  epsilon=GetParameter(CT_EPSILON,pr);
713  delta=GetParameter(CT_DELTA,pr);
714  k=(unsigned int)GetParameter(CT_N_DOF,pr);
715  e=GetParameter(CT_E,pr);
716  ce=GetParameter(CT_CE,pr);
717  r=GetParameter(CT_R,pr);
718 
719  /* Assume that the CD is already initialized */
720 
721  GetJacobianSize(&a,&b,sJ);
722  if (b!=m)
723  Error("Jacobian size missmatch in ConnectSamplesChart");
724 
725  if ((ns!=NULL)&&(path!=NULL))
726  InitSamples(&ms,ns,path);
727  else
728  ms=0;
729 
730  /* we keep track of the current sample in the path
731  and the previous one to compute the displacement
732  between them */
733  if (lastSample==NULL)
734  { NEW(s,m,double); }
735  else
736  s=lastSample;
737  memcpy(s,s1,m*sizeof(double)); /* current sample */
738  NEW(s_next,m,double); /* space for the next sample */
739  NEW(s_goal,m,double); /* goal sample projected to the current chart */
740  NEW(t,k,double); /* parameters for 's' */
741  NEW(t_next,k,double); /* parameters for 's_next' */
742  NEW(t_goal,k,double); /* parameters for 's_goal' */
743 
744  d12=DistanceTopology(m,tp,s1,s2);
745  dist=0.0;
746  distFull=0.0;
747  *reached=(DistanceTopology(m,tp,s1,s2)<delta);
748  *collision=FALSE;
749  blocked=FALSE;
750 
751  while ((!*reached)&&(!blocked)&&(!*collision))
752  {
753  /* Initialize the chart at the current sample */
754  if (tp!=NULL)
755  ArrayPi2Pi(m,tp,s);
756 
757  //fprintf(stderr,"New chart\n");
758  if (InitChart(pr,TRUE,domain,tp,m,k,s,e,ce,r,sJ,w,&c)!=0)
759  {
760  //fprintf(stderr," Blocked (error init chart)\n");
761  blocked=TRUE;
762  }
763  else
764  {
765  /* 's' is 0 in the new chart */
766  for(i=0;i<k;i++)
767  t[i]=0.0;
768 
769  /* Determine the advance direction */
770  DifferenceVectorTopology(m,tp,s2,s,s_goal);
771  AccumulateVector(m,s,s_goal);
772  Manifold2Chart(s_goal,NULL,t_goal,&c); /* without topology now */
773  nt=Norm(k,t_goal);
774  if (nt<1e-3)
775  {
776  //fprintf(stderr," Blocked (small projection)\n");
777  blocked=TRUE; /* it is not clear which direction to take */
778  }
779  else
780  {
781  //fprintf(stderr,"tg:%g\n",Norm(k,t_goal));
782  /* when far from the goal, we move the goal far enough to ensure
783  the creation of new charts, if necessary. */
784  if (DistanceTopology(m,tp,s2,s)>r)
785  ScaleVector(2.0*r/nt,k,t_goal); /* move the goal far enough */
786 
787  /* Move in the current chart as far as possible */
788  validChart=TRUE;
789  step=delta;
790  first=TRUE;
791  while ((validChart)&&(!*reached)&&(!blocked)&&(!*collision))
792  {
793  /* Define t_next from t and t_goal */
794  DifferenceVector(k,t_goal,t,t_next);
795  nt=Norm(k,t_next);
796  if (nt>step)
797  {
798  SumVectorScale(k,t,step/nt,t_next,t_next);
799  nt=step;
800  }
801  else
802  AccumulateVector(k,t,t_next);
803  //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));
804 
805  if (0) //(Norm(k,t_next)>r) /* TO BE REMOVED */
806  {
807  /* We use the current chart as much as possible. 'r' limits the use of the
808  chart without any reason. It is only useful when storing the charts to
809  generate regular pavings of the manifold. */
810  //fprintf(stderr," Change chart (r)\n");
811  validChart=FALSE;
812  }
813  else
814  {
815  /* and project to the manifold to obtain q_next */
816  if (Chart2Manifold(pr,sJ,t_next,NULL,s,s_next,&c)>e)
817  {
818  //fprintf(stderr," Change chart (e)\n");
819  validChart=FALSE;
820  }
821  else
822  {
823  if ((nt<epsilon)&&(DistanceTopology(m,tp,s_next,s2)>delta))
824  {
825  /* we reached the targed in tangent space but the projection does not correspond
826  to the actual goal. Try to advance generating a new chart. */
827  if (first)
828  {
829  /* Note that it is not worth to reduce the step since we are already taking
830  steps of size epsilon. */
831  //fprintf(stderr," Blocked (tangent vs ambient)\n");
832  blocked=TRUE;
833  }
834  else
835  {
836  /* The current sample does not have a chart. Try to define a new chart on in
837  Hopefully, the projection of the goal in the new chart will define a more
838  clear advance direction. */
839  //fprintf(stderr," Change chart (tangent vs ambient)\n");
840  validChart=FALSE;
841  }
842  }
843  else
844  {
845  if ((!PointInBoxTopology(NULL,FALSE,m,s_next,epsilon,tp,domain))||
846  (!CS_WD_SIMP_INEQUALITIES_HOLD(pr,s_next,w)))
847  {
848  //fprintf(stderr," Blocked (out of domain)\n");
849  blocked=TRUE;
850  }
851  else
852  {
853  if (checkCollisions)
854  CS_WD_IN_COLLISION(*collision,pr,s_next,s,w);
855  if (*collision)
856  {
857  //fprintf(stderr," Blocked (collision)\n");
858  blocked=TRUE;
859  }
860  else
861  {
862  df=DistanceTopology(m,tp,s,s_next);
863  if (sv==NULL)
864  d=df;
865  else
866  d=DistanceTopologySubset(m,tp,sv,s,s_next);
867  dtmp=dist+d;
868  distFull+=df;
869 
870  /* Sanity check to avoid infinite connections or even oscilations.
871  The constants (2 and 1.5) could be changed, but they are resonable in our experience. */
872  if ((dtmp>md)||(distFull>2.0*d12)||(DistanceTopology(m,tp,s1,s_next)>1.5*d12))
873  {
874  /*
875  if (dtmp>md)
876  fprintf(stderr," Blocked (max dist reached)\n");
877  if (dtmp>2.0*d12)
878  fprintf(stderr," Blocked (path too long)\n");
879  if (DistanceTopology(m,tp,s1,s_next)>1.5*d12)
880  fprintf(stderr," Blocked (too far from origin)\n");
881  */
882  blocked=TRUE;
883  }
884  else
885  {
886  /* Check if it is time to define a new chart along the connection */
887  /* We do the check before copying s_next to s -> the new chart is
888  defined in the last good sample */
889  if (nt/df<(1.0-ce)) /* nt=actual step in tangent space (can be < step at the end) */
890  {
891  //fprintf(stderr," Change chart (ce)\n");
892  validChart=FALSE;
893  }
894  else
895  {
896  dist=dtmp;
897 
898  *reached=(DistanceTopology(m,tp,s_next,s2)<delta);
899  /*
900  fprintf(stderr," New sample (%g)\n",dist);
901  if (*reached)
902  fprintf(stderr," Reached\n");
903  */
904  /* since everything is ok, we will continue from the last point */
905  memcpy(t,t_next,k*sizeof(double));
906  memcpy(s,s_next,m*sizeof(double));
907 
908  /* t_next and s_next alredy backed up-> we can modify them */
909  if (tp!=NULL)
910  ArrayPi2Pi(m,tp,s_next);
911 
912  /* Here we have a new valid sample */
913  if ((ms>0)&&(!(*reached)))
914  AddSample2Samples(m,s_next,m,NULL,&ms,ns,path);
915 
916  first=FALSE; /* we already have one sample */
917  step=delta; /* the following steps will be normal size */
918  }
919  }
920  }
921  }
922  }
923  }
924  }
925  if ((!validChart)&&(first))
926  {
927  /* we already have a chart at the last 's' -> we need sample close to 's'
928  where to generate the new chart. This only happens on very curved
929  manifolds or close to singularities. */
930  step*=0.9;
931  if (step<epsilon)
932  {
933  //fprintf(stderr," Blocked (impossible)\n");
934  blocked=TRUE;
935  }
936  validChart=TRUE;
937  }
938  }
939  }
940  DeleteChart(&c);
941  }
942  }
943 
944  if (*reached)
945  dist+=d; /* epsilon/delta size */
946  else
947  {
948  if (ms>0)
949  {
950  DeleteSamples(*ns,*path);
951  ns=0;
952  }
953  }
954 
955  if (lastSample==NULL)
956  free(s);
957  free(s_next);
958  free(s_goal);
959  free(t);
960  free(t_next);
961  free(t_goal);
962 
963  return(dist);
964 }
965 
966 double ConnectSamples(Tparameters *pr,unsigned int *tp,
967  boolean *sv,Tbox *domain,
968  unsigned int m,unsigned int n,
969  double *s1,double *s2,
970  double md,boolean checkCollisions,
971  TJacobian *sJ,
972  boolean *reached,boolean *collision,
973  double *lastSample,unsigned int *ns,double ***path,
974  TAtlasBase *w)
975 {
976  boolean blocked;
977  double c;
978  double *v,*s,*s_next,*s_tmp;
979  double delta,epsilon,d,small,d1, d_new;
980  unsigned int ms;
981 
982  delta=GetParameter(CT_DELTA,pr);
983  epsilon=GetParameter(CT_EPSILON,pr);
984  small=delta/50; // for delta=0.05 delta/50 gives the value used by Berenson
985  // Use 10 for RRT*, to avoid too irregular samples
986  if (tp!=NULL)
987  {
988  ArrayPi2Pi(m,tp,s1);
989  ArrayPi2Pi(m,tp,s2);
990  }
991 
992  if ((ns!=NULL)&&(path!=NULL))
993  InitSamples(&ms,ns,path);
994  else
995  ms=0;
996 
997  /* displacement from current sample to goal */
998  NEW(v,m,double);
999  /* we keep track of the current sample in the path
1000  and the previous one to compute the displacement
1001  between them */
1002  if (lastSample==NULL)
1003  { NEW(s,m,double); }
1004  else
1005  s=lastSample;
1006  memcpy(s,s1,sizeof(double)*m);
1007  NEW(s_next,m,double);
1008  NEW(s_tmp,m,double);
1009 
1010  DifferenceVectorTopology(m,tp,s2,s,v);
1011  d=Norm(m,v);
1012  *reached=(d<delta);
1013  *collision=FALSE;
1014  blocked=FALSE;
1015 
1016  c=0;
1017 
1018  while ((!*reached)&&(!blocked)&&(!*collision))
1019  {
1020  if (d<delta+small)
1021  memcpy(s_next,s2,m*sizeof(double));
1022  else
1023  {
1024  SumVectorScale(m,s,delta/d,v,s_next);
1025 
1026  if (n==0)
1027  blocked=FALSE;
1028  else
1029  blocked=(CS_WD_NEWTON_IN_SIMP(pr,s_next,w)==DIVERGED);
1030  }
1031 
1032  if (!blocked)
1033  {
1034  memcpy(s_tmp,s_next,m*sizeof(double));
1035  if (tp!=NULL)
1036  ArrayPi2Pi(m,tp,s_tmp);
1037  if ((!PointInBoxTopology(NULL,FALSE,m,s_tmp,epsilon,tp,domain))||
1038  (!CS_WD_SIMP_INEQUALITIES_HOLD(pr,s_tmp,w)))
1039  blocked=TRUE;
1040  else
1041  {
1042  if (checkCollisions)
1043  CS_WD_IN_COLLISION(*collision,pr,s_next,s,w);
1044  if (!*collision)
1045  {
1046  d1=DistanceTopology(m,tp,s,s_next);
1047  if (d1<small) /* If stalled -> stop */
1048  blocked=TRUE;
1049  else
1050  {
1051  if (sv!=NULL)
1052  d1=DistanceTopologySubset(m,tp,sv,s,s_next);
1053  if ((c+d1)>md) /* If beyond the max displacement -> stop */
1054  blocked=TRUE;
1055  else
1056  {
1057  memcpy(s,s_next,sizeof(double)*m);
1058  DifferenceVectorTopology(m,tp,s2,s,v);
1059  d_new=Norm(m,v);
1060  if(d_new>d)
1061  blocked=TRUE;
1062  else
1063  {
1064  c+=d1;
1065  d=d_new;
1066  *reached=(d<epsilon);
1067  if ((ms>0)&&(!(*reached)))
1068  AddSample2Samples(m,s_next,m,NULL,&ms,ns,path);
1069  }
1070  }
1071  }
1072  }
1073  }
1074  }
1075  }
1076  if (*reached)
1077  c+=DistanceTopologySubset(m,tp,sv,s,s2); /* epsilon size */
1078  else
1079  {
1080  if (ms>0)
1081  {
1082  DeleteSamples(*ns,*path);
1083  ns=0;
1084  }
1085  }
1086 
1087  free(v);
1088  if (lastSample==NULL)
1089  free(s);
1090  free(s_next);
1091  free(s_tmp);
1092 
1093  return(c);
1094 }
1095 
1096 double PathLength(unsigned int *tp,
1097  boolean *sv,unsigned int m,
1098  unsigned int np,double **point)
1099 {
1100  double l;
1101  unsigned int i;
1102 
1103  /* do not execute in parallel: it's simple and it's used
1104  inside other parellel context. */
1105  l=0.0;
1106  for(i=1;i<np;i++)
1107  l+=DistanceTopologySubset(m,tp,sv,point[i-1],point[i]);
1108 
1109  return(l);
1110 }
1111 
1112 double StepDispersion(Tparameters *p,unsigned int *tp,
1113  boolean *sv,unsigned int m,
1114  double *xp,double *x,double *xn,
1115  unsigned int current,
1116  Tchart *cp,Tchart *c,Tchart *cn,
1117  Tworld *w)
1118 {
1119  double d1,d2,e;
1120 
1121  /* dispersion is not defined for the first/last step on the path */
1122  if ((xp!=NULL)&&(x!=NULL)&&(xn!=NULL))
1123  {
1124  d1=DistanceTopologySubset(m,tp,sv,xp,x);
1125  d2=DistanceTopologySubset(m,tp,sv,x,xn);
1126  e=(d1-d2);
1127  e*=e;
1128  }
1129  else
1130  e=0.0;
1131 
1132  return(e);
1133 }
1134 
1135 boolean StepDispersionGradient(Tparameters *p,unsigned int *tp,
1136  boolean *sv,unsigned int m,
1137  double *xpp,double *xp,double *x,double *xn,double *xnn,
1138  Tchart *cpp,Tchart *cp,Tchart *c,Tchart *cn,Tchart *cnn,
1139  TJacobian *sJ,TStepCost cost,
1140  double *u,Tworld *w)
1141 {
1142  unsigned int k;
1143  double epsilon;
1144  double *s1,*s2,*g;
1145  double d0,d1,d2,d3,c1,c2;
1146  boolean ok=TRUE;
1147 
1148  if ((xp==NULL)||(x==NULL)||(xn==NULL))
1149  ok=FALSE;
1150  else
1151  {
1152  k=(unsigned int)GetParameter(CT_N_DOF,p);
1153  epsilon=GetParameter(CT_EPSILON,p);
1154 
1155  NEW(s1,3*m,double);
1156  s2=&(s1[m]);
1157  g=&(s2[m]);
1158 
1159  DifferenceVectorTopologySubset(m,tp,sv,x,xp,s1);
1160  d1=Norm(m,s1);
1161  if (d1<epsilon)
1162  ok=FALSE;
1163  else
1164  {
1165  DifferenceVectorTopologySubset(m,tp,sv,xn,x,s2);
1166  d2=Norm(m,s2);
1167  if (d2<epsilon)
1168  ok=FALSE;
1169  else
1170  {
1171  if (xpp==NULL)
1172  c1=d1-d2;
1173  else
1174  {
1175  d0=DistanceTopologySubset(m,tp,sv,xpp,xp);
1176  c1=2*d1-d0-d2;
1177  }
1178  if (xnn==NULL)
1179  c2=-(d1-d2);
1180  else
1181  {
1182  d3=DistanceTopologySubset(m,tp,sv,xn,xnn);
1183  c2=2*d2-d1-d3;
1184  }
1185 
1186  memcpy(g,s1,m*sizeof(double));
1187  ScaleVector(2*c1/d1,m,g);
1188  SumVectorScale(m,g,-2*c2/d2,s2,g);
1189 
1190  if (k==m)
1191  memcpy(u,g,k*sizeof(double));
1192  else
1194  }
1195  }
1196 
1197  free(s1);
1198  }
1199 
1200  return(ok);
1201 }
1202 
1203 double StepLength(Tparameters *p,unsigned int *tp,
1204  boolean *sv,unsigned int m,
1205  double *xp,double *x,double *xn,
1206  unsigned int current,
1207  Tchart *cp,Tchart *c,Tchart *cn,Tworld *w)
1208 {
1209  double cost;
1210 
1211  if (current>2)
1212  Error("Wrong 'current' parameter in StepLength");
1213 
1214  if (current==2)
1215  cost=0.0;
1216  else
1217  /* path length from xp to x */
1218  /* d = norm(x-xp) = sqrt( sum_j (x^j-xp^j)^2 ) */
1219  cost=DistanceTopologySubset(m,tp,sv,x,xp);
1220 
1221  return(cost);
1222 }
1223 
1224 boolean StepLengthGradient(Tparameters *p,unsigned int *tp,
1225  boolean *sv,unsigned int m,
1226  double *xpp,double *xp,double *x,double *xn,double *xnn,
1227  Tchart *cpp,Tchart *cp,Tchart *c,Tchart *cn,Tchart *cnn,
1228  TJacobian *sJ,TStepCost cost,
1229  double *u,Tworld *w)
1230 {
1231  boolean ok=FALSE;
1232 
1233  if ((xp!=NULL)&&(x!=NULL)&&(xn!=NULL))
1234  {
1235  double d,epsilon,*v,*g;
1236  unsigned int k;
1237 
1238  epsilon=GetParameter(CT_EPSILON,p);
1239  k=(unsigned int)GetParameter(CT_N_DOF,p);
1240 
1241  NEW(v,2*m,double);
1242  g=&(v[m]);
1243 
1244  /* gradient = diff d / diff x1 */
1245  DifferenceVectorTopologySubset(m,tp,sv,x,xp,g);
1246  d=Norm(m,g);
1247  if (d>epsilon)
1248  {
1249  ScaleVector(1.0/d,m,g);
1250 
1251  /* gradient = diff d / diff x2 */
1252  DifferenceVectorTopologySubset(m,tp,sv,xn,x,v);
1253  d=Norm(m,v);
1254  if (d>epsilon)
1255  {
1256  SumVectorScale(m,g,-1.0/d,v,g);
1257  ok=TRUE;
1258  }
1259  }
1260 
1261  if (ok)
1262  {
1263  /* Project to tangent space: the only feasible motions are in this space */
1264  if (k==m)
1265  memcpy(u,g,k*sizeof(double));
1266  else
1268  }
1269 
1270  free(v);
1271  }
1272  return(ok);
1273 }
1274 
1275 double StepEffort(Tparameters *p,unsigned int *tp,
1276  boolean *sv,unsigned int m,
1277  double *xp,double *x,double *xn,
1278  unsigned int current,
1279  Tchart *cp,Tchart *c,Tchart *cn,Tworld *w)
1280 {
1281  double epsilon,cost;
1282  double *s; /* speed vector in ambient space */
1283  double *coa; /* control in ambient space */
1284  double *co; /* control in tangent space */
1285  double d; /* norm of s */
1286  double *f; /* force field in ambient space */
1287  unsigned int k;
1288 
1289  if (w==NULL)
1290  Error("Control effort can not be computed on cuik files");
1291 
1292  /* The computation of the control effort relies on some assumptions that only
1293  hold when working in full configuration space and not only on a sub-set of
1294  it. For instance, the control projects vectors to the tangent space (as
1295  defined in the full conf. space) and the vectors from a point to the next
1296  must be close to this tangent space. This is typically the case for paths
1297  with small steps in conf. space, but not for paths when projected to a
1298  subspace, even if the steps are small. */
1299  if (KINEMATIC_SUBSPACE)
1300  Error("ControlEffort assumes full configuration space (not kinematic sub-space)");
1301 
1302  if (current>2)
1303  Error("Wrong 'current' parameter in StepEffort");
1304 
1305  if (current==2)
1306  cost=0.0;
1307  else
1308  {
1309  /* control effort for going from xp to x */
1310  epsilon=GetParameter(CT_EPSILON,p);
1311  k=(unsigned int)GetParameter(CT_N_DOF,p);
1312  cost=0.0;
1313 
1314  NEW(s,2*m+k,double);
1315  DifferenceVectorTopologySubset(m,tp,sv,x,xp,s);
1316  d=Norm(m,s);
1317  if (d>epsilon)
1318  {
1319  coa=&(s[m]);
1320  co=&(s[2*m]);
1321 
1322  ScaleVector(1.0/d,m,s);
1323 
1324  WorldForceField(p,TRUE,xp,&f,w);
1325  DifferenceVector(m,s,f,coa); /* c=s-f */
1326  if (k==m)
1327  cost=GeneralDotProduct(m,coa,coa)*d;
1328  else
1329  {
1331  cost=GeneralDotProduct(k,co,co)*d;
1332  }
1333 
1334  free(f);
1335  }
1336  else
1337  cost=d;
1338 
1339  free(s);
1340  }
1341  return(cost);
1342 }
1343 
1344 boolean StepEffortGradient(Tparameters *p,unsigned int *tp,
1345  boolean *sv,unsigned int m,
1346  double *xpp,double *xp,double *x,double *xn,double *xnn,
1347  Tchart *cpp,Tchart *cp,Tchart *c,Tchart *cn,Tchart *cnn,
1348  TJacobian *SJ,TStepCost cost,
1349  double *u,Tworld *w)
1350 {
1351  boolean ok=FALSE;
1352 
1353  if ((xp!=NULL)&&(x!=NULL)&&(xn!=NULL))
1354  {
1355  unsigned int k,j;
1356  double *g,*f,*fg,*coa,*s,*co,*proj,d,nco,epsilon;
1357 
1358  if (w==NULL)
1359  Error("Control effort can not be computed on cuik files");
1360 
1361  epsilon=GetParameter(CT_EPSILON,p);
1362  k=(unsigned int)GetParameter(CT_N_DOF,p);
1363 
1364  NEW(s,4*m+k,double);
1365  coa=&(s[m]);
1366  proj=&(coa[m]);
1367  g=&(proj[m]);
1368  co=&(g[m]);
1369 
1370  /*
1371  cost(previous->current) = co^T co * d
1372  with
1373  co = Phi_p^T (s-f)
1374  Phi_p -> tangent space at xp
1375  s = unit speed at xp
1376  f = force field at xp
1377  d = distance xp->x (norm(x-xp))
1378 
1379  diff cost(previous->current) / diff x
1380  diff cost = diff (co^T co) * d
1381  + co^T co * diff d^T
1382  with
1383  diff (co^T co) = 2 * co^T * diff co
1384  diff co = Phi_p^T * [ diff s - diff f ]
1385  diff s = diff (x-xp)/d
1386  = 1/d^2 * [ diff (x-xp) * d - (x-xp) * diff d^T]
1387  diff (x-xp) = I_m (identity of size 'm' with 1's
1388  only in the 'sv' entries)
1389  diff d = s
1390  diff f = 0
1391  So summarizing
1392  diff cost = 2 * co^T * Phi_p^T * [ I_m - s * s^T]
1393  + co^T co * s^T
1394  = [ 2 * Phi_p * co ]^T * [ I_m - s * s^T]
1395  + co^T co * s^T
1396  = proj^T * [ I_m - s * s^T] + nco * s^T
1397  with
1398  proj = [ 2 * Phi_p * co ]
1399  nco = co^T co
1400  The most efficient evaluation of this formula is (avoid the generation
1401  of large matrices):
1402  diff cost = proj^T - [ proj^T * s ] * s^T + nco * s^T
1403  diff cost = proj^T + [ nco - proj^T * s ] * s^T
1404  */
1405  /* s = (x-xp) / | x-xp |
1406  We store the values in s_i actually since this is the space for this thread */
1407  DifferenceVectorTopologySubset(m,tp,sv,x,xp,s);
1408  d=Norm(m,s);
1409  if (d>epsilon)
1410  {
1411  ScaleVector(1.0/d,m,s);
1412 
1413  /* co = Phi_p^T * (s-f) */
1414  WorldForceField(p,TRUE,xp,&f,w);
1415  DifferenceVector(m,s,f,coa);
1416  if (k==m)
1417  memcpy(co,coa,k*sizeof(double));
1418  else
1420 
1421  /* nco = co^T * co */
1422  nco=GeneralDotProduct(k,co,co);
1423 
1424  /* p = 2 * \Phi_p * co */
1425  if (k==m)
1426  memcpy(proj,co,m*sizeof(double));
1427  else
1428  MatrixVectorProduct(m,k,GetChartTangentSpace(cp),co,proj);
1429  ScaleVector(2,m,proj);
1430 
1431  /* g = proj^T (only for the 'sv' entries) */
1432  if (sv==NULL)
1433  memcpy(g,proj,m*sizeof(double));
1434  else
1435  {
1436  for(j=0;j<m;j++)
1437  {
1438  if (sv[j])
1439  g[j]=proj[j];
1440  else
1441  g[j]=0.0;
1442  }
1443  }
1444 
1445  /* g -= [ proj_^T * s + nco] * s^T*/
1446  SumVectorScale(m,g,nco-GeneralDotProduct(m,proj,s),s,g);
1447 
1448  free(f);
1449 
1450  ok=TRUE;
1451  }
1452 
1453  if (ok)
1454  {
1455  /*
1456  cost(current->next) = co^T co * d
1457  with
1458  co = Phi^T (s-f)
1459  Phi -> tangent space at x
1460  s = unit speed at x
1461  f = force field at x
1462  d = distance x->xn (norm(xn-x))
1463 
1464  diff cost(current->next) / diff x
1465  diff cost = diff (co^T co) * d
1466  + co^T co * diff d^T
1467  with
1468  diff (co^T co) = 2 * co^T * diff co
1469  diff co = Phi^T * [ diff s - diff f ]
1470  diff s = diff (xn-x)/d
1471  = 1/d^2 * [ diff (xn-x) * d - (xn-x) * diff d^T]
1472  diff (xn-x) = -I_m (identity of size 'm' with 1's
1473  only in the 'sv' entries)
1474  diff d = -s
1475  diff f = H (hessian matix)
1476  So summarizing
1477  diff cost = 2 * co^T * Phi_1^T * [ -I_m + s * s^T - H * d ]
1478  - co^T co * s^T
1479  = [ 2 * Phi_1 * co ]^T * [ -I_m + s * s^T - H * d ]
1480  - co^T co * s^T
1481  = proj^T * [ -I_m + s * s^T - H * d ] - nco * s^T
1482  with
1483  proj = [ 2 * Phi_1 * co ]
1484  nco = co^T co
1485  The most efficient evaluation of this formula is (avoid the generation
1486  of large matrices):
1487  diff cost = - proj^T + [ proj^T * s -nco ] * s^T - d * pH
1488  with
1489  pH = proj^T*H
1490 
1491  Note that actually Phi would change with x, but we assume it constant
1492  (we approximate Phi(x) by Phi at x
1493  */
1494  DifferenceVectorTopologySubset(m,tp,sv,xn,x,s);
1495  d=Norm(m,s);
1496  if (d<epsilon)
1497  ok=FALSE;
1498  else
1499  {
1500  ScaleVector(1/d,m,s);
1501 
1502  /* co = Phi^T * (s-f) */
1503  WorldForceField(p,TRUE,x,&f,w);
1504  DifferenceVector(m,s,f,coa);
1505  if (k==m)
1506  memcpy(co,coa,k*sizeof(double));
1507  else
1509 
1510  /* nco = co^T * co */
1511  nco=GeneralDotProduct(k,co,co);
1512 
1513  /* p = 2 * \Phi * co */
1514  if (k==m)
1515  memcpy(proj,co,m*sizeof(double));
1516  else
1517  MatrixVectorProduct(m,k,GetChartTangentSpace(c),co,proj);
1518  ScaleVector(2,m,proj);
1519 
1520  /* g -= proj^T (only for the 'sv' entries) */
1521  if (sv==NULL)
1522  SumVectorScale(m,g,-1,proj,g);
1523  else
1524  {
1525  for(j=0;j<m;j++)
1526  {
1527  if (sv[j])
1528  g[j]-=proj[j];
1529  }
1530  }
1531 
1532  /* g += [ proj_^T * s - nco] * s^T*/
1533  SumVectorScale(m,g,GeneralDotProduct(m,proj,s)-nco,s,g);
1534 
1535  /* g -= d * ( proj^T * H ) */
1536  WorldForceFieldProjectedGradient(p,TRUE,proj,x,&fg,w);
1537  SumVectorScale(m,g,-d,fg,g);
1538 
1539  free(f);
1540  free(fg);
1541  }
1542  }
1543 
1544  if (ok)
1545  {
1546  /* Project to tangent space: the only feasible motions are in this space */
1547  if (k==m)
1548  memcpy(u,g,k*sizeof(double));
1549  else
1551  }
1552 
1553  free(s);
1554  }
1555  return(ok);
1556 }
1557 
1558 boolean StepCostNumericalGradient(Tparameters *p,unsigned int *tp,
1559  boolean *sv,unsigned int m,
1560  double *xpp,double *xp,double *x,double *xn,double *xnn,
1561  Tchart *cpp,Tchart *cp,Tchart *c,Tchart *cn,Tchart *cnn,
1562  TJacobian *sJ,TStepCost cost,
1563  double *u,Tworld *w)
1564 {
1565  boolean ok=FALSE;
1566 
1567  if (x!=NULL)
1568  {
1569  double epsilon,st,ct,new_ct,*delta,*newX;
1570  unsigned int i,k;
1571 
1572  epsilon=GetParameter(CT_EPSILON,p);
1573  k=(unsigned int)GetParameter(CT_N_DOF,p);
1574 
1575  st=5*epsilon; /* small step */
1576 
1577  ct=cost(p,tp,sv,m,xpp,xp,x,2,cpp,cp,c,w)+
1578  cost(p,tp,sv,m,xp,x,xn,1,cp,cn,c,w)+
1579  cost(p,tp,sv,m,x,xn,xnn,0,c,cn,cnn,w);
1580 
1581  NEW(delta,k+m,double);
1582  newX=&(delta[k]);
1583  for(i=0;i<k;i++)
1584  delta[i]=0.0;
1585 
1586  ok=TRUE;
1587  for(i=0;((ok)&&(i<k));i++)
1588  {
1589  delta[i]=st;
1590 
1591  if (Chart2Manifold(p,sJ,delta,tp,NULL,newX,c)==INF)
1592  ok=FALSE;
1593  else
1594  {
1595  /* newCost using 'c' as an approximation of the chart at newX */
1596  new_ct=cost(p,tp,sv,m,xpp,xp,newX,2,cpp,cp,c,w)+
1597  cost(p,tp,sv,m,xp,newX,xn,1,cp,c,cn,w)+
1598  cost(p,tp,sv,m,newX,xn,xnn,0,c,cn,cnn,w);
1599 
1600  u[i]=(new_ct-ct)/st;
1601 
1602  delta[i]=0.0;
1603  }
1604  }
1605 
1606  free(delta);
1607  }
1608  return(ok);
1609 }
1610 
1611 inline double CombineCosts(TStepCost c1,TStepCost c2,
1612  Tparameters *p,unsigned int *tp,
1613  boolean *sv,unsigned int m,
1614  double *xp,double *x,double *xn,
1615  unsigned int current,
1616  Tchart *cp,Tchart *c,Tchart *cn,Tworld *w)
1617 {
1618  return(c1(p,tp,sv,m,xp,x,xn,current,cp,c,cn,w)+
1619  c2(p,tp,sv,m,xp,x,xn,current,cp,c,cn,w));
1620 }
1621 
1623  TStepCostGradient g2,
1624  Tparameters *p,unsigned int *tp,
1625  boolean *sv,unsigned int m,
1626  double *xpp,double *xp,double *x,double *xn,double *xnn,
1627  Tchart *cpp,Tchart *cp,Tchart *c,Tchart *cn,Tchart *cnn,
1628  TJacobian *sJ,TStepCost cost,
1629  double *u,Tworld *w)
1630 {
1631  boolean ok;
1632  unsigned int k;
1633  double *ud;
1634 
1635  k=(unsigned int)GetParameter(CT_N_DOF,p);
1636  NEW(ud,k,double);
1637 
1638  ok=g1(p,tp,sv,m,
1639  xpp,xp,x,xn,xnn,
1640  cpp,cp,c,cn,cnn,
1641  sJ,StepLength,u,w);
1642 
1643  if (ok)
1644  {
1645  ok=g2(p,tp,sv,m,
1646  xpp,xp,x,xn,xnn,
1647  cpp,cp,c,cn,cnn,
1648  sJ,StepDispersion,ud,w);
1649  if (ok)
1650  SumVector(k,u,ud,u);
1651  }
1652 
1653  free(ud);
1654 
1655  return(ok);
1656 }
1657 
1658 inline double LengthAndDispersion(Tparameters *p,unsigned int *tp,
1659  boolean *sv,unsigned int m,
1660  double *xp,double *x,double *xn,
1661  unsigned int current,
1662  Tchart *cp,Tchart *c,Tchart *cn,Tworld *w)
1663 {
1665  p,tp,sv,m,xp,x,xn,current,cp,c,cn,w));
1666 }
1667 
1668 inline boolean LengthAndDispersionGradient(Tparameters *p,unsigned int *tp,
1669  boolean *sv,unsigned int m,
1670  double *xpp,double *xp,double *x,double *xn,double *xnn,
1671  Tchart *cpp,Tchart *cp,Tchart *c,Tchart *cn,Tchart *cnn,
1672  TJacobian *sJ,TStepCost cost,
1673  double *u,Tworld *w)
1674 {
1676  p,tp,sv,m,
1677  xpp,xp,x,xn,xnn,
1678  cpp,cp,c,cn,cnn,
1679  sJ,StepLength,u,w));
1680 }
1681 
1682 
1683 inline double EffortAndDispersion(Tparameters *p,unsigned int *tp,
1684  boolean *sv,unsigned int m,
1685  double *xp,double *x,double *xn,
1686  unsigned int current,
1687  Tchart *cp,Tchart *c,Tchart *cn,Tworld *w)
1688 {
1690  p,tp,sv,m,xp,x,xn,current,cp,c,cn,w));
1691 }
1692 
1693 inline boolean EffortAndDispersionGradient(Tparameters *p,unsigned int *tp,
1694  boolean *sv,unsigned int m,
1695  double *xpp,double *xp,double *x,double *xn,double *xnn,
1696  Tchart *cpp,Tchart *cp,Tchart *c,Tchart *cn,Tchart *cnn,
1697  TJacobian *sJ,TStepCost cost,
1698  double *u,Tworld *w)
1699 {
1701  p,tp,sv,m,
1702  xpp,xp,x,xn,xnn,
1703  cpp,cp,c,cn,cnn,
1704  sJ,StepLength,u,w));
1705 }
1706 
1707 
1709  unsigned int m,unsigned int np,double **point,
1710  Tworld *w)
1711 {
1712  TAtlasBase wb;
1713  unsigned int i;
1714  unsigned int ms;
1715  double cost;
1716  double **pointS;
1717  Tbox domain;
1718  unsigned int *tp;
1719  unsigned int k;
1720  double e,ce,r;
1721  Tchart *c;
1722  TJacobian sJ;
1723  boolean *sv;
1724  double *ct;
1725 
1726  if (np<2)
1727  Error("Too short path in PathEffort");
1728 
1729  CS_WD_FROM_WORLD(w,&wb);
1730 
1731  GetWorldSimpInitialBox(p,&domain,w);
1732  ms=GetWorldSimpTopology(p,&tp,w);
1733 
1734  if (KINEMATIC_SUBSPACE)
1735  WorldSimpKinematicVars(p,&sv,w);
1736  else
1737  sv=NULL;
1738 
1739  /* simplify the input points and generate a chart on each
1740  one of the simplified points */
1741  NEW(pointS,np,double*);
1742  k=(unsigned int)GetParameter(CT_N_DOF,p);
1743  if (k==0)
1744  {
1745  k=WorldManifoldDimension(p,point[0],w);
1746  ChangeParameter(CT_N_DOF,(double)k,p);
1747  }
1748  e=GetParameter(CT_E,p);
1749  ce=GetParameter(CT_CE,p);
1750  r=GetParameter(CT_R,p);
1751  GetWorldSimpJacobian(p,&sJ,w);
1752  NEW(c,np,Tchart);
1753  NEW(ct,np,double);
1754 
1755  #pragma omp parallel for private(i) schedule(dynamic)
1756  for(i=0;i<np;i++)
1757  {
1758  double *od;
1759 
1760  /* recover the dummy variables from the system ones
1761  for the point in original system */
1762  RegenerateWorldSolutionPoint(p,point[i],&od,w);
1763  /* Get the point in the simplified form */
1764  WorldGenerateSimplifiedPoint(p,od,&(pointS[i]),w);
1765  free(od);
1766 
1767  if (InitTrustedChart(p,TRUE,&domain,tp,
1768  ms,k,pointS[i],e,ce,r,
1769  &sJ,&wb,&(c[i]))!=0)
1770  Error("Can not init a chart in a path step");
1771  }
1772 
1773  #pragma omp parallel for private(i) schedule(dynamic)
1774  for(i=1;i<np;i++)
1775  ct[i]=StepEffort(p,tp,sv,ms,
1776  pointS[i-1],pointS[i],NULL,
1777  1,
1778  &(c[i-1]),&(c[i]),NULL,
1779  w);
1780 
1781  cost=0.0;
1782  for(i=1;i<np;i++)
1783  cost+=ct[i];
1784 
1785  #pragma omp parallel for private(i) schedule(dynamic)
1786  for(i=0;i<np;i++)
1787  {
1788  free(pointS[i]);
1789  DeleteChart(&(c[i]));
1790  }
1791  free(ct);
1792  free(pointS);
1793  free(c);
1794  DeleteJacobian(&sJ);
1795  DeleteBox(&domain);
1796  free(tp);
1797  if (sv!=NULL)
1798  free(sv);
1799 
1800  return(cost);
1801 }
1802 
1803 int cmpUInt(const void *a,const void *b)
1804 {
1805  int out;
1806 
1807  out=0;
1808 
1809  if ((*(unsigned int *)a)<(*(unsigned int *)b))
1810  out=-1;
1811  else
1812  {
1813  if ((*(unsigned int *)a)>(*(unsigned int *)b))
1814  out=+1;
1815  }
1816  return(out);
1817 }
1818 
1819 void ShortcutSmooth(Tparameters *pr,boolean *sv,Tstatistics *stime,
1820  unsigned int *np,double ***point,TAtlasBase *w)
1821 {
1822  unsigned int c,g,b,i,k,m,s,ne;
1823  double ll,nl;
1824  unsigned int newNS,lastNS,lastS;
1825  boolean pathOk,collision,done;
1826  double **newPath,**lastPath;
1827  unsigned int *tp;
1828  Tbox domain;
1829  TJacobian sJ;
1830 
1831  CS_WD_GENERATE_SIMP_INITIAL_BOX(pr,&domain,w);
1832  m=CS_WD_GET_SIMP_TOPOLOGY(pr,&tp,w);
1833  ne=m-(unsigned int)GetParameter(CT_N_DOF,pr);
1834  CS_WD_INIT_CD(pr,1,w);
1835  CS_WD_GET_SIMP_JACOBIAN(pr,&sJ,w);
1836 
1837  //fprintf(stderr,"Initial num steps: %u\n",*np);
1838  c=0; /* current */
1839  while(c<*np-1)
1840  {
1841  /* dichotomic search of the largest cut possible from l and up to the end of the path*/
1842  /* 'g' and 'b' provide a lower/upper bound the index of the futhers step that can
1843  be reached from 'l' without any collision. This searched index is 's' */
1844  g=c+1; /* last known good step (step we can directly connect from 'c') */
1845  b=*np-1; /* last bad step (step that can not be directly connected from 'c') */
1846  s=g;
1847  pathOk=FALSE;
1848  lastPath=NULL;
1849  done=FALSE;
1850  while (!done)
1851  {
1852  s=(g+b)/2;
1853  //fprintf(stderr,"[%u - %u - %u]\n",g,s,b);
1854 
1855  ll=PathLength(tp,sv,m,s-c+1,&((*point)[c]));
1856 
1857  nl=DEFAULT_CONNECT(pr,tp,sv,&domain,m,ne,(*point)[c],(*point)[s],ll,
1858  TRUE,&sJ,&pathOk,&collision,NULL,
1859  &newNS,&newPath,w);
1860 
1861  //nl=10000;pathOk=FALSE;
1862  if (pathOk)
1863  {
1864  /* keep the last valid path */
1865  if (lastPath!=NULL)
1866  {
1867  for(i=0;i<lastNS;i++)
1868  free(lastPath[i]);
1869  free(lastPath);
1870  }
1871  lastNS=newNS;
1872  lastPath=newPath;
1873  lastS=s;
1874 
1875  // fprintf(stderr," [%u %u] good\n",c,s);
1876  if (s==g) /*[g,b] has size 0 or 1*/
1877  done=TRUE;
1878  else
1879  g=s+1; /* the searched index is larger than 's' */
1880  }
1881  else
1882  {
1883  // fprintf(stderr," [%u %u] blocked (%u)\n",c,s,collision);
1884  if (s==g) /*[g,b] has size 0 or 1*/
1885  done=TRUE;
1886  else
1887  b=s-1; /* the searched index is lower than 's' */
1888  }
1889  }
1890 
1891  //fprintf(stderr,"Test %u->%u : steps %u->%u length %g->%g\n",c,lastS,lastS-c+1,lastNS,ll,nl);
1892  if ((lastPath!=NULL)&&(c<lastS-1)&&(lastNS<lastS-c+1)&&(nl<ll)) /* replace only if theres is a minimum gap and the num
1893  of steps of the new path is lower */
1894  {
1895  // fprintf(stderr,"Shorcut from %u to %u in %u steps (%u) ",c,lastS,lastNS,lastS-c+1);
1896 
1897  /* replace segment c->lastS with the new path */
1898  for(i=c+1;i<lastS;i++)
1899  free((*point)[i]);
1900 
1901  for(i=0,k=c+1;i<lastNS;i++,k++)
1902  (*point)[k]=lastPath[i];
1903  free(lastPath);
1904 
1905  for(i=lastS,k=c+1+lastNS;i<*np;i++,k++)
1906  (*point)[k]=(*point)[i];
1907 
1908  *np=c+1+lastNS+(*np-lastS);
1909  c=c+1+lastNS;
1910 
1911  fprintf(stderr,"Steps: %u Path length %g Time: %g\n",*np,
1912  PathLength(tp,sv,m,*np,*point),GetElapsedTime(stime));
1913  }
1914  else
1915  {
1916  if (lastPath!=NULL)
1917  DeleteSamples(lastNS,lastPath);
1918  /* continue from 'b' */
1919  c=lastS;
1920  }
1921  }
1922  DeleteJacobian(&sJ);
1923  free(tp);
1924  DeleteBox(&domain);
1925 }
1926 
1927 void RandomSmooth(Tparameters *pr,unsigned int nCores,unsigned int maxIterations,
1928  boolean *sv,Tstatistics *stime,
1929  unsigned int *np,double ***point,TAtlasBase *w)
1930 {
1931  unsigned int s,n,i,n1,n2,k,ne;
1932  double nl,pl;
1933  unsigned int lns;
1934  double **lpath;
1935  unsigned int m;
1936  Tbox domain;
1937  unsigned int *tp;
1938  double **newPoint;
1939  unsigned int nnp;
1940  boolean reached,collision;
1941  unsigned int *step,*newStep,j,dnCores,*localLength,*newLocalLength,*newLocalNS;
1942  unsigned int *first,*middle,*last;
1943  boolean *localPathOK;
1944  double ***newLocalPath;
1945  unsigned int nCuts;
1946  TJacobian sJ;
1947 
1948  CS_WD_GENERATE_SIMP_INITIAL_BOX(pr,&domain,w);
1949  m=CS_WD_GET_SIMP_TOPOLOGY(pr,&tp,w);
1950  ne=m-(unsigned int)GetParameter(CT_N_DOF,pr);
1951  CS_WD_INIT_CD(pr,nCores,w);
1952  CS_WD_GET_SIMP_JACOBIAN(pr,&sJ,w); /* depending on the ConnectSamples method this might be
1953  not used. */
1954 
1955  if (nCores>1)
1956  {
1957  dnCores=2*nCores;
1958  NEW(step,dnCores,unsigned int);
1959  NEW(newStep,nCores,unsigned int);
1960  NEW(first,nCores,unsigned int);
1961  NEW(middle,nCores,unsigned int);
1962  NEW(last,nCores,unsigned int);
1963  NEW(localLength,nCores,unsigned int);
1964  NEW(localPathOK,nCores,boolean);
1965  NEW(newLocalLength,nCores,unsigned int);
1966  NEW(newLocalNS,nCores,unsigned int);
1967  NEW(newLocalPath,nCores,double**);
1968  }
1969 
1970  n=(*np);
1971  for (s=0;s<maxIterations*n;s++)
1972  {
1973  if (nCores>1)
1974  {
1975  /* Select the start/end of the tentative shortcuts (note that we can have repeated random numbers!) */
1976  /* It is not convenient to process in parallel too many shorctus since then all shortcuts are
1977  small which results in poor path improvement. */
1978  nCuts=randomMax(nCores-2)+1;
1979  for(j=0;j<2*nCuts;j++)
1980  step[j]=randomMax(*np-1);
1981  qsort(step,2*nCuts,sizeof(unsigned int),cmpUInt);
1982 
1983  /* Compute the possible shorcuts */
1984  #pragma omp parallel for private(j,n1,n2,collision) schedule(static) if(nCores>1)
1985  for(j=0;j<nCuts;j++)
1986  {
1987  n1=step[2*j];
1988  n2=step[2*j+1];
1989 
1990  /*
1991  The path is split in 'nCuts' different segments. For each
1992  segment first[j] is the first element in the segment,
1993  middle[j] is the element just after the segment to (potentially)
1994  improve and last[j] is the last element included in the segment.
1995  */
1996  first[j]=n1+1;
1997  middle[j]=n2+(n1<n2 ? 0:1); /* if n1==n2, n1 belongs to the previous segment, skip it */
1998  last[j]=(j==nCuts-1?(*np)-1:step[2*j+2]);
1999 
2000  if (first[j]<middle[j])
2001  {
2002  /* We hava at least one point in the segment to optimize */
2003  localLength[j]=PathLength(tp,sv,m,n2-n1+1,&((*point)[n1]));
2004  newLocalLength[j]=DEFAULT_CONNECT(pr,tp,sv,&domain,m,ne,(*point)[n1],(*point)[n2],localLength[j],
2005  TRUE,&sJ,&(localPathOK[j]),&collision,NULL,
2006  &(newLocalNS[j]),&(newLocalPath[j]),w);
2007  }
2008  else
2009  {
2010  localLength[j]=0;
2011  newLocalLength[j]=0;
2012  localPathOK[j]=FALSE;
2013  }
2014  }
2015  /* New number of path steps taking into account the possible new shortcuts */
2016  nnp=first[0];
2017  for(j=0;j<nCuts;j++)
2018  {
2019  newStep[j]=nnp;
2020 
2021  if ((localPathOK[j])&&(newLocalLength[j]<localLength[j]))
2022  nnp+=newLocalNS[j];
2023  else
2024  nnp+=(middle[j]-first[j]);
2025 
2026  /* now the connecting segments in betwen the possibly smoothed ones */
2027  nnp+=(last[j]-middle[j])+1;
2028  }
2029 
2030  NEW(newPoint,nnp,double*);
2031 
2032  /* The initial non-smoothed segment */
2033  for(i=0;i<first[0];i++)
2034  newPoint[i]=(*point)[i];
2035 
2036  /* now the possibly smoothed segments (in parallel since they store
2037  different parts of the new path) */
2038  #pragma omp parallel for private(i,j,k) schedule(static) if(nCores>1)
2039  for(j=0;j<nCuts;j++)
2040  {
2041  k=newStep[j];
2042  if ((localPathOK[j])&&(newLocalLength[j]<localLength[j]))
2043  {
2044  /* keep the new path segment */
2045  for(i=0;i<newLocalNS[j];i++)
2046  { newPoint[k]=newLocalPath[j][i]; k++; }
2047 
2048  /* delete the previous path segment */
2049  for(i=step[2*j]+1;i<step[2*j+1];i++)
2050  free((*point)[i]);
2051  }
2052  else
2053  {
2054  if (first[j]<middle[j])
2055  {
2056  /* Delete the new path segment, if any */
2057  if (localPathOK[j])
2058  DeleteSamples(newLocalNS[j],newLocalPath[j]);
2059 
2060  /* keep the previous path segment */
2061  for(i=first[j];i<middle[j];i++)
2062  { newPoint[k]=(*point)[i]; k++; }
2063  }
2064  }
2065 
2066  /* The non-smoothed segment till next step (or the tail
2067  non-smoothed segment). */
2068  for(i=middle[j];i<=last[j];i++)
2069  { newPoint[k]=(*point)[i]; k++; }
2070  }
2071 
2072  /* update the path */
2073  free(*point);
2074  *point=newPoint;
2075  *np=nnp;
2076  }
2077  else
2078  {
2079  /* nCores==1 */
2080  n1=randomMax(*np-2);
2081  n2=n1+1+randomMax(*np-2-n1);
2082 
2083  pl=PathLength(tp,sv,m,n2-n1+1,&((*point)[n1]));
2084 
2085  nl=DEFAULT_CONNECT(pr,tp,sv,&domain,m,ne,(*point)[n1],(*point)[n2],pl,
2086  TRUE,&sJ,&reached,&collision,NULL,&lns,&lpath,w);
2087  /*
2088  if (reached)
2089  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))?"(*)":" "));
2090  else
2091  fprintf(stderr,"Test %u->%u. pl:%g st:%u failed \n",n1,n2,pl,n2-n1+1);
2092  */
2093  if ((reached)&&(nl<pl))
2094  {
2095  /* Replace the path segment with the new path */
2096 
2097  /* Concat the sequences in a temporary buffer */
2098  nnp=n1+1+lns+(*np-n2); /* new number of points */
2099  NEW(newPoint,nnp,double*);
2100  k=0;
2101  for(i=0;i<=n1;i++)
2102  { newPoint[k]=(*point)[i]; k++; }
2103 
2104  for(i=0;i<lns;i++)
2105  { newPoint[k]=lpath[i]; k++; }
2106 
2107  for(i=n1+1;i<n2;i++)
2108  free((*point)[i]);
2109 
2110  for(i=n2;i<*np;i++)
2111  { newPoint[k]=(*point)[i]; k++; }
2112 
2113  /* Replace the points by the new points */
2114  free(*point);
2115  *point=newPoint;
2116  *np=nnp;
2117  }
2118  else
2119  {
2120  if (reached)
2121  DeleteSamples(lns,lpath);
2122  }
2123  }
2124 
2125  /* Print info for debug only */
2126  if (s%10==0)
2127  fprintf(stderr,"Shortcut (%u/%u) Path steps: %u Path length %g Time: %g\n",
2128  s,n*maxIterations,*np,PathLength(tp,sv,m,*np,*point),GetElapsedTime(stime));
2129  }
2130 
2131  if (nCores>1)
2132  {
2133  free(step);
2134  free(newStep);
2135  free(first);
2136  free(middle);
2137  free(last);
2138  free(localLength);
2139  free(localPathOK);
2140  free(newLocalLength);
2141  free(newLocalNS);
2142  free(newLocalPath);
2143  }
2144  DeleteJacobian(&sJ);
2145  DeleteBox(&domain);
2146  free(tp);
2147 }
2148 
2150  unsigned int nCores,unsigned int maxIterations,
2151  boolean *sv,Tstatistics *stime,
2152  unsigned int *np,double ***point,
2153  TStepCost stepCost,
2154  TStepCostGradient stepCostGradient,
2155  TAtlasBase *w)
2156 {
2157  unsigned int i,k,m,n,it;
2158  double delta,epsilon,e2,e,ce,r;
2159  Tbox domain;
2160  TJacobian sJ;
2161  unsigned int *tp;
2162  double stepSize,maxStepSize;
2163  double *ct; /* Cost of each step */
2164  Tchart *c; /* A chart at each point in the path*/
2165  #if (!APPROXIMATED_CHARTS_IN_GRADIENT)
2166  Tchart *newc; /* A chart at each new point in the path (possible replacement for c[.])*/
2167  #endif
2168  #if (REMOVE_POINTS)
2169  boolean *remove; /* points that can be safely removed */
2170  unsigned int new_np; /* new number of points (after trimming points that are too close) */
2171  #endif
2172  unsigned int npStart; /* Number of points at start. Used to deallocate space. */
2173  double **u; /* Gradient in tangent space */
2174  double *nu; /* Squared norm of the gradient at a given path step */
2175  double **newPoint; /* New point after the gradient displacement */
2176 
2177  boolean *stalled; /* Points that must remain fixed */
2178  double cost,newCost,ng,sc,sa;
2179  TStepCostGradient gradient;
2180  boolean done;
2181 
2182  k=(unsigned int)GetParameter(CT_N_DOF,pr);
2183  epsilon=GetParameter(CT_EPSILON,pr);
2184  e2=epsilon*epsilon;
2185  delta=GetParameter(CT_DELTA,pr);
2186  e=GetParameter(CT_E,pr);
2187  ce=GetParameter(CT_CE,pr);
2188  r=GetParameter(CT_R,pr);
2189 
2190  CS_WD_GENERATE_SIMP_INITIAL_BOX(pr,&domain,w);
2191  CS_WD_GET_SIMP_JACOBIAN(pr,&sJ,w);
2192  CS_WD_GET_SIMP_TOPOLOGY(pr,&tp,w);
2193  CS_WD_INIT_CD(pr,nCores,w);
2194 
2195  GetJacobianSize(&n,&m,&sJ); /* 'n' not used */
2196 
2197  if (stepCostGradient!=NULL)
2198  gradient=stepCostGradient;
2199  else
2200  gradient=StepCostNumericalGradient;
2201 
2202  /* Apply gradient-based length reduction */
2203  /* Allocate memory for the minimization process */
2204  NEW(ct,*np,double); /* cost of each step */
2205  NEW(c,*np,Tchart); /* chart at x_i */
2206  #if (!APPROXIMATED_CHARTS_IN_GRADIENT)
2207  NEW(newc,*np,Tchart); /* chart at temptative x_i */
2208  #endif
2209  #if (REMOVE_POINTS)
2210  NEW(remove,*np,boolean);
2211  #endif
2212  NEW(u,*np,double*); /* gradient at x_i (in tangent space) */
2213  NEW(nu,*np,double); /* squared norm of u */
2214  NEW(newPoint,*np,double*); /* new point after gradient motion */
2215  NEW(stalled,*np,boolean); /* TRUE for points that can not move */
2216 
2217  #pragma omp parallel for private(i) schedule(dynamic) if(nCores>1)
2218  for(i=0;i<*np;i++)
2219  {
2220  /* allocate space */
2221  NEWZ(u[i],k,double);
2222  NEW(newPoint[i],m,double);
2223 
2224  /* Initialize the distance charts over points */
2225  if (InitTrustedChart(pr,TRUE,&domain,tp,
2226  m,k,(*point)[i],e,ce,r,
2227  &sJ,w,&(c[i]))!=0)
2228  Error("Can not define a chart on a path step");
2229 
2230  /* default values */
2231  stalled[i]=FALSE;
2232  ct[i]=0.0;
2233  nu[i]=0.0;
2234  #if (REMOVE_POINTS)
2235  remove[i]=FALSE;
2236  #endif
2237  }
2238  npStart=*np; /* store the number of points to deallocate */
2239 
2240  /* The start/end of the path have to remain fixed */
2241  stalled[0]=TRUE;
2242  stalled[*np-1]=TRUE;
2243 
2244  done=FALSE;
2245  maxStepSize=2*delta;
2246  stepSize=delta;
2247 
2248  for(it=0;((!done)&&(it<maxIterations));it++)
2249  {
2250  #if (REMOVE_POINTS)
2251  /* try to reduce the number of points in the path. This speeds up the
2252  computation and helps to reduce the cost (if length-related). This
2253  is only robust if we penalize the uneven distribution of points
2254  along the smoothed path. */
2255  remove[0]=FALSE;
2256  for(i=1;i<*np-1;i++)
2257  remove[i]=((!stalled[i])&&(!remove[i-1])&&
2258  (DistanceTopology(m,tp,(*point)[i-1],(*point)[i+1])<delta));
2259  remove[*np-1]=FALSE;
2260 
2261  new_np=1; /* point[0] is keep for sure */
2262  for(i=1;i<*np;i++)
2263  {
2264  if (remove[i])
2265  DeleteChart(&(c[i]));
2266  else
2267  {
2268  /* info(new_np) <- info(i) */
2269  if (new_np!=i)
2270  {
2271  stalled[new_np]=stalled[i];
2272  memcpy((*point)[new_np],(*point)[i],m*sizeof(double));
2273  CopyChart(&(c[new_np]),&(c[i]));
2274  DeleteChart(&(c[i]));
2275  }
2276  new_np++;
2277  }
2278  }
2279  *np=new_np;
2280  #endif
2281 
2282  /* Compute the gradient */
2283  #pragma omp parallel for private(i) schedule(dynamic) if(nCores>1)
2284  for(i=1;i<*np-1;i++)
2285  {
2286  if (stalled[i])
2287  nu[i]=0.0;
2288  else
2289  {
2290  /*
2291  StepCostNumericalGradient(pr,tp,sv,m,
2292  (i<2?NULL:(*point)[i-2]),
2293  (*point)[i-1],(*point)[i],(*point)[i+1],
2294  (i>*np-3?NULL:(*point)[i+2]),
2295  (i<2?NULL:&(c[i-2])),
2296  &(c[i-1]),&(c[i]),&(c[i+1]),
2297  (i>*np-3?NULL:&(c[i+2])),
2298  &sJ,stepCost,
2299  u[i],GET_WORLD(w));
2300  PrintVector(stderr,"u_numeric ",k,u[i]);
2301 
2302  stepCostGradient(pr,tp,sv,m,
2303  (i<2?NULL:(*point)[i-2]),
2304  (*point)[i-1],(*point)[i],(*point)[i+1],
2305  (i>*np-3?NULL:(*point)[i+2]),
2306  (i<2?NULL:&(c[i-2])),
2307  &(c[i-1]),&(c[i]),&(c[i+1]),
2308  (i>*np-3?NULL:&(c[i+2])),
2309  &sJ,stepCost,
2310  u[i],GET_WORLD(w));
2311  PrintVector(stderr,"u_analitic",k,u[i]);
2312  fprintf(stderr,"\n");
2313  if (i>100)
2314  exit(0);
2315  */
2316 
2317  /* Evaluate the gradient of the user-provided cost */
2318  stalled[i]=!gradient(pr,tp,sv,m,
2319  (i<2?NULL:(*point)[i-2]),
2320  (*point)[i-1],(*point)[i],(*point)[i+1],
2321  (i>*np-3?NULL:(*point)[i+2]),
2322  (i<2?NULL:&(c[i-2])),
2323  &(c[i-1]),&(c[i]),&(c[i+1]),
2324  (i>*np-3?NULL:&(c[i+2])),
2325  &sJ,stepCost,
2326  u[i],GET_WORLD(w));
2327 
2328  if (stalled[i])
2329  nu[i]=0.0;
2330  else
2331  nu[i]=GeneralDotProduct(k,u[i],u[i]); /* squared norm of nu[i] */
2332  }
2333  }
2334 
2335  /* first/last are stalled */
2336  nu[0]=nu[*np-1]=0.0;
2337 
2338  /* Compute the norm of the whole gradient */
2339  ng=0.0;
2340  for(i=0;i<*np;i++)
2341  ng+=nu[i];
2342  ng=sqrt(ng);
2343 
2344  if (ng<(*np)*epsilon)
2345  {
2346  done=TRUE; /* If too small, stop the minimization */
2347  fprintf(stderr,"Null gradient\n");
2348  }
2349  else
2350  {
2351  /* Compute the current cost (updated as we move the path) */
2352  #pragma omp parallel for private(i) schedule(dynamic) if(nCores>1)
2353  for(i=1;i<*np;i++)
2354  ct[i]=stepCost(pr,tp,sv,m,
2355  (*point)[i-1],(*point)[i],(i<*np-1?(*point)[i+1]:NULL),
2356  1,
2357  &(c[i-1]),&(c[i]),(i<*np-1?&(c[i+1]):NULL),GET_WORLD(w));
2358 
2359  cost=0.0;
2360  for(i=1;i<*np;i++)
2361  cost+=ct[i];
2362 
2363  stepSize*=2.0;
2364  if (stepSize>maxStepSize)
2365  stepSize=maxStepSize;
2366 
2367  /* Print information about the minimization process */
2368  if (it%100==0)
2369  fprintf(stderr,"Iteration %u Path steps: %u Step size: %g Path cost: %g Time: %g Norm g: %.12f\n",
2370  it,*np,stepSize,cost,GetElapsedTime(stime),ng);
2371 
2372  sc=-stepSize/ng;
2373  sa=fabs(sc); /* abs value used to scale the norm of the gradient */
2374 
2375  /* Displace the whole path along the gradient */
2376  do {
2377  #pragma omp parallel for private(i) schedule(dynamic) if(nCores>1)
2378  for(i=0;i<*np;i++)
2379  {
2380  if (stalled[i])
2381  memcpy(newPoint[i],(*point)[i],m*sizeof(double));
2382  else
2383  {
2384  ScaleVector(sc,k,u[i]); /* Normalize/Scale */
2385  nu[i]*=(sa*sa); /* correct the squared norm of this gradient */
2386 
2387  if ((nu[i]<e2)||
2388  (Chart2Manifold(pr,&sJ,u[i],tp,NULL,newPoint[i],&(c[i]))==INF))
2389  memcpy(newPoint[i],(*point)[i],m*sizeof(double));
2390  }
2391  }
2392 
2393  /* We changed the individual gradients -> change the global norm */
2394  ng*=sa;
2395  done=(ng<(*np)*epsilon);
2396  if (done)
2397  fprintf(stderr,"Can not decrease error\n");
2398 
2399  if (!done)
2400  {
2401  sc=sa=0.5; /* in next steps, if any, we scale the gradient */
2402  stepSize*=0.5; /* this will affect future iterations */
2403 
2404  #if (!APPROXIMATED_CHARTS_IN_GRADIENT)
2405  /* compute the cost at the modified path */
2406  #pragma omp parallel for private(i) schedule(dynamic) if(nCores>1)
2407  for(i=0;i<*np;i++)
2408  {
2409  if (!stalled[i])
2410  {
2411  /* for fixed points, the chart remains stable */
2412  if (InitTrustedChart(pr,TRUE,&domain,tp,
2413  m,k,newPoint[i],e,ce,r,
2414  &sJ,w,&(newc[i]))!=0)
2415  {
2416  /* If we can not define a chart in the new point, we fix the point */
2417  stalled[i]=TRUE;
2418  memcpy(newPoint[i],(*point)[i],m*sizeof(double));
2419  }
2420  }
2421  }
2422  #endif
2423 
2424  #pragma omp parallel for private(i) schedule(dynamic) if(nCores>1)
2425  for(i=1;i<*np;i++)
2426  {
2427  #if (APPROXIMATED_CHARTS_IN_GRADIENT)
2428  ct[i]=stepCost(pr,tp,sv,m,
2429  newPoint[i-1],newPoint[i],(i<*np-1?newPoint[i+1]:NULL),
2430  1,
2431  &(c[i-1]),&(c[i]),(i<*np-1?&(c[i+1]):NULL),
2432  GET_WORLD(w));
2433  #else
2434  ct[i]=stepCost(pr,tp,sv,m,
2435  newPoint[i-1],newPoint[i],(i<*np-1?newPoint[i+1]:NULL),
2436  1,
2437  (stalled[i-1]?&(c[i-1]):&(newc[i-1])),
2438  (stalled[i]?&(c[i]):&(newc[i])),
2439  (i<*np-1?(stalled[i+1]?&(c[i+1]):&(newc[i+1])):NULL),
2440  GET_WORLD(w));
2441  #endif
2442  }
2443 
2444  newCost=0;
2445  for(i=1;i<*np;i++)
2446  newCost+=ct[i];
2447 
2448  #if (!APPROXIMATED_CHARTS_IN_GRADIENT)
2449  if (newCost>cost)
2450  {
2451  /* if we will iterate once more -> release the newc since we will
2452  eventually recompute them */
2453  #pragma omp parallel for private(i) schedule(dynamic) if(nCores>1)
2454  for(i=0;i<*np;i++)
2455  {
2456  if (!stalled[i])
2457  DeleteChart(&(newc[i]));
2458  }
2459  }
2460  #endif
2461  }
2462 
2463  } while((!done)&&(newCost>cost));
2464 
2465  if (!done)
2466  {
2467  cost=newCost;
2468 
2469  /* Update the path and check if any of its steps become
2470  stalled (in collision or out of domain). Note that we
2471  allow for small penetration (size delta) */
2472  #pragma omp parallel for private(i) schedule(dynamic) if(nCores>1)
2473  for(i=0;i<*np;i++)
2474  {
2475  if (!stalled[i])
2476  {
2477  DeleteChart(&(c[i]));
2478  #if (APPROXIMATED_CHARTS_IN_GRADIENT)
2479  /* now we have the new set of points -> compute the charts at them */
2480  if (InitTrustedChart(pr,TRUE,&domain,tp,
2481  m,k,newPoint[i],e,ce,r,
2482  &sJ,w,&(c[i]))!=0)
2483  {
2484  /* If we can not define a chart in the new point,
2485  move to the previous point and fix it. */
2486  stalled[i]=TRUE;
2487  memcpy(newPoint[i],(*point)[i],m*sizeof(double));
2488  if (InitTrustedChart(pr,TRUE,&domain,tp,
2489  m,k,newPoint[i],e,ce,r,
2490  &sJ,w,&(c[i]))!=0)
2491  Error("Error redefining a chart?");
2492  }
2493  #else
2494  /* using pointers to charts this would be more efficient but... */
2495  CopyChart(&(c[i]),&(newc[i]));
2496  DeleteChart(&(newc[i]));
2497  #endif
2498 
2499  if (!stalled[i])
2500  {
2501  memcpy((*point)[i],newPoint[i],m*sizeof(double));
2502 
2503  stalled[i]=(!PointInBoxTopology(NULL,FALSE,m,(*point)[i],epsilon,tp,&domain));
2504  if (!stalled[i])
2505  CS_WD_IN_COLLISION(stalled[i],pr,(*point)[i],NULL,w);
2506  }
2507  }
2508  }
2509  }
2510  }
2511  }
2512 
2513  #pragma omp parallel for private(i) schedule(dynamic) if(nCores>1)
2514  for(i=0;i<*np;i++)
2515  DeleteChart(&(c[i]));
2516  for(i=*np;i<npStart;i++)
2517  free((*point)[i]);
2518  for(i=0;i<npStart;i++)
2519  {
2520  free(u[i]);
2521  free(newPoint[i]);
2522  }
2523  free(newPoint);
2524  free(nu);
2525  free(u);
2526  free(c);
2527  free(ct);
2528  free(stalled);
2529  #if (!APPROXIMATED_CHARTS_IN_GRADIENT)
2530  free(newc);
2531  #endif
2532  #if (REMOVE_POINTS)
2533  free(remove);
2534  #endif
2535 
2536  DeleteBox(&domain);
2537  free(tp);
2538  DeleteJacobian(&sJ);
2539 }
2540 
2541 void SmoothSamples(Tparameters *pr,boolean parallel,int mode,unsigned int maxIterations,
2542  unsigned int ns,double **path,
2543  unsigned int *sns,double ***spath,TAtlasBase *w)
2544 {
2545  unsigned int i,m,np,mp,k;
2546  double **point;
2547  double *o,l,delta;
2548  unsigned int *tp;
2549  unsigned int sms;
2550  unsigned int nv,nvs;
2551  boolean *systemVars;
2552  Tstatistics stime;
2553  unsigned int nCores;
2554  boolean *sv;
2555 
2556  if (ns<3)
2557  Error("Too small path in SmoothSamples (only paths with at least 3 samples are considered)");
2558 
2559  if (parallel)
2560  {
2561  #ifdef _OPENMP
2562  nCores=omp_get_max_threads();
2563  if (ns<nCores*3)
2564  nCores=1;
2565  #else
2566  nCores=1;
2567  #endif
2568  }
2569  else
2570  nCores=1;
2571 
2572  delta=GetParameter(CT_DELTA,pr);
2573  k=(unsigned int)GetParameter(CT_N_DOF,pr);
2574  if (k==0)
2575  {
2576  k=CS_WD_MANIFOLD_DIMENSION(pr,path[0],w);
2577  ChangeParameter(CT_N_DOF,(double)k,pr);
2578  }
2579 
2580  InitStatistics(nCores,0,&stime);
2581 
2582  /* Simplify the path */
2583  m=CS_WD_GET_SIMP_TOPOLOGY(pr,&tp,w);
2584 
2585  if ((!KINEMATIC_SUBSPACE)||(ON_CUIKSYSTEM(w)))
2586  sv=NULL;
2587  else
2588  {
2589  /* Path length on kinematic variables only */
2590  if (WorldSimpKinematicVars(pr,&sv,GET_WORLD(w))!=m)
2591  Error("Missmatch in the number of variables");
2592  }
2593 
2594  mp=ns;
2595  np=ns;
2596 
2597  NEW(point,mp,double*);
2598  #pragma omp parallel for private(i) schedule(dynamic) if(nCores>1)
2599  for(i=0;i<np;i++)
2600  {
2601  double *od;
2602 
2603  /* recover the dummy variables from the system ones
2604  for the point in original system */
2605  CS_WD_REGENERATE_SOLUTION_POINT(pr,path[i],&od,w);
2606  /* Get the point in the simplified form */
2607  CS_WD_GENERATE_SIMPLIFIED_POINT(pr,od,&(point[i]),w);
2608 
2609  free(od);
2610  }
2611  /********************************************/
2612  /* Compute the initial lenght */
2613  l=PathLength(tp,sv,m,np,point);
2614  fprintf(stderr,"Initial lenght : %g\n",l);
2615  fprintf(stderr,"Number of processing cores : %u\n",nCores);
2616 
2617  /* If the path is not densily sampled the Euclidean
2618  distance between samples is too far from the Geodesic
2619  one and then the smoothing is not meaningful */
2620  if ((m!=k)&&(l/(double)np)>2*delta)
2621  Error("The input path is not dense enough");
2622 
2623  fprintf(stderr,"Path steps : %u\n",np);
2624  switch(mode)
2625  {
2626  case SMOOTH_RANDOM:
2627  fprintf(stderr,"Smooth method : RANDOM\n");
2628  fprintf(stderr,"Measure to optimize : Path length\n");
2629  RandomSmooth(pr,nCores,maxIterations,sv,&stime,&np,&point,w);
2630  break;
2631  case SMOOTH_GRADIENT:
2632  fprintf(stderr,"Smooth method : GRADIENT\n");
2633  #if (DISPERSION)
2634  fprintf(stderr,"Measure to optimize : Path length + dispersion\n");
2635  GradientSmooth(pr,nCores,maxIterations,sv,&stime,&np,&point,
2637  #else
2638  fprintf(stderr,"Measure to optimize : Path length\n");
2639  GradientSmooth(pr,nCores,maxIterations,sv,&stime,&np,&point,
2641  #endif
2642  break;
2643  case SMOOTH_SHORTCUT:
2644  fprintf(stderr,"Smooth method : SHORTCUT\n");
2645  fprintf(stderr,"Measure to optimize : Path length\n");
2646  ShortcutSmooth(pr,sv,&stime,&np,&point,w);
2647  break;
2648  case SMOOTH_EFFORT:
2649  fprintf(stderr,"Smooth method : GRADIENT\n");
2650  #if (DISPERSION)
2651  fprintf(stderr,"Measure to optimize : Control effort + dispersion\n");
2652  GradientSmooth(pr,nCores,maxIterations,sv,&stime,&np,&point,
2654  #else
2655  fprintf(stderr,"Measure to optimize : Control effort\n");
2656  GradientSmooth(pr,nCores,maxIterations,sv,&stime,&np,&point,
2658  #endif
2659  break;
2660  case SMOOTH_DISPERSION:
2661  fprintf(stderr,"Smooth method : GRADIENT\n");
2662  fprintf(stderr,"Measure to optimize : Dispersion\n");
2663  GradientSmooth(pr,nCores,maxIterations,sv,&stime,&np,&point,
2665  break;
2666  default:
2667  Error("Unknown smoothing method in SmoothSamples");
2668  }
2669 
2670  if (mode!=SMOOTH_EFFORT)
2671  fprintf(stderr,"Final length (in simplified system): %g\n",PathLength(tp,sv,m,np,point));
2672 
2673  /********************************************/
2674  /* The minimization is done, now define the new optimized path */
2675  nv=CS_WD_GET_SYSTEM_VARS(&systemVars,w);
2676  nvs=0;
2677  for(i=0;i<nv;i++)
2678  {
2679  if (systemVars[i])
2680  nvs++;
2681  }
2682  sms=np;
2683  NEW(*spath,sms,double*);
2684  *sns=0;
2685  for(i=0;i<np;i++)
2686  {
2687  CS_WD_REGENERATE_ORIGINAL_POINT(pr,point[i],&o,w);
2688  AddSample2Samples(nv,o,nvs,systemVars,&sms,sns,spath);
2689  free(o);
2690  }
2691  free(systemVars);
2692 
2693  /********************************************/
2694  /* Release the data used to represent the path */
2695  for(i=0;i<np;i++)
2696  free(point[i]);
2697  free(point);
2698  if (sv!=NULL)
2699  free(sv);
2700  free(tp);
2701 
2702  fprintf(stderr,"Optimization time: %g\n",GetElapsedTime(&stime));
2703  DeleteStatistics(&stime);
2704 }
2705 
2706 void ConcatSamples(unsigned int nvs,
2707  unsigned int ns1,double **path1,
2708  unsigned int ns2,double **path2,
2709  unsigned int *ns,double ***path)
2710 {
2711  unsigned int i;
2712 
2713  *ns=(ns1+ns2);
2714  NEW((*path),*ns,double*);
2715  for(i=0;i<ns1;i++)
2716  {
2717  NEW((*path)[i],nvs,double);
2718  memcpy((*path)[i],path1[i],nvs*sizeof(double));
2719  }
2720  for(i=0;i<ns2;i++)
2721  {
2722  NEW((*path)[ns1+i],nvs,double);
2723  memcpy((*path)[ns1+i],path2[i],nvs*sizeof(double));
2724  }
2725 }
2726 
2727 void ReverseConcatSamples(unsigned int nvs,
2728  unsigned int ns1,double **path1,
2729  unsigned int ns2,double **path2,
2730  unsigned int *ns,double ***path)
2731 {
2732  unsigned int i;
2733  signed int s;
2734 
2735  *ns=(ns1+ns2);
2736 
2737  NEW((*path),*ns,double*);
2738  for(i=0;i<ns1;i++)
2739  {
2740  NEW((*path)[i],nvs,double);
2741  memcpy((*path)[i],path1[i],nvs*sizeof(double));
2742  }
2743  s=ns2-1;
2744  for(i=0;i<ns2;i++)
2745  {
2746  NEW((*path)[ns1+i],nvs,double);
2747  memcpy((*path)[ns1+i],path2[s],nvs*sizeof(double));
2748  s--;
2749  }
2750 }
2751 
2752 void ReverseSamples(unsigned int ns,double **path)
2753 {
2754  if (ns>0)
2755  {
2756  unsigned int s,k;
2757  double *step;
2758 
2759  s=ns-1;
2760  k=0;
2761  while(s>k)
2762  {
2763  step=path[s];
2764  path[s]=path[k];
2765  path[k]=step;
2766  s--;
2767  k++;
2768  }
2769  }
2770 }
2771 
2772 unsigned int ReadOneSample(Tparameters *p,char *fname,unsigned int nvs,double **s)
2773 {
2774  unsigned int i;
2775  Tfilename fsamples;
2776  FILE *f;
2777  unsigned int r;
2778 
2779  NEW(*s,nvs,double);
2780 
2781  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
2782  if (r==REP_JOINTS)
2783  CreateFileName(NULL,fname,NULL,JOINTS_EXT,&fsamples);
2784  else
2785  CreateFileName(NULL,fname,NULL,LINKS_EXT,&fsamples);
2786  f=fopen(GetFileFullName(&fsamples),"r");
2787  if (!f)
2788  Error("Could not open sample file");
2789 
2790  fprintf(stderr,"Reading samples from : %s\n",GetFileFullName(&fsamples));
2791  for(i=0;i<nvs;i++)
2792  {
2793  if (fscanf(f,"%lf",&((*s)[i]))!=1)
2794  Error("Sample in file with less values than system variables");
2795  }
2796  fclose(f);
2797 
2798  DeleteFileName(&fsamples);
2799 
2800  return(nvs);
2801 }
2802 
2803 unsigned int ReadTwoSamples(Tparameters *p,char *fname,unsigned int nvs,double **s1,double **s2)
2804 {
2805  unsigned int i;
2806  Tfilename fsamples;
2807  FILE *f;
2808  unsigned int r;
2809  double epsilon;
2810 
2811  NEW(*s1,nvs,double);
2812  NEW(*s2,nvs,double);
2813 
2814  epsilon=GetParameter(CT_EPSILON,p);
2815  r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
2816  if (r==REP_JOINTS)
2817  CreateFileName(NULL,fname,NULL,JOINTS_EXT,&fsamples);
2818  else
2819  CreateFileName(NULL,fname,NULL,LINKS_EXT,&fsamples);
2820  f=fopen(GetFileFullName(&fsamples),"r");
2821  if (!f)
2822  Error("Could not open sample file");
2823 
2824  fprintf(stderr,"Reading samples from : %s\n",GetFileFullName(&fsamples));
2825  for(i=0;i<nvs;i++)
2826  {
2827  if (fscanf(f,"%lf",&((*s1)[i]))!=1)
2828  Error("Sample in file with less values than system variables");
2829  }
2830  for(i=0;i<nvs;i++)
2831  {
2832  if (fscanf(f,"%lf",&((*s2)[i]))!=1)
2833  Error("Sample in file with less values than system variables");
2834  }
2835  fclose(f);
2836 
2837  if (Distance(nvs,*s1,*s2)<epsilon)
2838  Error("Repeated sample in input file");
2839 
2840  DeleteFileName(&fsamples);
2841 
2842  return(nvs);
2843 }
2844 
2845 void SaveSamplesInt(Tfilename *fpath,unsigned int nvs,
2846  unsigned int ns,double **path)
2847 {
2848  Tbox bp;
2849  unsigned int i,j;
2850  FILE *f;
2851 
2852  f=fopen(GetFileFullName(fpath),"w");
2853  if (!f)
2854  Error("Could not open file to store the solution path on mainfold");
2855 
2856  fprintf(stderr,"Writing solution path to : %s\n",GetFileFullName(fpath));
2857 
2858  fprintf(f,"%u\n",ns);
2859  for(i=0;i<ns;i++)
2860  {
2861  for(j=0;j<nvs;j++)
2862  fprintf(f,"%.12f ",path[i][j]);
2863 
2864  fprintf(f,"\n");
2865  }
2866 
2867  /*We will also write the path in the form of boxes (for animation)*/
2868  for(i=0;i<ns;i++)
2869  {
2870  InitBoxFromPoint(nvs,path[i],&bp);
2871  PrintBox(f,&bp);
2872  DeleteBox(&bp);
2873  }
2874 
2875  fclose(f);
2876 }
2877 
2878 
2879 void SaveSamples(char *fname,char *suffix,unsigned int nvs,
2880  unsigned int ns,double **path)
2881 {
2882  Tfilename fpath;
2883 
2884  if (suffix!=NULL)
2885  CreateFileName(NULL,fname,suffix,SOL_EXT,&fpath);
2886  else
2887  CreateFileName(NULL,fname,"_path",SOL_EXT,&fpath);
2888 
2889  SaveSamplesInt(&fpath,nvs,ns,path);
2890 
2891  DeleteFileName(&fpath);
2892 }
2893 
2894 void SaveSamplesN(char *fname,boolean smooth,unsigned int n,unsigned int nvs,
2895  unsigned int ns,double **path)
2896 {
2897  Tfilename fpath;
2898  char s[100];
2899 
2900  if (smooth)
2901  sprintf(s,"_spath_%u",n);
2902  else
2903  sprintf(s,"_path_%u",n);
2904 
2905  CreateFileName(NULL,fname,s,SOL_EXT,&fpath);
2906 
2907  SaveSamplesInt(&fpath,nvs,ns,path);
2908 
2909  DeleteFileName(&fpath);
2910 }
2911 
2912 
2913 boolean LoadSamples(Tfilename *fname, unsigned int *nvs,
2914  unsigned int *ns,double ***path)
2915 {
2916  FILE *f;
2917  Tlist lb;
2918  Tbox *b;
2919  Titerator i;
2920  unsigned int k;
2921 
2922  f=fopen(GetFileFullName(fname),"r");
2923  if (f)
2924  {
2925  /*Load the path from a _path.sol file */
2926  fprintf(stderr,"Loading solution points from : %s\n",GetFileFullName(fname));
2927 
2928  /* We take advantage that the sample list is stored as a box list to read it */
2929  ReadListOfBoxes(GetFileFullName(fname),&lb);
2930 
2931  *ns=ListSize(&lb);
2932  NEW(*path,*ns,double *);
2933  InitIterator(&i,&lb);
2934  First(&i);
2935  k=0;
2936  while(!EndOfList(&i))
2937  {
2938  b=(Tbox *)GetCurrent(&i);
2939  if (k==0)
2940  *nvs=GetBoxNIntervals(b);
2941  else
2942  {
2943  if (*nvs!=GetBoxNIntervals(b))
2944  Error("Incongruent size of step in path");
2945  }
2946  NEW((*path)[k],*nvs,double);
2947  GetBoxCenter(NULL,(*path)[k],b);
2948  Advance(&i);
2949  k++;
2950  }
2951  fclose(f);
2952  DeleteListOfBoxes(&lb);
2953 
2954  return(TRUE);
2955  }
2956  else
2957  return(FALSE);
2958 }
2959 
2961  unsigned int xID,unsigned int yID,unsigned int zID,
2962  unsigned int ns,double **path)
2963 {
2964  unsigned int i;
2965  double *x,*y,*z;
2966  Tcolor pathColor;
2967  double cx,cy,cz;
2968  #if (PATH_NODES)
2969  double xc[2],yc[2],zc[2];
2970  Tcolor blue;
2971  #endif
2972 
2973  cx=GetParameter(CT_CUT_X,p);
2974  cy=GetParameter(CT_CUT_Y,p);
2975  cz=GetParameter(CT_CUT_Z,p);
2976 
2977  NEW(x,ns,double);
2978  NEW(y,ns,double);
2979  NEW(z,ns,double);
2980 
2981  for(i=0;i<ns;i++)
2982  {
2983  /* For the paths we only store system variables */
2984  x[i]=path[i][xID];
2985  y[i]=path[i][yID];
2986  z[i]=path[i][zID];
2987 
2988  if ((cx<0)&&(x[i]<cx))
2989  x[i]+=M_2PI;
2990  if ((cx>0)&&(x[i]>cx))
2991  x[i]-=M_2PI;
2992  if ((cy<0)&&(y[i]<cy))
2993  y[i]+=M_2PI;
2994  if ((cy>0)&&(y[i]>cy))
2995  y[i]-=M_2PI;
2996  if ((cz<0)&&(z[i]<cz))
2997  z[i]+=M_2PI;
2998  if ((cz>0)&&(z[i]>cz))
2999  z[i]-=M_2PI;
3000  }
3001 
3002  NewColor(0,1,0,&pathColor); /* green */
3003  StartNew3dObject(&pathColor,p3d);
3004  PlotVect3d(ns,x,y,z,p3d);
3005  Close3dObject(p3d);
3006  DeleteColor(&pathColor);
3007 
3008  #if (PATH_NODES)
3009  if (ns>2)
3010  {
3011  NewColor(0,0,1,&blue);
3012  StartNew3dObject(&blue,p3d);
3013  for(i=0;i<ns;i++)
3014  {
3015  xc[0]=x[i]-0.01;
3016  yc[0]=y[i];
3017  zc[0]=z[i];
3018 
3019  xc[1]=x[i]+0.01;
3020  yc[1]=y[i];
3021  zc[1]=z[i];
3022 
3023  PlotVect3d(2,xc,yc,zc,p3d);
3024 
3025  xc[0]=x[i];
3026  yc[0]=y[i]-0.01;
3027  zc[0]=z[i];
3028 
3029  xc[1]=x[i];
3030  yc[1]=y[i]+0.01;
3031  zc[1]=z[i];
3032 
3033  PlotVect3d(2,xc,yc,zc,p3d);
3034 
3035  xc[0]=x[i];
3036  yc[0]=y[i];
3037  zc[0]=z[i]-0.01;
3038 
3039  xc[1]=x[i];
3040  yc[1]=y[i];
3041  zc[1]=z[i]+0.01;
3042 
3043  PlotVect3d(2,xc,yc,zc,p3d);
3044  }
3045 
3046  DeleteColor(&blue);
3047  Close3dObject(p3d);
3048  }
3049  #endif
3050 
3051  free(x);
3052  free(y);
3053  free(z);
3054 }
3055 
3057  unsigned int xID,unsigned int yID,unsigned int zID,
3058  Tworld *w,
3059  unsigned int ns,double **sols)
3060 {
3061  unsigned int i,k,nv,nx,ny,nz;
3062  boolean *systemVars;
3063  double *fullS,*ff,**f,**s,mg,epsilon;
3064  double n,maxn,minn; /* to compute the max norm of the force field. */
3065  Tcolor ffColor;
3066 
3067  mg=2.0*GetParameter(CT_DELTA,p); /* maximum size of the lines
3068  representing the ff */
3069  epsilon=GetParameter(CT_EPSILON,p);
3070 
3071  /* Translate indices from system variables only to
3072  the whole set of variables */
3073  nv=GetWorldSystemVars(&systemVars,w);
3074  k=0;
3075  nx=ny=nz=NO_UINT;
3076  for(i=0;i<nv;i++)
3077  {
3078  if (systemVars[i])
3079  {
3080  if (xID==k)
3081  nx=i;
3082  if (yID==k)
3083  ny=i;
3084  if (zID==k)
3085  nz=i;
3086  k++;
3087  }
3088  }
3089  if ((nx==NO_UINT)||(ny==NO_UINT)||(nz==NO_UINT))
3090  Error("Undefined projection dimension in PlotForceField");
3091 
3092  /* Compute the projection of the pionts where to evalute the
3093  force filed and the projection of the force field at
3094  each of these points. */
3095  NEW(s,ns,double*);
3096  NEW(f,ns,double*);
3097  maxn=0;
3098  minn=INF;
3099  for(i=0;i<ns;i++)
3100  {
3101  NEW(s[i],3,double);
3102  RegenerateWorldSolutionPoint(p,sols[i],&fullS,w);
3103  s[i][0]=fullS[nx];
3104  s[i][1]=fullS[ny];
3105  s[i][2]=fullS[nz];
3106 
3107  NEW(f[i],3,double);
3108  WorldForceField(p,FALSE,fullS,&ff,w);
3109  f[i][0]=ff[nx];
3110  f[i][1]=ff[ny];
3111  f[i][2]=ff[nz];
3112 
3113  n=Norm(3,f[i]);
3114  //n=Norm(nv,ff);
3115  if (n>maxn)
3116  maxn=n;
3117  else
3118  {
3119  if (n<minn)
3120  minn=n;
3121  }
3122  free(ff);
3123  free(fullS);
3124  }
3125 
3126  if (maxn<epsilon)
3127  Error("Null force field");
3128  if ((maxn-minn)<epsilon)
3129  Warning("Constant force field");
3130  else
3131  fprintf(stderr,"Force field range : [%g,%g]\n",minn,maxn);
3132 
3133  /* Plot the scaled force field at the given points */
3134  NewColor(0,0,0,&ffColor); /* black */
3135  StartNew3dObject(&ffColor,p3d);
3136 
3137  for(i=0;i<ns;i++)
3138  {
3139  ScaleVector(mg/maxn,3,f[i]);
3140  f[i][0]+=s[i][0];
3141  f[i][1]+=s[i][1];
3142  f[i][2]+=s[i][2];
3143  PlotLine(s[i],f[i],p3d);
3144  }
3145 
3146  Close3dObject(p3d);
3147  DeleteColor(&ffColor);
3148 
3149  /* Deallocate memory */
3150  for(i=0;i<ns;i++)
3151  {
3152  free(s[i]);
3153  free(f[i]);
3154  }
3155  free(f);
3156  free(s);
3157 }
3158 
3159 void DeleteSamples(unsigned int ns,double **path)
3160 {
3161  unsigned int i;
3162 
3163  for(i=0;i<ns;i++)
3164  free(path[i]);
3165  free(path);
3166 }
#define CT_R
Ball radius in atlas.
Definition: parameters.h:257
unsigned int RegenerateWorldSolutionPoint(Tparameters *pr, double *p, double **v, Tworld *w)
Computes the missing values in a kinematic solution.
Definition: world.c:3468
#define SMOOTH_DISPERSION
One of the possible smoothing methods.
Definition: samples.h:90
void CopyChart(Tchart *c_dst, Tchart *c_src)
Copy constructor.
Definition: chart.c:836
void First(Titerator *i)
Moves an iterator to the first position of its associated list.
Definition: list.c:356
void PlotVect3d(unsigned int n, double *x, double *y, double *z, Tplot3d *p)
Adds a polyline to the current object.
Definition: plot3d.c:447
CBLAS_INLINE void ScaleVector(double f, unsigned int s, double *v)
Scales a vector.
Definition: basic_algebra.c:30
double StepEffort(Tparameters *p, unsigned int *tp, boolean *sv, unsigned int m, double *xp, double *x, double *xn, unsigned int current, Tchart *cp, Tchart *c, Tchart *cn, Tworld *w)
Control effort of a path step.
Definition: samples.c:1275
#define GET_WORLD(wcs)
Returns the world from an TAtlasBase.
Definition: wcs.h:150
double DistanceTopologySubset(unsigned int s, unsigned int *tp, boolean *subset, double *v1, double *v2)
Computes the distance of two points along a subset of components.
#define REP_JOINTS
One of the possible values of the REPRESENTATION parameter.
Definition: parameters.h:60
#define CT_EPSILON
Numerical tolerance.
Definition: parameters.h:194
#define FALSE
FALSE.
Definition: boolean.h:30
void GradientSmooth(Tparameters *pr, unsigned int nCores, unsigned int maxIterations, boolean *sv, Tstatistics *stime, unsigned int *np, double ***point, TStepCost stepCost, TStepCostGradient stepCostGradient, TAtlasBase *w)
Gradient-based path smoothing.
Definition: samples.c:2149
#define CS_WD_GENERATE_SIMPLIFIED_POINT(pr, p, r, wcs)
Generates a simplified point from an original one.
Definition: wcs.h:495
unsigned int ReadTwoSamples(Tparameters *p, char *fname, unsigned int nvs, double **s1, double **s2)
Reads two samples from a file.
Definition: samples.c:2803
double DistanceTopology(unsigned int s, unsigned int *tp, double *v1, double *v2)
Computes the distance of two points.
boolean(* TStepCostGradient)(Tparameters *, unsigned int *, boolean *, unsigned int, double *, double *, double *, double *, double *, Tchart *, Tchart *, Tchart *, Tchart *, Tchart *, TJacobian *, TStepCost, double *, Tworld *)
Template of step cost gradients.
Definition: samples.h:235
#define NEW(_var, _n, _type)
Allocates memory space.
Definition: defines.h:385
Data structure to hold the information about the name of a file.
Definition: filename.h:271
void ArrayPi2Pi(unsigned int n, unsigned int *t, double *a)
Applies PI2PI to an array.
void RandomSmooth(Tparameters *pr, unsigned int nCores, unsigned int maxIterations, boolean *sv, Tstatistics *stime, unsigned int *np, double ***point, TAtlasBase *w)
Random-based path smoothing.
Definition: samples.c:1927
#define SMOOTH_GRADIENT
One of the possible smoothing methods.
Definition: samples.h:57
unsigned int randomMax(unsigned int m)
Returns a random integer in the range [0,m].
Definition: random.c:77
#define NEWZ(_var, _n, _type)
Allocates and cleans memory space.
Definition: defines.h:394
double(* TStepCost)(Tparameters *, unsigned int *, boolean *, unsigned int, double *, double *, double *, unsigned int, Tchart *, Tchart *, Tchart *, Tworld *)
Template of step costs.
Definition: samples.h:183
void PlotSamples(Tparameters *p, Tplot3d *p3d, unsigned int xID, unsigned int yID, unsigned int zID, unsigned int ns, double **path)
Plots a 3D projection of a path.
Definition: samples.c:2960
double ConnectSamples(Tparameters *pr, unsigned int *tp, boolean *sv, Tbox *domain, unsigned int m, unsigned int n, double *s1, double *s2, double md, boolean checkCollisions, TJacobian *sJ, boolean *reached, boolean *collision, double *lastSample, unsigned int *ns, double ***path, TAtlasBase *w)
Determines the connection between two points on the manifold.
Definition: samples.c:966
double LengthAndDispersion(Tparameters *p, unsigned int *tp, boolean *sv, unsigned int m, double *xp, double *x, double *xn, unsigned int current, Tchart *cp, Tchart *c, Tchart *cn, Tworld *w)
Combines length and dispersion cost functions.
Definition: samples.c:1658
void InitStatistics(unsigned int np, double v, Tstatistics *t)
Constructor.
Definition: statistics.c:21
CBLAS_INLINE double Norm(unsigned int s, double *v)
Computes the norm of a vector.
double ConnectSamplesChart(Tparameters *pr, unsigned int *tp, boolean *sv, Tbox *domain, unsigned int m, unsigned int n, double *s1, double *s2, double md, boolean checkCollisions, TJacobian *sJ, boolean *reached, boolean *collision, double *lastSample, unsigned int *ns, double ***path, TAtlasBase *w)
Determines the connection between two points on the manifold.
Definition: samples.c:694
void SaveSamples(char *fname, char *suffix, unsigned int nvs, unsigned int ns, double **path)
Saves a set of samples to a file.
Definition: samples.c:2879
CBLAS_INLINE void SumVector(unsigned int s, double *v1, double *v2, double *v)
Adds two vectors.
Definition: basic_algebra.c:67
void DeleteJacobian(TJacobian *j)
Destructor.
Definition: jacobian.c:255
void DifferenceVectorTopology(unsigned int s, unsigned int *tp, double *v1, double *v2, double *v)
Substracts two vectors.
CBLAS_INLINE void AccumulateVector(unsigned int s, double *v1, double *v2)
Adds a vector to another vectors.
Definition: basic_algebra.c:55
boolean EffortAndDispersionGradient(Tparameters *p, unsigned int *tp, boolean *sv, unsigned int m, double *xpp, double *xp, double *x, double *xn, double *xnn, Tchart *cpp, Tchart *cp, Tchart *c, Tchart *cn, Tchart *cnn, TJacobian *sJ, TStepCost cost, double *u, Tworld *w)
Gradient of the effort and dispersion.
Definition: samples.c:1693
void ReverseConcatSamples(unsigned int nvs, unsigned int ns1, double **path1, unsigned int ns2, double **path2, unsigned int *ns, double ***path)
Reverses and concats a path.
Definition: samples.c:2727
double PathLength(unsigned int *tp, boolean *sv, unsigned int m, unsigned int np, double **point)
Length of a path formed by a set of samples.
Definition: samples.c:1096
#define INIT_NUM_SAMPLES
Initial number of samples (or steps) in a path.
Definition: samples.h:28
Definition of the Tfilename type and the associated functions.
int cmpUInt(const void *a, const void *b)
Compares two unsigned in integers.
Definition: samples.c:1803
#define TRUE
TRUE.
Definition: boolean.h:21
unsigned int WorldManifoldDimension(Tparameters *p, double *point, Tworld *w)
Dimensionality of the solution manifold.+.
Definition: world.c:3404
double Chart2Manifold(Tparameters *pr, TJacobian *sJ, double *t, unsigned int *tp, double *pInit, double *p, Tchart *c)
Returns the point in the manifold for a given set of parameteres.
Definition: chart.c:1069
void Error(const char *s)
General error function.
Definition: error.c:80
void DifferenceVectorTopologySubset(unsigned int s, unsigned int *tp, boolean *subset, double *v1, double *v2, double *v)
Substracts two vectors along some of its components.
All the necessary information to generate equations for mechanisms.
Definition: world.h:229
A color.
Definition: color.h:86
void PlotLine(double x0, double y0, double x1, double y1, Tplot *p)
Plots a segment.
Definition: plot.c:215
double CombineCosts(TStepCost c1, TStepCost c2, Tparameters *p, unsigned int *tp, boolean *sv, unsigned int m, double *xp, double *x, double *xn, unsigned int current, Tchart *cp, Tchart *c, Tchart *cn, Tworld *w)
Combines two cost functions.
Definition: samples.c:1611
void GetJacobianSize(unsigned int *nr, unsigned int *nc, TJacobian *j)
Returns the size of the Jacobian.
Definition: jacobian.c:49
#define ON_CUIKSYSTEM(wcs)
TRUE if the atlas is defined on a cuiksystem.
Definition: wcs.h:139
boolean ReadListOfBoxes(char *filename, Tlist *l)
Reads a list of boxes from a file.
Definition: box_list.c:286
double PathEffort(Tparameters *p, unsigned int m, unsigned int np, double **point, Tworld *w)
Approximated control effort of a path.
Definition: samples.c:1708
A chart on a manifold.
Definition: chart.h:70
unsigned int GetBoxNIntervals(Tbox *b)
Box dimensionality.
Definition: box.c:1016
void DeleteFileName(Tfilename *fn)
Destructor.
Definition: filename.c:205
A 3D plot.
Definition: plot3d.h:54
#define CT_CUT_X
Limit of domain of the X dimension of 3d plots.
Definition: parameters.h:420
void DeleteListOfBoxes(Tlist *l)
Destructor.
Definition: box_list.c:353
double Distance(unsigned int s, double *v1, double *v2)
Computes the distance of two points.
double StepDispersion(Tparameters *p, unsigned int *tp, boolean *sv, unsigned int m, double *xp, double *x, double *xn, unsigned int current, Tchart *cp, Tchart *c, Tchart *cn, Tworld *w)
Dispersion path step.
Definition: samples.c:1112
void PlotForceField(Tparameters *p, Tplot3d *p3d, unsigned int xID, unsigned int yID, unsigned int zID, Tworld *w, unsigned int ns, double **sols)
Plots the force-field on a set of points.
Definition: samples.c:3056
unsigned int InitChart(Tparameters *pr, boolean simple, Tbox *domain, unsigned int *tp, unsigned int m, unsigned int k, double *p, double e, double eCurv, double r, TJacobian *sJ, TAtlasBase *w, Tchart *c)
Constructor.
Definition: chart.c:795
CBLAS_INLINE double GeneralDotProduct(unsigned int s, double *v1, double *v2)
Computes the dot product of two general vectors.
Definition: basic_algebra.c:15
#define KINEMATIC_SUBSPACE
Focus on the kinematic sub-space.
Definition: samples.h:117
CBLAS_INLINE void MatrixVectorProduct(unsigned int r, unsigned int c, double *A, double *b, double *o)
Product of a matrix and a vector.
boolean EndOfList(Titerator *i)
Checks if an iterator is pointing at the end of the list.
Definition: list.c:445
#define CT_CUT_Z
Limit of domain of the Z dimension of 3d plots.
Definition: parameters.h:444
#define CS_WD_SIMP_INEQUALITIES_HOLD(pr, p, wcs)
Cheks if all inequalities hold.
Definition: wcs.h:207
#define SMOOTH_EFFORT
One of the possible smoothing methods.
Definition: samples.h:79
A generic list.
Definition: list.h:46
void InitIterator(Titerator *i, Tlist *list)
Constructor.
Definition: list.c:284
void DifferenceVector(unsigned int s, double *v1, double *v2, double *v)
Substracts two vectors.
Definition of the Tlist type and the associated functions.
#define CS_WD_NEWTON_IN_SIMP(pr, p, wcs)
Applies a Newton procedure to move a point towads the manifold.
Definition: wcs.h:580
#define SMOOTH_SHORTCUT
One of the possible smoothing methods.
Definition: samples.h:67
Statistics associated with a solving process.
Definition: statistics.h:28
double * GetChartTangentSpace(Tchart *c)
Gets the chart tangent space.
Definition: chart.c:973
#define CS_WD_MANIFOLD_DIMENSION(pr, p, wcs)
Computes the dimension of the solution set.
Definition: wcs.h:565
void WorldForceFieldProjectedGradient(Tparameters *p, boolean simp, double *proj, double *sol, double **g, void *w)
The projected gradient of the force field.
Definition: world.c:2776
unsigned int GetWorldSimpTopology(Tparameters *p, unsigned int **t, Tworld *w)
Gets the topology of the variables.
Definition: world.c:3153
void Warning(const char *s)
General warning function.
Definition: error.c:116
A table of parameters.
void CreateFileName(char *path, char *name, char *suffix, char *ext, Tfilename *fn)
Constructor.
Definition: filename.c:22
boolean StepDispersionGradient(Tparameters *p, unsigned int *tp, boolean *sv, unsigned int m, double *xpp, double *xp, double *x, double *xn, double *xnn, Tchart *cpp, Tchart *cp, Tchart *c, Tchart *cn, Tchart *cnn, TJacobian *sJ, TStepCost cost, double *u, Tworld *w)
Gradient of the step dispersion penalty term.
Definition: samples.c:1135
#define CS_WD_GET_SIMP_JACOBIAN(pr, J, wcs)
Computes the Jacobian of the simplified system.
Definition: wcs.h:553
boolean StepCostNumericalGradient(Tparameters *p, unsigned int *tp, boolean *sv, unsigned int m, double *xpp, double *xp, double *x, double *xn, double *xnn, Tchart *cpp, Tchart *cp, Tchart *c, Tchart *cn, Tchart *cnn, TJacobian *sJ, TStepCost cost, double *u, Tworld *w)
Generic numerical gradient.
Definition: samples.c:1558
double StepLength(Tparameters *p, unsigned int *tp, boolean *sv, unsigned int m, double *xp, double *x, double *xn, unsigned int current, Tchart *cp, Tchart *c, Tchart *cn, Tworld *w)
Length path step.
Definition: samples.c:1203
void ConcatSamples(unsigned int nvs, unsigned int ns1, double **path1, unsigned int ns2, double **path2, unsigned int *ns, double ***path)
Concats two path.
Definition: samples.c:2706
void DeleteChart(Tchart *c)
Destructor.
Definition: chart.c:2014
void WorldForceField(Tparameters *p, boolean simp, double *sol, double **g, void *w)
Evaluates the gradient of the potential energy of a configuration.
Definition: world.c:2722
Type defining the equations on which the atlas is defined.
Definition: wcs.h:30
#define CS_WD_GET_SYSTEM_VARS(sv, wcs)
Gets the system variables.
Definition: wcs.h:238
char * GetFileFullName(Tfilename *fn)
Gets the file full name (paht+name+extension).
Definition: filename.c:151
#define M_2PI
2*Pi.
Definition: defines.h:100
#define SOL_EXT
File extension for solution files.
Definition: filename.h:138
#define CT_REPRESENTATION
Representation.
Definition: parameters.h:215
CBLAS_INLINE void TMatrixVectorProduct(unsigned int r, unsigned int c, double *A, double *b, double *o)
Product of a transposed matrix and a vector.
void SmoothSamples(Tparameters *pr, boolean parallel, int mode, unsigned int maxIterations, unsigned int ns, double **path, unsigned int *sns, double ***spath, TAtlasBase *w)
Path smoothing.
Definition: samples.c:2541
unsigned int InitTrustedChart(Tparameters *pr, boolean simple, Tbox *domain, unsigned int *tp, unsigned int m, unsigned int k, double *p, double e, double eCurv, double r, TJacobian *sJ, TAtlasBase *w, Tchart *c)
Constructor.
Definition: chart.c:819
#define CS_WD_REGENERATE_ORIGINAL_POINT(pr, p, o, wcs)
Completes an original point from a simplified one.
Definition: wcs.h:293
A box.
Definition: box.h:83
#define SMOOTH_RANDOM
One of the possible smoothing methods.
Definition: samples.h:47
#define JOINTS_EXT
File extension for files with samples represented by the joint values.
Definition: filename.h:188
#define MEM_DUP(_var, _n, _type)
Duplicates a previously allocated memory space.
Definition: defines.h:414
void * GetCurrent(Titerator *i)
Gets the element pointed by the iterator.
Definition: list.c:299
#define NO_UINT
Used to denote an identifier that has not been initialized.
Definition: defines.h:435
CBLAS_INLINE void SumVectorScale(unsigned int s, double *v1, double w, double *v2, double *v)
Adds two vectors with a scale.
Definition: basic_algebra.c:86
void InitBoxFromPoint(unsigned int dim, double *p, Tbox *b)
Initializes a box from a point.
Definition: box.c:43
void ShortcutSmooth(Tparameters *pr, boolean *sv, Tstatistics *stime, unsigned int *np, double ***point, TAtlasBase *w)
Fixed-cut based smoothing.
Definition: samples.c:1819
void DeleteSamples(unsigned int ns, double **path)
Deletes the space used by a set of samples.
Definition: samples.c:3159
unsigned int ReadOneSample(Tparameters *p, char *fname, unsigned int nvs, double **s)
Reads one sample from a file.
Definition: samples.c:2772
unsigned int GetWorldSystemVars(boolean **sv, Tworld *w)
Gets the system vars of the kinematic cuiksystem.
Definition: world.c:2383
void DeleteBox(void *b)
Destructor.
Definition: box.c:1283
The Jacobian of a set of equations.
Definition: jacobian.h:23
#define CT_E
Chart error in atlas.
Definition: parameters.h:243
#define DEFAULT_CONNECT
Macro to easily switch the connection method.
Definition: samples.h:102
void SaveSamplesInt(Tfilename *fpath, unsigned int nvs, unsigned int ns, double **path)
Internal function to save a set of samples to a file.
Definition: samples.c:2845
double Manifold2Chart(double *p, unsigned int *tp, double *t, Tchart *c)
Returns the parametrization of a point.
Definition: chart.c:1039
double GetParameter(unsigned int n, Tparameters *p)
Gets the value for a particular parameter.
Definition: parameters.c:93
#define CS_WD_REGENERATE_SOLUTION_POINT(pr, p, r, wcs)
Compleates a solution point with the dummy variables.
Definition: wcs.h:480
#define CT_CUT_Y
Limit of domain of the Y dimension of 3d plots.
Definition: parameters.h:432
boolean LengthAndDispersionGradient(Tparameters *p, unsigned int *tp, boolean *sv, unsigned int m, double *xpp, double *xp, double *x, double *xn, double *xnn, Tchart *cpp, Tchart *cp, Tchart *c, Tchart *cn, Tchart *cnn, TJacobian *sJ, TStepCost cost, double *u, Tworld *w)
Gradient of the length and dispersion.
Definition: samples.c:1668
void DeleteColor(Tcolor *c)
Destructor.
Definition: color.c:143
#define CS_WD_IN_COLLISION(f, pr, s, sPrev, wcs)
Checks if a configuration is in collision.
Definition: wcs.h:355
#define INF
Infinite.
Definition: defines.h:70
void GetWorldSimpJacobian(Tparameters *p, TJacobian *J, Tworld *w)
Gets the simplified kinematic Jacobian.
Definition: world.c:3145
unsigned int WorldGenerateSimplifiedPoint(Tparameters *p, double *o, double **s, Tworld *w)
Reconstruct a point in simplified kinematic system from an original point.
Definition: world.c:3362
List iterator.
Definition: list.h:61
#define CT_DELTA
Size of the steps in the path connecting two configurations.
Definition: parameters.h:282
#define CT_N_DOF
Dimensionality of the solution space for the mechanism at hand.
Definition: parameters.h:318
void GetBoxCenter(boolean *used, double *c, Tbox *b)
Returns the box center along the selected dimensions.
Definition: box.c:721
Auxiliary functions to deal with sets of samples.
#define CS_WD_FROM_WORLD(ptr, wcs)
Initializes the equations from a world structure.
Definition: wcs.h:108
void InitSamples(unsigned int *ms, unsigned int *ns, double ***path)
Initializes a set of samples.
Definition: samples.c:665
Definition of basic randomization functions.
#define CS_WD_INIT_CD(pr, mt, wcs)
Initializes the collision detector.
Definition: wcs.h:308
double GetElapsedTime(Tstatistics *t)
Elapsed time.
Definition: statistics.c:92
void PrintBox(FILE *f, Tbox *b)
Prints a box.
Definition: box.c:1142
void NewColor(double r, double g, double b, Tcolor *c)
Constructor.
Definition: color.c:14
boolean StepEffortGradient(Tparameters *p, unsigned int *tp, boolean *sv, unsigned int m, double *xpp, double *xp, double *x, double *xn, double *xnn, Tchart *cpp, Tchart *cp, Tchart *c, Tchart *cn, Tchart *cnn, TJacobian *sJ, TStepCost cost, double *u, Tworld *w)
Gradient of the control effort of a path step.
Definition: samples.c:1344
unsigned int ListSize(Tlist *list)
Gets the number of elements in the list.
Definition: list.c:180
#define CS_WD_GENERATE_SIMP_INITIAL_BOX(pr, b, wcs)
Computes the global box for the simplified system.
Definition: wcs.h:539
unsigned int WorldSimpKinematicVars(Tparameters *p, boolean **kv, Tworld *w)
Creates a boolean array to identify kinematic variables.
Definition: world.c:2616
Definition of the Tparameters type and the associated functions.
void ChangeParameter(unsigned int n, double v, Tparameters *p)
Sets the value for a particular parameter.
Definition: parameters.c:164
unsigned int StartNew3dObject(Tcolor *c, Tplot3d *p)
Start a composed object.
Definition: plot3d.c:157
void GetWorldSimpInitialBox(Tparameters *p, Tbox *b, Tworld *w)
Gets the kinematic simplified search space for a given problem.
Definition: world.c:3396
void DeleteStatistics(Tstatistics *t)
Destructor.
Definition: statistics.c:274
boolean CombineGradients(TStepCostGradient g1, TStepCostGradient g2, Tparameters *p, unsigned int *tp, boolean *sv, unsigned int m, double *xpp, double *xp, double *x, double *xn, double *xnn, Tchart *cpp, Tchart *cp, Tchart *c, Tchart *cn, Tchart *cnn, TJacobian *sJ, TStepCost cost, double *u, Tworld *w)
Combines two gradients.
Definition: samples.c:1622
void AddSample2Samples(unsigned int nv, double *sample, unsigned int nvs, boolean *systemVars, unsigned int *ms, unsigned int *ns, double ***path)
Adds a sample to a set of samples.
Definition: samples.c:672
#define DIVERGED
One of the possible outputs of the Newton iteration.
Definition: cuiksystem.h:111
#define LINKS_EXT
File extension for files with samples represented by the link poses.
Definition: filename.h:181
#define CT_CE
Chart error in atlas.
Definition: parameters.h:250
boolean Advance(Titerator *i)
Moves an iterator to the next position of its associated list.
Definition: list.c:373
#define CS_WD_GET_SIMP_TOPOLOGY(pr, tp, wcs)
Gets the simplified variable topology.
Definition: wcs.h:450
void Close3dObject(Tplot3d *p)
Closes a composed object.
Definition: plot3d.c:171
boolean StepLengthGradient(Tparameters *p, unsigned int *tp, boolean *sv, unsigned int m, double *xpp, double *xp, double *x, double *xn, double *xnn, Tchart *cpp, Tchart *cp, Tchart *c, Tchart *cn, Tchart *cnn, TJacobian *sJ, TStepCost cost, double *u, Tworld *w)
Gradient of the length of a path step.
Definition: samples.c:1224
boolean LoadSamples(Tfilename *fname, unsigned int *nvs, unsigned int *ns, double ***path)
Reads a set of samples from file.
Definition: samples.c:2913
void SaveSamplesN(char *fname, boolean smooth, unsigned int n, unsigned int nvs, unsigned int ns, double **path)
Saves a set of samples to a file.
Definition: samples.c:2894
boolean PointInBoxTopology(boolean *used, boolean update, unsigned int n, double *v, double tol, unsigned int *tp, Tbox *b)
Checks if a point is included in a(sub-) box.
Definition: box.c:374
double EffortAndDispersion(Tparameters *p, unsigned int *tp, boolean *sv, unsigned int m, double *xp, double *x, double *xn, unsigned int current, Tchart *cp, Tchart *c, Tchart *cn, Tworld *w)
Combines control effort and dispersion cost functions.
Definition: samples.c:1683
void ReverseSamples(unsigned int ns, double **path)
Reverses a set of samples.
Definition: samples.c:2752