// Create the color-space window
colorwnd_t *colorwnd_init(rtk_app_t *app, mezz_mmap_t *mmap)
{
colorwnd_t *wnd;
wnd = malloc(sizeof(colorwnd_t));
wnd->canvas = rtk_canvas_create(app);
// Set up the canvas
rtk_canvas_movemask(wnd->canvas, 0);
rtk_canvas_size(wnd->canvas, 512, 512);
rtk_canvas_scale(wnd->canvas, 1, 1);
rtk_canvas_origin(wnd->canvas, 0, 0);
wnd->vufig = rtk_fig_create(wnd->canvas, NULL, 0);
rtk_fig_origin(wnd->vufig, -256, -256, 0);
rtk_fig_color(wnd->vufig, 0, 0, 0);
rtk_fig_rectangle(wnd->vufig, 128, 128, 0, 256, 256, 1);
wnd->yufig = rtk_fig_create(wnd->canvas, NULL, 0);
rtk_fig_origin(wnd->yufig, 0, -256, 0);
rtk_fig_color(wnd->yufig, 0, 0, 0);
rtk_fig_rectangle(wnd->yufig, 128, 128, 0, 256, 256, 1);
wnd->vyfig = rtk_fig_create(wnd->canvas, NULL, 0);
rtk_fig_origin(wnd->vyfig, -256, 0, 0);
rtk_fig_color(wnd->vyfig, 0, 0, 0);
rtk_fig_rectangle(wnd->vyfig, 128, 128, 0, 256, 256, 1);
return wnd;
}
// Initialise the sample interface
int sample_init(imagewnd_t *imagewnd, colorwnd_t *colorwnd, mezz_mmap_t *mmap)
{
int i;
double r;
sample = malloc(sizeof(sample_t));
sample->mmap = mmap;
sample->radius = 5;
// Create sample point figures on the image
for (i = 0; i < ARRAYSIZE(sample->figs); i++)
{
sample->figs[i] = rtk_fig_create(imagewnd->canvas, imagewnd->imagefig, 1);
rtk_fig_movemask(sample->figs[i], RTK_MOVE_TRANS);
rtk_fig_origin(sample->figs[i], sample->mmap->width/2, sample->mmap->height/2, 0);
rtk_fig_color(sample->figs[i], COLOR_SAMPLE);
r = sample->radius + 2;
rtk_fig_rectangle(sample->figs[i], 0, 0, 0, r, r, 0);
rtk_fig_line(sample->figs[i], -2 * r, 0, -r, 0);
rtk_fig_line(sample->figs[i], +2 * r, 0, +r, 0);
rtk_fig_line(sample->figs[i], 0, -2 * r, 0, -r);
rtk_fig_line(sample->figs[i], 0, +2 * r, 0, +r);
}
// Create figures to put on the color space
sample->vufig = rtk_fig_create(colorwnd->canvas, colorwnd->vufig, 2);
sample->yufig = rtk_fig_create(colorwnd->canvas, colorwnd->yufig, 2);
sample->vyfig = rtk_fig_create(colorwnd->canvas, colorwnd->vyfig, 2);
return 0;
}
// Draw the hitogram (kd tree)
void pf_draw_hist(pf_t *pf, rtk_fig_t *fig)
{
pf_sample_set_t *set;
set = pf->sets + pf->current_set;
rtk_fig_color(fig, 0.0, 0.0, 1.0);
pf_kdtree_draw(set->kdtree, fig);
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;
}
// Update the YUV plots
void sample_update_yuv()
{
int i;
int pixel, r, g, b;
rtk_fig_clear(sample->vufig);
rtk_fig_clear(sample->yufig);
rtk_fig_clear(sample->vyfig);
for (i = 0; i < sample->pixel_count; i++)
{
pixel = sample->pixels[i];
if (sample->mmap->depth == 16)
{
r = R_RGB16(pixel);
g = G_RGB16(pixel);
b = B_RGB16(pixel);
}
else if (sample->mmap->depth == 32)
{
r = R_RGB32(pixel);
g = G_RGB32(pixel);
b = B_RGB32(pixel);
}
rtk_fig_color(sample->vufig, r / 256.0, g / 256.0, b / 256.0);
rtk_fig_rectangle(sample->vufig, V_RGB(r, g, b), U_RGB(r, g, b), 0, 2, 2, 1);
rtk_fig_color(sample->yufig, r / 256.0, g / 256.0, b / 256.0);
rtk_fig_rectangle(sample->yufig, Y_RGB(r, g, b), U_RGB(r, g, b), 0, 2, 2, 1);
rtk_fig_color(sample->vyfig, r / 256.0, g / 256.0, b / 256.0);
rtk_fig_rectangle(sample->vyfig, V_RGB(r, g, b), Y_RGB(r, g, b), 0, 2, 2, 1);
}
}
// Update the objects
void ident_update()
{
int i, j;
double r;
char text[64];
double ax, ay, aa, bx, by, ba;
mezz_object_t *object;
rtk_fig_clear(ident->fig);
for (i = 0; i < ident->objectlist->count; i++)
{
object = ident->objectlist->objects + i;
if (!object->valid)
continue;
r = object->max_sep;
dewarp_world2image(object->px, object->py, &ax, &ay);
dewarp_world2image(object->px + r * cos(object->pa+M_PI_2),
object->py + r * sin(object->pa+M_PI_2), &bx, &by);
rtk_fig_color(ident->fig, COLOR_IDENT);
rtk_fig_arrow_ex(ident->fig, ax, ay, bx, by, 5);
snprintf(text, sizeof(text), "obj [%d] (%.2f, %.2f)\n (%.0f, %.0f)",
i, object->px, object->py,
(object->ablob.ox + object->bblob.ox) / 2.0,
(object->ablob.oy + object->bblob.oy) / 2.0);
rtk_fig_text(ident->fig, ax + 10, ay - 20, 0, text);
for (j = 0; j < 4; j++)
{
aa = object->pa + M_PI/4 + j * M_PI/2;
ba = object->pa + M_PI/4 + (j + 1) * M_PI/2;
dewarp_world2image(object->px + r * cos(aa),
object->py + r * sin(aa), &ax, &ay);
dewarp_world2image(object->px + r * cos(ba),
object->py + r * sin(ba), &bx, &by);
rtk_fig_line(ident->fig, ax, ay, bx, by);
}
}
}
// Draw the map scan
void map_draw(map_t *map)
{
double scale = map->proxy->resolution;
double color;
char val;
char* cellsDrawn=(char*)calloc(map->proxy->width*map->proxy->height,sizeof(char));
int mapWidth=map->proxy->width;
int mapHeight=map->proxy->height;
int x,y;
double ox,oy;
double rectangleWidth,rectangleHeight;
int startx, starty;
int endx,endy;
rtk_fig_show(map->fig, 1);
rtk_fig_clear(map->fig);
puts( "map draw" );
rtk_fig_color(map->fig, 0.5, 0.5, 0.5 );
rtk_fig_rectangle(map->fig, 0,0,0, mapWidth * scale, mapHeight* scale, 1 );
for( y=0; y<mapHeight; y++ )
for( x=0; x<mapWidth; x++)
{
if(cellsDrawn[x+y*mapWidth]==TRUE)
continue;
startx=x;
starty=y;
endy=mapHeight-1;
val = map->proxy->cells[ x + y * mapWidth ];
if(val==0)
continue;
while(x < mapWidth)
{
int yy = y;
while(yy+1 < mapHeight)
{
if(map->proxy->cells[x+(yy+1)*mapWidth]==val)
yy++;
else
break;
}
if( yy < endy )
endy=yy;
if(x+1<mapWidth &&
map->proxy->cells[(x+1)+y*mapWidth]==val &&
cellsDrawn[(x+1)+y*mapWidth]==FALSE)
x++;
else
break;
}
endx=x;
map_mark_cells(cellsDrawn,mapWidth,mapHeight,startx,starty,endx,endy);
rectangleWidth=(endx-startx+1)*scale;
rectangleHeight=(endy-starty+1)*scale;
ox=((startx-mapWidth/2.0f)*scale+rectangleWidth/2.0f);
oy=((starty-mapHeight/2.0f)*scale+rectangleHeight/2.0f);
color = (double)val/map->proxy->data_range; // scale to[-1,1]
color *= -1.0; //flip sign for coloring occupied to black
color = (color + 1.0)/2.0; // scale to [0,1]
rtk_fig_color(map->fig, color, color, color );
rtk_fig_rectangle(map->fig, ox, oy, 0, rectangleWidth, rectangleHeight,1);
}
free(cellsDrawn);
return;
}
