erl_geometry/include/erl_geometry/marching_squares.hpp at main · ExistentialRobotics/erl_geometry · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
#pragma once

#include "erl_common/eigen.hpp"

namespace erl::geometry {

    struct MarchingSquares {
        struct Edge {
            long v1x, v1y, v2x, v2y;

            bool
            operator==(const Edge &other) const {
                return v1x == other.v1x && v1y == other.v1y && v2x == other.v2x && v2y == other.v2y;
            }

            template<typename H>
            friend H
            AbslHashValue(H h, const Edge &e) {
                return H::combine(std::move(h), e.v1x, e.v1y, e.v2x, e.v2y);
            }
        };

        struct HashEdge {
            std::size_t
            operator()(const Edge &e) const noexcept {
                constexpr std::hash<long> long_hash;
                std::size_t &&h_1 = long_hash(e.v1x);
                std::size_t &&h_2 = long_hash(e.v1y);
                std::size_t &&h_3 = long_hash(e.v2x);
                std::size_t &&h_4 = long_hash(e.v2y);
                return h_1 ^ h_2 << 1 ^ h_3 << 1 ^ h_4 << 1;
            }
        };

        static const std::array<Edge, 5> kBaseEdgeTable;
        static const std::array<std::array<Edge, 4>, 16> kEdgePairTable;
        static const int kUniqueEdgeIndexTable[16][5];
        static const int kLineVertexIndexTable[16][5];
        static const int kEdgeVertexIndexTable[4][2];
        static const int kSquareVertexCodes[4][2];
        static const int kSquareEdgeCodes[4][3];

        static const int *
        GetEdgeIndices(int config) {
            if (config <= 0 || config >= 15) { return nullptr; }
            return kUniqueEdgeIndexTable[config];
        }

        static const int *
        GetVertexIndices(int config) {
            if (config <= 0 || config >= 15) { return nullptr; }
            return kLineVertexIndexTable[config];
        }

        static const int *
        GetUniqueEdgeIndices(int config) {
            if (config <= 0 || config >= 15) { return nullptr; }
            return kUniqueEdgeIndexTable[config];
        }

        static const int *
        GetEdgeVertexIndices(int edge_index) {
            if (edge_index < 0 || edge_index >= 4) { return nullptr; }
            return kEdgeVertexIndexTable[edge_index];
        }

        static const int *
        GetVertexCode(int vertex_index) {
            if (vertex_index < 0 || vertex_index >= 4) { return nullptr; }
            return kSquareVertexCodes[vertex_index];
        }

        static const int *
        GetEdgeCode(int edge_index) {
            if (edge_index < 0 || edge_index >= 4) { return nullptr; }
            return kSquareEdgeCodes[edge_index];
        }

        static int
        CalculateVertexConfigIndex(const double *vertex_values, double iso_value);

        static int
        CalculateVertexConfigIndex(const float *vertex_values, float iso_value);

        static void
        SingleSquare(
            const Eigen::Ref<const Eigen::Matrix<double, 2, 4>> &vertex_coords,
            const Eigen::Ref<const Eigen::Vector<double, 4>> &values,
            double iso_value,
            std::vector<Eigen::Vector2d> &vertices,
            std::vector<Eigen::Vector2i> &lines);

        static void
        SingleSquare(
            const Eigen::Ref<const Eigen::Matrix<float, 2, 4>> &vertex_coords,
            const Eigen::Ref<const Eigen::Vector<float, 4>> &values,
            float iso_value,
            std::vector<Eigen::Vector2f> &vertices,
            std::vector<Eigen::Vector2i> &lines);

        static void
        Run(const Eigen::Ref<const Eigen::MatrixXd> &img,
            double iso_value,
            Eigen::Matrix2Xd &vertices,
            Eigen::Matrix2Xi &lines_to_vertices,
            Eigen::Matrix2Xi &objects_to_lines);

        static void
        Run(const Eigen::Ref<const Eigen::MatrixXf> &img,
            float iso_value,
            Eigen::Matrix2Xf &vertices,
            Eigen::Matrix2Xi &lines_to_vertices,
            Eigen::Matrix2Xi &objects_to_lines);

        static void
        SortLinesToObjects(Eigen::Matrix2Xi &lines_to_vertices, Eigen::Matrix2Xi &objects_to_lines);
    };
}  // namespace erl::geometry