1 package SocialSteeringsBeta;
2
3 import SteeringProperties.SteeringProperties;
4 import SteeringStuff.ISteering;
5 import SteeringStuff.RefBoolean;
6 import SteeringStuff.SteeringTools;
7 import cz.cuni.amis.pogamut.base3d.worldview.object.Location;
8 import cz.cuni.amis.pogamut.ut2004.bot.impl.UT2004Bot;
9 import java.util.ArrayList;
10 import javax.vecmath.Tuple3d;
11 import javax.vecmath.Vector3d;
12
13
14
15
16
17 public class TriangleSteer implements ISocialSteering {
18
19 protected UT2004Bot botself;
20 protected TriangleSteeringProperties properties;
21 private static final int KDefaultAttraction = 600;
22 private static final double K90deg = 90;
23 private static final double K360deg = 360;
24 private static final double K180deg = 180;
25 private static final double KMinimalDistance = 50;
26 private static final String KTowards = "towards";
27
28 public TriangleSteer(UT2004Bot botself) {
29 this.botself = botself;
30
31 }
32
33 @Override
34 public Vector3d run(Vector3d scaledActualVelocity, RefBoolean wantsToGoFaster, RefBoolean wantsToStop, Location focus) {
35
36 if (properties == null) {
37 if (SOC_STEER_LOG.DEBUG) {
38 SOC_STEER_LOG.AddLogLineWithDate("no properties", "triangleError");
39
40 }
41 }
42
43 Location newFocus = getFocus();
44 if(newFocus !=null)
45 {
46 focus.setTo(newFocus);
47 }
48
49
50
51 Location targetLocation = WhereToGo(botself, properties);
52
53
54 SteeringResult nextVelocity = new SteeringResult(new Vector3d(0, 0, 0),1);
55
56
57 if(targetLocation != null)
58 {
59
60
61
62 targetLocation.z = botself.getLocation().z;
63 Vector3d vectorToTarget = targetLocation.sub(botself.getLocation()).asVector3d();
64
65 double distFromTarget = vectorToTarget.length();
66 nextVelocity.setMult(distFromTarget/100);
67 if(distFromTarget < KMinimalDistance)
68 {
69 wantsToStop.setValue(true);
70 return new SteeringResult(new Vector3d(0, 0, 0),1);
71 }
72
73 double attractiveForce = KDefaultAttraction ;/
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125 private Location WhereToGo(UT2004Bot me, TriangleSteeringProperties propers) {
126 UT2004Bot first = propers.getFstBot();
127 UT2004Bot second = propers.getSndBot();
128
129
130
131
132 if (SOC_STEER_LOG.DEBUG) {
133 SOC_STEER_LOG.AddLogLineWithDate(me.getName() + " " + me.getLocation().toString() + " --------------------------------------------", "triangle");
134 if (propers == null) {
135 SOC_STEER_LOG.AddLogLineWithDate(me.getName() + "I have no properties!!!", "triangleError");
136 }
137 }
138
139
140 ArrayList<Location> edgePoints = new ArrayList<Location>();
141 Location S1 = first.getLocation();
142 Location S2 = second.getLocation();
143 int r1min = propers.getFstDistance().getMin();
144 int r1max = propers.getFstDistance().getMax();
145 int r2min = propers.getSndDistance().getMin();
146 int r2max = propers.getSndDistance().getMax();
147
148
149
150 if (isSuitable(me.getLocation(), S1, S2, propers, 0)) {
151 SOC_STEER_LOG.AddLogLineWithDate(me.getName() + "is in suitable place.", "triangle");
152 return me.getLocation();
153 }
154
155
156
157 double othersDistance = S1.getDistance2D(S2);
158 if (r1max + r2max < othersDistance) {
159 SOC_STEER_LOG.AddLogLineWithDate(me.getName() + "see: " + first.getName() + "to far from: " + second.getName(), "triangle");
160 float factor = (float) r1max / (float) (r1max + r2max);
161 return S1.add((S2.sub(S1)).scale(factor));
162 }
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186 double r3min = 0;
187 double r3max = 0;
188 for (int i = 0; i < 2; i++) {
189
190 if (SOC_STEER_LOG.DEBUG) {
191 if (propers == null) {
192 SOC_STEER_LOG.AddLogLine("nemam TP pro TS", SOC_STEER_LOG.KError);
193 }
194 if (propers.getAngle() == null) {
195 SOC_STEER_LOG.AddLogLine("nemam uhel v TP", SOC_STEER_LOG.KError);
196 }
197 }
198 double angle = i == 0 ? propers.getAngle().getMin() : propers.getAngle().getMax();
199 double gamma = angle < K90deg ? 2 * angle : K360deg - 2 * angle;
200 double alpha = (K180deg - gamma) / 2;
201 double s = S1.getDistance2D(S2);
202 double alphaRad = SteeringTools.degreesToRadians(alpha);
203 double cosAlpha = Math.cos(alphaRad);
204 double b = s / (2 * cosAlpha);
205 if (i == 0) {
206 r3min = b;
207 } else {
208 r3max = b;
209 }
210 }
211
212
213
214
215 Location S3max1 = null;
216 Location S3min1 = null;
217 Location S3max2 = null;
218 Location S3min2 = null;
219 Location[] tmp;
220
221 tmp = SteeringTools.commonPoints(S2, r3max, S1, r3max);
222 if (tmp[0] != null) S3max1 = tmp[0];
223 if (tmp[1] != null) S3max2 = tmp[1];
224 tmp = SteeringTools.commonPoints(S2, r3min, S1, r3min);
225 if (tmp[0] != null) S3min1 = tmp[0];
226 if (tmp[1] != null) S3min2 = tmp[1];
227
228
229
230
231 int edgeSize = edgePoints.size();
232
233 tmp = SteeringTools.commonPoints(S1, r1min, S2, r2min);
234 if (tmp[0] != null) edgePoints.add(tmp[0]);
235 if (tmp[1] != null) edgePoints.add(tmp[1]);
236
237 tmp = SteeringTools.commonPoints(S1, r1max, S2, r2min);
238 if (tmp[0] != null) edgePoints.add(tmp[0]);
239 if (tmp[1] != null) edgePoints.add(tmp[1]);
240
241 tmp = SteeringTools.commonPoints(S1, r1max, S2, r2max);
242 if (tmp[0] != null) edgePoints.add(tmp[0]);
243 if (tmp[1] != null) edgePoints.add(tmp[1]);
244
245 tmp = SteeringTools.commonPoints(S1, r1min, S2, r2max);
246 if (tmp[0] != null) edgePoints.add(tmp[0]);
247 if (tmp[1] != null) edgePoints.add(tmp[1]);
248
249 if (SOC_STEER_LOG.DEBUG) {
250 if (edgeSize == edgePoints.size()) {
251 SOC_STEER_LOG.AddLogLine(me.getName() + " no distance-circles cross", "triangle");
252 SOC_STEER_LOG.AddLogLine("- S1: " + S1 + " S2: " + S2 + " r1: " + r1min + "-" + r1max + " r2: " + r2min + "-" + r2max, "triangle");
253 }
254 }
255 edgeSize = edgePoints.size();
256
257
258
259
260
261 if (S3min1 != null) {
262 tmp = SteeringTools.commonPoints(S3min1, r3min, S2, r2max);
263 if (tmp[0] != null) edgePoints.add(tmp[0]);
264 if (tmp[1] != null) edgePoints.add(tmp[1]);
265
266 tmp = SteeringTools.commonPoints(S3min1, r3min, S2, r2min);
267 if (tmp[0] != null) edgePoints.add(tmp[0]);
268 if (tmp[1] != null) edgePoints.add(tmp[1]);
269
270 tmp = SteeringTools.commonPoints(S3min1, r3min, S1, r1min);
271 if (tmp[0] != null) edgePoints.add(tmp[0]);
272 if (tmp[1] != null) edgePoints.add(tmp[1]);
273
274 tmp = SteeringTools.commonPoints(S3min1, r3min, S1, r1max);
275 if (tmp[0] != null) edgePoints.add(tmp[0]);
276 if (tmp[1] != null) edgePoints.add(tmp[1]);
277
278 if (SOC_STEER_LOG.DEBUG) {
279 if (edgeSize == edgePoints.size()) {
280 SOC_STEER_LOG.AddLogLine(me.getName() + " no S3min1 cross", "triangle");
281 SOC_STEER_LOG.AddLogLine("- S3min1: " + S3min1 + " r3min: " + r3min + " r1: " + r1min + "-" + r1max + " r2: " + r2min + "-" + r2max, "triangle");
282 }
283 }
284 edgeSize = edgePoints.size();
285
286 } else {
287
288 if (SOC_STEER_LOG.DEBUG) {
289 SOC_STEER_LOG.AddLogLine(me.getName() + " no S3min1", "triangle");
290 }
291 }
292
293
294
295
296 if (S3max1 != null) {
297 tmp = SteeringTools.commonPoints(S3max1, r3max, S2, r2max);
298 if (tmp[0] != null) edgePoints.add(tmp[0]);
299 if (tmp[1] != null) edgePoints.add(tmp[1]);
300
301 tmp = SteeringTools.commonPoints(S3max1, r3max, S2, r2min);
302 if (tmp[0] != null) edgePoints.add(tmp[0]);
303 if (tmp[1] != null) edgePoints.add(tmp[1]);
304
305 tmp = SteeringTools.commonPoints(S3max1, r3max, S1, r1min);
306 if (tmp[0] != null) edgePoints.add(tmp[0]);
307 if (tmp[1] != null) edgePoints.add(tmp[1]);
308
309 tmp = SteeringTools.commonPoints(S3max1, r3max, S1, r1max);
310 if (tmp[0] != null) edgePoints.add(tmp[0]);
311 if (tmp[1] != null) edgePoints.add(tmp[1]);
312
313 if (SOC_STEER_LOG.DEBUG) {
314 if (edgeSize == edgePoints.size()) {
315 SOC_STEER_LOG.AddLogLine(me.getName() + " no S3max1 cross", "triangle");
316 SOC_STEER_LOG.AddLogLine("- S3max1: " + S3max1 + " r3max: " + r3max + " r1: " + r1min + "-" + r1max + " r2: " + r2min + "-" + r2max, "triangle");
317 }
318 }
319 edgeSize = edgePoints.size();
320
321 } else {
322
323 if (SOC_STEER_LOG.DEBUG) {
324 SOC_STEER_LOG.AddLogLine(me.getName() + " no S3max1", "triangle");
325 }
326 }
327
328
329
330
331 if (S3min2 != null) {
332 tmp = SteeringTools.commonPoints(S3min2, r3min, S2, r2max);
333 if (tmp[0] != null) edgePoints.add(tmp[0]);
334 if (tmp[1] != null) edgePoints.add(tmp[1]);
335
336 tmp = SteeringTools.commonPoints(S3min2, r3min, S2, r2min);
337 if (tmp[0] != null) edgePoints.add(tmp[0]);
338 if (tmp[1] != null) edgePoints.add(tmp[1]);
339
340 tmp = SteeringTools.commonPoints(S3min2, r3min, S1, r1min);
341 if (tmp[0] != null) edgePoints.add(tmp[0]);
342 if (tmp[1] != null) edgePoints.add(tmp[1]);
343
344 tmp = SteeringTools.commonPoints(S3min2, r3min, S1, r1max);
345 if (tmp[0] != null) edgePoints.add(tmp[0]);
346 if (tmp[1] != null) edgePoints.add(tmp[1]);
347
348 if (SOC_STEER_LOG.DEBUG) {
349 if (edgeSize == edgePoints.size()) {
350 SOC_STEER_LOG.AddLogLine(me.getName() + " no S3min2 cross", "triangle");
351 SOC_STEER_LOG.AddLogLine("- S3min2: " + S3min2 + " r3min: " + r3min + " r1: " + r1min + "-" + r1max + " r2: " + r2min + "-" + r2max, "triangle");
352 }
353 }
354 edgeSize = edgePoints.size();
355
356 } else {
357
358 if (SOC_STEER_LOG.DEBUG) {
359 SOC_STEER_LOG.AddLogLine(me.getName() + " no S3min2", "triangle");
360 }
361 }
362
363
364
365
366 if (S3max2 != null) {
367 tmp = SteeringTools.commonPoints(S3max2, r3max, S2, r2max);
368 if (tmp[0] != null) edgePoints.add(tmp[0]);
369 if (tmp[1] != null) edgePoints.add(tmp[1]);
370
371 tmp = SteeringTools.commonPoints(S3max2, r3max, S2, r2min);
372 if (tmp[0] != null) edgePoints.add(tmp[0]);
373 if (tmp[1] != null) edgePoints.add(tmp[1]);
374
375 tmp = SteeringTools.commonPoints(S3max2, r3max, S1, r1min);
376 if (tmp[0] != null) edgePoints.add(tmp[0]);
377 if (tmp[1] != null) edgePoints.add(tmp[1]);
378
379 tmp = SteeringTools.commonPoints(S3max2, r3max, S1, r1max);
380 if (tmp[0] != null) edgePoints.add(tmp[0]);
381 if (tmp[1] != null) edgePoints.add(tmp[1]);
382
383 if (SOC_STEER_LOG.DEBUG) {
384 if (edgeSize == edgePoints.size()) {
385 SOC_STEER_LOG.AddLogLine(me.getName() + " no S3max2 cross", "triangle");
386 SOC_STEER_LOG.AddLogLine("- S3max2: " + S3max2 + " r3max: " + r3max + " r1: " + r1min + "-" + r1max + " r2: " + r2min + "-" + r2max, "triangle");
387 }
388 }
389 edgeSize = edgePoints.size();
390
391 } else {
392
393 if (SOC_STEER_LOG.DEBUG) {
394 SOC_STEER_LOG.AddLogLine(me.getName() + " no S3max2", "triangle");
395 }
396 }
397
398
399
400
401 for (int i = 0; i < edgePoints.size(); i++) {
402 if (!isSuitable(edgePoints.get(i), S1, S2, propers, 8)) {
403 edgePoints.set(i, null);
404 }
405 }
406
407 Location result = calculateBest(edgePoints, me.getLocation(),propers);
408
409 if (SOC_STEER_LOG.DEBUG && result == null) {
410 SOC_STEER_LOG.AddLogLineWithDate(me.getName() + ": no suitable edge points from total of:" + String.valueOf(edgePoints.size()), "triangleError");
411 }
412 if (SOC_STEER_LOG.DEBUG && result != null) {
413 SOC_STEER_LOG.AddLogLine(me.getName() + ": has best suitable edge point:" + result.toString()
414 + "distance: " + String.valueOf(me.getLocation().getDistance2D(result)), "triangle");
415 }
416 if (result == null) {
417 if (Math.min(r1min, r2min) > Math.max(me.getLocation().getDistance2D(S2), me.getLocation().getDistance2D(S1))) {
418 SOC_STEER_LOG.AddLogLineWithDate(me.getName() + " is to close to, going away from both", "triangle");
419 Location mine = me.getLocation();
420 Location middle = S1.add((S2.sub(S1)).scale(1 / 2));
421 return mine.add(middle.sub(mine).scale(-1));
422 }
423 }
424
425 if (SOC_STEER_LOG.DEBUG) {
426 SOC_STEER_LOG.AddLogLine("--------------------------------------------", "triangle");
427 }
428
429 return result;
430
431 }
432
433
434
435
436 private static boolean isSuitable(Location point, Location fst, Location snd, TriangleSteeringProperties props, double tolerance) {
437 if (props.getFstDistance().in((int) point.getDistance2D(fst), tolerance)
438 && props.getSndDistance().in((int) point.getDistance2D(snd), tolerance)) {
439 double a = point.getDistance2D(fst);
440 double b = point.getDistance2D(snd);
441 double c = fst.getDistance2D(snd);
442 double gamma = Math.acos((c * c - a * a - b * b) / (-2 * a * b)) * (180 / Math.PI);
443 if (props.getAngle().in((int) gamma, tolerance / 5)) {
444
445 if (SOC_STEER_LOG.DEBUG) {
446
447 }
448
449 return true;
450
451 } else {
452 return false;
453 }
454 } else {
455 return false;
456 }
457 }
458
459
460
461
462
463 private static Location calculateBest(ArrayList<Location> edgePoints, Location me,TriangleSteeringProperties props) {
464
465
466 Location avgL = new Location(0,0,0);
467 Location avgR = new Location(0,0,0);
468 int countL = 0;
469 int countR = 0;
470 Vector3d divider = new Vector3d(props.getFstBot().getLocation().x - props.getSndBot().getLocation().x,
471 props.getFstBot().getLocation().y - props.getSndBot().getLocation().y,0
472 );
473 for(Location ll : edgePoints)
474 {
475 if(ll == null) continue;
476 Vector3d point = (ll.sub(props.getSndBot().getLocation())).asVector3d();
477 if(SteeringTools.pointIsLeftFromTheVector(point, divider))
478 {
479 avgL = avgL.add(ll);
480 countL++;
481 }else
482 {
483 avgR = avgR.add(ll);
484 countR++;
485 }
486
487 }
488 boolean Lsuit = false;
489 if (countL != 0) {
490 avgL = avgL.scale(1 / (double) countL);
491 if (isSuitable(avgL, props.getFstBot().getLocation(), props.getSndBot().getLocation(), props, 1)) {
492
493 if (SOC_STEER_LOG.DEBUG) {
494 SOC_STEER_LOG.AddLogLine("suitable těžiště L: " + avgL.toString(), "triangle");
495 }
496
497 Lsuit = true;
498 }
499 }
500 boolean Rsuit = false;
501 if (countR != 0) {
502 avgR = avgR.scale(1 / (double) countR);
503 if (isSuitable(avgR, props.getFstBot().getLocation(), props.getSndBot().getLocation(), props, 1)) {
504
505 if (SOC_STEER_LOG.DEBUG) {
506 SOC_STEER_LOG.AddLogLine("suitable těžiště R: " + avgR.toString(), "triangle");
507 }
508
509 Rsuit = true;
510 }
511
512 }
513
514 if((Lsuit && avgL.getDistance2D(me) < avgR.getDistance2D(me) && Rsuit ) || (Lsuit && !Rsuit))
515 {
516 return avgL;
517 }
518 else if((Rsuit && ! Lsuit) || (Rsuit && avgL.getDistance2D(me) >= avgR.getDistance2D(me) && Lsuit ))
519 {
520 return avgR;
521 }
522
523
524 Location best = null;
525 double min = Integer.MAX_VALUE;
526 for (Location p : edgePoints) {
527 if (p == null) continue;
528 double next = me.getDistance2D(p);
529 if (next < min) {
530 best = p;
531 min = next;
532 }
533 }
534 if(best == null)
535 {
536
537 if (SOC_STEER_LOG.DEBUG) {
538 SOC_STEER_LOG.AddLogLine("nenasli jsme vhodny bod, tzn s nami hybou uz jen pritazlivo odpudive sily vuci dalsim postavam ", "triangleError");
539 }
540
541 }
542 return best;
543
544
545 }
546
547
548
549
550
551 private Vector3d attraction(UT2004Bot botself, UT2004Bot other, double trashhold) {
552 trashhold = trashhold < 1 ? 1: trashhold;
553 double max = properties.getFstDistance().getMax();
554 double min = properties.getFstDistance().getMin();
555 double actual = botself.getLocation().getDistance2D(other.getLocation());
556 Vector3d result = new Vector3d(0,0,0);
557 if(actual > max * trashhold)
558 {
559 result = new Vector3d(other.getLocation().x - botself.getLocation().x,
560 other.getLocation().y - botself.getLocation().y, 0);
561 result.normalize();
562 result.scale(actual/max);
563
564 if (SOC_STEER_LOG.DEBUG) {
565 SOC_STEER_LOG.AddLogLine("!! " + botself.getName() + ": too far from " + other.getName() + " --strength " +result.length(), "triangle");
566 }
567
568 }
569 if(actual < min * trashhold)
570 {
571 result = new Vector3d(botself.getLocation().x - other.getLocation().x,
572 botself.getLocation().y - other.getLocation().y, 0);
573 result.normalize();
574 result.scale(min/actual);
575
576 if (SOC_STEER_LOG.DEBUG) {
577 SOC_STEER_LOG.AddLogLine("!! " + botself.getName() + ": too close to " + other.getName() + " --strength " +result.length(), "triangle");
578 }
579
580 }
581 return result;
582 }
583
584
585
586
587
588
589
590
591
592 private Location getFocus() {
593
594
595 if (properties.getHeadingType() == null) {
596 return null;
597 }
598 String headingType = properties.getHeadingType();
599 UT2004Bot fst = properties.getFstBot();
600 UT2004Bot snd = properties.getSndBot();
601 Interval headingValue = properties.getHeadingValue();
602 int fromFst = 0;
603 boolean headingToAgents = false;
604
605 if (headingType.compareTo(fst.getName().substring(0, 1)) == 0) {
606 fromFst = 100 - headingValue.avg();
607 headingToAgents = true;
608 } else if (headingType.compareTo(snd.getName().substring(0, 1)) == 0) {
609 fromFst = headingValue.avg();
610 headingToAgents = true;
611 }
612 if (headingToAgents) {
613 Location fstL = fst.getLocation();
614 Location sndL = snd.getLocation();
615
616 Location shift = (fstL.sub(sndL)).scale(((double) fromFst) / 100);
617 Location result = sndL.add(shift);
618 return result;
619 } else if (headingType.compareTo(KTowards) == 0) {
620 return null;
621 } else {
622 return null;
623 }
624
625
626 }
627
628 @Override
629 public SteeringResult runSocial(Vector3d scaledActualVelocity, RefBoolean wantsToGoFaster, RefBoolean wantsToStop, Location focus) {
630 return (SteeringResult)this.run(scaledActualVelocity, wantsToGoFaster, wantsToStop, focus);
631 }
632 }