GrxVrmlExporter.java
Go to the documentation of this file.
00001 package com.generalrobotix.ui.util;
00002 
00003 import java.io.BufferedWriter;
00004 import java.io.File;
00005 import java.io.FileWriter;
00006 import java.util.Vector;
00007 import java.util.regex.*;
00008 
00009 import jp.go.aist.hrp.simulator.AppearanceInfo;
00010 import jp.go.aist.hrp.simulator.MaterialInfo;
00011 import jp.go.aist.hrp.simulator.ShapeInfo;
00012 import jp.go.aist.hrp.simulator.ShapePrimitiveType;
00013 import jp.go.aist.hrp.simulator.TextureInfo;
00014 
00015 import com.generalrobotix.ui.item.GrxLinkItem;
00016 import com.generalrobotix.ui.item.GrxModelItem;
00017 import com.generalrobotix.ui.item.GrxSegmentItem;
00018 import com.generalrobotix.ui.item.GrxSensorItem;
00019 import com.generalrobotix.ui.item.GrxShapeItem;
00020 import com.generalrobotix.ui.item.GrxExtraJointItem;
00021 
00022 public class GrxVrmlExporter {
00029         public static boolean export(GrxModelItem model, String exportPath){
00030                 // TODO check existence of exportPath
00031                 int idx = exportPath.lastIndexOf(File.separatorChar);
00032                 if(idx==-1)
00033                         idx = exportPath.lastIndexOf('/');
00034                 String exportDir = exportPath.substring(0, idx);
00035 
00036                 BufferedWriter writer = null;
00037             try {
00038                 writer = new BufferedWriter(new FileWriter(exportPath));
00039                 
00040                 writer.write("#VRML V2.0 utf8\n");                                              
00041                 writer.write("\n");                                                             
00042                 writer.write("PROTO Joint [\n");                                                
00043                 writer.write("  exposedField     SFVec3f      center              0 0 0\n");    
00044                 writer.write("  exposedField     MFNode       children            []\n");       
00045                 writer.write("  exposedField     MFFloat      llimit              []\n");       
00046                 writer.write("  exposedField     MFFloat      lvlimit             []\n");       
00047                 writer.write("  exposedField     SFRotation   limitOrientation    0 0 1 0\n");  
00048                 writer.write("  exposedField     SFString     name                \"\"\n");       
00049                 writer.write("  exposedField     SFRotation   rotation            0 0 1 0\n");  
00050                 writer.write("  exposedField     SFVec3f      scale               1 1 1\n");    
00051                 writer.write("  exposedField     SFRotation   scaleOrientation    0 0 1 0\n");  
00052                 writer.write("  exposedField     MFFloat      stiffness           [ 0 0 0 ]\n");                                                                               
00053                 writer.write("  exposedField     SFVec3f      translation         0 0 0\n");    
00054                 writer.write("  exposedField     MFFloat      ulimit              []\n");       
00055                 writer.write("  exposedField     MFFloat      uvlimit             []\n");       
00056                 writer.write("  exposedField     SFString     jointType           \"\"\n");       
00057                 writer.write("  exposedField     SFInt32      jointId             -1\n");       
00058                 writer.write("  exposedField     SFVec3f      jointAxis           0 0 1\n");      
00059                 writer.write("\n");                                                             
00060                 writer.write("  exposedField     SFFloat      gearRatio           1\n");        
00061                 writer.write("  exposedField     SFFloat      rotorInertia        0\n");        
00062                 writer.write("  exposedField     SFFloat      rotorResistor       0\n");        
00063                 writer.write("  exposedField     SFFloat      torqueConst         1\n");        
00064                 writer.write("  exposedField     SFFloat      encoderPulse        1\n");        
00065                 writer.write("]\n");                                                            
00066                 writer.write("{\n");                                                            
00067                 writer.write("  Transform {\n");                                                
00068                 writer.write("    center           IS center\n");                               
00069                 writer.write("    children         IS children\n");                             
00070                 writer.write("    rotation         IS rotation\n");                             
00071                 writer.write("    scale            IS scale\n");                                
00072                 writer.write("    scaleOrientation IS scaleOrientation\n");                     
00073                 writer.write("    translation      IS translation\n");                          
00074                 writer.write("  }\n");                                                          
00075                 writer.write("}\n");                                                            
00076                 writer.write("\n");                                                             
00077                 writer.write("PROTO Segment [\n");                                              
00078                 writer.write("  field           SFVec3f     bboxCenter        0 0 0\n");        
00079                 writer.write("  field           SFVec3f     bboxSize          -1 -1 -1\n");     
00080                 writer.write("  exposedField    SFVec3f     centerOfMass      0 0 0\n");        
00081                 writer.write("  exposedField    MFNode      children          [ ]\n");          
00082                 writer.write("  exposedField    SFNode      coord             NULL\n");         
00083                 writer.write("  exposedField    MFNode      displacers        [ ]\n");          
00084                 writer.write("  exposedField    SFFloat     mass              0 \n");           
00085                 writer.write("  exposedField    MFFloat     momentsOfInertia  [ 0 0 0 0 0 0 0 0 0 ]\n");                                                                       
00086                 writer.write("  exposedField    SFString    name              \"\"\n");           
00087                 writer.write("  eventIn         MFNode      addChildren\n");                    
00088                 writer.write("  eventIn         MFNode      removeChildren\n");                 
00089                 writer.write("]\n");                                                            
00090                 writer.write("{\n");                                                            
00091                 writer.write("  Group {\n");                                                    
00092                 writer.write("    addChildren    IS addChildren\n");                            
00093                 writer.write("    bboxCenter     IS bboxCenter\n");                             
00094                 writer.write("    bboxSize       IS bboxSize\n");                               
00095                 writer.write("    children       IS children\n");                               
00096                 writer.write("    removeChildren IS removeChildren\n");                         
00097                 writer.write("  }\n");                                                          
00098                 writer.write("}\n");                                                            
00099                 writer.write("\n");                                                             
00100                 writer.write("PROTO Humanoid [\n");                                             
00101                 writer.write("  field           SFVec3f    bboxCenter            0 0 0\n");     
00102                 writer.write("  field           SFVec3f    bboxSize              -1 -1 -1\n");  
00103                 writer.write("  exposedField    SFVec3f    center                0 0 0\n");     
00104                 writer.write("  exposedField    MFNode     humanoidBody          [ ]\n");       
00105                 writer.write("  exposedField    MFString   info                  [ ]\n");       
00106                 writer.write("  exposedField    MFNode     joints                [ ]\n");       
00107                 writer.write("  exposedField    SFString   name                  \"\"\n");        
00108                 writer.write("  exposedField    SFRotation rotation              0 0 1 0\n");   
00109                 writer.write("  exposedField    SFVec3f    scale                 1 1 1\n");     
00110                 writer.write("  exposedField    SFRotation scaleOrientation      0 0 1 0\n");   
00111                 writer.write("  exposedField    MFNode     segments              [ ]\n");       
00112                 writer.write("  exposedField    MFNode     sites                 [ ]\n");       
00113                 writer.write("  exposedField    SFVec3f    translation           0 0 0\n");     
00114                 writer.write("  exposedField    SFString   version               \"1.1\"\n");     
00115                 writer.write("  exposedField    MFNode     viewpoints            [ ]\n");       
00116                 writer.write("]\n");                                                            
00117                 writer.write("{\n");                                                            
00118                 writer.write("  Transform {\n");                                                
00119                 writer.write("    bboxCenter       IS bboxCenter\n");                           
00120                 writer.write("    bboxSize         IS bboxSize\n");                             
00121                 writer.write("    center           IS center\n");                               
00122                 writer.write("    rotation         IS rotation\n");                             
00123                 writer.write("    scale            IS scale\n");                                
00124                 writer.write("    scaleOrientation IS scaleOrientation\n");                     
00125                 writer.write("    translation      IS translation\n");                          
00126                 writer.write("    children [\n");                                               
00127                 writer.write("      Group {\n");                                                
00128                 writer.write("        children IS viewpoints\n");                               
00129                 writer.write("      }\n");                                                      
00130                 writer.write("      Group {\n");                                                
00131                 writer.write("        children IS humanoidBody \n");                            
00132                 writer.write("      }\n");                                                      
00133                 writer.write("    ]\n");                                                        
00134                 writer.write("  }\n");                                                          
00135                 writer.write("}\n");                                                            
00136                 writer.write("\n");                                                             
00137                 writer.write("PROTO VisionSensor [\n");                                         
00138                 writer.write("  exposedField SFVec3f    translation       0 0 0\n");            
00139                 writer.write("  exposedField SFRotation rotation          0 0 1 0\n");          
00140                 writer.write("  exposedField MFNode     children          [ ]\n");              
00141                 writer.write("  exposedField SFFloat    fieldOfView       0.785398\n");         
00142                 writer.write("  exposedField SFString   name              \"\"\n");               
00143                 writer.write("  exposedField SFFloat    frontClipDistance 0.01\n");             
00144                 writer.write("  exposedField SFFloat    backClipDistance  10.0\n");             
00145                 writer.write("  exposedField SFString   type              \"NONE\"\n");           
00146                 writer.write("  exposedField SFInt32    sensorId          -1\n");               
00147                 writer.write("  exposedField SFInt32    width             320\n");              
00148                 writer.write("  exposedField SFInt32    height            240\n");              
00149                 writer.write("  exposedField SFFloat    frameRate         30\n");               
00150                 writer.write("]\n");                                                            
00151                 writer.write("{\n");                                                            
00152                 writer.write("  Transform {\n");                                                
00153                 writer.write("    rotation         IS rotation\n");                             
00154                 writer.write("    translation      IS translation\n");                          
00155                 writer.write("    children         IS children\n");                             
00156                 writer.write("  }\n");                                                          
00157                 writer.write("}\n");                                                            
00158                 writer.write("\n");                                                             
00159                 writer.write("\n");                                                             
00160                 writer.write("PROTO ForceSensor [  \n");                                        
00161                 writer.write("  exposedField SFVec3f    maxForce    -1 -1 -1\n");               
00162                 writer.write("  exposedField SFVec3f    maxTorque   -1 -1 -1\n");               
00163                 writer.write("  exposedField SFVec3f    translation 0 0 0\n");                  
00164                 writer.write("  exposedField SFRotation rotation    0 0 1 0\n");                
00165                 writer.write("  exposedField MFNode     children          [ ]\n");              
00166                 writer.write("  exposedField SFInt32    sensorId    -1\n");                     
00167                 writer.write("]\n");                                                            
00168                 writer.write("{\n");                                                            
00169                 writer.write("  Transform {\n");                                                
00170                 writer.write("    translation IS translation\n");                               
00171                 writer.write("    rotation    IS rotation\n");                                  
00172                 writer.write("    children    IS children\n");                             
00173                 writer.write("  }\n");                                                          
00174                 writer.write("}\n");                                                            
00175                 writer.write("\n");                                                             
00176                 writer.write("PROTO Gyro [\n");                                                 
00177                 writer.write("  exposedField SFVec3f    maxAngularVelocity -1 -1 -1\n");        
00178                 writer.write("  exposedField SFVec3f    translation        0 0 0\n");           
00179                 writer.write("  exposedField SFRotation rotation           0 0 1 0\n");         
00180                 writer.write("  exposedField MFNode     children          [ ]\n");              
00181                 writer.write("  exposedField SFInt32    sensorId           -1\n");              
00182                 writer.write("]\n");                                                            
00183                 writer.write("{\n");                                                            
00184                 writer.write("  Transform {\n");                                                
00185                 writer.write("    translation IS translation\n");                               
00186                 writer.write("    rotation    IS rotation\n");                                  
00187                 writer.write("    children    IS children\n");                             
00188                 writer.write("  }\n");                                                          
00189                 writer.write("}\n");                                                            
00190                 writer.write("\n");                                                             
00191                 writer.write("PROTO AccelerationSensor [\n");                                   
00192                 writer.write("  exposedField SFVec3f    maxAcceleration -1 -1 -1\n");           
00193                 writer.write("  exposedField SFVec3f    translation     0 0 0\n");              
00194                 writer.write("  exposedField SFRotation rotation        0 0 1 0\n");            
00195                 writer.write("  exposedField MFNode     children          [ ]\n");              
00196                 writer.write("  exposedField SFInt32    sensorId        -1\n");                 
00197                 writer.write("]\n");                                                            
00198                 writer.write("{\n");                                                            
00199                 writer.write("  Transform {\n");                                                
00200                 writer.write("    translation IS translation\n");                               
00201                 writer.write("    rotation    IS rotation\n");                                  
00202                 writer.write("    children    IS children\n");                             
00203                 writer.write("  }\n");                                                          
00204                 writer.write("}\n");                                                            
00205                 writer.write("\n");            
00206                 writer.write("PROTO RangeSensor [\n");                                              
00207                 writer.write("  exposedField SFVec3f    translation     0 0 0\n");              
00208                 writer.write("  exposedField SFRotation rotation        0 0 1 0\n");            
00209                 writer.write("  exposedField MFNode     children        [ ]\n");              
00210                 writer.write("  exposedField SFInt32    sensorId        -1\n");   
00211                 writer.write("  exposedField SFFloat    scanAngle       3.14159 #[rad]\n");
00212                 writer.write("  exposedField SFFloat    scanStep        0.1     #[rad]\n");
00213                 writer.write("  exposedField SFFloat    scanRate        10      #[Hz]\n");
00214                 writer.write("  exposedField SFFloat    maxDistance         10\n");
00215                 writer.write("]\n");                                                            
00216                 writer.write("{\n");                                                            
00217                 writer.write("  Transform {\n");                                                
00218                 writer.write("    translation IS translation\n");                               
00219                 writer.write("    rotation    IS rotation\n");                                  
00220                 writer.write("    children    IS children\n");                             
00221                 writer.write("  }\n");                                                          
00222                 writer.write("}\n");                                                            
00223                 writer.write("\n");                    
00224                 writer.write("PROTO Plane [\n");
00225                 writer.write("  exposedField SFVec3f size 10 10 0\n");
00226                 writer.write("]\n");
00227                 writer.write("{\n");
00228                 writer.write("  Box {\n");
00229                 writer.write("    size IS size\n");
00230                 writer.write("  }\n");
00231                 writer.write("}\n");
00232                 writer.write("\n");
00233                 writer.write("PROTO ExtraJoint [\n");
00234                 writer.write("  exposedField SFString   link1Name               \"\"\n"); 
00235                 writer.write("  exposedField SFString   link2Name               \"\"\n");
00236                 writer.write("  exposedField SFVec3f    link1LocalPos   0 0 0\n");      
00237                 writer.write("  exposedField SFVec3f    link2LocalPos   0 0 0\n");
00238                 writer.write("  exposedField SFString   jointType               \"xyz\"\n");
00239                 writer.write("  exposedField SFVec3f    jointAxis               1 0 0\n");
00240                 writer.write("]\n");
00241                 writer.write("{\n");
00242                 writer.write("}\n"); 
00243                 writer.write("\n");
00244                 writer.write("NavigationInfo {\n");                                             
00245                 writer.write("  avatarSize    0.5\n");                                          
00246                 writer.write("  headlight     TRUE\n");                                         
00247                 writer.write("  type  [\"EXAMINE\", \"ANY\"]\n");                                   
00248                 writer.write("}\n");                                                            
00249                 writer.write("\n");                                                             
00250                 writer.write("Background {\n");                                                 
00251                 writer.write("  skyColor 0.4 0.6 0.4\n");                                       
00252                 writer.write("}\n");                                                            
00253                 writer.write("\n");                                                             
00254                 writer.write("Viewpoint {\n");                                                  
00255                 writer.write("  position    3 0 0.835\n");                                      
00256                 writer.write("  orientation 0.5770 0.5775 0.5775 2.0935\n");                    
00257                 writer.write("}\n");                                                            
00258                 writer.write("\n");
00259                 for(int i=0; i<model.extraJoints_.size(); i++){
00260                         exportExtraJoint(writer, model.extraJoints_.get(i));
00261                         writer.write("\n");
00262                 }
00263                 writer.write("DEF "+model.getName()+" Humanoid{\n");
00264                 writer.write("  humanoidBody [\n");
00265                 Vector<GrxLinkItem> links = exportLink(writer, model.rootLink(), "    ", exportDir, model.getURL(false));
00266                 writer.write("  ]\n");
00267                 writer.write("  joints [\n");
00268                 for (int i=0; i<links.size(); i++){
00269                         writer.write("    USE "+links.get(i).getName()+",\n");
00270                 }
00271                 writer.write("  ]\n");
00272                 writer.write("  segments [\n");
00273                 for (int i=0; i<links.size(); i++){
00274                         GrxLinkItem link = links.get(i);
00275                         for (int j=0; j<link.children_.size(); j++){
00276                                         if (link.children_.get(j) instanceof GrxSegmentItem){
00277                                                 writer.write("    USE "+link.children_.get(j).getName()+",\n");
00278                                         }
00279                                 }
00280                 }
00281                 writer.write("  ]\n");
00282                 writer.write("}\n");
00283                 return true;
00284             } catch (Exception e) {
00285                 e.printStackTrace();
00286             } finally {
00287                 try {
00288                     if (writer != null) {
00289                         writer.flush();
00290                         writer.close();
00291                     }
00292                 } catch (Exception e) {
00293                 }
00294             }
00295             return false;
00296         }
00297 
00307         public static Vector<GrxLinkItem> exportLink(BufferedWriter writer, GrxLinkItem link, String indent, 
00308                         String exportDir, String mainPath){
00309                 Vector<GrxLinkItem> links = new Vector<GrxLinkItem>();
00310                 links.add(link);
00311                 try{
00312                         writer.write(indent+"DEF "+link.getName()+" Joint {\n");
00313                         writer.write(indent+"  jointType \""+link.jointType_+"\"\n");
00314                         if (link.jointId_ != -1) writer.write(indent+"  jointId "+link.jointId_+"\n");
00315                         if (link.jointType_.equals("rotate") || link.jointType_.equals("slide")){
00316                                 writer.write(indent+"  jointAxis "+link.getProperty("jointAxis")+"\n");
00317                         }
00318                         if (!valueEquals(link.getProperty("translation"),"0.0 0.0 0.0 ")){
00319                                 writer.write(indent+"  translation "+link.getProperty("translation")+"\n");
00320                         }
00321                         if (!isZero(link.getProperty("rotation"),3)){
00322                                 writer.write(indent+"  rotation "+link.getProperty("rotation")+"\n");
00323                         }
00324                         if (!link.getProperty("ulimit").equals("")) writer.write(indent+"  ulimit ["+link.getProperty("ulimit")+"]\n");
00325                         if (!link.getProperty("llimit").equals("")) writer.write(indent+"  llimit ["+link.getProperty("llimit")+"]\n");
00326                         if (!link.getProperty("uvlimit").equals("")) writer.write(indent+"  uvlimit ["+link.getProperty("uvlimit")+"]\n");
00327                         if (!link.getProperty("lvlimit").equals("")) writer.write(indent+"  lvlimit ["+link.getProperty("lvlimit")+"]\n");
00328                         if (link.gearRatio_ != 1.0) writer.write(indent+"  gearRatio "+link.gearRatio_+"\n");
00329                         if (link.rotorInertia_ != 0.0) writer.write(indent+"  rotorInertia "+link.rotorInertia_+"\n");
00330                         if (link.rotorResistor_ != 0.0) writer.write(indent+"  rotorResistor "+link.rotorResistor_+"\n");
00331                         if (link.torqueConst_ != 1.0) writer.write(indent+"  torqueConst "+link.torqueConst_+"\n");
00332                         if (link.encoderPulse_ != 1.0) writer.write(indent+"  encoderPulse "+link.encoderPulse_+"\n");
00333                         writer.write(indent+"  children[\n");
00334                         for (int i=0; i<link.children_.size(); i++){
00335                                 if (link.children_.get(i) instanceof GrxSensorItem){
00336                                 exportSensor(writer, (GrxSensorItem)link.children_.get(i), indent+"    ", exportDir, mainPath);
00337                                 }
00338                         }
00339                         for (int i=0; i<link.children_.size(); i++){
00340                                 if (link.children_.get(i) instanceof GrxSegmentItem){
00341                                 exportSegment(writer, (GrxSegmentItem)link.children_.get(i), indent+"    ", exportDir, mainPath);
00342                                 }
00343                         }
00344                         for (int i=0; i<link.children_.size(); i++){
00345                                 if (link.children_.get(i) instanceof GrxLinkItem){
00346                                         Vector<GrxLinkItem> childLinks = exportLink(writer, (GrxLinkItem)link.children_.get(i), 
00347                                                         indent+"    ", exportDir, mainPath);
00348                                         links.addAll(childLinks);
00349                                 }
00350                         }
00351                         writer.write(indent+"  ]\n");
00352                         writer.write(indent+"}\n");
00353                 }catch(Exception ex){
00354                         ex.printStackTrace();
00355                 }
00356                 return links;
00357         }
00358 
00359         public static void exportSegment(BufferedWriter writer, GrxSegmentItem segment, String indent, String exportDir, String mainPath){
00360                 try{
00361                         String translation = segment.getProperty("translation");
00362                         String rotation = segment.getProperty("rotation");
00363                         boolean useTransform = false;
00364                         if(!valueEquals(translation, "0.0 0.0 0.0 " ) || !isZero(rotation, 3 )){
00365                                 useTransform = true;
00366                                 writer.write(indent+"Transform {\n");
00367                                 writer.write(indent+"  translation "+translation+"\n");
00368                                 writer.write(indent+"  rotation "+rotation+"\n");
00369                                 writer.write(indent+"  children[\n");
00370                                 indent += "    ";
00371                         }
00372                         writer.write(indent+"DEF "+segment.getName()+" Segment{\n");
00373                         writer.write(indent+"  centerOfMass "+segment.getProperty("centerOfMass")+"\n");
00374                         writer.write(indent+"  mass "+segment.getProperty("mass")+"\n");
00375                         writer.write(indent+"  momentsOfInertia [ "+segment.getProperty("momentsOfInertia")+"]\n");
00376                         writer.write(indent+"  children[\n");
00377                         for (int i=0; i<segment.children_.size(); i++){
00378                                 if (segment.children_.get(i) instanceof GrxShapeItem){
00379                                         GrxShapeItem shape = (GrxShapeItem)segment.children_.get(i);
00380                                         exportShape(writer, shape, indent+"    ", exportDir, mainPath);
00381                                 }
00382                         }
00383                         writer.write(indent+"  ]\n");
00384                         writer.write(indent+"}\n");     
00385                         if(useTransform){
00386                                 writer.write(indent+"  ]\n");
00387                                 writer.write(indent+"}\n");     
00388                         }
00389                 }catch(Exception ex){
00390                         ex.printStackTrace();
00391                 }       
00392         }
00393         
00402         public static void exportSensor(BufferedWriter writer, GrxSensorItem sensor, String indent, String exportDir, String mainPath){
00403                 try{
00404                         String nodeType="unknown";
00405                         if (sensor.type_.equals("Force")){
00406                                 nodeType = "ForceSensor";
00407                         }else if(sensor.type_.equals("RateGyro")){
00408                                 nodeType = "Gyro"; 
00409                         }else if(sensor.type_.equals("Acceleration")){
00410                                 nodeType = "AccelerationSensor";
00411                         }else if(sensor.type_.equals("Vision")){
00412                                 nodeType = "VisionSensor";
00413                         }else if(sensor.type_.equals("Range")){
00414                                 nodeType = "RangeSensor";
00415                         }
00416                         writer.write(indent+"DEF "+sensor.getName()+" "+nodeType+" {\n");
00417                         writer.write(indent+"  sensorId "+sensor.id_+"\n");
00418                         if (!valueEquals(sensor.getProperty("translation"),"0.0 0.0 0.0 ")){
00419                                 writer.write(indent+"  translation "+sensor.getProperty("translation")+"\n");
00420                         }
00421                         if (!isZero(sensor.getProperty("rotation"),3)){
00422                                 writer.write(indent+"  rotation "+sensor.getProperty("rotation")+"\n");
00423                         }
00424                         // sensor specific parameters
00425                         if (nodeType.equals("VisionSensor")){
00426                                 if (!valueEquals(sensor.getProperty("fieldOfView"),"0.785398")){
00427                                         writer.write(indent+"  fieldOfView "+sensor.getProperty("fieldOfView")+"\n");
00428                                 }
00429                                 if (!valueEquals(sensor.getProperty("frontClipDistance"),"0.01")){
00430                                         writer.write(indent+"  frontClipDistance "+sensor.getProperty("frontClipDistance")+"\n");
00431                                 }
00432                                 if (!valueEquals(sensor.getProperty("backClipDistance"),"10.0")){
00433                                         writer.write(indent+"  backClipDistance "+sensor.getProperty("backClipDistance")+"\n");
00434                                 }
00435                                 if (!valueEquals(sensor.getProperty("width"),"320")){
00436                                         writer.write(indent+"  width "+sensor.getProperty("width")+"\n");
00437                                 }
00438                                 if (!valueEquals(sensor.getProperty("height"),"240")){
00439                                         writer.write(indent+"  height "+sensor.getProperty("height")+"\n");
00440                                 }
00441                                 if (!valueEquals(sensor.getProperty("frameRate"),"30.0")){
00442                                         writer.write(indent+"  frameRate "+sensor.getProperty("frameRate")+"\n");
00443                                 }
00444                                 if (!sensor.getProperty("cameraType").equals("NONE")){
00445                                         writer.write(indent+"  type \""+sensor.getProperty("cameraType")+"\"\n");
00446                                 }
00447                                 if (!sensor.getProperty("name").equals("")){
00448                                         writer.write(indent+"  name \""+sensor.getProperty("name")+"\"\n");
00449                                 }
00450                         }else if(nodeType.equals("ForceSensor")){
00451                                 String maxf = sensor.getProperty("maxForce");
00452                                 if (!valueEquals(maxf, "-1.0 -1.0 -1.0 ")){
00453                                         writer.write(indent+"  maxForce "+maxf+"\n");
00454                                 }
00455                                 String maxt = sensor.getProperty("maxTorque");
00456                                 if (!valueEquals(maxt, "-1.0 -1.0 -1.0 ")){
00457                                         writer.write(indent+"  maxTorque "+maxt+"\n");
00458                                 }
00459                         }else if(nodeType.equals("Gyro")){
00460                                 String max = sensor.getProperty("maxAngularVelocity");
00461                                 if (!valueEquals(max, "-1.0 -1.0 -1.0 ")){
00462                                         writer.write(indent+"  maxAngularVelocity "+max+"\n");
00463                                 }
00464                         }else if(nodeType.equals("AccelerationSensor")){
00465                                 String max = sensor.getProperty("maxAcceleration");
00466                                 if (!valueEquals(max, "-1.0 -1.0 -1.0 ")){
00467                                         writer.write(indent+"  maxAcceleration "+max+"\n");
00468                                 }
00469                         }else if(nodeType.equals("RangeSensor")){
00470                         String angle = sensor.getProperty("scanAngle");
00471                                 if (!valueEquals(angle, "3.14159")){
00472                                         writer.write(indent+"  scanAngle "+angle+"\n");
00473                                 }
00474                                 String step = sensor.getProperty("scanStep");
00475                                 if (!valueEquals(step, "0.1")){
00476                                         writer.write(indent+"  scanStep "+step+"\n");
00477                                 }
00478                                 String rate = sensor.getProperty("scanRate");
00479                                 if (!valueEquals(rate, "10")){
00480                                         writer.write(indent+"  scanRate "+rate+"\n");
00481                                 }
00482                                 String distance = sensor.getProperty("maxDistance");
00483                                 if (!valueEquals(distance, "10")){
00484                                         writer.write(indent+"  maxDistance "+distance+"\n");
00485                                 }
00486                         }
00487                         if (sensor.children_.size() > 0){
00488                                 writer.write(indent+"  children[\n");
00489                                 for (int i=0; i<sensor.children_.size(); i++){
00490                                         if (sensor.children_.get(i) instanceof GrxShapeItem){
00491                                                 exportShape(writer, (GrxShapeItem)sensor.children_.get(i), indent+"    ", exportDir, mainPath);
00492                                         }
00493                                 }
00494                                 writer.write(indent+"  ]\n");
00495                         }
00496                         writer.write(indent+"}\n");
00497                 }catch(Exception ex){
00498                         ex.printStackTrace();
00499                 }
00500         }
00501         
00510         public static void exportShape(BufferedWriter writer, GrxShapeItem shape, String indent, String exportDir, String mainPath){
00511                 System.out.println("mainPath = "+ mainPath);
00512                 System.out.println("url of shape = "+shape.getURL(false));
00513                 try{
00514                         // If this model is created from scratch and this shape is primitive, shape.getUrl() == null
00515                         // If this shape is loaded by Inline node, mainPath != shape.getUrl()
00516                         // If this shape is directry written in the main file, mainPath == shape.getUrl() 
00517                         if (!shape.isInline_){
00518                                 writer.write(indent+"Transform {\n");
00519                                 if (!valueEquals(shape.getProperty("translation"),"0.0 0.0 0.0 ")){
00520                                         writer.write(indent+"  translation "+shape.getProperty("translation")+"\n");
00521                                 }
00522                                 if (!isZero(shape.getProperty("rotation"),3)){
00523                                         writer.write(indent+"  rotation "+shape.getProperty("rotation")+"\n");
00524                                 }
00525                                 writer.write(indent+"  children[\n");
00526                                 writer.write(indent+"    Shape{\n");
00527                                 exportGeometry(writer, shape, indent+"      ");
00528                                 if (shape.appearances_[0] != null){
00529                                         writer.write(indent+"      appearance Appearance{\n");
00530                                         if (shape.materials_[0] != null){
00531                                                 MaterialInfo mat = shape.materials_[0];
00532                                                 writer.write(indent+"        material Material{\n");
00533                                                 writer.write(indent+"          diffuseColor "
00534                                                                 +mat.diffuseColor[0]+" "+mat.diffuseColor[1]+" "+mat.diffuseColor[2]+"\n");
00535                                                 writer.write(indent+"        }\n");
00536                                         }
00537                                         if(shape.textures_[0] != null){
00538                                                 TextureInfo tex = shape.textures_[0];
00539                                                 writer.write(indent+"        texture    ImageTexture {\n");
00540                                                 String url = tex.url;
00541                                                 url = url.replace('\\','/');
00542                                                 writer.write(indent+"          url \""+_absPath2relPath(url, exportDir)+"\"\n");
00543                                                 writer.write(indent+"        }\n");
00544                                         }
00545                                         writer.write(indent+"      }\n");
00546                                 }
00547                                 writer.write(indent+"    }\n");
00548                                 writer.write(indent+"  ]\n");
00549                                 writer.write(indent+"}\n");
00550                         }else{
00551                                 writer.write(indent+"Transform {\n");
00552                                 if (!valueEquals(shape.getProperty("translation"),"0.0 0.0 0.0 ")){
00553                                         writer.write(indent+"  translation "+shape.getProperty("translation")+"\n");
00554                                 }
00555                                 if (!isZero(shape.getProperty("rotation"),3)){
00556                                         writer.write(indent+"  rotation "+shape.getProperty("rotation")+"\n");
00557                                 }
00558                                 writer.write(indent+"  children[\n");
00559                                 writer.write(indent+"Inline { url \""+_absPath2relPath(shape.getURL(false), exportDir)+"\" }\n");
00560                                 writer.write(indent+"  ]\n");
00561                                 writer.write(indent+"}\n");
00562                         }
00563                 }catch(Exception ex){
00564                         ex.printStackTrace();
00565                 }
00566         }
00567         
00568         public static void exportExtraJoint(BufferedWriter writer, GrxExtraJointItem extraJoint){
00569                 try{
00570                         writer.write("DEF "+extraJoint.getName()+" ExtraJoint{\n");
00571                         writer.write("  link1Name \""+extraJoint.getProperty("link1Name")+"\"\n");
00572                         writer.write("  link2Name \""+extraJoint.getProperty("link2Name")+"\"\n");
00573                         writer.write("  link1LocalPos "+extraJoint.getProperty("link1LocalPos")+"\n");
00574                         writer.write("  link2LocalPos "+extraJoint.getProperty("link2LocalPos")+"\n");
00575                         writer.write("  jointType \""+extraJoint.getProperty("jointType")+"\"\n");
00576                         writer.write("  jointAxis "+extraJoint.getProperty("jointAxis")+"\n");
00577                         writer.write("}\n");
00578                 }catch(Exception ex){
00579                         ex.printStackTrace();
00580                 }       
00581         }
00582         
00589         private static String _absPath2relPath(String absPath, String baseDir){
00590                 Pattern localPattern = Pattern.compile("\\\\");
00591                 //Matcher localMatcher = localPattern.matcher(File.separator); は使わないで、すべて"/"として扱えるように前処理する
00592                 Matcher localMatcher = localPattern.matcher("/");
00593                 String localStr = localMatcher.replaceAll("\\\\\\\\");
00594                 
00595                 String [] dirs1 = absPath.split( localStr );
00596                 String [] dirs2 = baseDir.split( localStr );
00597                 String relPath = "";
00598                 int cnt=0;
00599                 //Linux MacOSの場合とWindowsの場合で処理を分岐する
00600         if (System.getProperty("os.name").equals("Linux") || System.getProperty("os.name").equals("Mac OS X")) { //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$ //$NON-NLS-4$
00601                 while (cnt < dirs1.length && cnt < dirs2.length && dirs1[cnt].equals(dirs2[cnt])) cnt++;
00602                 for (int i=0; i<dirs2.length-cnt; i++) relPath += "../";
00603                 for (int i=cnt; i<dirs1.length; i++){
00604                         relPath += dirs1[i];
00605                         if (i != dirs1.length-1) relPath += "/";
00606                 }
00607         }else{
00608                 if(dirs1[0].equals(dirs2[0])) {
00609                         while (cnt < dirs1.length && cnt < dirs2.length && dirs1[cnt].equals(dirs2[cnt])) cnt++;
00610                         for (int i=0; i<dirs2.length-cnt; i++) relPath += "../";
00611                 }
00612                 for (int i=cnt; i<dirs1.length; i++){
00613                         relPath += dirs1[i];
00614                         if (i != dirs1.length-1) relPath += "/";
00615                 }
00616         }
00617                 return relPath;
00618         }
00619         
00620         public static void exportGeometry(BufferedWriter writer, GrxShapeItem shape, String indent){
00621                 try{
00622                         ShapeInfo info = shape.shapes_[0];
00623                         ShapePrimitiveType ptype = info.primitiveType;
00624                         float [] pparams = info.primitiveParameters;
00625                         
00626                         if (ptype == ShapePrimitiveType.SP_MESH){
00627                                 writer.write(indent+"geometry IndexedFaceSet {\n");
00628                                 writer.write(indent+"ccw TRUE\n");
00629                                 writer.write(indent+"coord Coordinate {\n");
00630                                 writer.write(indent+"point [\n");
00631                                 for(int i=0; i<info.vertices.length;)
00632                                         writer.write(indent+info.vertices[i++]+" "+info.vertices[i++]+" "+info.vertices[i++]+",\n");
00633                                 writer.write(indent+"]\n");
00634                                 writer.write(indent+"}\n");
00635                                 writer.write(indent+"coordIndex [\n");
00636                                 for(int i=0; i<info.triangles.length;)
00637                                         writer.write(indent+info.triangles[i++]+" "+info.triangles[i++]+" "+info.triangles[i++]+" -1,\n");                      
00638                                 writer.write(indent+"]\n");
00639                                 AppearanceInfo appinfo = shape.appearances_[0];
00640                                 if(appinfo != null){
00641                                         if(appinfo.normals.length != 0){
00642                                                 if(!appinfo.normalPerVertex)
00643                                                         writer.write(indent+"normalPerVertex FALSE\n");
00644                                                 writer.write(indent+"normal Normal  {\n");
00645                                                 writer.write(indent+"vector [\n");
00646                                                 for(int i=0; i<appinfo.normals.length;)
00647                                                         writer.write(indent+appinfo.normals[i++]+" "+appinfo.normals[i++]+" "+appinfo.normals[i++]+",\n");
00648                                                 writer.write(indent+"]\n");
00649                                                 writer.write(indent+"}\n");
00650                                         }
00651                                         if(appinfo.normalIndices.length != 0){
00652                                                 writer.write(indent+"normalIndex [\n");
00653                                                 if(appinfo.normalPerVertex){
00654                                                         for(int i=0; i<appinfo.normalIndices.length;)
00655                                                                 writer.write(indent+appinfo.normalIndices[i++]+" "+appinfo.normalIndices[i++]+" "+appinfo.normalIndices[i++]+" -1,\n");
00656                                                 }else{
00657                                                         for(int i=0; i<appinfo.normalIndices.length; i++){
00658                                                                 if(i%3==0) writer.write(indent+"  ");
00659                                                                 writer.write(appinfo.normalIndices[i]+" ");
00660                                                                 if(i%3==2) writer.write("\n");
00661                                                         }
00662                                                 }
00663                                                 writer.write(indent+"]\n");
00664                                         }
00665                                         if(!appinfo.solid)
00666                                                 writer.write(indent+"solid FALSE\n");
00667                                         if (appinfo.creaseAngle != 0.0f)
00668                                                 writer.write(indent+"creaseAngle "+ appinfo.creaseAngle+"\n");
00669                                         if(appinfo.colors.length != 0){
00670                                                 if(!appinfo.colorPerVertex)
00671                                                         writer.write(indent+"colorPerVertex FALSE\n");
00672                                                 writer.write(indent+"color Color  {\n");
00673                                                 writer.write(indent+"color [\n");
00674                                                 for(int i=0; i<appinfo.colors.length;)
00675                                                         writer.write(indent+appinfo.colors[i++]+" "+appinfo.colors[i++]+" "+appinfo.colors[i++]+",\n");
00676                                                 writer.write(indent+"]\n");
00677                                                 writer.write(indent+"}\n");
00678                                         }
00679                                         if(appinfo.colorIndices.length != 0){
00680                                                 writer.write(indent+"colorIndex [\n");
00681                                                 if(appinfo.colorPerVertex){
00682                                                         for(int i=0; i<appinfo.colorIndices.length;)
00683                                                                 writer.write(indent+appinfo.colorIndices[i++]+" "+appinfo.colorIndices[i++]+" "+appinfo.colorIndices[i++]+" -1,\n");
00684                                                 }else{
00685                                                         for(int i=0; i<appinfo.colorIndices.length; i++){
00686                                                                 if(i%3==0) writer.write(indent+"  ");
00687                                                                 writer.write(appinfo.colorIndices[i]+" ");
00688                                                                 if(i%3==2) writer.write("\n");
00689                                                         }
00690                                                 }
00691                                                 writer.write(indent+"]\n");
00692                                         }                               
00693                                         TextureInfo texinfo = shape.textures_[0];
00694                                         if(texinfo != null){
00695                                                 writer.write(indent+"texCoord TextureCoordinate {\n");
00696                                                 writer.write(indent+"point [\n");
00697                                                 for(int i=0; i<appinfo.textureCoordinate.length;)
00698                                                         writer.write(indent+appinfo.textureCoordinate[i++]+" "+appinfo.textureCoordinate[i++]+",\n");
00699                                                 writer.write(indent+"]\n");
00700                                                 writer.write(indent+"}\n");
00701                                                 writer.write(indent+"texCoordIndex [\n");
00702                                                 for(int i=0; i<appinfo.textureCoordIndices.length;)
00703                                                         writer.write(indent+appinfo.textureCoordIndices[i++]+" "+appinfo.textureCoordIndices[i++]+" "+appinfo.textureCoordIndices[i++]+" -1,\n");                       
00704                                                 writer.write(indent+"]\n");
00705                                         }
00706                                 }
00707                                 writer.write(indent+"}\n");
00708                         }else if (ptype == ShapePrimitiveType.SP_BOX){
00709                                 writer.write(indent+"geometry Box{\n");
00710                                 writer.write(indent+"  size "
00711                                                 +pparams[0]+" "
00712                                                 +pparams[1]+" "
00713                                                 +pparams[2]+"\n");
00714                                 writer.write(indent+"}\n");
00715                         }else if (ptype == ShapePrimitiveType.SP_CYLINDER){
00716                                 writer.write(indent+"geometry Cylinder{\n");
00717                                 writer.write(indent+"  radius "+pparams[0]+"\n");
00718                                 writer.write(indent+"  height "+pparams[1]+"\n");
00719                                 if (pparams[2]==0){
00720                                         writer.write(indent+"  top FALSE\n");
00721                                 }
00722                                 if (pparams[3]==0){
00723                                         writer.write(indent+"  bottom FALSE\n");
00724                                 }
00725                                 if (pparams[4]==0){
00726                                         writer.write(indent+" side FALSE\n");
00727                                 }
00728                                 writer.write(indent+"}\n");                                     
00729                         }else if (ptype == ShapePrimitiveType.SP_CONE){
00730                                 writer.write(indent+"geometry Cone{\n");
00731                                 writer.write(indent+"  bottomRadius "+pparams[0]+"\n");
00732                                 writer.write(indent+"  height "+pparams[1]+"\n");
00733                                 if (pparams[2]==0){
00734                                         writer.write(indent+"  bottom FALSE\n");
00735                                 }
00736                                 if (pparams[3]==0){
00737                                         writer.write(indent+"  side FALSE\n");
00738                                 }
00739                                 writer.write(indent+"}\n");                                     
00740                         }else if (ptype == ShapePrimitiveType.SP_SPHERE){
00741                                 writer.write(indent+"geometry Sphere{\n");
00742                                 writer.write(indent+"  radius "+pparams[0]+"\n");
00743                                 writer.write(indent+"}\n");                                     
00744                         }else if (ptype == ShapePrimitiveType.SP_PLANE){
00745                                 writer.write(indent+"geometry Plane{\n");
00746                                 writer.write(indent+"  size "+pparams[0]+" "+pparams[1]+" "+pparams[2]+"\n");
00747                                 writer.write(indent+"}\n");
00748                         }
00749                 }catch(Exception ex){
00750                         ex.printStackTrace();
00751                 }
00752         }
00753         
00754         private static boolean valueEquals(String s1, String s2){
00755                 final double EPS = 0.00000001;
00756                 String[] ss1 = s1.split(" ");
00757                 String[] ss2 = s2.split(" ");
00758                 int n = ss1.length;
00759                 if(n!=ss2.length)
00760                         return false;
00761                 for(int i=0; i<n; i++){
00762                         try{
00763                                 if( Math.abs(Double.parseDouble(ss1[i]) - Double.parseDouble(ss2[i])) > EPS )
00764                                         return false;
00765                         }catch(Exception ex){
00766                                 return false;
00767                         }
00768                 }
00769                 return true;
00770         }
00771         
00772         private static boolean isZero(String s1, int i){
00773                 String[] ss1 = s1.split(" ");
00774                 if(i < ss1.length ){
00775                         try{
00776                                 if( Math.abs(Double.parseDouble(ss1[i])) < 1e-10 )
00777                                         return true;
00778                                 else
00779                                         return false;
00780                         }catch(Exception ex){
00781                                 return false;
00782                         }
00783                 }else
00784                         return false;
00785         }
00786 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:54