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;
}
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");
}
}
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");
}