// 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 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);
}
}
// Draw the ptz scan
void ptz_draw(ptz_t *ptz)
{
double ox, oy, d;
double ax, ay, bx, by;
double fx, fd;
// Camera field of view in x-direction (radians)
fx = ptz->proxy->zoom;
fd = 0.5 / tan(fx/2);
rtk_fig_show(ptz->data_fig, 1);
rtk_fig_clear(ptz->data_fig);
rtk_fig_show(ptz->data_fig_tilt, 1);
rtk_fig_clear(ptz->data_fig_tilt);
rtk_fig_color_rgb32(ptz->data_fig, COLOR_PTZ_DATA);
ox = 100 * cos(ptz->proxy->pan);
oy = 100 * sin(ptz->proxy->pan);
rtk_fig_line(ptz->data_fig, 0, 0, ox, oy);
ox = 100 * cos(ptz->proxy->pan + fx / 2);
oy = 100 * sin(ptz->proxy->pan + fx / 2);
rtk_fig_line(ptz->data_fig, 0, 0, ox, oy);
ox = 100 * cos(ptz->proxy->pan - fx / 2);
oy = 100 * sin(ptz->proxy->pan - fx / 2);
rtk_fig_line(ptz->data_fig, 0, 0, ox, oy);
// Draw in the zoom bar (2 m in length)
d = sqrt(fd * fd + 0.5 * 0.5);
ax = d * cos(ptz->proxy->pan + fx / 2);
ay = d * sin(ptz->proxy->pan + fx / 2);
bx = d * cos(ptz->proxy->pan - fx / 2);
by = d * sin(ptz->proxy->pan - fx / 2);
rtk_fig_line(ptz->data_fig, ax, ay, bx, by);
rtk_fig_color_rgb32(ptz->data_fig_tilt, COLOR_PTZ_DATA_TILT);
ox = 100 * cos(ptz->proxy->tilt);
oy = 100 * sin(ptz->proxy->tilt);
rtk_fig_line(ptz->data_fig_tilt, 0, 0, ox, oy);
}
// 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 ranger scan
void ranger_draw(ranger_t *ranger)
{
int ii = 0, jj = 0;
int point_count;
double point1[2], point2[2];
double (*points)[2];
double current_angle = 0.0f, temp = 0.0f;
unsigned int ranges_per_sensor = 0;
// Drawing type depends on the selected sensor type
// Singular sensors (e.g. sonar sensors):
// Draw a cone for the first range scan of each sensor
// Non-singular sensors (e.g. laser scanner):
// Draw the edge of the scan and empty space
// Calculate the number of ranges per sensor
if (ranger->proxy->sensor_count == 0)
ranges_per_sensor = ranger->proxy->ranges_count;
else
ranges_per_sensor = ranger->proxy->ranges_count / ranger->proxy->sensor_count;
if (rtk_menuitem_ischecked(ranger->device_item))
{
// Draw sonar-like
points = calloc(3, sizeof(double)*2);
temp = 20.0f * M_PI / 180.0f / 2.0f;
for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
{
rtk_fig_show(ranger->scan_fig[ii], 1);
rtk_fig_clear(ranger->scan_fig[ii]);
rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_SONAR_SCAN);
// Draw a cone for the first range for each sensor
// Assume the range is straight ahead (ignore min_angle and resolution properties)
points[0][0] = 0.0f;
points[0][1] = 0.0f;
points[1][0] = ranger->proxy->ranges[ii * ranges_per_sensor] * cos(-temp);
points[1][1] = ranger->proxy->ranges[ii * ranges_per_sensor] * sin(-temp);
points[2][0] = ranger->proxy->ranges[ii * ranges_per_sensor] * cos(temp);
points[2][1] = ranger->proxy->ranges[ii * ranges_per_sensor] * sin(temp);
rtk_fig_polygon(ranger->scan_fig[ii], 0, 0, 0, 3, points, 1);
// Draw the sensor itself
rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER);
rtk_fig_rectangle(ranger->scan_fig[ii], 0, 0, 0, ranger->proxy->sensor_sizes[ii].sw, ranger->proxy->sensor_sizes[ii].sl, 0);
}
free(points);
points=NULL;
}
else
{
// Draw laser-like
if (rtk_menuitem_ischecked(ranger->style_item))
{
// Draw each sensor in turn
points = calloc(ranger->proxy->sensor_count, sizeof(double)*2);
for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
{
rtk_fig_show(ranger->scan_fig[ii], 1);
rtk_fig_clear(ranger->scan_fig[ii]);
// Draw empty space
points[0][0] = ranger->proxy->sensor_poses[ii].px;
points[0][1] = ranger->proxy->sensor_poses[ii].py;
point_count = 1;
current_angle = ranger->start_angle;
// Loop over the ranges
for (jj = ii * ranges_per_sensor; jj < (ii + 1) * ranges_per_sensor; jj++)
{
range_to_point(ranger, jj, ii, current_angle, points[point_count]);
// Move round to the angle of the next range
current_angle += ranger->resolution;
point_count++;
}
rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER_EMP);
rtk_fig_polygon(ranger->scan_fig[ii], 0, 0, 0, point_count, points, 1);
// Draw occupied space
rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER_OCC);
current_angle = ranger->start_angle;
for (jj = ii * ranges_per_sensor; jj < (ii + 1) * ranges_per_sensor; jj++)
{
range_to_point(ranger, jj, ii, current_angle - ranger->resolution, point1);
range_to_point(ranger, jj, ii, current_angle + ranger->resolution, point2);
rtk_fig_line(ranger->scan_fig[ii], point1[0], point1[1], point2[0], point2[1]);
current_angle += ranger->resolution;
}
}
free(points);
points = NULL;
}
else
{
// Draw a range scan for each individual sensor in the device
for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
{
rtk_fig_show(ranger->scan_fig[ii], 1);
rtk_fig_clear(ranger->scan_fig[ii]);
rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER_OCC);
current_angle = ranger->start_angle;
// Get the first point
range_to_point(ranger, ii * ranges_per_sensor, ii, ranger->start_angle, point1);
// Loop over the rest of the ranges
for (jj = ii * ranges_per_sensor + 1; jj < (ii + 1) * ranges_per_sensor; jj++)
{
range_to_point(ranger, jj, ii, current_angle, point2);
// Draw a line from point 1 (previous point) to point 2 (current point)
rtk_fig_line(ranger->scan_fig[ii], point1[0], point1[1], point2[0], point2[1]);
point1[0] = point2[0];
point1[1] = point2[1];
// Move round to the angle of the next range
current_angle += ranger->resolution;
}
}
}
// For each sensor...
for (ii = 0; ii < ranger->proxy->sensor_count; ii++)
{
if (rtk_menuitem_ischecked(ranger->intns_item))
{
// Draw an intensity scan
if (ranger->proxy->intensities_count > 0)
{
current_angle = ranger->start_angle;
for (jj = ii * ranges_per_sensor; jj < (ii + 1) * ranges_per_sensor; jj++)
{
if (ranger->proxy->intensities[0] != 0)
{
range_to_point(ranger, jj, ii, current_angle, point1);
rtk_fig_rectangle(ranger->scan_fig[ii], point1[0], point1[1], 0, 0.05, 0.05, 1);
}
current_angle += ranger->resolution;
}
}
}
// Draw the sensor itself
rtk_fig_color_rgb32(ranger->scan_fig[ii], COLOR_LASER);
rtk_fig_rectangle(ranger->scan_fig[ii], 0, 0, 0, ranger->proxy->sensor_sizes[ii].sw, ranger->proxy->sensor_sizes[ii].sl, 0);
}
}
}
// Draw the laser scan
void laser_draw(laser_t *laser)
{
int i;
int style;
double ax, ay, bx, by;
double r, b, res;
int point_count;
double (*points)[2];
rtk_fig_show(laser->scan_fig, 1);
rtk_fig_clear(laser->scan_fig);
if (!rtk_menuitem_ischecked(laser->style_item))
{
rtk_fig_color_rgb32(laser->scan_fig, COLOR_LASER_OCC);
// Draw in the range scan
for (i = 0; i < laser->proxy->scan_count; i++)
{
bx = laser->proxy->point[i].px;
by = laser->proxy->point[i].py;
if (i == 0)
{
ax = bx;
ay = by;
}
rtk_fig_line(laser->scan_fig, ax, ay, bx, by);
ax = bx;
ay = by;
}
}
else
{
res = laser->proxy->scan_res / 2;
// Draw in the range scan (empty space)
points = calloc(laser->proxy->scan_count,sizeof(double)*2);
for (i = 0; i < laser->proxy->scan_count; i++)
{
r = laser->proxy->scan[i][0];
b = laser->proxy->scan[i][1];
points[i][0] = r * cos(b - res);
points[i][1] = r * sin(b - res);
}
rtk_fig_color_rgb32(laser->scan_fig, COLOR_LASER_EMP);
rtk_fig_polygon(laser->scan_fig, 0, 0, 0, laser->proxy->scan_count, points, 1);
free(points);
points = NULL;
// Draw in the range scan (occupied space)
rtk_fig_color_rgb32(laser->scan_fig, COLOR_LASER_OCC);
for (i = 0; i < laser->proxy->scan_count; i++)
{
r = laser->proxy->scan[i][0];
b = laser->proxy->scan[i][1];
ax = r * cos(b - res);
ay = r * sin(b - res);
bx = r * cos(b + res);
by = r * sin(b + res);
rtk_fig_line(laser->scan_fig, ax, ay, bx, by);
}
}
// Draw in the intensity scan
for (i = 0; i < laser->proxy->scan_count; i++)
{
if (laser->proxy->intensity[i] == 0)
continue;
ax = laser->proxy->point[i].px;
ay = laser->proxy->point[i].py;
rtk_fig_rectangle(laser->scan_fig, ax, ay, 0, 0.05, 0.05, 1);
}
// Draw in the laser itself
rtk_fig_color_rgb32(laser->scan_fig, COLOR_LASER);
rtk_fig_rectangle(laser->scan_fig, 0, 0, 0,
laser->proxy->size[0], laser->proxy->size[1], 0);
return;
}
