geometry/polyhedron.cc

#include <algorithm>
#include <limits>
#include <map>
#include <ostream>
#include <queue>
#include <set>

#include <vtestbed/base/constants.hh>
#include <vtestbed/base/mean.hh>
#include <vtestbed/config/real_type.hh>
#include <vtestbed/core/bisection.hh>
#include <vtestbed/geometry/bsp_tree.hh>
#include <vtestbed/geometry/inertia_tensor.hh>
#include <vtestbed/geometry/math.hh>
#include <vtestbed/geometry/polyhedron.hh>
#include <vtestbed/geometry/tetrahedron.hh>
#include <vtestbed/geometry/vertex.hh>

//#define VTB_DEBUG_DEDUPLICATE

namespace {

    template <class T>
    inline blitz::Array<T,1>
    to_blitz_array(const std::vector<T>& src) {
        const auto n = src.size();
        blitz::Array<T,1> result(blitz::shape(n));
        for (size_t i=0; i<n; ++i) {
            result(i) = src[i];
        }
        return result;
    }

    template <class T>
    inline void
    flip_vertices(std::vector<T>& vertices, int dim) {
        for (auto& v : vertices) {
            v(dim) = -v(dim);
        }
    }

    template <class T>
    inline void
    mirror_vertices(std::vector<T>& vertices, int dim) {
        const auto nvertices = vertices.size();
        vertices.resize(nvertices*2);
        for (size_t i=0; i<nvertices; ++i) {
            auto v = vertices[i];
            v(dim) = -v(dim);
            vertices[nvertices + i] = v;
        }
    }

    template <class Vertices, class Normals, class Rotate>
    inline void
    rotate_vertices(Vertices& vertices, Normals& normals, Rotate rotate) {
        for (auto& v : vertices) {
            v = rotate(v);
        }
        for (auto& v : normals) {
            v = rotate(v);
        }
    }

    template <int N>
    struct CompareFace {

        using value_type = vtb::geometry::Face<N>;

        inline bool
        operator()(value_type lhs, value_type rhs) const {
            lhs.sort(), rhs.sort();
            return lhs < rhs;
        }

    };

    inline bool
    face_has_duplicate_indices(const vtb::geometry::Face<3>& f) {
        return f[0] == f[1] || f[1] == f[2] || f[2] == f[0];
    }

    template <class T, int N>
    struct CompareTriangles {

        using value_type = vtb::geometry::Triangle<T,N>;

        inline bool
        operator()(const value_type& lhs, const value_type& rhs) const {
            const auto y0 = std::min({lhs[0][1], lhs[1][1], lhs[2][1]});
            const auto y1 = std::min({rhs[0][1], rhs[1][1], rhs[2][1]});
            return y0 < y1;
        }

    };

    template <class T, int N>
    void
    gnuplot(std::ostream& out, const vtb::geometry::Vertex<T,N>& v) {
        out << v(0);
        for (int i=1; i<N; ++i) { out << ' ' << v(i); }
    }

}

template <class T, int N>
vtb::geometry::Polyhedron<T,N>::Polyhedron(
    const blitz_vertex_array& vertices,
    const blitz_face_array& faces
):
_vertices(vertices.data(), vertices.data() + vertices.size()),
_faces(faces.data(), faces.data() + faces.size()) {}


template <class T, int N>
vtb::geometry::Polyhedron<T,N>::Polyhedron(const bsp_tree& tree) {
    auto& vertices = this->_vertices;
    auto& faces = this->_faces;
    index_type idx = 0;
    const auto n = tree.nodes().size();
    vertices.reserve(n*3);
    faces.reserve(n);
    for (const auto& node : tree.nodes()) {
        for (const auto& tr : node.polygons()) {
            vertices.emplace_back(tr[0]);
            vertices.emplace_back(tr[1]);
            vertices.emplace_back(tr[2]);
            faces.emplace_back(idx, idx+1, idx+2);
            idx += 3;
        }
    }
}

