GrxShapeTransformItem.java
Go to the documentation of this file.
00001 package com.generalrobotix.ui.item;
00002 
00003 import java.io.File;
00004 import java.util.ArrayList;
00005 
00006 import javax.media.j3d.BadTransformException;
00007 import javax.media.j3d.Transform3D;
00008 import javax.vecmath.Matrix3d;
00009 import javax.vecmath.Matrix4d;
00010 import javax.vecmath.Vector3d;
00011 
00012 import org.eclipse.jface.action.Action;
00013 import org.eclipse.jface.action.MenuManager;
00014 import org.eclipse.jface.dialogs.InputDialog;
00015 import org.eclipse.jface.dialogs.MessageDialog;
00016 import org.eclipse.swt.SWT;
00017 import org.eclipse.swt.widgets.FileDialog;
00018 
00019 import jp.go.aist.hrp.simulator.ModelLoader;
00020 import jp.go.aist.hrp.simulator.ModelLoaderHelper;
00021 import jp.go.aist.hrp.simulator.SceneInfo;
00022 import jp.go.aist.hrp.simulator.ShapePrimitiveType;
00023 import jp.go.aist.hrp.simulator.TransformedShapeIndex;
00024 import jp.go.aist.hrp.simulator.ModelLoaderPackage.ModelLoaderException;
00025 
00026 import com.generalrobotix.ui.GrxPluginManager;
00027 import com.generalrobotix.ui.grxui.GrxUIPerspectiveFactory;
00028 import com.generalrobotix.ui.util.AxisAngle4d;
00029 import com.generalrobotix.ui.util.GrxCorbaUtil;
00030 import com.generalrobotix.ui.util.MessageBundle;
00031 
00032 @SuppressWarnings("serial")
00033 public class GrxShapeTransformItem extends GrxTransformItem {
00034         // TransformGroup is parent link local 
00035         private ArrayList<ShapeTransform> shapeTransforms_;  
00036         private ArrayList<TransformedShapeIndex> transformedShapeIndices_;
00037         public GrxShapeTransformItem(String name, GrxPluginManager manager, GrxModelItem model) {
00038                 super(name, manager, model);
00039                 shapeTransforms_ = new ArrayList<ShapeTransform>();
00040                 transformedShapeIndices_ = new ArrayList<TransformedShapeIndex>(); 
00041         }
00042         
00043         protected void addTransformedShapeIndex(TransformedShapeIndex transformedShapeIndex){
00044                 transformedShapeIndices_.add(transformedShapeIndex);
00045         }
00046         
00047         protected void buildShapeTransforms(double[][] inlinedShapeTransformMatrices){
00048                 ArrayList<Integer> list = new ArrayList<Integer>();
00049                 for(int i=0; i<transformedShapeIndices_.size(); i++)
00050                         list.add(i);
00051                 while(!list.isEmpty()){
00052                 int inlinedSTMIndex = transformedShapeIndices_.get(list.get(0)).inlinedShapeTransformMatrixIndex;
00053                 ArrayList<Integer> indices = new ArrayList<Integer>();
00054                 indices.add(list.get(0));
00055                 if(inlinedSTMIndex == -1){ 
00056                         setShapeIndex(transformedShapeIndices_.get(list.get(0)));               
00057                 }else{  
00058                         for(int k=1; k<list.size(); k++){
00059                         Integer j = list.get(k);
00060                         if(inlinedSTMIndex == transformedShapeIndices_.get(j).inlinedShapeTransformMatrixIndex){
00061                                 indices.add(j);                         
00062                         }
00063                 }
00064                         setInlineShapeIndex(inlinedShapeTransformMatrices[inlinedSTMIndex], indices);
00065                 }
00066                 for(int k=0; k<indices.size(); k++)
00067                         list.remove(indices.get(k));
00068                 }
00069         }
00070 
00071         private void setShapeIndex(TransformedShapeIndex transformedShapeIndex){
00072                 Matrix4d mat = new Matrix4d(transformedShapeIndex.transformMatrix[0], transformedShapeIndex.transformMatrix[1], transformedShapeIndex.transformMatrix[2], transformedShapeIndex.transformMatrix[3],
00073                                 transformedShapeIndex.transformMatrix[4], transformedShapeIndex.transformMatrix[5], transformedShapeIndex.transformMatrix[6], transformedShapeIndex.transformMatrix[7],
00074                                 transformedShapeIndex.transformMatrix[8], transformedShapeIndex.transformMatrix[9], transformedShapeIndex.transformMatrix[10], transformedShapeIndex.transformMatrix[11],
00075                                 0,0,0,1 );
00076                 ShapeTransform _shapeTransform = new ShapeTransform(false, mat, null, transformedShapeIndex.shapeIndex);
00077                 shapeTransforms_.add(_shapeTransform);
00078         }
00079         
00080         private void setInlineShapeIndex(double[] inlinedMatrix, ArrayList<Integer> indices){
00081                 Matrix4d inlinedMat = new Matrix4d(inlinedMatrix[0], inlinedMatrix[1], inlinedMatrix[2], inlinedMatrix[3],
00082                                 inlinedMatrix[4], inlinedMatrix[5], inlinedMatrix[6], inlinedMatrix[7],
00083                                 inlinedMatrix[8], inlinedMatrix[9], inlinedMatrix[10], inlinedMatrix[11],
00084                                 0,0,0,1 );
00085                 Matrix4d[] mat = new Matrix4d[indices.size()];
00086                 int[] indices0 = new int[indices.size()];
00087                 for(int i=0; i<indices.size(); i++){
00088                         TransformedShapeIndex transformedShapeIndex = transformedShapeIndices_.get(indices.get(i));
00089                         mat[i] = new Matrix4d(transformedShapeIndex.transformMatrix[0], transformedShapeIndex.transformMatrix[1], transformedShapeIndex.transformMatrix[2], transformedShapeIndex.transformMatrix[3],
00090                                         transformedShapeIndex.transformMatrix[4], transformedShapeIndex.transformMatrix[5], transformedShapeIndex.transformMatrix[6], transformedShapeIndex.transformMatrix[7],
00091                                         transformedShapeIndex.transformMatrix[8], transformedShapeIndex.transformMatrix[9], transformedShapeIndex.transformMatrix[10], transformedShapeIndex.transformMatrix[11],
00092                                         0,0,0,1 );
00093                         indices0[i] = transformedShapeIndex.shapeIndex;
00094                 }
00095                 ShapeTransform _shapeTransform = new ShapeTransform(true, mat, inlinedMat, indices0);
00096                 shapeTransforms_.add(_shapeTransform);
00097                 
00098         }
00099         
00100         public void addShape(Matrix4d segmentT){
00101                 for(int i=0; i<shapeTransforms_.size(); i++){
00102                         GrxShapeItem shapeItem = new GrxShapeItem(getName()+"_shape_"+i, manager_, model_ ); 
00103                         if(!shapeTransforms_.get(i).isInline_)
00104                                 shapeItem.loadShape(shapeTransforms_.get(i).transform_[0], shapeTransforms_.get(i).shapeIndices_[0], segmentT);
00105                         else
00106                                 shapeItem.loadInlineShape(shapeTransforms_.get(i).transform_, shapeTransforms_.get(i).inlinedTransform_, shapeTransforms_.get(i).shapeIndices_, segmentT);
00107                         addChild(shapeItem);
00108                         manager_.itemChange(shapeItem, GrxPluginManager.ADD_ITEM);
00109                 }
00110         }
00111         
00116     public void addShape(String fPath){
00117                 if( fPath != null ) {
00118             try {
00119                 ModelLoader mloader = ModelLoaderHelper.narrow(
00120                         GrxCorbaUtil.getReference("ModelLoader"));
00121                 mloader._non_existent();
00122                 SceneInfo sInfo = mloader.loadSceneInfo(fPath);
00123                 int n=children_.size();
00124                 GrxShapeItem shapeItem = new GrxShapeItem(getName()+"_shape_"+n, manager_, model_);
00125                 shapeItem.loadnewInlineShape(sInfo);
00126                 shapeItem.setURL(fPath);
00127                 addChild(shapeItem);
00128                 
00129                 //manager_.reselectItems();
00130                 manager_.itemChange(shapeItem, GrxPluginManager.ADD_ITEM);
00131             } catch(ModelLoaderException me){
00132                 MessageDialog.openError(GrxUIPerspectiveFactory.getCurrentShell(),
00133                                         MessageBundle.get("GrxModelItem.dialog.title.error"), //$NON-NLS-1$
00134                                         MessageBundle.get("GrxModelItem.dialog.message.loadSceneError") +"\n" + //$NON-NLS-1$ //$NON-NLS-2$
00135                                         fPath + "\n\n" + me.description); //$NON-NLS-1$
00136                 System.out.println("Failed to load scene info:" + fPath);
00137                 me.printStackTrace();
00138             } catch(Exception ex){
00139                 MessageDialog.openError(GrxUIPerspectiveFactory.getCurrentShell(),
00140                         MessageBundle.get("GrxModelItem.dialog.title.error"), //$NON-NLS-1$
00141                         MessageBundle.get("GrxModelItem.dialog.message.NoModelLoader") ); 
00142                 System.out.println("Failed to load scene info:" + fPath);
00143                 ex.printStackTrace();
00144             }
00145                 }
00146 
00147     }
00148        
00149     public void addPrimitiveShape(String name){
00150                 int n=children_.size();
00151                 int type;
00152                 if(name.equals("Box"))
00153                         type = ShapePrimitiveType._SP_BOX;
00154                 else if(name.equals("Cone"))
00155                         type = ShapePrimitiveType._SP_CONE;
00156                 else if(name.equals("Cylinder"))
00157                         type = ShapePrimitiveType._SP_CYLINDER;
00158                 else if(name.equals("Sphere"))
00159                         type = ShapePrimitiveType._SP_SPHERE;
00160                 else
00161                         type = -1;
00162                 GrxShapeItem shapeItem = new GrxShapeItem(getName()+"_"+name+"_"+n, manager_, model_);
00163                 shapeItem.createnewPrimitiveShape(type);
00164                 addChild(shapeItem);
00165                 //manager_.reselectItems();
00166                 manager_.itemChange(shapeItem, GrxPluginManager.ADD_ITEM);
00167         }
00168          
00169         public void transform(double[] w){
00170                 Matrix3d m = new Matrix3d(w[0],w[1],w[2],w[4],w[5],w[6],w[8],w[9],w[10]);
00171         double[] p = {w[3], w[7], w[11]};
00172                 translation(p);
00173                 AxisAngle4d a4d = new AxisAngle4d();
00174         a4d.setMatrix(m);
00175         double [] rot = new double[4];
00176         a4d.get(rot);
00177         rotation(rot);
00178     }
00179     
00180     public Matrix4d getTransform(){
00181         Transform3D t3d = new Transform3D();
00182         tg_.getTransform(t3d);
00183         Matrix4d ret = new Matrix4d();
00184         t3d.get(ret);
00185         return ret;
00186     }
00187     
00188     public double[] translation(){
00189         Transform3D t3d = new Transform3D();
00190         tg_.getTransform(t3d);
00191         Vector3d v = new Vector3d();
00192         t3d.get(v);
00193         double[] ret = new double[3];
00194         v.get(ret);
00195         return ret;
00196         
00197     }
00198     
00199     public double[] rotation(){
00200         Transform3D t3d = new Transform3D();
00201         tg_.getTransform(t3d);
00202         Matrix3d mat = new Matrix3d();
00203         t3d.get(mat);
00204         AxisAngle4d aa = new AxisAngle4d();
00205         aa.setMatrix(mat);
00206         double[] ret = new double[4];
00207         aa.get(ret);
00208         return ret;
00209     }
00210     
00216     public boolean translation(String value){
00217         double [] pos = getDblAry(value);
00218         if (translation(pos)){
00219             return true;
00220         }else{
00221                 return false;
00222         }
00223     }
00224     
00230     public boolean translation(double[] pos){
00231         if (pos == null || pos.length != 3) return false;
00232         Vector3d v = new Vector3d(pos);
00233         Transform3D t3d = new Transform3D();
00234         tg_.getTransform(t3d);
00235         t3d.setTranslation(v);
00236         setDblAry("translation", pos, 4);
00237         if (model_ != null && parent_ != null) model_.notifyModified();
00238         try{
00239                 tg_.setTransform(t3d);
00240         }catch(BadTransformException e){
00241                 System.out.println("Invalid translation:"+v+" is applied to "+getName());
00242                 return false;
00243         }
00244         return true;
00245     }
00246 
00252     public boolean rotation(String value){
00253         double [] rot = getDblAry(value);
00254         if (rotation(rot)){
00255             return true;
00256         }else{
00257                 return false;
00258         }
00259     }
00260     
00266     public boolean rotation(double[] rot){
00267         if (rot == null || rot.length != 4) return false;
00268         Transform3D t3d = new Transform3D();
00269         tg_.getTransform(t3d);
00270         t3d.setRotation(new AxisAngle4d(rot));
00271         tg_.setTransform(t3d);
00272         setDblAry("rotation", rot, 4);
00273         if (model_ != null && parent_ != null) model_.notifyModified();
00274         return true;
00275     }
00276 
00277     protected void initMenu(){
00278         getMenu().clear();
00279                 Action item;
00280 
00281                 // rename
00282                 item = new Action(){
00283                         public String getText(){
00284                                 return MessageBundle.get("GrxSensorItem.menu.rename"); //$NON-NLS-1$
00285                         }
00286                         public void run(){
00287                                 InputDialog dialog = new InputDialog( null, getText(),
00288                                                 MessageBundle.get("GrxSensorItem.dialog.message.newName"), getName(),null); //$NON-NLS-1$
00289                                 if ( dialog.open() == InputDialog.OK && dialog.getValue() != null)
00290                                         rename( dialog.getValue() );
00291                         }
00292                 };
00293                 setMenuItem(item);
00294 
00295                 // delete
00296                 item = new Action(){
00297                         public String getText(){
00298                                 return MessageBundle.get("GrxSensorItem.menu.delete"); //$NON-NLS-1$
00299                         }
00300                         public void run(){
00301                                 if( MessageDialog.openQuestion( null, MessageBundle.get("GrxSensorItem.dialog.title.delete"), //$NON-NLS-1$
00302                                                 MessageBundle.get("GrxSensorItem.dialog.message.delete") + getName() + " ?") ) //$NON-NLS-1$ //$NON-NLS-2$
00303                                         delete();
00304                         }
00305                 };
00306                 setMenuItem(item);
00307                 
00308                 // menu item : add shape
00309                 item = new Action(){
00310                         public String getText(){
00311                                 return MessageBundle.get("GrxLinkItem.menu.VRML97"); //$NON-NLS-1$
00312                         }
00313                         public void run(){
00314                                 FileDialog fdlg = new FileDialog( GrxUIPerspectiveFactory.getCurrentShell(), SWT.OPEN);
00315                                 fdlg.setFilterExtensions(new String[]{"*.wrl"});
00316                                 fdlg.setFilterPath(getDefaultDir().getAbsolutePath());
00317                                 String fPath = fdlg.open();
00318                                 System.out.println("fPath = "+fPath); //$NON-NLS-1$
00319                                 if( fPath != null ) {
00320                                         addShape( fPath );
00321                                         setDefaultDirectory(new File(fPath).getParent());
00322                                 }
00323                         }
00324                 };
00325         setMenuItem(item);
00326         
00327         // menu item : add primitive shape
00328         MenuManager subMenu= new MenuManager(MessageBundle.get("GrxLinkItem.menu.primitiveShape")); //$NON-NLS-1$
00329         setSubMenu(subMenu);      
00330         item = new Action(){
00331                         public String getText(){
00332                                 return "Box"; //$NON-NLS-1$
00333                         }
00334                         public void run(){
00335                                 addPrimitiveShape("Box"); //$NON-NLS-1$
00336                         }
00337                 };
00338                 subMenu.add(item);
00339                 item = new Action(){
00340                         public String getText(){
00341                                 return "Cone"; //$NON-NLS-1$
00342                         }
00343                         public void run(){
00344                                 addPrimitiveShape("Cone"); //$NON-NLS-1$
00345                         }
00346                 };
00347                 subMenu.add(item);
00348                 item = new Action(){
00349                         public String getText(){
00350                                 return "Cylinder"; //$NON-NLS-1$
00351                         }
00352                         public void run(){
00353                                 addPrimitiveShape("Cylinder"); //$NON-NLS-1$
00354                         }
00355                 };
00356                 subMenu.add(item);
00357                 item = new Action(){
00358                         public String getText(){
00359                                 return "Sphere"; //$NON-NLS-1$
00360                         }
00361                         public void run(){
00362                                 addPrimitiveShape("Sphere"); //$NON-NLS-1$
00363                         }
00364                 };
00365                 subMenu.add(item);
00366                 setSubMenu(subMenu);
00367     }
00368     
00369     public void rename(String newName) {
00370         setName(newName);
00371         if (model_ != null) model_.notifyModified();
00372     }
00373 }
00374 
00375 class ShapeTransform {
00376         public boolean isInline_ = false;
00377         public Matrix4d[] transform_ = null;
00378         public Matrix4d inlinedTransform_ = null;
00379         public int[] shapeIndices_ = null;
00380         
00381         public ShapeTransform(boolean isInline, Matrix4d mat, Matrix4d inlinedMat, int index){
00382                 isInline_ = isInline;
00383                 transform_ = new Matrix4d[1];
00384                 transform_[0] = mat; 
00385                 inlinedTransform_ = inlinedMat;
00386                 shapeIndices_ = new int[1];
00387                 shapeIndices_[0] = index;
00388         }
00389         
00390         public ShapeTransform(boolean isInline, Matrix4d[] mat, Matrix4d inlinedMat, int[] indices){
00391                 isInline_ = isInline;
00392                 transform_ = mat;
00393                 inlinedTransform_ = inlinedMat;
00394                 shapeIndices_ = indices;
00395         }
00396 }


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