$search
00001 package edu.tum.cs.ias.knowrob; 00002 00003 00004 import java.io.BufferedReader; 00005 import java.io.IOException; 00006 import java.util.ArrayList; 00007 import processing.core.PApplet; 00008 00009 import ros.pkg.roslib.msg.Header; 00010 import ros.pkg.triangle_mesh_msgs.msg.TriangleMesh; 00011 import ros.pkg.triangle_mesh_msgs.msg.Triangle; 00012 import ros.pkg.geometry_msgs.msg.Point32; 00013 00014 00015 public class MeshVisApplet extends PApplet { 00016 00017 static final long serialVersionUID=0; 00018 00019 00020 00021 private ArrayList<float[]> meshpoints = new ArrayList<float[]>(); 00022 private ArrayList<int[]> meshtriangles = new ArrayList<int[]>(); 00023 float minX=1e10f, minY=1e10f, minZ=1e10f; 00024 float maxX=-1e10f, maxY=-1e10f, maxZ=-1e10f; 00025 00026 00027 00031 // 00032 // INITIALIZATION FUNCTIONS 00033 // 00034 00039 public void setup() { 00040 00041 size(700, 600, P3D); 00042 lights(); 00043 00044 noLoop(); 00045 hint(ENABLE_DEPTH_SORT); 00046 draw(); 00047 } 00048 00049 00050 00054 // 00055 // DRAW FUNCTIONS 00056 // 00057 00058 00062 public void draw() { 00063 00064 00065 background(20, 20, 20); 00066 fill(220); 00067 00068 stroke(150); 00069 strokeWeight(1.0f); 00070 00071 00072 pushMatrix(); 00073 00074 applyMatrix( 1, 0, 0, 0, 00075 0, 1, 0, 0, 00076 0, 0, -1, 0, 00077 0, 0, 0, 1); 00078 00079 pushMatrix(); 00080 rotateZ( -PI/2 ); 00081 rotateY( PI/2 ); 00082 00083 // shift to the center of the window 00084 translate(0, 350, -300.0f); 00085 00086 float SCALE = (300/ (maxZ-minZ)); 00087 translate(SCALE*(-maxX), SCALE*(-maxY+(maxY-minY)/2), SCALE*(-maxZ+(maxZ-minZ)/2)); 00088 00089 beginShape(TRIANGLES); 00090 00091 for (int i = 0; i < meshtriangles.size(); i++) { 00092 00093 int p0 = meshtriangles.get(i)[0]; 00094 int p1 = meshtriangles.get(i)[1]; 00095 int p2 = meshtriangles.get(i)[2]; 00096 00097 vertex(SCALE*meshpoints.get(p0)[0], SCALE*meshpoints.get(p0)[1], SCALE*meshpoints.get(p0)[2]); 00098 vertex(SCALE*meshpoints.get(p1)[0], SCALE*meshpoints.get(p1)[1], SCALE*meshpoints.get(p1)[2]); 00099 vertex(SCALE*meshpoints.get(p2)[0], SCALE*meshpoints.get(p2)[1], SCALE*meshpoints.get(p2)[2]); 00100 00101 } 00102 endShape(); 00103 00104 00105 popMatrix(); 00106 popMatrix(); 00107 00108 } 00109 00110 00111 00112 00113 // //////////////////////////////////////////////////////////////////////////////////////////// 00114 // //////////////////////////////////////////////////////////////////////////////////////////// 00116 // 00117 // HELPER FUNCTIONS 00118 // 00119 00120 00125 public void readMeshFile(String file) { 00126 00127 this.clear(); 00128 00129 BufferedReader reader = createReader(file); 00130 try{ 00131 00132 String line; 00133 boolean pointFlag=false, triangleFlag=false; 00134 00135 // offset needed when reading multiple meshes to a single array 00136 int ptOffset = meshpoints.size(); 00137 00138 while(true) { 00139 00140 line = reader.readLine(); 00141 if(line==null){break;} 00142 00143 00144 // read point data 00145 if(pointFlag && (line.trim().matches("\\-?[\\d]*\\.?[\\d]*e?\\-?[\\d]* \\-?[\\d]*\\.?[\\d]*e?\\-?[\\d]* \\-?[\\d]*\\.?[\\d]*e?\\-?[\\d]*"))) { 00146 String[] coords = line.split(" "); 00147 if(coords.length==3) { 00148 00149 float x = Float.valueOf(coords[0]); 00150 float y = Float.valueOf(coords[1]); 00151 float z = Float.valueOf(coords[2]); 00152 00153 this.meshpoints.add(new float[] {x,y,z}); 00154 00155 if(x<minX) minX=x; if(x>maxX) maxX=x; 00156 if(y<minY) minY=y; if(y>maxY) maxY=y; 00157 if(z<minZ) minZ=z; if(z>maxZ) maxZ=z; 00158 00159 } 00160 continue; 00161 } 00162 00163 00164 // read triangle data 00165 if(triangleFlag && (line.trim().matches("3 [\\d]* [\\d]* [\\d]*"))) { 00166 String[] pts = line.split(" "); 00167 if(pts.length==4) { 00168 00169 this.meshtriangles.add(new int[] { 00170 Integer.valueOf(pts[1])+ptOffset, 00171 Integer.valueOf(pts[2])+ptOffset, 00172 Integer.valueOf(pts[3])+ptOffset}); 00173 00174 } 00175 continue;} 00176 00177 00178 if(line.matches("POINTS.*")) { 00179 pointFlag=true; 00180 triangleFlag=false; 00181 continue; 00182 } 00183 00184 if(line.matches("POLYGONS.*")) { 00185 pointFlag=false; 00186 triangleFlag=true; 00187 continue; 00188 } 00189 } 00190 this.redraw(); 00191 00192 } catch (IOException e) {} 00193 } 00194 00195 00196 void clear() { 00197 this.meshpoints.clear(); 00198 this.meshtriangles.clear(); 00199 this.redraw(); 00200 00201 this.minX=1e10f; 00202 this.minY=1e10f; 00203 this.minZ=1e10f; 00204 00205 this.maxX=-1e10f; 00206 this.maxY=-1e10f; 00207 this.maxZ=-1e10f; 00208 00209 } 00210 00215 public void readTriangleMesh(TriangleMesh mesh) { 00216 00217 // read point data 00218 for(Point32 p : mesh.points) { 00219 00220 this.meshpoints.add(new float[] {p.x,p.y,p.z}); 00221 00222 if(p.x<minX) minX=p.x; if(p.x>maxX) maxX=p.x; 00223 if(p.y<minY) minY=p.y; if(p.y>maxY) maxY=p.y; 00224 if(p.z<minZ) minZ=p.z; if(p.z>maxZ) maxZ=p.z; 00225 } 00226 00227 // read triangle data 00228 for(Triangle t : mesh.triangles) { 00229 this.meshtriangles.add(new int[] { 00230 Integer.valueOf(t.i), 00231 Integer.valueOf(t.j), 00232 Integer.valueOf(t.k)}); 00233 } 00234 this.redraw(); 00235 } 00236 00237 00241 TriangleMesh getTriangleMesh() { 00242 00243 TriangleMesh res = new TriangleMesh(); 00244 00245 // fill points 00246 Point32[] points = new Point32[this.meshpoints.size()]; 00247 int i=0; 00248 for(float[] p : this.meshpoints) { 00249 points[i] = new Point32(); 00250 points[i].x = p[0]; 00251 points[i].y = p[1]; 00252 points[i++].z = p[2]; 00253 00254 } 00255 res.points = points; 00256 00257 00258 // fill triangles 00259 Triangle[] triangles = new Triangle[this.meshtriangles.size()]; 00260 i=0; 00261 for(int[] t : this.meshtriangles) { 00262 00263 triangles[i] = new Triangle(); 00264 triangles[i].i = t[0]; 00265 triangles[i].j = t[1]; 00266 triangles[i++].k = t[2]; 00267 00268 } 00269 res.triangles = triangles; 00270 00271 res.intensities = new float[1]; 00272 res.sending_node="mesh_label_test_client"; 00273 res.normals = new Point32[1]; 00274 res.normals[0] = new Point32(); 00275 res.normals[0].x=1f;res.normals[0].y=1f;res.normals[0].z=1f; 00276 res.header = new Header(); 00277 res.header.frame_id="bla"; 00278 res.header.seq=1; 00279 res.header.stamp = ros.communication.Time.now(); 00280 00281 00282 return res; 00283 } 00284 00285 } 00286 00287