cuikplotatlasrrt.c

Go to the documentation of this file.

#include "world.h"
#include "parameters.h"

#include "defines.h"
#include "error.h"
#include "filename.h"
#include "atlasrrt.h"

#include <stdlib.h>

int main(int argc, char **arg)
{
  TAtlasBase world;        /* The set of mechanism and obstacles.  */
  Tparameters parameters;  /* Parameters used in the Cuik process. */
  Tatlasrrt atlasRRT;      /* The atlas to plot. */

  unsigned int nx,ny,nz;
  unsigned int xID,yID,zID;
  FILE *f;
  unsigned int i,s,nv,nvs;  
  boolean *systemVars;

  Tfilename fparam;
  Tfilename fpath;  
  
  if (argc==5)
    { 
      /*Init parameters*/
      CreateFileName(NULL,arg[1],NULL,PARAM_EXT,&fparam);
      InitParametersFromFile(GetFileFullName(&fparam),&parameters);
      fprintf(stderr,"Reading parameters from           : %s\n",GetFileFullName(&fparam));

      /*Read the world from file*/
      CS_WD_INIT(&parameters,arg[1],&world);
      nv=CS_WD_GET_SYSTEM_VARS(&systemVars,&world);

      /* Read the Atlas, the RRT, and information linking them */
      LoadAtlasRRT(&parameters,arg[1],&world,&atlasRRT);
      
      nx=atoi(arg[2]);
      ny=atoi(arg[3]);
      nz=atoi(arg[4]);
          
      fprintf(stderr,"Projecting on variables           : ");
      PRINT_VARIABLE_NAME(stderr,CS_WD_GET_SYSTEM_VAR_NAME(nx,&world));fprintf(stderr,",");
      PRINT_VARIABLE_NAME(stderr,CS_WD_GET_SYSTEM_VAR_NAME(ny,&world));fprintf(stderr,",");
      PRINT_VARIABLE_NAME(stderr,CS_WD_GET_SYSTEM_VAR_NAME(nz,&world));fprintf(stderr,"\n");
      
      nvs=0;
      s=0;
      for(i=0;i<nv;i++)
        {
          if (systemVars[i])
            {
              /* Transform index from system variables only to
                 all variables (in the original system in both cases!!) */
              if (nx==nvs) {xID=i; s++;}
              if (ny==nvs) {yID=i; s++;}
              if (nz==nvs) {zID=i; s++;}
              nvs++;
            }
        }

      if (s!=3)
        Error("Projection dimensions are beyond the ambient space dimensionality");

      /* Plot the atlas, the RRT and the information linking them */
      PlotAtlasRRT(arg[1],argc,arg,&parameters,xID,yID,zID,&atlasRRT);
      
      CreateFileName(NULL,arg[1],"_path",SOL_EXT,&fpath);
      
      f=fopen(GetFileFullName(&fpath),"r");
      if (f)
        {
          /*If the path was actually saved*/
          unsigned int j,ns;
          double *x,*y,*z;
          double *step;
          Tfilename fpathplot;
          Tcolor red;
          Tplot3d p3d;

          NEW(step,nvs,double);

          fscanf(f,"%u",&ns);
          
          NEW(x,ns,double);
          NEW(y,ns,double);
          NEW(z,ns,double);

          for(i=0;i<ns;i++)
            {
              for(j=0;j<nvs;j++)
                fscanf(f,"%lf",&(step[j]));
              
              /* For the paths we only store system variables */
              x[i]=step[nx];
              y[i]=step[ny];
              z[i]=step[nz];
            }
          fclose(f);

          CreateFileName(NULL,arg[1],"_path",PLOT3D_EXT,&fpathplot);
          fprintf(stderr,"Generating path plot to           : %s\n",GetFileFullName(&fpathplot));

          NewColor(1,0,0,&red);
              
          InitPlot3d(GetFileFullName(&fpathplot),TRUE,argc,arg,&p3d);
          StartNew3dObject(&red,&p3d);
            PlotVect3d(ns,x,y,z,&p3d);
          Close3dObject(&p3d);
          ClosePlot3d(FALSE,0,0,0,&p3d);
          
          DeleteColor(&red);

          DeleteFileName(&fpathplot);
          
          free(step);
          free(x);
          free(y);
          free(z);
        }      

      DeleteParameters(&parameters);

      free(systemVars);

      CS_WD_DELETE(&world);

      DeleteAtlasRRT(&atlasRRT);

      DeleteFileName(&fpath);
      DeleteFileName(&fparam);
    }
 else
   {
     fprintf(stderr,"  Wrong number of parameters.\n");
     fprintf(stderr,"  Use:\n");   
     fprintf(stderr,"      cuikplotatlasrrt <problem filename> <xID> <yID> <zID>\n");
     fprintf(stderr,"  where <problem filename> is the atlas to plot.\n");
     fprintf(stderr,"        <xID> <yID> <zID> are the 3 projection dimensions.\n");
     fprintf(stderr,"          (indices of system variables as those in the _path.sol file from 0)\n");
     
   }
  return(EXIT_SUCCESS);
}