374 lines
14 KiB
C++
374 lines
14 KiB
C++
#include "parkinglotgraph.h"
|
|
#include "Road.h"
|
|
#include <QVector>
|
|
#include <QtMath>
|
|
#include <QStack>
|
|
#include <QPen>
|
|
#include <QDebug>
|
|
#include <QWidget>
|
|
#include <queue>
|
|
#include <QPainter>
|
|
|
|
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* pkl): pk(pkl)
|
|
{
|
|
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());
|
|
}
|
|
if (r->getAction() != Road::Action::none) {
|
|
if (r->getAction() != Road::entry) {
|
|
m_roadNodeList[2*i + r->getActionPos() - 1]->setAction(r->getAction()); // 添加动作
|
|
// 把有动作的节点添加到dict里，方便查找
|
|
m_actionList.append(m_roadNodeList[2*i + r->getActionPos() - 1]);
|
|
} else {
|
|
// 当前点为入口
|
|
m_roadNodeList[2*i + r->getActionPos() - 1]->setAction(r->getAction());
|
|
// 另一个点为队首
|
|
m_roadNodeList[2*i + 2 - r->getActionPos()]->setAction(Road::Action::queueHead);
|
|
m_actionList.append(m_roadNodeList[2 * i]);
|
|
m_actionList.append(m_roadNodeList[2 * i + 1]);
|
|
}
|
|
}
|
|
m_roadNodeList[2*i]->addPath(m_roadNodeList[2*i + 1]); // 一条路的两个端点连接起来
|
|
}
|
|
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);
|
|
}
|
|
}
|
|
}
|
|
|
|
uint ParkingLotGraph::getNodeId(ParkingLotGraph::Node::Type t, int n) {
|
|
if (t == Node::Type::space) {
|
|
return m_spaceList.at(n - 1)->getId();
|
|
} else if (t == Node::Type::act) {
|
|
return m_actionList.at(n - 1)->getId();
|
|
} else {
|
|
Road::Action act = static_cast<Road::Action>(t);
|
|
for (auto node: m_actionList) {
|
|
if (node->action == act && --n == 0)
|
|
return node->id;
|
|
}
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
qreal angle(const QPointF& p) {
|
|
return qRadiansToDegrees(qAtan2(p.y(), p.x()));
|
|
}
|
|
|
|
|
|
|
|
// Dijkstra 求最短路径
|
|
Path *ParkingLotGraph::findPath(ParkingLotGraph::Node::Type t1, int n1, ParkingLotGraph::Node::Type t2, int 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
|
|
|
|
n1 = qAbs(n1);
|
|
n2 = qAbs(n2);
|
|
|
|
uint id1 = getNodeId(t1, n1); // 通过number和类型获得id
|
|
uint id2 = getNodeId(t2, n2);
|
|
|
|
weight[id1] = 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, lastN;;
|
|
QPointF lastP;
|
|
while (n != id1) {
|
|
// 相邻两个点的xy都不相等，添加一个辅助顶点构成矩形
|
|
if (m_all[n]->data.x() != m_all[prev[n]]->data.x() &&
|
|
m_all[n]->data.y() != m_all[prev[n]]->data.y()) {
|
|
QPointF tp;
|
|
if (m_all[n]->type == Node::Type::space) {
|
|
ParkingSpaceWidget* space = pk->getSpaceList().at(m_all[n]->number - 1);
|
|
switch (space->getDir()) {
|
|
case ParkingSpaceWidget::direction::E:
|
|
case ParkingSpaceWidget::direction::W:
|
|
tp.setX(m_all[prev[n]]->data.x());
|
|
tp.setY(m_all[n]->data.y());
|
|
break;
|
|
case ParkingSpaceWidget::direction::N:
|
|
case ParkingSpaceWidget::direction::S:
|
|
tp.setX(m_all[n]->data.x());
|
|
tp.setY(m_all[prev[n]]->data.y());
|
|
break;
|
|
}
|
|
} else if (m_all[n]->type == Node::Type::road) {
|
|
Road* road = pk->getRoadList().at(m_all[n]->number - 1);
|
|
switch(road->getDir()) {
|
|
case Road::direction::horizontal:
|
|
tp.setX(m_all[prev[n]]->data.x());
|
|
tp.setY(m_all[n]->data.y());
|
|
break;
|
|
case Road::direction::vertical:
|
|
tp.setX(m_all[n]->data.x());
|
|
tp.setY(m_all[prev[n]]->data.y());
|
|
break;
|
|
}
|
|
}
|
|
if (lastP.isNull())
|
|
lastP = tp;
|
|
|
|
qreal ang1 = angle(lastP - m_all[n]->data);
|
|
qreal ang2 = angle(m_all[n]->data - tp);
|
|
|
|
path->addPoint(0, PathPoint(QPointF(m_all[n]->data), ang1, m_all[n]->action, m_all[n]->getId())); // 添加后一个点
|
|
path->addPoint(0, PathPoint(QPointF(tp), ang2)); // 添加辅助点
|
|
lastP = tp;
|
|
} else {
|
|
qreal ang = angle(m_all[n]->data - m_all[prev[n]]->data);
|
|
path->addPoint(0, PathPoint(QPointF(m_all[n]->data), ang, m_all[n]->action, m_all[n]->getId()));
|
|
lastP = QPointF(m_all[n]->data);
|
|
}
|
|
lastN = n;
|
|
n = prev[n];
|
|
}
|
|
if (!lastP.isNull())
|
|
path->addPoint(0, PathPoint(QPointF(m_all[n]->data),
|
|
angle(lastP - m_all[n]->data), m_all[n]->action, m_all[n]->getId())); // 把第一个点也加进去
|
|
else
|
|
path->addPoint(0, PathPoint(QPointF(m_all[n]->data),
|
|
angle(m_all[n]->data - m_all[lastN]->data), m_all[n]->action, m_all[n]->getId()));
|
|
path->regularize();
|
|
return path;
|
|
}
|
|
|
|
Path *ParkingLotGraph::findPath(Car *car, int entry)
|
|
{
|
|
Path* path = new Path();
|
|
int entry_id = getNodeId(Node::Type::entry, entry);
|
|
qreal ang = angle(QPointF(m_all[entry_id]->data) - car->getmPos());
|
|
path->addPoint(PathPoint(car->getmPos() + QPoint(20, 30), ang));
|
|
path->addPoint(PathPoint(m_all[entry_id]->data, ang, Road::Action::exit));
|
|
return path;
|
|
}
|
|
|
|
ParkingLotGraph::~ParkingLotGraph()
|
|
{
|
|
for (Node* n: m_all)
|
|
delete n;
|
|
delete m_pixmap;
|
|
}
|
|
|
|
QPixmap *ParkingLotGraph::getPixmap()
|
|
{
|
|
if (m_pixmap == nullptr)
|
|
generatePixmap();
|
|
return m_pixmap;
|
|
}
|
|
|
|
void ParkingLotGraph::generatePixmap()
|
|
{
|
|
m_pixmap = new QPixmap(pk->size());
|
|
m_pixmap->fill(Qt::transparent);
|
|
QPainter painter(m_pixmap);
|
|
QVector<Node*> vis(m_spaceList.size() + m_roadNodeList.size());
|
|
QStack<Node*> stack;
|
|
painter.setPen(Qt::red);
|
|
// 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());
|
|
if (n->action != Road::Action::none) {
|
|
painter.setBrush(Qt::red);
|
|
painter.drawEllipse(n->data.x(), n->data.y(), 5, 5);
|
|
painter.setBrush(Qt::NoBrush);
|
|
} else
|
|
painter.drawEllipse(n->data.x(), n->data.y(), 5, 5);
|
|
for(Node* adj: n->adjacent) {
|
|
if (vis.indexOf(adj) != -1)
|
|
continue;
|
|
stack.push(adj);
|
|
painter.drawLine(n->data.x(), n->data.y(), adj->data.x(), adj->data.y());
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
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);
|
|
}
|
|
|
|
Road::Action ParkingLotGraph::Node::getAction() const
|
|
{
|
|
return action;
|
|
}
|
|
|
|
int ParkingLotGraph::Node::getId() const
|
|
{
|
|
return id;
|
|
}
|