#include "HdLib.h" #include <string> #include "CoordSys.h" #include "FileSys.h" // #include "Log.h" #include "Profiler.h" #include "Timer.h" #include "Utils.h" using namespace jf; ShPtr_HdLib HdLib::m_spInstance = {}; ShPtr_HdLib HdLib::GetInstance() { if (nullptr == m_spInstance.get()) { ShPtr_HdLib temp(new HdLib()); m_spInstance.swap(temp); } return m_spInstance; } void HdLib::Init(const std::string &strProjectPath, const std::string &strMapConfigPath) { jf::MapConfig::Init(strProjectPath, strMapConfigPath); jf::HdWorld::GetSingleton()->LoadAllTilesOnce(); } ///////////////////////////////////////////////////////////////////// /***********第一部分:Link、Lane几何关系计算****************************/ //////////////////////////////////////////////////////////////////// /** * 查询一个点 在哪一个(或者几个)Link、Lane上(准确值) * @NOTE 输出的Link和Lane一一对应 没有去重 * @return */ bool HdLib::GetLocateLanes(const Point &ptPoint, std::vector<HdLinkId> &vcthlOutLinkId, std::vector<HdLaneId> &vcthlOutLaneId) { //先求这个点大约位置所处于的位置,查找半径:20m。 std::vector<const HdLink *> vcthlLink = {}; if (false == GetDataNearby(ptPoint, 20, 20, vcthlLink)) { return false; } for (const HdLink *hdLink : vcthlLink) { for (HdLane hdLane : hdLink->vcthlLane) { if (IsPointInLane(ptPoint, hdLink, hdLane)) { vcthlOutLaneId.push_back(hdLane.liId); vcthlOutLinkId.push_back(hdLink->liId); } } } if (!vcthlOutLaneId.empty()) { return true; } else { return false; } } /** * 查询一个点 一定矩形范围内 的包含的所有link * @NOTE 矩形范围大小参数: fLonIntervalHalfMetre * flatintervalhalfmetre * @return */ bool HdLib::GetDataNearby(const Point &ptPoint, float fLonIntervalHalfMetre, float flatintervalhalfmetre, std::vector<const HdLink *> &vcthlOutLink, GetDataNearby_param p /* = GetDataNearby_param::none */, bool bManageTile /* = true */) { std::set<const HdLink *> lks; Rect2D r = P2R(ptPoint, fLonIntervalHalfMetre, flatintervalhalfmetre); lks = m_hwHdWorld->GetNodesInRect(r); if (lks.size() == 0) { return false; } // vector元素去重 vcthlOutLink = jf::Utils::Set2Vector(lks); sort(vcthlOutLink.begin(), vcthlOutLink.end(), [](const HdLink *hlLink1, const HdLink *hlLink2) { return hlLink1->liId < hlLink2->liId; }); vcthlOutLink.erase(unique(vcthlOutLink.begin(), vcthlOutLink.end(), [](const HdLink *hlLink1, const HdLink *hlLink2) { return hlLink1->liId == hlLink2->liId; }), vcthlOutLink.end()); return true; } /** * 求某个点在一系列轨迹点上的Index位置 */ void HdLib::GetPosInGeoLink(const Point &ptPos, const std::vector<std::vector<double>> &vctvdGeoLink, double &dOutToHeadDis, double &dOutToTailDis) { double dGeoDistance = -1; Point ptFootPos = {}; int nNxtIndex = 0; HdLibMath::GetPerpendicularFoot(ptPos, vctvdGeoLink, ptFootPos, dGeoDistance, nNxtIndex); // 距离开头的距离 double dToHeadDis = 0.0; // 开始点到垂足前一点的距离(nNxtIndex至少为1) for (int i = 0; i < nNxtIndex - 1; ++i) { dToHeadDis += PointsDis(vctvdGeoLink.at(i), vctvdGeoLink.at(i + 1)); } // 垂足前一点到垂足点的距离 dToHeadDis += PointsDis(vctvdGeoLink.at(nNxtIndex - 1), ptFootPos); dOutToHeadDis = dToHeadDis; // 距离结束点的距离 double dToTailDis = 0.0; dToTailDis += PointsDis(ptFootPos, vctvdGeoLink.at(nNxtIndex)); for (int i = nNxtIndex; i < vctvdGeoLink.size() - 1; ++i) { dToTailDis += PointsDis(vctvdGeoLink.at(i), vctvdGeoLink.at(i + 1)); } dOutToTailDis = dToTailDis; return; } /** * 求某个点 距离哪个车道中心线最近 并求出中心线上最近的那个点 */ bool HdLib::GetNearestPointfromLaneGeo(const Point &ptPoint, Point &ptOutPoint, double &dOutDistance) { std::vector<const HdLink *> vcthlLink = {}; if (false == GetDataNearby(ptPoint, 20, 20, vcthlLink)) { return false; } double dMinDistance = std::numeric_limits<double>::max(); HdLane hdLaneResult; for (const HdLink *hdLink : vcthlLink) { for (HdLane hdLane : hdLink->vcthlLane) { Point pointTmp = {}; double distanceTmp; int IndexTmp; if ((true == HdLibMath::GetNearestPointFromArray(ptPoint, hdLane.vctvdGeoLink, pointTmp, distanceTmp, IndexTmp)) && (distanceTmp < dMinDistance)) { dMinDistance = distanceTmp; hdLaneResult = hdLane; } } } if (!hdLaneResult.vctvdGeoLink.empty()) { return HdLibMath::GetPerpendicularFoot(ptPoint, hdLaneResult.vctvdGeoLink, ptOutPoint, dOutDistance); } else { return false; } } /** * 判断一个点 是否在某一个lane内 * @return */ bool HdLib::IsPointInLane(const Point &point, const HdLink *cphlLink, const HdLane &hdLane) { const std::vector<std::vector<double>> &cvctvdLeftEdgeGeo = cphlLink->vctheEdge[GetEdgeIndexById(cphlLink, hdLane.lLeftEdgeId)].vctvdGeoLink; const std::vector<std::vector<double>> &cvctvdRightEdgeGeo = cphlLink->vctheEdge[GetEdgeIndexById(cphlLink, hdLane.lRightEdgeId)].vctvdGeoLink; /***左右边界点的存放顺序是否相反***/ bool isReverseFlag = false; if (hdLane.nWidth == 0) { if (PointsDis(cvctvdLeftEdgeGeo[0][0], cvctvdLeftEdgeGeo[0][1], cvctvdRightEdgeGeo[0][0], cvctvdRightEdgeGeo[0][1]) * 100 * 10 > 5 * 100 * 10) //没有车道宽度的默认5m isReverseFlag = true; } else { if (PointsDis(cvctvdLeftEdgeGeo[0][0], cvctvdLeftEdgeGeo[0][1], cvctvdRightEdgeGeo[0][0], cvctvdRightEdgeGeo[0][1]) * 100 * 10 > hdLane.nWidth + 2 * 100 * 10) isReverseFlag = true; //左右初始点的距离大于车道宽度+2m,则表示左右边界点存放顺序你相反 } return HdLibMath::IsInRecArea(point, cvctvdLeftEdgeGeo, cvctvdRightEdgeGeo, isReverseFlag); } ///////////////////////////////////////////////////////////////////// /***********第二部分:路径规划模块相关 **********************************/ //////////////////////////////////////////////////////////////////// /** * 根据当前的 LinkEndpoint获取有联通关系的Link,并形成LinkEndpoint输出 * @return */ bool HdLib::GetNextLinkEndpoints(const LinkEndpoint &leCurEndpoint, std::vector<LinkEndpoint> &vctleOutNextEndpoints) { // 加载 // LoadTile(leCurEndpoint.ptEpNode); const HdLink *hlCurLink = GetLinkData(leCurEndpoint.lLinkId); if (!hlCurLink) { return false; } std::vector<const HdLink *> vcthlNextLinks = {}; for (const HdLinkId &lLinkId : hlCurLink->vctliNxtLinkId) { const HdLink *hlNextLink = GetLinkData(lLinkId); if (hlNextLink) { vcthlNextLinks.push_back(hlNextLink); } } if (true == vcthlNextLinks.empty()) { return false; } else { for (const HdLink *cphlLink : vcthlNextLinks) { LinkEndpoint leEndpoint = {}; leEndpoint.lLinkId = cphlLink->liId; if (false == cphlLink->vctvdGeoLink.empty()) { leEndpoint.ptEpNode = Point(cphlLink->vctvdGeoLink[cphlLink->vctvdGeoLink.size() - 1][0], cphlLink->vctvdGeoLink[cphlLink->vctvdGeoLink.size() - 1][1], 0); } else { leEndpoint.ptEpNode = Point(cphlLink->vcthlLane[0].vctvdGeoLink[cphlLink->vcthlLane[0].vctvdGeoLink.size() - 1][0], cphlLink->vcthlLane[0].vctvdGeoLink[cphlLink->vcthlLane[0].vctvdGeoLink.size() - 1][1], 0); } leEndpoint.dLength = cphlLink->dLength; vctleOutNextEndpoints.push_back(leEndpoint); } return true; } } /** * 根据当前的 LaneEndpoint获取有联通关系的Lane,并形成LaneEndpoint输出 * @author muyusheng */ bool HdLib::GetNextLaneEndpoints(const LaneEndpoint &leCurEndpoint, std::vector<LaneEndpoint> &vctleOutNextEndpoints) { const HdLink *hlCurLink = GetLinkData(leCurEndpoint.lLinkId); if (!hlCurLink) { return false; } std::map<HdLinkId, std::vector<HdLane>> NextRoadAndLaneMap; //与当前laneEndpoint具有联通关系的lane以及lane所属的link for (const HdLinkId &lLinkId : hlCurLink->vctliNxtLinkId) { const HdLink *hlNextLink = GetLinkData(lLinkId); if (hlNextLink) { for (HdLane hlNextLane : hlNextLink->vcthlLane) { for (HdLaneId lNextLanePLaneId : hlNextLane.vctliPreLaneId) { if (lNextLanePLaneId == leCurEndpoint.lLaneId) { NextRoadAndLaneMap[lLinkId].push_back(hlNextLane); } } } } } if (true == NextRoadAndLaneMap.empty()) { return false; } for (std::map<HdLinkId, std::vector<HdLane>>::iterator it = NextRoadAndLaneMap.begin(); it != NextRoadAndLaneMap.end(); ++it) { //排序 sort(it->second.begin(), it->second.end(), [](HdLane hdLane1, HdLane hdLane2) { HdLaneId hdLaneId1 = hdLane1.liId; HdLaneId hdLaneId2 = hdLane2.liId; return hdLaneId1.lValue > hdLaneId2.lValue; }); //去重 it->second.erase(unique(it->second.begin(), it->second.end(), [](HdLane hdLane1, HdLane hdLane2) { HdLaneId hdLaneId1 = hdLane1.liId; HdLaneId hdLaneId2 = hdLane2.liId; return hdLaneId1.lValue == hdLaneId2.lValue; }), it->second.end()); const HdLink *RoadLinkTmp = GetLinkData(it->first); //遍历当前Endpoint联通的下一组Lane for (HdLane hdLane : it->second) { for (HdLane allHdLane : RoadLinkTmp->vcthlLane) { if (IsReachable(RoadLinkTmp, hdLane.liId, allHdLane.liId)) { LaneEndpoint leEp = {}; // note:HdLane、HdLink的startPoint、EndPoint、GeoLink属性完全来自原来地图的属性(矢量方向)。HdLane、HdLink的联通关系属性则经过车道方向条件后加工形成。 if (allHdLane.nDirection == 2) { //和矢量方向相同 leEp.ptEpNode = Point(allHdLane.vctvdGeoLink[allHdLane.vctvdGeoLink.size() - 1][0], allHdLane.vctvdGeoLink[allHdLane.vctvdGeoLink.size() - 1][1], allHdLane.vctvdGeoLink[allHdLane.vctvdGeoLink.size() - 1][2]); } else if (allHdLane.nDirection == 3) { //和矢量方向相反 leEp.ptEpNode = Point(allHdLane.vctvdGeoLink[0][0], allHdLane.vctvdGeoLink[0][1], allHdLane.vctvdGeoLink[0][2]); } else { std::cout << "=====ERROR=====lane" << allHdLane.liId << "方向需要特殊处理" << std::endl; } leEp.lLinkId = it->first; leEp.lLaneId = allHdLane.liId; // 取建立全联通后终点所在的车道线的ID leEp.nStartLaneNum = hdLane.nSeqNum; leEp.nEndLaneNum = allHdLane.nSeqNum; leEp.dLength = RoadLinkTmp->dLength; if (leEp.dLength >= abs(leEp.nStartLaneNum - leEp.nEndLaneNum) * 10) { //如果link长度太短,那么不会变道 vctleOutNextEndpoints.push_back(leEp); } } } } } return true; } bool HdLib::GetPreEndpoints(const LaneEndpoint &leCurEndpoint, std::vector<LaneEndpoint> &vctleOutPreEndpoint) { const HdLink *hlCurLink = GetLinkData(leCurEndpoint.lLinkId); if (!hlCurLink) { return false; } std::vector<const HdLink *> vctstPreLinks = {}; std::vector<HdLaneId> vctlCurLinkLanes = {}; for (const HdLinkId &lLinkId : hlCurLink->vctliPreLinkId) { const HdLink *cphlPreLink = GetLinkData(lLinkId); if (cphlPreLink) { for (HdLane hlPreLane : cphlPreLink->vcthlLane) { for (HdLaneId lPreLaneNLaneId : hlPreLane.vctliNxtLaneId) { if ((lPreLaneNLaneId == leCurEndpoint.lLaneId) || (true == IsReachable(hlCurLink, lPreLaneNLaneId, leCurEndpoint.lLaneId))) { vctlCurLinkLanes.push_back(lPreLaneNLaneId); vctstPreLinks.push_back(cphlPreLink); } } } } } // 去重 sort(vctstPreLinks.begin(), vctstPreLinks.end(), [](const HdLink *hlLink1, const HdLink *hlLink2) { return hlLink1->liId < hlLink2->liId; }); vctstPreLinks.erase(unique(vctstPreLinks.begin(), vctstPreLinks.end(), [](const HdLink *hlLink1, const HdLink *hlLink2) { return hlLink1->liId == hlLink2->liId; }), vctstPreLinks.end()); if ((true == vctstPreLinks.empty()) || true == (*vctstPreLinks.begin())->vcthlLane.empty()) { return false; } else { #if 0 // 返回link内所有的全联通lane for (HdLaneId lCurLaneId : vctlCurLinkLanes) { GenPreEndpoints(lCurLaneId, vctstNextLinks, vctleOutNextEndpoints); } #else // 返回由指定lane联通的全联通lane GenPreEndpoints(leCurEndpoint.lLaneId, vctstPreLinks, vctleOutPreEndpoint); #endif return true; } } /** * 判断从Lane1是否可以变道到Lane2 * @param hlLink Lane1、Lane2所属RoadLink * @param lLaneId1 * @param lLaneId2 * @return */ bool HdLib::IsReachable(const HdLink *hlLink, HdLaneId lLaneId1, HdLaneId lLaneId2) { int LaneSeqNum1 = -1; // lane1在vcthlLane数组中的Index int LaneSeqNum2 = -1; // lane2在vcthlLane数组中的Index for (int i = 0; i < hlLink->vcthlLane.size(); i++) { if (hlLink->vcthlLane[i].liId.lValue == lLaneId1.lValue) { LaneSeqNum1 = i; } if (hlLink->vcthlLane[i].liId.lValue == lLaneId2.lValue) { LaneSeqNum2 = i; } } if (LaneSeqNum1 == -1 || LaneSeqNum2 == -1) { return false; } else if (LaneSeqNum1 == LaneSeqNum2) { return true; } int nSeqNum1 = hlLink->vcthlLane[LaneSeqNum1].nSeqNum; // lane1的lane_no int nSeqNum2 = hlLink->vcthlLane[LaneSeqNum2].nSeqNum; // lane2的lane_no if (nSeqNum1 / nSeqNum2 < 0) { return false; } else if (nSeqNum1 > 0) { if (LaneSeqNum1 > LaneSeqNum2) { // lane1在lane2的右面 for (int j = LaneSeqNum1; j > LaneSeqNum2; j--) { int CrossType = GetEdgeCross(hlLink, hlLink->vcthlLane[j].lLeftEdgeId); if (!(CrossType == sc_nCrossDirec_BidirecAble || CrossType == sc_nCrossDirec_LeftAble)) { return false; } } return true; } if (LaneSeqNum1 < LaneSeqNum2) { // lane1在lane2的左面 for (int j = LaneSeqNum1; j < LaneSeqNum2; j++) { int CrossType = GetEdgeCross(hlLink, hlLink->vcthlLane[j].lRightEdgeId); if (!(CrossType == sc_nCrossDirec_BidirecAble || CrossType == sc_nCrossDirec_RightAble)) { return false; } } return true; } } else if (nSeqNum1 < 0) { if (LaneSeqNum1 > LaneSeqNum2) { // lane1在lane2的左面 for (int j = LaneSeqNum1; j > LaneSeqNum2; j--) { int CrossType = GetEdgeCross(hlLink, hlLink->vcthlLane[j].lRightEdgeId); if (!(CrossType == sc_nCrossDirec_BidirecAble || CrossType == sc_nCrossDirec_RightAble)) { return false; } } return true; } if (LaneSeqNum1 < LaneSeqNum2) { // lane1在lane2的右面 for (int j = LaneSeqNum1; j < LaneSeqNum2; j++) { int CrossType = GetEdgeCross(hlLink, hlLink->vcthlLane[j].lLeftEdgeId); if (!(CrossType == sc_nCrossDirec_BidirecAble || CrossType == sc_nCrossDirec_LeftAble)) { return false; } } return true; } } } /** * 查询一个点 在哪一个(或者几个)Link上 并形成LinkEndpoint输出(准确值) * @return */ bool HdLib::GetLocateEndpoints(const Point &ptPoint, float fLonIntervalHalfMetre, float flatintervalhalfmetre, std::vector<LinkEndpoint> &vctleOutLinkEp) { auto func_IsInLinkArea = [this](const Point &ptPoint, const HdLink *cphlLink) { const HdLane &chlFirstLane = cphlLink->vcthlLane[0]; const HdLane &chlLastLane = cphlLink->vcthlLane[cphlLink->vcthlLane.size() - 1]; const std::vector<std::vector<double>> &cvctvdLeftEdgeGeo = cphlLink->vctheEdge[GetEdgeIndexById(cphlLink, chlFirstLane.lLeftEdgeId)].vctvdGeoLink; const std::vector<std::vector<double>> &cvctvdRightEdgeGeo = cphlLink->vctheEdge[GetEdgeIndexById(cphlLink, chlLastLane.lRightEdgeId)].vctvdGeoLink; /***左右边界点的存放顺序是否相反***/ bool isReverseFlag = false; if (chlFirstLane.nWidth == 0) { if (PointsDis(cvctvdLeftEdgeGeo[0][0], cvctvdLeftEdgeGeo[0][1], cvctvdRightEdgeGeo[0][0], cvctvdRightEdgeGeo[0][1]) * 100 * 10 > cphlLink->vcthlLane.size() * 4 * 100 * 10) //没有车道宽度的默认4m isReverseFlag = true; } else { if (PointsDis(cvctvdLeftEdgeGeo[0][0], cvctvdLeftEdgeGeo[0][1], cvctvdRightEdgeGeo[0][0], cvctvdRightEdgeGeo[0][1]) * 100 * 10 > cphlLink->vcthlLane.size() * chlFirstLane.nWidth + 2 * 100 * 10) isReverseFlag = true; //左右初始点的距离大于车道宽度+2m,则表示左右边界点存放顺序你相反 } return HdLibMath::IsInRecArea(ptPoint, cvctvdLeftEdgeGeo, cvctvdRightEdgeGeo, isReverseFlag); }; std::vector<const HdLink *> vcthlLink = {}; if (false == GetDataNearby(ptPoint, fLonIntervalHalfMetre, flatintervalhalfmetre, vcthlLink)) { return false; } int nVctLinkCnt = vcthlLink.size(); for (size_t i = 0; i < nVctLinkCnt; i++) { if (true == func_IsInLinkArea(ptPoint, vcthlLink[i])) { LinkEndpoint leEndpoint = {}; const HdLink *cphlLink = vcthlLink[i]; leEndpoint.ptEpNode = Point(cphlLink->vctvdGeoLink[cphlLink->vctvdGeoLink.size() - 1][0], cphlLink->vctvdGeoLink[cphlLink->vctvdGeoLink.size() - 1][1], cphlLink->vctvdGeoLink[cphlLink->vctvdGeoLink.size() - 1][2]); leEndpoint.lLinkId = cphlLink->liId; leEndpoint.dLength = cphlLink->dLength; vctleOutLinkEp.push_back(leEndpoint); } } if (false == vctleOutLinkEp.empty()) { return true; } else { return false; } } /** * 查询一个点 在哪一个(或者几个)lane上 并形成laneEndpoint输出(准确值) * @return */ bool HdLib::GetLocateEndpoints(const Point &ptPoint, float fLonIntervalHalfMetre, float flatintervalhalfmetre, std::vector<LaneEndpoint> &vctleOutLaneEp) { //先求这个点大致位置所处于的link std::vector<const HdLink *> vcthlLink = {}; if (false == GetDataNearby(ptPoint, fLonIntervalHalfMetre, flatintervalhalfmetre, vcthlLink)) { return false; } for (const HdLink *hdLink : vcthlLink) { for (HdLane hdLane : hdLink->vcthlLane) { if (IsPointInLane(ptPoint, hdLink, hdLane)) { LaneEndpoint leEndpoint = {}; HdLane hlLane = hdLane; leEndpoint.lLinkId = hdLink->liId; leEndpoint.lLaneId = hlLane.liId; leEndpoint.nStartLaneNum = hlLane.nSeqNum; leEndpoint.nEndLaneNum = hlLane.nSeqNum; vctleOutLaneEp.push_back(leEndpoint); } } } if (false == vctleOutLaneEp.empty()) { return true; } else { return false; } } /** * 生成某一个Link中可穿越变道的车道lane_no队列 * @param hlLink * @param mapOutIdxLane * @param vctvnOutRightCrossable * @param vctvnOutLeftCrossable */ void HdLib::GenCrossable(const HdLink *hlLink, std::map<int, const HdLane *> &mapOutIdxLane, std::vector<std::vector<int>> &vctvnOutRightCrossable, std::vector<std::vector<int>> &vctvnOutLeftCrossable) { for (size_t i = 0; i < hlLink->vcthlLane.size(); i++) { const HdLane *hlLane = &hlLink->vcthlLane[i]; mapOutIdxLane[hlLane->nSeqNum] = hlLane; } // 找出可以向右变道的集合:按照从左到右的顺序存储 std::vector<int> vctnRightCrossable1 = {}; std::vector<int> vctnRightCrossable2 = {}; for (auto iterRight = mapOutIdxLane.begin(); iterRight != mapOutIdxLane.end(); ++iterRight) { int nRightCross = GetEdgeCross(hlLink, iterRight->second->lRightEdgeId); if ((nRightCross == sc_nCrossDirec_BidirecAble || nRightCross == sc_nCrossDirec_RightAble)) { if (iterRight->second->nSeqNum < 0) { //对于双向通行的link中的lane分类讨论 vctnRightCrossable1.push_back(iterRight->first); } else { vctnRightCrossable2.push_back(iterRight->first); } } } if (!vctnRightCrossable1.empty()) { vctvnOutRightCrossable.push_back(vctnRightCrossable1); } if (!vctnRightCrossable2.empty()) { vctvnOutRightCrossable.push_back(vctnRightCrossable2); } // 找出可以向左变道的集合:按照从右到左的顺序存储 std::vector<int> vctnLeftCrossable1 = {}; std::vector<int> vctnLeftCrossable2 = {}; for (auto iterLeft = mapOutIdxLane.rbegin(); iterLeft != mapOutIdxLane.rend(); ++iterLeft) { int nLeftCross = GetEdgeCross(hlLink, iterLeft->second->lLeftEdgeId); if ((nLeftCross == sc_nCrossDirec_BidirecAble || nLeftCross == sc_nCrossDirec_LeftAble)) { if (iterLeft->second->nSeqNum < 0) { //对于双向通行的link中的lane分类讨论 vctnLeftCrossable1.push_back(iterLeft->first); } else { vctnLeftCrossable2.push_back(iterLeft->first); } } } if (!vctnLeftCrossable1.empty()) { vctvnOutLeftCrossable.push_back(vctnLeftCrossable1); } if (!vctnLeftCrossable2.empty()) { vctvnOutLeftCrossable.push_back(vctnLeftCrossable2); } } bool HdLib::GetLinkPreNxt(const HdLinkId &lLinkId, std::vector<HdLinkId> &vctliOutPreLinkIds, std::vector<HdLinkId> &vctliOutNxtLinkdIds) { const HdLink *cphlLink = GetLinkData(lLinkId); if (nullptr == cphlLink) { return false; } vctliOutPreLinkIds = cphlLink->vctliPreLinkId; vctliOutNxtLinkdIds = cphlLink->vctliNxtLinkId; return true; } bool HdLib::GetNearEndpoints(const Point &ptPoint, float fLonIntervalHalfMetre, float flatintervalhalfmetre, std::vector<Point> &vctptOutPoint, std::vector<LinkEndpoint> &vctleOutLinkEp, std::vector<double> &vctdOutDistance) { std::vector<const HdLink *> vcthlLink = {}; if (false == GetDataNearby(ptPoint, fLonIntervalHalfMetre, flatintervalhalfmetre, vcthlLink)) { return false; } struct MidResult { int nIndexI = -1; double dDistance = -1; Point ptPoint = {}; }; std::vector<MidResult> vctmrResult = {}; int nVctLinkCnt = vcthlLink.size(); for (size_t i = 0; i < nVctLinkCnt; i++) { MidResult mrCurResult = {}; // 获取车道参考线上的所有点 int n = 0; const std::vector<std::vector<double>> &vctvdGeoLink = vcthlLink[i]->vctvdGeoLink; if (true == HdLibMath::GetNearestPointFromArray(ptPoint, vctvdGeoLink, mrCurResult.ptPoint, mrCurResult.dDistance, n)) { mrCurResult.nIndexI = i; vctmrResult.push_back(mrCurResult); } } auto func_SortByDis = [](MidResult &r1, MidResult &r2) { return r1.dDistance < r2.dDistance; }; sort(vctmrResult.begin(), vctmrResult.end(), func_SortByDis); for (auto mrResult : vctmrResult) { vctptOutPoint.push_back(mrResult.ptPoint); LinkEndpoint leEndpoint = {}; const std::vector<std::vector<double>> &vctvdGeoLink = vcthlLink[mrResult.nIndexI]->vctvdGeoLink; leEndpoint.ptEpNode = Point(vctvdGeoLink[vctvdGeoLink.size() - 1][0], vctvdGeoLink[vctvdGeoLink.size() - 1][1], vctvdGeoLink[vctvdGeoLink.size() - 1][2]); leEndpoint.lLinkId = vcthlLink[mrResult.nIndexI]->liId; leEndpoint.dLength = vcthlLink[mrResult.nIndexI]->dLength; vctleOutLinkEp.push_back(leEndpoint); vctdOutDistance.push_back(mrResult.dDistance); } if (false == vctmrResult.empty()) { return true; } else { return false; } } bool HdLib::GetNearEndpoints(const Point &ptPoint, float fLonIntervalHalfMetre, float flatintervalhalfmetre, std::vector<Point> &vctptOutPoint, std::vector<LaneEndpoint> &vctleOutLinkEp, std::vector<double> &vctdOutDistance) { std::vector<const HdLink *> vcthlLink = {}; if (false == GetDataNearby(ptPoint, fLonIntervalHalfMetre, flatintervalhalfmetre, vcthlLink)) { return false; } struct MidResult { int nIndexI = -1; int nIndexJ = -1; double dDistance = -1; Point ptPoint = {}; }; std::vector<MidResult> vctmrResult = {}; int nVctLinkCnt = vcthlLink.size(); for (size_t i = 0; i < nVctLinkCnt; i++) { int nVctLaneCnt = vcthlLink[i]->vcthlLane.size(); for (size_t j = 0; j < nVctLaneCnt; j++) { MidResult mrCurResult = {}; // 获取指定车道中心线上的所有点 int n = 0; const std::vector<std::vector<double>> &vctvdGeoLink = vcthlLink[i]->vcthlLane[j].vctvdGeoLink; if (true == HdLibMath::GetNearestPointFromArray(ptPoint, vctvdGeoLink, mrCurResult.ptPoint, mrCurResult.dDistance, n)) { mrCurResult.nIndexI = i; mrCurResult.nIndexJ = j; vctmrResult.push_back(mrCurResult); } } } auto func_SortByDis = [](MidResult &r1, MidResult &r2) { return r1.dDistance < r2.dDistance; }; sort(vctmrResult.begin(), vctmrResult.end(), func_SortByDis); for (auto mrResult : vctmrResult) { vctptOutPoint.push_back(mrResult.ptPoint); LaneEndpoint leEndpoint = {}; HdLane hlLane = vcthlLink[mrResult.nIndexI]->vcthlLane[mrResult.nIndexJ]; leEndpoint.ptEpNode = Point(hlLane.vctvdGeoLink[hlLane.vctvdGeoLink.size() - 1][0], hlLane.vctvdGeoLink[hlLane.vctvdGeoLink.size() - 1][1], hlLane.vctvdGeoLink[hlLane.vctvdGeoLink.size() - 1][2]); leEndpoint.lLinkId = vcthlLink[mrResult.nIndexI]->liId; leEndpoint.lLaneId = hlLane.liId; leEndpoint.nStartLaneNum = hlLane.nSeqNum; leEndpoint.nEndLaneNum = hlLane.nSeqNum; leEndpoint.dLength = vcthlLink[mrResult.nIndexI]->dLength; vctleOutLinkEp.push_back(leEndpoint); vctdOutDistance.push_back(mrResult.dDistance); } if (false == vctmrResult.empty()) { return true; } else { return false; } } bool HdLib::GetNearestEndpoint(const Point &ptPoint, float fLonIntervalHalfMetre, float flatintervalhalfmetre, Point &ptOutPoint, LinkEndpoint &leOutEndpoint, double &dOutDistance) { std::vector<const HdLink *> vcthlLink = {}; if (false == GetDataNearby(ptPoint, fLonIntervalHalfMetre, flatintervalhalfmetre, vcthlLink)) { return false; } int nVctLinkCnt = vcthlLink.size(); int nBestIndexI = -1; Point ptBestPoint = {}; double dMinDistance = std::numeric_limits<double>::max(); for (size_t i = 0; i < nVctLinkCnt; i++) { double d = 0; Point p = {}; // 获取车道参考线上的所有点 int n = 0; const std::vector<std::vector<double>> &vctvdGeoLink = vcthlLink[i]->vctvdGeoLink; if ((true == HdLibMath::GetNearestPointFromArray(ptPoint, vctvdGeoLink, p, d, n)) && (d < dMinDistance)) { nBestIndexI = i; ptBestPoint = p; dMinDistance = d; } } if (nBestIndexI != -1) { ptOutPoint = ptBestPoint; const std::vector<std::vector<double>> &vctvdGeoLink = vcthlLink[nBestIndexI]->vctvdGeoLink; leOutEndpoint.ptEpNode = Point(vctvdGeoLink[vctvdGeoLink.size() - 1][0], vctvdGeoLink[vctvdGeoLink.size() - 1][1], vctvdGeoLink[vctvdGeoLink.size() - 1][2]); leOutEndpoint.lLinkId = vcthlLink[nBestIndexI]->liId; leOutEndpoint.dLength = vcthlLink[nBestIndexI]->dLength; dOutDistance = dMinDistance; return true; } else { return false; } } bool HdLib::GetNearestEndpoint(const Point &ptPoint, float fLonIntervalHalfMetre, float flatintervalhalfmetre, Point &ptOutPoint, LaneEndpoint &leOutEndpoint, double &dOutDistance) { std::vector<const HdLink *> vcthlLink = {}; if (false == GetDataNearby(ptPoint, fLonIntervalHalfMetre, flatintervalhalfmetre, vcthlLink)) { return false; } int nVctLinkCnt = vcthlLink.size(); int nBestIndexI = -1; int nBestIndexJ = -1; Point ptBestPoint = {}; double dMinDistance = std::numeric_limits<double>::max(); for (size_t i = 0; i < nVctLinkCnt; i++) { double d = 0; Point p = {}; int nVctLaneCnt = vcthlLink[i]->vcthlLane.size(); for (size_t j = 0; j < nVctLaneCnt; j++) { // 获取指定车道中心线上的所有点 int n = 0; const std::vector<std::vector<double>> &vctvdGeoLink = vcthlLink[i]->vcthlLane[j].vctvdGeoLink; if ((true == HdLibMath::GetNearestPointFromArray(ptPoint, vctvdGeoLink, p, d, n)) && (d < dMinDistance)) { nBestIndexI = i; nBestIndexJ = j; ptBestPoint = p; dMinDistance = d; } } } if ((nBestIndexI != -1) && (nBestIndexJ != -1)) { ptOutPoint = ptBestPoint; HdLane lane = vcthlLink[nBestIndexI]->vcthlLane[nBestIndexJ]; leOutEndpoint.ptEpNode = Point(lane.vctvdGeoLink[lane.vctvdGeoLink.size() - 1][0], lane.vctvdGeoLink[lane.vctvdGeoLink.size() - 1][1], lane.vctvdGeoLink[lane.vctvdGeoLink.size() - 1][2]); leOutEndpoint.lLinkId = vcthlLink[nBestIndexI]->liId; leOutEndpoint.lLaneId = lane.liId; leOutEndpoint.nStartLaneNum = lane.nSeqNum; leOutEndpoint.nEndLaneNum = lane.nSeqNum; leOutEndpoint.dLength = vcthlLink[nBestIndexI]->dLength; dOutDistance = dMinDistance; return true; } else { return false; } } std::vector<Point> HdLib::LoadGpses() { return m_hwHdWorld->GetSave()->LoadGpses(); } void HdLib::GenPreEndpoints(HdLaneId lCurLaneId, const std::vector<const HdLink *> &vcthlPreLink, std::vector<LaneEndpoint> &vctleOutPreEps) { for (const HdLink *cphlPreLink : vcthlPreLink) { std::map<int, LaneEndpointExt> mapLaneNumEpExt = {}; for (const HdLane &hlLane : cphlPreLink->vcthlLane) { LaneEndpointExt leEndpointExt = {}; if (hlLane.vctvdGeoLink.empty()) { continue; } leEndpointExt.ptEpNode = Point(hlLane.vctvdGeoLink[hlLane.vctvdGeoLink.size() - 1][0], hlLane.vctvdGeoLink[hlLane.vctvdGeoLink.size() - 1][1], hlLane.vctvdGeoLink[0][2]); leEndpointExt.lLinkId = cphlPreLink->liId; leEndpointExt.lLaneId = hlLane.liId; leEndpointExt.nStartLaneNum = hlLane.nSeqNum; leEndpointExt.nEndLaneNum = hlLane.nSeqNum; leEndpointExt.dLength = cphlPreLink->dLength; leEndpointExt.ptSpNode = Point(hlLane.vctvdGeoLink[0][0], hlLane.vctvdGeoLink[0][1], hlLane.vctvdGeoLink[0][2]); leEndpointExt.nLaneNum = hlLane.nSeqNum; int nLeftCross = GetEdgeCross(cphlPreLink, hlLane.lLeftEdgeId); leEndpointExt.bLChangable = (nLeftCross == sc_nCrossDirec_BidirecAble || nLeftCross == sc_nCrossDirec_LeftAble) ? true : false; int nRightCross = GetEdgeCross(cphlPreLink, hlLane.lRightEdgeId); leEndpointExt.bRChangable = (nRightCross == sc_nCrossDirec_BidirecAble || nRightCross == sc_nCrossDirec_RightAble) ? true : false; leEndpointExt.vctNxtLanes = hlLane.vctliNxtLaneId; mapLaneNumEpExt[leEndpointExt.nLaneNum] = leEndpointExt; } GenPreRLanes(lCurLaneId, cphlPreLink, mapLaneNumEpExt, vctleOutPreEps); } } int HdLib::GetEdgeCross(const HdLink *hlLink, const HdEdgeId &lId) { for (HdEdge heEdge : hlLink->vctheEdge) { if (heEdge.lId == lId) { return heEdge.nCross; } } return -1; } void HdLib::GenPreRLanes(HdLaneId lCurLaneId, const HdLink *cphlPreLink, const std::map<int, LaneEndpointExt> &mapLaneNumEpExt, std::vector<LaneEndpoint> &vctleOutPreEps) { // 记录每个车道的终点上绑定的上一组hdEndpoint的集合 std::map<HdLaneId, std::vector<LaneEndpoint>> mapEndIdEps = {}; // 先将现有的车道加入map for (auto it = mapLaneNumEpExt.begin(); it != mapLaneNumEpExt.end(); ++it) { for (HdLaneId lLaneId : it->second.vctNxtLanes) { mapEndIdEps[lLaneId].push_back(it->second); } } // 检查向左向右的联通关系 std::map<int, const HdLane *> mapIdxLane = {}; std::vector<std::vector<int>> vctvnRightCrossable = {}; std::vector<std::vector<int>> vctvnLeftCrossable = {}; GenCrossable(cphlPreLink, mapIdxLane, vctvnRightCrossable, vctvnLeftCrossable); // 针对可以向右变道的车道,向右创建全联通关系 for (std::vector<int> vctnRightCrossable : vctvnRightCrossable) { if (false == vctnRightCrossable.empty()) { // 先排序 sort(vctnRightCrossable.begin(), vctnRightCrossable.end()); int nVctSize = vctnRightCrossable.size(); // for (size_t i = vctnRightCrossable[0]; (i <= vctnRightCrossable[nVctSize - 1]) && (mapLaneNumEpExt.find(i) != mapLaneNumEpExt.end()); ++i) { for (size_t i = vctnRightCrossable[0]; (i <= vctnRightCrossable[nVctSize - 1]) && (mapLaneNumEpExt.find(i) != mapLaneNumEpExt.end()); ++i) { for (size_t j = i + 1; (j <= vctnRightCrossable[nVctSize - 1]) && (mapLaneNumEpExt.find(j) != mapLaneNumEpExt.end()); ++j) { // 非直行车道不允许变道 // if ((mapIdxLane.find(j) != mapIdxLane.end()) && (true == mapIdxLane[j].arrows.empty() || std::string::npos == mapIdxLane[j].arrows.find(sc_cArrow_Straight))) { // continue; // } LaneEndpoint leEp = {}; leEp.ptEpNode = mapLaneNumEpExt.at(j).ptEpNode; leEp.lLinkId = mapLaneNumEpExt.at(j).lLinkId; leEp.lLaneId = mapLaneNumEpExt.at(j).lLaneId; // 取建立全联通后终点所在的车道线的ID leEp.nStartLaneNum = mapLaneNumEpExt.at(i).nStartLaneNum; leEp.nEndLaneNum = mapLaneNumEpExt.at(j).nEndLaneNum; // leEp.dLength = D2XYZ(P2XYZ(mapLaneNumEpExt.at(i).ptSpNode), P2XYZ(mapLaneNumEpExt.at(j).ptEpNode)); leEp.dLength = mapLaneNumEpExt.at(i).dLength; for (HdLaneId lLaneId : mapLaneNumEpExt.at(j).vctNxtLanes) { mapEndIdEps[lLaneId].push_back(leEp); } } } } } // 针对可以向左变道的车道,向左创建全联通关系 for (std::vector<int> vctnLeftCrossable : vctvnLeftCrossable) { if (false == vctnLeftCrossable.empty()) { // 先排序 sort(vctnLeftCrossable.begin(), vctnLeftCrossable.end()); int nVctSize = vctnLeftCrossable.size(); for (size_t i = vctnLeftCrossable[nVctSize - 1]; (i >= vctnLeftCrossable[0]) && (mapLaneNumEpExt.find(i) != mapLaneNumEpExt.end()); --i) { for (size_t j = i - 1; (j >= vctnLeftCrossable[0]) && (mapLaneNumEpExt.find(j) != mapLaneNumEpExt.end()); --j) { // 非直行车道不允许变道 // if ((mapIdxLane.find(j) != mapIdxLane.end()) && (true == mapIdxLane[j].arrows.empty() || std::string::npos == mapIdxLane[j].arrows.find(sc_cArrow_Straight))) { // continue; // } LaneEndpoint leEp = {}; leEp.ptEpNode = mapLaneNumEpExt.at(j).ptEpNode; leEp.lLinkId = mapLaneNumEpExt.at(j).lLinkId; leEp.lLaneId = mapLaneNumEpExt.at(j).lLaneId; // 取建立全联通后终点所在的车道线的ID leEp.nStartLaneNum = mapLaneNumEpExt.at(i).nStartLaneNum; leEp.nEndLaneNum = mapLaneNumEpExt.at(j).nEndLaneNum; // leEp.dLength = D2XYZ(P2XYZ(mapLaneNumEpExt.at(i).ptSpNode), P2XYZ(mapLaneNumEpExt.at(j).ptEpNode)); leEp.dLength = mapLaneNumEpExt.at(i).dLength; for (HdLaneId lLaneId : mapLaneNumEpExt.at(j).vctNxtLanes) { mapEndIdEps[lLaneId].push_back(leEp); } } } } } // 导出结果 for (LaneEndpoint leEp : mapEndIdEps[lCurLaneId]) { if (leEp.dLength >= abs(leEp.nStartLaneNum - leEp.nEndLaneNum) * 10) { vctleOutPreEps.push_back(leEp); } } } bool HdLib::LoadTile(const Point &ptPoint, bool bManageTile /* = true */) { Rect2D r = P2R(ptPoint, 1000, 1000); if (bManageTile) { m_hwHdWorld->LoadTileInRect(r); // m_hwHdWorld->RemoveUnsedTiles(ptPoint); } return true; } ///////////////////////////////////////////////////////////////////// /***********第三部分:获取Link、Lane原始属性的所有api********************/ //////////////////////////////////////////////////////////////////// //********************Link相关api********************// const HdLink *HdLib::GetLinkData(const HdLinkId &lLinkId) { const HdLink *ret = m_hwHdWorld->GetNodeData(lLinkId); if (!ret) { return nullptr; } return ret; } bool HdLib::GetLinkLength(const HdLinkId &lLinkId, double &dOutLinkLength) { const HdLink *cphlLink = GetLinkData(lLinkId); if (cphlLink != NULL) { dOutLinkLength = cphlLink->dLength; return true; } return false; } bool HdLib::GetLinkType(const HdLinkId &lLinkId, int &nOutLinkType) { const HdLink *cphlLink = GetLinkData(lLinkId); if (cphlLink != NULL) { nOutLinkType = cphlLink->nType; return true; } return false; } bool HdLib::GetLinkStartAngle(const HdLinkId &lLinkId, double &dOutStartAngle) { const HdLink *cphlLink = GetLinkData(lLinkId); if (cphlLink != NULL) { dOutStartAngle = cphlLink->dStartAngle; return true; } return false; } bool HdLib::GetLinkSpeedLimit(const HdLinkId &lLinkId, int &nOutSpeedLimit) { const HdLink *cphlLink = GetLinkData(lLinkId); if (cphlLink != NULL) { nOutSpeedLimit = cphlLink->nSpeedLimit; return true; } return false; } bool HdLib::GetLinkLimitValue(const HdLinkId &lLinkId, int nLimitType, int &nOutLimitValue) { // TODO // static const int sc_nIndFaciType1_Ban = 3; // const HdLink *cphlLink = GetLinkData(lLinkId); // if (cphlLink != NULL) { // for (hdindfacility hfFaci : cphlLink->indfacilities) { // if (hfFaci.type1 == sc_nIndFaciType1_Ban) { // if (hfFaci.type2 == nLimitType) { // nOutLimitValue = hfFaci.value; // } // } // } // return true; // } return false; } bool HdLib::GetLinkRank(const HdLinkId &lLinkId, int &nOutRank) { const HdLink *cphlLink = GetLinkData(lLinkId); if (cphlLink != NULL) { nOutRank = cphlLink->nRank; return true; } return false; } bool HdLib::GetLinkPre(const HdLinkId &lLinkId, std::vector<HdLinkId> &vctlOutPLinkId) { const HdLink *cphlLink = GetLinkData(lLinkId); if (cphlLink != NULL) { if (false == cphlLink->vctliPreLinkId.empty()) { vctlOutPLinkId = cphlLink->vctliPreLinkId; return true; } } return false; } bool HdLib::GetLinkNext(const HdLinkId &lLinkId, std::vector<HdLinkId> &vctlOutNLinkId) { const HdLink *cphlLink = GetLinkData(lLinkId); if (cphlLink != NULL) { if (false == cphlLink->vctliNxtLinkId.empty()) { vctlOutNLinkId = cphlLink->vctliNxtLinkId; return true; } } return false; } bool HdLib::GetLinkNext(const HdLinkId &lLinkId, int nLaneSeqNum, std::vector<HdLinkId> &vctlOutNLinkId) { const HdLink *cphlLink = GetLinkData(lLinkId); if (cphlLink != NULL) { if (false == cphlLink->vctliNxtLinkId.empty()) { std::vector<const HdLink *> vctstNextLinks = {}; for (const HdLinkId &lNLinkId : cphlLink->vctliNxtLinkId) { const HdLink *hlNextLink = GetLinkData(lNLinkId); if (hlNextLink) { for (HdLane hlNextLane : hlNextLink->vcthlLane) { for (HdLaneId lNextLanePLaneId : hlNextLane.vctliPreLaneId) { if (lNextLanePLaneId == cphlLink->vcthlLane[nLaneSeqNum - 1].liId) { vctstNextLinks.push_back(hlNextLink); } } } } } // 去重 sort(vctstNextLinks.begin(), vctstNextLinks.end(), [](const HdLink *hlLink1, const HdLink *hlLink2) { return hlLink1->liId < hlLink2->liId; }); vctstNextLinks.erase(unique(vctstNextLinks.begin(), vctstNextLinks.end(), [](const HdLink *hlLink1, const HdLink *hlLink2) { return hlLink1->liId == hlLink2->liId; }), vctstNextLinks.end()); for (const HdLink *cphlNLink : vctstNextLinks) { vctlOutNLinkId.push_back(cphlNLink->liId); } return true; } } return false; } //********************Lane相关api********************// /** * 根据linkId和lane_no获取lane对象 * @param lLinkId * @param lane_no 对应地图中的lane图层中的lane_no * @param hdLaneOut * @return */ bool HdLib::GetLaneData(const HdLinkId &lLinkId, int lane_no, HdLane &hdLaneOut) { const HdLink *ret = m_hwHdWorld->GetNodeData(lLinkId); for (const HdLane hdLane : ret->vcthlLane) { if (hdLane.nSeqNum == lane_no) { hdLaneOut = hdLane; return true; } } return false; } /** * 根据linkID和nLaneSeqNum 获取车道lane的ID * @param lLinkId * @param nLaneSeqNum 对应地图中的lane图层中的lane_no * @param lOutLaneId * @return */ bool HdLib::GetLaneId(const HdLinkId &lLinkId, int nLaneSeqNum, HdLaneId &lOutLaneId) { const HdLink *cphlLink = GetLinkData(lLinkId); if ((cphlLink != NULL)) { for (HdLane hdLane : cphlLink->vcthlLane) { if (hdLane.nSeqNum == nLaneSeqNum) { lOutLaneId = hdLane.liId; return true; } } } return false; } bool HdLib::GetLaneGeoLink(const HdLinkId &lLinkId, int nLaneSeqNum, std::vector<std::vector<double>> &vctvdOutGeoLink) { const HdLink *cphlLink = GetLinkData(lLinkId); if (cphlLink != NULL) { for (const HdLane &hlLane : cphlLink->vcthlLane) { if (hlLane.nSeqNum == nLaneSeqNum) { vctvdOutGeoLink = hlLane.vctvdGeoLink; return true; } } } return false; } bool HdLib::GetLaneNum(const HdLinkId &lLinkId, const HdLaneId &lLaneId, int &nOutLaneSeqNum) { const HdLink *cphlLink = GetLinkData(lLinkId); if (cphlLink != NULL) { for (size_t i = 0; i < cphlLink->vcthlLane.size(); i++) { const HdLane &hlLane = cphlLink->vcthlLane.at(i); if (hlLane.liId.lValue == lLaneId.lValue) { nOutLaneSeqNum = hlLane.nSeqNum; return true; } } } return false; } bool HdLib::GetLaneType(const HdLinkId &lLinkId, int nLaneSeqNum, int &nOutLaneType) { const HdLink *cphlLink = GetLinkData(lLinkId); if ((cphlLink != NULL) && (nLaneSeqNum >= 1 && nLaneSeqNum <= cphlLink->vcthlLane.size())) { nOutLaneType = cphlLink->vcthlLane[nLaneSeqNum - 1].nType; return true; } return false; } bool HdLib::GetLaneWidth(const HdLinkId &lLinkId, int nLaneSeqNum, int &nOutLaneWidth) { const HdLink *cphlLink = GetLinkData(lLinkId); if ((cphlLink != NULL) && (nLaneSeqNum >= 1 && nLaneSeqNum <= cphlLink->vcthlLane.size())) { nOutLaneWidth = cphlLink->vcthlLane[nLaneSeqNum - 1].nWidth; return true; } return false; } bool HdLib::GetLaneEdgeId(const HdLinkId &lLinkId, int nLaneSeqNum, bool bIsLeft, HdEdgeId &lOutEdgeId) { const HdLink *cphlLink = GetLinkData(lLinkId); if ((cphlLink != NULL) && (nLaneSeqNum >= 1 && nLaneSeqNum <= cphlLink->vcthlLane.size())) { lOutEdgeId = bIsLeft == true ? cphlLink->vcthlLane[nLaneSeqNum - 1].lLeftEdgeId : cphlLink->vcthlLane[nLaneSeqNum - 1].lRightEdgeId; return true; } return false; } bool HdLib::GetLaneNext(const HdLinkId &lLinkId, int nLaneSeqNum, std::vector<HdLaneId> &vctlOutNLaneId) { const HdLink *cphlLink = GetLinkData(lLinkId); if ((cphlLink != NULL) && (nLaneSeqNum >= 1 && nLaneSeqNum <= cphlLink->vcthlLane.size())) { vctlOutNLaneId = cphlLink->vcthlLane[nLaneSeqNum - 1].vctliNxtLaneId; return true; } return false; } bool HdLib::GetLaneNext(const HdLinkId &lLinkId, const HdLaneId &lLaneId, std::vector<HdLaneId> &vctlOutNLaneId) { const HdLink *cphlLink = GetLinkData(lLinkId); if (cphlLink != NULL) { for (const HdLane &hlLane : cphlLink->vcthlLane) { if (hlLane.liId == lLaneId) { vctlOutNLaneId = hlLane.vctliNxtLaneId; return true; } } } return false; } bool HdLib::GetLaneGeoLink(const HdLinkId &lLinkId, const HdLaneId &lLaneId, std::vector<std::vector<double>> &vctvdOutGeoLink) { const HdLink *cphlLink = GetLinkData(lLinkId); if (cphlLink != NULL) { for (const HdLane &chlLane : cphlLink->vcthlLane) { if (chlLane.liId == lLaneId) { vctvdOutGeoLink = chlLane.vctvdGeoLink; return true; } } } return false; } bool HdLib::GetLaneCurvature(const HdLinkId &lLinkId, const HdLaneId &lLaneId, const Point &ptInPoint, double &dOutCurvature) { auto func_GetNearestLaneAdas = [this](const std::vector<HdLaneAdas> vctlaLaneADASes, const Point &ptInPoint, int &nOutIndex) { double dMinDistance = std::numeric_limits<double>::max(); int nTmpIndex = 0; for (size_t j = 0; j < vctlaLaneADASes.size(); ++j) { Point ptGeo = Point(vctlaLaneADASes[j].dLon, vctlaLaneADASes[j].dLat, 0); double d = PointsDis(ptGeo, ptInPoint); if (d < dMinDistance) { dMinDistance = d; nTmpIndex = j; } } nOutIndex = nTmpIndex; return true; }; const HdLink *cphlLink = GetLinkData(lLinkId); if (cphlLink != NULL) { for (size_t i = 0; i < cphlLink->vcthlLane.size(); i++) { if (cphlLink->vcthlLane[i].liId == lLaneId) { int nLaneAdasIndex = 0; if (false == cphlLink->vcthlLane[i].vctlaLaneAdas.empty()) { func_GetNearestLaneAdas(cphlLink->vcthlLane[i].vctlaLaneAdas, ptInPoint, nLaneAdasIndex); dOutCurvature = cphlLink->vcthlLane[i].vctlaLaneAdas[nLaneAdasIndex].dOutCurvature; return true; } } } } return false; } bool HdLib::GetLaneRoll(const HdLinkId &lLinkId, const HdLaneId &lLaneId, const Point &ptInPoint, double &dOutRoll) { auto func_GetNearestLaneAdas = [this](const std::vector<HdLaneAdas> vctlaLaneADASes, const Point &ptInPoint, int &nOutIndex) { double dMinDistance = std::numeric_limits<double>::max(); int nTmpIndex = 0; for (size_t j = 0; j < vctlaLaneADASes.size(); ++j) { Point ptGeo = Point(vctlaLaneADASes[j].dLon, vctlaLaneADASes[j].dLat, 0); double d = PointsDis(ptGeo, ptInPoint); if (d < dMinDistance) { dMinDistance = d; nTmpIndex = j; } } nOutIndex = nTmpIndex; return true; }; const HdLink *cphlLink = GetLinkData(lLinkId); if (cphlLink != NULL) { for (size_t i = 0; i < cphlLink->vcthlLane.size(); i++) { if (cphlLink->vcthlLane[i].liId == lLaneId) { int nLaneAdasIndex = 0; if (false == cphlLink->vcthlLane[i].vctlaLaneAdas.empty()) { func_GetNearestLaneAdas(cphlLink->vcthlLane[i].vctlaLaneAdas, ptInPoint, nLaneAdasIndex); dOutRoll = cphlLink->vcthlLane[i].vctlaLaneAdas[nLaneAdasIndex].dOutRoll; return true; } } } } return false; } bool HdLib::GetLanePitch(const HdLinkId &lLinkId, const HdLaneId &lLaneId, const Point &ptInPoint, double &dOutPitch) { auto func_GetNearestLaneAdas = [this](const std::vector<HdLaneAdas> vctlaLaneADASes, const Point &ptInPoint, int &nOutIndex) { double dMinDistance = std::numeric_limits<double>::max(); int nTmpIndex = 0; for (size_t j = 0; j < vctlaLaneADASes.size(); ++j) { Point ptGeo = Point(vctlaLaneADASes[j].dLon, vctlaLaneADASes[j].dLat, 0); double d = PointsDis(ptGeo, ptInPoint); if (d < dMinDistance) { dMinDistance = d; nTmpIndex = j; } } nOutIndex = nTmpIndex; return true; }; const HdLink *cphlLink = GetLinkData(lLinkId); if (cphlLink != NULL) { for (size_t i = 0; i < cphlLink->vcthlLane.size(); i++) { if (cphlLink->vcthlLane[i].liId == lLaneId) { int nLaneAdasIndex = 0; if (false == cphlLink->vcthlLane[i].vctlaLaneAdas.empty()) { func_GetNearestLaneAdas(cphlLink->vcthlLane[i].vctlaLaneAdas, ptInPoint, nLaneAdasIndex); dOutPitch = cphlLink->vcthlLane[i].vctlaLaneAdas[nLaneAdasIndex].dOutPitch; return true; } } } } return false; } bool HdLib::GetLaneAzimuth(const HdLinkId &lLinkId, const HdLaneId &lLaneId, const Point &ptInPoint, double &dOutAzimuth) { auto func_GetNearestLaneAdas = [this](const std::vector<HdLaneAdas> vctlaLaneADASes, const Point &ptInPoint, int &nOutIndex) { double dMinDistance = std::numeric_limits<double>::max(); int nTmpIndex = 0; for (size_t j = 0; j < vctlaLaneADASes.size(); ++j) { Point ptGeo = Point(vctlaLaneADASes[j].dLon, vctlaLaneADASes[j].dLat, 0); double d = PointsDis(ptGeo, ptInPoint); if (d < dMinDistance) { dMinDistance = d; nTmpIndex = j; } } nOutIndex = nTmpIndex; return true; }; const HdLink *cphlLink = GetLinkData(lLinkId); if (cphlLink != NULL) { for (size_t i = 0; i < cphlLink->vcthlLane.size(); i++) { if (cphlLink->vcthlLane[i].liId == lLaneId) { int nLaneAdasIndex = 0; if (false == cphlLink->vcthlLane[i].vctlaLaneAdas.empty()) { func_GetNearestLaneAdas(cphlLink->vcthlLane[i].vctlaLaneAdas, ptInPoint, nLaneAdasIndex); dOutAzimuth = cphlLink->vcthlLane[i].vctlaLaneAdas[nLaneAdasIndex].dOutAzimuth; return true; } } } } return false; } bool HdLib::GetEdgeCnt(const HdLinkId &lLinkId, int &nOutEdgesCnt) { const HdLink *cphlLink = GetLinkData(lLinkId); if (cphlLink != NULL) { nOutEdgesCnt = cphlLink->vctheEdge.size(); return true; } return false; } bool HdLib::GetEdgeId(const HdLinkId &lLinkId, int nEdgeIdx, HdEdgeId &lOutEdgeId) { const HdLink *cphlLink = GetLinkData(lLinkId); if (cphlLink != NULL) { lOutEdgeId = cphlLink->vctheEdge[nEdgeIdx].lId; return true; } return false; } bool HdLib::GetEdgeCross(const HdLinkId &lLinkId, HdEdgeId lId, int &nOutEdgeCross) { const HdLink *cphlLink = GetLinkData(lLinkId); if (cphlLink != NULL) { for (size_t i = 0; i < cphlLink->vctheEdge.size(); i++) { if (cphlLink->vctheEdge[i].lId == lId) { nOutEdgeCross = cphlLink->vctheEdge[i].nCross; return true; } } } return false; } bool HdLib::GetEdgeColor(const HdLinkId &lLinkId, HdEdgeId lId, int &nOutEdgeColor) { const HdLink *cphlLink = GetLinkData(lLinkId); if (cphlLink != NULL) { for (size_t i = 0; i < cphlLink->vctheEdge.size(); i++) { if (cphlLink->vctheEdge[i].lId == lId) { if (true == cphlLink->vctheEdge[i].vcthmMark.empty()) { nOutEdgeColor = 1; // white for default } else { nOutEdgeColor = cphlLink->vctheEdge[i].vcthmMark[0].nColor; } return true; } } } return false; } bool HdLib::GetLaneEdgeCrossType(const HdLinkId &lLinkId, int nLaneSeqNum, bool bIsLeft, int &nOutCrossType) { const HdLink *cphlLink = GetLinkData(lLinkId); if ((cphlLink != NULL) && (nLaneSeqNum >= 1 && nLaneSeqNum <= cphlLink->vcthlLane.size())) { HdEdgeId lId = bIsLeft == true ? cphlLink->vcthlLane[nLaneSeqNum - 1].lLeftEdgeId : cphlLink->vcthlLane[nLaneSeqNum - 1].lRightEdgeId; int nEdgeIdx = 0; for (size_t i = 0; i < cphlLink->vctheEdge.size(); i++) { if (cphlLink->vctheEdge[i].lId == lId) { nOutCrossType = cphlLink->vctheEdge[i].nCross; return true; } } } return false; } bool HdLib::GetLaneEdgeGeoLink(const HdLinkId &lLinkId, int nLaneSeqNum, bool bIsLeft, std::vector<std::vector<double>> &vctvdOutGeoLink) { const HdLink *cphlLink = GetLinkData(lLinkId); if ((cphlLink != NULL) && (nLaneSeqNum >= 1 && nLaneSeqNum <= cphlLink->vcthlLane.size())) { HdEdgeId lId = bIsLeft == true ? cphlLink->vcthlLane[nLaneSeqNum - 1].lLeftEdgeId : cphlLink->vcthlLane[nLaneSeqNum - 1].lRightEdgeId; int nEdgeIdx = 0; for (size_t i = 0; i < cphlLink->vctheEdge.size(); i++) { if (cphlLink->vctheEdge[i].lId == lId) { vctvdOutGeoLink = cphlLink->vctheEdge[i].vctvdGeoLink; return true; } } } return false; } bool HdLib::GetEdgeGeoLink(const HdLinkId &lLinkId, HdEdgeId lId, std::vector<std::vector<double>> &vctvdOutGeoLink) { const HdLink *cphlLink = GetLinkData(lLinkId); if (cphlLink != NULL) { for (size_t i = 0; i < cphlLink->vctheEdge.size(); i++) { if (cphlLink->vctheEdge[i].lId == lId) { vctvdOutGeoLink = cphlLink->vctheEdge[i].vctvdGeoLink; return true; } } } return false; } bool HdLib::GetStopLineGeoLink(const HdLinkId &clLinkId, int nType, std::vector<std::vector<double>> &vctvdOutGeoLink) { const HdLink *cphlLink = GetLinkData(clLinkId); if ((cphlLink != NULL) && (false == cphlLink->vctslStopLine.empty())) { for (size_t i = 0; i < cphlLink->vctslStopLine.size(); i++) { if (cphlLink->vctslStopLine[i].nType == nType) { vctvdOutGeoLink = cphlLink->vctslStopLine[i].vctvdGeoLink; return true; } } } return false; } bool HdLib::GetTrafficLightGeoLink(const HdLinkId &clLinkId, int nLaneSeqNum, int nType, std::vector<std::vector<double>> &vctvdOutGeoLink) { return true; } bool HdLib::GetOverHeadGeoLink(const HdLinkId &clLinkId, int nLaneSeqNum, int nType, std::vector<std::vector<double>> &vctvdOutGeoLink) { const HdLink *cphlLink = GetLinkData(clLinkId); if (cphlLink != NULL) { for (ObjOverHead objOverHead : cphlLink->vctOverHead) { for (HdLaneId hdLaneId : objOverHead.vctlLaneId) { if (hdLaneId == cphlLink->vcthlLane[nLaneSeqNum - 1].liId) { vctvdOutGeoLink = objOverHead.vctvdGeoLink; return true; } } } } return false; } bool HdLib::GetTrafficSign(const HdLinkId &clLinkId, std::vector<int> &vctnOutShape, std::vector<int> &vctnOutType1, std::vector<int> &vctnOutType2, std::vector<std::string> &vctstOutText, std::vector<std::vector<std::vector<double>>> &vctvvOutGeoArea) { // const HdLink *cphlLink = GetLinkData(clLinkId); // if ((cphlLink != NULL) && (false == cphlLink->vcttsTrafSign.empty())) { // for (size_t i = 0; i < cphlLink->vcttsTrafSign.size(); i++) { // vctnOutShape.push_back(cphlLink->vcttsTrafSign[i].nShape); // vctnOutType1.push_back(cphlLink->vcttsTrafSign[i].nType1); // vctnOutType2.push_back(cphlLink->vcttsTrafSign[i].nType2); // vctstOutText.push_back(cphlLink->vcttsTrafSign[i].strText); // vctvvOutGeoArea.push_back(cphlLink->vcttsTrafSign[i].vctvdGeoArea); // } // return true; // } return false; } bool HdLib::GetTrafficSign(const Point &cptPoint, std::vector<int> &vctnOutShape, std::vector<int> &vctnOutType1, std::vector<int> &vctnOutType2, std::vector<std::string> &vctstOutText, std::vector<std::vector<std::vector<double>>> &vctvvOutGeoArea) { std::vector<HdLinkId> vcthlLinkId; std::vector<const HdLink *> vcthlLink = {}; if (true == GetDataNearby(cptPoint, MapConfig::tile_lon_interval, MapConfig::tile_lat_interval, vcthlLink)) { for (auto link : vcthlLink) { vcthlLinkId.push_back(link->liId); } for (const HdLinkId &lLinkId : vcthlLinkId) { GetTrafficSign(lLinkId, vctnOutShape, vctnOutType1, vctnOutType2, vctstOutText, vctvvOutGeoArea); } return true; } return false; } bool HdLib::GetTrafficSignGeoArea(const HdLinkId &clLinkId, int nShape, std::vector<std::vector<double>> &vctvdOutGeoArea) { // const HdLink *cphlLink = GetLinkData(clLinkId); // if ((cphlLink != NULL) && (false == cphlLink->vcttsTrafSign.empty())) { // for (size_t i = 0; i < cphlLink->vcttsTrafSign.size(); i++) { // if (cphlLink->vcttsTrafSign[i].nShape == nShape) { // vctvdOutGeoArea = cphlLink->vcttsTrafSign[i].vctvdGeoArea; // return true; // } // } // } return false; } bool HdLib::IsLaneContainArrow(const HdLinkId &lLinkId, int nLaneSeqNum, char cArrowType) { const HdLink *cphlLink = GetLinkData(lLinkId); if ((cphlLink != NULL) && (nLaneSeqNum >= 1 && nLaneSeqNum <= cphlLink->vcthlLane.size())) { if (cphlLink->vcthlLane[nLaneSeqNum - 1].strArrows.find(sc_cArrow_LeftConf) != std::string::npos) { return true; } } return false; } bool HdLib::GetLaneArrow(const HdLinkId &lLinkId, const HdLaneId &lLaneId, std::string &strOutArrows) { const HdLink *cphlLink = GetLinkData(lLinkId); if (cphlLink != NULL) { for (const HdLane &hlLane : cphlLink->vcthlLane) { if (hlLane.liId == lLaneId) { strOutArrows = hlLane.strArrows; for (const ObjArrow &arArrow : hlLane.vctarArrow) { strOutArrows += arArrow.strType; } return true; } } } return false; } bool HdLib::ConvertGeoLink(const std::vector<std::vector<double>> &vctvdGeoLink, std::vector<Point> &vctptOutGeoLink) { for (std::vector<double> geo : vctvdGeoLink) { vctptOutGeoLink.push_back(Point(geo[0], geo[1], geo[2])); } return true; } bool HdLib::ConvertGeoLink(const std::vector<Point> &vctptGeoLink, std::vector<std::vector<double>> &vctvdOutGeoLink) { for (Point ptPoint : vctptGeoLink) { std::vector<double> vctdGeo = {ptPoint.dLon, ptPoint.dLat, ptPoint.dAlt}; vctvdOutGeoLink.push_back(vctdGeo); } return true; } int HdLib::GetLinkSpeedLimit(const LaneEndpoint &leEndpoint) { // 加载 // LoadTile(leEndpoint.ptEpNode); const HdLink *hlCurLink = GetLinkData(leEndpoint.lLinkId); if (!hlCurLink) { return -1; } else { return hlCurLink->nSpeedLimit; } } bool HdLib::HasStopLine(const LaneEndpoint &leEndpoint) { // 加载 // LoadTile(leEndpoint.ptEpNode); const HdLink *hlCurLink = GetLinkData(leEndpoint.lLinkId); if (!hlCurLink) { return false; } else { for (const HdEdge &heEdge : hlCurLink->vctheEdge) { static const int sc_nEdgeType_StopLine = 7; if (sc_nEdgeType_StopLine == heEdge.nType) { return true; } } return false; } } bool HdLib::GetGeoLink(const LaneEndpoint &leEndpoint, std::vector<std::vector<double>> &vctvdOutGeoLink, bool bManageTile /* = false*/) { // 加载 if (true == bManageTile) { Rect2D r = P2R(leEndpoint.ptEpNode, MapConfig::tile_lon_interval, MapConfig::tile_lon_interval); m_hwHdWorld->LoadTileInRect(r); // m_hwHdWorld->RemoveUnsedTiles(leEndpoint.ptEpNode); } const HdLink *hlLink = GetLinkData(leEndpoint.lLinkId); if (!hlLink) { return false; } if (leEndpoint.nStartLaneNum == leEndpoint.nEndLaneNum) { vctvdOutGeoLink = hlLink->vcthlLane[leEndpoint.nEndLaneNum - 1].vctvdGeoLink; } else { vctvdOutGeoLink.push_back(hlLink->vcthlLane[leEndpoint.nStartLaneNum - 1].vctvdGeoLink[0]); vctvdOutGeoLink.push_back(hlLink->vcthlLane[leEndpoint.nEndLaneNum - 1].vctvdGeoLink[hlLink->vcthlLane[leEndpoint.nEndLaneNum - 1].vctvdGeoLink.size() - 1]); } return true; } bool HdLib::GetGeoLink(const HdLinkId &lLinkId, std::vector<std::vector<double>> &vctvdOutGeoLink) { const HdLink *hlLink = GetLinkData(lLinkId); if (!hlLink) { return false; } vctvdOutGeoLink = hlLink->vctvdGeoLink; return true; } double HdLib::GetDistanceToLeftEdge(const LaneEndpoint &leEndpoint, const Point &ptPoint) { // 加载 // LoadTile(leEndpoint.ptEpNode); const HdLink *hlLink = GetLinkData(leEndpoint.lLinkId); if (!hlLink) { return -1; } else { int nLeftestLaneIndex = 0; double dMinDistance = std::numeric_limits<double>::max(); int nIndex = GetEdgeIndexById(hlLink, hlLink->vcthlLane[nLeftestLaneIndex].lLeftEdgeId); if (nIndex != -1) { Point ptOutPoint = {}; double dOutDistance = 0; HdLibMath::GetPerpendicularFoot(ptPoint, hlLink->vctheEdge[nIndex].vctvdGeoLink, ptOutPoint, dOutDistance); dMinDistance = dOutDistance; } return dMinDistance; } } double HdLib::GetDistanceToRightEdge(const LaneEndpoint &leEndpoint, const Point &ptPoint) { // 加载 // LoadTile(leEndpoint.ptEpNode); const HdLink *hlLink = GetLinkData(leEndpoint.lLinkId); if (!hlLink) { return -1; } else { int nRightestLaneIndex = hlLink->vcthlLane.size() - 1; double dMinDistance = std::numeric_limits<double>::max(); int nIndex = GetEdgeIndexById(hlLink, hlLink->vcthlLane[nRightestLaneIndex].lRightEdgeId); if (nIndex != -1) { Point ptOutPoint = {}; double dOutDistance = 0; HdLibMath::GetPerpendicularFoot(ptPoint, hlLink->vctheEdge[nIndex].vctvdGeoLink, ptOutPoint, dOutDistance); dMinDistance = dOutDistance; } return dMinDistance; } } int HdLib::GetEdgeIndexById(const HdLink *hlLink, HdEdgeId lId) { for (int i = 0; i < hlLink->vctheEdge.size(); ++i) { if (hlLink->vctheEdge[i].lId == lId) { return i; } } return -1; } bool HdLib::GetLinkIdsInRect(const Point &ptPoint, float fLonIntervalHalfMetre, float flatintervalhalfmetre, std::vector<HdLinkId> &vctlOutLinkId, bool bManageTile /* = true */) { auto start = Timer::Now(); // rect r = P2R<hdconfig>(ingps); Rect2D r = P2R(ptPoint, fLonIntervalHalfMetre, flatintervalhalfmetre); if (bManageTile) { m_hwHdWorld->LoadTileInRect(r); // m_hwHdWorld->RemoveUnsedTiles(ingps); } std::set<const HdLink *> links = m_hwHdWorld->GetNodesInRect(r); std::vector<const HdLink *> ls = jf::Utils::Set2Vector(links); return true; } std::set<HdLinkId> HdLib::GetNodeIdsInRect(const Rect2D &rc2Rect) { return m_hwHdWorld->GetNodeIdsInRect(rc2Rect); } bool HdLib::GetPosInHdLink(const Point &ptCurPos, const HdLinkId &lLinkId, double &dOutToHeadDis, double &dOutToTailDis) { const HdLink *stLink = GetLinkData(lLinkId); if (!stLink) { return false; } Point ptFootPos = {}; double dNearDis = -1; int nNxtIndex = 0; if (false == HdLibMath::GetNearestPointFromArray(ptCurPos, stLink->vctvdGeoLink, ptFootPos, dNearDis, nNxtIndex)) { return false; } double dRealLength = 0; for (int i = 0; i < nNxtIndex - 1; i++) { dRealLength += PointsDis(Point(stLink->vctvdGeoLink[i]), Point(stLink->vctvdGeoLink[i + 1])); } dOutToHeadDis = dRealLength + PointsDis(Point(stLink->vctvdGeoLink[nNxtIndex - 1]), ptFootPos); dRealLength = PointsDis(ptFootPos, Point(stLink->vctvdGeoLink[nNxtIndex])); for (size_t i = nNxtIndex; i < stLink->vctvdGeoLink.size() - 1; ++i) { dRealLength += PointsDis(Point(stLink->vctvdGeoLink[i]), Point(stLink->vctvdGeoLink[i + 1])); } dOutToTailDis = dRealLength; return true; } bool HdLib::GetLanesInTypes(std::vector<int> &vctnLaneType, const HdLink *hlLink, std::vector<const HdLane *> &vcthlOutLanes) { for (auto &hlLane : hlLink->vcthlLane) { if (true == Utils::VectorContains(vctnLaneType, hlLane.nType)) { vcthlOutLanes.push_back(&hlLane); } } return true; } bool HdLib::GetLanesOutTypes(std::vector<int> &vctnLaneType, const HdLink *hlLink, std::vector<const HdLane *> &vcthlOutLanes) { for (auto &hlLane : hlLink->vcthlLane) { if (false == Utils::VectorContains(vctnLaneType, hlLane.nType)) { vcthlOutLanes.push_back(&hlLane); } } return true; }; bool HdLib::GetLaneCnt(const HdLinkId &lLinkId, int &nOutLaneCnt) { const HdLink *cphlLink = GetLinkData(lLinkId); if ((cphlLink != NULL)) { nOutLaneCnt = cphlLink->vcthlLane.size(); return true; } return false; }