freespace_impl.h

Go to the documentation of this file.

namespace frechet { namespace fs {

using namespace data;
using namespace numeric;

template<typename Float>
void FreeSpace::calculateFreeSpaceSegment(
        const Point& p, const Point& q, const Point& r,
        double epsilon,
        data::Interval& result)
{
    //  solve a quadratic equation
    Float a,b,c,d;

    a = euclidean_distance_sq<Float>(q,r);
    if (a==0.0) {
        /* q==r indicates a duplicate point
           it would create various problems later (e.g. in Feasible Paths finding).
           therefore, identical points must be removed on input.
        */
        Q_ASSERT(false);
        result = Interval::INVALID;    //  invalid
        return;
    }

    Float px = p.x();
    Float py = p.y();
    Float qx = q.x();
    Float qy = q.y();
    Float rx = r.x();
    Float ry = r.y();

    b = 2.0 * ((qx - px) * (rx - qx) + (qy - py) * (ry - qy));
    c = euclidean_distance_sq<Float>(p,q) - sq<Float>(epsilon);

    d = sq(b) - 4.0 * a * c;

    if (d < 0.0 || a==0.0) {
        result = Interval::INVALID;    //  invalid
        return;
    }

    Float sqrtd = sqrt<Float>(d);
    result.lower() = (double)((-b - sqrtd) / (2.0*a));
    result.upper() = (double)((-b + sqrtd) / (2.0*a));

    result &= Interval::UNIT;
    if (result.empty())
        result = Interval::INVALID;    //  invalid
}

} } // namespace