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