polyline.cc

#include <algorithm>

#include <vtestbed/config/real_type.hh>
#include <vtestbed/geometry/inertia_tensor.hh>
#include <vtestbed/geometry/line_segment.hh>
#include <vtestbed/geometry/math.hh>
#include <vtestbed/geometry/polygon.hh>
#include <vtestbed/geometry/polyline.hh>

namespace {

    template <class T, int N>
    inline bool
    fully_contains(
        const vtb::geometry::Line_segment<T,N>& lhs,
        const vtb::geometry::Line_segment<T,N>& rhs,
        T eps
    ) {
        return (lhs.contains(rhs.front(), eps) && lhs.contains(rhs.back(), eps));
    }

}

template <class T, int N>
void
vtb::geometry::Polyline<T,N>::remove_interior() {
    T eps = 0;
    auto& segments = *this;
    auto n = segments.size();
    std::vector<bool> mask(n);
    for (size_type i=0; i<n; ++i) {
        for (size_type j=i+1; j<n; ++j) {
            const auto& s1 = segments[i];
            const auto& s2 = segments[j];
            auto s3 = s2;
            s3.invert();
            if (near(s1,s2,eps) || near(s1,s3,eps) /*||
                fully_contains(s1,s2,eps) || fully_contains(s2,s1,eps)*/) {
                mask[i] = true, mask[j] = true;
            }
        }
    }
    for (size_type i=n; i>0; --i) {
        size_type j=i-1;
        if (mask[j]) {
            segments.erase(segments.begin()+j);
        }
    }
}

template <class T, int N>
void
vtb::geometry::Polyline<T,N>::simplify() {
    T eps = 1e-3;
    auto& segments = *this;
    auto n = segments.size();
start:
    for (size_type i=0; i<n; ++i) {
        for (size_type j=0; j<n; ++j) {
            if (i == j) { continue; }
            auto& s1 = segments[i];
            const auto& s2 = segments[j];
            if (near(s1.back(), s2.front(), eps) &&
                Line_segment<T,N>(s1.front(), s2.back()).contains(s1.back(), eps)) {
                s1.back() = s2.back();
                segments.erase(segments.begin()+j);
                --n;
                goto start;
            }
        }
    }
}

template <class T, int N>
void
vtb::geometry::Polyline<T,N>::gift_wrap() {
    T eps = 1e-3;
    auto& segments = *this;
    auto n = segments.size();
    if (n <= 3) { return; }
    // top right vertex
    auto p0 = std::max_element(
        segments.begin(), segments.end(),
        [] (const_reference lhs, const_reference rhs) {
            Compare_vertex<T,N> cmp;
            return cmp(lhs.front(), rhs.front());
        }
    )->front();
    Polyline<T,N> convex_hull;
    for (const auto& s : segments) {
        std::clog << "near(s.front(), s.back(), eps)=" << near(s.front(), s.back(), eps) << std::endl;
    }
    do {
        bool found = false;
        value_type candidate, orig;
        scalar_type diff = 0;
        size_type idx = 0;
        std::clog << "convex_hull.size()=" << convex_hull.size() << std::endl;
        for (size_type i=0; i<n; ++i) {
            const auto& s = segments[i];
            if (near(p0, s.front(), eps)) {
                if (!found) {
                    candidate = s;
                    orig = s;
                    diff = 0;
                    idx = i;
                    found = true;
                } else {
                    Vertex<T,N> v1 = s.back() - p0;
                    Vertex<T,N> v2 = candidate.back() - p0;
                    //auto cp = cross(v1,v2);
                    //if (all(cp > T{})) {
                    auto x0 = p0(0);
                    auto y0 = p0(1);
                    auto x1 = s.back()(0);
                    auto y1 = s.back()(1);
                    auto x2 = candidate.back()(0);
                    auto y2 = candidate.back()(1);
                    auto x3 = orig.back()(0);
                    auto y3 = orig.back()(1);
                    auto lhs = x2*y0 + x0*y1 + x1*y2;
                    auto rhs = x1*y0 + x2*y1 + x0*y2;
                    auto lhs2 = x3*y0 + x0*y1 + x1*y3;
                    auto rhs2 = x1*y0 + x3*y1 + x0*y3;
                    std::clog << "(lhs-rhs)=" << (lhs-rhs) << std::endl;
                    std::clog << "(lhs2-rhs2)=" << (lhs2-rhs2) << std::endl;
                    if (lhs2-rhs2 > diff) {
                        candidate = s;
                        idx = i;
                        diff = lhs2-rhs2;
                    }
                }
            }
        }
        std::clog << "candidate=" << candidate << std::endl;
        convex_hull.emplace_back(candidate);
        segments.erase(segments.begin()+idx);
        p0 = candidate.back();
    } while (!segments.empty() &&
            !near(convex_hull.front().front(), convex_hull.back().back(), eps));
    segments = std::move(convex_hull);
}

