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