cuikpathlength.c

Go to the documentation of this file.

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

#include "defines.h"
#include "error.h"
#include "filename.h"
#include "atlas.h"
#include "samples.h"
#include "random.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. */

  Tfilename fparam;
  Tfilename fpath;

  unsigned int *tp;        /* topology */
  
  /* input path */
  unsigned int nvs,ns;
  double **path;
  boolean *sv;  /* variables to use in the path length computation */
  
  if (argc>=2)
    {
      /*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);

      CS_WD_GET_TOPOLOGY(&parameters,&tp,&world);

      if (argc>2)
        CreateFileName(NULL,arg[2],NULL,SOL_EXT,&fpath);
      else
        CreateFileName(NULL,arg[1],"_path",SOL_EXT,&fpath);

      
      if ((!KINEMATIC_SUBSPACE)||(ON_CUIKSYSTEM(&world)))
        sv=NULL;
      else
        WorldSimpKinematicVars(&parameters,&sv,GET_WORLD(&world)); /* Path length on kinematic variables only */
      
      if (LoadSamples(&fpath,&nvs,&ns,&path))
        {
          fprintf(stderr,"Path length                       : %g\n",PathLength(tp,sv,nvs,ns,path));
          DeleteSamples(ns,path);
        }
      else
        Error("Could not read the input path file");

      free(tp);
      if (sv!=NULL)
        free(sv);
      
      DeleteParameters(&parameters);
      CS_WD_DELETE(&world);

      DeleteFileName(&fparam);
      DeleteFileName(&fpath);
    }
 else
   {
     fprintf(stderr,"  Wrong number of parameters.\n");
     fprintf(stderr,"  Use:\n");   
     fprintf(stderr,"      cuikpathlength <base filename> [<path file>]\n");
     fprintf(stderr,"  where <base filename> is used for the parameters, world, and path files.\n");
     fprintf(stderr,"        <path file> [optional] is the file with the path.\n");
   }
  return(EXIT_SUCCESS);
}