source: osgVisual/src/manip_Spacemouse/manip_nodeTrackerSpaceMouse.cpp @ 87

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

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