void track_agents(struct RoboAI *ai, struct blob *blobs)
{
////////////////////////////////////////////////////////////////////////
// ** DO NOT CHANGE THIS FUNCTION **
// This function does the tracking of each agent in the field. It looks
// for blobs that represent the bot, the ball, and our opponent (which
// colour is assigned to each bot is determined by a command line
// parameter).
// It keeps track within the robot's AI data structure of multiple
// parameters related to each agent:
// - Position
// - Velocity vector. Not valid while rotating, but possibly valid
// while turning.
// - Heading (a unit vector in the direction of motion). Not valid
// while rotating - possibly valid while turning
// - Pointers to the blob data structure for each agent
//
// This function will update the blob data structure with the velocity
// and heading information from tracking.
//
// In addition to this, if calibration data is available then this
// function adjusts the Y location of the bot and the opponent to
// adjust for perspective projection error. See the handout on how
// to perform the calibration process.
//
// Note that the blob data
// structure itself contains another useful vector with the blob
// orientation (obtained directly from the blob shape, valid at all
// times even under rotation, but can be pointing backward!)
//
// This function receives a pointer to the robot's AI data structure,
// and a list of blobs.
/////////////////////////////////////////////////////////////////////////
struct blob *p;
double mg,vx,vy,pink,doff,dmin,dmax,adj;
double NOISE_VAR=5;
// Reset ID flags
ai->st.ballID=0;
ai->st.selfID=0;
ai->st.oppID=0;
ai->st.ball=NULL; // Be sure you check these are not NULL before
ai->st.self=NULL; // trying to access data for the ball/self/opponent!
ai->st.opp=NULL;
// Find the ball
p=id_coloured_blob(ai,blobs,2);
if (p)
{
ai->st.ball=p; // New pointer to ball
ai->st.ballID=1; // Set ID flag for ball (we found it!)
ai->st.bvx=p->cx-ai->st.old_bcx; // Update ball velocity in ai structure and blob structure
ai->st.bvy=p->cy-ai->st.old_bcy;
ai->st.ball->vx=ai->st.bvx;
ai->st.ball->vy=ai->st.bvy;
ai->st.old_bcx=p->cx; // Update old position for next frame's computation
ai->st.old_bcy=p->cy;
ai->st.ball->idtype=3;
vx=ai->st.bvx; // Compute heading direction (normalized motion vector)
vy=ai->st.bvy;
mg=sqrt((vx*vx)+(vy*vy));
if (mg>NOISE_VAR) // Update heading vector if meaningful motion detected
{
vx/=mg;
vy/=mg;
ai->st.bmx=vx;
ai->st.bmy=vy;
}
ai->st.ball->mx=ai->st.bmx;
ai->st.ball->my=ai->st.bmy;
}
else {
ai->st.ball=NULL;
}
// ID our bot
if (ai->st.botCol==0) p=id_coloured_blob(ai,blobs,1);
else p=id_coloured_blob(ai,blobs,0);
if (p)
{
ai->st.self=p; // Update pointer to self-blob
// Adjust Y position if we have calibration data
if (fabs(p->adj_Y[0][0])>.1)
{
dmax=384.0-p->adj_Y[0][0];
dmin=767.0-p->adj_Y[1][0];
pink=(dmax-dmin)/(768.0-384.0);
adj=dmin+((p->adj_Y[1][0]-p->cy)*pink);
p->cy=p->cy+adj;
if (p->cy>767) p->cy=767;
if (p->cy<1) p->cy=1;
}
ai->st.selfID=1;
ai->st.svx=p->cx-ai->st.old_scx;
ai->st.svy=p->cy-ai->st.old_scy;
ai->st.self->vx=ai->st.svx;
ai->st.self->vy=ai->st.svy;
ai->st.old_scx=p->cx;
ai->st.old_scy=p->cy;
ai->st.self->idtype=1;
vx=ai->st.svx;
vy=ai->st.svy;
mg=sqrt((vx*vx)+(vy*vy));
if (mg>NOISE_VAR)
{
vx/=mg;
vy/=mg;
ai->st.smx=vx;
ai->st.smy=vy;
}
ai->st.self->mx=ai->st.smx;
ai->st.self->my=ai->st.smy;
}
else ai->st.self=NULL;
// ID our opponent
if (ai->st.botCol==0) p=id_coloured_blob(ai,blobs,0);
else p=id_coloured_blob(ai,blobs,1);
if (p)
{
ai->st.opp=p;
if (fabs(p->adj_Y[0][1])>.1)
{
dmax=384.0-p->adj_Y[0][1];
dmin=767.0-p->adj_Y[1][1];
pink=(dmax-dmin)/(768.0-384.0);
adj=dmin+((p->adj_Y[1][1]-p->cy)*pink);
p->cy=p->cy+adj;
if (p->cy>767) p->cy=767;
if (p->cy<1) p->cy=1;
}
ai->st.oppID=1;
ai->st.ovx=p->cx-ai->st.old_ocx;
ai->st.ovy=p->cy-ai->st.old_ocy;
ai->st.opp->vx=ai->st.ovx;
ai->st.opp->vy=ai->st.ovy;
ai->st.old_ocx=p->cx;
ai->st.old_ocy=p->cy;
ai->st.opp->idtype=2;
vx=ai->st.ovx;
vy=ai->st.ovy;
mg=sqrt((vx*vx)+(vy*vy));
if (mg>NOISE_VAR)
{
vx/=mg;
vy/=mg;
ai->st.omx=vx;
ai->st.omy=vy;
}
ai->st.opp->mx=ai->st.omx;
ai->st.opp->my=ai->st.omy;
}
else ai->st.opp=NULL;
}
void track_agents(struct RoboAI *ai, struct blob *blobs)
{
// ** DO NOT CHANGE THIS FUNCTION **
// Find the current blobs that correspond to the bot, opponent, and ball
// by detecting blobs with the specified colours. One bot is green,
// one bot is blue. Ball is always red.
// NOTE: Add a command-line parameter to specify WHICH bot is own
struct blob *p;
double mg, vx, vy;
double NOISE_VAR = 15;
// Find the ball
p = id_coloured_blob(ai, blobs, 0);
if (p)
{
if (ai->st.ball != NULL && ai->st.ball != p)
{
ai->st.ball->idtype = 0;
ai->st.ball->p = 0;
}
ai->st.ballID = 1;
ai->st.ball = p;
ai->st.bvx = p->cx[0] - ai->st.old_bcx;
ai->st.bvy = p->cy[0] - ai->st.old_bcy;
ai->st.old_bcx = p->cx[0];
ai->st.old_bcy = p->cy[0];
ai->st.ball->p = 0;
ai->st.ball->idtype = 3;
vx = ai->st.bvx;
vy = ai->st.bvy;
mg = sqrt((vx * vx) + (vy * vy));
if (mg > NOISE_VAR) // Enable? disable??
{
ai->st.ball->vx[0] = vx;
ai->st.ball->vx[1] = vy;
vx /= mg;
vy /= mg;
ai->st.ball->mx = vx;
ai->st.ball->my = vy;
}
}
else
{
ai->st.ball = NULL;
}
// ID our bot
if (ai->st.botCol == 0) p = id_coloured_blob(ai, blobs, 1);
else p = id_coloured_blob(ai, blobs, 2);
if (p)
{
if (ai->st.self != NULL && ai->st.self != p)
{
ai->st.self->idtype = 0;
ai->st.self->p = 0;
}
ai->st.selfID = 1;
ai->st.self = p;
ai->st.svx = p->cx[0] - ai->st.old_scx;
ai->st.svy = p->cy[0] - ai->st.old_scy;
ai->st.old_scx = p->cx[0];
ai->st.old_scy = p->cy[0];
ai->st.self->p = 0;
ai->st.self->idtype = 1;
}
else ai->st.self = NULL;
// ID our opponent
if (ai->st.botCol == 0) p = id_coloured_blob(ai, blobs, 2);
else p = id_coloured_blob(ai, blobs, 1);
if (p)
{
if (ai->st.opp != NULL && ai->st.opp != p)
{
ai->st.opp->idtype = 0;
ai->st.opp->p = 0;
}
ai->st.oppID = 1;
ai->st.opp = p;
ai->st.ovx = p->cx[0] - ai->st.old_ocx;
ai->st.ovy = p->cy[0] - ai->st.old_ocy;
ai->st.old_ocx = p->cx[0];
ai->st.old_ocy = p->cy[0];
ai->st.opp->p = 0;
ai->st.opp->idtype = 2;
}
else ai->st.opp = NULL;
}
