1 /*
2 * Copyright (C) 2013 AMIS research group, Faculty of Mathematics and Physics, Charles University in Prague, Czech Republic
3 *
4 * This program is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
8 *
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
13 *
14 * You should have received a copy of the GNU General Public License
15 * along with this program. If not, see <http://www.gnu.org/licenses/>.
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 * @author Jakub
31 * Node of BSP tree structure
32 * built for NavMesh
33 */
34 public class NavMeshBSPNode implements java.io.Serializable {
35
36 /**
37 * Auto-generated
38 */
39 private static final long serialVersionUID = 3893164811677500677L;
40
41 public transient NavMesh navmesh;
42 // list of polygon ids (actual polygons are stored in navmesh)
43 public ArrayList polys;
44 public NavMeshBSPNode parent;
45 // vertices separating polygons in this node (actual vertices are in navmesh)
46 public StraightLine2D sepLine;
47 // node children
48 public NavMeshBSPNode left;
49 public NavMeshBSPNode right;
50
51 private Random random;
52
53 /**
54 *
55 * @param m
56 * @param par
57 *
58 * Cretaes a new node pointing to parent par with navmesh m.
59 * Remember to fill polys propery.
60 * Then build method should be called.
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 //System.out.println("findSeparatingLine(): Trying to split " + polys.size() + " polygons...");
77 // alg 1:
78 // pick 3 polygons at random and try all treir edges. remember the best one
79 // criterium is that the greater half of polgons should be as small as it can be - ideally 0.5
80 double bestFoundSplitFactor = NavMeshConstants.maxAllowedSplitFactor;
81 StraightLine2D bestFoundSeparatingLine = null;
82
83 ArrayList candidatePolygons = (ArrayList) polys.clone();
84
85 // several tries for polygons...
86 for(int i = 0; i < NavMeshConstants.maxNumberOfPolygonsToTry; i++) {
87 if(candidatePolygons.isEmpty()) break;
88
89 int randomId = 0; //random.nextInt(candidatePolygons.size());
90 Integer polygonId = (Integer) candidatePolygons.get(randomId);
91 int[] polygon = navmesh.getPolygon(polygonId);
92
93 // pick all vertices... and try them
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 // we create 2Dpoints ignoring the third dimension
101 Point2D point2D1 = new Point2D(vertex1[0], vertex1[1]);
102 Point2D point2D2 = new Point2D(vertex2[0], vertex2[1]);
103
104 // horray, separating line
105 StraightLine2D separatingLine = new StraightLine2D(point2D1, point2D2);
106
107 // now lets divide all polygons to left and right part
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 // if this line is best found, we remember it
117 if(splitFactor < bestFoundSplitFactor) {
118 bestFoundSeparatingLine = separatingLine;
119 bestFoundSplitFactor = splitFactor;
120 }
121 }
122 // dont search this polygon again
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 //System.out.println("findSeparatingLine(): Returning a split line with splitFactor " + bestFoundSplitFactor);
127 return bestFoundSeparatingLine;
128 }
129
130 /*
131 * Splits the input polygons by line into left and right part.
132 * Polygons that this line intersects will occur in both sets
133 */
134 private ArrayList[] splitPolygonsByLine(ArrayList polysToSplit, StraightLine2D separatingLine) throws Exception {
135
136 ArrayList left = new ArrayList();
137 ArrayList right = new ArrayList();
138
139 // walk through each polygon
140 for(int i = 0; i < polysToSplit.size(); i++) {
141
142 // switches where the polygon lie
143 boolean isOnLeft = false;
144 boolean isOnRight = false;
145
146 Integer pId = (Integer) polysToSplit.get(i);
147 int[] polygon = navmesh.getPolygon(pId);
148
149 // walk through each vertex
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 * Recursive method building an antrige tree from this node as root
172 */
173 public void build() {
174
175 // test end of recursion
176 if (!shouldSplit()) {
177 // set biggest leaf?
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 // find separating line
186 try {
187 sepLine = findSeparatingLine();
188 }
189 catch(Exception e) {
190 //System.out.println("No separating line found for this node. Ok, stop splitting. Number of polygons in this node is " + polys.size());
191 // set biggest leaf?
192 if(polys.size() > navmesh.getNumberOfPolygonsInBiggestLeaf()) {
193 //System.out.println("Setting biggest leaf sofar. Number of polygons in this node is " + polys.size() + ". The polygons here are: " + polys.toString());
194 navmesh.setBiggestLeafInTree(this);
195 }
196 }
197
198 if(sepLine != null) {
199 try {
200 ArrayList[] splittedPolys = splitPolygonsByLine(polys, sepLine);
201 //System.out.println("build(): Left has " + splittedPolys[0].size() + " polygons.");
202 //System.out.println("build(): Right has " + splittedPolys[1].size() + " polygons.");
203 //System.out.println("build(): Line is crossing " + (splittedPolys[0].size() + splittedPolys[1].size() - polys.size()) + " polygons.");
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 }