1 package cz.cuni.amis.pogamut.ut2004.agent.navigation.stuckdetector;
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.IStuckDetector;
9 import cz.cuni.amis.pogamut.base.communication.worldview.IWorldView;
10 import cz.cuni.amis.pogamut.base.communication.worldview.object.IWorldObjectEvent;
11 import cz.cuni.amis.pogamut.base.communication.worldview.object.IWorldObjectListener;
12 import cz.cuni.amis.pogamut.base3d.worldview.object.ILocated;
13 import cz.cuni.amis.pogamut.base3d.worldview.object.Location;
14 import cz.cuni.amis.pogamut.ut2004.bot.impl.UT2004Bot;
15 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.Self;
16
17 public class UT2004PositionStuckDetector implements IStuckDetector {
18
19 private static double DEFAULT_MIN_DIAMETER = 60.0;
20
21 private static double DEFAULT_MIN_Z = 80.0;
22
23 private static int DEFAULT_HISTORY_LENGTH = 8;
24
25 private UT2004Bot bot;
26
27 private double minMovementDiameter;
28
29 private int historyLength;
30
31 private List<Location> locationHistory;
32
33 private boolean stuck = false;
34
35 private boolean botWaiting = false;
36
37 private boolean enabled = false;
38
39 private class SelfListener implements IWorldObjectListener<Self> {
40
41 public SelfListener(IWorldView worldView) {
42 worldView.addObjectListener(Self.class, this);
43 }
44
45 @Override
46 public void notify(IWorldObjectEvent<Self> event) {
47 eventSelf(event);
48 }
49
50 };
51
52 private SelfListener selfListener;
53
54 private Logger log;
55
56 private double minMovementZ;
57
58 public UT2004PositionStuckDetector(UT2004Bot bot) {
59 this(bot, DEFAULT_HISTORY_LENGTH, DEFAULT_MIN_DIAMETER, DEFAULT_MIN_Z);
60 }
61
62 public UT2004PositionStuckDetector(UT2004Bot bot, int historyLength, double minMovementDiameter, double minMovementZ) {
63 if (this.log == null) {
64 this.log = bot.getLogger().getCategory(this.getClass().getSimpleName());
65 }
66 this.bot = bot;
67 selfListener = new SelfListener(bot.getWorldView());
68
69 this.historyLength = historyLength;
70
71 if (this.historyLength < 0) throw new IllegalArgumentException("historyLength can't be < 0");
72
73 this.minMovementDiameter = minMovementDiameter;
74 this.minMovementZ = minMovementZ;
75
76 this.locationHistory = new ArrayList<Location>(this.historyLength);
77 }
78
79 @Override
80 public void setEnabled(boolean state) {
81 if (this.enabled == state) return;
82 this.enabled = state;
83 }
84
85 @Override
86 public void setBotWaiting(boolean state) {
87 this.botWaiting = state;
88 }
89
90 @Override
91 public void setBotTarget(ILocated target) {
92
93 }
94
95 public void eventSelf(IWorldObjectEvent<Self> event) {
96 if (!enabled) return;
97 if (botWaiting) return;
98
99 locationHistory.add(event.getObject().getLocation());
100 while (locationHistory.size() > historyLength) {
101 locationHistory.remove(0);
102 }
103
104 if (locationHistory.size() == historyLength) {
105 double maxDistance = Double.NEGATIVE_INFINITY;
106 double maxHeight = Double.NEGATIVE_INFINITY;
107 for (Location loc1 : locationHistory){
108 for (Location loc2: locationHistory) {
109 if (maxDistance < loc1.getDistance2D(loc2)) {
110 maxDistance = loc1.getDistance2D(loc2);
111 }
112 if (Math.abs(loc1.z - loc2.z) > maxHeight) {
113 maxHeight = Math.abs(loc1.z - loc2.z);
114 }
115 }
116 }
117 if (maxDistance < this.minMovementDiameter && maxHeight < this.minMovementZ) {
118 if (log != null && log.isLoggable(Level.WARNING)) log.warning("Bot stuck detected. Distance: " + maxDistance + " < " + this.minMovementDiameter + " && Height: " + maxHeight + " < " + minMovementZ);
119 stuck = true;
120 } else {
121 stuck = false;
122 }
123 }
124
125 }
126
127
128 @Override
129 public boolean isStuck() {
130 return stuck;
131 }
132
133 @Override
134 public void reset() {
135 if (log != null && log.isLoggable(Level.FINER)) log.finer("Reset.");
136 this.locationHistory.clear();
137 stuck = false;
138 }
139
140 }