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.base3d.worldview.object.ILocated;
20 import cz.cuni.amis.pogamut.base3d.worldview.object.IViewable;
21 import cz.cuni.amis.pogamut.base3d.worldview.object.Location;
22 import cz.cuni.amis.pogamut.unreal.communication.messages.UnrealId;
23 import cz.cuni.amis.pogamut.ut2004.agent.module.sensor.NavigationGraphBuilder;
24 import cz.cuni.amis.pogamut.ut2004.communication.messages.INavPoint;
25 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.Item;
26 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.NavPoint;
27 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.NavPointNeighbourLink;
28 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.Player;
29 import cz.cuni.amis.pogamut.ut2004.communication.translator.shared.events.MapPointListObtained;
30 import cz.cuni.amis.pogamut.ut2004.utils.LinkFlag;
31 import cz.cuni.amis.utils.IFilter;
32 import cz.cuni.amis.utils.collections.MyCollections;
33 import cz.cuni.amis.utils.exception.PogamutException;
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59 public class FloydWarshallMap extends SensorModule<IGhostAgent> implements IPathPlanner<NavPoint> {
60
61 public static class PathMatrixNode {
62
63 private float distance = Float.POSITIVE_INFINITY;
64 private Integer viaNode = null;
65 private List<NavPoint> path = null;
66
67
68
69
70 public PathMatrixNode() {
71 }
72
73 public PathMatrixNode(float distance) {
74 this.distance = distance;
75 }
76
77 public float getDistance() {
78 return distance;
79 }
80
81 public void setDistance(float distance) {
82 this.distance = distance;
83 }
84
85
86
87
88
89
90 public Integer getViaNode() {
91 return viaNode;
92 }
93
94 public void setViaNode(Integer indice) {
95 this.viaNode = indice;
96 }
97
98 public List<NavPoint> getPath() {
99 return path;
100 }
101
102 public void setPath(List<NavPoint> path) {
103 this.path = path;
104 }
105
106 }
107
108 private IWorldEventListener<MapPointListObtained> mapListener = new IWorldEventListener<MapPointListObtained>() {
109
110 @Override
111 public void notify(MapPointListObtained event) {
112 if (log.isLoggable(Level.INFO)) log.info("Map point list obtained.");
113 performFloydWarshall(event);
114 }
115 };
116
117
118
119
120 public static final int BAD_EDGE_FLAG = LinkFlag.FLY.get()
121 | LinkFlag.LADDER.get() | LinkFlag.PROSCRIBED.get()
122 | LinkFlag.SWIM.get() | LinkFlag.PLAYERONLY.get();
123
124 public static boolean isWalkable(int flag) {
125 return ((flag & BAD_EDGE_FLAG) == 0) && ((flag & LinkFlag.SPECIAL.get()) == 0);
126 }
127
128
129
130
131 protected int badEdgeFlag = 0;
132
133
134
135
136 protected Map<UnrealId, Integer> navPointIndices;
137
138
139
140
141
142
143
144
145 protected Map<Integer, NavPoint> indicesNavPoints;
146
147
148 protected PathMatrixNode[][] pathMatrix;
149
150
151
152
153
154
155 private boolean enabled = true;
156
157
158
159
160 protected Object mutex = new Object();
161
162 public FloydWarshallMap(IGhostAgent bot) {
163 this(bot, BAD_EDGE_FLAG, null);
164 }
165
166 public FloydWarshallMap(IGhostAgent bot, Logger log) {
167 this(bot, BAD_EDGE_FLAG, log);
168 }
169
170 public FloydWarshallMap(IGhostAgent bot, int badEdgeFlag, Logger log) {
171 super(bot, log);
172 this.badEdgeFlag = badEdgeFlag;
173 worldView.addEventListener(MapPointListObtained.class, mapListener);
174 }
175
176
177
178
179
180
181
182
183 public boolean isEnabled() {
184 return enabled;
185 }
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200 public void setEnabled(boolean enabled) {
201 synchronized(mutex) {
202 this.enabled = enabled;
203 if (!enabled) {
204 cleanUp();
205 }
206 }
207 }
208
209
210
211
212
213 public void refreshPathMatrix() {
214 synchronized(mutex) {
215 if (!enabled) {
216 if (log.isLoggable(Level.WARNING)) log.fine(this + ": Won't refresh path matrix, object is disabled.");
217 return;
218 }
219 if (log.isLoggable(Level.FINE)) log.fine(this + ": Refreshing path matrix...");
220 List<NavPoint> navPoints = MyCollections.asList(agent.getWorldView().getAll(NavPoint.class).values());
221 performFloydWarshall(navPoints);
222 if (log.isLoggable(Level.INFO)) log.info(this + ": Path matrix refreshed!");
223 }
224 }
225
226 protected void performFloydWarshall(MapPointListObtained map) {
227 List<NavPoint> navPoints = MyCollections.asList(map.getNavPoints().values());
228 performFloydWarshall(navPoints);
229 }
230
231 protected void performFloydWarshall(List<NavPoint> navPoints) {
232 if (!enabled) {
233 if (log.isLoggable(Level.WARNING)) log.fine(this + ": Should not be running Floyd-Warshall, object disabled.");
234 return;
235 }
236 if (log.isLoggable(Level.FINE)) log.fine(this + ": Beginning Floyd-Warshall algorithm...");
237 long start = System.currentTimeMillis();
238
239
240 int size = navPoints.size();
241 navPointIndices = new HashMap<UnrealId, Integer>(size);
242 indicesNavPoints = new HashMap<Integer, NavPoint>(size);
243 pathMatrix = new PathMatrixNode[size][size];
244
245
246 for (int i = 0; i < navPoints.size(); ++i) {
247 navPointIndices.put(navPoints.get(i).getId(), i);
248 indicesNavPoints.put(i, navPoints.get(i));
249 }
250
251
252 for (int i = 0; i < size; i++) {
253 for (int j = 0; j < size; j++) {
254 pathMatrix[i][j] = new PathMatrixNode((i == j) ? 0.0f
255 : Float.POSITIVE_INFINITY);
256 }
257 }
258
259
260 for (int i = 0; i < size; i++) {
261 Point3d location = navPoints.get(i).getLocation().getPoint3d();
262 for (NavPointNeighbourLink link : navPoints.get(i)
263 .getOutgoingEdges().values()) {
264 if (checkLink(link)) {
265 pathMatrix[i][navPointIndices.get(link.getToNavPoint()
266 .getId())].setDistance((float) location
267 .distance(link.getToNavPoint().getLocation()
268 .getPoint3d()));
269 }
270 }
271 }
272
273
274 for (int k = 0; k < size; k++) {
275 for (int i = 0; i < size; i++) {
276 for (int j = 0; j < size; j++) {
277 float newLen = pathMatrix[i][k].getDistance()
278 + pathMatrix[k][j].getDistance();
279 if (pathMatrix[i][j].getDistance() > newLen) {
280 pathMatrix[i][j].setDistance(newLen);
281 pathMatrix[i][j].setViaNode(k);
282 }
283 }
284 }
285 }
286
287
288 int count = 0;
289 for (int i = 0; i < size; i++) {
290 for (int j = 0; j < size; j++) {
291 if (pathMatrix[i][j].getDistance() == Float.POSITIVE_INFINITY) {
292 if (log.isLoggable(Level.FINER)) log.warning("Unreachable path from " + navPoints.get(i).getId().getStringId()
293 + " -> " + navPoints.get(j).getId().getStringId());
294 count++;
295 } else {
296
297 pathMatrix[i][j].setPath(retrievePath(i, j));
298 }
299 }
300 }
301
302 if (count > 0) {
303 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).");
304 }
305
306 if (log.isLoggable(Level.INFO)) log.info(this + ": computation for " + size + " navpoints took " + (System.currentTimeMillis() - start) + " millis");
307
308
309 indicesNavPoints = null;
310 }
311
312
313
314
315
316
317
318
319
320
321 public boolean checkLink(NavPointNeighbourLink edge) {
322
323 if ((edge.getFlags() & badEdgeFlag) != 0)
324 return false;
325
326
327 if ((edge.getFlags() & LinkFlag.SPECIAL.get()) != 0)
328 return true;
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350 if (edge.getNeededJump() != null && edge.getNeededJump().z > 680) {
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 }