1 package cz.cuni.amis.pogamut.ut2004.agent.navigation.floydwarshall;
2
3 import java.util.ArrayList;
4 import java.util.Collection;
5 import java.util.HashMap;
6 import java.util.List;
7 import java.util.Map;
8 import java.util.logging.Level;
9 import java.util.logging.Logger;
10
11 import javax.vecmath.Point3d;
12
13 import cz.cuni.amis.pogamut.base.agent.IGhostAgent;
14 import cz.cuni.amis.pogamut.base.agent.module.SensorModule;
15 import cz.cuni.amis.pogamut.base.agent.navigation.IPathFuture;
16 import cz.cuni.amis.pogamut.base.agent.navigation.IPathPlanner;
17 import cz.cuni.amis.pogamut.base.agent.navigation.impl.PrecomputedPathFuture;
18 import cz.cuni.amis.pogamut.base.communication.worldview.event.IWorldEventListener;
19 import cz.cuni.amis.pogamut.unreal.communication.messages.UnrealId;
20 import cz.cuni.amis.pogamut.ut2004.agent.module.sensor.NavigationGraphBuilder;
21 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.Item;
22 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.NavPoint;
23 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.NavPointNeighbourLink;
24 import cz.cuni.amis.pogamut.ut2004.communication.translator.shared.events.MapPointListObtained;
25 import cz.cuni.amis.pogamut.ut2004.utils.LinkFlag;
26 import cz.cuni.amis.utils.IFilter;
27 import cz.cuni.amis.utils.collections.MyCollections;
28 import cz.cuni.amis.utils.exception.PogamutException;
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54 public class FloydWarshallMap extends SensorModule<IGhostAgent> implements IPathPlanner<NavPoint> {
55
56 public static class PathMatrixNode {
57
58 private float distance = Float.POSITIVE_INFINITY;
59 private Integer viaNode = null;
60 private List<NavPoint> path = null;
61
62
63
64
65 public PathMatrixNode() {
66 }
67
68 public PathMatrixNode(float distance) {
69 this.distance = distance;
70 }
71
72 public float getDistance() {
73 return distance;
74 }
75
76 public void setDistance(float distance) {
77 this.distance = distance;
78 }
79
80
81
82
83
84
85 public Integer getViaNode() {
86 return viaNode;
87 }
88
89 public void setViaNode(Integer indice) {
90 this.viaNode = indice;
91 }
92
93 public List<NavPoint> getPath() {
94 return path;
95 }
96
97 public void setPath(List<NavPoint> path) {
98 this.path = path;
99 }
100
101 }
102
103 private IWorldEventListener<MapPointListObtained> mapListener = new IWorldEventListener<MapPointListObtained>() {
104
105 @Override
106 public void notify(MapPointListObtained event) {
107 if (log.isLoggable(Level.INFO)) log.info("Map point list obtained.");
108 performFloydWarshall(event);
109 }
110 };
111
112
113
114
115 public static final int BAD_EDGE_FLAG = LinkFlag.FLY.get()
116 | LinkFlag.LADDER.get() | LinkFlag.PROSCRIBED.get()
117 | LinkFlag.SWIM.get() | LinkFlag.PLAYERONLY.get();
118
119 public static boolean isWalkable(int flag) {
120 return ((flag & BAD_EDGE_FLAG) == 0) && ((flag & LinkFlag.SPECIAL.get()) == 0);
121 }
122
123
124
125
126 protected int badEdgeFlag = 0;
127
128
129
130
131 protected Map<UnrealId, Integer> navPointIndices;
132
133
134
135
136
137
138
139
140 protected Map<Integer, NavPoint> indicesNavPoints;
141
142
143 protected PathMatrixNode[][] pathMatrix;
144
145
146
147
148
149
150 private boolean enabled = true;
151
152
153
154
155 protected Object mutex = new Object();
156
157 public FloydWarshallMap(IGhostAgent bot) {
158 this(bot, BAD_EDGE_FLAG, null);
159 }
160
161 public FloydWarshallMap(IGhostAgent bot, Logger log) {
162 this(bot, BAD_EDGE_FLAG, log);
163 }
164
165 public FloydWarshallMap(IGhostAgent bot, int badEdgeFlag, Logger log) {
166 super(bot, log);
167 this.badEdgeFlag = badEdgeFlag;
168 worldView.addEventListener(MapPointListObtained.class, mapListener);
169 }
170
171
172
173
174
175
176
177
178 public boolean isEnabled() {
179 return enabled;
180 }
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195 public void setEnabled(boolean enabled) {
196 synchronized(mutex) {
197 this.enabled = enabled;
198 if (!enabled) {
199 cleanUp();
200 }
201 }
202 }
203
204
205
206
207
208 public void refreshPathMatrix() {
209 synchronized(mutex) {
210 if (!enabled) {
211 if (log.isLoggable(Level.WARNING)) log.fine(this + ": Won't refresh path matrix, object is disabled.");
212 return;
213 }
214 if (log.isLoggable(Level.FINE)) log.fine(this + ": Refreshing path matrix...");
215 List<NavPoint> navPoints = MyCollections.asList(agent.getWorldView().getAll(NavPoint.class).values());
216 performFloydWarshall(navPoints);
217 if (log.isLoggable(Level.WARNING)) log.warning(this + ": Path matrix refreshed!");
218 }
219 }
220
221 protected void performFloydWarshall(MapPointListObtained map) {
222 List<NavPoint> navPoints = MyCollections.asList(map.getNavPoints().values());
223 performFloydWarshall(navPoints);
224 }
225
226 protected void performFloydWarshall(List<NavPoint> navPoints) {
227 if (!enabled) {
228 if (log.isLoggable(Level.WARNING)) log.fine(this + ": Should not be running Floyd-Warshall, object disabled.");
229 return;
230 }
231 if (log.isLoggable(Level.FINE)) log.fine(this + ": Beginning Floyd-Warshall algorithm...");
232 long start = System.currentTimeMillis();
233
234
235 int size = navPoints.size();
236 navPointIndices = new HashMap<UnrealId, Integer>(size);
237 indicesNavPoints = new HashMap<Integer, NavPoint>(size);
238 pathMatrix = new PathMatrixNode[size][size];
239
240
241 for (int i = 0; i < navPoints.size(); ++i) {
242 navPointIndices.put(navPoints.get(i).getId(), i);
243 indicesNavPoints.put(i, navPoints.get(i));
244 }
245
246
247 for (int i = 0; i < size; i++) {
248 for (int j = 0; j < size; j++) {
249 pathMatrix[i][j] = new PathMatrixNode((i == j) ? 0.0f
250 : Float.POSITIVE_INFINITY);
251 }
252 }
253
254
255 for (int i = 0; i < size; i++) {
256 Point3d location = navPoints.get(i).getLocation().getPoint3d();
257 for (NavPointNeighbourLink link : navPoints.get(i)
258 .getOutgoingEdges().values()) {
259 if (checkLink(link)) {
260 pathMatrix[i][navPointIndices.get(link.getToNavPoint()
261 .getId())].setDistance((float) location
262 .distance(link.getToNavPoint().getLocation()
263 .getPoint3d()));
264 }
265 }
266 }
267
268
269 for (int k = 0; k < size; k++) {
270 for (int i = 0; i < size; i++) {
271 for (int j = 0; j < size; j++) {
272 float newLen = pathMatrix[i][k].getDistance()
273 + pathMatrix[k][j].getDistance();
274 if (pathMatrix[i][j].getDistance() > newLen) {
275 pathMatrix[i][j].setDistance(newLen);
276 pathMatrix[i][j].setViaNode(k);
277 }
278 }
279 }
280 }
281
282
283 int count = 0;
284 for (int i = 0; i < size; i++) {
285 for (int j = 0; j < size; j++) {
286 if (pathMatrix[i][j].getDistance() == Float.POSITIVE_INFINITY) {
287 if (log.isLoggable(Level.FINER)) log.finer("Unreachable path from " + navPoints.get(i).getId().getStringId()
288 + " -> " + navPoints.get(j).getId().getStringId());
289 count++;
290 } else {
291
292 pathMatrix[i][j].setPath(retrievePath(i, j));
293 }
294 }
295 }
296
297 if (count > 0) {
298 if (log.isLoggable(Level.WARNING)) log.warning(this + ": There are " + count + " unreachable nav point pairs (if you wish to see more, set logging to Level.FINER).");
299 }
300
301 if (log.isLoggable(Level.INFO)) log.info(this + ": computation for " + size + " navpoints took " + (System.currentTimeMillis() - start) + " millis");
302
303
304 indicesNavPoints = null;
305 }
306
307
308
309
310
311
312
313
314
315
316 public boolean checkLink(NavPointNeighbourLink edge) {
317
318 if ((edge.getFlags() & badEdgeFlag) != 0)
319 return false;
320
321
322 if ((edge.getFlags() & LinkFlag.SPECIAL.get()) != 0)
323 return true;
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345 if (edge.getNeededJump() != null && edge.getNeededJump().z > 680) {
346 return false;
347 }
348
349
350 if (edge.getToNavPoint().isLiftJumpExit()) {
351 return false;
352 }
353
354 return true;
355 }
356
357
358
359
360
361
362
363
364
365
366
367
368 private List<NavPoint> retrievePathInner(Integer from, Integer to) {
369 PathMatrixNode node = pathMatrix[from][to];
370 if (node.getDistance() == Float.POSITIVE_INFINITY)
371 return null;
372 if (node.getViaNode() == null) {
373 return new ArrayList<NavPoint>(0);
374 }
375 if (node.getViaNode() == null)
376 return new ArrayList<NavPoint>(0);
377
378 List<NavPoint> path = new ArrayList<NavPoint>();
379 path.addAll(retrievePathInner(from, node.getViaNode()));
380 path.add(indicesNavPoints.get(node.getViaNode()));
381 path.addAll(retrievePathInner(node.getViaNode(), to));
382
383 return path;
384 }
385
386
387
388
389
390
391
392
393
394
395
396
397 private List<NavPoint> retrievePath(Integer from, Integer to) {
398 List<NavPoint> path = new ArrayList<NavPoint>();
399 path.add(indicesNavPoints.get(from));
400 path.addAll(retrievePathInner(from, to));
401 path.add(indicesNavPoints.get(to));
402 return path;
403 }
404
405 protected PathMatrixNode getPathMatrixNode(NavPoint np1, NavPoint np2) {
406 return pathMatrix[navPointIndices.get(np1.getId())][navPointIndices
407 .get(np2.getId())];
408 }
409
410
411
412
413
414
415
416
417
418
419
420 public boolean reachable(NavPoint from, NavPoint to) {
421 if ((from == null) || (to == null))
422 return false;
423 return getPathMatrixNode(from, to).getDistance() != Float.POSITIVE_INFINITY;
424 }
425
426
427
428
429
430
431
432
433
434 public float getDistance(NavPoint from, NavPoint to) {
435 if ((from == null) || (to == null))
436 return Float.POSITIVE_INFINITY;
437 return getPathMatrixNode(from, to).getDistance();
438 }
439
440
441
442
443
444
445
446
447
448
449
450
451 public List<NavPoint> getPath(NavPoint from, NavPoint to) {
452 synchronized(mutex) {
453 if (!enabled) {
454 throw new PogamutException("Can't return path as the object is disabled, call .setEnabled(true) & .refreshPathMatrix() first!", log, this);
455 }
456 if ((from == null) || (to == null))
457 return null;
458 if (log.isLoggable(Level.FINE)) log.fine("Retrieving path: " + from.getId().getStringId() + "[" + from.getLocation() + "] -> " + to.getId().getStringId() + "[" + to.getLocation() + "]");
459 List<NavPoint> path = getPathMatrixNode(from, to).getPath();
460 if (path == null) {
461 if (log.isLoggable(Level.WARNING)) log.warning("PATH NOT EXIST: " + from.getId().getStringId() + "[" + from.getLocation() + "] -> " + to.getId().getStringId() + "[" + to.getLocation() + "]");
462 } else {
463 if (log.isLoggable(Level.FINE)) log.fine("Path exists - " + path.size() + " navpoints.");
464 }
465 return path;
466 }
467 }
468
469 @Override
470 protected void cleanUp() {
471 super.cleanUp();
472 pathMatrix = null;
473 navPointIndices = null;
474 }
475
476 @Override
477 public String toString() {
478 return "FloydWarshallMap";
479 }
480
481
482
483
484
485
486
487
488
489
490
491
492 @Override
493 public IPathFuture<NavPoint> computePath(NavPoint from, NavPoint to) {
494 return new PrecomputedPathFuture<NavPoint>(from, to, getPath(from, to));
495 }
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511 public <T extends NavPoint> T getNearestNavPoint(Collection<T> locations, T target) {
512 if (locations == null) return null;
513 if (target == null) return null;
514 T nearest = null;
515 double minDistance = Double.MAX_VALUE;
516 double d;
517 for(T l : locations) {
518 if (l.getLocation() == null) continue;
519 d = getDistance(target, l);
520 if(d < minDistance) {
521 minDistance = d;
522 nearest = l;
523 }
524 }
525 return nearest;
526 }
527
528
529
530
531
532
533
534
535
536
537
538
539 public <T extends NavPoint> T getNearestNavPoint(Collection<T> locations, NavPoint target, double maxDistance) {
540 if (locations == null) return null;
541 if (target == null) return null;
542 T nearest = null;
543 double minDistance = Double.MAX_VALUE;
544 double d;
545 for(T l : locations) {
546 d = getDistance(target, l);
547 if (d > maxDistance) continue;
548 if(d < minDistance) {
549 minDistance = d;
550 nearest = l;
551 }
552 }
553 return nearest;
554 }
555
556
557
558
559
560
561
562
563
564
565
566 public <T extends NavPoint> T getNearestFilteredNavPoint(Collection<T> locations, NavPoint target, IFilter<T> filter) {
567 if (locations == null) return null;
568 if (target == null) return null;
569 T nearest = null;
570 double minDistance = Double.MAX_VALUE;
571 double d;
572 for(T l : locations) {
573 if (!filter.isAccepted(l)) continue;
574 d = getDistance(target, l);
575 if(d < minDistance) {
576 minDistance = d;
577 nearest = l;
578 }
579 }
580 return nearest;
581 }
582
583
584
585
586
587
588
589
590
591
592
593 public <T extends NavPoint> T getSecondNearestNavPoint(Collection<T> locations, NavPoint target) {
594 if (locations == null) return null;
595 if (target == null) return null;
596 T secondNearest = null;
597 T nearest = null;
598 double closestDistance = Double.MAX_VALUE;
599 double secondClosestDistance = Double.MAX_VALUE;
600
601 for (T l : locations) {
602 double distance = getDistance(target, l);
603 if (distance < closestDistance) {
604 secondClosestDistance = closestDistance;
605 secondNearest = nearest;
606
607 closestDistance = distance;
608 nearest = l;
609 } else {
610 if(distance < secondClosestDistance) {
611 secondClosestDistance = distance;
612 secondNearest = l;
613 }
614 }
615 }
616 return secondNearest;
617 }
618
619
620
621
622
623
624
625
626
627
628
629 public <T extends Item> T getNearestItem(Collection<T> locations, NavPoint target) {
630 if (locations == null) return null;
631 if (target == null) return null;
632 T nearest = null;
633 double minDistance = Double.MAX_VALUE;
634 double d;
635 for(T l : locations) {
636 if (l.getLocation() == null) continue;
637 d = getDistance(target, l.getNavPoint());
638 if(d < minDistance) {
639 minDistance = d;
640 nearest = l;
641 }
642 }
643 return nearest;
644 }
645
646
647
648
649
650
651
652
653
654
655
656
657 public <T extends Item> T getNearestItem(Collection<T> locations, NavPoint target, double maxDistance) {
658 if (locations == null) return null;
659 if (target == null) return null;
660 T nearest = null;
661 double minDistance = Double.MAX_VALUE;
662 double d;
663 for(T l : locations) {
664 d = getDistance(target, l.getNavPoint());
665 if (d > maxDistance) continue;
666 if(d < minDistance) {
667 minDistance = d;
668 nearest = l;
669 }
670 }
671 return nearest;
672 }
673
674
675
676
677
678
679
680
681
682
683
684 public <T extends Item> T getNearestFilteredItem(Collection<T> locations, NavPoint target, IFilter<T> filter) {
685 if (locations == null) return null;
686 if (target == null) return null;
687 T nearest = null;
688 double minDistance = Double.MAX_VALUE;
689 double d;
690 for(T l : locations) {
691 if (!filter.isAccepted(l)) continue;
692 d = getDistance(target, l.getNavPoint());
693 if(d < minDistance) {
694 minDistance = d;
695 nearest = l;
696 }
697 }
698 return nearest;
699 }
700
701
702
703
704
705
706
707
708
709
710
711 public <T extends Item> T getSecondNearestItem(Collection<T> targets, NavPoint from) {
712 if (targets == null) return null;
713 if (from == null) return null;
714 T secondNearest = null;
715 T nearest = null;
716 double closestDistance = Double.MAX_VALUE;
717 double secondClosestDistance = Double.MAX_VALUE;
718
719 for (T l : targets) {
720 double distance = getDistance(from, l.getNavPoint());
721 if (distance < closestDistance) {
722 secondClosestDistance = closestDistance;
723 secondNearest = nearest;
724
725 closestDistance = distance;
726 nearest = l;
727 } else {
728 if(distance < secondClosestDistance) {
729 secondClosestDistance = distance;
730 secondNearest = l;
731 }
732 }
733 }
734 return secondNearest;
735 }
736
737 }