ObjectFittingHandler.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.util.*;
00020 import java.awt.*;
00021 import java.awt.event.*;
00022 import javax.media.j3d.*;
00023 import javax.vecmath.*;
00024 
00025 
00026 import com.generalrobotix.ui.item.GrxModelItem;
00027 import com.generalrobotix.ui.view.Grx3DView;
00028 import com.sun.j3d.utils.picking.*;
00029 
00030 class ObjectFittingHandler extends OperationHandler {
00031     //--------------------------------------------------------------------
00032     // 定数
00033     public static final int FITTING_FROM  = 1;
00034     public static final int FITTING_TO   = 2;
00035 
00036     //--------------------------------------------------------------------
00037     // インスタンス変数
00038     private FittingInfo fittingInfoFrom_;
00039     private FittingInfo fittingInfoTo_;
00040 
00041     private int mode_;
00042     private boolean bHaveFirst = false;
00043     private boolean bHaveSecond = false;
00044     private Point3f intersectVW_;
00045     private Point3d[] verticesVW_;
00046 
00047     //--------------------------------------------------------------------
00048     // コンストラクタ
00049     public ObjectFittingHandler () {
00050         fittingInfoFrom_ = new FittingInfo(
00051             new Color3f(1.0f,0.0f,0.0f),
00052             new Color3f(0.0f,1.0f,0.0f),
00053             false
00054         );
00055 
00056         fittingInfoTo_ = new FittingInfo(
00057             new Color3f(0.0f,0.0f,1.0f),
00058             new Color3f(0.0f,1.0f,0.0f),
00059             true
00060         );
00061     }
00062 
00063     //--------------------------------------------------------------------
00064     // 公開メソッド
00065     void setFittingMode(int mode) {
00066         mode_ = mode;
00067     }
00068 
00072     public boolean fit(BehaviorInfo info) {
00073         if (!bHaveSecond) {
00074             System.err.println("二番目の TG が選択されていません");
00075             return false;
00076         }
00077         if (!bHaveFirst) {
00078             System.err.println("一番目の TG が選択されていません");
00079             return false;
00080         }
00081 
00082         // すでに二つの物体が選択されていたらそれをくっつける
00083 
00084         // 二つのベクトルの Angle を求め、二つのベクトルの外積方向を
00085         // 軸にその angle 回転させる
00086         // その情報を t3dRotate へ入れる
00087         Vector3f v3fCross = new Vector3f();
00088         Transform3D t3dRotate = new Transform3D();
00089         Vector3f v3fNormal = fittingInfoFrom_.getNormalVector();
00090         Vector3f v3fNormalSecond = fittingInfoTo_.getNormalVector();
00091         v3fCross.cross(v3fNormal,v3fNormalSecond);
00092         // 同方向かどうかのテスト部分
00093         if (v3fNormal.angle(v3fNormalSecond) == 0.0f) {
00094             // 角度が 0 度の時の対策。もし要素のすべての符号が
00095             // いっしょなら同じ方向のベクトルだとみなす
00096             if (_isSameSignVector3f(v3fNormal, v3fNormalSecond)) {
00097                 // v3fCross に外積が入らないので AxisAngle は使えない
00098                 Vector3f vecXAxis =
00099                     new Vector3f(new Point3f(1.0f,0.0f,0.0f));
00100                 Vector3f vecYAxis =
00101                     new Vector3f(new Point3f(0.0f,1.0f,0.0f));
00102                 if (v3fNormal.x != -1.0d) {
00103                     v3fCross.cross(vecXAxis,v3fNormal);
00104                     t3dRotate.set(new AxisAngle4f(v3fCross,(float)Math.PI));
00105                 } else {
00106                     v3fCross.cross(vecYAxis,v3fNormal);
00107                     t3dRotate.set(new AxisAngle4f(v3fCross,(float)Math.PI));
00108                 }
00109             }
00110         } else {
00111             t3dRotate.set(
00112                 new AxisAngle4f(
00113                     v3fCross,
00114                     (float)Math.PI + v3fNormal.angle(v3fNormalSecond)
00115                 )
00116             );
00117         }
00118 
00119         // 現在の Transform3D を生成し t3dCur へ入れる
00120         Transform3D t3dCur = new Transform3D();
00121 
00122         GrxModelItem model = SceneGraphModifier.getModelFromTG(fittingInfoFrom_.getTransformGroup());
00123         if (model == null) return false;
00124         TransformGroup tgCur = model.getTransformGroupRoot();
00125         tgCur.getTransform(t3dCur);
00126 
00127         // 移動させる側の法線のスタートポイントへ回転処理をかけた場合の
00128         // 回転後の位置を p3dIntersect へ入れる
00129         Point3f p3dIntersect = fittingInfoFrom_.getIntersectPoint();
00130         Point3f p3dIntersectSecond = fittingInfoTo_.getIntersectPoint();
00131         t3dRotate.transform(p3dIntersect);
00132         // 移動させる処理
00133         Point3d p3dMove = new Point3d();
00134         p3dMove.x = p3dIntersectSecond.x - p3dIntersect.x;
00135         p3dMove.y = p3dIntersectSecond.y - p3dIntersect.y;
00136         p3dMove.z = p3dIntersectSecond.z - p3dIntersect.z;
00137         Vector3f v3fTranslate = new Vector3f(p3dMove);
00138         Transform3D t3dTranslate = new Transform3D();
00139 
00140         // Report された TG のぶんまでの LocalToVworld ぶんを逆変換する
00141         Transform3D t3dLocalToVworld = new Transform3D();
00142         Transform3D t3dCurrent = new Transform3D();
00143         tgCur.getLocalToVworld(t3dLocalToVworld);
00144         tgCur.getTransform(t3dCurrent);
00145 
00146         t3dLocalToVworld.mul(t3dCurrent);
00147         t3dLocalToVworld.invert();
00148 
00149         t3dTranslate.set(v3fTranslate);
00150 
00151         // Behavior に変更を任せる
00152         // 実際に Transform3D を multiply していく
00153         t3dRotate.mul(t3dCur);
00154         t3dTranslate.mul(t3dRotate);
00155 
00156         AxisAngle4f axis = new AxisAngle4f();
00157         Quat4f quat = new Quat4f();
00158         t3dTranslate.get(quat);
00159         t3dTranslate.get(v3fTranslate);
00160         axis.set(quat);
00161 
00162         model.setTransformRoot(t3dTranslate);
00163 
00164         // 別クラスに出す場合は broadcast する
00165         _transformChanged(info, tgCur);
00166         // この Behavior で TG を変更する場合
00167 
00168         // 処理が終わったら二つの表示用の線分を見えなくする
00169         fittingInfoFrom_.removeForDisplay();
00170         fittingInfoTo_.removeForDisplay();
00171 
00172         // くっつける処理が終わったら元の何も選択されていない状態に戻す
00173         bHaveFirst = false;
00174         bHaveSecond = false;
00175         fittingInfoFrom_.setTransformGroup(null);
00176         fittingInfoTo_.setTransformGroup(null);
00177         ((Grx3DView)info.drawable).showOption();
00178         return true;
00179     }
00180  
00181     //--------------------------------------------------------------------
00182     // BehaviorHandlerの実装
00183     public void processPicking(MouseEvent evt, BehaviorInfo info) {
00184         Point mouse = evt.getPoint();
00185 
00186         try {
00187             info.pickCanvas.setShapeLocation(mouse.x, mouse.y);
00188             
00189             PickResult pickResult[] = info.pickCanvas.pickAllSorted();
00190             if (pickResult == null) {
00191                 return;
00192             }
00193             TransformGroup tg = (TransformGroup)pickResult[0].getNode(PickResult.TRANSFORM_GROUP);
00194             Point3d startPoint = info.pickCanvas.getStartPosition();
00195             PickIntersection intersection = pickResult[0].getClosestIntersection(startPoint);
00196             GrxModelItem model = SceneGraphModifier.getModelFromTG(tg);
00197             if (model == null) 
00198                 return;
00199             else{
00200                 if(info.manager_.focusedItem()==model){
00201                         if( pickResult.length > 1){
00202                                 tg = (TransformGroup)pickResult[1].getNode(PickResult.TRANSFORM_GROUP );
00203                                 intersection = pickResult[1].getClosestIntersection(startPoint);
00204                         info.manager_.focusedItem(null);
00205                         }else
00206                                 return;
00207                 }
00208             }
00209             
00210             intersectVW_ = new Point3f(intersection.getPointCoordinatesVW());
00211             verticesVW_ = intersection.getPrimitiveCoordinatesVW();
00212             _enableIndicator(tg, info);
00213         } catch (CapabilityNotSetException ex) {
00214             // もう出ることはないと思うが、読み込むモデルによっては
00215             // 出るかもしれないので、スタックトレースは表示する。
00216             ex.printStackTrace();
00217             _disableIndicator();
00218         }
00219     }
00220 
00221     public void processStartDrag(MouseEvent evt, BehaviorInfo info) {}
00222     public void processDragOperation(MouseEvent evt, BehaviorInfo info) {}
00223     public void processReleased(MouseEvent evt, BehaviorInfo info) {}
00224     public boolean processTimerOperation(BehaviorInfo info) {
00225         return true;
00226     }
00227 
00228     //--------------------------------------------------------------------
00229     // OperationHandlerの実装
00230     public void disableHandler() {
00231         _disableIndicator();
00232     }
00233 
00234     public void setPickTarget(TransformGroup tg, BehaviorInfo info) {}
00235 
00236     //--------------------------------------------------------------------
00237     // プライベートメソッド
00238     private void _enableIndicator(TransformGroup tg, BehaviorInfo info) {
00239         // ObjectTranslationBehavior と ObjectRotationBehavior の場合は
00240         // BoundingBox の方向が pick されたのか
00241         // 判定して Behavior にモードを設定します
00242 
00243         String strFirst = null;
00244         String strSecond = null;
00245         Hashtable<String, Object> tgInfo;
00246         switch (mode_) {
00247         case FITTING_FROM:
00248             if (tg == fittingInfoFrom_.getArrowTransformGroup()) {
00249                 bHaveFirst = false;
00250                 fittingInfoFrom_.setTransformGroup(null);
00251                 fittingInfoFrom_.removeForDisplay();
00252                 return;
00253             }
00254 
00255             tgInfo = SceneGraphModifier.getHashtableFromTG(tg);
00256             if (tgInfo == null) return;
00257             GrxModelItem model = SceneGraphModifier.getModelFromTG(tg);
00258             strFirst = model.getName();
00259 
00260             tgInfo = SceneGraphModifier.getHashtableFromTG(
00261                 fittingInfoTo_.getTransformGroup()
00262             );
00263             model = SceneGraphModifier.getModelFromTG(fittingInfoTo_.getTransformGroup());
00264             if (model != null) {
00265                 strSecond = model.getName();
00266             }
00267 
00268             if (strFirst.equals(strSecond)) return;
00269 
00270             fittingInfoFrom_.setPickable(false);
00271             fittingInfoFrom_.setIntersectPoint(intersectVW_);
00272             fittingInfoFrom_.setPrimitiveCoordinates(verticesVW_);
00273             fittingInfoFrom_.setTransformGroup(tg);
00274 
00275             // 選択された面の情報を表示する
00276             fittingInfoFrom_.addForDisplay();
00277 
00278             bHaveFirst = true;
00279             break;
00280         case FITTING_TO:
00281             if (tg == fittingInfoTo_.getArrowTransformGroup()) {
00282                 bHaveSecond = false;
00283                 fittingInfoTo_.setTransformGroup(null);
00284                 fittingInfoTo_.removeForDisplay();
00285                 return;
00286             }
00287 
00288             tgInfo = SceneGraphModifier.getHashtableFromTG(tg);
00289             if (tgInfo == null) 
00290                 return;
00291             model = SceneGraphModifier.getModelFromTG(tg);
00292             strSecond = model.getName();
00293 
00294             tgInfo = SceneGraphModifier.getHashtableFromTG(
00295                 fittingInfoFrom_.getTransformGroup()
00296             );
00297             model = SceneGraphModifier.getModelFromTG(fittingInfoFrom_.getTransformGroup());
00298             if (model != null) {
00299                 strFirst = model.getName();
00300             }
00301             if (strSecond.equals(strFirst)) 
00302                 return;
00303 
00304             fittingInfoTo_.setPickable(false);
00305             fittingInfoTo_.setIntersectPoint(intersectVW_);
00306             fittingInfoTo_.setPrimitiveCoordinates(verticesVW_);
00307             fittingInfoTo_.setTransformGroup(tg);
00308 
00309             fittingInfoTo_.addForDisplay();
00310 
00311             bHaveSecond = true;
00312             break;
00313         }
00314     }
00315 
00316     private void _disableIndicator() {
00317         if (bHaveFirst) {
00318             bHaveFirst = false;
00319             fittingInfoFrom_.removeForDisplay();
00320             fittingInfoFrom_.setTransformGroup(null);
00321         }
00322 
00323         if (bHaveSecond) {
00324             bHaveSecond = false;
00325             fittingInfoTo_.removeForDisplay();
00326             fittingInfoTo_.setTransformGroup(null);
00327         }
00328     }
00329 
00330     private boolean _isSameSign(float x, float y) {
00331        return (((x >= 0) || (y >= 0)) && ((x <= 0) || (y <= 0)));
00332     }
00333 
00334     private boolean _isSameSignVector3f(Vector3f v1, Vector3f v2) {
00335        return (
00336            _isSameSign(v1.x, v2.x) &&
00337            _isSameSign(v1.y, v2.y) &&
00338            _isSameSign(v1.z, v2.z)
00339        );
00340     }
00341 
00342     private void _transformChanged(BehaviorInfo info, TransformGroup tg) {
00343         GrxModelItem model = SceneGraphModifier.getModelFromTG(tg);
00344         if (model == null) {
00345             System.out.println("no manipulatable.");
00346             return;
00347         }
00348         model.calcForwardKinematics();
00349         model.updateInitialTransformRoot();
00350     }
00351 }


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