template <>
void
vtb::geometry::Polyline<VTB_REAL_TYPE,2>::reorder() {
    using T = VTB_REAL_TYPE;
    Vertex<T,2> origin{T{}};
    for (const auto& segment : *this) {
        origin += segment[0];
        origin += segment[1];
    }
    origin /= 2*this->size();
    for (auto& segment : *this) {
        auto d = det(origin, segment[0], segment[1]);
        if (d < 0) { segment.flip(); }
    }
}

template <class T, int N>
void
vtb::geometry::Polyline<T,N>::stitch() {
    auto& segments = *this;
    const auto n = segments.size();
    if (n <= 1) { return; }
    for (size_t i=0; i<n; ++i) {
        const auto& back = segments[i].back();
        for (size_t j=i+1; j<n; ++j) {
            const auto& front = segments[j].front();
            if (all(front == back)) {
                if (j != i+1) { std::swap(segments[i+1], segments[j]); }
                break;
            }
        }
    }
}

template <class T, int N>
vtb::geometry::Polygon<T,N>
vtb::geometry::Polyline<T,N>::polygon() const {
    Polygon<T,N> result;
    for (const auto& segment : *this) {
        result.emplace_back(segment.front());
    }
    return result;
}

template <class T, int N>
void
vtb::geometry::Polyline<T,N>::gnuplot(std::ostream& out) const {
    for (const auto& segment : *this) { segment.gnuplot(out); out << "\n\n"; }
}

template <class T, int N>
vtb::geometry::Mass_moments<T,N>
vtb::geometry::mass_moments(const Polyline<T,N>& polyline) {
    static const T mult[6] = {
        T{1}/T{2},
        T{1}/T{6}, T{1}/T{6},
        T{1}/T{12}, T{1}/T{12},
        T{1}/T{24}
    };
    T intg[6] = {}; // 1, x, y, x^2, y^2, xy
    for (const auto& segment : polyline) {
        const auto& v0 = segment.front();
        const auto& v1 = segment.back();
        auto x0 = v0(0), y0 = v0(1), x1 = v1(0), y1 = v1(1);
        auto x0y1 = x0*y1, y0x1 = y0*x1;
        auto cp = x0y1 - y0x1;
        intg[0] += cp;
        intg[1] += cp*(x0 + x1);
        intg[2] += cp*(y0 + y1);
        intg[3] += cp*(y0*y0 + y0*y1 + y1*y1);
        intg[4] += cp*(x0*x0 + x0*x1 + x1*x1);
        intg[5] += cp*(x0y1 + 2*x0*y0 + 2*x1*y1 + y0x1);
    }
    for (int i=0; i<6; ++i) { intg[i] *= mult[i]; }
    Mass_moments<T,N> m;
    m.mass = intg[0];
    m.centre(0) = intg[1], m.centre(1) = intg[2];
    if (m.mass != 0) { m.centre /= m.mass; }
    auto& cm = m.centre;
    m.inertia.xx = intg[3] - m.mass*cm(0)*cm(0);
    m.inertia.yy = intg[4] - m.mass*cm(1)*cm(1);
    m.inertia.xy = intg[5] - m.mass*cm(0)*cm(1);
    return m;
}

template class vtb::geometry::Polyline<VTB_REAL_TYPE,2>;
template class vtb::geometry::Polyline<VTB_REAL_TYPE,3>;

template vtb::geometry::Mass_moments<VTB_REAL_TYPE,2>
vtb::geometry::mass_moments(const Polyline<VTB_REAL_TYPE,2>& polygon);