Trapezoids calculation remake - eliminate specific cases, design a generic method

[rayvburn]

Currently there are specific cases, especially when intersection values are very big, when Trapezoid vertices are not calculated properly.

A good starting point:

bool TrapezoidParted::update(const double &start, const double &end) {
    // Creates parameter configuration equal to the uninitialized fl::Trapezoid instance
    std::string params("nan nan nan nan");
    std::string params_wrap("nan nan nan nan");

    Angle an(start - intersection_);
    Angle bn(an.getRadian() + intersection_, false);

    double top_length = 0.0;
    if (start >= end) {
        top_length = (IGN_PI - start) + (end - (-IGN_PI));
    } else {
        top_length = end - start;
    }
    Angle cn(bn.getRadian() + top_length, false);
    Angle dn(cn.getRadian() + intersection_, false);

    Angle shift(-IGN_PI);
    if (bn.getRadian() > IGN_PI) {
        shift = Angle(+IGN_PI);
    }
    Angle aw(an.getRadian() + shift.getRadian());
    Angle bw(bn.getRadian() + shift.getRadian());
    Angle cw(cn.getRadian() + shift.getRadian());
    Angle dw(dn.getRadian() + shift.getRadian());

    params = generateParams(an.getRadian(), bn.getRadian(), cn.getRadian(), dn.getRadian(), 1.0);
    params_wrap = generateParams(aw.getRadian(), bw.getRadian(), cw.getRadian(), dw.getRadian(), 1.0);
    trapezoid_ptrs_.at(0)->configure(params);
    trapezoid_ptrs_.at(1)->configure(params_wrap);

    return true;

Follow notes from 2021.11.12. This change will most likely affect unit tests.

[rayvburn]

Debugging console outs stored in stashed changes named:

On fis-direction-output-part2: TRAPEZOIDS UNIT TESTS - couts added to TrapezoidParted, Processor and TrapezoidLocIndep