int main(int argc, char **argv)
{
rtk_app_t *app;
rtk_canvas_t *canvas;
rtk_fig_t *fig1, *fig2, *fig3;
int i;
app = rtk_app_create();
canvas = rtk_canvas_create(app);
rtk_app_start(app);
fig1 = rtk_fig_create(canvas, NULL);
rtk_fig_rectangle(fig1, 0, 0, 0, 1.0, 0.5);
rtk_fig_ellipse(fig1, 0, 0, 0, 1.0, 0.5);
rtk_fig_arrow(fig1, 0, 0, M_PI / 4, 1.0, 0.2);
fig2 = rtk_fig_create(canvas, fig1);
rtk_fig_origin(fig2, 1, 0, 0);
rtk_fig_scale(fig2, 0.5);
rtk_fig_rectangle(fig2, 0, 0, 0, 1.0, 0.5);
rtk_fig_ellipse(fig2, 0, 0, 0, 1.0, 0.5);
rtk_fig_arrow(fig2, 0, 0, M_PI / 4, 1.0, 0.2);
fig3 = rtk_fig_create(canvas, NULL);
i = 0;
while (!rtk_app_quit(app))
{
//rtk_fig_origin(fig1, 0, 0, i * 0.012);
rtk_fig_clear(fig3);
rtk_fig_origin(fig3, sin(i * 0.07), -1.0, 0);
rtk_fig_rectangle(fig3, 0, 0, i * 0.1, 0.5, 1.0);
rtk_fig_arrow(fig3, 0, 0, -i * 0.1, 0.8, 0.20);
rtk_fig_text(fig3, 0.0, 0.2, 0, "some text");
i++;
usleep(20000);
}
rtk_app_stop(app);
rtk_canvas_export(canvas, "test.fig");
//rtk_canvas_destroy(canvas);
rtk_app_destroy(app);
return 0;
}
// Draw the actarray scan
void actarray_draw(actarray_t *actarray)
{
double value;
double min, max;
double ax, ay, bx, by;
double fx, fd;
int ii;
actarray_allocate(actarray, actarray->proxy->actuators_count);
for(ii = 0; ii < actarray->proxy->actuators_count; ++ii)
{
value = actarray->proxy->actuators_data[ii].position;
min = -1;
max = 1;
if (actarray->proxy->actuators_geom && actarray->proxy->actuators_geom_count == actarray->proxy->actuators_count)
{
min = actarray->proxy->actuators_geom[ii].min;
max = actarray->proxy->actuators_geom[ii].max;
}
// now limit and scale the value to the actuator bar
if (value > max) value = max;
if (value < min) value = min;
value = 2*(value-min)/(max-min) -1;
rtk_fig_t * fig = actarray->actuator_fig[ii];
rtk_fig_show(fig, 1);
rtk_fig_clear(fig);
rtk_fig_origin(actarray->actuator_fig[ii], ARRAY_SPACING*ii +ARRAY_X_OFFSET, 0, 0);
rtk_fig_color_rgb32(fig, COLOR_ACTARRAY_DATA);
rtk_fig_line(fig, 0, -1, 0, 1);
rtk_fig_ellipse(actarray->actuator_fig[ii], 0, value, 0, 0.2, 0.2, 1);
}
}
void actarray_allocate(actarray_t *actarray, int size)
{
int ii;
if (size == actarray->fig_count)
return;
if (size < actarray->fig_count)
{
for (ii=size; ii < actarray->fig_count; ++ii)
{
rtk_fig_destroy(actarray->actuator_fig[ii]);
rtk_fig_destroy(actarray->actuator_fig_cmd[ii]);
}
}
actarray->actuator_fig = realloc(actarray->actuator_fig, size * sizeof(rtk_fig_t*));
actarray->actuator_fig_cmd = realloc(actarray->actuator_fig_cmd, size * sizeof(rtk_fig_t*));
actarray->lastvalue = realloc(actarray->lastvalue, size*sizeof(double));
if (size > actarray->fig_count)
{
for (ii=actarray->fig_count; ii < size; ++ii)
{
actarray->lastvalue[ii] = 1e10;
actarray->actuator_fig[ii] = rtk_fig_create(actarray->mainwnd->canvas, actarray->mainwnd->robot_fig, 10);
actarray->actuator_fig_cmd[ii] = rtk_fig_create(actarray->mainwnd->canvas, actarray->mainwnd->robot_fig, 11);
rtk_fig_movemask(actarray->actuator_fig_cmd[ii], RTK_MOVE_TRANS);
rtk_fig_origin(actarray->actuator_fig_cmd[ii], ARRAY_SPACING*ii+ARRAY_X_OFFSET, 0, 0);
rtk_fig_color_rgb32(actarray->actuator_fig_cmd[ii], COLOR_ACTARRAY_CMD);
rtk_fig_ellipse(actarray->actuator_fig_cmd[ii], 0, 0, 0, 0.2, 0.2, 0);
}
}
actarray->fig_count = size;
}
// Create a ptz device
ptz_t *ptz_create(mainwnd_t *mainwnd, opt_t *opt, playerc_client_t *client,
int index, const char *drivername, int subscribe)
{
char section[64];
char label[64];
ptz_t *ptz;
ptz = malloc(sizeof(ptz_t));
ptz->datatime = 0;
ptz->drivername = strdup(drivername);
ptz->proxy = playerc_ptz_create(client, index);
// Set initial device state
snprintf(section, sizeof(section), "ptz:%d", index);
if (subscribe)
{
if (playerc_ptz_subscribe(ptz->proxy, PLAYER_OPEN_MODE) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
}
// Construct the menu
snprintf(label, sizeof(label), "ptz:%d (%s)", index, ptz->drivername);
ptz->menu = rtk_menu_create_sub(mainwnd->device_menu, label);
ptz->subscribe_item = rtk_menuitem_create(ptz->menu, "Subscribe", 1);
ptz->command_item = rtk_menuitem_create(ptz->menu, "Command", 1);
// Set the initial menu state
rtk_menuitem_check(ptz->subscribe_item, ptz->proxy->info.subscribed);
// Construct figures
ptz->data_fig = rtk_fig_create(mainwnd->canvas, mainwnd->robot_fig, 10);
ptz->cmd_fig = rtk_fig_create(mainwnd->canvas, mainwnd->robot_fig, 11);
ptz->data_fig_tilt = rtk_fig_create(mainwnd->canvas, mainwnd->robot_fig, 12);
ptz->cmd_fig_tilt = rtk_fig_create(mainwnd->canvas, mainwnd->robot_fig, 13);
rtk_fig_movemask(ptz->cmd_fig, RTK_MOVE_TRANS);
rtk_fig_origin(ptz->cmd_fig, 1, 0, 0);
rtk_fig_color_rgb32(ptz->cmd_fig, COLOR_PTZ_CMD);
rtk_fig_ellipse(ptz->cmd_fig, 0, 0, 0, 0.2, 0.2, 0);
rtk_fig_movemask(ptz->cmd_fig_tilt, RTK_MOVE_TRANS);
rtk_fig_origin(ptz->cmd_fig_tilt, 0.8, 0, 0);
rtk_fig_color_rgb32(ptz->cmd_fig_tilt, COLOR_PTZ_CMD_TILT);
rtk_fig_ellipse(ptz->cmd_fig_tilt, 0, 0, 0, 0.2, 0.2, 0);
return ptz;
}
// Draw the cluster statistics
void pf_draw_cluster_stats(pf_t *pf, rtk_fig_t *fig)
{
int i;
pf_cluster_t *cluster;
pf_sample_set_t *set;
pf_vector_t mean;
pf_matrix_t cov;
pf_matrix_t r, d;
double weight, o, d1, d2;
set = pf->sets + pf->current_set;
for (i = 0; i < set->cluster_count; i++)
{
cluster = set->clusters + i;
weight = cluster->weight;
mean = cluster->mean;
cov = cluster->cov;
// Compute unitary representation S = R D R^T
pf_matrix_unitary(&r, &d, cov);
/* Debugging
printf("mean = \n");
pf_vector_fprintf(mean, stdout, "%e");
printf("cov = \n");
pf_matrix_fprintf(cov, stdout, "%e");
printf("r = \n");
pf_matrix_fprintf(r, stdout, "%e");
printf("d = \n");
pf_matrix_fprintf(d, stdout, "%e");
*/
// Compute the orientation of the error ellipse (first eigenvector)
o = atan2(r.m[1][0], r.m[0][0]);
d1 = 6 * sqrt(d.m[0][0]);
d2 = 6 * sqrt(d.m[1][1]);
if (d1 > 1e-3 && d2 > 1e-3)
{
// Draw the error ellipse
rtk_fig_ellipse(fig, mean.v[0], mean.v[1], o, d1, d2, 0);
rtk_fig_line_ex(fig, mean.v[0], mean.v[1], o, d1);
rtk_fig_line_ex(fig, mean.v[0], mean.v[1], o + M_PI / 2, d2);
}
// Draw a direction indicator
rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2], 0.50, 0.10);
rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2] + 3 * sqrt(cov.m[2][2]), 0.50, 0.10);
rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2] - 3 * sqrt(cov.m[2][2]), 0.50, 0.10);
}
return;
}
// Draw the CEP statistics
void pf_draw_cep_stats(pf_t *pf, rtk_fig_t *fig)
{
pf_vector_t mean;
double var;
pf_get_cep_stats(pf, &mean, &var);
var = sqrt(var);
rtk_fig_color(fig, 0, 0, 1);
rtk_fig_ellipse(fig, mean.v[0], mean.v[1], mean.v[2], 3 * var, 3 * var, 0);
return;
}
