JointRotationHandler.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 
00030 class JointRotationHandler extends OperationHandler {
00031     //--------------------------------------------------------------------
00032     // 定数
00033     private static final int MODE_NONE     = 0;
00034     private static final int DISK_MODE     = 1;
00035     private static final int CYLINDER_MODE = 2;
00036     private static final float FACTOR = 0.004f;
00037 
00038     // 物体のVectorと視点のVectorがこの角度以下の時CYLINDER_MODEに移行する
00039     private static final double THRESHOLD = 0.262f;
00040 
00041     //--------------------------------------------------------------------
00042     // インスタンス変数
00043     private int mode_;
00044     private TransformGroup tgTarget_;
00045     private Point prevPoint_ = new Point();
00046     private double angle_;
00047     private boolean isPicked_;
00048     private Vector3d vectorCylinder_;
00049 
00050     // DISK モードの時に使用する
00051     //protected Point3f point000 = new Point3f(0,0,0);
00052     private Point3d pointTarget_;
00053 
00054     //--------------------------------------------------------------------
00055     // BehaviorHandlerの実装
00056     public void processPicking(MouseEvent evt, BehaviorInfo info) {
00057         prevPoint_.x = evt.getPoint().x;
00058         prevPoint_.y = evt.getPoint().y;
00059 
00060         isPicked_ = false;
00061 
00062         try {
00063             info.pickCanvas.setShapeLocation(prevPoint_.x, prevPoint_.y);
00064             PickResult pickResult[] = info.pickCanvas.pickAllSorted();
00065             if (pickResult == null) {
00066                 return;
00067             }
00068             TransformGroup tg = (TransformGroup)pickResult[0].getNode(
00069                 PickResult.TRANSFORM_GROUP
00070             );
00071             GrxModelItem model = SceneGraphModifier.getModelFromTG(tg);
00072             if (model == null) 
00073                 return;
00074             else{
00075                 if(info.manager_.focusedItem()==model){
00076                         if( pickResult.length > 1)
00077                                 tg = (TransformGroup)pickResult[1].getNode(
00078                                     PickResult.TRANSFORM_GROUP );
00079                         else
00080                                 return;
00081                 }
00082             }
00083             setPickTarget(tg, info);
00084         } catch (CapabilityNotSetException ex) {
00085             ex.printStackTrace();
00086         }
00087 
00088         isPicked_ = true;
00089         //evt.consume();
00090     }
00091 
00092     public void processStartDrag(MouseEvent evt, BehaviorInfo info) {
00093         if (!isPicked_) {
00094             mode_ = MODE_NONE;
00095             return;
00096         }
00097         //Press されたら視点と物体の角度を取得し現在のモードを判定する
00098          // ターゲット座標系からワールド座標系への変換
00099         Transform3D target2vw = new Transform3D();
00100         Transform3D l2vw = new Transform3D();
00101         Transform3D tr = new Transform3D();
00102         tgTarget_.getLocalToVworld(l2vw);
00103         tgTarget_.getTransform(tr);
00104         target2vw.mul(l2vw, tr);
00105 
00106         // ターゲットの原点のワールド座標系での座標を求める。
00107         pointTarget_ = new Point3d();
00108         target2vw.transform(pointTarget_);
00109 
00110         // ワールド座標系から視点座標系への変換
00111         Transform3D vw2view = new Transform3D();
00112         tr = new Transform3D();
00113         l2vw = new Transform3D();
00114         TransformGroup tgView = info.drawable.getTransformGroupRoot();
00115         tgView.getLocalToVworld(l2vw);
00116         tgView.getTransform(tr);
00117         vw2view.mul(l2vw, tr);
00118         vw2view.invert();
00119         
00120         // (0,0,0) (0,0,1) の point を作成し、上で作成した
00121         // vw2view target2vw を使って変換する
00122         Point3f point000 = new Point3f(0,0,0);
00123         //軸情報取り出す
00124 
00125         GrxLinkItem l = SceneGraphModifier.getLinkFromTG(tgTarget_);
00126         if(l.jointAxis_==null)
00127                 return;
00128   
00129         vw2view.mul(target2vw);
00130         vw2view.transform(point000);
00131         Vector3d vectorView = new Vector3d(point000);
00132         vectorCylinder_ = new Vector3d(l.jointAxis_);
00133         vw2view.transform(vectorCylinder_);
00134         
00135         // 二つの Vector で angle の角度を得て diskAngle と
00136         // 比較しモードを設定
00137         double angle = vectorView.angle(vectorCylinder_);
00138         
00139         if(angle == Double.NaN) {
00140             System.err.println("無効な値が入りました");
00141         }
00142         // 角度が 90 以上の時のことも考えて if で分ける
00143         if (angle > Math.PI / 2.0) {
00144             // 両端に DISK_MODE の状態になる範囲があるので大きい
00145             // ほうの範囲を小さいほうへ移動する計算
00146             angle = Math.PI - angle;
00147         }
00148         
00149         if (angle < THRESHOLD) {
00150             mode_ = DISK_MODE;
00151         } else {
00152             mode_ = CYLINDER_MODE;
00153         }
00154     }
00155 
00156     public void processDragOperation(MouseEvent evt, BehaviorInfo info) {
00157 //        if (bbSwitch_ == null) {
00158 //            return;
00159 //        }
00160 
00161         Vector2d mouseMove = new Vector2d(
00162             FACTOR * (evt.getPoint().getX() - prevPoint_.getX()),
00163             FACTOR * (evt.getPoint().getY() - prevPoint_.getY())
00164         );
00165 
00166         angle_ = 0.0;  // 移動量を定義するためのラジアン
00167         // 現在のモードに応じてマウスの動きを回転に直す
00168         switch (mode_) {
00169         case DISK_MODE:
00170             Point2d pointMouseOnPlane = new Point2d();
00171             //Point3d pointTemp = new Point3d(point000);
00172             Point3d pointTemp = new Point3d(pointTarget_);
00173 
00174             Canvas3D canvas = info.pickCanvas.getCanvas();
00175             Transform3D vw2imagePlate = new Transform3D();
00176             canvas.getVworldToImagePlate(vw2imagePlate);
00177             vw2imagePlate.transform(pointTemp);
00178             canvas.getPixelLocationFromImagePlate(pointTemp, pointMouseOnPlane);
00179 
00180             Vector2d prev = new Vector2d(
00181                 prevPoint_.getX() - pointMouseOnPlane.x,
00182                 prevPoint_.getY() - pointMouseOnPlane.y
00183             );
00184 
00185             Vector2d current = new Vector2d(
00186                 evt.getPoint().getX() - pointMouseOnPlane.x,
00187                 evt.getPoint().getY() - pointMouseOnPlane.y
00188             );
00189 
00190             // 画面上の座標は下に行くほど y の値が逆転していくので
00191             // y の値を逆転させておく。
00192             angle_ = prev.angle(current);
00193 
00194             Vector3d cross = new Vector3d();
00195             cross.cross(vectorCylinder_, new Vector3d(prev.x, prev.y, 0.0));
00196 
00197             if (mouseMove.dot(new Vector2d(cross.x, cross.y)) > 0.0) {
00198                 angle_ = - angle_;
00199             }
00200             // 画面のどちらが我がシリンダーの表面を向いているかで
00201             // 動作が反転する
00202 /*
00203             if (vectorCylinder_.z < 0) {
00204                 angle_ = - angle_;
00205             }
00206 */
00207             break;
00208         case CYLINDER_MODE:
00209             // x,y 平面において軸方向と直行する方向のマウスの
00210             // 動作だけをシリンダーの回転にする
00211             // fDotProduct <- 内積 InnerProduct ともいう
00212             // 半時計回りに 90 度回転するので 4x4 の変換行列を
00213             // かましてやった値をつかって直行するベクトルを出す
00214             // マウスに対する回転量を DISK_MODE の二倍にする
00215             mouseMove.x *= -2.0;
00216             mouseMove.y *= 2.0;
00217 
00218             //vectorCylinder_.normalize();
00219             Vector2d vectorCylinder =
00220                 new Vector2d(- vectorCylinder_.y, vectorCylinder_.x);
00221 
00222             // このベクトルと直行する Vector を求める
00223             vectorCylinder.normalize();
00224             angle_ = vectorCylinder.dot(mouseMove);
00225             break;
00226         case MODE_NONE:
00227             return;
00228         }
00229 
00230         prevPoint_.x = evt.getPoint().x;
00231         prevPoint_.y = evt.getPoint().y;
00232         _jointAngleChanged(info);
00233         ((Grx3DView)info.drawable).showOption();
00234         
00235         evt.consume();
00236     }
00237 
00238     public void processReleased(MouseEvent evt, BehaviorInfo info) {
00239 //        if (bbSwitch_ != null) {
00240             evt.consume();
00241 //        }
00242     }
00243 
00244     public boolean processTimerOperation(BehaviorInfo info) {
00245         return true;
00246     }
00247 
00248     //--------------------------------------------------------------------
00249     // OperationHandlerの実装
00250     public void disableHandler() {
00251     }
00252 
00253     public void setPickTarget(TransformGroup tg, BehaviorInfo info) {
00254         if (tg != tgTarget_) {
00255             GrxLinkItem l = SceneGraphModifier.getLinkFromTG(tg);
00256             if (l == null) return;
00257             info.manager_.focusedItem(l);
00258                 tgTarget_ = l.tg_;
00259         }
00260     }
00261 
00262     private void _jointAngleChanged(BehaviorInfo info) {
00263         try {
00264             GrxModelItem model = SceneGraphModifier.getModelFromTG(tgTarget_);
00265             GrxLinkItem link = SceneGraphModifier.getLinkFromTG(tgTarget_);
00266             if( link.jointType_.equals("rotate") ||  link.jointType_.equals("slide") ){
00267                     link.jointValue(link.jointValue_+angle_);
00268                     link.setJointValuesWithinLimit();
00269                     model.updateInitialJointValue(link);
00270                     model.calcForwardKinematics();
00271             }
00272         } catch (Exception e) { 
00273                 e.printStackTrace();
00274         }
00275     }
00276 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:55