1 package cz.cuni.amis.pogamut.udk.agent.navigation.martinnavigator;
2
3 import java.util.Iterator;
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.utils.math.DistanceUtils;
9 import cz.cuni.amis.pogamut.base3d.worldview.object.ILocated;
10 import cz.cuni.amis.pogamut.base3d.worldview.object.Location;
11 import cz.cuni.amis.pogamut.udk.agent.module.sensor.AgentInfo;
12 import cz.cuni.amis.pogamut.udk.agent.navigation.AbstractUDKPathNavigator;
13 import cz.cuni.amis.pogamut.udk.agent.navigation.IUDKPathRunner;
14 import cz.cuni.amis.pogamut.udk.bot.command.AdvancedLocomotion;
15 import cz.cuni.amis.pogamut.udk.bot.impl.UDKBot;
16 import cz.cuni.amis.pogamut.udk.communication.messages.gbinfomessages.NavPoint;
17 import cz.cuni.amis.pogamut.udk.communication.messages.gbinfomessages.NavPointNeighbourLink;
18 import cz.cuni.amis.utils.NullCheck;
19
20
21
22
23
24
25 public class MartinNavigator<PATH_ELEMENT extends ILocated> extends AbstractUDKPathNavigator<PATH_ELEMENT>
26 {
27
28
29
30
31
32
33 public static final int CLOSE_ENOUGH = 60;
34 public static final int PRECISION = 100;
35
36 @Override
37 public double getPrecision() {
38 return PRECISION;
39 }
40
41
42
43
44 private Location navigDestination = null;
45
46
47
48
49 private Stage navigStage = Stage.COMPLETED;
50
51
52
53
54 @Override
55 protected void navigate(int pathElementIndex) {
56 switch (navigStage = keepNavigating()) {
57 case NAVIGATING:
58 case REACHING:
59
60 return;
61 case CRASHED:
62
63 executor.stuck();
64 return;
65 case COMPLETED:
66
67 executor.targetReached();
68 return;
69 }
70 }
71
72 @Override
73 public void reset() {
74
75 navigCurrentLocation = null;
76 navigCurrentNode = null;
77 navigCurrentLink = null;
78 navigDestination = null;
79 navigIterator = null;
80 navigNextLocation = null;
81 navigNextNode = null;
82 navigNextLocationOffset = 0;
83 navigStage = Stage.COMPLETED;
84 }
85
86 @Override
87 public void newPath(List<PATH_ELEMENT> path) {
88
89 Location dest = path.get(path.size()-1).getLocation();
90
91
92 initPathNavigation(dest, path);
93 }
94
95
96
97
98
99
100 protected void initPathNavigation(Location destination, List<PATH_ELEMENT> path)
101 {
102
103 if (log != null && log.isLoggable(Level.FINE))
104 log.fine (
105 "LoqueNavigator.initPathNavigation(): initializing path navigation"
106 + ", nodes " + path.size ()
107 );
108
109 if (!initAlongPath(destination, path))
110 {
111
112 initDirectNavigation(destination);
113 }
114 }
115
116
117
118
119
120
121
122 protected boolean initAlongPath(Location dest, List<PATH_ELEMENT> path)
123 {
124
125 navigDestination = dest;
126 navigIterator = path.iterator();
127
128 navigCurrentLocation = null;
129 navigCurrentNode = null;
130
131 prepareNextNode();
132
133 navigStage = Stage.NAVIGATING;
134
135 return switchToNextNode();
136 }
137
138
139
140
141
142
143 protected void initDirectNavigation (Location dest)
144 {
145 initDirectly (dest);
146 }
147
148
149
150
151
152
153
154 protected Stage initDirectly(Location dest)
155 {
156
157 navigDestination = dest;
158
159 runner.reset();
160
161 return navigStage = Stage.REACHING;
162 }
163
164
165
166
167
168
169
170 protected Stage keepNavigating ()
171 {
172
173 if (navigStage.terminated)
174 return navigStage;
175
176
177 switch (navigStage)
178 {
179 case REACHING:
180 navigStage = navigDirectly();
181 break;
182 default:
183 navigStage = navigAlongPath();
184 break;
185 }
186
187
188 if (log != null && log.isLoggable(Level.FINEST)) log.finest ("Navigator.keepNavigating(): navigation stage " + navigStage);
189 return navigStage;
190 }
191
192
193
194
195
196
197
198 private Stage navigDirectly ()
199 {
200
201 int distance = (int) memory.getLocation().getDistance(navigDestination);
202
203
204 if (distance <= getPrecision())
205 {
206 if (log != null && log.isLoggable(Level.FINE)) log.fine ("LoqueNavigator.navigDirectly(): destination close enough: " + distance);
207 return Stage.COMPLETED;
208 }
209
210
211 if (!runner.runToLocation (navigDestination, null, navigDestination, null, true))
212 {
213 if (log != null && log.isLoggable(Level.FINE)) log.fine ("LoqueNavigator.navigDirectly(): direct navigation failed");
214 return Stage.CRASHED;
215 }
216
217
218 if (log != null && log.isLoggable(Level.FINEST)) log.finest ("LoqueNavigator.navigDirectly(): traveling directly, distance = " + distance);
219 return navigStage;
220 }
221
222
223
224
225
226
227 private Iterator<PATH_ELEMENT> navigIterator = null;
228
229
230
231
232 private int navigNextLocationOffset = 0;
233
234
235
236
237 private Location navigCurrentLocation = null;
238
239
240
241
242
243 private NavPoint navigCurrentNode = null;
244
245
246
247
248
249 private NavPointNeighbourLink navigCurrentLink = null;
250
251
252
253
254 private Location navigNextLocation = null;
255
256
257
258
259
260 private NavPoint navigNextNode = null;
261
262
263
264
265
266
267
268
269 protected NavPoint getNavPoint(ILocated location) {
270 if (location instanceof NavPoint) return (NavPoint) location;
271 NavPoint np = DistanceUtils.getNearest(main.getWorldView().getAll(NavPoint.class).values(), location);
272 if (np.getLocation().getDistance(location.getLocation()) < CLOSE_ENOUGH) return np;
273 return null;
274 }
275
276
277
278
279
280
281
282 private Stage navigAlongPath()
283 {
284
285 int totalDistance = (int) memory.getLocation().getDistance(navigDestination);
286
287
288 if (totalDistance <= getPrecision())
289 {
290 log.finest ("Navigator.navigAlongPath(): destination close enough: " + totalDistance);
291 return Stage.COMPLETED;
292 }
293
294 return navigToCurrentNode();
295 }
296
297
298
299
300
301
302
303
304 private void prepareNextNode ()
305 {
306
307
308 ILocated located = null;
309 navigNextLocation = null;
310 navigNextLocationOffset = 0;
311 while ((located == null) && navigIterator.hasNext ())
312 {
313
314 located = navigIterator.next();
315 navigNextLocationOffset += 1;
316 if (located == null) {
317 continue;
318 }
319 }
320
321
322 if (located == null) {
323 navigNextLocationOffset = 0;
324 return;
325 }
326
327 if (executor.getPathElementIndex() + navigNextLocationOffset >= executor.getPath().size()) {
328 navigNextLocationOffset = 0;
329 }
330
331
332 navigNextLocation = located.getLocation();
333
334 navigNextNode = getNavPoint(located);
335 }
336
337
338
339
340
341 private boolean switchToNextNode ()
342 {
343
344 Location navigLastLocation = navigCurrentLocation;
345 NavPoint navigLastNode = navigCurrentNode;
346
347
348 if (null == (navigCurrentLocation = navigNextLocation))
349 {
350
351 if (log != null && log.isLoggable(Level.FINER)) log.finer ("Navigator.switchToNextNode(): no nodes left");
352 navigCurrentNode = null;
353 return false;
354 }
355
356 navigCurrentNode = navigNextNode;
357
358
359 navigCurrentLink = getNavPointsLink(navigLastNode, navigCurrentNode);
360
361
362 if (navigLastLocation == null) {
363 navigLastLocation = navigCurrentLocation;
364 navigLastNode = navigCurrentNode;
365 }
366
367
368 int localDistance = (int) memory.getLocation().getDistance(navigCurrentLocation.getLocation());
369
370 if (navigCurrentNode == null) {
371
372 runner.reset();
373 if (log != null && log.isLoggable(Level.FINE))
374 log.fine (
375 "LoqueNavigator.switchToNextNode(): switch to next location " + navigCurrentLocation
376 + ", distance " + localDistance
377
378 );
379 } else {
380
381 runner.reset();
382
383
384 if (log != null && log.isLoggable(Level.FINE))
385 log.fine (
386 "LoqueNavigator.switchToNextNode(): switch to next node " + navigCurrentNode.getId().getStringId()
387 + ", distance " + localDistance
388 + ", reachable " + isReachable(navigCurrentNode)
389 );
390 }
391
392
393 if (executor.getPathElementIndex() < 0) {
394 executor.switchToAnotherPathElement(0);
395 } else {
396 if (navigNextLocationOffset > 0) {
397 executor.switchToAnotherPathElement(executor.getPathElementIndex()+navigNextLocationOffset);
398 } else {
399 executor.switchToAnotherPathElement(executor.getPathElementIndex());
400 }
401 }
402 navigNextLocationOffset = 0;
403
404 return true;
405 }
406
407 private boolean isReachable(NavPoint node) {
408 if (node == null) return true;
409 int hDistance = (int) memory.getLocation().getDistance2D(node.getLocation());
410 int vDistance = (int) node.getLocation().getDistanceZ(memory.getLocation());
411 double angle;
412 if (hDistance == 0) {
413 angle = vDistance == 0 ? 0 : (vDistance > 0 ? Math.PI/2 : -Math.PI/2);
414 } else {
415 angle = Math.atan(vDistance / hDistance);
416 }
417 return Math.abs(vDistance) < 30 && Math.abs(angle) < Math.PI / 4;
418 }
419
420
421
422
423
424
425
426
427
428
429
430
431 private NavPointNeighbourLink getNavPointsLink(NavPoint start, NavPoint end) {
432 if (start == null) {
433
434 NavPoint tmp = getNavPoint(memory.getLocation());
435 if (tmp != null)
436 start = tmp;
437 else
438 return null;
439 }
440 if (end == null)
441 return null;
442
443 if (end.getIncomingEdges().containsKey(start.getId()))
444 return end.getIncomingEdges().get(start.getId());
445
446 return null;
447 }
448
449
450
451
452
453
454
455 private Stage navigToCurrentNode ()
456 {
457 if (navigCurrentNode != null) {
458
459 navigCurrentLocation = navigCurrentNode.getLocation();
460 }
461
462
463 int localDistance = (int) memory.getLocation().getDistance(navigCurrentLocation.getLocation());
464
465 int localDistance2 = (int) memory.getLocation().getDistance(
466 Location.add(navigCurrentLocation.getLocation(), new Location (0,0,100))
467 );
468
469
470 Location firstLocation = navigCurrentLocation.getLocation();
471
472 Location secondLocation = (navigNextNode != null && !navigNextNode.isLiftCenter() && !navigNextNode.isLiftCenter() ?
473 navigNextNode.getLocation() :
474 navigNextLocation);
475
476 Location focus = (navigNextLocation == null) ? firstLocation : navigNextLocation.getLocation();
477
478
479 if (!runner.runToLocation (firstLocation, secondLocation, focus, navigCurrentLink, (navigCurrentNode == null ? true : isReachable(navigCurrentNode)))) {
480 if (log != null && log.isLoggable(Level.FINE)) log.fine ("LoqueNavigator.navigToCurrentNode(): navigation to current node failed");
481 return Stage.CRASHED;
482 }
483
484
485 if (log != null && log.isLoggable(Level.FINEST)) log.finest ("LoqueNavigator.navigToCurrentNode(): traveling to current node, distance = " + localDistance);
486
487
488
489
490
491 if ((navigCurrentNode != null && navigCurrentNode == navigNextNode) || navigCurrentLocation.equals(navigNextLocation))
492 {
493
494 prepareNextNode();
495 }
496
497
498 int testDistance = 200;
499 if (navigCurrentNode != null && (navigCurrentNode.isLiftCenter() || navigCurrentNode.isLiftExit())) {
500
501 testDistance = 150;
502 }
503
504
505 if ( (localDistance < testDistance) || (localDistance2 < testDistance) )
506 {
507
508 if (!switchToNextNode ())
509 {
510
511 if (log != null && log.isLoggable(Level.FINE)) log.fine ("Navigator.navigToCurrentNode(): switch to direct navigation");
512 return initDirectly(navigDestination);
513 }
514 } else {
515
516 if (navigNextLocation != null) {
517 localDistance = (int) memory.getLocation().getDistance(navigNextLocation.getLocation());
518
519 localDistance2 = (int) memory.getLocation().getDistance(
520 Location.add(navigNextLocation.getLocation(), new Location (0,0,100))
521 );
522
523 if ( (localDistance < testDistance) || (localDistance2 < testDistance) )
524 {
525
526 if (!switchToNextNode ())
527 {
528
529 if (log != null && log.isLoggable(Level.FINE)) log.fine ("Navigator.navigToCurrentNode(): switch to direct navigation");
530 return initDirectly(navigDestination);
531 }
532 }
533 }
534 }
535
536
537 return navigStage;
538 }
539
540
541
542
543
544
545 private enum TerminatingStageType {
546
547 SUCCESS (false),
548
549 FAILURE (true);
550
551
552 public boolean failure;
553
554
555
556
557
558 private TerminatingStageType (boolean failure)
559 {
560 this.failure = failure;
561 }
562 };
563
564
565
566
567 public enum Stage
568 {
569
570
571
572 REACHING ()
573 {
574 protected Stage next () { return this; }
575 },
576
577
578
579 NAVIGATING ()
580 {
581 protected Stage next () { return this; }
582 },
583
584
585
586 CRASHED (TerminatingStageType.FAILURE)
587 {
588 protected Stage next () { return this; }
589 },
590
591
592
593 COMPLETED (TerminatingStageType.SUCCESS)
594 {
595 protected Stage next () { return this; }
596 };
597
598
599
600
601
602
603 public boolean terminated;
604
605
606
607 public boolean failure;
608
609
610
611
612
613
614 private Stage ()
615 {
616 this.terminated = false;
617 this.failure = false;
618 }
619
620
621
622
623
624 private Stage (TerminatingStageType type)
625 {
626 this.terminated = true;
627 this.failure = type.failure;
628 }
629
630
631
632
633
634
635
636
637
638 protected abstract Stage next ();
639
640
641
642 }
643
644
645
646
647
648
649 private IUDKPathRunner runner;
650
651
652
653
654 protected UDKBot main;
655
656 protected AgentInfo memory;
657
658 protected AdvancedLocomotion body;
659
660 protected Logger log;
661
662
663
664
665
666
667
668
669 public MartinNavigator (UDKBot bot, Logger log)
670 {
671
672 this.main = bot;
673 this.memory = new AgentInfo(bot);
674 this.body = new AdvancedLocomotion(bot, log);
675 this.log = log;
676
677
678 this.runner = new MartinRunner(bot, memory, body, log);
679 }
680
681
682
683
684
685
686 public MartinNavigator (UDKBot bot, IUDKPathRunner runner, Logger log)
687 {
688
689 this.main = bot;
690 this.memory = new AgentInfo(bot);
691 this.body = new AdvancedLocomotion(bot, log);
692 this.log = log;
693
694
695 this.runner = runner;
696 NullCheck.check(this.runner, "runner");
697 }
698
699 }