// Move the actarray
void actarray_move(actarray_t *actarray)
{
double ox, oy, oa, min, max;
double value;
int ii;
for(ii = 0; ii < actarray->fig_count; ++ii)
{
rtk_fig_get_origin(actarray->actuator_fig_cmd[ii], &ox, &oy, &oa);
value = oy;
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
value = ((oy+1)/2)*(max-min)+min;
if (value > max) value = max;
if (value < min) value = min;
if (actarray->lastvalue[ii] != value)
{
if (playerc_actarray_position_cmd(actarray->proxy, ii, value) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
actarray->lastvalue[ii] = value;
}
}
}
// Move the ptz
void ptz_move(ptz_t *ptz)
{
double ox, oy, oa, oxt, oyt, oat;
double pan, tilt, zoom, speed;
rtk_fig_get_origin(ptz->cmd_fig, &ox, &oy, &oa);
rtk_fig_get_origin(ptz->cmd_fig_tilt, &oxt, &oyt, &oat);
pan = atan2(oy, ox);
tilt = atan2(oyt,oxt);
zoom = 2 * atan2(0.5, sqrt(ox * ox + oy * oy));
speed = sqrt(oy*oy + ox*ox);
if (playerc_ptz_set_ws(ptz->proxy, pan, tilt, zoom,speed,0) != 0)
PRINT_ERR1("libplayerc error: %s", playerc_error_str());
}
// Collect samples
void sample_collect()
{
int i;
int index, pixel;
int x, y, dx, dy;
double ox, oy, oa;
sample->pixel_count = 0;
for (i = 0; i < ARRAYSIZE(sample->figs); i++)
{
// Get the current position of the sample
rtk_fig_get_origin(sample->figs[i], &ox, &oy, &oa);
x = (int) ox; y = (int) oy;
// Extract the pixels around the sample
for (dy = -sample->radius; dy <= sample->radius; dy++)
{
for (dx = -sample->radius; dx <= sample->radius; dx++)
{
if (x + dx < 0 || x + dx >= sample->mmap->width)
continue;
if (y + dy < 0 || y + dy >= sample->mmap->height)
continue;
index = (x + dx) + (y + dy) * sample->mmap->width;
if (sample->mmap->depth == 16)
pixel = ((uint16_t*) sample->mmap->image)[index];
else if (sample->mmap->depth == 32)
pixel = ((uint32_t*) sample->mmap->image)[index];
assert(sample->pixel_count < ARRAYSIZE(sample->pixels));
sample->pixels[sample->pixel_count] = pixel;
sample->pixel_count++;
}
}
}
}
