1 package Steerings;
2
3 import SteeringStuff.SteeringManager;
4 import SteeringStuff.SteeringTools;
5 import SteeringStuff.RefBoolean;
6 import SteeringProperties.LeaderFollowingProperties;
7 import SteeringProperties.LeaderFollowingProperties.LFtype;
8 import SteeringProperties.SteeringProperties;
9 import cz.cuni.amis.pogamut.base3d.worldview.object.Location;
10 import cz.cuni.amis.pogamut.ut2004.bot.impl.UT2004Bot;
11 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.ConfigChange;
12 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.Player;
13 import SteeringStuff.ISteering;
14 import java.util.Collection;
15 import java.util.LinkedList;
16 import java.util.Random;
17 import javax.vecmath.Tuple3d;
18 import javax.vecmath.Vector2d;
19 import javax.vecmath.Vector3d;
20
21
22
23
24
25
26
27
28 public class LeaderFollowingSteer implements ISteering {
29
30
31 private UT2004Bot botself;
32
33 private LeaderFollowingProperties p;
34
35
36 private int leaderForce;
37
38 private String leaderName;
39
40 private int distanceFromTheLeader;
41
42 private int forceDistance;
43
44 private LFtype myLFtype;
45
46 private boolean deceleration;
47
48 private double angleFromTheLeader;
49
50 private boolean velocityMemory;
51
52 private int sizeOfMemory;
53
54 private boolean circumvention;
55
56 private static int NEARLY_THERE_DISTANCE = 80;
57 private double innerDistanceFromTheLeader = Math.max(150,distanceFromTheLeader/2);
58
59
60 private LinkedList<Vector3d> leadersVelocities;
61
62 private Player leader;
63 private Vector3d leadersVelocity;
64 private Location leadersLocation;
65
66 private Location lastLeadersLoc;
67 private Vector3d lastLeadersVelocity;
68 private Vector3d lastLeadersNonZeroVelocity;
69
70 Random random = new Random();
71
72
73
74
75
76 public LeaderFollowingSteer(UT2004Bot bot)
77 {
78 botself=bot;
79 leadersVelocities = new LinkedList<Vector3d>();
80 }
81
82
83
84
85 public Vector3d run(Vector3d scaledActualVelocity, RefBoolean wantsToGoFaster, RefBoolean wantsToStop, Location focus) {
86
87 Vector3d nextVelocity;
88
89 Vector3d actualVelocity = botself.getVelocity().getVector3d();
90
91 nextVelocity = new Vector3d(0,0,0);
92
93
94 if (leader == null ) {
95 leader = getLeader();
96 if (leader == null) {
97 Vector3d turningVector = new Vector3d(actualVelocity.y, -actualVelocity.x, 0);
98 turningVector.scale(1/(Math.sqrt(2)));
99 Vector3d negativeVector = new Vector3d(-actualVelocity.x,-actualVelocity.y,0);
100 negativeVector.scale(1-1/Math.sqrt(2));
101 turningVector.add((Tuple3d)negativeVector);
102 return turningVector;
103 } else {
104 lastLeadersLoc = leader.getLocation();
105 lastLeadersVelocity = leader.getVelocity().getVector3d();
106 lastLeadersNonZeroVelocity = lastLeadersVelocity;
107 }
108 }
109
110 if (!leader.isVisible()) {
111 if (botself.getLocation().getDistance(lastLeadersLoc) < NEARLY_THERE_DISTANCE) {
112 Vector3d turningVector = new Vector3d(actualVelocity.y, -actualVelocity.x, 0);
113 turningVector.scale(1 / (Math.sqrt(2)));
114 Vector3d negativeVector = new Vector3d(-actualVelocity.x, -actualVelocity.y, 0);
115 negativeVector.scale(1 - 1 / Math.sqrt(2));
116 turningVector.add((Tuple3d) negativeVector);
117 return turningVector;
118 }
119 leadersLocation = lastLeadersLoc;
120 leadersVelocity = lastLeadersVelocity;
121 } else {
122 leadersLocation = leader.getLocation();
123 leadersVelocity = leader.getVelocity().getVector3d();
124 }
125
126 Vector3d botToLeader = new Vector3d(leadersLocation.getX() - botself.getLocation().getX(), leadersLocation.getY() - botself.getLocation().getY(), 0);
127 lastLeadersLoc = leader.getLocation();
128 lastLeadersVelocity = leader.getVelocity().getVector3d();
129 if (lastLeadersVelocity.length() != 0) lastLeadersNonZeroVelocity = lastLeadersVelocity;
130
131 if (velocityMemory) {
132 if (leadersVelocities.size() > sizeOfMemory)
133 leadersVelocities.removeFirst();
134 leadersVelocities.addLast(leadersVelocity);
135 }
136
137 if (SteeringManager.DEBUG) System.out.println("leaders velocity " + leadersVelocity.length());
138
139 if (myLFtype.equals(LFtype.BASIC)) {
140 if (SteeringManager.DEBUG) System.out.println("Distance should be "+distanceFromTheLeader+" and is "+botToLeader.length());
141 if (leadersVelocity.length() == 0 && Math.abs(botToLeader.length() - distanceFromTheLeader) <= NEARLY_THERE_DISTANCE ) {
142
143
144 wantsToStop.setValue(true);
145 if (SteeringManager.DEBUG) System.out.println("Leader stopped. We stop also.");
146 focus.setTo(leadersLocation);
147 if (SteeringManager.DEBUG) System.out.println("The focus should be "+focus);
148 return nextVelocity;
149 } else {
150 if (botToLeader.length() <= distanceFromTheLeader) {
151
152 if (deceleration) {
153
154
155
156 if (leadersVelocity.length() <= actualVelocity.length()) {
157
158
159
160 Vector3d fromTheLeader = new Vector3d(botToLeader.x, botToLeader.y, botToLeader.z);
161 fromTheLeader.negate();
162
163
164 fromTheLeader.normalize();
165 if (leadersVelocity.length() == 0) {
166 if (scaledActualVelocity.length() == 0)
167 fromTheLeader.scale(leaderForce);
168 else
169 fromTheLeader.scale(2*scaledActualVelocity.length());
170 if (SteeringManager.DEBUG) System.out.println("Leader stopped. But we are too near. => From the leader "+fromTheLeader.length());
171 } else {
172 fromTheLeader.scale(50*actualVelocity.length() / leadersVelocity.length());
173 }
174 nextVelocity.add((Tuple3d) fromTheLeader);
175 if (SteeringManager.DEBUG) System.out.println("Leader is slower => From the leader "+fromTheLeader.length());
176 }
177
178
179
180 double nextLength = scaledActualVelocity.length() * (botToLeader.length()/distanceFromTheLeader);
181 botToLeader.scale(nextLength/botToLeader.length());
182 nextVelocity.add((Tuple3d) botToLeader);
183 nextVelocity.sub(scaledActualVelocity);
184 if (SteeringManager.DEBUG) System.out.println("We are too near => From the leader "+nextVelocity.length());
185
186 } else {
187
188 nextVelocity.sub(botToLeader);
189 double multiplier = botToLeader.length()/distanceFromTheLeader;
190
191 nextVelocity.normalize();
192 nextVelocity.scale(leaderForce*multiplier*multiplier);
193 }
194 wantsToGoFaster.setValue(false);
195 } else {
196
197
198
199 if (SteeringManager.DEBUG) System.out.println("Bot to leader "+botToLeader.length());
200 nextVelocity.add((Tuple3d) botToLeader);
201 nextVelocity.normalize();
202 double distanceFromIdealPosition = botToLeader.length() - distanceFromTheLeader;
203 double scale = getForceOfTheDistance(distanceFromIdealPosition);
204 nextVelocity.scale(scale);
205 if (SteeringManager.DEBUG) System.out.println("Bot to leader after scaling " + nextVelocity.length());
206 wantsToGoFaster.setValue(true);
207 }
208 }
209 } else {
210 Vector3d averageLeadersVelocity;
211 if (velocityMemory) {
212 averageLeadersVelocity = getAverageLeadersVelocity();
213 } else {
214 averageLeadersVelocity = leadersVelocity;
215 }
216 if (leadersVelocity.length() == 0 || averageLeadersVelocity.length() == 0) {
217
218 Vector3d specialSituation = new Vector3d(lastLeadersNonZeroVelocity.x * Math.cos(angleFromTheLeader) - lastLeadersNonZeroVelocity.y * Math.sin(angleFromTheLeader), lastLeadersNonZeroVelocity.x * Math.sin(angleFromTheLeader) + lastLeadersNonZeroVelocity.y * Math.cos(angleFromTheLeader), 0);
219 specialSituation.scale(distanceFromTheLeader / specialSituation.length());
220 Vector3d botToSpSituation = new Vector3d(leadersLocation.x + specialSituation.x - botself.getLocation().x, leadersLocation.y + specialSituation.y - botself.getLocation().y, 0);
221 if (botToSpSituation.length() <= NEARLY_THERE_DISTANCE) {
222 wantsToStop.setValue(true);
223 if (SteeringManager.DEBUG) System.out.println("Leader stopped. We stop also. "+botToSpSituation.length());
224 focus.setTo(leadersLocation);
225 if (SteeringManager.DEBUG) System.out.println("The focus should be "+focus);
226 return nextVelocity;
227 } else {
228 if (SteeringManager.DEBUG) System.out.println("Leader stopped. But we must go to our position. "+botToSpSituation.length());
229 nextVelocity.add((Tuple3d)toTheSituation(botToSpSituation, wantsToGoFaster, leadersLocation));
230 wantsToGoFaster.setValue(false);
231 return nextVelocity;
232 }
233 } else {
234
235 ConfigChange cc = botself.getWorldView().getSingle(ConfigChange.class);
236 double oneTick = cc.getVisionTime();
237
238 Vector3d vectorSituationToTheLeader = new Vector3d(averageLeadersVelocity.x * Math.cos(angleFromTheLeader) - averageLeadersVelocity.y * Math.sin(angleFromTheLeader), averageLeadersVelocity.x * Math.sin(angleFromTheLeader) + averageLeadersVelocity.y * Math.cos(angleFromTheLeader), 0);
239
240 vectorSituationToTheLeader.scale(distanceFromTheLeader / vectorSituationToTheLeader.length());
241
242 Vector3d leadersShiftToNextTick = new Vector3d(oneTick*averageLeadersVelocity.x, oneTick*averageLeadersVelocity.y, oneTick*averageLeadersVelocity.z);
243
244 Vector3d l = new Vector3d(oneTick*leadersVelocity.x, oneTick*leadersVelocity.y, oneTick*leadersVelocity.z);
245 Location leadersNextLoc = new Location(leadersLocation.x + leadersShiftToNextTick.x, leadersLocation.y + leadersShiftToNextTick.y, leadersLocation.z + leadersShiftToNextTick.z);
246 if (SteeringManager.DEBUG) System.out.println("shift "+l.length()+" time "+oneTick+" 0.8*av "+(0.8*averageLeadersVelocity.length()));
247
248
249 Vector3d botToSituation = new Vector3d(leadersNextLoc.x + vectorSituationToTheLeader.x - botself.getLocation().x, leadersNextLoc.y + vectorSituationToTheLeader.y - botself.getLocation().y, 0);
250
251 nextVelocity.add((Tuple3d)toTheSituation(botToSituation, wantsToGoFaster, leadersNextLoc));
252 }
253 }
254
255 return nextVelocity;
256 }
257
258
259
260
261
262
263
264 private Vector3d toTheSituation(Vector3d botToSituation, RefBoolean wantsToGoFaster, Location leadersNextLoc) {
265 Vector3d result = new Vector3d(0,0,0);
266 Vector2d leadersLoc = new Vector2d((leadersLocation.x + leadersNextLoc.x)/2, (leadersLocation.y + leadersNextLoc.y)/2);
267 Vector3d botToLeader = new Vector3d(leadersLocation.x - botself.getLocation().x,leadersLocation.y - botself.getLocation().y,0);
268 Vector3d botToNextLeader = new Vector3d(leadersLoc.x - botself.getLocation().x,leadersLoc.y - botself.getLocation().y,0);
269
270 wantsToGoFaster.setValue(false);
271
272 if (circumvention && botToLeader.length() < botToSituation.length()) {
273 Vector3d turningVector;
274 double koef;
275 boolean turnLeft;
276 if (botToSituation.angle(botToNextLeader) == 0) {
277 turnLeft = random.nextBoolean();
278 if (SteeringManager.DEBUG) { System.out.println("Leader exactly front collision."); }
279 koef = 1;
280 } else {
281 Vector2d startVelocity = new Vector2d(botself.getLocation().x,botself.getLocation().y);
282 Vector2d endVelocity = new Vector2d(botself.getLocation().x + botToSituation.x,botself.getLocation().y + botToSituation.y);
283 Vector2d nearestPoint = SteeringTools.getNearestPoint(startVelocity, endVelocity, leadersLoc, true);
284 Vector2d nearestPointToLeadersLoc = new Vector2d(nearestPoint.x - leadersLoc.x, nearestPoint.y - leadersLoc.y);
285 koef = Math.max(0, (innerDistanceFromTheLeader - nearestPointToLeadersLoc.length())/innerDistanceFromTheLeader);
286 turnLeft = SteeringTools.pointIsLeftFromTheVector(botToLeader, botToSituation);
287 if (SteeringManager.DEBUG) { System.out.println("Leader nearly front collision."+nearestPointToLeadersLoc.length()); }
288 if (SteeringManager.DEBUG) { System.out.println("Distance "+nearestPointToLeadersLoc.length()+" koef "+koef); }
289 }
290 turningVector = SteeringTools.getTurningVector(botToSituation, turnLeft);
291 turningVector.scale(koef);
292 if (SteeringManager.DEBUG) { System.out.println("Turning vector "+turningVector.length()+" botToSit "+botToSituation.length()+" turn left "+turnLeft); }
293 result.add(turningVector);
294 }
295 result.add((Tuple3d) botToSituation);
296 double scale = getForceOfTheDistance(botToSituation.length());
297 result.normalize();
298 result.scale(scale);
299 return result;
300 }
301
302
303 private double getForceOfTheDistance(double distance) {
304 double scale = leaderForce * distance / forceDistance;
305 if (scale > SteeringManager.MAX_FORCE) {
306 scale = SteeringManager.MAX_FORCE;
307 }
308 return scale;
309 }
310
311
312 private Player getLeader()
313 {
314 Collection<Player> col=botself.getWorldView().getAll(Player.class).values();
315 for (Player pl : col) {
316 if (pl.getName().equals(leaderName)) {
317 if (SteeringManager.DEBUG) System.out.println("Got the leader");
318 leadersVelocities.addLast(pl.getVelocity().getVector3d());
319 return pl;
320 }
321 }
322 return null;
323 }
324
325 private Vector3d getAverageLeadersVelocity() {
326 double x = 0;
327 double y = 0;
328 for(Vector3d velo : leadersVelocities) {
329 x += velo.x;
330 y += velo.y;
331 }
332 x = x/leadersVelocities.size();
333 y = y/leadersVelocities.size();
334 Vector3d averageVelo = new Vector3d(x,y,0);
335 return averageVelo;
336 }
337
338 @Override
339 public void setProperties(SteeringProperties newProperties) {
340 this.leaderForce = ((LeaderFollowingProperties)newProperties).getLeaderForce();
341 this.leaderName = ((LeaderFollowingProperties)newProperties).getLeaderName();
342 this.distanceFromTheLeader = ((LeaderFollowingProperties)newProperties).getDistanceFromTheLeader();
343 this.forceDistance = ((LeaderFollowingProperties)newProperties).getForceDistance();
344 this.myLFtype = ((LeaderFollowingProperties)newProperties).getMyLFtype();
345 this.deceleration = ((LeaderFollowingProperties)newProperties).isDeceleration();
346 this.angleFromTheLeader = ((LeaderFollowingProperties)newProperties).getAngleFromTheLeader();
347 this.velocityMemory = ((LeaderFollowingProperties)newProperties).isVelocityMemory();
348 this.sizeOfMemory = ((LeaderFollowingProperties)newProperties).getSizeOfMemory();
349 this.circumvention = ((LeaderFollowingProperties)newProperties).isCircumvention();
350 if (forceDistance == 0) {
351 forceDistance = 1;
352 }
353 }
354
355 public LeaderFollowingProperties getProperties() {
356 LeaderFollowingProperties properties = new LeaderFollowingProperties();
357
358 properties.setLeaderForce(leaderForce);
359 properties.setLeaderName(leaderName);
360 properties.setDistanceFromTheLeader(distanceFromTheLeader);
361 properties.setForceDistance(forceDistance);
362 properties.setMyLFtype(myLFtype);
363 properties.setDeceleration(deceleration);
364 properties.setAngleFromTheLeader(angleFromTheLeader);
365 properties.setVelocityMemory(velocityMemory);
366 properties.setSizeOfMemory(sizeOfMemory);
367 properties.setCircumvention(circumvention);
368
369 return properties;
370 }
371 }