View Javadoc

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       * Draws navmesh in game
29       */   
30      public void draw() {
31      	if (navMesh == null || !navMesh.isLoaded()) return;
32      	
33          // draw polygons
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          // draw off-mesh connections
44          for(OffMeshPoint op : navMesh.getOffMeshPoints()) {
45              // draw all outgoing edges
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       * Draws given lines
59       * @param lines 
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       * Undraws all currently drawn lines
71       */
72      public void unDraw() {
73          DrawStayingDebugLines d = new DrawStayingDebugLines();
74          d.setClearAll(true);                
75          getServer().getAct().act(d);        
76      }
77      
78      /**
79       * Draws one polygon
80       * with the color set in color
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       * Draws one polygon
93       * with default color (yellow)
94       */   
95      public void drawOnePolygon(int i) {
96          drawOnePolygon(i, new Location(255,255,0));                         
97      }    
98      
99      /**
100      * Draws one atom (polygon or point)
101      * @param atom
102      * @param location 
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      * Draws entire list of polygons
113      * @param polygonPath 
114      */
115     public void drawPolygonPath(List<INavMeshAtom> polygonPath, Location location) {
116         for(INavMeshAtom atom : polygonPath) {
117             drawAtom(atom, location);
118         }
119     }
120     
121     /**
122      * Draws the given path 
123      * @param path 
124      */
125     public void drawPath(IPathFuture<ILocated> path, Location color) {
126         
127         // the commented code sometimes doesnt work for soem reason. maybe there is a corrupted point along the path?
128         //String lines = pathToDebugString(path);
129         //System.out.println("path to be drawn: " + lines);
130         //draw(lines,color);
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      * Debug method:  
146      * Draws only the polygons in the biggeswt leaf
147      * so that we can see whe they could not have been split any further
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             //System.out.println("polygon["+i+"] lines: " + lines);
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     // DEBUG STUFF
165     // ===========
166     
167 	/**
168      * Returns all lines in one long string.
169      */
170     public String toDebugString() {
171         StringBuilder result = new StringBuilder("");      
172         // projdeme vsechny polygony a vykreslime caru vzdy z vrcholu n do n+1
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                 // ziskame vrcholy v1 a v2 mezi kterymi vykreslime caru
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                 // pridani cary
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      * Restricted alternative to toDebugString() - returns only one polygon as string
191      */
192     public String polygonToDebugString(int polygonNumber) {
193         StringBuilder result = new StringBuilder("");      
194         // projdeme vsechny polygony a vykreslime caru vzdy z vrcholu n do n+1
195         int[] p = navMesh.getPolys().get(polygonNumber);
196         for(int j = 0; j<p.length; j++) {
197             if(result.length()>0) result.append(";");
198             // ziskame vrcholy v1 a v2 mezi kterymi vykreslime caru
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             // pridani cary
204             result.append(v1[0]+","+v1[1]+","+v1[2]+";"+v2[0]+","+v2[1]+","+v2[2]);
205         }     
206         return result.toString();    
207     }
208     
209     /**
210      * computes debug string of one off-mesh edge to be drawn.
211      * @param oe
212      * @return 
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         // add arrow at the end
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        // normalize the vector to small lenght
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        // to find edges of the arrow, we take 2D normal vector. And half size
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        // we have all the points, now just connect them 
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      * Cretaes a string of vector to be drawn from given path
251      * @param path
252      * @return 
253      */
254     private String pathToDebugString(IPathFuture<ILocated> path) {
255         StringBuilder result = new StringBuilder("");      
256         // projdeme vsechny body a vykreslime caru vzdy z vrcholu n do n+1
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 }