/**
* Extract tracks from a frame that are near the given track. Used when
* finding tracks from different cameras that need to be merged.
*
* @param list_out The list of tracks found to be within the distance
* tolerance.
* @param vt The track to compare other tracks against.
* @param curr The track in a list where the search should start. The function
* will continue the search until it reaches the end of the list.
* @param tolerance The maximum distance that other tracks have to be within
* before they are considered "near."
* @return The number of tracks in "list_out."
*/
static int vtExtractNear(struct lnMinList *list_out,
struct vision_track *vt,
struct vision_track *curr,
float tolerance)
{
int retval = 0;
assert(list_out != NULL);
assert(vt != NULL);
assert(tolerance > 0.0);
if (curr == NULL)
return retval;
while (curr->vt_link.ln_Succ != NULL) {
struct vision_track *vt_next;
float distance;
vt_next = (struct vision_track *)curr->vt_link.ln_Succ;
distance = hypotf(vt->vt_position.x - curr->vt_position.x,
vt->vt_position.y - curr->vt_position.y);
if (distance <= tolerance) {
lnRemove(&curr->vt_link);
lnAddTail(list_out, &curr->vt_link);
retval += 1;
}
curr = vt_next;
}
assert(lnEmptyList(list_out) || (retval > 0));
return retval;
}
struct robot_object *roDequeueRobot(ro_status_t old_status,
ro_status_t new_status)
{
struct robot_object *retval = NULL;
assert(old_status > ROS_MIN);
assert(old_status < ROS_MAX);
assert(new_status > ROS_MIN);
assert(new_status < ROS_MAX);
if (!lnEmptyList(&ro_data.rd_lists[old_status])) {
retval = (struct robot_object *)ro_data.rd_lists[old_status].lh_Head;
roMoveRobot(retval, new_status);
}
return retval;
}
