GrxModelItem.java
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * General Robotix Inc.
8  * National Institute of Advanced Industrial Science and Technology (AIST)
9  */
10 
11 /*
12  * GrxModelItem.java
13  *
14  * @author Yuichiro Kawasumi (General Robotix, Inc.)
15  * @author Shin'ichiro Nakaoka (AIST)
16  */
17 
18 package com.generalrobotix.ui.item;
19 
20 import java.io.File;
21 import java.io.IOException;
22 import java.util.*;
23 
24 import javax.media.j3d.*;
25 import javax.vecmath.*;
26 
27 import org.eclipse.jface.action.Action;
28 import org.eclipse.jface.dialogs.InputDialog;
29 import org.eclipse.jface.dialogs.MessageDialog;
30 import org.eclipse.osgi.util.NLS;
31 import org.eclipse.swt.SWT;
32 import org.eclipse.swt.widgets.FileDialog;
33 import org.w3c.dom.Element;
34 import org.w3c.dom.NodeList;
35 
37 import com.generalrobotix.ui.*;
38 import com.generalrobotix.ui.util.*;
42 
43 import jp.go.aist.hrp.simulator.*;
44 import jp.go.aist.hrp.simulator.ModelLoaderPackage.AABBdataType;
45 import jp.go.aist.hrp.simulator.ModelLoaderPackage.ModelLoadOption;
46 import jp.go.aist.hrp.simulator.ModelLoaderPackage.ModelLoaderException;
47 
48 @SuppressWarnings({ "unchecked", "serial" }) //$NON-NLS-1$ //$NON-NLS-2$
52 public class GrxModelItem extends GrxBaseItem implements Manipulatable {
53  public static final String TITLE = "Model"; //$NON-NLS-1$
54  public static final String DEFAULT_DIR = "/../model"; //$NON-NLS-1$
55  public static final String FILE_EXTENSION = "*"; //$NON-NLS-1$
56  private static final double DEFAULT_RADIUS = 0.05;
57 
58  // icons
59  private static final String robotIcon = "robot.png"; //$NON-NLS-1$
60  private static final String envIcon = "environment.png"; //$NON-NLS-1$
61 
62  private boolean isRobot_ = true;
63 
64  private BodyInfo bInfo_;
65  private boolean bModified_ = false; //< true if this model is modified, false otherwise
66 
67  public BranchGroup bgRoot_ = new BranchGroup();
68  public Vector<GrxLinkItem> links_ = new Vector<GrxLinkItem>();
69  public Vector<GrxExtraJointItem> extraJoints_ = new Vector<GrxExtraJointItem>();
70  // jontId -> link
71  private int[] jointToLink_;
72  public Map<String, GrxLinkItem> nameToLink_ = new HashMap<String, GrxLinkItem>();
73 
74  // list of cameras
75  private List<Camera_impl> cameraList_ = new ArrayList<Camera_impl>();
76 
77  public ShapeInfo[] shapes = null;
78  public AppearanceInfo[] appearances = null;
79  public MaterialInfo[] materials = null;
80  public TextureInfo[] textures = null;
81 
82  // CoM
83  private Switch switchCom_;
84  private TransformGroup tgCom_;
85 
86  // CoM projected on the floor
87  private Switch switchComZ0_;
88  private TransformGroup tgComZ0_;
89 
90  // bounding box of the whole body
91  private Switch switchBb_;
92 
96  public void notifyModified(){
97  //System.out.println(getName()+" : modification is notified");
98  notifyObservers("Modified");
99  bModified_ = true;
100  }
101 
102  public boolean isModified(){
103  return bModified_;
104  }
105 
106  public static final int MODIFIED_OK=0;
107  public static final int MODIFIED_NG=1;
108  public static final int MODIFIED_NOT=2;
109  public int checkModifiedModel(boolean reload){
110  if(bModified_){
111  String mes = MessageBundle.get("GrxProjectItem.dialog.message.changeModel.mes"); //$NON-NLS-1$
112  mes = NLS.bind(mes, new String[]{getName()});
113 
114  MessageDialog msgDlg =
115  new MessageDialog(GrxUIPerspectiveFactory.getCurrentShell(),
116  MessageBundle.get("GrxProjectItem.dialog.message.changeModel.title"),
117  null,
118  mes,
119  MessageDialog.INFORMATION,
120  new String[] {MessageBundle.get("GrxProjectItem.dialog.message.changeModel.btn.save"),
121  MessageBundle.get("GrxProjectItem.dialog.message.changeModel.btn.reverse"),
122  MessageBundle.get("GrxProjectItem.dialog.message.changeModel.btn.cancel")},
123  2);
124 
125  switch(msgDlg.open())
126  {
127  case 0:
128  if(reload){
129  if(!saveAndLoad())
130  return MODIFIED_NG;
131  else
132  return MODIFIED_OK;
133  }else{
134  if(!_saveAs())
135  return MODIFIED_NG;
136  cancelModified();
137  return MODIFIED_OK;
138  }
139  case 1:
140  if(reload){
141  if(!reload())
142  return MODIFIED_NG;
143  else
144  return MODIFIED_OK;
145  }else{
146  cancelModified();
147  return MODIFIED_OK;
148  }
149  case 2:
150  default:
151  return MODIFIED_NG;
152  }
153  }
154  return MODIFIED_NOT;
155  }
156 
157  public void cancelModified(){
158  bModified_ = false;
159  notifyObservers("ClearModified");
160  }
161 
162  public boolean saveAndLoad(){
163  if(!_saveAs())
164  return false;
165  reload();
166  return true;
167  }
168 
169  public boolean reload(){
170  File f = new File(getURL(true));
171  load0(f);
172  sameUrlModelLoad();
173  return true;
174  }
175 
176  private void sameUrlModelLoad(){
177  File f = new File(getURL(true));
178  List<GrxModelItem> sameModels = getSameUrlModels();
179  Iterator<GrxModelItem> it = sameModels.iterator();
180  while(it.hasNext()){
181  it.next().load0(f);
182  }
183  }
184 
189  public BodyInfo getBodyInfo(){
190  String url = getURL(true);
191  if (bModified_ || url == null || url.equals("")) //$NON-NLS-1$
192  return null;
193  else
194  return bInfo_;
195  }
196 
200  class MenuChangeType extends Action {
201  public MenuChangeType() {
202  if (isRobot_) {
203  setText( MessageBundle.get("GrxModelItem.menu.changeEnv") ); //$NON-NLS-1$
204  } else {
205  setText( MessageBundle.get("GrxModelItem.menu.changeRobot") ); //$NON-NLS-1$
206  }
207  }
208  public void run(){
209  _setModelType(!isRobot_);
210  }
211  };
212  MenuChangeType menuChangeType_ = new MenuChangeType();
213 
220  super(name, item);
221  setIcon(robotIcon);
222  _initMenu();
223 
224  bgRoot_.setCapability(BranchGroup.ALLOW_DETACH);
225  bgRoot_.setCapability(BranchGroup.ALLOW_CHILDREN_READ);
226  bgRoot_.setCapability(BranchGroup.ALLOW_CHILDREN_WRITE);
227  bgRoot_.setCapability(BranchGroup.ALLOW_CHILDREN_EXTEND);
228 
229  // create root link
230  GrxLinkItem link = new GrxLinkItem("root", manager_, this); //$NON-NLS-1$
231  manager_.itemChange(link, GrxPluginManager.ADD_ITEM);
232  link.jointType("free"); //$NON-NLS-1$
233  bgRoot_.addChild(link.bg_);
234 
235  setProperty("url",""); //$NON-NLS-1$ //$NON-NLS-2$
236 
237  _setupMarks();
238  }
239 
246  public void addLink(GrxLinkItem link){
247  //System.out.println("link is added : "+link.getName());
248  links_.add(link);
249  notifyModified();
250  }
251 
256  public void removeLink(GrxLinkItem link){
257  //System.out.println("link is removed : "+link.getName());
258  links_.remove(link);
259  notifyModified();
260  }
261 
265  private void _initMenu() {
266  // menu item : reload
267  setMenuItem(new Action(){
268  public String getText(){ return MessageBundle.get("GrxModelItem.menu.reload"); } //$NON-NLS-1$
269  public void run(){
270  boolean ret = load0(file_);
271  if(ret){
272  List<GrxModelItem> sameModels = getSameUrlModels();
273  Iterator<GrxModelItem> it = sameModels.iterator();
274  while(it.hasNext()){
275  it.next().load0(file_);
276  }
277  }
278  }
279  });
280 
281  setMenuItem(menuChangeType_);
282 
283  // menu item : save
284  setMenuItem(new Action(){
285  public String getText() { return MessageBundle.get("GrxModelItem.menu.save"); } //$NON-NLS-1$
286  public void run(){
287  String url = getStr("url"); //$NON-NLS-1$
288  if (url == null || url.equals("")){ //$NON-NLS-1$
289  _saveAs();
290  }else{
292  }
293  if(bModified_)
294  reload();
295  else
296  sameUrlModelLoad();
297  }
298  });
299 
300  // menu item : save as
301  setMenuItem(new Action(){
302  public String getText() { return MessageBundle.get("GrxModelItem.menu.saveAs"); } //$NON-NLS-1$
303  public void run(){
304  _saveAs();
305  if(bModified_)
306  reload();
307  else
308  sameUrlModelLoad();
309  }
310  });
311 
312  // menu item : add ExtraJoint
313  setMenuItem(new Action(){
314  public String getText() { return MessageBundle.get("GrxModelItem.menu.addExtraJoint"); } //$NON-NLS-1$
315  public void run(){
316  InputDialog dialog = new InputDialog( null, getText(),
317  MessageBundle.get("GrxModelItem.dialog.message.extraJointName"), null,null); //$NON-NLS-1$
318  if ( dialog.open() == InputDialog.OK && dialog.getValue() != null)
319  addExtraJoint( dialog.getValue() );
320  }
321  });
322 
323  /* disable copy and paste menus until they are implemented
324  // menu item : copy
325  setMenuItem( new Action(){
326  public String getText(){
327  return "copy";
328  }
329  public void run(){
330  GrxDebugUtil.println("GrxModelItem.GrxModelItem copy Action");
331  manager_.setSelectedGrxBaseItemList();
332  }
333  });
334 
335  // menu item : paste
336  setMenuItem(new Action(){
337  public String getText(){
338  return "paste";
339  }
340 
341  public void run(){
342  }
343  });
344  */
345  }
346 
350  private boolean _saveAs(){
351  FileDialog fdlg = new FileDialog( GrxUIPerspectiveFactory.getCurrentShell(), SWT.SAVE);
352  fdlg.setFilterPath(getDefaultDir().getAbsolutePath());
353  String fPath = fdlg.open();
354  if( fPath != null ) {
355  fPath = fPath.replace('\\','/');
356  if (GrxVrmlExporter.export(GrxModelItem.this, fPath)){
357  setURL(fPath);
358  setDefaultDirectory(new File(fPath).getParent());
359  }
360  return true;
361  }else
362  return false;
363  }
368  public boolean create() {
369  return true;
370  }
371 
376  if (links_.size() > 0){
377  return links_.get(0);
378  }else{
379  return null;
380  }
381  }
382 
386  public void restoreProperties() {
387  //super.restoreProperties(); プロパティNumOfAABBは、毎回propertyChanged実行すると遅くなるので、ModelItemだけここで実装する //
388  boolean flg=false;
389  if (element_ != null) {
390  NodeList props = element_.getElementsByTagName(PROPERTY_TAG);
391  for (int j = 0; j < props.getLength(); j++) {
392  Element propEl = (Element) props.item(j);
393  String key = propEl.getAttribute("name"); //$NON-NLS-1$
394  String val = propEl.getAttribute("value"); //$NON-NLS-1$
395  if(key.contains("NumOfAABB")){
396  setProperty(key, val);
397  String[] linkName = key.split("\\.");
398  nameToLink_.get(linkName[0]).setProperty("NumOfAABB", val);
399  flg=true;
400  }else{
401  if (!propertyChanged(key, val)){
402  setProperty(key, val);
403  }
404  }
405  }
406  }
408  if (getStr("markRadius")==null) setDbl("markRadius", DEFAULT_RADIUS); //$NON-NLS-1$ //$NON-NLS-2$
409 
410  _setModelType(isTrue("isRobot", isRobot_)); //$NON-NLS-1$
411 
412  if (getDblAry(rootLink().getName()+".translation", null) == null && //$NON-NLS-1$
413  getDblAry(rootLink().getName()+".rotation", null) == null) //$NON-NLS-1$
414  updateInitialTransformRoot();
415 
416  for (int i=0; i<jointToLink_.length; i++) {
417  GrxLinkItem l = links_.get(jointToLink_[i]);
418  Double d = getDbl(l.getName()+".angle", null); //$NON-NLS-1$
419  if (d == null)
420  setDbl(l.getName()+".angle", 0.0); //$NON-NLS-1$
421  }
422 
423  if(flg)
424  makeAABBforSameUrlModels();
425 
426  for (int i=0; i<links_.size(); i++) {
427  GrxLinkItem l = links_.get(i);
428  if(!l.jointType_.equals("fixed")){
429  String s = this.getProperty(l.getName()+".mode", null); //$NON-NLS-1$
430  if (s == null)
431  setProperty(l.getName()+".mode", "Torque"); //$NON-NLS-1$
432  }
433  }
434  }
435 
442  public boolean propertyChanged(String property, String value) {
443  if (super.propertyChanged(property, value)){
444  }else if(property.equals("isRobot")){ //$NON-NLS-1$
445  _setModelType(value);
446  }else if(property.equals("controlTime")){ //$NON-NLS-1$
447  try{
448  double t = Double.parseDouble(value);
449  if (t > 0){
450  setProperty("controlTime", value); //$NON-NLS-1$
451  }
452  }catch(Exception ex){
453  }
454  }else if(property.equals(rootLink().getName()+".translation")){ //$NON-NLS-1$
455  if (rootLink().localTranslation(value)){
456  setProperty(rootLink().getName()+".translation", value); //$NON-NLS-1$
457  calcForwardKinematics();
458  }
459  }else if(property.equals(rootLink().getName()+".rotation")){ //$NON-NLS-1$
460  if (rootLink().localRotation(value)){
461  setProperty(rootLink().getName()+".rotation", value); //$NON-NLS-1$
462  calcForwardKinematics();
463  }
464  }else if(property.equals(rootLink().getName()+".velocity")){ //$NON-NLS-1$
465  setProperty(rootLink().getName()+".velocity", value); //$NON-NLS-1$
466  rootLink().setProperty("velocity", value);
467  }else if(property.equals(rootLink().getName()+".angularVelocity")){ //$NON-NLS-1$
468  setProperty(rootLink().getName()+".angularVelocity", value); //$NON-NLS-1$
469  rootLink().setProperty("angulaerVelocity", value);
470  }else{
471  for (int j = 0; j < links_.size(); j++){
472  GrxLinkItem link = links_.get(j);
473  if (property.equals(link.getName()+".angle")){ //$NON-NLS-1$
474  if (link.jointValue(value)){
475  calcForwardKinematics();
476  setProperty(link.getName()+".angle", value); //$NON-NLS-1$
477  }
478  return true;
479  }else if(property.equals(link.getName()+".jointVelocity")){ //$NON-NLS-1$
480  setProperty(link.getName()+".jointVelocity", value); //$NON-NLS-1$
481  link.setProperty("jointVelocity", value);
482  return true;
483  }else if(property.equals(link.getName()+".NumOfAABB")){ //$NON-NLS-1$
484  if(link.propertyChanged("NumOfAABB", value)){
485  setProperty(link.getName()+".NumOfAABB", value); //$NON-NLS-1$
486  }
487  return true;
488  }else if(property.equals(link.getName()+".mode")){ //$NON-NLS-1$
489  setProperty(link.getName()+".mode", value); //$NON-NLS-1$
490  link.setProperty("mode", value);
491  return true;
492  }
493  }
494  return false;
495  }
496  return true;
497 
498  }
499 
504  private void _setModelType(String value){
505  Boolean b = Boolean.parseBoolean(value);
506  if (b != null){
507  _setModelType(b);
508  }
509  }
510 
515  private void _setModelType(boolean isRobot) {
516  isRobot_ = isRobot;
517  if (isRobot_) {
518  setIcon(robotIcon);
519  menuChangeType_.setText(MessageBundle.get("GrxModelItem.menu.changeEnv")); //$NON-NLS-1$
520  } else {
521  menuChangeType_.setText( MessageBundle.get("GrxModelItem.menu.changeRobot") ); //$NON-NLS-1$
522  setIcon(envIcon);
523  setVisibleCoM(false);
524  setVisibleCoMonFloor(false);
525  }
526  setProperty("isRobot", String.valueOf(isRobot_)); //$NON-NLS-1$
527  }
528 
536  private void _setupMarks() {
537  double radius = getDbl("markRadius", DEFAULT_RADIUS); //$NON-NLS-1$
538  switchCom_ = GrxShapeUtil.createBall(radius, new Color3f(1.0f, 1.0f, 0.0f));
539  switchComZ0_= GrxShapeUtil.createBall(radius, new Color3f(0.0f, 1.0f, 0.0f));
540  tgCom_ = (TransformGroup)switchCom_.getChild(0);
541  tgComZ0_ = (TransformGroup)switchComZ0_.getChild(0);
542  TransformGroup root = getTransformGroupRoot();
543  root.addChild(switchCom_);
544  root.addChild(switchComZ0_);
545 
547  modifier.init_ = true;
549 
550  Color3f color = new Color3f(0.0f, 1.0f, 0.0f);
551  switchBb_ = SceneGraphModifier._makeSwitchNode(modifier._makeBoundingBox(color));
552  root.addChild(switchBb_);
553 
554  }
555 
561  public boolean load(File f) {
562  boolean ret = load0(f);
563  if(ret){
564  List<GrxModelItem> sameModels = getSameUrlModels();
565  if(!sameModels.isEmpty()){
566  GrxModelItem model = sameModels.get(0);
567  boolean flg=false;
568  for(int i=0; i<model.links_.size(); i++){
569  if(!model.links_.get(i).getStr("NumOfAABB").equals("original data")){
570  links_.get(i).setInt("NumOfAABB", model.links_.get(i).getInt("NumOfAABB",1));
571  setInt(links_.get(i).getName()+".NumOfAABB", model.links_.get(i).getInt("NumOfAABB",1));
572  flg = true;
573  }
574  }
575  if(flg){
576  BodyInfo bodyInfo = model.getBodyInfoFromModelLoader();
577  makeAABB(bodyInfo);
578  }
579  }
580  return true;
581  }else
582  return false;
583 
584  }
585 
586  private boolean load0(File f) {
587  long load_stime = System.currentTimeMillis();
588  file_ = f;
589  String url=null;
590  try {
591  url = f.getCanonicalPath();
592  } catch (IOException e) {
593  e.printStackTrace();
594  }
595  GrxDebugUtil.println("Loading " + url); //$NON-NLS-1$
596  try {
597  ModelLoader mloader = ModelLoaderHelper.narrow(
598  GrxCorbaUtil.getReference("ModelLoader")); //$NON-NLS-1$
599  mloader._non_existent();
600  setURL(url);
601  bInfo_ = mloader.loadBodyInfo(getURL(true));
602  boolean ret = registerCharacter();
603  long load_etime = System.currentTimeMillis();
604  System.out.println("load time = " + (load_etime-load_stime) + "ms"); //$NON-NLS-1$ //$NON-NLS-2$
605  return ret;
606  }catch(ModelLoaderException me){
607  MessageDialog.openError(GrxUIPerspectiveFactory.getCurrentShell(),
608  MessageBundle.get("GrxModelItem.dialog.title.error"), //$NON-NLS-1$
609  MessageBundle.get("GrxModelItem.dialog.message.loadError") +"\n" + //$NON-NLS-1$ //$NON-NLS-2$
610  url + "\n\n" + me.description); //$NON-NLS-1$
611  System.out.println("Failed to load vrml model:" + url); //$NON-NLS-1$
612  me.printStackTrace();
613  return false;
614  } catch (Exception ex) {
615  MessageDialog.openError(GrxUIPerspectiveFactory.getCurrentShell(),
616  MessageBundle.get("GrxModelItem.dialog.title.error"), //$NON-NLS-1$
617  MessageBundle.get("GrxModelItem.dialog.message.NoModelLoader") );
618  System.out.println("Failed to load vrml model:" + url); //$NON-NLS-1$
619  ex.printStackTrace();
620  return false;
621  }
622  }
623 
624  public boolean registerCharacter(BodyInfo bInfo){
625  bInfo_ = bInfo;
626  return registerCharacter();
627  }
628 
629  private boolean registerCharacter(){
630  manager_.focusedItem(null);
631  manager_.setSelectedItem(this, false);
632  bgRoot_.detach();
633  bgRoot_ = new BranchGroup();
634  bgRoot_.setCapability(BranchGroup.ALLOW_DETACH);
635  bgRoot_.setCapability(BranchGroup.ALLOW_CHILDREN_READ);
636  bgRoot_.setCapability(BranchGroup.ALLOW_CHILDREN_WRITE);
637  bgRoot_.setCapability(BranchGroup.ALLOW_CHILDREN_EXTEND);
638 
639  LinkInfo[] linkInfoList = bInfo_.links();
640 
641  // delete existing model data
642  if (rootLink() != null){
643  rootLink().delete();
644  }
645 
646  for (int i=0; i<cameraList_.size(); i++){
647  cameraList_.get(i).destroy();
648  }
649  cameraList_.clear();
650 
651  int jointCount = 0;
652  nameToLink_.clear();
653 
654  int massZeroLink = -1;
655  for (int i = 0; i < linkInfoList.length; i++) {
656  GrxLinkItem link = new GrxLinkItem(linkInfoList[i].name, manager_, this, linkInfoList[i]);
657  manager_.itemChange(link, GrxPluginManager.ADD_ITEM);
658  if (link.jointId_ >= 0){
659  jointCount++;
660  }
661  nameToLink_.put(link.getName(), link);
662  if(linkInfoList[i].mass <= 0.0)
663  massZeroLink = i;
664  }
665  if(massZeroLink >= 0)
666  MessageDialog.openWarning(null, getName(), linkInfoList[massZeroLink].name+"::"+
667  MessageBundle.get("GrxModelItem.dialog.message.massZero"));
668  System.out.println("links_.size() = "+links_.size()); //$NON-NLS-1$
669 
670  // Search root node.
671  int rootIndex = -1;
672  for( int i = 0 ; i < links_.size() ; i++ ) {
673  if( links_.get(i).parentIndex_ < 0 ){
674  if( rootIndex < 0 ) {
675  rootIndex = i;
676  } else {
677  System.out.println( "Error. Two or more root node exist." ); //$NON-NLS-1$
678  }
679  }
680  }
681  if( rootIndex < 0 ){
682  System.out.println( "Error, root node doesn't exist." ); //$NON-NLS-1$
683  }
684 
685  createLink(rootIndex);
686 
687  jointToLink_ = new int[jointCount];
688  for (int i=0; i<jointCount; i++) {
689  for (int j=0; j<links_.size(); j++) {
690  if (links_.get(j).jointId_ == i) {
691  jointToLink_[i] = j;
692  }
693  }
694  }
695 
696  ExtraJointInfo[] extraJointList = bInfo_.extraJoints();
697  extraJoints_.clear();
698  for (int i = 0; i < extraJointList.length; i++) {
699  GrxExtraJointItem extraJoint = new GrxExtraJointItem(extraJointList[i].name, manager_, this, extraJointList[i]);
700  extraJoints_.add(extraJoint);
701  manager_.itemChange(extraJoint, GrxPluginManager.ADD_ITEM);
702  }
703 
704  long stime = System.currentTimeMillis();
705  try {
706  _loadVrmlScene(linkInfoList);
707  } catch (BadLinkStructureException e) {
708  e.printStackTrace();
709  return false;
710  }
711  long etime = System.currentTimeMillis();
712  System.out.println("_loadVrmlScene time = " + (etime-stime) + "ms"); //$NON-NLS-1$ //$NON-NLS-2$
713 
714  calcForwardKinematics();
715  updateInitialTransformRoot();
716  updateInitialJointValues();
717 
718  _setupMarks();
719  setProperty("isRobot", Boolean.toString(isRobot_)); //$NON-NLS-1$
720  for (int i=0; i<links_.size(); i++) {
721  GrxLinkItem l = links_.get(i);
722  if(!l.jointType_.equals("fixed")){
723  setProperty(l.getName()+".mode", "Torque"); //$NON-NLS-1$
724  l.setProperty("mode", "Torque");
725  }
726  }
727 
728  manager_.setSelectedItem(this, true);
729  cancelModified();
730  return true;
731  }
732 
737  private void createLink( int index ){
738 
739  GrxLinkItem link = links_.get(index);
740 
741  // register this to children field of parent link
742  if (link.parentIndex_ != -1){
743  links_.get(link.parentIndex_).addLink(link);
744  }else{
745  bgRoot_.addChild(link.bg_);
746  }
747 
748  // gather cameras
749  for (int i=0; i< link.children_.size(); i++){
750  if (link.children_.get(i) instanceof GrxSensorItem){
751  GrxSensorItem sensor = (GrxSensorItem)link.children_.get(i);
752  if (sensor.isCamera()){
753  cameraList_.add(sensor.getCamera());
754  }
755  }
756  }
757 
758  for( int i = 0 ; i < link.childIndices_.length ; i++ )
759  {
760  // call recursively
761  int childIndex = link.childIndices_[i];
762  createLink( childIndex );
763  }
764  }
765 
771  private void _loadVrmlScene(LinkInfo[] links) throws BadLinkStructureException {
772  shapes = bInfo_.shapes();
773  appearances = bInfo_.appearances();
774  materials = bInfo_.materials();
775  textures = bInfo_.textures();
776 
777  int numLinks = links.length;
778  for(int linkIndex = 0; linkIndex < numLinks; linkIndex++) {
779  GrxLinkItem link = links_.get(linkIndex);
780  for (int i=0; i<link.children_.size(); i++){
781  if(link.children_.get(i) instanceof GrxSegmentItem){
782  GrxSegmentItem segment = (GrxSegmentItem)link.children_.get(i);
783  segment.addShape(segment.getTransform());
784  }else if (link.children_.get(i) instanceof GrxSensorItem){
785  GrxSensorItem sensor = (GrxSensorItem)link.children_.get(i);
786  sensor.addShape(new Matrix4d(1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1));
787  }else if (link.children_.get(i) instanceof GrxHwcItem){
788  GrxHwcItem hwc = (GrxHwcItem)link.children_.get(i);
789  hwc.addShape(new Matrix4d(1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1));
790  }
791  }
792  }
793  setDblAry(rootLink().getName()+".translation", rootLink().localTranslation()); //$NON-NLS-1$
794  setDblAry(rootLink().getName()+".rotation", rootLink().localRotation()); //$NON-NLS-1$
795  setDblAry(rootLink().getName()+".velocity", new double[]{0,0,0} ); //$NON-NLS-1$
796  setDblAry(rootLink().getName()+".angularVelocity", new double[]{0,0,0} ); //$NON-NLS-1$
797 
798  for (int i=0; i<links_.size(); i++) {
799  Node n = links_.get(i).tg_.getChild(0);
800  if (n.getCapability(Node.ENABLE_PICK_REPORTING))
801  n.clearCapability(Node.ENABLE_PICK_REPORTING);
802  }
803 
804  }
805 
810  public void addRobot(File f, GrxLinkItem parentLink){
811  try {
812  GrxDebugUtil.println("Loading " + f.getCanonicalPath());
813  ModelLoader mloader = ModelLoaderHelper.narrow(
814  GrxCorbaUtil.getReference("ModelLoader")); //$NON-NLS-1$
815  mloader._non_existent();
816  BodyInfo bInfo = mloader.loadBodyInfo(f.getCanonicalPath());
817  LinkInfo[] linkInfoList = bInfo.links();
818  int numOfLink = links_.size();
819  int numOfJoint = getDOF();
820  Vector<String> sensorType = new Vector<String>();
821  Vector<String>[] sensorNames = new Vector[GrxSensorItem.sensorType.length];
822  for(int i=0; i<GrxSensorItem.sensorType.length; i++){
823  sensorType.add(GrxSensorItem.sensorType[i]);
824  sensorNames[i] = new Vector<String>();
825  String[] names = getSensorNames(GrxSensorItem.sensorType[i]);
826  if(names!=null)
827  for(int j=0; j<names.length; j++)
828  sensorNames[i].add(names[j]);
829  }
830  int rootIndex = -1;
831  int jointCount = 0;
832  for (int i = 0; i < linkInfoList.length; i++) {
833  if(linkInfoList[i].parentIndex < 0 ){
834  linkInfoList[i].parentIndex = (short) links_.indexOf(parentLink);
835  rootIndex = i+numOfLink;
836  }else
837  linkInfoList[i].parentIndex += numOfLink;
838  for(int j=0; j<linkInfoList[i].childIndices.length; j++)
839  linkInfoList[i].childIndices[j] += numOfLink;
840  if(linkInfoList[i].jointId >= 0){
841  linkInfoList[i].jointId += numOfJoint;
842  jointCount++;
843  }
844  SensorInfo[] sensors = linkInfoList[i].sensors;
845  for(int j=0; j<sensors.length; j++){
846  int type = sensorType.indexOf(sensors[j].type);
847  sensors[j].id += sensorNames[type].size();
848  String sensorName = sensors[j].name;
849  for(int k=0; sensorNames[type].indexOf(sensorName)!=-1; k++)
850  sensorName = sensors[j].name + "_" + k;
851  sensors[j].name = sensorName;
852  }
853  String linkName = linkInfoList[i].name;
854  for(int j=0; nameToLink_.get(linkName)!=null; j++ )
855  linkName = linkInfoList[i].name + "_" + j;
856  GrxLinkItem link = new GrxLinkItem(linkName, manager_, this, linkInfoList[i]);
857  nameToLink_.put(linkName, link);
858  }
859  createLink(rootIndex);
860  jointToLink_ = new int[numOfJoint+jointCount];
861  for (int i=0; i<numOfJoint+jointCount; i++) {
862  for (int j=0; j<links_.size(); j++) {
863  if (links_.get(j).jointId_ == i) {
864  jointToLink_[i] = j;
865  }
866  }
867  }
868  for(int i=numOfLink; i<numOfLink+linkInfoList.length; i++)
869  manager_.itemChange(links_.get(i), GrxPluginManager.ADD_ITEM);
870 
871  shapes = bInfo.shapes();
872  appearances = bInfo.appearances();
873  materials = bInfo.materials();
874  textures = bInfo.textures();
875 
876  for(int linkIndex = numOfLink; linkIndex < links_.size(); linkIndex++) {
877  GrxLinkItem link = links_.get(linkIndex);
878  for (int i=0; i<link.children_.size(); i++){
879  if(link.children_.get(i) instanceof GrxSegmentItem){
880  GrxSegmentItem segment = (GrxSegmentItem)link.children_.get(i);
881  segment.addShape(segment.getTransform());
882  }else if (link.children_.get(i) instanceof GrxSensorItem){
883  GrxSensorItem sensor = (GrxSensorItem)link.children_.get(i);
884  sensor.addShape(new Matrix4d(1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1));
885  }else if (link.children_.get(i) instanceof GrxHwcItem){
886  GrxHwcItem hwc = (GrxHwcItem)link.children_.get(i);
887  hwc.addShape(new Matrix4d(1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1));
888  }
889  }
890  }
891  }catch(Exception ex){
892  MessageDialog.openError(GrxUIPerspectiveFactory.getCurrentShell(),
893  MessageBundle.get("GrxModelItem.dialog.title.error"), //$NON-NLS-1$
894  MessageBundle.get("GrxModelItem.dialog.message.NoModelLoader") );
895  }
896  }
897 
901  public class NormalRender {
902  private LineArray nline = null;
903 
904  public NormalRender(GeometryArray geom, float scale, Matrix4d T) {
905 
906  Point3f[] vertices = new Point3f[geom.getVertexCount()];
907  Vector3f[] normals = new Vector3f[geom.getVertexCount()];
908  for (int i=0; i<geom.getVertexCount(); i++) {
909  vertices[i] = new Point3f();
910  normals[i] = new Vector3f();
911  }
912  geom.getCoordinates(0, vertices);
913  geom.getNormals(0, normals);
914  Point3f[] nvertices = new Point3f[vertices.length * 2];
915  int n = 0;
916  for (int i=0; i<vertices.length; i++ ){
917 
918  T.transform(normals[i]);
919  normals[i].normalize();
920  T.transform(vertices[i]);
921 
922  nvertices[n++] = new Point3f( vertices[i] );
923  nvertices[n++] = new Point3f( vertices[i].x + scale * normals[i].x,
924  vertices[i].y + scale * normals[i].y,
925  vertices[i].z + scale * normals[i].z );
926  }
927  nline = new LineArray(nvertices.length, GeometryArray.COORDINATES);
928  nline.setCoordinates(0, nvertices);
929  }
930 
931  public LineArray getLineArray() { return nline; }
932  }
933 
934 
940  public void setCharacterPos(LinkPosition[] lpos, double[] q) {
941  boolean isAllPosProvided = true;
942  if (q != null) {
943  for (int i=0; i<jointToLink_.length; i++)
944  links_.get(jointToLink_[i]).jointValue(q[i]);
945  }
946  for (int i=0; i<links_.size(); i++) {
947  if (lpos[i].p == null || lpos[i].R == null)
948  isAllPosProvided = false;
949  else{
950  if(i==0){
951  links_.get(i).localTranslation(lpos[0].p);
952  links_.get(i).localRotation(lpos[0].R);
953  }
954  links_.get(i).absTransform(lpos[i].p, lpos[i].R);
955  }
956  }
957  if (isAllPosProvided)
958  updateCoM();
959  else
960  calcForwardKinematics();
961  }
962 
968  public void setTransformRoot(Vector3d pos, Matrix3d rot) {
969  _setTransform(0, pos, rot);
970  }
971 
977  public void setTransformRoot(Transform3D tform) {
978  Vector3d pos = new Vector3d();
979  Matrix3d rot = new Matrix3d();
980  tform.get(rot, pos);
981  _setTransform(0, pos, rot);
982  }
983 
990  private void _setTransform(int linkId, Vector3d pos, Matrix3d rot) {
991  GrxLinkItem link = links_.get(linkId);
992  if (link != null )
993  if(link.parent_ == null ) link.setTransform(pos, rot);
994  }
995 
1000  public void setJointValues(final double[] values) {
1001  for (int j = 0; j < links_.size(); j++){
1002  GrxLinkItem link = links_.get(j);
1003  if (link.jointId_ >= 0 && link.jointId_ < values.length){
1004  link.jointValue(values[link.jointId_]);
1005  }
1006  }
1007  }
1008 
1013  rootLink().setJointValuesWithinLimit();
1014  }
1015 
1020  Transform3D t3d = new Transform3D();
1021  Matrix3d m3d = new Matrix3d();
1022  Vector3d v3d = new Vector3d();
1023 
1024  getTransformGroupRoot().getTransform(t3d);
1025  t3d.get(m3d, v3d);
1026  setDblAry(rootLink().getName()+".translation", new double[]{v3d.x, v3d.y, v3d.z}); //$NON-NLS-1$
1027 
1028  AxisAngle4d a4d = new AxisAngle4d();
1029  a4d.setMatrix(m3d);
1030  setDblAry(rootLink().getName()+".rotation", new double[]{a4d.x, a4d.y, a4d.z, a4d.angle}); //$NON-NLS-1$
1031  }
1032 
1038  if (link != null)
1039  setDbl(link.getName()+".angle", link.jointValue_); //$NON-NLS-1$
1040  }
1041 
1046  for (int i=0; i<jointToLink_.length; i++) {
1047  GrxLinkItem l = links_.get(jointToLink_[i]);
1048  setDbl(l.getName()+".angle", l.jointValue_); //$NON-NLS-1$
1049  }
1050  }
1051 
1055  public void calcForwardKinematics() {
1056  rootLink().calcForwardKinematics();
1057  updateCoM();
1058  }
1059 
1063  public void updateCoM() {
1064  if (switchCom_.getWhichChild() == Switch.CHILD_ALL ||
1065  switchComZ0_.getWhichChild() == Switch.CHILD_ALL) {
1066  Vector3d v3d = new Vector3d();
1067  getCoM(v3d);
1068  Vector3d vz0 = new Vector3d(v3d);
1069 
1070  _globalToRoot(v3d);
1071  Transform3D t3d = new Transform3D();
1072  t3d.set(v3d);
1073  tgCom_.setTransform(t3d);
1074 
1075  vz0.z = 0.0;
1076  _globalToRoot(vz0);
1077  t3d.set(vz0);
1078  tgComZ0_.setTransform(t3d);
1079  }
1080  }
1081 
1086  private void _globalToRoot(Vector3d pos) {
1087  Transform3D t3d = new Transform3D();
1088  getTransformGroupRoot().getTransform(t3d);
1089  Vector3d p = new Vector3d();
1090  t3d.get(p);
1091  t3d.invert();
1092  pos.sub(p);
1093  t3d.transform(pos);
1094  }
1095 
1100  public void getCoM(Vector3d pos) {
1101  pos.x = 0.0;
1102  pos.y = 0.0;
1103  pos.z = 0.0;
1104  double totalMass = 0.0;
1105 
1106  for (int i = 0; i < links_.size(); i++) {
1107  GrxLinkItem link = links_.get(i);
1108  totalMass += link.linkMass_;
1109  Vector3d absCom = link.absCoM();
1110  absCom.scale(link.linkMass_);
1111  pos.add(absCom);
1112  }
1113  pos.scale(1.0 / totalMass);
1114  }
1115 
1116  public List<GrxSensorItem> getSensors(String type){
1117  List<GrxSensorItem> sensors = new ArrayList<GrxSensorItem>();
1118  rootLink().gatherSensors(type, sensors);
1119  if (sensors.size() > 0){
1120  return sensors;
1121  }else{
1122  return null;
1123  }
1124  }
1125 
1126  public String[] getSensorNames(String type) {
1127  List<GrxSensorItem> l = getSensors(type);
1128  if (l == null)
1129  return null;
1130 
1131  String[] ret = new String[l.size()];
1132  for (int i=0; i<ret.length; i++)
1133  ret[l.get(i).id_] = l.get(i).getName();
1134  return ret;
1135  }
1136 
1137  public TransformGroup getTransformGroupRoot() {
1138  return rootLink().tg_;
1139  }
1140 
1146  public double[] getTransformArray(GrxLinkItem link) {
1147  Transform3D t3d = new Transform3D();
1148  link.tg_.getTransform(t3d);
1149  Matrix3d mat = new Matrix3d();
1150  Vector3d vec = new Vector3d();
1151  t3d.get(mat, vec);
1152 
1153  double[] ret = new double[12];
1154  vec.get(ret);
1155  ret[3] = mat.m00; ret[4] = mat.m01; ret[5] = mat.m02;
1156  ret[6] = mat.m10; ret[7] = mat.m11; ret[8] = mat.m12;
1157  ret[9] = mat.m20; ret[10]= mat.m21; ret[11]= mat.m22;
1158 
1159  return ret;
1160  }
1161 
1162  public double[] getInitialTransformArray(GrxLinkItem link) {
1163  double[] ret = getTransformArray(link);
1164 
1165  double[] p = getDblAry(link.getName()+".translation", null); //$NON-NLS-1$
1166  if (p != null && p.length == 3) {
1167  System.arraycopy(p, 0, ret, 0, 3);
1168  }
1169 
1170  double[] r = getDblAry(link.getName()+".rotation", null); //$NON-NLS-1$
1171  if (r != null && r.length == 4) {
1172  Matrix3d mat = new Matrix3d();
1173  mat.set(new AxisAngle4d(r));
1174  ret[3] = mat.m00; ret[4] = mat.m01; ret[5] = mat.m02;
1175  ret[6] = mat.m10; ret[7] = mat.m11; ret[8] = mat.m12;
1176  ret[9] = mat.m20; ret[10]= mat.m21; ret[11]= mat.m22;
1177  }
1178 
1179  return ret;
1180  }
1181 
1182  public double[] getInitialVelocity(GrxLinkItem link){
1183  double[] ret = {0,0,0,0,0,0};
1184  double[] v = getDblAry(link.getName()+".velocity", null);
1185  if (v != null && v.length == 3) {
1186  System.arraycopy(v, 0, ret, 0, 3);
1187  }
1188  double[] w = getDblAry(link.getName()+".angularVelocity", null);
1189  if (w != null && w.length == 3) {
1190  System.arraycopy(w, 0, ret, 3, 3);
1191  }
1192 
1193  return ret;
1194  }
1195 
1196  public int getDOF() {
1197  if (jointToLink_ == null)
1198  return 0;
1199  return jointToLink_.length;
1200  }
1201 
1202  public String[] getJointNames() {
1203  String[] names = new String[jointToLink_.length];
1204  for (int i=0; i<jointToLink_.length; i++)
1205  names[i] = links_.get(jointToLink_[i]).getName();
1206  return names;
1207  }
1208 
1209  public double[] getJointValues() {
1210  double[] vals = new double[jointToLink_.length];
1211  for (int i=0; i<jointToLink_.length; i++)
1212  vals[i] = links_.get(jointToLink_[i]).jointValue_;
1213  return vals;
1214  }
1215 
1216  public double[] getInitialJointValues() {
1217  double[] ret = new double[jointToLink_.length];
1218  for (int i=0; i<ret.length; i++) {
1219  GrxLinkItem l = links_.get(jointToLink_[i]);
1220  String jname = l.getName();
1221  ret[i] = getDbl(jname+".angle", l.jointValue_); //$NON-NLS-1$
1222  }
1223  return ret;
1224  }
1225 
1226  public double[] getInitialJointVelocity(){
1227  double[] ret = new double[jointToLink_.length];
1228  for (int i=0; i<ret.length; i++) {
1229  GrxLinkItem l = links_.get(jointToLink_[i]);
1230  String jname = l.getName();
1231  ret[i] = getDbl(jname+".jointVelocity", 0.0); //$NON-NLS-1$
1232  }
1233  return ret;
1234  }
1235  /* public double[] getInitialJointMode() {
1236  double[] ret = new double[jointToLink_.length];
1237  for (int i=0; i<ret.length; i++) {
1238  String jname = links_.get(jointToLink_[i]).getName();
1239  String mode = getStr(jname+".mode", "Torque");
1240  ret[i] = mode.equals("HighGain") ? 1.0 : 0.0;
1241  }
1242  return ret;
1243  }*/
1244  public double[] getInitialJointMode() {
1245  double[] ret = new double[links_.size()];
1246  for (int i=0; i<ret.length; i++) {
1247  String lname = links_.get(i).getName();
1248  String mode = getStr(lname+".mode", "Torque"); //$NON-NLS-1$ //$NON-NLS-2$
1249  ret[i] = mode.equals("HighGain") ? 1.0 : 0.0; //$NON-NLS-1$
1250  }
1251  return ret;
1252  }
1253 
1254  public boolean isRobot() {
1255  return isRobot_;
1256  }
1257 
1258  public void setSelected(boolean b) {
1259  super.setSelected(b);
1260  /*
1261  if (!b) {
1262  bgRoot_.detach();
1263  for (int i=0; i<cameraList_.size(); i++)
1264  cameraList_.get(i).getBranchGroup().detach();
1265  }
1266  */
1267  }
1268 
1275  public void setFocused(boolean b){
1276  if (b) resizeBoundingBox();
1277  switchBb_.setWhichChild(b ? Switch.CHILD_ALL : Switch.CHILD_NONE);
1278  }
1279 
1283  public void delete() {
1284  super.delete();
1285  Iterator<Camera_impl> it = cameraList_.iterator();
1286  while(it.hasNext())
1287  it.next().destroy();
1288  if(bgRoot_.isLive()) bgRoot_.detach();
1289  Map<?, ?> m = manager_.pluginMap_.get((GrxCollisionPairItem.class));
1290  GrxCollisionPairItem[] collisionPairItems = m.values().toArray(new GrxCollisionPairItem[0]);
1291  for (int i=0; i<collisionPairItems.length; i++) {
1292  GrxCollisionPairItem item = (GrxCollisionPairItem) collisionPairItems[i];
1293  String name = getName();
1294  if(name.equals(item.getStr("objectName1", ""))){ //$NON-NLS-1$ //$NON-NLS-2$
1295  item.delete();
1296  }else if(name.equals(item.getStr("objectName2", ""))){ //$NON-NLS-1$ //$NON-NLS-2$
1297  item.delete();
1298  }
1299  }
1300  m = manager_.pluginMap_.get((GrxExtraJointItem.class));
1301  GrxExtraJointItem[] extraJointItems = m.values().toArray(new GrxExtraJointItem[0]);
1302  for (int i=0; i<extraJointItems.length; i++) {
1303  GrxExtraJointItem item = extraJointItems[i];
1304  String name = getName();
1305  if(name.equals(item.getStr("object1Name", ""))){ //$NON-NLS-1$ //$NON-NLS-2$
1306  item.delete();
1307  }else if(name.equals(item.getStr("object2Name", ""))){ //$NON-NLS-1$ //$NON-NLS-2$
1308  item.delete();
1309  }
1310  }
1311  GrxLinkItem root = rootLink();
1312  if(root != null)
1313  root.delete();
1314  }
1315 
1319  public void resizeBoundingBox() {
1321  modifier.init_ = true;
1323 
1324  // ノードのRootのTransformGroupを取得
1325  TransformGroup tg = getTransformGroupRoot();
1326  Transform3D t3dLocal = new Transform3D();
1327  tg.getTransform(t3dLocal);
1328  t3dLocal.invert();
1329  for (int i=0; i<links_.size(); i++) {
1330  modifier._calcUpperLower(links_.get(i).tg_, t3dLocal);
1331  }
1332  Shape3D shapeNode = (Shape3D)switchBb_.getChild(0);
1333  Geometry gm = (Geometry)shapeNode.getGeometry(0);
1334 
1335  Point3f[] p3fW = modifier._makePoints();
1336  if (gm instanceof QuadArray) {
1337  QuadArray qa = (QuadArray) gm;
1338  qa.setCoordinates(0, p3fW); // 座標
1339  }
1340  }
1341 
1346  public void setVisibleCoM(boolean b) {
1347  if (isRobot()) {
1348  switchCom_.setWhichChild(b? Switch.CHILD_ALL:Switch.CHILD_NONE);
1349  calcForwardKinematics();
1350  } else {
1351  switchCom_.setWhichChild(Switch.CHILD_NONE);
1352  }
1353  }
1354 
1359  public void setVisibleCoMonFloor(boolean b) {
1360  if (isRobot()) {
1361  switchComZ0_.setWhichChild(b? Switch.CHILD_ALL:Switch.CHILD_NONE);
1362  calcForwardKinematics();
1363  } else {
1364  switchComZ0_.setWhichChild(Switch.CHILD_NONE);
1365  }
1366  }
1367 
1372  public void setWireFrame(boolean b) {
1373  setWireFrame(b, bgRoot_);
1374  }
1375 
1381  public void setWireFrame(boolean b, Node node){
1382  if (node instanceof Switch) {
1383  return;
1384  } else if (node instanceof Group) {
1385  Group g = (Group) node;
1386  for (int i = 0; i < g.numChildren(); i++)
1387  setWireFrame(b, g.getChild(i));
1388 
1389  } else if (node instanceof Link) {
1390  Link l = (Link) node;
1391  SharedGroup sg = l.getSharedGroup();
1392  for (int i = 0; i < sg.numChildren(); i++)
1393  setWireFrame(b, sg.getChild(i));
1394 
1395  } else if (node instanceof Shape3D) {
1396  Shape3D s3d = (Shape3D) node;
1397  Appearance app = s3d.getAppearance();
1398  if (app != null) {
1399  PolygonAttributes pa = app.getPolygonAttributes();
1400  if (pa != null){
1401  if (b) {
1402  pa.setPolygonMode(PolygonAttributes.POLYGON_LINE);
1403  } else {
1404  pa.setPolygonMode(PolygonAttributes.POLYGON_FILL);
1405  }
1406  }
1407  }
1408  }
1409  }
1410 
1411  public void setVisibleAABB(boolean b){
1412  Iterator<GrxLinkItem> it = links_.iterator();
1413  while(it.hasNext())
1414  it.next().setVisibleAABB(b);
1415  }
1416 
1421  public void setTransparencyMode(boolean b) {
1422  setTransparencyMode(b, bgRoot_);
1423  }
1424 
1430  private void setTransparencyMode(boolean b, Node node) {
1431  if (node instanceof Switch) {
1432  return;
1433  } else if (node instanceof Group) {
1434  Group g = (Group) node;
1435  for (int i = 0; i < g.numChildren(); i++)
1436  setTransparencyMode(b, g.getChild(i));
1437 
1438  } else if (node instanceof Link) {
1439  Link l = (Link) node;
1440  SharedGroup sg = l.getSharedGroup();
1441  for (int i = 0; i < sg.numChildren(); i++)
1442  setTransparencyMode(b, sg.getChild(i));
1443 
1444  } else if (node instanceof Shape3D) {
1445  Shape3D s3d = (Shape3D) node;
1446  Appearance app = s3d.getAppearance();
1447  if (app != null) {
1448  TransparencyAttributes ta = app.getTransparencyAttributes();
1449  if (ta != null) {
1450  if (b) {
1451  ta.setTransparency(0.5f);
1452  ta.setTransparencyMode(TransparencyAttributes.FASTEST);
1453  } else {
1454  ta.setTransparency(0f);
1455  ta.setTransparencyMode(TransparencyAttributes.NONE);
1456  }
1457  }
1458  }
1459  }
1460  }
1461 
1466  public List<Camera_impl> getCameraSequence () {
1467  return cameraList_;
1468  }
1469 
1475  public void setJointColor(int jid, java.awt.Color color) {
1476  GrxLinkItem l = links_.get(jointToLink_[jid]);
1477  if (l != null) l.setColor(color);
1478  }
1479 
1480 
1481  /* this method is disabled to hide paste menu
1482  public void paste(String clipVal){
1483 
1484  Clipboard clp = Toolkit.getDefaultToolkit().getSystemClipboard();
1485  Transferable data = clp.getContents(null);
1486 
1487  String strClip = "";
1488 
1489  if (data == null || !data.isDataFlavorSupported(DataFlavor.stringFlavor)){
1490  strClip = "転送に失敗しました";
1491  } else {
1492  try {
1493  strClip = (String)data.getTransferData( DataFlavor.stringFlavor );
1494  } catch(Exception e) {
1495  GrxDebugUtil.printErr("GrxModelItem.paste: " , e);
1496  }
1497  }
1498  }
1499  */
1500 
1506  GrxModelItem ret = (GrxModelItem)super.clone();
1507  ret.bInfo_ = (BodyInfo)bInfo_._duplicate();
1508  //ret.bgRoot_ = (BodyInfo)bgRoot_.cloneTree();
1509 
1510  ret.links_ = new Vector<GrxLinkItem>(links_);
1511  //ret.activeLink_ = activeLink_.clone();
1512 /*
1513  Deep copy suspension list
1514 
1515  public BodyInfo bInfo_;
1516 
1517  public BranchGroup bgRoot_ = new BranchGroup();
1518  public Vector<GrxLinkItem> links_ = new Vector<GrxLinkItem>();
1519  public GrxLinkItem activeLink_;
1520  private int[] jointToLink_; // length = joint number
1521  private final Map<String, GrxLinkItem> linkMap_ = new HashMap<String, GrxLinkItem>();
1522  private final Vector<Shape3D> shapeVector_ = new Vector<Shape3D>();
1523  // sensor type name -> list of sensors
1524  private final Map<String, List<GrxSensorItem>> sensorMap_ = new HashMap<String, List<GrxSensorItem>>();
1525  // list of cameras
1526  private List<Camera_impl> cameraList_ = new ArrayList<Camera_impl>();
1527 
1528  private Switch switchCom_;
1529  private TransformGroup tgCom_;
1530  private Switch switchComZ0_;
1531  private TransformGroup tgComZ0_;
1532 
1533  // temporary variables for computation
1534  private Transform3D t3d_ = new Transform3D();
1535  private Vector3d v3d_ = new Vector3d();
1536 */
1537 
1538  return ret;
1539  }
1540 
1546  public GrxLinkItem getLink(String name){
1547  return nameToLink_.get(name);
1548  }
1549 
1550  private BodyInfo getBodyInfoFromModelLoader(){
1551  short[] depth = new short[links_.size()];
1552  for(int i=0; i<links_.size(); i++)
1553  depth[i] = links_.get(i).getInt("NumOfAABB", 1).shortValue();
1554  try {
1555  ModelLoader mloader = ModelLoaderHelper.narrow(GrxCorbaUtil.getReference("ModelLoader")); //$NON-NLS-1$
1556  mloader._non_existent();
1557  ModelLoadOption option = new ModelLoadOption();
1558  option.readImage = false;
1559  option.AABBtype = AABBdataType.AABB_NUM;
1560  option.AABBdata = depth;
1561 
1562  return mloader.getBodyInfoEx(getURL(false), option);
1563  }catch(Exception e){
1564  MessageDialog.openError(GrxUIPerspectiveFactory.getCurrentShell(),
1565  MessageBundle.get("GrxModelItem.dialog.title.error"), //$NON-NLS-1$
1566  MessageBundle.get("GrxModelItem.dialog.message.NoModelLoader") );
1567  return null;
1568  }
1569  }
1570 
1571  private void makeAABB(BodyInfo binfo){
1572  bInfo_ = binfo;
1573 
1574  LinkInfo[] links = bInfo_.links();
1575  shapes = bInfo_.shapes();
1576  appearances = bInfo_.appearances();
1577  materials = bInfo_.materials();
1578  textures = bInfo_.textures();
1579 
1580  int numLinks = links.length;
1581  for(int i = 0; i < numLinks; i++) {
1582  links_.get(i).clearAABB();
1583  TransformedShapeIndex[] tsi = links[i].shapeIndices;
1584  for(int j=0; j<tsi.length; j++){
1585  links_.get(i).makeAABB(shapes[tsi[j].shapeIndex], tsi[j].transformMatrix );
1586  }
1587  }
1588  notifyObservers("BodyInfoChange");
1589  }
1590 
1591  public List<GrxModelItem> getSameUrlModels(){
1592  List<GrxModelItem> models = manager_.<GrxModelItem>getSelectedItemList(GrxModelItem.class);
1593  Iterator<GrxModelItem> it = models.iterator();
1594  while(it.hasNext()){
1595  GrxModelItem model = it.next();
1596  if(!model.getURL(true).equals(getURL(true)) || model == this)
1597  it.remove();
1598  }
1599  return models;
1600  }
1601 
1603  BodyInfo bodyInfo = getBodyInfoFromModelLoader();
1604  makeAABB(bodyInfo);
1605  List<GrxModelItem> sameModels = getSameUrlModels();
1606  Iterator<GrxModelItem> it = sameModels.iterator();
1607  while(it.hasNext()){
1608  GrxModelItem _model = it.next();
1609  for(int i=0; i<links_.size(); i++){
1610  String value = links_.get(i).getStr("NumOfAABB");
1611  _model.nameToLink_.get(links_.get(i).getName()).setProperty("NumOfAABB", value);
1612  _model.setProperty(links_.get(i).getName()+".NumOfAABB", value);
1613  }
1614  _model.makeAABB(bodyInfo);
1615  }
1616  }
1617 
1622  public void rename(String newName) {
1623  String oldName = getName();
1624 
1625  super.rename(newName);
1626 
1627  OrderedHashMap mcoll = manager_.pluginMap_.get(GrxCollisionPairItem.class);
1628  if(mcoll != null)
1629  {
1630  Iterator it = mcoll.values().iterator();
1631  while(it.hasNext())
1632  {
1634  if(oldName.equals(ci.getProperty("objectName1")))
1635  ci.setProperty("objectName1", newName);
1636 
1637  if(oldName.equals(ci.getProperty("objectName2")))
1638  ci.setProperty("objectName2", newName);
1639  }
1640  }
1641  mcoll = manager_.pluginMap_.get(GrxExtraJointItem.class);
1642  if(mcoll != null)
1643  {
1644  Iterator it = mcoll.values().iterator();
1645  while(it.hasNext())
1646  {
1647  GrxExtraJointItem extraJoint = (GrxExtraJointItem) it.next();
1648  if(oldName.equals(extraJoint.getProperty("object1Name")))
1649  extraJoint.setProperty("object1Name", newName);
1650 
1651  if(oldName.equals(extraJoint.getProperty("object2Name")))
1652  extraJoint.setProperty("object2Name", newName);
1653  }
1654  }
1655  }
1656 
1657  @Override
1658  public ValueEditType GetValueEditType(String key) {
1659  if(key.matches(".+\\.mode"))
1660  {
1661  return new ValueEditCombo(modeComboItem_);
1662  }else if(key.equals("isRobot")){
1663  return new ValueEditCombo(booleanComboItem_);
1664  }
1665  return super.GetValueEditType(key);
1666  }
1667 
1668  public void removeExtraJoint(GrxExtraJointItem extraJoint) {
1669  extraJoints_.remove(extraJoint);
1670  notifyModified();
1671  }
1672 
1673  private void addExtraJoint(String name) {
1674  GrxExtraJointItem extraJoint = new GrxExtraJointItem(name, manager_, this, null);
1675  this.extraJoints_.add(extraJoint);
1676  notifyModified();
1677  manager_.itemChange(extraJoint, GrxPluginManager.ADD_ITEM);
1678  }
1679 }
void _setupMarks()
create spheres to display CoM and projected CoM
void addLink(String name)
create and add a new link as a child
void setJointValuesWithinLimit()
modify joint value if it exceeds limit values
void setColor(java.awt.Color color)
boolean registerCharacter(BodyInfo bInfo)
static final String get(String key)
NormalRender(GeometryArray geom, float scale, Matrix4d T)
void _setTransform(int linkId, Vector3d pos, Matrix3d rot)
set transformation of linkId th joint
void updateInitialTransformRoot()
update initial transformation property from current Transform3D
png_infop png_charp png_int_32 png_int_32 int * type
Definition: png.h:2332
void resizeBoundingBox()
resize bounding box which covers the whole body
void setTransform(Vector3d pos, Matrix3d rot)
set new position and rotation in global frame
* x
Definition: IceUtils.h:98
boolean _saveAs()
save this model as a VRML file
#define null
our own NULL pointer
Definition: IceTypes.h:57
double [] getInitialTransformArray(GrxLinkItem link)
short [] childIndices_
子リンクインデックス列
void getCoM(Vector3d pos)
compute center of mass
void setWireFrame(boolean b)
switch display mode between fill and line
Vector3d absCoM()
compute CoM in global frame
GrxLinkItem getLink(String name)
get link from name
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 setFocused(boolean b)
set/unset focus on this item
void restoreProperties()
restore properties
void updateCoM()
update CoM and projected CoM positions
w
png_uint_32 i
Definition: png.h:2735
JCOPY_OPTION option
Definition: transupp.h:131
long b
Definition: jpegint.h:371
boolean load(File f)
load a model from file
void _setModelType(String value)
set model type(robot or environment) from String
void notifyModified()
notify this model is modified
void addLink(GrxLinkItem link)
add a link
void addRobot(File f, GrxLinkItem parentLink)
load and add a new robot as a child
double [] getTransformArray(GrxLinkItem link)
get transform of link in array form
boolean propertyChanged(String property, String value)
check validity of new value of property and update if valid
void _initMenu()
initialize right-click menu
Map< String, GrxLinkItem > nameToLink_
boolean create()
create a new model
GrxLinkItem rootLink()
get root link
List< GrxSensorItem > getSensors(String type)
final void setInt(String key, int value)
associate int value to key
void removeExtraJoint(GrxExtraJointItem extraJoint)
static boolean export(GrxModelItem model, String exportPath)
export model item to a VRML97 file
void setVisibleCoMonFloor(boolean b)
set visibility of CoM projected on the floor
void updateInitialJointValue(GrxLinkItem link)
update joint value property from current joint value
ValueEditType GetValueEditType(String key)
def run(tree, args)
int val
Definition: jpeglib.h:956
t
void setTransformRoot(Vector3d pos, Matrix3d rot)
set transformation of the root joint
void setVisibleCoM(boolean b)
set visibility of CoM
void delete()
delete this item
void setTransparencyMode(boolean b, Node node)
void calcForwardKinematics()
compute forward kinematics
Object setProperty(String key, String value)
set property value associated with a keyword
double [] getInitialVelocity(GrxLinkItem link)
void _setModelType(boolean isRobot)
set model type(robot or environment)
static org.omg.CORBA.Object getReference(String id)
get CORBA object which is associated with id
GrxModelItem clone()
Override clone method.
final String getStr(String key)
get value associated to keyword
void setJointValues(final double[] values)
set joint values
void jointType(String type)
set joint type
void _globalToRoot(Vector3d pos)
convert global position into robot local
void createLink(int index)
make connections between links and gather cameras
GrxModelItem(String name, GrxPluginManager item)
constructor
プラグイン管理クラス GrxUIの核になるクラス。プラグインのロード等の、初期化を実行する。 プラグインとそ...
void jointValue(double jv)
set new joint value
functions to make various basic shapes
static Joint * createLink(::World *world, const char *charname, int index, LinkInfoSequence_var iLinks, Joint *pjoint)
def getText(self, nodes=None)
void setTransformRoot(Transform3D tform)
set transformation of the root joint
void setCharacterPos(LinkPosition[] lpos, double[] q)
set transformation of the root joint and all joint values
void setJointColor(int jid, java.awt.Color color)
set color of joint
void setWireFrame(boolean b, Node node)
switch display mode between fill and line
static Switch createBall(double radius, Color3f c)
create ball with switch node
org
void removeLink(GrxLinkItem link)
remove a link
void rename(String newName)
rename this Model
* y
Definition: IceUtils.h:97
boolean propertyChanged(String property, String value)
check validity of new value of property and update if valid
void _loadVrmlScene(LinkInfo[] links)
create shapes of links
List< Camera_impl > getCameraSequence()
get sequence of cameras
void _calcUpperLower(Node node, Transform3D t3dParent)


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