MeshVisApplet.java
Go to the documentation of this file.
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 


mesh_label_node
Author(s): Moritz Tenorth
autogenerated on Mon Oct 6 2014 09:19:55