Exemple #1
0
int PlotArea::do_plot(const char *commands, bool clear)
{
    // Discard all commands up to `G' command, if any
    int discard = -1;

    {
      const char *cmds = commands;
      while (*cmds != '\0')
	{
	  if (cmds[0] == 'G' && cmds[1] == '\n')
	    {
	      if (pending_plots > 0)
		pending_plots--;
	      
	      discard = (cmds - commands);
	    }
	  
	  while (*cmds != '\n' && *cmds != '\0')
	    cmds++;
	  if (*cmds != '\0')
	    cmds++;
	}
    }

    // Process commands
    const char *cmds = commands;
    if (discard >= 0)
    {
	cmds += discard;
	assert(cmds[0] == 'G');
	assert(cmds[1] == '\n');
    }

#if 0				// FIXME: Not thoroughly tested yet  -AZ
    if (discard < 0 && pending_plots > 0)
	return discard;
#endif

    while (cmds[0] != '\0')
    {
	const char *command_begin = cmds;

	// Move CMDS to the next line
	while (*cmds != '\0' && *cmds != '\n')
	    cmds++;
	if (*cmds == '\0')
	    break;		// Command is incomplete - don't do it
	assert(*cmds == '\n');
	cmds++;

	// Copy current command to a NULL-terminated string. Otherwise, 
        // sscanf() takes far too much time.
	const int len_ = cmds-command_begin-1;
	assert(len_ >= 0);
	const string command_s(command_begin,len_);

	const char *command = command_s.chars();
	switch (command[0])
	{
	case 'V':
	    if (win)
		plot_vector(command);
	    break;

	case 'M':
	    if (win)
		plot_move(command);
	    break;

	case 'T':
	    if (win)
		plot_text(command);
	    break;

	case 'J':
	    if (win)
		plot_justify(command);
	    break;

	case 'L':
	    if (win)
		plot_linetype(command);
	    break;

	case 'P':
	    if (win)
		plot_point(command);
	    break;

	case 'G':
	    plot_reset(command);
	    if (win && clear)
		plot_clear(command);
	    break;

	case 'E':
	case 'R':
	    if (win)
		plot_nop(command);
	    break;

	default:
	    plot_unknown(command);
	    break;
	}
    }

    return discard;
}
Exemple #2
0
int transform(const char * source, const char * destination, const char * output){

 char src_pts_name[256];
 char dest_pts_name[256];
 char out_param_name[256];
 int n=3;
 int m=0;
 int m2=0;
 int k,l;
 double **src_mat=NULL;
 double **dest_mat=NULL;
 double **dest_mat_T=NULL;
 double **src_mat_T=NULL;
 double **E_mat=NULL;
 double **C_mat=NULL;
 double **C_mat_interm=NULL;
 double **D_mat_interm=NULL;
 double **P_mat=NULL;
 double *D_vec=NULL;
 double *T_vec=NULL;
 double *one_vec=NULL;
 double **D_mat=NULL;
 double **Q_mat=NULL;
 double **P_mat_T=NULL;
 double **R_mat=NULL;
 double trace1=0.0;
 double trace2=0.0;
 double scal=0.0;
 double ppm=0.0;
 FILE *outfile;


   printf("\n*******************************\n");
   printf(  "*      helmparms3d v%1.2f      *\n",VERS);
   printf(  "*   (c) U. Niethammer 2011    *\n");
   printf(  "*  http://helmparms3d.sf.net  *\n");
   printf(  "*******************************\n");
   memset(src_pts_name,0,sizeof(src_pts_name));
   memset(dest_pts_name,0,sizeof(dest_pts_name));
   memset(out_param_name,0,sizeof(out_param_name));
   strcpy(src_pts_name, source);
   strcpy(dest_pts_name, destination);
   strcpy(out_param_name, output);

   m=get_m_size(src_pts_name);
   m2=get_m_size(dest_pts_name);
   if(m2!=m){
      printf("Error, number of source and destination points is not equal!\n");
   }
   else
   {
   src_mat=matrix(m,m, src_mat);
   dest_mat=matrix(m,m, dest_mat);

   read_points(src_pts_name, src_mat);
   read_points(dest_pts_name, dest_mat);


   D_vec=vector(n, D_vec);

   E_mat=matrix(m, m, E_mat);
   P_mat=matrix(m, m, P_mat);
   D_mat=matrix(m, m, D_mat);
   Q_mat=matrix(m, m, Q_mat);
   P_mat_T=matrix(m, m, P_mat_T);
   R_mat=matrix(m, m, R_mat);
   dest_mat_T=matrix(m, m, dest_mat_T);
   C_mat=matrix(m, m, C_mat); 
   C_mat_interm=matrix(m, m, C_mat_interm); 
   src_mat_T=matrix(m, m, src_mat_T);
   D_mat_interm=matrix(m, m, D_mat_interm);


   transpose_matrix(m, m, dest_mat, dest_mat_T);
   if(debug)printf("%s_T:\n",dest_pts_name);
   if(debug)plot_matrix(stdout,  n, m, dest_mat_T);


   for(k=0;k<m;k++){
      for(l=0;l<m;l++){
         if(k!=l){
            E_mat[k][l]=-1.0/(double)m;
         }
         else{
            E_mat[k][l]=1.0-1.0/(double)m;
         }
      }
   }
   if(debug)printf("E:\n");
   if(debug)plot_matrix(stdout,  m, m, E_mat);



   if(debug)printf("dest_mat_T:\n");
   if(debug)plot_matrix(stdout,  n, m, dest_mat_T);

   matmult(dest_mat_T, m, m, E_mat, m, m,  C_mat_interm, m, n);
   if(debug)printf("C_interm:\n");
   if(debug)plot_matrix(stdout,  n, m, C_mat_interm);


   matmult(C_mat_interm, n, m, src_mat, m, n,  C_mat, n, n);
   if(debug)printf("C:\n");
   if(debug)plot_matrix(stdout,  n, n, C_mat);

   copy_matrix(n,n,C_mat,P_mat);
   if(debug)printf("P:\n");
   if(debug)plot_matrix(stdout,  n, n, P_mat);
   //Given matrix C[m][n], m>=n, using svd decomposition C = P D Q' to get P[m][n], diag D[n] and Q[n][n].
   svd(n, n, C_mat, P_mat, D_vec, Q_mat);
   transpose_matrix(n, n, P_mat, P_mat_T);

   if(debug)printf("P\n");
   if(debug)plot_matrix(stdout,  n, n, P_mat);
   if(debug)printf("P_T\n");
   if(debug)plot_matrix(stdout,  n, n, P_mat_T);


   if(debug)printf("D_vec\n");
   if(debug)plot_vector(stdout,  n, D_vec);
   for(k=0;k<n;k++){
      for(l=0;l<n;l++){
         D_mat[k][l]=0.0;
         D_mat[l][l]=D_vec[l];

      }
   }
   if(debug)printf("D\n");
   if(debug)plot_matrix(stdout,  n, n, D_mat);

   matmult(Q_mat, n, n, P_mat_T, n, n,  R_mat, n, n);
   if(debug)printf("R_trans:\n");
   if(debug)plot_matrix(stdout, n, n, R_mat);

   matmult(C_mat, m, n, R_mat, n, m,  C_mat_interm, m, n);
   if(debug)printf("C_interm:\n");
   if(debug)plot_matrix(stdout,  n, n, C_mat_interm);
   trace1=trace(n,n,C_mat_interm);
   if(debug)printf("\ntra=%lf\n\n",trace1);



   transpose_matrix(m, m, src_mat, src_mat_T);
   if(debug)printf("%s_T:\n",src_pts_name);
   if(debug)plot_matrix(stdout,  n, m, src_mat_T);


   init_matrix(m,m,C_mat);
   init_matrix(m,m,C_mat_interm);
   matmult(src_mat_T, m, m, E_mat, m, m,  C_mat_interm, n, n);
   if(debug)printf("C_interm:\n");
   if(debug)plot_matrix(stdout,  n, m, C_mat_interm);
   matmult(C_mat_interm, n, m, src_mat, m, n,  C_mat, n, n);
   if(debug)printf("C:\n");
   if(debug)plot_matrix(stdout,  n, n, C_mat);
   trace2=trace(n,n,C_mat);
   if(debug)printf("\ntra=%lf\n\n",trace2);

   scal=trace1/trace2;
   ppm=scal-1.0;
   if(debug)printf("\nscal = %10.10lf\nscal = %10.10lf ppm\n\n",scal, ppm);


   init_matrix(m,m,C_mat);
   init_matrix(m,m,C_mat_interm);

   matmult(src_mat, m, n, R_mat, n,m,  D_mat_interm, m, n);
   if(debug)printf("C_mat_interm:\n");
   if(debug)plot_matrix(stdout,  m, n, D_mat_interm);

   scal_matrix(m, n, scal, D_mat_interm, C_mat_interm);
   if(debug)printf("C_mat_interm:\n");
   if(debug)plot_matrix(stdout,  m, n, C_mat_interm);

   subtract_matrix(m, n, dest_mat, C_mat_interm, D_mat_interm);
   if(debug)plot_matrix(stdout,  m, n, D_mat_interm);
   scal_matrix(m, n, 1.0/m, D_mat_interm, C_mat_interm);
   if(debug)plot_matrix(stdout,  m, n, C_mat_interm);
   init_matrix(m,m,src_mat_T);
   transpose_matrix(m, m, C_mat_interm, src_mat_T);
   if(debug)plot_matrix(stdout,  n, m, src_mat_T);

   T_vec=vector(m, T_vec);
   one_vec=vector(m, one_vec);
   for(k=0;k<m;k++){
      one_vec[k]=1.0;
   }
   matrix_multiply(n, m, src_mat_T, one_vec, T_vec);
   if(debug)printf("T:\n");
   if(debug)plot_vector(stdout, 3, T_vec);

   outfile = fopen(out_param_name, "w");
   if(outfile == NULL){
      printf("Error writing %s\r\n",out_param_name);
      exit(-1);
   }
   init_matrix(m,m,src_mat_T);
   transpose_matrix(m, m, R_mat, src_mat_T);
   plot_matrix(outfile, n, n, src_mat_T);
   printf("R =\n");fflush(stdout);
   plot_matrix(stdout, n, n, src_mat_T);
   printf("\n");fflush(stdout);
   plot_vector(outfile, 3, T_vec);
   printf("T =\n");fflush(stdout);
   plot_vector(stdout, 3, T_vec);
   printf("\n");fflush(stdout);
   fprintf(outfile, "%10.10lf\n", scal);
   printf("s = %10.10lf (= %10.10lf ppm)\n\n",scal, ppm);fflush(stdout);
   fclose(outfile);

   freevector(D_vec);
   freevector(T_vec);
   freevector(one_vec);

   freematrix(m, src_mat);
   freematrix(m, dest_mat);
   freematrix(m, E_mat);
   freematrix(m, P_mat);
   freematrix(m, D_mat);
   freematrix(m, Q_mat);
   freematrix(m, P_mat_T);
   freematrix(m, R_mat);
   freematrix(m, dest_mat_T);
   freematrix(m, C_mat); 
   freematrix(m, C_mat_interm); 
   freematrix(m, src_mat_T);
   freematrix(m, D_mat_interm);
   printf("\n...done\n");
   }
}
Exemple #3
0
void viz_update(void) {
    SDL_Rect rect;
    object_list *l;
    object *o;
    int i;
    object *last_object;
    object *last_point;
    
    
    // clear screen
    set_color(1, 1, 1);
    SDL_FillRect(screen, NULL, current_color);
    
    
    
    
    // draw known objects
    for (l = static_objects; l; l = l->next) {
        o = l->data;
        switch (o->kind) {
            case 'b':
                set_color(.4, .4, .4);
                break;
            case 'c':
                set_color(.7, .4, .3);
                break;
            case 'h':
                set_color(0, 1, 0);
                break;
            default:
                set_color(1, 0, 1);
                break;
        }
        plot_point(o->x, o->y, o->a);

    }
    for (l = martians; l; l = l->next) {
        set_color(1, 0, 0);
        o = l->data;
        plot_point(o->x, o->y, 3);
        plot_vector(o->x, o->y, o->a, o->b);
    }
    
    last_point = NULL;
    last_object = NULL;
    for (l = viz_checked_points; l; l = l->next) {
        o = l->data;
        
        if (last_object == NULL) {
            last_object = o;
        }
        
        if (last_point == NULL) {
            last_point = o;
            i = 0;
        }
        
        if (last_object->kind != viz_decided_path) {
            set_color(.5, .5, 1);
        } else {
            set_color(0, 0, 1);
        }
        
        if (last_object->kind != o->kind) {
            plot_line(last_point->x, last_point->y, last_object->x, last_object->y);
            last_point = NULL;
            i = 0;
        }
        
        if (i > 5) {
            plot_line(last_point->x, last_point->y, o->x, o->y);
            last_point = o;
            i = 0;
        }
        
        last_object = o;
        
        i++;
    }
    if (last_point && last_object) {
        plot_line(last_point->x, last_point->y, last_object->x, last_object->y);
    }
    
    // draw destination
    //set_color(.5, .5, 1);
    //plot_point(destination_x, destination_y, 0);
    
    // draw rover
    set_color(0, .5, 0);
    plot_point(last_telemetry.vehicle_x, last_telemetry.vehicle_y, 3);
    
    // draw rover's vector
    plot_vector(last_telemetry.vehicle_x, last_telemetry.vehicle_y, last_telemetry.vehicle_dir, last_telemetry.vehicle_speed);
    
    SDL_Flip(screen);
    
    //fprintf(stderr, "Done\n");
}