InvKinemaHandler.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 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 InvKinemaHandler extends OperationHandler {
00031     //--------------------------------------------------------------------
00032     // 定数
00033     public static final int FROM_MODE        = 1;
00034     public static final int ROTATION_MODE    = 2;
00035     public static final int TRANSLATION_MODE = 3;
00036 
00037     public static final int UNKNOWN = 0;
00038     public static final int ROTATION_WITH_X = 1;
00039     public static final int ROTATION_WITH_Y = 2;
00040     public static final int ROTATION_WITH_Z = 4;
00041     public static final int AXIS_FLAGS = 7;
00042     public static final int MINUS = 8;
00043 
00044     public static final int XY_TRANSLATION = 1;
00045     public static final int YZ_TRANSLATION = 2;
00046     public static final int ZX_TRANSLATION = 4;
00047 
00048     private static final float ROTATION_FACTOR    = (float)Math.PI / 360.0f;
00049     private static final float ROTATION_LIMIT     = (float)Math.PI / 360.0f;
00050     private static final float TRANSLATION_FACTOR = 0.001f;
00051     private static final float TRANSLATION_LIMIT  = 0.003f;
00052     
00053     //--------------------------------------------------------------------
00054     // インスタンス変数
00055     protected int rotationMode_ = UNKNOWN;
00056     protected int translationMode_ = UNKNOWN;
00057     
00058     private float limit_;
00059 
00061     protected Vector3f v3fAnotherAxis = new Vector3f();
00062     protected Vector3f v3fAxisRotate = new Vector3f();
00063     
00065     //protected Transform3D t3dWorld = new Transform3D();
00066     //protected Transform3D t3dView = new Transform3D();
00067     
00069     //protected Vector2f v2fAnotherAxis;
00070     protected Vector2f v2fDetermine;
00071 
00073     protected Vector3f v3fAxisFirst = new Vector3f();
00075     protected Vector3f v3fAxisSecond = new Vector3f();
00076 
00080     protected Point3f point000 = new Point3f(0,0,0);
00081 
00082     private Vector3f normal_;
00083     private Point3f intersect_;
00084 
00085     private Switch bbSwitchFrom_;
00086     private Switch bbSwitchTo_;
00087 
00088     //private boolean bBoundsSecond;
00089 
00090     private TransformGroup tgTarget_;
00091 
00092     private int mode_;
00093     private Point startPoint_ = new Point();
00094     private Point point_ = new Point();
00095 
00096     private InvKinemaResolver resolver_;
00097 
00098     private boolean isPicked_;
00099 
00100     //--------------------------------------------------------------------
00101     // 公開メソッド
00102     public void setInvKinemaMode(int mode) {
00103         mode_ = mode;
00104     }
00105 
00106     public void setInvKinemaResolver(InvKinemaResolver resolver) {
00107         resolver_ = resolver;
00108     }
00109 
00110     //--------------------------------------------------------------------
00111     // BehaviorHandlerの実装
00112     public void processPicking(MouseEvent evt, BehaviorInfo info) {
00113         startPoint_.x = evt.getPoint().x;
00114         startPoint_.y = evt.getPoint().y;
00115 
00116         intersect_ = null;
00117         normal_ = null;
00118         tgTarget_ = null;
00119         isPicked_ = false;
00120 
00121         info.pickCanvas.setShapeLocation(startPoint_.x, startPoint_.y);
00122         PickResult pickResult[] = info.pickCanvas.pickAllSorted();
00123         if (pickResult == null) {
00124             return;
00125         }
00126         TransformGroup tg = (TransformGroup)pickResult[0].getNode(PickResult.TRANSFORM_GROUP);
00127         Point3d startPoint = info.pickCanvas.getStartPosition();
00128         PickIntersection intersection = pickResult[0].getClosestIntersection(startPoint);
00129         GrxModelItem model = SceneGraphModifier.getModelFromTG(tg);
00130         if (model == null) 
00131                 return;
00132         else{
00133                 if(info.manager_.focusedItem()==model){
00134                         if( pickResult.length > 1){
00135                         tg = (TransformGroup)pickResult[1].getNode(PickResult.TRANSFORM_GROUP );
00136                         intersection = pickResult[1].getClosestIntersection(startPoint);
00137                         info.manager_.focusedItem(null);
00138                         }else
00139                                 return;
00140                 }
00141         }
00142 
00143         intersect_ = new Point3f(intersection.getPointCoordinates());
00144         normal_ = new Vector3f(intersection.getPointNormal());
00145         if (_setInvKinema(tg, info)) {
00146             isPicked_ = true;
00147             // ピックが成功してもconsume()しなければ、ViewHandlerが
00148             // ピック処理をするので、視点の回転中心が設定される。
00149             //evt.consume();
00150         } else {
00151             intersect_ = null;
00152                 normal_ = null;
00153         }
00154     }
00155 
00156     public void processStartDrag(MouseEvent evt, BehaviorInfo info) {
00157         if (isPicked_) {
00158             switch (mode_) {
00159             case FROM_MODE:
00160                 break;
00161             case ROTATION_MODE:
00162                 _decideRotationAxis(evt, info);
00163                 limit_ = ROTATION_LIMIT;
00164                 break;
00165             case TRANSLATION_MODE:
00166                 _decideTranslationAxis(evt, info);
00167                 limit_ = TRANSLATION_LIMIT;
00168                 break;
00169             default:
00170                 break;
00171             }
00172             evt.consume();
00173         }
00174     }
00175 
00176     public void processDragOperation(MouseEvent evt, BehaviorInfo info) {
00177         if (isPicked_) {
00178             switch (mode_) {
00179             case FROM_MODE:
00180                 break;
00181             case ROTATION_MODE:
00182             case TRANSLATION_MODE:
00183                 point_.x = evt.getPoint().x;
00184                 point_.y = evt.getPoint().y;
00185                 break;
00186             default:
00187                 break;
00188             }
00189             evt.consume();
00190         }
00191     }
00192 
00193     public void processReleased(MouseEvent evt, BehaviorInfo info) {
00194         if (isPicked_) {
00195             evt.consume();
00196         }
00197     }
00198 
00199     public boolean processTimerOperation(BehaviorInfo info) {
00200         switch (mode_) {
00201         case FROM_MODE:
00202             break;
00203         case ROTATION_MODE:
00204             _rotation(info);
00205             break;
00206         case TRANSLATION_MODE:
00207             _translation(info);
00208             break;
00209         default:
00210             break;
00211         }
00212         ((Grx3DView)info.drawable).showOption();
00213         return true;
00214     }
00215 
00216     //--------------------------------------------------------------------
00217     // OperationHandlerの実装
00218     public void disableHandler() {
00219         _disableBoundingBox();
00220     }
00221 
00222     public void setPickTarget(TransformGroup tg, BehaviorInfo info) {
00223         switch(mode_) {
00224         case FROM_MODE:
00225             _disableBoundingBox();
00226             _enableBoundingBoxFrom(tg);
00227             break;
00228         case ROTATION_MODE:
00229         case TRANSLATION_MODE:
00230             if (bbSwitchFrom_ == null) {
00231                 return;
00232             }
00233 
00234             if (tgTarget_ != tg) {
00235                 _enableBoundingBoxTo(tg);
00236             }
00237             break;
00238         }
00239     }
00240 
00241     //--------------------------------------------------------------------
00242     // プライベートメソッド
00243     private void _disableBoundingBox() {
00244         if (bbSwitchFrom_ != null) {
00245             bbSwitchFrom_.setWhichChild(Switch.CHILD_NONE);
00246         }
00247 
00248         if (bbSwitchTo_ != null) {
00249             bbSwitchTo_.setWhichChild(Switch.CHILD_NONE);
00250         }
00251 
00252         tgTarget_ = null;
00253         bbSwitchFrom_ = null;
00254         bbSwitchTo_ = null;
00255     }
00256 
00257     private boolean _setInvKinema(TransformGroup tg, BehaviorInfo info) {
00258         switch(mode_) {
00259         case FROM_MODE:
00260             _disableBoundingBox();
00261             if (!_enableBoundingBoxFrom(tg)) {
00262                 return false;
00263             }
00264             break;
00265         case ROTATION_MODE:
00266             if (bbSwitchFrom_ == null) {
00267                 return false;
00268             }
00269 
00270             if (tgTarget_ != tg) {
00271                 if (!_enableBoundingBoxTo(tg)) {
00272                     return false;
00273                 }
00274             }
00275             _setRotationMode();
00276             break;
00277         case TRANSLATION_MODE:
00278             if (bbSwitchFrom_ == null) {
00279                 return false;
00280             }
00281 
00282             if (tgTarget_ != tg) {
00283                 if (!_enableBoundingBoxTo(tg)) {
00284                     return false;
00285                 }
00286             }
00287             _setTranslationMode();
00288             break;
00289         default:
00290             return false;
00291         }
00292         return true;
00293     }
00294 
00295     private boolean _enableBoundingBoxFrom(TransformGroup tg) {
00296         GrxModelItem model = SceneGraphModifier.getModelFromTG(tg);
00297         if (model == null) {
00298             return false;
00299         }
00300         
00301         GrxLinkItem link = SceneGraphModifier.getLinkFromTG(tg);
00302         if (link == null) {
00303             return false;
00304         }
00305         
00306         resolver_.setFromJoint(model, link);
00307         bbSwitchFrom_ = link.getBBSwitch();
00308         bbSwitchFrom_.setWhichChild(Switch.CHILD_ALL);
00309         return true;
00310      }
00311 
00312     private boolean _enableBoundingBoxTo(TransformGroup tg) {
00313         if (bbSwitchTo_ != null) {
00314             bbSwitchTo_.setWhichChild(Switch.CHILD_NONE);
00315         }
00316         GrxLinkItem link = SceneGraphModifier.getLinkFromTG(tg);
00317         GrxModelItem model = SceneGraphModifier.getModelFromTG(tg);
00318         bbSwitchTo_ = link.getBBSwitch();
00319         // fromジョイントと同じジョイントをPickした場合
00320         if (bbSwitchFrom_ == bbSwitchTo_) {
00321             bbSwitchTo_ = null;
00322             return false;
00323         }
00324         if (bbSwitchTo_ != null && resolver_.setToJoint(model, link)) {
00325             bbSwitchTo_.setWhichChild(Switch.CHILD_ALL);
00326             tgTarget_ = tg;
00327             return true;
00328         } else {
00329             return false;
00330         }
00331     }
00332 
00333     private void _setRotationMode() {
00334         if (Math.abs(normal_.x) == 1.0f) {
00335             rotationMode_ = ROTATION_WITH_X;
00336         } else if(Math.abs(normal_.y) == 1.0f) {
00337             rotationMode_ = ROTATION_WITH_Y;
00338         } else if(Math.abs(normal_.z) == 1.0f) {
00339             rotationMode_ = ROTATION_WITH_Z;
00340         }
00341     }
00342 
00343     private void _setTranslationMode() {
00344         if (Math.abs(normal_.x) == 1.0f) {
00345             translationMode_ = YZ_TRANSLATION;
00346         } else if(Math.abs(normal_.y) == 1.0f) {
00347             translationMode_ = ZX_TRANSLATION;
00348         } else if (Math.abs(normal_.z) == 1.0f) {
00349             translationMode_ = XY_TRANSLATION;
00350         }
00351     }
00352 
00353     private void _decideTranslationAxis(MouseEvent evt, BehaviorInfo info) {
00354         // 現在のモードにしたがった内積を取る対象の二軸の設定をします
00355         switch(translationMode_) {
00356         case XY_TRANSLATION:
00357             v3fAxisFirst.set(1.0f,0.0f,0.0f);
00358             v3fAxisSecond.set(0.0f,1.0f,0.0f);
00359             break;
00360         case YZ_TRANSLATION:
00361             v3fAxisFirst.set(0.0f,1.0f,0.0f);
00362             v3fAxisSecond.set(0.0f,0.0f,1.0f);
00363             break;
00364         case ZX_TRANSLATION:
00365             v3fAxisFirst.set(0.0f,0.0f,1.0f);
00366             v3fAxisSecond.set(1.0f,0.0f,0.0f);
00367             break;
00368         }
00369 
00370         // 軸を現在のワールド座標へ変換する
00371         Transform3D t3dLocalToVworld = new Transform3D();
00372         Transform3D t3dCurrent = new Transform3D();
00373         Transform3D t3dWorld = new Transform3D();
00374         Transform3D t3dView = new Transform3D();
00375 
00376         tgTarget_.getLocalToVworld(t3dLocalToVworld);
00377         tgTarget_.getTransform(t3dCurrent);
00378         // それぞれの単位ベクトルを transform するとワールド座標
00379         // におけるそれぞれの軸の方向が得られる。
00380 
00381         // この TG がワールドへの変換に相当する
00382         t3dLocalToVworld.mul(t3dCurrent);
00383         t3dWorld.set(t3dLocalToVworld);
00384 
00385         TransformGroup tgView = info.drawable.getTransformGroupRoot();
00386         tgView.getLocalToVworld(t3dLocalToVworld);
00387         tgView.getTransform(t3dCurrent);
00388 
00389         // この TG がカメラへの変換に相当する
00390         t3dLocalToVworld.mul(t3dCurrent);
00391         t3dView.set(t3dLocalToVworld);
00392         t3dView.invert();
00393 
00394         t3dView.mul(t3dWorld);
00395         t3dView.transform(v3fAxisFirst);
00396         t3dView.transform(v3fAxisSecond);
00397 
00398         point000.set(0f,0f,0f);
00399         t3dView.transform(point000);
00400         if (intersect_ != null) {
00401             Point3f pointPicked = new Point3f(intersect_);
00402             t3dView.transform(pointPicked);
00403         }
00404 
00405         info.setTimerEnabled(true);
00406     }
00407 
00408     private void _decideRotationAxis(MouseEvent evt, BehaviorInfo info) {
00409         // 指定された時間が経過しました
00410         // マウスの前回からの移動量をベクトルに直す
00411         int dx = evt.getPoint().x - startPoint_.x;
00412         int dy = evt.getPoint().y - startPoint_.y;
00413 
00414         Point2f pointMouse =
00415             new Point2f(
00416                 (float)dx * ROTATION_FACTOR,
00417                 (float)dy * ROTATION_FACTOR
00418             );
00419         Vector2f v2fMouse = new Vector2f(pointMouse);
00420 
00421         v2fMouse.y *= -1.0f;
00422 
00423         // 最初の動きで回転の軸を決定します
00424         Vector3f v3fAxisFirst = new Vector3f();
00425         Vector3f v3fAxisSecond = new Vector3f();
00426 
00427         // 軸を現在のワールド座標へ変換する
00428         Transform3D t3dLocalToVworld = new Transform3D();
00429         Transform3D t3dCurrent = new Transform3D();
00430         Transform3D t3dWorld = new Transform3D();
00431         Transform3D t3dView = new Transform3D();
00432 
00433         tgTarget_.getLocalToVworld(t3dLocalToVworld);
00434         tgTarget_.getTransform(t3dCurrent);
00435         // それぞれの単位ベクトルを transform すると
00436         // ワールド座標におけるそれぞれの軸の方向が得られる。
00437         t3dLocalToVworld.mul(t3dCurrent);  // ワールドへの変換に相当する
00438         t3dWorld.set(t3dLocalToVworld);
00439 
00440         TransformGroup tgView = info.drawable.getTransformGroupRoot();
00441         tgView.getLocalToVworld(t3dLocalToVworld);
00442         tgView.getTransform(t3dCurrent);
00443         t3dLocalToVworld.mul(t3dCurrent);  // がカメラへの変換に相当する
00444         t3dView.set(t3dLocalToVworld);
00445         t3dView.invert();
00446 
00447         t3dView.mul(t3dWorld);
00448 
00449         switch(rotationMode_ & AXIS_FLAGS) {
00450         case ROTATION_WITH_X:
00451             v3fAxisFirst.set(0.0f,1.0f,0.0f);
00452             v3fAxisSecond.set(0.0f,0.0f,1.0f);
00453             break;
00454         case ROTATION_WITH_Y:
00455             v3fAxisFirst.set(0.0f,0.0f,1.0f);
00456             v3fAxisSecond.set(1.0f,0.0f,0.0f);
00457             break;
00458         case ROTATION_WITH_Z:
00459             v3fAxisFirst.set(1.0f,0.0f,0.0f);
00460             v3fAxisSecond.set(0.0f,1.0f,0.0f);
00461             break;
00462         }
00463 
00464         t3dView.transform(v3fAxisFirst);
00465         t3dView.transform(v3fAxisSecond);
00466 
00467         // 移動量を定義するためのラジアン
00468         float fDotProductFirst = 0.0f;
00469         float fDotProductSecond = 0.0f;
00470 
00471         // Canvas 平面へ投j影
00472         Vector2f v2fAxisFirst = new Vector2f(v3fAxisFirst.x, v3fAxisFirst.y);
00473         Vector2f v2fAxisSecond = new Vector2f(v3fAxisSecond.x, v3fAxisSecond.y);
00474 
00475         v2fAxisFirst.normalize();
00476         v2fAxisSecond.normalize();
00477 
00478         fDotProductFirst = v2fAxisFirst.dot(v2fMouse);
00479         fDotProductSecond = v2fAxisSecond.dot(v2fMouse);
00480 
00481         // 以前の回転軸の決定パターン
00482         switch(rotationMode_ & AXIS_FLAGS) {
00483         case ROTATION_WITH_X:
00484             if (Math.abs(fDotProductFirst) < Math.abs(fDotProductSecond)) {
00485                 v3fAnotherAxis = v3fAxisSecond;
00486                 v3fAxisRotate.set(0.0f, 1.0f, 0.0f);
00487             } else {
00488                 v3fAnotherAxis = v3fAxisFirst;
00489                 v3fAxisRotate.set(0.0f, 0.0f, 1.0f);
00490             }
00491             break;
00492         case ROTATION_WITH_Y:
00493             if (Math.abs(fDotProductFirst) < Math.abs(fDotProductSecond)) {
00494                 v3fAnotherAxis = v3fAxisSecond;
00495                 v3fAxisRotate.set(0.0f, 0.0f, 1.0f);
00496             } else {
00497                 v3fAnotherAxis = v3fAxisFirst;
00498                 v3fAxisRotate.set(1.0f, 0.0f, 0.0f);
00499             }
00500             break;
00501         case ROTATION_WITH_Z:
00502             if (Math.abs(fDotProductFirst) < Math.abs(fDotProductSecond)) {
00503                 v3fAnotherAxis = v3fAxisSecond;
00504                 v3fAxisRotate.set(1.0f, 0.0f, 0.0f);
00505             } else {
00506                 v3fAnotherAxis = v3fAxisFirst;
00507                 v3fAxisRotate.set(0.0f, 1.0f, 0.0f);
00508             }
00509             break;
00510         }
00511 
00512         Vector3f v3fTemp = new Vector3f();
00513         v3fTemp.set(v3fAxisRotate);
00514         t3dView.transform(v3fAxisRotate);
00515 
00516         // 平面状の回転軸ベクトルから -90 度回転した二次元平面状の軸
00517         // とマウスの動作のベクトルの内積が正だったらそのまま。
00518         // 負だったら正負を反転して使う。
00519 
00520         Vector2f v2fAxisRotate = new Vector2f(v3fAxisRotate.x, v3fAxisRotate.y);
00521 
00522         //v2fAnotherAxis = new Vector2f(v3fAnotherAxis.x, v3fAnotherAxis.y);
00523 
00524         // 回転軸を -90 回転したもの
00525         v2fDetermine = new Vector2f(v2fAxisRotate.y, -v2fAxisRotate.x);
00526 
00527         v3fAxisRotate.set(v3fTemp);
00528 
00529         v2fMouse.normalize();
00530         //float fDetermine = v2fMouse.dot(v2fDetermine);
00531 
00532         info.setTimerEnabled(true);
00533     }
00534 
00535     private void _translation(BehaviorInfo info) {
00536         // 指定された時間が経過しました
00537         // マウスの前回からの移動量をベクトルに直す
00538         int dx = point_.x - startPoint_.x;
00539         int dy = point_.y - startPoint_.y;
00540         // ?_factor でマウスの移動量に対する回転量の倍率を指定
00541         // マウスの動作においては下がプラスになっているので反転しておく
00542         Point2f pointMouse =
00543             new Point2f(
00544                 (float)dx * TRANSLATION_FACTOR,
00545                 (float)dy * TRANSLATION_FACTOR
00546             );
00547         Vector2f v2fMouse = new Vector2f(pointMouse);
00548 
00549         v2fMouse.y *= -1.0f;
00550 
00551         float fDotProductFirst = 0.0f;  // 移動量を定義するためのラジアン
00552         float fDotProductSecond = 0.0f;  // 移動量を定義するためのラジアン
00553 
00554         // Canvas 平面へ投影
00555         Vector2f v2fAxisFirst = new Vector2f(v3fAxisFirst.x, v3fAxisFirst.y);
00556         Vector2f v2fAxisSecond = new Vector2f(v3fAxisSecond.x, v3fAxisSecond.y);
00557 
00558         // 軸の追加
00559         if (v2fAxisFirst.length() == 0) {
00560             if (intersect_ != null) {
00561                 Point3f pointPicked = new Point3f(intersect_);
00562                 pointPicked.sub(point000);  // View からみた point 000 で割る
00563                 pointPicked.scale(-1.0f);
00564                 v3fAxisFirst.set(pointPicked);
00565                 v2fAxisFirst.set(v3fAxisFirst.x,v3fAxisFirst.y);
00566             } else {
00567                 // 軸が判定できなかったとき用
00568                 v2fAxisFirst.set(0.0f,1.0f);
00569             }
00570         }
00571 
00572         if (v2fAxisSecond.length() == 0) {
00573             if (intersect_ != null) {
00574                 Point3f pointPicked = new Point3f(intersect_);
00575                 pointPicked.sub(point000);
00576                 pointPicked.scale(-1.0f);
00577                 v3fAxisSecond.set(pointPicked);
00578                 v2fAxisSecond.set(v3fAxisSecond.x,v3fAxisSecond.y);
00579             } else {
00580                 // 軸が判定できなかったとき用
00581                 v2fAxisSecond.set(1.0f,0.0f);
00582             }
00583         }
00584         v2fAxisFirst.normalize();
00585         v2fAxisSecond.normalize();
00586 
00587         fDotProductFirst = v2fAxisFirst.dot(v2fMouse);
00588         fDotProductSecond = v2fAxisSecond.dot(v2fMouse);
00589 
00590         if (Float.isNaN(fDotProductFirst)) {
00591             fDotProductFirst = 0;
00592         }
00593 
00594         if (Float.isNaN(fDotProductSecond)) {
00595             fDotProductSecond = 0;
00596         }
00597 
00598         Transform3D t3dCur = new Transform3D();
00599         Transform3D t3dTranslate = new Transform3D();
00600         tgTarget_.getTransform(t3dCur);
00601         Vector3f v3fTranslate = new Vector3f();
00602 
00603         // 閾値チェック
00604         if (Math.abs(fDotProductFirst) > TRANSLATION_LIMIT) {
00605             if (fDotProductFirst > TRANSLATION_LIMIT) {
00606                 fDotProductFirst = TRANSLATION_LIMIT;
00607             } else {
00608                 fDotProductFirst = -TRANSLATION_LIMIT;
00609             }
00610         }
00611         if (Math.abs(fDotProductSecond) > TRANSLATION_LIMIT) {
00612             if (fDotProductSecond > TRANSLATION_LIMIT) {
00613                 fDotProductSecond = TRANSLATION_LIMIT;
00614             } else {
00615                 fDotProductSecond = -TRANSLATION_LIMIT;
00616             }
00617         }
00618 
00619         switch(translationMode_) {
00620         case XY_TRANSLATION:
00621             v3fTranslate.x += fDotProductFirst;
00622             v3fTranslate.y += fDotProductSecond;
00623             break;
00624         case YZ_TRANSLATION:
00625             v3fTranslate.y += fDotProductFirst;
00626             v3fTranslate.z += fDotProductSecond;
00627             break;
00628         case ZX_TRANSLATION:
00629             v3fTranslate.z += fDotProductFirst;
00630             v3fTranslate.x += fDotProductSecond;
00631             break;
00632         }
00633         
00634         t3dTranslate.set(v3fTranslate);
00635         t3dCur.mul(t3dTranslate);
00636         tgTarget_.setTransform(t3dCur);
00637 
00638         _resolve();
00639         // 変換情報を通知
00640         //broadcastEvent();
00641     }
00642 
00643     private void _rotation(BehaviorInfo info) {
00644         int dx = point_.x - startPoint_.x;
00645         int dy = point_.y - startPoint_.y;
00646 
00647         Point2f pointMouse = new Point2f(
00648             (float)dx * ROTATION_FACTOR,
00649             (float)dy * ROTATION_FACTOR
00650         );
00651         Vector2f v2fMouse = new Vector2f(pointMouse);
00652         // Swing だと不要?
00653         v2fMouse.y *= -1.0f;
00654         float fDotProductRotate = v2fDetermine.dot(v2fMouse);
00655 
00656         Transform3D t3dCur = new Transform3D();
00657         Transform3D t3dRotate = new Transform3D();
00658         tgTarget_.getTransform(t3dCur);
00659 
00660         // 閾値チェック
00661         if (Math.abs(fDotProductRotate) > ROTATION_LIMIT) {
00662             if (fDotProductRotate > ROTATION_LIMIT) {
00663                 fDotProductRotate = ROTATION_LIMIT;
00664             }
00665         } else {
00666             fDotProductRotate = - ROTATION_LIMIT;
00667         }
00668 
00669         AxisAngle4f axis =
00670                 new AxisAngle4f(v3fAxisRotate, fDotProductRotate);
00671         t3dRotate.set(axis);
00672         t3dCur.mul(t3dRotate);
00673         tgTarget_.setTransform(t3dCur);
00674 
00675         _resolve();
00676         // 変換情報を通知
00677         //broadcastEvent();
00678     }
00679 
00680     private void _resolve() {
00681         Transform3D t3dCurrent = new Transform3D();
00682         tgTarget_.getTransform(t3dCurrent);
00683         Transform3D t3dLocalToVworld = new Transform3D();
00684         tgTarget_.getLocalToVworld(t3dLocalToVworld);
00685         Transform3D t3dCur = new Transform3D();
00686         tgTarget_.getTransform(t3dCur);
00687 
00688         // まず Tn の変換を求める
00689         // Transform3D 同士の変換を結合する
00690         t3dLocalToVworld.mul(t3dCur);
00691         t3dCurrent = t3dLocalToVworld;
00692 
00693         if (resolver_.resolve(t3dCurrent)) {
00694             // Bounding Box の resize
00695             // 成功したら limit 値をあげる
00696             if (mode_ == ROTATION_MODE) {
00697                 limit_ += (float)(Math.PI/1440);
00698             } else {
00699                 limit_ += 0.0001f;
00700             }
00701         } else {
00702             // 失敗したら limit 値をさげる
00703             if (mode_ == ROTATION_MODE) {
00704                 if (limit_ > (float)Math.PI/1440) {
00705                     limit_ -= Math.PI/1440;
00706                 }
00707             } else {
00708                 if (limit_ > 0.0001f) {
00709                     limit_ -= 0.0001f;
00710                 }
00711             }
00712         }
00713     }   
00714 }


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