source: osgVisual/trunk/src/manip_Spacemouse/manip_nodeTrackerSpaceMouse.cpp @ 280

Last change on this file since 280 was 230, checked in by Torben Dannhauer, 14 years ago

Clean up the spacemouse Node tracker a little bit.
File size: 20.9 KB
Line 
1/* -*-c++-*- osgVisual - Copyright (C) 2009-2011 Torben Dannhauer
2 *
3 * This library is based on OpenSceneGraph, open source and may be redistributed and/or modified under
4 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
5 * (at your option) any later version.  The full license is in LICENSE file
6 * included with this distribution, and on the openscenegraph.org website.
7 *
8 * osgVisual requires for some proprietary modules a license from the correspondig manufacturer.
9 * You have to aquire licenses for all used proprietary modules.
10 *
11 * This library is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14 * OpenSceneGraph Public License for more details.
15*/
16
17#include <manip_nodeTrackerSpaceMouse.h>
18
19
20
21using namespace osg;
22using namespace osgVisual;
23
24
25NodeTrackerSpaceMouse::NodeTrackerSpaceMouse(SpaceMouse* spacemouse) : _spaceMouse(spacemouse)
26{
27    _trackerMode = NODE_CENTER_AND_ROTATION; 
28    _rotationMode = TRACKBALL; 
29
30    _distance = 1.0;
31
32    _thrown = false;
33        _autohoming = false;
34        _ah_init = false;
35        _distanceDependendAV = false;
36
37        TZ=0;
38        RX=0;
39        RY=0;
40        RZ=0;
41}
42
43
44NodeTrackerSpaceMouse::~NodeTrackerSpaceMouse()
45{
46}
47
48void NodeTrackerSpaceMouse::setTrackNode(osg::Node* node)
49{
50    if (!node)
51    {
52        osg::notify(osg::NOTICE)<<"NodeTrackerSpaceMouse::setTrackNode(Node*):  Unable to set tracked node due to null Node*"<<std::endl;
53        return;
54    }
55
56    osg::NodePathList parentNodePaths = node->getParentalNodePaths();
57
58    if (!parentNodePaths.empty())
59    {
60        osg::notify(osg::INFO)<<"NodeTrackerSpaceMouse::setTrackNode(Node*): Path set"<<std::endl;
61        setTrackNodePath(parentNodePaths[0]);
62    }
63    else
64    {
65        osg::notify(osg::NOTICE)<<"NodeTrackerSpaceMouse::setTrackNode(Node*): Unable to set tracked node due to empty parental path."<<std::endl;
66    }
67}
68
69osg::Node* NodeTrackerSpaceMouse::getTrackNode()
70{
71    osg::NodePath nodePath;
72    if (_trackNodePath.getNodePath(nodePath)) return nodePath.back();
73    else return 0;
74}
75
76const osg::Node* NodeTrackerSpaceMouse::getTrackNode() const
77{
78    osg::NodePath nodePath;
79    if (_trackNodePath.getNodePath(nodePath)) return nodePath.back();
80    else return 0;
81}
82
83void NodeTrackerSpaceMouse::setTrackerMode(TrackerMode mode)
84{
85    _trackerMode = mode;
86}
87
88void NodeTrackerSpaceMouse::setRotationMode(RotationMode mode)
89{
90    _rotationMode = mode;
91    if (getAutoComputeHomePosition()) computeHomePosition();   
92}
93
94void NodeTrackerSpaceMouse::setNode(osg::Node* node)
95{
96    _node = node;
97   
98    if (_node.get())
99    {
100        const osg::BoundingSphere& boundingSphere=_node->getBound();
101        const float minimumDistanceScale = 0.001f;
102        _minimumDistance = osg::clampBetween(
103            float(boundingSphere._radius) * minimumDistanceScale,
104            0.00001f,1.0f);
105           
106        osg::notify(osg::INFO)<<"Setting Tracker manipulator _minimumDistance to "<<_minimumDistance<<std::endl;
107    }
108    if (getAutoComputeHomePosition()) computeHomePosition();   
109}
110
111const osg::Node* NodeTrackerSpaceMouse::getNode() const
112{
113    return _node.get();
114}
115
116
117osg::Node* NodeTrackerSpaceMouse::getNode()
118{
119    return _node.get();
120}
121
122
123void NodeTrackerSpaceMouse::home(const GUIEventAdapter& ,GUIActionAdapter& us)
124{
125    if (getAutoComputeHomePosition()) computeHomePosition();
126
127    computePosition(_homeEye, _homeCenter, _homeUp);
128    us.requestRedraw();
129}
130
131void NodeTrackerSpaceMouse::computeHomePosition()
132{
133    osg::NodePath nodePath;
134    _trackNodePath.getNodePath(nodePath);
135
136    osg::Node* node = nodePath.empty() ? getNode() : nodePath.back();
137   
138    if(node)
139    {
140        const osg::BoundingSphere& boundingSphere=node->getBound();
141
142        setHomePosition(boundingSphere._center+osg::Vec3( 0.0,-3.5f * boundingSphere._radius,0.0f),
143                        boundingSphere._center,
144                        osg::Vec3(0.0f,0.0f,1.0f),
145                        _autoComputeHomePosition);
146    }
147}
148
149
150
151void NodeTrackerSpaceMouse::init(const GUIEventAdapter& ,GUIActionAdapter& )
152{
153    flushMouseEventStack();
154}
155
156
157void NodeTrackerSpaceMouse::getUsage(osg::ApplicationUsage& usage) const
158{
159    usage.addKeyboardMouseBinding("SN Tracker: Space","Toggle camera auto homing");
160        usage.addKeyboardMouseBinding("SN Tracker: m","Toggle Nodetracker Mode");
161        usage.addKeyboardMouseBinding("SN Tracker: n","Toggle distance dependend angular velocity");
162    usage.addKeyboardMouseBinding("SN Tracker: +","When in stereo, increase the fusion distance");
163    usage.addKeyboardMouseBinding("SN Tracker: -","When in stereo, reduce the fusion distance");
164}
165
166bool NodeTrackerSpaceMouse::handle(const GUIEventAdapter& ea,GUIActionAdapter& us)
167{
168    switch(ea.getEventType())
169    {
170        case(GUIEventAdapter::PUSH):
171        {
172            flushMouseEventStack();
173            addMouseEvent(ea);
174            if (calcMovement()) us.requestRedraw();
175            us.requestContinuousUpdate(false);
176            _thrown = false;
177            return true;
178        }
179
180        case(GUIEventAdapter::RELEASE):
181        {
182            if (ea.getButtonMask()==0)
183            {
184
185                double timeSinceLastRecordEvent = _ga_t0.valid() ? (ea.getTime() - _ga_t0->getTime()) : DBL_MAX;
186                if (timeSinceLastRecordEvent>0.02) flushMouseEventStack();
187
188                if (isMouseMoving())
189                {
190                    if (calcMovement())
191                    {
192                        us.requestRedraw();
193                        us.requestContinuousUpdate(true);
194                        _thrown = true;
195                    }
196                }
197                else
198                {
199                    flushMouseEventStack();
200                    addMouseEvent(ea);
201                    if (calcMovement()) us.requestRedraw();
202                    us.requestContinuousUpdate(false);
203                    _thrown = false;
204                }
205
206            }
207            else
208            {
209                flushMouseEventStack();
210                addMouseEvent(ea);
211                if (calcMovement()) us.requestRedraw();
212                us.requestContinuousUpdate(false);
213                _thrown = false;
214            }
215            return true;
216        }
217
218        case(GUIEventAdapter::DRAG):
219        {
220            addMouseEvent(ea);
221            if (calcMovement()) us.requestRedraw();
222            us.requestContinuousUpdate(false);
223            _thrown = false;
224            return true;
225        }
226
227        case(GUIEventAdapter::MOVE):
228        {
229            return false;
230        }
231
232        case(GUIEventAdapter::KEYDOWN):
233            if (ea.getKey()==' ')
234            {
235                                if (_autohoming)
236                                        _autohoming=false;
237                                else
238                                        _autohoming=true;
239                return true;
240            }
241                        if (ea.getKey()=='m')
242            {
243                                switch(_trackerMode)
244                                {
245                                        case NODE_CENTER: _trackerMode=NODE_CENTER_AND_AZIM;
246                                                break;
247                                        case NODE_CENTER_AND_AZIM: _trackerMode=NODE_CENTER_AND_ROTATION;
248                                                break;
249                                        case  NODE_CENTER_AND_ROTATION: _trackerMode=NODE_CENTER;
250                                                break;
251                                        default: _trackerMode = NODE_CENTER;
252                                                break;
253                                };
254                return true;
255            }
256                        if (ea.getKey()=='n')
257            {
258                                if (_distanceDependendAV)
259                                        _distanceDependendAV=false;
260                                else
261                                        _distanceDependendAV=true;
262                return true;
263            }
264            return false;
265        case(GUIEventAdapter::FRAME):
266            if (_thrown)
267            {
268                if (calcMovement()) us.requestRedraw();
269            }
270
271                        computeHomePosition();
272                        computePosition(_homeEye, _homeCenter, _homeUp);
273                        if (calcMovementSpaceMouse())
274                                us.requestRedraw();
275
276            return false;
277        default:
278            return false;
279    }
280}
281
282
283bool NodeTrackerSpaceMouse::isMouseMoving()
284{
285    if (_ga_t0.get()==NULL || _ga_t1.get()==NULL) return false;
286
287    static const float velocity = 0.1f;
288
289    float dx = _ga_t0->getXnormalized()-_ga_t1->getXnormalized();
290    float dy = _ga_t0->getYnormalized()-_ga_t1->getYnormalized();
291    float len = sqrtf(dx*dx+dy*dy);
292    float dt = _ga_t0->getTime()-_ga_t1->getTime();
293
294    return (len>dt*velocity);
295}
296
297
298void NodeTrackerSpaceMouse::flushMouseEventStack()
299{
300    _ga_t1 = NULL;
301    _ga_t0 = NULL;
302}
303
304
305void NodeTrackerSpaceMouse::addMouseEvent(const GUIEventAdapter& ea)
306{
307    _ga_t1 = _ga_t0;
308    _ga_t0 = &ea;
309}
310
311void NodeTrackerSpaceMouse::setByMatrix(const osg::Matrixd& matrix)
312{
313    osg::Vec3d eye,center,up;
314    matrix.getLookAt(eye,center,up,_distance);
315    computePosition(eye,center,up);
316}
317
318void NodeTrackerSpaceMouse::computeNodeWorldToLocal(osg::Matrixd& worldToLocal) const
319{
320    osg::NodePath nodePath;
321    if (_trackNodePath.getNodePath(nodePath))
322    {
323        worldToLocal = osg::computeWorldToLocal(nodePath);
324    }
325}
326
327void NodeTrackerSpaceMouse::computeNodeLocalToWorld(osg::Matrixd& localToWorld) const
328{
329    osg::NodePath nodePath;
330    if (_trackNodePath.getNodePath(nodePath))
331    {
332        localToWorld = osg::computeLocalToWorld(nodePath);
333    }
334
335}
336
337void NodeTrackerSpaceMouse::computeNodeCenterAndRotation(osg::Vec3d& nodeCenter, osg::Quat& nodeRotation) const
338{
339        if (_trackNodePath.empty())
340                return;
341
342    osg::Matrixd localToWorld;
343        computeNodeLocalToWorld(localToWorld);
344
345    osg::NodePath nodePath;
346        if (_trackNodePath.getNodePath(nodePath) && !nodePath.empty())
347        nodeCenter = osg::Vec3d(nodePath.back()->getBound().center())*localToWorld;
348    else
349        nodeCenter = osg::Vec3d(0.0f,0.0f,0.0f)*localToWorld;
350
351
352    switch(_trackerMode)
353    {
354        case(NODE_CENTER_AND_AZIM):
355        {
356            CoordinateFrame coordinateFrame = getCoordinateFrame(nodeCenter);
357            osg::Matrixd localToFrame(localToWorld*osg::Matrixd::inverse(coordinateFrame));
358
359            double azim = atan2(-localToFrame(0,1),localToFrame(0,0));
360            osg::Quat nodeRotationRelToFrame, rotationOfFrame;
361            nodeRotationRelToFrame.makeRotate(-azim,0.0,0.0,1.0);
362            rotationOfFrame = coordinateFrame.getRotate();
363            nodeRotation = nodeRotationRelToFrame*rotationOfFrame;
364            break;
365        }
366        case(NODE_CENTER_AND_ROTATION):
367        {
368            // scale the matrix to get rid of any scales before we extract the rotation.
369            double sx = 1.0/sqrt(localToWorld(0,0)*localToWorld(0,0) + localToWorld(1,0)*localToWorld(1,0) + localToWorld(2,0)*localToWorld(2,0));
370            double sy = 1.0/sqrt(localToWorld(0,1)*localToWorld(0,1) + localToWorld(1,1)*localToWorld(1,1) + localToWorld(2,1)*localToWorld(2,1));
371            double sz = 1.0/sqrt(localToWorld(0,2)*localToWorld(0,2) + localToWorld(1,2)*localToWorld(1,2) + localToWorld(2,2)*localToWorld(2,2));
372            localToWorld = localToWorld*osg::Matrixd::scale(sx,sy,sz);
373
374            nodeRotation = localToWorld.getRotate();
375            break;
376        }
377        case(NODE_CENTER):
378        default:
379        {
380            CoordinateFrame coordinateFrame = getCoordinateFrame(nodeCenter);
381            nodeRotation = coordinateFrame.getRotate();
382            break;
383        }
384    }
385
386}
387
388
389osg::Matrixd NodeTrackerSpaceMouse::getMatrix() const
390{
391    osg::Vec3d nodeCenter;
392    osg::Quat nodeRotation;
393    computeNodeCenterAndRotation(nodeCenter,nodeRotation);
394    return osg::Matrixd::translate(0.0,0.0,_distance)*osg::Matrixd::rotate(_rotation)*osg::Matrixd::rotate(nodeRotation)*osg::Matrix::translate(nodeCenter);
395}
396
397osg::Matrixd NodeTrackerSpaceMouse::getInverseMatrix() const
398{
399    osg::Vec3d nodeCenter;
400    osg::Quat nodeRotation;
401    computeNodeCenterAndRotation(nodeCenter,nodeRotation);
402    return osg::Matrixd::translate(-nodeCenter)*osg::Matrixd::rotate(nodeRotation.inverse())*osg::Matrixd::rotate(_rotation.inverse())*osg::Matrixd::translate(0.0,0.0,-_distance);
403}
404
405void NodeTrackerSpaceMouse::computePosition(const osg::Vec3d& eye,const osg::Vec3d& center,const osg::Vec3d& up)
406{
407    if (!_node) return;
408
409    // compute rotation matrix
410    osg::Vec3 lv(center-eye);
411    _distance = lv.length();
412   
413    osg::Matrixd lookat;
414    lookat.makeLookAt(eye,center,up);
415   
416    _rotation = lookat.getRotate().inverse();
417}
418
419bool NodeTrackerSpaceMouse::calcMovement()
420{
421    // return if less then two events have been added.
422    if (_ga_t0.get()==NULL || _ga_t1.get()==NULL) return false;
423
424    double dx = _ga_t0->getXnormalized()-_ga_t1->getXnormalized();
425    double dy = _ga_t0->getYnormalized()-_ga_t1->getYnormalized();
426
427
428    float distance = sqrtf(dx*dx + dy*dy);
429    // return if movement is too fast, indicating an error in event values or change in screen.
430    if (distance>0.5)
431    {
432        return false;
433    }
434   
435    // return if there is no movement.
436    if (distance==0.0f)
437    {
438        return false;
439    }
440
441    osg::Vec3d nodeCenter;
442    osg::Quat nodeRotation;
443    computeNodeCenterAndRotation(nodeCenter, nodeRotation);
444
445    unsigned int buttonMask = _ga_t1->getButtonMask();
446
447    if (buttonMask==GUIEventAdapter::LEFT_MOUSE_BUTTON)
448    {
449
450        if (_rotationMode==TRACKBALL)
451        {
452
453            // rotate camera.
454            osg::Vec3 axis;
455            double angle;
456
457            double px0 = _ga_t0->getXnormalized();
458            double py0 = _ga_t0->getYnormalized();
459
460            double px1 = _ga_t1->getXnormalized();
461            double py1 = _ga_t1->getYnormalized();
462
463
464            trackball(axis,angle,px1,py1,px0,py0);
465
466            osg::Quat new_rotate;
467            new_rotate.makeRotate(angle,axis);
468
469            _rotation = _rotation*new_rotate;
470        }
471        else
472        {
473            osg::Matrix rotation_matrix;
474            rotation_matrix.makeRotate(_rotation);
475
476            osg::Vec3d lookVector = -getUpVector(rotation_matrix);
477            osg::Vec3d sideVector = getSideVector(rotation_matrix);
478            osg::Vec3d upVector = getFrontVector(rotation_matrix);
479           
480            osg::Vec3d localUp(0.0f,0.0f,1.0f);
481
482            osg::Vec3d forwardVector = localUp^sideVector;
483            sideVector = forwardVector^localUp;
484
485            forwardVector.normalize();
486            sideVector.normalize();
487           
488            osg::Quat rotate_elevation;
489            rotate_elevation.makeRotate(dy,sideVector);
490
491            osg::Quat rotate_azim;
492            rotate_azim.makeRotate(-dx,localUp);
493           
494            _rotation = _rotation * rotate_elevation * rotate_azim;
495           
496        }
497       
498        return true;
499
500    }
501    else if (buttonMask==GUIEventAdapter::MIDDLE_MOUSE_BUTTON ||
502        buttonMask==(GUIEventAdapter::LEFT_MOUSE_BUTTON|GUIEventAdapter::RIGHT_MOUSE_BUTTON))
503    {
504        return true;
505    }
506    else if (buttonMask==GUIEventAdapter::RIGHT_MOUSE_BUTTON)
507    {
508
509        // zoom model.
510
511        double fd = _distance;
512        double scale = 1.0f+dy;
513        if (fd*scale>_minimumDistance)
514        {
515
516            _distance *= scale;
517
518        } else
519        {
520            _distance = _minimumDistance;
521        }
522       
523        return true;
524
525    }
526
527    return false;
528}
529
530bool NodeTrackerSpaceMouse::calcMovementSpaceMouse()
531{
532        // TRACKBALL mode is not possible, because it uses the mouseevent, which does not happen with Spacemouse.
533
534   double dTX, dTY, dTZ;        // Only TZ is used later.
535    _spaceMouse->getTranslations(dTX, dTY, dTZ);
536
537    double dRX, dRY, dRZ;
538    _spaceMouse->getRotations(dRX, dRY, dRZ);
539
540        //OSG_NOTIFY( osg::INFO ) << "Recieved Spacemousevalues: dRX:" << dRX << ", dRY:" << dRY << ", dRZ:" << dRZ << ", dTX:" << dTX << std::endl;
541
542        if(!_ah_init)
543        {
544                TZ=dTZ;
545                RX=dRX;
546                RY=dRY;
547                RZ=dRZ;
548                _ah_init=true;
549        }
550
551        if (_autohoming)        // The factors are required to allow macroscopic movements.
552        {
553                TZ=dTZ*5;
554                if (!_distanceDependendAV)
555                {
556                        RX=dRX*100;
557                        RY=dRY*100;
558                        RZ=dRZ*100;
559                }
560        }
561        else            // NOT Autohoming
562        {
563                TZ+=dTZ;
564                //OSG_NOTIFY( osg::INFO ) << "Stored Spacemousevalues: RX:" << RX << ", RY:" << RY << ", RZ:" << RZ << ", TX:" << TX << std::endl;
565                if (_distanceDependendAV)
566                {
567                        RX+=(dRX*800/TZ);
568                        RY+=(dRY*800/TZ);
569                        RZ+=(dRZ*800/TZ);
570                }
571                else
572                {
573                        RX+=dRX;
574                        RY+=dRY;
575                        RZ+=dRZ;
576                }
577        }
578
579    osg::Vec3d nodeCenter;
580    osg::Quat nodeRotation;
581    computeNodeCenterAndRotation(nodeCenter, nodeRotation);
582
583    // ROTATION PART
584    if (_rotationMode==TRACKBALL)
585    {
586                // return if less then two events have been added.
587                if (_ga_t0.get()==NULL || _ga_t1.get()==NULL) return false;
588
589        // rotate camera.
590        osg::Vec3 axis;
591        double angle;
592
593        double px0 = _ga_t0->getXnormalized();
594        double py0 = _ga_t0->getYnormalized();
595
596        double px1 = _ga_t1->getXnormalized();
597        double py1 = _ga_t1->getYnormalized();
598
599
600        trackball(axis,angle,px1,py1,px0,py0);
601
602        osg::Quat new_rotate;
603        new_rotate.makeRotate(angle,axis);
604
605        _rotation = _rotation*new_rotate;
606    }
607    else
608    {
609        osg::Matrix rotation_matrix;
610        rotation_matrix.makeRotate(_rotation);
611
612        osg::Vec3d lookVector = -getUpVector(rotation_matrix);
613        osg::Vec3d sideVector = getSideVector(rotation_matrix);
614        osg::Vec3d upVector = getFrontVector(rotation_matrix);
615       
616        osg::Vec3d localUp(0.0f,0.0f,1.0f);
617
618        osg::Vec3d forwardVector = localUp^sideVector;
619        sideVector = forwardVector^localUp;
620
621        forwardVector.normalize();
622        sideVector.normalize();
623       
624        osg::Quat rotate_elevation;
625                rotate_elevation.makeRotate(osg::DegreesToRadians(RX*90), sideVector);
626
627        osg::Quat rotate_azim;
628                rotate_azim.makeRotate(osg::DegreesToRadians(RY*90), localUp);
629       
630        _rotation = _rotation * rotate_elevation * rotate_azim;
631       
632    }
633
634    // TRANSLATION PART
635    double scale = 1.0f+TZ/100;
636        if (_distance*scale>_minimumDistance)
637    {
638
639        _distance *= scale;
640
641    } else
642    {
643                TZ = (_minimumDistance/_distance - 1)*100;      // Reset TZ to the value f(_minimalDistance, _distance, scale-function).
644        _distance = _minimumDistance;
645    }
646    return true;
647}
648
649void NodeTrackerSpaceMouse::clampOrientation()
650{
651}
652
653
654/*
655 * This size should really be based on the distance from the center of
656 * rotation to the point on the object underneath the mouse.  That
657 * point would then track the mouse as closely as possible.  This is a
658 * simple example, though, so that is left as an Exercise for the
659 * Programmer.
660 */
661const float TRACKBALLSIZE = 0.8f;
662
663/*
664 * Ok, simulate a track-ball.  Project the points onto the virtual
665 * trackball, then figure out the axis of rotation, which is the cross
666 * product of P1 P2 and O P1 (O is the center of the ball, 0,0,0)
667 * Note:  This is a deformed trackball-- is a trackball in the center,
668 * but is deformed into a hyperbolic sheet of rotation away from the
669 * center.  This particular function was chosen after trying out
670 * several variations.
671 *
672 * It is assumed that the arguments to this routine are in the range
673 * (-1.0 ... 1.0)
674 */
675void NodeTrackerSpaceMouse::trackball(osg::Vec3& axis,double & angle, double  p1x, double  p1y, double  p2x, double  p2y)
676{
677    /*
678     * First, figure out z-coordinates for projection of P1 and P2 to
679     * deformed sphere
680     */
681
682    osg::Matrix rotation_matrix(_rotation);
683
684
685    osg::Vec3d uv = osg::Vec3d(0.0,1.0,0.0)*rotation_matrix;
686    osg::Vec3d sv = osg::Vec3d(1.0,0.0,0.0)*rotation_matrix;
687    osg::Vec3d lv = osg::Vec3d(0.0,0.0,-1.0)*rotation_matrix;
688
689    osg::Vec3d p1 = sv*p1x+uv*p1y-lv*tb_project_to_sphere(TRACKBALLSIZE,p1x,p1y);
690    osg::Vec3d p2 = sv*p2x+uv*p2y-lv*tb_project_to_sphere(TRACKBALLSIZE,p2x,p2y);
691
692    /*
693     *  Now, we want the cross product of P1 and P2
694     */
695
696// Robert,
697//
698// This was the quick 'n' dirty  fix to get the trackball doing the right
699// thing after fixing the Quat rotations to be right-handed.  You may want
700// to do something more elegant.
701//   axis = p1^p2;
702axis = p2^p1;
703    axis.normalize();
704
705    /*
706     *  Figure out how much to rotate around that axis.
707     */
708    double t = (p2-p1).length() / (2.0*TRACKBALLSIZE);
709
710    /*
711     * Avoid problems with out-of-control values...
712     */
713    if (t > 1.0) t = 1.0;
714    if (t < -1.0) t = -1.0;
715    angle = inRadians(asin(t));
716
717}
718
719
720/*
721 * Project an x,y pair onto a sphere of radius r OR a hyperbolic sheet
722 * if we are away from the center of the sphere.
723 */
724double NodeTrackerSpaceMouse::tb_project_to_sphere(double  r, double  x, double  y)
725{
726    float d, t, z;
727
728    d = sqrt(x*x + y*y);
729                                 /* Inside sphere */
730    if (d < r * 0.70710678118654752440)
731    {
732        z = sqrt(r*r - d*d);
733    }                            /* On hyperbola */
734    else
735    {
736        t = r / 1.41421356237309504880;
737        z = t*t / d;
738    }
739    return z;
740}
Note: See TracBrowser for help on using the repository browser.