cuikexplore.c

Go to the documentation of this file.

#include "cuikexplore.h"

#include "world.h"
#include "interval.h"
#include "chart.h"
#include "defines.h"
#include "jacobian.h"
#include "samples.h"
#include "error.h"
#include "algebra.h"

#include "cuikexplore_interface.h"
#include "cuikplay_support.h"

#include <gtk/gtk.h>

#include <string.h>
#include <unistd.h>
#include <time.h>

int main(int argc, char **arg)
{
  
  if (argc>1)
    {
      TAtlasBase world;
      Tworld *worldBase;
      Tparameters parameters;

      Tfilename fworld;
      Tfilename fparam;

      Tplot3d pt;

      TCuikexploreControl status;
      unsigned int i,rep;
      double epsilon;

      unsigned int k,m,nit;
      double r,e,ce;

      TJacobian J;

      GtkWidget *window_cuikexplore;
      
      Tchart chart,chartTmp;
      Tbox ambient;
      unsigned int *tp;
      boolean hasS;
      boolean collision;

      double *newPoint,*t,*initialPoint;
      
      status.fw=arg[1];

      CreateFileName(NULL,arg[1],NULL,PARAM_EXT,&fparam);
      fprintf(stderr,"Reading parameters from           : %s\n",GetFileFullName(&fparam));
      InitParametersFromFile(GetFileFullName(&fparam),&parameters);

      rep=(unsigned int)(GetParameter(CT_REPRESENTATION,&parameters));
      if (rep!=REP_JOINTS)
        ChangeParameter(CT_REPRESENTATION,(double)REP_JOINTS,&parameters);

      nit=(unsigned int)(GetParameter(CT_MAX_NEWTON_ITERATIONS,&parameters));
      if (nit==0)
        ChangeParameter(CT_MAX_NEWTON_ITERATIONS,50,&parameters);

      epsilon=GetParameter(CT_EPSILON,&parameters);
      r=GetParameter(CT_R,&parameters);
      e=GetParameter(CT_E,&parameters);
      ce=GetParameter(CT_CE,&parameters);
      k=(unsigned int)GetParameter(CT_N_DOF,&parameters);  
      status.k=k;

      /* space for parameters in conf. space */
      NEW(t,k,double);

      /* Read the equation info from world  */
      CreateFileName(NULL,arg[1],NULL,WORLD_EXT,&fworld);
      fprintf(stderr,"Reading world from                : %s\n",GetFileFullName(&fworld));
      NEW(worldBase,1,Tworld);
      InitWorldFromFile(&parameters,&fworld,worldBase);
      InitWorldCD(&parameters,1,worldBase);
      CS_WD_FROM_WORLD(worldBase,&world);

      /* check that k is lower than 'm' (num variables of the problem)? */
      m=CS_WD_GET_NUM_SYSTEM_VARS(&world);
      if (k>=m)
        Error("The mobility is larger than the number of variables?");
      status.m=m;

      /* Space for the vector of displacements */
      NEW(status.vect,m,double);
      for(i=0;i<m;i++)
        status.vect[i]=0;
      
      /* Read the start configuration */
      ReadOneSample(&parameters,arg[1],m,&status.point);
      NEW(newPoint,m,double); /* temporary space */
      NEW(initialPoint,m,double); /* for the reset */
      memcpy(initialPoint,status.point,m*sizeof(double));

      /* Initialize the chart */
      CS_WD_GENERATE_SIMP_INITIAL_BOX(&parameters,&ambient,&world);
      if (CS_WD_GET_SIMP_TOPOLOGY(&parameters,&tp,&world)!=m)
        Error("Missmatch number of variables.");

      /* Search for a non-real variable */
      hasS=FALSE;
      i=0;
      while((i<m)&&(!hasS))
        {
          hasS=(tp[i]==TOPOLOGY_S);
          i++;
        }
      if (!hasS)
        {
          free(tp);
          tp=NULL;
        }
      CS_WD_GET_SIMP_JACOBIAN(&parameters,&J,&world);

      if (InitChart(&parameters,TRUE,&ambient,tp,m,k,status.point,e,ce,r,
                    &J,&world,&chart)!=0)
        Error("Could not define a chart in the start configuration.");
      
      /* indices of the variables used as a parameters */
      NEW(status.ndx,k,unsigned int);
      if (argc>1+k)
        {
          for(i=0;i<k;i++)
            status.ndx[i]=atoi(arg[2+i]);
        }
      else
        {
          for(i=0;i<k;i++)
            status.ndx[i]=i;
        }
        
      status.changed=FALSE;
      status.end=FALSE;
      status.reset=FALSE;
      status.step=0.01;

      status.p=&parameters;
      status.w=&world;
      
      gtk_set_locale();
      gtk_init(&argc,&arg);
      
      window_cuikexplore=create_window_cuikexplore((gpointer)(&status));
      gtk_widget_show(window_cuikexplore);

      InitPlot3d(NULL,FALSE,argc,arg,&pt);
      PlotWorld(&parameters,&pt,0.0,GET_WORLD(&world));
      MoveWorldDOF(&parameters,&pt,status.point,GET_WORLD(&world));

      while(!status.end)
        {
          if (status.changed)
            {
              /* project status.vect on the current chart and generate a new chart in 
                 the resulting direction.*/
               for(i=0;i<m;i++)
                 newPoint[i]=status.point[i]+status.vect[i];
               Manifold2Chart(newPoint,tp,t,&chart);
               if (Norm(k,t)>(1e-2*status.step))
                 {
                   if (Chart2Manifold(&parameters,&J,t,tp,status.point,newPoint,&chart)<INF)
                     {
                       if (PointInBoxTopology(NULL,TRUE,m,newPoint,epsilon,tp,&ambient))
                         { 
                           CS_WD_IN_COLLISION(collision,&parameters,newPoint,status.point,&world);
                           if (!collision)
                             {
                               CopyChart(&chartTmp,&chart);
                               DeleteChart(&chart);
                               if (InitChart(&parameters,TRUE,&ambient,tp,m,k,newPoint,e,ce,r,
                                             &J,&world,&chart)==0)
                                 {
                                   memcpy(status.point,newPoint,m*sizeof(double));
                                   MoveWorldDOF(&parameters,&pt,status.point,GET_WORLD(&world));
                                 }
                               else
                                 {
                                   CopyChart(&chart,&chartTmp);
                                   fprintf(stderr,"Error defining chart. Staying in previous point.");
                                 }
                               DeleteChart(&chartTmp);
                             }
                           else
                             fprintf(stderr,"Collision! (Reduce step?)\n");
                         }
                       else
                         fprintf(stderr,"Out of the ambient space\n");
                     }
                   else
                     fprintf(stderr,"Error in parametrization. Change it?\n");
                 }
               else
                 fprintf(stderr,"Singularity. Change the parametrization\n");

               /* Reset the displacement vector (if only one step must be executed) */
               if (status.oneStep)
                 {
                   for(i=0;i<m;i++)
                     status.vect[i]=0;
                   status.changed=FALSE;
                   status.oneStep=FALSE;              
                 }
               else
                 usleep(10000);
            }
          if (status.reset)
            {
              memcpy(status.point,initialPoint,m*sizeof(double));
              DeleteChart(&chart);
              if (InitChart(&parameters,TRUE,&ambient,tp,m,k,status.point,e,ce,r,
                            &J,&world,&chart)!=0)
                Error("Can not move to the initial point?");
              MoveWorldDOF(&parameters,&pt,status.point,GET_WORLD(&world));
              status.reset=FALSE;
            }
          gtk_main_iteration_do(FALSE);
        }      

      ClosePlot3d(TRUE,0,0,0,&pt);

      DeleteChart(&chart);
      DeleteBox(&ambient);
      if (tp!=NULL)
        free(tp);
      DeleteJacobian(&J);

      free(newPoint);
      free(t);
      free(initialPoint);

      free(status.point);
      free(status.ndx);
      free(status.vect);

      CS_WD_DELETE(&world);
      DeleteParameters(&parameters);

      DeleteFileName(&fparam);
      DeleteFileName(&fworld);
    }
  else
    {
      fprintf(stdout,"  Wrong number of parameters.\n");
      fprintf(stdout,"  Use:\n");
      fprintf(stdout,"     cuikexplore <world>.world [dof_1 ... dof_k]\n");
      fprintf(stdout,"  Where:\n");
      fprintf(stdout,"     <world>:  File describing the problem.\n");
      fprintf(stdout,"     [dof_1 ... dof_k] Index of the variables to be used\n");
      fprintf(stdout,"                       as parameters.\n");
      fprintf(stdout,"  File extensions are not required\n");
    }
    
  return(EXIT_SUCCESS);
}