Developer Documentation

wayfind.cc
1 /*===========================================================================*\
2 * *
3 * OpenFlipper *
4 * Copyright (c) 2001-2015, RWTH-Aachen University *
5 * Department of Computer Graphics and Multimedia *
6 * All rights reserved. *
7 * www.openflipper.org *
8 * *
9 *---------------------------------------------------------------------------*
10 * This file is part of OpenFlipper. *
11 *---------------------------------------------------------------------------*
12 * *
13 * Redistribution and use in source and binary forms, with or without *
14 * modification, are permitted provided that the following conditions *
15 * are met: *
16 * *
17 * 1. Redistributions of source code must retain the above copyright notice, *
18 * this list of conditions and the following disclaimer. *
19 * *
20 * 2. Redistributions in binary form must reproduce the above copyright *
21 * notice, this list of conditions and the following disclaimer in the *
22 * documentation and/or other materials provided with the distribution. *
23 * *
24 * 3. Neither the name of the copyright holder nor the names of its *
25 * contributors may be used to endorse or promote products derived from *
26 * this software without specific prior written permission. *
27 * *
28 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS *
29 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED *
30 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A *
31 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER *
32 * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, *
33 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, *
34 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR *
35 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF *
36 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING *
37 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS *
38 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *
39 * *
40 \*===========================================================================*/
41
42
43
44 //== INCLUDES =================================================================
45 #include <QRectF>
46 #include <QVector>
47 #include <QHash>
48 #include <QLinkedList>
49 #include <QTime>
50
51 #include <iostream>
52
53 #include <cmath>
54
55 #include "wayfind.hh"
56
57 #include "graphicsScene.hh"
58 #include "elementArea.hh"
59 #include "sceneElement.hh"
60 #include "connection.hh"
61 #include "elementInput.hh"
62 #include "elementOutput.hh"
63
64 #define PAIR(p) QPair<int,int>((p).x (),(p).y ())
65
66 //== NAMESPACES ===============================================================
67 namespace VSI {
68
69 //=============================================================================
70 //
71 // CLASS VSI::WayFind - IMPLEMENTATION
72 //
73 //=============================================================================
74
76 WayFind::WayFind(GraphicsScene *_scene) :
77 scene_ (_scene),
78 counter_ (0),
79 ll_(0)
80 {
81 }
82
83 //------------------------------------------------------------------------------
84
86 WayFind::~WayFind()
87 {
88 foreach (Node *n, nodes_)
89 delete n;
90 }
91
92 //------------------------------------------------------------------------------
93
95 QPolygonF WayFind::findWay(Connection *_conn, QPoint _from, QPoint _to)
96 {
97 // our maximum working area
98 QRect bb = scene_->elementArea ()->childrenBoundingRect ().toRect ().adjusted (-100, -100, 100, 100);
99
100 QRegion eR;
101
102 // combine all scene element bounding boxes and connections into one region
103 foreach (SceneElement *e, scene_->elements())
104 {
105 eR += e->mapRectToParent (e->boundingRect ()).toRect ().adjusted (-15,-15,15,15);
106 foreach (ElementInput *i, e->inputs ())
107 foreach (Connection *c, i->connections ())
108 {
109 // ignore connection from the same output
110 if (c->output () == _conn->output ())
111 continue;
112 if (c->way ().isEmpty ())
113 continue;
114 QPoint p1 = c->way ().first ().toPoint ();
115 QPoint p2;
116 for (int i = 1; i < c->way ().size (); i++)
117 {
118 p2 = c->way ()[i].toPoint ();
119 eR += QRect (qMin (p1.x (), p2.x ()), qMin (p1.y (), p2.y ()), abs (p1.x () - p2.x ()), abs (p1.y () - p2.y ())).adjusted (-2, -2, 2, 2);
120 p1 = p2;
121 }
122 }
123 if (e->dataIn ())
124 {
125 foreach (Connection *c, e->dataIn ()->connections ())
126 {
127 if (c->way ().isEmpty ())
128 continue;
129 QPoint p1 = c->way ().first ().toPoint ();
130 QPoint p2;
131 for (int i = 1; i < c->way ().size (); i++)
132 {
133 p2 = c->way ()[i].toPoint ();
134 eR += QRect (qMin (p1.x (), p2.x ()), qMin (p1.y (), p2.y ()), abs (p1.x () - p2.x ()), abs (p1.y () - p2.y ())).adjusted (-4, -4, 4, 4);
135 p1 = p2;
136 }
137 }
138 }
139 }
140
141 // 5px wide grid
142 int step = 5;
143
144 // round to grid size
145 int x1 = (_from.x () / step) * step;
146 int y1 = (_from.y () / step) * step;
147 int x2 = (_to.x () / step) * step;
148 int y2 = (_to.y () / step) * step;
149
150 QPoint newTo = QPoint (x2, y2);
151
152 // make sure that start and end get different nodes
153 if (_from.x () != _to.x () && x1 == x2)
154 {
155 if (_from.x () > _to.x ())
156 x1 += step;
157 else
158 x2 += step;
159 }
160
161 if (_from.y () != _to.y () && y1 == y2)
162 {
163 if (_from.y () > _to.y ())
164 y1 += step;
165 else
166 y2 += step;
167 }
168
169 bool found = false;
170 Node *n = NULL, *tmp, *tmp2, *lastIn = 0;
171
172 // reuse old calculation if nothing changed
173 if (_from == oldFrom_ && eR == oldReg_)
174 {
175 if (map_.contains (PAIR(newTo)))
176 {
177 n = map_[PAIR(newTo)];
178 if (n->closed_ && n->counter_ == counter_)
179 found = true;
180 }
181 }
182
183 if (!found)
184 {
185 // increase usage counter
186 counter_++;
187
188 // create start node if not avaiable
189 if (!map_.contains (QPair<int,int>(x1, y1)))
190 {
191 ll_ = new Node (counter_);
192 nodes_.append (ll_);
193
194 ll_->pos_ = QPoint (x1, y1);
195 map_.insert (QPair<int,int>(x1, y1), ll_);
196
197 for (unsigned int k = 0; k < 4; k++)
198 {
199 QPoint p2 = validPos (k, step, ll_->pos_);
200
201 if (map_.contains (PAIR(p2)))
202 {
203 ll_->n_[k] = map_[PAIR(p2)];
204 map_[PAIR(p2)]->n_[(k + 2) & 3] = ll_;
205 }
206 }
207 }
208 else
209 {
210 ll_ = map_[QPair<int,int>(x1, y1)];
211 ll_->counter_ = counter_;
212 ll_->prev_ = 0;
213 ll_->next_ = 0;
214 ll_->from_ = 0;
215 ll_->f_ = 100000000;
216 ll_->closed_ = false;
217 ll_->cost_ = 5;
218 }
219
220 ll_->g_ = 0;
221 ll_->h_ = heuristicDistance (_from, _to);
222 ll_->type_ = Node::Horizontal;
223
224 }
225
226 while (ll_ && !found)
227 {
228 // take first node of the list
229 n = ll_;
230 ll_ = n->next_;
231 if (ll_)
232 ll_->prev_ = 0;
233
234 n->closed_ = true;
235
236 // stop if end node is found
237 if (n->pos_.y () == y2 && n->pos_.x () == x2)
238 {
239 found = true;
240 break;
241 }
242
243 // Add neighbor nodes if not yet present or reset old ones if not yet used during this round
244 for (unsigned int i = 0; i < 4; i++)
245 if (!n->n_[i])
246 {
247 QPoint p = validPos (i, step, n->pos_);
248 if (bb.contains (p))
249 {
250 Node *nn = new Node (counter_);
251 nodes_.append (nn);
252
253 n->n_[i] = nn;
254
255 nn->n_[(i + 2) & 3] = n;
256 nn->pos_ = p;
257 nn->h_ = heuristicDistance (p, newTo);
258
259 if (eR.contains (p))
260 nn->cost_ = 50;
261
262 map_.insert (PAIR(nn->pos_), nn);
263
264 for (unsigned int j = 0; j < 3; j++)
265 {
266 unsigned int k = (i - 1 + j) & 3;
267 QPoint p2 = validPos (k, step, p);
268
269 if (map_.contains (PAIR(p2)))
270 {
271 nn->n_[k] = map_[PAIR(p2)];
272 map_[PAIR(p2)]->n_[(k + 2) & 3] = nn;
273 }
274 }
275 }
276 }
277 else if (n->n_[i]->counter_ != counter_)
278 {
279 tmp = n->n_[i];
280 tmp->counter_ = counter_;
281 tmp->prev_ = 0;
282 tmp->next_ = 0;
283 tmp->from_ = 0;
284 tmp->g_ = 100000000;
285 tmp->f_ = 100000000;
286 tmp->h_ = 100000000;
287 tmp->closed_ = false;
288 if (eR.contains (tmp->pos_))
289 tmp->cost_ = 50;
290 else
291 tmp->cost_ = 5;
292 tmp->h_ = heuristicDistance (tmp->pos_, newTo);
293 }
294
295 // update nodes in all directions
296 for (unsigned int i = 0; i < 4; i++)
297 {
298 if (!n->n_[i])
299 continue;
300
301 if (n->n_[i]->closed_)
302 continue;
303
304 tmp = n->n_[i];
305
306 unsigned int g = n->g_;
307
308 // direction change?
309 if ((n->type_ == Node::Horizontal && (i & 1)) || (n->type_ == Node::Vertical && (i & 1) == 0))
310 g += n->cost_;
311 else
312 g += n->cost_ * 2;
313
314 if (g < tmp->g_)
315 {
316 tmp->from_ = n;
317 tmp->type_ = (i & 1)? Node::Horizontal : Node::Vertical;
318
319 tmp->g_ = g;
320 tmp->f_ = g + tmp->h_;
321
322 // remove node from list
323 if (tmp->prev_)
324 tmp->prev_->next_ = tmp->next_;
325 if (tmp->next_)
326 tmp->next_->prev_ = tmp->prev_;
327 if (tmp == ll_)
328 ll_ = tmp->next_;
329
330 // insert node at right position in sorted list
331 if (!ll_ || tmp->f_ <= ll_->f_)
332 {
333 tmp->next_ = ll_;
334 if (ll_)
335 ll_->prev_ = tmp;
336 ll_ = tmp;
337 lastIn = tmp;
338 }
339 else
340 {
341 if (lastIn && lastIn->f_ <= tmp->f_ && lastIn != tmp)
342 tmp2 = lastIn;
343 else
344 tmp2 = ll_;
345 while (tmp2->next_ && tmp2->next_->f_ < tmp->f_)
346 tmp2 = tmp2->next_;
347 tmp->next_ = tmp2->next_;
348 if (tmp->next_)
349 tmp->next_->prev_ = tmp;
350 tmp2->next_ = tmp;
351 tmp->prev_ = tmp2;
352 lastIn = tmp2;
353 }
354 }
355 }
356 }
357
358 QPolygonF rv;
359
360 // convert found way into polygon
361 if (found)
362 {
363 bool dir = true;
364 if (n->from_)
365 if (n->pos_.y () != n->from_->pos_.y ())
366 dir = false;
367
368 int lastX = n->pos_.x ();
369 int lastY = n->pos_.y ();
370
371 QPoint last = _to;
372
373 while (n)
374 {
375 if (dir && n->pos_.y () != lastY)
376 {
377 dir = false;
378 lastX = n->pos_.x ();
379 lastY = n->pos_.y ();
380 rv.append (last);
381 last = QPoint (n->pos_.x (), last.y ());
382 }
383 else if (!dir && n->pos_.x () != lastX)
384 {
385 dir = true;
386 lastX = n->pos_.x ();
387 lastY = n->pos_.y ();
388 rv.append (last);
389 last = QPoint (last.x (), n->pos_.y ());
390 }
391
392 n = n->from_;
393 }
394
395 if (dir)
396 last.setY (_from.y ());
397 else
398 last.setX (_from.x ());
399
400 rv.append(QPointF (last));
401 rv.append(QPointF (_from));
402 }
403 else
404 {
405 rv.append(QPointF (_from));
406 rv.append(QPointF (_to));
407 std::cerr << "Not Found" << std::endl;
408 }
409
410 oldFrom_ = _from;
411 oldReg_ = eR;
412
413 // free unused nodes
414 cleanup ();
415
416 return rv;
417 }
418
419 //------------------------------------------------------------------------------
420
421 // Node constructor
422 WayFind::Node::Node(unsigned int _counter) :
423 counter_ (_counter),
424 type_ (Horizontal),
425 prev_ (0),
426 next_ (0),
427 from_ (0),
428 g_ (100000000),
429 f_ (100000000),
430 h_ (100000000),
431 cost_ (5),
432 closed_ (false)
433 {
434 n_[0] = 0;
435 n_[1] = 0;
436 n_[2] = 0;
437 n_[3] = 0;
438 }
439
440 //------------------------------------------------------------------------------
441
442 // Node destructor
443 WayFind::Node::~ Node()
444 {
445 }
446
447 //------------------------------------------------------------------------------
448
449 // Next position in distance _step from _pnt in direction _dir
450 QPoint WayFind::validPos(unsigned int _dir, int _step, QPoint _pnt)
451 {
452
453 QPoint rv = _pnt;
454 if (_dir == 0 || _dir == 3)
455 _step = -_step;
456
457 if (_dir == 1 || _dir == 3)
458 rv += QPoint (_step, 0);
459 else
460 rv += QPoint (0, _step);
461
462 return rv;
463 }
464
465 //------------------------------------------------------------------------------
466
467 // Heuristic distance from _from to _to
468 int WayFind::heuristicDistance(const QPoint &_from, const QPoint &_to) const
469 {
470 QPoint p = _from - _to;
471 return abs (p.x ()) + abs (p.y ());
472 }
473
474 //------------------------------------------------------------------------------
475
476 // cleanup ununsed nodes
477 void VSI::WayFind::cleanup()
478 {
479 // only every 128 runs
480 if ((counter_ & 0x7f) != 0)
481 return;
482
483 QLinkedList<Node *>::iterator it = nodes_.begin();
484
485 while (it != nodes_.end ())
486 {
487 Node* n = *it;
488 // nodes that hasn't be used in the last 64 rounds
489 if (counter_ - n->counter_ > 64)
490 {
491 for (unsigned int i = 0; i < 4; i++)
492 if (n->n_[i])
493 n->n_[i]->n_[(i+2)&3] = NULL;
494 it = nodes_.erase(it);
495 map_.remove (PAIR(n->pos_));
496 delete n;
497 }
498 else
499 ++it;
500 }
501 }
502
503 //------------------------------------------------------------------------------
504 }
VSI::WayFind::findWay
QPolygonF findWay(Connection *_conn, QPoint _from, QPoint _to)
Finds a way from _from to _to ignoring any already existent connections from _conn.
Definition: wayfind.cc:95
VSI::GraphicsScene::elements
const QList< SceneElement * > & elements() const
Scene elements.
Definition: graphicsScene.cc:348
VSI::GraphicsScene
Definition: graphicsScene.hh:80
VSI::WayFind::Node
Definition: wayfind.hh:79
VSI::ElementInput
Definition: elementInput.hh:67
VSI::WayFind::~WayFind
~WayFind()
Destructor.
Definition: wayfind.cc:86
VSI::SceneElement::dataIn
ElementInput * dataIn()
Scene input.
Definition: sceneElement.hh:100
VSI::GraphicsScene::elementArea
ElementArea * elementArea() const
Element area.
Definition: graphicsScene.hh:123
VSI::Connection
Definition: connection.hh:66
VSI::Connection::way
const QPolygonF & way() const
way of the connection
Definition: connection.cc:257
VSI::Connection::output
ElementOutput * output()
Output of this connection.
Definition: connection.cc:248
VSI
Definition: baseWidget.cc:72
VSI::WayFind::WayFind
WayFind(GraphicsScene *_scene)
Constructor.
Definition: wayfind.cc:76
VSI::SceneElement
Definition: sceneElement.hh:73
VSI::SceneElement::inputs
QVector< ElementInput * > inputs()
Inputs.
Definition: sceneElement.hh:91
VSI::ElementInOut::connections
QList< Connection * > connections() const
Connections.
Definition: elementInOut.hh:98