#include "pch.h" // use stdafx.h in Visual Studio 2017 and earlier #include "Astar.h" Astar astar; void DSU::init(int cnt) { for (int i = 1; i <= cnt; i++) f[i] = i; } int DSU::getrt(int x) { if (x != f[x]) return f[x] = getrt(f[x]); return x; } void DSU::merge(int x, int y) { if (x != f[x]) x = getrt(x); if (y != f[y]) y = getrt(y); f[x] = y; } bool Node::operator<(Node B) const { // if(turncnt!=B.turncnt)return turncnt>B.turncnt; return dis > B.dis; } Node::Node() { xs = 0; dis = 0; turncnt = 0; } Node::Node(int xs1, int turnc, double dis1) { xs = xs1; dis = dis1; turncnt = turnc; } Astar::Astar() { pnum = 0; l = bend_r = 0; tnum = 0; pclipSet = &clipSet; } void Astar::addBranchTree(BranchTree *pbranchTree) { this->pbranchTree = pbranchTree; } // 初始化 void Astar::init() { pnum += 2; } // 添加一个卡箍点 void Astar::add_clip(P &p, P &d) { pnum++; tnum++; points[pnum] = p; points[pnum].ref = pnum; P dir = p - d; points[pnum].dx = dir.x; points[pnum].dy = dir.y; points[pnum].dz = dir.z; block[pnum] = 0; } void Astar::add_clip(Clip &cp) { pnum++; tnum++; points[pnum] = cp.coord; block[pnum] = 0; } // 添加一个分支点 void Astar::add_branchPoint(BranchPoint &bp) { tnum = max(tnum, clipSet.clipnum + 2); tnum++; points[tnum] = bp.coord; block[tnum] = 0; } Path Astar::search_pair(P &start, P &goal, P &start_dir, P &goal_dir, double dia) { this->start = start; this->goal = goal; this->start_dir = start_dir; this->goal_dir = goal_dir; path.dia = dia; pnum -= 2; path.points.clear(); path.wirelist.clear(); path.dia = dia; search(); return path; } Path Astar::search_pair(P &start, P &goal, double dia) { this->start = start; this->goal = goal; path.dia = dia; pnum -= 2; path.points.clear(); path.wirelist.clear(); path.dia = dia; search(); return path; } Path Astar::search_pair(Bund &bd) { this->start = bd.start; this->goal = bd.goal; pnum -= 2; path.points.clear(); path.inOut.clear(); path.wirelist = bd.wirenamelist; path.dia = bd.dia; tnum = max(tnum, clipSet.clipnum + 2); search(); return path; } void Astar::searchForPreProcess() { while (!pq.empty()) pq.pop(); points[++pnum] = start; points[++pnum] = goal; S = pnum - 1; T = pnum; for (int i = 1; i <= 2 * tnum + 2; i++) vis[i] = 0, dis[i] = Node(i, 1000, 1e9), pre[i] = 0; dis[S * 2] = Node(S * 2, 0, 0); assert(pre[2 * S] == 0); pq.push(dis[S * 2]); vector> tempvec = basicEdge.getEXTNextPoint(points[T], pnum, 1); vector> pointsConnectedToGoal; for (int i = 0; i < tempvec.size(); i++) { int xs = tempvec[i].first; if (!bvh.iscollect(points[xs], points[T])) pointsConnectedToGoal.push_back(tempvec[i]); } bool testflag = true; while (!pq.empty()) { Node node = pq.top(); // cout< currentNode = make_pair(xs, inOut); if (xs == T) break; vector> vec; if (xs != S) vec = basicEdge.getNextPoint(xs, inOut, pnum); else if (xs == S && nearClip[points[xs]] != make_pair(0, 0)) vec.push_back(nearClip[points[S]]); else { vec = basicEdge.getEXTNextPoint(points[S], pnum, 0); // cout<transitable(v,0,path.dia)))continue; // if(block[v])continue; ; } // cout<computeWeight(points[v], mode, 0); } if (points[v].type == 1 || points[v].type == 2) { int bid = points[v].ref; BranchPoint *pb = branchPointSet.getBranchPointPointer(bid); cst = pb->computeWeight(points[xs], inOut, 1); } cst += +dis[xs * 2 + inOut].dis; if (dis[2 * v + mode] < Node(2 * v + mode, 0, cst)) { dis[2 * v + mode] = Node(2 * v + mode, 0, cst); assert(v != pnum - 1); pre[2 * v + mode] = xs * 2 + inOut; pq.push(dis[2 * v + mode]); } } } int top = 0; int tag = T * 2; while (pre[tag]) { st[++top] = tag; tag = pre[tag]; if (testflag) cout << "nodeid " << tag << endl; } if (top) { st[++top] = S * 2; } for (int i = top; i > 0; i--) { int id = st[i]; if (top > 2) { if (id / 2 != S && id / 2 != T) { if (i == top - 1) nearClip[points[S]] = make_pair(id / 2, id % 2); if (i == 2) nearClip[points[T]] = make_pair(id / 2, id % 2); } } // if(i!=top&&i!=1)pclipSet->addwire(id,dia); path.points.push_back(points[id / 2]); if (i != top && i != 1 && id / 2 < pnum) path.inOut.push_back(id % 2); else path.inOut.push_back(-1 - id % 2); } } void Astar::search() { searchNum++; cout << "searchNum " << searchNum << endl; while (!pq.empty()) pq.pop(); points[++pnum] = start; points[++pnum] = goal; S = pnum - 1; T = pnum; for (int i = 1; i <= 2 * tnum + 2; i++) vis[i] = 0, dis[i] = Node(i, 1000, 1e9), pre[i] = 0; dis[S * 2] = Node(S * 2, 0, 0); assert(pre[2 * S] == 0); pq.push(dis[S * 2]); vector> tempvec = basicEdge.getEXTNextPoint(points[T], pnum, 1); vector> pointsConnectedToGoal; for (int i = 0; i < tempvec.size(); i++) { int xs = tempvec[i].first; if (!bvh.iscollect(points[xs], points[T])) pointsConnectedToGoal.push_back(tempvec[i]); } bool testflag = true; int whilenum = 0; while (!pq.empty()) { Node node = pq.top(); // cout< currentNode = make_pair(xs, inOut); if (xs == T) break; vector> vec; if (xs != S) vec = basicEdge.getNextPoint(xs, inOut, pnum); else if (xs == S && nearClip[points[xs]] != make_pair(0, 0)) vec.push_back(nearClip[points[S]]); else { vec = basicEdge.getEXTNextPoint(points[S], pnum, 0); // cout<transitable(v,0,path.dia)))continue; // if(block[v])continue; ; } // cout<Dis) / 2; } else { BranchPoint *pb = branchPointSet.getBranchPointPointer(bid); cst = pb->computeWeight(points[v], mode, 0); updatePreBranchPoint = true; tempPos1 = pb->t; } } if (points[v].type == 1 || points[v].type == 2) { int bid = points[v].ref; BranchPoint *pb = branchPointSet.getBranchPointPointer(bid); if (mode == 1) { cst = (pb->Dis) / 2; } else { cst = pb->computeWeight(points[xs], inOut, 1); updateNextBranchPoint = true; tempPos2 = pb->t; } } cst += dis[xs * 2 + inOut].dis; if (dis[2 * v + mode] < Node(2 * v + mode, 0, cst)) { if (updatePreBranchPoint) { this->preBranchPointPos[2 * v + mode] = tempPos1; } if (updateNextBranchPoint) { int bid = points[v].ref; BranchPoint *pb = branchPointSet.getBranchPointPointer(bid); pb->recordPos(0); } dis[2 * v + mode] = Node(2 * v + mode, 0, cst); assert(v != pnum - 1); pre[2 * v + mode] = xs * 2 + inOut; pq.push(dis[2 * v + mode]); } } } int top = 0; int tag = T * 2; while (pre[tag]) // pre[tag] 值为0导致无结果, { st[++top] = tag; tag = pre[tag]; if (testflag) { cout << "nodeid " << tag << " "; if (tag < (pnum - 1) * 2) cout << basicChannel.clipType(tag / 2) << endl; else cout << -1 << endl; } } if (top) { st[++top] = S * 2; } vector prePos; for (int i = top; i > 0; i--) { int id = st[i]; if (top > 2) { if (id / 2 != S && id / 2 != T) { if (i == top - 1) nearClip[points[S]] = make_pair(id / 2, id % 2); if (i == 2) nearClip[points[T]] = make_pair(id / 2, id % 2); } } // if(i!=top&&i!=1)pclipSet->addwire(id,dia); path.points.push_back(points[id / 2]); if (i != top && i != 1 && id / 2 < pnum) path.inOut.push_back(id % 2); else { path.inOut.push_back(-1 - id % 2); } prePos.push_back(preBranchPointPos[id]); } for (int i = 0; i < path.points.size(); i++) { P &p = path.points[i]; if (p.type == 1 || p.type == 2) { int mode = -1 - path.inOut[i]; int bid = p.ref; BranchPoint *pb = branchPointSet.getBranchPointPointer(bid); pb->mode = mode; if (mode == 1) { pb->t = prePos[i + 1]; pb->recordPos(1); } } } }