]> gerrit.simantics Code Review - simantics/3d.git/blob - org.simantics.g3d.vtk/src/org/simantics/g3d/vtk/swt/RotateAction.java
1b2d791432a7c4c8037e03cc669694b51380abd1
[simantics/3d.git] / org.simantics.g3d.vtk / src / org / simantics / g3d / vtk / swt / RotateAction.java
1 /*******************************************************************************
2  * Copyright (c) 2012, 2013 Association for Decentralized Information Management in
3  * Industry THTH ry.
4  * All rights reserved. This program and the accompanying materials
5  * are made available under the terms of the Eclipse Public License v1.0
6  * which accompanies this distribution, and is available at
7  * http://www.eclipse.org/legal/epl-v10.html
8  *
9  * Contributors:
10  *     VTT Technical Research Centre of Finland - initial API and implementation
11  *******************************************************************************/
12 package org.simantics.g3d.vtk.swt;
13
14
15 import java.awt.event.KeyEvent;
16 import java.awt.event.MouseEvent;
17
18 import javax.vecmath.AxisAngle4d;
19 import javax.vecmath.Point3d;
20 import javax.vecmath.Quat4d;
21 import javax.vecmath.Vector3d;
22
23 import org.eclipse.swt.SWT;
24 import org.eclipse.swt.graphics.Cursor;
25 import org.eclipse.swt.widgets.Display;
26 import org.simantics.g3d.math.EulerTools;
27 import org.simantics.g3d.math.EulerTools.Order;
28 import org.simantics.g3d.math.MathTools;
29 import org.simantics.g3d.math.Ray;
30 import org.simantics.g3d.preferences.PreferenceConstants;
31 import org.simantics.g3d.scenegraph.IG3DNode;
32 import org.simantics.g3d.scenegraph.structural.IStructuralNode;
33 import org.simantics.g3d.vtk.Activator;
34 import org.simantics.g3d.vtk.common.VTKNodeMap;
35 import org.simantics.g3d.vtk.gizmo.RotateAxisGizmo;
36 import org.simantics.g3d.vtk.utils.vtkUtil;
37 import org.simantics.utils.threads.AWTThread;
38 import org.simantics.utils.threads.ThreadUtils;
39
40 import vtk.vtkProp;
41 /**
42  * FIXME: complete rewrite.
43  * 
44  * @author Marko Luukkainen <marko.luukkainen@vtt.fi>
45  *
46  */
47 public class RotateAction extends vtkSwtAction{
48         
49         public static final int X = 0;
50     public static final int Y = 1;
51     public static final int Z = 2;
52     public static final int P = 3;
53
54         private VTKNodeMap nodeMap;
55         //private TranslateGizmo  gizmo = new TranslateGizmo();
56         private RotateAxisGizmo gizmo = new RotateAxisGizmo();
57         private IG3DNode node;
58         
59         
60         
61         private Cursor activeCursor;// = Cursor.getPredefinedCursor(Cursor.HAND_CURSOR);
62         private Cursor dragCursor;// = Cursor.getPredefinedCursor(Cursor.MOVE_CURSOR);
63         
64         
65         int stepMethod = 1;
66         Order order = Order.YXZ;
67         
68     private int steps; 
69     private double angles[];
70         
71         int index = P;
72         boolean valid = false;
73         private boolean worldCoord = true;
74         //private AxisAngle4d aa = null;
75         private Quat4d parentWorldOrientation = null;
76         
77         //AxisAngle4d rotation = new AxisAngle4d();
78         Quat4d worldOrientation = new Quat4d();
79         
80         public void setNode(IG3DNode node) {
81                 this.node = node;
82                 if ((node instanceof IStructuralNode) && ((IStructuralNode)node).isPartOfInstantiatedModel() && !((IStructuralNode)node).isInstantiatedModelRoot()) {
83                         setEnabled(false);
84                 } else {
85                         setEnabled(true);
86                 }
87                 
88                 String set = org.simantics.g3d.Activator.getDefault().getPreferenceStore().getString(PreferenceConstants.ORIENTATION_PRESENTATION);
89                 if (set.equals("aa")) {
90                         stepMethod = 0;
91                 } else if (set.equals("euler")){
92                         stepMethod = 1;
93                         String eulerOrder = org.simantics.g3d.Activator.getDefault().getPreferenceStore().getString(PreferenceConstants.EULER_ANGLE_ORDER);
94                         try {
95                                 order = Order.valueOf(eulerOrder);
96                         } catch (Exception e) {
97                                 order = Order.YXZ;
98                         }
99                 } else {
100                         stepMethod = 2;
101                 }
102         }
103         
104         public IG3DNode getNode() {
105                 return node;
106         }
107         
108         public RotateAction(InteractiveVtkComposite panel, VTKNodeMap nodeMap) {
109                 super(panel);
110                 setImageDescriptor(Activator.imageDescriptorFromPlugin("com.famfamfam.silk", "icons/arrow_rotate_clockwise.png"));
111                 setText("Rotate");
112                 this.nodeMap = nodeMap;
113                 
114                 
115                 steps = 36;
116         angles = new double[steps+1];
117         for (int i = 0; i < angles.length; i++) {
118             angles[i] = - Math.PI + (Math.PI * i * 2.0 / steps);
119         }
120         
121         activeCursor = Display.getCurrent().getSystemCursor(SWT.CURSOR_HAND);
122         dragCursor = Display.getCurrent().getSystemCursor(SWT.CURSOR_CROSS);
123         }
124         
125         public void attach() {
126                 if (node == null)
127                         return;
128                 
129                 super.attach();
130                 ThreadUtils.asyncExec(panel.getThreadQueue(), new Runnable() {
131                         public void run() {
132                                 attachUI();
133                                 update();
134                         }
135                 });
136                 
137                 
138                 
139         }
140         
141         public void deattach() {
142                 
143                 node = null;
144                 nodeMap.commit();
145                 deattachUI();
146                 super.deattach();
147                 panel.refresh();
148         }
149         
150         private void attachUI() {
151                 panel.getComponent().setCursor(activeCursor);
152                 gizmo.attach(panel);
153         }
154         
155         private void deattachUI() {
156                 panel.getComponent().setCursor(Display.getCurrent().getSystemCursor(SWT.CURSOR_ARROW));
157                 gizmo.deattach();
158         }
159         
160         @Override
161         public boolean keyPressed(KeyEvent e) {
162                 if (e.getKeyCode() == KeyEvent.VK_ESCAPE)
163                         panel.useDefaultAction();
164                 if (valid)
165                         return true;
166                 if (e.getKeyCode() == KeyEvent.VK_X) {
167                         if (index != X)
168                                 index = X;
169                         else
170                                 index = P;
171                 }
172                 if (e.getKeyCode() == KeyEvent.VK_Y) {
173                         if (index != Y)
174                                 index = Y;
175                         else
176                                 index = P;
177                 }
178                 if (e.getKeyCode() == KeyEvent.VK_Z) {
179                         if (index != Z)
180                                 index = Z;
181                         else
182                                 index = P;
183                 }
184                 if (e.getKeyCode() == KeyEvent.VK_G) {
185                         worldCoord = !worldCoord;
186                 }
187                 gizmo.setType(index);
188                 panel.refresh();
189                 return true;
190         }
191         
192         @Override
193         public boolean keyReleased(KeyEvent e) {
194                 return false;
195         }
196         
197         
198         
199         @Override
200         public boolean mouseClicked(MouseEvent e) {
201                 if (e.getClickCount() > 1) {
202                         if (isOverNode(e)) {
203                                 return true;
204                         }
205                         panel.useDefaultAction();
206                         //if(!gizmo.isPartOf(actor))
207                         //      panel.useDefaultAction();
208                         return true;
209                 }
210                 return false;
211         }
212         
213         
214         
215         
216
217         
218         
219         public void setWorldCoord(boolean b) {
220                 if (worldCoord == b)
221                         return;
222                 worldCoord = b;
223                 update();
224                                         
225         }
226         
227         
228         private void update() {
229                 Vector3d nodePos = node.getWorldPosition();
230                 System.out.println(nodePos);
231                 gizmo.setPosition(nodePos);
232                 if (worldCoord) {
233                         gizmo.setRotation(new AxisAngle4d());
234                         parentWorldOrientation = null;
235                 } else {
236                         AxisAngle4d aa = new AxisAngle4d();
237                         parentWorldOrientation = ((IG3DNode)node.getParent()).getWorldOrientation();
238                         aa.set(parentWorldOrientation);
239                         gizmo.setRotation(aa);
240                 }
241
242                 Point3d camPos = new Point3d(panel.getRenderer().GetActiveCamera().GetPosition());
243                 Vector3d p = new Vector3d(nodePos);
244                 p.sub(camPos);
245                 
246                 if (parentWorldOrientation != null) {
247                         Quat4d qi = new Quat4d(parentWorldOrientation);
248                         qi.inverse();
249                         MathTools.rotate(parentWorldOrientation, p, p);
250                 }
251                 if (panel.getRenderer().GetActiveCamera().GetParallelProjection() == 0) {
252                         double distance = p.length();
253                         p.negate();
254             double fov = panel.getRenderer().GetActiveCamera().GetViewAngle();
255             float s = (float) (Math.sin(fov) * distance * 0.1); 
256
257             Vector3d scale = new Vector3d(1., 1., 1.);
258             
259 //            if (p.x > 0.f)
260 //                scale.x = -1.;
261 //            if (p.y > 0.f)
262 //                scale.y = -1.;
263 //            if (p.z > 0.f)
264 //                scale.z = -1.;
265             scale.scale(s);
266             gizmo.setScale(scale);
267                         
268                 } else {
269                         Vector3d scale = new Vector3d(1.f, 1.f, 1.f);
270             double s = panel.getRenderer().GetActiveCamera().GetParallelScale() / 5.;
271 //            if (p.x > 0.f)
272 //                scale.x = -1.;
273 //            if (p.y > 0.f)
274 //                scale.y = -1.;
275 //            if (p.z > 0.f)
276 //                scale.z = -1.;
277             scale.scale(s);
278             gizmo.setScale(scale);
279                 }
280                 
281                 panel.refresh();
282         }
283         
284         private boolean isOverNode(MouseEvent e) {
285                 vtkProp picked[] = panel.pick(e.getX(), e.getY());
286                 if (picked !=null) {
287                         for (int i = 0; i < picked.length; i++) {
288                                 if (node.equals(nodeMap.getNode(picked[i])))
289                                         return true;
290                         }
291                 }
292                 return false;
293         }
294         
295
296         
297         @Override
298         public boolean mousePressed(MouseEvent e) {
299                 if (e.getButton() == MouseEvent.BUTTON1) {
300         
301         
302                         if (isOverNode(e)) {
303                                 valid = true;
304                                 if ((e.getModifiers() & MouseEvent.CTRL_MASK) > 0) {
305                                         useStep = true;
306                     } else {
307                         useStep = false;
308                     }
309                                 worldOrientation = node.getWorldOrientation();
310                                 doChanges(true, e.getX(), e.getY());
311                                 
312                                 panel.getComponent().setCursor(dragCursor);
313                         } else {
314                                 valid = false;
315                                 getDefaultAction().mousePressed(e);
316                                 panel.getComponent().setCursor(activeCursor);
317                         }
318                 } else {
319                         getDefaultAction().mousePressed(e);
320                 }
321                 return true;
322         }
323         
324         
325         
326         @Override
327         public boolean mouseReleased(MouseEvent e) {
328                 if (e.getButton() == MouseEvent.BUTTON1) {
329                         valid = false;
330                         worldOrientation = null;
331                         panel.getComponent().setCursor(activeCursor);
332                 } else {
333                         getDefaultAction().mouseReleased(e);
334                 }
335                 return true;
336         }
337         
338         @Override
339         public boolean mouseDragged(MouseEvent e) {
340                 if ((e.getModifiersEx() & MouseEvent.BUTTON1_DOWN_MASK) > 0 && valid) { 
341                         if ((e.getModifiers() & MouseEvent.CTRL_MASK) > 0) {
342                                 useStep = true;
343             } else {
344                 useStep = false;
345             }
346                         doChanges(false, e.getX(), e.getY());
347                         
348                         //nodeMap.modified(node);
349                         update();
350                 } else {
351                         getDefaultAction().mouseDragged(e);
352                         update();
353                 }
354                 return true;
355         }
356         
357          Vector3d axis = null;
358         
359         @Override
360         public boolean keyTyped(KeyEvent e) {
361                  if (e.getKeyCode() == KeyEvent.VK_LEFT) {
362                 inputType = InputType.KEY;
363                 axis = new Vector3d(0.0,1.0,0.0);
364              } else if (e.getKeyCode() == KeyEvent.VK_RIGHT) {
365                         inputType = InputType.KEY;
366                         axis = new Vector3d(0.0,-1.0,0.0);
367              } else if (e.getKeyCode() ==KeyEvent.VK_UP) {
368                         inputType = InputType.KEY;
369                         axis = new Vector3d(1.0,0.0,0.0);
370              } else if (e.getKeyCode() == KeyEvent.VK_DOWN) {
371                         inputType = InputType.KEY;
372                         axis = new Vector3d(-1.0,0.0,0.0);
373              } 
374                  return true;
375         }
376         
377     public void doChanges(boolean pressed, int x, int y) {
378         Ray ray = vtkUtil.createMouseRay(panel.getRenderer(),x, y);
379         Vector3d p = node.getWorldPosition();
380         
381         if (pressed) {
382             Vector3d axis = getRotationAxis();
383             if (axis != null) {
384                 if (!worldCoord) {
385                         MathTools.rotate(parentWorldOrientation, axis, axis);
386                 }
387
388                
389                 double s[] = new double[2];
390                 Vector3d i2 = new Vector3d();
391                
392                 boolean intersect = MathTools.intersectStraightPlane(ray.pos, ray.dir, p, axis, i2, s);
393                 double dot = Math.abs(ray.dir.dot(axis));
394                 if (intersect &&  dot > 0.4)
395                         inputType = InputType.INTERSECT;
396                 else
397                         inputType = InputType.NONINTERSECT;
398                         
399                 
400                 if (inputType == InputType.INTERSECT) {
401                     // picking ray and plane defined by gizmo's center point and
402                     // rotation axis can intersect
403                     // vector from center point to intersection point
404                     i2.sub(p);
405                     // creating vectors i and j that are lying on the plane and
406                     // are perpendicular
407                     // vectors are used to calculate polar coordinate for
408                     // intersection point
409                     j.set(i2);
410                     i.cross(j, axis);
411                     System.out.println("I,J " + i + " " + j);
412                     double angleI = i2.angle(i);
413                     double angleJ = i2.angle(j);
414                     prevAngle = Math.atan2(Math.cos(angleJ), Math.cos(angleI));
415                 } else {
416                     // picking ray and plane defined by gizmo's center point and
417                     // rotation axis are parallel,
418                     // so we'll use cross product of rotation axis and picking
419                     // ray to detect amount of rotation
420                     i.cross(ray.dir, axis);
421                     MathTools.intersectStraightStraight(ray.pos, ray.dir, p, i, new Vector3d(), new Vector3d(), s);
422                     prevS = s[1];
423                 }
424             }
425            
426             
427         }
428
429         if (inputType != InputType.KEY)
430                 axis = getRotationAxis();
431         if (axis == null) {
432             return;   
433         }
434         Vector3d taxis = null;
435         if (!worldCoord) {
436                 taxis = new Vector3d(axis);
437                 MathTools.rotate(parentWorldOrientation, axis, axis);
438         }
439         System.out.println(inputType);
440         if (inputType == InputType.INTERSECT) {
441
442             double s[] = new double[2];
443             Vector3d i2 = new Vector3d();
444             MathTools.intersectStraightPlane(ray.pos, ray.dir, p, axis, i2, s);
445             i2.sub(p);
446             double angleI = i2.angle(i);
447             double angleJ = i2.angle(j);
448             double angle = Math.atan2(Math.cos(angleJ), Math.cos(angleI));
449             System.out.println("Angle " + angle + " i " + angleI + " j " + angleJ + " prev " + prevAngle);
450             if(!worldCoord)
451                 axis = taxis;
452             if (useStep) {
453
454                 //setOrientation(MathTools.getQuat(rotation));
455                 AxisAngle4d rot = new AxisAngle4d(axis,angle-prevAngle);
456                 Quat4d qrot = new Quat4d();
457                 MathTools.getQuat(rot, qrot);
458                 //prevAngle = angle;
459                 qrot.mulInverse(worldOrientation);
460                 
461           
462                 if (stepMethod == 0) {
463                         rot.set(qrot);
464                         rot.angle = roundAngle(rot.angle);
465                         //qrot.set(rot);
466                         MathTools.getQuat(rot,qrot);
467                         setOrientation(qrot);
468                 } else if (stepMethod == 1){
469                           
470                         //Vector3d euler = MathTools.getEuler(qrot);
471                         Vector3d euler = EulerTools.getEulerFromQuat(order, qrot);
472                         euler.x = roundAngle(euler.x);
473                         euler.y = roundAngle(euler.y);
474                         euler.z = roundAngle(euler.z);
475                         //Quat4d q = MathTools.getQuat(euler);
476                         Quat4d q = EulerTools.getQuatFromEuler(order, euler);
477                         setOrientation(q);
478                         System.out.println(" (" + MathTools.radToDeg(euler.x) + " " + MathTools.radToDeg(euler.y) + " " + MathTools.radToDeg(euler.z) +  ") " + qrot + " "+ q);
479                 } else {
480                         setOrientation(qrot);
481                 }
482                 
483             } else {
484                 if (worldCoord) {
485                         //G3DTools.multiplyOrientation(mo.getG3DNode(graph).getWorldOrientation(), new AxisAngle4d(axis,angle-prevAngle));
486                         AxisAngle4d aa = MathTools.getAxisAngle(node.getWorldOrientation());
487                         AxisAngle4d rot = new AxisAngle4d(axis,angle-prevAngle);
488                         MathTools.multiplyOrientation(aa, rot);
489                         setWorldOrientation(MathTools.getQuat(rot));
490                 } else {
491                         AxisAngle4d aa = MathTools.getAxisAngle(node.getOrientation());
492                         AxisAngle4d rot = new AxisAngle4d(axis,angle-prevAngle);
493                         MathTools.multiplyOrientation(aa, rot);
494                         setOrientation(MathTools.getQuat(rot));
495                         //G3DTools.multiplyOrientation(mo.getG3DNode(graph).getLocalOrientation(), new AxisAngle4d(axis,angle-prevAngle));
496                 }
497                 prevAngle = angle;
498             }
499             
500         } else if (inputType == InputType.NONINTERSECT){
501
502             double s[] = new double[2];
503             MathTools.intersectStraightStraight(ray.pos, ray.dir, p, i, new Vector3d(), new Vector3d(), s);
504             if(!worldCoord)
505                 axis = taxis;
506             if (useStep) {
507                 //setOrientation(MathTools.getQuat(rotation));
508                 AxisAngle4d rot = new AxisAngle4d(axis,s[1] - prevS);
509                 
510                 Quat4d qrot = new Quat4d();
511                 //qrot.set(rot);
512                 MathTools.getQuat(rot, qrot);
513                 //prevAngle = angle;
514                 qrot.mulInverse(worldOrientation);
515                 
516           
517                 if (stepMethod == 0) {
518                         rot.set(qrot);
519                         rot.angle = roundAngle(rot.angle);
520                         //qrot.set(rot);
521                         MathTools.getQuat(rot,qrot);
522                         setOrientation(qrot);
523                 } else if (stepMethod == 1){
524                           
525                         //Vector3d euler = MathTools.getEuler(qrot);
526                         Vector3d euler = EulerTools.getEulerFromQuat(order, qrot);
527                         euler.x = roundAngle(euler.x);
528                         euler.y = roundAngle(euler.y);
529                         euler.z = roundAngle(euler.z);
530                         //Quat4d q = MathTools.getQuat(euler);
531                         Quat4d q = EulerTools.getQuatFromEuler(order, euler);
532                         setOrientation(q);
533                         System.out.println(" (" + MathTools.radToDeg(euler.x) + " " + MathTools.radToDeg(euler.y) + " " + MathTools.radToDeg(euler.z) +  ") " + qrot + " "+ q);
534                 } else {
535                         setOrientation(qrot);
536                 }
537                 prevS = s[1];
538                 
539 //                    G3DTools.setOrientation(mo.getG3DNode(graph).getLocalOrientation(), rotations.get(mo));
540 //                    G3DTools.multiplyOrientation(mo.getG3DNode(graph).getWorldOrientation(), new AxisAngle4d(axis,s[1] - prevS));
541 //                    AxisAngle4d aa = G3DTools.getOrientation(mo.getG3DNode(graph).getLocalOrientation());
542 //                    rotations.put(mo, aa);
543 //                    Vector3d euler = MathTools.getEuler(aa);
544 //                    euler.x = roundAngle(euler.x);
545 //                    euler.y = roundAngle(euler.y);
546 //                    euler.z = roundAngle(euler.z);
547 //                    aa = MathTools.getFromEuler2(euler);
548 //                    prevS = s[1];
549 //                    G3DTools.setOrientation(mo.getG3DNode(graph).getLocalOrientation(), aa);
550 //                    Vector3d e = MathTools.getEuler(G3DTools.getOrientation(mo.getG3DNode(graph).getLocalOrientation()));
551 //                    e.scale(180.0/Math.PI);
552 //                    text += G3DTools.getOrientation(mo.getG3DNode(graph).getLocalOrientation()) + " " + e + " ";
553                 
554                 
555             } else {
556                         if (worldCoord) {
557                                 AxisAngle4d aa =  MathTools.getAxisAngle(node.getWorldOrientation());
558                         AxisAngle4d rot = new AxisAngle4d(axis,s[1] - prevS);
559                         MathTools.multiplyOrientation(aa, rot);
560                         setWorldOrientation(MathTools.getQuat(rot));
561                                 //G3DTools.multiplyOrientation(mo.getG3DNode(graph).getWorldOrientation(), new AxisAngle4d(axis,s[1] - prevS));
562                         } else {
563                                 AxisAngle4d aa =  MathTools.getAxisAngle(node.getOrientation());
564                         AxisAngle4d rot = new AxisAngle4d(axis,s[1] - prevS);
565                         MathTools.multiplyOrientation(aa, rot);
566                         setOrientation(MathTools.getQuat(rot));
567                                 //G3DTools.multiplyOrientation(mo.getG3DNode(graph).getLocalOrientation(), new AxisAngle4d(axis,s[1] - prevS));
568                         }
569                         //text += G3DTools.getOrientation(mo.getG3DNode(graph).getLocalOrientation()) + " " + MathTools.getEuler(G3DTools.getOrientation(mo.getG3DNode(graph).getLocalOrientation())) + " ";
570                 prevS = s[1];
571                 
572             }
573            
574         } else {
575                 if (worldCoord) {
576                         AxisAngle4d aa = MathTools.getAxisAngle(node.getWorldOrientation());
577                 AxisAngle4d rot = new AxisAngle4d(axis,Math.PI * 0.5);
578                 MathTools.multiplyOrientation(aa, rot);
579                 setWorldOrientation(MathTools.getQuat(rot));
580                         //G3DTools.multiplyOrientation(mo.getG3DNode(graph).getWorldOrientation(), new AxisAngle4d(axis,Math.PI * 0.5));
581                 } else {
582                         AxisAngle4d aa = MathTools.getAxisAngle(node.getOrientation());
583                 AxisAngle4d rot = new AxisAngle4d(axis,Math.PI * 0.5);
584                 MathTools.multiplyOrientation(aa, rot);
585                 setOrientation(MathTools.getQuat(rot));
586                         //G3DTools.multiplyOrientation(mo.getG3DNode(graph).getLocalOrientation(), new AxisAngle4d(axis,Math.PI * 0.5));
587                 }
588              //   text += G3DTools.getOrientation(mo.getG3DNode(graph).getLocalOrientation()) + " " + MathTools.getEuler(G3DTools.getOrientation(mo.getG3DNode(graph).getLocalOrientation())) + " ";
589             
590         }
591         //setInfoText(text);
592  
593     }
594     
595     protected void setOrientation(Quat4d q) {
596         node.setOrientation(q);
597     }
598     
599     protected void setWorldOrientation(Quat4d q) {
600         node.setWorldOrientation(q);
601     }
602         
603         @Override
604         public boolean mouseMoved(MouseEvent e) {
605                 return getDefaultAction().mouseMoved(e);
606         }
607         
608         private Vector3d getRotationAxis() {
609                 switch (index) {
610                 case X:
611                         return new Vector3d(1.0, 0.0, 0.0);
612                 case Y:
613                         return new Vector3d(0.0, 1.0, 0.0);
614                 case Z:
615                         return new Vector3d(0.0, 0.0, 1.0);
616                 case P:
617                         Vector3d axis = new Vector3d(panel.getRenderer().GetActiveCamera()
618                                         .GetDirectionOfProjection());
619                         axis.normalize();
620                         return axis;
621                 default:
622                         return null;
623                 }
624         }
625         
626          private double prevS = 0.0;
627             
628     private Vector3d i = new Vector3d();
629     private Vector3d j = new Vector3d();
630     private double prevAngle = 0;
631
632     enum InputType{INTERSECT,NONINTERSECT,KEY,NONE};
633     InputType inputType;
634     private boolean useStep = false;
635     
636     
637     
638     private double roundAngle(double angle) {
639         while (angle < - Math.PI)
640             angle += Math.PI*2.0;
641         while (angle > Math.PI)
642             angle -= Math.PI*2.0;
643         
644         
645         int index = 0;
646         while (angle > angles[index])
647             index++;
648         if (index == 0) {
649             angle = angles[0];
650         } else {
651             double d = angle - angles[index - 1];
652             double d2 = angles[index] - angle;
653             if (d < d2)
654                 angle = angles[index - 1];
655             else
656                 angle = angles[index];
657         }
658         return angle;
659     }
660         
661 }