JointRotationHandler.java
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * General Robotix Inc.
8  * National Institute of Advanced Industrial Science and Technology (AIST)
9  */
17 package com.generalrobotix.ui.view.tdview;
18 
19 import java.awt.*;
20 import java.awt.event.*;
21 import javax.media.j3d.*;
22 import javax.vecmath.*;
23 
24 
28 import com.sun.j3d.utils.picking.*;
29 
30 class JointRotationHandler extends OperationHandler {
31  //--------------------------------------------------------------------
32  // 定数
33  private static final int MODE_NONE = 0;
34  private static final int DISK_MODE = 1;
35  private static final int CYLINDER_MODE = 2;
36  private static final float FACTOR = 0.004f;
37 
38  // 物体のVectorと視点のVectorがこの角度以下の時CYLINDER_MODEに移行する
39  private static final double THRESHOLD = 0.262f;
40 
41  //--------------------------------------------------------------------
42  // インスタンス変数
43  private int mode_;
44  private TransformGroup tgTarget_;
45  private Point prevPoint_ = new Point();
46  private double angle_;
47  private boolean isPicked_;
48  private Vector3d vectorCylinder_;
49 
50  // DISK モードの時に使用する
51  //protected Point3f point000 = new Point3f(0,0,0);
52  private Point3d pointTarget_;
53 
54  //--------------------------------------------------------------------
55  // BehaviorHandlerの実装
56  public void processPicking(MouseEvent evt, BehaviorInfo info) {
57  prevPoint_.x = evt.getPoint().x;
58  prevPoint_.y = evt.getPoint().y;
59 
60  isPicked_ = false;
61 
62  try {
63  info.pickCanvas.setShapeLocation(prevPoint_.x, prevPoint_.y);
64  PickResult pickResult[] = info.pickCanvas.pickAllSorted();
65  if (pickResult == null) {
66  return;
67  }
68  TransformGroup tg = (TransformGroup)pickResult[0].getNode(
69  PickResult.TRANSFORM_GROUP
70  );
71  GrxModelItem model = SceneGraphModifier.getModelFromTG(tg);
72  if (model == null)
73  return;
74  else{
75  if(info.manager_.focusedItem()==model){
76  if( pickResult.length > 1)
77  tg = (TransformGroup)pickResult[1].getNode(
78  PickResult.TRANSFORM_GROUP );
79  else
80  return;
81  }
82  }
83  setPickTarget(tg, info);
84  } catch (CapabilityNotSetException ex) {
85  ex.printStackTrace();
86  }
87 
88  isPicked_ = true;
89  //evt.consume();
90  }
91 
92  public void processStartDrag(MouseEvent evt, BehaviorInfo info) {
93  if (!isPicked_) {
94  mode_ = MODE_NONE;
95  return;
96  }
97  //Press されたら視点と物体の角度を取得し現在のモードを判定する
98  // ターゲット座標系からワールド座標系への変換
99  Transform3D target2vw = new Transform3D();
100  Transform3D l2vw = new Transform3D();
101  Transform3D tr = new Transform3D();
102  tgTarget_.getLocalToVworld(l2vw);
103  tgTarget_.getTransform(tr);
104  target2vw.mul(l2vw, tr);
105 
106  // ターゲットの原点のワールド座標系での座標を求める。
107  pointTarget_ = new Point3d();
108  target2vw.transform(pointTarget_);
109 
110  // ワールド座標系から視点座標系への変換
111  Transform3D vw2view = new Transform3D();
112  tr = new Transform3D();
113  l2vw = new Transform3D();
114  TransformGroup tgView = info.drawable.getTransformGroupRoot();
115  tgView.getLocalToVworld(l2vw);
116  tgView.getTransform(tr);
117  vw2view.mul(l2vw, tr);
118  vw2view.invert();
119 
120  // (0,0,0) (0,0,1) の point を作成し、上で作成した
121  // vw2view target2vw を使って変換する
122  Point3f point000 = new Point3f(0,0,0);
123  //軸情報取り出す
124 
125  GrxLinkItem l = SceneGraphModifier.getLinkFromTG(tgTarget_);
126  if(l.jointAxis_==null)
127  return;
128 
129  vw2view.mul(target2vw);
130  vw2view.transform(point000);
131  Vector3d vectorView = new Vector3d(point000);
132  vectorCylinder_ = new Vector3d(l.jointAxis_);
133  vw2view.transform(vectorCylinder_);
134 
135  // 二つの Vector で angle の角度を得て diskAngle と
136  // 比較しモードを設定
137  double angle = vectorView.angle(vectorCylinder_);
138 
139  if(angle == Double.NaN) {
140  System.err.println("無効な値が入りました");
141  }
142  // 角度が 90 以上の時のことも考えて if で分ける
143  if (angle > Math.PI / 2.0) {
144  // 両端に DISK_MODE の状態になる範囲があるので大きい
145  // ほうの範囲を小さいほうへ移動する計算
146  angle = Math.PI - angle;
147  }
148 
149  if (angle < THRESHOLD) {
150  mode_ = DISK_MODE;
151  } else {
152  mode_ = CYLINDER_MODE;
153  }
154  }
155 
156  public void processDragOperation(MouseEvent evt, BehaviorInfo info) {
157 // if (bbSwitch_ == null) {
158 // return;
159 // }
160 
161  Vector2d mouseMove = new Vector2d(
162  FACTOR * (evt.getPoint().getX() - prevPoint_.getX()),
163  FACTOR * (evt.getPoint().getY() - prevPoint_.getY())
164  );
165 
166  angle_ = 0.0; // 移動量を定義するためのラジアン
167  // 現在のモードに応じてマウスの動きを回転に直す
168  switch (mode_) {
169  case DISK_MODE:
170  Point2d pointMouseOnPlane = new Point2d();
171  //Point3d pointTemp = new Point3d(point000);
172  Point3d pointTemp = new Point3d(pointTarget_);
173 
174  Canvas3D canvas = info.pickCanvas.getCanvas();
175  Transform3D vw2imagePlate = new Transform3D();
176  canvas.getVworldToImagePlate(vw2imagePlate);
177  vw2imagePlate.transform(pointTemp);
178  canvas.getPixelLocationFromImagePlate(pointTemp, pointMouseOnPlane);
179 
180  Vector2d prev = new Vector2d(
181  prevPoint_.getX() - pointMouseOnPlane.x,
182  prevPoint_.getY() - pointMouseOnPlane.y
183  );
184 
185  Vector2d current = new Vector2d(
186  evt.getPoint().getX() - pointMouseOnPlane.x,
187  evt.getPoint().getY() - pointMouseOnPlane.y
188  );
189 
190  // 画面上の座標は下に行くほど y の値が逆転していくので
191  // y の値を逆転させておく。
192  angle_ = prev.angle(current);
193 
194  Vector3d cross = new Vector3d();
195  cross.cross(vectorCylinder_, new Vector3d(prev.x, prev.y, 0.0));
196 
197  if (mouseMove.dot(new Vector2d(cross.x, cross.y)) > 0.0) {
198  angle_ = - angle_;
199  }
200  // 画面のどちらが我がシリンダーの表面を向いているかで
201  // 動作が反転する
202 /*
203  if (vectorCylinder_.z < 0) {
204  angle_ = - angle_;
205  }
206 */
207  break;
208  case CYLINDER_MODE:
209  // x,y 平面において軸方向と直行する方向のマウスの
210  // 動作だけをシリンダーの回転にする
211  // fDotProduct <- 内積 InnerProduct ともいう
212  // 半時計回りに 90 度回転するので 4x4 の変換行列を
213  // かましてやった値をつかって直行するベクトルを出す
214  // マウスに対する回転量を DISK_MODE の二倍にする
215  mouseMove.x *= -2.0;
216  mouseMove.y *= 2.0;
217 
218  //vectorCylinder_.normalize();
219  Vector2d vectorCylinder =
220  new Vector2d(- vectorCylinder_.y, vectorCylinder_.x);
221 
222  // このベクトルと直行する Vector を求める
223  vectorCylinder.normalize();
224  angle_ = vectorCylinder.dot(mouseMove);
225  break;
226  case MODE_NONE:
227  return;
228  }
229 
230  prevPoint_.x = evt.getPoint().x;
231  prevPoint_.y = evt.getPoint().y;
232  _jointAngleChanged(info);
233  ((Grx3DView)info.drawable).showOption();
234 
235  evt.consume();
236  }
237 
238  public void processReleased(MouseEvent evt, BehaviorInfo info) {
239 // if (bbSwitch_ != null) {
240  evt.consume();
241 // }
242  }
243 
244  public boolean processTimerOperation(BehaviorInfo info) {
245  return true;
246  }
247 
248  //--------------------------------------------------------------------
249  // OperationHandlerの実装
250  public void disableHandler() {
251  }
252 
253  public void setPickTarget(TransformGroup tg, BehaviorInfo info) {
254  if (tg != tgTarget_) {
255  GrxLinkItem l = SceneGraphModifier.getLinkFromTG(tg);
256  if (l == null) return;
257  info.manager_.focusedItem(l);
258  tgTarget_ = l.tg_;
259  }
260  }
261 
262  private void _jointAngleChanged(BehaviorInfo info) {
263  try {
264  GrxModelItem model = SceneGraphModifier.getModelFromTG(tgTarget_);
265  GrxLinkItem link = SceneGraphModifier.getLinkFromTG(tgTarget_);
266  if( link.jointType_.equals("rotate") || link.jointType_.equals("slide") ){
267  link.jointValue(link.jointValue_+angle_);
268  link.setJointValuesWithinLimit();
269  model.updateInitialJointValue(link);
270  model.calcForwardKinematics();
271  }
272  } catch (Exception e) {
273  e.printStackTrace();
274  }
275  }
276 }
#define null
our own NULL pointer
Definition: IceTypes.h:57
item corresponds to a robot model
Vector3 cross(const Vector3 &v1, const Vector3 &v2)
Definition: Tvmet2Eigen.h:15
backing_store_ptr info
Definition: jmemsys.h:181


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:04