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
20 import java.util.ArrayList;
21 import java.util.Random;
22 import java.util.logging.Level;
23 import java.util.logging.Logger;
24 import math.geom2d.Point2D;
25 import math.geom2d.line.Line2D;
26 import math.geom2d.line.StraightLine2D;
27
28
29
30
31
32
33
34 public class NavMeshBSPNode implements java.io.Serializable {
35
36
37
38
39 private static final long serialVersionUID = 3893164811677500677L;
40
41 public transient NavMesh navmesh;
42
43 public ArrayList polys;
44 public NavMeshBSPNode parent;
45
46 public StraightLine2D sepLine;
47
48 public NavMeshBSPNode left;
49 public NavMeshBSPNode right;
50
51 private Random random;
52
53
54
55
56
57
58
59
60
61
62
63 public NavMeshBSPNode(NavMesh m, NavMeshBSPNode par) {
64 navmesh = m;
65 parent = par;
66 polys = new ArrayList();
67 random = new Random();
68 sepLine = null;
69 }
70
71 public boolean shouldSplit() {
72 return (polys.size() > NavMeshConstants.stopSplittingNumberOfPolygons);
73 }
74
75 public StraightLine2D findSeparatingLine () throws Exception {
76
77
78
79
80 double bestFoundSplitFactor = NavMeshConstants.maxAllowedSplitFactor;
81 StraightLine2D bestFoundSeparatingLine = null;
82
83 ArrayList candidatePolygons = (ArrayList) polys.clone();
84
85
86 for(int i = 0; i < NavMeshConstants.maxNumberOfPolygonsToTry; i++) {
87 if(candidatePolygons.isEmpty()) break;
88
89 int randomId = 0;
90 Integer polygonId = (Integer) candidatePolygons.get(randomId);
91 int[] polygon = navmesh.getPolygon(polygonId);
92
93
94 for(int j = 0; j<polygon.length; j++) {
95 int v1 = polygon[j];
96 int v2 = polygon[(j+1) % polygon.length];
97 double[] vertex1 = navmesh.getVertex(v1);
98 double[] vertex2 = navmesh.getVertex(v2);
99
100
101 Point2D point2D1 = new Point2D(vertex1[0], vertex1[1]);
102 Point2D point2D2 = new Point2D(vertex2[0], vertex2[1]);
103
104
105 StraightLine2D separatingLine = new StraightLine2D(point2D1, point2D2);
106
107
108 ArrayList[] splittedPolys = splitPolygonsByLine(polys, separatingLine);
109 ArrayList left = splittedPolys[0];
110 ArrayList right = splittedPolys[1];
111
112 int intersectedPolygons = left.size() + right.size() - polys.size();
113
114 double splitFactor = max(left.size() / polys.size(), right.size() / polys.size());
115
116
117 if(splitFactor < bestFoundSplitFactor) {
118 bestFoundSeparatingLine = separatingLine;
119 bestFoundSplitFactor = splitFactor;
120 }
121 }
122
123 candidatePolygons.remove(polygonId);
124 }
125 if(bestFoundSeparatingLine==null) throw new Exception("No good separating line have been found. Splitting " +polys.size()+ " polygons unsuccessfull." );
126
127 return bestFoundSeparatingLine;
128 }
129
130
131
132
133
134 private ArrayList[] splitPolygonsByLine(ArrayList polysToSplit, StraightLine2D separatingLine) throws Exception {
135
136 ArrayList left = new ArrayList();
137 ArrayList right = new ArrayList();
138
139
140 for(int i = 0; i < polysToSplit.size(); i++) {
141
142
143 boolean isOnLeft = false;
144 boolean isOnRight = false;
145
146 Integer pId = (Integer) polysToSplit.get(i);
147 int[] polygon = navmesh.getPolygon(pId);
148
149
150 for(int j = 0; j < polygon.length; j++) {
151 int vId = polygon[j];
152 double[] vertex = navmesh.getVertex(vId);
153 Point2D point2D = new Point2D(vertex[0], vertex[1]);
154 double dist = separatingLine.getSignedDistance(point2D);
155 if(dist<0) isOnLeft = true;
156 if(dist>0) isOnRight = true;
157 if(isOnLeft && isOnRight) break;
158 }
159 if(isOnLeft) left.add(pId);
160 if(isOnRight) right.add(pId);
161 if(!isOnLeft && !isOnRight) throw new Exception("Something is wrong. The polygon number " +pId+ " seems to be on neither side of the separating line.");
162
163 }
164 ArrayList[] ret = new ArrayList[2];
165 ret[0] = left;
166 ret[1] = right;
167 return ret;
168 }
169
170
171
172
173 public void build() {
174
175
176 if (!shouldSplit()) {
177
178 if(polys.size() > navmesh.getNumberOfPolygonsInBiggestLeaf()) {
179 System.out.println("Setting biggest leaf sofar. Number of polygons in this node is " + polys.size());
180 navmesh.setBiggestLeafInTree(this);
181 }
182 return;
183 }
184
185
186 try {
187 sepLine = findSeparatingLine();
188 }
189 catch(Exception e) {
190
191
192 if(polys.size() > navmesh.getNumberOfPolygonsInBiggestLeaf()) {
193
194 navmesh.setBiggestLeafInTree(this);
195 }
196 }
197
198 if(sepLine != null) {
199 try {
200 ArrayList[] splittedPolys = splitPolygonsByLine(polys, sepLine);
201
202
203
204 left = new NavMeshBSPNode(navmesh, this);
205 left.polys = splittedPolys[0];
206 left.build();
207 right = new NavMeshBSPNode(navmesh, this);
208 right.polys = splittedPolys[1];
209 right.build();
210 } catch (Exception e) {
211 System.out.println("Could not split polys by the given line. This should not happen.");
212 e.printStackTrace();
213 }
214 }
215 }
216
217 private double max(double d1, double d2) {
218 return d1 > d2 ? d1 : d2;
219 }
220
221 public boolean isLeaf() {
222 return (left == null && right == null);
223 }
224
225
226 }