optimise_impl.h

Go to the documentation of this file.

template<typename R> int Algorithm::binarySearch(
        const std::vector<double>& values, int lo, int hi,
        R (Algorithm::* decideFunction) (FreeSpace::ptr, reach::FSPath::ptr, double, bool),
        R& result)
{
    //  (2) binsearch on critical points
    //  operate on private free-space
    Q_ASSERT(local_fs);
    reach::FSPath::ptr no_path;

    while(lo < hi) {
        int mid = (lo+hi)/2;
        Q_ASSERT(mid >=0 && mid < values.size());
        double epsilon = values[mid];

        local_fs->calculateFreeSpace(epsilon);
        R mresult = (this->*decideFunction) (local_fs,no_path,epsilon,false);

        if ((bool)mresult) {
            hi = mid;
            result = mresult;
        }
        else {
            lo = mid+1;
        }
    }
    return hi;
}

template<typename R> double Algorithm::intervalSearch(
        double lower_bound, double upper_bound, double precision,
        R (Algorithm::* decideFunction) (FreeSpace::ptr, reach::FSPath::ptr, double, bool),
        R& result)
{
    Q_ASSERT(local_fs);
    reach::FSPath::ptr no_path;

    double mepsilon;
    R mresult;

    while((upper_bound-lower_bound) > precision)
    {
        mepsilon = (lower_bound+upper_bound)/2;

        local_fs->calculateFreeSpace(mepsilon);
        mresult = (this->*decideFunction)(local_fs, no_path, mepsilon, false);

        if ((bool)mresult) {
            upper_bound=mepsilon;
            result=mresult;
        }
        else {
            lower_bound = mepsilon;
        }
    }
    return upper_bound;
}

template<typename Float>
Float Algorithm::segment_distance(const QLineF& segment, const Point& q1)
{
    //  distance from a point to a segment
    Float d1 = euclidean_segment_distance<Float>(segment,q1);
    Float d2 = euclidean_distance<Float>(segment.p1(),q1);
    Float d3 = euclidean_distance<Float>(segment.p2(),q1);
    return min(min(d1,d2),d3);
    //  right ??
}

template<typename Float>
Float Algorithm::bisector_distance(
        const Float& p1x, const Float& p1y,
        const Float& p2x, const Float& p2y,
        const Float& q1x, const Float& q1y,
        const Float& q2x, const Float& q2y,
        const Float& tolerance)
{
    //  bisector between q1, q2
    //  intersect with segment p1..p2
    //  distance of intersection to q1,q2 (which is equal)

    Float t_denom = 2*((p1x-p2x)*(q1x-q2x) + (p1y-p2y)*(q1y-q2y));
    if (t_denom==0.0) return -1;
    
    Float t_nom = (q2x + q1x - 2 * p1x)*(q2x - q1x) + (q2y + q1y - 2 * p1y)*(q2y - q1y);
    Float t = t_nom/t_denom;

    if (t < -tolerance || t > 1+tolerance)
        return -1;  //  bisector does not hit [p1..p2]

    Q_ASSERT(t>=-tolerance && t<=1+tolerance);

    Float rx = p1x + t*(p2x-p1x);
    Float ry = p1y + t*(p2y-p1y);
    Q_ASSERT(abs(
            euclidean_distance<Float>(rx,ry, q1x,q1y)-
            euclidean_distance<Float>(rx,ry, q2x,q2y))
            <= 1e-9);
    return euclidean_distance<Float>(rx,ry, q1x,q1y);
}

template<typename Float>
Float Algorithm::bisector_distance(
        const Point& q1, const Point& q2,
        const QLineF& segment,
        Float tolerance)
{
    //  bisector between q1, q2
    //  intersect with segment
    //  distance of intersection to q1,q2 (which is equal)
    const Point& p1 = segment.p1();
    const Point& p2 = segment.p2();

    return bisector_distance<Float> (
            p1.x(),p1.y(), p2.x(),p2.y(),
            q1.x(),q1.y(), q2.x(),q2.y(), tolerance);
}