1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package cz.cuni.amis.pogamut.ut2004.agent.navigation.navmesh;
18
19 import cz.cuni.amis.pogamut.base.agent.navigation.IPathFuture;
20 import cz.cuni.amis.pogamut.base.agent.navigation.IPathPlanner;
21 import cz.cuni.amis.pogamut.base.agent.navigation.impl.PrecomputedPathFuture;
22 import cz.cuni.amis.pogamut.base.communication.worldview.IWorldView;
23 import cz.cuni.amis.pogamut.base.communication.worldview.object.IWorldObjectEvent;
24 import cz.cuni.amis.pogamut.base.communication.worldview.object.IWorldObjectEventListener;
25 import cz.cuni.amis.pogamut.base.communication.worldview.object.WorldObjectId;
26 import cz.cuni.amis.pogamut.base.utils.logging.IAgentLogger;
27 import cz.cuni.amis.pogamut.base.utils.logging.LogCategory;
28 import cz.cuni.amis.pogamut.base.utils.math.DistanceUtils;
29 import cz.cuni.amis.pogamut.base3d.worldview.object.ILocated;
30 import cz.cuni.amis.pogamut.base3d.worldview.object.Location;
31 import cz.cuni.amis.pogamut.unreal.communication.messages.UnrealId;
32 import cz.cuni.amis.pogamut.ut2004.agent.module.sensor.NavigationGraphBuilder;
33 import cz.cuni.amis.pogamut.ut2004.agent.navigation.UT2004EdgeChecker;
34 import cz.cuni.amis.pogamut.ut2004.agent.navigation.floydwarshall.FloydWarshallMap;
35 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbcommands.DrawStayingDebugLines;
36 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.GameInfo;
37 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.NavPoint;
38 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbinfomessages.NavPointNeighbourLink;
39 import cz.cuni.amis.pogamut.ut2004.factory.guice.remoteagent.UT2004ServerFactory;
40 import cz.cuni.amis.pogamut.ut2004.factory.guice.remoteagent.UT2004ServerModule;
41 import cz.cuni.amis.pogamut.ut2004.server.impl.UT2004Server;
42 import cz.cuni.amis.pogamut.ut2004.utils.LinkFlag;
43 import cz.cuni.amis.pogamut.ut2004.utils.UT2004ServerRunner;
44 import cz.cuni.amis.utils.ExceptionToString;
45 import cz.cuni.amis.utils.NullCheck;
46 import cz.cuni.amis.utils.exception.PogamutException;
47 import java.io.*;
48 import java.util.*;
49 import java.util.logging.Level;
50 import java.util.logging.Logger;
51 import javax.vecmath.Vector2d;
52 import math.geom2d.Point2D;
53 import math.geom2d.Shape2D;
54 import math.geom2d.line.Line2D;
55 import math.geom2d.line.StraightLine2D;
56 import math.geom3d.Point3D;
57 import math.geom3d.Vector3D;
58 import math.geom3d.line.StraightLine3D;
59 import math.geom3d.plane.Plane3D;
60
61
62
63
64
65
66
67
68
69 public class NavMesh implements IPathPlanner<ILocated> {
70
71 private IWorldView worldView;
72 private Logger log;
73
74 private Random random;
75
76
77
78
79 private boolean loaded = false;
80 private GameInfo loadedForMap = null;
81
82
83
84
85 private ArrayList<double[]> verts = new ArrayList<double[]>();
86 private ArrayList<int[]> polys = new ArrayList<int[]>();
87 private ArrayList<ArrayList<Integer>> vertsToPolys;
88 private ArrayList<Boolean> safeVertex;
89 private NavMeshBSPNode bspTree;
90 private NavMeshBSPNode biggestLeafInTree;
91 private ArrayList<OffMeshPoint> offMeshPoints;
92 private ArrayList<ArrayList<OffMeshPoint>> polysToOffMeshPoints;
93 private Map<UnrealId, NavPoint> nps = null;
94
95
96
97
98 private FloydWarshallMap fwMap;
99 private boolean isFwMapAvailable = false;
100 private boolean hasTeleports = false;
101
102 public NavMesh(IWorldView worldView, Logger log) {
103 this.log = log;
104 if (this.log == null) {
105 this.log = new LogCategory("NavMesh");
106 }
107 random = new Random();
108
109 this.worldView = worldView;
110 NullCheck.check(worldView, "worldView");
111 }
112
113
114
115
116
117
118
119
120
121
122
123 public boolean isLoaded() {
124 return loaded;
125 }
126
127
128
129
130 public int polyCount() {
131 return polys.size();
132 }
133
134
135
136
137 public int vertCount() {
138 return verts.size();
139 }
140
141
142
143
144
145
146 public ArrayList<int[]> getPolys() {
147 return polys;
148 }
149
150
151
152
153
154 public ArrayList<double[]> getVerts() {
155 return verts;
156 }
157
158
159
160
161 public int[] getPolygon(int polygonId) {
162 int[] p = ((int[]) polys.get(polygonId)).clone();
163 return p;
164 }
165
166
167
168
169 public double[] getVertex(int vertexId) {
170 double[] v = ((double[]) verts.get(vertexId)).clone();
171 return v;
172 }
173
174
175
176
177 @SuppressWarnings("unchecked")
178 public ArrayList<Integer> getPolygonsByVertex(int vertexId) {
179 return (ArrayList<Integer>) (this.vertsToPolys.get(vertexId)).clone();
180 }
181
182
183
184
185 public ArrayList<Integer> getNeighbourIdsToPolygon(int polygonId) {
186
187 ArrayList<Integer> neighbours = new ArrayList<Integer>();
188 int[] p = this.getPolygon(polygonId);
189
190 for (int j = 0; j < p.length; j++) {
191 ArrayList<Integer> p2 = getPolygonsByVertex(p[j]);
192 for (int k = 0; k < p2.size(); k++) {
193 int candidateId = p2.get(k);
194
195
196 int secondVertex = p[((j == p.length - 1) ? 0 : j + 1)];
197 int[] candidatePolygon = this.getPolygon(candidateId);
198 for (int l = 0; l < candidatePolygon.length; l++) {
199 if (candidatePolygon[l] == secondVertex) {
200
201 if (!neighbours.contains(candidateId) && candidateId != polygonId) {
202 neighbours.add(candidateId);
203 }
204 break;
205 }
206 }
207
208 }
209 }
210 return neighbours;
211 }
212
213
214
215
216
217 public ArrayList<OffMeshPoint> getOffMeshPoints() {
218 return offMeshPoints;
219 }
220
221
222
223
224
225
226
227 public List<OffMeshPoint> getOffMeshPointsOnPolygon(int polygonId) {
228 return polysToOffMeshPoints.get(polygonId);
229 }
230
231
232
233
234
235
236 public int getNumberOfPolygonsInBiggestLeaf() {
237 if (biggestLeafInTree != null) {
238 return biggestLeafInTree.polys.size();
239 } else {
240 return -1;
241 }
242 }
243
244
245
246
247 public void setBiggestLeafInTree(NavMeshBSPNode node) {
248 biggestLeafInTree = node;
249 }
250
251
252
253
254
255
256 public int getPolygonId(Point3D point3D) {
257
258
259 Point2D point2D = new Point2D(point3D.getX(), point3D.getY());
260
261 NavMeshBSPNode node = bspTree;
262 while (!node.isLeaf()) {
263
264 StraightLine2D sepLine = node.sepLine;
265 double dist = sepLine.getSignedDistance(point2D);
266 if (dist < 0) {
267
268 node = node.left;
269 }
270 if (dist > 0) {
271
272 node = node.right;
273 }
274 if (dist == 0) {
275
276 point2D = new Point2D(point3D.getX() + random.nextDouble() - 0.5, point3D.getY() + random.nextDouble() - 0.5);
277 }
278 }
279
280
281
282
283 ArrayList<Integer> candidatePolygons = new ArrayList<Integer>();
284
285
286 for (int i = 0; i < node.polys.size(); i++) {
287 Integer pId = (Integer) node.polys.get(i);
288
289 if (polygonContainsPoint(pId, point2D)) {
290
291 candidatePolygons.add(pId);
292 } else {
293
294 }
295 }
296
297
298
299
300 if (candidatePolygons.isEmpty()) {
301 return -1;
302 }
303
304
305
306 double minDist = NavMeshConstants.maxDistanceBotPolygon;
307 int retPId = -2;
308 for (int i = 0; i < candidatePolygons.size(); i++) {
309 Integer pId = (Integer) candidatePolygons.get(i);
310 int[] polygon = getPolygon(pId);
311
312
313 double[] v1 = getVertex(polygon[0]);
314 double[] v2 = getVertex(polygon[1]);
315 double[] v3 = getVertex(polygon[2]);
316 Point3D p1 = new Point3D(v1[0], v1[1], v1[2]);
317 Vector3D vector1 = new Vector3D(v2[0] - v1[0], v2[1] - v1[1], v2[2] - v1[2]);
318 Vector3D vector2 = new Vector3D(v3[0] - v1[0], v3[1] - v1[1], v3[2] - v1[2]);
319 Plane3D plane = new Plane3D(p1, vector1, vector2);
320 double dist = plane.getDistance(point3D);
321 if (dist < minDist) {
322
323
324
325
326
327 if (polygon.length > 3) {
328 v1 = getVertex(polygon[0]);
329 v2 = getVertex(polygon[1]);
330 v3 = getVertex(polygon[3]);
331 p1 = new Point3D(v1[0], v1[1], v1[2]);
332 vector1 = new Vector3D(v2[0] - v1[0], v2[1] - v1[1], v2[2] - v1[2]);
333 vector2 = new Vector3D(v3[0] - v1[0], v3[1] - v1[1], v3[2] - v1[2]);
334 plane = new Plane3D(p1, vector1, vector2);
335 dist = plane.getDistance(point3D);
336 if (dist < minDist) {
337
338
339
340 if (polygon.length > 4) {
341 v1 = getVertex(polygon[0]);
342 v2 = getVertex(polygon[2]);
343 v3 = getVertex(polygon[4]);
344 p1 = new Point3D(v1[0], v1[1], v1[2]);
345 vector1 = new Vector3D(v2[0] - v1[0], v2[1] - v1[1], v2[2] - v1[2]);
346 vector2 = new Vector3D(v3[0] - v1[0], v3[1] - v1[1], v3[2] - v1[2]);
347 plane = new Plane3D(p1, vector1, vector2);
348 dist = plane.getDistance(point3D);
349 if (dist < minDist) {
350 retPId = pId;
351 minDist = dist;
352 } else {
353
354 continue;
355 }
356 } else {
357
358 retPId = pId;
359 minDist = dist;
360 }
361 } else {
362
363 continue;
364 }
365 } else {
366
367 retPId = pId;
368 minDist = dist;
369 }
370
371 }
372 }
373 log.fine("Distance of a point from polygon " + retPId + " is " + minDist);
374 return retPId;
375 }
376
377
378
379
380
381
382
383 public int getPolygonId(Location location) {
384 return getPolygonId(new Point3D(location.x, location.y, location.z));
385 }
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400 public boolean load(GameInfo info, boolean shouldReloadNavMesh) {
401 if (info == null) {
402 log.severe("Could not load for 'null' GameInfo!");
403 return false;
404 }
405 if (loaded) {
406 if (loadedForMap == null) {
407
408 clear();
409 } else {
410 if (loadedForMap.getLevel().equals(info.getLevel())) {
411
412 return true;
413 }
414 }
415 }
416
417
418 String mapName = info.getLevel();
419 log.warning("Loading NavMesh for: " + mapName);
420
421 String processedNavMeshFileName = NavMeshConstants.processedMeshDir + "/" + mapName + ".navmesh.processed";
422 File processedNavMeshFile = new File(processedNavMeshFileName);
423
424 String pureMeshFileName = NavMeshConstants.pureMeshReadDir + "/" + mapName + ".navmesh";
425 File pureMeshFile = new File(pureMeshFileName);
426
427 if (shouldReloadNavMesh) {
428
429 if (processedNavMeshFile.exists() && processedNavMeshFile.isFile()) {
430 if (pureMeshFile.exists() && pureMeshFile.isFile()) {
431 log.warning("Going to RELOAD / REPROCESS navmesh.");
432 log.warning("Deleting old processed navmesh file: " + processedNavMeshFile.getAbsolutePath());
433 processedNavMeshFile.delete();
434 } else {
435 log.warning("NavMesh RELOAD flag true, but .navmesh file for a given map not found (at " + pureMeshFile.getAbsolutePath() + "), going to load .navmesh.processed file.");
436 }
437 }
438 }
439
440
441 try {
442 if (!processedNavMeshFile.exists()) {
443 if (!shouldReloadNavMesh) {
444 log.warning("Processed NavMesh does not exist at: " + processedNavMeshFile.getAbsolutePath());
445 }
446 } else {
447
448 loadNavMeshFromCoreFile(processedNavMeshFile);
449
450 log.warning("NavMesh LOADED SUCCESSFULLY.");
451
452 loaded = true;
453 loadedForMap = info;
454 return true;
455 }
456 } catch (Exception e) {
457 log.warning(ExceptionToString.process("NavMesh could not be loaded from previously stored binary file: " + processedNavMeshFile.getAbsolutePath(), e));
458 }
459
460
461 try {
462 if (!pureMeshFile.exists()) {
463 log.warning("NavMesh source text file does not exist at: " + pureMeshFile.getAbsolutePath());
464 log.severe("NavMesh COULD NOT INITIALIZE FOR MAP: " + mapName);
465 return false;
466
467 } else {
468
469
470
471 loadSourceFile(pureMeshFile);
472 }
473 } catch (Exception e) {
474 log.warning(ExceptionToString.process("NavMesh could not be loaded from source text file: " + pureMeshFile.getAbsolutePath(), e));
475 log.severe("NavMesh COULD NOT HAVE BEEN INITIALIZED FOR MAP: " + mapName);
476 return false;
477 }
478
479
480 resetVertsToPolys();
481
482
483 resetSafeVerts();
484
485
486 resetBSPTree();
487
488
489 if (!eliminateUnreachablePolygons()) {
490 return false;
491 }
492
493
494 resetOffMeshConnections();
495
496
497 saveNavMeshCore(mapName);
498
499 loaded = true;
500 loadedForMap = info;
501 return true;
502 }
503
504 protected void loadNavMeshFromCoreFile(File processedNavMeshFile) throws FileNotFoundException, IOException, ClassNotFoundException {
505 log.warning("Loading previously stored NavMesh from binary file: " + processedNavMeshFile.getAbsolutePath());
506
507 ObjectInputStream in = new ObjectInputStream(new FileInputStream(processedNavMeshFile));
508 NavMeshCore core = (NavMeshCore) in.readObject();
509 this.biggestLeafInTree = core.biggestLeafInTree;
510 this.bspTree = core.bspTree;
511 this.polys = core.polys;
512 this.verts = core.verts;
513 this.vertsToPolys = core.vertsToPolys;
514 this.offMeshPoints = core.offMeshPoints;
515 this.polysToOffMeshPoints = core.polysToOffMeshPoints;
516 this.safeVertex = core.safeVertex;
517 }
518
519 protected void loadSourceFile(File pureMeshFile) throws NumberFormatException, IOException {
520 log.warning("Loading NavMesh from text file: " + pureMeshFile.getAbsolutePath());
521
522 BufferedReader br = new BufferedReader(new FileReader(pureMeshFile));
523 String line;
524
525 while ((line = br.readLine()) != null) {
526 String[] toks = line.split("[ \\t]");
527
528 if (toks[0].equals("v")) {
529 double[] v = new double[3];
530 v[0] = Double.parseDouble(toks[1]);
531 v[1] = Double.parseDouble(toks[2]);
532 v[2] = Double.parseDouble(toks[3]);
533 verts.add(v);
534 }
535 if (toks[0].equals("p")) {
536 int[] p = new int[toks.length - 1];
537 for (int i = 0; i < toks.length - 1; i++) {
538 p[i] = Integer.parseInt(toks[i + 1]);
539 }
540 polys.add(p);
541 }
542 }
543 }
544
545
546
547
548 protected void resetVertsToPolys() {
549 log.info("Setting vertsToPolys mapping array...");
550 vertsToPolys = new ArrayList<ArrayList<Integer>>();
551 for (int i = 0; i < verts.size(); i++) {
552 vertsToPolys.add(new ArrayList<Integer>());
553 }
554 for (int i = 0; i < polys.size(); i++) {
555 int[] p = (int[]) polys.get(i);
556 for (int j = 0; j < p.length; j++) {
557 ArrayList<Integer> p2 = (ArrayList<Integer>) vertsToPolys.get(p[j]);
558 if (!p2.contains(i)) {
559 p2.add(i);
560 }
561 }
562 }
563 }
564
565
566
567
568
569 protected void resetSafeVerts() {
570 log.info("Setting safe vertices...");
571 safeVertex = new ArrayList<Boolean>();
572 int safeCount = 0;
573
574
575 for (int v1 = 0; v1 < verts.size(); v1++) {
576 log.fine("Looking at vertex " + v1 + "...");
577
578
579 double sum = 0;
580
581 ArrayList<Integer> polys = vertsToPolys.get(v1);
582
583 for (Integer pId : polys) {
584
585
586 int[] polygon = getPolygon(pId);
587
588 int v0 = -1, v2 = -1;
589 for (int i = 0; i < polygon.length; i++) {
590 if (polygon[i] == v1) {
591 v0 = (i == 0 ? polygon[polygon.length - 1] : polygon[i - 1]);
592 v2 = (i == polygon.length - 1 ? polygon[0] : polygon[i + 1]);
593 break;
594 }
595 }
596
597 double[] vv0 = getVertex(v0);
598 double[] vv1 = getVertex(v1);
599 double[] vv2 = getVertex(v2);
600
601 double a = Math.sqrt((vv1[0] - vv0[0]) * (vv1[0] - vv0[0]) + (vv1[1] - vv0[1]) * (vv1[1] - vv0[1]) + (vv1[2] - vv0[2]) * (vv1[2] - vv0[2]));
602 double b = Math.sqrt((vv2[0] - vv1[0]) * (vv2[0] - vv1[0]) + (vv2[1] - vv1[1]) * (vv2[1] - vv1[1]) + (vv2[2] - vv1[2]) * (vv2[2] - vv1[2]));
603 double c = Math.sqrt((vv2[0] - vv0[0]) * (vv2[0] - vv0[0]) + (vv2[1] - vv0[1]) * (vv2[1] - vv0[1]) + (vv2[2] - vv0[2]) * (vv2[2] - vv0[2]));
604
605 double gama = Math.acos((a * a + b * b - c * c) / (2 * a * b));
606 log.fine("Angle gama is " + gama);
607
608 sum += gama;
609 }
610 log.fine("Sum angle is " + sum);
611
612 if (sum >= 2 * 3.14) {
613 safeVertex.add(v1, true);
614 safeCount++;
615 } else {
616 safeVertex.add(v1, false);
617 }
618 }
619 log.info("There are " + safeCount + " safe and " + (verts.size() + safeCount) + " unsafe vertices.");
620 }
621
622
623
624
625 protected void resetBSPTree() {
626 log.info("Creating BSP tree...");
627 bspTree = new NavMeshBSPNode(this, null);
628 for (int i = 0; i < polys.size(); i++) {
629 bspTree.polys.add(i);
630 }
631 biggestLeafInTree = null;
632 bspTree.build();
633 log.info("BSP tree for NavMesh polygons has been built. Biggest leaf has " + biggestLeafInTree.polys.size() + " polygons.");
634 }
635
636
637
638
639
640
641 protected boolean eliminateUnreachablePolygons() {
642 log.info("eliminateUnreachablePolygons() starts...");
643
644
645 Map<UnrealId, NavPoint> navPoints = nps;
646 if (navPoints == null) {
647 navPoints = (Map<UnrealId, NavPoint>) (Map) worldView.getAll(NavPoint.class);
648 }
649
650 if (navPoints == null || navPoints.size() == 0) {
651
652
653 log.warning("There are no navpoints present within the worldview, could not eliminateUnreachablePolygons() ...");
654 return false;
655 }
656
657
658 boolean[] reachable = new boolean[polys.size()];
659
660 log.info("Marking reachable polygons...");
661 for (NavPoint navPoint : navPoints.values()) {
662 Point3D point3D = navPoint.getLocation().asPoint3D();
663 int pId = getPolygonId(point3D);
664 if (pId < 0) {
665 continue;
666 }
667 markAsReachableRecursive(pId, reachable);
668 }
669
670
671 int reachableCount = 0;
672 int polyDelCount = 0;
673 int vertDelCount = 0;
674 for (int i = 0; i < polys.size(); i++) {
675 if (reachable[i]) {
676 reachableCount++;
677 }
678 }
679
680 if (polys.size() == reachableCount) {
681 log.warning("Marking complete. All " + reachableCount + " polygons are reachable, no need to delete anything.");
682 return true;
683 }
684
685 log.warning("Marking complete. There are " + reachableCount + " reachable polygons and " + (polys.size() - reachableCount) + " unreachable polygons.");
686
687 log.warning("Deleting unreachable polygons...");
688 for (int i = polys.size() - 1; i >= 0; i--) {
689 if (!reachable[i]) {
690 polys.remove(i);
691 polyDelCount++;
692 }
693 }
694
695 resetVertsToPolys();
696
697 log.warning("Deleting unused vertices...");
698 for (int i = vertsToPolys.size() - 1; i >= 0; i--) {
699 ArrayList<Integer> polygons = (ArrayList<Integer>) vertsToPolys.get(i);
700 if (polygons.isEmpty()) {
701 verts.remove(i);
702 vertDelCount++;
703
704 for (int j = 0; j < polys.size(); j++) {
705 int[] polygon = (int[]) polys.get(j);
706 for (int k = 0; k < polygon.length; k++) {
707 if (polygon[k] > i) {
708 polygon[k]--;
709 }
710 }
711 }
712 }
713 }
714
715 log.warning("Deleting done. " + polyDelCount + " polygons and " + vertDelCount + " vertices were deleted.");
716
717
718 resetVertsToPolys();
719 resetSafeVerts();
720 resetBSPTree();
721
722 return true;
723 }
724
725
726
727
728
729
730
731 private void markAsReachableRecursive(int pId, boolean[] reachable) {
732 if (reachable[pId]) {
733 return;
734 }
735 reachable[pId] = true;
736 ArrayList neighbours = getNeighbourIdsToPolygon(pId);
737 for (int i = 0; i < neighbours.size(); i++) {
738 markAsReachableRecursive((Integer) neighbours.get(i), reachable);
739 }
740 }
741
742
743
744
745
746
747
748
749 protected void resetOffMeshConnections() {
750 log.info("Creating off-mesh connections...");
751
752 Map<UnrealId, OffMeshPoint> offPoints = new HashMap<UnrealId, OffMeshPoint>();
753
754
755 Map<UnrealId, NavPoint> navPoints = nps;
756 if (navPoints == null) {
757 navPoints = (Map<UnrealId, NavPoint>) (Map) worldView.getAll(NavPoint.class);
758 }
759
760
761
762
763
764 for (Map.Entry<UnrealId, NavPoint> entry : navPoints.entrySet()) {
765 NavPoint np = entry.getValue();
766 UnrealId uId = entry.getKey();
767 int pId = this.getPolygonId(np.getLocation().asPoint3D());
768
769
770 if (np.isLiftCenter()) {
771 pId = -1;
772 }
773 OffMeshPoint op = new OffMeshPoint(np, pId);
774 offPoints.put(uId, op);
775 }
776
777
778
779 for (Map.Entry<UnrealId, NavPoint> entry : navPoints.entrySet()) {
780 NavPoint np = entry.getValue();
781 UnrealId uId = entry.getKey();
782
783
784 for (Map.Entry<UnrealId, NavPointNeighbourLink> entry2 : np.getOutgoingEdges().entrySet()) {
785 NavPointNeighbourLink link = entry2.getValue();
786 UnrealId uId2 = entry2.getKey();
787
788 NavPoint target = link.getToNavPoint();
789
790 log.fine("Checking edge from navpoint " + uId + " to navpoint " + target.getId() + "...");
791
792
793
794
795
796 boolean forceIgnore = false;
797 boolean forceAdd = false;
798 boolean addThisEdge = false;
799
800 if (np.isLiftCenter()) {
801 forceAdd = true;
802 }
803 if (target.isLiftCenter()) {
804 forceAdd = true;
805 }
806
807
808 forceIgnore = !UT2004EdgeChecker.checkLink(link);
809
810 if (!forceAdd && !forceIgnore) {
811
812
813 Line2D linkAsLine2D = new Line2D(link.getFromNavPoint().getLocation().x, link.getFromNavPoint().getLocation().y, link.getToNavPoint().getLocation().x, link.getToNavPoint().getLocation().y);
814
815
816
817
818
819 int currentPolygonId = getPolygonId(link.getFromNavPoint().getLocation().asPoint3D());
820 int targetPolygonId = this.getPolygonId(link.getToNavPoint().getLocation().asPoint3D());
821 int tabooPolygon = -1;
822 while (currentPolygonId >= 0 && currentPolygonId != targetPolygonId) {
823 int newPolygon = -1;
824
825
826 List<Integer> neighbours = this.getNeighbourIdsToPolygon(currentPolygonId);
827 for (Integer neighbour : neighbours) {
828 if (neighbour.intValue() == tabooPolygon) {
829 continue;
830 }
831
832
833 Line2D sharedEdge = null;
834 int[] polygon1 = getPolygon(currentPolygonId);
835 int[] polygon2 = getPolygon(neighbour);
836 for (int i = 0; i < polygon1.length; i++) {
837 int v1 = polygon1[i];
838 int v2 = polygon1[((i == polygon1.length - 1) ? 0 : i + 1)];
839
840 boolean containsV1 = false, containsV2 = false;
841 for (int j = 0; j < polygon2.length; j++) {
842 if (polygon2[j] == v1) {
843 containsV1 = true;
844 }
845 if (polygon2[j] == v2) {
846 containsV2 = true;
847 }
848 }
849 if (containsV1 && containsV2) {
850 double[] vertex1 = this.getVertex(v1);
851 double[] vertex2 = this.getVertex(v2);
852 sharedEdge = new Line2D(vertex1[0], vertex1[1], vertex2[0], vertex2[1]);
853 }
854 }
855
856
857 if (sharedEdge == null) {
858 log.severe("Shared edge between polygon " + currentPolygonId + " and " + neighbour + " was not found!");
859 }
860
861
862 if (linkAsLine2D.getIntersection(sharedEdge) != null) {
863 log.fine("Crossed a line into polygon " + neighbour);
864 tabooPolygon = currentPolygonId;
865 newPolygon = neighbour;
866 break;
867 }
868 }
869 currentPolygonId = newPolygon;
870 }
871
872
873 if (currentPolygonId >= 0) {
874
875 addThisEdge = false;
876 } else {
877
878 addThisEdge = true;
879 }
880 }
881
882 else {
883
884 if (forceAdd) {
885 addThisEdge = true;
886 }
887 if (forceIgnore) {
888 addThisEdge = false;
889 }
890 }
891
892
893 if (addThisEdge) {
894 log.fine("This edge is off-mesh: " + uId.getStringId() + " -> " + target.getId().getStringId());
895 OffMeshPoint op1 = offPoints.get(uId);
896 OffMeshPoint op2 = offPoints.get(target.getId());
897 OffMeshEdge oe = new OffMeshEdge(op1, op2, link);
898 op1.getOutgoingEdges().add(oe);
899 op2.getIncomingEdges().add(oe);
900 } else {
901 log.finer("This edge is not off-mesh.");
902 }
903 }
904 }
905
906
907 offMeshPoints = new ArrayList<OffMeshPoint>();
908 int offCount = 0;
909 for (OffMeshPoint op : offPoints.values()) {
910 if (op.getOutgoingEdges().isEmpty() && op.getIncomingEdges().isEmpty()) {
911
912 } else {
913 offMeshPoints.add(op);
914 offCount++;
915 }
916 }
917 log.warning("We found " + offCount + " offMeshPoints from total of " + offPoints.size() + " NavPoints.");
918
919
920 polysToOffMeshPoints = new ArrayList<ArrayList<OffMeshPoint>>();
921 for (int i = 0; i < polys.size(); i++) {
922 polysToOffMeshPoints.add(new ArrayList<OffMeshPoint>());
923 }
924 for (OffMeshPoint op : offMeshPoints) {
925 int pId = op.getPId();
926 if (pId >= 0) {
927 polysToOffMeshPoints.get(pId).add(op);
928 }
929 }
930
931 log.info("Off-mesh connections done.");
932 }
933
934 protected void saveNavMeshCore(String mapName) {
935 String coreFileName = NavMeshConstants.processedMeshDir + "\\" + mapName + ".navmesh.processed";
936 File coreFile = new File(coreFileName);
937
938 log.info("Writing NavMesh core to a file: " + coreFile.getAbsolutePath());
939
940 if (coreFile.exists()) {
941 log.warning("NavMesh core file exist, rewriting: " + coreFile.getAbsolutePath());
942 coreFile.delete();
943 }
944
945 coreFile.getParentFile().mkdirs();
946
947 try {
948 NavMeshCore core = new NavMeshCore();
949 core.biggestLeafInTree = this.biggestLeafInTree;
950 core.bspTree = this.bspTree;
951 core.polys = this.polys;
952 core.verts = this.verts;
953 core.vertsToPolys = this.vertsToPolys;
954 core.offMeshPoints = this.offMeshPoints;
955 core.polysToOffMeshPoints = this.polysToOffMeshPoints;
956 core.safeVertex = this.safeVertex;
957 ObjectOutputStream out = new ObjectOutputStream(new FileOutputStream(coreFile));
958 out.writeObject(core);
959 log.warning("NavMesh core binary file saved at: " + coreFile.getAbsolutePath());
960 } catch (Exception e) {
961 log.severe(ExceptionToString.process("Failed to write/serialize NavMesh core file to disk.", e));
962 }
963 }
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979 private boolean polygonContainsPoint(Integer pId, Point2D point2D) {
980 boolean result = true;
981 double rightSide = 0.0;
982
983 int[] polygon = getPolygon(pId);
984 double[] v1, v2;
985 for (int i = 0; i < polygon.length; i++) {
986 v1 = getVertex(polygon[i]);
987 if (i < polygon.length - 1) {
988 v2 = getVertex(polygon[i + 1]);
989 } else {
990 v2 = getVertex(polygon[0]);
991 }
992 Point2D p1 = new Point2D(v1[0], v1[1]);
993 Point2D p2 = new Point2D(v2[0], v2[1]);
994 StraightLine2D line = new StraightLine2D(p1, p2);
995 double dist = line.getSignedDistance(point2D);
996
997 if (rightSide == 0.0) {
998 rightSide = Math.signum(dist);
999 } else {
1000 if (rightSide * dist < 0) {
1001 result = false;
1002 break;
1003 }
1004 }
1005 }
1006 return result;
1007 }
1008
1009
1010
1011
1012
1013
1014
1015
1016
1017
1018
1019
1020
1021
1022
1023 @Override
1024 public IPathFuture<ILocated> computePath(ILocated from, ILocated to) {
1025 return new PrecomputedPathFuture<ILocated>(from, to, getPath(from, to));
1026 }
1027
1028 @Override
1029 public double getDistance(ILocated from, ILocated to) {
1030 IPathFuture<ILocated> path = computePath(from, to);
1031 if (path.isDone()) {
1032 List<ILocated> list = path.get();
1033 if (list.size() == 0) return Double.POSITIVE_INFINITY;
1034 ILocated location = list.get(0);
1035 double result = 0;
1036 for (int i = 1; i < list.size(); ++i) {
1037 ILocated next = list.get(i);
1038 result += location.getLocation().getDistance(next.getLocation());
1039 location = next;
1040 }
1041 return result;
1042 } else {
1043 return Double.POSITIVE_INFINITY;
1044 }
1045 }
1046
1047
1048
1049
1050
1051
1052
1053
1054
1055 public List<INavMeshAtom> getPolygonPath(INavMeshAtom fromAtom, INavMeshAtom toAtom) {
1056
1057 List<AStarNode> pickable = new ArrayList<AStarNode>();
1058
1059 List<AStarNode> expanded = new ArrayList<AStarNode>();
1060 AStarNode firstNode = new AStarNode(null, fromAtom, this, fromAtom, toAtom);
1061 pickable.add(firstNode);
1062
1063
1064 AStarNode targetNode = null;
1065
1066
1067 if (fromAtom.equals(toAtom)) {
1068 targetNode = firstNode;
1069 }
1070
1071 while (targetNode == null) {
1072
1073
1074 if (pickable.isEmpty()) {
1075 return null;
1076 }
1077
1078
1079
1080 AStarNode best = pickable.get(0);
1081 for (AStarNode node : pickable) {
1082 if (node.getEstimatedTotalDistance() < best.getEstimatedTotalDistance()) {
1083 best = node;
1084 }
1085 }
1086
1087
1088 List<INavMeshAtom> neighbours = best.getAtom().getNeighbours(this);
1089 for (INavMeshAtom atom : neighbours) {
1090 boolean add = true;
1091
1092
1093 for (AStarNode expNode : expanded) {
1094 if (expNode.getAtom().equals(atom)) {
1095 add = false;
1096 }
1097 }
1098
1099 if (add) {
1100 AStarNode newNode = new AStarNode(best, atom, this, fromAtom, toAtom);
1101 best.getFollowers().add(newNode);
1102 pickable.add(newNode);
1103
1104 if (atom.equals(toAtom)) {
1105 targetNode = newNode;
1106 }
1107 }
1108 }
1109
1110 pickable.remove(best);
1111 expanded.add(best);
1112 }
1113
1114
1115 List<INavMeshAtom> path = new ArrayList<INavMeshAtom>();
1116 AStarNode node = targetNode;
1117 while (node != null) {
1118 path.add(node.getAtom());
1119 node = node.getFrom();
1120 }
1121 Collections.reverse(path);
1122 return path;
1123 }
1124
1125
1126
1127
1128
1129
1130
1131
1132
1133 public List<INavMeshAtom> getPolygonPath(Location from, Location to) {
1134 INavMeshAtom fromAtom = getNearestAtom(from);
1135 INavMeshAtom toAtom = getNearestAtom(to);
1136 return getPolygonPath(fromAtom, toAtom);
1137 }
1138
1139
1140
1141
1142
1143
1144
1145
1146
1147
1148
1149
1150
1151
1152
1153
1154
1155
1156 private List<ILocated> getPolygonCentrePath(ILocated from, ILocated to, List<INavMeshAtom> polygonPath) {
1157 List path = new ArrayList<ILocated>();
1158 path.add(from);
1159 INavMeshAtom lastAtom = null;
1160 for (INavMeshAtom atom : polygonPath) {
1161
1162 if (atom.getClass() == OffMeshPoint.class) {
1163 NavPoint np = (NavPoint) worldView.get(((OffMeshPoint) atom).getNavPointId());
1164 path.add(np);
1165 } else {
1166
1167 NavMeshPolygon polygon = (NavMeshPolygon) atom;
1168
1169 if (lastAtom == null || lastAtom.getClass() == OffMeshPoint.class) {
1170 OffMeshPoint op = (OffMeshPoint) lastAtom;
1171
1172
1173 boolean inside;
1174 if (lastAtom == null) {
1175 inside = polygonContainsPoint(polygon.getPolygonId(), new Point2D(from.getLocation().x, from.getLocation().y));
1176 } else {
1177 inside = false;
1178 List<OffMeshPoint> offPs = polysToOffMeshPoints.get(polygon.getPolygonId());
1179 for (OffMeshPoint op2 : offPs) {
1180 if (op2.equals(op)) {
1181 inside = true;
1182 }
1183 }
1184 }
1185
1186 if (inside) {
1187
1188 } else {
1189
1190 path.add(getLocation(atom));
1191 }
1192 }
1193 else {
1194
1195 NavMeshPolygon polygon2 = (NavMeshPolygon) lastAtom;
1196 int[] p1 = this.getPolygon(polygon.getPolygonId());
1197 int[] p2 = this.getPolygon(polygon2.getPolygonId());
1198 int v1 = -1;
1199 int v2 = -1;
1200 outer:
1201 for (int i = 0; i < p1.length; i++) {
1202 for (int j = 0; j < p2.length; j++) {
1203 if (p1[i] == p2[j]) {
1204 if (v1 == -1) {
1205 v1 = p1[i];
1206 } else {
1207 if (p1[i] != v1) {
1208 v2 = p1[i];
1209 }
1210 break outer;
1211 }
1212 }
1213 }
1214 }
1215 double[] vv1 = getVertex(v1);
1216 double[] vv2 = getVertex(v2);
1217 path.add(new Location((vv1[0] + vv2[0]) / 2, (vv1[1] + vv2[1]) / 2, (vv1[2] + vv2[2]) / 2 + NavMeshConstants.liftPolygonLocation));
1218 }
1219 }
1220 lastAtom = atom;
1221 }
1222 path.add(to);
1223 return path;
1224 }
1225
1226
1227
1228
1229
1230
1231
1232
1233
1234 private List<ILocated> getFunneledPath(ILocated from, ILocated to, List<INavMeshAtom> polygonPath) {
1235 List<ILocated> path = new ArrayList<ILocated>();
1236 path.add(from);
1237 INavMeshAtom lastAtom = null;
1238 INavMeshAtom atom = null;
1239 int index = -1;
1240 while (index < polygonPath.size() - 1) {
1241 index++;
1242 if (index > 0) {
1243 lastAtom = polygonPath.get(index - 1);
1244 } else {
1245 lastAtom = null;
1246 }
1247 atom = polygonPath.get(index);
1248
1249
1250 if (atom.getClass() == OffMeshPoint.class) {
1251 NavPoint np = (NavPoint) worldView.get(((OffMeshPoint) atom).getNavPointId());
1252 path.add(np);
1253 }
1254 else {
1255
1256 NavMeshPolygon polygon = (NavMeshPolygon) atom;
1257
1258 if (lastAtom == null || lastAtom.getClass() == OffMeshPoint.class) {
1259 OffMeshPoint op = (OffMeshPoint) lastAtom;
1260
1261
1262 boolean inside;
1263 if (lastAtom == null) {
1264 inside = polygonContainsPoint(polygon.getPolygonId(), new Point2D(from.getLocation().x, from.getLocation().y));
1265 } else {
1266 inside = false;
1267 List<OffMeshPoint> offPs = polysToOffMeshPoints.get(polygon.getPolygonId());
1268 for (OffMeshPoint op2 : offPs) {
1269 if (op2.equals(op)) {
1270 inside = true;
1271 }
1272 }
1273 }
1274
1275 if (inside) {
1276
1277 } else {
1278
1279 path.add(getLocation(atom));
1280 }
1281 }
1282 else {
1283
1284
1285
1286 ILocated start = path.get(path.size() - 1);
1287
1288
1289 NavMeshPolygon polygon2 = (NavMeshPolygon) lastAtom;
1290 int[] p1 = this.getPolygon(polygon.getPolygonId());
1291 int[] p2 = this.getPolygon(polygon2.getPolygonId());
1292 int v1 = -1;
1293 int v2 = -1;
1294 outer:
1295 for (int i = 0; i < p1.length; i++) {
1296 for (int j = 0; j < p2.length; j++) {
1297 if (p1[i] == p2[j]) {
1298 if (v1 == -1) {
1299 v1 = p1[i];
1300 } else {
1301 if (p1[i] != v1) {
1302 v2 = p1[i];
1303 }
1304 break outer;
1305 }
1306 }
1307 }
1308 }
1309 double[] vv1 = getVertex(v1);
1310 double[] vv2 = getVertex(v2);
1311 Line2D gateway = new Line2D(vv1[0], vv1[1], vv2[0], vv2[1]);
1312
1313
1314 if ((start.getLocation().x == vv2[0] && start.getLocation().y == vv2[1])
1315 || (start.getLocation().x == vv1[0] && start.getLocation().y == vv1[1])) {
1316 log.fine("We are already in the next polygon. No comparation, let's just continue.");
1317 continue;
1318 }
1319
1320
1321 double dist = gateway.getSignedDistance(start.getLocation().x, start.getLocation().y);
1322
1323
1324 Line2D leftMantinel = new Line2D(start.getLocation().x, start.getLocation().y, vv2[0], vv2[1]);
1325 Line2D rightMantinel = new Line2D(start.getLocation().x, start.getLocation().y, vv1[0], vv1[1]);
1326 if (dist < 0) {
1327 Line2D swap = leftMantinel;
1328 leftMantinel = rightMantinel;
1329 rightMantinel = swap;
1330 vv1 = getVertex(v2);
1331 vv2 = getVertex(v1);
1332 gateway = new Line2D(vv1[0], vv1[1], vv2[0], vv2[1]);
1333 }
1334
1335
1336 int leftMantinelIndex = index;
1337 double leftMantinelZ = vv2[2];
1338 Location leftMantinelTarget;
1339 if (safeVertex.get(v2)) {
1340 leftMantinelTarget = new Location(vv2[0], vv2[1], vv2[2] + NavMeshConstants.liftPolygonLocation);
1341 } else {
1342 if (gateway.getLength() <= 2 * NavMeshConstants.agentRadius) {
1343 leftMantinelTarget = new Location((vv2[0] + vv1[0]) / 2, (vv2[1] + vv1[1]) / 2, (vv2[2] + vv1[2]) / 2 + NavMeshConstants.liftPolygonLocation);
1344 } else {
1345 leftMantinelTarget = new Location(vv2[0] + (vv1[0] - vv2[0]) / gateway.getLength() * NavMeshConstants.agentRadius,
1346 vv2[1] + (vv1[1] - vv2[1]) / gateway.getLength() * NavMeshConstants.agentRadius,
1347 vv2[2] + (vv1[2] - vv2[2]) / gateway.getLength() * NavMeshConstants.agentRadius + NavMeshConstants.liftPolygonLocation);
1348 }
1349 }
1350
1351 int rightMantinelIndex = index;
1352 double rightMantinelZ = vv1[2];
1353 Location rightMantinelTarget;
1354 if (safeVertex.get(v1)) {
1355 rightMantinelTarget = new Location(vv1[0], vv1[1], vv1[2] + NavMeshConstants.liftPolygonLocation);
1356 } else {
1357 if (gateway.getLength() <= 2 * NavMeshConstants.agentRadius) {
1358 rightMantinelTarget = new Location((vv2[0] + vv1[0]) / 2, (vv2[1] + vv1[1]) / 2, (vv2[2] + vv1[2]) / 2 + NavMeshConstants.liftPolygonLocation);
1359 } else {
1360 rightMantinelTarget = new Location(vv1[0] + (vv2[0] - vv1[0]) / gateway.getLength() * NavMeshConstants.agentRadius,
1361 vv1[1] + (vv2[1] - vv1[1]) / gateway.getLength() * NavMeshConstants.agentRadius,
1362 vv1[2] + (vv2[2] - vv1[2]) / gateway.getLength() * NavMeshConstants.agentRadius + NavMeshConstants.liftPolygonLocation);
1363 }
1364 }
1365
1366
1367 boolean targetAdded = false;
1368 boolean outOfMantinels = false;
1369 boolean endOfPolygonPathReached = false;
1370 while (!targetAdded && !outOfMantinels) {
1371 index++;
1372 lastAtom = polygonPath.get(index - 1);
1373 if (index < polygonPath.size()) {
1374 atom = polygonPath.get(index);
1375 }
1376 else {
1377 endOfPolygonPathReached = true;
1378 }
1379
1380 polygon2 = (NavMeshPolygon) lastAtom;
1381
1382
1383
1384
1385 if (endOfPolygonPathReached || atom.getClass() == OffMeshPoint.class) {
1386 NavPoint np = null;
1387 if (!endOfPolygonPathReached) {
1388 np = (NavPoint) worldView.get(((OffMeshPoint) atom).getNavPointId());
1389 }
1390
1391 ILocated target;
1392 if (endOfPolygonPathReached) {
1393 target = to;
1394 } else {
1395 target = np;
1396 }
1397
1398
1399
1400 Line2D virtualGateway1 = new Line2D(target.getLocation().x, target.getLocation().y, leftMantinel.p2.x, leftMantinel.p2.y);
1401 dist = virtualGateway1.getSignedDistance(start.getLocation().x, start.getLocation().y);
1402 if (dist < 0) {
1403
1404
1405 path.add(leftMantinelTarget);
1406
1407 outOfMantinels = true;
1408 index = leftMantinelIndex;
1409 } else {
1410
1411
1412 Line2D virtualGateway2 = new Line2D(rightMantinel.p2.x, rightMantinel.p2.y, target.getLocation().x, target.getLocation().y);
1413 dist = virtualGateway2.getSignedDistance(start.getLocation().x, start.getLocation().y);
1414 if (dist < 0) {
1415
1416
1417 path.add(rightMantinelTarget);
1418
1419 outOfMantinels = true;
1420
1421 index = rightMantinelIndex;
1422 } else {
1423
1424 if (!endOfPolygonPathReached) {
1425 path.add(np);
1426 }
1427 targetAdded = true;
1428 }
1429 }
1430 }
1431 else {
1432 polygon = (NavMeshPolygon) atom;
1433 Point2D middleOfOldGateway = new Point2D((gateway.p1.x + gateway.p2.x) / 2, (gateway.p1.y + gateway.p2.y) / 2);
1434
1435 p1 = this.getPolygon(polygon.getPolygonId());
1436 p2 = this.getPolygon(polygon2.getPolygonId());
1437 v1 = -1;
1438 v2 = -1;
1439 outer:
1440 for (int i = 0; i < p1.length; i++) {
1441 for (int j = 0; j < p2.length; j++) {
1442 if (p1[i] == p2[j]) {
1443 if (v1 == -1) {
1444 v1 = p1[i];
1445 } else {
1446 if (p1[i] != v1) {
1447 v2 = p1[i];
1448 }
1449 break outer;
1450 }
1451 }
1452 }
1453 }
1454 vv1 = getVertex(v1);
1455 vv2 = getVertex(v2);
1456 gateway = new Line2D(vv1[0], vv1[1], vv2[0], vv2[1]);
1457
1458
1459 dist = gateway.getSignedDistance(middleOfOldGateway);
1460 if (dist < 0) {
1461 vv1 = getVertex(v2);
1462 vv2 = getVertex(v1);
1463 gateway = new Line2D(vv1[0], vv1[1], vv2[0], vv2[1]);
1464 }
1465
1466
1467
1468
1469 dist = leftMantinel.getSignedDistance(gateway.p2);
1470
1471 if (dist < 0) {
1472
1473
1474 dist = rightMantinel.getSignedDistance(gateway.p2);
1475
1476 if (dist > 0) {
1477
1478 leftMantinel = new Line2D(leftMantinel.p1, gateway.p2);
1479 leftMantinelIndex = index;
1480 leftMantinelZ = vv2[2];
1481 if (safeVertex.get(v2)) {
1482 leftMantinelTarget = new Location(vv2[0], vv2[1], vv2[2] + NavMeshConstants.liftPolygonLocation);
1483 } else {
1484 if (gateway.getLength() <= 2 * NavMeshConstants.agentRadius) {
1485 leftMantinelTarget = new Location((vv2[0] + vv1[0]) / 2, (vv2[1] + vv1[1]) / 2, (vv2[2] + vv1[2]) / 2 + NavMeshConstants.liftPolygonLocation);
1486 } else {
1487 leftMantinelTarget = new Location(vv2[0] + (vv1[0] - vv2[0]) / gateway.getLength() * NavMeshConstants.agentRadius,
1488 vv2[1] + (vv1[1] - vv2[1]) / gateway.getLength() * NavMeshConstants.agentRadius,
1489 vv2[2] + (vv1[2] - vv2[2]) / gateway.getLength() * NavMeshConstants.agentRadius + NavMeshConstants.liftPolygonLocation);
1490 }
1491 }
1492 } else {
1493
1494
1495
1496 path.add(rightMantinelTarget);
1497 outOfMantinels = true;
1498 index = rightMantinelIndex;
1499 }
1500 } else {
1501
1502
1503
1504 }
1505
1506
1507 dist = rightMantinel.getSignedDistance(gateway.p1);
1508
1509 if (dist > 0) {
1510
1511
1512
1513
1514 dist = leftMantinel.getSignedDistance(gateway.p1);
1515
1516 if (dist < 0) {
1517
1518 rightMantinel = new Line2D(rightMantinel.p1, gateway.p1);
1519 rightMantinelIndex = index;
1520 rightMantinelZ = vv1[2];
1521 if (safeVertex.get(v1)) {
1522 rightMantinelTarget = new Location(vv1[0], vv1[1], vv1[2] + NavMeshConstants.liftPolygonLocation);
1523 } else {
1524 if (gateway.getLength() <= 2 * NavMeshConstants.agentRadius) {
1525 rightMantinelTarget = new Location((vv2[0] + vv1[0]) / 2, (vv2[1] + vv1[1]) / 2, (vv2[2] + vv1[2]) / 2 + NavMeshConstants.liftPolygonLocation);
1526 } else {
1527 rightMantinelTarget = new Location(vv1[0] + (vv2[0] - vv1[0]) / gateway.getLength() * NavMeshConstants.agentRadius,
1528 vv1[1] + (vv2[1] - vv1[1]) / gateway.getLength() * NavMeshConstants.agentRadius,
1529 vv1[2] + (vv2[2] - vv1[2]) / gateway.getLength() * NavMeshConstants.agentRadius + NavMeshConstants.liftPolygonLocation);
1530 }
1531 }
1532 } else {
1533
1534
1535
1536 path.add(leftMantinelTarget);
1537 outOfMantinels = true;
1538 index = leftMantinelIndex;
1539 }
1540 } else {
1541
1542
1543
1544 }
1545 }
1546 }
1547 }
1548 }
1549 }
1550 path.add(to);
1551 return path;
1552 }
1553
1554
1555
1556
1557
1558
1559
1560
1561
1562 public List<ILocated> getPath(ILocated from, ILocated to) {
1563
1564 List<INavMeshAtom> polygonPath = null;
1565
1566
1567 if (nps == null) {
1568 initNavPoints();
1569 }
1570 if (isFwMapAvailable && nps != null) {
1571
1572 NavPoint fromNp = DistanceUtils.getNearest(nps.values(), from);
1573 NavPoint toNp = DistanceUtils.getNearest(nps.values(), to);
1574
1575 if (hasTeleports) {
1576 List<NavPoint> path = fwMap.getPath(fromNp, toNp);
1577 if (path != null) {
1578 INavMeshAtom atomFrom = getNearestAtom(from.getLocation());
1579 boolean skip = false;
1580
1581 for (NavPoint np : path) {
1582 if (skip) {
1583
1584 atomFrom = getNearestOffmeshPoint(np.getLocation());
1585 skip = false;
1586
1587 } else if (np.isTeleporter()) {
1588
1589 INavMeshAtom atomTo = getNearestOffmeshPoint(np.getLocation());
1590 List<INavMeshAtom> pathPart = getPolygonPath(atomFrom, atomTo);
1591 if (pathPart == null) {
1592 polygonPath = null;
1593 break;
1594 }
1595 if (polygonPath == null) {
1596 polygonPath = pathPart;
1597 } else {
1598 polygonPath.addAll(pathPart);
1599 }
1600 skip = true;
1601 }
1602 }
1603
1604 if (polygonPath != null) {
1605 INavMeshAtom atomTo = getNearestAtom(to.getLocation());
1606 List<INavMeshAtom> pathPart = getPolygonPath(atomFrom, atomTo);
1607 if (pathPart != null) {
1608 polygonPath.addAll(pathPart);
1609 } else {
1610 polygonPath = null;
1611 }
1612 }
1613 }
1614 }
1615 }
1616
1617
1618
1619 if (polygonPath == null) {
1620 polygonPath = getPolygonPath(from.getLocation(), to.getLocation());
1621 }
1622 if (polygonPath == null) {
1623 return null;
1624 }
1625
1626
1627 List<ILocated> path;
1628
1629
1630
1631 path = getFunneledPath(from, to, polygonPath);
1632
1633 return path;
1634 }
1635
1636 private INavMeshAtom getNearestAtom(Location location) {
1637 return getNearestAtom(location, true);
1638 }
1639
1640 public NavMeshPolygon getNearestPolygon(Location location) {
1641 return (NavMeshPolygon) getNearestAtom(location, false);
1642 }
1643
1644
1645
1646
1647
1648
1649
1650 private INavMeshAtom getNearestAtom(Location location, boolean includeOffMeshPoints) {
1651
1652
1653 int pId = getPolygonId(location);
1654 if (pId >= 0) {
1655 return new NavMeshPolygon(pId);
1656 } else {
1657
1658
1659
1660
1661 INavMeshAtom nearest = null;
1662 if (includeOffMeshPoints) {
1663 nearest = getNearestOffmeshPoint(location);
1664 }
1665 double minDist = Double.MAX_VALUE;
1666
1667
1668 if (nearest == null) {
1669 for (int i = 0; i < polys.size(); i++) {
1670 NavMeshPolygon p = new NavMeshPolygon(i);
1671 Location pl = getLocation(p);
1672 double dist = location.getDistance(pl);
1673 if (dist < minDist) {
1674 nearest = p;
1675 minDist = dist;
1676 }
1677 }
1678 }
1679
1680 return nearest;
1681 }
1682 }
1683
1684
1685
1686
1687
1688
1689
1690
1691
1692 protected double getDistance(INavMeshAtom atom1, INavMeshAtom atom2) {
1693 if (atom1.equals(atom2)) {
1694 return 0.0;
1695 }
1696 Location l1, l2;
1697 l1 = getLocation(atom1);
1698 l2 = getLocation(atom2);
1699 return l1.getDistance(l2);
1700 }
1701
1702
1703
1704
1705
1706
1707
1708
1709 private Location getLocation(INavMeshAtom atom1) {
1710 if (atom1.getClass() == OffMeshPoint.class) {
1711 return getLocation((OffMeshPoint) atom1);
1712 }
1713 if (atom1.getClass() == NavMeshPolygon.class) {
1714 return getLocation((NavMeshPolygon) atom1);
1715 }
1716 throw new UnsupportedOperationException("Not implemented. Not now. Not ever.");
1717 }
1718
1719
1720
1721
1722
1723
1724
1725 private Location getLocation(OffMeshPoint op) {
1726 NavPoint np = (NavPoint) worldView.get(op.getNavPointId());
1727 return np.getLocation();
1728 }
1729
1730
1731
1732
1733
1734
1735
1736 private Location getLocation(NavMeshPolygon p) {
1737 int[] polygon = this.getPolygon(p.getPolygonId());
1738 double sumX = 0.0;
1739 double sumY = 0.0;
1740 double sumZ = 0.0;
1741 for (int i = 0; i < polygon.length; i++) {
1742 double[] v = getVertex(polygon[i]);
1743 sumX += v[0];
1744 sumY += v[1];
1745 sumZ += v[2];
1746 }
1747 return new Location(sumX / polygon.length, sumY / polygon.length, (sumZ / polygon.length) + NavMeshConstants.liftPolygonLocation);
1748 }
1749
1750
1751
1752
1753
1754
1755
1756
1757
1758
1759 public double getDistanceFromEdge(Location location, Vector2d vector, double rayLength) {
1760 if (rayLength <= 0) {
1761 return 0;
1762 }
1763
1764
1765 vector.normalize();
1766 vector.x *= rayLength;
1767 vector.y *= rayLength;
1768 Location end = new Location(location.x + vector.x, location.y + vector.y);
1769
1770
1771 Line2D ray = new Line2D(location.x, location.y, end.x, end.y);
1772
1773 int pId = this.getPolygonId(location);
1774 if (pId < 0) {
1775 return 0;
1776 }
1777
1778
1779
1780
1781
1782
1783 int currentPolygonId = pId;
1784 int lastPolygonId = -1;
1785 int nextPolygonId = -1;
1786
1787
1788 Point2D cross = null;
1789 int v1 = -1, v2 = -1;
1790 int[] polygon = getPolygon(currentPolygonId);
1791 for (int i = 0; i < polygon.length; i++) {
1792 v1 = polygon[i];
1793 v2 = polygon[((i == polygon.length - 1) ? 0 : i + 1)];
1794 double[] vertex1 = getVertex(v1);
1795 double[] vertex2 = getVertex(v2);
1796 Line2D edge = new Line2D(vertex1[0], vertex1[1], vertex2[0], vertex2[1]);
1797 cross = ray.getIntersection(edge);
1798 if (cross != null) {
1799 if (cross.x <= Math.max(edge.p1.x, edge.p2.x) && cross.x >= Math.min(edge.p1.x, edge.p2.x)
1800 && cross.x <= Math.max(ray.p1.x, ray.p2.x) && cross.x >= Math.min(ray.p1.x, ray.p2.x)) {
1801
1802 break;
1803 } else {
1804
1805 cross = null;
1806 }
1807 }
1808 }
1809
1810 double distanceToPolygonEdge = ray.p1.getDistance(cross);
1811
1812
1813 if (cross == null || distanceToPolygonEdge >= rayLength) {
1814 return 0;
1815 }
1816
1817
1818 nextPolygonId = getNeighbourPolygon(currentPolygonId, v1, v2);
1819 if (nextPolygonId == -1) {
1820
1821 return distanceToPolygonEdge;
1822 } else {
1823
1824
1825 vector = ((Vector2d) vector.clone());
1826 vector.normalize();
1827 Location crossLocation = new Location(cross.x + vector.x, cross.y + vector.y, location.z);
1828
1829 return distanceToPolygonEdge + getDistanceFromEdge(crossLocation, vector, rayLength - distanceToPolygonEdge);
1830
1831 }
1832 }
1833
1834
1835
1836
1837
1838
1839
1840
1841
1842 public double getDistanceFromEdge(Location location, Vector2d vector) {
1843
1844 return getDistanceFromEdge(location, vector, 10000);
1845 }
1846
1847
1848
1849
1850
1851
1852
1853
1854
1855
1856 public int getNeighbourPolygon(int currentPolygonId, int v1, int v2) {
1857
1858 List<Integer> neighbours = this.getNeighbourIdsToPolygon(currentPolygonId);
1859 for (Integer neighbour : neighbours) {
1860
1861 int[] polygon2 = getPolygon(neighbour);
1862
1863 boolean containsV1 = false, containsV2 = false;
1864 for (int j = 0; j < polygon2.length; j++) {
1865 if (polygon2[j] == v1) {
1866 containsV1 = true;
1867 }
1868 if (polygon2[j] == v2) {
1869 containsV2 = true;
1870 }
1871 }
1872
1873 if (containsV1 && containsV2) {
1874 return neighbour;
1875 }
1876 }
1877
1878 return -1;
1879 }
1880
1881 public NavMeshBSPNode getBiggestLeafInTree() {
1882 return biggestLeafInTree;
1883 }
1884
1885
1886
1887
1888
1889
1890
1891
1892 protected void setFwMap(FloydWarshallMap fwMap) {
1893 this.fwMap = fwMap;
1894 isFwMapAvailable = fwMap != null;
1895 }
1896
1897 private void initNavPoints() {
1898 nps = (Map<UnrealId, NavPoint>) (Map) worldView.getAll(NavPoint.class);
1899 if (nps != null) {
1900 for (NavPoint np : nps.values()) {
1901 if (np.isTeleporter()) {
1902 hasTeleports = true;
1903 break;
1904 }
1905 }
1906 }
1907 }
1908
1909 private INavMeshAtom getNearestOffmeshPoint(Location location) {
1910 INavMeshAtom nearest = null;
1911 double minDist = Double.MAX_VALUE;
1912
1913 for (OffMeshPoint op : offMeshPoints) {
1914 double dist = location.getDistance(op.getLocation());
1915 if (dist < minDist) {
1916 nearest = op;
1917 minDist = dist;
1918 }
1919 }
1920 return nearest;
1921 }
1922
1923
1924
1925
1926 protected void clear() {
1927 log.warning("NavMesh has been cleared...");
1928
1929 verts = new ArrayList<double[]>();
1930 polys = new ArrayList<int[]>();
1931 vertsToPolys = null;
1932 safeVertex = null;
1933 bspTree = null;
1934 biggestLeafInTree = null;
1935 offMeshPoints = null;
1936 polysToOffMeshPoints = null;
1937
1938 loaded = false;
1939 loadedForMap = null;
1940 }
1941
1942 }