232 lines
7.9 KiB
C++
232 lines
7.9 KiB
C++
#include "parkinglotgraph.h"
|
|
#include "Road.h"
|
|
#include <QVector>
|
|
#include <QtMath>
|
|
#include <QStack>
|
|
#include <QPen>
|
|
#include <QDebug>
|
|
#include <QWidget>
|
|
#include <queue>
|
|
|
|
bool inRect(const QWidget* r, const QPoint& p) {
|
|
if (p.x() > r->pos().x() && p.x() < r->pos().x() + r->width() &&
|
|
p.y() > r->pos().y() && p.y() < r->pos().y() + r->height())
|
|
return true;
|
|
else
|
|
return false;
|
|
}
|
|
|
|
QPoint toCenter(const QWidget* w) {
|
|
QPoint p;
|
|
p.setX(w->pos().x() + w->width() / 2);
|
|
p.setY(w->pos().y() + w->height() / 2);
|
|
return p;
|
|
}
|
|
|
|
ParkingLotGraph::ParkingLotGraph(const ParkingLotWidget* pk)
|
|
{
|
|
QVector<ParkingSpaceWidget*> spaces = pk->getSpaceList();
|
|
QVector<Road*> roads = pk->getRoadList();
|
|
QVector<Node*> unFinished;
|
|
m_roadNodeList.resize(roads.size() * 2); // 每一条路有两个节点，分别为2i和2i+1
|
|
for (int i = 0; i < roads.size(); i++) { // 初始化每条路的节点
|
|
QPoint p;
|
|
Road* r = roads.at(i);
|
|
if (r->getDir() == Road::horizontal) {
|
|
p.setX(r->pos().x() + 5);
|
|
p.setY(r->pos().y() + r->height() / 2);
|
|
m_roadNodeList[2*i] = addNode(p, Node::Type::road, r->getNumber());
|
|
p.setX(r->pos().x() + r->width() - 5);
|
|
m_roadNodeList[2*i + 1] = addNode(p, Node::Type::road, r->getNumber());
|
|
} else {
|
|
p.setY(r->pos().y() + 5);
|
|
p.setX(r->pos().x() + r->width() / 2);
|
|
m_roadNodeList[2*i] = addNode(p, Node::Type::road, r->getNumber());
|
|
p.setY(r->pos().y() + r->height() - 5);
|
|
m_roadNodeList[2*i+1] = addNode(p, Node::Type::road, r->getNumber());
|
|
}
|
|
}
|
|
for (int i = 0; i < roads.size(); i++) { // 再遍历路，找到路与路的交点
|
|
Road* r = roads.at(i);
|
|
QPoint p = r->pos();
|
|
if (r->getDir() == Road::horizontal) {
|
|
p.setY(p.y() + r->height() / 2);
|
|
p.setX(p.x() - 30); // 向外找
|
|
} else {
|
|
p.setX(p.x() + r->width() / 2);
|
|
p.setY(p.y() - 30); // 向外找
|
|
}
|
|
for (int j = 0; j < roads.size(); j++) {
|
|
Road* r2 = roads.at(j);
|
|
QPoint crossPoint = p;
|
|
if (i != j && inRect(r2, p)) {
|
|
m_roadNodeList[2*i]->addPath(m_roadNodeList[2*j]);
|
|
m_roadNodeList[2*i]->addPath(m_roadNodeList[2*j + 1]);
|
|
if (r2->getDir() == Road::horizontal)
|
|
crossPoint.setY(r2->pos().y() + r2->height() / 2);
|
|
else
|
|
crossPoint.setX(r2->pos().x() + r2->width() / 2);
|
|
unFinished.push_back(addNode(crossPoint, Node::Type::road, r2->getNumber()));
|
|
break;
|
|
}
|
|
}
|
|
if (r->getDir() == Road::horizontal) {
|
|
p.setX(p.x() + r->width() + 60); // 补偿x减掉的30
|
|
} else {
|
|
p.setY(p.y() + r->height() + 60);
|
|
}
|
|
for (int j = 0; j < roads.size(); j++) {
|
|
Road* r2 = roads.at(j);
|
|
QPoint crossPoint = p;
|
|
if (i != j && inRect(r2, p)) {
|
|
m_roadNodeList[2*i+1]->addPath(m_roadNodeList[2*j]);
|
|
m_roadNodeList[2*i+1]->addPath(m_roadNodeList[2*j + 1]);
|
|
if (r2->getDir() == Road::horizontal)
|
|
crossPoint.setY(r2->pos().y() + r2->height() / 2);
|
|
else
|
|
crossPoint.setX(r2->pos().x() + r2->width() / 2);
|
|
unFinished.push_back(addNode(crossPoint, Node::Type::road, r2->getNumber()));
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
// 连接岔路
|
|
for (Node* cross: unFinished) {
|
|
Node* endPoint = m_roadNodeList.at(2 * (cross->number - 1));
|
|
for (Node* adj: endPoint->adjacent) {
|
|
cross->addPath(adj);
|
|
}
|
|
}
|
|
|
|
for(ParkingSpaceWidget* sp : spaces) {
|
|
QPoint p = toCenter(sp), offset(0, 0);
|
|
Node* spaceNode = addNode(p, Node::Type::space, sp->getNumber());
|
|
m_spaceList.push_back(spaceNode); // 保证按编号顺序
|
|
switch(sp->getDir()) {
|
|
case ParkingSpaceWidget::N: offset.setY(-1);break;
|
|
case ParkingSpaceWidget::S: offset.setY(1);break;
|
|
case ParkingSpaceWidget::E: offset.setX(1);break;
|
|
case ParkingSpaceWidget::W: offset.setX(-1);break;
|
|
}
|
|
while(inRect(sp, p)) p += offset; // 直到走出这个widget
|
|
p+=offset*5; // 再走两步
|
|
for (int i = 0; i < roads.size(); i++) { // 遍历所有路，找到与这个点相连的路，添加路径
|
|
Road* r = roads.at(i);
|
|
if (inRect(r, p)) {
|
|
m_roadNodeList[2*i]->addPath(spaceNode);
|
|
m_roadNodeList[2*i + 1]->addPath(spaceNode);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
void ParkingLotGraph::paint(QGraphicsScene *scene)
|
|
{
|
|
QVector<Node*> vis(m_spaceList.size() + m_roadNodeList.size());
|
|
QStack<Node*> stack;
|
|
QPen pen(Qt::blue);
|
|
qDebug() << "共有" << QString::number(m_spaceList.size()) << "个车位节点";
|
|
qDebug() << "共有" << QString::number(m_roadNodeList.size()) << "个路节点";
|
|
for (int i = 0; i < m_roadNodeList.size(); i++) {
|
|
qDebug() << i << " " << m_roadNodeList.at(i)->adjacent.size();
|
|
}
|
|
stack.push(m_spaceList.first());
|
|
while (!stack.empty()) {
|
|
Node* n = stack.pop();
|
|
if (vis.indexOf(n) != -1)
|
|
continue;
|
|
vis.push_back(n);
|
|
// qDebug() << "关联节点共有" << QString::number(n->adjacent.size());
|
|
scene->addEllipse(n->data.x(), n->data.y(), 5, 5, pen);
|
|
for(Node* adj: n->adjacent) {
|
|
if (vis.indexOf(adj) != -1)
|
|
continue;
|
|
stack.push(adj);
|
|
scene->addLine(n->data.x(), n->data.y(), adj->data.x(), adj->data.y(), pen);
|
|
}
|
|
}
|
|
}
|
|
|
|
// Dijkstra 求最短路径
|
|
Path *ParkingLotGraph::finaPath(ParkingLotGraph::Node::Type t1, uint n1, ParkingLotGraph::Node::Type t2, uint n2)
|
|
{
|
|
struct HeapNode {
|
|
uint d, u;//d为到该点的总路径，u为这一点
|
|
HeapNode(uint a, uint b) :d(a), u(b) {}
|
|
bool operator < (const HeapNode& rhs) const { return d > rhs.d;}//重载优先级队列的小于号
|
|
};
|
|
bool vis[m_all.size()] = {0};
|
|
uint weight[m_all.size()];
|
|
uint prev[m_all.size()] = {0};
|
|
for (int i = 0; i < m_all.size(); i++)
|
|
weight[i] = 0x3f3f3f; // inf
|
|
|
|
uint id1, id2;
|
|
if (t1 == Node::Type::space) {
|
|
id1 = m_spaceList.at(n1 - 1)->getId();
|
|
}
|
|
if (t2 == Node::Type::space) {
|
|
id2 = m_spaceList.at(n2 - 1)->getId();
|
|
}
|
|
|
|
weight[m_spaceList.at(n1 - 1)->id] = 0;
|
|
|
|
std::priority_queue<HeapNode> queue;
|
|
|
|
queue.push(HeapNode(0, id1));
|
|
|
|
while (!queue.empty()) {
|
|
HeapNode cur = queue.top();
|
|
queue.pop();
|
|
if (vis[cur.u])
|
|
continue;
|
|
vis[cur.u] = true;
|
|
Node* node = m_all[cur.u];
|
|
for (int i = 0; i < node->adjacent.size(); i++) {
|
|
if (weight[cur.u] + node->weight[i] < weight[node->adjacent[i]->id]) {
|
|
weight[node->adjacent[i]->id] = weight[cur.u] + node->weight[i];
|
|
prev[node->adjacent[i]->id] = cur.u;
|
|
queue.push(HeapNode(weight[node->adjacent[i]->id], node->adjacent[i]->id));
|
|
}
|
|
}
|
|
}
|
|
Path* path = new Path();
|
|
uint n = id2;
|
|
while (n != id1) {
|
|
path->addPoint(0, *(new PathPoint(QPointF(m_all[n]->data), 0)));
|
|
n = prev[n];
|
|
}
|
|
return path;
|
|
}
|
|
|
|
ParkingLotGraph::~ParkingLotGraph()
|
|
{
|
|
for (Node* n: m_spaceList)
|
|
delete n;
|
|
for (Node* n: m_roadNodeList)
|
|
delete n;
|
|
}
|
|
|
|
|
|
void ParkingLotGraph::Node::addPath(ParkingLotGraph::Node *another)
|
|
{
|
|
for (Node* adj: adjacent)
|
|
if (adj == another)
|
|
return;
|
|
adjacent.push_back(another);
|
|
another->adjacent.push_back(this);
|
|
int dx = data.x() - another->data.x();
|
|
int dy = data.y() - another->data.y();
|
|
float w = qSqrt(dx * dx + dy * dy);
|
|
weight.push_back(w);
|
|
another->weight.push_back(w);
|
|
}
|
|
|
|
uint ParkingLotGraph::Node::getId() const
|
|
{
|
|
return id;
|
|
}
|