14.12 getClosestTeammatePos

This function determines which of my teammates is the one closest to the ball and returns a VecPosition of where they are on the field.

Using the first approach below, this task is really simply as we have a sorted vector of all our teammates positions relative to the ball generated by GenerateTeamToTargetDistanceVector where the target vector is the ball. With that we can simply look for the first agent which is not the current player, since we dont want to pass to ourselves. Once we have the index of the next closest player to the ball represented by i we can use that to firstly identify the index of the agent at i using TeamDistToBall[i].second. Then we can use worldModel->getWorldObject()->pos to get the position of that agent.

VecPosition NaoBehavior::getClosestTeammatePos(int _playerNumber,vector<pair<double,int > > TeamDistToBall){
    
    int i=0;
    while(TeamDistToBall[i].second == _playerNumber){
        i++;
    }
    return worldModel->getWorldObject(TeamDistToBall[i].second)->pos;
    
}

The second approach once again is done through iterating through the teammates and querying their distance to the ball. While this process is being conducted we keep track of the id of the closest teammate to the ball. We can then latter use this id to reference the WorldObject identified by that id using the worldModel. Once we have a reference to the WorldObject we can get the pos this is achieved with return worldModel->getWorldObject(id)->pos;

VecPosition NaoBehavior::getClosestTeammatePos(int _playerNumber,double* _teamMateDistances){
    int arrlengthTeam = NUM_AGENTS;

    //printf("Size = %d",arrlengthTeam);
    double minTeamDis = MAXFLOAT; 
    int id = -1;
    for(int z=0;z<arrlengthTeam;++z){
        if(_teamMateDistances[z]< minTeamDis){
            if(_playerNumber != z+1){
                minTeamDis = _teamMateDistances[z];
                id = z+1;
            }
        } 
    }

    return worldModel->getWorldObject(id)->pos;
    

}