14.26 isThereObstructionBetweenTargetAndBall

This function checks if there is any obstruction (be it a teammate or an opponent) between the ball and the target. threshold is the minimum distance a teammate has to be from the target to be classified as an obstruction. This is achieved by defining s1,s2,e1,e2 which make up the four points of a rectangle in between the ball and the VecPosition target. auto isBetweenLines = [&](VecPosition vec, SIM::Line2D l1, SIM::Line2D l2) defines an inline function which can determine if a VecPosition is within two lines which are generated using s1,s2,e1,e2. Lastly we iterate over all gameobjects on the field to determine if the position of each of those objects lies between those lines. If there is case where isBetweenLines returns true then that means there is an obstruction present between ball and target.

Figure 14.1: Visualisation of Rectangle Corridor used to check obstructions

bool NaoBehavior::isThereObstructionBetweenTargetAndBall(VecPosition target, double threshold, double offset){
    // todo what about the actual player to kick and the target if it's a player?
    VecPosition vector_to_target = target - ball;
    auto normal = vector_to_target.normalize();
    auto orthogonal1 = VecPosition(-normal.getY(), normal.getX());
    
    auto s1 = ball + orthogonal1*offset;
    auto s2 = ball - orthogonal1*offset;
    auto e1 = target + orthogonal1*offset;
    auto e2 = target - orthogonal1*offset;


    SIM::Line2D line1 = SIM::Line2D(SIM::Point2D(s1.getX(), s1.getY()), SIM::Point2D(e1.getX(), e1.getY()));
    SIM::Line2D line2 = SIM::Line2D(SIM::Point2D(s2.getX(), s2.getY()), SIM::Point2D(e2.getX(), e2.getY()));
    
    SIM::Line2D lineStart = SIM::Line2D(
        SIM::Point2D(s1.getX(), s1.getY()),
        SIM::Point2D(s2.getX(), s2.getY())
    );

    SIM::Line2D lineEnd = SIM::Line2D(
        SIM::Point2D(e1.getX(), e1.getY()),
        SIM::Point2D(e2.getX(), e2.getY())
    );

    line1.m_c *=-1;
    line2.m_c *=-1;
    lineStart.m_c *= -1;
    lineEnd.m_c *= -1;
    

    if (line1.m_c > line2.m_c){
        std::swap(line1, line2);
    }
    if (lineStart.m_c > lineEnd.m_c){
        std::swap(lineStart, lineEnd);
    }
    
    
    // DEBUG DRAW
    worldModel->getRVSender()->drawLine("ObstructionZone",
        s1.getX(), s1.getY(),
        e1.getX(), e1.getY(), 1, 0, 0
    );
    worldModel->getRVSender()->drawLine("ObstructionZone",
        s2.getX(), s2.getY(),
        e2.getX(), e2.getY(), 0, 0, 1
    );
    worldModel->getRVSender()->drawLine("ObstructionZone",
        s2.getX(), s2.getY(),
        s1.getX(), s1.getY(), 0, 1, 1
    );
    worldModel->getRVSender()->drawLine("ObstructionZone",
        e2.getX(), e2.getY(),
        e1.getX(), e1.getY(), 1, 1, 0
    );
   
    
    // now, the slopes should be the same and just the c's should differ.
    double mx = line1.m_a;
    double my = line1.m_b;
    
    double c1 = line1.m_c;
    double c2 = line2.m_c;

    if (c1 > c2){
        std::swap(c1, c2);
    }

    auto isBetweenLines = [&](VecPosition vec, SIM::Line2D l1, SIM::Line2D l2){
        // l1.c must be less than l2.c
        auto v = vec;// - ball;
        double c_from_vec = v.getX() * l1.m_b + v.getY() * l1.m_a;
        if (l1.m_c <= c_from_vec && c_from_vec <= l2.m_c){
            return true;
        }
        return false;
    };

    // now iterate over all team mates and enemies.
    for (int start_idx: {WO_TEAMMATE1, WO_OPPONENT1}){ // WO_OPPONENT1, 
        for (int i=start_idx; i < start_idx + NUM_AGENTS; ++i){
            VecPosition pos = worldModel->getWorldObject(i)->pos;
            if (i == worldModel->getUNum()){
                pos = worldModel->getMyPosition();
            }
            if (start_idx == WO_TEAMMATE1){
                // we are looking at a teammate.
                float distance_to_target = pos.getDistanceTo(target);
                if (distance_to_target < threshold){
                    // if this teammate is too close to the target, then don't count it as an obstruction.
                    continue;
                }
            }
            bool l12 = isBetweenLines(pos, line1, line2), lse = isBetweenLines(pos, lineStart, lineEnd);
            if (l12 && lse){
                // worldModel->getRVSender()->drawCircle(pos.getX(), pos.getY(), 2, 1, 1, 1 );
                return true;
            }
        }
    }

    return false;
}