ObjectRotationHandler.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  */
00017 package com.generalrobotix.ui.view.tdview;
00018 
00019 import java.awt.*;
00020 import java.awt.event.*;
00021 import javax.media.j3d.*;
00022 import javax.vecmath.*;
00023 
00024 
00025 import com.generalrobotix.ui.item.GrxLinkItem;
00026 import com.generalrobotix.ui.item.GrxModelItem;
00027 import com.generalrobotix.ui.view.Grx3DView;
00028 import com.sun.j3d.utils.picking.*;
00029 
00033 class ObjectRotationHandler extends OperationHandler {
00034     private static final float ROTATION_FACTOR = 0.006f;
00035 
00036     private TransformGroup tgTarget_;
00037     private Vector3f norm_;
00038     private Vector3f axis_;
00039     private Vector3f dir_ = new Vector3f();
00040     private Point prevPoint_ = new Point();
00041     private boolean isPicked_;
00042 
00043     //--------------------------------------------------------------------
00044     // BehaviorHandlerの実装
00045     public void processPicking(MouseEvent evt, BehaviorInfo info) {
00046         prevPoint_.x = evt.getPoint().x;
00047         prevPoint_.y = evt.getPoint().y;
00048 
00049         norm_ = null;
00050         isPicked_ = false;
00051 
00052         try {
00053                 info.pickCanvas.setShapeLocation(prevPoint_.x, prevPoint_.y);
00054             PickResult pickResult = info.pickCanvas.pickClosest();
00055             if (pickResult == null)
00056                 return;
00057             TransformGroup tg =
00058                 (TransformGroup)pickResult.getNode(PickResult.TRANSFORM_GROUP);
00059             GrxModelItem model = SceneGraphModifier.getModelFromTG(tg);
00060             if (model == null) 
00061                 return;
00062             else
00063                 tg = model.getTransformGroupRoot();
00064             if (tg == null)
00065                 return;
00066 
00067             if (_enableBoundingBox(tg, info)) {
00068                 isPicked_ = true;
00069                 Point3d startPoint = info.pickCanvas.getStartPosition();
00070                 PickIntersection intersection = pickResult.getClosestIntersection(startPoint);
00071                 norm_ = new Vector3f(intersection.getPointNormal());
00072             }
00073         } catch (CapabilityNotSetException ex) {
00074             // もう出ることはないと思うが、読み込むモデルによっては
00075             // 出るかもしれないので、スタックトレースは表示する。
00076             ex.printStackTrace();
00077             _disableBoundingBox();
00078         }
00079     }
00080 
00081     public void processStartDrag(MouseEvent evt, BehaviorInfo info) {
00082         if (isPicked_) {
00083             if (norm_ != null) {
00084                 // ターゲットの座標系から視点座標系への変換を求める。
00085                 Transform3D tr = new Transform3D();
00086                 Transform3D l2vw = new Transform3D();
00087                 Transform3D trTarget2View = new Transform3D();
00088          
00089                 tgTarget_.getLocalToVworld(l2vw);
00090                 tgTarget_.getTransform(tr);
00091                 trTarget2View.mul(l2vw, tr);
00092          
00093                 TransformGroup tgView = info.drawable.getTransformGroupRoot();
00094                 tgView.getLocalToVworld(l2vw);
00095                 tgView.getTransform(tr);
00096                 l2vw.mul(tr);
00097                 l2vw.invert();
00098          
00099                 l2vw.mul(trTarget2View);
00100                 trTarget2View.set(l2vw);
00101          
00102                 // マウスの動きをターゲットの座標系の動きに変換
00103                 float fdx = (float)(evt.getPoint().getX() - prevPoint_.getX());
00104                 float fdy = (float)(evt.getPoint().getY() - prevPoint_.getY());
00105                 Vector3f mouse = new Vector3f(fdx, - fdy, 0.0f);
00106                 Vector3f normal = new Vector3f();
00107          
00108                 trTarget2View.transform(norm_, normal);
00109          
00110                 float inner = normal.dot(mouse);
00111                 normal.scale(inner);
00112                 mouse.sub(normal);
00113          
00114                 trTarget2View.invert();
00115                 trTarget2View.transform(mouse);
00116                 mouse.normalize();
00117          
00118                 axis_ = _createAxisVector(mouse);
00119          
00120                 if (axis_ == null) {
00121                     System.out.println("axis is null");
00122                     isPicked_ = false;
00123                     _disableBoundingBox();
00124                     return;
00125                 }
00126          
00127                 dir_.cross(axis_, norm_);
00128             }
00129             evt.consume();
00130         }
00131     }
00132 
00133     public void processDragOperation(MouseEvent evt, BehaviorInfo info) {
00134         if (isPicked_) {
00135             // tgViewからみたマウスの軌跡ベクトルmouseを求める。
00136             // mouse-(norm_,mouse)norm_ がオブジェクトの移動ベクトル
00137             if (norm_ != null) {
00138                 // ターゲットの座標系から視点座標系への変換を求める。
00139                 Transform3D tr = new Transform3D();
00140                 Transform3D l2vw = new Transform3D();
00141                 Transform3D trTarget2View = new Transform3D();
00142             
00143                 tgTarget_.getLocalToVworld(l2vw);
00144                 tgTarget_.getTransform(tr);
00145                 trTarget2View.mul(l2vw, tr);
00146             
00147                 TransformGroup tgView = info.drawable.getTransformGroupRoot();
00148                 tgView.getLocalToVworld(l2vw);
00149                 tgView.getTransform(tr);
00150                 l2vw.mul(tr);
00151                 l2vw.invert();
00152             
00153                 l2vw.mul(trTarget2View);
00154                 trTarget2View.set(l2vw);
00155             
00156                 // マウスの動きをターゲットの座標系の動きに変換
00157                 float fdx = (float)(evt.getPoint().getX() - prevPoint_.getX());
00158                 float fdy = (float)(evt.getPoint().getY() - prevPoint_.getY());
00159                 Vector3f mouse = new Vector3f(fdx, - fdy, 0.0f);
00160                 Vector3f normal = new Vector3f();
00161             
00162                 trTarget2View.transform(norm_, normal);
00163             
00164                 float inner = normal.dot(mouse);
00165                 normal.scale(inner);
00166                 mouse.sub(normal);
00167             
00168                 trTarget2View.invert();
00169                 trTarget2View.transform(mouse);
00170             
00171                 float angle = ROTATION_FACTOR * mouse.dot(dir_);
00172                 tgTarget_.getTransform(l2vw);
00173                 tr.set(new AxisAngle4f(axis_, angle));
00174                 l2vw.mul(tr);
00175                 GrxLinkItem link = SceneGraphModifier.getLinkFromTG(tgTarget_);
00176                 link.setTransform(l2vw);
00177                 _transformChanged(info);
00178 
00179                 prevPoint_.x = evt.getPoint().x;
00180                 prevPoint_.y = evt.getPoint().y;
00181             }
00182             ((Grx3DView)info.drawable).showOption();
00183             evt.consume();
00184         }
00185     }
00186 
00187     public void processReleased(MouseEvent evt, BehaviorInfo info) {
00188         if (isPicked_) {
00189             evt.consume();
00190         }
00191     }
00192 
00193     public boolean processTimerOperation(BehaviorInfo info) {
00194         return true;
00195     }
00196 
00197     //--------------------------------------------------------------------
00198     // OperationHandlerの実装
00199     public void disableHandler() {
00200         _disableBoundingBox();
00201     }
00202 
00203     public void setPickTarget(TransformGroup tg, BehaviorInfo info) {
00204         if (tg != tgTarget_) {
00205             _enableBoundingBox(tg, info);
00206         }      
00207     }
00208 
00209     //--------------------------------------------------------------------
00210     // プライベートメソッド
00211     private void _disableBoundingBox() {
00212         if (tgTarget_ != null) {
00213             tgTarget_ = null;
00214             norm_ = null;
00215         }
00216     }
00217 
00218     private boolean _enableBoundingBox(TransformGroup tg, BehaviorInfo info) {
00219         GrxModelItem model = SceneGraphModifier.getModelFromTG(tg);
00220         if (model == null) 
00221                 return false; 
00222         
00223         tgTarget_ = tg;
00224         info.manager_.focusedItem(model);
00225         return true;
00226     }
00227 
00228     private Vector3f _createAxisVector(Vector3f mouse) {
00229         if (1.0f - Math.abs(norm_.x) < 0.000001) {
00230             if (Math.abs(mouse.y) < Math.abs(mouse.z)) {
00231                 if (mouse.y > 0.0f) {
00232                     return new Vector3f(0.0f, 1.0f, 0.0f);
00233                 } else {
00234                     return new Vector3f(0.0f, -1.0f, 0.0f);
00235                 }
00236             } else {
00237                 if (mouse.z > 0.0f) {
00238                     return new Vector3f(0.0f, 0.0f, 1.0f);
00239                 } else {
00240                     return new Vector3f(0.0f, 0.0f, -1.0f);
00241                 }
00242             }
00243         } else if (1.0f - Math.abs(norm_.y) < 0.000001) {
00244             if (Math.abs(mouse.x) < Math.abs(mouse.z)) {
00245                 if (mouse.x > 0.0f) {
00246                     return new Vector3f(1.0f, 0.0f, 0.0f);
00247                 } else {
00248                     return new Vector3f(-1.0f, 0.0f, 0.0f);
00249                 }
00250             } else {
00251                 if (mouse.z > 0.0f) {
00252                     return new Vector3f(0.0f, 0.0f, 1.0f);
00253                 } else {
00254                     return new Vector3f(0.0f, 0.0f, -1.0f);
00255                 }
00256             }
00257         } else if (1.0f - Math.abs(norm_.z) < 0.000001) {
00258             if (Math.abs(mouse.x) < Math.abs(mouse.y)) {
00259                 if (mouse.x > 0.0f) {
00260                     return new Vector3f(1.0f, 0.0f, 0.0f);
00261                 } else {
00262                     return new Vector3f(-1.0f, 0.0f, 0.0f);
00263                 }
00264             } else {
00265                 if (mouse.y > 0.0f) {
00266                     return new Vector3f(0.0f, 1.0f, 0.0f);
00267                 } else {
00268                     return new Vector3f(0.0f, -1.0f, 0.0f);
00269                 }
00270             }
00271         }
00272 
00273         System.out.println("norm=" + norm_);
00274         return null;
00275     }
00276 
00277     private void _transformChanged(BehaviorInfo info) {
00278         GrxModelItem model = SceneGraphModifier.getModelFromTG(tgTarget_);
00279             if (model == null) {
00280             System.out.println("can't get manipulatable.");
00281             return;
00282         }
00283             
00284         model.calcForwardKinematics();
00285         model.updateInitialTransformRoot();
00286     }
00287 }


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:17