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