ObjectFittingHandler.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.util.*;
20 import java.awt.*;
21 import java.awt.event.*;
22 import javax.media.j3d.*;
23 import javax.vecmath.*;
24 
25 
28 import com.sun.j3d.utils.picking.*;
29 
30 class ObjectFittingHandler extends OperationHandler {
31  //--------------------------------------------------------------------
32  // 定数
33  public static final int FITTING_FROM = 1;
34  public static final int FITTING_TO = 2;
35 
36  //--------------------------------------------------------------------
37  // インスタンス変数
38  private FittingInfo fittingInfoFrom_;
39  private FittingInfo fittingInfoTo_;
40 
41  private int mode_;
42  private boolean bHaveFirst = false;
43  private boolean bHaveSecond = false;
44  private Point3f intersectVW_;
45  private Point3d[] verticesVW_;
46 
47  //--------------------------------------------------------------------
48  // コンストラクタ
49  public ObjectFittingHandler () {
50  fittingInfoFrom_ = new FittingInfo(
51  new Color3f(1.0f,0.0f,0.0f),
52  new Color3f(0.0f,1.0f,0.0f),
53  false
54  );
55 
56  fittingInfoTo_ = new FittingInfo(
57  new Color3f(0.0f,0.0f,1.0f),
58  new Color3f(0.0f,1.0f,0.0f),
59  true
60  );
61  }
62 
63  //--------------------------------------------------------------------
64  // 公開メソッド
65  void setFittingMode(int mode) {
66  mode_ = mode;
67  }
68 
72  public boolean fit(BehaviorInfo info) {
73  if (!bHaveSecond) {
74  System.err.println("二番目の TG が選択されていません");
75  return false;
76  }
77  if (!bHaveFirst) {
78  System.err.println("一番目の TG が選択されていません");
79  return false;
80  }
81 
82  // すでに二つの物体が選択されていたらそれをくっつける
83 
84  // 二つのベクトルの Angle を求め、二つのベクトルの外積方向を
85  // 軸にその angle 回転させる
86  // その情報を t3dRotate へ入れる
87  Vector3f v3fCross = new Vector3f();
88  Transform3D t3dRotate = new Transform3D();
89  Vector3f v3fNormal = fittingInfoFrom_.getNormalVector();
90  Vector3f v3fNormalSecond = fittingInfoTo_.getNormalVector();
91  v3fCross.cross(v3fNormal,v3fNormalSecond);
92  // 同方向かどうかのテスト部分
93  if (v3fNormal.angle(v3fNormalSecond) == 0.0f) {
94  // 角度が 0 度の時の対策。もし要素のすべての符号が
95  // いっしょなら同じ方向のベクトルだとみなす
96  if (_isSameSignVector3f(v3fNormal, v3fNormalSecond)) {
97  // v3fCross に外積が入らないので AxisAngle は使えない
98  Vector3f vecXAxis =
99  new Vector3f(new Point3f(1.0f,0.0f,0.0f));
100  Vector3f vecYAxis =
101  new Vector3f(new Point3f(0.0f,1.0f,0.0f));
102  if (v3fNormal.x != -1.0d) {
103  v3fCross.cross(vecXAxis,v3fNormal);
104  t3dRotate.set(new AxisAngle4f(v3fCross,(float)Math.PI));
105  } else {
106  v3fCross.cross(vecYAxis,v3fNormal);
107  t3dRotate.set(new AxisAngle4f(v3fCross,(float)Math.PI));
108  }
109  }
110  } else {
111  t3dRotate.set(
112  new AxisAngle4f(
113  v3fCross,
114  (float)Math.PI + v3fNormal.angle(v3fNormalSecond)
115  )
116  );
117  }
118 
119  // 現在の Transform3D を生成し t3dCur へ入れる
120  Transform3D t3dCur = new Transform3D();
121 
122  GrxModelItem model = SceneGraphModifier.getModelFromTG(fittingInfoFrom_.getTransformGroup());
123  if (model == null) return false;
124  TransformGroup tgCur = model.getTransformGroupRoot();
125  tgCur.getTransform(t3dCur);
126 
127  // 移動させる側の法線のスタートポイントへ回転処理をかけた場合の
128  // 回転後の位置を p3dIntersect へ入れる
129  Point3f p3dIntersect = fittingInfoFrom_.getIntersectPoint();
130  Point3f p3dIntersectSecond = fittingInfoTo_.getIntersectPoint();
131  t3dRotate.transform(p3dIntersect);
132  // 移動させる処理
133  Point3d p3dMove = new Point3d();
134  p3dMove.x = p3dIntersectSecond.x - p3dIntersect.x;
135  p3dMove.y = p3dIntersectSecond.y - p3dIntersect.y;
136  p3dMove.z = p3dIntersectSecond.z - p3dIntersect.z;
137  Vector3f v3fTranslate = new Vector3f(p3dMove);
138  Transform3D t3dTranslate = new Transform3D();
139 
140  // Report された TG のぶんまでの LocalToVworld ぶんを逆変換する
141  Transform3D t3dLocalToVworld = new Transform3D();
142  Transform3D t3dCurrent = new Transform3D();
143  tgCur.getLocalToVworld(t3dLocalToVworld);
144  tgCur.getTransform(t3dCurrent);
145 
146  t3dLocalToVworld.mul(t3dCurrent);
147  t3dLocalToVworld.invert();
148 
149  t3dTranslate.set(v3fTranslate);
150 
151  // Behavior に変更を任せる
152  // 実際に Transform3D を multiply していく
153  t3dRotate.mul(t3dCur);
154  t3dTranslate.mul(t3dRotate);
155 
156  AxisAngle4f axis = new AxisAngle4f();
157  Quat4f quat = new Quat4f();
158  t3dTranslate.get(quat);
159  t3dTranslate.get(v3fTranslate);
160  axis.set(quat);
161 
162  model.setTransformRoot(t3dTranslate);
163 
164  // 別クラスに出す場合は broadcast する
165  _transformChanged(info, tgCur);
166  // この Behavior で TG を変更する場合
167 
168  // 処理が終わったら二つの表示用の線分を見えなくする
169  fittingInfoFrom_.removeForDisplay();
170  fittingInfoTo_.removeForDisplay();
171 
172  // くっつける処理が終わったら元の何も選択されていない状態に戻す
173  bHaveFirst = false;
174  bHaveSecond = false;
175  fittingInfoFrom_.setTransformGroup(null);
176  fittingInfoTo_.setTransformGroup(null);
177  ((Grx3DView)info.drawable).showOption();
178  return true;
179  }
180 
181  //--------------------------------------------------------------------
182  // BehaviorHandlerの実装
183  public void processPicking(MouseEvent evt, BehaviorInfo info) {
184  Point mouse = evt.getPoint();
185 
186  try {
187  info.pickCanvas.setShapeLocation(mouse.x, mouse.y);
188 
189  PickResult pickResult[] = info.pickCanvas.pickAllSorted();
190  if (pickResult == null) {
191  return;
192  }
193  TransformGroup tg = (TransformGroup)pickResult[0].getNode(PickResult.TRANSFORM_GROUP);
194  Point3d startPoint = info.pickCanvas.getStartPosition();
195  PickIntersection intersection = pickResult[0].getClosestIntersection(startPoint);
196  GrxModelItem model = SceneGraphModifier.getModelFromTG(tg);
197  if (model == null)
198  return;
199  else{
200  if(info.manager_.focusedItem()==model){
201  if( pickResult.length > 1){
202  tg = (TransformGroup)pickResult[1].getNode(PickResult.TRANSFORM_GROUP );
203  intersection = pickResult[1].getClosestIntersection(startPoint);
204  info.manager_.focusedItem(null);
205  }else
206  return;
207  }
208  }
209 
210  intersectVW_ = new Point3f(intersection.getPointCoordinatesVW());
211  verticesVW_ = intersection.getPrimitiveCoordinatesVW();
212  _enableIndicator(tg, info);
213  } catch (CapabilityNotSetException ex) {
214  // もう出ることはないと思うが、読み込むモデルによっては
215  // 出るかもしれないので、スタックトレースは表示する。
216  ex.printStackTrace();
217  _disableIndicator();
218  }
219  }
220 
221  public void processStartDrag(MouseEvent evt, BehaviorInfo info) {}
222  public void processDragOperation(MouseEvent evt, BehaviorInfo info) {}
223  public void processReleased(MouseEvent evt, BehaviorInfo info) {}
224  public boolean processTimerOperation(BehaviorInfo info) {
225  return true;
226  }
227 
228  //--------------------------------------------------------------------
229  // OperationHandlerの実装
230  public void disableHandler() {
231  _disableIndicator();
232  }
233 
234  public void setPickTarget(TransformGroup tg, BehaviorInfo info) {}
235 
236  //--------------------------------------------------------------------
237  // プライベートメソッド
238  private void _enableIndicator(TransformGroup tg, BehaviorInfo info) {
239  // ObjectTranslationBehavior と ObjectRotationBehavior の場合は
240  // BoundingBox の方向が pick されたのか
241  // 判定して Behavior にモードを設定します
242 
243  String strFirst = null;
244  String strSecond = null;
245  Hashtable<String, Object> tgInfo;
246  switch (mode_) {
247  case FITTING_FROM:
248  if (tg == fittingInfoFrom_.getArrowTransformGroup()) {
249  bHaveFirst = false;
250  fittingInfoFrom_.setTransformGroup(null);
251  fittingInfoFrom_.removeForDisplay();
252  return;
253  }
254 
255  tgInfo = SceneGraphModifier.getHashtableFromTG(tg);
256  if (tgInfo == null) return;
257  GrxModelItem model = SceneGraphModifier.getModelFromTG(tg);
258  strFirst = model.getName();
259 
260  tgInfo = SceneGraphModifier.getHashtableFromTG(
261  fittingInfoTo_.getTransformGroup()
262  );
263  model = SceneGraphModifier.getModelFromTG(fittingInfoTo_.getTransformGroup());
264  if (model != null) {
265  strSecond = model.getName();
266  }
267 
268  if (strFirst.equals(strSecond)) return;
269 
270  fittingInfoFrom_.setPickable(false);
271  fittingInfoFrom_.setIntersectPoint(intersectVW_);
272  fittingInfoFrom_.setPrimitiveCoordinates(verticesVW_);
273  fittingInfoFrom_.setTransformGroup(tg);
274 
275  // 選択された面の情報を表示する
276  fittingInfoFrom_.addForDisplay();
277 
278  bHaveFirst = true;
279  break;
280  case FITTING_TO:
281  if (tg == fittingInfoTo_.getArrowTransformGroup()) {
282  bHaveSecond = false;
283  fittingInfoTo_.setTransformGroup(null);
284  fittingInfoTo_.removeForDisplay();
285  return;
286  }
287 
288  tgInfo = SceneGraphModifier.getHashtableFromTG(tg);
289  if (tgInfo == null)
290  return;
291  model = SceneGraphModifier.getModelFromTG(tg);
292  strSecond = model.getName();
293 
294  tgInfo = SceneGraphModifier.getHashtableFromTG(
295  fittingInfoFrom_.getTransformGroup()
296  );
297  model = SceneGraphModifier.getModelFromTG(fittingInfoFrom_.getTransformGroup());
298  if (model != null) {
299  strFirst = model.getName();
300  }
301  if (strSecond.equals(strFirst))
302  return;
303 
304  fittingInfoTo_.setPickable(false);
305  fittingInfoTo_.setIntersectPoint(intersectVW_);
306  fittingInfoTo_.setPrimitiveCoordinates(verticesVW_);
307  fittingInfoTo_.setTransformGroup(tg);
308 
309  fittingInfoTo_.addForDisplay();
310 
311  bHaveSecond = true;
312  break;
313  }
314  }
315 
316  private void _disableIndicator() {
317  if (bHaveFirst) {
318  bHaveFirst = false;
319  fittingInfoFrom_.removeForDisplay();
320  fittingInfoFrom_.setTransformGroup(null);
321  }
322 
323  if (bHaveSecond) {
324  bHaveSecond = false;
325  fittingInfoTo_.removeForDisplay();
326  fittingInfoTo_.setTransformGroup(null);
327  }
328  }
329 
330  private boolean _isSameSign(float x, float y) {
331  return (((x >= 0) || (y >= 0)) && ((x <= 0) || (y <= 0)));
332  }
333 
334  private boolean _isSameSignVector3f(Vector3f v1, Vector3f v2) {
335  return (
336  _isSameSign(v1.x, v2.x) &&
337  _isSameSign(v1.y, v2.y) &&
338  _isSameSign(v1.z, v2.z)
339  );
340  }
341 
342  private void _transformChanged(BehaviorInfo info, TransformGroup tg) {
343  GrxModelItem model = SceneGraphModifier.getModelFromTG(tg);
344  if (model == null) {
345  System.out.println("no manipulatable.");
346  return;
347  }
348  model.calcForwardKinematics();
349  model.updateInitialTransformRoot();
350  }
351 }
int intersection(const fLineVec &lv1, const fLineVec &lv2, fVec3 &c1, fVec3 &c2, double &d, double eps)
Definition: fLineVec.cpp:36
* x
Definition: IceUtils.h:98
#define null
our own NULL pointer
Definition: IceTypes.h:57
item corresponds to a robot model
backing_store_ptr info
Definition: jmemsys.h:181
* y
Definition: IceUtils.h:97


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