ObjectRotationHandler.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 
33 class ObjectRotationHandler extends OperationHandler {
34  private static final float ROTATION_FACTOR = 0.006f;
35 
36  private TransformGroup tgTarget_;
37  private Vector3f norm_;
38  private Vector3f axis_;
39  private Vector3f dir_ = new Vector3f();
40  private Point prevPoint_ = new Point();
41  private boolean isPicked_;
42 
43  //--------------------------------------------------------------------
44  // BehaviorHandlerの実装
45  public void processPicking(MouseEvent evt, BehaviorInfo info) {
46  prevPoint_.x = evt.getPoint().x;
47  prevPoint_.y = evt.getPoint().y;
48 
49  norm_ = null;
50  isPicked_ = false;
51 
52  try {
53  info.pickCanvas.setShapeLocation(prevPoint_.x, prevPoint_.y);
54  PickResult pickResult = info.pickCanvas.pickClosest();
55  if (pickResult == null)
56  return;
57  TransformGroup tg =
58  (TransformGroup)pickResult.getNode(PickResult.TRANSFORM_GROUP);
59  GrxModelItem model = SceneGraphModifier.getModelFromTG(tg);
60  if (model == null)
61  return;
62  else
63  tg = model.getTransformGroupRoot();
64  if (tg == null)
65  return;
66 
67  if (_enableBoundingBox(tg, info)) {
68  isPicked_ = true;
69  Point3d startPoint = info.pickCanvas.getStartPosition();
70  PickIntersection intersection = pickResult.getClosestIntersection(startPoint);
71  norm_ = new Vector3f(intersection.getPointNormal());
72  }
73  } catch (CapabilityNotSetException ex) {
74  // もう出ることはないと思うが、読み込むモデルによっては
75  // 出るかもしれないので、スタックトレースは表示する。
76  ex.printStackTrace();
77  _disableBoundingBox();
78  }
79  }
80 
81  public void processStartDrag(MouseEvent evt, BehaviorInfo info) {
82  if (isPicked_) {
83  if (norm_ != null) {
84  // ターゲットの座標系から視点座標系への変換を求める。
85  Transform3D tr = new Transform3D();
86  Transform3D l2vw = new Transform3D();
87  Transform3D trTarget2View = new Transform3D();
88 
89  tgTarget_.getLocalToVworld(l2vw);
90  tgTarget_.getTransform(tr);
91  trTarget2View.mul(l2vw, tr);
92 
93  TransformGroup tgView = info.drawable.getTransformGroupRoot();
94  tgView.getLocalToVworld(l2vw);
95  tgView.getTransform(tr);
96  l2vw.mul(tr);
97  l2vw.invert();
98 
99  l2vw.mul(trTarget2View);
100  trTarget2View.set(l2vw);
101 
102  // マウスの動きをターゲットの座標系の動きに変換
103  float fdx = (float)(evt.getPoint().getX() - prevPoint_.getX());
104  float fdy = (float)(evt.getPoint().getY() - prevPoint_.getY());
105  Vector3f mouse = new Vector3f(fdx, - fdy, 0.0f);
106  Vector3f normal = new Vector3f();
107 
108  trTarget2View.transform(norm_, normal);
109 
110  float inner = normal.dot(mouse);
111  normal.scale(inner);
112  mouse.sub(normal);
113 
114  trTarget2View.invert();
115  trTarget2View.transform(mouse);
116  mouse.normalize();
117 
118  axis_ = _createAxisVector(mouse);
119 
120  if (axis_ == null) {
121  System.out.println("axis is null");
122  isPicked_ = false;
123  _disableBoundingBox();
124  return;
125  }
126 
127  dir_.cross(axis_, norm_);
128  }
129  evt.consume();
130  }
131  }
132 
133  public void processDragOperation(MouseEvent evt, BehaviorInfo info) {
134  if (isPicked_) {
135  // tgViewからみたマウスの軌跡ベクトルmouseを求める。
136  // mouse-(norm_,mouse)norm_ がオブジェクトの移動ベクトル
137  if (norm_ != null) {
138  // ターゲットの座標系から視点座標系への変換を求める。
139  Transform3D tr = new Transform3D();
140  Transform3D l2vw = new Transform3D();
141  Transform3D trTarget2View = new Transform3D();
142 
143  tgTarget_.getLocalToVworld(l2vw);
144  tgTarget_.getTransform(tr);
145  trTarget2View.mul(l2vw, tr);
146 
147  TransformGroup tgView = info.drawable.getTransformGroupRoot();
148  tgView.getLocalToVworld(l2vw);
149  tgView.getTransform(tr);
150  l2vw.mul(tr);
151  l2vw.invert();
152 
153  l2vw.mul(trTarget2View);
154  trTarget2View.set(l2vw);
155 
156  // マウスの動きをターゲットの座標系の動きに変換
157  float fdx = (float)(evt.getPoint().getX() - prevPoint_.getX());
158  float fdy = (float)(evt.getPoint().getY() - prevPoint_.getY());
159  Vector3f mouse = new Vector3f(fdx, - fdy, 0.0f);
160  Vector3f normal = new Vector3f();
161 
162  trTarget2View.transform(norm_, normal);
163 
164  float inner = normal.dot(mouse);
165  normal.scale(inner);
166  mouse.sub(normal);
167 
168  trTarget2View.invert();
169  trTarget2View.transform(mouse);
170 
171  float angle = ROTATION_FACTOR * mouse.dot(dir_);
172  tgTarget_.getTransform(l2vw);
173  tr.set(new AxisAngle4f(axis_, angle));
174  l2vw.mul(tr);
175  GrxLinkItem link = SceneGraphModifier.getLinkFromTG(tgTarget_);
176  link.setTransform(l2vw);
177  _transformChanged(info);
178 
179  prevPoint_.x = evt.getPoint().x;
180  prevPoint_.y = evt.getPoint().y;
181  }
182  ((Grx3DView)info.drawable).showOption();
183  evt.consume();
184  }
185  }
186 
187  public void processReleased(MouseEvent evt, BehaviorInfo info) {
188  if (isPicked_) {
189  evt.consume();
190  }
191  }
192 
193  public boolean processTimerOperation(BehaviorInfo info) {
194  return true;
195  }
196 
197  //--------------------------------------------------------------------
198  // OperationHandlerの実装
199  public void disableHandler() {
200  _disableBoundingBox();
201  }
202 
203  public void setPickTarget(TransformGroup tg, BehaviorInfo info) {
204  if (tg != tgTarget_) {
205  _enableBoundingBox(tg, info);
206  }
207  }
208 
209  //--------------------------------------------------------------------
210  // プライベートメソッド
211  private void _disableBoundingBox() {
212  if (tgTarget_ != null) {
213  tgTarget_ = null;
214  norm_ = null;
215  }
216  }
217 
218  private boolean _enableBoundingBox(TransformGroup tg, BehaviorInfo info) {
219  GrxModelItem model = SceneGraphModifier.getModelFromTG(tg);
220  if (model == null)
221  return false;
222 
223  tgTarget_ = tg;
224  info.manager_.focusedItem(model);
225  return true;
226  }
227 
228  private Vector3f _createAxisVector(Vector3f mouse) {
229  if (1.0f - Math.abs(norm_.x) < 0.000001) {
230  if (Math.abs(mouse.y) < Math.abs(mouse.z)) {
231  if (mouse.y > 0.0f) {
232  return new Vector3f(0.0f, 1.0f, 0.0f);
233  } else {
234  return new Vector3f(0.0f, -1.0f, 0.0f);
235  }
236  } else {
237  if (mouse.z > 0.0f) {
238  return new Vector3f(0.0f, 0.0f, 1.0f);
239  } else {
240  return new Vector3f(0.0f, 0.0f, -1.0f);
241  }
242  }
243  } else if (1.0f - Math.abs(norm_.y) < 0.000001) {
244  if (Math.abs(mouse.x) < Math.abs(mouse.z)) {
245  if (mouse.x > 0.0f) {
246  return new Vector3f(1.0f, 0.0f, 0.0f);
247  } else {
248  return new Vector3f(-1.0f, 0.0f, 0.0f);
249  }
250  } else {
251  if (mouse.z > 0.0f) {
252  return new Vector3f(0.0f, 0.0f, 1.0f);
253  } else {
254  return new Vector3f(0.0f, 0.0f, -1.0f);
255  }
256  }
257  } else if (1.0f - Math.abs(norm_.z) < 0.000001) {
258  if (Math.abs(mouse.x) < Math.abs(mouse.y)) {
259  if (mouse.x > 0.0f) {
260  return new Vector3f(1.0f, 0.0f, 0.0f);
261  } else {
262  return new Vector3f(-1.0f, 0.0f, 0.0f);
263  }
264  } else {
265  if (mouse.y > 0.0f) {
266  return new Vector3f(0.0f, 1.0f, 0.0f);
267  } else {
268  return new Vector3f(0.0f, -1.0f, 0.0f);
269  }
270  }
271  }
272 
273  System.out.println("norm=" + norm_);
274  return null;
275  }
276 
277  private void _transformChanged(BehaviorInfo info) {
278  GrxModelItem model = SceneGraphModifier.getModelFromTG(tgTarget_);
279  if (model == null) {
280  System.out.println("can't get manipulatable.");
281  return;
282  }
283 
284  model.calcForwardKinematics();
285  model.updateInitialTransformRoot();
286  }
287 }
int intersection(const fLineVec &lv1, const fLineVec &lv2, fVec3 &c1, fVec3 &c2, double &d, double eps)
Definition: fLineVec.cpp:36
#define null
our own NULL pointer
Definition: IceTypes.h:57
item corresponds to a robot model
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