GrxShapeTransformItem.java
Go to the documentation of this file.
1 package com.generalrobotix.ui.item;
2 
3 import java.io.File;
4 import java.util.ArrayList;
5 
6 import javax.media.j3d.BadTransformException;
7 import javax.media.j3d.Transform3D;
8 import javax.vecmath.Matrix3d;
9 import javax.vecmath.Matrix4d;
10 import javax.vecmath.Vector3d;
11 
12 import org.eclipse.jface.action.Action;
13 import org.eclipse.jface.action.MenuManager;
14 import org.eclipse.jface.dialogs.InputDialog;
15 import org.eclipse.jface.dialogs.MessageDialog;
16 import org.eclipse.swt.SWT;
17 import org.eclipse.swt.widgets.FileDialog;
18 
19 import jp.go.aist.hrp.simulator.ModelLoader;
20 import jp.go.aist.hrp.simulator.ModelLoaderHelper;
21 import jp.go.aist.hrp.simulator.SceneInfo;
22 import jp.go.aist.hrp.simulator.ShapePrimitiveType;
23 import jp.go.aist.hrp.simulator.TransformedShapeIndex;
24 import jp.go.aist.hrp.simulator.ModelLoaderPackage.ModelLoaderException;
25 
31 
32 @SuppressWarnings("serial")
34  // TransformGroup is parent link local
35  private ArrayList<ShapeTransform> shapeTransforms_;
36  private ArrayList<TransformedShapeIndex> transformedShapeIndices_;
37  public GrxShapeTransformItem(String name, GrxPluginManager manager, GrxModelItem model) {
38  super(name, manager, model);
39  shapeTransforms_ = new ArrayList<ShapeTransform>();
40  transformedShapeIndices_ = new ArrayList<TransformedShapeIndex>();
41  }
42 
43  protected void addTransformedShapeIndex(TransformedShapeIndex transformedShapeIndex){
44  transformedShapeIndices_.add(transformedShapeIndex);
45  }
46 
47  protected void buildShapeTransforms(double[][] inlinedShapeTransformMatrices){
48  ArrayList<Integer> list = new ArrayList<Integer>();
49  for(int i=0; i<transformedShapeIndices_.size(); i++)
50  list.add(i);
51  while(!list.isEmpty()){
52  int inlinedSTMIndex = transformedShapeIndices_.get(list.get(0)).inlinedShapeTransformMatrixIndex;
53  ArrayList<Integer> indices = new ArrayList<Integer>();
54  indices.add(list.get(0));
55  if(inlinedSTMIndex == -1){
56  setShapeIndex(transformedShapeIndices_.get(list.get(0)));
57  }else{
58  for(int k=1; k<list.size(); k++){
59  Integer j = list.get(k);
60  if(inlinedSTMIndex == transformedShapeIndices_.get(j).inlinedShapeTransformMatrixIndex){
61  indices.add(j);
62  }
63  }
64  setInlineShapeIndex(inlinedShapeTransformMatrices[inlinedSTMIndex], indices);
65  }
66  for(int k=0; k<indices.size(); k++)
67  list.remove(indices.get(k));
68  }
69  }
70 
71  private void setShapeIndex(TransformedShapeIndex transformedShapeIndex){
72  Matrix4d mat = new Matrix4d(transformedShapeIndex.transformMatrix[0], transformedShapeIndex.transformMatrix[1], transformedShapeIndex.transformMatrix[2], transformedShapeIndex.transformMatrix[3],
73  transformedShapeIndex.transformMatrix[4], transformedShapeIndex.transformMatrix[5], transformedShapeIndex.transformMatrix[6], transformedShapeIndex.transformMatrix[7],
74  transformedShapeIndex.transformMatrix[8], transformedShapeIndex.transformMatrix[9], transformedShapeIndex.transformMatrix[10], transformedShapeIndex.transformMatrix[11],
75  0,0,0,1 );
76  ShapeTransform _shapeTransform = new ShapeTransform(false, mat, null, transformedShapeIndex.shapeIndex);
77  shapeTransforms_.add(_shapeTransform);
78  }
79 
80  private void setInlineShapeIndex(double[] inlinedMatrix, ArrayList<Integer> indices){
81  Matrix4d inlinedMat = new Matrix4d(inlinedMatrix[0], inlinedMatrix[1], inlinedMatrix[2], inlinedMatrix[3],
82  inlinedMatrix[4], inlinedMatrix[5], inlinedMatrix[6], inlinedMatrix[7],
83  inlinedMatrix[8], inlinedMatrix[9], inlinedMatrix[10], inlinedMatrix[11],
84  0,0,0,1 );
85  Matrix4d[] mat = new Matrix4d[indices.size()];
86  int[] indices0 = new int[indices.size()];
87  for(int i=0; i<indices.size(); i++){
88  TransformedShapeIndex transformedShapeIndex = transformedShapeIndices_.get(indices.get(i));
89  mat[i] = new Matrix4d(transformedShapeIndex.transformMatrix[0], transformedShapeIndex.transformMatrix[1], transformedShapeIndex.transformMatrix[2], transformedShapeIndex.transformMatrix[3],
90  transformedShapeIndex.transformMatrix[4], transformedShapeIndex.transformMatrix[5], transformedShapeIndex.transformMatrix[6], transformedShapeIndex.transformMatrix[7],
91  transformedShapeIndex.transformMatrix[8], transformedShapeIndex.transformMatrix[9], transformedShapeIndex.transformMatrix[10], transformedShapeIndex.transformMatrix[11],
92  0,0,0,1 );
93  indices0[i] = transformedShapeIndex.shapeIndex;
94  }
95  ShapeTransform _shapeTransform = new ShapeTransform(true, mat, inlinedMat, indices0);
96  shapeTransforms_.add(_shapeTransform);
97 
98  }
99 
100  public void addShape(Matrix4d segmentT){
101  for(int i=0; i<shapeTransforms_.size(); i++){
102  GrxShapeItem shapeItem = new GrxShapeItem(getName()+"_shape_"+i, manager_, model_ );
103  if(!shapeTransforms_.get(i).isInline_)
104  shapeItem.loadShape(shapeTransforms_.get(i).transform_[0], shapeTransforms_.get(i).shapeIndices_[0], segmentT);
105  else
106  shapeItem.loadInlineShape(shapeTransforms_.get(i).transform_, shapeTransforms_.get(i).inlinedTransform_, shapeTransforms_.get(i).shapeIndices_, segmentT);
107  addChild(shapeItem);
108  manager_.itemChange(shapeItem, GrxPluginManager.ADD_ITEM);
109  }
110  }
111 
116  public void addShape(String fPath){
117  if( fPath != null ) {
118  try {
119  ModelLoader mloader = ModelLoaderHelper.narrow(
120  GrxCorbaUtil.getReference("ModelLoader"));
121  mloader._non_existent();
122  SceneInfo sInfo = mloader.loadSceneInfo(fPath);
123  int n=children_.size();
124  GrxShapeItem shapeItem = new GrxShapeItem(getName()+"_shape_"+n, manager_, model_);
125  shapeItem.loadnewInlineShape(sInfo);
126  shapeItem.setURL(fPath);
127  addChild(shapeItem);
128 
129  //manager_.reselectItems();
130  manager_.itemChange(shapeItem, GrxPluginManager.ADD_ITEM);
131  } catch(ModelLoaderException me){
132  MessageDialog.openError(GrxUIPerspectiveFactory.getCurrentShell(),
133  MessageBundle.get("GrxModelItem.dialog.title.error"), //$NON-NLS-1$
134  MessageBundle.get("GrxModelItem.dialog.message.loadSceneError") +"\n" + //$NON-NLS-1$ //$NON-NLS-2$
135  fPath + "\n\n" + me.description); //$NON-NLS-1$
136  System.out.println("Failed to load scene info:" + fPath);
137  me.printStackTrace();
138  } catch(Exception ex){
139  MessageDialog.openError(GrxUIPerspectiveFactory.getCurrentShell(),
140  MessageBundle.get("GrxModelItem.dialog.title.error"), //$NON-NLS-1$
141  MessageBundle.get("GrxModelItem.dialog.message.NoModelLoader") );
142  System.out.println("Failed to load scene info:" + fPath);
143  ex.printStackTrace();
144  }
145  }
146 
147  }
148 
149  public void addPrimitiveShape(String name){
150  int n=children_.size();
151  int type;
152  if(name.equals("Box"))
153  type = ShapePrimitiveType._SP_BOX;
154  else if(name.equals("Cone"))
155  type = ShapePrimitiveType._SP_CONE;
156  else if(name.equals("Cylinder"))
157  type = ShapePrimitiveType._SP_CYLINDER;
158  else if(name.equals("Sphere"))
159  type = ShapePrimitiveType._SP_SPHERE;
160  else
161  type = -1;
162  GrxShapeItem shapeItem = new GrxShapeItem(getName()+"_"+name+"_"+n, manager_, model_);
163  shapeItem.createnewPrimitiveShape(type);
164  addChild(shapeItem);
165  //manager_.reselectItems();
166  manager_.itemChange(shapeItem, GrxPluginManager.ADD_ITEM);
167  }
168 
169  public void transform(double[] w){
170  Matrix3d m = new Matrix3d(w[0],w[1],w[2],w[4],w[5],w[6],w[8],w[9],w[10]);
171  double[] p = {w[3], w[7], w[11]};
172  translation(p);
173  AxisAngle4d a4d = new AxisAngle4d();
174  a4d.setMatrix(m);
175  double [] rot = new double[4];
176  a4d.get(rot);
177  rotation(rot);
178  }
179 
180  public Matrix4d getTransform(){
181  Transform3D t3d = new Transform3D();
182  tg_.getTransform(t3d);
183  Matrix4d ret = new Matrix4d();
184  t3d.get(ret);
185  return ret;
186  }
187 
188  public double[] translation(){
189  Transform3D t3d = new Transform3D();
190  tg_.getTransform(t3d);
191  Vector3d v = new Vector3d();
192  t3d.get(v);
193  double[] ret = new double[3];
194  v.get(ret);
195  return ret;
196 
197  }
198 
199  public double[] rotation(){
200  Transform3D t3d = new Transform3D();
201  tg_.getTransform(t3d);
202  Matrix3d mat = new Matrix3d();
203  t3d.get(mat);
204  AxisAngle4d aa = new AxisAngle4d();
205  aa.setMatrix(mat);
206  double[] ret = new double[4];
207  aa.get(ret);
208  return ret;
209  }
210 
216  public boolean translation(String value){
217  double [] pos = getDblAry(value);
218  if (translation(pos)){
219  return true;
220  }else{
221  return false;
222  }
223  }
224 
230  public boolean translation(double[] pos){
231  if (pos == null || pos.length != 3) return false;
232  Vector3d v = new Vector3d(pos);
233  Transform3D t3d = new Transform3D();
234  tg_.getTransform(t3d);
235  t3d.setTranslation(v);
236  setDblAry("translation", pos, 4);
237  if (model_ != null && parent_ != null) model_.notifyModified();
238  try{
239  tg_.setTransform(t3d);
240  }catch(BadTransformException e){
241  System.out.println("Invalid translation:"+v+" is applied to "+getName());
242  return false;
243  }
244  return true;
245  }
246 
252  public boolean rotation(String value){
253  double [] rot = getDblAry(value);
254  if (rotation(rot)){
255  return true;
256  }else{
257  return false;
258  }
259  }
260 
266  public boolean rotation(double[] rot){
267  if (rot == null || rot.length != 4) return false;
268  Transform3D t3d = new Transform3D();
269  tg_.getTransform(t3d);
270  t3d.setRotation(new AxisAngle4d(rot));
271  tg_.setTransform(t3d);
272  setDblAry("rotation", rot, 4);
273  if (model_ != null && parent_ != null) model_.notifyModified();
274  return true;
275  }
276 
277  protected void initMenu(){
278  getMenu().clear();
279  Action item;
280 
281  // rename
282  item = new Action(){
283  public String getText(){
284  return MessageBundle.get("GrxSensorItem.menu.rename"); //$NON-NLS-1$
285  }
286  public void run(){
287  InputDialog dialog = new InputDialog( null, getText(),
288  MessageBundle.get("GrxSensorItem.dialog.message.newName"), getName(),null); //$NON-NLS-1$
289  if ( dialog.open() == InputDialog.OK && dialog.getValue() != null)
290  rename( dialog.getValue() );
291  }
292  };
293  setMenuItem(item);
294 
295  // delete
296  item = new Action(){
297  public String getText(){
298  return MessageBundle.get("GrxSensorItem.menu.delete"); //$NON-NLS-1$
299  }
300  public void run(){
301  if( MessageDialog.openQuestion( null, MessageBundle.get("GrxSensorItem.dialog.title.delete"), //$NON-NLS-1$
302  MessageBundle.get("GrxSensorItem.dialog.message.delete") + getName() + " ?") ) //$NON-NLS-1$ //$NON-NLS-2$
303  delete();
304  }
305  };
306  setMenuItem(item);
307 
308  // menu item : add shape
309  item = new Action(){
310  public String getText(){
311  return MessageBundle.get("GrxLinkItem.menu.VRML97"); //$NON-NLS-1$
312  }
313  public void run(){
314  FileDialog fdlg = new FileDialog( GrxUIPerspectiveFactory.getCurrentShell(), SWT.OPEN);
315  fdlg.setFilterExtensions(new String[]{"*.wrl"});
316  fdlg.setFilterPath(getDefaultDir().getAbsolutePath());
317  String fPath = fdlg.open();
318  System.out.println("fPath = "+fPath); //$NON-NLS-1$
319  if( fPath != null ) {
320  addShape( fPath );
321  setDefaultDirectory(new File(fPath).getParent());
322  }
323  }
324  };
325  setMenuItem(item);
326 
327  // menu item : add primitive shape
328  MenuManager subMenu= new MenuManager(MessageBundle.get("GrxLinkItem.menu.primitiveShape")); //$NON-NLS-1$
329  setSubMenu(subMenu);
330  item = new Action(){
331  public String getText(){
332  return "Box"; //$NON-NLS-1$
333  }
334  public void run(){
335  addPrimitiveShape("Box"); //$NON-NLS-1$
336  }
337  };
338  subMenu.add(item);
339  item = new Action(){
340  public String getText(){
341  return "Cone"; //$NON-NLS-1$
342  }
343  public void run(){
344  addPrimitiveShape("Cone"); //$NON-NLS-1$
345  }
346  };
347  subMenu.add(item);
348  item = new Action(){
349  public String getText(){
350  return "Cylinder"; //$NON-NLS-1$
351  }
352  public void run(){
353  addPrimitiveShape("Cylinder"); //$NON-NLS-1$
354  }
355  };
356  subMenu.add(item);
357  item = new Action(){
358  public String getText(){
359  return "Sphere"; //$NON-NLS-1$
360  }
361  public void run(){
362  addPrimitiveShape("Sphere"); //$NON-NLS-1$
363  }
364  };
365  subMenu.add(item);
366  setSubMenu(subMenu);
367  }
368 
369  public void rename(String newName) {
370  setName(newName);
371  if (model_ != null) model_.notifyModified();
372  }
373 }
374 
375 class ShapeTransform {
376  public boolean isInline_ = false;
377  public Matrix4d[] transform_ = null;
378  public Matrix4d inlinedTransform_ = null;
379  public int[] shapeIndices_ = null;
380 
381  public ShapeTransform(boolean isInline, Matrix4d mat, Matrix4d inlinedMat, int index){
382  isInline_ = isInline;
383  transform_ = new Matrix4d[1];
384  transform_[0] = mat;
385  inlinedTransform_ = inlinedMat;
386  shapeIndices_ = new int[1];
387  shapeIndices_[0] = index;
388  }
389 
390  public ShapeTransform(boolean isInline, Matrix4d[] mat, Matrix4d inlinedMat, int[] indices){
391  isInline_ = isInline;
392  transform_ = mat;
393  inlinedTransform_ = inlinedMat;
394  shapeIndices_ = indices;
395  }
396 }
ArrayList< TransformedShapeIndex > transformedShapeIndices_
boolean translation(String value)
set new translation from string
static final String get(String key)
png_infop png_charp png_int_32 png_int_32 int * type
Definition: png.h:2332
#define null
our own NULL pointer
Definition: IceTypes.h:57
void setShapeIndex(TransformedShapeIndex transformedShapeIndex)
png_infop png_charpp name
Definition: png.h:2382
png_voidp int value
Definition: png.h:2113
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
item corresponds to a robot model
void setURL(String url)
set URL property
png_uint_32 i
Definition: png.h:2735
item which have a transformation
boolean translation(double[] pos)
set new translation to TransformGroup
void buildShapeTransforms(double[][] inlinedShapeTransformMatrices)
list index
def run(tree, args)
void setInlineShapeIndex(double[] inlinedMatrix, ArrayList< Integer > indices)
void loadInlineShape(Matrix4d[] shapeT, Matrix4d inlinedT, int[] index, Matrix4d segmentT)
static org.omg.CORBA.Object getReference(String id)
get CORBA object which is associated with id
void addShape(String fPath)
create and add a new shape as a child
プラグイン管理クラス GrxUIの核になるクラス。プラグインのロード等の、初期化を実行する。 プラグインとそ...
def getText(self, nodes=None)
void addTransformedShapeIndex(TransformedShapeIndex transformedShapeIndex)
void loadShape(Matrix4d shapeT, int index, Matrix4d segmentT)
org
boolean rotation(String value)
set new rotation from string
GrxShapeTransformItem(String name, GrxPluginManager manager, GrxModelItem model)


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:03