SemanticMapVisApplet.java
Go to the documentation of this file.
00001 package edu.tum.cs.ias.knowrob.vis.applets;
00002 
00003 import java.awt.Color;
00004 import java.awt.event.ActionEvent;
00005 import java.awt.event.ActionListener;
00006 import java.awt.event.KeyEvent;
00007 import java.awt.event.MouseEvent;
00008 import java.awt.event.MouseListener;
00009 import java.awt.event.MouseMotionListener;
00010 
00011 import java.util.ArrayList;
00012 import java.util.HashMap;
00013 import java.util.Vector;
00014 
00015 import javax.swing.JFrame;
00016 import javax.swing.JOptionPane;
00017 import javax.swing.Timer;
00018 import javax.vecmath.Matrix4d;
00019 import javax.vecmath.Vector3d;
00020 import javax.vecmath.Vector3f;
00021 
00022 import peasy.PeasyCam;
00023 import peasy.CameraState;
00024 import peasy.org.apache.commons.math.geometry.*;
00025 
00026 import processing.core.PFont;
00027 import processing.core.PGraphics3D;
00028 import processing.core.PMatrix;
00029 
00030 
00031 import edu.tum.cs.ias.knowrob.owl.OWLThing;
00032 import edu.tum.cs.ias.knowrob.prolog.PrologInterface;
00033 import edu.tum.cs.ias.knowrob.util.datastructures.Point;
00034 import edu.tum.cs.ias.knowrob.vis.AnimatedCanvas;
00035 import edu.tum.cs.ias.knowrob.vis.Canvas;
00036 import edu.tum.cs.ias.knowrob.vis.items.*;
00037 import edu.tum.cs.vis.model.ItemModel;
00038 import edu.tum.cs.vis.model.Properties;
00039 
00040 public class SemanticMapVisApplet extends AnimatedCanvas implements MouseListener, MouseMotionListener {
00041 
00042         static final long serialVersionUID=0;
00043 
00044         public static final double ROTATE_Y_DELTA = 0.05;
00045 
00047         // DISPLAY PROPERTIES (ROTATION, ZOOM, ...)
00048         //private float leftMouseX=-1.0f, leftMouseY=-1.0f, rightMouseX=-1.0f, rightMouseY=-1.0f, centerMouseY=-1.0f;
00049         //private float xRotDisplay=143.6f, yRotDisplay=-9.14f, xShiftDisplay=420.5f, yShiftDisplay=359.0f, zoomDisplay=1.19f;
00050 
00052         // SETTINGS
00053         //private String meshFile = "/home/tenorth/work/kipm/meshdata/unclassified300p.vtk";
00054 
00055 
00057         // MISC
00058         private PFont verdana;
00059         private PFont verdanaBold;
00060         private PFont dejavu;
00061         private ArrayList<int[]> colors = new ArrayList<int[]>(12);
00062         private MapObjClickListener objectClickListener = null;
00063         public MenuHuman menu_human = new MenuHuman();
00064 
00065 //      private Publisher<ros.pkg.std_msgs.msg.String> clickPub = null;
00066 //      private NodeHandle rosNode = null;
00067 //      private Ros ros = null;
00068         
00070         // COLOR STUFF / CLICK STUFF
00071         private int[] grayValues = new int[] {160,190,210,170,220,180,200,165,185,205,175,195,215};
00072         private static int grayLevelCounter = 0;
00073         private EmptyCanvas buffer;
00074         private JFrame bufferFrame;
00075         //private PMatrix bufferMatrix;
00076 
00077 
00079         // BUFFERS, DATA
00080 
00085         private HashMap<String,ItemBase> animatedItemsRef = new HashMap<String, ItemBase>();
00086         private ArrayList<ItemBase> allItems = new ArrayList<ItemBase>();
00087         private float[] framesToTimes;
00088 
00089 
00091         // INFO ABOUT CURRENT DISPLAY
00092         private int numberFrames = 1;
00093         private int currentFrame = 0;
00094 
00095         PeasyCam cam;
00096 
00097 
00098 
00100         // FLAGS
00101         public boolean isInitialized = false;
00102         private boolean playingForward = true;
00103         boolean record = false;
00104 
00105 
00109         // 
00110         // INITIALIZATION FUNCTIONS
00111         // 
00112 
00117         public void setup() {
00118 
00119                 size(700, 600, PGraphics3D.P3D);
00120 
00121 
00122                 // define camera parameters
00123                 cam = new PeasyCam(this, 0,0,0, 50);
00124 
00125                 // values for zooming (min distance to which one can zoom in)
00126                 cam.setMinimumDistance(0.01);
00127                 cam.setMaximumDistance(500);
00128 
00129                 cam.setRightDragHandler(cam.getPanDragHandler());
00130                 cam.setLeftDragHandler(cam.getRotateDragHandler());
00131                 cam.setWheelScale(0.2);
00132                 
00133                 // initialize camera view parameters
00134                 cam.setRotations(1.808507, -0.46793357, 0.056346834);
00135                 cam.setDistance(75.6322);
00136                 cam.lookAt(10.443167, -4.4923606, -3.8713489);
00137                 
00138                 frameRate(25);
00139                 buffer = new EmptyCanvas(700,600,PGraphics3D.P3D);
00140 
00141                 verdana = createFont("Verdana", 11);
00142                 verdanaBold = createFont("Verdana-Bold", 18);
00143                 dejavu = createFont("DejaVu Sans",13);
00144                 textFont(verdana);
00145                 textFont(verdanaBold);
00146                 textFont(dejavu);    
00147 
00148                 ellipseMode(RADIUS);
00149                 sphereDetail(10);
00150                 setColors();
00151 
00152                 hint(ENABLE_DEPTH_TEST);
00153                 //    hint(DISABLE_DEPTH_TEST);
00154 
00155                 // Don't enable, causes huge performance break
00156                 // hint(ENABLE_DEPTH_SORT);
00157                 //    hint(DISABLE_DEPTH_SORT);
00158 
00159                 //drawBackground();
00160                 draw();
00161                 isInitialized = true;
00162 
00163                                 
00164 //               // Generate a Publisher for the RDF Identifier that
00165 //               // currently was clicked.
00166 //              try {
00167 //                      ros = Ros.getInstance();
00168 //                      if(!ros.isInitialized()) {
00169 //                              ros.init("mod_vis");
00170 //                      }
00171 //                      rosNode = ros.createNodeHandle();
00172 //                      clickPub = rosNode.advertise("/mod_vis/mod_vis_clicked", new ros.pkg.std_msgs.msg.String(), 100);
00173 //              }
00174 //              catch(RosException e) {
00175 //                      e.printStackTrace();
00176 //                      ros.logError("Could not advertise /mod_vis_clicked topic.");
00177 //              }
00178 
00179         }
00180 
00181 
00182 
00183 
00187         // 
00188         // DRAW FUNCTIONS
00189         // 
00190 
00191         private void setLights()
00192         {
00193                 float camPos[] = cam.getPosition();
00194                 
00195                 lights();
00196                 pointLight(80f, 80f, 100f, camPos[0],camPos[1], camPos[2]);
00197         }
00198 
00202         @Override
00203         public void draw() {
00204                 PMatrix save = getMatrix();
00205                 try{
00206 
00207                         //System.out.println(cam.getRotations()[0] + ", " + cam.getRotations()[1] + ", " + cam.getRotations()[2]);
00208                         //System.out.println(cam.getDistance() + ", lookat  " + cam.getLookAt()[0] + ", " + cam.getLookAt()[1] + ", " + cam.getLookAt()[2] );
00209 
00210                         if (record) {
00211                                 beginRaw(PDF, "output.pdf");
00212                         }
00213 
00214                         scale(10);
00215 
00216                         background(40);
00217 
00218                         pushMatrix();
00219 
00220                         applyMatrix(    1,  0,  0, 0,
00221                                         0,  1,  0, 0,
00222                                         0,  0, -1, 0,
00223                                         0,  0,  0, 1);
00224                         //lights();
00225                         
00226                         setLights();
00227                         
00228 
00229                         strokeWeight(1.5f);
00230                         stroke(255, 0, 0);      line(0, 0, 0,   500, 0, 0);
00231                         stroke(0, 255, 0);      line(0, 0, 0,   0, 500, 0); 
00232                         stroke(0, 0, 255);      line(0, 0, 0,   0, 0, 500);
00233                         stroke(0, 0, 0);
00234 
00235                         // draw the meshes
00236                         //if(drawMeshes)
00237                         //      drawMeshes();
00238 
00239                         // draw all Items
00240                         for(int i=0;i<allItems.size();i++) {
00241                                 //hint(ENABLE_DEPTH_TEST);
00242                                 ItemBase it = allItems.get(i); 
00243                                 it.draw(this, currentFrame);
00244                                 
00245                         }
00246 
00247                         popMatrix();
00248 
00249                         if (record) {
00250                                 endRaw();
00251                                 System.err.println("Writing PDF...");
00252                                 record = false;
00253                         }
00254 
00255                 } catch(ArrayIndexOutOfBoundsException e) {
00256                         
00257                         System.out.println("Some Drawing error occured... ");
00258                         resetMatrix();
00259                         record = false;
00260                         applyMatrix(save);
00261                 }
00262         }
00263 
00264         private String getItemAt(int x, int y) {
00265 
00266                 String[] id = null;
00267                 int clicked = -1;
00268 
00269                 try{
00270 
00271 
00272                         buffer.setSize(700, 600);
00273 
00274 
00275                         buffer.resetMatrix();
00276                         cam.getState().apply(buffer);
00277                         buffer.noLights();
00278                         buffer.scale(10);
00279 
00280                         buffer.pushMatrix();        
00281                         //buffer.background(60, 60, 60);
00282 
00283 
00284                         buffer.applyMatrix(     1,  0,  0, 0,
00285                                         0,  1,  0, 0,
00286                                         0,  0, -1, 0,
00287                                         0,  0,  0, 1);
00288                         buffer.ellipseMode(RADIUS);
00289                         buffer.pushMatrix();
00290 
00291                         // draw all Items
00292                         id = new String[allItems.size()];
00293                         for(int i=0;i<allItems.size();i++) {
00294                                 ItemBase itm = allItems.get(i);
00295                                 itm.setOverrideColor(new Color(0xff000000 | ( 1*i + 1)));
00296                                 allItems.get(i).draw(buffer, currentFrame);
00297                                 itm.setOverrideColor(null);
00298                                 id[i] = itm.name;
00299                         }
00300 
00301                         // get color
00302                         clicked = ((buffer.get(x,y)-1) & 0x00ffffff)/1;
00303 
00304                         buffer.freeze();
00305                         buffer.redraw();
00306 
00307                         buffer.popMatrix();
00308                         buffer.popMatrix();
00309 
00310                 }catch(Exception e){}
00311 
00312                 if(clicked >= 0 && clicked<id.length)
00313                         return id[clicked];
00314                 else
00315                         return null;
00316         }
00317 
00321         /*private void drawTime(){
00322                 DecimalFormat df2 = new DecimalFormat( "##0.00" );
00323                 fill(0, 255, 0);
00324                 textFont(verdana);
00325                 textMode(SCREEN);
00326                 textAlign(LEFT); 
00327                 text("Action: "+queryString,10,50);
00328                 text("Duration: "+df2.format(startTime)+"-"+df2.format(endTime),10,62);
00329                 text("Frame: "+currentFrame+" / "+(numberFrames-1),10,86);
00330 
00331                 if(framesToTimes != null)
00332                         text("Time: "+df2.format(framesToTimes[currentFrame]), 10, 98);
00333         }*/
00334 
00339         private void displayMessage(String message){
00340                 System.out.println(message);
00341                 return;
00342         }
00343         public void setMapObjClickListener(MapObjClickListener c){
00344                 objectClickListener = c;
00345         }
00346 
00347 
00351         // 
00352         // METHODS FOR ADDING AND REMOVING OBJECTS
00353         // 
00354         //
00355 
00356 
00357         public void addItem(ItemBase item) {
00358                 this.allItems.add(item);
00359                 animatedItemsRef.put(item.name, item);
00360         }
00361 
00362         public void addObject(String identifier) {
00363 
00364                 ItemBase item = getItem(identifier);
00365 
00366                 if(item != null) {
00367                         removeObject(identifier);
00368                         addItem(item);
00369                 }
00370         }
00371 
00372         public void addObjectWithChildren(String identifier) {
00373                 
00374                 identifier = OWLThing.addSingleQuotes(OWLThing.removeSingleQuotes(identifier));
00375                 
00376                 HashMap<String, Vector<String>> physicalParts = PrologInterface.executeQuery(
00377                                 "rdf_reachable("+identifier+", knowrob:properPhysicalParts, PART)");
00378 
00379                 if(physicalParts!=null && physicalParts.get("PART") != null)
00380                         for(int i=0;i<physicalParts.get("PART").size();i++)
00381                                 if(!physicalParts.get("PART").get(i).toString().equals(identifier))
00382                                         addObject(physicalParts.get("PART").get(i));
00383 
00384                 HashMap<String, Vector<String>> mapParts = PrologInterface.executeQuery(
00385                                 "rdf_reachable(PART, knowrob:describedInMap, "+identifier+")");
00386 
00387 
00388                 if(mapParts!=null &&  mapParts.get("PART") != null)
00389                         for(int i=0;i<mapParts.get("PART").size();i++)
00390                                 if(!mapParts.get("PART").get(i).toString().equals(identifier))
00391                                 {
00392                                         HashMap<String, Vector<String>> parts = PrologInterface.executeQuery(
00393                                                         "rdf_reachable("+mapParts.get("PART").get(i).toString()+", knowrob:properPhysicalParts, P)");
00394 
00395                                         if(parts!=null) { 
00396                                                 Vector<String> p = parts.get("P");
00397 
00398                                                 if(p != null)
00399                                                         for(int j=0;j<p.size();j++)
00400                                                                 if(!p.get(j).toString().equals(mapParts.get("PART").get(i)))
00401                                                                         addObject(p.get(j).toString());
00402 
00403                                                 addObject(mapParts.get("PART").get(i).toString());      
00404                                         }
00405                                 }
00406                 addObject(identifier);
00407 
00408         }
00409 
00410 
00411         public void removeItem(ItemBase item) {
00412                 allItems.remove(item);
00413                 animatedItemsRef.remove(item.name);
00414         }
00415 
00416 
00417         public void removeObject(String identifier) {
00418 
00419                 ItemBase item = animatedItemsRef.get(PrologInterface.addSingleQuotes(identifier));
00420                 if(item == null) {
00421                         return;
00422                 }
00423                 removeItem(item);
00424         }
00425 
00426 
00427         public void removeObjectWithChildren(String identifier) {
00428                 HashMap<String, Vector<String>> physicalParts = PrologInterface.executeQuery(
00429                                 "rdf_reachable("+identifier+", knowrob:properPhysicalParts, PART)");
00430 
00431                 if(physicalParts!=null && physicalParts.get("PART") != null)
00432                         for(int i=0;i<physicalParts.get("PART").size();i++)
00433                                 if(!physicalParts.get("PART").get(i).equals(identifier))
00434                                         removeObject(physicalParts.get("PART").get(i));
00435 
00436                 HashMap<String, Vector<String>> mapParts = PrologInterface.executeQuery(
00437                                 "rdf_reachable(PART, knowrob:describedInMap, "+identifier+")");
00438 
00439                 if(mapParts != null && mapParts.get("PART") != null)
00440                         for(int i=0;i<mapParts.get("PART").size();i++)
00441                                 if(!mapParts.get("PART").get(i).equals(identifier))
00442                                 {
00443                                         HashMap<String, Vector<String>> parts = PrologInterface.executeQuery(
00444                                                         "rdf_reachable("+mapParts.get("PART").get(i)+", knowrob:properPhysicalParts, P)");
00445 
00446                                         if(parts != null) {
00447                                                 Vector<String> p = parts.get("P");
00448 
00449                                                 if(p != null)
00450                                                         for(int j=0;j<p.size();j++)
00451                                                                 if(!p.get(j).toString().equals(mapParts.get("PART").get(i)))
00452                                                                         addObject(p.get(j));
00453 
00454                                                 removeObject(mapParts.get("PART").get(i));      
00455                                         }
00456                                 }
00457                 removeObject(identifier);
00458 
00459         }
00460 
00461 
00465         // 
00466         // METHODS FOR HIGHLIGHTING OBJECTS
00467         // 
00468         //
00469 
00470 
00471 
00476         public void highlightItem(String identifier, boolean highlight, int color) {
00477                 ItemBase itm = animatedItemsRef.get(identifier);
00478                 if(itm == null) {
00479                         displayMessage("item "+identifier+" not in scene");
00480                         return;
00481                 }
00482 
00483                 if(highlight)
00484                         itm.setColor(color);
00485                 else
00486                         itm.setColor(itm.defaultColor);
00487         }
00488 
00489         public void highlightItem(String identifier, boolean highlight) {
00490                 highlightItem(identifier, highlight, convertColor(255, 0, 0, 255));
00491         }
00492 
00496         public void highlightReachable(String identifier, boolean highlight) {
00497                 highlightItem(identifier, highlight);
00498 
00499                 HashMap<String, Vector<String>> others = PrologInterface.executeQuery(
00500                                 "rdf_reachable("+identifier+", knowrob:properPhysicalParts, I)");
00501 
00502                 if(others == null) return;
00503                 
00504                 Vector<String> itms = others.get("I");
00505                 if(itms == null) return;
00506 
00507                 for(String o : itms)
00508                         highlightItem(o, highlight);
00509 
00510         }
00511 
00512         public void clearHighlights() {
00513                 // draw all Items
00514                 for(String j : animatedItemsRef.keySet()) {
00515                         animatedItemsRef.get(j).setColor(animatedItemsRef.get(j).defaultColor);
00516                 }
00517         }
00518 
00519 
00520 
00521 
00522 
00526         // 
00527         // METHODS FOR ADDING AND REMOVING ACTION AND TRAJECTORY INFORMATION
00528         // 
00529         //
00530 
00531 
00538         public void displayAction(String identifier)
00539         {
00540                 // reset everything
00541                 clear();
00542                 addObjectWithChildren("'http://ias.cs.tum.edu/kb/ccrl2_semantic_map.owl#SemanticEnvironmentMap0'");
00543                 addObjectWithChildren("'http://ias.cs.tum.edu/kb/ias_map_addons.owl#table0'");
00544 
00545 
00546                 addObject("'http://ias.cs.tum.edu/kb/knowrob.owl#placemat1'");
00547                 addObject("'http://ias.cs.tum.edu/kb/ias_entities.owl#napkin1'");
00548                 addObject("'http://ias.cs.tum.edu/kb/ias_entities.owl#spoon1'");
00549                 addObject("'http://ias.cs.tum.edu/kb/ias_entities.owl#plate1'");
00550                 addObject("'http://ias.cs.tum.edu/kb/ias_entities.owl#fork1'");
00551                 addObject("'http://ias.cs.tum.edu/kb/ias_entities.owl#knife1'");
00552 
00553                 try{
00554                         HashMap<String, Vector<String>> c = PrologInterface.executeQuery(
00555                                         "rdf_has("+identifier+", 'http://ias.cs.tum.edu/kb/knowrob.owl#startTime', S), "
00556                                                         +"rdf_has("+identifier+", 'http://ias.cs.tum.edu/kb/knowrob.owl#endTime', E)"); 
00557 
00558                         if(c==null) return;
00559                         
00560                         String startTimeStr = (String)c.get("S").get(0);
00561                         startTimeStr = startTimeStr.substring(1,startTimeStr.length()-1);
00562 
00563                         if(startTimeStr.contains("#")) {
00564                                 startTimeStr=startTimeStr.split("#")[1];
00565                                 startTimeStr=startTimeStr.split("_")[1];
00566                         }
00567                         System.out.println(startTimeStr);
00568 
00569                         float startTime = Float.parseFloat(startTimeStr);
00570 
00571 
00572                         String endTimeStr = (String)c.get("E").get(0);
00573                         endTimeStr = endTimeStr.substring(1,endTimeStr.length()-1);
00574                         if(endTimeStr.contains("#")) {
00575                                 endTimeStr=endTimeStr.split("#")[1];
00576                                 endTimeStr=endTimeStr.split("_")[1];
00577                         }
00578                         float endTime = Float.parseFloat(endTimeStr);
00579 
00580 
00581                         c = PrologInterface.executeQuery(
00582                                         "rdf_has(A, rdf:type, knowrob:'Posture-Configuration'), rdf_has(A, knowrob:bodyPartsUsed, 'left'), rdf_triple(knowrob:startTime, A, T)");       
00583 
00584                         if(c==null) return;
00585                         
00586                         Vector<String> allTimePoints = c.get("T");
00587                         BodyPoseSequence skeleton = new BodyPoseSequence();
00588                         float[] poses = null;
00589                         int totalFrames = 0;
00590 
00591                         //                      System.out.println("total timepoints: "+allTimePoints.size()+", start: "+startTime+", end: "+endTime);
00592 
00593                         for(String s : allTimePoints) {
00594 
00595 
00596                                 String timePointStr = s.substring(1, s.length()-1);
00597                                 timePointStr = timePointStr.substring(1,timePointStr.length()-1);
00598                                 if(timePointStr.contains("#")) {
00599                                         timePointStr=timePointStr.split("#")[1];
00600                                         timePointStr=timePointStr.split("_")[1];
00601                                 }
00602                                 float timePoint = Float.parseFloat(timePointStr);
00603 
00604                                 if(timePoint >= startTime && timePoint <= endTime) {
00605                                         try{            
00606                                                 HashMap<String, Vector<String>> frameInfo = PrologInterface.executeQuery(
00607                                                                 "rdf_triple(ias_human:postureAtTimePoint,  "+s.toString()+", P)");
00608                                                 
00609                                                 if(frameInfo!=null) {
00610                                                         if(frameInfo.get("P") == null || frameInfo.get("P").size() == 0){
00611                                                                 if(poses != null){
00612                                                                         skeleton.addPose(0, timePoint, "", poses);
00613                                                                         System.out.println("pose at point "+s.toString()+" not found; re-added previous pose.");
00614                                                                 }
00615                                                                 else
00616                                                                 {
00617                                                                         System.out.println("pose at point "+s.toString()+" not found.");
00618                                                                 }
00619                                                                 continue;
00620                                                         }
00621 
00622                                                         String[] split = frameInfo.get("P").get(0).split("\\)-\\(");
00623 
00624                                                         poses = new float[84];                  
00625 
00626                                                         for(int j=0;j<83;j++) {
00627                                                                 poses[j] = Float.parseFloat(split[j+4]);
00628                                                         }
00629                                                         poses[83] = Float.parseFloat(split[87].substring(0,split[87].length()-2));
00630                                                         skeleton.addPose(0, timePoint, "", poses);
00631                                                         totalFrames++;
00632                                                         //System.out.println("added pose at point: "+s.toString());
00633                                                 }
00634                                         }catch (Exception e) {
00635                                                 e.printStackTrace();
00636                                                 System.out.println("ERROR adding point: "+s.toString());
00637                                         }                                       
00638                                 }
00639 
00640                         }
00641 
00642                         allItems.add(skeleton);
00643                         animatedItemsRef.put("skeleton", skeleton);
00644 
00645                         numberFrames = totalFrames;
00646 
00647                         // add fromLocation and toLocation as colored spheres, if they are present
00648                         c = PrologInterface.executeQuery(
00649                                         "rdf_triple('http://ias.cs.tum.edu/kb/knowrob.owl#fromLocation', "+identifier+", From)");
00650                         if(c!=null && c.get("From")!= null) {
00651                                 String fromLoc = (String)c.get("From").get(0);
00652                                 if(fromLoc!=null) {
00653                                         addObject(fromLoc);
00654                                 }
00655                         }
00656 
00657                         c = PrologInterface.executeQuery(
00658                                         "rdf_triple('http://ias.cs.tum.edu/kb/knowrob.owl#toLocation', "+identifier+", To)");
00659                         if(c!=null && c.get("To")!=null) {
00660                                 String toLoc = (String)c.get("To").get(0);
00661                                 if(toLoc!=null) {
00662                                         addObject(toLoc);
00663                                 }
00664                         }
00665 
00666                 }
00667                 catch(NullPointerException e)
00668                 {
00669                         displayMessage("Action "+identifier+" not found.");
00670                         drawBackground();
00671                 }
00672                 catch(Exception e)
00673                 {
00674                         displayMessage("Error displaying Action "+identifier+".");
00675                         e.printStackTrace();
00676                         drawBackground();
00677                 }
00678         }
00679 
00686         public void displayHumanTrajectory(String identifier, String handUsed) {
00687                 Integer Occ=0, auxOcc=0;
00688                 boolean band=true;
00689                 int row=0, contP=0, col=0;
00690                 float[][] Xp=new float[80][19];
00691                 float[][] Yp=new float[80][19];
00692                 float[][] Zp=new float[80][19];
00693 
00694                 Trajectories traj = new Trajectories();
00695 
00696                 HashMap<String, Vector<String>> o= PrologInterface.executeQuery(""+"handTrajectory("+handUsed+", "+identifier+", T, P, X, Y, Z)");
00697                 if(o==null) return;
00698                 
00699                 Vector<String> pointID = o.get("P");
00700                 Vector<String> Xc      = o.get("X");
00701                 Vector<String> Yc      = o.get("Y");
00702                 Vector<String> Zc      = o.get("Z");
00703 
00704                 for (Object p: pointID){
00705                         String s_p=p.toString(); //example s_p=p_1_2_36 (Ep, Occ, Inst)
00706                         String[] tokenP=s_p.split("_");
00707                         if (Occ.equals(Integer.parseInt(tokenP[2])) && band==true){
00708                                 Xp[row][col]=Float.parseFloat(Xc.get(contP));
00709                                 Yp[row][col]=Float.parseFloat(Yc.get(contP));
00710                                 Zp[row][col]=Float.parseFloat(Zc.get(contP));
00711                                 row=row+1;
00712                                 //System.out.println("Xp ="+Float.parseFloat(Xc.get(contP).toString()));
00713                         } else {
00714                                 Occ=Integer.parseInt(tokenP[2]);
00715                                 if (Occ > auxOcc) {
00716                                         col=col+1; row=0; band=true; auxOcc=Occ;
00717                                         System.out.println("Occ=" +Occ);
00718                                 } else {band=false;}
00719 
00720                         }
00721                         contP=contP+1;  
00722                 }
00723                 System.out.println("columnas"+Xp[0].length);
00724 
00725                 for (int j=0;j<Xp[0].length;j++ ){ //number of columns from Xp
00726                         //System.out.println("New traj");
00727                         for (int i=0;i<(Xp.length)-1; i++) {
00728                                 if (Xp[i][j]!=0.0 && Yp[i][j]!=0){
00729                                         traj.addTraj(Xp[i][j], Yp[i][j], Zp[i][j]);
00730                                         //System.out.println("traj= "+Xp[i][j]);
00731                                 }
00732                         }
00733                         traj.addTraj(0, 0, 0);
00734 
00735                 }
00736                 allItems.add(traj);
00737                 animatedItemsRef.put("traj", traj); 
00738         }
00739 
00744         public void displayEyeTrajectory (String identifier) {
00745                 Integer Occ=1, auxOcc=1; 
00746                 int col=0, contP=0 ; 
00747                 int[][] Xc, Yc;
00748                 boolean band=true;
00749 
00750                 Trajectories traj = new Trajectories();
00751 
00752                 HashMap<String, Vector<String>> o= PrologInterface.executeQuery(""+ "readEyeTrajectory("+identifier+", T, P, XCoor, YCoor)");
00753                 if(o==null) return;
00754                 
00755                 Vector<String> pointId = o.get("P");
00756                 Vector<String> XCoor = o.get("XCoor");
00757                 Vector<String> YCoor = o.get("YCoor");
00758                 Xc= new int[35][4];
00759                 Yc= new int[35][4];
00760                 for (String p: pointId){
00761                         String s_p=p.toString(); //example s_p=p_1_0
00762                         String[] tokenP=s_p.split("_");
00763                         // example: tokenP[0]=p, tokenP[1]=1 (occurrence), tokenP[2]=0 (instance)
00764                         if (Occ.equals(Integer.parseInt(tokenP[1])) && band==true) {
00765                                 //Save the values of the coordinates in 2 vectors
00766                                 //displayMessage("instance_nr: "+ Integer.parseInt(tokenP[2]));
00767                                 Xc[Integer.parseInt(tokenP[2])][col] = Integer.parseInt(XCoor.get(contP));
00768                                 Yc[Integer.parseInt(tokenP[2])][col] = Integer.parseInt(YCoor.get(contP));
00769                                 //displayMessage("Xc["+Integer.parseInt(tokenP[2])+"]["+col+"]= "+Xc[Integer.parseInt(tokenP[2])][col]);
00770 
00771 
00772                         } else {
00773                                 Occ=Integer.parseInt(tokenP[1]); //new occurrence_nr
00774                                 if (Occ>auxOcc) {
00775                                         col=Occ-1; band=true; auxOcc=Occ; //displayMessage("auxOcc: "+auxOcc);
00776                                         Xc[Integer.parseInt(tokenP[2])][col] = Integer.parseInt(XCoor.get(contP));
00777                                         Yc[Integer.parseInt(tokenP[2])][col] = Integer.parseInt(YCoor.get(contP));
00778                                 }else{ band=false;}
00779                         }
00780                         contP=contP+1;
00781                 }
00782 
00783                 for (int j=0;j<Xc[0].length;j++ ){
00784                         for (int i=0;i<(Xc.length)-1; i++) {
00785                                 traj.addTraj(Xc[i][j], Yc[i][j], 0);
00786 
00787                         }
00788                 }
00789                 allItems.add(traj);
00790                 animatedItemsRef.put("traj", traj);
00791 
00792         }
00793 
00800         public void displayActionFixedIdent(String identifier)
00801         {
00802                 HashMap<String, Vector<String>> c = PrologInterface.executeQuery(""+
00803                                 "readPostureSeqForAction("+identifier+", P, EPISODENR, INSTANCENR, TIME, BECX, BECY, BECZ, ULWX, ULWY, ULWZ, OLWX, OLWY, OLWZ, UBWX, UBWY, UBWZ, OBWX, OBWY, OBWZ, UHWX, UHWY, UHWZ, BRKX, BRKY, BRKZ, OHWX, OHWY, OHWZ, KOX, KOY, KOZ, SEHX, SEHY, SEHZ, OSLX, OSLY, OSLZ, USLX, USLY, USLZ, FULX, FULY, FULZ, FBLX, FBLY, FBLZ, OSRX, OSRY, OSRZ, USRX, USRY, USRZ, FURX, FURY, FURZ, FBRX, FBRY, FBRZ, SBLX, SBLY, SBLZ, OALX, OALY, OALZ, UALX, UALY, UALZ, HALX, HALY, HALZ, FILX, FILY, FILZ, SBRX, SBRY, SBRZ, OARX, OARY, OARZ, UARX, UARY, UARZ, HARX, HARY, HARZ, FIRX, FIRY, FIRZ)");                
00804 
00805                 if(c==null || c.get("TIME") == null)
00806                 {
00807                         drawBackground();
00808                         displayMessage("could not find Action "+identifier+"");
00809                         numberFrames = 1;
00810                         return;
00811                 }
00812 
00813 
00814                 Vector<String> times = c.get("TIME");
00815                 framesToTimes = new float[times.size()];
00816                 for(int i=0;i<framesToTimes.length;i++) framesToTimes[i] = Float.parseFloat(times.get(i));
00817 
00818                 BodyPoseSequence skeleton = new BodyPoseSequence();
00819 
00820                 float[][] poses = new float[times.size()][];
00821                 for(int i=0;i<poses.length;i++) poses[i] = new float[84];
00822                 String[] posIdents = "BECX, BECY, BECZ, ULWX, ULWY, ULWZ, OLWX, OLWY, OLWZ, UBWX, UBWY, UBWZ, OBWX, OBWY, OBWZ, UHWX, UHWY, UHWZ, BRKX, BRKY, BRKZ, OHWX, OHWY, OHWZ, KOX, KOY, KOZ, SEHX, SEHY, SEHZ, OSLX, OSLY, OSLZ, USLX, USLY, USLZ, FULX, FULY, FULZ, FBLX, FBLY, FBLZ, OSRX, OSRY, OSRZ, USRX, USRY, USRZ, FURX, FURY, FURZ, FBRX, FBRY, FBRZ, SBLX, SBLY, SBLZ, OALX, OALY, OALZ, UALX, UALY, UALZ, HALX, HALY, HALZ, FILX, FILY, FILZ, SBRX, SBRY, SBRZ, OARX, OARY, OARZ, UARX, UARY, UARZ, HARX, HARY, HARZ, FIRX, FIRY, FIRZ".split(", ");
00823 
00824                 for(int j=0;j<84;j++) {
00825                         Vector<String> d = c.get(posIdents[j]);
00826                         for(int i=0;i<d.size() && i<poses.length;i++) {
00827                                 poses[i][j] = Float.parseFloat(d.get(i));
00828                         }
00829                 }
00830 
00831                 for(int i=0;i<poses.length;i++) 
00832                         skeleton.addPose(0, Float.parseFloat(times.get(i).toString()), "", poses[i]);
00833 
00834                 allItems.add(skeleton);
00835                 animatedItemsRef.put("skeleton", skeleton);
00836 
00837                 //startTime = Double.parseDouble(times.get(0).toString());
00838                 //endTime = Double.parseDouble(times.get(times.size()-1).toString());
00839                 numberFrames = times.size();
00840 
00841                 System.out.print("ALL DONE");
00842         }
00843 
00844 
00845 
00846 
00847 
00854         public void drawBackground() {
00855                 clear();
00856                 // get the current semantic map
00857                 HashMap<String, Vector<String>> tpe = PrologInterface.executeQuery(
00858                                 "rdf_has(SUBJECT, rdf:type, knowrob:'SemanticEnvironmentMap')");
00859                 String type = null;
00860 
00861                 // check if exists
00862                 try{
00863                         type = tpe.get("SUBJECT").get(0);
00864                         addObjectWithChildren(type);
00865                 } catch(Exception e) {
00866                         displayMessage("Semantic map not found");
00867                 }  
00868         }
00869 
00870 
00874         public void clear() {
00875                 allItems.clear();
00876                 animatedItemsRef.clear();
00877                 numberFrames = 1;
00878                 currentFrame = 0;
00879                 grayLevelCounter = 0;
00880         }
00881 
00882 
00883 
00884 
00885 
00886 
00887 
00891         // 
00892         // INTERNAL HELPERS
00893         // 
00894 
00895 
00896         private ItemBase getItem(String identifier) {
00897 
00898                 // make sure identifier is surrounded with single quotes:
00899                 identifier = PrologInterface.addSingleQuotes(PrologInterface.removeSingleQuotes(identifier));
00900                 
00901                 // get type
00902                 HashMap<String, Vector<String>> tpe = PrologInterface.executeQuery(
00903                                 "rdf_has("+identifier+", rdf:type, OBJECTCLASS)," + 
00904                                                 "OBJECTCLASS\\='http://www.w3.org/2002/07/owl#NamedIndividual'");
00905                 String type = null;
00906 
00907 
00908                 // check if exists
00909                 try{
00910                         type = tpe.get("OBJECTCLASS").get(0);
00911                 } catch(Exception e) {
00912                         displayMessage("item "+identifier+" not found");
00913                         return null;
00914                 }
00915 
00916                 // ignore list (not displayable):
00917                 if(type.equals("'http://ias.cs.tum.edu/kb/knowrob.owl#SemanticEnvironmentMap'")
00918                                 || type.equals("'http://ias.cs.tum.edu/kb/knowrob.owl#Vector'")
00919                                 || type.equals("'http://ias.cs.tum.edu/kb/knowrob.owl#WallOfAConstruction'")
00920                                 ) {
00921                         return null;
00922 
00923                 }
00924                 
00925                 if(type.endsWith("#OccupancyGridMap'")) {
00926                         return new OccupancyGridMap(identifier);
00927                 }
00928                 
00929                 Matrix4d pose = null;
00930                 Vector3d dim = null;
00931                 
00932                 
00933                 double[] p = getOrientationOfItem(identifier);
00934                 if(p!=null) {
00935                         pose = new Matrix4d(p);
00936                 }
00937                 
00938                 double[] v = getDimensionsOfItem(identifier);
00939                 if(v!=null) {
00940                         dim = new Vector3d(v);
00941                 }
00942 
00943                 // check whether CAD model is specified:
00944                 ItemModel model = Properties.getModelOfItem(identifier);
00945                 
00946                 
00947                 if (model != null && model.getParser()!=null) {
00948                         CadModelItem it = new CadModelItem(pose, dim);
00949                         it.name = identifier;
00950                         it.setModel(model);
00951                         return it;
00952                 }
00953                 
00954                 
00955                 // else: check if one of the simple types
00956                 ItemBase it = itemForObjType(type);
00957 
00958                 if(it!=null) {
00959                         
00960                         it.name = identifier;
00961                         if(pose!=null)
00962                                 it.setPose(pose);
00963                         
00964                         if(dim!=null)
00965                                 it.setDimensions((float)dim.x, (float)dim.y, (float)dim.z);
00966 
00967                         return it;
00968 
00969                         
00970                 } else {
00971 
00972                         // check if it is a storage facility, if it is: create; WITHOUT handles
00973                         HashMap<String, Vector<String>> storage = PrologInterface.executeQuery(
00974                                         "rdf_has("+identifier+", rdf:type, OBJECTCLASS)," +
00975                                                         "rdf_reachable(OBJECTCLASS, rdfs:subClassOf, knowrob:'StorageConstruct')");
00976                         if (storage !=null && storage.get("OBJECTCLASS") != null && storage.get("OBJECTCLASS").size() > 0) {
00977                                 
00978                                 if(storage.get("OBJECTCLASS").get(0).endsWith("Cupboard'")) {
00979                                         it = new Cupboard(pose, dim);
00980                                 } else {
00981                                         it = new Drawer(pose, dim);
00982                                 }
00983                                 
00984                                 int col = grayValues[(++grayLevelCounter) % grayValues.length];
00985 
00986                                 it.defaultColor = convertColor(col, col, col, 170);
00987                                 it.setColor(it.defaultColor);
00988                                 it.name = identifier;
00989 
00990                                 return it;
00991                         }
00992 
00993 
00994 
00995                         // check if Table
00996                         HashMap<String, Vector<String>> tables = PrologInterface.executeQuery(
00997                                         "rdf_has("+identifier+", rdf:type, OBJECTCLASS)," +
00998                                                         "rdf_reachable(OBJECTCLASS, rdfs:subClassOf, knowrob:'Table-PieceOfFurniture')");
00999                         
01000                         if (tables !=null && tables.get("OBJECTCLASS") != null && tables.get("OBJECTCLASS").size() > 0)
01001                         {
01002                                 it =  new Table(pose, dim);
01003 
01004                                 int col = grayValues[(++grayLevelCounter) % grayValues.length];      
01005                                 it.defaultColor = convertColor(col, col, col, 255);
01006                                 it.setColor(it.defaultColor);
01007                                 it.name = identifier;
01008                                 return it;
01009                         }
01010 
01011 
01012                         // check if it is a CounterTop
01013                         HashMap<String, Vector<String>> counter = PrologInterface.executeQuery(
01014                                         "rdf_has("+identifier+", rdf:type, OBJECTCLASS)," +
01015                                                         "rdf_reachable(OBJECTCLASS, rdfs:subClassOf, knowrob:'CounterTop')");
01016 
01017                         if (counter !=null && counter.get("OBJECTCLASS") != null && counter.get("OBJECTCLASS").size() > 0) {
01018 
01019                                 it = new CounterTop(pose, dim);
01020 
01021                                 int col = grayValues[(++grayLevelCounter) % grayValues.length];      
01022                                 it.defaultColor = convertColor(col, col, col, 170);
01023                                 it.setColor(it.defaultColor);
01024                                 it.name = identifier;
01025 
01026                                 return it;
01027                         }
01028 
01029                         
01030                         
01031                         HashMap<String, Vector<String>> building = PrologInterface.executeQuery(
01032                                         "rdf_has("+identifier+", rdf:type, OBJECTCLASS)," +
01033                                                         "rdf_reachable(OBJECTCLASS, rdfs:subClassOf, knowrob:'Building')");
01034 
01035                         if(building !=null && building.get("OBJECTCLASS") != null && building.get("OBJECTCLASS").size() > 0) {
01036 
01037                                 it = new Building(pose, dim);
01038 
01039                                 int col = grayValues[(++grayLevelCounter) % grayValues.length];      
01040                                 it.defaultColor = convertColor(col, col, col, 255);
01041                                 it.setColor(it.defaultColor);
01042                                 it.name = identifier;
01043 
01044                                 return it;
01045                         }
01046 
01047 
01048                         HashMap<String, Vector<String>> level = PrologInterface.executeQuery(
01049                                         "rdf_has("+identifier+", rdf:type, OBJECTCLASS)," +
01050                                                         "rdf_reachable(OBJECTCLASS, rdfs:subClassOf, knowrob:'LevelOfAConstruction')");
01051 
01052                         if(level !=null && level.get("OBJECTCLASS") != null && level.get("OBJECTCLASS").size() > 0) {
01053 
01054                                 it = new LevelOfAConstruction(pose, dim);
01055 
01056                                 int col = grayValues[(++grayLevelCounter) % grayValues.length];      
01057                                 it.defaultColor = convertColor(col, col, col, 255);
01058                                 it.setColor(it.defaultColor);
01059                                 it.name = identifier;
01060 
01061                                 return it;
01062                         }
01063 
01064 
01065 
01066                         HashMap<String, Vector<String>> room = PrologInterface.executeQuery(
01067                                         "rdf_has("+identifier+", rdf:type, OBJECTCLASS)," +
01068                                                         "( rdf_reachable(OBJECTCLASS, rdfs:subClassOf, knowrob:'RoomInAConstruction');" +
01069                                                         "  rdf_reachable(OBJECTCLASS, rdfs:subClassOf, knowrob:'MultiRoomUnit') )");
01070 
01071                         if(room !=null && room.get("OBJECTCLASS") != null && room.get("OBJECTCLASS").size() > 0) {
01072 
01073                                 it = new RoomInAConstruction(pose, dim);
01074 
01075                                 int col = grayValues[(++grayLevelCounter) % grayValues.length];      
01076                                 it.defaultColor = convertColor(col, col, col, 255);
01077                                 it.setColor(it.defaultColor);
01078                                 it.name = identifier;
01079 
01080                                 return it;
01081                         }
01082 
01083                         
01084                         
01085 //                      HashMap<String, Vector<String>> place = PrologInterface.executeQuery(
01086 //                                      "rdf_has("+identifier+", rdf:type, OBJECTCLASS)," +
01087 //                                                      "rdf_reachable(OBJECTCLASS, rdfs:subClassOf, knowrob:'Place')");
01088 //
01089 //                      if(place !=null && place.get("OBJECTCLASS") != null && place.get("OBJECTCLASS").size() > 0) {
01090 //
01091 //                              it = new Pose(new Matrix4d(pose), new Vector3d(0.35f,0.35f,0.0f));
01092 //
01093 //                              int col = grayValues[(++grayLevelCounter) % grayValues.length];      
01094 //                              it.defaultColor = convertColor(col, col, col, 255);
01095 //                              it.setColor(it.defaultColor);
01096 //                              it.name = identifier;
01097 //
01098 //                              return it;
01099 //                      }
01100 
01101                         
01102                         // check if it is some kind of box (e.g. Bed)
01103                         HashMap<String, Vector<String>> box = PrologInterface.executeQuery(
01104                                         "rdf_has("+identifier+", rdf:type, OBJECTCLASS)," +
01105                                                         "( rdf_reachable(OBJECTCLASS, rdfs:subClassOf, knowrob:'ConstructionArtifact');" +
01106                                                         "  rdf_reachable(OBJECTCLASS, rdfs:subClassOf, knowrob:'Box-Container');" +
01107                                                         "rdf_reachable(OBJECTCLASS, rdfs:subClassOf, knowrob:'FurniturePiece'))");
01108 
01109                         if(box !=null && box.get("OBJECTCLASS") != null && box.get("OBJECTCLASS").size() > 0) {
01110 
01111                                 it = new Box(pose, dim);
01112 
01113                                 int col = grayValues[(++grayLevelCounter) % grayValues.length];      
01114                                 it.defaultColor = convertColor(col, col, col, 255);
01115                                 it.setColor(it.defaultColor);
01116                                 it.name = identifier;
01117 
01118                                 return it;
01119                         }
01120                         
01121                         // check if it is a Door
01122                         HashMap<String, Vector<String>> door = PrologInterface.executeQuery(
01123                                         "rdf_has("+identifier+", rdf:type, OBJECTCLASS)," +
01124                                         "rdf_reachable(OBJECTCLASS, rdfs:subClassOf, knowrob:'Door')");
01125 
01126                         if(door !=null && door.get("OBJECTCLASS") != null && door.get("OBJECTCLASS").size() > 0) {
01127 
01128                                 it = new Door(pose, dim);
01129                                 it.defaultColor = convertColor(255, 175, 0, 255);
01130                                 it.setColor(it.defaultColor);
01131                                 it.name = identifier;
01132 
01133                                 return it;
01134                         }
01135 
01136                         // check if it is a cylinder
01137                         HashMap<String, Vector<String>> cyl = PrologInterface.executeQuery(
01138                                         "rdf_has("+identifier+", rdf:type, OBJECTCLASS)," +
01139                                         "rdf_reachable(OBJECTCLASS, rdfs:subClassOf, knowrob:'Cylinder')," +
01140                                         "rdf_triple(knowrob:longitudinalDirection, C, Dir)," +
01141                                         "rdf_has(Dir, knowrob:vectorX, literal(type(xsd:'float',DirX)))," +
01142                                         "rdf_has(Dir, knowrob:vectorY, literal(type(xsd:'float',DirY)))," +
01143                                         "rdf_has(Dir, knowrob:vectorZ, literal(type(xsd:'float',DirZ)))");
01144 
01145                         if(cyl !=null && cyl.get("OBJECTCLASS") != null && cyl.get("OBJECTCLASS").size() > 0) {
01146 
01147                                 Vector3d dir = new Vector3d(Double.valueOf(cyl.get("DirX").get(0)), 
01148                                                 Double.valueOf(cyl.get("DirY").get(0)), 
01149                                                 Double.valueOf(cyl.get("DirZ").get(0)));
01150                                 
01151                                 it = new Cone(pose, new Vector3d(), dir, 0.01f,0.01f,0.04f);
01152                                 it.defaultColor = convertColor(255, 225, 0, 255);
01153                                 it.setColor(it.defaultColor);
01154                                 it.name = identifier;
01155 
01156                                 return it;
01157                         }
01158 
01159                 }
01160                 displayMessage("could not find how to add Item "+identifier+"\nType was: "+type);
01161                 return null;
01162 
01163         }
01164 
01165         private double[] getDimensionsOfItem(String identifier) {
01166                 try{
01167                         HashMap<String, Vector<String>> nfo = PrologInterface.executeQuery(
01168                                         "rdf_has("+identifier+",knowrob:widthOfObject,literal(type(_,_W)))," + 
01169                                                         "rdf_has("+identifier+",knowrob:heightOfObject,literal(type(_,_H))), " + 
01170                                                         "rdf_has("+identifier+",knowrob:depthOfObject,literal(type(_,_D))), " +
01171                                                         "atom_to_term(_W,W,_), atom_to_term(_H,H,_), atom_to_term(_D,D,_)");
01172 
01173                         return new double[] {
01174                                         Double.valueOf(nfo.get("D").get(0)),
01175                                         Double.valueOf(nfo.get("W").get(0)),
01176                                         Double.valueOf(nfo.get("H").get(0))};
01177 
01178                 } catch(Exception e) {
01179                         return null;
01180                 }
01181         }
01182 
01183 
01184         private double[] getOrientationOfItem(String identifier) {
01185                 try{
01186                         
01187                         // get orientation
01188                         HashMap<String, Vector<String>> nfo = PrologInterface.executeQuery(
01189                                         "rdf_triple(knowrob:orientation,"+identifier+",Or), " +
01190 
01191                                         "rdf_triple(knowrob:m00,Or,_M00l), util:strip_literal_type(_M00l, _M00), term_to_atom(M00,_M00)," +
01192                                         "rdf_triple(knowrob:m01,Or,_M01l), util:strip_literal_type(_M01l, _M01), term_to_atom(M01,_M01)," +
01193                                         "rdf_triple(knowrob:m02,Or,_M02l), util:strip_literal_type(_M02l, _M02), term_to_atom(M02,_M02)," +
01194                                         "rdf_triple(knowrob:m03,Or,_M03l), util:strip_literal_type(_M03l, _M03), term_to_atom(M03,_M03)," +
01195 
01196                                         "rdf_triple(knowrob:m10,Or,_M10l), util:strip_literal_type(_M10l, _M10), term_to_atom(M10,_M10)," +
01197                                         "rdf_triple(knowrob:m11,Or,_M11l), util:strip_literal_type(_M11l, _M11), term_to_atom(M11,_M11)," +
01198                                         "rdf_triple(knowrob:m12,Or,_M12l), util:strip_literal_type(_M12l, _M12), term_to_atom(M12,_M12)," +
01199                                         "rdf_triple(knowrob:m13,Or,_M13l), util:strip_literal_type(_M13l, _M13), term_to_atom(M13,_M13)," +
01200 
01201                                         "rdf_triple(knowrob:m20,Or,_M20l), util:strip_literal_type(_M20l, _M20), term_to_atom(M20,_M20)," +
01202                                         "rdf_triple(knowrob:m21,Or,_M21l), util:strip_literal_type(_M21l, _M21), term_to_atom(M21,_M21)," +
01203                                         "rdf_triple(knowrob:m22,Or,_M22l), util:strip_literal_type(_M22l, _M22), term_to_atom(M22,_M22)," +
01204                                         "rdf_triple(knowrob:m23,Or,_M23l), util:strip_literal_type(_M23l, _M23), term_to_atom(M23,_M23)," +
01205 
01206                                         "rdf_triple(knowrob:m30,Or,_M30l), util:strip_literal_type(_M30l, _M30), term_to_atom(M30,_M30)," +
01207                                         "rdf_triple(knowrob:m31,Or,_M31l), util:strip_literal_type(_M31l, _M31), term_to_atom(M31,_M31)," +
01208                                         "rdf_triple(knowrob:m32,Or,_M32l), util:strip_literal_type(_M32l, _M32), term_to_atom(M32,_M32)," +
01209                                         "rdf_triple(knowrob:m33,Or,_M33l), util:strip_literal_type(_M33l, _M33), term_to_atom(M33,_M33)");
01210                         return new double[] {
01211                                         Double.valueOf(nfo.get("M00").get(0)),
01212                                         Double.valueOf(nfo.get("M01").get(0)),
01213                                         Double.valueOf(nfo.get("M02").get(0)),
01214                                         Double.valueOf(nfo.get("M03").get(0)),
01215                                         Double.valueOf(nfo.get("M10").get(0)),
01216                                         Double.valueOf(nfo.get("M11").get(0)),
01217                                         Double.valueOf(nfo.get("M12").get(0)),
01218                                         Double.valueOf(nfo.get("M13").get(0)),
01219                                         Double.valueOf(nfo.get("M20").get(0)),
01220                                         Double.valueOf(nfo.get("M21").get(0)),
01221                                         Double.valueOf(nfo.get("M22").get(0)),
01222                                         Double.valueOf(nfo.get("M23").get(0)),
01223                                         Double.valueOf(nfo.get("M30").get(0)),
01224                                         Double.valueOf(nfo.get("M31").get(0)),
01225                                         Double.valueOf(nfo.get("M32").get(0)),
01226                                         Double.valueOf(nfo.get("M33").get(0))};
01227                 } catch(Exception e) {
01228                         displayMessage("No orientation found for: " + identifier);
01229                         //e.printStackTrace();
01230                         return null;
01231                 }
01232         }
01233 
01234         
01235 
01236 
01240         // 
01241         // EVENT HANDLER
01242         // 
01243         //
01244         //      public void mousePressed(MouseEvent e) {
01245         //
01246         //              // general: save mouse positions for calculating rotation and translation
01247         //              if(e.getButton()==MouseEvent.BUTTON1) {
01248         //                      leftMouseX = e.getX();
01249         //                      leftMouseY = e.getY();
01250         //              } else if(e.getButton()==MouseEvent.BUTTON3) {
01251         //                      rightMouseX = e.getX();
01252         //                      rightMouseY = e.getY();
01253         //              } else if (e.getButton()==MouseEvent.BUTTON2) {
01254         //                      centerMouseY = e.getY();
01255         //              }
01256         //
01257         //
01258         //
01259         //              //        // ------------- menu stuff -------------- 
01260         //              //        // play forward
01261         //              //        if(menu_human.mouseInTriangle(e.getX(), e.getY(), menu_human.xpl1, menu_human.ypl1, menu_human.xpl2, menu_human.ypl2, menu_human.xpl3, menu_human.ypl3)){
01262         //              //                playingForward = true;
01263         //              //                timer.start();                          
01264         //              //        }
01265         //              //        
01266         //              //        // pause
01267         //              //        else if(menu_human.mouseInRectangle(e.getX(), e.getY(), menu_human.xps, menu_human.yps, menu_human.widthps, menu_human.heightps)){
01268         //              //                timer.stop();
01269         //              //        }
01270         //              //        
01271         //              //        // play backward
01272         //              //        else if(menu_human.mouseInTriangle(e.getX(), e.getY(), menu_human.xrew1, menu_human.yrew1, menu_human.xrew2, menu_human.yrew2, menu_human.xrew3, menu_human.yrew3)){            
01273         //              //                playingForward = false;
01274         //              //                timer.start();          
01275         //              //        }
01276         //              //        
01277         //              //        // stop button
01278         //              //        else if(menu_human.mouseInRectangle(e.getX(), e.getY(), menu_human.xstop, menu_human.ystop, menu_human.widthstop, menu_human.heightstop)){
01279         //              //                timer.stop();
01280         //              //                currentFrame = 0;
01281         //              //        }
01282         //              //        
01283         //              //        // step forward
01284         //              //        else if(menu_human.mouseInRectangle(e.getX(), e.getY(), menu_human.xpls, menu_human.ypls, menu_human.widthpls, menu_human.heightpls)){
01285         //              //                timer.stop();
01286         //              //                currentFrame++;
01287         //              //        }
01288         //              //        
01289         //              //        // step backward
01290         //              //        else if(menu_human.mouseInRectangle(e.getX(), e.getY(), menu_human.xrews, menu_human.yrews, menu_human.widthrews, menu_human.heightrews)){
01291         //              //                timer.stop();
01292         //              //                currentFrame--;
01293         //              //        } 
01294         //
01295         //
01296         //
01297         //
01298         //
01299         //              if(currentFrame < 0) currentFrame = 0;
01300         //              if(currentFrame>= numberFrames) currentFrame = numberFrames-1;
01301         //
01302         //
01303         //
01304         //
01305         //
01306         //
01307         //
01308         //              redraw();
01309         //      }
01310 
01311         public void delay(int millis)
01312         {
01313 
01314                 try {
01315                         Thread.sleep(100);
01316                 } catch (InterruptedException e) {
01317                 }
01318         }
01319 
01320         public void mouseClicked(MouseEvent e) {
01321                 String clickedOn = getItemAt(e.getX(), e.getY());
01322                 displayMessage(clickedOn+"");
01323                 if(clickedOn != null) {
01324                         objectClickListener.mapObjectClicked(clickedOn);
01325                 }
01326                 
01327                 // code for on-click publisher
01328 //              ros.pkg.std_msgs.msg.String m = new ros.pkg.std_msgs.msg.String();
01329 //              m.data = clickedOn;
01330 //              clickPub.publish(m);
01331                 
01332                 delay(100);
01333                 redraw();
01334         }
01335 
01336 
01337         //
01338         //
01339         //      public void mouseReleased(MouseEvent e) {
01340         //
01341         //              if(e.getButton()==MouseEvent.BUTTON1) { // reset the buffers
01342         //                      leftMouseX = -1.0f;
01343         //                      leftMouseY = -1.0f;
01344         //              } else if(e.getButton()==MouseEvent.BUTTON3) {  // reset the buffers
01345         //                      rightMouseX = -1.0f;
01346         //                      rightMouseY = -1.0f;
01347         //              } else if (e.getButton()==MouseEvent.BUTTON2) {
01348         //                      centerMouseY = -1.0f;
01349         //              }
01350         //              redraw();
01351         //      }
01352         //
01353         //      public void mouseDragged(MouseEvent e) {
01354         //
01355         //
01356         //              if(leftMouseX != -1.0f) {       // change rotation
01357         //                      yRotDisplay-= (e.getY()-leftMouseY) * 0.05;
01358         //                      xRotDisplay+= (e.getX()-leftMouseX) * 0.05;
01359         //                      leftMouseX = e.getX();
01360         //                      leftMouseY = e.getY();
01361         //
01362         //              }else if(rightMouseX != -1.0f) {        // change translation
01363         //                      yShiftDisplay+= (e.getY()-rightMouseY) * 0.5;
01364         //                      xShiftDisplay+= (e.getX()-rightMouseX) * 0.5;
01365         //                      rightMouseX = e.getX();
01366         //                      rightMouseY = e.getY();
01367         //
01368         //              } else if (centerMouseY != -1.0f) {     // zoom
01369         //                      zoomDisplay+= (e.getY()-centerMouseY) * 0.02;
01370         //                      if(zoomDisplay<0.01){zoomDisplay=0.01f;}
01371         //                      centerMouseY = e.getY();
01372         //              }
01373         //
01374         //              redraw();
01375         //      }
01376 
01377 
01378         public void keyPressed(){
01379 
01380                 switch(keyCode) {
01381                 case RIGHT:
01382                         currentFrame++;
01383                         break;
01384                 case LEFT:
01385                         currentFrame--;
01386                         break;
01387                 case java.awt.event.KeyEvent.VK_PAGE_UP:
01388                         currentFrame+=10;
01389                         break;
01390                 case java.awt.event.KeyEvent.VK_PAGE_DOWN:
01391                         currentFrame-=10;
01392                         timer.stop();
01393                         break;
01394                 case java.awt.event.KeyEvent.VK_SPACE:
01395                         if(timer.isRunning())
01396                                 timer.stop();
01397                         else
01398                                 timer.start();
01399                         break;
01400                 case KeyEvent.VK_C:
01401                         clear();                
01402                         break;
01403                 case KeyEvent.VK_B:
01404                         drawBackground();
01405                         break;
01406                 case KeyEvent.VK_X:
01407                         bufferFrame.setVisible(!bufferFrame.isVisible());
01408                         break;
01409                 case KeyEvent.VK_Y:
01410                         buffer.resetMatrix();
01411                         this.resetMatrix();
01412                 case KeyEvent.VK_P:
01413                         this.record = true;
01414                         break;
01415                 case KeyEvent.VK_Q:
01416                         cam.setRotations(-Math.PI/2,Math.PI/4,-Math.PI);
01417                         cam.lookAt(0.0,0.0,0.0,cam.getDistance());
01418                         break;
01419                 case KeyEvent.VK_E:
01420                         cam.setRotations(-Math.PI,0.0,0.0);
01421                         cam.lookAt(0.0,0.0,0.0,cam.getDistance());
01422                         break;
01423                 case KeyEvent.VK_A:
01424                         cam.rotateY(-ROTATE_Y_DELTA);
01425                         break;
01426                 case KeyEvent.VK_D:
01427                         cam.rotateY(ROTATE_Y_DELTA);
01428                         break;
01429                 case KeyEvent.VK_W:
01430                         float[] rpy = cam.getRotations();
01431                         float[] lat = cam.getLookAt();
01432                         //float[] pos = cam.getPosition();
01433                         double dist = cam.getDistance();
01434                         CameraState state = new CameraState(new Rotation(RotationOrder.XYZ,
01435                                         rpy[0],
01436                                         rpy[1],
01437                                         rpy[2]),
01438                                         new Vector3D((lat[0])* 0.75,
01439                                                         (lat[1])* 0.75,
01440                                                         (lat[2])* 0.75),
01441                                                         dist);
01442                         state.apply(this);
01443                         cam.setState(state);
01444                         break;
01445                 case KeyEvent.VK_S:
01446                         float[] rpy2 = cam.getRotations();
01447                         float[] lat2 = cam.getLookAt();
01448                         float[] pos2 = cam.getPosition();
01449                         double dist2 = cam.getDistance();
01450                         CameraState state2 = new CameraState(new Rotation(RotationOrder.XYZ,
01451                                         rpy2[0],
01452                                         rpy2[1],
01453                                         rpy2[2]),
01454                                         new Vector3D((lat2[0] + pos2[0])/2,
01455                                                         (lat2[1] + pos2[1])/2,
01456                                                         (lat2[2] + pos2[2])/2),
01457                                                         dist2);
01458                         state2.apply(this);
01459                         cam.setState(state2);
01460 
01461                         break;
01462                 case KeyEvent.VK_J:
01463                         hint(DISABLE_DEPTH_SORT);
01464                         break;
01465                 case KeyEvent.VK_K:
01466                         hint(ENABLE_DEPTH_SORT);
01467                         break;
01468 
01469                 }
01470                 if(currentFrame < 0) currentFrame = 0;
01471                 if(currentFrame>= numberFrames) currentFrame = numberFrames-1;
01472                 delay(100);
01473                 redraw();
01474         } 
01475 
01476 
01477 
01478         // ////////////////////////////////////////////////////////////////////////////////////////////
01479         // ////////////////////////////////////////////////////////////////////////////////////////////
01481         // 
01482         // HELPER FUNCTIONS
01483         // 
01484 
01485         public void setViewParameters(float xShift, float yShift, float xRot, float yRot, float zoom) {
01486                 this.xShiftDisplay = xShift;
01487                 this.yShiftDisplay = yShift;
01488                 this.xRotDisplay = xRot;
01489                 this.yRotDisplay = yRot;
01490                 this.zoomDisplay   = zoom;
01491         } 
01492 
01493         // sort the array jointTrajectoriesData by time
01494         public ArrayList<Point[]> sortArray(ArrayList<Point[]> actionsArray){
01495 
01496                 for(int i = 0; i < actionsArray.size()-1; i++){
01497                         for(int j = i+1; j < actionsArray.size(); j++){
01498                                 if(actionsArray.get(i)[0].time > actionsArray.get(j)[0].time){
01499                                         Point aux = new Point();
01500                                         for (int l = 0; l < 28; l++){
01501                                                 aux = actionsArray.get(j)[l];
01502                                                 actionsArray.get(j)[l] = actionsArray.get(i)[l];
01503                                                 actionsArray.get(i)[l] = aux;
01504                                         }
01505                                 }
01506                         }
01507                 }
01508                 return actionsArray;
01509         }
01510 
01511 
01512         public int[] makeColor(int r,int g,int b){
01513                 int[] color = new int[3];
01514                 color[0] = r; color[1] = g; color[2] = b;
01515                 return color;
01516         }
01517 
01518 
01519         //fills the array colors with two colors for each action, one for the trajectory and one for the human pose
01520         public void setColors(){
01521 
01522                 colors.add(makeColor(0,0,255));                   
01523                 colors.add(makeColor(30,144,255));
01524 
01525                 colors.add(makeColor(0,255,255));
01526                 colors.add(makeColor(180,255,255));
01527 
01528                 colors.add(makeColor(255,215,0));
01529                 colors.add(makeColor(238,221,130));
01530 
01531                 colors.add(makeColor(250,128,114));
01532                 colors.add(makeColor(255,160,122));
01533 
01534                 colors.add(makeColor(255,105,180));
01535                 colors.add(makeColor(255,228,225));
01536 
01537                 colors.add(makeColor(186,85,211));
01538                 colors.add(makeColor(221,160,221));       
01539         }       
01540 
01541         final Timer timer = new Timer(40, new ActionListener() {
01542                 public void actionPerformed(ActionEvent event) {
01543                         int frameBefore = currentFrame;
01544                         if(playingForward == true) currentFrame++;
01545                         if(playingForward == false) currentFrame--;
01546                         if(currentFrame < 0) currentFrame = 0;
01547                         if(currentFrame>= numberFrames) currentFrame = numberFrames-1;   
01548                         if(currentFrame != frameBefore) redraw();
01549                 }
01550         });
01551 
01552         // menu for the visualization jointTrajectories
01553         public class MenuHuman{
01554 
01555                 // tracking button
01556                 int xtr = 10;           int ytr = 10;
01557                 int widthtr = 70;       int heighttr = 20;
01558                 int xtxt = 15;          int ytxt = 15;
01559                 int widthtxt = 60;      int heighttxt = 10;
01560 
01561                 // rewind button
01562                 int xrew1 = 90;         int xrew2 = 115;        int xrew3 = 115;
01563                 int yrew1 = 20;         int yrew2 = 10;         int yrew3 = 30;
01564 
01565                 // pause button
01566                 int xps = 125;          int xps1 = 128;         int xps2 = 137;
01567                 int yps = 10;           int yps1 = 10;          int yps2 = 10;
01568                 int widthps = 20;       int widthps1 = 5;       int widthps2 = 5;
01569                 int heightps = 20;      int heightps1 = 20;     int heightps2 = 20;
01570 
01571 
01572                 // play button
01573                 int xpl1 = 155;         int xpl2 = 155;         int xpl3 = 180;
01574                 int ypl1 = 10;          int ypl2 = 30;          int ypl3 = 20;
01575 
01576                 // stop button
01577                 int xstop = 190;        int ystop = 10;
01578                 int widthstop = 20;     int heightstop = 20;    
01579 
01580                 // rewind stepwise
01581                 int xrews = 220;        int yrews = 10;
01582                 int widthrews = 25;     int heightrews = 20;
01583 
01584                 int xrews1 = 220;       int xrews2 = 235;       int xrews3 = 235;       int xrews4 = 239;
01585                 int yrews1 = 20;        int yrews2 = 10;        int yrews3 = 30;        int yrews4 = 10;
01586                 int widthrews4 = 5;     int heightrews4 = 20;
01587 
01588                 // play stepwise
01589                 int widthpls = 25;      int heightpls = 20;     int widthpls4 = 5;      int heightpls4 = 20;
01590 
01591                 int xpls = 255;         int xpls1 = 264;        int xpls2 = 264;        int xpls3 = 279;        int xpls4 = 255;
01592                 int ypls = 10;          int ypls1 = 10;         int ypls2 = 30;         int ypls3 = 20;         int ypls4 = 10;
01593 
01594 
01595                 public void drawMenu(){                                         
01596                         noStroke();
01597 
01598                         fill(238,238,224);
01599                         rect(xtr, ytr, widthtr, heighttr);
01600                         fill(238,238,224);
01601                         triangle(xrew1, yrew1, xrew2, yrew2, xrew3, yrew3);                     
01602                         rect(xps1, yps1, widthps1, heightps1);
01603                         rect(xps2, yps2, widthps2, heightps2);
01604                         triangle(xpl1, ypl1, xpl2, ypl2, xpl3, ypl3);
01605                         rect(xstop, ystop, widthstop, heightstop);                      
01606                         triangle(xpls1, ypls1, xpls2, ypls2, xpls3, ypls3);
01607                         rect(xpls4, ypls4, widthpls4, heightpls4);                      
01608                         triangle(xrews1, yrews1, xrews2, yrews2, xrews3, yrews3);
01609                         rect(xrews4, yrews4, widthrews4, heightrews4);
01610 
01611 
01612                         fill(0,0,0);stroke(0);
01613                         textFont(verdanaBold);
01614                         textMode(SCREEN);
01615                         textAlign(LEFT);
01616                         String s = "Trajectory";
01617                         text(s, xtxt, ytxt, widthtxt, heighttxt);
01618                 }
01619 
01620                 public boolean mouseInTriangle(int xm, int ym, int x1, int y1, int x2, int y2, int x3, int y3){                               
01621                         int fAB = (ym-y1)*(x2-x1) - (xm-x1)*(y2-y1);
01622                         int fCA = (ym-y3)*(x1-x3) - (xm-x3)*(y1-y3);
01623                         int fBC = (ym-y2)*(x3-x2) - (xm-x2)*(y3-y2);
01624 
01625                         if ((fAB*fBC > 0) && (fBC*fCA>0))
01626                                 return true;
01627                         return false;
01628                 }
01629 
01630                 public boolean mouseInRectangle(int xm, int ym, int xr, int yr, int width, int height){
01631                         if ((xm >= xr) && (xm <= xr+width) && (ym >= yr) && (ym <= yr+height)) {
01632                                 return true;
01633                         } else {
01634                                 return false;
01635                         }
01636                 }
01637 
01638                 public int trackingDialog(){
01639 
01640                         String[] possibilities = {"BEC", "ULW", "OLW", "UBW", "OBW", "UHW", "BRK", "OHW", "KO", "SEH", "OSL", "USL", "FUL", "FBL", 
01641                                         "OST", "USR", "FUR", "FBR", "SBL", "OAL", "UAL", "HAL", "FIL", "SBR", "OAR", "UAR", "HAR", "FIR"};
01642 
01643 
01644                         for(int i = 0; i < possibilities.length; i++){
01645                                 for (int j = i+1; j < 28; j++){
01646                                         if (possibilities[i].compareTo(possibilities[j]) > 0){
01647                                                 String aux = possibilities[i];
01648                                                 possibilities[i] = possibilities[j];
01649                                                 possibilities[j] = aux;
01650                                         }
01651                                 }
01652                         }
01653                         JOptionPane.showInputDialog(null, "Select the body part you want to track:\n","Customized Dialog",                                  
01654                                         JOptionPane.PLAIN_MESSAGE, null, possibilities, "");
01655                         // TOFIX                        
01656                         //                      ArrayList<Point[]> jointPositions = bodyPoseSeq.getSequence();
01657                         //                      if ((s != null) && (s.length() > 0))
01658                         //                              for (int i = 0 ; i < 28; i++){
01659                         //                                      if (jointPositions.get(0)[i].name == s)
01660                         //                                              return i;
01661                         //                              }       
01662                         return -1;              
01663                 }
01664 
01665         }  
01666 
01667         public static int convertColor(int red, int green, int blue, int alpha) {
01668                 return (((((alpha << 8) + red) << 8) + green) << 8) + blue;
01669         }
01670 
01671         ItemBase itemForObjType(String type) {
01672 
01674                 // tableware
01675 
01676                 if(type.endsWith("#Cup'")) {
01677                         return new Cup(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01678 
01679                 } else if(type.endsWith("#DrinkingMug'")) { // todo: make drinking mug item
01680                         return new Cup(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01681 
01682                 } else if(type.endsWith("#DinnerPlate'")) { 
01683                         return new Plate(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01684 
01685                 } else if(type.endsWith("#Saucer'")) { 
01686                         return new Plate(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01687 
01688                 } else if (type.endsWith("#DrinkingGlass'")) {
01689                         return new DrinkingGlass(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01690 
01691                 } else if (type.endsWith("#Bowl-Eating'")) {
01692                         return new BowlEating(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01693 
01694                 } else if (type.endsWith("#DrinkingBottle'")) {
01695                         return new Bottle(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0.05f,0.05f,0.15f);
01696 
01697                 } else if (type.endsWith("#Kettle'")) {
01698                         return new Kettle(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01699 
01701                         // silverware   
01702 
01703                 } else if(type.endsWith("#DinnerFork'")) {
01704                         return new Fork(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01705 
01706                 } else if(type.endsWith("#TableKnife'")) {
01707                         return new Knife(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01708 
01709                 } else if(type.endsWith("#Knife'")) { // toso: make general knife item?
01710                         return new Knife(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01711 
01712                 } else if(type.endsWith("#SoupSpoon'")) {
01713                         return new Spoon(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01714 
01715 
01717                         // serving and cooking
01718 
01719                 } else if(type.endsWith("#Sponge-CleaningImplement'")) {
01720                         return new Box(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0.05f,0.10f,0.05f);
01721 
01722                 } else if(type.endsWith("#Napkin'")) {
01723                         return new Napkin(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01724 
01725                 } else if(type.endsWith("#PlaceMat'")) {
01726                         return new PlaceMat(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01727 
01728                 } else if(type.endsWith("#Tray'")) { //todo: make Tray item
01729                         return new PlaceMat(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01730 
01731                 } else if (type.endsWith("#Platter'")) {
01732                         return new Platter(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);              
01733 
01734                 } else if(type.endsWith("#CookingPot'")) {
01735                         return new CookingPot(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01736 
01737                 } else if(type.endsWith("#Spatula'")) {
01738                         return new Spatula(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);      
01739 
01740                 } else if(type.endsWith("#PancakeMaker'")) {
01741                         return new PancakeMaker(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01742 
01743 
01745                         // breakfast consumables        
01746 
01747                 } else if (type.endsWith("#Bread'")) {
01748                         return new Bread(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01749 
01750                 } else if (type.endsWith("#Cheese'")) {
01751                         return new Cheese(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01752 
01753                 } else if (type.endsWith("#Butter'")) {
01754                         return new Cheese(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01755 
01756                 } else if (type.endsWith("#Sausage'")) {
01757                         return new Sausage(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01758 
01759                 } else if (type.endsWith("#Cake'")) {
01760                         return new Cake(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01761 
01762                 } else if (type.endsWith("#BreakfastCereal'")) {
01763                         return new CerealBox(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01764 
01765                 } else if (type.endsWith("#PancakeMix'")) {
01766                         return new PancakeMix(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01767 
01768                 } else if (type.endsWith("#Pancake'")) {
01769                         return new Pancake(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01770 
01771                 } else if (type.endsWith("#Yogurt'")) {
01772                         return new DrinkingGlass(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01773 
01774 
01776                         // lunch/dinner consumables
01777 
01778                 } else if (type.endsWith("#Pizza'")) {
01779                         return new Pizza(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01780 
01781                 } else if (type.endsWith("#Salad'")) {
01782                         return new SaladBowl(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01783 
01784                 } else if (type.endsWith("#Pasta'")) {
01785                         return new SaladBowl(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01786 
01787                 } else if (type.endsWith("#Soup'")) {
01788                         return new SoupPlate(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01789 
01790 
01791 
01793                         // drinks
01794 
01795                 } else if (type.endsWith("#Water'")) {
01796                         return new Bottle(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01797 
01798                 } else if (type.endsWith("#Pitcher'")) {
01799                         return new Thermos(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01800 
01801                 } else if (type.endsWith("#Tea-Beverage'")) {
01802                         return new Bread(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01803 
01804                 } else if (type.endsWith("#Coffee-Beverage'")) {
01805                         return new Thermos(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01806 
01807                 } else if (type.endsWith("#Juice'")) {
01808                         return new Tetrapak(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01809 
01810                 } else if (type.endsWith("#Tea-Iced'")) {
01811                         return new Tetrapak(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01812 
01813                 } else if(type.endsWith("#Tetrapak'")) {
01814                         return new Tetrapak(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01815 
01816                 } else if (type.endsWith("#CardboardBox'")) {
01817                         return new Bread(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01818 
01819                 } else if (type.endsWith("#CowsMilk-Product'")) {
01820                         return new Tetrapak(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01821 
01822                 } else if (type.endsWith("#Buttermilk'")) {
01823                         return new Tetrapak(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01824 
01825 
01827                         // furniture
01828 
01829                 } else if(type.endsWith("#Chair-PieceOfFurniture'")) {
01830                         return new Chair(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01831 
01832                 } else if(type.endsWith("#Handle'")) {
01833                         return new BoxHandle(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0.03f,0.03f,0.03f);
01834 
01835                 } else if(type.endsWith("#ControlKnob'")) {
01836                         return new Sphere(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0.03f,0.03f,0.03f);
01837 
01838 
01839                 } else if(type.endsWith("#HingedJoint'")) {
01840                         HingedJoint h = new HingedJoint(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0.03f,0.03f,0.03f);
01841                         h.setColor(SemanticMapVisApplet.convertColor(70, 120, 255, 255));
01842                         return h; 
01843                         
01844                 } else if(type.endsWith("#PrismaticJoint'")) {
01845                         PrismaticJoint h = new PrismaticJoint(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0.03f,0.03f,0.03f);
01846                         h.setColor(SemanticMapVisApplet.convertColor(70, 255, 120, 255));
01847                         return h; 
01848                         
01850                         // dummies
01851 
01852                 } else if(type.endsWith("#SpatialThing-Localized'")) {
01853                         return new Ellipse(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0,0,0);
01854 
01855                         // } else if(type.endsWith("#Place'")) {
01856                         //   return new Ellipse(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1, 0.0f,0.0f,0f);
01857 
01858                 } else if(type.endsWith("#Point3D'")) {
01859                         return new Sphere(1,0,0,0,  0,1,0,0,  0,0,1,0,  0,0,0,1,  0.03f,0.03f,0.03f);
01860                 }
01861                 return null;    
01862 
01863         }
01864 
01865 
01866 
01867 
01868 }
01869 
01870 
01871 class EmptyCanvas extends Canvas {
01872         private int[] snapshot;
01873         public boolean drawSelf = false;
01874         private static final long serialVersionUID = 1L;
01875         public EmptyCanvas(int width, int height, String renderer) {
01876                 super();
01877                 this.width = width;
01878                 this.height = height;           
01879                 init();
01880                 snapshot = new int[width*height];
01881                 //              noLoop();
01882                 try{super.size(width, height, PGraphics3D.P3D);}catch(RendererChangeException e) {};
01883                 noLoop();
01884                 //this.resize(width, height);
01885         }
01886 
01887 
01888         @Override
01889         public void draw() {
01890                 if(drawSelf)
01891                         for(int i=0;i<width;i++)
01892                                 for(int j=0;j<height;j++)
01893                                         set(i, j, snapshot[i*height+j]);
01894         };
01895         //      @Override
01896         //      public void keyPressed() {};
01897         //      @Override
01898         //      public void mousePressed(MouseEvent e) {};
01899         //      @Override
01900         //      public void mouseReleased(MouseEvent e) {};
01901         //      @Override
01902         //      public void mouseDragged(MouseEvent e) {};
01905         @Override
01906         public void runMain() {};
01907         @Override
01908         public void size(int width, int height, String renderer) {};
01909 
01910 
01911         public void freeze() {
01912                 for(int i=0;i<width;i++)
01913                         for(int j=0;j<height;j++)
01914                                 snapshot[i*height+j] = get(i, j);
01915                 drawSelf = true;
01916         };
01917 
01918         public void unFreeze() {
01919                 drawSelf = false;
01920         }
01921 }
01922 


mod_vis
Author(s): Moritz Tenorth, Jakob Engel
autogenerated on Sat Dec 28 2013 17:09:49