template <class T, int N>
vtb::geometry::Polyhedron<T,N>::Polyhedron(const triangle_array& triangles) {
    auto& vertices = this->_vertices;
    auto& faces = this->_faces;
    index_type idx = 0;
    vertices.reserve(triangles.size()*3);
    faces.reserve(triangles.size());
    for (const auto& tr : triangles) {
        vertices.emplace_back(tr[0]);
        vertices.emplace_back(tr[1]);
        vertices.emplace_back(tr[2]);
        faces.emplace_back(idx, idx+1, idx+2);
        idx += 3;
    }
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::normalise() {
    const auto& vertices = this->_vertices;
    const auto& faces = this->_faces;
    const auto nfaces = faces.size();
    auto& vnormals = this->_vertex_normals;
    vnormals.assign(vertices.size(), T{0});
    auto& fnormals = this->_face_normals;
    fnormals.assign(faces.size(), T{0});
    for (size_type i=0; i<nfaces; ++i) {
        const auto& face = this->_faces[i];
        const auto i0 = face[0];
        const auto i1 = face[1];
        const auto i2 = face[2];
        const auto& v0 = vertices[i0];
        const auto& v1 = vertices[i1];
        const auto& v2 = vertices[i2];
        vertex_type a = v1-v0, b = v2-v0;
        vertex_type n = cross(a, b);
        vnormals[i0] += n, vnormals[i1] += n, vnormals[i2] += n;
        fnormals[i] = unit(n);
    }
    for (auto& n : vnormals) {
        n = unit(n);
    }
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::normalise_faces() {
    const auto& vertices = this->_vertices;
    const auto& faces = this->_faces;
    const auto nfaces = faces.size();
    auto& fnormals = this->_face_normals;
    fnormals.assign(faces.size(), T{0});
    for (size_type i=0; i<nfaces; ++i) {
        const auto& face = this->_faces[i];
        const auto i0 = face[0];
        const auto i1 = face[1];
        const auto i2 = face[2];
        const auto& v0 = vertices[i0];
        const auto& v1 = vertices[i1];
        const auto& v2 = vertices[i2];
        vertex_type a = v1-v0, b = v2-v0;
        vertex_type n = cross(a, b);
        fnormals[i] = unit(n);
    }
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::reorder() {
    using vtb::geometry::centroid;
    const auto& vf = this->vertex_faces();
    const auto& cf = this->component_faces(vf);
    auto& faces = this->_faces;
    const auto& origin = centroid(vertices());
    index_type nfaces = faces.size();
    for (const auto& face_indices : cf) {
        index_type rfi = this->reference_face_index(face_indices, origin);
        if (rfi == std::numeric_limits<index_type>::max()) { continue; }
        this->reorder(rfi, origin);
        std::queue<index_type> queue;
        std::vector<bool> computed(nfaces, false);
        queue.push(rfi);
        computed[rfi] = true;
        while (!queue.empty()) {
            const auto& face_a = faces[queue.front()];
            queue.pop();
            for (index_type vertex_index : face_a){
                for (index_type i : vf[vertex_index]){
                    if (computed[i]) { continue; }
                    if (!faces[i].reorder(face_a, vertex_index)) { continue; }
                    computed[i] = true;
                    queue.push(i);
                }
            }
        }
    }
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::unique() {
    #if defined(VTB_DEBUG_DEDUPLICATE)
    size_type old_nfaces = this->_faces.size();
    size_type old_nvertices = this->_vertices.size();
    #endif
    this->remove_duplicate_vertices();
    this->remove_duplicate_faces();
    #if defined(VTB_DEBUG_DEDUPLICATE)
    size_type new_nfaces = this->_faces.size();
    size_type new_nvertices = this->_vertices.size();
    std::clog << "removed " << (old_nfaces-new_nfaces)
        << " faces and " << (old_nvertices-new_nvertices)
        << " vertices" << std::endl;
    #endif
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::remove_duplicate_vertices() {
    using map_type = std::map<vertex_type,index_type,Compare_vertex<T,N>>;
    using iterator = typename map_type::iterator;
    auto& vertices = this->_vertices;
    auto& vertex_normals = this->_vertex_normals;
    auto& faces = this->_faces;
    if (!vertex_normals.empty()) {
        throw std::invalid_argument("vertex normals are not empty");
    }
    map_type unique_vertices;
    vertex_array new_vertex_normals;
    auto insert = [&] (const index_type& i) -> index_type {
        const auto& v = vertices[i];
        iterator it; bool success;
        index_type index = unique_vertices.size();
        std::tie(it,success) = unique_vertices.insert({v, index});
        return !success ? it->second : index;
    };
    for (auto& f : faces) {
        auto i0 = insert(f[0]);
        auto i1 = insert(f[1]);
        auto i2 = insert(f[2]);
        f = face_type(i0,i1,i2);
    }
    vertices.resize(unique_vertices.size());
    for (const auto& entry : unique_vertices) {
        vertices[entry.second] = entry.first;
    }
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::remove_duplicate_faces() {
    auto& faces = this->_faces;
    std::set<face_type,CompareFace<3>> unique_faces;
    for (const auto& f : faces) {
        if (face_has_duplicate_indices(f)) { continue; }
        unique_faces.insert(f);
    }
    faces.assign(unique_faces.begin(), unique_faces.end());
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::cut_duplicate_faces() {
    auto& faces = this->_faces;
    std::set<face_type,CompareFace<3>> unique_faces, trash;
    for (const auto& f : faces) {
        if (face_has_duplicate_indices(f)) { continue; }
        auto result = unique_faces.insert(f);
        if (!result.second) { trash.insert(*result.first); }
    }
    auto end = trash.end();
    auto first = unique_faces.begin();
    while (first != unique_faces.end()) {
        if (trash.find(*first) != end) { first = unique_faces.erase(first); }
        else { ++first; }
    }
    faces.assign(unique_faces.begin(), unique_faces.end());
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::remove_face(const vertex_type& vertex) {
    auto& vertices = this->_vertices;
    auto first = vertices.begin();
    Equal_vertex<T,N> eq;
    while (first != vertices.end()) {
        if (eq(*first, vertex)) {
            index_type idx = first - vertices.begin();
            remove_face(idx);
        }
        ++first;
    }
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::remove_face(index_type vertex) {
    auto& faces = this->_faces;
    faces.erase(
        std::remove_if(
            faces.begin(), faces.end(),
            [vertex] (const face_type& f) { return f.contains(vertex); }
        ),
        faces.end()
    );
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::merge(const Polyhedron& rhs) {
    auto& vertices = this->_vertices;
    auto& vertex_normals = this->_vertex_normals;
    auto& faces = this->_faces;
    auto& face_normals = this->_face_normals;
    const auto offset = vertices.size();
    for (const auto& v : rhs.vertices()) { vertices.emplace_back(v); }
    for (const auto& n : rhs.vertex_normals()) {
        vertex_normals.emplace_back(n);
    }
    for (auto f : rhs.faces()) { faces.emplace_back(f += offset); }
    for (const auto& n : rhs.face_normals()) {
        face_normals.emplace_back(n);
    }
}

template <class T, int N>
auto
vtb::geometry::Polyhedron<T,N>::vertex_faces() const -> index_array_2d {
    index_array_2d vf(this->_vertices.size());
    const auto& faces = this->_faces;
    int nfaces = faces.size();
    for (int i=0; i<nfaces; ++i) {
        const auto& face = faces[i];
        vf[face[0]].push_back(i);
        vf[face[1]].push_back(i);
        vf[face[2]].push_back(i);
    }
    return vf;
}

template <class T, int N>
auto
vtb::geometry::Polyhedron<T,N>::component_faces(
    const index_array_2d& vf
) const -> index_array_2d {
    const auto& faces = this->_faces;
    index_type nfaces = faces.size();
    std::queue<index_type> queue;
    std::vector<bool> computed(nfaces, false);
    index_array_2d cf;
    for (index_type i=0; i<nfaces; ++i) {
        if (computed[i]) { continue; }
        queue.push(i);
        computed[i] = true;
        cf.emplace_back();
        auto& face_indices = cf.back();
        while (!queue.empty()) {
            auto face_index = queue.front();
            queue.pop();
            face_indices.push_back(face_index);
            for (index_type vertex_index : faces[face_index]) {
                for (index_type face : vf[vertex_index]){
                    if (computed[face]) { continue; }
                    computed[face] = true;
                    queue.push(face);
                }
            }
        }
    }
    return cf;
}

template <class T, int N>
auto
vtb::geometry::Polyhedron<T,N>::reference_face_index(
    const index_array& indices,
    const vertex_type& origin
) const -> index_type {
    const auto& vertices = this->_vertices;
    const auto& faces = this->_faces;
//  vertex_type origin = T{0};
//  for (int i : indices) {
//      const auto& face = faces[i];
//      const auto& v0 = vertices[face[0]];
//      const auto& v1 = vertices[face[1]];
//      const auto& v2 = vertices[face[2]];
//      origin += (v0 + v1 + v2) / 3;
//  }
//  origin /= indices.size();
    T min_length = std::numeric_limits<T>::max();
    index_type face_with_min_length = std::numeric_limits<index_type>::max();
    for (index_type i : indices) {
        const auto& face = faces[i];
        const auto& v0 = vertices[face[0]];
        const auto& v1 = vertices[face[1]];
        const auto& v2 = vertices[face[2]];
        vertex_type face_centre = (v0+v1+v2) / 3;
        T new_length = distance(face_centre, origin);
//      T new_length = std::min({v0(1),v1(1),v2(1)});
        if (new_length < min_length) {
            min_length = new_length;
            face_with_min_length = i;
        }
    }
    return face_with_min_length;
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::reorder(
    index_type reference_face_index,
    const vertex_type& geometry_centre
) {
    const auto& vertices = this->_vertices;
    auto& faces = this->_faces;
    auto& face = faces[reference_face_index];
    const auto& v0 = vertices[face[0]];
    const auto& v1 = vertices[face[1]];
    const auto& v2 = vertices[face[2]];
    vertex_type face_centre = (v0+v1+v2) / 3;
    vertex_type n = surface_normal(v0,v1,v2);
    if (dot(n, face_centre-geometry_centre) < 0) {
        face.flip_winding();
    }
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::clear() {
    this->_vertices.clear();
    this->_vertex_normals.clear();
    this->_faces.clear();
    this->_face_normals.clear();
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::clear_normals() {
    this->_vertex_normals.clear();
    this->_face_normals.clear();
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::shrink_to_fit() {
    this->_vertices.shrink_to_fit();
    this->_vertex_normals.shrink_to_fit();
    this->_faces.shrink_to_fit();
    this->_face_normals.shrink_to_fit();
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::swap(Polyhedron<T,N>& rhs) {
    using std::swap;
    swap(this->_vertices, rhs._vertices);
    swap(this->_vertex_normals, rhs._vertex_normals);
    swap(this->_faces, rhs._faces);
    swap(this->_face_normals, rhs._face_normals);
}

template <class T, int N>
T
vtb::geometry::Polyhedron<T,N>::average_face_area() const {
    using vtb::base::Mean;
    Mean<T> sum;
    const auto& v = this->_vertices;
    for (const auto& f : this->_faces) {
        sum.update(area(Triangle<T,3>{v[f[0]], v[f[1]], v[f[2]]}));
    }
    return sum.mean();
}

template <class T, int N>
T
vtb::geometry::Polyhedron<T,N>::signed_volume() const {
    using vtb::geometry::signed_volume;
    vertex_type origin = this->centre();
    T volume{};
    const auto& v = this->_vertices;
    const auto& faces = this->_faces;
    for (const auto& f : faces) {
        Tetrahedron<T,N> t{origin, v[f[0]], v[f[1]], v[f[2]]};
        volume += signed_volume(t);
    }
    return volume;
}

template <class T, int N>
T
vtb::geometry::Polyhedron<T,N>::signed_volume_below(int d, T level) const {
    using vtb::geometry::signed_volume;
    vertex_type origin = this->centre();
    origin(d) = level;
    T volume{};
    const auto& v = this->_vertices;
    const auto& faces = this->_faces;
    for (const auto& f : faces) {
        const auto v0 = v[f[0]];
        const auto v1 = v[f[1]];
        const auto v2 = v[f[2]];
        if (v0(d) > level || v1(d) > level || v2(d) > level) { continue; }
        Tetrahedron<T,N> t{origin,v0,v1,v2};
        volume += signed_volume(t);
    }
    return volume;
}

template <class T, int N>
auto
vtb::geometry::Polyhedron<T,N>::centre() const -> vertex_type {
    vertex_type origin{T{}};
    for (const auto& v : vertices()) { origin += v; }
    const auto nvertices = vertices().size();
    if (nvertices > 0) { origin /= nvertices; }
    return origin;
}

template <class T, int N>
auto
vtb::geometry::Polyhedron<T,N>::centroid() const -> vertex_type {
    return mass_moments().centre;
}

template <class T, int N>
auto
vtb::geometry::Polyhedron<T,N>::bounding_box() const -> box_type {
    vertex_type min{std::numeric_limits<T>::max()};
    vertex_type max{std::numeric_limits<T>::lowest()};
    for (const auto& v : this->_vertices) {
        for (int i=0; i<N; ++i) {
            T m = v(i);
            if (m < min(i)) {
                min(i) = m;
            }
            if (max(i) < m) {
                max(i) = m;
            }
        }
    }
    return Rectangle<T,N>{min,max};
}

template <class T, int N>
auto
vtb::geometry::Polyhedron<T,N>::bounds(int dimension) const -> Bounds<T> {
    scalar_type min = std::numeric_limits<T>::max();
    scalar_type max = std::numeric_limits<T>::lowest();
    for (const auto& v : this->_vertices) {
        T m = v(dimension);
        if (m < min) { min = m; }
        if (max < m) { max = m; }
    }
    return Bounds<T>{min,max};
}

template <class T, int N>
auto
vtb::geometry::Polyhedron<T,N>::triangles() const -> triangle_array {
    triangle_array triangles;
    const auto& v = this->_vertices;
    const auto& faces = this->_faces;
    for (const auto& f : faces) {
        triangles.emplace_back(v[f[0]], v[f[1]], v[f[2]]);
    }
    return triangles;
}

template <class T, int N>
auto
vtb::geometry::Polyhedron<T,N>::blitz_vertices() const -> blitz_vertex_array {
    return to_blitz_array(this->_vertices);
}

template <class T, int N>
auto
vtb::geometry::Polyhedron<T,N>::blitz_vertex_normals() const -> blitz_vertex_array {
    return to_blitz_array(this->_vertex_normals);
}

template <class T, int N>
auto
vtb::geometry::Polyhedron<T,N>::blitz_faces() const -> blitz_face_array {
    return to_blitz_array(this->_faces);
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::move_to_centre_of_mass() {
    move(-mass_moments().centre);
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::move_to_centre_of_mass(T draught) {
    //move_to_centre_of_mass();
    using base::constants;
    using vtb::core::Bisection;
    using vtb::geometry::centroid;
    using vtb::geometry::signed_volume;
    using bool3 = Vertex<bool,3>;
    using vec3 = Vertex<T,3>;
    auto level = bounding_box().min()(2) + draught;
    auto mass = std::abs(signed_volume_below(2, level)) * constants<T>::water_density();
    auto goal_function = [&] (T delta) -> T {
        auto poly = *this;
        poly.move({0,0,delta});
        vec3 centre_of_gravity = T{};
        vec3 origin = centre_of_gravity;
        origin(2) = 0;
        vec3 centre_of_floatation{T{}}, centre_of_buoyancy{T{}}, centre_of_wind{T{}};
        vec3 wind_force{T{}}, buoyancy_force{T{}};
        T waterline_length = 0, total_dry_volume = 0, total_wet_volume = 0;
        const auto& vertices = poly._vertices;
        const auto& faces = poly._faces;
        constexpr auto p0 = constants<T>::atmospheric_pressure();
        constexpr auto g = constants<T>::g();
        constexpr auto air_density = constants<T>::air_density();
        constexpr auto water_density = constants<T>::water_density();
        for (const auto& f : faces) {
            Triangle<T,3> tr{vertices[f[0]],vertices[f[1]],vertices[f[2]]};
            auto n = surface_normal(tr);
            auto area = vtb::geometry::area(tr);
            auto volume = signed_volume(Tetrahedron<T,3>(origin,tr));
            auto centre = centroid(tr);
            T wet_area = 0, dry_area = 0;
            T wet_volume = 0, dry_volume = 0;
            bool3 above{tr[0][2] > 0, tr[1][2] > 0, tr[2][2] > 0};
            vec3 wet_centre{T{}}, dry_centre{T{}};
            bool all_dry = all(above);
            bool all_wet = !any(above);
            if (!all_wet && !all_dry) {
                vec3 a, b;
                bool x01 = above(0) ^ above(1);
                bool x02 = above(0) ^ above(2);
                bool x12 = above(1) ^ above(2);
                int outstanding = 0;
                if (x01 && x02) { outstanding = 0, a = tr[1], b = tr[2]; }
                else if (x01 && x12) { outstanding = 1, a = tr[0], b = tr[2]; }
                else { outstanding = 2, a = tr[0], b = tr[1]; }
                const auto& outstanding_vertex = tr[outstanding];
                Triangle<T,3> subtr{
                    outstanding_vertex,
                    a + (0-a(2))/(outstanding_vertex(2)-a(2))*(outstanding_vertex-a),
                    b + (0-b(2))/(outstanding_vertex(2)-b(2))*(outstanding_vertex-b),
                };
                auto subarea = vtb::geometry::area(subtr);
                {
                    Line_segment<T,3> waterline_segment(subtr[1],subtr[2]);
                    T segment_length = waterline_segment.length();
                    centre_of_floatation += waterline_segment.centre()*segment_length;
                    waterline_length += segment_length;
                }
                auto new_volume = signed_volume(Tetrahedron<T,3>(origin,subtr));
                if (outstanding_vertex(2) > 0) {
                    wet_area = area - subarea;
                    wet_volume = volume - new_volume;
                    wet_centre = T{0.25} * (subtr[1] + subtr[2] + a + b);
                    dry_area = subarea;
                    dry_volume = new_volume;
                    dry_centre = centroid(subtr);
                } else {
                    wet_area = subarea;
                    wet_volume = new_volume;
                    wet_centre = centroid(subtr);
                    dry_area = area - subarea;
                    dry_volume = volume - new_volume;
                    dry_centre = T{0.25} * (subtr[1] + subtr[2] + a + b);
                }
            }
            if (all_dry) {
                dry_centre = centre, dry_area = area, dry_volume = volume;
            }
            if (all_wet) {
                wet_centre = centre, wet_area = area, wet_volume = volume;
            }
            //n = unit(n);
            if (dry_area != 0) {
                auto z = dry_centre(2);
                centre_of_wind += dry_centre*dry_volume;
                total_dry_volume += dry_volume;
                wind_force += (p0 - air_density*g*z) * (-n) * dry_area;
            }
            if (wet_area != 0) {
                auto z = wet_centre(2);
                centre_of_buoyancy += wet_centre*wet_volume;
                total_wet_volume += wet_volume;
                buoyancy_force += (p0 - water_density*g*z) * (-n) * wet_area;
            }
        }
        buoyancy_force /= mass, wind_force /= mass;
        if (waterline_length != 0) { centre_of_floatation /= waterline_length; }
        if (total_wet_volume != 0) { centre_of_buoyancy /= total_wet_volume; }
        if (total_dry_volume != 0) { centre_of_wind /= total_dry_volume; }
        vec3 gravity_force{0,0,-g};
        vec3 force = gravity_force + buoyancy_force + wind_force;
        //const auto& G = centre_of_gravity;
        //const auto& B = centre_of_buoyancy;
        //const auto& W = centre_of_wind;
        //const auto& O = centre_of_floatation;
        //vec3 torque = cross(vec3(G-O), gravity_force) +
        //    cross(vec3(B-O), buoyancy_force) +
        //    cross(vec3(W-O), wind_force);
        return force(2);
    };
    Bisection<T,1> bisection;
    bisection.max_iterations(100);
    bisection.argument_precision(T{1e-5});
    bisection.function_precision(T{1e-5});
    auto bounds = bounding_box();
    auto delta = bisection(bounds.min()(2), bounds.max()(2), goal_function);
    move({0,0,delta});
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::move_to_centre_of_bounding_box() {
    using vtb::geometry::centroid;
    move(-centroid(bounding_box()));
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::move(const vertex_type& delta) {
    for (auto& v : this->_vertices) { v += delta; }
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::scale(const vertex_type& amount) {
    for (auto& v : this->_vertices) { v *= amount; }
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::wall(scalar_type thickness) {
    if (vertices().empty()) { return; }
    for (auto& v : this->_vertices) {
        auto v_length = length(v);
        auto s = std::max(T{}, (v_length-thickness)/v_length);
        v *= s;
    }
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::flip(int dim) {
    flip_vertices(this->_vertices, dim);
    flip_vertices(this->_vertex_normals, dim);
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::mirror(int dim) {
    auto& vertices = this->_vertices;
    const auto nvertices = vertices.size();
    if (nvertices == 0) {
        return;
    }
    T min = std::numeric_limits<T>::max();
    for (const auto& v : vertices) {
        if (v(dim) < min) {
            min = v(dim);
        }
    }
    this->move(-min);
    mirror_vertices(vertices, dim);
    auto& faces = this->_faces;
    const auto nfaces = faces.size();
    if (nfaces != 0) {
        faces.resize(nfaces*2);
        for (size_t i=0; i<nfaces; ++i) {
            auto f = faces[i];
            f += nvertices;
            faces[nfaces + i] = f;
        }
    }
    if (!this->_vertex_normals.empty()) {
        mirror_vertices(this->_vertex_normals, dim);
    }
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::rotate(int dim, int degrees) {
    using vtb::geometry::rotate;
    if (dim < 0 || dim > 2) {
        throw std::invalid_argument{"bad dimension"};
    }
    if (degrees % 90 != 0) {
        throw std::invalid_argument{"bad degrees"};
    }
    if (degrees == 0) {
        return;
    }
    degrees %= 360;
    if (degrees < 0) {
        degrees += 360;
    }
    auto& vertices = this->_vertices;
    auto& normals = this->_vertex_normals;
    if (degrees == 90) {
        if (dim == 0) {
            rotate_vertices(vertices, normals, rotate<0,90,T,N>);
        } else if (dim == 1) {
            rotate_vertices(vertices, normals, rotate<1,90,T,N>);
        } else {
            rotate_vertices(vertices, normals, rotate<2,90,T,N>);
        }
    } else if (degrees == 180) {
        if (dim == 0) {
            rotate_vertices(vertices, normals, rotate<0,180,T,N>);
        } else if (dim == 1) {
            rotate_vertices(vertices, normals, rotate<1,180,T,N>);
        } else {
            rotate_vertices(vertices, normals, rotate<2,180,T,N>);
        }
    } else {
        if (dim == 0) {
            rotate_vertices(vertices, normals, rotate<0,270,T,N>);
        } else if (dim == 1) {
            rotate_vertices(vertices, normals, rotate<1,270,T,N>);
        } else {
            rotate_vertices(vertices, normals, rotate<2,270,T,N>);
        }
    }
}

template <class T, int N>
auto
vtb::geometry::Polyhedron<T,N>
::mass_moments_below(int d, scalar_type level) const -> Mass_moments<T,N> {
    #define VTB_GEOMETRY_SUBEXPRESSIONS(w0,w1,w2, f1,f2,f3, g0,g1,g2) \
        tmp0 = w0+w1; f1 = tmp0+w2; \
        tmp1 = w0*w0; tmp2 = tmp1+w1*tmp0;  f2 = tmp2+w2*f1; \
        f3 = w0*tmp1 + w1*tmp2 + w2*f2; \
        g0 = f2+w0*(f1+w0); g1 = f2+w1*(f1+w1); g2 = f2+w2*(f1+w2)
    static const T mult[10] = {
        T{1}/T{6},
        T{1}/T{24}, T{1}/T{24}, T{1}/T{24},
        T{1}/T{60}, T{1}/T{60}, T{1}/T{60},
        T{1}/T{120}, T{1}/T{120}, T{1}/T{120},
    };
    T intg[10] = {}; // 1, x, y, z, x^2, y^2, z^2, xy, yz, zy
    const auto& v = this->_vertices;
    const auto& faces = this->_faces;
    for (const auto& f : faces) {
        // vertices
        Triangle<T,N> t{v[f[0]], v[f[1]], v[f[2]]};
        if (t[0][d] > level || t[1][d] > level || t[2][d] > level) { continue; }
        auto x0 = t[0][0], y0 = t[0][1], z0 = t[0][2];
        auto x1 = t[1][0], y1 = t[1][1], z1 = t[1][2];
        auto x2 = t[2][0], y2 = t[2][1], z2 = t[2][2];
        // edges
        auto a1 = x1-x0, b1 = y1-y0, c1 = z1-z0;
        auto a2 = x2-x0, b2 = y2-y0, c2 = z2-z0;
        // cross products
        auto d0 = b1*c2-b2*c1, d1 = a2*c1-a1*c2, d2 = a1*b2-a2*b1;
        // common subexpressions
        T tmp0, tmp1, tmp2;
        T f1x, f2x, f3x, f1y, f2y, f3y, f1z, f2z, f3z;
        T g0x, g1x, g2x, g0y, g1y, g2y, g0z, g1z, g2z;
        VTB_GEOMETRY_SUBEXPRESSIONS(x0,x1,x2, f1x,f2x,f3x, g0x,g1x,g2x);
        VTB_GEOMETRY_SUBEXPRESSIONS(y0,y1,y2, f1y,f2y,f3y, g0y,g1y,g2y);
        VTB_GEOMETRY_SUBEXPRESSIONS(z0,z1,z2, f1z,f2z,f3z, g0z,g1z,g2z);
        // integrals
        intg[0] += d0*f1x;
        intg[1] += d0*f2x; intg[2] += d1*f2y; intg[3] += d2*f2z;
        intg[4] += d0*f3x; intg[5] += d1*f3y; intg[6] += d2*f3z;
        intg[7] += d0*(y0*g0x + y1*g1x + y2*g2x);
        intg[8] += d1*(z0*g0y + z1*g1y + z2*g2y);
        intg[9] += d2*(x0*g0z + x1*g1z + x2*g2z);
    }
    for (int i=0; i<10; ++i) { intg[i] *= mult[i]; }
    Mass_moments<T,N> prop;
    auto& cm = prop.centre;
    auto& mass = prop.volume;
    mass = intg[0];
    cm(0) = intg[1];
    cm(1) = intg[2];
    cm(2) = intg[3];
    if (mass != 0) { cm /= mass; }
    prop.inertia.xx = intg[5] + intg[6] - mass*(cm(1)*cm(1)+cm(2)*cm(2));
    prop.inertia.yy = intg[4] + intg[6] - mass*(cm(2)*cm(2)+cm(0)*cm(0));
    prop.inertia.zz = intg[4] + intg[5] - mass*(cm(0)*cm(0)+cm(1)*cm(1));
    prop.inertia.xy = (intg[7] - mass*cm(0)*cm(1));
    prop.inertia.yz = (intg[8] - mass*cm(1)*cm(2));
    prop.inertia.xz = (intg[9] - mass*cm(2)*cm(0));
    if (mass != 0) {
        prop.inertia.xx /= mass; prop.inertia.yy /= mass; prop.inertia.zz /= mass;
        prop.inertia.xy /= mass; prop.inertia.yz /= mass; prop.inertia.xz /= mass;
    }
    return prop;
    #undef VTB_GEOMETRY_SUBEXPRESSIONS
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::gnuplot(std::ostream& out) const {
    std::vector<Triangle<T,N>> triangles;
    const auto& vertices = this->_vertices;
    const auto& faces = this->_faces;
    for (const auto& f : faces) {
        const auto& v0 = vertices[f[0]];
        const auto& v1 = vertices[f[1]];
        const auto& v2 = vertices[f[2]];
        triangles.emplace_back(v0,v1,v2);
    }
    std::sort(triangles.begin(), triangles.end(), CompareTriangles<T,N>{});
    std::reverse(triangles.begin(), triangles.end());
    for (const auto& t : triangles) {
        const auto& v0 = t[0];
        const auto& v1 = t[1];
        const auto& v2 = t[2];
        ::gnuplot(out, v0); out << '\n';
        ::gnuplot(out, v1); out << "\n\n";
        ::gnuplot(out, v2); out << '\n';
        ::gnuplot(out, v2); out << "\n\n\n";
    }
}

template <class T, int N>
void
vtb::geometry::Polyhedron<T,N>::object(std::ostream& out) const {
    const auto& geometry = *this;
    for (const auto& v : geometry.vertices()) {
        out << "v " << v(0) << ' ' << v(1) << ' ' << v(2) << '\n';
    }
    for (const auto& v : geometry.vertex_normals()) {
        out << "vn " << v(0) << ' ' << v(1) << ' ' << v(2) << '\n';
    }
    for (const auto& face : geometry.faces()) {
        out << 'f';
        for (const auto& idx : face) {
            out << ' ' << (idx + 1);
        }
        out << '\n';
    }
}

template class vtb::geometry::Polyhedron<VTB_REAL_TYPE,3>;

template <class T, int N>
vtb::core::bstream&
vtb::geometry::operator<<(vtb::core::bstream& out, const Polyhedron<T,N>& rhs) {
    out << rhs.vertices();
    out << rhs.vertex_normals();
    out << rhs.faces();
    out << rhs.face_normals();
    return out;
}

template vtb::core::bstream&
vtb::geometry::operator<<(
    vtb::core::bstream& out,
    const Polyhedron<VTB_REAL_TYPE,3>& rhs
);

template <class T, int N>
vtb::core::bstream&
vtb::geometry::operator>>(vtb::core::bstream& in, Polyhedron<T,N>& rhs) {
    in >> rhs._vertices;
    in >> rhs._vertex_normals;
    in >> rhs._faces;
    in >> rhs._face_normals;
    return in;
}

template vtb::core::bstream&
vtb::geometry::operator>>(
    vtb::core::bstream& in,
    Polyhedron<VTB_REAL_TYPE,3>& rhs
);

template <class T,int N>
T
vtb::geometry::winding_number(const Polyhedron<T,N>& geometry, const Vertex<T,N>& origin) {
    const auto& v = geometry.vertices();
    const auto& faces = geometry.faces();
    T wn = 0;
    for (const auto& f : faces) {
        Tetrahedron<T,N> t{origin, v[f[0]], v[f[1]], v[f[2]]};
        wn += solid_angle(t);
    }
    return wn / vtb::base::constants<T>::pi(4);
}

template VTB_REAL_TYPE
vtb::geometry::winding_number(const Polyhedron<VTB_REAL_TYPE,3>&, const Vertex<VTB_REAL_TYPE,3>&);

template <class T, int N>
auto
vtb::geometry::inertia_tensor(const Polyhedron<T,N>& body) -> Inertia_tensor<T,N> {
    using std::abs;
    using blitz::pow2;
    Inertia_tensor<T,N> I;
    const auto& v = body.vertices();
    const auto& faces = body.faces();
    const auto& origin = centroid(body);
    T total_volume = 0;
    for (const auto& f : faces) {
        Tetrahedron<T,3> t{origin, v[f[0]], v[f[1]], v[f[2]]};
        auto t_signed_volume = signed_volume(t);
        auto t_volume = abs(t_signed_volume);
        #define x(i) t[i-1][0]
        #define y(i) t[i-1][1]
        #define z(i) t[i-1][2]
        I.x += t_volume*(
            pow2(y(1)) + y(1)*y(2) +
            pow2(y(2)) + y(1)*y(3) + y(2)*y(3) +
            pow2(y(3)) + y(1)*y(4) + y(2)*y(4) + y(3)*y(4) +
            pow2(y(4)) +
            pow2(z(1)) + z(1)*z(2) +
            pow2(z(2)) + z(1)*z(3) + z(2)*z(3) +
            pow2(z(3)) + z(1)*z(4) + z(2)*z(4) + z(3)*z(4) +
            pow2(z(4))) / 10;
        I.y += t_volume*(
            pow2(x(1)) + x(1)*x(2) +
            pow2(x(2)) + x(1)*x(3) + x(2)*x(3) +
            pow2(x(3)) + x(1)*x(4) + x(2)*x(4) + x(3)*x(4) +
            pow2(x(4)) +
            pow2(z(1)) + z(1)*z(2) +
            pow2(z(2)) + z(1)*z(3) + z(2)*z(3) +
            pow2(z(3)) + z(1)*z(4) + z(2)*z(4) + z(3)*z(4) +
            pow2(z(4))) / 10;
        I.z += t_volume*(
            pow2(x(1)) + x(1)*x(2) +
            pow2(x(2)) + x(1)*x(3) + x(2)*x(3) +
            pow2(x(3)) + x(1)*x(4) + x(2)*x(4) + x(3)*x(4) +
            pow2(x(4)) +
            pow2(y(1)) + y(1)*y(2) +
            pow2(y(2)) + y(1)*y(3) + y(2)*y(3) +
            pow2(y(3)) + y(1)*y(4) + y(2)*y(4) + y(3)*y(4) +
            pow2(y(4))) / 10;
        I.yz += t_volume*(
            2*y(1)*z(1) + y(2)*z(1) + y(3)*z(1) + y(4)*z(1) + y(1)*z(2) +
            2*y(2)*z(2) + y(3)*z(2) + y(4)*z(2) + y(1)*z(3) + y(2)*z(3) +
            2*y(3)*z(3) + y(4)*z(3) + y(1)*z(4) + y(2)*z(4) + y(3)*z(4) +
            2*y(4)*z(4)) / 20;
        I.xz += t_volume*(
            2*x(1)*z(1) + x(2)*z(1) + x(3)*z(1) + x(4)*z(1) + x(1)*z(2) +
            2*x(2)*z(2) + x(3)*z(2) + x(4)*z(2) + x(1)*z(3) + x(2)*z(3) +
            2*x(3)*z(3) + x(4)*z(3) + x(1)*z(4) + x(2)*z(4) + x(3)*z(4) +
            2*x(4)*z(4)) / 20;
        I.xy += t_volume*(
            2*x(1)*y(1) + x(2)*y(1) + x(3)*y(1) + x(4)*y(1) + x(1)*y(2) +
            2*x(2)*y(2) + x(3)*y(2) + x(4)*y(2) + x(1)*y(3) + x(2)*y(3) +
            2*x(3)*y(3) + x(4)*y(3) + x(1)*y(4) + x(2)*y(4) + x(3)*y(4) +
            2*x(4)*y(4)) / 20;
        #undef x
        #undef y
        #undef z
        total_volume += t_signed_volume;
    }
    if (total_volume != T{}) { I.scale(T{1}/total_volume); }
    return I;
}

template auto
vtb::geometry::inertia_tensor(const Polyhedron<VTB_REAL_TYPE,3>& body) -> Inertia_tensor<VTB_REAL_TYPE,3>;