chart.c
Go to the documentation of this file.
1 #include "chart.h"
2 
3 #include "defines.h"
4 #include "error.h"
5 #include "random.h"
6 #include "algebra.h"
7 #include "samples.h"
8 
9 #include <string.h>
10 #include <math.h>
11 
70 unsigned int InitChartInt(boolean trusted,boolean singular,boolean forceRS,Tparameters *pr,boolean simple,
71  Tbox *domain,unsigned int *tp,unsigned int m,unsigned int k,double *p,double e,
72  double eCurv,double r,double *T,TJacobian *sJ,TAtlasBase *w,Tchart *c);
73 
96 unsigned int ComputeJacobianKernelBasis(double epsilon,TJacobian *sJ,Tchart *c);
97 
106 void LinkChart(unsigned int id,Tchart *c);
107 
126 boolean IntersectChartsInt(Tparameters *pr,boolean cut,
127  unsigned int *tp,Tbox *ambient,
128  unsigned int id1,Tchart *c1,
129  unsigned int id2,Tchart *c2);
130 
131 
164 void PlotChartAsPolygon(Tparameters *pr,FILE *fcost,TJacobian *sJ,
165  unsigned int xID,unsigned int yID,unsigned int zID,
166  Tplot3d *p3d,Tchart *c);
167 
168 
185 void PlotChartAsBox(Tparameters *pr,
186  unsigned int xID,unsigned int yID,unsigned int zID,
187  Tplot3d *p3d,Tchart *c);
188 
189 /****************************************************************************/
190 /****************************************************************************/
191 /****************************************************************************/
192 
193 unsigned int ComputeJacobianKernelBasis(double epsilon,TJacobian *sJ,Tchart *c)
194 {
195  unsigned int out;
196  double *JT;
197 
198  NEW(JT,c->m*c->nrJ,double);
199  EvaluateTransposedJacobianInVector(c->center,c->m,c->nrJ,JT,sJ);
200 
201  #if (_DEBUG>5)
202  fprintf(stderr," Computing kernel+basis\n");
203  PrintVector(stderr," c",c->m,c->center);
204  PrintTMatrix(stderr," J",c->m,c->nrJ,JT);
205  #endif
206 
207  out=FindKernelAndIndependentRows(c->nrJ,c->m,JT,c->k,epsilon,&(c->singular),&(c->BJ),&(c->T));
208 
209  #if (_DEBUG>5)
210  PrintTMatrix(stderr," T",c->m,c->k,c->T);
211  #endif
212 
213  free(JT);
214 
215  return(out);
216 }
217 
218 void LinkChart(unsigned int id,Tchart *c)
219 {
220  if (!c->singular)
221  Error("Only singular maps can be linked to other singular maps");
222 
223  if (c->nl==c->ml)
224  MEM_DUP(c->l,c->ml,unsigned int);
225 
226  c->l[c->nl]=id;
227  c->nl++;
228 }
229 
230 void AddBorderConstraint(Tparameters *pr,double *t,unsigned int *tp,Tbox *ambient,Tchart *c)
231 {
232  double nm;
233 
234  nm=Norm(c->k,t);
235  /* To avoid numerical instabilities, do not use too small vectors. */
236  //if (nm>MIN_SAMPLING_RADIUS)
237  {
238  if (c->simple)
239  CutSPolytopeWithFace(t,nm*nm/2,NO_UINT,c->sp);
240  else
241  CutPolytopeWithFace(pr,t,nm*nm/2,NO_UINT,(void *)c->w,(void *)c,c->m,tp,ambient,c->p);
242  }
243 }
244 
245 void ForceChartCut(Tparameters *pr,unsigned int *tp,Tbox *ambient,
246  unsigned int id1,Tchart *c1,
247  unsigned int id2,Tchart *c2)
248 {
249  double *t;
250 
251  NEW(t,c1->k,double);
252 
253  Manifold2Chart(c2->center,tp,t,c1);
254  if (c1->simple)
255  CutSPolytope(t,c2->r,id2,c1->sp);
256  else
257  CutPolytope(pr,t,c2->r,id2,(void *)c1->w,(void *)c1,c1->m,tp,ambient,c1->p);
258 
259  Manifold2Chart(c1->center,tp,t,c2);
260  if (c2->simple)
261  CutSPolytope(t,c1->r,id1,c2->sp);
262  else
263  CutPolytope(pr,t,c1->r,id1,(void *)c2->w,(void *)c2,c2->m,tp,ambient,c2->p);
264 
265  free(t);
266 }
267 
268 boolean IntersectChartsInt(Tparameters *pr,boolean cut,
269  unsigned int *tp,Tbox *ambient,
270  unsigned int id1,Tchart *c1,
271  unsigned int id2,Tchart *c2)
272 {
273  boolean neighbours;
274 
275  neighbours=FALSE;
276 
277  if ((c1->w==c2->w)&&(c1->m==c2->m)&&(c1->k==c2->k))
278  {
279  double d,sr;
280  double *t1,*t2;
281  double n1,n2;
282  double e;
283 
284  d=DistanceTopology(c1->m,tp,c1->center,c2->center);
285 
286  /* At a given point we used a reduced radius for
287  the ball projected in the "other" tangent space.
288  We even mention this in our papers but we have
289  to fix it.
290  Using a reduced radius is appealing since the
291  projection of a ball has less area/volume than
292  the original ball. However, intersecting balls
293  with different radius does not warrantee the
294  balls to intersect at all and, if they do, the
295  plane defined by the intersection does not
296  necessarity cuts out the polytope vertex used
297  to generate the new chart. This is specially true
298  when the new chart is generated very close
299  to the previous one (this occurs in high curvature
300  ares). If the vertex used to generate the new
301  chart is not removed from the polytope the algorithm
302  iterates forever selecting the same vertice and
303  generating the same new chart forever.
304  To avoid this problem we use keep the original
305  radius for the projected ball. Two balls with the
306  same radius always intersect and the resulting plane
307  always crosses the line in between the two ball
308  centers cropping one vertex from the polyedron.
309  */
310  sr=c1->r+c2->r;
311 
312  if (d<sr)
313  {
314  /*center of chart1 in chart2*/
315  NEW(t1,c2->k,double);
316  n1=Manifold2Chart(c1->center,tp,t1,c2);
317 
318  /* n1 = norm(t1) */
319  /* Points are typically selected on the ball of radius 'r'
320  and we allow for some error in the back-projection */
321  //if ((n1<1.1*c2->r)&&(n1/d>(1.0-c2->eCurv)))
322  {
323  /* compute 'e': distance from the center of c1 to c2 */
324  e=d*d-n1*n1;
325 
326  if (e<-ZERO)
327  Error("Non-orthonormal tangent space in IntersectChartsInt (1)?");
328  if (e<ZERO) e=0.0; /* tiny neg. values are set to zero */
329  e=sqrt(e);
330 
331  if (e<c2->error)
332  {
333  /*center of chart2 in chart1*/
334  NEW(t2,c1->k,double);
335  n2=Manifold2Chart(c2->center,tp,t2,c1);
336 
337  /* n2 = norm(t2) */
338  //if ((n2<1.1*c1->r)&&(n2/d>(1.0-c1->eCurv)))
339  {
340  /* compute 'e': distance from the center of c2 to c1 */
341  e=d*d-n2*n2;
342  if (e<-ZERO)
343  Error("Non-orthonormal tangent space in IntersectChartsInt (2)?");
344  if (e<ZERO) e=0.0; /* tiny neg. values are set to zero */
345  e=sqrt(e);
346 
347  if (e<c1->error)
348  {
349  boolean compare;
350 
351  compare=((c1->n==0)||(CompareTangentSpaces(c1,c2)));
352  if (compare)
353  {
354  neighbours=TRUE;
355  if (cut)
356  {
357  if (c1->simple)
358  CutSPolytope(t2,c2->r,id2,c1->sp);
359  else
360  CutPolytope(pr,t2,c2->r,id2,(void *)c1->w,(void *)c1,c1->m,tp,ambient,c1->p);
361 
362  if (c2->simple)
363  CutSPolytope(t1,c1->r,id1,c2->sp);
364  else
365  CutPolytope(pr,t1,c1->r,id1,(void *)c2->w,(void *)c2,c2->m,tp,ambient,c2->p);
366  }
367  }
368  }
369  }
370  free(t2);
371  }
372  }
373  free(t1);
374  }
375  }
376  else
377  Error("Intersecting non-compatible local charts");
378 
379  return(neighbours);
380 }
381 
382 
383 void PlotChartAsPolygon(Tparameters *pr,FILE *fcost,TJacobian *sJ,
384  unsigned int xID,unsigned int yID,unsigned int zID,
385  Tplot3d *p3d,Tchart *c)
386 {
387  double cost;
388  Tcolor color;
389 
390  if (c->k!=2)
391  Error("PlotChartAsFace only works for 2d manifolds");
392 
393  /* Read the cost associated with the chart, if any. */
394  if (fcost!=NULL)
395  {
396  if (fscanf(fcost,"%lf",&cost)!=1)
397  Error("No enough data in the cost file");
398  if (cost>0)
399  CostColor(cost,1e-4,&color);
400  }
401 
402  /* When plotting with a cost function the frontier charts are not plotted (?) */
403  if ((fcost==NULL)||(!c->frontier))
404  {
405  unsigned int nv;
406  double **v;
407  Tcpolytope *pol;
408 
409  if (c->simple)
410  {
411  NEW(pol,1,Tcpolytope);
412  SPolytope2Polytope(pr,c->sp,pol);
413  }
414  else
415  pol=c->p;
416 
417  GetPolytopeVertices(c->k,&nv,&v,pol);
418 
419  if (nv>0)
420  {
421  unsigned int ne;
422  unsigned int *v1;
423  unsigned int *v2;
424  unsigned int i,k;
425  double **p,*g,*o;
426  unsigned int current,n1,n2,nc;
427  boolean found;
428  boolean *visited;
429  unsigned int *fv;
430  boolean e;
431 
432  NEW(p,nv,double *);
433  NEW(g,c->m,double);
434  NEW(visited,nv,boolean);
435  NEW(fv,nv,unsigned int); /*face vertices in order*/
436 
437  for(i=0;i<nv;i++)
438  {
439  NEW(p[i],3,double);
440  if ((!PLOT_ON_MANIFOLD)||
441  (Chart2Manifold(pr,sJ,v[i],NULL,NULL,g,c)==INF))
442  Local2Global(v[i],NULL,g,c);
443  CS_WD_REGENERATE_ORIGINAL_POINT(pr,g,&o,c->w);
444 
445  p[i][0]=o[xID];
446  p[i][1]=o[yID];
447  p[i][2]=o[zID];
448 
449  free(o);
450 
451  visited[i]=FALSE;
452  }
453 
454  //if (!ON_CUIKSYSTEM(c->w))
455  {
456  unsigned int rep;
457 
458  rep=(unsigned int)(GetParameter(CT_REPRESENTATION,pr));
459  if (rep==REP_JOINTS)
460  {
461  double ox,oy,oz;
462  double cx,cy,cz;
463 
464  cx=GetParameter(CT_CUT_X,pr);
465  cy=GetParameter(CT_CUT_Y,pr);
466  cz=GetParameter(CT_CUT_Z,pr);
467 
468  /* If any of the points in the polygon is above the cut
469  displace the whole polygon -2pi */
470  ox=0;
471  oy=0;
472  oz=0;
473  for(i=0;i<nv;i++)
474  {
475  if ((cx<0)&&(p[i][0]<cx))
476  ox=+M_2PI;
477  if ((cx>0)&&(p[i][0]>cx))
478  ox=-M_2PI;
479  if ((cy<0)&&(p[i][1]<cy))
480  oy=+M_2PI;
481  if ((cy>0)&&(p[i][1]>cy))
482  oy=-M_2PI;
483  if ((cz<0)&&(p[i][2]<cz))
484  oz=+M_2PI;
485  if ((cz>0)&&(p[i][2]>cz))
486  oz=-M_2PI;
487  }
488 
489  for(i=0;i<nv;i++)
490  {
491  p[i][0]+=ox;
492  p[i][1]+=oy;
493  p[i][2]+=oz;
494  }
495  }
496  }
497 
498  GetPolytopeEdges(&ne,&v1,&v2,pol);
499 
500  /* Start the face at the first vertex*/
501  fv[0]=0;
502  current=0;
503  visited[0]=TRUE;
504  nc=1; /*number of vertices already in the face*/
505  e=FALSE; /* error in the polygon */
506 
507  while(nc<nv)
508  {
509  /*look for a not visited neighbour*/
510  found=FALSE;
511  i=0;
512  while((!found)&&(i<ne))
513  {
514  /*Extremes of the edges*/
515  n1=v1[i];
516  n2=v2[i];
517 
518  if ((n1==current)&&(!visited[n2]))
519  {
520  found=TRUE;
521  k=n2;
522  }
523  else
524  {
525  if ((n2==current)&&(!visited[n1]))
526  {
527  found=TRUE;
528  k=n1;
529  }
530  else
531  i++;
532  }
533  }
534  if (!found)
535  {
536  Warning("A vertex without neighbours!!");
537  //e=TRUE;
538  }
539  visited[k]=TRUE;
540  fv[nc]=k;
541  current=k;
542  nc++;
543  }
544 
545  if (!e)
546  {
547  if (fcost!=NULL)
548  {
549  if (cost>0)
550  Plot3dObjectWithColor(nv,1,0,p,&nv,&fv,&color,p3d);
551  }
552  else
553  Plot3dObject(nv,1,0,p,&nv,&fv,p3d);
554  }
555 
556  for(i=0;i<nv;i++)
557  {
558  free(v[i]);
559  free(p[i]);
560  }
561 
562  free(p);
563  free(v);
564  free(v1);
565  free(v2);
566  free(g);
567  free(visited);
568  free(fv);
569  }
570 
571  if (c->simple)
572  {
573  DeletePolytope(pol);
574  free(pol);
575  }
576  }
577 }
578 
579 
580 void PlotChartAsBox(Tparameters *pr,
581  unsigned int xID,unsigned int yID,unsigned int zID,
582  Tplot3d *p3d,Tchart *c)
583 {
584  double *o;
585 
586  if (c->k!=3)
587  Error("PlotChartAsBox only works for 3d manifolds");
588 
589  CS_WD_REGENERATE_ORIGINAL_POINT(pr,c->center,&o,c->w);
590 
591  PlotBox3d(o[xID]-c->r,o[xID]+c->r,
592  o[yID]-c->r,o[yID]+c->r,
593  o[zID]-c->r,o[zID]+c->r,
594  p3d);
595 
596  free(o);
597 }
598 
599 void InitCSWDFromFile(Tparameters *pr,char *name,TAtlasBase *wcs)
600 {
601  FILE *f;
602  Tfilename fn;
603 
604  CreateFileName(NULL,name,NULL,CUIK_EXT,&fn);
605  f=fopen(GetFileFullName(&fn),"r");
606  if (f)
607  {
608  /* The cuik file exists -> read from it */
609  fclose(f);
610  fprintf(stderr,"Reading cuik from : %s\n",GetFileFullName(&fn));
611 
612  wcs->isCS=TRUE;
613  wcs->w=NULL;
614  NEW(wcs->cs,1,TCuikSystem);
615  InitCuikSystemFromFile(pr,GetFileFullName(&fn),wcs->cs);
616  }
617  else
618  {
619  /* The cuik file does not exits -> try to read the world one */
620  wcs->isCS=FALSE;
621  wcs->cs=NULL;
622  NEW(wcs->w,1,Tworld);
623  InitWorldFromFile(pr,FALSE,TRUE,name,wcs->w);
624  }
625  DeleteFileName(&fn);
626 }
627 
628 unsigned int InitChartInt(boolean trusted,boolean singular,boolean forceRS,Tparameters *pr,boolean simple,
629  Tbox *domain,unsigned int *tp,unsigned int m,unsigned int k,double *p,double e,
630  double eCurv,double r,double *T,TJacobian *sJ,TAtlasBase *w,Tchart *c)
631 {
632  double epsilon;
633  double error;
634  double sr; /* sampling radious */
635  unsigned int out;
636  double delta;
637  unsigned int nrJ,ncJ;
638 
639  out=0; /* no problem generating the chart. */
640 
641  epsilon=GetParameter(CT_EPSILON,pr);
642  delta=GetParameter(CT_DELTA,pr);
643 
644  sr=GetParameter(CT_SR,pr);
645  if (sr==0)
646  sr=(double)k;
647  if (sr<r+2*delta)
648  sr=r+2*delta;
649 
650  c->w=w;
651 
652  c->m=m; /* number of variables */
653  c->k=k; /* dimensionality of the manifold*/
654  c->da=0; /* no linearized dynamics on the chart so far */
655  c->n=c->m-c->k; /* number of non-redundant equations (equalities) */
656 
657  if ((c->k==0)||(c->m==0)||(c->k>c->m))
658  Error("Dimension missmatch in InitChart (2)");
659 
660  GetJacobianSize(&nrJ,&ncJ,sJ);
661  if ((c->m!=ncJ)||(c->n>nrJ))
662  Error("Jacobian-system missmatch in InitChart (1)");
663 
664  /* Default initialization */
665  c->center=NULL;
666  c->T=NULL;
667  c->BJ=NULL;
668  c->singular=FALSE;
669  c->ml=0;
670  c->nl=0;
671  c->l=NULL;
672  c->degree=NO_UINT; /* We defer the computation of the degree until needed */
673  c->simple=simple;
674  c->p=NULL;
675  c->sp=NULL;
676 
677  /* Copy the center */
678  NEW(c->center,c->m,double);
679  memcpy(c->center,p,c->m*sizeof(double));
680 
681  /* Note that PointInBoxTopology can modify the center !! */
682  if (trusted)
683  {
684  c->frontier=FALSE;
685  if (tp!=NULL)
686  ArrayPi2Pi(c->m,tp,c->center);
687  }
688  else
689  c->frontier=((!PointInBoxTopology(NULL,TRUE,c->m,c->center,epsilon,tp,domain))||
690  (!CS_WD_SIMP_INEQUALITIES_HOLD(pr,c->center,c->w)));
691 
692  /* Check if the (modified) center is actually on the manifold */
693  if ((trusted)||(c->n==0))
694  error=0.0;
695  else
696  error=CS_WD_ERROR_IN_SIMP_EQUATIONS(pr,c->center,c->w);
697 
698  if (error>epsilon)
699  out=4; /* The map can not be defined since the given point is not in the manifold */
700  else
701  {
702  c->error=e;
703  c->eCurv=eCurv;
704  c->r=r;
705 
706  if (trusted)
707  c->collision=FALSE;
708  else
709  { CS_WD_IN_COLLISION(c->collision,pr,c->center,NULL,c->w); }
710 
711  c->frontier=((c->frontier)||(c->collision));
712 
713  c->nrJ=nrJ;
714 
715  if (c->n>0) /* no empty problem */
716  {
717  if (T!=NULL)
718  {
719  NEW(c->T,c->m*c->k,double);
720  memcpy(c->T,T,sizeof(double)*(c->m*c->k));
721  }
722  else
723  {
724  /* Initialize c->T, c->BJ and c->singular */
725  out=ComputeJacobianKernelBasis(epsilon,sJ,c);
726 
727  if ((!singular)&&(forceRS)&&(c->singular))
728  {
729  PrintVector(stderr,"c",c->m,c->center);
730  Error("The point is not regular (and is supposed to be)");
731  }
732  }
733  }
734 
735  if (out<(singular?2:1)) /* If the chart could be actually created */
736  {
737  if ((singular)||(T!=NULL))
738  {
739  /* Even if ComputeJacobianKernelBasis determines that the point
740  is not singular, we force it to be singular (we are for sure
741  close to a singularity). */
742  if ((forceRS)||(T!=NULL))
743  c->singular=TRUE;
744 
745  /* Initialize the list of related charts (at a singular point
746  we can define more than one chart (one for each branch). */
747  c->ml=1;
748  NEW(c->l,c->ml,unsigned int);
749  }
750 
751  /* Create the polytope to expand the chart. Note that we use k1
752  (i.e., we only take into account the feasible velocity sub-space). */
753  if (c->simple)
754  {
755  NEW(c->sp,1,Tscpolytope);
756  InitEmptySPolytope(delta,c->k,r,sr,c->sp);
757  }
758  else
759  {
760  NEW(c->p,1,Tcpolytope);
761 
762  InitEmptyPolytope(c->k,r,c->p);
763  }
764  }
765 
766  }
767 
768  /* We reset the dynamic information. If necessary, the dynamic information must
769  be explicitly added using SetLinearizedDynamics */
770  c->A=NULL;
771  c->B=NULL;
772  c->c=NULL;
773  c->d=NULL;
774  c->iRBt=NULL;
775  c->BiRBt=NULL;
776 
777  /* If there was any error in the map definition release (possibly) used memory*/
778  if (out>(singular?1:0))
779  DeleteChart(c);
780 
781  /*
782  0 if we could actually define the chart, 1 if there the kernel is
783  too large (i.e., the given point is singular), 2 if the chart could not be defined
784  since the kernel is too small at the given point, 3 if there is an error while
785  performing QR decomposition, and 4 if the error w.r.t. the manifold is
786  too large. Most of these outputs come directly from \ref AnalyzeKernel.
787  */
788  return(out);
789 }
790 
791 unsigned int InitChart(Tparameters *pr,boolean simple,Tbox *domain,unsigned int *tp,
792  unsigned int m,unsigned int k,double *p,double e,double eCurv,double r,
793  TJacobian *sJ,TAtlasBase *w,Tchart *c)
794 {
795  /* trusted=FALSE singular=FALSE forceRS=TRUE (error if not regular) */
796  return(InitChartInt(FALSE,FALSE,TRUE,pr,simple,domain,tp,m,k,p,e,eCurv,r,NULL,sJ,w,c));
797 }
798 
799 unsigned int InitPossiblySingularChart(Tparameters *pr,boolean simple,Tbox *domain,unsigned int *tp,
800  unsigned int m,unsigned int k,double *p,double e, double eCurv, double r,
801  TJacobian *sJ,TAtlasBase *w,Tchart *c)
802 {
803  /* trusted=FALSE singular=TRUE forceRS=FALSE (no error if singular and do not force to mark it as singular) */
804  return(InitChartInt(FALSE,TRUE,FALSE,pr,simple,domain,tp,m,k,p,e,eCurv,r,NULL,sJ,w,c));
805 }
806 
807 unsigned int InitSingularChart(Tparameters *pr,boolean simple,Tbox *domain,unsigned int *tp,
808  unsigned int m,unsigned int k,double *p,double e, double eCurv,double r,
809  TJacobian *sJ,TAtlasBase *w,Tchart *c)
810 {
811  /* trusted=FALSE singular=TRUE forceRS=TRUE (mark as singular even if not detected as such) */
812  return(InitChartInt(FALSE,TRUE,TRUE,pr,simple,domain,tp,m,k,p,e,eCurv,r,NULL,sJ,w,c));
813 }
814 
815 unsigned int InitTrustedChart(Tparameters *pr,boolean simple,Tbox *domain,unsigned int *tp,
816  unsigned int m,unsigned int k,double *p,double e, double eCurv,double r,
817  TJacobian *sJ,TAtlasBase *w,Tchart *c)
818 {
819  /* trusted=TRUE singular=FALSE forceRS=TRUE (error if not regular) */
820  return(InitChartInt(TRUE,FALSE,TRUE,pr,simple,domain,tp,m,k,p,e,eCurv,r,NULL,sJ,w,c));
821 }
822 
823 unsigned int InitChartWithTangent(Tparameters *pr,boolean simple,Tbox *domain,unsigned int *tp,
824  unsigned int m,unsigned int k,double *p,double *T,
825  double e, double eCurv, double r,
826  TJacobian *sJ,TAtlasBase *w,Tchart *c)
827 {
828  /* singular=FALSE forceRS=TRUE (but forceRS plays no role since we give T) */
829  return(InitChartInt(FALSE,TRUE,TRUE,pr,simple,domain,tp,m,k,p,e,eCurv,r,T,sJ,w,c));
830 }
831 
832 void SetLinearizedDynamics(unsigned int da,double *mA,double *mB,double *vc,double *iR,Tchart *c)
833 {
834  #if (COMPUTE_LQRPOLICY_IN_T)
835  TLinearSystem ls;
836  #endif
837 
838  if (c->k%2!=0)
839  Error("Using SetLinearizedDynamics on a manifold that can not be a state space");
840 
841  c->da=da;
842  c->A=mA;
843  c->B=mB;
844  c->c=vc;
845 
846  NEW(c->iRBt,c->da*c->k,double);
847  MatrixTMatrixProduct(c->da,c->da,iR,c->k,mB,c->iRBt);
848 
849  NEW(c->BiRBt,c->k*c->k,double);
850  MatrixMatrixProduct(c->k,c->da,mB,c->k,c->iRBt,c->BiRBt);
851  SymmetrizeMatrix(c->k,c->BiRBt); /* To improve numerical stability */
852 
853  #if (COMPUTE_LQRPOLICY_IN_T)
854  InitLS(c->k,c->k,1,&ls);
855  memcpy(GetLSMatrixBuffer(&ls),mA,c->k*c->k*sizeof(double));
856  memcpy(GetLSRHBuffer(&ls),vc,c->k*sizeof(double));
857  if (LSSolve(&ls))
858  Error("Singular A matrix in SetLinearizedDynamics");
859 
860  NEW(c->d,c->k,double);
861  memcpy(c->d,GetLSSolutionBuffer(&ls),c->k*sizeof(double));
862 
863  DeleteLS(&ls);
864  #else
865  /* c->d will be not used but we define something just to avoid
866  errors when saving the chart */
867  NEWZ(c->d,c->k,double);
868  #endif
869 
870 }
871 
872 void GetLinearizedDynamics(double **mA,double **mAt,double **mB,double**vc,double **vd,
873  double **iRBt,double **BiRBt,Tchart *c)
874 {
875  *mA=c->A;
876  *mB=c->B;
877  *vc=c->c;
878  *vd=c->d;
879  *iRBt=c->iRBt;
880  *BiRBt=c->BiRBt;
881 }
882 
883 void CopyChart(Tchart *c_dst,Tchart *c_src)
884 {
885  /* Copy chart */
886  c_dst->w=c_src->w;
887 
888  c_dst->m=c_src->m;
889  c_dst->k=c_src->k;
890  c_dst->da=c_src->da;
891  c_dst->n=c_src->n;
892  c_dst->nrJ=c_src->nrJ;
893 
894  c_dst->error=c_src->error;
895  c_dst->eCurv= c_src->eCurv;
896  c_dst->r=c_src->r;
897  c_dst->degree=c_src->degree;
898  c_dst->frontier=c_src->frontier;
899  c_dst->singular=c_src->singular;
900 
901  NEW(c_dst->center,c_dst->m,double);
902  memcpy(c_dst->center,c_src->center,c_dst->m*sizeof(double));
903 
904  if ((c_src->n==0)||(c_src->T==NULL))
905  c_dst->T=NULL;
906  else
907  {
908  NEW(c_dst->T,c_dst->m*c_dst->k,double);
909  memcpy(c_dst->T,c_src->T,sizeof(double)*(c_dst->m*c_dst->k));
910  }
911 
912  /* maps defined with given tangent space don't have Jacobian basis */
913  if ((c_src->nrJ==0)||(c_src->BJ==NULL)) /* c_src->singular==TRUE */
914  c_dst->BJ=NULL;
915  else
916  {
917  NEW(c_dst->BJ,c_dst->nrJ,boolean);
918  memcpy(c_dst->BJ,c_src->BJ,c_dst->nrJ*sizeof(boolean));
919  }
920 
921  c_dst->ml=c_src->ml;
922  if (c_dst->ml>0)
923  {
924  c_dst->nl=c_src->nl;
925  NEW(c_dst->l,c_dst->ml,unsigned int);
926  memcpy(c_dst->l,c_src->l,c_dst->nl*sizeof(unsigned int));
927  }
928  else
929  {
930  c_dst->nl=0;
931  c_dst->l=NULL;
932  }
933 
934  /* Copy Polytope */
935  c_dst->simple=c_src->simple;
936 
937  if (c_src->sp!=NULL)
938  {
939  NEW(c_dst->sp,1,Tscpolytope);
940  CopySPolytope(c_dst->sp,c_src->sp);
941  c_dst->p=NULL;
942  }
943 
944  if (c_src->p!=NULL)
945  {
946  NEW(c_dst->p,1,Tcpolytope);
947  CopyPolytope(c_dst->p,c_src->p);
948  c_dst->sp=NULL;
949  }
950 
951  if (c_src->A==NULL)
952  {
953  c_dst->A=NULL;
954  c_dst->B=NULL;
955  c_dst->c=NULL;
956  c_dst->iRBt=NULL;
957  c_dst->BiRBt=NULL;
958  }
959  else
960  {
961  NEW(c_dst->A,c_dst->k*c_dst->k,double);
962  memcpy(c_dst->A,c_src->A,c_dst->k*c_dst->k*sizeof(double));
963 
964  NEW(c_dst->B,c_dst->k*(c_dst->da),double);
965  memcpy(c_dst->B,c_src->B,c_dst->k*(c_dst->da)*sizeof(double));
966 
967  NEW(c_dst->c,c_dst->k,double);
968  memcpy(c_dst->c,c_src->c,c_dst->k*sizeof(double));
969 
970  NEW(c_dst->d,c_dst->k,double);
971  memcpy(c_dst->d,c_src->d,c_dst->k*sizeof(double));
972 
973  NEW(c_dst->iRBt,(c_dst->da)*c_dst->k,double);
974  memcpy(c_dst->iRBt,c_src->iRBt,(c_dst->da)*c_dst->k*sizeof(double));
975 
976  NEW(c_dst->BiRBt,c_dst->k*c_dst->k,double);
977  memcpy(c_dst->BiRBt,c_src->BiRBt,c_dst->k*c_dst->k*sizeof(double));
978  }
979 }
980 
981 boolean CompareTangentSpaces(Tchart *c1,Tchart *c2)
982 {
983  double d;
984  double *aData;
985 
986  if (c1->w!=c2->w)
987  Error("Comparing maps defined on diferent manifolds");
988 
989  /* we can assume c1->k==c2->k */
990 
991  NEW(aData,c1->k*c1->k,double);
992 
993  TMatrixMatrixProduct(c1->m,c1->k,c1->T,c2->k,c2->T,aData);
994 
995  d=fabs(MatrixDeterminant(c1->k,aData));
996 
997  free(aData);
998 
999  return(fabs(d-1)<=c1->eCurv);
1000 }
1001 
1002 double MinCosinusBetweenCharts(Tchart *c1,Tchart *c2)
1003 {
1004  double mc;
1005 
1006  if (c1->w!=c2->w)
1007  Error("Comparing maps defined on diferent manifolds");
1008 
1009  mc=MinCosinusBetweenSubSpaces(c1->m,c1->k,c1->T,c2->T);
1010 
1011  return(mc);
1012 }
1013 
1014 TAtlasBase *GetChartWorld(Tchart *c)
1015 {
1016  return(c->w);
1017 }
1018 
1019 double *GetChartCenter(Tchart *c)
1020 {
1021  return(c->center);
1022 }
1023 
1024 double GetChartRadius(Tchart *c)
1025 {
1026  return(c->r);
1027 }
1028 
1029 double GetChartSamplingRadius(Tchart *c)
1030 {
1031  if (!c->simple)
1032  Error("GetChartSamplingRadius is only defined for simple charts");
1033  return(SPolytopeGetSamplingRadius(c->sp));
1034 }
1035 
1036 double GetChartMaxError(Tchart *c)
1037 {
1038  return(c->error);
1039 }
1040 
1041 double GetChartMaxCurvError(Tchart *c)
1042 {
1043  return(c->eCurv);
1044 }
1045 
1046 unsigned int GetChartAmbientDim(Tchart *c)
1047 {
1048  return(c->m);
1049 }
1050 
1051 unsigned int GetChartManifoldDim(Tchart *c)
1052 {
1053  return(c->k);
1054 }
1055 
1056 double *GetChartTangentSpace(Tchart *c)
1057 {
1058  return(c->T);
1059 }
1060 
1061 boolean *GetChartJacobianBasis(Tchart *c)
1062 {
1063  return(c->BJ);
1064 }
1065 
1066 boolean SingularChart(Tchart *c)
1067 {
1068  return(c->singular);
1069 }
1070 
1071 unsigned int GetChartDegree(Tparameters *pr,double *T,TJacobian *sJ,
1072  boolean *singular,Tchart *c)
1073 {
1074  double *A;
1075  int sg;
1076  unsigned int d;
1077 
1078  d=0; /* default output*/
1079 
1080  if (c->singular)
1081  *singular=TRUE;
1082  else
1083  {
1084  if (c->BJ==NULL)
1085  Error("A non-singular chart without Jacobian basis?");
1086 
1087  *singular=FALSE;
1088 
1089  if ((T==c->T)&&(c->degree!=NO_UINT))
1090  d=c->degree;
1091  else
1092  {
1093  /* Evaluate the Jacobian */
1094  NEW(A,c->m*c->m,double);
1095 
1096  /* We only take the c->n independent rows of the Jacobian */
1097  EvaluateJacobianSubSetInVector(c->center,c->BJ,c->m,c->m,A,sJ);
1098 
1099  /* The lower part of A is T' */
1100  SubMatrixFromTMatrix(c->m,c->k,T,c->n,0,c->m,c->m,A);
1101 
1102  #if (_DEBUG>5)
1103  PrintVector(stdout,NULL,c->m,c->center);
1104  PrintMatrix(stdout,NULL,c->m,c->m,A);
1105  #endif
1106 
1107  sg=MatrixDeterminantSgn(GetParameter(CT_EPSILON,pr),c->m,A);
1108 
1109  if (sg>0) d=1;
1110  else
1111  {
1112  if (sg<0) d=0;
1113  else *singular=TRUE;
1114  }
1115 
1116  free(A);
1117  }
1118  }
1119  return(d);
1120 }
1121 
1122 double Manifold2Chart(double *p,unsigned int *tp,double *t,Tchart *c)
1123 {
1124  if (c->n==0) /* k==m */
1125 
1126  {
1127  if (tp!=NULL)
1128  DifferenceVectorTopology(c->m,tp,p,c->center,t);
1129  else
1130  DifferenceVector(c->m,p,c->center,t);
1131 
1132  return(Norm(c->m,t));
1133  }
1134  else
1135  {
1136  double *p1;
1137 
1138  /* Project a point to the tangent space */
1139  /* t=T'*(p-center)*/
1140  NEW(p1,c->m,double);
1141 
1142  DifferenceVector(c->m,p,c->center,p1);
1143  if (tp!=NULL)
1144  ArrayPi2Pi(c->m,tp,p1);
1145 
1146  TMatrixVectorProduct(c->m,c->k,c->T,p1,t);
1147 
1148  free(p1);
1149 
1150  return(Norm(c->k,t));
1151  }
1152 }
1153 
1154 
1155 
1156 double Chart2Manifold(Tparameters *pr,TJacobian *sJ,
1157  double *t,unsigned int *tp,double *pInit,
1158  double *p,Tchart *c)
1159 {
1160  if (c->n==0) /* k==m */
1161  {
1162  SumVector(c->m,c->center,t,p);
1163  if (tp!=NULL)
1164  ArrayPi2Pi(c->m,tp,p);
1165  return(0.0);
1166  }
1167  else
1168  {
1169  double d;
1170  double epsilon;
1171  unsigned int i,it,maxIterations,nr,nrJ;
1172  boolean converged;
1173 
1174  TLinearSystem ls;
1175  double *aData;
1176  double *bData;
1177  double *xData;
1178  double error;
1179  double *p0;
1180  int err;
1181  double *dif;
1182 
1183  epsilon=GetParameter(CT_EPSILON,pr);
1184  maxIterations=(unsigned int)GetParameter(CT_MAX_NEWTON_ITERATIONS,pr);
1185 
1186  if (maxIterations==0)
1187  Error("MAX_NEWTON_ITERATIONS must be larger than 0 to use Map2Manifold");
1188 
1189  /* If we already identified a basis of the Jacobian (most of the cases), we
1190  can use these rows only. This produces a smaller system without changing
1191  the solution. If actually produces a squared sytem which are solved faster
1192  (non-squared systems are solved via less-squares which is slower). */
1193  if (c->BJ==NULL)
1194  nrJ=c->nrJ;
1195  else
1196  nrJ=c->n; /* squared system */
1197  nr=nrJ+c->k;
1198 
1199  /* Initializes the structure to solve linear systems */
1200  InitLS(nr,c->m,1,&ls);
1201 
1202  /* direct acces to the LS structures */
1203  aData=GetLSMatrixBuffer(&ls);
1204  bData=GetLSRHBuffer(&ls);
1205  xData=GetLSSolutionBuffer(&ls);
1206 
1207  /* Point on the tangent space corresponding to the given paremeters 't' */
1208  NEW(p0,c->m,double);
1209  Local2Global(t,tp,p0,c);
1210 
1211  /* If the user provides an initial estimation for the output use it
1212  (otherwise we will depart from the tangent space estimation. */
1213  if (pInit!=NULL)
1214  {
1215  /* pInit can be pi2pi-ed and this can cause a large error in
1216  the intial estimation when there is not such error.
1217  This is why we have to use DifferenceVectorTopology inside
1218  the loop. */
1219  if (p!=pInit)
1220  memcpy(p,pInit,sizeof(double)*c->m);
1221  }
1222  else
1223  memcpy(p,p0,sizeof(double)*c->m);
1224 
1225  /* Space for the difference between current point (p) and p0
1226  taking into account topology */
1227  NEW(dif,c->m,double);
1228 
1229  /* Iterate from this point to a point on the manifold */
1230  it=0;
1231  converged=FALSE;
1232  err=0;
1233  while((!converged)&&(it<maxIterations))
1234  {
1235  /* Define the residual (right-had side of the system) */
1236  if (c->BJ==NULL)
1237  { CS_WD_EVALUATE_SIMP_EQUATIONS(pr,p,bData,c->w); }
1238  else
1239  { CS_WD_EVALUATE_SUBSET_SIMP_EQUATIONS(pr,c->BJ,p,bData,c->w); }
1240 
1241  /* Complete the the right-hand side of the system of equations */
1242  DifferenceVectorTopology(c->m,tp,p,p0,dif);
1243  TMatrixVectorProduct(c->m,c->k,c->T,dif,&(bData[nrJ]));
1244 
1245  error=Norm(nr,bData);
1246 
1247  if (error<epsilon)
1248  //if ((it>0)&&(error<epsilon))
1249  converged=TRUE;
1250  else
1251  {
1252  /* To avoid overflows we assume that convergence
1253  is impossible with too large errors */
1254  if (error>1e10)
1255  it=maxIterations;
1256  else
1257  {
1258  /* Fill in the part of the 'A' corresponding to the Jacobian */
1259  if (c->BJ==NULL)
1260  EvaluateJacobianInVector(p,nr,c->m,aData,sJ);
1261  else
1262  EvaluateJacobianSubSetInVector(p,c->BJ,nr,c->m,aData,sJ);
1263 
1264  /* Now the part of the 'A' matrix corresponding to the tangent
1265  space (transposed) */
1266  SubMatrixFromTMatrix(c->m,c->k,c->T,nrJ,0,nr,c->m,aData);
1267 
1268  /* Once A and b are defined, solve the system */
1269  err=LSSolve(&ls);
1270 
1271  /* Update the estimate */
1272  if (err)
1273  it=maxIterations;
1274  else
1275  {
1276  for(i=0;i<c->m;i++)
1277  {
1278  p[i]-=xData[i];
1279  if ((tp!=NULL)&&(tp[i]==TOPOLOGY_S))
1280  PI2PI(p[i]);
1281  }
1282  it++;
1283  }
1284  }
1285  }
1286  }
1287 
1288  free(dif);
1289  DeleteLS(&ls);
1290 
1291  if (it<maxIterations)
1292  d=DistanceTopology(c->m,tp,p,p0);
1293  else
1294  d=INF;
1295  free(p0);
1296 
1297  return(d);
1298  }
1299 }
1300 
1301 void Local2Global(double *t,unsigned int *tp,double *p,Tchart *c)
1302 {
1303  if (c->n==0) /* k==m */
1304  {
1305  /*p=center+t*/
1306  SumVector(c->m,c->center,t,p);
1307  }
1308  else
1309  {
1310  /*p=center+T*t*/
1311  MatrixVectorProduct(c->m,c->k,c->T,t,p);
1312  AccumulateVector(c->m,c->center,p);
1313  }
1314  if (tp!=NULL)
1315  ArrayPi2Pi(c->m,tp,p);
1316 }
1317 
1318 double ChartErrorFromParameters(double *t,double *p,unsigned int *tp,
1319  Tchart *c)
1320 {
1321  if (c->n==0)
1322  return(0.0);
1323  else
1324  {
1325  double *p2,e;
1326 
1327  NEW(p2,c->m,double);
1328  Local2Global(t,tp,p2,c);
1329  e=DistanceTopology(c->m,tp,p,p2);
1330  free(p2);
1331 
1332  return(e);
1333  }
1334 }
1335 
1336 double Error2Chart(double *p,unsigned int *tp,Tchart *c)
1337 {
1338  if (c->n==0)
1339  return(0.0);
1340  else
1341  {
1342  double *t,e;
1343 
1344  NEW(t,c->k,double);
1345  Manifold2Chart(p,tp,t,c);
1346  e=ChartErrorFromParameters(t,p,tp,c);
1347  free(t);
1348 
1349  return(e);
1350  }
1351 }
1352 
1353 inline boolean CloseCharts(Tparameters *pr,unsigned int *tp,
1354  Tchart *c1,Tchart *c2)
1355 {
1356  return(IntersectChartsInt(pr,FALSE,tp,NULL,NO_UINT,c1,NO_UINT,c2));
1357 }
1358 
1359 boolean IntersectCharts(Tparameters *pr,unsigned int *tp,Tbox *ambient,
1360  unsigned int id1,Tchart *c1,
1361  unsigned int id2,Tchart *c2)
1362 {
1363  return(IntersectChartsInt(pr,TRUE,tp,ambient,id1,c1,id2,c2));
1364 }
1365 
1366 inline boolean CollisionChart(Tchart *c)
1367 {
1368  return(c->collision);
1369 }
1370 
1371 inline boolean FrontierChart(Tchart *c)
1372 {
1373  return(c->frontier);
1374 }
1375 
1376 inline void ChartIsFrontier(Tchart *c)
1377 {
1378  c->frontier=TRUE;
1379 }
1380 
1381 inline boolean ExpandibleChart(Tchart *c)
1382 {
1383  if (c->simple)
1384  Error("ExpandibleChart is undefined for simple charts");
1385 
1386  return((!c->frontier)&&(ExpandiblePolytope(c->p)));
1387 }
1388 
1389 inline boolean OpenChart(Tchart *c)
1390 {
1391  return((c->simple)||(ExpandiblePolytope(c->p)));
1392 }
1393 
1394 void WrongCorner(unsigned int nv,Tchart *c)
1395 {
1396  if (c->simple)
1397  Error("WrongCorner is undefined for simple charts");
1398 
1399  WrongPolytopeCorner(nv,c->p);
1400 }
1401 
1402 boolean InsideChartPolytope(double *t,Tchart *c)
1403 {
1404  if (c->simple)
1405  return(InsideSPolytope(t,c->sp));
1406  else
1407  return(InsidePolytope(t,c->p));
1408 }
1409 
1410 unsigned int DetermineChartNeighbour(double epsilon,double *t,Tchart *c)
1411 {
1412  if (c->simple)
1413  return(DetermineSPolytopeNeighbour(epsilon,t,c->sp));
1414  else
1415  {
1416  Error("DetermineChartNeighbour is only defined for simple charts");
1417  return(NO_UINT);
1418  }
1419 }
1420 
1421 void EnlargeChart(double *t,Tchart *c)
1422 {
1423  if (c->simple)
1424  EnlargeSPolytope(t,c->sp);
1425  else
1426  Error("EnlargeChart is only defined for simple charts");
1427 }
1428 
1429 boolean BoundaryPointFromExternalCorner(boolean rand,unsigned int *nv,double *t,Tchart *c)
1430 {
1431  boolean ok;
1432 
1433  if (c->simple)
1434  Error("BoundaryPointFromExternalCorner is undefined for simple charts");
1435 
1436  ok=PolytopeBoundaryPointFromExternalCorner(c->r,rand,nv,t,c->p);
1437 
1438  return(ok);
1439 }
1440 
1441 void BoundaryPointsFromExternalCorners(unsigned int *n,unsigned int **nv,double ***t,Tchart *c)
1442 {
1443  if (c->simple)
1444  Error("BoundaryPointsFromExternalCorners is undefined for simple charts");
1445 
1446  PolytopeBoundaryPointsFromExternalCorners(c->r,n,nv,t,c->p);
1447 }
1448 
1449 boolean RandomPointOnBoundary(double *t,Tchart *c)
1450 {
1451  boolean ok;
1452 
1453  if (c->simple)
1454  ok=SPolytopeRandomPointOnBoundary(c->r,t,c->sp);
1455  else
1456  ok=PolytopeRandomPointOnBoundary(c->r,t,c->p);
1457 
1458  return(ok);
1459 }
1460 
1461 boolean RandomPointInChart(Tparameters *pr,double scale,unsigned int *tp,double *t,double *p,Tchart *c)
1462 {
1463  boolean canSample;
1464 
1465  if (c->simple)
1466  canSample=RandomPointInSPolytope(scale,t,c->sp);
1467  else
1468  canSample=RandomPointInPolytope(t,c->p);
1469 
1470  if (canSample)
1471  Local2Global(t,tp,p,c);
1472 
1473  return(canSample);
1474 }
1475 
1476 void IncreaseChartSamplingRadius(Tchart *c)
1477 {
1478  if (c->simple)
1479  SPolytopeIncreaseSamplingRadius(c->sp);
1480 }
1481 
1482 void DecreaseChartSamplingRadius(Tchart *c)
1483 {
1484  if (c->simple)
1485  SPolytopeDecreaseSamplingRadius(c->sp);
1486 }
1487 
1488 double ChartMaxVolume(Tchart *c)
1489 {
1490  double v;
1491 
1492  /* use the cached volume stored in the charts. This
1493  significantly speeds up the query if we ask for the
1494  volume of a chart many times */
1495  if (c->simple)
1496  v=SPolytopeVolume(c->sp);
1497  else
1498  v=PolytopeVolume(c->p);
1499 
1500  return(v);
1501 }
1502 
1503 double ChartVolume(Tparameters *pr,boolean collisionFree,
1504  unsigned int *tp,TJacobian *sJ,Tchart *c)
1505 {
1506  double v;
1507 
1508  if (!collisionFree)
1509  v=ChartMaxVolume(c);
1510  else
1511  {
1512  double *t,*s;
1513  unsigned int i,n,in;
1514  boolean valid,inCollision;
1515 
1516  /* If collisions must be taken into account, we have to replicate
1517  the rejection sampling implemented in Polytope and SPolytope
1518  but including rejection due to obstacles */
1519 
1520  NEW(t,c->k,double);
1521  NEW(s,c->m,double);
1522 
1523  n=(unsigned int)floor(pow(10,c->k));
1524  if (n>10000) n=10000;
1525  in=0;
1526  for(i=0;i<n;i++)
1527  {
1528  if (c->simple)
1529  valid=RandomPointInSPolytope(1.0,t,c->sp);
1530  else
1531  valid=RandomPointInPolytope(t,c->p);
1532  if (valid)
1533  {
1534  if (Chart2Manifold(pr,sJ,t,tp,NULL,s,c)<INF)
1535  {
1536  CS_WD_IN_COLLISION(inCollision,pr,s,NULL,c->w);
1537  if (!inCollision)
1538  in++;
1539  }
1540  }
1541  }
1542  free(t);
1543  free(s);
1544 
1545  if (c->simple)
1546  v=SPolytopeMaxVolume(c->sp);
1547  else
1548  v=PolytopeMaxVolume(c->p);
1549 
1550  v*=((double)in/(double)n);
1551  }
1552  return(v);
1553 }
1554 
1555 boolean FocusedPointOnBoundary(double *p,unsigned int *tp,
1556  double *t,Tchart *c)
1557 {
1558  if (c->simple)
1559  Error("FocusedPointOnBoundary is undefined for simple charts");
1560 
1561  if (ExpandibleChart(c))
1562  {
1563  double nm;
1564 
1565  /* 't' is 'p' in tangent */
1566  nm=Manifold2Chart(p,tp,t,c);
1567 
1568  /* find out the intersection of the ellipsoid and the line passing by the origin and 't' */
1569  /* This is a quadratic equation with
1570  a=t^T *W *t
1571  b=0
1572  c=-r^2
1573  whose solution is l=sqrt(-c)/sqrt(a)
1574  Observe that Manifold2Chart already returns the sqrt(a) and that sqrt(-c)=r.
1575  Thus the solution is trivially c->r/nm
1576  */
1577 
1578  /* and scale the vector with the solution of the quadratic equation */
1579  ScaleVector(c->r/nm,c->k,t);
1580 
1581  return(InsidePolytope(t,c->p));
1582  }
1583  else
1584  return(FALSE);
1585 }
1586 
1587 boolean PathInChart(Tparameters *pr,double *t,unsigned int *tp,TJacobian *sJ,
1588  unsigned int nvs,boolean *systemVars,
1589  unsigned int *ms,
1590  double *pl,unsigned int *ns,double ***path,
1591  Tchart *c)
1592 {
1593  double *nt,*p,*pPrev;
1594  double delta,n;
1595  boolean projectionOk,collision,reached,close;
1596  unsigned int step,nv;
1597  double *o,*oPrev;
1598 
1599  delta=GetParameter(CT_DELTA,pr);
1600 
1601  NEW(p,c->m,double);
1602  NEW(pPrev,c->m,double);
1603  NEW(nt,c->k,double);
1604 
1605  /* We move from the center of the chart toward 't' */
1606  step=0; /* start at 0 to add the center */
1607  projectionOk=TRUE;
1608  collision=FALSE;
1609  n=Norm(c->k,t);
1610  reached=(n<delta);
1611  close=FALSE;
1612 
1613  /* depart from the center */
1614  memcpy(pPrev,c->center,c->m*sizeof(double));
1615  nv=CS_WD_REGENERATE_ORIGINAL_POINT(pr,pPrev,&oPrev,c->w);
1616 
1617  while((projectionOk)&&(!reached))
1618  {
1619  memcpy(nt,t,c->k*sizeof(double));
1620 
1621  /* If in the last step we where close to 't' just use
1622  it as a new set of parameters. Otherwise approach it */
1623  if (close)
1624  reached=TRUE;
1625  else
1626  ScaleVector(((double)step)*delta/n,c->k,nt);
1627 
1628  projectionOk=(Chart2Manifold(pr,sJ,nt,tp,pPrev,p,c)<=c->error);
1629 
1630  if (projectionOk)
1631  {
1632  (*pl)+=DistanceTopology(c->m,tp,pPrev,p);
1633 
1634  nv=CS_WD_REGENERATE_ORIGINAL_POINT(pr,p,&o,c->w);
1635  AddSample2Samples(nv,o,nvs,systemVars,ms,ns,path);
1636 
1637  collision=((collision)||(CS_WD_ORIGINAL_IN_COLLISION(pr,o,oPrev,c->w)));
1638 
1639  memcpy(oPrev,o,nv*sizeof(double));
1640  free(o);
1641 
1642  memcpy(pPrev,p,c->m*sizeof(double));
1643  close=(Distance(c->k,nt,t)<delta);
1644  }
1645 
1646  step++;
1647  }
1648  free(p);
1649  free(pPrev);
1650  free(nt);
1651  free(oPrev);
1652 
1653  return(!collision);
1654 }
1655 
1656 double DistanceOnChart(Tparameters *pr,double *t,unsigned int *tp,TJacobian *sJ,
1657  Tchart *c)
1658 {
1659  double *nt,*p,*pPrev;
1660  double delta,n,d;
1661  boolean projectionOk,inCollision,reached;
1662  unsigned int step;
1663 
1664  delta=GetParameter(CT_DELTA,pr);
1665 
1666  NEW(p,c->m,double);
1667  NEW(pPrev,c->m,double);
1668  NEW(nt,c->k,double);
1669 
1670  /* We move from the center of the chart toward 't' */
1671  step=1;
1672  projectionOk=TRUE;
1673  inCollision=FALSE;
1674  n=Norm(c->k,t);
1675  reached=(n<delta);
1676  d=0.0;
1677  memcpy(pPrev,c->center,c->m*sizeof(double));
1678 
1679  while((projectionOk)&&(!inCollision)&&(!reached)&&(d<INF))
1680  {
1681  memcpy(nt,t,c->k*sizeof(double));
1682  ScaleVector(((double)step)*delta/n,c->k,nt);
1683 
1684  projectionOk=(Chart2Manifold(pr,sJ,nt,tp,pPrev,p,c)<=c->error);
1685 
1686  if (projectionOk)
1687  {
1688  CS_WD_IN_COLLISION(inCollision,pr,p,pPrev,c->w);
1689 
1690  if (!inCollision)
1691  {
1692  d+=DistanceTopology(c->m,tp,pPrev,p);
1693  memcpy(pPrev,p,c->m*sizeof(double));
1694  reached=(Distance(c->k,nt,t)<delta);
1695  }
1696  else
1697  d=INF;
1698  }
1699  else
1700  d=INF;
1701 
1702  step++;
1703  }
1704 
1705  free(p);
1706  free(pPrev);
1707  free(nt);
1708 
1709  return(d);
1710 }
1711 
1712 boolean PointOnChart(Tparameters *pr,TJacobian *sJ,
1713  double *p,unsigned int *tp,
1714  double *t,Tchart *c)
1715 {
1716  boolean inside;
1717 
1718  inside=FALSE;
1719 
1720  if ((Manifold2Chart(p,tp,t,c)<c->r)&&
1721  (InsideChartPolytope(t,c)))
1722  {
1723  double eps;
1724  double *p1;
1725 
1726  eps=GetParameter(CT_EPSILON,pr);
1727 
1728  NEW(p1,c->m,double);
1729 
1730  inside=((Chart2Manifold(pr,sJ,t,tp,NULL,p1,c)<c->error)&&
1731  (DistanceTopology(c->m,tp,p1,p)<eps));
1732 
1733  free(p1);
1734  }
1735 
1736  return(inside);
1737 }
1738 
1739 unsigned int ClassifyPointInChart(Tparameters *pr,boolean error,TJacobian *sJ,
1740  double *p,unsigned int *tp,
1741  double *t,Tchart *c)
1742 {
1743  double eps;
1744  double *p1;
1745  unsigned int cs;
1746 
1747  eps=GetParameter(CT_EPSILON,pr);
1748  NEW(p1,c->m,double);
1749 
1750  if (Manifold2Chart(p,tp,t,c)>c->r)
1751  {
1752  /* out of the ball */
1753  if (InsideChartPolytope(t,c)) /* but still inside the polytope */
1754  cs=1; /* In the chart scope but out of the ball */
1755  else
1756  cs=4; /* Completely out of the scope of the chart */
1757  }
1758  else
1759  {
1760  /* In the ball */
1761  if (InsideChartPolytope(t,c))
1762  {
1763  if ((error)&&((Chart2Manifold(pr,sJ,t,tp,NULL,p1,c)>c->error)||
1764  (DistanceTopology(c->m,tp,p1,p)>eps)))
1765  cs=3; /* Out of the scope due to lack of convergence or large error */
1766  else
1767  cs=0; /* In inside the scope of this chart */
1768  }
1769  else
1770  cs=2; /* Was inside the scope of this chart but is in a neighbouring chart now */
1771  }
1772 
1773  free(p1);
1774 
1775  return(cs);
1776 }
1777 
1778 void LinkCharts(unsigned int id1,Tchart *c1,unsigned int id2,Tchart *c2)
1779 {
1780  LinkChart(id1,c2);
1781  LinkChart(id2,c1);
1782 }
1783 
1784 unsigned int ChartNumNeighbours(Tchart *c)
1785 {
1786  unsigned int n;
1787 
1788  if (c->simple)
1789  n=SPolytopeNumNeighbours(c->sp);
1790  else
1791  n=PolytopeNumNeighbours(c->p);
1792 
1793  return(c->nl+n);
1794 }
1795 
1796 unsigned int ChartNeighbourID(unsigned int n,Tchart *c)
1797 {
1798  unsigned int id;
1799 
1800  if (n<c->nl)
1801  id=c->l[n];
1802  else
1803  {
1804  if (c->simple)
1805  id=SPolytopeNeighbourID(n-c->nl,c->sp);
1806  else
1807  id=PolytopeNeighbourID(n-c->nl,c->p);
1808  }
1809 
1810  return(id);
1811 }
1812 
1813 void GetChartNeighboursFromVertices(Tparameters *pr,
1814  unsigned int *nn,unsigned int **cID1,unsigned int **cID2,Tchart *c)
1815 {
1816  Tcpolytope *p;
1817 
1818  if (c->simple)
1819  {
1820  NEW(p,1,Tcpolytope);
1821  SPolytope2Polytope(pr,c->sp,p);
1822  }
1823  else
1824  p=c->p;
1825 
1826  GetPolytopeNeighboursFromVertices(nn,cID1,cID2,p);
1827 
1828  if (c->simple)
1829  {
1830  DeletePolytope(p);
1831  free(p);
1832  }
1833 }
1834 
1835 void PlotChart(Tparameters *pr,FILE *fcost,TJacobian *sJ,
1836  unsigned int xID,unsigned int yID,unsigned int zID,
1837  Tplot3d *p3d,Tchart *c)
1838 {
1839  if (c->k>3)
1840  Error("PlotChart only works for 2/3d manifolds");
1841 
1842  if ((PLOT_AS_POLYGONS)&&((c->k==2)||(c->k==3)))
1843  {
1844  if (c->k==2)
1845  PlotChartAsPolygon(pr,fcost,sJ,xID,yID,zID,p3d,c);
1846  else
1847  PlotChartAsBox(pr,xID,yID,zID,p3d,c);
1848  }
1849  else
1850  {
1851  unsigned int nv;
1852  double **v;
1853  Tcpolytope *p;
1854 
1855  if (c->simple)
1856  {
1857  NEW(p,1,Tcpolytope);
1858  SPolytope2Polytope(pr,c->sp,p);
1859  }
1860  else
1861  p=c->p;
1862 
1863  GetPolytopeVertices(c->k,&nv,&v,p);
1864 
1865  if (nv>0)
1866  {
1867  unsigned int i,k,n1,n2;
1868  unsigned int ne;
1869  unsigned int *v1;
1870  unsigned int *v2;
1871  double *x,*y,*z;
1872  double *g,*o;
1873  double ox,oy,oz;
1874  double cx,cy,cz;
1875 
1876  /* Collect the edges of the polytope bounding the chart (local coordinates) */
1877  GetPolytopeEdges(&ne,&v1,&v2,p);
1878 
1879  /* Transform the polytope edges from local to gobal */
1880 
1881  NEW(g,c->m,double); /* space for the points in global */
1882 
1883  NEW(x,2*ne,double); /* space for the vertices in global */
1884  NEW(y,2*ne,double);
1885  NEW(z,2*ne,double);
1886 
1887  for(i=0,k=0;i<ne;i++,k+=2)
1888  {
1889  /*Extremes of the edges*/
1890  n1=v1[i];
1891  n2=v2[i];
1892 
1893  if ((!PLOT_ON_MANIFOLD)||
1894  (Chart2Manifold(pr,sJ,v[n1],NULL,NULL,g,c)==INF))
1895  Local2Global(v[n1],NULL,g,c);
1896  CS_WD_REGENERATE_ORIGINAL_POINT(pr,g,&o,c->w);
1897  x[k]=o[xID];
1898  y[k]=o[yID];
1899  z[k]=o[zID];
1900  free(o);
1901 
1902  if ((!PLOT_ON_MANIFOLD)||
1903  (Chart2Manifold(pr,sJ,v[n2],NULL,NULL,g,c)==INF))
1904  Local2Global(v[n2],NULL,g,c);
1905  CS_WD_REGENERATE_ORIGINAL_POINT(pr,g,&o,c->w);
1906  x[k+1]=o[xID];
1907  y[k+1]=o[yID];
1908  z[k+1]=o[zID];
1909  free(o);
1910  }
1911 
1912  /* If any of the vertices is out of the box in global coordiantes
1913  offset the whole polygon */
1914  cx=GetParameter(CT_CUT_X,pr);
1915  cy=GetParameter(CT_CUT_Y,pr);
1916  cz=GetParameter(CT_CUT_Z,pr);
1917 
1918  ox=0;
1919  oy=0;
1920  oz=0;
1921 
1922  for(k=0;k<2*ne;k++)
1923  {
1924  if ((cx<0)&&(x[k]<cx))
1925  ox=+M_2PI;
1926  if ((cx>0)&&(x[k]>cx))
1927  ox=-M_2PI;
1928  if ((cy<0)&&(y[k]<cy))
1929  oy=+M_2PI;
1930  if ((cy>0)&&(y[k]>cy))
1931  oy=-M_2PI;
1932  if ((cz<0)&&(z[k]<cz))
1933  oz=+M_2PI;
1934  if ((cz>0)&&(z[k]>cz))
1935  oz=-M_2PI;
1936  }
1937 
1938  /* Apply the offset if any */
1939  for(k=0;k<2*ne;k++)
1940  {
1941  x[k]+=ox;
1942  y[k]+=oy;
1943  z[k]+=oz;
1944  }
1945 
1946  /* Plot the edges defining the polygon (possibly with offset) */
1947  for(k=0;k<2*ne;k+=2)
1948  PlotVect3d(2,&(x[k]),&(y[k]),&(z[k]),p3d);
1949 
1950  /* Release allocated space */
1951  free(g);
1952 
1953  free(x);
1954  free(y);
1955  free(z);
1956 
1957  for(i=0;i<nv;i++)
1958  free(v[i]);
1959  free(v);
1960  free(v1);
1961  free(v2);
1962  }
1963  if (c->simple)
1964  {
1965  DeletePolytope(p);
1966  free(p);
1967  }
1968  }
1969 }
1970 
1971 unsigned int ChartMemSize(Tchart *c)
1972 {
1973  unsigned int b;
1974 
1975  b=sizeof(double)*c->m; /* center */
1976  b+=sizeof(double)*(c->m*c->k); /* T */
1977 
1978  if (c->simple)
1979  b+=SPolytopeMemSize(c->sp);
1980  else
1981  b+=PolytopeMemSize(c->p);
1982 
1983  return(b);
1984 }
1985 
1986 void SaveChart(FILE *f,Tchart *c)
1987 {
1988  unsigned int i;
1989 
1990  /* Save map */
1991  fprintf(f,"%u\n",c->m);
1992  fprintf(f,"%u\n",c->k);
1993  fprintf(f,"%u\n",c->da);
1994  fprintf(f,"%u\n",c->n);
1995  fprintf(f,"%u\n",c->nrJ);
1996 
1997  fprintf(f,"%f\n",c->error);
1998  fprintf(f,"%f\n",c->eCurv);
1999  fprintf(f,"%f\n0\n",c->r); /* the 0 is for rSample (back-compatibility) */
2000  fprintf(f,"%u\n",c->degree);
2001  fprintf(f,"%u\n",c->frontier);
2002  fprintf(f,"%u\n",c->singular);
2003 
2004  for(i=0;i<c->m;i++)
2005  fprintf(f,"%.16f ",c->center[i]);
2006  fprintf(f,"\n");
2007 
2008  if (c->n>0)
2009  {
2010  for(i=0;i<c->m*c->k;i++)
2011  fprintf(f,"%.16f ",c->T[i]);
2012  fprintf(f,"\n");
2013 
2014  if (c->BJ==NULL)
2015  fprintf(f,"0\n");
2016  else
2017  {
2018  fprintf(f,"1\n");
2019  for(i=0;i<c->nrJ;i++)
2020  fprintf(f,"%u ",c->BJ[i]);
2021  fprintf(f,"\n");
2022  }
2023  }
2024 
2025  fprintf(f,"%u\n",c->ml);
2026  if (c->ml>0)
2027  {
2028  fprintf(f,"%u\n",c->nl);
2029  for(i=0;i<c->nl;i++)
2030  fprintf(f,"%u ",c->l[i]);
2031  fprintf(f,"\n");
2032  }
2033 
2034  /* Save polytope */
2035  fprintf(f,"%u\n",c->simple);
2036  if (c->simple)
2037  SaveSPolytope(f,c->sp);
2038  else
2039  SavePolytope(f,c->p);
2040 
2041  if (c->A==NULL)
2042  fprintf(f,"0\n");
2043  else
2044  {
2045  fprintf(f,"1\n");
2046 
2047  for(i=0;i<c->k*c->k;i++)
2048  fprintf(f,"%.16f ",c->A[i]);
2049  fprintf(f,"\n");
2050 
2051  for(i=0;i<c->k*c->da;i++)
2052  fprintf(f,"%.16f ",c->B[i]);
2053  fprintf(f,"\n");
2054 
2055  for(i=0;i<c->k;i++)
2056  fprintf(f,"%.16f ",c->c[i]);
2057  fprintf(f,"\n");
2058 
2059  for(i=0;i<c->k;i++)
2060  fprintf(f,"%.16f ",c->d[i]);
2061  fprintf(f,"\n");
2062 
2063  for(i=0;i<c->da*c->k;i++)
2064  fprintf(f,"%.16f ",c->iRBt[i]);
2065  fprintf(f,"\n");
2066 
2067  for(i=0;i<c->k*c->k;i++)
2068  fprintf(f,"%.16f ",c->BiRBt[i]);
2069  fprintf(f,"\n");
2070  }
2071 }
2072 
2073 void LoadChart(FILE *f,TAtlasBase *w,Tchart *c)
2074 {
2075  unsigned int i,flag;
2076  double rSample;
2077 
2078  /* Load map */
2079  c->w=w;
2080 
2081  fscanf(f,"%u",&(c->m));
2082  fscanf(f,"%u",&(c->k));
2083  fscanf(f,"%u",&(c->da));
2084  fscanf(f,"%u",&(c->n));
2085  fscanf(f,"%u",&(c->nrJ));
2086 
2087  fscanf(f,"%lf",&(c->error));
2088  fscanf(f,"%lf",&(c->eCurv));
2089  fscanf(f,"%lf",&(c->r));
2090  fscanf(f,"%lf",&rSample); /*for back-compatibility*/
2091  fscanf(f,"%u",&(c->degree));
2092  fscanf(f,"%u",&(c->frontier));
2093  fscanf(f,"%u",&(c->singular));
2094 
2095  NEW(c->center,c->m,double);
2096  for(i=0;i<c->m;i++)
2097  fscanf(f,"%lf",&(c->center[i]));
2098 
2099  if (c->n==0)
2100  {
2101  c->T=NULL;
2102  c->BJ=NULL;
2103  }
2104  else
2105  {
2106  NEW(c->T,c->m*c->k,double);
2107  for(i=0;i<c->m*c->k;i++)
2108  fscanf(f,"%lf",&(c->T[i]));
2109 
2110  /* Do not load the Jacobian Basis. We keep the
2111  read code for back-compatibility. */
2112  fscanf(f,"%u",&flag);
2113  if (flag)
2114  {
2115  NEW(c->BJ,c->nrJ,boolean);
2116  for(i=0;i<c->nrJ;i++)
2117  fscanf(f,"%u",&(c->BJ[i]));
2118  }
2119  else
2120  c->BJ=NULL;
2121  }
2122 
2123  fscanf(f,"%u",&(c->ml));
2124 
2125  if (c->ml>0)
2126  {
2127  NEW(c->l,c->ml,unsigned int);
2128  fscanf(f,"%u",&(c->nl));
2129  for(i=0;i<c->nl;i++)
2130  fscanf(f,"%u",&(c->l[i]));
2131  }
2132  else
2133  {
2134  c->nl=0;
2135  c->l=NULL;
2136  }
2137 
2138  /* Load Polytope */
2139  fscanf(f,"%u\n",&(c->simple));
2140  if (c->simple)
2141  {
2142  NEW(c->sp,1,Tscpolytope);
2143  LoadSPolytope(f,c->sp);
2144  c->p=NULL;
2145  }
2146  else
2147  {
2148  NEW(c->p,1,Tcpolytope);
2149  LoadPolytope(f,c->p);
2150  c->sp=NULL;
2151  }
2152 
2153  fscanf(f,"%u",&(flag));
2154  if (flag==0)
2155  {
2156  c->A=NULL;
2157  c->B=NULL;
2158  c->c=NULL;
2159  c->d=NULL;
2160  c->iRBt=NULL;
2161  c->BiRBt=NULL;
2162  }
2163  else
2164  {
2165  NEW(c->A,c->k*c->k,double);
2166  for(i=0;i<c->k*c->k;i++)
2167  fscanf(f,"%lf",&(c->A[i]));
2168 
2169  NEW(c->B,c->k*c->da,double);
2170  for(i=0;i<c->k*c->da;i++)
2171  fscanf(f,"%lf",&(c->B[i]));
2172 
2173  NEW(c->c,c->k,double);
2174  for(i=0;i<c->k;i++)
2175  fscanf(f,"%lf",&(c->c[i]));
2176 
2177  NEW(c->d,c->k,double);
2178  for(i=0;i<c->k;i++)
2179  fscanf(f,"%lf",&(c->d[i]));
2180 
2181  NEW(c->iRBt,c->da*c->k,double);
2182  for(i=0;i<c->da*c->k;i++)
2183  fscanf(f,"%lf",&(c->iRBt[i]));
2184 
2185  NEW(c->BiRBt,c->k*c->k,double);
2186  for(i=0;i<c->k*c->k;i++)
2187  fscanf(f,"%lf",&(c->BiRBt[i]));
2188  }
2189 }
2190 
2191 void PrintChartDefines(FILE *f)
2192 {
2193  fprintf(f,"\n %% Chart defines\n");
2194  fprintf(f," COMPUTE_LQRPOLICY_IN_T %u\n",COMPUTE_LQRPOLICY_IN_T);
2195  fprintf(f," PLOT_ON_MANIFOLD %u\n",PLOT_ON_MANIFOLD);
2196  fprintf(f," PLOT_AS_POLYGONS %u\n",PLOT_AS_POLYGONS);
2197  fprintf(f," MIN_SAMPLING_RADIUS %.4f\n",MIN_SAMPLING_RADIUS);
2198  fprintf(f," HALF_PLANES %u\n",HALF_PLANES); // This is defined in scpolitope.h
2199  fprintf(f," RANDOM_HALF_PLANES %u\n",RANDOM_HALF_PLANES); // This is defined in scpolitope.h
2200 }
2201 
2202 void DeleteChart(Tchart *c)
2203 {
2204  /* Delete map center */
2205  if (c->center!=NULL)
2206  free(c->center);
2207 
2208  /* Delete tangent space */
2209  if (c->T!=NULL)
2210  free(c->T);
2211 
2212  /* Delete Jacobian basis */
2213  if (c->BJ!=NULL)
2214  free(c->BJ);
2215 
2216  /* Delete the list of linked charts */
2217  if (c->l!=NULL)
2218  free(c->l);
2219 
2220  /* Delete Polytope */
2221  if (c->p!=NULL)
2222  {
2223  DeletePolytope(c->p);
2224  free(c->p);
2225  }
2226  if (c->sp!=NULL)
2227  {
2228  DeleteSPolytope(c->sp);
2229  free(c->sp);
2230  }
2231  if (c->A!=NULL)
2232  {
2233  free(c->A);
2234  free(c->B);
2235  free(c->c);
2236  free(c->d);
2237  free(c->iRBt);
2238  free(c->BiRBt);
2239  }
2240 }