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 
77  scene_ (_scene),
78  counter_ (0),
79  ll_(0)
80 {
81 }
82 
83 //------------------------------------------------------------------------------
84 
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 }
const QList< SceneElement * > & elements() const
Scene elements.
ElementArea * elementArea() const
Element area.
QList< Connection * > connections() const
Connections.
Definition: elementInOut.hh:98
~WayFind()
Destructor.
Definition: wayfind.cc:86
ElementOutput * output()
Output of this connection.
Definition: connection.cc:248
WayFind(GraphicsScene *_scene)
Constructor.
Definition: wayfind.cc:76
QVector< ElementInput * > inputs()
Inputs.
Definition: sceneElement.hh:91
const QPolygonF & way() const
way of the connection
Definition: connection.cc:257
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
ElementInput * dataIn()
Scene input.