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       * @param drawMesh whether to draw mesh polygons
31       * @param drawOffMeshLinks whether to draw off-mesh-links
32       */   
33      public boolean draw(boolean drawMesh, boolean drawOffMeshLinks) {
34      	if (navMesh == null || !navMesh.isLoaded()) return false;
35      	
36      	if (drawMesh) {
37      	    // draw polygons
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  	        // draw off-mesh connections
50  	        for(OffMeshPoint op : navMesh.getOffMeshPoints()) {
51  	            // draw all outgoing edges
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       * Draws given lines
68       * @param lines 
69       * @param color 
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       * Undraws all currently drawn lines
81       */
82      public void unDraw() {
83          DrawStayingDebugLines d = new DrawStayingDebugLines();
84          d.setClearAll(true);                
85          getServer().getAct().act(d);        
86      }
87      
88      /**
89       * Draws one polygon
90       * with the color set in color
91       * @param i
92       * @param color
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      * Draws one polygon
105      * with default color (yellow)
106      * @param i
107      */   
108     public void drawOnePolygon(int i) {
109         drawOnePolygon(i, new Location(255,255,0));                         
110     }    
111     
112     /**
113      * Draws one atom (polygon or point)
114      * @param atom
115      * @param location 
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      * Draws entire list of polygons
126      * @param polygonPath 
127      * @param location 
128      */
129     public void drawPolygonPath(List<INavMeshAtom> polygonPath, Location location) {
130         for(INavMeshAtom atom : polygonPath) {
131             drawAtom(atom, location);
132         }
133     }
134     
135     /**
136      * Draws the given path 
137      * @param path 
138      * @param color 
139      */
140     public void drawPath(IPathFuture<ILocated> path, Location color) {
141         
142         // the commented code sometimes doesnt work for soem reason. maybe there is a corrupted point along the path?
143         //String lines = pathToDebugString(path);
144         //System.out.println("path to be drawn: " + lines);
145         //draw(lines,color);
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      * Debug method:  
161      * Draws only the polygons in the biggest leaf
162      * so that we can see why they could not have been split any further
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             //System.out.println("polygon["+i+"] lines: " + lines);
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     // DEBUG STUFF
180     // ===========
181     
182 	/**
183      * Returns all lines in one long string.
184      */
185     public String toDebugString() {
186         StringBuilder result = new StringBuilder("");      
187         // projdeme vsechny polygony a vykreslime caru vzdy z vrcholu n do n+1
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                 // ziskame vrcholy v1 a v2 mezi kterymi vykreslime caru
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                 // pridani cary
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      * Restricted alternative to toDebugString() - returns only one polygon as string
206      */
207     public String polygonToDebugString(int polygonNumber) {
208         StringBuilder result = new StringBuilder("");      
209         // projdeme vsechny polygony a vykreslime caru vzdy z vrcholu n do n+1
210         int[] p = navMesh.getPolys().get(polygonNumber);
211         for(int j = 0; j<p.length; j++) {
212             if(result.length()>0) result.append(";");
213             // ziskame vrcholy v1 a v2 mezi kterymi vykreslime caru
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             // pridani cary
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      * computes debug string of one off-mesh edge to be drawn.
226      * @param oe
227      * @return 
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         // add arrow at the end
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        // normalize the vector to small lenght
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        // to find edges of the arrow, we take 2D normal vector. And half size
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        // we have all the points, now just connect them 
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      * Creates a string of vector to be drawn from given path
266      * @param path
267      * @return 
268      */
269     private String pathToDebugString(IPathFuture<ILocated> path) {
270         StringBuilder result = new StringBuilder("");      
271         // projdeme vsechny body a vykreslime caru vzdy z vrcholu n do n+1
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 }