Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00017 package com.generalrobotix.ui.view.tdview;
00018
00019 import java.util.Iterator;
00020 import java.util.List;
00021 import java.util.Map;
00022 import java.util.Vector;
00023
00024 import javax.media.j3d.BranchGroup;
00025 import javax.media.j3d.Transform3D;
00026 import javax.media.j3d.TransformGroup;
00027
00028 import org.eclipse.jface.dialogs.MessageDialog;
00029 import org.eclipse.swt.widgets.Display;
00030
00031 import jp.go.aist.hrp.simulator.BodyInfo;
00032 import jp.go.aist.hrp.simulator.DynamicsSimulator;
00033 import jp.go.aist.hrp.simulator.DynamicsSimulatorFactory;
00034 import jp.go.aist.hrp.simulator.DynamicsSimulatorFactoryHelper;
00035 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.IntegrateMethod;
00036 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.LinkDataType;
00037 import jp.go.aist.hrp.simulator.DynamicsSimulatorPackage.SensorOption;
00038 import jp.go.aist.hrp.simulator.*;
00039
00040 import com.generalrobotix.ui.GrxBaseItem;
00041 import com.generalrobotix.ui.GrxPluginManager;
00042 import com.generalrobotix.ui.item.GrxCollisionPairItem;
00043 import com.generalrobotix.ui.item.GrxLinkItem;
00044 import com.generalrobotix.ui.item.GrxModelItem;
00045 import com.generalrobotix.ui.util.Grx3DViewClickListener;
00046 import com.generalrobotix.ui.util.GrxCorbaUtil;
00047 import com.generalrobotix.ui.util.GrxDebugUtil;
00048 import com.generalrobotix.ui.util.MessageBundle;
00049 import com.generalrobotix.ui.view.Grx3DView;
00050 import com.sun.j3d.utils.picking.PickCanvas;
00051 import com.sun.j3d.utils.picking.PickTool;
00052
00058 public class BehaviorManager implements WorldReplaceListener {
00059
00060
00061
00062 public static final int OPERATION_MODE_NONE = 0;
00063 public static final int OBJECT_ROTATION_MODE = 1;
00064 public static final int OBJECT_TRANSLATION_MODE = 2;
00065 public static final int JOINT_ROTATION_MODE = 3;
00066 public static final int FITTING_FROM_MODE = 4;
00067 public static final int FITTING_TO_MODE = 5;
00068 public static final int INV_KINEMA_FROM_MODE = 6;
00069 public static final int INV_KINEMA_TRANSLATION_MODE = 7;
00070 public static final int INV_KINEMA_ROTATION_MODE = 8;
00071
00072
00073 public static final int WALK_VIEW_MODE = 1;
00074 public static final int ROOM_VIEW_MODE = 2;
00075 public static final int PARALLEL_VIEW_MODE = 3;
00076
00077
00078 DynamicsSimulator currentDynamics_;
00079 private IseBehavior behavior_;
00080 private IseBehaviorHandler handler_;
00081 private int viewMode_;
00082 private int operationMode_;
00083 private ThreeDDrawable drawable_;
00084 private BehaviorInfo info_;
00085 private BehaviorHandler indicator_;
00086 private GrxPluginManager manager_;
00087 private Grx3DView viewer_;
00088 private InvKinemaResolver resolver_;
00089 private boolean itemChangeFlag_ = false;
00090 private boolean messageSkip_ = false;
00091
00092 private List<GrxModelItem> currentModels_ = null;
00093 private List<GrxCollisionPairItem> currentCollisionPairs_ = null;
00094
00095
00096
00097 public BehaviorManager(GrxPluginManager manager) {
00098 manager_ = manager;
00099 resolver_ = new InvKinemaResolver(manager_);
00100
00101 }
00102
00103
00104 public void setThreeDViewer(Grx3DView viewer) {
00105 viewer_ = viewer;
00106 PickCanvas pickCanvas = new PickCanvas(
00107 viewer.getCanvas3D(),
00108 viewer.getBranchGroupRoot()
00109 );
00110 pickCanvas.setMode(PickTool.GEOMETRY_INTERSECT_INFO);
00111
00112 handler_ = new IseBehaviorHandler(this);
00113
00114 behavior_ = new IseBehavior(handler_);
00115
00116 BranchGroup bg = new BranchGroup();
00117 bg.addChild(behavior_);
00118
00119 BranchGroup bgRoot = viewer_.getBranchGroupRoot();
00120 bgRoot.addChild(bg);
00121
00122
00123 drawable_ = viewer;
00124
00125 ViewInfo viewInfo = drawable_.getViewInfo();
00126 switch (viewInfo.getViewMode()) {
00127 case ViewInfo.VIEW_MODE_ROOM:
00128 setViewMode(ROOM_VIEW_MODE);
00129 break;
00130 case ViewInfo.VIEW_MODE_WALK:
00131 setViewMode(WALK_VIEW_MODE);
00132 break;
00133 case ViewInfo.VIEW_MODE_PARALLEL:
00134 setViewMode(PARALLEL_VIEW_MODE);
00135 break;
00136 }
00137
00138 info_ = new BehaviorInfo(manager_, pickCanvas, drawable_);
00139
00140 behavior_.setBehaviorInfo(info_);
00141 }
00142
00143 public void setViewIndicator(BehaviorHandler handler) {
00144 indicator_ = handler;
00145 }
00146
00147 public void setViewMode(int mode) {
00148 viewMode_ = mode;
00149 handler_.setViewMode(viewMode_);
00150 }
00151
00152 public void setOperationMode(int mode) {
00153 operationMode_ = mode;
00154 handler_.setOperationMode(operationMode_);
00155 }
00156
00157 public void setViewHandlerMode(String str) {
00158 handler_.setViewHandlerMode(str);
00159 }
00160
00161 public boolean fit() {
00162 return handler_.fit(info_);
00163 }
00164
00165 public void setPickTarget(TransformGroup tg) {
00166 BehaviorInfo info = behavior_.getBehaviorInfo();
00167 handler_.setPickTarget(tg, info);
00168 }
00169
00170
00171
00172 public void viewChanged(ViewChangeEvent evt) {
00173 Transform3D transform = new Transform3D();
00174 evt.getTransformGroup().getTransform(transform);
00175
00176
00177 drawable_.setTransform(transform);
00178 }
00179
00180
00181
00182 public void replaceWorld(List<GrxBaseItem> list) {
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197 if (handler_ != null)
00198 handler_.setViewIndicator(indicator_);
00199 }
00200
00206 public DynamicsSimulator getDynamicsSimulator(boolean update) {
00207
00208 if (update && currentDynamics_ != null) {
00209 try {
00210 currentDynamics_.destroy();
00211 } catch (Exception e) {
00212 GrxDebugUtil.printErr("getDynamicsSimulator: destroy failed.");
00213 }
00214 currentDynamics_ = null;
00215 }
00216
00217 if (currentDynamics_ == null) {
00218 try {
00219 org.omg.CORBA.Object obj =
00220 GrxCorbaUtil.getReference("DynamicsSimulatorFactory");
00221 DynamicsSimulatorFactory ifactory = DynamicsSimulatorFactoryHelper
00222 .narrow(obj);
00223 currentDynamics_ = ifactory.create();
00224 currentDynamics_._non_existent();
00225
00226
00227 } catch (Exception e) {
00228 GrxDebugUtil.printErr("getDynamicsSimulator: create failed.");
00229 currentDynamics_ = null;
00230 }
00231 }
00232 return currentDynamics_;
00233 }
00234
00235 public int getOperationMode() {
00236 return operationMode_;
00237 }
00238
00246 public boolean initDynamicsSimulator() {
00247 boolean ModelModified = false;
00248 Iterator<GrxModelItem> itr = currentModels_.iterator();
00249 while(itr.hasNext()){
00250 GrxModelItem model = itr.next();
00251 if(model.isModified()){
00252 ModelModified = true;
00253 if(!messageSkip_){
00254 final String name = model.getName();
00255 Display display = Display.getDefault();
00256 display.syncExec(new Runnable(){
00257 public void run(){
00258 MessageDialog.openInformation(null, "", MessageBundle.get("BehaviorManager.dialog.message.reloadModel")+name+") ");
00259 }
00260 });
00261 }
00262 }
00263 if(ModelModified) return false;
00264 }
00265 if(!itemChangeFlag_) return true;
00266 if(getDynamicsSimulator(true) == null) return false;
00267
00268 try {
00269
00270 for (int i=0; i<currentModels_.size(); i++) {
00271 final GrxModelItem model = currentModels_.get(i);
00272 BodyInfo bodyInfo = model.getBodyInfo();
00273 if(bodyInfo==null) return false;
00274 currentDynamics_.registerCharacter(model.getName(), bodyInfo);
00275 }
00276
00277 IntegrateMethod m = IntegrateMethod.EULER;
00278 currentDynamics_.init(0.005, m, SensorOption.ENABLE_SENSOR);
00279 currentDynamics_.setGVector(new double[] { 0.0, 0.0, 9.8});
00280
00281
00282 for (int i=0; i<currentModels_.size(); i++) {
00283 GrxModelItem model = currentModels_.get(i);
00284 if (model.links_ == null)
00285 continue;
00286
00287 GrxLinkItem base = model.rootLink();
00288 currentDynamics_.setCharacterLinkData(
00289 model.getName(), base.getName(), LinkDataType.ABS_TRANSFORM,
00290 model.getTransformArray(base));
00291
00292 currentDynamics_.setCharacterAllLinkData(
00293 model.getName(), LinkDataType.JOINT_VALUE,
00294 model.getJointValues());
00295 }
00296
00297
00298 Map<String, GrxModelItem> modelmap = (Map<String, GrxModelItem>)manager_.getItemMap(GrxModelItem.class);
00299 for (int i=0; i<currentCollisionPairs_.size(); i++) {
00300 GrxCollisionPairItem item = (GrxCollisionPairItem) currentCollisionPairs_.get(i);
00301 GrxModelItem m1 = modelmap.get(item.getStr("objectName1", ""));
00302 GrxModelItem m2 = modelmap.get(item.getStr("objectName2", ""));
00303 if (m1 == null || m2 == null) continue;
00304 Vector<GrxLinkItem> links1, links2;
00305 String lname1 = item.getStr("jointName1","");
00306 if (lname1.equals("")){
00307 links1 = m1.links_;
00308 }else{
00309 links1 = new Vector<GrxLinkItem>();
00310 GrxLinkItem l = m1.getLink(lname1);
00311 if (l != null) links1.add(l);
00312 }
00313 String lname2 = item.getStr("jointName2","");
00314 if (lname2.equals("")){
00315 links2 = m2.links_;
00316 }else{
00317 links2 = new Vector<GrxLinkItem>();
00318 GrxLinkItem l = m2.getLink(lname2);
00319 if (l != null) links2.add(l);
00320 }
00321 for (int j=0; j<links1.size(); j++){
00322 for (int k=0; k<links2.size(); k++){
00323 currentDynamics_.registerIntersectionCheckPair(
00324 m1.getName(), links1.get(j).getName(),
00325 m2.getName(), links2.get(k).getName(),
00326 links1.get(j).getDbl("tolerance",0.0)+links2.get(k).getDbl("tolerance",0.0));
00327 }
00328 }
00329 }
00330
00331 } catch (Exception e) {
00332 GrxDebugUtil.printErr("initDynamicsSimulator:", e);
00333 return false;
00334 }
00335 resolver_.setDynamicsSimulator(currentDynamics_);
00336 handler_.setInvKinemaResolver(resolver_);
00337 itemChangeFlag_ = false;
00338 return true;
00339 }
00340
00341 public void addClickListener( Grx3DViewClickListener listener ){
00342 behavior_.addClickListener( listener );
00343 }
00344 public void removeClickListener( Grx3DViewClickListener listener ){
00345 behavior_.removeClickListener( listener );
00346 }
00347
00348 private boolean setCharacterData(){
00349 if(currentCollisionPairs_.isEmpty() || currentModels_.isEmpty()) return false;
00350 if(!initDynamicsSimulator()) return false;
00351 if (currentDynamics_ == null) return false;
00352 for (int i=0; i<currentModels_.size(); i++) {
00353 GrxModelItem model = currentModels_.get(i);
00354 String name = model.getName();
00355 GrxLinkItem base = model.rootLink();
00356 double[] data = model.getTransformArray(base);
00357 currentDynamics_.setCharacterLinkData(name, base.getName(), LinkDataType.ABS_TRANSFORM, data);
00358 data = model.getJointValues();
00359 currentDynamics_.setCharacterAllLinkData(name, LinkDataType.JOINT_VALUE, data);
00360 }
00361 return true;
00362 }
00363
00371 public Collision[] getCollision() {
00372 if(!setCharacterData())
00373 return null;
00374 if (currentDynamics_.checkCollision(true)){
00375 WorldStateHolder wsH = new WorldStateHolder();
00376 currentDynamics_.getWorldState(wsH);
00377 return wsH.value.collisions;
00378 }else{
00379 return null;
00380 }
00381 }
00382
00390 public Distance[] getDistance() {
00391 if(!setCharacterData())
00392 return null;
00393 return currentDynamics_.checkDistance();
00394 }
00395
00403 public LinkPair[] getIntersection() {
00404 if(!setCharacterData())
00405 return null;
00406 return currentDynamics_.checkIntersection(true);
00407 }
00408
00409 public void setItem(List<GrxModelItem> models, List<GrxCollisionPairItem> cols){
00410 currentModels_ = models;
00411 currentCollisionPairs_ = cols;
00412 itemChangeFlag_ = true;
00413 }
00414
00415 public void destroyDynamicsSimulator(){
00416 try {
00417 currentDynamics_.destroy();
00418 } catch (Exception e) {
00419 GrxDebugUtil.printErr("getDynamicsSimulator: destroy failed.");
00420 }
00421 currentDynamics_ = null;
00422 }
00423
00424 public void setMessageSkip(boolean flg){
00425 messageSkip_ = flg;
00426 }
00427
00428 public void setItemChange(){
00429 itemChangeFlag_ = true;
00430 }
00431 }