14.3 DistanceToBallArrayTeammates

As demonstrated in the longer form tutorials this function is used to generate and array which contains the distances of each of our teammates relative to the ball. This function therefore doesn’t return anything but updates values with respect to a pointer to a 1D array which it receives as input. The function works by iterating through all the teammate WorldObject’s which are indexed using WO_TEAMMATE1. We then get the position of that WorldObject and with that we can get the distance to another VecPosition using getDistanceTo() in this case since we need the distance to ball we say getDistanceTo(ball). It is important to note that an agent cant get its own distance in the same way it can get other WorldObject’s distances. It is for this reason we have an if statement to differentiate the cases. For when an agent is trying to get it’s own position we need to use worldModel->getMyPosition();.

void NaoBehavior::DistanceToBallArrayTeammates(int mynum, double* _teamMateDistances){
    for(int i = WO_TEAMMATE1; i<WO_TEAMMATE1+NUM_AGENTS;i++){ //OUR PLAYERS
        WorldObject* teammate = worldModel->getWorldObject(i);
        VecPosition temp;

        if(i == mynum){
            temp = worldModel->getMyPosition();
        }
        else{
            temp = teammate->pos;
        }
        temp.setZ(0);

        float distance = temp.getDistanceTo(ball);
        _teamMateDistances[i-WO_TEAMMATE1] = distance;


        worldModel->getRVSender()->drawText("distTEAM",std::to_string(distance),temp.getX(),temp.getY()+2*mynum,0,0,0);

    }
}