#ifndef sdimporter_h #define sdimporter_h #include <memory> #include "MapConfig.h" #include "SdTypes.h" #include "Tile.h" #include "Utils.h" #include "World.h" namespace jf { class SdImporter { private: struct ImportNodeCallback { SdImporter *i; void operator()(const SdNode &n) { std::shared_ptr<SdTile> t = PutNodeToWorld(n); i->m.insert(std::make_pair(n.id_p, t)); } std::shared_ptr<SdTile> PutNodeToWorld(const SdNode &nd) { TileId tid = P2TId(nd.id_p.p); std::shared_ptr<SdTile> t = SdWorld::GetSingleton()->FindOrAddTile(tid); const_cast<SdNode &>(nd).interpolated_node = false; t->PutNode(nd.id_p.p, nd.id_p, nd); return t; } ImportNodeCallback(SdImporter *ini) : i(ini) {} }; struct ImportWayCallback { SdImporter *im; void operator()(const BaseWay &way) { if (way.nodes.empty()) { return; } auto Import = [this](const BaseWay &way) { // 将整个link的方向作为上边所有点的参考方向 // 1. link几乎都比较直 // 2. 方向在绑路中只是用来参考 // 地图中有环形路段 // assert(start != end); std::shared_ptr<SdTile> t_start = nullptr; std::shared_ptr<SdTile> t_end = nullptr; assert(Utils::MapValue(im->m, way.nodes[0], t_start)); assert(Utils::MapValue(im->m, way.nodes[way.nodes.size() - 1], t_end)); const SdNode *startNodeData = t_start->GetNodeData(way.nodes[0]); const SdNode *endNodeData = t_end->GetNodeData(way.nodes[way.nodes.size() - 1]); assert(startNodeData); assert(endNodeData); Rot r(jf::P2XYZ(startNodeData->id_p.p), jf::P2XYZ(endNodeData->id_p.p)); for (int i = 0; i < int(way.nodes.size()) - 1; i++) { SdNodeId from = way.nodes[i]; SdNodeId to = way.nodes[i + 1]; std::shared_ptr<SdTile> t_from = nullptr; assert(Utils::MapValue(im->m, from, t_from)); PutLinkToWorld(im, from, t_from->GetTId(), to, r, way); } }; BaseWay wayback = way; std::reverse(wayback.nodes.begin(), wayback.nodes.end()); if (way.direction == 1) { Import(way); Import(wayback); } else if (way.direction == 2) { Import(way); } else if (way.direction == 3) { Import(wayback); } } void PutLinkToWorld(class SdImporter *i, SdNodeId fromnid, TileId fromtid, SdNodeId tonid, Rot &r, const BaseWay &wayData) { const SdNode *nd_from = SdWorld::GetSingleton()->GetNodeData(fromnid); const SdNode *nd_to = SdWorld::GetSingleton()->GetNodeData(tonid); assert(nd_from); assert(nd_to); // 包括起点终点 std::vector<Point> interps; std::vector<SdRadas> rs; GetInterpolatePoints(nd_from, nd_to, interps, rs); // 至少包括起点终点 assert(interps.size() >= 2); for (int i = 0, j = 1; i < interps.size() - 1; ++i, ++j) { Point p_from = interps[i]; Point p_to = interps[j]; SdNodeId nid_from = SdNodeId{p_from}; SdNodeId nid_to = SdNodeId{p_to}; TileId tid_from = P2TId(p_from); TileId tid_to = P2TId(p_to); std::shared_ptr<SdTile> tile_from = nullptr; std::shared_ptr<SdTile> tile_to = nullptr; // 这里可能没有,如下,右上tile没有数据,所以插值时候,可能插到右上 // ___ // |_|_ // |_|_| // if (!SdWorld::GetSingleton()->GetTile(tid_from, tile_from)) { tile_from = SdWorld::GetSingleton()->NewTile(tid_from, TId2Rect(tid_from)); } if (!SdWorld::GetSingleton()->GetTile(tid_to, tile_to)) { tile_to = SdWorld::GetSingleton()->NewTile(tid_to, TId2Rect(tid_to)); } // 不是起点,因为起点已经PutNode了 const SdNode *sdnd_from = tile_from->GetNodeData(nid_from); const SdNode *sdnd_to = tile_to->GetNodeData(nid_to); if (!sdnd_from) { SdNode n_from{}; n_from.id_p = nid_from; n_from.speed_limit_big = nd_from->speed_limit_big; n_from.speed_limit_small = nd_from->speed_limit_small; n_from.level = nd_from->level; n_from.radas = rs[i]; n_from.interpolated_node = (nid_from != fromnid && nid_to != tonid); tile_from->PutNode(n_from.id_p.p, n_from.id_p, n_from); } if (!sdnd_to) { SdNode n_to{}; n_to.id_p = nid_to; n_to.speed_limit_big = nd_from->speed_limit_big; n_to.speed_limit_small = nd_from->speed_limit_small; n_to.level = nd_from->level; n_to.radas = rs[j]; n_to.interpolated_node = (nid_to != fromnid && nid_to != tonid); tile_to->PutNode(n_to.id_p.p, n_to.id_p, n_to); } PutLinkToTile(nid_from, tile_from.get(), nid_to, tile_to.get(), wayData.id, wayData.level, r); } } void PutLinkToTile(SdNodeId nid_from, SdTile *t_from, SdNodeId nid_to, SdTile *t_to, RoadId wid, int level, Rot &r) { auto func_from = [nid_from, wid, nid_to, level, r, t_to](std::map<SdNodeId, SdNode> &_m) { SdNode _; assert(Utils::MapValue(_m, nid_from, _)); SdNode &nd_from = _m[nid_from]; nd_from.outDegree.push_back(nid_to); const SdNode *toNode = t_to->GetNodeData(nid_to); nd_from.outLengths.push_back(PointsDis(_m[nid_from].id_p.p, toNode->id_p.p)); // assert(!util::VectorContains(nd_from.outRids, wid)); nd_from.outRids.push_back(wid); nd_from.level = level; }; t_from->UpdateNodesData(func_from); auto func_to = [nid_from, wid, nid_to, level, r, t_from](std::map<SdNodeId, SdNode> &_m) { SdNode _; assert(Utils::MapValue(_m, nid_to, _)); SdNode &nd_to = _m[nid_to]; nd_to.inDegree.push_back(nid_from); const SdNode *fromNode = t_from->GetNodeData(nid_from); nd_to.inLengths.push_back(PointsDis(_m[nid_to].id_p.p, fromNode->id_p.p)); // assert(!util::VectorContains(nd_to.inRids, wid)); nd_to.inRids.push_back(wid); nd_to.level = level; }; t_to->UpdateNodesData(func_to); } void GetInterpolatePoints(const SdNode *from, const SdNode *to, std::vector<Point> &ret, std::vector<SdRadas> &rs) { if (from->id_p == to->id_p) { ret.push_back(from->id_p.p); ret.push_back(to->id_p.p); } else { double d = GetValidZoneWidth(from->id_p.p, to->id_p.p); XYZ xyz_from = P2XYZ(from->id_p.p, d); XYZ xyz_to = P2XYZ(to->id_p.p, d); double l = DXYZ(xyz_to, xyz_from); XYZ n = norm(xyz_from, xyz_to); // 坐标转换会造成误差,所以对已经设置的点不做二次转换 ret.push_back(from->id_p.p); SdRadas radas_from = from->radas; SdRadas radas_to = to->radas; #define radas_check(r) assert(r.curvature<1e100 && r.curvature> - 1e100 && r.roll < 1e100 && r.roll > -1e100 && r.pitch < 1e100 && r.pitch > -1e100 && r.azimuth < 1e100 && r.azimuth > -1e100) radas_check(radas_from); rs.push_back(radas_from); for (int i = 1; MapConfig::node_interpolate * i < l; ++i) { XYZ interpo = xyz_from + n * (i * MapConfig::node_interpolate); ret.push_back(XYZ2P(interpo, d)); SdRadas r; double div = (double(MapConfig::node_interpolate * i) / l); #define radas_interp(from, to, div) (from + (to - from) * div) r.curvature = radas_interp(radas_from.curvature, radas_to.curvature, div); r.roll = radas_interp(radas_from.roll, radas_to.roll, div); r.pitch = radas_interp(radas_from.pitch, radas_to.pitch, div); r.azimuth = radas_interp(radas_from.azimuth, radas_to.azimuth, div); radas_check(r); rs.push_back(r); } ret.push_back(to->id_p.p); radas_check(radas_to); rs.push_back(radas_to); } } ImportWayCallback(SdImporter *ini) : im(ini) {} }; struct ImportBoundCallback { SdImporter *i; void operator()(const BaseBound &b) {} ImportBoundCallback(SdImporter *ini) : i(ini) {} }; struct ImportRelationCallback { SdImporter *i; void operator()() {} ImportRelationCallback(SdImporter *ini) : i(ini) {} }; ImportNodeCallback nc; ImportWayCallback wc; ImportBoundCallback bc; ImportRelationCallback rc; public: SdImporter() : nc(this), wc(this), bc(this), rc(this) {} ImportNodeCallback GetNodeCallback() { return nc; } ImportWayCallback GetWayCallback() { return wc; } ImportBoundCallback GetBoundCallback() { return bc; }; ImportRelationCallback GetRelationCallback() { return rc; }; std::shared_ptr<SdTile> GetTile(SdNodeId nid) const { std::shared_ptr<SdTile> ret = nullptr; assert(Utils::MapValue(m, nid, ret)); return ret; } SdWorld *w; std::map<SdNodeId, std::shared_ptr<SdTile>> m; }; } // namespace jf #endif