1 package cz.cuni.amis.pogamut.ut2004.agent.navigation.navmesh.pathfollowing;
2
3 import java.util.ArrayList;
4 import java.util.List;
5 import java.util.logging.Level;
6 import java.util.logging.Logger;
7
8 import cz.cuni.amis.pogamut.base.agent.navigation.IPathExecutorState;
9 import cz.cuni.amis.pogamut.base.agent.navigation.IPathFuture;
10 import cz.cuni.amis.pogamut.base.agent.navigation.IPathPlanner;
11 import cz.cuni.amis.pogamut.base.communication.worldview.event.IWorldEventListener;
12 import cz.cuni.amis.pogamut.base.utils.logging.LogCategory;
13 import cz.cuni.amis.pogamut.base.utils.math.DistanceUtils;
14 import cz.cuni.amis.pogamut.base3d.worldview.object.ILocated;
15 import cz.cuni.amis.pogamut.base3d.worldview.object.Location;
16 import cz.cuni.amis.pogamut.ut2004.agent.module.sensor.AgentInfo;
17 import cz.cuni.amis.pogamut.ut2004.agent.navigation.IUT2004GetBackToNavGraph;
18 import cz.cuni.amis.pogamut.ut2004.agent.navigation.IUT2004Navigation;
19 import cz.cuni.amis.pogamut.ut2004.agent.navigation.IUT2004PathExecutor;
20 import cz.cuni.amis.pogamut.ut2004.agent.navigation.IUT2004RunStraight;
21 import cz.cuni.amis.pogamut.ut2004.agent.navigation.NavigationState;
22 import cz.cuni.amis.pogamut.ut2004.agent.navigation.UT2004GetBackToNavGraph;
23 import cz.cuni.amis.pogamut.ut2004.agent.navigation.UT2004Navigation;
24 import cz.cuni.amis.pogamut.ut2004.agent.navigation.UT2004RunStraight;
25 import cz.cuni.amis.pogamut.ut2004.agent.navigation.navmesh.NavMesh;
26 import cz.cuni.amis.pogamut.ut2004.agent.navigation.navmesh.NavMeshModule;
27 import cz.cuni.amis.pogamut.ut2004.agent.navigation.stuckdetector.AccUT2004DistanceStuckDetector;
28 import cz.cuni.amis.pogamut.ut2004.agent.navigation.stuckdetector.AccUT2004PositionStuckDetector;
29 import cz.cuni.amis.pogamut.ut2004.agent.navigation.stuckdetector.AccUT2004TimeStuckDetector;
30 import cz.cuni.amis.pogamut.ut2004.bot.command.AdvancedLocomotion;
31 import cz.cuni.amis.pogamut.ut2004.bot.impl.UT2004Bot;
32 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbcommands.Stop;
33 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.BotKilled;
34 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.EndMessage;
35 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.Item;
36 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.NavPoint;
37 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.Player;
38 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.Self;
39 import cz.cuni.amis.utils.flag.Flag;
40 import cz.cuni.amis.utils.flag.FlagListener;
41
42
43
44
45
46
47
48
49
50
51
52
53 public class NavMeshNavigation implements IUT2004Navigation {
54
55 public static double EXTEND_PATH_THRESHOLD = 500;
56
57
58
59 protected LogCategory log;
60
61
62
63 protected IUT2004PathExecutor<ILocated> pathExecutor;
64
65
66
67 protected NavMeshModule navMeshModule;
68
69
70
71 protected IPathPlanner<ILocated> pathPlanner;
72
73
74
75 protected UT2004Bot bot;
76
77
78
79 protected IUT2004GetBackToNavGraph getBackToNavGraph;
80
81
82
83 protected IUT2004RunStraight runStraight;
84
85
86
87
88 protected double extendPathThreshold;
89
90
91
92 protected static final int NEW_PATH_DISTANCE_THRESHOLD = 40;
93
94
95
96
97 protected static final int ARRIVED_AT_LOCATION_XY_THRESHOLD = 50;
98
99
100
101
102 protected static final int ARRIVED_AT_LOCATION_Z_THRESHOLD = 100;
103
104
105
106
107 protected static final double PLAYER_DISTANCE_TRASHOLD = 600;
108
109
110
111 public static final double AT_PLAYER = 100;
112
113
114
115 protected Flag<NavigationState> state = new Flag<NavigationState>(NavigationState.STOPPED);
116
117
118
119
120 FlagListener<IPathExecutorState> myUT2004PathExecutorStateListener = new FlagListener<IPathExecutorState>() {
121
122 @Override
123 public void flagChanged(IPathExecutorState changedValue) {
124 switch (changedValue.getState()) {
125 case TARGET_REACHED:
126 targetReached();
127 break;
128 case PATH_COMPUTATION_FAILED:
129 noPath();
130 break;
131 case STUCK:
132 if (log != null && log.isLoggable(Level.WARNING)) {
133 log.warning("stuck() - Path executor reported stuck!");
134 }
135 stuck();
136 break;
137 }
138 }
139 };
140 protected IWorldEventListener<EndMessage> endMessageListener = new IWorldEventListener<EndMessage>() {
141 @Override
142 public void notify(EndMessage event) {
143 navigate();
144 }
145 };
146
147 protected IWorldEventListener<BotKilled> botKilledMessageListener = new IWorldEventListener<BotKilled>() {
148 @Override
149 public void notify(BotKilled event) {
150 reset(true, NavigationState.STOPPED);
151 }
152 };
153
154
155
156
157
158
159
160
161
162
163
164
165
166 public NavMeshNavigation(UT2004Bot bot, AgentInfo info, AdvancedLocomotion move, NavMeshModule navMeshModule) {
167 this.log = bot.getLogger().getCategory(this.getClass().getSimpleName());
168 this.bot = bot;
169 this.navMeshModule = navMeshModule;
170 this.pathPlanner = navMeshModule.getNavMesh();
171
172 this.pathExecutor = new UT2004AcceleratedPathExecutor<ILocated>(
173 bot, info, move,
174
175 new NavMeshNavigator<ILocated>(bot, info, move, new NavMeshRunner(bot, info, move, log, navMeshModule.getNavMesh()), log));
176
177 pathExecutor.addStuckDetector(new AccUT2004TimeStuckDetector(bot, 3000, 100000));
178
179
180 pathExecutor.addStuckDetector(new AccUT2004PositionStuckDetector(bot));
181
182
183
184 pathExecutor.addStuckDetector(new AccUT2004DistanceStuckDetector(bot));
185
186
187 this.getBackToNavGraph = new UT2004GetBackToNavGraph(bot, info, move);
188 this.runStraight = new UT2004RunStraight(bot, info, move);
189
190 initListeners();
191 }
192
193 @Override
194 public Logger getLog() {
195 return log;
196 }
197
198 public void setLogLevel(Level level) {
199 getLog().setLevel(level);
200 pathExecutor.getLog().setLevel(level);
201 getBackToNavGraph.getLog().setLevel(level);
202 runStraight.getLog().setLevel(level);
203 }
204
205 private void initListeners() {
206 this.pathExecutor.getState().addListener(myUT2004PathExecutorStateListener);
207 bot.getWorldView().addEventListener(EndMessage.class, endMessageListener);
208 bot.getWorldView().addEventListener(BotKilled.class, botKilledMessageListener);
209 }
210
211
212
213
214
215 @Override
216 public void addStrongNavigationListener(FlagListener<NavigationState> listener) {
217 state.addStrongListener(listener);
218 }
219
220 @Override
221 public void removeStrongNavigationListener(FlagListener<NavigationState> listener) {
222 state.removeListener(listener);
223 }
224
225 @Override
226 public IUT2004PathExecutor<ILocated> getPathExecutor() {
227 return pathExecutor;
228 }
229
230 @Override
231 public IUT2004GetBackToNavGraph getBackToNavGraph() {
232 return getBackToNavGraph;
233 }
234
235 @Override
236 public IUT2004RunStraight getRunStraight() {
237 return runStraight;
238 }
239
240
241
242
243
244
245
246
247
248 public boolean isAvailable() {
249 return navMeshModule.isInitialized();
250 }
251
252 @Override
253 public boolean isNavigating() {
254 return navigating;
255 }
256
257 @Override
258 public boolean isNavigatingToNavPoint() {
259 return isNavigating() && getCurrentTarget() instanceof NavPoint;
260 }
261
262 @Override
263 public boolean isNavigatingToItem() {
264 return isNavigating() && getCurrentTarget() instanceof Item;
265 }
266
267 @Override
268 public boolean isNavigatingToPlayer() {
269 return isNavigating() && getCurrentTarget() instanceof Player;
270 }
271
272 @Override
273 public boolean isTryingToGetBackToNav() {
274 return getBackToNavGraph.isExecuting();
275 }
276
277 @Override
278 public boolean isPathExecuting() {
279 return pathExecutor.isExecuting();
280 }
281
282 @Override
283 public boolean isRunningStraight() {
284 return runStraight.isExecuting();
285 }
286
287 @Override
288 public ILocated getFocus() {
289 return pathExecutor.getFocus();
290 }
291
292 @Override
293 public void setFocus(ILocated located) {
294 pathExecutor.setFocus(located);
295 getBackToNavGraph.setFocus(located);
296 runStraight.setFocus(located);
297 }
298
299 @Override
300 public void stopNavigation() {
301 reset(true, NavigationState.STOPPED);
302 bot.getAct().act(new Stop());
303 }
304
305 @Override
306 public void navigate(ILocated target) {
307 if (!isAvailable()) {
308 log.severe("Cannot navigate as NavMesh was not successfully loaded! Check log during bot initialization to obtain more details.");
309 return;
310 }
311
312 if (target == null) {
313 if (log != null && log.isLoggable(Level.WARNING)) {
314 log.warning("Cannot navigate to NULL target!");
315 }
316 reset(true, NavigationState.STOPPED);
317 return;
318 }
319
320 if (target instanceof Player) {
321
322 navigate((Player) target);
323 return;
324 }
325
326 if (navigating) {
327 if (currentTarget == target || currentTarget.getLocation().equals(target.getLocation())) {
328
329 return;
330 }
331
332
333 reset(false, null);
334 }
335
336 if (log != null && log.isLoggable(Level.FINE)) {
337 log.fine("Start navigating to: " + target);
338 }
339
340 navigating = true;
341 switchState(NavigationState.NAVIGATING);
342
343 currentTarget = target;
344
345 navigate();
346 }
347
348 @Override
349 public void navigate(Player player) {
350 if (!isAvailable()) {
351 log.severe("Cannot navigate as NavMesh was not successfully loaded! Check log during bot initialization to obtain more details.");
352 return;
353 }
354
355 if (player == null) {
356 if (log != null && log.isLoggable(Level.WARNING)) {
357 log.warning("Cannot navigate to NULL player!");
358 }
359 return;
360 }
361
362 if (navigating) {
363 if (currentTarget == player) {
364
365 return;
366 }
367
368
369 reset(false, null);
370 }
371
372 if (log != null && log.isLoggable(Level.FINE)) {
373 log.fine("Start pursuing: " + player);
374 }
375
376 navigating = true;
377 switchState(NavigationState.NAVIGATING);
378
379
380
381
382 currentTarget = player;
383 currentTargetPlayer = player;
384
385 navigate();
386 }
387
388 @Override
389 public void navigate(IPathFuture<ILocated> pathHandle) {
390 if (!isAvailable()) {
391 log.severe("Cannot navigate as NavMesh was not successfully loaded! Check log during bot initialization to obtain more details.");
392 return;
393 }
394
395 if (pathHandle == null) {
396 if (log != null && log.isLoggable(Level.WARNING)) {
397 log.warning("Cannot navigate to NULL pathHandle!");
398 }
399 return;
400 }
401
402 if (navigating) {
403 if (currentPathFuture == pathHandle) {
404
405 return;
406 }
407
408
409 reset(false, null);
410 }
411
412 if (log != null && log.isLoggable(Level.FINE)) {
413 log.fine("Start running along the path to target: " + pathHandle.getPathTo());
414 }
415
416 navigating = true;
417 switchState(NavigationState.NAVIGATING);
418
419 currentTarget = pathHandle.getPathTo();
420 currentPathFuture = pathHandle;
421
422 navigate();
423 }
424
425
426 @Override
427 public NavPoint getNearestNavPoint(ILocated location) {
428 if (location == null) {
429 return null;
430 }
431 if (location instanceof NavPoint) {
432 return (NavPoint) location;
433 }
434 if (location instanceof Item) {
435 if (((Item) location).getNavPoint() != null) {
436 return ((Item) location).getNavPoint();
437 }
438 }
439 return DistanceUtils.getNearest(bot.getWorldView().getAll(NavPoint.class).values(), location);
440 }
441
442 public ILocated getContinueTo() {
443 return continueTo;
444 }
445
446 public void setContinueTo(ILocated continueTo) {
447 if (!isNavigating()) {
448 log.warning("Cannot continueTo(" + continueTo + ") as navigation is not navigating!");
449 return;
450 }
451 if (isNavigatingToPlayer()) {
452 log.warning("Cannot continueTo(" + continueTo + ") as we're navigating to player!");
453 return;
454 }
455 this.continueTo = continueTo;
456
457 NavPoint from = getNearestNavPoint(currentTarget);
458 NavPoint to = getNearestNavPoint(continueTo);
459
460 this.continueToPath = pathPlanner.computePath(from, to);
461
462 checkExtendPath();
463 }
464
465 @Override
466 public List<ILocated> getCurrentPathCopy() {
467 List<ILocated> result = new ArrayList();
468 if (currentPathFuture != null) {
469 result.addAll(currentPathFuture.get());
470 }
471 return result;
472 }
473
474 @Override
475 public List<ILocated> getCurrentPathDirect() {
476 if (currentPathFuture != null) {
477 return currentPathFuture.get();
478 }
479 return null;
480 }
481
482 @Override
483 public ILocated getCurrentTarget() {
484 return currentTarget;
485 }
486
487 @Override
488 public Player getCurrentTargetPlayer() {
489 return currentTargetPlayer;
490 }
491
492 @Override
493 public Item getCurrentTargetItem() {
494 if (currentTarget instanceof Item) {
495 return (Item) currentTarget;
496 }
497 return null;
498 }
499
500 @Override
501 public NavPoint getCurrentTargetNavPoint() {
502 if (currentTarget instanceof NavPoint) {
503 return (NavPoint) currentTarget;
504 }
505 return null;
506 }
507
508 @Override
509 public ILocated getLastTarget() {
510 return lastTarget;
511 }
512
513 @Override
514 public Player getLastTargetPlayer() {
515 return lastTargetPlayer;
516 }
517
518 @Override
519 public Item getLastTargetItem() {
520 if (lastTarget instanceof Item) {
521 return (Item) lastTarget;
522 }
523 return null;
524 }
525
526 @Override
527 public double getRemainingDistance() {
528 if (!isNavigating()) {
529 return 0;
530 }
531 if (isNavigatingToPlayer()) {
532 if (isPathExecuting()) {
533 return pathExecutor.getRemainingDistance() + pathExecutor.getPathTo().getLocation().getDistance(currentTargetPlayer.getLocation());
534 } else {
535
536 NavPoint from = getNearestNavPoint(bot.getLocation());
537 NavPoint to = getNearestNavPoint(currentTargetPlayer.getLocation());
538 IPathFuture<ILocated> pathFuture = pathPlanner.computePath(from, to);
539 if (pathFuture.isDone()) {
540 return bot.getLocation().getDistance(from.getLocation()) + getPathDistance(pathFuture.get()) + to.getLocation().getDistance(currentTargetPlayer.getLocation());
541 } else {
542
543 return -1;
544 }
545 }
546 } else {
547 if (isPathExecuting()) {
548 return pathExecutor.getRemainingDistance();
549 } else {
550
551 NavPoint from = getNearestNavPoint(bot.getLocation());
552 NavPoint to = getNearestNavPoint(currentTarget.getLocation());
553 IPathFuture<ILocated> pathFuture = pathPlanner.computePath(from, to);
554 if (pathFuture.isDone()) {
555 return bot.getLocation().getDistance(from.getLocation()) + getPathDistance(pathFuture.get()) + to.getLocation().getDistance(currentTarget.getLocation());
556 } else {
557
558 return -1;
559 }
560 }
561 }
562 }
563
564
565
566
567
568
569
570
571 private double getPathDistance(List list) {
572 if (list == null || list.size() <= 0) {
573 return 0;
574 }
575 double distance = 0;
576 ILocated curr = (ILocated) list.get(0);
577 for (int i = 1; i < list.size(); ++i) {
578 ILocated next = (ILocated) list.get(i);
579 distance += curr.getLocation().getDistance(next.getLocation());
580 curr = next;
581 }
582 return distance;
583 }
584
585
586
587
588
589
590
591
592 protected ILocated lastTarget = null;
593
594
595
596 protected Player lastTargetPlayer = null;
597
598
599
600 protected ILocated currentTarget = null;
601
602
603
604 protected Player currentTargetPlayer = null;
605
606
607
608
609 protected NavPoint fromNavPoint;
610
611
612
613 protected NavPoint toNavPoint;
614
615
616
617 protected IPathFuture currentPathFuture;
618
619
620
621 protected boolean navigating = false;
622
623
624
625 protected boolean runningStraightToPlayer = false;
626
627
628
629 protected Location runningStraightToPlayerFailedAt = null;
630
631
632
633 protected boolean usingGetBackToNavGraph = false;
634
635
636
637 protected ILocated continueTo;
638
639
640
641 protected IPathFuture<ILocated> continueToPath;
642
643
644
645
646
647 protected void navigate() {
648 if (!navigating) {
649 return;
650 }
651
652 if (log != null && log.isLoggable(Level.FINE)) {
653 log.fine("NAVIGATING");
654 }
655 if (currentTargetPlayer != null) {
656 if (log != null && log.isLoggable(Level.FINE)) {
657 log.log(Level.FINE, "Pursuing {0}", currentTargetPlayer);
658 }
659 navigatePlayer();
660 } else {
661 if (log != null && log.isLoggable(Level.FINE)) {
662 log.log(Level.FINE, "Navigating to {0}", currentTarget);
663 }
664 navigateLocation();
665 }
666 }
667
668 private void navigateLocation() {
669 if (isPathExecuting()) {
670
671
672 checkExtendPath();
673 if (log != null && log.isLoggable(Level.FINE)) {
674 log.fine("Path executor running");
675 }
676 return;
677 }
678
679
680
681
682
683 if (!usingGetBackToNavGraph && navMeshModule.getNavMesh().getPolygonId(bot.getSelf().getLocation()) >= 0) {
684
685 } else {
686
687 if (!getBackToNavGraph.isOnNavGraph()) {
688
689
690 if (log != null && log.isLoggable(Level.FINE)) {
691 log.fine("Getting back to navigation graph");
692 }
693 if (getBackToNavGraph.isExecuting()) {
694
695 return;
696 }
697 if (usingGetBackToNavGraph) {
698
699
700 if (log != null && log.isLoggable(Level.WARNING)) {
701 log.warning("stuck() - GetBackToNavGraph was already called && stopped && we're still not on nav graph.");
702 }
703 stuck();
704 return;
705 }
706 getBackToNavGraph.backToNavGraph();
707
708 usingGetBackToNavGraph = true;
709 return;
710 } else {
711 usingGetBackToNavGraph = false;
712 }
713
714
715 }
716
717 if (currentPathFuture == null) {
718 fromNavPoint = getNearestNavPoint(bot.getLocation());
719 toNavPoint = getNearestNavPoint(currentTarget);
720
721 if (log != null && log.isLoggable(Level.FINE)) {
722 log.fine("Computing path from " + fromNavPoint.getId().getStringId() + " to " + toNavPoint.getId().getStringId());
723 }
724
725 currentPathFuture = pathPlanner.computePath(fromNavPoint, toNavPoint);
726 }
727
728 switch (currentPathFuture.getStatus()) {
729 case FUTURE_IS_READY:
730
731 break;
732 case FUTURE_IS_BEING_COMPUTED:
733 if (log != null && log.isLoggable(Level.FINE)) {
734 log.fine("Waiting for the path to be computed...");
735 }
736 return;
737 case CANCELED:
738 if (log != null && log.isLoggable(Level.WARNING)) {
739 log.warning("Path computation has been canceled.");
740 }
741 noPath();
742 return;
743 case COMPUTATION_EXCEPTION:
744 if (log != null && log.isLoggable(Level.WARNING)) {
745 log.warning("Path computation has failed with an exception.");
746 }
747 noPath();
748 return;
749 }
750
751
752
753 if (!processPathFuture(currentPathFuture, currentTarget)) {
754 noPath();
755 return;
756 }
757
758 pathExecutor.followPath(currentPathFuture);
759 }
760
761 private void checkExtendPath() {
762 if (continueTo == null) {
763 return;
764 }
765 if (continueToPath == null) {
766 log.severe("continueTo specified, but continueToPath is NULL!");
767 return;
768 }
769 if (isNavigatingToPlayer()) {
770 log.warning("continueTo specified, but navigating to Player, INVALID!");
771 return;
772 }
773 if (isPathExecuting()) {
774 double remainingDistance = getRemainingDistance();
775 if (remainingDistance < extendPathThreshold) {
776 if (!continueToPath.isDone()) {
777 log.warning("Should extend path, remainingDistance = " + remainingDistance + " < " + extendPathThreshold + " = extendPathThreshold, but continueToPath.isDone() == false, cannot extend path!");
778 return;
779 }
780 log.info("Extending path to continue to " + continueTo);
781
782 pathExecutor.extendPath(((List) continueToPath.get()));
783
784
785 currentPathFuture = pathExecutor.getPathFuture();
786 lastTarget = currentTarget;
787 currentTarget = continueTo;
788 fromNavPoint = getNearestNavPoint(((IPathFuture<ILocated>) currentPathFuture).get().get(0).getLocation());
789 toNavPoint = getNearestNavPoint(((IPathFuture<ILocated>) currentPathFuture).get().get(currentPathFuture.get().size() - 1).getLocation());
790
791 continueTo = null;
792 continueToPath = null;
793 }
794 }
795
796 }
797
798
799 private void navigatePlayer() {
800 double vDistance = bot.getLocation().getDistanceZ(currentTargetPlayer.getLocation());
801 double hDistance = bot.getLocation().getDistance2D(currentTargetPlayer.getLocation());
802
803 if (hDistance < AT_PLAYER && vDistance < 50) {
804
805 if (log != null && log.isLoggable(Level.FINE)) {
806 log.fine("Player reached");
807 }
808 if (pathExecutor.isExecuting()) {
809 pathExecutor.getPath().set(pathExecutor.getPath().size() - 1, bot.getLocation());
810 } else {
811 targetReached();
812 }
813 return;
814 }
815
816 if (hDistance < 400 && Math.abs(vDistance) < 50) {
817
818 if (runningStraightToPlayer) {
819 if (runStraight.isFailed()) {
820 runningStraightToPlayer = false;
821 runningStraightToPlayerFailedAt = bot.getLocation();
822 }
823 } else {
824 if (runningStraightToPlayerFailedAt == null ||
825 bot.getLocation().getDistance(runningStraightToPlayerFailedAt) > 500
826 ) {
827 if (getBackToNavGraph.isExecuting()) {
828 getBackToNavGraph.stop();
829 usingGetBackToNavGraph = false;
830 }
831 if (pathExecutor.isExecuting()) {
832 pathExecutor.stop();
833 }
834 runningStraightToPlayer = true;
835 runningStraightToPlayerFailedAt = null;
836 runStraight.runStraight(currentTargetPlayer);
837 }
838 }
839 if (runningStraightToPlayer) {
840 if (log != null && log.isLoggable(Level.FINE)) {
841 log.fine("Running straight to player");
842 }
843 return;
844 }
845 } else {
846 if (runningStraightToPlayer) {
847 runningStraightToPlayer = false;
848 runStraight.stop(false);
849 }
850 }
851
852 if (pathExecutor.isExecuting()) {
853
854 if (log != null && log.isLoggable(Level.FINE)) {
855 log.fine("Path executor running");
856 }
857
858 double distance = currentTarget.getLocation().getDistance(currentTargetPlayer.getLocation());
859
860 if (distance < PLAYER_DISTANCE_TRASHOLD) {
861
862
863 return;
864 }
865
866 if (log != null && log.isLoggable(Level.FINE)) {
867 log.fine("Player moved " + distance + " from its original location, checking path...");
868 }
869
870 NavPoint newToNavPoint = getNearestNavPoint(currentTargetPlayer);
871 if (newToNavPoint != toNavPoint) {
872
873 if (log != null && log.isLoggable(Level.FINE)) {
874 log.fine("Replanning path to get to " + currentTargetPlayer);
875 }
876 pathExecutor.stop();
877 currentPathFuture = null;
878 } else {
879 if (log != null && log.isLoggable(Level.FINE)) {
880 log.fine("Path remains the same");
881 }
882 return;
883 }
884 }
885
886
887
888
889
890
891 if (!getBackToNavGraph.isOnNavGraph()) {
892
893
894 if (log != null && log.isLoggable(Level.FINE)) {
895 log.fine("Getting back to navigation graph");
896 }
897 if (getBackToNavGraph.isExecuting()) {
898
899 return;
900 }
901 if (usingGetBackToNavGraph) {
902
903
904 if (log != null && log.isLoggable(Level.WARNING)) {
905 log.warning("stuck() - GetBackToNavGraph was already called && stopped && we're still not on nav graph.");
906 }
907 stuck();
908 return;
909 }
910 getBackToNavGraph.backToNavGraph();
911
912 usingGetBackToNavGraph = true;
913 return;
914 } else {
915 usingGetBackToNavGraph = false;
916 }
917
918
919
920 if (currentPathFuture == null) {
921 fromNavPoint = getNearestNavPoint(bot.getLocation());
922 toNavPoint = getNearestNavPoint(currentTarget);
923
924 if (log != null && log.isLoggable(Level.FINE)) {
925 log.fine("Computing path from " + fromNavPoint.getId().getStringId() + " to " + toNavPoint.getId().getStringId());
926 }
927
928 currentPathFuture = pathPlanner.computePath(fromNavPoint, toNavPoint);
929 }
930
931 switch (currentPathFuture.getStatus()) {
932 case FUTURE_IS_READY:
933
934 break;
935 case FUTURE_IS_BEING_COMPUTED:
936 if (log != null && log.isLoggable(Level.FINE)) {
937 log.fine("Waiting for the path to be computed...");
938 }
939 return;
940 case CANCELED:
941 if (log != null && log.isLoggable(Level.WARNING)) {
942 log.warning("Path computation has been canceled.");
943 }
944 noPath();
945 return;
946 case COMPUTATION_EXCEPTION:
947 if (log != null && log.isLoggable(Level.WARNING)) {
948 log.warning("Path computation has failed with an exception.");
949 }
950 noPath();
951 return;
952 }
953
954
955
956 if (!processPathFuture(currentPathFuture, currentTarget)) {
957 noPath();
958 return;
959 }
960
961 pathExecutor.followPath(currentPathFuture);
962 }
963
964
965
966
967
968
969
970 protected boolean processPathFuture(IPathFuture futurePath, ILocated currentTarget) {
971 List<ILocated> pathList = futurePath.get();
972
973 if (pathList == null) {
974
975 return false;
976 }
977
978 if (currentTarget == null) {
979
980
981
982 if (pathList.size() == 0) {
983 return false;
984 }
985 currentTarget = pathList.get(pathList.size() - 1);
986 } else if (pathList.size() == 0) {
987 currentPathFuture.get().add(currentTarget);
988 } else {
989 ILocated lastPathElement = pathList.get(pathList.size() - 1);
990 if (lastPathElement.getLocation().getDistance(currentTarget.getLocation()) > NEW_PATH_DISTANCE_THRESHOLD) {
991 currentPathFuture.get().add(currentTarget);
992 }
993 }
994 return true;
995 }
996
997 protected void switchState(NavigationState newState) {
998 state.setFlag(newState);
999 }
1000
1001 protected void noPath() {
1002
1003 reset(true, NavigationState.PATH_COMPUTATION_FAILED);
1004 }
1005
1006
1007 protected void stuck() {
1008
1009 reset(true, NavigationState.STUCK);
1010 }
1011
1012 protected void targetReached() {
1013
1014 reset(true, NavigationState.TARGET_REACHED);
1015 }
1016
1017 protected void reset(boolean stopGetBackToNavGraph, NavigationState resultState) {
1018 if (currentTarget != null) {
1019 lastTarget = currentTarget;
1020 lastTargetPlayer = currentTargetPlayer;
1021 }
1022
1023 navigating = false;
1024
1025 currentTarget = null;
1026 currentTargetPlayer = null;
1027
1028 fromNavPoint = null;
1029 toNavPoint = null;
1030
1031 currentPathFuture = null;
1032
1033 runningStraightToPlayer = false;
1034 runningStraightToPlayerFailedAt = null;
1035
1036 continueTo = null;
1037 continueToPath = null;
1038
1039 pathExecutor.stop();
1040 runStraight.stop(false);
1041 if (stopGetBackToNavGraph) {
1042 getBackToNavGraph.stop();
1043 usingGetBackToNavGraph = false;
1044 }
1045
1046
1047 if (resultState == null) {
1048 return;
1049 }
1050 switchState(resultState);
1051 }
1052
1053 @Override
1054 public Flag<NavigationState> getState() {
1055 return state.getImmutable();
1056 }
1057 }