Tzx/0122 (#5)

* lint code

* update

* there is a bug

* fix, sync code to headers

* update docs, ready to release

Co-authored-by: TANG ZHIXIONG <[email protected]>

CMakeLists.txt

@@ -20,7 +20,7 @@ elseif(CMAKE_BUILD_TYPE STREQUAL "Release")
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
 endif()
 
-include_directories(BEFORE ${PROJECT_SOURCE_DIR}/headers/include
+include_directories(${PROJECT_SOURCE_DIR}/headers/include
                     ${PROJECT_SOURCE_DIR}/headers/include/cubao)
 
 set(CMAKE_CXX_STANDARD 17)

Makefile

@@ -77,6 +77,13 @@ tar.gz:
 	tar -cvz --exclude .git -f ../fast_crossing.tar.gz .
 	ls -alh ../fast_crossing.tar.gz
 
+
+SYNC_OUTPUT_DIR := headers/include/cubao
+sync_headers:
+	cp src/fast_crossing.hpp $(SYNC_OUTPUT_DIR)
+	cp src/pybind11_fast_crossing.hpp $(SYNC_OUTPUT_DIR)
+	cp src/pybind11_flatbush.hpp $(SYNC_OUTPUT_DIR)
+
 # https://stackoverflow.com/a/25817631
 echo-%  : ; @echo -n $($*)
 Echo-%  : ; @echo $($*)

README.md

@@ -1 +1,209 @@
 # fast crossing
+
+## Installation
+
+### via pip
+
+```
+pip install fast-crossing
+```
+
+### from source
+
+```bash
+git clone --recursive https://github.com/cubao/fast-crossing
+pip install ./fast-crossing
+```
+
+Or
+
+```
+pip install git+https://github.com/cubao/fast-crossing.git
+```
+
+(you can build wheels for later reuse by ` pip wheel git+https://github.com/cubao/fast-crossing.git`)
+
+## Usage & Tests
+
+See [`tests/test_basic.py`](tests/test_basic.py):
+
+```python
+import numpy as np
+import pytest
+from fast_crossing import FastCrossing
+
+
+def test_fast_crossing():
+    fc = FastCrossing()
+
+    # add your polylines
+    """
+                    2 C
+                    |
+                    1 D
+    0               |                  5
+    A---------------o------------------B
+                    |
+                    |
+                    -2 E
+    """
+    fc.add_polyline(np.array([[0.0, 0.0], [5.0, 0.0]]))  # AB
+    fc.add_polyline(np.array([[2.5, 2.0], [2.5, 1.0], [2.5, -2.0]]))  # CDE
+
+    # build index
+    fc.finish()
+
+    # num_poylines
+    assert 2 == fc.num_poylines()
+    rulers = fc.polyline_rulers()
+    assert len(rulers) == 2
+    ruler0 = fc.polyline_ruler(0)
+    ruler1 = fc.polyline_ruler(1)
+    assert not ruler0.is_wgs84()
+    assert not ruler1.is_wgs84()
+    assert ruler0.length() == 5
+    assert ruler1.length() == 4
+    assert fc.polyline_ruler(10) is None
+
+    # intersections
+    ret = fc.intersections([1.5, 0], [3.5, 2])
+    assert len(ret) == 2
+    assert np.linalg.norm(fc.coordinates(ret[0]) - [1.5, 0, 0]) < 1e-15
+    assert np.linalg.norm(fc.coordinates(ret[1]) - [2.5, 1, 0]) < 1e-15
+    xyz = fc.coordinates(0, 0, 0.2)
+    assert np.linalg.norm(xyz - [1.0, 0, 0]) < 1e-15
+    with pytest.raises(IndexError) as excinfo:
+        xyz = fc.coordinates(2, 0, 0.5)
+    assert "map::at" in str(excinfo)
+
+    # query all line segment intersections
+    # [
+    #    (array([2.5, 0. ]),
+    #     array([0.5       , 0.33333333]),
+    #     array([0, 0], dtype=int32),
+    #     array([1, 1], dtype=int32))
+    # ]
+    ret = fc.intersections()
+    # print(ret)
+    assert len(ret) == 1
+    for xy, ts, label1, label2 in ret:
+        # xy 是交点，即图中的 o 点坐标
+        # t,s 是分位点，(0.5, 0.33)
+        #        0.5  ->   o 在 AB 1/2 处
+        #        0.33 ->   o 在 DE 1/3 处
+        # label1 是 line segment 索引，(polyline_index, point_index)
+        #        e.g. (0, 0)，polyline AB 的第一段
+        # label2 是另一个条 line seg 的索引
+        #        e.g. (1, 1)，polyline CDE 的第二段（DE 段）
+        # print(xy)
+        # print(ts)
+        # print(label1)
+        # print(label2)
+        assert np.all(xy == [2.5, 0])
+        assert np.all(ts == [0.5, 1 / 3.0])
+        assert np.all(label1 == [0, 0])
+        assert np.all(label2 == [1, 1])
+
+    # query intersections against provided polyline
+    polyline = np.array([[-6.0, -1.0], [-5.0, 1.0], [5.0, -1.0]])
+    ret = fc.intersections(polyline)
+    ret = np.array(ret)  # 还是转化成 numpy 比较好用
+    xy = ret[:, 0]  # 直接取出所有交点
+    ts = ret[:, 1]  # 所有分位点
+    label1 = ret[:, 2]  # 所有 label1（当前 polyline 的 label）
+    label2 = ret[:, 3]  # tree 中 line segs 的 label
+    # print(ret, xy, ts, label1, label2)
+    assert np.all(xy[0] == [0, 0])
+    assert np.all(xy[1] == [2.5, -0.5])
+    assert np.all(ts[0] == [0.5, 0])
+    assert np.all(ts[1] == [0.75, 0.5])
+    assert np.all(label1 == [[0, 1], [0, 1]])
+    assert np.all(label2 == [[0, 0], [1, 1]])
+
+    polyline2 = np.column_stack((polyline, np.zeros(len(polyline))))
+    ret2 = np.array(fc.intersections(polyline2[:, :2]))
+    assert str(ret) == str(ret2)
+
+
+def test_fast_crossing_intersection3d():
+    fc = FastCrossing()
+    """
+                    2 C
+                    |
+                    1 D
+    0               |                  5
+    A---------------o------------------B
+                    |
+                    |
+                    -2 E
+    """
+    fc.add_polyline(np.array([[0.0, 0.0, 0.0], [5.0, 0.0, 100]]))  # AB
+    fc.add_polyline(np.array([[2.5, 2.0, 0.0], [2.5, 1.0, 100], [2.5, -2.0, 0]]))  # CDE
+    fc.finish()
+    ret = fc.intersections()
+    assert len(ret) == 1
+    ret = ret[0]
+    xyz1 = fc.coordinates(ret, second=False)
+    xyz2 = fc.coordinates(ret)
+    assert np.linalg.norm(xyz1 - [2.5, 0, 50]) < 1e-10
+    assert np.linalg.norm(xyz2 - [2.5, 0, 2 / 3 * 100.0]) < 1e-10
+
+
+def test_fast_crossing_auto_rebuild_flatbush():
+    fc = FastCrossing()
+    fc.add_polyline(np.array([[0.0, 0.0, 0.0], [5.0, 0.0, 100]]))  # AB
+    fc.add_polyline(np.array([[2.5, 2.0, 0.0], [2.5, 1.0, 100], [2.5, -2.0, 0]]))  # CDE
+    ret = fc.intersections()
+    assert len(ret) == 1
+
+    fc.add_polyline([[1.5, 0], [3.5, 2]])
+    ret = fc.intersections()
+    assert len(ret) == 4  # should dedup to 3?
+
+
+def test_fast_crossing_filter_by_z():
+    fc = FastCrossing()
+    fc.add_polyline([[0, 0, 0], [1, 0, 0]])
+    fc.add_polyline([[0, 0, 10], [1, 0, 10]])
+    fc.add_polyline([[0, 0, 20], [1, 0, 20]])
+    ret = fc.intersections([[0.5, -1], [0.5, 1]])
+    assert len(ret) == 3
+
+    ret = fc.intersections([[0.5, -1], [0.5, 1]], z_min=-1, z_max=1)
+    assert len(ret) == 1
+    assert fc.coordinates(ret[0])[2] == 0
+
+    ret = fc.intersections([[0.5, -1, 10], [0.5, 1, 10]], z_min=-1, z_max=1)
+    assert len(ret) == 1
+    assert fc.coordinates(ret[0])[2] == 10
+
+    ret = fc.intersections([[0.5, -1, 20], [0.5, 1, 20]], z_min=-1, z_max=1)
+    assert len(ret) == 1
+    assert fc.coordinates(ret[0])[2] == 20
+
+    ret = fc.intersections([[0.5, -1, 15], [0.5, 1, 15]], z_min=-6, z_max=6)
+    assert len(ret) == 2
+    assert fc.coordinates(ret[0])[2] == 10
+    assert fc.coordinates(ret[1])[2] == 20
+
+
+def test_fast_crossing_dedup():
+    # should be stable
+    for _ in range(100):
+        fc = FastCrossing()
+        fc.add_polyline([[0, 0, 0], [1, 0, 0], [2, 0, 0]])
+        fc.add_polyline([[0, 1, 0], [1, 1, 0], [2, 1, 0]])
+
+        ret = fc.intersections([[1, -1], [1, 1]])
+        assert len(ret) == 2
+        assert np.all(ret[0][-1] == [0, 0]), ret
+        assert np.all(ret[1][-1] == [1, 0]), ret
+        assert ret[0][1][1] == 1.0, ret
+        assert ret[1][1][1] == 1.0, ret
+
+        ret = fc.intersections([[1, -1], [1, 1]], dedup=False)
+        # for idx, row in enumerate(ret):
+        #     print(idx, row)
+        assert len(ret) == 4
+
+```

setup.py

@@ -124,7 +124,7 @@ def build_extension(self, ext):
 # logic and declaration, and simpler if you include description/version in a file.
 setup(
     name="fast_crossing",
-    version="0.0.1",
+    version="0.0.2",
     author="tzx",
     author_email="[email protected]",
     url="https://github.com/cubao/fast-crossing",

src/fast_crossing.hpp

@@ -141,7 +141,13 @@ struct FastCrossing
                 label1, label2);
         }
         std::sort(ret.begin(), ret.end(), [](const auto &t1, const auto &t2) {
-            return std::get<1>(t1)[0] < std::get<1>(t2)[0];
+            double tt1 = std::get<1>(t1)[0];
+            double tt2 = std::get<1>(t2)[0];
+            if (tt1 != tt2) {
+                return tt1 < tt2;
+            }
+            return std::make_tuple(std::get<3>(t1)[0], std::get<3>(t1)[1]) <
+                   std::make_tuple(std::get<3>(t2)[0], std::get<3>(t2)[1]);
         });
         if (dedup) {
             auto last = std::unique(
@@ -150,7 +156,8 @@ struct FastCrossing
                     if (std::get<3>(t1)[0] != std::get<3>(t2)[0]) {
                         return false;
                     }
-                    return std::get<3>(t1) == std::get<3>(t2) ||
+                    return (std::get<3>(t1)[1] == std::get<3>(t2)[1] &&
+                            std::get<1>(t1)[1] == std::get<1>(t2)[1]) ||
                            ((std::get<3>(t1)[1] + 1) == std::get<3>(t2)[1] &&
                             std::get<1>(t1)[1] == 1.0 &&
                             std::get<1>(t2)[1] == 0.0) ||
@@ -187,13 +194,16 @@ struct FastCrossing
                 auto &curr = hit.front();
                 auto &prev_label = std::get<3>(prev);
                 auto &curr_label = std::get<3>(curr);
-                if (prev_label[0] == curr_label[0] &&
-                    (((prev_label[1] + 1) == curr_label[1] &&
-                      std::get<2>(prev)[1] == 1.0 &&
-                      std::get<2>(curr)[1] == 0.0) ||
-                     ((prev_label[1] - 1) == curr_label[1] &&
-                      std::get<2>(prev)[1] == 0.0 &&
-                      std::get<2>(curr)[1] == 1.0))) {
+                if ((prev_label == curr_label &&
+                     std::get<1>(prev)[1] == std::get<1>(curr)[1]) ||
+                    (prev_label[0] == curr_label[0] &&
+                     (prev_label[1] + 1) == curr_label[1] &&
+                     std::get<1>(prev)[1] == 1.0 &&
+                     std::get<1>(curr)[1] == 0.0) ||
+                    (prev_label[0] == curr_label[0] &&
+                     (prev_label[1] - 1) == curr_label[1] &&
+                     std::get<1>(prev)[1] == 0.0 &&
+                     std::get<1>(curr)[1] == 1.0)) {
                     has_dup = true;
                 }
             }
@@ -202,13 +212,14 @@ struct FastCrossing
         return ret;
     }
 
-    std::vector<IntersectionType> intersections(
-        const Eigen::Ref<const FlatBush::PolylineType> &polyline) const
+    std::vector<IntersectionType>
+    intersections(const Eigen::Ref<const FlatBush::PolylineType> &polyline,
+                  bool dedup = true) const
     {
         PolylineType Nx3(polyline.rows(), 3);
         Nx3.leftCols<2>() = polyline;
         Nx3.col(2).setZero();
-        return intersections(Nx3, true);
+        return intersections(Nx3, dedup);
     }
 
     static Eigen::Vector3d coordinates(const RowVectors &xyzs, int seg_index,

src/pybind11_fast_crossing.hpp

@@ -63,13 +63,12 @@ CUBAO_INLINE void bind_fast_crossing(py::module &m)
                  &FastCrossing::intersections, py::const_),
              "polyline"_a, py::kw_only(), "dedup"_a = true,
              "crossing intersections with polyline (sorted by t ratio)")
-        .def(
-            "intersections",
-            py::overload_cast<
-                const Eigen::Ref<const FastCrossing::FlatBush::PolylineType> &>(
-                &FastCrossing::intersections, py::const_),
-            "polyline"_a,
-            "crossing intersections with polyline (sorted by t ratio)")
+        .def("intersections",
+             py::overload_cast<
+                 const Eigen::Ref<const FastCrossing::FlatBush::PolylineType> &,
+                 bool>(&FastCrossing::intersections, py::const_),
+             "polyline"_a, py::kw_only(), "dedup"_a = true,
+             "crossing intersections with polyline (sorted by t ratio)")
         .def("intersections",
              py::overload_cast<const FastCrossing::PolylineType &, double,
                                double, bool>(&FastCrossing::intersections,

tests/test_basic.py

@@ -155,3 +155,23 @@ def test_fast_crossing_filter_by_z():
     assert len(ret) == 2
     assert fc.coordinates(ret[0])[2] == 10
     assert fc.coordinates(ret[1])[2] == 20
+
+
+def test_fast_crossing_dedup():
+    # should be stable
+    for _ in range(100):
+        fc = FastCrossing()
+        fc.add_polyline([[0, 0, 0], [1, 0, 0], [2, 0, 0]])
+        fc.add_polyline([[0, 1, 0], [1, 1, 0], [2, 1, 0]])
+
+        ret = fc.intersections([[1, -1], [1, 1]])
+        assert len(ret) == 2
+        assert np.all(ret[0][-1] == [0, 0]), ret
+        assert np.all(ret[1][-1] == [1, 0]), ret
+        assert ret[0][1][1] == 1.0, ret
+        assert ret[1][1][1] == 1.0, ret
+
+        ret = fc.intersections([[1, -1], [1, 1]], dedup=False)
+        # for idx, row in enumerate(ret):
+        #     print(idx, row)
+        assert len(ret) == 4