void vtAgeTracks(struct lnMinList *now,
struct lnMinList *old,
struct lnMinList *pool)
{
struct vision_track *vt;
assert(now != NULL);
assert(old != NULL);
while ((vt = (struct vision_track *)lnRemHead(old)) != NULL) {
if (vt->vt_age < MAX_TRACK_AGE) {
float distance = MERGE_TOLERANCE;
if (vtFindMin(vt,
(struct vision_track *)now->lh_Head,
&distance) == NULL) {
vt->vt_age += 1;
lnAddHead(now, &vt->vt_link);
vt = NULL;
}
}
if (vt != NULL)
lnAddHead(pool, &vt->vt_link);
}
}
void vtMatch(struct lnMinList *pool,
struct lnMinList *prev,
struct lnMinList *now)
{
struct vision_track *vt;
assert(pool != NULL);
assert(prev != NULL);
assert(now != NULL);
/*
* Walk through the tracks in the current frame trying to find tracks in
* the previous frame, within some threshold.
*/
vt = (struct vision_track *)now->lh_Head;
while (vt->vt_link.ln_Succ != NULL) {
float distance = MERGE_TOLERANCE, dummy = MERGE_TOLERANCE;
struct vision_track *vt_prev;
void dump_vision_list(struct lnMinList *list);
/*
* Check both ways, the current track is the close to one in the
* previous frame, and the track from the previous frame is closest to
* the one in the current frame.
*/
if (((vt_prev = vtFindMin(vt,
(struct vision_track *)prev->lh_Head,
&distance)) != NULL) &&
(vtFindMin(vt_prev,
(struct vision_track *)now->lh_Head,
&dummy) == vt)) {
vt->vt_userdata = vt_prev->vt_userdata;
vt->ma = vt_prev->ma;
lnRemove(&vt_prev->vt_link);
lnAddHead(pool, &vt_prev->vt_link);
}
else {
#if 0
info("no match %.2f %.2f\n", vt->vt_position.x, vt->vt_position.y);
dump_vision_list(now);
info("==\n");
dump_vision_list(prev);
#endif
}
vt = (struct vision_track *)vt->vt_link.ln_Succ;
}
}
int mypcap_read(pcap_t *pd, void *user)
{
static struct lnMinNode *node = NULL;
int idx, cc, retval = 0;
char *bp, *ep;
pthread_mutex_lock(&mypcap_data.mpd_mutex);
if (node != NULL) {
lnAddHead(&mypcap_data.mpd_freelist, node);
}
while ((node = lnRemHead(&mypcap_data.mpd_buflist)) == NULL) {
pthread_cond_wait(&mypcap_data.mpd_cond,
&mypcap_data.mpd_mutex);
}
pthread_mutex_unlock(&mypcap_data.mpd_mutex);
idx = node - mypcap_data.mpd_nodes;
bp = mypcap_data.mpd_buffer[idx];
cc = mypcap_data.mpd_buflen[idx];
/*
* Loop through each packet.
*/
#define bhp ((struct bpf_hdr *)bp)
ep = bp + cc;
while (bp < ep) {
register int caplen, hdrlen;
caplen = bhp->bh_caplen;
hdrlen = bhp->bh_hdrlen;
/*
* XXX A bpf_hdr matches a pcap_pkthdr.
*/
(*mypcap_data.mpd_callback)(user,
(struct pcap_pkthdr*)bp,
bp + hdrlen);
bp += BPF_WORDALIGN(caplen + hdrlen);
}
#undef bhp
return retval;
}
