1 package cz.cuni.amis.pogamut.ut2004.agent.navigation.navmesh.drawing;
2
3 import java.util.List;
4 import java.util.logging.Logger;
5
6 import math.geom3d.Point3D;
7 import cz.cuni.amis.pogamut.base.agent.navigation.IPathFuture;
8 import cz.cuni.amis.pogamut.base3d.worldview.object.ILocated;
9 import cz.cuni.amis.pogamut.base3d.worldview.object.Location;
10 import cz.cuni.amis.pogamut.ut2004.agent.navigation.navmesh.INavMeshAtom;
11 import cz.cuni.amis.pogamut.ut2004.agent.navigation.navmesh.NavMesh;
12 import cz.cuni.amis.pogamut.ut2004.agent.navigation.navmesh.NavMeshConstants;
13 import cz.cuni.amis.pogamut.ut2004.agent.navigation.navmesh.NavMeshPolygon;
14 import cz.cuni.amis.pogamut.ut2004.agent.navigation.navmesh.OffMeshEdge;
15 import cz.cuni.amis.pogamut.ut2004.agent.navigation.navmesh.OffMeshPoint;
16 import cz.cuni.amis.pogamut.ut2004.communication.messages.gbcommands.DrawStayingDebugLines;
17
18 public class NavMeshDraw extends UT2004Draw {
19
20 private NavMesh navMesh;
21
22 public NavMeshDraw(NavMesh navMesh, Logger log, IUT2004ServerProvider serverProvider) {
23 super(log, serverProvider);
24 this.navMesh = navMesh;
25 }
26
27
28
29
30
31
32
33 public boolean draw(boolean drawMesh, boolean drawOffMeshLinks) {
34 if (navMesh == null || !navMesh.isLoaded()) return false;
35
36 if (drawMesh) {
37
38 for(int i = 0; i < navMesh.polyCount(); i++) {
39 DrawStayingDebugLines d = new DrawStayingDebugLines();
40 String lines = this.polygonToDebugString(i);
41 d.setVectors(lines);
42 d.setColor(new Location(255, 255, 255));
43 d.setClearAll(i==0);
44 getServer().getAct().act(d);
45 }
46 }
47
48 if (drawOffMeshLinks) {
49
50 for(OffMeshPoint op : navMesh.getOffMeshPoints()) {
51
52 for(OffMeshEdge oe : op.getOutgoingEdges()) {
53 DrawStayingDebugLines d = new DrawStayingDebugLines();
54 String lines = offMeshEdgeToDebugString(oe);
55 d.setVectors(lines);
56 d.setColor(NavMeshConstants.getColorForOffMeshConnection(oe, getServer()));
57 d.setClearAll(false);
58 getServer().getAct().act(d);
59 }
60 }
61 }
62
63 return true;
64 }
65
66
67
68
69
70
71 public void draw(String lines, Location color) {
72 DrawStayingDebugLines d = new DrawStayingDebugLines();
73 d.setVectors(lines);
74 d.setColor(color);
75 d.setClearAll(false);
76 getServer().getAct().act(d);
77 }
78
79
80
81
82 public void unDraw() {
83 DrawStayingDebugLines d = new DrawStayingDebugLines();
84 d.setClearAll(true);
85 getServer().getAct().act(d);
86 }
87
88
89
90
91
92
93
94 public void drawOnePolygon(int i, Location color) {
95 DrawStayingDebugLines d = new DrawStayingDebugLines();
96 String lines = this.polygonToDebugString(i);
97 d.setVectors(lines);
98 d.setColor(color);
99 d.setClearAll(false);
100 getServer().getAct().act(d);
101 }
102
103
104
105
106
107
108 public void drawOnePolygon(int i) {
109 drawOnePolygon(i, new Location(255,255,0));
110 }
111
112
113
114
115
116
117 private void drawAtom(INavMeshAtom atom, Location location) {
118 if(atom.getClass()==NavMeshPolygon.class) {
119 NavMeshPolygon p = (NavMeshPolygon) atom;
120 drawOnePolygon(p.getPolygonId(), location);
121 }
122 }
123
124
125
126
127
128
129 public void drawPolygonPath(List<INavMeshAtom> polygonPath, Location location) {
130 for(INavMeshAtom atom : polygonPath) {
131 drawAtom(atom, location);
132 }
133 }
134
135
136
137
138
139
140 public void drawPath(IPathFuture<ILocated> path, Location color) {
141
142
143
144
145
146 List<ILocated> pathList = path.get();
147 for(int i = 0; i<pathList.size()-1; i++) {
148 StringBuilder lines = new StringBuilder();
149 lines.append(pathList.get(i).getLocation().x).append(",");
150 lines.append(pathList.get(i).getLocation().y).append(",");
151 lines.append(pathList.get(i).getLocation().z + 40).append(";");
152 lines.append(pathList.get(i+1).getLocation().x).append(",");
153 lines.append(pathList.get(i+1).getLocation().y).append(",");
154 lines.append(pathList.get(i+1).getLocation().z + 40).append("");
155 draw(lines.toString(), color);
156 }
157 }
158
159
160
161
162
163
164 public void drawOnlyBiggestLeaf() {
165 for(int i = 0; i < navMesh.polyCount(); i++) {
166 if(!navMesh.getBiggestLeafInTree().polys.contains(i)) continue;
167
168 DrawStayingDebugLines d = new DrawStayingDebugLines();
169 String lines = this.polygonToDebugString(i);
170
171 d.setVectors(lines);
172 d.setColor(new Location(255, 255, 0));
173 d.setClearAll(false);
174 getServer().getAct().act(d);
175 }
176 }
177
178
179
180
181
182
183
184
185 public String toDebugString() {
186 StringBuilder result = new StringBuilder("");
187
188 for(int i = 0; i < navMesh.getPolys().size(); i++) {
189 int[] p = navMesh.getPolys().get(i);
190 for(int j = 0; j<p.length; j++) {
191 if(result.length()>0) result.append(";");
192
193 double[] v1,v2;
194 v1 = navMesh.getVerts().get(p[j]);
195 if(j==p.length-1) v2 = navMesh.getVerts().get(p[0]);
196 else v2 = navMesh.getVerts().get(p[j+1]);
197
198 result.append(v1[0]+","+v1[1]+","+v1[2]+";"+v2[0]+","+v2[1]+","+v2[2]);
199 }
200 }
201 return result.toString();
202 }
203
204
205
206
207 public String polygonToDebugString(int polygonNumber) {
208 StringBuilder result = new StringBuilder("");
209
210 int[] p = navMesh.getPolys().get(polygonNumber);
211 for(int j = 0; j<p.length; j++) {
212 if(result.length()>0) result.append(";");
213
214 double[] v1,v2;
215 v1 = navMesh.getVerts().get(p[j]);
216 if(j==p.length-1) v2 = (double[]) navMesh.getVerts().get(p[0]);
217 else v2 = navMesh.getVerts().get(p[j+1]);
218
219 result.append(v1[0]+","+v1[1]+","+(v1[2] + 30)+";"+v2[0]+","+v2[1]+","+(v2[2] + 30));
220 }
221 return result.toString();
222 }
223
224
225
226
227
228
229 private String offMeshEdgeToDebugString(OffMeshEdge oe) {
230 StringBuilder result = new StringBuilder("");
231 Location l1 = oe.getFrom().getLocation();
232 Location l2 = oe.getTo().getLocation();
233 result.append(l1.x+","+l1.y+","+l1.z+";"+l2.x+","+l2.y+","+l2.z);
234
235
236 double[] vector = new double[3];
237 vector[0] = l1.x - l2.x;
238 vector[1] = l1.y - l2.y;
239 vector[2] = l1.z - l2.z;
240
241 double length = Math.sqrt(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]);
242 vector[0] *= 1/length * 20;
243 vector[1] *= 1/length * 20;
244 vector[2] *= 1/length * 20;
245 Point3D cross = new Point3D(l2.x+vector[0], l2.y+vector[1], l2.z+vector[2]);
246
247 double[] vector2 = new double[2];
248 vector2[0] = vector[1]/2;
249 vector2[1] = -vector[0]/2;
250 Point3D arrowPoint1 = new Point3D(cross.getX()+vector2[0], cross.getY()+vector2[1], cross.getZ());
251 Point3D arrowPoint2 = new Point3D(cross.getX()-vector2[0], cross.getY()-vector2[1], cross.getZ());
252
253 result.append(";");
254 result.append(arrowPoint1.getX()+","+arrowPoint1.getY()+","+arrowPoint1.getZ()+";"+l2.x+","+l2.y+","+l2.z);
255 result.append(";");
256 result.append(arrowPoint2.getX()+","+arrowPoint2.getY()+","+arrowPoint2.getZ()+";"+l2.x+","+l2.y+","+l2.z);
257 result.append(";");
258 result.append(arrowPoint1.getX()+","+arrowPoint1.getY()+","+arrowPoint1.getZ()+";"+arrowPoint2.getX()+","+arrowPoint2.getY()+","+arrowPoint2.getZ());
259
260
261 return result.toString();
262 }
263
264
265
266
267
268
269 private String pathToDebugString(IPathFuture<ILocated> path) {
270 StringBuilder result = new StringBuilder("");
271
272 ILocated p0 = null;
273 List<ILocated> pathList = path.get();
274 for(ILocated p1 : pathList) {
275 if(result.length()>0) result.append(";");
276 if(p0 != null) {
277 result.append(Math.round(p0.getLocation().x)+","+Math.round(p0.getLocation().y)+","+Math.round(p0.getLocation().z)+";"+Math.round(p1.getLocation().x)+","+Math.round(p1.getLocation().y)+","+Math.round(p1.getLocation().z));
278 }
279 p0 = p1;
280 }
281 return result.toString();
282 }
283
284
285 }