1 package Steerings;
2
3 import SocialSteeringsBeta.RefLocation;
4 import SteeringStuff.SteeringManager;
5 import SteeringStuff.SteeringTools;
6 import SteeringStuff.RefBoolean;
7 import SteeringProperties.SteeringProperties;
8 import SteeringProperties.WallFollowingProperties;
9 import SteeringStuff.IRaysFlagChanged;
10 import cz.cuni.amis.pogamut.base3d.worldview.object.Location;
11 import javax.vecmath.Vector3d;
12 import cz.cuni.amis.pogamut.ut2004.bot.impl.UT2004Bot;
13 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.AutoTraceRay;
14 import SteeringStuff.ISteering;
15 import SteeringStuff.RaycastingManager;
16 import SteeringStuff.SteeringRay;
17 import SteeringStuff.SteeringType;
18 import cz.cuni.amis.pogamut.ut2004.utils.UnrealUtils;
19 import java.util.HashMap;
20 import java.util.LinkedList;
21 import java.util.Random;
22 import java.util.concurrent.ExecutionException;
23 import java.util.concurrent.Future;
24 import java.util.logging.Level;
25 import java.util.logging.Logger;
26 import javax.vecmath.Tuple3d;
27
28
29
30
31
32
33
34 public class WallFollowingSteer implements ISteering, IRaysFlagChanged {
35
36 private UT2004Bot botself;
37 private RaycastingManager rayManager;
38
39 private int wallForce;
40
41 private int orderOfTheForce;
42
43 private double attractiveForceWeight;
44
45 private double repulsiveForceWeight;
46
47 private double convexEdgesForceWeight;
48
49 private double concaveEdgesForceWeight;
50
51 private boolean justMySide;
52
53 private boolean specialDetection;
54
55 private boolean frontCollisions;
56
57
58
59 int DISTANCE_FROM_THE_WALL = 166;
60
61 private static final int DEFAULTCOUNTER = 10;
62
63 private boolean goFast = false;
64
65
66 enum State { NOTHING, LEFT, RIGHT };
67
68
69
70 State state;
71
72
73 int counter;
74
75
76 boolean convexTurning;
77
78 private boolean raysReady;
79 private HashMap<String, Future<AutoTraceRay>> myFutureRays;
80
81
82 protected static final String NLEFTFRONT = "wleftfront";
83 protected static final String NLEFT = "wleft";
84 protected static final String NRIGHTFRONT = "wrightfront";
85 protected static final String NRIGHT = "wright";
86 protected static final String NFRONT = "wfront";
87
88 Random random = new Random();
89
90
91
92
93 AutoTraceRay nleft, nright, nleftfront, nrightfront, nfront;
94
95
96
97
98 int shortSideRayLength;
99 int shortSideFrontRayLength;
100 int longSideRayLength;
101 int longSideFrontRayLength;
102 int shortFrontRayLength;
103 int longFrontRayLength;
104
105
106
107
108
109
110
111
112 public WallFollowingSteer(UT2004Bot bot, RaycastingManager rayManager, SteeringProperties newProperties) {
113 botself = bot;
114 this.rayManager = rayManager;
115 setProperties(newProperties);
116 state = State.NOTHING;
117 counter = 0;
118 convexTurning = false;
119 prepareRays();
120 }
121
122
123
124
125
126
127 public WallFollowingSteer(UT2004Bot bot, RaycastingManager rayManager) {
128 botself = bot;
129 this.rayManager = rayManager;
130 setProperties(new WallFollowingProperties());
131 state = State.NOTHING;
132 counter = 0;
133 convexTurning = false;
134 prepareRays();
135 }
136
137
138
139
140 private void prepareRays() {
141
142
143 int shortLength = 8;
144 int longLength = 12;
145
146 shortSideRayLength = (int) (UnrealUtils.CHARACTER_COLLISION_RADIUS * shortLength * DISTANCE_FROM_THE_WALL / 166f);
147 longSideRayLength = (int) (UnrealUtils.CHARACTER_COLLISION_RADIUS * longLength * DISTANCE_FROM_THE_WALL / 166f);
148 shortSideFrontRayLength = (int) (UnrealUtils.CHARACTER_COLLISION_RADIUS * shortLength * 2 * DISTANCE_FROM_THE_WALL / 166f);
149 longSideFrontRayLength = (int) (UnrealUtils.CHARACTER_COLLISION_RADIUS * longLength * 2 * DISTANCE_FROM_THE_WALL / 166f);
150 shortFrontRayLength = (int) (UnrealUtils.CHARACTER_COLLISION_RADIUS * shortLength * Math.sqrt(3) * DISTANCE_FROM_THE_WALL / 166f);
151 longFrontRayLength = (int) (UnrealUtils.CHARACTER_COLLISION_RADIUS * longLength * Math.sqrt(3) * DISTANCE_FROM_THE_WALL / 166f);
152
153
154 LinkedList<SteeringRay> rayList = new LinkedList<SteeringRay>();
155 rayList.add(new SteeringRay(NLEFT, new Vector3d(0, -1, 0), longSideRayLength));
156 rayList.add(new SteeringRay(NLEFTFRONT, new Vector3d(Math.sqrt(3), -1, 0), longSideFrontRayLength));
157 rayList.add(new SteeringRay(NRIGHTFRONT, new Vector3d(Math.sqrt(3), 1, 0), longSideFrontRayLength));
158 rayList.add(new SteeringRay(NRIGHT, new Vector3d(0, 1, 0), longSideRayLength));
159 rayList.add(new SteeringRay(NFRONT, new Vector3d(1, 0, 0), longFrontRayLength));
160 rayManager.addRays(SteeringType.WALL_FOLLOWING, rayList, this);
161 raysReady = false;
162
163 }
164
165
166 public void flagRaysChanged() {
167 myFutureRays = rayManager.getMyFutureRays(SteeringType.WALL_FOLLOWING);
168 raysReady = false;
169 listenToRays();
170 }
171
172 private void listenToRays() {
173 for(String rayId : myFutureRays.keySet()) {
174 Future<AutoTraceRay> fr = myFutureRays.get(rayId);
175 if (fr.isDone()) {
176
177 try {
178 if (rayId.equals(NLEFTFRONT)) {
179 nleftfront = fr.get();
180 } else if (rayId.equals(NRIGHTFRONT)) {
181 nrightfront = fr.get();
182 } else if (rayId.equals(NLEFT)) {
183 nleft = fr.get();
184 } else if (rayId.equals(NRIGHT)) {
185 nright = fr.get();
186 } else if (rayId.equals(NFRONT)) {
187 nfront = fr.get();
188 }
189 raysReady = true;
190 } catch (InterruptedException ex) {
191 Logger.getLogger(ObstacleAvoidanceSteer.class.getName()).log(Level.SEVERE, null, ex);
192 } catch (ExecutionException ex) {
193 Logger.getLogger(ObstacleAvoidanceSteer.class.getName()).log(Level.SEVERE, null, ex);
194 }
195 } else {
196
197 }
198 }
199 }
200
201
202
203
204
205
206 @Override
207 public Vector3d run(Vector3d scaledActualVelocity, RefBoolean wantsToGoFaster, RefBoolean wantsToStop, RefLocation focus) {
208
209
210
211 wantsToGoFaster.setValue(false);
212
213 Vector3d nextVelocity = new Vector3d(0,0,0);
214
215 if (!raysReady) {
216 listenToRays();
217 return nextVelocity;
218 }
219
220
221 Vector3d botToHitLocation;
222
223
224 Vector3d normal;
225
226
227 double multiplier;
228
229
230 Vector3d actualVelocity = botself.getVelocity().getVector3d();
231
232
233 boolean shortrays = false;
234
235
236 boolean sensornLeft = false;
237 boolean sensornRight = false;
238 boolean sensornLeftFront = false;
239 boolean sensornRightFront = false;
240 boolean sensornFront = false;
241
242
243 if (nleftfront != null)
244 sensornLeftFront = nleftfront.isResult();
245 if (nrightfront != null)
246 sensornRightFront = nrightfront.isResult();
247 if (nleft != null)
248 sensornLeft = nleft.isResult();
249 if (nright != null)
250 sensornRight = nright.isResult();
251 if (nfront != null)
252 sensornFront = nfront.isResult();
253
254
255
256
257
258
259 if (state == State.NOTHING) {
260
261
262 if (sensornLeft && sensornLeftFront) {
263 state = State.LEFT;
264 counter = DEFAULTCOUNTER;
265 } else if (sensornRight && sensornRightFront) {
266 state = State.RIGHT;
267 counter = DEFAULTCOUNTER;
268 } else if (sensornFront) {
269 if (frontCollisions) {
270 botToHitLocation = new Vector3d(nfront.getHitLocation().x - botself.getLocation().x, nfront.getHitLocation().y - botself.getLocation().y, 0);
271 normal = nfront.getHitNormal();
272 if (Math.PI - normal.angle(actualVelocity) < Math.PI/5) {
273 boolean turnLeft;
274 if (normal.angle(actualVelocity) == 180) {
275 turnLeft = random.nextBoolean();
276 if (SteeringManager.DEBUG) {
277 System.out.println("Wall exactly front collision.");
278 }
279 } else {
280 turnLeft = SteeringTools.pointIsLeftFromTheVector(actualVelocity, normal);
281 if (SteeringManager.DEBUG) {
282 System.out.println("Wall nearly front collision.");
283 }
284 }
285 if (SteeringManager.DEBUG) {
286 System.out.println("We turn left " + turnLeft);
287 }
288 Vector3d turn = SteeringTools.getTurningVector(actualVelocity, turnLeft);
289 turn.normalize();
290 turn.scale(0.5);
291 normal.add(turn);
292 normal.normalize();
293 multiplier = Math.pow(((longFrontRayLength - botToHitLocation.length()) * 2f / longFrontRayLength), orderOfTheForce);
294 normal.scale(multiplier * wallForce);
295 nextVelocity.add((Tuple3d) normal);
296 return nextVelocity;
297 }
298 }
299 }
300 } else if (state == State.LEFT) {
301 if (sensornLeft && sensornLeftFront) {
302 counter = DEFAULTCOUNTER;
303 }
304 } else if (state == State.RIGHT) {
305 if (sensornRight && sensornRightFront) {
306 counter = DEFAULTCOUNTER;
307 }
308 }
309
310
311
312
313 if (justMySide) {
314 if (state.equals(State.LEFT)) {
315 sensornRight = false;
316 sensornRightFront = false;
317 } else if (state.equals(State.RIGHT)) {
318 sensornLeft = false;
319 sensornLeftFront = false;
320 }
321 }
322
323
324
325
326 if (goFast) {
327 if (sensornFront) {
328 wantsToGoFaster.setValue(false);
329 } else {
330 wantsToGoFaster.setValue(true);
331 }
332 } else {
333 if (!shortrays) {
334 wantsToGoFaster.setValue(true);
335 } else {
336 wantsToGoFaster.setValue(false);
337 }
338 }
339
340
341
342
343
344
345
346
347
348 if (sensornLeft) {
349
350 botToHitLocation = new Vector3d(nleft.getHitLocation().x - botself.getLocation().x, nleft.getHitLocation().y - botself.getLocation().y, 0);
351 normal = nleft.getHitNormal();
352 multiplier = Math.pow(1 - (longSideRayLength - botToHitLocation.length()) / longSideRayLength, orderOfTheForce);
353 normal.scale(attractiveForceWeight * multiplier * wallForce);
354 nextVelocity.sub((Tuple3d) normal);
355
356
357
358 if (botToHitLocation.length() < shortSideRayLength) {
359 shortrays = true;
360 multiplier = Math.pow(((shortSideRayLength - botToHitLocation.length()) * 2f / shortSideRayLength), orderOfTheForce);
361 normal.normalize();
362 normal.scale(repulsiveForceWeight * multiplier * wallForce);
363 nextVelocity.add((Tuple3d) normal);
364
365 }
366 }
367
368 if (sensornRight) {
369
370 botToHitLocation = new Vector3d(nright.getHitLocation().x - botself.getLocation().x, nright.getHitLocation().y - botself.getLocation().y, 0);
371 normal = nright.getHitNormal();
372 multiplier = Math.pow(1 - (longSideRayLength - botToHitLocation.length()) / longSideRayLength, orderOfTheForce);
373 normal.scale(attractiveForceWeight * multiplier * wallForce);
374 nextVelocity.sub((Tuple3d) normal);
375
376
377
378 if (botToHitLocation.length() < shortSideRayLength) {
379 shortrays = true;
380 multiplier = Math.pow(((shortSideRayLength - botToHitLocation.length()) * 2f / shortSideRayLength), orderOfTheForce);
381 normal.normalize();
382 normal.scale(repulsiveForceWeight * multiplier * wallForce);
383 nextVelocity.add((Tuple3d) normal);
384
385 }
386 }
387 if (sensornLeftFront) {
388
389 botToHitLocation = new Vector3d(nleftfront.getHitLocation().x - botself.getLocation().x, nleftfront.getHitLocation().y - botself.getLocation().y, 0);
390 normal = nleftfront.getHitNormal();
391 multiplier = Math.pow(1 - (longSideFrontRayLength - botToHitLocation.length()) / longSideFrontRayLength, orderOfTheForce);
392 normal.scale(attractiveForceWeight * multiplier * wallForce);
393 nextVelocity.sub((Tuple3d) normal);
394
395
396
397 if (botToHitLocation.length() < shortSideFrontRayLength) {
398 shortrays = true;
399 multiplier = Math.pow(((shortSideFrontRayLength - botToHitLocation.length()) * 2f / shortSideFrontRayLength), orderOfTheForce);
400 normal.normalize();
401 normal.scale(repulsiveForceWeight * multiplier * wallForce);
402 nextVelocity.add((Tuple3d) normal);
403
404 }
405
406 }
407 if (sensornRightFront) {
408
409 botToHitLocation = new Vector3d(nrightfront.getHitLocation().x - botself.getLocation().x, nrightfront.getHitLocation().y - botself.getLocation().y, 0);
410 normal = nrightfront.getHitNormal();
411 multiplier = Math.pow(1 - (longSideFrontRayLength - botToHitLocation.length()) / longSideFrontRayLength, orderOfTheForce);
412 normal.scale(attractiveForceWeight * multiplier * wallForce);
413 nextVelocity.sub((Tuple3d) normal);
414
415
416
417 if (botToHitLocation.length() < shortSideFrontRayLength) {
418 shortrays = true;
419 multiplier = Math.pow(((shortSideFrontRayLength - botToHitLocation.length()) * 2f / shortSideFrontRayLength), orderOfTheForce);
420 normal.normalize();
421 normal.scale(repulsiveForceWeight * multiplier * wallForce);
422 nextVelocity.add((Tuple3d) normal);
423
424 }
425 }
426 if (sensornFront) {
427
428 botToHitLocation = new Vector3d(nfront.getHitLocation().x - botself.getLocation().x, nfront.getHitLocation().y - botself.getLocation().y, 0);
429 normal = nfront.getHitNormal();
430
431
432
433
434
435 if (botToHitLocation.length() < shortFrontRayLength) {
436 shortrays = true;
437 multiplier = Math.pow(((shortFrontRayLength - botToHitLocation.length()) * 2f / shortFrontRayLength), orderOfTheForce);
438 normal.normalize();
439 normal.scale(repulsiveForceWeight * 3 * multiplier * wallForce);
440
441 nextVelocity.add((Tuple3d) normal);
442 }
443 }
444
445
446 boolean nextTurning = false;
447
448
449
450 if (specialDetection) {
451 if (state.equals(State.LEFT)) {
452 if ( (sensornFront && (convexTurning || (Math.abs(nfront.getHitNormal().angle(actualVelocity) - Math.PI) < Math.PI/4)) ) ||
453 ( (sensornLeftFront && (Math.abs(nleftfront.getHitNormal().angle(actualVelocity) - Math.PI) < Math.PI/2) && convexTurning) ||
454 (sensornLeftFront && (Math.abs(nleftfront.getHitNormal().angle(actualVelocity) - Math.PI) < Math.PI/4)) ||
455 (sensornLeft && (Math.abs(nleft.getHitNormal().angle(actualVelocity) - Math.PI) < Math.PI/2) && convexTurning) ) ) {
456 if (SteeringManager.DEBUG) System.out.println("Left convex edge. " + SteeringTools.radiansToDegrees(nleftfront.getHitNormal().angle(actualVelocity)));
457 if (SteeringManager.DEBUG) {
458 if (sensornFront) System.out.println("Left convex edge front collision. " + SteeringTools.radiansToDegrees(nfront.getHitNormal().angle(actualVelocity)));
459 else if (sensornLeftFront) System.out.println("Left convex edge side front. " + SteeringTools.radiansToDegrees(nleftfront.getHitNormal().angle(actualVelocity)));
460 else System.out.println("Left convex edge side. " + SteeringTools.radiansToDegrees(nleft.getHitNormal().angle(actualVelocity)));
461 }
462
463 nextTurning = true;
464 double k = 1;
465 if (sensornFront) {
466 k = nfront.getHitNormal().angle(actualVelocity) / Math.PI;
467 }
468 else if (sensornLeftFront) {
469 k = 0.6*nleftfront.getHitNormal().angle(actualVelocity) / Math.PI;
470 }
471 else {
472 k = 0.3*nleft.getHitNormal().angle(actualVelocity) / Math.PI;
473 }
474 if (SteeringManager.DEBUG) {
475 System.out.println("Turning vector. " + k*SteeringTools.getTurningVector2(actualVelocity, false).length());
476 }
477 Vector3d turningVector = SteeringTools.getTurningVector2(actualVelocity, false);
478 turningVector.scale(k*convexEdgesForceWeight);
479
480
481 counter = DEFAULTCOUNTER;
482 convexTurning = nextTurning;
483 return turningVector;
484
485 }
486 } else if (state.equals(State.RIGHT)) {
487 if ( (sensornFront && (convexTurning || (Math.abs(nfront.getHitNormal().angle(actualVelocity) - Math.PI) < Math.PI/4)) ) ||
488 ( (sensornRightFront && (Math.abs(nrightfront.getHitNormal().angle(actualVelocity) - Math.PI) < Math.PI/2) && convexTurning) ||
489 (sensornRightFront && (Math.abs(nrightfront.getHitNormal().angle(actualVelocity) - Math.PI) < Math.PI/4)) ||
490 (sensornRight && (Math.abs(nright.getHitNormal().angle(actualVelocity) - Math.PI) < Math.PI/2) && convexTurning) ) ) {
491 if (SteeringManager.DEBUG) {
492 if (sensornFront) System.out.println("Right convex edge front collision. " + SteeringTools.radiansToDegrees(nfront.getHitNormal().angle(actualVelocity)));
493 else if (sensornRightFront) System.out.println("Right convex edge side front. " + SteeringTools.radiansToDegrees(nrightfront.getHitNormal().angle(actualVelocity)));
494 else System.out.println("Right convex edge side. " + SteeringTools.radiansToDegrees(nright.getHitNormal().angle(actualVelocity)));
495 }
496 nextTurning = true;
497 double k = 1;
498 if (sensornFront) {
499 k = nfront.getHitNormal().angle(actualVelocity) / Math.PI;
500 }
501 else if (sensornRightFront) {
502 k = 0.6*nrightfront.getHitNormal().angle(actualVelocity) / Math.PI;
503 }
504 else {
505 k = 0.3*nright.getHitNormal().angle(actualVelocity) / Math.PI;
506 }
507 if (SteeringManager.DEBUG) {
508 System.out.println("Turning vector. " + k*SteeringTools.getTurningVector2(actualVelocity, true).length());
509 }
510 Vector3d turningVector = SteeringTools.getTurningVector2(actualVelocity, true);
511 turningVector.scale(k*convexEdgesForceWeight);
512
513
514 counter = DEFAULTCOUNTER;
515 convexTurning = nextTurning;
516 return turningVector;
517
518 }
519 }
520 } else {
521
522 if (state == State.LEFT) {
523 if (sensornLeftFront && sensornFront) {
524
525
526 Vector3d turningVector = SteeringTools.getTurningVector2(actualVelocity, false);
527 turningVector.scale(convexEdgesForceWeight*0.8);
528 return turningVector;
529
530
531
532 }
533 } else if (state == State.RIGHT) {
534 if (sensornRightFront && sensornFront) {
535
536
537 Vector3d turningVector = SteeringTools.getTurningVector2(actualVelocity, true);
538 turningVector.scale(convexEdgesForceWeight*0.8);
539 return turningVector;
540
541
542
543 }
544 }
545 }
546
547
548
549
550
551
552
553
554 if (state == State.LEFT) {
555 if (!sensornLeft && !sensornLeftFront) {
556 if (SteeringManager.DEBUG) System.out.println("counter "+counter);
557 if (counter > 0) {
558 counter--;
559 if (SteeringManager.DEBUG) System.out.println("Left concave edge. ");
560 Vector3d turningVector = SteeringTools.getTurningVector2(actualVelocity, true);
561 if (convexTurning) turningVector.scale(0.5);
562 turningVector.scale(concaveEdgesForceWeight*0.8);
563 return turningVector;
564 } else {
565 state = State.NOTHING;
566 }
567 }
568 } else if (state == State.RIGHT) {
569 if (!sensornRight && !sensornRightFront) {
570 if (SteeringManager.DEBUG) System.out.println("counter "+counter);
571 if (counter > 0) {
572 counter--;
573 if (SteeringManager.DEBUG) System.out.println("Right concave edge. ");
574 Vector3d turningVector = SteeringTools.getTurningVector2(actualVelocity, false);
575 if (convexTurning) turningVector.scale(0.5);
576 turningVector.scale(concaveEdgesForceWeight*0.8);
577 return turningVector;
578
579
580 } else {
581 state = State.NOTHING;
582 }
583 }
584 }
585
586
587 convexTurning = nextTurning;
588 return nextVelocity;
589 }
590
591 @Override
592 public void setProperties(SteeringProperties newProperties) {
593 this.wallForce = ((WallFollowingProperties)newProperties).getWallForce();
594 this.orderOfTheForce = ((WallFollowingProperties)newProperties).getOrderOfTheForce();
595 this.attractiveForceWeight = ((WallFollowingProperties)newProperties).getAttractiveForceWeight();
596 this.repulsiveForceWeight = ((WallFollowingProperties)newProperties).getRepulsiveForceWeight();
597 this.concaveEdgesForceWeight = ((WallFollowingProperties)newProperties).getConcaveEdgesForceWeight();
598 this.convexEdgesForceWeight = ((WallFollowingProperties)newProperties).getConvexEdgesForceWeight();
599 this.justMySide = ((WallFollowingProperties)newProperties).isJustMySide();
600 this.specialDetection = ((WallFollowingProperties)newProperties).isSpecialDetection();
601 this.frontCollisions = ((WallFollowingProperties)newProperties).isFrontCollisions();
602 }
603
604 public WallFollowingProperties getProperties() {
605 WallFollowingProperties properties = new WallFollowingProperties();
606 properties.setWallForce(wallForce);
607 properties.setOrderOfTheForce(orderOfTheForce);
608 properties.setAttractiveForceWeight(attractiveForceWeight);
609 properties.setRepulsiveForceWeight(repulsiveForceWeight);
610 properties.setConcaveEdgesForceWeight(concaveEdgesForceWeight);
611 properties.setConvexEdgesForceWeight(convexEdgesForceWeight);
612 properties.setJustMySide(justMySide);
613 properties.setSpecialDetection(specialDetection);
614 properties.setFrontCollisions(frontCollisions);
615 return properties;
616 }
617 }