GrxModelItem.java
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * General Robotix Inc.
00008  * National Institute of Advanced Industrial Science and Technology (AIST)
00009  */
00010 
00011 /*
00012  *  GrxModelItem.java
00013  *
00014  *  @author Yuichiro Kawasumi (General Robotix, Inc.)
00015  *  @author Shin'ichiro Nakaoka (AIST)
00016  */
00017 
00018 package com.generalrobotix.ui.item;
00019 
00020 import java.io.File;
00021 import java.io.IOException;
00022 import java.util.*;
00023 
00024 import javax.media.j3d.*;
00025 import javax.vecmath.*;
00026 
00027 import org.eclipse.jface.action.Action;
00028 import org.eclipse.jface.dialogs.InputDialog;
00029 import org.eclipse.jface.dialogs.MessageDialog;
00030 import org.eclipse.osgi.util.NLS;
00031 import org.eclipse.swt.SWT;
00032 import org.eclipse.swt.widgets.FileDialog;
00033 import org.w3c.dom.Element;
00034 import org.w3c.dom.NodeList;
00035 
00036 import com.generalrobotix.ui.grxui.GrxUIPerspectiveFactory;
00037 import com.generalrobotix.ui.*;
00038 import com.generalrobotix.ui.util.*;
00039 import com.generalrobotix.ui.util.AxisAngle4d;
00040 import com.generalrobotix.ui.view.tdview.*;
00041 import com.generalrobotix.ui.view.vsensor.Camera_impl;
00042 
00043 import jp.go.aist.hrp.simulator.*;
00044 import jp.go.aist.hrp.simulator.ModelLoaderPackage.AABBdataType;
00045 import jp.go.aist.hrp.simulator.ModelLoaderPackage.ModelLoadOption;
00046 import jp.go.aist.hrp.simulator.ModelLoaderPackage.ModelLoaderException;
00047 
00048 @SuppressWarnings({ "unchecked", "serial" }) //$NON-NLS-1$ //$NON-NLS-2$
00052 public class GrxModelItem extends GrxBaseItem implements Manipulatable {
00053     public static final String TITLE = "Model"; //$NON-NLS-1$
00054     public static final String DEFAULT_DIR = "/../model"; //$NON-NLS-1$
00055     public static final String FILE_EXTENSION = "*"; //$NON-NLS-1$
00056     private static final double DEFAULT_RADIUS = 0.05;
00057 
00058     // icons
00059         private static final String robotIcon = "robot.png"; //$NON-NLS-1$
00060         private static final String envIcon = "environment.png"; //$NON-NLS-1$
00061         
00062     private boolean isRobot_ = true;
00063 
00064     private BodyInfo bInfo_;
00065     private boolean bModified_ = false; //< true if this model is modified, false otherwise
00066 
00067     public BranchGroup bgRoot_ = new BranchGroup();
00068     public Vector<GrxLinkItem> links_ = new Vector<GrxLinkItem>();
00069     public Vector<GrxExtraJointItem> extraJoints_ = new Vector<GrxExtraJointItem>();
00070     // jontId -> link
00071     private int[] jointToLink_; 
00072     public Map<String, GrxLinkItem> nameToLink_ = new HashMap<String, GrxLinkItem>();
00073     
00074     // list of cameras
00075     private List<Camera_impl> cameraList_ = new ArrayList<Camera_impl>();
00076 
00077     public ShapeInfo[] shapes = null;
00078     public AppearanceInfo[] appearances = null;
00079     public MaterialInfo[] materials = null;
00080     public TextureInfo[] textures = null;
00081     
00082     // CoM
00083     private Switch switchCom_;
00084     private TransformGroup tgCom_;
00085     
00086     // CoM projected on the floor
00087     private Switch switchComZ0_;
00088     private TransformGroup tgComZ0_;
00089 
00090     // bounding box of the whole body
00091     private Switch switchBb_;
00092        
00096     public void notifyModified(){
00097         //System.out.println(getName()+" : modification is notified");
00098         notifyObservers("Modified");
00099         bModified_ = true;
00100     }
00101     
00102     public boolean isModified(){
00103         return bModified_;
00104     }
00105     
00106     public static final int MODIFIED_OK=0;
00107     public static final int MODIFIED_NG=1;
00108     public static final int MODIFIED_NOT=2;
00109     public int checkModifiedModel(boolean reload){
00110         if(bModified_){
00111                 String mes = MessageBundle.get("GrxProjectItem.dialog.message.changeModel.mes"); //$NON-NLS-1$
00112                 mes = NLS.bind(mes, new String[]{getName()});
00113                 
00114                 MessageDialog msgDlg =
00115                     new MessageDialog(GrxUIPerspectiveFactory.getCurrentShell(),
00116                                           MessageBundle.get("GrxProjectItem.dialog.message.changeModel.title"),
00117                                       null,
00118                                       mes,
00119                                       MessageDialog.INFORMATION, 
00120                                       new String[] {MessageBundle.get("GrxProjectItem.dialog.message.changeModel.btn.save"), 
00121                                               MessageBundle.get("GrxProjectItem.dialog.message.changeModel.btn.reverse"), 
00122                                               MessageBundle.get("GrxProjectItem.dialog.message.changeModel.btn.cancel")},
00123                                       2);
00124                 
00125                 switch(msgDlg.open())
00126                 {
00127                 case 0:
00128                         if(reload){
00129                                 if(!saveAndLoad())
00130                                         return MODIFIED_NG;
00131                                 else
00132                                         return MODIFIED_OK;
00133                         }else{
00134                             if(!_saveAs())
00135                                 return MODIFIED_NG;
00136                             cancelModified();
00137                             return MODIFIED_OK;
00138                         }
00139                 case 1:
00140                         if(reload){
00141                                  if(!reload())
00142                                          return MODIFIED_NG;
00143                                  else
00144                                          return MODIFIED_OK;
00145                         }else{
00146                             cancelModified();
00147                             return MODIFIED_OK;
00148                         }
00149                 case 2:
00150                 default:
00151                     return MODIFIED_NG;
00152                 }
00153             }
00154         return MODIFIED_NOT;
00155     }
00156     
00157     public void cancelModified(){
00158         bModified_ = false;
00159         notifyObservers("ClearModified");
00160     }
00161     
00162     public boolean saveAndLoad(){
00163         if(!_saveAs())
00164                 return false;
00165         reload();
00166         return true;
00167     }
00168 
00169     public boolean reload(){
00170         File f = new File(getURL(true));
00171         load0(f);
00172         sameUrlModelLoad();
00173         return true;
00174     }
00175     
00176     private void sameUrlModelLoad(){
00177         File f = new File(getURL(true));
00178         List<GrxModelItem> sameModels = getSameUrlModels();
00179                 Iterator<GrxModelItem> it = sameModels.iterator();
00180                 while(it.hasNext()){
00181                         it.next().load0(f);
00182                 }
00183     }
00184     
00189     public BodyInfo getBodyInfo(){
00190         String url = getURL(true);
00191         if (bModified_ || url == null || url.equals("")) //$NON-NLS-1$
00192                 return null;
00193         else
00194                 return bInfo_;
00195     }
00196     
00200         class MenuChangeType extends Action {
00201                 public MenuChangeType() {
00202                         if (isRobot_) {
00203                     setText( MessageBundle.get("GrxModelItem.menu.changeEnv") ); //$NON-NLS-1$
00204                 } else {
00205                         setText( MessageBundle.get("GrxModelItem.menu.changeRobot") ); //$NON-NLS-1$
00206                 }
00207                 }
00208                 public void run(){
00209                         _setModelType(!isRobot_);
00210                 }
00211         };
00212         MenuChangeType menuChangeType_ = new MenuChangeType();
00213 
00219         public GrxModelItem(String name, GrxPluginManager item) {
00220                 super(name, item);
00221                 setIcon(robotIcon);
00222                 _initMenu();
00223                 
00224         bgRoot_.setCapability(BranchGroup.ALLOW_DETACH);
00225         bgRoot_.setCapability(BranchGroup.ALLOW_CHILDREN_READ);
00226         bgRoot_.setCapability(BranchGroup.ALLOW_CHILDREN_WRITE);
00227         bgRoot_.setCapability(BranchGroup.ALLOW_CHILDREN_EXTEND);
00228 
00229         // create root link
00230         GrxLinkItem link = new GrxLinkItem("root", manager_, this); //$NON-NLS-1$
00231         manager_.itemChange(link, GrxPluginManager.ADD_ITEM);
00232         link.jointType("free"); //$NON-NLS-1$
00233         bgRoot_.addChild(link.bg_);
00234         
00235         setProperty("url",""); //$NON-NLS-1$ //$NON-NLS-2$
00236         
00237         _setupMarks();
00238     }
00239 
00246         public void addLink(GrxLinkItem link){
00247                 //System.out.println("link is added : "+link.getName());
00248                 links_.add(link);
00249                 notifyModified();
00250         }
00251         
00256         public void removeLink(GrxLinkItem link){
00257                 //System.out.println("link is removed : "+link.getName());
00258                 links_.remove(link);
00259                 notifyModified();
00260         }
00261         
00265         private void _initMenu() {
00266                 // menu item : reload
00267                 setMenuItem(new Action(){
00268             public String getText(){ return MessageBundle.get("GrxModelItem.menu.reload"); } //$NON-NLS-1$
00269                         public void run(){
00270                                 boolean ret = load0(file_);
00271                                 if(ret){
00272                                         List<GrxModelItem> sameModels = getSameUrlModels();
00273                                         Iterator<GrxModelItem> it = sameModels.iterator();
00274                                         while(it.hasNext()){
00275                                                 it.next().load0(file_);
00276                                         }
00277                                 }
00278                         }
00279                 });
00280 
00281         setMenuItem(menuChangeType_);
00282 
00283         // menu item : save
00284         setMenuItem(new Action(){
00285                 public String getText() { return MessageBundle.get("GrxModelItem.menu.save"); } //$NON-NLS-1$
00286                 public void run(){
00287                         String url = getStr("url"); //$NON-NLS-1$
00288                         if (url == null || url.equals("")){ //$NON-NLS-1$
00289                                 _saveAs();
00290                         }else{
00291                                         GrxVrmlExporter.export(GrxModelItem.this, url);
00292                                 }
00293                         if(bModified_)
00294                                 reload();
00295                         else
00296                                 sameUrlModelLoad();
00297                 }
00298         });
00299 
00300         // menu item : save as
00301         setMenuItem(new Action(){
00302                 public String getText() { return MessageBundle.get("GrxModelItem.menu.saveAs"); } //$NON-NLS-1$
00303                 public void run(){
00304                         _saveAs();
00305                         if(bModified_)
00306                                 reload();
00307                         else
00308                                 sameUrlModelLoad();
00309                 }
00310         });
00311 
00312      // menu item : add ExtraJoint
00313         setMenuItem(new Action(){
00314                 public String getText() { return MessageBundle.get("GrxModelItem.menu.addExtraJoint"); } //$NON-NLS-1$
00315                 public void run(){
00316                         InputDialog dialog = new InputDialog( null, getText(),
00317                                                 MessageBundle.get("GrxModelItem.dialog.message.extraJointName"), null,null); //$NON-NLS-1$
00318                                 if ( dialog.open() == InputDialog.OK && dialog.getValue() != null)
00319                                         addExtraJoint( dialog.getValue() );
00320                 }
00321         });
00322         
00323         /* disable copy and paste menus until they are implemented
00324         // menu item : copy
00325         setMenuItem( new Action(){
00326             public String getText(){
00327                 return "copy";
00328             }
00329             public void run(){
00330                 GrxDebugUtil.println("GrxModelItem.GrxModelItem copy Action");
00331                 manager_.setSelectedGrxBaseItemList();
00332             }
00333         });
00334 
00335         // menu item : paste
00336         setMenuItem(new Action(){
00337             public String getText(){
00338                 return "paste";
00339             }
00340 
00341             public void run(){
00342             }
00343         });
00344         */
00345         }
00346 
00350         private boolean _saveAs(){
00351                 FileDialog fdlg = new FileDialog( GrxUIPerspectiveFactory.getCurrentShell(), SWT.SAVE);
00352                 fdlg.setFilterPath(getDefaultDir().getAbsolutePath());
00353                 String fPath = fdlg.open();
00354                 if( fPath != null ) {
00355                         fPath = fPath.replace('\\','/');
00356                         if (GrxVrmlExporter.export(GrxModelItem.this, fPath)){
00357                                 setURL(fPath);
00358                                 setDefaultDirectory(new File(fPath).getParent());
00359                         }
00360                         return true;
00361                 }else
00362                         return false;
00363         }
00368         public boolean create() {
00369                 return true;
00370         }
00371 
00375         public GrxLinkItem rootLink() {
00376                 if (links_.size() > 0){
00377                         return links_.get(0);
00378                 }else{
00379                         return null;
00380                 }
00381         }
00382 
00386     public void restoreProperties() {
00387         //super.restoreProperties();   プロパティNumOfAABBは、毎回propertyChanged実行すると遅くなるので、ModelItemだけここで実装する  //
00388         boolean flg=false;
00389         if (element_ != null) {
00390                         NodeList props = element_.getElementsByTagName(PROPERTY_TAG);
00391                         for (int j = 0; j < props.getLength(); j++) {
00392                                 Element propEl = (Element) props.item(j);
00393                                 String key = propEl.getAttribute("name"); //$NON-NLS-1$
00394                                 String val = propEl.getAttribute("value"); //$NON-NLS-1$
00395                                 if(key.contains("NumOfAABB")){
00396                                         setProperty(key, val);
00397                                         String[] linkName = key.split("\\.");
00398                                         nameToLink_.get(linkName[0]).setProperty("NumOfAABB", val);
00399                                         flg=true;
00400                                 }else{
00401                                         if (!propertyChanged(key, val)){
00402                                                 setProperty(key, val);
00403                                         }
00404                                 }
00405                         }
00406         }
00408         if (getStr("markRadius")==null) setDbl("markRadius", DEFAULT_RADIUS); //$NON-NLS-1$ //$NON-NLS-2$
00409 
00410         _setModelType(isTrue("isRobot", isRobot_)); //$NON-NLS-1$
00411 
00412         if (getDblAry(rootLink().getName()+".translation", null) == null && //$NON-NLS-1$
00413             getDblAry(rootLink().getName()+".rotation", null) == null) //$NON-NLS-1$
00414             updateInitialTransformRoot();
00415 
00416         for (int i=0; i<jointToLink_.length; i++) {
00417             GrxLinkItem l = links_.get(jointToLink_[i]);
00418             Double d = getDbl(l.getName()+".angle", null); //$NON-NLS-1$
00419             if (d == null)
00420                 setDbl(l.getName()+".angle", 0.0); //$NON-NLS-1$
00421         }
00422         
00423         if(flg)
00424                 makeAABBforSameUrlModels();
00425         
00426         for (int i=0; i<links_.size(); i++) {
00427             GrxLinkItem l = links_.get(i);
00428             if(!l.jointType_.equals("fixed")){
00429                     String s = this.getProperty(l.getName()+".mode", null); //$NON-NLS-1$
00430                     if (s == null)
00431                         setProperty(l.getName()+".mode", "Torque"); //$NON-NLS-1$
00432             }
00433         }
00434     }
00435 
00442     public boolean propertyChanged(String property, String value) {
00443         if (super.propertyChanged(property, value)){
00444         }else if(property.equals("isRobot")){ //$NON-NLS-1$
00445                 _setModelType(value);
00446         }else if(property.equals("controlTime")){ //$NON-NLS-1$
00447                 try{
00448                         double t = Double.parseDouble(value);
00449                         if (t > 0){
00450                                 setProperty("controlTime", value); //$NON-NLS-1$
00451                         }
00452                 }catch(Exception ex){
00453                 }
00454         }else if(property.equals(rootLink().getName()+".translation")){ //$NON-NLS-1$
00455                 if (rootLink().localTranslation(value)){
00456                         setProperty(rootLink().getName()+".translation", value); //$NON-NLS-1$
00457                         calcForwardKinematics();
00458                 }
00459         }else if(property.equals(rootLink().getName()+".rotation")){ //$NON-NLS-1$
00460                 if (rootLink().localRotation(value)){
00461                         setProperty(rootLink().getName()+".rotation", value); //$NON-NLS-1$
00462                         calcForwardKinematics();
00463                 }
00464         }else if(property.equals(rootLink().getName()+".velocity")){ //$NON-NLS-1$
00465                 setProperty(rootLink().getName()+".velocity", value); //$NON-NLS-1$
00466                 rootLink().setProperty("velocity", value);
00467         }else if(property.equals(rootLink().getName()+".angularVelocity")){ //$NON-NLS-1$
00468                 setProperty(rootLink().getName()+".angularVelocity", value); //$NON-NLS-1$
00469                 rootLink().setProperty("angulaerVelocity", value);
00470         }else{
00471             for (int j = 0; j < links_.size(); j++){
00472                 GrxLinkItem link = links_.get(j);
00473                 if (property.equals(link.getName()+".angle")){ //$NON-NLS-1$
00474                         if (link.jointValue(value)){
00475                         calcForwardKinematics();
00476                         setProperty(link.getName()+".angle", value); //$NON-NLS-1$
00477                         }
00478                     return true;
00479                 }else if(property.equals(link.getName()+".jointVelocity")){ //$NON-NLS-1$
00480                         setProperty(link.getName()+".jointVelocity", value); //$NON-NLS-1$
00481                         link.setProperty("jointVelocity", value);
00482                         return true;
00483                 }else if(property.equals(link.getName()+".NumOfAABB")){ //$NON-NLS-1$
00484                         if(link.propertyChanged("NumOfAABB", value)){
00485                                 setProperty(link.getName()+".NumOfAABB", value); //$NON-NLS-1$
00486                         }
00487                         return true;
00488                 }else if(property.equals(link.getName()+".mode")){ //$NON-NLS-1$
00489                         setProperty(link.getName()+".mode", value); //$NON-NLS-1$
00490                         link.setProperty("mode", value);
00491                         return true;
00492                 }
00493             }
00494                 return false;
00495         }
00496         return true;
00497 
00498     }
00499 
00504     private void _setModelType(String value){
00505         Boolean b = Boolean.parseBoolean(value);
00506                 if (b != null){
00507                         _setModelType(b);
00508                 }
00509     }
00510 
00515     private void _setModelType(boolean isRobot) {
00516         isRobot_ = isRobot;
00517         if (isRobot_) {
00518             setIcon(robotIcon);
00519             menuChangeType_.setText(MessageBundle.get("GrxModelItem.menu.changeEnv")); //$NON-NLS-1$
00520         } else {
00521                 menuChangeType_.setText( MessageBundle.get("GrxModelItem.menu.changeRobot") ); //$NON-NLS-1$
00522             setIcon(envIcon);
00523             setVisibleCoM(false);
00524             setVisibleCoMonFloor(false);
00525         }
00526         setProperty("isRobot", String.valueOf(isRobot_)); //$NON-NLS-1$
00527     }
00528 
00536     private void _setupMarks() {
00537         double radius = getDbl("markRadius", DEFAULT_RADIUS); //$NON-NLS-1$
00538         switchCom_ = GrxShapeUtil.createBall(radius, new Color3f(1.0f, 1.0f, 0.0f));
00539         switchComZ0_= GrxShapeUtil.createBall(radius, new Color3f(0.0f, 1.0f, 0.0f));
00540         tgCom_ = (TransformGroup)switchCom_.getChild(0);
00541         tgComZ0_ = (TransformGroup)switchComZ0_.getChild(0);
00542         TransformGroup root = getTransformGroupRoot();
00543         root.addChild(switchCom_);
00544         root.addChild(switchComZ0_);
00545 
00546         SceneGraphModifier modifier = SceneGraphModifier.getInstance();
00547         modifier.init_ = true;
00548         modifier.mode_ = SceneGraphModifier.CREATE_BOUNDS;
00549         
00550         Color3f color = new Color3f(0.0f, 1.0f, 0.0f);
00551         switchBb_ = SceneGraphModifier._makeSwitchNode(modifier._makeBoundingBox(color));
00552         root.addChild(switchBb_);
00553         
00554     }
00555 
00561     public boolean load(File f) {
00562         boolean ret = load0(f);
00563         if(ret){
00564                 List<GrxModelItem> sameModels = getSameUrlModels();
00565                 if(!sameModels.isEmpty()){
00566                         GrxModelItem model = sameModels.get(0);
00567                         boolean flg=false;
00568                         for(int i=0; i<model.links_.size(); i++){                               
00569                                         if(!model.links_.get(i).getStr("NumOfAABB").equals("original data")){
00570                                                 links_.get(i).setInt("NumOfAABB", model.links_.get(i).getInt("NumOfAABB",1));
00571                                                 setInt(links_.get(i).getName()+".NumOfAABB", model.links_.get(i).getInt("NumOfAABB",1));
00572                                                 flg = true;
00573                                         }
00574                                 }
00575                         if(flg){
00576                                 BodyInfo bodyInfo = model.getBodyInfoFromModelLoader();
00577                                 makeAABB(bodyInfo);
00578                         }
00579                 }
00580                 return true;
00581         }else
00582                 return false;
00583         
00584     }
00585     
00586     private boolean load0(File f) {
00587         long load_stime = System.currentTimeMillis();
00588         file_ = f; 
00589         String url=null;
00590                 try {
00591                         url = f.getCanonicalPath();
00592                 } catch (IOException e) {
00593                         e.printStackTrace();
00594                 }
00595         GrxDebugUtil.println("Loading " + url); //$NON-NLS-1$
00596         try {
00597             ModelLoader mloader = ModelLoaderHelper.narrow(
00598                 GrxCorbaUtil.getReference("ModelLoader")); //$NON-NLS-1$
00599             mloader._non_existent();
00600             setURL(url);
00601             bInfo_ = mloader.loadBodyInfo(getURL(true));
00602             boolean ret = registerCharacter();
00603             long load_etime = System.currentTimeMillis();
00604             System.out.println("load time = " + (load_etime-load_stime) + "ms"); //$NON-NLS-1$ //$NON-NLS-2$
00605             return ret;
00606         }catch(ModelLoaderException me){
00607             MessageDialog.openError(GrxUIPerspectiveFactory.getCurrentShell(),
00608                     MessageBundle.get("GrxModelItem.dialog.title.error"), //$NON-NLS-1$
00609                     MessageBundle.get("GrxModelItem.dialog.message.loadError") +"\n" + //$NON-NLS-1$ //$NON-NLS-2$
00610                     url + "\n\n" + me.description); //$NON-NLS-1$
00611                         System.out.println("Failed to load vrml model:" + url); //$NON-NLS-1$
00612                         me.printStackTrace();
00613                         return false;
00614                 } catch (Exception ex) {
00615                         MessageDialog.openError(GrxUIPerspectiveFactory.getCurrentShell(),
00616                     MessageBundle.get("GrxModelItem.dialog.title.error"), //$NON-NLS-1$
00617                     MessageBundle.get("GrxModelItem.dialog.message.NoModelLoader") ); 
00618                         System.out.println("Failed to load vrml model:" + url); //$NON-NLS-1$
00619                         ex.printStackTrace();
00620                         return false;
00621                 }
00622     }
00623     
00624     public boolean registerCharacter(BodyInfo bInfo){
00625         bInfo_ = bInfo;
00626         return registerCharacter();
00627     }
00628     
00629     private boolean registerCharacter(){
00630         manager_.focusedItem(null);
00631         manager_.setSelectedItem(this, false);
00632         bgRoot_.detach();
00633         bgRoot_ = new BranchGroup();
00634         bgRoot_.setCapability(BranchGroup.ALLOW_DETACH);
00635         bgRoot_.setCapability(BranchGroup.ALLOW_CHILDREN_READ);
00636         bgRoot_.setCapability(BranchGroup.ALLOW_CHILDREN_WRITE);
00637         bgRoot_.setCapability(BranchGroup.ALLOW_CHILDREN_EXTEND);
00638 
00639             LinkInfo[] linkInfoList = bInfo_.links();
00640 
00641             // delete existing model data
00642             if (rootLink() != null){
00643                 rootLink().delete();
00644             }
00645 
00646             for (int i=0; i<cameraList_.size(); i++){
00647                 cameraList_.get(i).destroy();
00648             }
00649             cameraList_.clear();
00650 
00651             int jointCount = 0;
00652             nameToLink_.clear();
00653 
00654             int massZeroLink = -1;
00655             for (int i = 0; i < linkInfoList.length; i++) {
00656                 GrxLinkItem link = new GrxLinkItem(linkInfoList[i].name, manager_, this, linkInfoList[i]); 
00657                 manager_.itemChange(link, GrxPluginManager.ADD_ITEM);
00658                 if (link.jointId_ >= 0){
00659                     jointCount++;
00660                 }
00661                 nameToLink_.put(link.getName(), link);
00662                 if(linkInfoList[i].mass <= 0.0)
00663                         massZeroLink = i;
00664             }
00665             if(massZeroLink >= 0)
00666                 MessageDialog.openWarning(null, getName(), linkInfoList[massZeroLink].name+"::"+ 
00667                                         MessageBundle.get("GrxModelItem.dialog.message.massZero"));
00668             System.out.println("links_.size() = "+links_.size()); //$NON-NLS-1$
00669 
00670             // Search root node.
00671             int rootIndex = -1;
00672             for( int i = 0 ; i < links_.size() ; i++ ) {
00673                 if( links_.get(i).parentIndex_ < 0 ){
00674                     if( rootIndex < 0 ) {
00675                         rootIndex = i;
00676                     } else {
00677                         System.out.println( "Error. Two or more root node exist." ); //$NON-NLS-1$
00678                     }
00679                 }
00680             }
00681             if( rootIndex < 0 ){
00682                 System.out.println( "Error, root node doesn't exist." ); //$NON-NLS-1$
00683             }
00684 
00685             createLink(rootIndex);
00686 
00687             jointToLink_ = new int[jointCount];
00688             for (int i=0; i<jointCount; i++) {
00689                 for (int j=0; j<links_.size(); j++) {
00690                     if (links_.get(j).jointId_ == i) {
00691                         jointToLink_[i] = j;
00692                     }
00693                 }
00694             }
00695 
00696             ExtraJointInfo[] extraJointList = bInfo_.extraJoints();
00697             extraJoints_.clear();
00698             for (int i = 0; i < extraJointList.length; i++) {
00699                 GrxExtraJointItem extraJoint = new GrxExtraJointItem(extraJointList[i].name, manager_, this, extraJointList[i]);
00700                 extraJoints_.add(extraJoint);
00701                 manager_.itemChange(extraJoint, GrxPluginManager.ADD_ITEM);
00702             }
00703             
00704             long stime = System.currentTimeMillis();
00705             try {
00706                                 _loadVrmlScene(linkInfoList);
00707                         } catch (BadLinkStructureException e) {
00708                                 e.printStackTrace();
00709                                 return false;
00710                         }
00711             long etime = System.currentTimeMillis();
00712             System.out.println("_loadVrmlScene time = " + (etime-stime) + "ms"); //$NON-NLS-1$ //$NON-NLS-2$
00713 
00714             calcForwardKinematics();
00715             updateInitialTransformRoot();
00716             updateInitialJointValues();
00717 
00718             _setupMarks();
00719             setProperty("isRobot", Boolean.toString(isRobot_)); //$NON-NLS-1$
00720             for (int i=0; i<links_.size(); i++) {
00721                 GrxLinkItem l = links_.get(i);
00722                 if(!l.jointType_.equals("fixed")){
00723                         setProperty(l.getName()+".mode", "Torque"); //$NON-NLS-1$
00724                         l.setProperty("mode", "Torque");
00725                 }
00726             }
00727             
00728             manager_.setSelectedItem(this, true);
00729             cancelModified();
00730             return true;
00731     }
00732 
00737     private void createLink( int index ){
00738 
00739         GrxLinkItem link = links_.get(index);
00740 
00741         // register this to children field of parent link
00742         if (link.parentIndex_ != -1){
00743                 links_.get(link.parentIndex_).addLink(link);
00744         }else{
00745             bgRoot_.addChild(link.bg_);
00746         }
00747 
00748         // gather cameras
00749         for (int i=0; i< link.children_.size(); i++){
00750                 if (link.children_.get(i) instanceof GrxSensorItem){
00751                         GrxSensorItem sensor = (GrxSensorItem)link.children_.get(i);
00752                         if (sensor.isCamera()){
00753                         cameraList_.add(sensor.getCamera());
00754                         }
00755                 }
00756         }
00757 
00758         for( int i = 0 ; i < link.childIndices_.length ; i++ )
00759             {
00760                 // call recursively
00761                 int childIndex = link.childIndices_[i];
00762                 createLink( childIndex );
00763             }
00764     }
00765 
00771     private void _loadVrmlScene(LinkInfo[] links) throws BadLinkStructureException {
00772         shapes = bInfo_.shapes();
00773         appearances = bInfo_.appearances();
00774         materials = bInfo_.materials();
00775         textures = bInfo_.textures();
00776         
00777         int numLinks = links.length;
00778         for(int linkIndex = 0; linkIndex < numLinks; linkIndex++) {
00779             GrxLinkItem link = links_.get(linkIndex);
00780             for (int i=0; i<link.children_.size(); i++){
00781                 if(link.children_.get(i) instanceof GrxSegmentItem){
00782                         GrxSegmentItem segment = (GrxSegmentItem)link.children_.get(i);
00783                         segment.addShape(segment.getTransform());
00784                 }else if (link.children_.get(i) instanceof GrxSensorItem){
00785                         GrxSensorItem sensor = (GrxSensorItem)link.children_.get(i);
00786                         sensor.addShape(new Matrix4d(1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1));
00787                 }else if (link.children_.get(i) instanceof GrxHwcItem){
00788                         GrxHwcItem hwc = (GrxHwcItem)link.children_.get(i);
00789                         hwc.addShape(new Matrix4d(1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1));
00790                 }
00791             }
00792         }
00793         setDblAry(rootLink().getName()+".translation", rootLink().localTranslation()); //$NON-NLS-1$
00794         setDblAry(rootLink().getName()+".rotation", rootLink().localRotation()); //$NON-NLS-1$
00795         setDblAry(rootLink().getName()+".velocity", new double[]{0,0,0} ); //$NON-NLS-1$
00796         setDblAry(rootLink().getName()+".angularVelocity", new double[]{0,0,0} ); //$NON-NLS-1$
00797         
00798         for (int i=0; i<links_.size(); i++) {
00799             Node n = links_.get(i).tg_.getChild(0);
00800             if (n.getCapability(Node.ENABLE_PICK_REPORTING))
00801                 n.clearCapability(Node.ENABLE_PICK_REPORTING);
00802         }
00803         
00804     }
00805 
00810     public void addRobot(File f, GrxLinkItem parentLink){
00811         try {
00812             GrxDebugUtil.println("Loading " + f.getCanonicalPath());
00813                 ModelLoader mloader = ModelLoaderHelper.narrow(
00814                                 GrxCorbaUtil.getReference("ModelLoader")); //$NON-NLS-1$
00815                 mloader._non_existent();
00816                 BodyInfo bInfo = mloader.loadBodyInfo(f.getCanonicalPath());
00817                 LinkInfo[] linkInfoList = bInfo.links();
00818                 int numOfLink = links_.size();
00819                 int numOfJoint = getDOF();
00820                 Vector<String> sensorType = new Vector<String>();
00821                 Vector<String>[] sensorNames = new Vector[GrxSensorItem.sensorType.length];
00822                 for(int i=0; i<GrxSensorItem.sensorType.length; i++){
00823                         sensorType.add(GrxSensorItem.sensorType[i]);
00824                         sensorNames[i] = new Vector<String>();
00825                         String[] names = getSensorNames(GrxSensorItem.sensorType[i]);
00826                         if(names!=null)
00827                                 for(int j=0; j<names.length; j++)
00828                                         sensorNames[i].add(names[j]);
00829                 }
00830                 int rootIndex = -1;
00831                 int jointCount  = 0;
00832                 for (int i = 0; i < linkInfoList.length; i++) {
00833                         if(linkInfoList[i].parentIndex < 0 ){
00834                                 linkInfoList[i].parentIndex = (short) links_.indexOf(parentLink);
00835                                 rootIndex = i+numOfLink;
00836                         }else
00837                                 linkInfoList[i].parentIndex += numOfLink;
00838                         for(int j=0; j<linkInfoList[i].childIndices.length; j++)
00839                                 linkInfoList[i].childIndices[j] += numOfLink;
00840                         if(linkInfoList[i].jointId >= 0){
00841                                 linkInfoList[i].jointId += numOfJoint;
00842                                 jointCount++;
00843                         }
00844                         SensorInfo[] sensors = linkInfoList[i].sensors;
00845                         for(int j=0; j<sensors.length; j++){
00846                                 int type = sensorType.indexOf(sensors[j].type);
00847                                 sensors[j].id += sensorNames[type].size();
00848                                 String sensorName = sensors[j].name;
00849                                 for(int k=0; sensorNames[type].indexOf(sensorName)!=-1; k++)
00850                                         sensorName = sensors[j].name + "_" + k;
00851                                 sensors[j].name = sensorName;
00852                         }
00853                         String linkName = linkInfoList[i].name;
00854                         for(int j=0; nameToLink_.get(linkName)!=null; j++ )
00855                                 linkName = linkInfoList[i].name + "_" + j;
00856                 GrxLinkItem link = new GrxLinkItem(linkName, manager_, this, linkInfoList[i]);
00857                 nameToLink_.put(linkName, link);
00858             }
00859                 createLink(rootIndex);
00860                 jointToLink_ = new int[numOfJoint+jointCount];
00861                 for (int i=0; i<numOfJoint+jointCount; i++) {
00862                         for (int j=0; j<links_.size(); j++) {
00863                                 if (links_.get(j).jointId_ == i) {
00864                                         jointToLink_[i] = j;
00865                                 }
00866                         }
00867                 }               
00868                 for(int i=numOfLink; i<numOfLink+linkInfoList.length; i++)
00869                         manager_.itemChange(links_.get(i), GrxPluginManager.ADD_ITEM);
00870                 
00871                 shapes = bInfo.shapes();
00872                 appearances = bInfo.appearances();
00873                 materials = bInfo.materials();
00874                 textures = bInfo.textures();
00875             
00876             for(int linkIndex = numOfLink; linkIndex < links_.size(); linkIndex++) {
00877                 GrxLinkItem link = links_.get(linkIndex);
00878                 for (int i=0; i<link.children_.size(); i++){
00879                         if(link.children_.get(i) instanceof GrxSegmentItem){
00880                                 GrxSegmentItem segment = (GrxSegmentItem)link.children_.get(i);
00881                                 segment.addShape(segment.getTransform());
00882                         }else if (link.children_.get(i) instanceof GrxSensorItem){
00883                                 GrxSensorItem sensor = (GrxSensorItem)link.children_.get(i);
00884                                 sensor.addShape(new Matrix4d(1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1));
00885                         }else if (link.children_.get(i) instanceof GrxHwcItem){
00886                                 GrxHwcItem hwc = (GrxHwcItem)link.children_.get(i);
00887                                 hwc.addShape(new Matrix4d(1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1));
00888                         }
00889                 }
00890             }
00891         }catch(Exception ex){
00892                 MessageDialog.openError(GrxUIPerspectiveFactory.getCurrentShell(),
00893                     MessageBundle.get("GrxModelItem.dialog.title.error"), //$NON-NLS-1$
00894                     MessageBundle.get("GrxModelItem.dialog.message.NoModelLoader") ); 
00895         }
00896     }
00897 
00901     public class NormalRender {
00902         private LineArray nline = null;
00903 
00904         public NormalRender(GeometryArray geom, float scale, Matrix4d T) {
00905 
00906             Point3f[] vertices = new Point3f[geom.getVertexCount()];
00907             Vector3f[] normals = new Vector3f[geom.getVertexCount()];
00908             for (int i=0; i<geom.getVertexCount(); i++) {
00909                 vertices[i] = new Point3f();
00910                 normals[i] = new Vector3f();
00911             }
00912             geom.getCoordinates(0, vertices);
00913             geom.getNormals(0, normals);
00914             Point3f[] nvertices = new Point3f[vertices.length * 2];
00915             int n = 0;
00916             for (int i=0; i<vertices.length; i++ ){
00917 
00918                 T.transform(normals[i]);
00919                 normals[i].normalize();
00920                 T.transform(vertices[i]);
00921 
00922                 nvertices[n++] = new Point3f( vertices[i] );
00923                 nvertices[n++] = new Point3f( vertices[i].x + scale * normals[i].x,
00924                                               vertices[i].y + scale * normals[i].y,
00925                                               vertices[i].z + scale * normals[i].z );
00926             }
00927             nline = new LineArray(nvertices.length, GeometryArray.COORDINATES);
00928             nline.setCoordinates(0, nvertices);
00929         }
00930 
00931         public LineArray getLineArray() { return nline; }
00932     }
00933 
00934 
00940     public void setCharacterPos(LinkPosition[] lpos, double[] q) {
00941         boolean isAllPosProvided = true;
00942         if (q != null) {
00943             for (int i=0; i<jointToLink_.length; i++)
00944                 links_.get(jointToLink_[i]).jointValue(q[i]);
00945         }
00946         for (int i=0; i<links_.size(); i++) {
00947             if (lpos[i].p == null || lpos[i].R == null)
00948                 isAllPosProvided = false;
00949             else{
00950                 if(i==0){
00951                         links_.get(i).localTranslation(lpos[0].p);
00952                         links_.get(i).localRotation(lpos[0].R);
00953                 }
00954                 links_.get(i).absTransform(lpos[i].p, lpos[i].R);
00955             }
00956         }    
00957         if (isAllPosProvided)
00958             updateCoM();
00959         else
00960             calcForwardKinematics();
00961     }
00962 
00968     public void setTransformRoot(Vector3d pos, Matrix3d rot) {
00969         _setTransform(0, pos, rot);
00970     }
00971 
00977     public void setTransformRoot(Transform3D tform) {
00978         Vector3d pos = new Vector3d();
00979         Matrix3d rot = new Matrix3d();
00980         tform.get(rot, pos);
00981         _setTransform(0, pos, rot);
00982     }
00983 
00990     private void _setTransform(int linkId, Vector3d pos, Matrix3d rot) {
00991         GrxLinkItem link = links_.get(linkId);
00992         if (link != null )
00993                 if(link.parent_ == null ) link.setTransform(pos, rot);
00994     }
00995 
01000     public void setJointValues(final double[] values) {
01001          for (int j = 0; j < links_.size(); j++){
01002                 GrxLinkItem link = links_.get(j);
01003                 if (link.jointId_ >= 0 && link.jointId_ < values.length){
01004                         link.jointValue(values[link.jointId_]);
01005                 }
01006          }
01007     }
01008 
01012     public void setJointValuesWithinLimit() {
01013         rootLink().setJointValuesWithinLimit();
01014     }
01015 
01019     public void updateInitialTransformRoot() {
01020         Transform3D t3d = new Transform3D();
01021         Matrix3d m3d = new Matrix3d();
01022         Vector3d v3d = new Vector3d();
01023 
01024         getTransformGroupRoot().getTransform(t3d);
01025         t3d.get(m3d, v3d);
01026         setDblAry(rootLink().getName()+".translation", new double[]{v3d.x, v3d.y, v3d.z}); //$NON-NLS-1$
01027 
01028         AxisAngle4d a4d = new AxisAngle4d();
01029         a4d.setMatrix(m3d);
01030         setDblAry(rootLink().getName()+".rotation", new double[]{a4d.x, a4d.y, a4d.z, a4d.angle}); //$NON-NLS-1$
01031     }
01032 
01037     public void updateInitialJointValue(GrxLinkItem link) {
01038         if (link != null)
01039             setDbl(link.getName()+".angle", link.jointValue_); //$NON-NLS-1$
01040     }
01041 
01045     public void updateInitialJointValues() {
01046         for (int i=0; i<jointToLink_.length; i++) {
01047             GrxLinkItem l = links_.get(jointToLink_[i]);
01048             setDbl(l.getName()+".angle", l.jointValue_); //$NON-NLS-1$
01049         }
01050     }
01051 
01055     public  void calcForwardKinematics() {
01056         rootLink().calcForwardKinematics();
01057         updateCoM();
01058     }
01059 
01063     public void updateCoM() {
01064         if (switchCom_.getWhichChild() == Switch.CHILD_ALL ||
01065             switchComZ0_.getWhichChild() == Switch.CHILD_ALL) {
01066                 Vector3d v3d = new Vector3d();
01067             getCoM(v3d);
01068             Vector3d vz0 = new Vector3d(v3d);
01069 
01070             _globalToRoot(v3d);
01071             Transform3D t3d = new Transform3D();
01072             t3d.set(v3d);
01073             tgCom_.setTransform(t3d);
01074 
01075             vz0.z = 0.0;
01076             _globalToRoot(vz0);
01077             t3d.set(vz0);
01078             tgComZ0_.setTransform(t3d);
01079         }
01080     }
01081 
01086     private void _globalToRoot(Vector3d pos) {
01087         Transform3D t3d = new Transform3D();
01088         getTransformGroupRoot().getTransform(t3d);
01089         Vector3d p = new Vector3d();
01090         t3d.get(p);
01091         t3d.invert();
01092         pos.sub(p);
01093         t3d.transform(pos);
01094     }
01095 
01100     public void getCoM(Vector3d pos) {
01101         pos.x = 0.0;
01102         pos.y = 0.0;
01103         pos.z = 0.0;
01104         double totalMass = 0.0;
01105 
01106         for (int i = 0; i < links_.size(); i++) {
01107                 GrxLinkItem link = links_.get(i);
01108             totalMass += link.linkMass_;
01109             Vector3d absCom = link.absCoM();
01110             absCom.scale(link.linkMass_);
01111             pos.add(absCom);
01112         }
01113         pos.scale(1.0 / totalMass);
01114     }
01115 
01116     public List<GrxSensorItem> getSensors(String type){
01117         List<GrxSensorItem> sensors = new ArrayList<GrxSensorItem>();
01118         rootLink().gatherSensors(type, sensors);
01119         if (sensors.size() > 0){
01120                 return sensors;
01121         }else{
01122                 return null;
01123         }
01124     }
01125     
01126     public String[] getSensorNames(String type) {
01127         List<GrxSensorItem> l = getSensors(type);
01128         if (l == null)
01129             return null;
01130 
01131         String[] ret = new String[l.size()];
01132         for (int i=0; i<ret.length; i++)
01133                 ret[l.get(i).id_] = l.get(i).getName();
01134         return ret;
01135     }
01136 
01137     public TransformGroup getTransformGroupRoot() {
01138         return rootLink().tg_;
01139     }
01140 
01146     public double[] getTransformArray(GrxLinkItem link) {
01147         Transform3D t3d = new Transform3D();
01148         link.tg_.getTransform(t3d);
01149         Matrix3d mat = new Matrix3d();
01150         Vector3d vec = new Vector3d();
01151         t3d.get(mat, vec);
01152 
01153         double[] ret = new double[12];
01154         vec.get(ret);
01155         ret[3] = mat.m00; ret[4] = mat.m01; ret[5] = mat.m02;
01156         ret[6] = mat.m10; ret[7] = mat.m11; ret[8] = mat.m12;
01157         ret[9] = mat.m20; ret[10]= mat.m21; ret[11]= mat.m22;
01158 
01159         return ret;
01160     }
01161 
01162     public double[] getInitialTransformArray(GrxLinkItem link) {
01163         double[] ret = getTransformArray(link);
01164 
01165         double[] p = getDblAry(link.getName()+".translation", null); //$NON-NLS-1$
01166         if (p != null && p.length == 3) {
01167             System.arraycopy(p, 0, ret, 0, 3);
01168         }
01169 
01170         double[] r = getDblAry(link.getName()+".rotation", null); //$NON-NLS-1$
01171         if (r != null && r.length == 4) {
01172             Matrix3d mat = new Matrix3d();
01173             mat.set(new AxisAngle4d(r));
01174             ret[3] = mat.m00; ret[4] = mat.m01; ret[5] = mat.m02;
01175             ret[6] = mat.m10; ret[7] = mat.m11; ret[8] = mat.m12;
01176             ret[9] = mat.m20; ret[10]= mat.m21; ret[11]= mat.m22;
01177         }
01178 
01179         return ret;
01180     }
01181 
01182     public double[] getInitialVelocity(GrxLinkItem link){
01183         double[] ret = {0,0,0,0,0,0};
01184         double[] v = getDblAry(link.getName()+".velocity", null);
01185         if (v != null && v.length == 3) {
01186             System.arraycopy(v, 0, ret, 0, 3);
01187         }
01188         double[] w = getDblAry(link.getName()+".angularVelocity", null);
01189         if (w != null && w.length == 3) {
01190             System.arraycopy(w, 0, ret, 3, 3);
01191         }
01192         
01193         return ret;
01194     }
01195     
01196     public int getDOF() {
01197         if (jointToLink_ == null)
01198             return 0;
01199         return jointToLink_.length;
01200     }
01201 
01202     public String[] getJointNames() {
01203         String[] names = new String[jointToLink_.length];
01204         for (int i=0; i<jointToLink_.length; i++)
01205             names[i] = links_.get(jointToLink_[i]).getName();
01206         return names;
01207     }
01208 
01209     public double[] getJointValues() {
01210         double[] vals = new double[jointToLink_.length];
01211         for (int i=0; i<jointToLink_.length; i++)
01212             vals[i] = links_.get(jointToLink_[i]).jointValue_;
01213         return vals;
01214     }
01215 
01216     public double[] getInitialJointValues() {
01217         double[] ret = new double[jointToLink_.length];
01218         for (int i=0; i<ret.length; i++) {
01219             GrxLinkItem l = links_.get(jointToLink_[i]);
01220             String jname = l.getName();
01221             ret[i] = getDbl(jname+".angle", l.jointValue_); //$NON-NLS-1$
01222         }
01223         return ret;
01224     }
01225 
01226     public double[] getInitialJointVelocity(){
01227         double[] ret = new double[jointToLink_.length];
01228         for (int i=0; i<ret.length; i++) {
01229             GrxLinkItem l = links_.get(jointToLink_[i]);
01230             String jname = l.getName();
01231             ret[i] = getDbl(jname+".jointVelocity", 0.0); //$NON-NLS-1$
01232         }
01233         return ret;
01234     }
01235    /* public double[] getInitialJointMode() {
01236         double[] ret = new double[jointToLink_.length];
01237         for (int i=0; i<ret.length; i++) {
01238             String jname = links_.get(jointToLink_[i]).getName();
01239             String mode = getStr(jname+".mode", "Torque");
01240             ret[i] = mode.equals("HighGain") ? 1.0 : 0.0;
01241         }
01242         return ret;
01243     }*/
01244     public double[] getInitialJointMode() {
01245         double[] ret = new double[links_.size()];
01246         for (int i=0; i<ret.length; i++) {
01247             String lname = links_.get(i).getName();
01248             String mode = getStr(lname+".mode", "Torque"); //$NON-NLS-1$ //$NON-NLS-2$
01249             ret[i] = mode.equals("HighGain") ? 1.0 : 0.0; //$NON-NLS-1$
01250         }
01251         return ret;
01252     }
01253 
01254     public boolean isRobot() {
01255         return isRobot_;
01256     }
01257 
01258     public void setSelected(boolean b) {
01259         super.setSelected(b);
01260         /*
01261         if (!b) {
01262             bgRoot_.detach();
01263             for (int i=0; i<cameraList_.size(); i++)
01264                 cameraList_.get(i).getBranchGroup().detach();
01265         }
01266         */
01267     }
01268     
01275     public void setFocused(boolean b){
01276         if (b) resizeBoundingBox();
01277         switchBb_.setWhichChild(b ? Switch.CHILD_ALL : Switch.CHILD_NONE);
01278     }
01279 
01283     public void delete() {
01284         super.delete();
01285         Iterator<Camera_impl> it = cameraList_.iterator();
01286         while(it.hasNext())
01287                 it.next().destroy();
01288         if(bgRoot_.isLive()) bgRoot_.detach();
01289         Map<?, ?> m = manager_.pluginMap_.get((GrxCollisionPairItem.class));
01290         GrxCollisionPairItem[] collisionPairItems = m.values().toArray(new GrxCollisionPairItem[0]);
01291         for (int i=0; i<collisionPairItems.length; i++) {
01292                         GrxCollisionPairItem item = (GrxCollisionPairItem) collisionPairItems[i];
01293                         String name = getName();
01294                         if(name.equals(item.getStr("objectName1", ""))){ //$NON-NLS-1$ //$NON-NLS-2$
01295                                 item.delete();
01296                         }else if(name.equals(item.getStr("objectName2", ""))){ //$NON-NLS-1$ //$NON-NLS-2$
01297                                 item.delete();
01298                         }
01299         }
01300         m = manager_.pluginMap_.get((GrxExtraJointItem.class));
01301         GrxExtraJointItem[] extraJointItems = m.values().toArray(new GrxExtraJointItem[0]);
01302         for (int i=0; i<extraJointItems.length; i++) {
01303                         GrxExtraJointItem item = extraJointItems[i];
01304                         String name = getName();
01305                         if(name.equals(item.getStr("object1Name", ""))){ //$NON-NLS-1$ //$NON-NLS-2$
01306                                 item.delete();
01307                         }else if(name.equals(item.getStr("object2Name", ""))){ //$NON-NLS-1$ //$NON-NLS-2$
01308                                 item.delete();
01309                         }
01310         }
01311         GrxLinkItem root = rootLink();
01312         if(root != null)
01313                 root.delete();
01314     }
01315     
01319     public void resizeBoundingBox() {
01320         SceneGraphModifier modifier = SceneGraphModifier.getInstance();
01321         modifier.init_  = true;
01322         modifier.mode_ = SceneGraphModifier.RESIZE_BOUNDS;
01323 
01324         // ノードのRootのTransformGroupを取得
01325         TransformGroup tg = getTransformGroupRoot();
01326         Transform3D t3dLocal = new Transform3D();
01327         tg.getTransform(t3dLocal);
01328                 t3dLocal.invert();
01329                 for (int i=0; i<links_.size(); i++) {
01330                 modifier._calcUpperLower(links_.get(i).tg_, t3dLocal);
01331                 }
01332         Shape3D shapeNode = (Shape3D)switchBb_.getChild(0);
01333         Geometry gm = (Geometry)shapeNode.getGeometry(0);
01334 
01335         Point3f[] p3fW = modifier._makePoints();
01336         if (gm instanceof QuadArray) {
01337                 QuadArray qa = (QuadArray) gm;
01338                 qa.setCoordinates(0, p3fW);  // 座標
01339                 }
01340     }
01341 
01346     public void setVisibleCoM(boolean b) {
01347         if (isRobot()) {
01348             switchCom_.setWhichChild(b? Switch.CHILD_ALL:Switch.CHILD_NONE);
01349             calcForwardKinematics();
01350         } else {
01351             switchCom_.setWhichChild(Switch.CHILD_NONE);
01352         }
01353     }
01354 
01359     public void setVisibleCoMonFloor(boolean b) {
01360         if (isRobot()) {
01361             switchComZ0_.setWhichChild(b? Switch.CHILD_ALL:Switch.CHILD_NONE);
01362             calcForwardKinematics();
01363         } else {
01364             switchComZ0_.setWhichChild(Switch.CHILD_NONE);
01365         }
01366     }
01367 
01372     public void setWireFrame(boolean b) {
01373         setWireFrame(b, bgRoot_);
01374     }
01375     
01381     public void setWireFrame(boolean b, Node node){
01382         if (node instanceof Switch) {
01383             return;
01384         } else if (node instanceof Group) {
01385             Group g = (Group) node;
01386             for (int i = 0; i < g.numChildren(); i++)
01387                 setWireFrame(b, g.getChild(i));
01388 
01389         } else if (node instanceof Link) {
01390             Link l = (Link) node;
01391             SharedGroup sg = l.getSharedGroup();
01392             for (int i = 0; i < sg.numChildren(); i++)
01393                 setWireFrame(b, sg.getChild(i));
01394 
01395         } else if (node instanceof Shape3D) {
01396             Shape3D s3d = (Shape3D) node;
01397             Appearance app = s3d.getAppearance();
01398             if (app != null) {
01399                 PolygonAttributes pa = app.getPolygonAttributes();
01400                 if (pa != null){
01401                     if (b) {
01402                         pa.setPolygonMode(PolygonAttributes.POLYGON_LINE);
01403                     } else {
01404                         pa.setPolygonMode(PolygonAttributes.POLYGON_FILL);
01405                     }
01406                 }
01407             }
01408         }       
01409     }
01410 
01411     public void setVisibleAABB(boolean b){
01412         Iterator<GrxLinkItem> it = links_.iterator();
01413         while(it.hasNext())
01414                 it.next().setVisibleAABB(b);
01415      }
01416     
01421     public void setTransparencyMode(boolean b) {
01422         setTransparencyMode(b, bgRoot_);
01423     }
01424 
01430     private void setTransparencyMode(boolean b, Node node) {
01431         if (node instanceof Switch) {
01432             return;
01433         } else if (node instanceof Group) {
01434             Group g = (Group) node;
01435             for (int i = 0; i < g.numChildren(); i++)
01436                 setTransparencyMode(b, g.getChild(i));
01437 
01438         } else if (node instanceof Link) {
01439             Link l = (Link) node;
01440             SharedGroup sg = l.getSharedGroup();
01441             for (int i = 0; i < sg.numChildren(); i++)
01442                 setTransparencyMode(b, sg.getChild(i));
01443 
01444         } else if (node instanceof Shape3D) {
01445             Shape3D s3d = (Shape3D) node;
01446             Appearance app = s3d.getAppearance();
01447             if (app != null) {
01448                 TransparencyAttributes ta = app.getTransparencyAttributes();
01449                 if (ta != null) {
01450                     if (b) {
01451                         ta.setTransparency(0.5f);
01452                         ta.setTransparencyMode(TransparencyAttributes.FASTEST);
01453                     } else {
01454                         ta.setTransparency(0f);
01455                         ta.setTransparencyMode(TransparencyAttributes.NONE);
01456                     }
01457                 }
01458             }
01459         }       
01460         }
01461 
01466     public List<Camera_impl> getCameraSequence () {
01467         return cameraList_;
01468     }
01469 
01475     public void setJointColor(int jid, java.awt.Color color) {
01476         GrxLinkItem l = links_.get(jointToLink_[jid]);
01477         if (l != null) l.setColor(color);
01478     }
01479 
01480 
01481     /* this method is disabled to hide paste menu 
01482     public void paste(String clipVal){
01483 
01484         Clipboard clp = Toolkit.getDefaultToolkit().getSystemClipboard();
01485         Transferable data = clp.getContents(null);
01486 
01487         String strClip = "";
01488 
01489         if (data == null || !data.isDataFlavorSupported(DataFlavor.stringFlavor)){
01490             strClip = "転送に失敗しました";
01491         } else {
01492             try {
01493                 strClip = (String)data.getTransferData( DataFlavor.stringFlavor );
01494             } catch(Exception e) {
01495                 GrxDebugUtil.printErr("GrxModelItem.paste: " , e);
01496             }
01497         }
01498     }
01499     */
01500     
01505     public GrxModelItem clone(){
01506         GrxModelItem ret = (GrxModelItem)super.clone();
01507         ret.bInfo_ = (BodyInfo)bInfo_._duplicate(); 
01508         //ret.bgRoot_ = (BodyInfo)bgRoot_.cloneTree();
01509         
01510         ret.links_ = new Vector<GrxLinkItem>(links_);
01511         //ret.activeLink_ = activeLink_.clone();
01512 /*      
01513         Deep copy suspension list
01514 
01515     public BodyInfo bInfo_;
01516 
01517     public BranchGroup bgRoot_ = new BranchGroup();
01518     public Vector<GrxLinkItem> links_ = new Vector<GrxLinkItem>();
01519     public GrxLinkItem activeLink_;
01520     private int[] jointToLink_; // length = joint number
01521     private final Map<String, GrxLinkItem> linkMap_ = new HashMap<String, GrxLinkItem>();
01522     private final Vector<Shape3D> shapeVector_ = new Vector<Shape3D>();
01523     // sensor type name -> list of sensors
01524     private final Map<String, List<GrxSensorItem>> sensorMap_ = new HashMap<String, List<GrxSensorItem>>();
01525     // list of cameras
01526     private List<Camera_impl> cameraList_ = new ArrayList<Camera_impl>();
01527 
01528     private Switch switchCom_;
01529     private TransformGroup tgCom_;
01530     private Switch switchComZ0_;
01531     private TransformGroup tgComZ0_;
01532 
01533     // temporary variables for computation
01534     private Transform3D t3d_ = new Transform3D();
01535     private Vector3d v3d_ = new Vector3d();
01536 */
01537         
01538         return ret;
01539     }
01540     
01546     public GrxLinkItem getLink(String name){
01547         return nameToLink_.get(name);
01548     }
01549     
01550     private BodyInfo getBodyInfoFromModelLoader(){
01551         short[] depth = new short[links_.size()];
01552         for(int i=0; i<links_.size(); i++)
01553                 depth[i] = links_.get(i).getInt("NumOfAABB", 1).shortValue();
01554         try {
01555             ModelLoader mloader = ModelLoaderHelper.narrow(GrxCorbaUtil.getReference("ModelLoader")); //$NON-NLS-1$
01556             mloader._non_existent();
01557             ModelLoadOption option = new ModelLoadOption();
01558             option.readImage = false;
01559             option.AABBtype = AABBdataType.AABB_NUM;
01560             option.AABBdata = depth;
01561 
01562             return mloader.getBodyInfoEx(getURL(false), option);
01563         }catch(Exception e){
01564                 MessageDialog.openError(GrxUIPerspectiveFactory.getCurrentShell(),
01565                     MessageBundle.get("GrxModelItem.dialog.title.error"), //$NON-NLS-1$
01566                     MessageBundle.get("GrxModelItem.dialog.message.NoModelLoader") ); 
01567                 return null;
01568         }
01569     }
01570     
01571     private void makeAABB(BodyInfo binfo){
01572         bInfo_ = binfo;
01573           
01574         LinkInfo[] links = bInfo_.links();
01575         shapes = bInfo_.shapes();
01576         appearances = bInfo_.appearances();
01577         materials = bInfo_.materials();
01578         textures = bInfo_.textures();
01579            
01580         int numLinks = links.length;
01581         for(int i = 0; i < numLinks; i++) {
01582                 links_.get(i).clearAABB();
01583                 TransformedShapeIndex[] tsi = links[i].shapeIndices;
01584                 for(int j=0; j<tsi.length; j++){
01585                         links_.get(i).makeAABB(shapes[tsi[j].shapeIndex], tsi[j].transformMatrix );
01586                 }
01587         }
01588         notifyObservers("BodyInfoChange");
01589     }
01590     
01591     public List<GrxModelItem> getSameUrlModels(){
01592         List<GrxModelItem> models = manager_.<GrxModelItem>getSelectedItemList(GrxModelItem.class);
01593                 Iterator<GrxModelItem> it = models.iterator();
01594                 while(it.hasNext()){
01595                         GrxModelItem model = it.next();
01596                         if(!model.getURL(true).equals(getURL(true)) || model == this)
01597                                 it.remove();
01598                 }
01599                 return models;
01600     }
01601     
01602     public void makeAABBforSameUrlModels(){
01603         BodyInfo bodyInfo = getBodyInfoFromModelLoader();
01604                 makeAABB(bodyInfo);
01605                 List<GrxModelItem> sameModels = getSameUrlModels();
01606                 Iterator<GrxModelItem> it = sameModels.iterator();
01607                 while(it.hasNext()){
01608                         GrxModelItem _model = it.next();
01609                         for(int i=0; i<links_.size(); i++){
01610                                 String value = links_.get(i).getStr("NumOfAABB");
01611                                 _model.nameToLink_.get(links_.get(i).getName()).setProperty("NumOfAABB", value);
01612                                 _model.setProperty(links_.get(i).getName()+".NumOfAABB", value);
01613                         }
01614                         _model.makeAABB(bodyInfo); 
01615                 }
01616     }
01617 
01622         public void rename(String newName) {
01623         String oldName = getName();
01624         
01625                 super.rename(newName);
01626 
01627         OrderedHashMap mcoll = manager_.pluginMap_.get(GrxCollisionPairItem.class);
01628         if(mcoll != null)
01629         {
01630                 Iterator it = mcoll.values().iterator();
01631                 while(it.hasNext())
01632                 {
01633                         GrxCollisionPairItem ci = (GrxCollisionPairItem)it.next();
01634                         if(oldName.equals(ci.getProperty("objectName1")))
01635                                 ci.setProperty("objectName1", newName);
01636 
01637                         if(oldName.equals(ci.getProperty("objectName2")))
01638                                 ci.setProperty("objectName2", newName);
01639                 }
01640         }
01641         mcoll = manager_.pluginMap_.get(GrxExtraJointItem.class);
01642         if(mcoll != null)
01643         {
01644                 Iterator it = mcoll.values().iterator();
01645                 while(it.hasNext())
01646                 {
01647                         GrxExtraJointItem extraJoint = (GrxExtraJointItem) it.next();
01648                         if(oldName.equals(extraJoint.getProperty("object1Name")))
01649                                 extraJoint.setProperty("object1Name", newName);
01650 
01651                         if(oldName.equals(extraJoint.getProperty("object2Name")))
01652                                 extraJoint.setProperty("object2Name", newName);
01653                 }
01654         }
01655         }
01656 
01657     @Override
01658     public ValueEditType GetValueEditType(String key) {
01659         if(key.matches(".+\\.mode"))
01660         {
01661             return new ValueEditCombo(modeComboItem_);
01662         }else if(key.equals("isRobot")){
01663             return new ValueEditCombo(booleanComboItem_);
01664         }
01665         return super.GetValueEditType(key);
01666     }
01667 
01668         public void removeExtraJoint(GrxExtraJointItem extraJoint) {
01669                 extraJoints_.remove(extraJoint);
01670                 notifyModified();
01671         }
01672         
01673         private void addExtraJoint(String name) {
01674                 GrxExtraJointItem extraJoint = new GrxExtraJointItem(name, manager_, this, null);
01675         this.extraJoints_.add(extraJoint);
01676         notifyModified();
01677         manager_.itemChange(extraJoint, GrxPluginManager.ADD_ITEM);
01678         }
01679 }

openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:16