InvKinemaHandler.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 
28 import com.sun.j3d.utils.picking.*;
29 
30 class InvKinemaHandler extends OperationHandler {
31  //--------------------------------------------------------------------
32  // 定数
33  public static final int FROM_MODE = 1;
34  public static final int ROTATION_MODE = 2;
35  public static final int TRANSLATION_MODE = 3;
36 
37  public static final int UNKNOWN = 0;
38  public static final int ROTATION_WITH_X = 1;
39  public static final int ROTATION_WITH_Y = 2;
40  public static final int ROTATION_WITH_Z = 4;
41  public static final int AXIS_FLAGS = 7;
42  public static final int MINUS = 8;
43 
44  public static final int XY_TRANSLATION = 1;
45  public static final int YZ_TRANSLATION = 2;
46  public static final int ZX_TRANSLATION = 4;
47 
48  private static final float ROTATION_FACTOR = (float)Math.PI / 360.0f;
49  private static final float ROTATION_LIMIT = (float)Math.PI / 360.0f;
50  private static final float TRANSLATION_FACTOR = 0.001f;
51  private static final float TRANSLATION_LIMIT = 0.003f;
52 
53  //--------------------------------------------------------------------
54  // インスタンス変数
55  protected int rotationMode_ = UNKNOWN;
56  protected int translationMode_ = UNKNOWN;
57 
58  private float limit_;
59 
61  protected Vector3f v3fAnotherAxis = new Vector3f();
62  protected Vector3f v3fAxisRotate = new Vector3f();
63 
65  //protected Transform3D t3dWorld = new Transform3D();
66  //protected Transform3D t3dView = new Transform3D();
67 
69  //protected Vector2f v2fAnotherAxis;
70  protected Vector2f v2fDetermine;
71 
73  protected Vector3f v3fAxisFirst = new Vector3f();
75  protected Vector3f v3fAxisSecond = new Vector3f();
76 
80  protected Point3f point000 = new Point3f(0,0,0);
81 
82  private Vector3f normal_;
83  private Point3f intersect_;
84 
85  private Switch bbSwitchFrom_;
86  private Switch bbSwitchTo_;
87 
88  //private boolean bBoundsSecond;
89 
90  private TransformGroup tgTarget_;
91 
92  private int mode_;
93  private Point startPoint_ = new Point();
94  private Point point_ = new Point();
95 
96  private InvKinemaResolver resolver_;
97 
98  private boolean isPicked_;
99 
100  //--------------------------------------------------------------------
101  // 公開メソッド
102  public void setInvKinemaMode(int mode) {
103  mode_ = mode;
104  }
105 
106  public void setInvKinemaResolver(InvKinemaResolver resolver) {
107  resolver_ = resolver;
108  }
109 
110  //--------------------------------------------------------------------
111  // BehaviorHandlerの実装
112  public void processPicking(MouseEvent evt, BehaviorInfo info) {
113  startPoint_.x = evt.getPoint().x;
114  startPoint_.y = evt.getPoint().y;
115 
116  intersect_ = null;
117  normal_ = null;
118  tgTarget_ = null;
119  isPicked_ = false;
120 
121  info.pickCanvas.setShapeLocation(startPoint_.x, startPoint_.y);
122  PickResult pickResult[] = info.pickCanvas.pickAllSorted();
123  if (pickResult == null) {
124  return;
125  }
126  TransformGroup tg = (TransformGroup)pickResult[0].getNode(PickResult.TRANSFORM_GROUP);
127  Point3d startPoint = info.pickCanvas.getStartPosition();
128  PickIntersection intersection = pickResult[0].getClosestIntersection(startPoint);
129  GrxModelItem model = SceneGraphModifier.getModelFromTG(tg);
130  if (model == null)
131  return;
132  else{
133  if(info.manager_.focusedItem()==model){
134  if( pickResult.length > 1){
135  tg = (TransformGroup)pickResult[1].getNode(PickResult.TRANSFORM_GROUP );
136  intersection = pickResult[1].getClosestIntersection(startPoint);
137  info.manager_.focusedItem(null);
138  }else
139  return;
140  }
141  }
142 
143  intersect_ = new Point3f(intersection.getPointCoordinates());
144  normal_ = new Vector3f(intersection.getPointNormal());
145  if (_setInvKinema(tg, info)) {
146  isPicked_ = true;
147  // ピックが成功してもconsume()しなければ、ViewHandlerが
148  // ピック処理をするので、視点の回転中心が設定される。
149  //evt.consume();
150  } else {
151  intersect_ = null;
152  normal_ = null;
153  }
154  }
155 
156  public void processStartDrag(MouseEvent evt, BehaviorInfo info) {
157  if (isPicked_) {
158  switch (mode_) {
159  case FROM_MODE:
160  break;
161  case ROTATION_MODE:
162  _decideRotationAxis(evt, info);
163  limit_ = ROTATION_LIMIT;
164  break;
165  case TRANSLATION_MODE:
166  _decideTranslationAxis(evt, info);
167  limit_ = TRANSLATION_LIMIT;
168  break;
169  default:
170  break;
171  }
172  evt.consume();
173  }
174  }
175 
176  public void processDragOperation(MouseEvent evt, BehaviorInfo info) {
177  if (isPicked_) {
178  switch (mode_) {
179  case FROM_MODE:
180  break;
181  case ROTATION_MODE:
182  case TRANSLATION_MODE:
183  point_.x = evt.getPoint().x;
184  point_.y = evt.getPoint().y;
185  break;
186  default:
187  break;
188  }
189  evt.consume();
190  }
191  }
192 
193  public void processReleased(MouseEvent evt, BehaviorInfo info) {
194  if (isPicked_) {
195  evt.consume();
196  }
197  }
198 
199  public boolean processTimerOperation(BehaviorInfo info) {
200  switch (mode_) {
201  case FROM_MODE:
202  break;
203  case ROTATION_MODE:
204  _rotation(info);
205  break;
206  case TRANSLATION_MODE:
207  _translation(info);
208  break;
209  default:
210  break;
211  }
212  ((Grx3DView)info.drawable).showOption();
213  return true;
214  }
215 
216  //--------------------------------------------------------------------
217  // OperationHandlerの実装
218  public void disableHandler() {
219  _disableBoundingBox();
220  }
221 
222  public void setPickTarget(TransformGroup tg, BehaviorInfo info) {
223  switch(mode_) {
224  case FROM_MODE:
225  _disableBoundingBox();
226  _enableBoundingBoxFrom(tg);
227  break;
228  case ROTATION_MODE:
229  case TRANSLATION_MODE:
230  if (bbSwitchFrom_ == null) {
231  return;
232  }
233 
234  if (tgTarget_ != tg) {
235  _enableBoundingBoxTo(tg);
236  }
237  break;
238  }
239  }
240 
241  //--------------------------------------------------------------------
242  // プライベートメソッド
243  private void _disableBoundingBox() {
244  if (bbSwitchFrom_ != null) {
245  bbSwitchFrom_.setWhichChild(Switch.CHILD_NONE);
246  }
247 
248  if (bbSwitchTo_ != null) {
249  bbSwitchTo_.setWhichChild(Switch.CHILD_NONE);
250  }
251 
252  tgTarget_ = null;
253  bbSwitchFrom_ = null;
254  bbSwitchTo_ = null;
255  }
256 
257  private boolean _setInvKinema(TransformGroup tg, BehaviorInfo info) {
258  switch(mode_) {
259  case FROM_MODE:
260  _disableBoundingBox();
261  if (!_enableBoundingBoxFrom(tg)) {
262  return false;
263  }
264  break;
265  case ROTATION_MODE:
266  if (bbSwitchFrom_ == null) {
267  return false;
268  }
269 
270  if (tgTarget_ != tg) {
271  if (!_enableBoundingBoxTo(tg)) {
272  return false;
273  }
274  }
275  _setRotationMode();
276  break;
277  case TRANSLATION_MODE:
278  if (bbSwitchFrom_ == null) {
279  return false;
280  }
281 
282  if (tgTarget_ != tg) {
283  if (!_enableBoundingBoxTo(tg)) {
284  return false;
285  }
286  }
287  _setTranslationMode();
288  break;
289  default:
290  return false;
291  }
292  return true;
293  }
294 
295  private boolean _enableBoundingBoxFrom(TransformGroup tg) {
296  GrxModelItem model = SceneGraphModifier.getModelFromTG(tg);
297  if (model == null) {
298  return false;
299  }
300 
301  GrxLinkItem link = SceneGraphModifier.getLinkFromTG(tg);
302  if (link == null) {
303  return false;
304  }
305 
306  resolver_.setFromJoint(model, link);
307  bbSwitchFrom_ = link.getBBSwitch();
308  bbSwitchFrom_.setWhichChild(Switch.CHILD_ALL);
309  return true;
310  }
311 
312  private boolean _enableBoundingBoxTo(TransformGroup tg) {
313  if (bbSwitchTo_ != null) {
314  bbSwitchTo_.setWhichChild(Switch.CHILD_NONE);
315  }
316  GrxLinkItem link = SceneGraphModifier.getLinkFromTG(tg);
317  GrxModelItem model = SceneGraphModifier.getModelFromTG(tg);
318  bbSwitchTo_ = link.getBBSwitch();
319  // fromジョイントと同じジョイントをPickした場合
320  if (bbSwitchFrom_ == bbSwitchTo_) {
321  bbSwitchTo_ = null;
322  return false;
323  }
324  if (bbSwitchTo_ != null && resolver_.setToJoint(model, link)) {
325  bbSwitchTo_.setWhichChild(Switch.CHILD_ALL);
326  tgTarget_ = tg;
327  return true;
328  } else {
329  return false;
330  }
331  }
332 
333  private void _setRotationMode() {
334  if (Math.abs(normal_.x) == 1.0f) {
335  rotationMode_ = ROTATION_WITH_X;
336  } else if(Math.abs(normal_.y) == 1.0f) {
337  rotationMode_ = ROTATION_WITH_Y;
338  } else if(Math.abs(normal_.z) == 1.0f) {
339  rotationMode_ = ROTATION_WITH_Z;
340  }
341  }
342 
343  private void _setTranslationMode() {
344  if (Math.abs(normal_.x) == 1.0f) {
345  translationMode_ = YZ_TRANSLATION;
346  } else if(Math.abs(normal_.y) == 1.0f) {
347  translationMode_ = ZX_TRANSLATION;
348  } else if (Math.abs(normal_.z) == 1.0f) {
349  translationMode_ = XY_TRANSLATION;
350  }
351  }
352 
353  private void _decideTranslationAxis(MouseEvent evt, BehaviorInfo info) {
354  // 現在のモードにしたがった内積を取る対象の二軸の設定をします
355  switch(translationMode_) {
356  case XY_TRANSLATION:
357  v3fAxisFirst.set(1.0f,0.0f,0.0f);
358  v3fAxisSecond.set(0.0f,1.0f,0.0f);
359  break;
360  case YZ_TRANSLATION:
361  v3fAxisFirst.set(0.0f,1.0f,0.0f);
362  v3fAxisSecond.set(0.0f,0.0f,1.0f);
363  break;
364  case ZX_TRANSLATION:
365  v3fAxisFirst.set(0.0f,0.0f,1.0f);
366  v3fAxisSecond.set(1.0f,0.0f,0.0f);
367  break;
368  }
369 
370  // 軸を現在のワールド座標へ変換する
371  Transform3D t3dLocalToVworld = new Transform3D();
372  Transform3D t3dCurrent = new Transform3D();
373  Transform3D t3dWorld = new Transform3D();
374  Transform3D t3dView = new Transform3D();
375 
376  tgTarget_.getLocalToVworld(t3dLocalToVworld);
377  tgTarget_.getTransform(t3dCurrent);
378  // それぞれの単位ベクトルを transform するとワールド座標
379  // におけるそれぞれの軸の方向が得られる。
380 
381  // この TG がワールドへの変換に相当する
382  t3dLocalToVworld.mul(t3dCurrent);
383  t3dWorld.set(t3dLocalToVworld);
384 
385  TransformGroup tgView = info.drawable.getTransformGroupRoot();
386  tgView.getLocalToVworld(t3dLocalToVworld);
387  tgView.getTransform(t3dCurrent);
388 
389  // この TG がカメラへの変換に相当する
390  t3dLocalToVworld.mul(t3dCurrent);
391  t3dView.set(t3dLocalToVworld);
392  t3dView.invert();
393 
394  t3dView.mul(t3dWorld);
395  t3dView.transform(v3fAxisFirst);
396  t3dView.transform(v3fAxisSecond);
397 
398  point000.set(0f,0f,0f);
399  t3dView.transform(point000);
400  if (intersect_ != null) {
401  Point3f pointPicked = new Point3f(intersect_);
402  t3dView.transform(pointPicked);
403  }
404 
405  info.setTimerEnabled(true);
406  }
407 
408  private void _decideRotationAxis(MouseEvent evt, BehaviorInfo info) {
409  // 指定された時間が経過しました
410  // マウスの前回からの移動量をベクトルに直す
411  int dx = evt.getPoint().x - startPoint_.x;
412  int dy = evt.getPoint().y - startPoint_.y;
413 
414  Point2f pointMouse =
415  new Point2f(
416  (float)dx * ROTATION_FACTOR,
417  (float)dy * ROTATION_FACTOR
418  );
419  Vector2f v2fMouse = new Vector2f(pointMouse);
420 
421  v2fMouse.y *= -1.0f;
422 
423  // 最初の動きで回転の軸を決定します
424  Vector3f v3fAxisFirst = new Vector3f();
425  Vector3f v3fAxisSecond = new Vector3f();
426 
427  // 軸を現在のワールド座標へ変換する
428  Transform3D t3dLocalToVworld = new Transform3D();
429  Transform3D t3dCurrent = new Transform3D();
430  Transform3D t3dWorld = new Transform3D();
431  Transform3D t3dView = new Transform3D();
432 
433  tgTarget_.getLocalToVworld(t3dLocalToVworld);
434  tgTarget_.getTransform(t3dCurrent);
435  // それぞれの単位ベクトルを transform すると
436  // ワールド座標におけるそれぞれの軸の方向が得られる。
437  t3dLocalToVworld.mul(t3dCurrent); // ワールドへの変換に相当する
438  t3dWorld.set(t3dLocalToVworld);
439 
440  TransformGroup tgView = info.drawable.getTransformGroupRoot();
441  tgView.getLocalToVworld(t3dLocalToVworld);
442  tgView.getTransform(t3dCurrent);
443  t3dLocalToVworld.mul(t3dCurrent); // がカメラへの変換に相当する
444  t3dView.set(t3dLocalToVworld);
445  t3dView.invert();
446 
447  t3dView.mul(t3dWorld);
448 
449  switch(rotationMode_ & AXIS_FLAGS) {
450  case ROTATION_WITH_X:
451  v3fAxisFirst.set(0.0f,1.0f,0.0f);
452  v3fAxisSecond.set(0.0f,0.0f,1.0f);
453  break;
454  case ROTATION_WITH_Y:
455  v3fAxisFirst.set(0.0f,0.0f,1.0f);
456  v3fAxisSecond.set(1.0f,0.0f,0.0f);
457  break;
458  case ROTATION_WITH_Z:
459  v3fAxisFirst.set(1.0f,0.0f,0.0f);
460  v3fAxisSecond.set(0.0f,1.0f,0.0f);
461  break;
462  }
463 
464  t3dView.transform(v3fAxisFirst);
465  t3dView.transform(v3fAxisSecond);
466 
467  // 移動量を定義するためのラジアン
468  float fDotProductFirst = 0.0f;
469  float fDotProductSecond = 0.0f;
470 
471  // Canvas 平面へ投j影
472  Vector2f v2fAxisFirst = new Vector2f(v3fAxisFirst.x, v3fAxisFirst.y);
473  Vector2f v2fAxisSecond = new Vector2f(v3fAxisSecond.x, v3fAxisSecond.y);
474 
475  v2fAxisFirst.normalize();
476  v2fAxisSecond.normalize();
477 
478  fDotProductFirst = v2fAxisFirst.dot(v2fMouse);
479  fDotProductSecond = v2fAxisSecond.dot(v2fMouse);
480 
481  // 以前の回転軸の決定パターン
482  switch(rotationMode_ & AXIS_FLAGS) {
483  case ROTATION_WITH_X:
484  if (Math.abs(fDotProductFirst) < Math.abs(fDotProductSecond)) {
485  v3fAnotherAxis = v3fAxisSecond;
486  v3fAxisRotate.set(0.0f, 1.0f, 0.0f);
487  } else {
488  v3fAnotherAxis = v3fAxisFirst;
489  v3fAxisRotate.set(0.0f, 0.0f, 1.0f);
490  }
491  break;
492  case ROTATION_WITH_Y:
493  if (Math.abs(fDotProductFirst) < Math.abs(fDotProductSecond)) {
494  v3fAnotherAxis = v3fAxisSecond;
495  v3fAxisRotate.set(0.0f, 0.0f, 1.0f);
496  } else {
497  v3fAnotherAxis = v3fAxisFirst;
498  v3fAxisRotate.set(1.0f, 0.0f, 0.0f);
499  }
500  break;
501  case ROTATION_WITH_Z:
502  if (Math.abs(fDotProductFirst) < Math.abs(fDotProductSecond)) {
503  v3fAnotherAxis = v3fAxisSecond;
504  v3fAxisRotate.set(1.0f, 0.0f, 0.0f);
505  } else {
506  v3fAnotherAxis = v3fAxisFirst;
507  v3fAxisRotate.set(0.0f, 1.0f, 0.0f);
508  }
509  break;
510  }
511 
512  Vector3f v3fTemp = new Vector3f();
513  v3fTemp.set(v3fAxisRotate);
514  t3dView.transform(v3fAxisRotate);
515 
516  // 平面状の回転軸ベクトルから -90 度回転した二次元平面状の軸
517  // とマウスの動作のベクトルの内積が正だったらそのまま。
518  // 負だったら正負を反転して使う。
519 
520  Vector2f v2fAxisRotate = new Vector2f(v3fAxisRotate.x, v3fAxisRotate.y);
521 
522  //v2fAnotherAxis = new Vector2f(v3fAnotherAxis.x, v3fAnotherAxis.y);
523 
524  // 回転軸を -90 回転したもの
525  v2fDetermine = new Vector2f(v2fAxisRotate.y, -v2fAxisRotate.x);
526 
527  v3fAxisRotate.set(v3fTemp);
528 
529  v2fMouse.normalize();
530  //float fDetermine = v2fMouse.dot(v2fDetermine);
531 
532  info.setTimerEnabled(true);
533  }
534 
535  private void _translation(BehaviorInfo info) {
536  // 指定された時間が経過しました
537  // マウスの前回からの移動量をベクトルに直す
538  int dx = point_.x - startPoint_.x;
539  int dy = point_.y - startPoint_.y;
540  // ?_factor でマウスの移動量に対する回転量の倍率を指定
541  // マウスの動作においては下がプラスになっているので反転しておく
542  Point2f pointMouse =
543  new Point2f(
544  (float)dx * TRANSLATION_FACTOR,
545  (float)dy * TRANSLATION_FACTOR
546  );
547  Vector2f v2fMouse = new Vector2f(pointMouse);
548 
549  v2fMouse.y *= -1.0f;
550 
551  float fDotProductFirst = 0.0f; // 移動量を定義するためのラジアン
552  float fDotProductSecond = 0.0f; // 移動量を定義するためのラジアン
553 
554  // Canvas 平面へ投影
555  Vector2f v2fAxisFirst = new Vector2f(v3fAxisFirst.x, v3fAxisFirst.y);
556  Vector2f v2fAxisSecond = new Vector2f(v3fAxisSecond.x, v3fAxisSecond.y);
557 
558  // 軸の追加
559  if (v2fAxisFirst.length() == 0) {
560  if (intersect_ != null) {
561  Point3f pointPicked = new Point3f(intersect_);
562  pointPicked.sub(point000); // View からみた point 000 で割る
563  pointPicked.scale(-1.0f);
564  v3fAxisFirst.set(pointPicked);
565  v2fAxisFirst.set(v3fAxisFirst.x,v3fAxisFirst.y);
566  } else {
567  // 軸が判定できなかったとき用
568  v2fAxisFirst.set(0.0f,1.0f);
569  }
570  }
571 
572  if (v2fAxisSecond.length() == 0) {
573  if (intersect_ != null) {
574  Point3f pointPicked = new Point3f(intersect_);
575  pointPicked.sub(point000);
576  pointPicked.scale(-1.0f);
577  v3fAxisSecond.set(pointPicked);
578  v2fAxisSecond.set(v3fAxisSecond.x,v3fAxisSecond.y);
579  } else {
580  // 軸が判定できなかったとき用
581  v2fAxisSecond.set(1.0f,0.0f);
582  }
583  }
584  v2fAxisFirst.normalize();
585  v2fAxisSecond.normalize();
586 
587  fDotProductFirst = v2fAxisFirst.dot(v2fMouse);
588  fDotProductSecond = v2fAxisSecond.dot(v2fMouse);
589 
590  if (Float.isNaN(fDotProductFirst)) {
591  fDotProductFirst = 0;
592  }
593 
594  if (Float.isNaN(fDotProductSecond)) {
595  fDotProductSecond = 0;
596  }
597 
598  Transform3D t3dCur = new Transform3D();
599  Transform3D t3dTranslate = new Transform3D();
600  tgTarget_.getTransform(t3dCur);
601  Vector3f v3fTranslate = new Vector3f();
602 
603  // 閾値チェック
604  if (Math.abs(fDotProductFirst) > TRANSLATION_LIMIT) {
605  if (fDotProductFirst > TRANSLATION_LIMIT) {
606  fDotProductFirst = TRANSLATION_LIMIT;
607  } else {
608  fDotProductFirst = -TRANSLATION_LIMIT;
609  }
610  }
611  if (Math.abs(fDotProductSecond) > TRANSLATION_LIMIT) {
612  if (fDotProductSecond > TRANSLATION_LIMIT) {
613  fDotProductSecond = TRANSLATION_LIMIT;
614  } else {
615  fDotProductSecond = -TRANSLATION_LIMIT;
616  }
617  }
618 
619  switch(translationMode_) {
620  case XY_TRANSLATION:
621  v3fTranslate.x += fDotProductFirst;
622  v3fTranslate.y += fDotProductSecond;
623  break;
624  case YZ_TRANSLATION:
625  v3fTranslate.y += fDotProductFirst;
626  v3fTranslate.z += fDotProductSecond;
627  break;
628  case ZX_TRANSLATION:
629  v3fTranslate.z += fDotProductFirst;
630  v3fTranslate.x += fDotProductSecond;
631  break;
632  }
633 
634  t3dTranslate.set(v3fTranslate);
635  t3dCur.mul(t3dTranslate);
636  tgTarget_.setTransform(t3dCur);
637 
638  _resolve();
639  // 変換情報を通知
640  //broadcastEvent();
641  }
642 
643  private void _rotation(BehaviorInfo info) {
644  int dx = point_.x - startPoint_.x;
645  int dy = point_.y - startPoint_.y;
646 
647  Point2f pointMouse = new Point2f(
648  (float)dx * ROTATION_FACTOR,
649  (float)dy * ROTATION_FACTOR
650  );
651  Vector2f v2fMouse = new Vector2f(pointMouse);
652  // Swing だと不要?
653  v2fMouse.y *= -1.0f;
654  float fDotProductRotate = v2fDetermine.dot(v2fMouse);
655 
656  Transform3D t3dCur = new Transform3D();
657  Transform3D t3dRotate = new Transform3D();
658  tgTarget_.getTransform(t3dCur);
659 
660  // 閾値チェック
661  if (Math.abs(fDotProductRotate) > ROTATION_LIMIT) {
662  if (fDotProductRotate > ROTATION_LIMIT) {
663  fDotProductRotate = ROTATION_LIMIT;
664  }
665  } else {
666  fDotProductRotate = - ROTATION_LIMIT;
667  }
668 
669  AxisAngle4f axis =
670  new AxisAngle4f(v3fAxisRotate, fDotProductRotate);
671  t3dRotate.set(axis);
672  t3dCur.mul(t3dRotate);
673  tgTarget_.setTransform(t3dCur);
674 
675  _resolve();
676  // 変換情報を通知
677  //broadcastEvent();
678  }
679 
680  private void _resolve() {
681  Transform3D t3dCurrent = new Transform3D();
682  tgTarget_.getTransform(t3dCurrent);
683  Transform3D t3dLocalToVworld = new Transform3D();
684  tgTarget_.getLocalToVworld(t3dLocalToVworld);
685  Transform3D t3dCur = new Transform3D();
686  tgTarget_.getTransform(t3dCur);
687 
688  // まず Tn の変換を求める
689  // Transform3D 同士の変換を結合する
690  t3dLocalToVworld.mul(t3dCur);
691  t3dCurrent = t3dLocalToVworld;
692 
693  if (resolver_.resolve(t3dCurrent)) {
694  // Bounding Box の resize
695  // 成功したら limit 値をあげる
696  if (mode_ == ROTATION_MODE) {
697  limit_ += (float)(Math.PI/1440);
698  } else {
699  limit_ += 0.0001f;
700  }
701  } else {
702  // 失敗したら limit 値をさげる
703  if (mode_ == ROTATION_MODE) {
704  if (limit_ > (float)Math.PI/1440) {
705  limit_ -= Math.PI/1440;
706  }
707  } else {
708  if (limit_ > 0.0001f) {
709  limit_ -= 0.0001f;
710  }
711  }
712  }
713  }
714 }
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
UNKNOWN
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