diff --git a/.vscode/settings.json b/.vscode/settings.json index c83d163..310e3fd 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -93,7 +93,12 @@ "regex": "cpp", "future": "cpp", "__bits": "cpp", - "bit": "cpp" + "bit": "cpp", + "charconv": "cpp", + "concepts": "cpp", + "memory_resource": "cpp", + "ranges": "cpp", + "stop_token": "cpp" }, "workbench.colorCustomizations": { // "activityBar.background": "#b80ae3", diff --git a/src/bindings/pybind11_helpers.hpp b/src/bindings/pybind11_helpers.hpp new file mode 100644 index 0000000..fe36188 --- /dev/null +++ b/src/bindings/pybind11_helpers.hpp @@ -0,0 +1,145 @@ +#pragma once + +// migrated from +// https://github.com/cubao/geobuf-cpp/blob/dev/src/geobuf/pybind11_helpers.hpp + +#include +#include +#include +#include + +#include "nano_fmm/types.hpp" +#include "nano_fmm/rapidjson_helpers.hpp" + +namespace nano_fmm +{ +namespace py = pybind11; +using namespace pybind11::literals; +using rvp = py::return_value_policy; + +inline RapidjsonValue __py_int_to_rapidjson(const py::handle &obj) +{ + try { + auto num = obj.cast(); + if (py::int_(num).equal(obj)) { + return RapidjsonValue(num); + } + } catch (...) { + } + try { + auto num = obj.cast(); + if (py::int_(num).equal(obj)) { + return RapidjsonValue(num); + } + } catch (...) { + } + throw std::runtime_error( + "failed to convert to rapidjson, invalid integer: " + + py::repr(obj).cast()); +} + +inline RapidjsonValue to_rapidjson(const py::handle &obj, + RapidjsonAllocator &allocator) +{ + if (obj.ptr() == nullptr || obj.is_none()) { + return {}; + } + if (py::isinstance(obj)) { + return RapidjsonValue(obj.cast()); + } + if (py::isinstance(obj)) { + return __py_int_to_rapidjson(obj); + } + if (py::isinstance(obj)) { + return RapidjsonValue(obj.cast()); + } + if (py::isinstance(obj)) { + // https://github.com/pybind/pybind11_json/blob/master/include/pybind11_json/pybind11_json.hpp#L112 + py::module base64 = py::module::import("base64"); + auto str = base64.attr("b64encode")(obj) + .attr("decode")("utf-8") + .cast(); + return RapidjsonValue(str.data(), str.size(), allocator); + } + if (py::isinstance(obj)) { + auto str = obj.cast(); + return RapidjsonValue(str.data(), str.size(), allocator); + } + if (py::isinstance(obj) || py::isinstance(obj)) { + RapidjsonValue arr(rapidjson::kArrayType); + for (const py::handle &value : obj) { + arr.PushBack(to_rapidjson(value, allocator), allocator); + } + return arr; + } + if (py::isinstance(obj)) { + RapidjsonValue kv(rapidjson::kObjectType); + for (const py::handle &key : obj) { + auto k = py::str(key).cast(); + kv.AddMember(RapidjsonValue(k.data(), k.size(), allocator), + to_rapidjson(obj[key], allocator), allocator); + } + return kv; + } + if (py::isinstance(obj)) { + auto ptr = py::cast(obj); + return deepcopy(*ptr, allocator); + } + throw std::runtime_error( + "to_rapidjson not implemented for this type of object: " + + py::repr(obj).cast()); +} + +inline RapidjsonValue to_rapidjson(const py::handle &obj) +{ + RapidjsonAllocator allocator; + return to_rapidjson(obj, allocator); +} + +inline py::object to_python(const RapidjsonValue &j) +{ + if (j.IsNull()) { + return py::none(); + } else if (j.IsBool()) { + return py::bool_(j.GetBool()); + } else if (j.IsNumber()) { + if (j.IsUint64()) { + return py::int_(j.GetUint64()); + } else if (j.IsInt64()) { + return py::int_(j.GetInt64()); + } else { + return py::float_(j.GetDouble()); + } + } else if (j.IsString()) { + return py::str(std::string{j.GetString(), j.GetStringLength()}); + } else if (j.IsArray()) { + py::list ret; + for (const auto &e : j.GetArray()) { + ret.append(to_python(e)); + } + return ret; + } else { + py::dict ret; + for (auto &m : j.GetObject()) { + ret[py::str( + std::string{m.name.GetString(), m.name.GetStringLength()})] = + to_python(m.value); + } + return ret; + } +} +} // namespace nano_fmm + +#ifndef BIND_PY_FLUENT_ATTRIBUTE +#define BIND_PY_FLUENT_ATTRIBUTE(Klass, type, var) \ + .def( \ + #var, [](Klass &self) -> type & { return self.var; }, \ + rvp::reference_internal) \ + .def( \ + #var, \ + [](Klass &self, const type &v) -> Klass & { \ + self.var = v; \ + return self; \ + }, \ + rvp::reference_internal) +#endif diff --git a/src/bindings/pybind11_network.cpp b/src/bindings/pybind11_network.cpp index 78469dd..6e371f0 100644 --- a/src/bindings/pybind11_network.cpp +++ b/src/bindings/pybind11_network.cpp @@ -16,13 +16,24 @@ using rvp = py::return_value_policy; void bind_network(py::module &m) { py::class_(m, "ProjectedPoint", py::module_local()) // - .def(py::init(), - "position"_a = Eigen::Vector3d(0, 0, 0), "distance"_a = 0.0, - "road_id"_a = 0, "offset"_a = 0.0) + .def(py::init(), + py::kw_only(), // + "position"_a = Eigen::Vector3d(0, 0, 0), // + "direction"_a = Eigen::Vector3d(0, 0, 1), // + "distance"_a = 0.0, // + "road_id"_a = 0, // + "offset"_a = 0.0) // .def_property_readonly( "position", [](const ProjectedPoint &self) { return self.position_; }) + .def_property_readonly( + "direction", + [](const ProjectedPoint &self) { return self.direction_; }) .def_property_readonly( "distance", [](const ProjectedPoint &self) { return self.distance_; }) @@ -34,24 +45,30 @@ void bind_network(py::module &m) ; py::class_(m, "UbodtRecord", py::module_local()) // - .def(py::init<>()) + .def(py::init(), + py::kw_only(), + "source_road"_a = 0, // + "target_road"_a = 0, // + "source_next"_a = 0, // + "target_prev"_a = 0, // + "cost"_a = 0.0) // .def_property_readonly( "source_road", - [](const UbodtRecord &self) { return self.source_road; }) + [](const UbodtRecord &self) { return self.source_road_; }) .def_property_readonly( "target_road", - [](const UbodtRecord &self) { return self.target_road; }) + [](const UbodtRecord &self) { return self.target_road_; }) .def_property_readonly( "source_next", - [](const UbodtRecord &self) { return self.source_next; }) + [](const UbodtRecord &self) { return self.source_next_; }) .def_property_readonly( "target_prev", - [](const UbodtRecord &self) { return self.target_prev; }) + [](const UbodtRecord &self) { return self.target_prev_; }) .def_property_readonly( - "cost", [](const UbodtRecord &self) { return self.cost; }) + "cost", [](const UbodtRecord &self) { return self.cost_; }) .def_property_readonly( - "next", [](const UbodtRecord &self) { return self.next; }, + "next", [](const UbodtRecord &self) { return self.next_; }, rvp::reference_internal) // .def(py::self == py::self) @@ -60,9 +77,9 @@ void bind_network(py::module &m) .def("__repr__", [](const UbodtRecord &self) { return fmt::format( "UbodtRecord(s->t=[{}->{}], cost:{}, sn:{},tp:{})", - self.source_road, self.target_road, // - self.cost, // - self.source_next, self.target_prev); + self.source_road_, self.target_road_, // + self.cost_, // + self.source_next_, self.target_prev_); }); // ; @@ -112,7 +129,14 @@ void bind_network(py::module &m) std::optional>(&Network::build_ubodt, py::const_), "roads"_a, py::kw_only(), "thresh"_a = std::nullopt) - .def("load_ubodt", &Network::load_ubodt, "path"_a) + .def("clear_ubodt", &Network::clear_ubodt) + .def("load_ubodt", + py::overload_cast &>( + &Network::load_ubodt), + "rows"_a) + .def("load_ubodt", + py::overload_cast(&Network::load_ubodt), + "path"_a) .def("dump_ubodt", &Network::dump_ubodt, "path"_a, py::kw_only(), "thresh"_a = std::nullopt) // diff --git a/src/bindings/pybind11_randoms.cpp b/src/bindings/pybind11_randoms.cpp new file mode 100644 index 0000000..ea97a81 --- /dev/null +++ b/src/bindings/pybind11_randoms.cpp @@ -0,0 +1,31 @@ +#include +#include +#include +#include +#include + +#include "nano_fmm/randoms.hpp" + +namespace nano_fmm +{ +namespace py = pybind11; +using namespace pybind11::literals; +using rvp = py::return_value_policy; + +void bind_randoms(py::module &m) +{ + m // + .def("hsv2rgb", &hsv_to_rgb, "h"_a, "s"_a, "v"_a) + // + ; + py::class_(m, "RandomColor", py::module_local()) // + .def(py::init(), py::kw_only(), "on_black"_a = true) + .def(py::init(), "seed"_a, py::kw_only(), + "on_black"_a = true) + // + .def("next_rgb", &RandomColor::next_rgb) + .def("next_hex", &RandomColor::next_hex) + // + ; +} +} // namespace nano_fmm diff --git a/src/bindings/pybind11_rapidjson.cpp b/src/bindings/pybind11_rapidjson.cpp new file mode 100644 index 0000000..a4d924c --- /dev/null +++ b/src/bindings/pybind11_rapidjson.cpp @@ -0,0 +1,364 @@ +// migrated from +// https://github.com/cubao/geobuf-cpp/blob/dev/src/pybind11_rapidjson.cpp + +#include +#include +#include +#include + +#include "rapidjson/error/en.h" +#include "rapidjson/filereadstream.h" +#include "rapidjson/filewritestream.h" +#include "rapidjson/prettywriter.h" +#include "rapidjson/stringbuffer.h" +#include +#include + +#include "nano_fmm/types.hpp" +#include "pybind11_helpers.hpp" + +namespace nano_fmm +{ +namespace py = pybind11; +using namespace pybind11::literals; +using rvp = py::return_value_policy; + +void bind_rapidjson(py::module &m) +{ + auto rj = + py::class_(m, "rapidjson") // + .def(py::init<>()) + .def(py::init( + [](const py::object &obj) { return to_rapidjson(obj); })) + // type checks + .def("GetType", &RapidjsonValue::GetType) // + .def("IsNull", &RapidjsonValue::IsNull) // + .def("IsFalse", &RapidjsonValue::IsFalse) // + .def("IsTrue", &RapidjsonValue::IsTrue) // + .def("IsBool", &RapidjsonValue::IsBool) // + .def("IsObject", &RapidjsonValue::IsObject) // + .def("IsArray", &RapidjsonValue::IsArray) // + .def("IsNumber", &RapidjsonValue::IsNumber) // + .def("IsInt", &RapidjsonValue::IsInt) // + .def("IsUint", &RapidjsonValue::IsUint) // + .def("IsInt64", &RapidjsonValue::IsInt64) // + .def("IsUint64", &RapidjsonValue::IsUint64) // + .def("IsDouble", &RapidjsonValue::IsDouble) // + .def("IsFloat", &RapidjsonValue::IsFloat) // + .def("IsString", &RapidjsonValue::IsString) // + // + .def("IsLosslessDouble", &RapidjsonValue::IsLosslessDouble) // + .def("IsLosslessFloat", &RapidjsonValue::IsLosslessFloat) // + // + .def("SetNull", &RapidjsonValue::SetNull) // + .def("SetObject", &RapidjsonValue::SetObject) // + .def("SetArray", &RapidjsonValue::SetArray) // + .def("SetInt", &RapidjsonValue::SetInt) // + .def("SetUint", &RapidjsonValue::SetUint) // + .def("SetInt64", &RapidjsonValue::SetInt64) // + .def("SetUint64", &RapidjsonValue::SetUint64) // + .def("SetDouble", &RapidjsonValue::SetDouble) // + .def("SetFloat", &RapidjsonValue::SetFloat) // + // setstring + // get string + // + .def("Empty", + [](const RapidjsonValue &self) { return !__bool__(self); }) + .def("__bool__", + [](const RapidjsonValue &self) { return __bool__(self); }) + .def( + "Size", + [](const RapidjsonValue &self) -> int { return __len__(self); }) + .def( + "__len__", + [](const RapidjsonValue &self) -> int { return __len__(self); }) + .def("HasMember", + [](const RapidjsonValue &self, const std::string &key) { + return self.HasMember(key.c_str()); + }) + .def("__contains__", + [](const RapidjsonValue &self, const std::string &key) { + return self.HasMember(key.c_str()); + }) + .def("keys", + [](const RapidjsonValue &self) { + std::vector keys; + if (self.IsObject()) { + keys.reserve(self.MemberCount()); + for (auto &m : self.GetObject()) { + keys.emplace_back(m.name.GetString(), + m.name.GetStringLength()); + } + } + return keys; + }) + .def( + "values", + [](RapidjsonValue &self) { + std::vector values; + if (self.IsObject()) { + values.reserve(self.MemberCount()); + for (auto &m : self.GetObject()) { + values.push_back(&m.value); + } + } + return values; + }, + rvp::reference_internal) + // load/dump file + .def( + "load", + [](RapidjsonValue &self, + const std::string &path) -> RapidjsonValue & { + self = load_json(path); + return self; + }, + rvp::reference_internal) + .def( + "dump", + [](const RapidjsonValue &self, const std::string &path, + bool indent, bool sort_keys) -> bool { + return dump_json(path, self, indent, sort_keys); + }, + "path"_a, py::kw_only(), "indent"_a = false, "sort_keys"_a = false) + // loads/dumps string + .def( + "loads", + [](RapidjsonValue &self, + const std::string &json) -> RapidjsonValue & { + self = loads(json); + return self; + }, + rvp::reference_internal) + .def( + "dumps", + [](const RapidjsonValue &self, bool indent, bool sort_keys) -> std::string { + return dumps(self, indent, sort_keys); + }, + py::kw_only(), "indent"_a = false, "sort_keys"_a = false) + // sort_keys + .def("sort_keys", [](RapidjsonValue &self) -> RapidjsonValue & { + sort_keys_inplace(self); + return self; + }, rvp::reference_internal) + .def("round", [](RapidjsonValue &self, double precision, int depth, // + const std::vector &skip_keys) -> RapidjsonValue & { + round_rapidjson(self, std::pow(10, precision), depth, skip_keys); + return self; + }, rvp::reference_internal, py::kw_only(), // + "precision"_a = 3, // + "depth"_a = 32, // + "skip_keys"_a = std::vector{}) + .def("round_geojson_non_geometry", [](RapidjsonValue &self, int precision) -> RapidjsonValue & { + round_geojson_non_geometry(self, std::pow(10, precision)); + return self; + }, rvp::reference_internal, py::kw_only(), "precision"_a = 3) + .def("round_geojson_geometry", [](RapidjsonValue &self, const std::array &precision) -> RapidjsonValue & { + round_geojson_geometry(self, { + std::pow(10, precision[0]), + std::pow(10, precision[1]), + std::pow(10, precision[2])}); + return self; + }, rvp::reference_internal, py::kw_only(), "precision"_a = std::array{8, 8, 3}) + .def("strip_geometry_z_0", [](RapidjsonValue &self) -> RapidjsonValue & { + strip_geometry_z_0(self); + return self; + }, rvp::reference_internal) + .def("denoise_double_0", [](RapidjsonValue &self) -> RapidjsonValue & { + denoise_double_0_rapidjson(self); + return self; + }, rvp::reference_internal) + .def("normalize", [](RapidjsonValue &self, + bool sort_keys, + bool strip_geometry_z_0, + std::optional round_geojson_non_geometry, + const std::optional> &round_geojson_geometry, + bool denoise_double_0) -> RapidjsonValue & { + normalize_json(self, + sort_keys, + round_geojson_non_geometry, + round_geojson_geometry, + strip_geometry_z_0, + denoise_double_0); + return self; + }, py::kw_only(), // + "sort_keys"_a = true, // + "strip_geometry_z_0"_a = true, + "round_geojson_non_geometry"_a = 3, + "round_geojson_geometry"_a = std::array{8, 8, 3}, + "denoise_double_0"_a = true) + .def( + "get", + [](RapidjsonValue &self, + const std::string &key) -> RapidjsonValue * { + auto itr = self.FindMember(key.c_str()); + if (itr == self.MemberEnd()) { + return nullptr; + } else { + return &itr->value; + } + }, + "key"_a, rvp::reference_internal) + .def( + "__getitem__", + [](RapidjsonValue &self, + const std::string &key) -> RapidjsonValue * { + auto itr = self.FindMember(key.c_str()); + if (itr == self.MemberEnd()) { + throw pybind11::key_error(key); + } + return &itr->value; + }, + rvp::reference_internal) + .def( + "__getitem__", + [](RapidjsonValue &self, int index) -> RapidjsonValue & { + return self[index >= 0 ? index : index + (int)self.Size()]; + }, + rvp::reference_internal) + .def("__delitem__", + [](RapidjsonValue &self, const std::string &key) { + return self.EraseMember(key.c_str()); + }) + .def("__delitem__", + [](RapidjsonValue &self, int index) { + self.Erase( + self.Begin() + + (index >= 0 ? index : index + (int)self.Size())); + }) + .def("clear", + [](RapidjsonValue &self) -> RapidjsonValue & { + if (self.IsObject()) { + self.RemoveAllMembers(); + } else if (self.IsArray()) { + self.Clear(); + } + return self; + }, rvp::reference_internal) + // get (python copy) + .def("GetBool", &RapidjsonValue::GetBool) + .def("GetInt", &RapidjsonValue::GetInt) + .def("GetUint", &RapidjsonValue::GetUint) + .def("GetInt64", &RapidjsonValue::GetInt64) + .def("GetUInt64", &RapidjsonValue::GetUint64) + .def("GetFloat", &RapidjsonValue::GetFloat) + .def("GetDouble", &RapidjsonValue::GetDouble) + .def("GetString", + [](const RapidjsonValue &self) { + return std::string{self.GetString(), + self.GetStringLength()}; + }) + .def("GetStringLength", &RapidjsonValue::GetStringLength) + // https://pybind11.readthedocs.io/en/stable/advanced/pycpp/numpy.html?highlight=MemoryView#memory-view + .def("GetRawString", [](const RapidjsonValue &self) { + return py::memoryview::from_memory( + self.GetString(), + self.GetStringLength() + ); + }, rvp::reference_internal) + .def("Get", + [](const RapidjsonValue &self) { return to_python(self); }) + .def("__call__", + [](const RapidjsonValue &self) { return to_python(self); }) + // set + .def( + "set", + [](RapidjsonValue &self, + const py::object &obj) -> RapidjsonValue & { + self = to_rapidjson(obj); + return self; + }, + rvp::reference_internal) + .def( + "set", + [](RapidjsonValue &self, + const RapidjsonValue &obj) -> RapidjsonValue & { + self = deepcopy(obj); + return self; + }, + rvp::reference_internal) + .def( // same as set + "copy_from", + [](RapidjsonValue &self, + const RapidjsonValue &obj) -> RapidjsonValue & { + self = deepcopy(obj); + return self; + }, + rvp::reference_internal) + .def( + "__setitem__", + [](RapidjsonValue &self, int index, const py::object &obj) { + self[index >= 0 ? index : index + (int)self.Size()] = + to_rapidjson(obj); + return obj; + }, + "index"_a, "value"_a, rvp::reference_internal) + .def( + "__setitem__", + [](RapidjsonValue &self, const std::string &key, + const py::object &obj) { + auto itr = self.FindMember(key.c_str()); + if (itr == self.MemberEnd()) { + RapidjsonAllocator allocator; + self.AddMember( + RapidjsonValue(key.data(), key.size(), allocator), + to_rapidjson(obj, allocator), allocator); + } else { + RapidjsonAllocator allocator; + itr->value = to_rapidjson(obj, allocator); + } + return obj; + }, + rvp::reference_internal) + .def( + "push_back", + [](RapidjsonValue &self, + const py::object &obj) -> RapidjsonValue & { + RapidjsonAllocator allocator; + self.PushBack(to_rapidjson(obj), allocator); + return self; + }, + rvp::reference_internal) + // + .def( + "pop_back", + [](RapidjsonValue &self) -> RapidjsonValue + & { + self.PopBack(); + return self; + }, + rvp::reference_internal) + // https://pybind11.readthedocs.io/en/stable/advanced/classes.html?highlight=__deepcopy__#deepcopy-support + .def("__copy__", + [](const RapidjsonValue &self, py::dict) -> RapidjsonValue { + return deepcopy(self); + }) + .def( + "__deepcopy__", + [](const RapidjsonValue &self, py::dict) -> RapidjsonValue { + return deepcopy(self); + }, + "memo"_a) + .def("clone", + [](const RapidjsonValue &self) -> RapidjsonValue { + return deepcopy(self); + }) + // https://pybind11.readthedocs.io/en/stable/advanced/classes.html?highlight=pickle#pickling-support + .def(py::pickle( + [](const RapidjsonValue &self) { return to_python(self); }, + [](py::object o) -> RapidjsonValue { return to_rapidjson(o); })) + .def(py::self == py::self) + .def(py::self != py::self) + // + ; + py::enum_(rj, "type") + .value("kNullType", rapidjson::kNullType) + .value("kFalseType", rapidjson::kFalseType) + .value("kTrueType", rapidjson::kTrueType) + .value("kObjectType", rapidjson::kObjectType) + .value("kArrayType", rapidjson::kArrayType) + .value("kStringType", rapidjson::kStringType) + .value("kNumberType", rapidjson::kNumberType) + .export_values(); +} +} // namespace nano_fmm diff --git a/src/main.cpp b/src/main.cpp index c86d806..5ed6a38 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -13,6 +13,8 @@ void bind_benchmarks(py::module &m); void bind_network(py::module &m); void bind_packedrtree(py::module &m); void bind_polyline(py::module &m); +void bind_randoms(py::module &m); +void bind_rapidjson(py::module &m); void bind_utils(py::module &m); } // namespace nano_fmm @@ -38,5 +40,7 @@ PYBIND11_MODULE(_nano_fmm, m) nano_fmm::bind_network(m); nano_fmm::bind_packedrtree(flatbush); nano_fmm::bind_polyline(m); + nano_fmm::bind_randoms(m); + nano_fmm::bind_rapidjson(m); nano_fmm::bind_utils(utils); } diff --git a/src/nano_fmm/network.cpp b/src/nano_fmm/network.cpp index fcd597e..889c341 100644 --- a/src/nano_fmm/network.cpp +++ b/src/nano_fmm/network.cpp @@ -134,7 +134,8 @@ Network::query(const Eigen::Vector3d &position, double radius, if (d > radius) { continue; } - nearests.push_back({P, d, pair.first, poly.range(s, t)}); + nearests.emplace_back(P, poly.segment(s).dir(), d, // + pair.first, poly.range(s, t)); } std::sort(nearests.begin(), nearests.end(), [](auto &n1, auto &n2) { return n1.distance_ < n2.distance_; }); @@ -160,6 +161,45 @@ Network::query(const Eigen::Vector4d &bbox) const return ret; } +MatchResult Network::match(const RowVectors &trajectory) const +{ + // MatchResult FastMapMatch::match_traj(const Trajectory &traj, const + // FastMapMatchConfig &config) + + // Traj_Candidates tc = network_.search_tr_cs_knn(traj.geom, config.k, + // config.radius); if (tc.empty()) + // return MatchResult{}; + + // TransitionGraph tg(tc, config.gps_error); + // update_tg(&tg, traj, config.reverse_tolerance); + // TGOpath tg_opath = tg.backtrack(); + // SPDLOG_DEBUG("Optimal path size {}", tg_opath.size()); + + // MatchedCandidatePath matched_candidate_path(tg_opath.size()); + // std::transform( + // tg_opath.begin(), tg_opath.end(), matched_candidate_path.begin(), + // [](const TGNode *a) { + // return MatchedCandidate{*(a->c), a->ep, a->tp, a->sp_dist}; + // }); + + // O_Path opath(tg_opath.size()); + // std::transform(tg_opath.begin(), tg_opath.end(), opath.begin(), + // [](const TGNode *a) { return a->c->edge->id; }); + + // std::vector indices; + // const std::vector &edges = network_.get_edges(); + // C_Path cpath = ubodt_->construct_complete_path( + // traj.id, tg_opath, edges, &indices, config.reverse_tolerance); + + // LineString mgeom = network_.complete_path_to_geometry(traj.geom, cpath); + + // return MatchResult{traj.id, matched_candidate_path, opath, cpath, + // indices, + // mgeom}; + MatchResult ret; + return ret; +} + void Network::build() const { for (auto &pair : roads_) { @@ -219,6 +259,26 @@ Network::build_ubodt(const std::vector &roads, return records; } +size_t Network::clear_ubodt() +{ + size_t count = ubodt_.size(); + ubodt_.clear(); + return count; +} + +size_t Network::load_ubodt(const std::vector &rows) +{ + // ubodt_; + size_t count = 0; + for (auto &row : rows) { + if (ubodt_.emplace(IndexIJ(row.source_road_, row.target_road_), row) + .second) { + ++count; + } + } + return count; +} + bool Network::load_ubodt(const std::string &path) { // diff --git a/src/nano_fmm/network.hpp b/src/nano_fmm/network.hpp index 1206d90..42c63cc 100644 --- a/src/nano_fmm/network.hpp +++ b/src/nano_fmm/network.hpp @@ -4,6 +4,10 @@ #include "nano_fmm/config.hpp" #include "nano_fmm/polyline.hpp" +#include "nano_fmm/network/projected_point.hpp" +#include "nano_fmm/network/ubodt.hpp" +#include "nano_fmm/network/match_result.hpp" + #include "packedrtree.h" #include @@ -15,54 +19,6 @@ namespace nano_fmm { -struct ProjectedPoint -{ - ProjectedPoint(const Eigen::Vector3d &position = {0.0, 0.0, 0.0}, // - double distance = 0.0, // - int64_t road_id = 0, double offset = 0.0) - : position_(position), distance_(distance), // - road_id_(road_id), offset_(offset_) - { - } - Eigen::Vector3d position_; - Eigen::Vector3d direction_; - double distance_; - int64_t road_id_; - double offset_; -}; - -struct UbodtRecord -{ - int64_t source_road{0}; - int64_t target_road{0}; - int64_t source_next{0}; - int64_t target_prev{0}; - double cost{0.0}; - UbodtRecord *next{nullptr}; - - bool operator<(const UbodtRecord &rhs) const - { - if (source_road != rhs.source_road) { - return source_road < rhs.source_road; - } - if (cost != rhs.cost) { - return cost < rhs.cost; - } - if (source_next != rhs.source_next) { - return source_next < rhs.source_next; - } - return std::make_tuple(target_prev, target_road, next) < - std::make_tuple(rhs.target_prev, rhs.target_road, rhs.next); - } - bool operator==(const UbodtRecord &rhs) const - { - return source_road == rhs.source_road && - target_road == rhs.target_road && - source_next == rhs.source_next && - target_prev == rhs.target_prev && next == rhs.next; - } -}; - struct Network { Network(bool is_wgs84 = false) : is_wgs84_(is_wgs84) {} @@ -94,6 +50,9 @@ struct Network std::map, RowVectors> query(const Eigen::Vector4d &bbox) const; + // traj + MatchResult match(const RowVectors &trajectory) const; + // build cache (not necessary) void build() const; @@ -110,6 +69,8 @@ struct Network std::vector build_ubodt(const std::vector &roads, std::optional thresh = std::nullopt) const; + size_t clear_ubodt(); + size_t load_ubodt(const std::vector &rows); bool load_ubodt(const std::string &path); bool dump_ubodt(const std::string &path, std::optional thresh) const; @@ -137,5 +98,6 @@ struct Network void single_source_upperbound_dijkstra(int64_t source, double distance, // IndexMap &predecessor_map, DistanceMap &distance_map) const; + std::unordered_map> ubodt_; }; } // namespace nano_fmm diff --git a/src/nano_fmm/network/match_result.hpp b/src/nano_fmm/network/match_result.hpp new file mode 100644 index 0000000..61e5515 --- /dev/null +++ b/src/nano_fmm/network/match_result.hpp @@ -0,0 +1,16 @@ +#pragma once + +#include "nano_fmm/types.hpp" + +namespace nano_fmm +{ +struct MatchResult +{ + // opt_candidates + // opath + // cpath + // indices + // geom +}; + +} // namespace nano_fmm diff --git a/src/nano_fmm/network/opath.hpp b/src/nano_fmm/network/opath.hpp new file mode 100644 index 0000000..e9fb428 --- /dev/null +++ b/src/nano_fmm/network/opath.hpp @@ -0,0 +1,9 @@ +#pragma once + +#include "nano_fmm/types.hpp" + +namespace nano_fmm +{ +// + +} diff --git a/src/nano_fmm/network/projected_point.hpp b/src/nano_fmm/network/projected_point.hpp new file mode 100644 index 0000000..0fc8eb1 --- /dev/null +++ b/src/nano_fmm/network/projected_point.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include "nano_fmm/types.hpp" + +namespace nano_fmm +{ +struct ProjectedPoint +{ + ProjectedPoint() {} + ProjectedPoint(const Eigen::Vector3d &position, + const Eigen::Vector3d &direction, double distance, + int64_t road_id, + double offset) + : position_(position), // + direction_(direction), // + distance_(distance), // + road_id_(road_id), // + offset_(offset_) + { + } + Eigen::Vector3d position_{0.0, 0.0, 0.0}; + Eigen::Vector3d direction_{0.0, 0.0, 1.0}; + double distance_{0.0}; + int64_t road_id_{0}; + double offset_{0.0}; +}; +} // namespace nano_fmm diff --git a/src/nano_fmm/network/ubodt.hpp b/src/nano_fmm/network/ubodt.hpp new file mode 100644 index 0000000..97f620d --- /dev/null +++ b/src/nano_fmm/network/ubodt.hpp @@ -0,0 +1,54 @@ +#pragma once + +#include "nano_fmm/types.hpp" + +namespace nano_fmm +{ +struct UbodtRecord +{ + UbodtRecord() {} + UbodtRecord(int64_t source_road, int64_t target_road, int64_t source_next, + int64_t target_prev, double cost, UbodtRecord *next) + : source_road_(source_road), target_road_(target_road), + source_next_(source_next), target_prev_(target_prev), cost_(cost), + next_(next) + { + } + UbodtRecord(int64_t source_road, int64_t target_road, int64_t source_next, + int64_t target_prev, double cost) + : UbodtRecord(source_road, target_road, source_next, target_prev, cost, + nullptr) + { + } + + int64_t source_road_{0}; + int64_t target_road_{0}; + int64_t source_next_{0}; + int64_t target_prev_{0}; + double cost_{0.0}; + UbodtRecord *next_{nullptr}; + + bool operator<(const UbodtRecord &rhs) const + { + if (source_road_ != rhs.source_road_) { + return source_road_ < rhs.source_road_; + } + if (cost_ != rhs.cost_) { + return cost_ < rhs.cost_; + } + if (source_next_ != rhs.source_next_) { + return source_next_ < rhs.source_next_; + } + return std::make_tuple(target_prev_, target_road_, next_) < + std::make_tuple(rhs.target_prev_, rhs.target_road_, rhs.next_); + } + bool operator==(const UbodtRecord &rhs) const + { + return source_road_ == rhs.source_road_ && + target_road_ == rhs.target_road_ && + source_next_ == rhs.source_next_ && + target_prev_ == rhs.target_prev_ && next_ == rhs.next_; + } +}; + +} // namespace nano_fmm diff --git a/src/nano_fmm/polyline.hpp b/src/nano_fmm/polyline.hpp index ae58c43..8f80b8c 100644 --- a/src/nano_fmm/polyline.hpp +++ b/src/nano_fmm/polyline.hpp @@ -166,7 +166,7 @@ struct Polyline return *ranges_; } Eigen::VectorXd ranges(N_); - ranges.setZero(); + ranges[0] = 0.0; int idx = 0; for (auto &seg : segments()) { ranges[idx + 1] = ranges[idx] + seg.length(); diff --git a/src/nano_fmm/randoms.hpp b/src/nano_fmm/randoms.hpp new file mode 100644 index 0000000..a8e12bf --- /dev/null +++ b/src/nano_fmm/randoms.hpp @@ -0,0 +1,79 @@ +#pragma + +#include +#include +#include "spdlog/spdlog.h" + +namespace nano_fmm +{ +using RGB = std::array; +inline RGB hsv_to_rgb(float h, float s, float v) +{ + float r, g, b; + int i = std::floor(h * 6); + float f = h * 6 - i; + float p = v * (1 - s); + float q = v * (1 - f * s); + float t = v * (1 - (1 - f) * s); + + switch (i % 6) { + case 0: + r = v, g = t, b = p; + break; + case 1: + r = q, g = v, b = p; + break; + case 2: + r = p, g = v, b = t; + break; + case 3: + r = p, g = q, b = v; + break; + case 4: + r = t, g = p, b = v; + break; + case 5: + r = v, g = p, b = q; + break; + } + return {static_cast(r * 255), // + static_cast(g * 255), // + static_cast(b * 255)}; +} + +struct RandomColor +{ + RandomColor(bool on_black = true) : on_black_(on_black) + { + std::random_device rd; + mt_ = std::mt19937(rd()); + } + RandomColor(int seed, bool on_black = true) : on_black_(on_black) + { + mt_ = std::mt19937(seed); + } + RGB next_rgb() + { + float h = std::uniform_real_distribution(0.0f, 1.f)(mt_); + float s = std::uniform_real_distribution(0.4f, 1.f)(mt_); + float v = std::uniform_real_distribution(0.7f, 1.f)(mt_); + if (!on_black_) { + s = 1.f - s; + v = 1.f - v; + } + return hsv_to_rgb(h, s, v); + } + + std::string next_hex() + { + auto rgb = next_rgb(); + return fmt::format("#{:02x}{:02x}{:02x}", rgb[0], rgb[1], rgb[2]); + } + + private: + // random stroke good for black/dark background + // else good for white/bright background + const bool on_black_{true}; + std::mt19937 mt_; +}; +} // namespace nano_fmm diff --git a/src/nano_fmm/rapidjson_helpers.hpp b/src/nano_fmm/rapidjson_helpers.hpp new file mode 100644 index 0000000..cfd1f08 --- /dev/null +++ b/src/nano_fmm/rapidjson_helpers.hpp @@ -0,0 +1,448 @@ +#pragma once + +#include +#include +#include + +#include "rapidjson/error/en.h" +#include "rapidjson/filereadstream.h" +#include "rapidjson/filewritestream.h" +#include "rapidjson/prettywriter.h" +#include "rapidjson/stringbuffer.h" + +#include "nano_fmm/types.hpp" + +namespace nano_fmm +{ +constexpr const auto RJFLAGS = rapidjson::kParseDefaultFlags | // + rapidjson::kParseCommentsFlag | // + rapidjson::kParseFullPrecisionFlag | // + rapidjson::kParseTrailingCommasFlag; + +inline RapidjsonValue deepcopy(const RapidjsonValue &json, + RapidjsonAllocator &allocator) +{ + RapidjsonValue copy; + copy.CopyFrom(json, allocator); + return copy; +} +inline RapidjsonValue deepcopy(const RapidjsonValue &json) +{ + RapidjsonAllocator allocator; + return deepcopy(json, allocator); +} + +template RapidjsonValue int_to_rapidjson(T const &num) +{ + if (sizeof(T) < sizeof(int64_t)) { + return std::is_signed::value + ? RapidjsonValue(static_cast(num)) + : RapidjsonValue(static_cast(num)); + } else { + return std::is_signed::value + ? RapidjsonValue(static_cast(num)) + : RapidjsonValue(static_cast(num)); + } +} + +inline void sort_keys_inplace(RapidjsonValue &json) +{ + if (json.IsArray()) { + for (auto &e : json.GetArray()) { + sort_keys_inplace(e); + } + } else if (json.IsObject()) { + auto obj = json.GetObject(); + // https://rapidjson.docsforge.com/master/sortkeys.cpp/ + std::sort(obj.MemberBegin(), obj.MemberEnd(), [](auto &lhs, auto &rhs) { + return strcmp(lhs.name.GetString(), rhs.name.GetString()) < 0; + }); + for (auto &kv : obj) { + sort_keys_inplace(kv.value); + } + } +} + +inline void round_rapidjson(RapidjsonValue &json, double scale, int depth = 1, + const std::vector &skip_keys = {}) +{ + if (--depth < 0) { + return; + } + if (json.IsArray()) { + for (auto &e : json.GetArray()) { + round_rapidjson(e, scale, depth, skip_keys); + } + } else if (json.IsObject()) { + auto obj = json.GetObject(); + for (auto &kv : obj) { + if (!skip_keys.empty() && + std::find(skip_keys.begin(), skip_keys.end(), + std::string(kv.name.GetString(), + kv.name.GetStringLength())) != + skip_keys.end()) { + continue; + } + round_rapidjson(kv.value, scale, depth, skip_keys); + } + } else if (json.IsDouble()) { + // see round_coords in geojson_helpers + json.SetDouble(std::floor(json.GetDouble() * scale + 0.5) / scale); + } +} + +inline void round_non_geojson(RapidjsonValue &json, double scale) +{ + if (json.IsObject()) { + auto itr = json.FindMember("type"); + if (itr != json.MemberEnd() && itr->value.IsString()) { + const auto type = std::string(itr->value.GetString(), + itr->value.GetStringLength()); + if ( // + type == "FeatureCollection" // + || type == "Feature" // + || type == "Point" // + || type == "MultiPoint" // + || type == "LineString" // + || type == "MultiLineString" // + || type == "Polygon" // + || type == "MultiPolygon" // + || type == "GeometryCollection") { + return; + } + } + } + round_rapidjson(json, scale, INT_MAX); +} + +inline void round_geojson_non_geometry(RapidjsonValue &json, double scale) +{ + if (!json.IsObject()) { + return; + } + auto itr = json.FindMember("type"); + if (itr == json.MemberEnd() || !itr->value.IsString()) { + return; + } + const auto type = + std::string(itr->value.GetString(), itr->value.GetStringLength()); + if (type == "Feature") { + round_rapidjson(json, scale, INT_MAX, {"geometry"}); + round_geojson_non_geometry(json["geometry"], scale); + } else if (type == "FeatureCollection") { + round_rapidjson(json, scale, INT_MAX, {"features"}); + for (auto &f : json["features"].GetArray()) { + round_geojson_non_geometry(f, scale); + } + } else if (type == "Point" || type == "MultiPoint" || + type == "LineString" || type == "MultiLineString" || + type == "Polygon" || type == "MultiPolygon") { + round_rapidjson(json, scale, INT_MAX, {"coordinates"}); + } else if (type == "GeometryCollection") { + round_rapidjson(json, scale, INT_MAX, {"geometries"}); + for (auto &g : json["geometries"].GetArray()) { + round_geojson_non_geometry(g, scale); + } + } +} + +inline void __round_geojson_geometry(RapidjsonValue &json, + const Eigen::Vector3d &scale) +{ + if (!json.IsArray() || json.Empty()) { + return; + } + if (!json[0].IsNumber()) { + for (auto &e : json.GetArray()) { + __round_geojson_geometry(e, scale); + } + return; + } + const int N = std::min(3, (int)json.Size()); + for (int i = 0; i < N; ++i) { + if (json[i].IsDouble()) { + json[i].SetDouble(std::floor(json[i].GetDouble() * scale[i] + 0.5) / + scale[i]); + } + } +} + +inline void round_geojson_geometry(RapidjsonValue &json, + const Eigen::Vector3d &scale) +{ + if (!json.IsObject()) { + return; + } + auto itr = json.FindMember("type"); + if (itr == json.MemberEnd() || !itr->value.IsString()) { + return; + } + const auto type = + std::string(itr->value.GetString(), itr->value.GetStringLength()); + if (type == "Feature") { + round_geojson_geometry(json["geometry"], scale); + } else if (type == "FeatureCollection") { + for (auto &f : json["features"].GetArray()) { + round_geojson_geometry(f["geometry"], scale); + } + } else if (type == "Point" || type == "MultiPoint" || + type == "LineString" || type == "MultiLineString" || + type == "Polygon" || type == "MultiPolygon") { + __round_geojson_geometry(json["coordinates"], scale); + } else if (type == "GeometryCollection") { + for (auto &g : json["geometries"].GetArray()) { + round_geojson_geometry(g, scale); + } + } +} + +inline void denoise_double_0_rapidjson(RapidjsonValue &json) +{ + if (json.IsArray()) { + for (auto &e : json.GetArray()) { + denoise_double_0_rapidjson(e); + } + } else if (json.IsObject()) { + auto obj = json.GetObject(); + for (auto &kv : obj) { + denoise_double_0_rapidjson(kv.value); + } + } else if (json.IsDouble()) { + double d = json.GetDouble(); + if (std::floor(d) == d) { + if (d >= 0) { + auto i = static_cast(d); + if (i == d) { + json.SetUint64(i); + } + } else { + auto i = static_cast(d); + if (i == d) { + json.SetInt64(i); + } + } + } + } +} + +inline bool __all_is_z0(RapidjsonValue &json) +{ + // [x, y, 0.0], [[x, y, 0.0], ...], [[[x, y, 0.0], ..], ..] + if (!json.IsArray()) { + return false; + } + if (json.Empty()) { + return true; + } + if (!json[0].IsNumber()) { + for (auto &e : json.GetArray()) { + if (!__all_is_z0(e)) { + return false; + } + } + return true; + } + if (json.Size() != 3 || !json[2].IsNumber()) { + return false; + } + return json[2].GetDouble() == 0.0; +} + +inline void __strip_geometry_z_0(RapidjsonValue &json) +{ + if (!json.IsArray() || json.Empty()) { + return; + } + if (!json[0].IsNumber()) { + for (auto &e : json.GetArray()) { + __strip_geometry_z_0(e); + } + return; + } + if (json.Size() == 3) { + json.PopBack(); + } +} + +inline void strip_geometry_z_0(RapidjsonValue &json) +{ + if (json.IsObject()) { + auto itr = json.FindMember("type"); + if (itr == json.MemberEnd() || !itr->value.IsString()) { + return; + } + const auto type = + std::string(itr->value.GetString(), itr->value.GetStringLength()); + if (type == "Feature") { + strip_geometry_z_0(json["geometry"]); + } else if (type == "FeatureCollection") { + for (auto &f : json["features"].GetArray()) { + strip_geometry_z_0(f["geometry"]); + } + } else if (type == "Point" || type == "MultiPoint" || + type == "LineString" || type == "MultiLineString" || + type == "Polygon" || type == "MultiPolygon") { + strip_geometry_z_0(json["coordinates"]); + } else if (type == "GeometryCollection") { + for (auto &g : json["geometries"].GetArray()) { + strip_geometry_z_0(g); + } + } + return; + } + + if (!__all_is_z0(json)) { + return; + } + __strip_geometry_z_0(json); +} + +inline void +normalize_json(RapidjsonValue &json, // + bool sort_keys = true, // + std::optional round_geojson_non_geometry = 3, // + const std::optional> &round_geojson_geometry = + std::array{8, 8, 3}, // + std::optional round_non_geojson = 3, // + bool denoise_double_0 = true, // + bool strip_geometry_z_0 = true) +{ + if (sort_keys) { + sort_keys_inplace(json); + } + if (round_geojson_non_geometry) { + double scale = std::pow(10.0, *round_geojson_non_geometry); + nano_fmm::round_geojson_non_geometry(json, scale); + } + if (round_geojson_geometry) { + auto &precision = *round_geojson_geometry; + nano_fmm::round_geojson_geometry(json, {std::pow(10.0, precision[0]), + std::pow(10.0, precision[1]), + std::pow(10.0, precision[2])}); + } + if (round_non_geojson) { + double scale = std::pow(10.0, *round_non_geojson); + nano_fmm::round_non_geojson(json, scale); + } + if (strip_geometry_z_0) { + nano_fmm::strip_geometry_z_0(json); + } + if (denoise_double_0) { + denoise_double_0_rapidjson(json); + } +} + +inline RapidjsonValue sort_keys(const RapidjsonValue &json) +{ + RapidjsonAllocator allocator; + RapidjsonValue copy; + copy.CopyFrom(json, allocator); + sort_keys_inplace(copy); + return copy; +} + +inline RapidjsonValue load_json(const std::string &path) +{ + FILE *fp = fopen(path.c_str(), "rb"); + if (!fp) { + throw std::runtime_error("can't open for reading: " + path); + } + char readBuffer[65536]; + rapidjson::FileReadStream is(fp, readBuffer, sizeof(readBuffer)); + RapidjsonDocument d; + d.ParseStream(is); + fclose(fp); + return RapidjsonValue{std::move(d.Move())}; +} +inline bool dump_json(const std::string &path, const RapidjsonValue &json, + bool indent = false, bool sort_keys = false) +{ + FILE *fp = fopen(path.c_str(), "wb"); + if (!fp) { + std::cerr << "can't open for writing: " + path << std::endl; + return false; + } + using namespace rapidjson; + char writeBuffer[65536]; + bool succ = false; + FileWriteStream os(fp, writeBuffer, sizeof(writeBuffer)); + if (indent) { + PrettyWriter writer(os); + if (sort_keys) { + succ = nano_fmm::sort_keys(json).Accept(writer); + } else { + succ = json.Accept(writer); + } + } else { + Writer writer(os); + if (sort_keys) { + succ = nano_fmm::sort_keys(json).Accept(writer); + } else { + succ = json.Accept(writer); + } + } + fclose(fp); + return succ; +} + +inline RapidjsonValue loads(const std::string &json) +{ + RapidjsonDocument d; + rapidjson::StringStream ss(json.data()); + d.ParseStream(ss); + if (d.HasParseError()) { + throw std::invalid_argument( + "invalid json, offset: " + std::to_string(d.GetErrorOffset()) + + ", error: " + rapidjson::GetParseError_En(d.GetParseError())); + } + return RapidjsonValue{std::move(d.Move())}; +} +inline std::string dumps(const RapidjsonValue &json, bool indent = false, + bool sort_keys = false) +{ + if (sort_keys) { + return dumps(nano_fmm::sort_keys(json), indent, !sort_keys); + } + rapidjson::StringBuffer buffer; + if (indent) { + rapidjson::PrettyWriter writer(buffer); + json.Accept(writer); + } else { + rapidjson::Writer writer(buffer); + json.Accept(writer); + } + return buffer.GetString(); +} + +inline bool __bool__(const RapidjsonValue &self) +{ + if (self.IsArray()) { + return !self.Empty(); + } else if (self.IsObject()) { + return !self.ObjectEmpty(); + } else if (self.IsString()) { + return self.GetStringLength() != 0u; + } else if (self.IsBool()) { + return self.GetBool(); + } else if (self.IsNumber()) { + if (self.IsUint64()) { + return self.GetUint64() != 0; + } else if (self.IsInt64()) { + return self.GetInt64() != 0; + } else { + return self.GetDouble() != 0.0; + } + } + return !self.IsNull(); +} + +inline int __len__(const RapidjsonValue &self) +{ + if (self.IsArray()) { + return self.Size(); + } else if (self.IsObject()) { + return self.MemberCount(); + } + return 0; +} +} // namespace nano_fmm diff --git a/src/nano_fmm/types.hpp b/src/nano_fmm/types.hpp index a77dd74..5b5e36c 100644 --- a/src/nano_fmm/types.hpp +++ b/src/nano_fmm/types.hpp @@ -9,6 +9,7 @@ #include #include #include +#include namespace nano_fmm { @@ -22,6 +23,14 @@ using VectorUi64 = Eigen::Matrix; using IndexIJ = Eigen::Matrix; using IndexIJK = Eigen::Matrix; +// Use the CrtAllocator, because the MemoryPoolAllocator is broken on ARM +// https://github.com/miloyip/rapidjson/issues/200, 301, 388 +using RapidjsonAllocator = rapidjson::CrtAllocator; +using RapidjsonDocument = + rapidjson::GenericDocument, RapidjsonAllocator>; +using RapidjsonValue = + rapidjson::GenericValue, RapidjsonAllocator>; + // https://github.com/cubao/pybind11-rdp/blob/master/src/main.cpp struct LineSegment { diff --git a/src/nano_fmm/utils.hpp b/src/nano_fmm/utils.hpp index 2bd2b90..03d4c09 100644 --- a/src/nano_fmm/utils.hpp +++ b/src/nano_fmm/utils.hpp @@ -219,7 +219,8 @@ douglas_simplify_mask(const Eigen::Ref &coords, Eigen::VectorXi mask(coords.rows()); mask.setZero(); if (is_wgs84) { - auto enus = lla2enu(coords); + auto enus = + lla2enu(coords, {}, cheap_ruler_k_lookup_table(coords(0, 1))); __douglas_simplify(enus, mask, 0, mask.size() - 1, epsilon); } else { __douglas_simplify(coords, mask, 0, mask.size() - 1, epsilon); diff --git a/tests/test_basic.py b/tests/test_basic.py index 2b20250..7961ca2 100644 --- a/tests/test_basic.py +++ b/tests/test_basic.py @@ -38,6 +38,13 @@ def test_segment(): assert 5.0 == seg.distance([-4.0, 3.0, 0.0]) assert 13.0 == seg.distance([5.0, 12.0, 0.0]) + seg = LineSegment([0, 0, 0], [0, 0, 0]) + assert seg.length == 0.0 + assert seg.length2 == 0.0 + assert seg.distance([0, 1, 0]) == 1.0 + pt, d, t = seg.nearest([1, 0, 0]) + assert np.all(pt == [0, 0, 0]) and d == 1.0 and t == 0.0 + def test_utils(): k0 = fmm.utils.cheap_ruler_k(0.0) @@ -386,3 +393,13 @@ def test_dijkstra(): {"source": "BC", "target": "AB", "next": "CB", "prev": "BA", "cost": 3.0}, {"source": "BC", "target": "AE", "next": "CB", "prev": "BA", "cost": 3.0}, ] + + +def test_random_stroke(): + for _ in range(3): + rc = fmm.RandomColor(0) + stroke = rc.next_hex() + assert stroke == "#38b5e9" + rc = fmm.RandomColor() + stroke = rc.next_hex() + assert stroke != "#38b5e9"