Developer Documentation
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Modules Pages
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 * $Revision$ *
45 * $LastChangedBy$ *
46 * $Date$ *
47 * *
48 \*===========================================================================*/
49 
50 //== INCLUDES =================================================================
51 #include <QRectF>
52 #include <QVector>
53 #include <QHash>
54 #include <QLinkedList>
55 #include <QTime>
56 
57 #include <iostream>
58 
59 #include <cmath>
60 
61 #include "wayfind.hh"
62 
63 #include "graphicsScene.hh"
64 #include "elementArea.hh"
65 #include "sceneElement.hh"
66 #include "connection.hh"
67 #include "elementInput.hh"
68 #include "elementOutput.hh"
69 
70 #define PAIR(p) QPair<int,int>((p).x (),(p).y ())
71 
72 //== NAMESPACES ===============================================================
73 namespace VSI {
74 
75 //=============================================================================
76 //
77 // CLASS VSI::WayFind - IMPLEMENTATION
78 //
79 //=============================================================================
80 
83  scene_ (_scene),
84  counter_ (0),
85  ll_(0)
86 {
87 }
88 
89 //------------------------------------------------------------------------------
90 
93 {
94  foreach (Node *n, nodes_)
95  delete n;
96 }
97 
98 //------------------------------------------------------------------------------
99 
101 QPolygonF WayFind::findWay(Connection *_conn, QPoint _from, QPoint _to)
102 {
103  // our maximum working area
104  QRect bb = scene_->elementArea ()->childrenBoundingRect ().toRect ().adjusted (-100, -100, 100, 100);
105 
106  QRegion eR;
107 
108  // combine all scene element bounding boxes and connections into one region
109  foreach (SceneElement *e, scene_->elements())
110  {
111  eR += e->mapRectToParent (e->boundingRect ()).toRect ().adjusted (-15,-15,15,15);
112  foreach (ElementInput *i, e->inputs ())
113  foreach (Connection *c, i->connections ())
114  {
115  // ignore connection from the same output
116  if (c->output () == _conn->output ())
117  continue;
118  if (c->way ().isEmpty ())
119  continue;
120  QPoint p1 = c->way ().first ().toPoint ();
121  QPoint p2;
122  for (int i = 1; i < c->way ().size (); i++)
123  {
124  p2 = c->way ()[i].toPoint ();
125  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);
126  p1 = p2;
127  }
128  }
129  if (e->dataIn ())
130  {
131  foreach (Connection *c, e->dataIn ()->connections ())
132  {
133  if (c->way ().isEmpty ())
134  continue;
135  QPoint p1 = c->way ().first ().toPoint ();
136  QPoint p2;
137  for (int i = 1; i < c->way ().size (); i++)
138  {
139  p2 = c->way ()[i].toPoint ();
140  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);
141  p1 = p2;
142  }
143  }
144  }
145  }
146 
147  // 5px wide grid
148  int step = 5;
149 
150  // round to grid size
151  int x1 = (_from.x () / step) * step;
152  int y1 = (_from.y () / step) * step;
153  int x2 = (_to.x () / step) * step;
154  int y2 = (_to.y () / step) * step;
155 
156  QPoint newTo = QPoint (x2, y2);
157 
158  // make sure that start and end get different nodes
159  if (_from.x () != _to.x () && x1 == x2)
160  {
161  if (_from.x () > _to.x ())
162  x1 += step;
163  else
164  x2 += step;
165  }
166 
167  if (_from.y () != _to.y () && y1 == y2)
168  {
169  if (_from.y () > _to.y ())
170  y1 += step;
171  else
172  y2 += step;
173  }
174 
175  bool found = false;
176  Node *n = NULL, *tmp, *tmp2, *lastIn = 0;
177 
178  // reuse old calculation if nothing changed
179  if (_from == oldFrom_ && eR == oldReg_)
180  {
181  if (map_.contains (PAIR(newTo)))
182  {
183  n = map_[PAIR(newTo)];
184  if (n->closed_ && n->counter_ == counter_)
185  found = true;
186  }
187  }
188 
189  if (!found)
190  {
191  // increase usage counter
192  counter_++;
193 
194  // create start node if not avaiable
195  if (!map_.contains (QPair<int,int>(x1, y1)))
196  {
197  ll_ = new Node (counter_);
198  nodes_.append (ll_);
199 
200  ll_->pos_ = QPoint (x1, y1);
201  map_.insert (QPair<int,int>(x1, y1), ll_);
202 
203  for (unsigned int k = 0; k < 4; k++)
204  {
205  QPoint p2 = validPos (k, step, ll_->pos_);
206 
207  if (map_.contains (PAIR(p2)))
208  {
209  ll_->n_[k] = map_[PAIR(p2)];
210  map_[PAIR(p2)]->n_[(k + 2) & 3] = ll_;
211  }
212  }
213  }
214  else
215  {
216  ll_ = map_[QPair<int,int>(x1, y1)];
217  ll_->counter_ = counter_;
218  ll_->prev_ = 0;
219  ll_->next_ = 0;
220  ll_->from_ = 0;
221  ll_->f_ = 100000000;
222  ll_->closed_ = false;
223  ll_->cost_ = 5;
224  }
225 
226  ll_->g_ = 0;
227  ll_->h_ = heuristicDistance (_from, _to);
228  ll_->type_ = Node::Horizontal;
229 
230  }
231 
232  while (ll_ && !found)
233  {
234  // take first node of the list
235  n = ll_;
236  ll_ = n->next_;
237  if (ll_)
238  ll_->prev_ = 0;
239 
240  n->closed_ = true;
241 
242  // stop if end node is found
243  if (n->pos_.y () == y2 && n->pos_.x () == x2)
244  {
245  found = true;
246  break;
247  }
248 
249  // Add neighbor nodes if not yet present or reset old ones if not yet used during this round
250  for (unsigned int i = 0; i < 4; i++)
251  if (!n->n_[i])
252  {
253  QPoint p = validPos (i, step, n->pos_);
254  if (bb.contains (p))
255  {
256  Node *nn = new Node (counter_);
257  nodes_.append (nn);
258 
259  n->n_[i] = nn;
260 
261  nn->n_[(i + 2) & 3] = n;
262  nn->pos_ = p;
263  nn->h_ = heuristicDistance (p, newTo);
264 
265  if (eR.contains (p))
266  nn->cost_ = 50;
267 
268  map_.insert (PAIR(nn->pos_), nn);
269 
270  for (unsigned int j = 0; j < 3; j++)
271  {
272  unsigned int k = (i - 1 + j) & 3;
273  QPoint p2 = validPos (k, step, p);
274 
275  if (map_.contains (PAIR(p2)))
276  {
277  nn->n_[k] = map_[PAIR(p2)];
278  map_[PAIR(p2)]->n_[(k + 2) & 3] = nn;
279  }
280  }
281  }
282  }
283  else if (n->n_[i]->counter_ != counter_)
284  {
285  tmp = n->n_[i];
286  tmp->counter_ = counter_;
287  tmp->prev_ = 0;
288  tmp->next_ = 0;
289  tmp->from_ = 0;
290  tmp->g_ = 100000000;
291  tmp->f_ = 100000000;
292  tmp->h_ = 100000000;
293  tmp->closed_ = false;
294  if (eR.contains (tmp->pos_))
295  tmp->cost_ = 50;
296  else
297  tmp->cost_ = 5;
298  tmp->h_ = heuristicDistance (tmp->pos_, newTo);
299  }
300 
301  // update nodes in all directions
302  for (unsigned int i = 0; i < 4; i++)
303  {
304  if (!n->n_[i])
305  continue;
306 
307  if (n->n_[i]->closed_)
308  continue;
309 
310  tmp = n->n_[i];
311 
312  unsigned int g = n->g_;
313 
314  // direction change?
315  if ((n->type_ == Node::Horizontal && (i & 1)) || (n->type_ == Node::Vertical && (i & 1) == 0))
316  g += n->cost_;
317  else
318  g += n->cost_ * 2;
319 
320  if (g < tmp->g_)
321  {
322  tmp->from_ = n;
323  tmp->type_ = (i & 1)? Node::Horizontal : Node::Vertical;
324 
325  tmp->g_ = g;
326  tmp->f_ = g + tmp->h_;
327 
328  // remove node from list
329  if (tmp->prev_)
330  tmp->prev_->next_ = tmp->next_;
331  if (tmp->next_)
332  tmp->next_->prev_ = tmp->prev_;
333  if (tmp == ll_)
334  ll_ = tmp->next_;
335 
336  // insert node at right position in sorted list
337  if (!ll_ || tmp->f_ <= ll_->f_)
338  {
339  tmp->next_ = ll_;
340  if (ll_)
341  ll_->prev_ = tmp;
342  ll_ = tmp;
343  lastIn = tmp;
344  }
345  else
346  {
347  if (lastIn && lastIn->f_ <= tmp->f_ && lastIn != tmp)
348  tmp2 = lastIn;
349  else
350  tmp2 = ll_;
351  while (tmp2->next_ && tmp2->next_->f_ < tmp->f_)
352  tmp2 = tmp2->next_;
353  tmp->next_ = tmp2->next_;
354  if (tmp->next_)
355  tmp->next_->prev_ = tmp;
356  tmp2->next_ = tmp;
357  tmp->prev_ = tmp2;
358  lastIn = tmp2;
359  }
360  }
361  }
362  }
363 
364  QPolygonF rv;
365 
366  // convert found way into polygon
367  if (found)
368  {
369  bool dir = true;
370  if (n->from_)
371  if (n->pos_.y () != n->from_->pos_.y ())
372  dir = false;
373 
374  int lastX = n->pos_.x ();
375  int lastY = n->pos_.y ();
376 
377  QPoint last = _to;
378 
379  while (n)
380  {
381  if (dir && n->pos_.y () != lastY)
382  {
383  dir = false;
384  lastX = n->pos_.x ();
385  lastY = n->pos_.y ();
386  rv.append (last);
387  last = QPoint (n->pos_.x (), last.y ());
388  }
389  else if (!dir && n->pos_.x () != lastX)
390  {
391  dir = true;
392  lastX = n->pos_.x ();
393  lastY = n->pos_.y ();
394  rv.append (last);
395  last = QPoint (last.x (), n->pos_.y ());
396  }
397 
398  n = n->from_;
399  }
400 
401  if (dir)
402  last.setY (_from.y ());
403  else
404  last.setX (_from.x ());
405 
406  rv.append(QPointF (last));
407  rv.append(QPointF (_from));
408  }
409  else
410  {
411  rv.append(QPointF (_from));
412  rv.append(QPointF (_to));
413  std::cerr << "Not Found" << std::endl;
414  }
415 
416  oldFrom_ = _from;
417  oldReg_ = eR;
418 
419  // free unused nodes
420  cleanup ();
421 
422  return rv;
423 }
424 
425 //------------------------------------------------------------------------------
426 
427 // Node constructor
428 WayFind::Node::Node(unsigned int _counter) :
429  counter_ (_counter),
430  type_ (Horizontal),
431  prev_ (0),
432  next_ (0),
433  from_ (0),
434  g_ (100000000),
435  f_ (100000000),
436  h_ (100000000),
437  cost_ (5),
438  closed_ (false)
439 {
440  n_[0] = 0;
441  n_[1] = 0;
442  n_[2] = 0;
443  n_[3] = 0;
444 }
445 
446 //------------------------------------------------------------------------------
447 
448 // Node destructor
449 WayFind::Node::~ Node()
450 {
451 }
452 
453 //------------------------------------------------------------------------------
454 
455 // Next position in distance _step from _pnt in direction _dir
456 QPoint WayFind::validPos(unsigned int _dir, int _step, QPoint _pnt)
457 {
458 
459  QPoint rv = _pnt;
460  if (_dir == 0 || _dir == 3)
461  _step = -_step;
462 
463  if (_dir == 1 || _dir == 3)
464  rv += QPoint (_step, 0);
465  else
466  rv += QPoint (0, _step);
467 
468  return rv;
469 }
470 
471 //------------------------------------------------------------------------------
472 
473 // Heuristic distance from _from to _to
474 int WayFind::heuristicDistance(const QPoint &_from, const QPoint &_to) const
475 {
476  QPoint p = _from - _to;
477  return abs (p.x ()) + abs (p.y ());
478 }
479 
480 //------------------------------------------------------------------------------
481 
482 // cleanup ununsed nodes
483 void VSI::WayFind::cleanup()
484 {
485  // only every 128 runs
486  if ((counter_ & 0x7f) != 0)
487  return;
488 
489  QLinkedList<Node *>::iterator it = nodes_.begin();
490 
491  while (it != nodes_.end ())
492  {
493  Node* n = *it;
494  // nodes that hasn't be used in the last 64 rounds
495  if (counter_ - n->counter_ > 64)
496  {
497  for (unsigned int i = 0; i < 4; i++)
498  if (n->n_[i])
499  n->n_[i]->n_[(i+2)&3] = NULL;
500  it = nodes_.erase(it);
501  map_.remove (PAIR(n->pos_));
502  delete n;
503  }
504  else
505  ++it;
506  }
507 }
508 
509 //------------------------------------------------------------------------------
510 }
~WayFind()
Destructor.
Definition: wayfind.cc:92
QVector< ElementInput * > inputs()
Inputs.
Definition: sceneElement.hh:97
const QList< SceneElement * > & elements() const
Scene elements.
WayFind(GraphicsScene *_scene)
Constructor.
Definition: wayfind.cc:82
QList< Connection * > connections() const
Connections.
ElementInput * dataIn()
Scene input.
const QPolygonF & way() const
way of the connection
Definition: connection.cc:263
ElementOutput * output()
Output of this connection.
Definition: connection.cc:254
ElementArea * elementArea() const
Element area.
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:101