]> gerrit.simantics Code Review - simantics/3d.git/blob - org.simantics.g3d.vtk/src/org/simantics/g3d/vtk/awt/RotateAction.java
Mark undo pints when committing changes to the graph
[simantics/3d.git] / org.simantics.g3d.vtk / src / org / simantics / g3d / vtk / awt / 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.awt;
13
14 import java.awt.Cursor;
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.simantics.g3d.math.EulerTools;
24 import org.simantics.g3d.math.EulerTools.Order;
25 import org.simantics.g3d.math.MathTools;
26 import org.simantics.g3d.math.Ray;
27 import org.simantics.g3d.preferences.PreferenceConstants;
28 import org.simantics.g3d.scenegraph.IG3DNode;
29 import org.simantics.g3d.scenegraph.structural.IStructuralNode;
30 import org.simantics.g3d.vtk.Activator;
31 import org.simantics.g3d.vtk.common.VTKNodeMap;
32 import org.simantics.g3d.vtk.gizmo.RotateAxisGizmo;
33 import org.simantics.g3d.vtk.utils.vtkUtil;
34 import org.simantics.utils.threads.AWTThread;
35 import org.simantics.utils.threads.ThreadUtils;
36
37 import vtk.vtkProp;
38 /**
39  * FIXME: complete rewrite.
40  * 
41  * @author Marko Luukkainen <marko.luukkainen@vtt.fi>
42  *
43  */
44 public class RotateAction extends vtkAwtAction{
45         
46         public static final int X = 0;
47     public static final int Y = 1;
48     public static final int Z = 2;
49     public static final int P = 3;
50
51         private VTKNodeMap nodeMap;
52         //private TranslateGizmo  gizmo = new TranslateGizmo();
53         private RotateAxisGizmo gizmo = new RotateAxisGizmo();
54         private IG3DNode node;
55         
56         
57         
58         private Cursor activeCursor = Cursor.getPredefinedCursor(Cursor.HAND_CURSOR);
59         private Cursor dragCursor = Cursor.getPredefinedCursor(Cursor.MOVE_CURSOR);
60         
61         
62         int stepMethod = 1;
63         Order order = Order.YXZ;
64         
65     private int steps; 
66     private double angles[];
67         
68         int index = P;
69         boolean valid = false;
70         private boolean worldCoord = true;
71         //private AxisAngle4d aa = null;
72         private Quat4d parentWorldOrientation = null;
73         
74         //AxisAngle4d rotation = new AxisAngle4d();
75         Quat4d worldOrientation = new Quat4d();
76         
77         public void setNode(IG3DNode node) {
78                 this.node = node;
79                 if ((node instanceof IStructuralNode) && ((IStructuralNode)node).isPartOfInstantiatedModel() && !((IStructuralNode)node).isInstantiatedModelRoot()) {
80                         setEnabled(false);
81                 } else {
82                         setEnabled(true);
83                 }
84                 
85                 String set = org.simantics.g3d.Activator.getDefault().getPreferenceStore().getString(PreferenceConstants.ORIENTATION_PRESENTATION);
86                 if (set.equals("aa")) {
87                         stepMethod = 0;
88                 } else if (set.equals("euler")){
89                         stepMethod = 1;
90                         String eulerOrder = org.simantics.g3d.Activator.getDefault().getPreferenceStore().getString(PreferenceConstants.EULER_ANGLE_ORDER);
91                         try {
92                                 order = Order.valueOf(eulerOrder);
93                         } catch (Exception e) {
94                                 order = Order.YXZ;
95                         }
96                 } else {
97                         stepMethod = 2;
98                 }
99         }
100         
101         public IG3DNode getNode() {
102                 return node;
103         }
104         
105         public RotateAction(InteractiveVtkPanel panel, VTKNodeMap nodeMap) {
106                 super(panel);
107                 setImageDescriptor(Activator.imageDescriptorFromPlugin("com.famfamfam.silk", "icons/arrow_rotate_clockwise.png"));
108                 setText("Rotate");
109                 this.nodeMap = nodeMap;
110                 
111                 
112                 steps = 36;
113         angles = new double[steps+1];
114         for (int i = 0; i < angles.length; i++) {
115             angles[i] = - Math.PI + (Math.PI * i * 2.0 / steps);
116         }
117         }
118         
119         public void attach() {
120                 if (node == null)
121                         return;
122                 
123                 super.attach();
124                 ThreadUtils.asyncExec(AWTThread.getThreadAccess(), new Runnable() {
125                         public void run() {
126                                 attachUI();
127                                 update();
128                         }
129                 });
130                 
131                 
132                 
133         }
134         
135         public void deattach() {
136                 
137                 node = null;
138                 nodeMap.commit("Rotate");
139                 deattachUI();
140                 super.deattach();
141                 panel.repaint();
142         }
143         
144         private void attachUI() {
145                 panel.setCursor(activeCursor);
146                 gizmo.attach(panel);
147         }
148         
149         private void deattachUI() {
150                 panel.setCursor(Cursor.getPredefinedCursor(Cursor.DEFAULT_CURSOR));
151                 gizmo.deattach();
152         }
153         
154         @Override
155         public void keyPressed(KeyEvent e) {
156                 if (e.getKeyCode() == KeyEvent.VK_ESCAPE)
157                         panel.useDefaultAction();
158                 if (valid)
159                         return;
160                 if (e.getKeyCode() == KeyEvent.VK_X) {
161                         if (index != X)
162                                 index = X;
163                         else
164                                 index = P;
165                 }
166                 if (e.getKeyCode() == KeyEvent.VK_Y) {
167                         if (index != Y)
168                                 index = Y;
169                         else
170                                 index = P;
171                 }
172                 if (e.getKeyCode() == KeyEvent.VK_Z) {
173                         if (index != Z)
174                                 index = Z;
175                         else
176                                 index = P;
177                 }
178                 if (e.getKeyCode() == KeyEvent.VK_G) {
179                         worldCoord = !worldCoord;
180                 }
181                 gizmo.setType(index);
182                 panel.repaint();
183         }
184         
185         @Override
186         public void keyReleased(KeyEvent e) {
187                 
188         }
189         
190         
191         
192         @Override
193         public void mouseClicked(MouseEvent e) {
194                 if (e.getClickCount() > 1) {
195                         if (isOverNode(e)) {
196                                 return;
197                         }
198                         panel.useDefaultAction();
199                         //if(!gizmo.isPartOf(actor))
200                         //      panel.useDefaultAction();
201                         
202                 }
203         }
204         
205         @Override
206         public void mouseEntered(MouseEvent e) {
207                 
208         }
209         
210         @Override
211         public void mouseExited(MouseEvent e) {
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.Render();
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 void 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.setCursor(dragCursor);
313                         } else {
314                                 valid = false;
315                                 getDefaultAction().mousePressed(e);
316                                 panel.setCursor(activeCursor);
317                         }
318                 } else {
319                         getDefaultAction().mousePressed(e);
320                 }
321         }
322         
323         
324         
325         @Override
326         public void mouseReleased(MouseEvent e) {
327                 if (e.getButton() == MouseEvent.BUTTON1) {
328                         valid = false;
329                         worldOrientation = null;
330                         panel.setCursor(activeCursor);
331                 } else {
332                         getDefaultAction().mouseReleased(e);
333                 }
334         }
335         
336         @Override
337         public void mouseDragged(MouseEvent e) {
338                 if ((e.getModifiersEx() & MouseEvent.BUTTON1_DOWN_MASK) > 0 && valid) { 
339                         if ((e.getModifiers() & MouseEvent.CTRL_MASK) > 0) {
340                                 useStep = true;
341             } else {
342                 useStep = false;
343             }
344                         doChanges(false, e.getX(), e.getY());
345                         
346                         //nodeMap.modified(node);
347                         update();
348                 } else {
349                         getDefaultAction().mouseDragged(e);
350                         update();
351                 }
352         }
353         
354          Vector3d axis = null;
355         
356         @Override
357         public void keyTyped(KeyEvent e) {
358                  if (e.getKeyCode() == KeyEvent.VK_LEFT) {
359                 inputType = InputType.KEY;
360                 axis = new Vector3d(0.0,1.0,0.0);
361              } else if (e.getKeyCode() == KeyEvent.VK_RIGHT) {
362                         inputType = InputType.KEY;
363                         axis = new Vector3d(0.0,-1.0,0.0);
364              } else if (e.getKeyCode() ==KeyEvent.VK_UP) {
365                         inputType = InputType.KEY;
366                         axis = new Vector3d(1.0,0.0,0.0);
367              } else if (e.getKeyCode() == KeyEvent.VK_DOWN) {
368                         inputType = InputType.KEY;
369                         axis = new Vector3d(-1.0,0.0,0.0);
370              } 
371         }
372         
373     public void doChanges(boolean pressed, int x, int y) {
374         Ray ray = vtkUtil.createMouseRay(panel.GetRenderer(),x, y);
375         Vector3d p = node.getWorldPosition();
376         
377         if (pressed) {
378             Vector3d axis = getRotationAxis();
379             if (axis != null) {
380                 if (!worldCoord) {
381                         MathTools.rotate(parentWorldOrientation, axis, axis);
382                 }
383
384                
385                 double s[] = new double[2];
386                 Vector3d i2 = new Vector3d();
387                
388                 boolean intersect = MathTools.intersectStraightPlane(ray.pos, ray.dir, p, axis, i2, s);
389                 double dot = Math.abs(ray.dir.dot(axis));
390                 if (intersect &&  dot > 0.4)
391                         inputType = InputType.INTERSECT;
392                 else
393                         inputType = InputType.NONINTERSECT;
394                         
395                 
396                 if (inputType == InputType.INTERSECT) {
397                     // picking ray and plane defined by gizmo's center point and
398                     // rotation axis can intersect
399                     // vector from center point to intersection point
400                     i2.sub(p);
401                     // creating vectors i and j that are lying on the plane and
402                     // are perpendicular
403                     // vectors are used to calculate polar coordinate for
404                     // intersection point
405                     j.set(i2);
406                     i.cross(j, axis);
407                     System.out.println("I,J " + i + " " + j);
408                     double angleI = i2.angle(i);
409                     double angleJ = i2.angle(j);
410                     prevAngle = Math.atan2(Math.cos(angleJ), Math.cos(angleI));
411                 } else {
412                     // picking ray and plane defined by gizmo's center point and
413                     // rotation axis are parallel,
414                     // so we'll use cross product of rotation axis and picking
415                     // ray to detect amount of rotation
416                     i.cross(ray.dir, axis);
417                     MathTools.intersectStraightStraight(ray.pos, ray.dir, p, i, new Vector3d(), new Vector3d(), s);
418                     prevS = s[1];
419                 }
420             }
421            
422             
423         }
424
425         if (inputType != InputType.KEY)
426                 axis = getRotationAxis();
427         if (axis == null) {
428             return;   
429         }
430         Vector3d taxis = null;
431         if (!worldCoord) {
432                 taxis = new Vector3d(axis);
433                 MathTools.rotate(parentWorldOrientation, axis, axis);
434         }
435         System.out.println(inputType);
436         if (inputType == InputType.INTERSECT) {
437
438             double s[] = new double[2];
439             Vector3d i2 = new Vector3d();
440             MathTools.intersectStraightPlane(ray.pos, ray.dir, p, axis, i2, s);
441             i2.sub(p);
442             double angleI = i2.angle(i);
443             double angleJ = i2.angle(j);
444             double angle = Math.atan2(Math.cos(angleJ), Math.cos(angleI));
445             System.out.println("Angle " + angle + " i " + angleI + " j " + angleJ + " prev " + prevAngle);
446             if(!worldCoord)
447                 axis = taxis;
448             if (useStep) {
449
450                 //setOrientation(MathTools.getQuat(rotation));
451                 AxisAngle4d rot = new AxisAngle4d(axis,angle-prevAngle);
452                 Quat4d qrot = new Quat4d();
453                 MathTools.getQuat(rot, qrot);
454                 //prevAngle = angle;
455                 qrot.mulInverse(worldOrientation);
456                 
457           
458                 if (stepMethod == 0) {
459                         rot.set(qrot);
460                         rot.angle = roundAngle(rot.angle);
461                         //qrot.set(rot);
462                         MathTools.getQuat(rot,qrot);
463                         setOrientation(qrot);
464                 } else if (stepMethod == 1){
465                           
466                         //Vector3d euler = MathTools.getEuler(qrot);
467                         Vector3d euler = EulerTools.getEulerFromQuat(order, qrot);
468                         euler.x = roundAngle(euler.x);
469                         euler.y = roundAngle(euler.y);
470                         euler.z = roundAngle(euler.z);
471                         //Quat4d q = MathTools.getQuat(euler);
472                         Quat4d q = EulerTools.getQuatFromEuler(order, euler);
473                         setOrientation(q);
474                         System.out.println(" (" + MathTools.radToDeg(euler.x) + " " + MathTools.radToDeg(euler.y) + " " + MathTools.radToDeg(euler.z) +  ") " + qrot + " "+ q);
475                 } else {
476                         setOrientation(qrot);
477                 }
478                 
479             } else {
480                 if (worldCoord) {
481                         //G3DTools.multiplyOrientation(mo.getG3DNode(graph).getWorldOrientation(), new AxisAngle4d(axis,angle-prevAngle));
482                         AxisAngle4d aa = MathTools.getAxisAngle(node.getWorldOrientation());
483                         AxisAngle4d rot = new AxisAngle4d(axis,angle-prevAngle);
484                         MathTools.multiplyOrientation(aa, rot);
485                         setWorldOrientation(MathTools.getQuat(rot));
486                 } else {
487                         AxisAngle4d aa = MathTools.getAxisAngle(node.getOrientation());
488                         AxisAngle4d rot = new AxisAngle4d(axis,angle-prevAngle);
489                         MathTools.multiplyOrientation(aa, rot);
490                         setOrientation(MathTools.getQuat(rot));
491                         //G3DTools.multiplyOrientation(mo.getG3DNode(graph).getLocalOrientation(), new AxisAngle4d(axis,angle-prevAngle));
492                 }
493                 prevAngle = angle;
494             }
495             
496         } else if (inputType == InputType.NONINTERSECT){
497
498             double s[] = new double[2];
499             MathTools.intersectStraightStraight(ray.pos, ray.dir, p, i, new Vector3d(), new Vector3d(), s);
500             if(!worldCoord)
501                 axis = taxis;
502             if (useStep) {
503                 //setOrientation(MathTools.getQuat(rotation));
504                 AxisAngle4d rot = new AxisAngle4d(axis,s[1] - prevS);
505                 
506                 Quat4d qrot = new Quat4d();
507                 //qrot.set(rot);
508                 MathTools.getQuat(rot, qrot);
509                 //prevAngle = angle;
510                 qrot.mulInverse(worldOrientation);
511                 
512           
513                 if (stepMethod == 0) {
514                         rot.set(qrot);
515                         rot.angle = roundAngle(rot.angle);
516                         //qrot.set(rot);
517                         MathTools.getQuat(rot,qrot);
518                         setOrientation(qrot);
519                 } else if (stepMethod == 1){
520                           
521                         //Vector3d euler = MathTools.getEuler(qrot);
522                         Vector3d euler = EulerTools.getEulerFromQuat(order, qrot);
523                         euler.x = roundAngle(euler.x);
524                         euler.y = roundAngle(euler.y);
525                         euler.z = roundAngle(euler.z);
526                         //Quat4d q = MathTools.getQuat(euler);
527                         Quat4d q = EulerTools.getQuatFromEuler(order, euler);
528                         setOrientation(q);
529                         System.out.println(" (" + MathTools.radToDeg(euler.x) + " " + MathTools.radToDeg(euler.y) + " " + MathTools.radToDeg(euler.z) +  ") " + qrot + " "+ q);
530                 } else {
531                         setOrientation(qrot);
532                 }
533                 prevS = s[1];
534                 
535 //                    G3DTools.setOrientation(mo.getG3DNode(graph).getLocalOrientation(), rotations.get(mo));
536 //                    G3DTools.multiplyOrientation(mo.getG3DNode(graph).getWorldOrientation(), new AxisAngle4d(axis,s[1] - prevS));
537 //                    AxisAngle4d aa = G3DTools.getOrientation(mo.getG3DNode(graph).getLocalOrientation());
538 //                    rotations.put(mo, aa);
539 //                    Vector3d euler = MathTools.getEuler(aa);
540 //                    euler.x = roundAngle(euler.x);
541 //                    euler.y = roundAngle(euler.y);
542 //                    euler.z = roundAngle(euler.z);
543 //                    aa = MathTools.getFromEuler2(euler);
544 //                    prevS = s[1];
545 //                    G3DTools.setOrientation(mo.getG3DNode(graph).getLocalOrientation(), aa);
546 //                    Vector3d e = MathTools.getEuler(G3DTools.getOrientation(mo.getG3DNode(graph).getLocalOrientation()));
547 //                    e.scale(180.0/Math.PI);
548 //                    text += G3DTools.getOrientation(mo.getG3DNode(graph).getLocalOrientation()) + " " + e + " ";
549                 
550                 
551             } else {
552                         if (worldCoord) {
553                                 AxisAngle4d aa =  MathTools.getAxisAngle(node.getWorldOrientation());
554                         AxisAngle4d rot = new AxisAngle4d(axis,s[1] - prevS);
555                         MathTools.multiplyOrientation(aa, rot);
556                         setWorldOrientation(MathTools.getQuat(rot));
557                                 //G3DTools.multiplyOrientation(mo.getG3DNode(graph).getWorldOrientation(), new AxisAngle4d(axis,s[1] - prevS));
558                         } else {
559                                 AxisAngle4d aa =  MathTools.getAxisAngle(node.getOrientation());
560                         AxisAngle4d rot = new AxisAngle4d(axis,s[1] - prevS);
561                         MathTools.multiplyOrientation(aa, rot);
562                         setOrientation(MathTools.getQuat(rot));
563                                 //G3DTools.multiplyOrientation(mo.getG3DNode(graph).getLocalOrientation(), new AxisAngle4d(axis,s[1] - prevS));
564                         }
565                         //text += G3DTools.getOrientation(mo.getG3DNode(graph).getLocalOrientation()) + " " + MathTools.getEuler(G3DTools.getOrientation(mo.getG3DNode(graph).getLocalOrientation())) + " ";
566                 prevS = s[1];
567                 
568             }
569            
570         } else {
571                 if (worldCoord) {
572                         AxisAngle4d aa = MathTools.getAxisAngle(node.getWorldOrientation());
573                 AxisAngle4d rot = new AxisAngle4d(axis,Math.PI * 0.5);
574                 MathTools.multiplyOrientation(aa, rot);
575                 setWorldOrientation(MathTools.getQuat(rot));
576                         //G3DTools.multiplyOrientation(mo.getG3DNode(graph).getWorldOrientation(), new AxisAngle4d(axis,Math.PI * 0.5));
577                 } else {
578                         AxisAngle4d aa = MathTools.getAxisAngle(node.getOrientation());
579                 AxisAngle4d rot = new AxisAngle4d(axis,Math.PI * 0.5);
580                 MathTools.multiplyOrientation(aa, rot);
581                 setOrientation(MathTools.getQuat(rot));
582                         //G3DTools.multiplyOrientation(mo.getG3DNode(graph).getLocalOrientation(), new AxisAngle4d(axis,Math.PI * 0.5));
583                 }
584              //   text += G3DTools.getOrientation(mo.getG3DNode(graph).getLocalOrientation()) + " " + MathTools.getEuler(G3DTools.getOrientation(mo.getG3DNode(graph).getLocalOrientation())) + " ";
585             
586         }
587         //setInfoText(text);
588  
589     }
590     
591     protected void setOrientation(Quat4d q) {
592         node.setOrientation(q);
593     }
594     
595     protected void setWorldOrientation(Quat4d q) {
596         node.setWorldOrientation(q);
597     }
598         
599         @Override
600         public void mouseMoved(MouseEvent e) {
601                 getDefaultAction().mouseMoved(e);
602         }
603         
604         private Vector3d getRotationAxis() {
605                 switch (index) {
606                 case X:
607                         return new Vector3d(1.0, 0.0, 0.0);
608                 case Y:
609                         return new Vector3d(0.0, 1.0, 0.0);
610                 case Z:
611                         return new Vector3d(0.0, 0.0, 1.0);
612                 case P:
613                         Vector3d axis = new Vector3d(panel.GetRenderer().GetActiveCamera()
614                                         .GetDirectionOfProjection());
615                         axis.normalize();
616                         return axis;
617                 default:
618                         return null;
619                 }
620         }
621         
622          private double prevS = 0.0;
623             
624     private Vector3d i = new Vector3d();
625     private Vector3d j = new Vector3d();
626     private double prevAngle = 0;
627
628     enum InputType{INTERSECT,NONINTERSECT,KEY,NONE};
629     InputType inputType;
630     private boolean useStep = false;
631     
632     
633     
634     private double roundAngle(double angle) {
635         while (angle < - Math.PI)
636             angle += Math.PI*2.0;
637         while (angle > Math.PI)
638             angle -= Math.PI*2.0;
639         
640         
641         int index = 0;
642         while (angle > angles[index])
643             index++;
644         if (index == 0) {
645             angle = angles[0];
646         } else {
647             double d = angle - angles[index - 1];
648             double d2 = angles[index] - angle;
649             if (d < d2)
650                 angle = angles[index - 1];
651             else
652                 angle = angles[index];
653         }
654         return angle;
655     }
656         
657 }