atlas.c
Go to the documentation of this file.
1 #include "atlas.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 #ifdef _OPENMP
12  #include <omp.h>
13 #endif
14 
25 /***************************************************************/
26 
32 typedef struct {
33  unsigned int st;
34  unsigned int nc;
36  unsigned int ns;
37  unsigned int ms;
38  double **path;
39 } TMinTrace;
40 
41 
42 
43 /***************************************************************/
44 
50 typedef struct {
51  double cost;
52  double heuristic;
53  unsigned int status;
54 } TAStarInfo;
55 
56 /***************************************************************/
57 
66 
75 
145 
162 
195 
208 
217 
229 
239 
249 
266 
267 /***********************************************************************/
268 /***********************************************************************/
269 /***********************************************************************/
270 
282 boolean HaveChartAtPoint(double eps,double *p,Tatlas *a);
283 
299 void PostProcessNewCharts(Tparameters *pr,boolean bif,unsigned int parentID,
300  TAtlasStatistics *st,Tatlas *a);
301 
327 void NewChartFromPoint(Tparameters *pr,unsigned int parentID,double *t,
328  Tchart **newChart,Tatlas *a);
329 
351 void ReconstructAtlasPath(Tparameters *pr,unsigned int *pred,
352  unsigned int mID,double *goal,unsigned int nv,
353  boolean *systemVars,
354  double *pl,unsigned int *ns,double ***path,Tatlas *a);
355 
395 boolean DetermineChartNeighbours(Tparameters *pr,boolean bif,
396  unsigned int cmID,unsigned int parentID,Tatlas *a);
397 
425 double GeodesicDistance(Tparameters *pr,unsigned int parentID,unsigned int childID,
426  Tatlas *a);
427 
439 void ComputeAtlasHessian(Tatlas *a);
440 
448 void DeleteAtlasHessian(Tatlas *a);
449 
458 void SetAtlasTopology(Tparameters *pr,Tatlas *a);
459 
460 /***********************************************************************/
461 /***********************************************************************/
462 /***********************************************************************/
463 
481 boolean DetectBifurcation(Tparameters *pr,unsigned int mID1,unsigned int mID2,Tatlas *a);
482 
504 boolean FindSingularPoint(Tparameters *pr,unsigned int bID,Tatlas *a);
505 
537 boolean RefineSingularPoint(Tparameters *pr,unsigned int bID,Tatlas *a);
538 
565 unsigned int FindRightNullVector(Tparameters *pr,unsigned int bID,
566  double **phi,Tatlas *a);
567 
568 
607 boolean FindPointInOtherBranch(Tparameters *pr,unsigned int bID,double *phi,double **p,Tatlas *a);
608 
609 
628 void DefineChartsAtBifurcation(Tparameters *pr,unsigned int bID,double *v,
629  TAtlasStatistics *ast,Tatlas *a);
630 
647 void ProcessBifurcation(Tparameters *pr,unsigned int bID,TAtlasStatistics *ast,Tatlas *a);
648 
657 void LoadBifurcations(FILE *f,Tatlas *a);
658 
667 void SaveBifurcations(FILE *f,Tatlas *a);
668 
685 void PlotBifurcations(Tparameters *pr,Tplot3d *p3d,
686  unsigned int xID,unsigned int yID,unsigned int zID,Tatlas *a);
687 
695 void DeleteBifurcations(Tatlas *a);
696 
697 /***********************************************************************/
698 /***********************************************************************/
699 /***********************************************************************/
700 
702 {
703  if (ast!=NULL)
704  {
705  ast->nExtensions=0;
706  ast->nBoundaryAttempts=0;
707  ast->nNotInBoundary=0;
708  ast->nLargeError=0;
709  ast->nNonRegularPoint=0;
710  ast->nNotInManifold=0;
711  ast->nQRSVDError=0;
712  ast->nFarFromParent=0;
713  ast->nInCollision=0;
714  ast->nRadiousChange=0;
715  ast->nGoodExtension=0;
716  ast->nImpossible=0;
717  ast->nSingImpossible=0;
718  ast->nBifurcations=0;
719  ast->nSmallAngle=0;
720  ast->nSPMissed=0;
721  ast->nNoSingularEnough=0;
722  ast->nNoJumpBranch=0;
723  }
724 }
725 
727 {
728  if (ast!=NULL)
729  ast->nExtensions++;
730 }
731 
733 {
734  if (ast!=NULL)
735  ast->nBoundaryAttempts++;
736 }
737 
739 {
740  if (ast!=NULL)
741  ast->nNotInBoundary++;
742 }
743 
745 {
746  if (ast!=NULL)
747  ast->nLargeError++;
748 }
749 
751 {
752  if (ast!=NULL)
753  ast->nNonRegularPoint++;
754 }
755 
757 {
758  if (ast!=NULL)
759  ast->nNotInManifold++;
760 }
761 
763 {
764  if (ast!=NULL)
765  ast->nQRSVDError++;
766 }
767 
769 {
770  if (ast!=NULL)
771  ast->nFarFromParent++;
772 }
773 
775 {
776  if (ast!=NULL)
777  ast->nInCollision++;
778 }
779 
781 {
782  if (ast!=NULL)
783  ast->nRadiousChange++;
784 }
785 
787 {
788  if (ast!=NULL)
789  ast->nGoodExtension++;
790 }
791 
793 {
794  if (ast!=NULL)
795  ast->nImpossible++;
796 }
797 
799 {
800  if (ast!=NULL)
801  ast->nSingImpossible++;
802 }
803 
805 {
806  if (ast!=NULL)
807  ast->nBifurcations++;
808 }
809 
811 {
812  if (ast!=NULL)
813  ast->nSmallAngle++;
814 }
815 
817 {
818  if (ast!=NULL)
819  ast->nSPMissed++;
820 }
821 
823 {
824  if (ast!=NULL)
825  ast->nNoSingularEnough++;
826 }
827 
829 {
830  if (ast!=NULL)
831  ast->nNoJumpBranch++;
832 }
833 
835 {
836  unsigned int db;
837 
838  if (ast!=NULL)
839  {
840  printf("Boundary attempts: %u\n",ast->nBoundaryAttempts);
841  printf(" Not in Boundary : %u (%3.2f)\n",
842  ast->nNotInBoundary,
843  100*(double)ast->nNotInBoundary/(double)ast->nBoundaryAttempts);
844  printf("Num Extensions : %u\n",ast->nExtensions);
845  printf(" Errors : %u (%3.2f)\n",
846  ast->nImpossible,
847  100*(double)ast->nImpossible/(double)ast->nExtensions);
848  printf(" Errors (Sing) : %u (%3.2f)\n",
849  ast->nSingImpossible,
850  100*(double)ast->nSingImpossible/(double)ast->nExtensions);
851  printf(" Large Error : %u (%3.2f)\n",
852  ast->nLargeError,
853  100*(double)ast->nLargeError/(double)ast->nExtensions);
854  printf(" Non-regular point : %u (%3.2f)\n",
855  ast->nNonRegularPoint,
856  100*(double)ast->nNonRegularPoint/(double)ast->nExtensions);
857  printf(" Not in Manifold : %u (%3.2f)\n",
858  ast->nNotInManifold,
859  100*(double)ast->nNotInManifold/(double)ast->nExtensions);
860  printf(" Decomp. Error : %u (%3.2f)\n",
861  ast->nQRSVDError,
862  100*(double)ast->nQRSVDError/(double)ast->nExtensions);
863  printf(" Far From Parent : %u (%3.2f)\n",
864  ast->nFarFromParent,
865  100*(double)ast->nFarFromParent/(double)ast->nExtensions);
866  printf(" Radius Changes : %u (%3.2f)\n",
867  ast->nRadiousChange,
868  100*(double)ast->nRadiousChange/(double)ast->nExtensions);
869  printf(" Collisions : %u (%3.2f)\n",
870  ast->nInCollision,
871  100*(double)ast->nInCollision/(double)ast->nExtensions);
872  printf(" Good ones : %u (%3.2f)\n",
873  ast->nGoodExtension,
874  100*(double)ast->nGoodExtension/(double)ast->nExtensions);
875 
876  db=(unsigned int)GetParameter(CT_DETECT_BIFURCATIONS,pr);
877  if (db==0)
878  printf("Bifurcation detection disabled\n");
879  else
880  {
881  printf("Detected bifurcations: %u\n",ast->nBifurcations);
882  if (ast->nBifurcations>0)
883  {
884  unsigned int n;
885 
886  n=(ast->nBifurcations-ast->nSPMissed-ast->nNoSingularEnough-ast->nNoJumpBranch);
887  printf(" Correct jumps : %u (%3.2f)\n",
888  n,100*n/(double)ast->nBifurcations);
889  printf(" Num Small angle : %u (%3.2f)\n",ast->nSmallAngle,
890  100*(double)ast->nSmallAngle/(double)n);
891  printf(" Missed singular points: %u (%3.2f)\n",ast->nSPMissed,
892  100*(double)ast->nSPMissed/(double)ast->nBifurcations);
893  printf(" Not enough singular : %u (%3.2f)\n",ast->nNoSingularEnough,
894  100*(double)ast->nNoSingularEnough/(double)ast->nBifurcations);
895  printf(" Error jumping : %u (%3.2f)\n",ast->nNoJumpBranch,
896  100*(double)ast->nNoJumpBranch/(double)ast->nBifurcations);
897  }
898  }
899  }
900 }
901 
902 /***********************************************************************/
903 /***********************************************************************/
904 /***********************************************************************/
905 
906 boolean HaveChartAtPoint(double eps,double *p,Tatlas *a)
907 {
908  boolean found;
909 
910  #if (USE_ATLAS_TREE)
911  found=PointInBTree(eps,p,&(a->tree));
912  #else
913  unsigned int i;
914  double e2;
915 
916  found=FALSE;
917  e2=eps*eps;
918  for(i=0;((!found)&&(i<a->currentChart));i++)
919  {
920  d=SquaredDistanceTopologyMin(e2,a->m,a->tp,
921  p,GetChartCenter(a->charts[i]));
922  found=(d<e2);
923  }
924  #endif
925  return(found);
926 }
927 
928 void PostProcessNewCharts(Tparameters *pr,boolean bif,unsigned int parentID,
929  TAtlasStatistics *st,Tatlas *a)
930 {
931  unsigned int i;
932 
933  for(i=a->npCharts;i<a->currentChart;i++)
934  {
935  if (!DetermineChartNeighbours(pr,bif,i,parentID,a))
936  Warning("Bifurcation undetected in PostProcessNewCharts. Redude epsilon?");
937  }
938  a->npCharts=a->currentChart;
939 
940  /* If parentID is NO_UINT we are defining charts at a bifurcation (we are inside
941  ProcessBifurcation) and, thus, we have to avoid infitinte loops. */
942  if (parentID!=NO_UINT)
943  {
944  /* if bif==FALSE no new bifurcations will be detected and, thus, the code
945  below is not used (nBifurcations==npBifurcations). In some cases, though
946  bifurcations are detected externally (see AddChart2AtlasNS) and, thus
947  they are processed here. */
948  for(i=a->npBifurcations;i<a->nBifurcations;i++)
949  ProcessBifurcation(pr,i,st,a);
951  }
952 }
953 
954 void NewChartFromPoint(Tparameters *pr,unsigned int parentID,double *t,
955  Tchart **newChart,Tatlas *a)
956 {
957  /* If the vertex is already inside the ball, we are refining the approximation of the
958  domain border. Thus, we have to avoid the eventual creation of new charts.*/
959  if (Norm(a->k,t)<0.99*a->r)
960  *newChart=NULL;
961  else
962  {
963  double s;
964  boolean canContinue,chartCreated,smallError,closeEnough,wrong,outOfDomain;
965  double *pt,*ct;
966  unsigned int chartCode;
967  double epsilon;
968 
969  NEW(pt,a->m,double);
970 
971  NEW(*newChart,1,Tchart);
972 
973  /* current set of parameters */
974  NEW(ct,a->k,double);
975 
976  epsilon=GetParameter(CT_EPSILON,pr);
977 
978  s=1.0;
979  canContinue=TRUE;
980  chartCreated=FALSE;
981  closeEnough=FALSE;
982  smallError=FALSE;
983  outOfDomain=FALSE;
984 
985  do {
986  /* update the current set of paremter (ct) using the given parameters (t)
987  and the current scale factor (s) */
988  memcpy(ct,t,a->k*sizeof(double));
989  ScaleVector(s,a->k,ct);
990 
991  smallError=(Chart2Manifold(pr,&(a->J),ct,NULL,NULL,pt,a->charts[parentID])<=a->e);
992 
993  if (smallError)
994  {
995  chartCode=InitChart(pr,a->simpleChart,a->ambient,a->tp,
996  a->m,a->k,pt,a->e,a->ce, a->r,&(a->J),a->w,
997  *newChart);
998  chartCreated=(chartCode==0);
999 
1000  if (chartCreated)
1001  {
1002  if ((CollisionChart(*newChart))||
1003  (!PointInBoxTopology(NULL,TRUE,a->m,pt,epsilon,a->tp,a->ambient))||
1004  (!CS_WD_SIMP_INEQUALITIES_HOLD(pr,pt,a->w)))
1005  outOfDomain=TRUE;
1006  else
1007  closeEnough=(CloseCharts(pr,a->tp,
1008  a->charts[parentID],
1009  *newChart));
1010  }
1011  }
1012 
1013  if (!outOfDomain)
1014  {
1015  wrong=((!smallError)||(!chartCreated)||(!closeEnough));
1016 
1017  if (wrong)
1018  {
1019  canContinue=(s>MIN_SAMPLING_RADIUS);
1020  if (canContinue)
1021  {
1023 
1024  /* We will create a new chart -> delete the just generated one, if any */
1025  if (chartCreated)
1026  {
1027  DeleteChart(*newChart);
1028  chartCreated=FALSE;
1029  }
1030  }
1031  }
1032  }
1033  } while((!outOfDomain)&&(wrong)&&(canContinue));
1034 
1035  /* If we didn't manage to create the new chart, we remove any possibly created chart. */
1036  if ((outOfDomain)||(!canContinue))
1037  {
1038  if (outOfDomain)
1039  ChartIsFrontier(a->charts[parentID]);
1040  if (chartCreated)
1041  DeleteChart(*newChart);
1042  free(*newChart);
1043  *newChart=NULL;
1044  }
1045  free(ct);
1046  free(pt);
1047  }
1048 }
1049 
1050 boolean ExtendAtlasFromPoint(Tparameters *pr,unsigned int parentID,double *t,
1051  TAtlasStatistics *st,Tatlas *a)
1052 {
1053  double s;
1054  boolean canContinue,canInitChart,smallError,closeEnough,wrong;
1055  boolean searchBorder,inside;
1056  double *pt,*ct;
1057  boolean ok;
1058  unsigned int chartCode;
1059  double epsilon;
1060  #if (!SIMPLE_BORDER)
1061  boolean outOfDomain;
1062  double l,u;
1063  #endif
1064 
1065  ok=FALSE; /* No new chart is generated */
1066 
1067  NEW(pt,a->m,double);
1068 
1069  if (a->currentChart==a->maxCharts)
1070  MEM_DUP(a->charts,a->maxCharts,Tchart *);
1071 
1072  NEW(a->charts[a->currentChart],1,Tchart);
1073 
1074  /* current set of parameters */
1075  NEW(ct,a->k,double);
1076 
1077  /* If the vertex is already inside the ball, we are refining the approximation of the
1078  domain border. Thus, we have to avoid the eventual creation of new charts.*/
1079  inside=(Norm(a->k,t)<0.99*a->r);
1080 
1081  epsilon=GetParameter(CT_EPSILON,pr);
1082 
1083  s=1.0;
1084  canContinue=TRUE;
1085  canInitChart=FALSE;
1086  closeEnough=FALSE;
1087  smallError=FALSE;
1088  searchBorder=FALSE;
1089  #if (!SIMPLE_BORDER)
1090  outOfDomain=FALSE;
1091  #endif
1092 
1093  do {
1094  NewAtlasExtension(st);
1095 
1096  canInitChart=FALSE; /* Just to indicate that no chart has been created so far in this iteration */
1097 
1098  /* update the current set of paremter (ct) using the given parameters (t)
1099  and the current scale factor (s) */
1100  memcpy(ct,t,a->k*sizeof(double));
1101  ScaleVector(s,a->k,ct);
1102 
1103  smallError=(Chart2Manifold(pr,&(a->J),ct,NULL,NULL,pt,a->charts[parentID])<=a->e);
1104 
1105  if (smallError)
1106  {
1107  /* We skip the chart construction during the dichotomic search for the border.
1108  A valid chart was already been defined when triggering the search. */
1109  if (searchBorder)
1110  {
1111  #if (!SIMPLE_BORDER)
1112  outOfDomain=((!PointInBoxTopology(NULL,TRUE,a->m,pt,epsilon,a->tp,a->ambient))||
1113  (!CS_WD_SIMP_INEQUALITIES_HOLD(pr,pt,a->w)));
1114  if (!outOfDomain)
1115  CS_WD_IN_COLLISION(outOfDomain,pr,pt,NULL,a->w);
1116  #endif
1117 
1118  chartCode=NO_UINT; /* -> chart actually not created */
1119  }
1120  else
1121  {
1122  chartCode=InitChart(pr,a->simpleChart,a->ambient,a->tp,
1123  a->m,a->k,pt,a->e,a->ce, a->r,&(a->J),a->w,
1124  a->charts[a->currentChart]);
1125  canInitChart=(chartCode==0);
1126 
1127  if (canInitChart)
1128  {
1129  if ((CollisionChart(a->charts[a->currentChart]))||
1130  (!PointInBoxTopology(NULL,TRUE,a->m,pt,epsilon,a->tp,a->ambient))||
1131  (!CS_WD_SIMP_INEQUALITIES_HOLD(pr,pt,a->w)))
1132  {
1133  #if (!SIMPLE_BORDER)
1134  outOfDomain=TRUE;
1135  #endif
1136  searchBorder=TRUE;
1137  #if (!SIMPLE_BORDER)
1138  l=0;
1139  u=s;
1140  #endif
1141  }
1142  else
1143  {
1144  #if (!SIMPLE_BORDER)
1145  outOfDomain=FALSE;
1146  #endif
1147 
1148  closeEnough=(CloseCharts(pr,a->tp,
1149  a->charts[parentID],
1150  a->charts[a->currentChart]));
1151  if (!closeEnough)
1152  {
1153  NewFarFromParent(st);
1154  searchBorder=FALSE; /* if searching for border -> stop the search */
1155  }
1156  else
1157  NewGoodExtension(st);
1158  }
1159  }
1160  else
1161  {
1162  switch (chartCode)
1163  {
1164  case 1:
1165  case 2:
1166  NewNonRegularPoint(st);
1167  break;
1168  case 3:
1170  break;
1171  case 4:
1172  NewNotInManifold(st);
1173  break;
1174  default:
1175  Error("Unknown chart creation error code");
1176  }
1177  }
1178  }
1179  }
1180  else
1181  {
1182  NewLargeError(st);
1183  searchBorder=FALSE; /* if searching for border -> stop the search */
1184  }
1185 
1186  wrong=((searchBorder)||(!smallError)||(!canInitChart)||(!closeEnough));
1187 
1188  if (wrong)
1189  {
1190  /* If something when wrong but we created a chart -> delete it */
1191  if (canInitChart)
1192  {
1194  canInitChart=FALSE; /* this indicates that the chart is already deleted */
1195  }
1196 
1197  if (searchBorder)
1198  {
1199  /* dichotomic search for the border */
1200  #if (SIMPLE_BORDER)
1201  /* Simple boder -> vertices out of the domain are marked as such
1202  and no refinement is triggered. */
1203  canContinue=FALSE;
1204  #else
1205  canContinue=((u-l)>MIN_SAMPLING_RADIUS);
1206  if (canContinue)
1207  {
1208  if (outOfDomain)
1209  u=s;
1210  else
1211  l=s;
1212  s=(u+l)/2.0;
1213  }
1214  #endif
1215  }
1216  else
1217  {
1218  canContinue=(s>MIN_SAMPLING_RADIUS);
1219  if (canContinue)
1220  {
1222  NewRadiousChange(st);
1223  }
1224  }
1225  }
1226  } while((wrong)&&(canContinue));
1227 
1228  if (searchBorder)
1229  {
1230  #if (!SIMPLE_BORDER)
1231  /* Define a linear constraint to the parent chart along the border */
1232  AddBorderConstraint(pr,ct,a->tp,a->ambient,a->charts[parentID]);
1233  #endif
1234 
1235  ChartIsFrontier(a->charts[parentID]);
1236  }
1237  else
1238  {
1239  if (!inside)
1240  {
1241  if (!canContinue)
1242  {
1243  if (SingularChart(a->charts[parentID]))
1244  {
1246  printf(" Could not extend singular chart\n");
1247  }
1248  else
1249  {
1250  //Error("The impossible happened (I)");
1251  NewImpossible(st);
1252  }
1253  }
1254  else
1255  {
1256  #if (ATLAS_VERBOSE)
1257  printf(" New Chart %u\n",a->currentChart);
1258  #endif
1259  a->currentChart++;
1260  PostProcessNewCharts(pr,TRUE,parentID,st,a);
1261  ok=TRUE; /* We actually generated a new chart */
1262  }
1263  }
1264  }
1265 
1266  /* If no chart is actually generated, just release the allocated memory */
1267  if (!ok)
1268  {
1269  /* Just in case we cancelled the chart */
1270  if (canInitChart)
1272  free(a->charts[a->currentChart]);
1273  }
1274 
1275  free(ct);
1276  free(pt);
1277 
1278  return(ok);
1279 }
1280 
1281 boolean ExtendAtlasTowardPoint(Tparameters *pr,unsigned int parentID,double *t,
1282  boolean collisionStops,
1283  TAtlasStatistics *st,Tatlas *a)
1284 {
1285  double delta,s;
1286  double *tp,*pt,*ptGood;
1287  Tchart mTmp;
1288  boolean moved,inCollision,canInitChart,smallError,closeEnough;
1289  double r,n;
1290  unsigned int chartCode;
1291  double currentDelta;
1292  double epsilon;
1293 
1294  NEW(pt,a->m,double);
1295  NEW(tp,a->k,double);
1296  /* ptGood is the last point where we managed to generate a valid chart.
1297  Inintially, the center of the current chart but we try to move
1298  far from here. */
1299  NEW(ptGood,a->m,double);
1300  memcpy(ptGood,GetChartCenter(a->charts[parentID]),a->m*sizeof(double));
1301 
1302  n=Norm(a->k,t);
1303  r=GetChartRadius(a->charts[parentID]);
1304  epsilon=GetParameter(CT_EPSILON,pr);
1305  delta=GetParameter(CT_DELTA,pr);
1306  s=0.0; /* how much we advanced so far */
1307  currentDelta=delta; /* size of current step */
1308 
1309  inCollision=FALSE;
1310  canInitChart=TRUE;
1311  closeEnough=TRUE;
1312  smallError=TRUE;
1313  moved=FALSE;
1314 
1315  /* When extending a chart it is crucial to be able to generate at least
1316  one point in the given direction (except if there is an obstacle
1317  blocking this direction). Once we have at least one point in the
1318  given direction, we can generate a new chart on it that extends
1319  the atlas. Therefore we carefully adjust the advance step
1320  in case we are in a problematic area (singular, large curvature). */
1321  do {
1322  NewAtlasExtension(st);
1323  NewRadiousChange(st);
1324  /* See if vector t scaled by 's' is collision free and has small error */
1325  memcpy(tp,t,a->k*sizeof(double));
1326  ScaleVector((s+currentDelta)/n,a->k,tp);
1327 
1328  smallError=(Chart2Manifold(pr,&(a->J),tp,NULL,ptGood,pt,a->charts[parentID])<=a->e);
1329 
1330  if (smallError)
1331  {
1332  chartCode=InitChart(pr,a->simpleChart,a->ambient,a->tp,
1333  a->m,a->k,pt,a->e,a->ce,a->r,
1334  &(a->J),a->w,&mTmp);
1335  canInitChart=(chartCode==0);
1336 
1337  if (canInitChart)
1338  {
1339  closeEnough=(CloseCharts(pr,a->tp,
1340  a->charts[parentID],&mTmp));
1341  if (closeEnough)
1342  {
1343  if (collisionStops)
1344  inCollision=CollisionChart(&mTmp);
1345 
1346  if (!inCollision)
1347  {
1348  moved=TRUE;
1349  /* consolidate the advance */
1350  s+=currentDelta;
1351 
1352  /* If we moved beyond the chart radious-> stop here */
1353  if (s>=r)
1354  smallError=FALSE;
1355 
1356  /* Store the just generated valid point (this is how
1357  far we managed to get so far). */
1358  memcpy(ptGood,pt,sizeof(double)*a->m);
1359 
1360  NewGoodExtension(st);
1361 
1362  if (currentDelta!=delta)
1363  {
1364  /* We managed to generate a problematic
1365  initial point -> stop here */
1366  closeEnough=FALSE;
1367  }
1368  }
1369  else
1370  NewInCollision(st);
1371  }
1372  else
1373  NewFarFromParent(st);
1374  DeleteChart(&mTmp);
1375  }
1376  else
1377  {
1378  switch (chartCode)
1379  {
1380  case 1:
1381  case 2:
1382  NewNonRegularPoint(st);
1383  break;
1384  case 3:
1386  break;
1387  case 4:
1388  NewNotInManifold(st);
1389  break;
1390  default:
1391  Error("Unknown chart creation error code");
1392  }
1393  }
1394  }
1395  else
1396  NewLargeError(st);
1397 
1398  /* If not moved (i.e., we do not have a good point)
1399  for a reason different from a collision, try to
1400  adjust the avance step */
1401  if ((!moved)&&(!inCollision))
1402  {
1403  /* If we are not trying to jump over a singularity */
1404  if (currentDelta<=delta)
1405  {
1406  if (currentDelta>epsilon)
1407  {
1408  /* We are trying to generate the first point in
1409  a difficult area (singular or with very high
1410  curvature). Try to reduce the advance step with
1411  the purpose of finding a point just outside this
1412  problematic area. Note that the center of the
1413  current chart is outside this area and, thus
1414  we try to approach it.
1415  */
1416  currentDelta*=0.5;
1417  canInitChart=TRUE;
1418  smallError=TRUE;
1419  closeEnough=TRUE;
1420  }
1421  else
1422  {
1423  if (!canInitChart) /* assume charCode==1 */
1424  {
1425  /* We are in a singular area and the current
1426  chart is just at the border of this singular
1427  area (we can not create any other chart closer
1428  to the singular area). In this case we try
1429  to jump over the singular area with a large
1430  jump. Note that this can produce some
1431  collisions that are not detected. This large
1432  jump is only tried once. If it does not work
1433  the extension is declared a failure and a warning
1434  is issued.
1435 
1436  In theory, singularities are zero volum areas but
1437  in practice they have some thickness that is given
1438  by the used numerical accuracy (epsilon)
1439  */
1440  currentDelta=5*delta;
1441  canInitChart=TRUE;
1442  smallError=TRUE;
1443  closeEnough=TRUE;
1444  }
1445  }
1446  }
1447  }
1448 
1449  } while((smallError)&&(canInitChart)&&(closeEnough)&&(!inCollision));
1450 
1451  if (!moved)
1452  {
1453  /* Could not extend at all */
1454  if (!inCollision)
1455  {
1456  if ((currentDelta>delta)||(!canInitChart)||(SingularChart(a->charts[parentID])))
1457  {
1459  printf(" Could not jump over a singularity (decrease epsilon?)");
1460  }
1461  else
1462  {
1463  /* !smallError or !closeEnough */
1464  NewImpossible(st);
1465  Error(" The impossible happened (II)");
1466  }
1467  }
1468  }
1469  else
1470  {
1471  /* Generate the new chart. In the case where extending the atlas in parallel,
1472  this part has to be executed sequentially. */
1473  if (a->currentChart==a->maxCharts)
1474  MEM_DUP(a->charts,a->maxCharts,Tchart *);
1475 
1476  NEW(a->charts[a->currentChart],1,Tchart);
1477 
1478  if (InitChart(pr,a->simpleChart,a->ambient,a->tp,
1479  a->m,a->k,ptGood,a->e,a->ce, a->r,
1480  &(a->J),a->w,a->charts[a->currentChart])!=0)
1481  Error("Can not create a chart already created just above?");
1482 
1483  #if (ATLAS_VERBOSE)
1484  printf(" New Chart %u\n",a->currentChart);
1485  #endif
1486 
1487  a->currentChart++;
1488  PostProcessNewCharts(pr,TRUE,parentID,st,a);
1489  }
1490 
1491  free(pt);
1492  free(ptGood);
1493  free(tp);
1494 
1495  return(moved);
1496 }
1497 
1498 void ReconstructAtlasPath(Tparameters *pr,unsigned int *pred,
1499  unsigned int mID,double *goal,unsigned int nv,
1500  boolean *systemVars,
1501  double *pl,unsigned int *ns,double ***path,Tatlas *a)
1502 {
1503  unsigned int nvs,i,k;
1504  unsigned int ms;
1505  unsigned int *chartID;
1506  signed int s;
1507  double *t,*o;
1508  Tchart *cp,*c;
1509 
1510  *pl=0;
1511 
1512  NEW(t,a->k,double);
1513 
1514  nvs=0;
1515  for(i=0;i<nv;i++)
1516  {
1517  if (systemVars[i])
1518  nvs++;
1519  }
1520 
1521  InitSamples(&ms,ns,path);
1522 
1523  /* Count the number of charts to be visited along the path */
1524  i=mID;
1525  k=0;
1526  while (i!=NO_UINT)
1527  {
1528  k++;
1529  i=pred[i];
1530  }
1531 
1532  /* Store the charts to be visited along the path (in the correct
1533  order) */
1534  NEW(chartID,k,unsigned int);
1535  s=k-1;
1536  i=mID;
1537  while (i!=NO_UINT)
1538  {
1539  chartID[s]=i;
1540  s--;
1541  i=pred[i];
1542  }
1543 
1544  /* Reconstruct the path in each chart (from the center of the
1545  chart to the projection of the center of next chart) */
1546  cp=a->charts[chartID[0]];
1547  for(i=1;i<k;i++)
1548  {
1549  c=a->charts[chartID[i]];
1550  Manifold2Chart(GetChartCenter(c),a->tp,t,cp);
1551  #if (_DEBUG>2)
1552  printf(" Step to %u %f\n",chartID[i],Norm(a->k,t));
1553  #endif
1554  if (!PathInChart(pr,t,a->tp,&(a->J),nvs,systemVars,&ms,pl,ns,path,cp))
1555  {
1556  if (ATLAS_VERBOSE)
1557  Warning("Collision when reconstructing path in atlas");
1558  }
1559  cp=c;
1560  }
1561 
1562  /* And now the path on last chart to the goal */
1563  Manifold2Chart(goal,a->tp,t,cp);
1564  #if (_DEBUG>2)
1565  printf(" Last step %f\n",Norm(a->k,t));
1566  #endif
1567  if (!PathInChart(pr,t,a->tp,&(a->J),nvs,systemVars,&ms,pl,ns,path,cp))
1568  {
1569  if (ATLAS_VERBOSE)
1570  Warning("Collision when reconstructing path in atlas");
1571  }
1572 
1573  /* and add the goal */
1574  CS_WD_REGENERATE_ORIGINAL_POINT(pr,goal,&o,c->w);
1575  AddSample2Samples(nv,o,nvs,systemVars,&ms,ns,path);
1576  free(o);
1577 
1578  free(chartID);
1579  free(t);
1580 }
1581 
1582 boolean DetermineChartNeighbours(Tparameters *pr,boolean bif,
1583  unsigned int cmID,unsigned int parentID,
1584  Tatlas *a)
1585 {
1586  unsigned int i;
1587  unsigned int db;
1588  boolean singular;
1589 
1590  singular=FALSE;
1591 
1592  db=(unsigned int)GetParameter(CT_DETECT_BIFURCATIONS,pr);
1593 
1594  if ((a->n==0)||(db==0))
1595  /* Overide the caller setting if we do not want to detect bifurcations */
1596  bif=FALSE;
1597 
1598  if (ChartNumNeighbours(a->charts[cmID])>0)
1599  Error("DetermineChartNeighbours must be used for charts without any neighbour.");
1600 
1601  #if (USE_ATLAS_TREE)
1602  unsigned int nn=0,id;
1603  unsigned int *n=NULL;
1604  boolean intersectWithParent;
1605 
1606  SearchInBtree(a->charts[cmID],&nn,&n,&(a->tree));
1607  intersectWithParent=FALSE;
1608  for(i=0;i<nn;i++)
1609  {
1610  id=n[i];
1611  if (id==cmID)
1612  Error("The chart was not supposed to be in the tree");
1613 
1614  #if (_DEBUG>1)
1615  if (Distance(a->m,GetChartCenter(a->charts[cmID]),GetChartCenter(a->charts[id]))<1e-4)
1616  Error("Repeated chart center");
1617  #endif
1618 
1619  if (IntersectCharts(pr,a->tp,a->ambient,
1620  id,a->charts[id],cmID,a->charts[cmID]))
1621  {
1622  #if (ATLAS_VERBOSE)
1623  fprintf(stderr," Chart %u intersects with chart %u\n",cmID,id);
1624  #endif
1625  if ((bif)&&((db>1)||(parentID==NO_UINT)||(id==parentID)))
1626  singular|=(!DetectBifurcation(pr,cmID,id,a)); /* id=parent*/
1627 
1628  if (id==parentID)
1629  intersectWithParent=TRUE;
1630  }
1631  }
1632 
1633  if ((parentID!=NO_UINT)&&(!intersectWithParent))
1634  Error("A chart that does not intersect with its parent");
1635  if (nn>0)
1636  free(n);
1637 
1638  /* We add the chart to the tree at the end to avoid self-intersection. */
1639  AddChart2Btree(cmID,a->charts[cmID],&(a->tree));
1640  #else
1641  for(i=0;i<a->currentChart;i++)
1642  {
1643  if (i!=cmID)
1644  {
1645  if (IntersectCharts(pr,a->tp,a->ambient,
1646  i,a->charts[i],cmID,a->charts[cmID]))
1647  {
1648 
1649  #if (ATLAS_VERBOSE)
1650  fprintf(stderr," Chart %u intersects with chart %u\n",cmID,id);
1651  #endif
1652  if ((bif)&&((db>1)||(parentID==NO_UINT)||(i==parentID)))
1653  singular|=(!DetectBifurcation(pr,cmID,i,a));
1654  }
1655  else
1656  {
1657  if (i==parentID)
1658  Error("A chart that does not intersect with its parent");
1659  }
1660  }
1661  }
1662  #endif
1663 
1664  return(!singular);
1665 }
1666 
1667 double GeodesicDistance(Tparameters *pr,unsigned int parentID,unsigned int childID,
1668  Tatlas *a)
1669 {
1670  double d;
1671 
1672  if (a->n==0)
1673  d=DistanceTopology(a->m,a->tp,
1674  GetChartCenter(a->charts[parentID]),
1675  GetChartCenter(a->charts[childID]));
1676  else
1677  {
1678  double *v,*pt,*ptPrev;
1679  Tchart *c;
1680  double *start,*goal;
1681  double dStep;
1682  double delta;
1683  boolean projectionOk,inCollision,reached;
1684  unsigned int step;
1685  #if (PROJECT_WITH_PLANE)
1686  double *ptp;
1687  #endif
1688 
1689  NEW(v,a->m,double);
1690  NEW(pt,a->m,double);
1691  NEW(ptPrev,a->m,double);
1692  #if (PROJECT_WITH_PLANE)
1693  NEW(ptp,a->m,double);
1694  #endif
1695 
1696  c=a->charts[parentID];
1697  start=GetChartCenter(c);
1698  goal=GetChartCenter(a->charts[childID]);
1699 
1700  DifferenceVectorTopology(a->m,a->tp,goal,start,v);
1701 
1702  delta=GetParameter(CT_DELTA,pr);
1703  d=0.0;
1704  step=1;
1705  memcpy(ptPrev,start,a->m*sizeof(double));
1706 
1707  projectionOk=TRUE;
1708  inCollision=FALSE;
1709  reached=FALSE;
1710 
1711  while((projectionOk)&&(!inCollision)&&(!reached)&&(d<INF))
1712  {
1713  SumVectorScale(a->m,start,(double)step*delta,v,pt);
1714 
1715  #if (PROJECT_WITH_PLANE)
1716  memcpy(ptp,pt,a->m*sizeof(double));
1717  projectionOk=Newton2ManifoldPlane(pr,ptp,v,ptPrev,pt,a);
1718  #else
1719  projectionOk=(CS_WD_NEWTON_IN_SIMP(pr,pt,a->w)!=DIVERGED);
1720  #endif
1721 
1722  if (projectionOk)
1723  {
1724  CS_WD_IN_COLLISION(inCollision,pr,pt,ptPrev,a->w);
1725 
1726  if (!inCollision)
1727  {
1728  dStep=DistanceTopology(a->m,a->tp,ptPrev,pt);
1729  if (dStep<(delta/100.0))
1730  d=INF; /* The advance is stalled-> declare impossible path */
1731  else
1732  {
1733  d+=dStep;
1734  memcpy(ptPrev,pt,a->m*sizeof(double));
1735  reached=(DistanceTopology(a->m,a->tp,pt,goal)<delta);
1736  }
1737  }
1738  else
1739  {
1740  d=INF;
1741  //printf("d[%u,%u]=INF due to collision (%u)\n",parentID,childID,step);
1742  }
1743  }
1744  else
1745  {
1746  /* This should never happen. We print a message to warn */
1747  d=INF;
1748  //printf("d[%u,%u]=INF due to not convergence (%u)\n",parentID,childID,step);
1749  }
1750 
1751  step++;
1752  }
1753 
1754  //if (d<INF)
1755  // printf("d[%u,%u]=%g\n",parentID,childID,d);
1756 
1757  free(v);
1758  free(pt);
1759  free(ptPrev);
1760  #if (PROJECT_WITH_PLANE)
1761  free(ptp);
1762  #endif
1763  }
1764  return(d);
1765 }
1766 
1767 /******************************************************************/
1768 /******************************************************************/
1769 /******************************************************************/
1770 
1771 boolean DetectBifurcation(Tparameters *pr,unsigned int mID1,unsigned int mID2,Tatlas *a)
1772 {
1773  double *T;
1774  Tchart *mp1,*mp2;
1775  unsigned int d1,d2;
1776  boolean singular,singular1,singular2;
1777 
1778  singular=FALSE;
1779 
1780  mp1=a->charts[mID1];
1781  mp2=a->charts[mID2];
1782 
1783  if ((!SingularChart(mp1))&&(!SingularChart(mp2)))
1784  {
1785  T=GetChartTangentSpace(mp1);
1786  d1=GetChartDegree(pr,T,&(a->J),&singular1,mp1);
1787  d2=GetChartDegree(pr,T,&(a->J),&singular2,mp2);
1788 
1789  singular=((singular1)||(singular2));
1790 
1791  #if (_DEBUG>2)
1792  printf(" *Degree of chart %u -> %u [%u]\n",mID1,d1,singular1);
1793  printf(" -Degree of chart %u -> %u [%u]\n",mID2,d2,singular2);
1794  #endif
1795 
1796  /* When very close to a singularity, the chart might be declared
1797  non singular in creation but be singular in practice when
1798  computing it degree. */
1799  if ((!singular1)&&(!singular2)&&(d1!=d2))
1800  {
1801  if (a->nBifurcations==a->mBifurcations)
1803 
1805  a->bifurcation[a->nBifurcations]->mID1=mID1;
1806  a->bifurcation[a->nBifurcations]->d1=d1;
1807  a->bifurcation[a->nBifurcations]->mID2=mID2;
1808  a->bifurcation[a->nBifurcations]->d2=d2;
1809  a->bifurcation[a->nBifurcations]->p=NULL;
1810  a->nBifurcations++;
1811  }
1812  }
1813  return(!singular);
1814 }
1815 
1816 boolean FindSingularPoint(Tparameters *pr,unsigned int bID,Tatlas *a)
1817 {
1818  /* Detect the singularity point */
1819  unsigned int degreeLow,degreeUp,degree;
1820  double *d,*tg1,*tg2,*p,*T;
1821  boolean done,error;
1822  Tchart tmpM,*m1,*m2;
1823  double epsilon;
1824  unsigned int nm;
1825  double t,tLow,tUp;
1826  double c1,c2;
1827  boolean singular;
1828  double *p1,*p2,*tg;
1829 
1830  epsilon=GetParameter(CT_EPSILON,pr);
1831 
1832  degreeLow=a->bifurcation[bID]->d1;
1833  degreeUp =a->bifurcation[bID]->d2;
1834 
1835  if (degreeLow==degreeUp)
1836  Error("Degrees must be different at FindSingularPoint");
1837 
1838  m1=a->charts[a->bifurcation[bID]->mID1];
1839  m2=a->charts[a->bifurcation[bID]->mID2];
1840 
1841  NEW(tg1,a->k,double);
1842  NEW(tg2,a->k,double);
1843 
1844  NEW(p1,a->m,double);
1845  memcpy(p1,GetChartCenter(m1),a->m*sizeof(double));
1846  Manifold2Chart(p1,a->tp,tg1,m1); /*tg1 is zero*/
1847 
1848  NEW(p2,a->m,double);
1849  memcpy(p2,GetChartCenter(m2),a->m*sizeof(double));
1850  Manifold2Chart(p2,a->tp,tg2,m1);
1851 
1852  T=GetChartTangentSpace(m1);
1853 
1854  NEW(d,a->k,double);
1855  NEW(tg,a->k,double);
1856  NEW(p,a->m,double);
1857  memcpy(p,p2,a->m*sizeof(double));
1858 
1859  done=FALSE;
1860  error=FALSE;
1861  if (CompareTangentSpaces(m1,m2))
1862  error=FALSE;
1863  else
1864  {
1865  printf(" Incompatible initial charts in FindSingularPoint\n");
1866  error=TRUE;
1867  }
1868 
1869  DifferenceVector(a->k,tg2,tg1,d);
1870  tLow=0.0;
1871  tUp=1.0;
1872 
1873  while((!done)&&(!error))
1874  {
1875  t=(tLow+tUp)/2.0;
1876 
1877  SumVectorScale(a->k,tg1,t,d,tg);
1878 
1879  if (Chart2Manifold(pr,&(a->J),tg,NULL,p,p,m1)>a->e)
1880  error=TRUE;
1881 
1882  if ((!done)&&(!error))
1883  {
1884  /* Some of the chart generated in the search might be almost singular. We
1885  do not trigger an error if they are singular but we do not force the
1886  chart to be singular anyway. Just use the information from the numerical
1887  procedure without pre-defining if the point is regular or not. */
1889  a->m,a->k,p,a->e,a->ce,a->r,&(a->J),a->w,&tmpM);
1890 
1891  if (nm<2) /* 0: no error 1: singular chart */
1892  {
1893  if (SingularChart(&tmpM))
1894  done=TRUE;
1895  else
1896  {
1897  c1=MinCosinusBetweenCharts(m1,&tmpM);
1898  c2=MinCosinusBetweenCharts(&tmpM,m2);
1899 
1900  if ((c1<(1-a->ce))||(c2<(1-a->ce)))
1901  {
1902  /* This is typically the case where, accidentally, we ended up
1903  in the other branch */
1904  printf(" Could not find singular point. Large tangent error %f (%f %f)\n",tUp-tLow,c1,c2);
1905  error=TRUE;
1906  }
1907  else
1908  {
1909  degree=GetChartDegree(pr,T,&(a->J),&singular,&tmpM);
1910 
1911  if (singular)
1912  done=TRUE;
1913  else
1914  {
1915  if (degree==degreeLow)
1916  {
1917  tLow=t;
1918  memcpy(p1,p,a->m*sizeof(double));
1919  }
1920  else
1921  {
1922  if (degree==degreeUp)
1923  {
1924  tUp=t;
1925  memcpy(p2,p,a->m*sizeof(double));
1926  }
1927  else
1928  Error("Incoherent degree change");
1929  }
1930 
1931  done=((tUp-tLow)<100*epsilon);
1932  }
1933  }
1934  }
1935  DeleteChart(&tmpM);
1936  }
1937  else
1938  Error("Error initializing a new chart in FindSingularPoint");
1939  }
1940  }
1941  if (!error)
1942  {
1943  NEW(a->bifurcation[bID]->p,a->m,double);
1944  memcpy(a->bifurcation[bID]->p,p,a->m*sizeof(double));
1945 
1946  /* Try to improve the singular point estimation */
1947  //RefineSingularPoint(pr,bID,a);
1948  }
1949 
1950  free(tg);
1951  free(p1);
1952  free(p2);
1953  free(tg1);
1954  free(tg2);
1955  free(p);
1956  free(d);
1957 
1958  return(!error);
1959 }
1960 
1961 boolean RefineSingularPoint(Tparameters *pr,unsigned int bID,Tatlas *a)
1962 {
1963  double epsilon,nullSingularValue;
1964  unsigned int maxIterations;
1965  unsigned int i,j,k;
1966  double *y,*x,*lambda;
1967  unsigned int s,c,r;
1968  Tchart *c1;
1969  double *p1;
1970  double *phi;
1971  unsigned int it;
1972  boolean converged;
1973  double dif,d1,d2,d3;
1974 
1975  TNewton newton;
1976  double *J;
1977  double *bData;
1978  double *aData;
1979  double *ptr; /* used to access bData */
1980 
1981  int err;
1982 
1983  double ***h;
1984 
1985  /* This routine requires the Hessian, compute it if no
1986  already computed */
1987  if (a->H==NULL)
1988  {
1989  NEW(a->H,1,THessian);
1990  InitHessian(&(a->J),a->H);
1991  }
1992 
1993  epsilon=GetParameter(CT_EPSILON,pr);
1994  maxIterations=(unsigned int)GetParameter(CT_MAX_NEWTON_ITERATIONS,pr);
1995  nullSingularValue=epsilon/100;
1996 
1997  c1=a->charts[a->bifurcation[bID]->mID1];
1998  p1=GetChartCenter(c1);
1999  phi=GetChartTangentSpace(c1); /*This is m x k = ncJ x k*/
2000 
2001  /* As an improvement we could use an interpolation between the
2002  tangent spaces for c1 and c2 */
2003 
2004  /* Number of variables/columns of the system adjusted via Newton*/
2005  c=a->ncJ+a->ncJ; /* The original variables plus lambda */
2006  /* Number of equations/rows of the system adjusted via Newton */
2007  r=(a->nrJ+a->nrJ+a->k+1); /*F + J*lambda + phi*lambda + 1*/
2008 
2009  /* Space for the current estimation */
2010  NEW(y,c,double);
2011  /* Direct pointers to the two components of 'y' */
2012  x=y;
2013  lambda=&(y[a->ncJ]);
2014 
2015  /* Set up the initial value for 'y' */
2016 
2017  /* Initialize x to the current estimation of the singular point */
2018  if (a->bifurcation[bID]->p!=NULL)
2019  {
2020  /* We have an estimation of the singular point and want to
2021  refine it*/
2022  memcpy(x,a->bifurcation[bID]->p,sizeof(double)*a->ncJ);
2023  }
2024  else
2025  {
2026  /* Search the bifurcation from scratch. Depart from the
2027  center of the first chart */
2028  memcpy(x,p1,sizeof(double)*a->ncJ);
2029  }
2030 
2031  /* Space for the Jacobian evaluation */
2032  NEW(J,a->nrJ*a->ncJ,double); /* ncJ==m */
2033 
2034  #if (0)
2035  if (a->bifurcation[bID]->p!=NULL)
2036  {
2037  /* If we have an inintial estimation of the singular point, initialize lambda
2038  with the eigenvector for the smaller non-null eigenvalue.
2039  For this vector J*lambda is close to zero. */
2040 
2041  double *K;
2042 
2043  NEW(K,a->ncJ*(a->k+1),double);
2044 
2045  /* Re-use vector 'J' to store J^t */
2046  EvaluateTransposedJacobianInVector(a->bifurcation[bID]->p,a->ncJ,a->nrJ,J,&(a->J));
2047 
2048  FindKernel(a->nrJ,a->ncJ,J,a->k+1,FALSE,epsilon,&K);
2049 
2050  /* The kernel is returned from largest eigen value to smaller */
2051  GetColumn(0,a->ncJ,a->k+1,K,lambda);
2052 
2053  free(K);
2054  }
2055  else
2056  #else
2057  {
2058  lambda[0]=1;
2059  for(i=1;i<a->ncJ;i++)
2060  lambda[i]=0;
2061  }
2062  #endif
2063 
2064  /* Allocate space for the matrices to be used at each Newton step */
2065  InitNewton(r,c,&newton);
2066  aData=GetNewtonMatrixBuffer(&newton);
2067  /* set all entries to zero (many blocks remain zero all along the process) */
2068  s=r*c;
2069  for(i=0;i<s;i++)
2070  aData[i]=0.0;
2071 
2072  bData=GetNewtonRHBuffer(&newton); /* r entries */
2073 
2075 
2076  it=0;
2077  converged=FALSE;
2078  while((!converged)&&(it<maxIterations))
2079  {
2080  /* Set up the matrix */
2081 
2082  /* Get the Jacobian, we will use it in different steps below */
2083  EvaluateJacobianInVector(x,a->nrJ,a->ncJ,J,&(a->J));
2084 
2085  /* We first set up part of the matrix and then the residual since the
2086  residual evaluation needs the Jacobian, included in the matrix.*/
2087 
2088  /* The first block of the matrix comes from
2089  rows of the Jacobian (and a block of zeros at the end of each row) */
2090  SubMatrixFromMatrix(a->nrJ,a->ncJ,J,0,0,r,c,aData);
2091 
2092  /* Set up part of the residual (the one for 'x'). */
2093  /*F(x)=0*/
2094  CS_WD_EVALUATE_SIMP_EQUATIONS(pr,x,bData,a->w);
2095 
2096  /* Set up the rest of the residual (the one for 'lambda' that uses
2097  J just stored in aData) */
2098  /* J*lambda=0 */
2099  ptr=&(bData[a->nrJ]);
2100  MatrixVectorProduct(a->nrJ,a->ncJ,J,lambda,ptr);
2101 
2102  /* Now force lambda to be different from the vectors in the known
2103  tangent space (approximatted by the tangent space at the initial
2104  point, p1)
2105  phi' * lambda = 0 */
2106  TMatrixVectorProduct(a->m,a->k,phi,lambda,&(bData[a->nrJ+a->nrJ]));
2107 
2108  /* last element in the residual is lambda' lambda -1 =0 */
2109  bData[r-1]=GeneralDotProduct(a->ncJ,lambda,lambda)-1.0;
2110 
2111  d1=Norm(a->nrJ,bData);
2112  d2=Norm(a->nrJ,&(bData[a->nrJ]));
2113  d3=Norm(a->k,&(bData[a->nrJ+a->nrJ]));
2114  //d4=fabs(bData[r-1]);
2115  //if ((d1<epsilon)&&(d2<epsilon)&&(d3<epsilon)&&(d4<epsilon))
2116  if ((d1<10*epsilon)&&(d2<10*epsilon)&&(d3<10*epsilon))
2117  converged=TRUE;
2118  else
2119  {
2120  /* The Hessian part*/
2121  EvaluateHessian(x,h,a->H);
2122 
2123  /* The second block of the matrix comes from the Hessian */
2124  for(i=0;i<a->nrJ;i++)
2125  {
2126  for(j=0;j<a->ncJ;j++)
2127  {
2128  aData[RC2INDEX(a->nrJ+i,j,r,c)]=0.0;
2129  for(k=0;k<a->ncJ;k++)
2130  aData[RC2INDEX(a->nrJ+i,j,r,c)]+=(h[i][k][j]*lambda[k]);
2131  }
2132  }
2133 
2134  /* now the Jacobian part (take the already computed one) */
2135  SubMatrixFromMatrix(a->nrJ,a->ncJ,J,a->nrJ,a->ncJ,r,c,aData);
2136 
2137  /* The known tangent space part (transposed tangent) */
2138  SubMatrixFromTMatrix(a->m,a->k,phi,a->nrJ+a->nrJ,a->ncJ,r,c,aData);
2139 
2140  /* The last row of the matrix only inclues a block of zeros and 2*lambda*/
2141  for(i=0;i<a->ncJ;i++)
2142  aData[RC2INDEX(r-1,a->ncJ+i,r,c)]=(2.0*lambda[i]);
2143 
2144  /* Improve the solution */
2145  err=NewtonStep(nullSingularValue,y,&dif,&newton);
2146 
2147  if (err)
2148  it=maxIterations;
2149  else
2150  it++;
2151  }
2152  }
2153 
2154  if (converged)
2155  {
2156  if (a->tp!=NULL)
2157  ArrayPi2Pi(a->m,a->tp,x);
2158 
2159  /* If we do not have an estimation for the singular point we
2160  initialize it, oherwise we improve the estimation. */
2161  if (a->bifurcation[bID]->p==NULL)
2162  NEW(a->bifurcation[bID]->p,a->m,double);
2163 
2164  /* Singular point found. Update the information about the bifurcation */
2165  memcpy(a->bifurcation[bID]->p,x,a->m*sizeof(double));
2166  }
2167 
2168  /* release used memory */
2169  DeleteNewton(&newton);
2170  FreeHessianEvaluation(h,a->H);
2171  free(J);
2172  free(y);
2173 
2174  return(it<maxIterations);
2175 }
2176 
2177 unsigned int FindRightNullVector(Tparameters *pr,unsigned int bID,
2178  double **phi,Tatlas *a)
2179 {
2180  double *JTData;
2181  double *K=NULL;
2182  double s,sMin,*p;
2183  unsigned int it,itMin,ck;
2184  unsigned int out;
2185  double *T;
2186  double eps;
2187 
2188  //eps=sqrt(GetParameter(CT_EPSILON,pr)); /* typically 1e-3 */
2189  /* We use a large threshold just to avoid errors. This basically gets
2190  the k+1 vectors with smaller eigenvalues, irrespectively of the
2191  value of these eigenvalues. */
2192  eps=0.01;
2193 
2194  /* Number of columns of the matrix K defined below */
2195  ck=a->k+1;
2196 
2197  /*Use the tangent space at mp1 as an approximation of the tangent space at the
2198  bifurcation*/
2199  T=GetChartTangentSpace(a->charts[a->bifurcation[bID]->mID1]);
2200 
2201  if (a->bifurcation[bID]->p==NULL)
2202  Error("FindRightNullVector needs a singular point");
2203 
2204  /* The (almost) singular point */
2205  p=a->bifurcation[bID]->p;
2206 
2207  /* Space for the Jacobian */
2208  NEW(JTData,a->ncJ*a->nrJ,double);
2209  EvaluateTransposedJacobianInVector(p,a->ncJ,a->nrJ,JTData,&(a->J));
2210  #if (_DEBUG>1)
2211  PrintTMatrix(stdout,"J",a->ncJ,a->nrJ,JTData);
2212  #endif
2213 
2214  /* Now we retrive the k+1 vectors defining the kernel at the singularity.
2215  This is the same to that of \ref InitChart where we compute the
2216  kernel at a non-singular point. The difference is that here we
2217  have k+1 vector (instead of k) and that we have to be more generous with the
2218  criteria to consider eigen value as zero (the input point might not
2219  be exactly singular). In the extreme we can set the 'check' parameter
2220  to FALSE and then we just get the k+1 vectors for the smaller eigenvalues,
2221  irrespectively of their actual value. */
2222  out=FindKernel(a->nrJ,a->ncJ,JTData,ck,TRUE,eps,&K);
2223 
2224  #if (_DEBUG>1)
2225  PrintTMatrix(stdout,"T",a->m,a->k,T);
2226  PrintTMatrix(stdout,"K",a->m,ck,K);
2227  #endif
2228 
2229  /* Look for the vector more different from those in T.
2230  Note that we just get the vector that is more different
2231  for those in T, without any minimal dissimilarity.
2232  We could define a threshold and return only a valid
2233  vector it is different 'enough' from those in T */
2234 
2235  /* Space for the output vector (not used if out!=0) */
2236 
2237  if (out==0)
2238  {
2239  double *Proj;
2240 
2241  NEW((*phi),a->m,double);
2242  NEW(Proj,a->k*ck,double);
2243 
2244  TMatrixMatrixProduct(a->m,a->k,T,ck,K,Proj);
2245 
2246  #if (_DEBUG>1)
2247  PrintMatrix(stdout,"Proj",a->k,ck,Proj);
2248  #endif
2249 
2250  #if (_DEBUG>0)
2251  printf(" Proj(T):[ ");
2252  #endif
2253 
2254  /* Compute the norm of the columns of Proj */
2255  sMin=INF;
2256  itMin=0; /* dummy value */
2257  for(it=0;it<=a->k;it++)
2258  {
2259  s=ColumnSquaredNorm(it,a->k,ck,Proj);
2260  #if (_DEBUG>0)
2261  printf("%g ",s);
2262  #endif
2263  if (s<sMin)
2264  {
2265  sMin=s;
2266  itMin=it;
2267  }
2268  }
2269  #if (_DEBUG>0)
2270  printf("]\n");
2271  #endif
2272  free(Proj);
2273 
2274  /* Get the column of K corresponding to the smaller
2275  projection on T */
2276  GetColumn(itMin,a->m,ck,K,(*phi));
2277  }
2278  else
2279  *phi=NULL;
2280 
2281  /* Release allocated space */
2282  if (K!=NULL)
2283  free(K);
2284 
2285  free(JTData);
2286 
2287  return(out);
2288 }
2289 
2290 boolean Newton2ManifoldPlane(Tparameters *pr,double *point,double *vector,
2291  double *pInit,double *p,Tatlas *a)
2292 {
2293  if (a->n==0)
2294  {
2295  memcpy(p,point,sizeof(double)*a->m);
2296  return(TRUE);
2297  }
2298  else
2299  {
2300  double epsilon,nullSingularValue;
2301  unsigned int j,it,maxIterations;
2302  boolean converged;
2303 
2304  TNewton newton;
2305  double *bData;
2306  double *aData;
2307  double *ptr;
2308 
2309  double errorVal;
2310  int err;
2311 
2312  epsilon=GetParameter(CT_EPSILON,pr);
2313  maxIterations=(unsigned int)GetParameter(CT_MAX_NEWTON_ITERATIONS,pr);
2314  nullSingularValue=epsilon/100;
2315 
2316  if (maxIterations==0)
2317  Error("MAX_NEWTON_ITERATIONS must be larger than 0 to use Newton2ManifoldPlane");
2318 
2319  InitNewton(a->nrJ+1,a->ncJ,&newton);
2320  aData=GetNewtonMatrixBuffer(&newton);
2321  bData=GetNewtonRHBuffer(&newton);
2322 
2323  if (pInit==NULL)
2324  memcpy(p,point,sizeof(double)*a->m);
2325  else
2326  memcpy(p,pInit,sizeof(double)*a->m);
2327 
2328  /* Iterate from this point to a point on the manifold */
2329  it=0;
2330  converged=FALSE;
2331  while((!converged)&&(it<maxIterations))
2332  {
2333  /* Define the residual (right-had side of the system) */
2334  CS_WD_EVALUATE_SIMP_EQUATIONS(pr,p,bData,a->w);
2335 
2336  /* Complete the residual with the equation defining the
2337  plane that might intersect with the other branch */
2338  ptr=&(bData[a->nrJ]);
2339  *ptr=0;
2340  for(j=0;j<a->ncJ;j++) /* ncJ=m */
2341  (*ptr)+=(vector[j]*(p[j]-point[j]));
2342 
2343  errorVal=Norm(a->nrJ+1,bData);
2344 
2345  if (errorVal<epsilon)
2346  converged=TRUE;
2347  else
2348  {
2349  /* Fill in the part of the 'A' corresponding to the Jacobian */
2350  EvaluateJacobianInVector(p,a->nrJ+1,a->ncJ,aData,&(a->J));
2351 
2352  /* Now the part of the 'A' matrix corresponding to the plane
2353  that might intersect with the manifold. */
2354  SetRow(vector,a->nrJ,a->nrJ+1,a->ncJ,aData); /*ncJ==m*/
2355 
2356  /* Define the residual (right-had side of the system) */
2357  CS_WD_EVALUATE_SIMP_EQUATIONS(pr,p,bData,a->w);
2358 
2359  /* Complete the residual with the equation defining the
2360  plane that might intersect with the other branch */
2361  ptr=&(bData[a->nrJ]);
2362  *ptr=0;
2363  for(j=0;j<a->ncJ;j++) /* ncJ=m */
2364  (*ptr)+=(vector[j]*(p[j]-point[j]));
2365 
2366  err=NewtonStep(nullSingularValue,p,&errorVal,&newton);
2367 
2368  if (err)
2369  it=maxIterations;
2370  else
2371  {
2372  if(errorVal<epsilon)
2373  converged=TRUE;
2374  it++;
2375  }
2376  }
2377  }
2378 
2379  if ((converged)&&(a->tp!=NULL))
2380  ArrayPi2Pi(a->m,a->tp,p);
2381 
2382  /* release used memory */
2383  DeleteNewton(&newton);
2384 
2385  return(it<maxIterations);
2386  }
2387 }
2388 
2389 boolean FindPointInOtherBranch(Tparameters *pr,unsigned int bID,
2390  double *phi,double **p,Tatlas *a)
2391 {
2392  double d;
2393  double *p0;
2394  boolean converged;
2395 
2396  if (a->bifurcation[bID]->p==NULL)
2397  Error("FindPointInOtherBranch needs a singular point");
2398 
2399  /* Displace a bit along phi to get a point outside the manifold */
2400  NEW(p0,a->m,double);
2401  SumVectorScale(a->m,a->bifurcation[bID]->p,0.01*a->r,phi,p0);
2402 
2403  /* Search for the intersection of a plane defined by
2404  the new vector of the kernel at the bifurcation found using
2405  \ref FindRightNullVector passing by a point just out
2406  of the variety. This point is also determined using
2407  the new vector of the kernel. */
2408 
2409  NEW((*p),a->m,double);
2410  converged=Newton2ManifoldPlane(pr,p0,phi,NULL,*p,a);
2411 
2412  if (converged)
2413  {
2414  d=DistanceTopology(a->m,a->tp,a->bifurcation[bID]->p,*p);
2415  if (d>a->r)
2416  {
2417  converged=FALSE;
2418  printf(" Converged too far from singularity\n");
2419  }
2420  }
2421  else
2422  {
2423  printf(" Could not jump to the other branch\n");
2424  free(*p);
2425  }
2426 
2427  free(p0);
2428 
2429  return(converged);
2430 }
2431 
2432 void DefineChartsAtBifurcation(Tparameters *pr,unsigned int bID,
2433  double *v,TAtlasStatistics *ast,Tatlas *a)
2434 {
2435  double c;
2436 
2437  if (a->bifurcation[bID]->p==NULL)
2438  Error("DefineChartsAtBifurcation needs a singular point");
2439 
2440  /* 3.1.- We define a new chart on the singularity */
2441  if (a->currentChart==a->maxCharts)
2442  MEM_DUP(a->charts,a->maxCharts,Tchart *);
2443 
2444  NEW(a->charts[a->currentChart],1,Tchart);
2445 
2446  if (InitChartWithTangent(pr,FALSE,a->ambient,a->tp,a->m,a->k,
2447  a->bifurcation[bID]->p,
2449  a->e,a->ce, a->r,
2450  &(a->J),a->w,a->charts[a->currentChart])>1)
2451  Warning("Can not create a chart on the singular point");
2452  else
2453  {
2454  a->currentChart++;
2456 
2457  /* 3.2.- Define a new chart in the other branch */
2458  if (a->currentChart==a->maxCharts)
2459  MEM_DUP(a->charts,a->maxCharts,Tchart *);
2460 
2461  NEW(a->charts[a->currentChart],1,Tchart);
2462 
2463  if (InitSingularChart(pr,FALSE,a->ambient,a->tp,a->m,a->k,v,a->e,a->ce,a->r,
2464  &(a->J),a->w,a->charts[a->currentChart])>1)
2465  Warning("Can not create a chart on the other branch");
2466  else
2467  {
2469  if (c>(1-a->ce))
2471 
2472  printf(" New Bifurcation Chart %u (%f)\n",a->currentChart,c);
2473 
2474  a->currentChart++;
2476 
2477  /* 3.3.- Link the two charts together */
2479  a->currentChart-1,a->charts[a->currentChart-1]);
2480  }
2481  }
2482 }
2483 
2484 void ProcessBifurcation(Tparameters *pr,unsigned int bID,TAtlasStatistics *ast,Tatlas *a)
2485 {
2486  if ((bID>=a->npBifurcations)&&(bID<a->nBifurcations))
2487  {
2488  NewBifurcation(ast);
2489  /*
2490  Now we have to detect a point in the "other" branch of the manifold,
2491  start a chart there (IntChart) and search for neighbours
2492  (DetectChartNeighbours).
2493  */
2494  double *phi,*v;
2495 
2496  printf(" Searching for bifurcation\n");
2497 
2498  /* 1.- Determine the bifurcation point. This is an approximation since
2499  all our processes are epsilon accurate. The result is that the
2500  singular point is actually close to singular but not singular */
2501 
2502  if (FindSingularPoint(pr,bID,a))
2503  {
2504  /* 2.- Find the right null vector (the vector for the smallest
2505  eigenvalue (it sould be zero but is not due to numerical
2506  problems) */
2507  if (FindRightNullVector(pr,bID,&phi,a)==0)
2508  {
2509  /* 3.- Find a point in the other branch */
2510  if (FindPointInOtherBranch(pr,bID,phi,&v,a))
2511  {
2512  /* Define on chart at the current branch of the singularity,
2513  another at the new branch and link then.*/
2514  DefineChartsAtBifurcation(pr,bID,v,ast,a);
2515 
2516  /* Release intermediate variables */
2517  free(v);
2518  }
2519  else
2520  NewNoJumpBranch(ast);
2521 
2522  /*Release other intermediate variables*/
2523  free(phi);
2524  }
2525  else
2526  NewNoSingularEnough(ast);
2527  }
2528  else
2530  }
2531 }
2532 
2533 void LoadBifurcations(FILE *f,Tatlas *a)
2534 {
2535  unsigned int i,j,p;
2536 
2537  fscanf(f,"%u",&(a->mBifurcations));
2538  fscanf(f,"%u",&(a->nBifurcations));
2539  fscanf(f,"%u",&(a->npBifurcations));
2540 
2542 
2543 
2544  for(i=0;i<a->nBifurcations;i++)
2545  {
2546  NEW(a->bifurcation[i],1,Tbifurcation);
2547  fscanf(f,"%u",&(a->bifurcation[i]->mID1));
2548  fscanf(f,"%u",&(a->bifurcation[i]->d1));
2549  fscanf(f,"%u",&(a->bifurcation[i]->mID2));
2550  fscanf(f,"%u",&(a->bifurcation[i]->d2));
2551  fscanf(f,"%u",&p);
2552  if (p!=0)
2553  {
2554  NEW(a->bifurcation[i]->p,a->m,double);
2555  for(j=0;j<a->m;j++)
2556  fscanf(f,"%lf",&(a->bifurcation[i]->p[j]));
2557  }
2558  else
2559  a->bifurcation[i]->p=NULL;
2560  }
2561 }
2562 
2563 void SaveBifurcations(FILE *f,Tatlas *a)
2564 {
2565  unsigned int i,j;
2566 
2567  fprintf(f,"%u\n",a->mBifurcations);
2568  fprintf(f,"%u\n",a->nBifurcations);
2569  fprintf(f,"%u\n",a->npBifurcations);
2570 
2571  for(i=0;i<a->nBifurcations;i++)
2572  {
2573  fprintf(f,"%u %u %u %u %u\n",
2574  a->bifurcation[i]->mID1,a->bifurcation[i]->d1,
2575  a->bifurcation[i]->mID2,a->bifurcation[i]->d2,
2576  (a->bifurcation[i]->p!=NULL));
2577  if (a->bifurcation[i]->p!=NULL)
2578  {
2579  for(j=0;j<a->m;j++)
2580  fprintf(f,"%.12f ",a->bifurcation[i]->p[j]);
2581  fprintf(f,"\n");
2582  }
2583  }
2584 }
2585 
2587  unsigned int xID,unsigned int yID,unsigned int zID,Tatlas *a)
2588 {
2589  unsigned int i;
2590  double *p;
2591  double x[2],y[2],z[2];
2592  double *o;
2593  Tchart *m;
2594  Tcolor color;
2595 
2596  NewColor(1,0,0,&color); /* red */
2597  StartNew3dObject(&color,p3d);
2598 
2599  for(i=0;i<a->nBifurcations;i++)
2600  {
2601  m=a->charts[a->bifurcation[i]->mID1];
2602  p=GetChartCenter(m);
2603  CS_WD_REGENERATE_ORIGINAL_POINT(pr,p,&o,a->w);
2604  x[0]=o[xID];
2605  y[0]=o[yID];
2606  z[0]=o[zID];
2607  free(o);
2608 
2609  m=a->charts[a->bifurcation[i]->mID2];
2610  p=GetChartCenter(m);
2611  CS_WD_REGENERATE_ORIGINAL_POINT(pr,p,&o,a->w);
2612  x[1]=o[xID];
2613  y[1]=o[yID];
2614  z[1]=o[zID];
2615  free(o);
2616 
2617  PlotVect3d(2,x,y,z,p3d);
2618  }
2619  Close3dObject(p3d);
2620  DeleteColor(&color);
2621 }
2622 
2624 {
2625  unsigned int i;
2626 
2627  for(i=0;i<a->nBifurcations;i++)
2628  {
2629  if (a->bifurcation[i]->p!=NULL)
2630  free(a->bifurcation[i]->p);
2631  free(a->bifurcation[i]);
2632  }
2633  free(a->bifurcation);
2634 
2635  a->mBifurcations=0;
2636  a->nBifurcations=0;
2637  a->npBifurcations=0;
2638 }
2639 
2640 void InitAtlasHeapElement(unsigned int mID,double c,double beta,TAtlasHeapElement *he)
2641 {
2642  he->chartID=mID;
2643  he->cost=c;
2644  if (beta<1)
2645  Error("The heap element penalty factor must be >1");
2646  he->beta=beta;
2647  he->nPenalized=1;
2648 }
2649 
2650 void CopyAtlasHeapElement(void *he1,void *he2)
2651 {
2652  ((TAtlasHeapElement *)he1)->chartID=((TAtlasHeapElement *)he2)->chartID;
2653  ((TAtlasHeapElement *)he1)->cost=((TAtlasHeapElement *)he2)->cost;
2654  ((TAtlasHeapElement *)he1)->beta=((TAtlasHeapElement *)he2)->beta;
2655  ((TAtlasHeapElement *)he1)->nPenalized=((TAtlasHeapElement *)he2)->nPenalized;
2656 }
2657 
2659 {
2660  return(he->chartID);
2661 }
2662 
2664 {
2665  return(he->cost);
2666 }
2667 
2668 boolean LessThanAtlasHeapElement(void *he1,void *he2,void *userData)
2669 {
2670  double t,b1,b2,c1,c2;
2671 
2672  t=((TAtlasHeapElement *)he1)->nPenalized;
2673  b1=((TAtlasHeapElement *)he1)->beta;
2674  c1=pow(b1,t-1)*((TAtlasHeapElement *)he1)->cost;
2675 
2676  t=((TAtlasHeapElement *)he2)->nPenalized;
2677  b2=((TAtlasHeapElement *)he2)->beta;
2678  c2=pow(b2,t-1)*((TAtlasHeapElement *)he2)->cost;
2679 
2680  return(c1<=c2);
2681 }
2682 
2684 {
2685  he->nPenalized++;
2686 }
2687 
2689 {
2690 }
2691 
2693 {
2694  boolean hasS;
2695  unsigned int i;
2696 
2697  if (CS_WD_GET_SIMP_TOPOLOGY(pr,&(a->tp),a->w)!=a->m)
2698  Error("Missmatch number of variables in SetAtlasTopology");
2699 
2700  /* Search for a non-real variable */
2701  hasS=FALSE;
2702  i=0;
2703  while((i<a->m)&&(!hasS))
2704  {
2705  hasS=(a->tp[i]==TOPOLOGY_S);
2706  i++;
2707  }
2708  if (!hasS)
2709  {
2710  /* if all variables are real no need to consider topology */
2711  free(a->tp);
2712  a->tp=NULL;
2713  }
2714 }
2715 
2716 void InitAtlas(Tparameters *pr,boolean parallel,boolean simpleChart,
2717  unsigned int k,double e,double ce, double r,TAtlasBase *w,
2718  Tatlas *a)
2719 {
2720  a->w=w;
2721 
2723  a->currentChart=0;
2724  a->npCharts=0;
2725 
2726  NEW(a->charts,a->maxCharts,Tchart *);
2727 
2728  NEW(a->ambient,1,Tbox);
2729 
2730  /* Since atlas are defined on the simplified system we get the
2731  corresponding Jacobian. */
2732  CS_WD_GET_SIMP_JACOBIAN(pr,&(a->J),a->w);
2733  GetJacobianSize(&(a->nrJ),&(a->ncJ),&(a->J));
2735 
2736  /* differ the Hessian computation */
2737  a->H=NULL;
2738 
2739  a->k=k;
2740  a->m=a->ncJ;
2741  a->n=a->m-a->k;
2742  a->r=r;
2743  a->e=e;
2744  a->ce=ce;
2745 
2747  a->nBifurcations=0;
2748  a->npBifurcations=0;
2750 
2751  a->simpleChart=simpleChart;
2752 
2753  SetAtlasTopology(pr,a);
2754 
2755  /* The openMP parallel execution is only possible if OpenMP is active and
2756  it is only worth if the problem is large (in number of variables or
2757  dimension of the solution set). Moreover, collision detection
2758  is typically not re-entrant. Do not use parallel execution in
2759  this case (if operating on a world instead than on a cuik file). */
2760 
2761  a->parallel=FALSE; /* The default is not to execute in parallel */
2762 
2763  #ifdef _OPENMP
2764  if (parallel)
2765  {
2766  a->nCores=omp_get_max_threads();
2767  /* even if having many cores and OpenMP we only execute in parallel
2768  large problems. */
2769  a->parallel=((a->nCores>1)&&((a->m>10)||(a->k>2)));
2770  }
2771  #endif
2772  //a->parallel=FALSE;
2773  if (!a->parallel)
2774  a->nCores=1;
2775 
2776  fprintf(stderr,"Number of computing cores (atlas) : %u\n",a->nCores);
2777 
2778  /* When using world, collisions are considered (if defined) */
2779  CS_WD_INIT_CD(pr,a->nCores,a->w);
2780 }
2781 
2782 boolean InitAtlasFromPoint(Tparameters *pr,boolean parallel,boolean simpleChart,double *p,
2783  TAtlasBase *w,Tatlas *a)
2784 {
2785  unsigned int chartCode;
2786  boolean initOK;
2787  double *ps,*pWithDummies;
2788  unsigned int k;
2789  double e,ce,r;
2790 
2791  k=(unsigned int)GetParameter(CT_N_DOF,pr);
2792  r=GetParameter(CT_R,pr);
2793  e=GetParameter(CT_E,pr);
2794  ce=GetParameter(CT_CE,pr);
2795 
2796  InitAtlas(pr,parallel,simpleChart,k,e,ce,r,w,a);
2797 
2798  /* We have to recover the full solution point from the values of the system
2799  variables in sample 'p' and then define the a point in the simplified system
2800  that is where the atlas is defined.
2801  Note that the actual dimension of the ambient space is that of the number
2802  of variables in the simplified system. Moreover, note that both the simplified
2803  and the original system describe the same manifold (k is the same is the to of
2804  them)
2805  */
2806  CS_WD_REGENERATE_SOLUTION_POINT(pr,p,&pWithDummies,a->w);
2807 
2808  if (CS_WD_ORIGINAL_IN_COLLISION(pr,pWithDummies,NULL,a->w))
2809  Error("Starting point for the atlas is in collision");
2810 
2811  a->m=CS_WD_GENERATE_SIMPLIFIED_POINT(pr,pWithDummies,&ps,a->w);
2812  free(pWithDummies);
2813 
2814  if (a->k==0)
2815  {
2816  /* We have to determine the dof assuming that the given point is regular */
2817  Warning("N_DOF parameter set to 0. Assuming that the initial point is regular");
2818 
2819  a->k=CS_WD_MANIFOLD_DIMENSION(pr,p,a->w);
2820 
2821  if (a->k==0)
2822  Error("System with mobility 0");
2823 
2824  fprintf(stderr,"Setting N_DOF=%u\n\n",a->k);
2825 
2826  /* Change the parameter !! */
2827  ChangeParameter(CT_N_DOF,(double)a->k,pr);
2828  }
2829 
2830  NEW(a->charts[0],1,Tchart);
2831  chartCode=InitChart(pr,a->simpleChart,a->ambient,a->tp,a->m,a->k,ps,a->e,a->ce,a->r,
2832  &(a->J),a->w,a->charts[0]);
2833 
2834  switch (chartCode)
2835  {
2836  case 1:
2837  Error("The expected mobility is too low (increase N_DOF)");
2838  break;
2839  case 2:
2840  Error("The expected mobility is too high (decrease N_DOF)");
2841  break;
2842  case 3:
2843  Error("There is a numerical error when computing the tangent space");
2844  break;
2845  case 4:
2846  Error("The initial point is not on the manifold");
2847  break;
2848  }
2849 
2850  initOK=(chartCode==0);
2851 
2852  free(ps);
2853 
2854  if (initOK)
2855  {
2856  if (FrontierChart(a->charts[0]))
2857  Error("The initial chart is out of the ambient space domain");
2858 
2859  a->currentChart=1;
2860  a->npCharts=1; /* No need to post-process the first chart. */
2861  #if (USE_ATLAS_TREE)
2862  InitBTree(0,a->charts[0],a->ambient,a->tp,&(a->tree));
2863  #endif
2864  }
2865  else
2866  free(a->charts[0]);
2867 
2868  return(initOK);
2869 }
2870 
2871 unsigned int AddChart2Atlas(Tparameters *pr,double *ps,unsigned int parentID,boolean *singular,Tatlas *a)
2872 {
2873  boolean initOK;
2874  unsigned int mID;
2875 
2876  *singular=FALSE;
2877 
2878  /* Generate the new chart */
2879  if (a->currentChart==a->maxCharts)
2880  MEM_DUP(a->charts,a->maxCharts,Tchart *);
2881 
2882  NEW(a->charts[a->currentChart],1,Tchart);
2883 
2884  initOK=(InitChart(pr,a->simpleChart,a->ambient,a->tp,a->m,a->k,ps,a->e,a->ce, a->r,
2885  &(a->J),a->w,a->charts[a->currentChart])==0);
2886  if (initOK)
2887  {
2888  #if (ATLAS_VERBOSE)
2889  printf(" New Chart %u\n",a->currentChart);
2890  #endif
2891 
2892  mID=a->currentChart;
2893  a->currentChart++;
2894  PostProcessNewCharts(pr,TRUE,parentID,NULL,a);
2895  }
2896  else
2897  {
2898  mID=NO_UINT;
2899  free(a->charts[a->currentChart]);
2900  }
2901 
2902  return(mID);
2903 }
2904 
2905 unsigned int AddTrustedChart2Atlas(Tparameters *pr,double *ps,unsigned int parentID,boolean *singular,Tatlas *a)
2906 {
2907  boolean initOK;
2908  unsigned int mID;
2909  unsigned int db;
2910 
2911  *singular=FALSE;
2912 
2913  /* Generate the new chart */
2914  if (a->currentChart==a->maxCharts)
2915  MEM_DUP(a->charts,a->maxCharts,Tchart *);
2916 
2917  NEW(a->charts[a->currentChart],1,Tchart);
2918 
2919  initOK=(InitTrustedChart(pr,a->simpleChart,a->ambient,a->tp,a->m,a->k,ps,a->e,a->ce, a->r,
2920  &(a->J),a->w,a->charts[a->currentChart])==0);
2921 
2922  if (initOK)
2923  {
2924  #if (ATLAS_VERBOSE)
2925  printf(" New Chart %u (%p)\n",a->currentChart,a->charts[a->currentChart]);
2926  #endif
2927 
2928  mID=a->currentChart;
2929  a->currentChart++;
2930 
2931  if (a->n>0)
2932  {
2933  /* and now detect (and process) bifurcation from parent to child, even
2934  if they are not neighbours yet. */
2935  db=(unsigned int)GetParameter(CT_DETECT_BIFURCATIONS,pr);
2936 
2937  if ((db>0)&&(parentID!=NO_UINT))
2938  *singular=DetectBifurcation(pr,parentID,mID,a);
2939  }
2940  /* detect neighbours and deal with the bifurcations if any */
2941  /* Intersect with the rest of charts (but without concern about singularities
2942  and without enforcing intersection with parent). */
2943  PostProcessNewCharts(pr,FALSE,NO_UINT,NULL,a);
2944  }
2945  else
2946  {
2947  mID=NO_UINT;
2948  free(a->charts[a->currentChart]);
2949  }
2950 
2951  return(mID);
2952 }
2953 
2954 void BuildAtlasFromPoint(Tparameters *pr,double *p,boolean simpleChart,
2955  TAtlasBase *w,Tatlas *a)
2956 {
2957  Theap h;
2958  TAtlasHeapElement he,he2;
2959  unsigned int mID;
2960  double *t;
2961  TAtlasStatistics *st;
2962  unsigned int cm,cmPrev;
2963  double d;
2964  unsigned int nv,k;
2965  double beta;
2966  unsigned int maxCharts;
2967  /* just in case parallel=TRUE */
2968  unsigned int i,*cornerID,*chartID,maxInProcess,ncInProcess;
2969  double **tv;
2970  Tchart **newCharts;
2971  TAtlasHeapElement *heInProcess;
2972 
2973  /* Init the atlas trying to exploit parallelism, if available and worth */
2974  if (!InitAtlasFromPoint(pr,TRUE,simpleChart,p,w,a))
2975  Error("Can not start atlas from the given point (BuildAtlasFromPoint)");
2976 
2977  k=(unsigned int)GetParameter(CT_N_DOF,pr);
2978 
2979  maxCharts=(unsigned int)GetParameter(CT_MAX_CHARTS,pr);
2980 
2981  InitHeap(sizeof(TAtlasHeapElement),
2986 
2987  beta=GetParameter(CT_ATLASGBF_BETA,pr);
2988  /* one-dim solution sets (k==1) -> process charts in sequence */
2989  InitAtlasHeapElement(0,(k==1?1:0),beta,&he);
2990  AddElement2Heap(NO_UINT,(void *)(&he),&h);
2991 
2992  if (a->parallel)
2993  {
2994  fprintf(stderr,"Executing in %u parallel threads\n",a->nCores);
2995  /* some charts are difficult to process it's better to have more
2996  charts in queue... let the other threads to process the simple
2997  ones */
2998  maxInProcess=(unsigned int)(1.5*a->nCores);
2999  NEW(chartID,maxInProcess,unsigned int);
3000  NEW(cornerID,maxInProcess,unsigned int);
3001  NEW(tv,maxInProcess,double *);
3002  for(i=0;i<maxInProcess;i++)
3003  NEW(tv[i],a->k,double);
3004  NEW(heInProcess,maxInProcess,TAtlasHeapElement);
3005  NEW(newCharts,maxInProcess,Tchart*);
3006  st=NULL;
3007  }
3008  else
3009  {
3010  NEW(t,a->k,double);
3011  NEW(st,1,TAtlasStatistics);
3012  InitAtlasStatistics(st);
3013  }
3014 
3015  while((!HeapEmpty(&h))&&(a->currentChart<maxCharts))
3016  {
3017  cmPrev=a->currentChart;
3018 
3019  if (!a->parallel)
3020  {
3021  ExtractMinElement((void *)(&he),&h);
3022  mID=GetAtlasHeapElementID(&he);
3023 
3024  //if (ExpandibleChart(a->charts[mID]))
3025  if (OpenChart(a->charts[mID]))
3026  {
3027  printf("Extending chart %u\n",mID);
3028 
3029  NewBoundaryAttempt(st);
3030 
3031  /* Treat one external corner at a time */
3033  {
3034  /* This generates a new chart on the direction of 't', if possible. */
3035  ExtendAtlasFromPoint(pr,mID,t,st,a);
3036 
3037  /* Just in case the selected vertex is not actually used, we eliminate it
3038  from the list of expandible vetexes. In general this has no effect since
3039  the extension already eliminated the vertex (and created new vertexes) */
3040  WrongCorner(nv,a->charts[mID]);
3041  }
3042  else
3043  NewNotInBoundary(st);
3044 
3045  /* Just in case the chart still has external corners (still open) */
3046  AddElement2Heap(NO_UINT,(void *)(&he),&h);
3047  }
3048  }
3049  else
3050  {
3051  /* Select several open charts, and a corner of each chart */
3052  ncInProcess=0;
3053  while((!HeapEmpty(&h))&&(ncInProcess<maxInProcess))
3054  {
3055  ExtractMinElement((void *)(&(heInProcess[ncInProcess])),&h);
3056  chartID[ncInProcess]=GetAtlasHeapElementID(&(heInProcess[ncInProcess]));
3057  if (OpenChart(a->charts[chartID[ncInProcess]]))
3058  ncInProcess++;
3059  }
3060 
3061  #pragma omp parallel for private(i) schedule(dynamic)
3062  for(i=0;i<ncInProcess;i++)
3063  {
3064  if (BoundaryPointFromExternalCorner(RANDOMNESS,&(cornerID[i]),
3065  tv[i],a->charts[chartID[i]]))
3066  {
3067  printf("Extending chart %u (%u)\n",chartID[i],omp_get_thread_num());
3068  newCharts[i]=NULL;
3069  NewChartFromPoint(pr,chartID[i],tv[i],&(newCharts[i]),a);
3070  }
3071  /* We selected ncInProcess *different* charts, we can do the
3072  'WrongCorner' in parallel too */
3073  WrongCorner(cornerID[i],a->charts[chartID[i]]);
3074  }
3075 
3076  /* The post process of the generated charts is serialized */
3077  /* Add the charts to the atlas one at a time. */
3078  for(i=0;i<ncInProcess;i++)
3079  {
3080  if (newCharts[i]!=NULL)
3081  {
3082  if (HaveChartAtPoint(1e-6,GetChartCenter(newCharts[i]),a))
3083  {
3084  /* We generated a redundant chart -> remove it
3085  (the nasty side-effect of executing in parallel and
3086  selecting nearby charts for expansion).
3087  1e-6 is hard-coded in the kd-tree */
3088  DeleteChart(newCharts[i]);
3089  newCharts[i]=NULL;
3090  }
3091  else
3092  {
3093  if (a->currentChart==a->maxCharts)
3094  MEM_DUP(a->charts,a->maxCharts,Tchart *);
3095 
3096  /* We directly re-use the charts to safe one copy. */
3097  a->charts[a->currentChart]=newCharts[i];
3098  a->currentChart++;
3099 
3100  /* Now detect neighbours and the possible bifurcations */
3101  PostProcessNewCharts(pr,TRUE,chartID[i],st,a);
3102  }
3103  }
3104  }
3105 
3106  /* Add the charts just processed to the heap, just in case they are still open */
3107  for(i=0;i<ncInProcess;i++)
3108  AddElement2Heap(NO_UINT,(void *)(&(heInProcess[i])),&h);
3109  }
3110 
3111  /* The above can generate several charts (when not using OpenMP or due to bifurcations) */
3112  /* Now we add them to the heap */
3113  for(cm=cmPrev;cm<a->currentChart;cm++)
3114  {
3115  /* Now add the new chart to the heap */
3116  if (k==1) /* one-dim solution sets -> process charts in sequence */
3117  d=1/(double)(cm+1);
3118  else
3119  {
3120  /* when executing in parallel we assign the expansion priority at random
3121  to minimize the prob. of expanding close charts in parallel. This produces
3122  more charts than those really necessary. */
3123  if (a->parallel)
3124  d=randomDouble();
3125  else
3126  d=DistanceTopology(a->m,a->tp,p,GetChartCenter(a->charts[cm]));
3127  }
3128  InitAtlasHeapElement(cm,d,beta,&he2);
3129  AddElement2Heap(NO_UINT,(void *)(&he2),&h);
3130  }
3131  }
3132 
3133  if (a->parallel)
3134  {
3135  free(chartID);
3136  free(cornerID);
3137  for(i=0;i<a->nCores;i++)
3138  free(tv[i]);
3139  free(tv);
3140  free(newCharts);
3141  free(heInProcess);
3142  }
3143  else
3144  {
3145  PrintAtlasStatistics(pr,st);
3146  free(st);
3147  free(t);
3148  }
3149 
3150  DeleteHeap(&h);
3151 }
3152 
3153 boolean MinimizeOnAtlas(Tparameters *pr,char *fname,double *p,TAtlasBase *w,
3154  unsigned int maxSteps,
3155  double (*costF)(Tparameters*,boolean,double*,void*),
3156  void (*costG)(Tparameters*,boolean,double*,double**,void*),
3157  void *costData,
3158  Tatlas *a)
3159 {
3160  unsigned int i,j,k,l,nv,nvs,nt,mt,pnc;
3161  TMinTrace **t;
3162  boolean *systemVars,done;
3163  double *o,*currentPoint;
3164  double cost,cost1;
3165  double *g,*newPoint,*d,ng,*gFull;
3166  double delta,epsilon,e;
3167  Tchart *currentChart;
3168  unsigned int db;
3169  boolean sing,ok;
3170  boolean retraction;
3171  double stepSize;
3172 
3173  db=(unsigned int)GetParameter(CT_DETECT_BIFURCATIONS,pr);
3174 
3175  if (db>1)
3177 
3178  delta=GetParameter(CT_DELTA,pr);
3179  epsilon=GetParameter(CT_EPSILON,pr);
3180  e=GetParameter(CT_E,pr);
3181 
3182  /* Init the atlas */
3183  if (!InitAtlasFromPoint(pr,FALSE,FALSE,p,w,a))
3184  Error("Can not start atlas from the given point (MinimizeOnAtlas)");
3185 
3186  k=(unsigned int)GetParameter(CT_N_DOF,pr);
3187 
3188  /* Get the system varibles */
3189  nv=CS_WD_GET_SYSTEM_VARS(&systemVars,a->w);
3190  nvs=0;
3191  for(i=0;i<nv;i++)
3192  {
3193  if (systemVars[i])
3194  nvs++;
3195  }
3196 
3197  /* Init the set of traces */
3198  mt=5; /* In general only one trace will be followed */
3199  NEW(t,mt,TMinTrace*);
3200  nt=1; /* So far we only have one trace */
3201  /* Init the first trace */
3202  NEW(t[0],1,TMinTrace);
3203  t[0]->st=0;
3204  t[0]->nc=0; /* Start the minimization from the first chart. */
3205  InitSamples(&(t[0]->ms),&(t[0]->ns),&(t[0]->path));
3206 
3207  NEW(g,k,double); /* gradient (in tangent space) */
3208  NEW(d,k,double); /* displacement */
3209  NEW(newPoint,a->m,double); /* new temptative point */
3210 
3211  /* Start by tracing the path from the given point */
3212  l=0;
3213 
3214  /* Add the first point to the path */
3215  currentPoint=GetChartCenter(a->charts[t[l]->nc]);
3216  CS_WD_REGENERATE_ORIGINAL_POINT(pr,currentPoint,&o,a->w);
3217  AddSample2Samples(nv,o,nvs,systemVars,&(t[l]->ms),&(t[l]->ns),&(t[l]->path));
3218  free(o);
3219 
3220  /* Compute the initial cost (debug purposes only) */
3221  cost=costF(pr,TRUE,currentPoint,costData);
3222  fprintf(stderr," Initial cost: %g\n",cost);
3223 
3224 
3225  stepSize=delta;
3226  ok=TRUE; /* if false at the end, indicates that we defined a chart at a singularity */
3227 
3228  while(l<nt)
3229  {
3230  done=FALSE;
3231 
3232  while (!done)
3233  {
3234  /* Point from where to continue the minimization */
3235  currentChart=a->charts[t[l]->nc];
3236  currentPoint=GetChartCenter(currentChart);
3237 
3238  /* Evaluation of -gradient */
3239  if (costG==NULL)
3240  {
3241  /* Numerical */
3242  cost=costF(pr,TRUE,currentPoint,costData);
3243 
3244  for(i=0;i<k;i++)
3245  d[i]=0;
3246 
3247  for(i=0;i<k;i++)
3248  {
3249  d[i]=100*epsilon;
3250  if (Chart2Manifold(pr,&(a->J),d,NULL,currentPoint,newPoint,currentChart)<INF)
3251  {
3252  cost1=costF(pr,TRUE,newPoint,costData);
3253  g[i]=-(cost1-cost)/(100*epsilon);
3254  }
3255  else
3256  Error("Can not converge to the manifold in MinimizeOnAtlas");
3257  d[i]=0.0;
3258  }
3259  }
3260  else
3261  {
3262  /* using a user-provided function */
3263  costG(pr,TRUE,currentPoint,&gFull,costData);
3264 
3265  if (a->k==a->m)
3266  memcpy(g,gFull,a->k*sizeof(double));
3267  else
3268  TMatrixVectorProduct(a->m,a->k,GetChartTangentSpace(currentChart),gFull,g);
3269 
3270  free(gFull);
3271 
3272  ScaleVector(-1,a->k,g);
3273  }
3274  ng=Norm(k,g);
3275  done=(Norm(k,g)<epsilon);
3276 
3277  if (!done)
3278  {
3279  /* start with a temptative step sizer of twice
3280  the size of last step */
3281  stepSize*=2;
3282  if (stepSize>delta)
3283  stepSize=delta;
3284 
3285  if (ng<stepSize)
3286  ScaleVector(stepSize/ng,k,g);
3287 
3288  pnc=a->currentChart; /* charts in the atlas before adding the new one */
3289 
3290  do {
3291  retraction=FALSE;
3292 
3293  if (Chart2Manifold(pr,&(a->J),g,NULL,currentPoint,newPoint,currentChart)>e)
3294  retraction=TRUE;
3295  else
3296  {
3297  cost1=costF(pr,TRUE,newPoint,costData);
3298  if (cost1>cost)
3299  retraction=TRUE;
3300  else
3301  {
3302  //AddChart2Atlas(pr,newPoint,t[l]->nc,&sing,a);
3303  AddChart2Atlas(pr,newPoint,NO_UINT,&sing,a);
3304  if (pnc==a->currentChart)
3305  retraction=TRUE;
3306  else
3307  {
3308  /* at least one chart created -> check it */
3309  done=FrontierChart(a->charts[pnc]);
3310  ok&=(!sing);
3311  }
3312  }
3313  }
3314 
3315  if (retraction)
3316  {
3317  ScaleVector(0.5,k,g);
3318  done=(Norm(k,g)<epsilon);
3319  stepSize*=0.5;
3320  }
3321  } while((retraction)&&(!done));
3322 
3323  if ((maxSteps<150)||(t[l]->st%100==1))
3324  {
3325  fprintf(stderr," Branch %u step %u: point: [%g %g ...]\n",l,t[l]->st,newPoint[0],newPoint[1]);
3326  fprintf(stderr," cost: %g\n",cost1);
3327  fprintf(stderr," norm gradient: %g\n",ng);
3328  fprintf(stderr," norm disp.: %g\n",Norm(k,g));
3329  }
3330 
3331  if (!done)
3332  {
3333  if (a->currentChart>pnc+1)
3334  {
3335  fprintf(stderr," ******* BIFURCATION *******\n");
3336  /* We stepped over a singularity -> bifurcate the path */
3337  /* duplicate the path */
3338  if (nt==mt)
3339  { MEM_DUP(t,mt,TMinTrace*); }
3340 
3341  NEW(t[nt],1,TMinTrace);
3342 
3343  InitSamples(&(t[nt]->ms),&(t[nt]->ns),&(t[nt]->path));
3344  for(j=0;j<t[l]->ns;j++)
3345  AddSample2Samples(nvs,t[l]->path[j],nvs,NULL,
3346  &(t[nt]->ms),&(t[nt]->ns),&(t[nt]->path));
3347  t[nt]->st=t[l]->st;
3348 
3349  /* Now extend the current path */
3350  currentPoint=GetChartCenter(a->charts[pnc+1]); /* point at bifurcation */
3351  CS_WD_REGENERATE_ORIGINAL_POINT(pr,currentPoint,&o,a->w);
3352  AddSample2Samples(nv,o,nvs,systemVars,&(t[l]->ms),&(t[l]->ns),&(t[l]->path));
3353  free(o);
3354  currentPoint=GetChartCenter(a->charts[pnc+0]); /* point at current branch (== newPoint) */
3355  CS_WD_REGENERATE_ORIGINAL_POINT(pr,currentPoint,&o,a->w);
3356  AddSample2Samples(nv,o,nvs,systemVars,&(t[l]->ms),&(t[l]->ns),&(t[l]->path));
3357  free(o);
3358  t[l]->nc=pnc+0;
3359 
3360  /* And extend the new path */
3361  currentPoint=GetChartCenter(a->charts[pnc+1]); /* point at bifurcation */
3362  CS_WD_REGENERATE_ORIGINAL_POINT(pr,currentPoint,&o,a->w);
3363  AddSample2Samples(nv,o,nvs,systemVars,&(t[nt]->ms),&(t[nt]->ns),&(t[nt]->path));
3364  free(o);
3365  currentPoint=GetChartCenter(a->charts[pnc+2]); /* point in new brach */
3366  CS_WD_REGENERATE_ORIGINAL_POINT(pr,currentPoint,&o,a->w);
3367  AddSample2Samples(nv,o,nvs,systemVars,&(t[nt]->ms),&(t[nt]->ns),&(t[nt]->path));
3368  free(o);
3369  t[nt]->nc=pnc+2;
3370 
3371  /* We have a new path already stareted */
3372  nt++;
3373  }
3374  else
3375  {
3376  /* Add the point to the path */
3377  currentPoint=GetChartCenter(a->charts[pnc]); /* == newPoint */
3378  CS_WD_REGENERATE_ORIGINAL_POINT(pr,currentPoint,&o,a->w);
3379  AddSample2Samples(nv,o,nvs,systemVars,&(t[l]->ms),&(t[l]->ns),&(t[l]->path));
3380  free(o);
3381  t[l]->nc=pnc;
3382  }
3383 
3384  t[l]->st++;
3385  done=(t[l]->st>maxSteps);
3386  }
3387  }
3388  }
3389 
3390  /* Continue next pending minimization path */
3391  l++;
3392  }
3393 
3394  free(d);
3395  free(g);
3396  free(newPoint);
3397 
3398  if (!ok)
3399  Warning("A singularity was detected during the minimization but it could not be determined if it was a bifurcation. Try a smaller epsilon or a larger delta?");
3400 
3401  /* Save the stored paths */
3402  if (nt==1)
3403  SaveSamples(fname,FALSE,nvs,t[0]->ns,t[0]->path);
3404  else
3405  {
3406  for(l=0;l<nt;l++)
3407  SaveSamplesN(fname,FALSE,l,nvs,t[l]->ns,t[l]->path);
3408  }
3409 
3410  free(systemVars);
3411 
3412  return(ok);
3413 }
3414 
3416 {
3417  return(a->r);
3418 }
3419 
3421 {
3422  return(a->e);
3423 }
3424 
3426 {
3427  return(a->ce);
3428 }
3429 
3431 {
3432  return(a->w);
3433 }
3434 
3435 unsigned int GetAtlasAmbientDim(Tatlas *a)
3436 {
3437  return(a->m);
3438 }
3439 
3440 unsigned int GetAtlasManifoldDim(Tatlas *a)
3441 {
3442  return(a->k);
3443 }
3444 
3445 boolean AtlasAStar(Tparameters *pr,double *p,double *time,
3446  double *pl,unsigned int *ns,double ***path,Tatlas *a)
3447 {
3448  Theap h;
3449  unsigned int i;
3450  TAtlasHeapElement he,he2,*hePtr;
3451  unsigned int *chartID;
3452  unsigned int mID;
3453  TAStarInfo *info,*infoID,*infoParent;
3454  /* The 'pred' must be stored apart to re-use the path reconstruction function */
3455  unsigned int *pred;
3456  boolean found;
3457  double *t;
3458  double epsilon;
3459  unsigned int nn,id;
3460  double c,dst;
3461  double *pID,*pmID;
3462  double *ps,*pWithDummies;
3463  double er;
3464  unsigned int nv;
3465  boolean *systemVars;
3466  unsigned int cm,cmPrev,mmPrev;
3467  TAtlasStatistics *st;
3468  unsigned int nc;
3469  unsigned int it;
3470  Tstatistics stime;
3471  double beta;
3472  boolean collision=FALSE;
3473  double maxTime;
3474  unsigned int maxCharts;
3475  unsigned int nce;
3476  unsigned int *vs; /* set of vertex indices (only used in parallel execution). */
3477  double **ts; /* set of vertex coordinates (only used in parallel execution). */
3478  Tchart **newCharts; /* set of generated charts (only used in parallel execution). */
3479 
3480  if (a->currentChart!=1)
3481  Error("AtlasAStar assumes an initial atlas with only one local chart");
3482 
3483  epsilon=GetParameter(CT_EPSILON,pr);
3484  beta=GetParameter(CT_ATLASGBF_BETA,pr);
3485  maxTime=GetParameter(CT_MAX_PLANNING_TIME,pr);
3486  maxCharts=(unsigned int)GetParameter(CT_MAX_CHARTS,pr);
3487 
3488  nv=CS_WD_GET_SYSTEM_VARS(&systemVars,a->w);
3489  CS_WD_REGENERATE_SOLUTION_POINT(pr,p,&pWithDummies,a->w);
3490  if (CS_WD_GENERATE_SIMPLIFIED_POINT(pr,pWithDummies,&ps,a->w)!=a->m)
3491  Error("Dimension missmatch in AtlasAStar");
3492  free(pWithDummies);
3493  if (a->n==0)
3494  er=0.0;
3495  else
3496  er=CS_WD_ERROR_IN_SIMP_EQUATIONS(pr,ps,a->w);
3497  if (er>epsilon)
3498  Error("Target point for the AtlasAStar is not on the manifold");
3499 
3500  collision=CS_WD_ORIGINAL_IN_COLLISION(pr,pWithDummies,NULL,a->w);
3501  if (collision)
3502  Warning("Target point for the AtlasAStar is in collision");
3503 
3504  InitHeap(sizeof(TAtlasHeapElement),
3509 
3510  NEW(t,a->k,double);
3511 
3512  NEW(info,a->maxCharts,TAStarInfo);
3513  NEW(pred,a->maxCharts,unsigned int);
3514 
3515  info[0].cost=0;
3516  info[0].heuristic=DistanceTopology(a->m,a->tp,ps,GetChartCenter(a->charts[0]));
3517  info[0].status=1; /*open*/
3518  pred[0]=NO_UINT;
3519 
3520  if (a->parallel)
3521  {
3522  st=NULL;
3523  NEW(chartID,a->nCores,unsigned int);
3524  NEW(newCharts,a->nCores,Tchart *);
3525  NEW(vs,a->nCores,unsigned int);
3526  NEW(ts,a->nCores,double*);
3527  for(i=0;i<a->nCores;i++)
3528  NEW(ts[i],a->k,double);
3529  }
3530  else
3531  {
3532  NEW(st,1,TAtlasStatistics);
3533  InitAtlasStatistics(st);
3534  }
3535 
3536  InitAtlasHeapElement(0,info[0].cost+info[0].heuristic,beta,&he);
3537  AddElement2Heap(NO_UINT,(void *)(&he),&h);
3538 
3539  found=FALSE;
3540 
3541  it=0;
3542 
3543  InitStatistics(a->nCores,0,&stime);
3544  *time=0.0;
3545 
3546  while ((!found)&&(!HeapEmpty(&h))&&(*time<maxTime)&&(a->currentChart<maxCharts))
3547  {
3548  if ((ATLAS_VERBOSE)||((it%1000)==0))
3549  {
3550  printf("Iteration %u (c:%u t:%g)\n",it,a->currentChart,*time);
3551  fflush(stdout);
3552  }
3553 
3554  ExtractMinElement((void *)(&he),&h);
3555  mID=GetAtlasHeapElementID(&he);
3556  infoParent=&(info[mID]);
3557 
3558  /* If the current chart already includes the goal we are done */
3559  if ((PointOnChart(pr,&(a->J),ps,a->tp,t,a->charts[mID]))&&
3560  (DistanceOnChart(pr,t,a->tp,&(a->J),a->charts[mID])<INF))
3561  found=TRUE;
3562  else
3563  {
3564  /* already closed element? The heap can hold alternative paths to
3565  the same node that have been improved over time. Instead of
3566  removing them from the heap as better paths are found we leave
3567  them as just ignore them when extracted.
3568  Moreover, we do not expand nodes whose cost is INF
3569  */
3570  if ((infoParent->status!=2)&&(infoParent->cost<INF))
3571  {
3572  cmPrev=a->currentChart;
3573  mmPrev=a->maxCharts;
3574 
3575  if (a->parallel)
3576  {
3577  while(ExpandibleChart(a->charts[mID]))
3578  {
3579  /* We have to genere new neighbors for chart 'mID'.
3580  Select up to 'nCores' other expandible charts and generate neighbours
3581  in parallel with the aim of saving time in future interations. */
3582  chartID[0]=mID;
3583  nce=1;
3584  i=0;
3585  while((i<HeapSize(&h))&&(nce<a->nCores))
3586  {
3587  hePtr=(TAtlasHeapElement*)GetHeapElement(i,&h); /* does not remove from heap */
3588 
3589  chartID[nce]=GetAtlasHeapElementID(hePtr);
3590  if (ExpandibleChart(a->charts[chartID[nce]]))
3591  nce++;
3592  i++;
3593  }
3594  #pragma omp parallel for private(i) schedule(static)
3595  for(i=0;i<nce;i++)
3596  {
3597  newCharts[i]=NULL;
3598  if (BoundaryPointFromExternalCorner(FALSE,&(vs[i]),ts[i],a->charts[chartID[i]]))
3599  {
3600  NewChartFromPoint(pr,chartID[i],ts[i],&(newCharts[i]),a);
3601 
3602  /* ensure that the selected corner is maked as "already used" */
3603  WrongCorner(vs[i],a->charts[chartID[i]]);
3604  }
3605  }
3606 
3607  /* Add the new charts to the atlas */
3608  for(i=0;i<nce;i++)
3609  {
3610  if (newCharts[i]!=NULL)
3611  {
3612  if (a->currentChart==a->maxCharts)
3613  MEM_DUP(a->charts,a->maxCharts,Tchart *);
3614 
3615  /* We directly re-use the charts to safe one copy. */
3616  a->charts[a->currentChart]=newCharts[i];
3617  a->currentChart++;
3618 
3619  /* Now detect neighbours and the possible bifurcations */
3620  PostProcessNewCharts(pr,TRUE,chartID[i],st,a);
3621  }
3622  }
3623  }
3624  }
3625  else
3626  {
3627  /* Generating all neighbours for this chart: one for each external vertex */
3628  while(ExpandibleChart(a->charts[mID]))
3629  {
3630  NewBoundaryAttempt(st);
3631  if (!BoundaryPointFromExternalCorner(FALSE,&nc,t,a->charts[mID]))
3632  {
3633  NewNotInBoundary(st);
3634  Error("Can not generate neighbouring charts");
3635  }
3636  else
3637  {
3638  ExtendAtlasFromPoint(pr,mID,t,st,a);
3639 
3640  /* ensure that the selected corner is maked as "already used" */
3641  WrongCorner(nc,a->charts[mID]);
3642  }
3643  } /* end of generate all neighbours */
3644  }
3645 
3646  /* set the A*-related information for the new charts */
3647  if (mmPrev<a->maxCharts)
3648  {
3649  MEM_EXPAND(info,a->maxCharts,TAStarInfo);
3650  MEM_EXPAND(pred,a->maxCharts,unsigned int);
3651  }
3652 
3653  for(cm=cmPrev;cm<a->currentChart;cm++)
3654  {
3655  /*Mark the new charts as un-visited*/
3656  info[cm].status=0;/* not open nor closed*/
3657  info[cm].heuristic=0;
3658  info[cm].cost=0;
3659  pred[cm]=NO_UINT;
3660  }
3661 
3662  /* Now that we have all the neighbours explicited proceed with
3663  the A* */
3664  pmID=GetChartCenter(a->charts[mID]);
3665 
3666  nn=ChartNumNeighbours(a->charts[mID]);
3667  for(i=0;i<nn;i++)
3668  {
3669  id=ChartNeighbourID(i,a->charts[mID]);
3670  if (id!=NO_UINT)
3671  {
3672  infoID=&(info[id]);
3673 
3674  #if (_DEBUG>1)
3675  printf(" N[%u->%u][%u] : ",mID,id,infoID->status);
3676  #endif
3677 
3678  if (infoID->status!=2) /*not closed (closed can not be improved)*/
3679  {
3680  pID=GetChartCenter(a->charts[id]);
3681 
3682  /* Geodesic distance is more accurate than euclidean and
3683  includes the presence of obstacles, if any. If obstacles
3684  are for sure not present, we just use the Euclidean
3685  distance.
3686 
3687  Caution: The GeodesicDistance function is not always
3688  able to find the path when it exists. This can induce
3689  sub-optimatility in the search. Moreover, the returned
3690  path is not the true geodesic, but at least is a path
3691  on the manifold. Use with caution.
3692  */
3693  if (ON_CUIKSYSTEM(a->w))
3694  dst=DistanceTopology(a->m,a->tp,pmID,pID);
3695  else
3696  dst=GeodesicDistance(pr,mID,id,a); /* This might be parallelized */
3697 
3698  c=infoParent->cost+dst;
3699 
3700  if ((infoID->status==0)|| /* 1st time we see this node */
3701  (c<infoID->cost)) /* status must be 1 (open) */
3702  {
3703  #if (_DEBUG>1)
3704  if (infoID->status==0)
3705  printf("new %g -> %g\n",dst,c);
3706  else
3707  printf("cheaper %g -> %g<%g\n",dst,c,infoID->cost);
3708  #endif
3709  /*not open not closed (1st time we see this node) */
3710  pred[id]=mID;
3711  infoID->cost=c;
3712  infoID->heuristic=DistanceTopology(a->m,a->tp,pID,ps);
3713 
3714  /* if open we are re-adding it to the heap. The cheapest
3715  path will have higher priority in the heap. */
3716  InitAtlasHeapElement(id,infoID->cost+infoID->heuristic,beta,
3717  &he2);
3718  AddElement2Heap(NO_UINT,(void *)(&he2),&h);
3719 
3720  infoID->status=1; /*open*/
3721  }
3722  #if (_DEBUG>1)
3723  else
3724  printf("%g > %g\n",c,infoID->cost);
3725  #endif
3726  }
3727  #if (_DEBUG>1)
3728  else
3729  printf("closed (%g)\n",infoID->cost);
3730  #endif
3731  }
3732  }
3733 
3734  infoParent->status=2; /*we close the current node node (all neighbors already explored) */
3735 
3736  } /*alrady closed element*/
3737  } /*if objective found*/
3738 
3739  it++;
3740  *time=GetElapsedTime(&stime);
3741  } /*loop until objective found*/
3742 
3743  DeleteStatistics(&stime);
3744 
3745  /* Use the pred to reconstruct the path */
3746  if (found)
3747  ReconstructAtlasPath(pr,pred,mID,ps,nv,systemVars,pl,ns,path,a);
3748  else
3749  {
3750  *ns=0;
3751  path=NULL;
3752  }
3753 
3754  if (a->parallel)
3755  {
3756  free(chartID);
3757  free(newCharts);
3758  free(vs);
3759  for(i=0;i<a->nCores;i++)
3760  free(ts[i]);
3761  free(ts);
3762  }
3763 
3764  if (st!=NULL)
3765  {
3766  PrintAtlasStatistics(pr,st);
3767  free(st);
3768  }
3769 
3770  free(info);
3771  free(pred);
3772 
3773  free(t);
3774  free(ps);
3775  DeleteHeap(&h);
3776  free(systemVars);
3777 
3778  return(found);
3779 }
3780 
3781 boolean AtlasGBF(Tparameters *pr,double *p,double *time,
3782  double *pl,unsigned int *ns,double ***path,Tatlas *a)
3783 {
3784  Theap h;
3785  TAtlasHeapElement he,he2;
3786  unsigned int mID;
3787  double heuristic;
3788  unsigned int *pred;
3789  boolean found;
3790  double *t;
3791  double epsilon;
3792  double *ps,*pWithDummies;
3793  double er;
3794  unsigned int nv;
3795  boolean *systemVars;
3796  unsigned int cm,cmPrev,mmPrev;
3797  TAtlasStatistics st;
3798  unsigned int it;
3799  Tstatistics stime;
3800  double beta;
3801  boolean collision=FALSE;
3802  double maxTime;
3803  unsigned int maxCharts;
3804 
3805  if (a->currentChart!=1)
3806  Error("Connect2Atlas assumes an initial atlas with only one local chart");
3807 
3808  epsilon=GetParameter(CT_EPSILON,pr);
3809  beta=GetParameter(CT_ATLASGBF_BETA,pr);
3810  maxTime=GetParameter(CT_MAX_PLANNING_TIME,pr);
3811  maxCharts=(unsigned int)GetParameter(CT_MAX_CHARTS,pr);
3812 
3813  nv=CS_WD_GET_SYSTEM_VARS(&systemVars,a->w);
3814 
3815  CS_WD_REGENERATE_SOLUTION_POINT(pr,p,&pWithDummies,a->w);
3816  if (CS_WD_GENERATE_SIMPLIFIED_POINT(pr,pWithDummies,&ps,a->w)!=a->m)
3817  Error("Dimension missmatch in AtlasGBF");
3818  if (a->n==0)
3819  er=0.0;
3820  else
3821  er=CS_WD_ERROR_IN_SIMP_EQUATIONS(pr,ps,a->w);
3822  if (er>epsilon)
3823  Error("Target point for the AtlasGBF is not on the manifold");
3824 
3825  collision=CS_WD_ORIGINAL_IN_COLLISION(pr,pWithDummies,NULL,a->w);
3826  if (collision)
3827  Warning("Target point for the AtlasGBF is in collision");
3828  free(pWithDummies);
3829 
3830  InitHeap(sizeof(TAtlasHeapElement),
3835 
3836  NEW(t,a->k,double);
3837 
3838  NEW(pred,a->maxCharts,unsigned int);
3839 
3840  heuristic=DistanceTopology(a->m,a->tp,GetChartCenter(a->charts[0]),ps);
3841  pred[0]=NO_UINT;
3842 
3843  InitAtlasStatistics(&st);
3844 
3845  InitAtlasHeapElement(0,heuristic,beta,&he);
3846  AddElement2Heap(NO_UINT,(void *)(&he),&h);
3847 
3848  found=FALSE;
3849 
3850  it=0;
3851  InitStatistics(a->nCores,0,&stime);
3852  *time=0.0;
3853 
3854  while ((!found)&&(!HeapEmpty(&h))&&(*time<maxTime)&&(a->currentChart<maxCharts))
3855  {
3856  ExtractMinElement((void *)(&he),&h);
3857  mID=GetAtlasHeapElementID(&he);
3858 
3859  if ((ATLAS_VERBOSE)||((it%1000)==0))
3860  {
3861  printf("Iteration %u (c:%u t:%g)\n",it,a->currentChart,*time);
3862  fflush(stdout);
3863  }
3864 
3865  if ((PointOnChart(pr,&(a->J),ps,a->tp,t,a->charts[mID]))&&
3866  (DistanceOnChart(pr,t,a->tp,&(a->J),a->charts[mID])<INF))
3867  found=TRUE;
3868  else
3869  {
3870  if (ExpandibleChart(a->charts[mID]))
3871  {
3872  NewBoundaryAttempt(&st);
3873 
3874  if (!RandomPointOnBoundary(t,a->charts[mID]))
3875  {
3876  NewNotInBoundary(&st);
3878  }
3879  else
3880  {
3881  /*printf(" New random point: ");PrintVector(stdout,a->k,t);*/
3882 
3883  cmPrev=a->currentChart;
3884  mmPrev=a->maxCharts;
3885  if (ExtendAtlasTowardPoint(pr,mID,t,TRUE,&st,a))
3886  {
3887  /* If we extended the room for charts, we have to
3888  extend the associated information accordingly*/
3889  if (mmPrev<a->maxCharts)
3890  MEM_EXPAND(pred,a->maxCharts,unsigned int);
3891 
3892  for(cm=cmPrev;cm<a->currentChart;cm++)
3893  {
3894  pred[cm]=mID;
3895  heuristic=DistanceTopology(a->m,a->tp,
3896  GetChartCenter(a->charts[cm]),
3897  ps);
3898  InitAtlasHeapElement(cm,heuristic,beta,&he2);
3899  AddElement2Heap(NO_UINT,(void *)(&he2),&h);
3900  }
3901  }
3902  else
3904  }
3905 
3906  AddElement2Heap(NO_UINT,(void *)(&he),&h);
3907  } /* if ExpandibleChart() = chart that can be expanded*/
3908  } /*if objective found*/
3909 
3910  it++;
3911  *time=GetElapsedTime(&stime);
3912  } /*loop until objective found*/
3913  DeleteStatistics(&stime);
3914 
3915  /* Use the pred to reconstruct the path */
3916  if (found)
3917  ReconstructAtlasPath(pr,pred,mID,ps,nv,systemVars,pl,ns,path,a);
3918  else
3919  {
3920  *ns=0;
3921  path=NULL;
3922  }
3923 
3924  PrintAtlasStatistics(pr,&st);
3925 
3926  free(pred);
3927  free(t);
3928  free(ps);
3929  DeleteHeap(&h);
3930  free(systemVars);
3931 
3932  return(found);
3933 }
3934 
3935 unsigned int AtlasMemSize(Tatlas *a)
3936 {
3937  unsigned int b,i;
3938 
3939  b=0;
3940  for(i=0;i<a->currentChart;i++)
3941  b+=ChartMemSize(a->charts[i]); /* charts[][] */
3942 
3943  return(b);
3944 }
3945 
3947 {
3948  FILE *f;
3949  unsigned int i;
3950 
3951  f=fopen(GetFileFullName(fname),"w");
3952  if (!f)
3953  Error("Could not open fiel to store atlas");
3954 
3955  fprintf(f,"%u\n",a->m);
3956  fprintf(f,"%u\n",a->k);
3957  fprintf(f,"%u\n",a->n);
3958  fprintf(f,"%.12f\n",a->e);
3959  fprintf(f,"%.12f\n",a->ce);
3960  fprintf(f,"%.12f\n",a->r);
3961  fprintf(f,"%u\n",a->simpleChart);
3962 
3963  fprintf(f,"%u\n",a->maxCharts);
3964  fprintf(f,"%u\n",a->currentChart);
3965  for(i=0;i<a->currentChart;i++)
3966  SaveChart(f,a->charts[i]);
3967 
3968  SaveBifurcations(f,a);
3969 
3970  fclose(f);
3971 }
3972 
3974 {
3975  FILE *f;
3976  unsigned int i;
3977 
3978  f=fopen(GetFileFullName(fname),"r");
3979  if (!f)
3980  Error("Could not open file to read atlas");
3981 
3982  fscanf(f,"%u",&(a->m));
3983  fscanf(f,"%u",&(a->k));
3984  fscanf(f,"%u",&(a->n));
3985  fscanf(f,"%lf",&(a->e));
3986  fscanf(f,"%lf",&(a->ce));
3987  fscanf(f,"%lf",&(a->r));
3988  fscanf(f,"%u",&(a->simpleChart));
3989 
3990  a->w=w;
3991  SetAtlasTopology(pr,a);
3992 
3993  NEW(a->ambient,1,Tbox);
3995 
3996  fscanf(f,"%u",&(a->maxCharts));
3997  fscanf(f,"%u",&(a->currentChart));
3998  NEW(a->charts,a->maxCharts,Tchart *);
3999  for(i=0;i<a->currentChart;i++)
4000  {
4001  NEW(a->charts[i],1,Tchart);
4002  LoadChart(f,a->w,a->charts[i]);
4003 
4004  #if (USE_ATLAS_TREE)
4005  if (i==0)
4006  InitBTree(i,a->charts[i],a->ambient,a->tp,&(a->tree));
4007  else
4008  AddChart2Btree(i,a->charts[i],&(a->tree));
4009  #endif
4010  }
4011 
4012  LoadBifurcations(f,a);
4013 
4014  fclose(f);
4015 
4016  /* The Jacobian is not saved but re-computed */
4017  CS_WD_GET_SIMP_JACOBIAN(pr,&(a->J),a->w);
4018  GetJacobianSize(&(a->nrJ),&(a->ncJ),&(a->J));
4019 
4020  /* delay the Hessian computation */
4021  a->H=NULL;
4022 
4023  /* The parallelism information is not stored! */
4024  #ifdef _OPENMP
4025  a->nCores=omp_get_max_threads();
4026  a->parallel=((a->nCores>1)&&((a->m>25)||(a->k>2)));
4027  #else
4028  a->parallel=FALSE;
4029  #endif
4030 
4031  if (!a->parallel)
4032  a->nCores=1;
4033 }
4034 
4035 unsigned int GetAtlasNumCharts(Tatlas *a)
4036 {
4037  return(a->currentChart);
4038 }
4039 
4040 Tchart *GetAtlasChart(unsigned int id,Tatlas *a)
4041 {
4042  if (id<a->currentChart)
4043  return(a->charts[id]);
4044  else
4045  return(NULL);
4046 }
4047 
4048 boolean RandomPointInAtlas(Tparameters *pr,double scale,double *w,
4049  unsigned int *nm,
4050  double *t,double *p,Tatlas *a)
4051 {
4052  boolean havePoint;
4053 
4054  if (w==NULL)
4055  {
4056  /* Uniform distribution on charts*/
4057  *nm=randomMax(a->currentChart-1);
4058  }
4059  else
4060  {
4061  /* Select the charts from the given distribution */
4063  }
4064  /* Get a point from the selected chart */
4065  havePoint=RandomPointInChart(pr,scale,a->tp,t,p,a->charts[*nm]);
4066 
4067  return(havePoint);
4068 }
4069 
4070 double AtlasVolume(Tparameters *pr,boolean collisionFree,Tatlas *a)
4071 {
4072  unsigned int i;
4073  double v,vc;
4074 
4075  v=0.0;
4076  for(i=0;i<a->currentChart;i++)
4077  {
4078  if (!FrontierChart(a->charts[i]))
4079  {
4080  vc=ChartVolume(pr,collisionFree,a->tp,&(a->J),a->charts[i]);
4081  #if (ATLAS_VERBOSE)
4082  printf(" Volume of chart %u: %g\n",i,vc);
4083  #endif
4084  v+=vc;
4085  }
4086  }
4087 
4088  return(v);
4089 }
4090 
4091 void SaveAtlasGraph(char *fname,Tatlas *a)
4092 {
4093  Tfilename fg;
4094  FILE *f;
4095  unsigned int i,j,n;
4096 
4097  CreateFileName(NULL,fname,NULL,ATLAS_GRAPH_EXT,&fg);
4098  f=fopen(GetFileFullName(&fg),"w");
4099  if (!f)
4100  Error("Could not open the file to store the atlas graph in SaveAtlasGraph");
4101  fprintf(stderr,"Writing atlas graph to : %s\n",GetFileFullName(&fg));
4102 
4103  fprintf(f,"%u\n",a->currentChart);
4104  for(i=0;i<a->currentChart;i++)
4105  {
4106  n=ChartNumNeighbours(a->charts[i]);
4107  fprintf(f,"%u ",n);
4108  for(j=0;j<n;j++)
4109  fprintf(f,"%u ",ChartNeighbourID(j,a->charts[i]));
4110  fprintf(f,"\n");
4111  }
4112 
4113  fclose(f);
4114  DeleteFileName(&fg);
4115 }
4116 
4117 void SaveChartCenters(Tparameters *pr,char *fname,
4118  boolean saveWithDummies,Tatlas *a)
4119 {
4120  double *o;
4121  unsigned int i,j,nv,nvs;
4122  boolean *systemVars;
4123  double *c;
4124  FILE *fo;
4125  Tfilename fsol;
4126 
4127  if (saveWithDummies)
4128  CreateFileName(NULL,fname,"_center",SOL_WITH_DUMMIES_EXT,&fsol);
4129  else
4130  CreateFileName(NULL,fname,"_center",SOL_EXT,&fsol);
4131  fprintf(stderr,"Writing boxes to : %s\n",GetFileFullName(&fsol));
4132  fo=fopen(GetFileFullName(&fsol),"w");
4133  if (!fo)
4134  Error("Could not open the file to store the boxes in GetChartCenters");
4135 
4136  nv=CS_WD_GET_SYSTEM_VARS(&systemVars,a->w);
4137  nvs=0;
4138  for(j=0;j<nv;j++)
4139  {
4140  if (systemVars[j])
4141  nvs++;
4142  }
4143 
4144  for(i=0;i<a->currentChart;i++)
4145  {
4146  c=GetChartCenter(a->charts[i]);
4147 
4148  CS_WD_REGENERATE_ORIGINAL_POINT(pr,c,&o,a->w);
4149 
4150  if (saveWithDummies)
4151  fprintf(fo,"%u { %u ",i,nv);
4152  else
4153  fprintf(fo,"%u { %u ",i,nvs);
4154  for(j=0;j<nv;j++)
4155  {
4156  if ((saveWithDummies)||(systemVars[j]))
4157  fprintf(fo,"[%.12g,%.12g] ",o[j],o[j]);
4158  }
4159  fprintf(fo,"}\n");
4160 
4161  free(o);
4162  }
4163 
4164  fclose(fo);
4165 
4166  free(systemVars);
4167 
4168  DeleteFileName(&fsol);
4169 }
4170 
4171 void SaveSingularCharts(Tparameters *pr,char *fname,
4172  boolean saveWithDummies,Tatlas *a)
4173 {
4174  double *o;
4175  unsigned int i,j,nv,nvs;
4176  boolean *systemVars;
4177  double *c;
4178  FILE *fo;
4179  Tfilename fsol;
4180 
4181  if (saveWithDummies)
4182  CreateFileName(NULL,fname,"_singular",SOL_WITH_DUMMIES_EXT,&fsol);
4183  else
4184  CreateFileName(NULL,fname,"_singular",SOL_EXT,&fsol);
4185  fprintf(stderr,"Writing boxes to : %s\n",GetFileFullName(&fsol));
4186  fo=fopen(GetFileFullName(&fsol),"w");
4187  if (!fo)
4188  Error("Could not open the file to store the boxes in GetChartCenters");
4189 
4190  nv=CS_WD_GET_SYSTEM_VARS(&systemVars,a->w);
4191  nvs=0;
4192  for(j=0;j<nv;j++)
4193  {
4194  if (systemVars[j])
4195  nvs++;
4196  }
4197 
4198  for(i=0;i<a->currentChart;i++)
4199  {
4200  if (SingularChart(a->charts[i]))
4201  {
4202  c=GetChartCenter(a->charts[i]);
4203 
4204  CS_WD_REGENERATE_ORIGINAL_POINT(pr,c,&o,a->w);
4205 
4206  if (saveWithDummies)
4207  fprintf(fo,"%u { %u ",i,nv);
4208  else
4209  fprintf(fo,"%u { %u ",i,nvs);
4210  for(j=0;j<nv;j++)
4211  {
4212  if ((saveWithDummies)||(systemVars[j]))
4213  fprintf(fo,"[%.12g,%.12g] ",o[j],o[j]);
4214  }
4215  fprintf(fo,"}\n");
4216 
4217  free(o);
4218  }
4219  }
4220  fclose(fo);
4221 
4222  free(systemVars);
4223 
4224  DeleteFileName(&fsol);
4225 }
4226 
4227 void PlotAtlas(char *fname,int argc,char **arg,Tparameters *pr,FILE *fcost,
4228  unsigned int xID,unsigned int yID,unsigned int zID,Tatlas *a)
4229 {
4230  Tplot3d p3d;
4231  unsigned int i;
4232  Tcolor color;
4233  unsigned int obj,np;
4234 
4235  InitPlot3d(fname,FALSE,argc,arg,&p3d);
4236 
4237  /* When plotting as polytopes all charts are plotted in blue and not
4238  bifurcation info is plotted */
4239 
4240  NewColor(0.5,0.5,1,&color); /*default color :blue*/
4241  obj=StartNew3dObject(&color,&p3d);
4242  np=0;
4243  for(i=0;i<a->currentChart;i++)
4244  {
4245  if ((fcost!=NULL)||(a->simpleChart)||
4247  (!SingularChart(a->charts[i])&&(!FrontierChart(a->charts[i])))))
4248  {
4249  PlotChart(pr,fcost,&(a->J),xID,yID,zID,&p3d,a->charts[i]);
4250  np++;
4251  }
4252  }
4253  if ((a->k==2)&&(fcost!=NULL))
4254  Close3dObjectNoColor(&p3d); /* the cost already gave the color when plotting */
4255  else
4256  Close3dObject(&p3d);
4257  if (np==0)
4258  Delete3dObject(obj,&p3d);
4259  DeleteColor(&color);
4260 
4261  if ((PLOT_ATLAS_IN_COLORS)&&(fcost==NULL)&&(!a->simpleChart))
4262  {
4263  /* Singular charts in green */
4264  NewColor(0,1,0,&color); /*green*/
4265  obj=StartNew3dObject(&color,&p3d);
4266  np=0;
4267  for(i=0;i<a->currentChart;i++)
4268  {
4269  if ((!ExpandibleChart(a->charts[i]))&&(SingularChart(a->charts[i])&&(!FrontierChart(a->charts[i]))))
4270  {
4271  np++;
4272  PlotChart(pr,NULL,&(a->J),xID,yID,zID,&p3d,a->charts[i]);
4273  //PrintVector(stderr,NULL,a->m,a->charts[i]->center);
4274  }
4275  }
4276 
4277  Close3dObject(&p3d);
4278  DeleteColor(&color);
4279  if (np==0)
4280  Delete3dObject(obj,&p3d);
4281 
4282  /* Open charts if red (Open = expansion is still possible) */
4283  NewColor(1,0.5,0.5,&color); /*red*/
4284  obj=StartNew3dObject(&color,&p3d);
4285  np=0;
4286  for(i=0;i<a->currentChart;i++)
4287  {
4288  if ((ExpandibleChart(a->charts[i]))&&(!SingularChart(a->charts[i])&&(!FrontierChart(a->charts[i]))))
4289  {
4290  np++;
4291  PlotChart(pr,NULL,&(a->J),xID,yID,zID,&p3d,a->charts[i]);
4292  }
4293  }
4294 
4295  Close3dObject(&p3d);
4296  DeleteColor(&color);
4297  if (np==0)
4298  Delete3dObject(obj,&p3d);
4299 
4300  /* Expandible singular charts in yellow */
4301  NewColor(1,1,0,&color); /*yellow*/
4302  obj=StartNew3dObject(&color,&p3d);
4303  np=0;
4304  for(i=0;i<a->currentChart;i++)
4305  {
4306  if ((ExpandibleChart(a->charts[i]))&&(SingularChart(a->charts[i])&&(!FrontierChart(a->charts[i]))))
4307  {
4308  np++;
4309  PlotChart(pr,NULL,&(a->J),xID,yID,zID,&p3d,a->charts[i]);
4310  }
4311  }
4312 
4313  Close3dObject(&p3d);
4314  DeleteColor(&color);
4315  if (np==0)
4316  Delete3dObject(obj,&p3d);
4317 
4318  /* Frontier charts in dark red */
4319  NewColor(0.25,0,0,&color); /* */
4320  obj=StartNew3dObject(&color,&p3d);
4321  np=0;
4322  for(i=0;i<a->currentChart;i++)
4323  {
4324  //if ((!ExpandibleChart(a->charts[i]))&&(!SingularChart(a->charts[i])&&(FrontierChart(a->charts[i]))))
4325  if (FrontierChart(a->charts[i]))
4326  {
4327  np++;
4328  PlotChart(pr,NULL,&(a->J),xID,yID,zID,&p3d,a->charts[i]);
4329  }
4330  }
4331 
4332  Close3dObject(&p3d);
4333  DeleteColor(&color);
4334  if (np==0)
4335  Delete3dObject(obj,&p3d);
4336 
4337  /* Now plot a line connecting chart at the two sides of a singularity */
4338  //PlotBifurcations(pr,&p3d,xID,yID,zID,a);
4339  }
4340 
4341  /*line for the neighbouring relations*/
4342 #if (0)
4343  {
4344  unsigned int j;
4345  double *pi,*pj;
4346  double x[2],y[2],z[2];
4347  unsigned int nn,nj;
4348  double *o;
4349 
4350  NewColor(1,1,0,&color);
4351  StartNew3dObject(&color,&p3d);
4352  for(i=0;i<a->currentChart;i++)
4353  {
4354  pi=GetChartCenter(a->charts[i]);
4355  CS_WD_REGENERATE_ORIGINAL_POINT(pr,pi,&o,a->w);
4356  x[0]=o[xID];
4357  y[0]=o[yID];
4358  z[0]=o[zID];
4359  free(o);
4360 
4361  nn=ChartNumNeighbours(a->charts[i]);
4362  if (nn==0)
4363  fprintf(stderr,"Chart %u has no neighbours",i);
4364  for(j=0;j<nn;j++)
4365  {
4366  nj=ChartNeighbourID(j,a->charts[i]);
4367  if ((nj!=NO_UINT)&&(nj>i))
4368  {
4369  pj=GetChartCenter(a->charts[nj]);
4370  CS_WD_REGENERATE_ORIGINAL_POINT(pr,pj,&o,a->w);
4371  x[1]=o[xID];
4372  y[1]=o[yID];
4373  z[1]=o[zID];
4374  free(o);
4375  PlotVect3d(2,x,y,z,&p3d);
4376  }
4377  }
4378  }
4379  Close3dObject(&p3d);
4380  DeleteColor(&color);
4381  }
4382 #endif
4383 
4384  ClosePlot3d(FALSE,0,0,0,&p3d);
4385 }
4386 
4387 void TriangulateAtlas(char *fname,int argc,char **arg,Tparameters *pr,FILE *fcost,
4388  unsigned int xID,unsigned int yID,unsigned int zID,Tatlas *a)
4389 {
4390  Tplot3d p3d;
4391  unsigned int i,j,k;
4392  Tcolor color;
4393  double *p,*o;
4394  double cost;
4395  double **v; /* Vertices */
4396  Tcolor *c; /* One color per vertex (might not be used) */
4397  unsigned int mf,nf; /* Max faces (space allocated for faces) and current number
4398  of faces */
4399  unsigned int *nvf,**fv; /* Vertex per face, and vertices in each face. */
4400  unsigned int nTmp;
4401  double v1[3],v2[3],cosAngle;
4402  boolean *corrected,*used;
4403  double **normal;
4404  unsigned int nc;
4405  boolean correctionDone;
4406  unsigned int nn,*nID1,*nID2;
4407  boolean found;
4408  unsigned int *tp;
4409  unsigned int rep;
4410  double cx,cy,cz;
4411 
4412  if (a->k!=2)
4413  Error("TriangulateAtlas only operates on surfaces (manifold dim=2)");
4414 
4415  NEW(tp,3,unsigned int);
4416  tp[0]=CS_WD_GET_VAR_TOPOLOGY(xID,a->w);
4417  tp[1]=CS_WD_GET_VAR_TOPOLOGY(yID,a->w);
4418  tp[2]=CS_WD_GET_VAR_TOPOLOGY(zID,a->w);
4419 
4420  if ((tp[0]==TOPOLOGY_R)&&(tp[1]==TOPOLOGY_R)&&(tp[2]==TOPOLOGY_R))
4421  {
4422  free(tp);
4423  tp=NULL;
4424  }
4425 
4426  InitPlot3d(fname,FALSE,argc,arg,&p3d);
4427 
4428  /* Now store the vertices, one per chart (i.e., the center), possibly
4429  with associated colors */
4430  NEW(v,a->currentChart,double*);
4431  if (fcost!=NULL)
4432  NEW(c,a->currentChart,Tcolor);
4433 
4434  cx=GetParameter(CT_CUT_X,pr);
4435  cy=GetParameter(CT_CUT_Y,pr);
4436  cz=GetParameter(CT_CUT_Z,pr);
4437  rep=(unsigned int)(GetParameter(CT_REPRESENTATION,pr));
4438 
4439  for(i=0;i<a->currentChart;i++)
4440  {
4441  NEW(v[i],3,double);
4442  p=GetChartCenter(a->charts[i]);
4443  CS_WD_REGENERATE_ORIGINAL_POINT(pr,p,&o,a->w);
4444  v[i][0]=o[xID];
4445  v[i][1]=o[yID];
4446  v[i][2]=o[zID];
4447  if (rep==REP_JOINTS)
4448  {
4449  if ((cx<0)&&(v[i][0]<cx))
4450  v[i][0]+=M_2PI;
4451  if ((cx>0)&&(v[i][0]>cx))
4452  v[i][0]-=M_2PI;
4453  if ((cy<0)&&(v[i][1]<cy))
4454  v[i][1]+=M_2PI;
4455  if ((cy>0)&&(v[i][1]>cy))
4456  v[i][1]-=M_2PI;
4457  if ((cz<0)&&(v[i][2]<cz))
4458  v[i][2]+=M_2PI;
4459  if ((cz>0)&&(v[i][2]>cz))
4460  v[i][2]-=M_2PI;
4461  }
4462  free(o);
4463  if (fcost!=NULL)
4464  {
4465  /* Set up the color for this vertex */
4466  if (fscanf(fcost,"%lf",&cost)!=1)
4467  Error("No enough data in the cost file");
4468 
4469  CostColor(cost,1e-1,&(c[i]));
4470  }
4471  }
4472 
4473  /* Now set up the faces */
4474  mf=a->currentChart;
4475  nf=0;
4476  NEW(fv,mf,unsigned int*);
4477  for(i=0;i<a->currentChart;i++)
4478  {
4479  GetChartNeighboursFromVertices(pr,&nn,&nID1,&nID2,a->charts[i]);
4480  for(j=0;j<nn;j++)
4481  {
4482  /* Vertices corresponding to the border of the atlas still
4483  have the default faces with NO_UINT as ID -> not use these
4484  vertices*/
4485  if ((nID1[j]!=NO_UINT)&&(nID2[j]!=NO_UINT))
4486  {
4487  if (nf==mf)
4488  MEM_DUP(fv,mf,unsigned int*);
4489  NEW(fv[nf],3,unsigned int);
4490 
4491  fv[nf][0]=i;
4492  fv[nf][1]=nID1[j];
4493  fv[nf][2]=nID2[j];
4494 
4495  /* Triangles that cross the borders imposed by the topology
4496  should not be considered. */
4497  if (tp==NULL)
4498  found=FALSE;
4499  else
4500  {
4501  /*
4502  found=((CrossTopologyBorder(a->m,a->tp,
4503  GetChartCenter(a->charts[i]),
4504  GetChartCenter(a->charts[nID1[j]])))||
4505  (CrossTopologyBorder(a->m,a->tp,
4506  GetChartCenter(a->charts[i]),
4507  GetChartCenter(a->charts[nID2[j]])))||
4508  (CrossTopologyBorder(a->m,a->tp,
4509  GetChartCenter(a->charts[nID1[j]]),
4510  GetChartCenter(a->charts[nID1[j]]))));
4511  */
4512  found=((CrossTopologyBorder(3,tp,
4513  v[i],
4514  v[nID1[j]]))||
4515  (CrossTopologyBorder(3,tp,
4516  v[i],
4517  v[nID2[j]]))||
4518  (CrossTopologyBorder(3,tp,
4519  v[nID1[j]],
4520  v[nID2[j]])));
4521  }
4522 
4523  k=0;
4524  while ((!found)&&(k<nf))
4525  {
4526  found=SameTriangle(fv[nf],fv[k]);
4527  k++;
4528  }
4529  if (!found)
4530  nf++;
4531  }
4532  }
4533  free(nID1);
4534  free(nID2);
4535  }
4536  if (tp!=NULL)
4537  free(tp);
4538 
4539  /* All faces have 3 vertices */
4540  NEW(nvf,nf,unsigned int);
4541  for(i=0;i<nf;i++)
4542  nvf[i]=3;
4543  /* Fix the orientation for the triangles */
4544  NEW(used,nf,boolean);
4545  NEW(corrected,nf,boolean);
4546  NEW(normal,nf,double*);
4547  for(i=0;i<nf;i++)
4548  {
4549  used[i]=FALSE;
4550  corrected[i]=FALSE;
4551  NEW(normal[i],3,double);
4552  }
4553  /* Start from the first triangle */
4554  corrected[0]=TRUE;
4555  DifferenceVector(3,v[fv[0][1]],v[fv[0][0]],v1);
4556  DifferenceVector(3,v[fv[0][2]],v[fv[0][0]],v2);
4557  CrossProduct(v1,v2,normal[0]);
4558  nc=1;
4559  correctionDone=FALSE;
4560  while((!correctionDone)&&(nc<nf))
4561  {
4562  /* Search for a corrected no used face */
4563  i=0;
4564  while ((i<nf)&&((used[i])||(!corrected[i])))
4565  i++;
4566  if (i==nf)
4567  correctionDone=TRUE;
4568 
4569  if (!correctionDone)
4570  {
4571  /* Align all neighbours with the normal at this face */
4572  j=0;
4573  while(j<nf)
4574  {
4575  if (!corrected[j])
4576  {
4577  if (NeighbouringTriangles(fv[i],fv[j]))
4578  {
4579  DifferenceVector(3,v[fv[j][1]],v[fv[j][0]],v1);
4580  DifferenceVector(3,v[fv[j][2]],v[fv[j][0]],v2);
4581  CrossProduct(v1,v2,normal[j]);
4582 
4583  cosAngle=DotProduct(normal[i],normal[j]);
4584  if (cosAngle<0)
4585  {
4586  /* Swap the orientation of face j */
4587  nTmp=fv[j][1];
4588  fv[j][1]=fv[j][2];
4589  fv[j][2]=nTmp;
4590  ScaleVector(-1.0,3,normal[j]);
4591  }
4592  corrected[j]=TRUE;
4593  nc++;
4594  }
4595  }
4596  j++;
4597  }
4598  used[i]=TRUE;
4599  }
4600  }
4601  free(used);
4602  free(corrected);
4603  for(i=0;i<nf;i++)
4604  free(normal[i]);
4605  free(normal);
4606 
4607  /* Define the object */
4608  /* Normal charts in blue */
4609  NewColor(-1,-1,-1,&color); /*blue*/
4610  StartNew3dObject(&color,&p3d);
4611 
4612  if (fcost==NULL)
4613  Plot3dObject(a->currentChart,nf,1,v,nvf,fv,&p3d);
4614  else
4615  Plot3dObjectWithColors(a->currentChart,nf,1,v,c,nvf,fv,&p3d);
4616 
4617  Close3dObject(&p3d);
4618  ClosePlot3d(FALSE,0,0,0,&p3d);
4619 
4620  /* Release memory */
4621  for(i=0;i<a->currentChart;i++)
4622  free(v[i]);
4623  free(v);
4624 
4625  if (fcost!=NULL)
4626  free(c);
4627 
4628  for(i=0;i<nf;i++)
4629  free(fv[i]);
4630  free(fv);
4631  free(nvf);
4632 }
4633 
4635 {
4636  unsigned int i;
4637 
4638  DeleteJacobian(&(a->J));
4639 
4640  if (a->H!=NULL)
4641  {
4642  DeleteHessian(a->H);
4643  free(a->H);
4644  }
4645 
4646  for(i=0;i<a->currentChart;i++)
4647  {
4648  DeleteChart(a->charts[i]);
4649  free(a->charts[i]);
4650  }
4651 
4652  free(a->charts);
4653 
4654  if (a->ambient!=NULL)
4655  {
4656  DeleteBox(a->ambient);
4657  free(a->ambient);
4658  }
4659 
4660  if (a->tp!=NULL)
4661  free(a->tp);
4662 
4663  DeleteBifurcations(a);
4664 
4665  #if (USE_ATLAS_TREE)
4666  if (a->currentChart>0)
4667  DeleteBTree(&(a->tree));
4668  #endif
4669 }
#define CT_R
Ball radius in atlas.
Definition: parameters.h:257
void NewSingularImpossible(TAtlasStatistics *ast)
New time we could not extend a singular chart.
Definition: atlas.c:798
boolean AtlasAStar(Tparameters *pr, double *p, double *time, double *pl, unsigned int *ns, double ***path, Tatlas *a)
Expands the atlas to reach a given point.
Definition: atlas.c:3445
void NewBifurcation(TAtlasStatistics *ast)
New processed bifurcation.
Definition: atlas.c:804
unsigned int nImpossible
Definition: atlas.h:118
void SaveChartCenters(Tparameters *pr, char *fname, boolean saveWithDummies, Tatlas *a)
Stores the centers of the charts.
Definition: atlas.c:4117
boolean ExtendAtlasFromPoint(Tparameters *pr, unsigned int parentID, double *t, TAtlasStatistics *st, Tatlas *a)
Tries to expand the atlas from a chart from a given point.
Definition: atlas.c:1050
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
boolean PointOnChart(Tparameters *pr, TJacobian *sJ, double *p, unsigned int *tp, double *t, Tchart *c)
Identify points on a chart.
Definition: chart.c:1605
void SaveChart(FILE *f, Tchart *c)
Stores the chart information on a file.
Definition: chart.c:1880
unsigned int d2
Definition: atlas.h:163
void AddChart2Btree(unsigned int id, Tchart *mp, TBTree *t)
Adds a chart to the tree.
Definition: btree.c:75
unsigned int ns
Definition: atlas.c:36
unsigned int ChartMemSize(Tchart *c)
Memory used by a given chart.
Definition: chart.c:1865
void LoadAtlas(Tparameters *pr, Tfilename *fname, TAtlasBase *w, Tatlas *a)
Defines an atlas from the information on a file.
Definition: atlas.c:3973
boolean FindPointInOtherBranch(Tparameters *pr, unsigned int bID, double *phi, double **p, Tatlas *a)
Searches for a point in the other manifold branch.
Definition: atlas.c:2389
#define ATLAS_GRAPH_EXT
File extension for files atlas graph information.
Definition: filename.h:207
double SquaredDistanceTopologyMin(double t2, unsigned int s, unsigned int *tp, double *v1, double *v2)
Computes the squared distance of two points.
unsigned int d1
Definition: atlas.h:161
double ce
Definition: atlas.h:296
#define REP_JOINTS
One of the possible values of the REPRESENTATION parameter.
Definition: parameters.h:60
unsigned int nLargeError
Definition: atlas.h:121
unsigned int st
Definition: atlas.c:33
#define CS_WD_ERROR_IN_SIMP_EQUATIONS(pr, p, wcs)
Computes the error in the simplified system for a given point.
Definition: wcs.h:509
#define MIN_SAMPLING_RADIUS
Relative minimum sampling radius.
Definition: chart.h:59
#define CT_EPSILON
Numerical tolerance.
Definition: parameters.h:194
#define FALSE
FALSE.
Definition: boolean.h:30
unsigned int nSmallAngle
Definition: atlas.h:136
boolean CompareTangentSpaces(Tchart *c1, Tchart *c2)
Checks if the tangent spaces are similar.
Definition: chart.c:904
#define CS_WD_GENERATE_SIMPLIFIED_POINT(pr, p, r, wcs)
Generates a simplified point from an original one.
Definition: wcs.h:495
CBLAS_INLINE void SetRow(double *v, unsigned int k, unsigned int r, unsigned int c, double *m)
Sets a row of a matrix.
unsigned int FindKernel(unsigned int nr, unsigned int nc, double *mT, unsigned int dof, boolean check, double epsilon, double **T)
Computes the kernel of a matrix.
Definition: algebra.c:1119
unsigned int GetAtlasManifoldDim(Tatlas *a)
Manifold dimensionality.
Definition: atlas.c:3440
#define INIT_NUM_CHARTS_IN_ATLAS_HEAP
Initial number of elements in the heap of local charts.
Definition: atlas.h:102
double DistanceTopology(unsigned int s, unsigned int *tp, double *v1, double *v2)
Computes the distance of two points.
TJacobian J
Definition: atlas.h:315
unsigned int m
Definition: atlas.h:292
void DeleteHessian(THessian *h)
Destructor.
Definition: hessian.c:77
#define NEW(_var, _n, _type)
Allocates memory space.
Definition: defines.h:385
void SearchInBtree(Tchart *mp, unsigned int *nn, unsigned int **n, TBTree *t)
Search for neighbouring charts.
Definition: btree.c:142
#define CS_WD_GET_VAR_TOPOLOGY(varID, wcs)
Gets the variable topology.
Definition: wcs.h:277
void ComputeAtlasHessian(Tatlas *a)
Computes the Hessian from the Jacobian.
void EvaluateTransposedJacobianInVector(double *v, unsigned int nr, unsigned int nc, double *m, TJacobian *j)
Evaluates the transposed Jacobian.
Definition: jacobian.c:143
Data structure to hold the information about the name of a file.
Definition: filename.h:271
#define RC2INDEX(i, j, nr, nc)
Index in a vector of a matrix element.
Definition: basic_algebra.h:81
void ArrayPi2Pi(unsigned int n, unsigned int *t, double *a)
Applies PI2PI to an array.
Tbox * ambient
Definition: atlas.h:320
unsigned int status
Definition: atlas.c:53
boolean RandomPointInAtlas(Tparameters *pr, double scale, double *w, unsigned int *nm, double *t, double *p, Tatlas *a)
Samples a random point on the atlas.
Definition: atlas.c:4048
boolean BoundaryPointFromExternalCorner(boolean rand, unsigned int *nv, double *t, Tchart *c)
Random point on the chart boundary from the polytope vetices.
Definition: chart.c:1343
Structure used for the elements of the heap of charts.
Definition: atlas.h:176
void AllocateHessianEvaluation(double ****m, THessian *h)
Allocate space for the Hessian evaluation.
Definition: hessian.c:33
void EvaluateHessian(double *v, double ***m, THessian *h)
Evaluates the Hessian.
Definition: hessian.c:46
double GetAtlasError(Tatlas *a)
Maximum error for all the charts.
Definition: atlas.c:3420
unsigned int chartID
Definition: atlas.h:177
unsigned int randomMax(unsigned int m)
Returns a random integer in the range [0,m].
Definition: random.c:77
void ProcessBifurcation(Tparameters *pr, unsigned int bID, TAtlasStatistics *ast, Tatlas *a)
Processes the bifurcation points not yet processed.
Definition: atlas.c:2484
CBLAS_INLINE void TMatrixMatrixProduct(unsigned int ra, unsigned int ca, double *A, unsigned int cb, double *B, double *C)
C = A^t * B.
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.
void LinkCharts(unsigned int id1, Tchart *c1, unsigned int id2, Tchart *c2)
Connect charts at singularities.
Definition: chart.c:1672
unsigned int npCharts
Definition: atlas.h:308
unsigned int * tp
Definition: atlas.h:304
unsigned int FindRightNullVector(Tparameters *pr, unsigned int bID, double **phi, Tatlas *a)
Finds the right null vector that does not belong to the tangent space.
Definition: atlas.c:2177
void DeleteJacobian(TJacobian *j)
Destructor.
Definition: jacobian.c:255
#define CT_MAX_PLANNING_TIME
Maximum time for path planning.
Definition: parameters.h:475
void DifferenceVectorTopology(unsigned int s, unsigned int *tp, double *v1, double *v2, double *v)
Substracts two vectors.
void DeleteBTree(TBTree *t)
Chart tree destructor.
Definition: btree.c:184
#define PLOT_ATLAS_IN_COLORS
Activate/deactivate the colors in the atlas plot.
Definition: atlas.h:39
unsigned int nc
Definition: atlas.c:34
double GetAtlasRadius(Tatlas *a)
Radius used in the charts.
Definition: atlas.c:3415
void NewSingularPointMissed(TAtlasStatistics *ast)
New missed singular point.
Definition: atlas.c:816
boolean PointInBTree(double eps, double *p, TBTree *t)
Point in a tree.
Definition: btree.c:113
unsigned int randomWithDistribution(unsigned int m, double s, double *d)
Random number with a given discrete distribution.
Definition: random.c:86
void PenalizeAtlasHeapElement(TAtlasHeapElement *he)
Penalized the cost stored in a atlas heap element.
Definition: atlas.c:2683
void NewBoundaryAttempt(TAtlasStatistics *ast)
New attempt to generate a point on the boundary.
Definition: atlas.c:732
void InitPlot3d(char *name, boolean axes, int argc, char **arg, Tplot3d *p)
Constructor.
Definition: plot3d.c:41
#define TRUE
TRUE.
Definition: boolean.h:21
double * GetNewtonRHBuffer(TNewton *n)
Buffer to store the Newton right hand.
Definition: algebra.c:1152
double MinCosinusBetweenCharts(Tchart *c1, Tchart *c2)
Computes the angle between the tangent spaces in the charts.
Definition: chart.c:923
double r
Definition: atlas.h:297
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 DeleteAtlas(Tatlas *a)
Destructor.
Definition: atlas.c:4634
void Error(const char *s)
General error function.
Definition: error.c:80
boolean MinimizeOnAtlas(Tparameters *pr, char *fname, double *p, TAtlasBase *w, unsigned int maxSteps, double(*costF)(Tparameters *, boolean, double *, void *), void(*costG)(Tparameters *, boolean, double *, double **, void *), void *costData, Tatlas *a)
Gradient minimization on an manifold.
Definition: atlas.c:3153
#define TOPOLOGY_R
One of the possible topologies.
Definition: defines.h:122
A color.
Definition: color.h:86
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
unsigned int nInCollision
Definition: atlas.h:130
Representation of a bifurcation.
Definition: atlas.h:159
unsigned int mBifurcations
Definition: atlas.h:324
void Delete3dObject(unsigned int nobj, Tplot3d *p)
Deletes a previously created geometric object.
Definition: plot3d.c:152
boolean simpleChart
Definition: atlas.h:298
boolean NeighbouringTriangles(unsigned int *v1, unsigned int *v2)
Determines if two sets of vertices define a neighbouring triangle.
Definition: geom.c:657
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
#define CT_ATLASGBF_BETA
Cost penalty factor for atlasGBF.
Definition: parameters.h:289
A chart on a manifold.
Definition: chart.h:70
unsigned int GetAtlasAmbientDim(Tatlas *a)
Ambient dimensionality.
Definition: atlas.c:3435
unsigned int nQRSVDError
Definition: atlas.h:127
void DeleteAtlasHeapElement(void *he)
Atlas heap element destructor.
Definition: atlas.c:2688
unsigned int nrJ
Definition: atlas.h:313
double randomDouble()
Returns a random double in the [0,1] interval.
Definition: random.c:33
void AddElement2Heap(unsigned int id, void *e, Theap *heap)
Adds an element to the heap.
Definition: heap.c:294
void InitAtlasHeapElement(unsigned int mID, double c, double beta, TAtlasHeapElement *he)
Constructor of TAtlasHeapElement.
Definition: atlas.c:2640
void NewGoodExtension(TAtlasStatistics *ast)
New time we actually succeeded in creating a new chart.
Definition: atlas.c:786
unsigned int GetAtlasNumCharts(Tatlas *a)
Number of charts in the atlas.
Definition: atlas.c:4035
int NewtonStep(double nullSingularValue, double *x, double *dif, TNewton *n)
One step in a Newton iteration.
boolean DetectBifurcation(Tparameters *pr, unsigned int mID1, unsigned int mID2, Tatlas *a)
Determines if there is a bifurcation between the center of two charts.
Definition: atlas.c:1771
unsigned int nNonRegularPoint
Definition: atlas.h:123
Error and warning functions.
unsigned int nFarFromParent
Definition: atlas.h:129
void DeleteFileName(Tfilename *fn)
Destructor.
Definition: filename.c:205
#define CT_MAX_CHARTS
Maximum number of charts in an atlas.
Definition: parameters.h:490
unsigned int maxCharts
Definition: atlas.h:306
A 3D plot.
Definition: plot3d.h:54
unsigned int n
Definition: atlas.h:294
double GetChartRadius(Tchart *c)
Returns de range of the chart.
Definition: chart.c:941
#define CT_CUT_X
Limit of domain of the X dimension of 3d plots.
Definition: parameters.h:420
boolean HaveChartAtPoint(double eps, double *p, Tatlas *a)
Determines if there is a chart close to a given point.
Definition: atlas.c:906
void ReconstructAtlasPath(Tparameters *pr, unsigned int *pred, unsigned int mID, double *goal, unsigned int nv, boolean *systemVars, double *pl, unsigned int *ns, double ***path, Tatlas *a)
Reconstructs the path between the stat/goal configurations.
Definition: atlas.c:1498
double e
Definition: atlas.h:295
unsigned int k
Definition: atlas.h:293
double Distance(unsigned int s, double *v1, double *v2)
Computes the distance of two points.
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
double DotProduct(double *v1, double *v2)
Computes the dot product of two 3d vectors.
Definition: geom.c:652
double GeodesicDistance(Tparameters *pr, unsigned int parentID, unsigned int childID, Tatlas *a)
Approximates the geodesic distance between the centers of two charts.
Definition: atlas.c:1667
void NewFarFromParent(TAtlasStatistics *ast)
New time the point was too far from parent.
Definition: atlas.c:768
void InitBTree(unsigned int id, Tchart *mp, Tbox *ambient, unsigned int *topology, TBTree *t)
Initializes a binary tree of charts.
Definition: btree.c:19
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
Tbifurcation ** bifurcation
Definition: atlas.h:329
void EvaluateJacobianInVector(double *v, unsigned int nr, unsigned int nc, double *m, TJacobian *j)
Evaluates the Jacobian.
Definition: jacobian.c:103
void AddBorderConstraint(Tparameters *pr, double *t, unsigned int *tp, Tbox *ambient, Tchart *c)
Crops the domain for a given chart.
Definition: chart.c:229
void * GetHeapElement(unsigned int i, Theap *heap)
Returns a pointer to a heap element.
Definition: heap.c:331
CBLAS_INLINE double GeneralDotProduct(unsigned int s, double *v1, double *v2)
Computes the dot product of two general vectors.
Definition: basic_algebra.c:15
CBLAS_INLINE void MatrixVectorProduct(unsigned int r, unsigned int c, double *A, double *b, double *o)
Product of a matrix and a vector.
unsigned int HeapSize(Theap *heap)
Gets the number of elements in a heap.
Definition: heap.c:284
boolean parallel
Definition: atlas.h:301
#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
void NewImpossible(TAtlasStatistics *ast)
New Error when extending from/toward a point.
Definition: atlas.c:792
boolean RandomPointInChart(Tparameters *pr, double scale, unsigned int *tp, double *t, double *p, Tchart *c)
Samples a random point in the area covered by the chart.
Definition: chart.c:1367
void Close3dObjectNoColor(Tplot3d *p)
Closes a composed object without assigning any color.
Definition: plot3d.c:184
boolean LessThanAtlasHeapElement(void *he1, void *he2, void *userData)
Comparison between atlas heap elements.
Definition: atlas.c:2668
unsigned int nSingImpossible
Definition: atlas.h:119
Definitions of constants and macros used in several parts of the cuik library.
Tchart ** charts
Definition: atlas.h:311
void NewChartFromPoint(Tparameters *pr, unsigned int parentID, double *t, Tchart **newChart, Tatlas *a)
Generates a chart from a vertex taken from a parent chart.
Definition: atlas.c:954
void NewRadiousChange(TAtlasStatistics *ast)
New time we had to change the sampling radious to get a new chart.
Definition: atlas.c:780
boolean CollisionChart(Tchart *c)
Identifies collision charts.
Definition: chart.c:1280
boolean RefineSingularPoint(Tparameters *pr, unsigned int bID, Tatlas *a)
Improves the locatin f a singular point.
Definition: atlas.c:1961
void DifferenceVector(unsigned int s, double *v1, double *v2, double *v)
Substracts two vectors.
#define CS_WD_NEWTON_IN_SIMP(pr, p, wcs)
Applies a Newton procedure to move a point towads the manifold.
Definition: wcs.h:580
double cost
Definition: atlas.c:51
unsigned int nBoundaryAttempts
Definition: atlas.h:112
void NewSmallAngleAtBifurcation(TAtlasStatistics *ast)
Small angle between the charts at a bifurcation.
Definition: atlas.c:810
boolean CrossTopologyBorder(unsigned int s, unsigned int *tp, double *v1, double *v2)
Determines if the line between two points crosses the topology boder.
unsigned int nNoSingularEnough
Definition: atlas.h:139
Statistics associated with a solving process.
Definition: statistics.h:28
void SaveSingularCharts(Tparameters *pr, char *fname, boolean saveWithDummies, Tatlas *a)
Stores the centers of the singular charts.
Definition: atlas.c:4171
void NewNoJumpBranch(TAtlasStatistics *ast)
The jump to the other branch failed.
Definition: atlas.c:828
THessian * H
Definition: atlas.h:318
double * GetChartTangentSpace(Tchart *c)
Gets the chart tangent space.
Definition: chart.c:973
double ** path
Definition: atlas.c:38
unsigned int nExtensions
Definition: atlas.h:116
#define CS_WD_MANIFOLD_DIMENSION(pr, p, wcs)
Computes the dimension of the solution set.
Definition: wcs.h:565
unsigned int nBifurcations
Definition: atlas.h:325
double DistanceOnChart(Tparameters *pr, double *t, unsigned int *tp, TJacobian *sJ, Tchart *c)
Distance between the center of a chart and a point on this chart.
Definition: chart.c:1549
void DeleteAtlasHessian(Tatlas *a)
Removes the Hessian data.
boolean HeapEmpty(Theap *heap)
Checks if a heap is empty.
Definition: heap.c:289
double ChartVolume(Tparameters *pr, boolean collisionFree, unsigned int *tp, TJacobian *sJ, Tchart *c)
Estimate the volume of a chart.
Definition: chart.c:1409
void PostProcessNewCharts(Tparameters *pr, boolean bif, unsigned int parentID, TAtlasStatistics *st, Tatlas *a)
Post-processes the charts added since the last call to this function.
Definition: atlas.c:928
boolean Newton2ManifoldPlane(Tparameters *pr, double *point, double *vector, double *pInit, double *p, Tatlas *a)
Finds a point in the intersection of the manifold and a plane.
Definition: atlas.c:2290
Information of each node (chart) being visited in the A* search.
Definition: atlas.c:50
void DeleteHeap(Theap *heap)
Destructor.
Definition: heap.c:388
void Warning(const char *s)
General warning function.
Definition: error.c:116
A table of parameters.
void DeleteNewton(TNewton *n)
Releases a Newton structure.
double GetAtlasHeapElementCost(TAtlasHeapElement *he)
Gets the cost.
Definition: atlas.c:2663
void CreateFileName(char *path, char *name, char *suffix, char *ext, Tfilename *fn)
Constructor.
Definition: filename.c:22
void GetChartNeighboursFromVertices(Tparameters *pr, unsigned int *nn, unsigned int **cID1, unsigned int **cID2, Tchart *c)
Returns the identifier of neighbouring charts coincident at a vertex.
Definition: chart.c:1707
unsigned int nCores
Definition: atlas.h:300
boolean DetermineChartNeighbours(Tparameters *pr, boolean bif, unsigned int cmID, unsigned int parentID, Tatlas *a)
Determines the neighbours of a chart.
Definition: atlas.c:1582
#define CS_WD_GET_SIMP_JACOBIAN(pr, J, wcs)
Computes the Jacobian of the simplified system.
Definition: wcs.h:553
void NewNoSingularEnough(TAtlasStatistics *ast)
New not singular engoug point.
Definition: atlas.c:822
#define SAMPLING_RADIUS_REDUCTION_FACTOR
Ratio at which the sampling radius is reduced.
Definition: atlas.h:85
boolean FrontierChart(Tchart *c)
Identifies frontier charts.
Definition: chart.c:1285
void DeleteChart(Tchart *c)
Destructor.
Definition: chart.c:2014
Type defining the equations on which the atlas is defined.
Definition: wcs.h:30
boolean FindSingularPoint(Tparameters *pr, unsigned int bID, Tatlas *a)
Searches for a singular point.
Definition: atlas.c:1816
Definition of an atlas on a manifold.
void NewNonRegularPoint(TAtlasStatistics *ast)
New time we could not initialize a new chart.
Definition: atlas.c:750
#define CS_WD_GET_SYSTEM_VARS(sv, wcs)
Gets the system variables.
Definition: wcs.h:238
TAtlasBase * w
Definition: chart.h:72
char * GetFileFullName(Tfilename *fn)
Gets the file full name (paht+name+extension).
Definition: filename.c:151
boolean IntersectCharts(Tparameters *pr, unsigned int *tp, Tbox *ambient, unsigned int id1, Tchart *c1, unsigned int id2, Tchart *c2)
Intersects two local charts.
Definition: chart.c:1273
#define M_2PI
2*Pi.
Definition: defines.h:100
#define SOL_EXT
File extension for solution files.
Definition: filename.h:138
boolean AtlasGBF(Tparameters *pr, double *p, double *time, double *pl, unsigned int *ns, double ***path, Tatlas *a)
Expands the atlas to reach a given point.
Definition: atlas.c:3781
#define CT_REPRESENTATION
Representation.
Definition: parameters.h:215
unsigned int mID2
Definition: atlas.h:162
void NewLargeError(TAtlasStatistics *ast)
New time the point in the tangent space was too far form the manifold.
Definition: atlas.c:744
void SetAtlasTopology(Tparameters *pr, Tatlas *a)
Initializes the topology array in the atlas.
Definition: atlas.c:2692
CBLAS_INLINE void TMatrixVectorProduct(unsigned int r, unsigned int c, double *A, double *b, double *o)
Product of a transposed matrix and a vector.
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
void NewDecompositionError(TAtlasStatistics *ast)
New time we could not initialize a new chart.
Definition: atlas.c:762
void InitHeap(unsigned int ele_size, void(*Copy)(void *, void *), void(*Delete)(void *), boolean(*LessThan)(void *, void *, void *), void *userData, boolean hasIDs, unsigned int max_ele, Theap *heap)
Constructor.
Definition: heap.c:237
boolean ExpandibleChart(Tchart *c)
Identifies boundary charts.
Definition: chart.c:1295
A atlas on a manifold.
Definition: atlas.h:289
unsigned int AtlasMemSize(Tatlas *a)
Memory used by a given atlas.
Definition: atlas.c:3935
void NewNotInBoundary(TAtlasStatistics *ast)
New time we failed to generate a point on the boundary.
Definition: atlas.c:738
A box.
Definition: box.h:83
void PrintAtlasStatistics(Tparameters *pr, TAtlasStatistics *ast)
Print the information stored in the statistics..
Definition: atlas.c:834
CBLAS_INLINE double ColumnSquaredNorm(unsigned int k, unsigned int r, unsigned int c, double *m)
Computes the squared norm of a column of a matrix.
unsigned int nPenalized
Definition: atlas.h:180
void ChartIsFrontier(Tchart *c)
Marks a chart as a frontier chart.
Definition: chart.c:1290
void InitHessian(TJacobian *j, THessian *h)
Constructor.
Definition: hessian.c:16
#define MEM_DUP(_var, _n, _type)
Duplicates a previously allocated memory space.
Definition: defines.h:414
#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
double AtlasVolume(Tparameters *pr, boolean collisionFree, Tatlas *a)
Approximates the volume of the manifold.
Definition: atlas.c:4070
void InitSamples(unsigned int *ms, unsigned int *ns, double ***path)
Initializes a set of samples.
Definition: samples.c:665
void SaveBifurcations(FILE *f, Tatlas *a)
Saves the bifurcation information to a file.
Definition: atlas.c:2563
double cost
Definition: atlas.h:178
double * GetChartCenter(Tchart *c)
Returns the center of the chart.
Definition: chart.c:936
boolean RandomPointOnBoundary(double *t, Tchart *c)
Random point on the boundary of the chart.
Definition: chart.c:1359
void SaveAtlasGraph(char *fname, Tatlas *a)
Saves the graph structure of the atlas.
Definition: atlas.c:4091
Tchart * GetAtlasChart(unsigned int id, Tatlas *a)
Gets one of the charts of the chart.
Definition: atlas.c:4040
TAtlasBase * GetAtlasWorld(Tatlas *a)
Gets the world structure on which the atlas is defined.
Definition: atlas.c:3430
void NewNotInManifold(TAtlasStatistics *ast)
New time we could not initialize a new chart.
Definition: atlas.c:756
#define CS_WD_EVALUATE_SIMP_EQUATIONS(pr, p, r, wcs)
Evaluates the simplified set of equations.
Definition: wcs.h:176
CBLAS_INLINE void SubMatrixFromMatrix(unsigned int nr1, unsigned int nc1, double *m1, unsigned int nri, unsigned int nci, unsigned int nr, unsigned int nc, double *m)
Defines a submatrix in a matrix.
unsigned int GetAtlasHeapElementID(TAtlasHeapElement *he)
Gets the chart identifier.
Definition: atlas.c:2658
void SaveAtlas(Tparameters *pr, Tfilename *fname, Tatlas *a)
Stores the atlas information on a file.
Definition: atlas.c:3946
void DeleteBox(void *b)
Destructor.
Definition: box.c:1283
unsigned int nRadiousChange
Definition: atlas.h:131
A minimization trace.
Definition: atlas.c:32
void InitAtlas(Tparameters *pr, boolean parallel, boolean simpleChart, unsigned int k, double e, double ce, double r, TAtlasBase *w, Tatlas *a)
Constructor.
Definition: atlas.c:2716
void InitAtlasStatistics(TAtlasStatistics *ast)
Init the atlas built statistics.
Definition: atlas.c:701
#define SOL_WITH_DUMMIES_EXT
File extension for solution files with dummies.
Definition: filename.h:144
boolean SameTriangle(unsigned int *v1, unsigned int *v2)
Identifies triangles formed by the same set of vertices.
Definition: geom.c:671
unsigned int AddTrustedChart2Atlas(Tparameters *pr, double *ps, unsigned int parentID, boolean *singular, Tatlas *a)
Defines a new chart and adds it to the atlas.
Definition: atlas.c:2905
boolean OpenChart(Tchart *c)
Identifies charts not fully sorrounded by other charts.
Definition: chart.c:1303
#define CT_MAX_NEWTON_ITERATIONS
Maximum number of iterations in the Newton-Raphson function.
Definition: parameters.h:311
#define CT_E
Chart error in atlas.
Definition: parameters.h:243
unsigned int ExtractMinElement(void *e, Theap *heap)
Extracts and removes the minimal element of a heap.
Definition: heap.c:356
Hessian of a set of equations.
Definition: hessian.h:24
boolean ExtendAtlasTowardPoint(Tparameters *pr, unsigned int parentID, double *t, boolean collisionStops, TAtlasStatistics *st, Tatlas *a)
Tries to expand the atlas from a chart toward a given point.
Definition: atlas.c:1281
double Manifold2Chart(double *p, unsigned int *tp, double *t, Tchart *c)
Returns the parametrization of a point.
Definition: chart.c:1039
#define MEM_EXPAND(_var, _n, _type)
Expands a previously allocated memory space.
Definition: defines.h:404
boolean CloseCharts(Tparameters *pr, unsigned int *tp, Tchart *c1, Tchart *c2)
Identifies close local charts.
Definition: chart.c:1267
double GetParameter(unsigned int n, Tparameters *p)
Gets the value for a particular parameter.
Definition: parameters.c:93
void DefineChartsAtBifurcation(Tparameters *pr, unsigned int bID, double *v, TAtlasStatistics *ast, Tatlas *a)
Defines two related charts on a bifurcation point.
Definition: atlas.c:2432
unsigned int currentChart
Definition: atlas.h:307
A generic binary heap.
Definition: heap.h:105
void WrongCorner(unsigned int nv, Tchart *c)
Mark a vertex as wrong.
Definition: chart.c:1308
#define CS_WD_REGENERATE_SOLUTION_POINT(pr, p, r, wcs)
Compleates a solution point with the dummy variables.
Definition: wcs.h:480
void NewAtlasExtension(TAtlasStatistics *ast)
New attempt to extend the atlas.
Definition: atlas.c:726
void NewInCollision(TAtlasStatistics *ast)
New time the point was in collision.
Definition: atlas.c:774
#define CT_CUT_Y
Limit of domain of the Y dimension of 3d plots.
Definition: parameters.h:432
unsigned int ChartNumNeighbours(Tchart *c)
Number of neighbours of the chart.
Definition: chart.c:1678
Data about the atlas construction.
Definition: atlas.h:111
unsigned int InitPossiblySingularChart(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:803
unsigned int ncJ
Definition: atlas.h:314
void DeleteColor(Tcolor *c)
Destructor.
Definition: color.c:143
void Plot3dObjectWithColors(unsigned int nv, unsigned int nf, unsigned int ne, double **v, Tcolor *c, unsigned int *nvf, unsigned int **fv, Tplot3d *p)
Adds a colored polytope to the current object.
Definition: plot3d.c:334
double beta
Definition: atlas.h:179
#define ATLAS_VERBOSE
Vebosity of the atlas operations.
Definition: atlas.h:29
#define CS_WD_IN_COLLISION(f, pr, s, sPrev, wcs)
Checks if a configuration is in collision.
Definition: wcs.h:355
unsigned int InitChartWithTangent(Tparameters *pr, boolean simple, Tbox *domain, unsigned int *tp, unsigned int m, unsigned int k, double *p, double *T, double e, double eCurv, double r, TJacobian *sJ, TAtlasBase *w, Tchart *c)
Constructor.
Definition: chart.c:827
#define INF
Infinite.
Definition: defines.h:70
void PrintMatrix(FILE *f, char *label, unsigned int r, unsigned int c, double *m)
Prints a matrix.
CBLAS_INLINE void SubMatrixFromTMatrix(unsigned int nr1, unsigned int nc1, double *m1, unsigned int nri, unsigned int nci, unsigned int nr, unsigned int nc, double *m)
Defines a submatrix in a matrix.
#define RANDOMNESS
If 0 we avoid any randomness in the cuiksuite behavior.
Definition: defines.h:55
void LoadBifurcations(FILE *f, Tatlas *a)
Loads the bifurcation information from a file.
Definition: atlas.c:2533
#define CT_DELTA
Size of the steps in the path connecting two configurations.
Definition: parameters.h:282
unsigned int nGoodExtension
Definition: atlas.h:133
#define CT_N_DOF
Dimensionality of the solution space for the mechanism at hand.
Definition: parameters.h:318
Auxiliary functions to deal with sets of samples.
double * p
Definition: atlas.h:164
#define CT_DETECT_BIFURCATIONS
TRUE (or 1) if bifurcation must be detected.
Definition: parameters.h:468
unsigned int ChartNeighbourID(unsigned int n, Tchart *c)
Returns the identifier of one of the neighbours of a chart.
Definition: chart.c:1690
boolean InitAtlasFromPoint(Tparameters *pr, boolean parallel, boolean simpleChart, double *p, TAtlasBase *w, Tatlas *a)
Initializes an atlas from a given point.
Definition: atlas.c:2782
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 NewColor(double r, double g, double b, Tcolor *c)
Constructor.
Definition: color.c:14
void PlotChart(Tparameters *pr, FILE *fcost, TJacobian *sJ, unsigned int xID, unsigned int yID, unsigned int zID, Tplot3d *p3d, Tchart *c)
Plots a 3d projection of a local chart.
Definition: chart.c:1729
#define INIT_NUM_BIFURCATIONS
Initial number of bifurcations of an atlas.
Definition: atlas.h:66
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
void Plot3dObject(unsigned int nv, unsigned int nf, unsigned int ne, double **v, unsigned int *nvf, unsigned int **fv, Tplot3d *p)
Adds a polytope to the current object.
Definition: plot3d.c:276
void TriangulateAtlas(char *fname, int argc, char **arg, Tparameters *pr, FILE *fcost, unsigned int xID, unsigned int yID, unsigned int zID, Tatlas *a)
Pots the triangular mesh defined by the an atlas.
Definition: atlas.c:4387
void LoadChart(FILE *f, TAtlasBase *w, Tchart *c)
Defines a chart from the information on a file.
Definition: chart.c:1935
#define CS_WD_GENERATE_SIMP_INITIAL_BOX(pr, b, wcs)
Computes the global box for the simplified system.
Definition: wcs.h:539
void CopyAtlasHeapElement(void *he1, void *he2)
Copy constructor.
Definition: atlas.c:2650
unsigned int ms
Definition: atlas.c:37
unsigned int GetChartDegree(Tparameters *pr, double *T, TJacobian *sJ, boolean *singular, Tchart *c)
Returns the chart degree.
Definition: chart.c:988
unsigned int AddChart2Atlas(Tparameters *pr, double *ps, unsigned int parentID, boolean *singular, Tatlas *a)
Defines a new chart and adds it to the atlas.
Definition: atlas.c:2871
void ChangeParameter(unsigned int n, double v, Tparameters *p)
Sets the value for a particular parameter.
Definition: parameters.c:164
TAtlasBase * w
Definition: atlas.h:290
void CrossProduct(double *v1, double *v2, double *v3)
Computes the cross product of two 3d vectors.
Definition: geom.c:639
unsigned int nNotInBoundary
Definition: atlas.h:114
unsigned int mID1
Definition: atlas.h:160
#define INIT_NUM_CHARTS
Initial number of charts of an atlas.
Definition: atlas.h:60
unsigned int nNotInManifold
Definition: atlas.h:125
unsigned int StartNew3dObject(Tcolor *c, Tplot3d *p)
Start a composed object.
Definition: plot3d.c:157
void PrintTMatrix(FILE *f, char *label, unsigned int r, unsigned int c, double *m)
Prints a transposed matrix.
void DeleteStatistics(Tstatistics *t)
Destructor.
Definition: statistics.c:274
boolean PathInChart(Tparameters *pr, double *t, unsigned int *tp, TJacobian *sJ, unsigned int nvs, boolean *systemVars, unsigned int *ms, double *pl, unsigned int *ns, double ***path, Tchart *c)
Defines the path to a point in the chart.
Definition: chart.c:1480
#define DIVERGED
One of the possible outputs of the Newton iteration.
Definition: cuiksystem.h:111
void CostColor(double cost, double minCost, Tcolor *c)
Definees a color in function of a cost.
Definition: color.c:90
void DeleteBifurcations(Tatlas *a)
Deletes the bifurcation information.
Definition: atlas.c:2623
void FreeHessianEvaluation(double ***m, THessian *h)
Release space for the Hessian evaluation.
Definition: hessian.c:64
#define CT_CE
Chart error in atlas.
Definition: parameters.h:250
boolean SingularChart(Tchart *c)
Identify charts defined on singularities.
Definition: chart.c:983
double * GetNewtonMatrixBuffer(TNewton *n)
Buffer to store the Newton matrix.
Definition: algebra.c:1147
double heuristic
Definition: atlas.c:52
#define CS_WD_GET_SIMP_TOPOLOGY(pr, tp, wcs)
Gets the simplified variable topology.
Definition: wcs.h:450
void PlotAtlas(char *fname, int argc, char **arg, Tparameters *pr, FILE *fcost, unsigned int xID, unsigned int yID, unsigned int zID, Tatlas *a)
Pots a projection of an atlas.
Definition: atlas.c:4227
void Close3dObject(Tplot3d *p)
Closes a composed object.
Definition: plot3d.c:171
void InitNewton(unsigned int nr, unsigned int nc, TNewton *n)
Defines a Newton structure.
void BuildAtlasFromPoint(Tparameters *pr, double *p, boolean simpleChart, TAtlasBase *w, Tatlas *a)
Defines an atlas from a given point.
Definition: atlas.c:2954
unsigned int npBifurcations
Definition: atlas.h:326
unsigned int nSPMissed
Definition: atlas.h:138
unsigned int InitSingularChart(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:811
void ClosePlot3d(boolean quit, double average_x, double average_y, double average_z, Tplot3d *p)
Destructor.
Definition: plot3d.c:473
double GetAtlasErrorCurv(Tatlas *a)
Maximum curvature error for all the charts.
Definition: atlas.c:3425
unsigned int nBifurcations
Definition: atlas.h:135
#define TOPOLOGY_S
One of the possible topologies.
Definition: defines.h:139
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
void PlotBifurcations(Tparameters *pr, Tplot3d *p3d, unsigned int xID, unsigned int yID, unsigned int zID, Tatlas *a)
Plots the bifurcation information.
Definition: atlas.c:2586
CBLAS_INLINE void GetColumn(unsigned int k, unsigned int r, unsigned int c, double *m, double *v)
Gets a column from a matrix.
#define CS_WD_ORIGINAL_IN_COLLISION(pr, o, oPrev, wcs)
Checks if a configuration is in collision.
Definition: wcs.h:397
unsigned int nNoJumpBranch
Definition: atlas.h:140