00001
00002
00003
00004
00005
00006
00007
00008
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
00066
00067
00069
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
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
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
00148
00149
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
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
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
00379
00380
00381
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
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
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
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
00517
00518
00519
00520 Vector2f v2fAxisRotate = new Vector2f(v3fAxisRotate.x, v3fAxisRotate.y);
00521
00522
00523
00524
00525 v2fDetermine = new Vector2f(v2fAxisRotate.y, -v2fAxisRotate.x);
00526
00527 v3fAxisRotate.set(v3fTemp);
00528
00529 v2fMouse.normalize();
00530
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
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
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);
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
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
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
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
00689
00690 t3dLocalToVworld.mul(t3dCur);
00691 t3dCurrent = t3dLocalToVworld;
00692
00693 if (resolver_.resolve(t3dCurrent)) {
00694
00695
00696 if (mode_ == ROTATION_MODE) {
00697 limit_ += (float)(Math.PI/1440);
00698 } else {
00699 limit_ += 0.0001f;
00700 }
00701 } else {
00702
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 }