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

Last change on this file since 228 was 228, checked in by Torben Dannhauer, 13 years ago
File size: 20.5 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::computeNodeCenterAndRotation(osg::Vec3d& nodeCenter, osg::Quat& nodeRotation) const
319{
320        if (_trackNodePath.empty())
321                return;
322
323    osg::Matrixd localToWorld, worldToLocal;
324    osg::NodePath nodePath;
325       
326    if (  _trackNodePath.getNodePath(nodePath))
327    {
328        worldToLocal = osg::computeWorldToLocal(nodePath);
329        localToWorld = osg::computeLocalToWorld(nodePath);
330        nodeCenter = osg::Vec3d(nodePath.back()->getBound().center())*localToWorld;
331    }
332    else
333    {
334        nodeCenter = osg::Vec3d(0.0f,0.0f,0.0f)*localToWorld;
335    }
336
337
338    switch(_trackerMode)
339    {
340        case(NODE_CENTER_AND_AZIM):
341        {
342            CoordinateFrame coordinateFrame = getCoordinateFrame(nodeCenter);
343            osg::Matrixd localToFrame(localToWorld*osg::Matrixd::inverse(coordinateFrame));
344
345            double azim = atan2(-localToFrame(0,1),localToFrame(0,0));
346            osg::Quat nodeRotationRelToFrame, rotationOfFrame;
347            nodeRotationRelToFrame.makeRotate(-azim,0.0,0.0,1.0);
348            rotationOfFrame = coordinateFrame.getRotate();
349            nodeRotation = nodeRotationRelToFrame*rotationOfFrame;
350            break;
351        }
352        case(NODE_CENTER_AND_ROTATION):
353        {
354            // scale the matrix to get rid of any scales before we extract the rotation.
355            double sx = 1.0/sqrt(localToWorld(0,0)*localToWorld(0,0) + localToWorld(1,0)*localToWorld(1,0) + localToWorld(2,0)*localToWorld(2,0));
356            double sy = 1.0/sqrt(localToWorld(0,1)*localToWorld(0,1) + localToWorld(1,1)*localToWorld(1,1) + localToWorld(2,1)*localToWorld(2,1));
357            double sz = 1.0/sqrt(localToWorld(0,2)*localToWorld(0,2) + localToWorld(1,2)*localToWorld(1,2) + localToWorld(2,2)*localToWorld(2,2));
358            localToWorld = localToWorld*osg::Matrixd::scale(sx,sy,sz);
359
360            nodeRotation = localToWorld.getRotate();
361            break;
362        }
363        case(NODE_CENTER):
364        default:
365        {
366            CoordinateFrame coordinateFrame = getCoordinateFrame(nodeCenter);
367            nodeRotation = coordinateFrame.getRotate();
368            break;
369        }
370    }
371
372}
373
374
375osg::Matrixd NodeTrackerSpaceMouse::getMatrix() const
376{
377    osg::Vec3d nodeCenter;
378    osg::Quat nodeRotation;
379    computeNodeCenterAndRotation(nodeCenter,nodeRotation);
380    return osg::Matrixd::translate(0.0,0.0,_distance)*osg::Matrixd::rotate(_rotation)*osg::Matrixd::rotate(nodeRotation)*osg::Matrix::translate(nodeCenter);
381}
382
383osg::Matrixd NodeTrackerSpaceMouse::getInverseMatrix() const
384{
385    osg::Vec3d nodeCenter;
386    osg::Quat nodeRotation;
387    computeNodeCenterAndRotation(nodeCenter,nodeRotation);
388    return osg::Matrixd::translate(-nodeCenter)*osg::Matrixd::rotate(nodeRotation.inverse())*osg::Matrixd::rotate(_rotation.inverse())*osg::Matrixd::translate(0.0,0.0,-_distance);
389}
390
391void NodeTrackerSpaceMouse::computePosition(const osg::Vec3d& eye,const osg::Vec3d& center,const osg::Vec3d& up)
392{
393    if (!_node) return;
394
395    // compute rotation matrix
396    osg::Vec3 lv(center-eye);
397    _distance = lv.length();
398   
399    osg::Matrixd lookat;
400    lookat.makeLookAt(eye,center,up);
401   
402    _rotation = lookat.getRotate().inverse();
403}
404
405bool NodeTrackerSpaceMouse::calcMovement()
406{
407    // return if less then two events have been added.
408    if (_ga_t0.get()==NULL || _ga_t1.get()==NULL) return false;
409
410    double dx = _ga_t0->getXnormalized()-_ga_t1->getXnormalized();
411    double dy = _ga_t0->getYnormalized()-_ga_t1->getYnormalized();
412
413
414    float distance = sqrtf(dx*dx + dy*dy);
415    // return if movement is too fast, indicating an error in event values or change in screen.
416    if (distance>0.5)
417    {
418        return false;
419    }
420   
421    // return if there is no movement.
422    if (distance==0.0f)
423    {
424        return false;
425    }
426
427    osg::Vec3d nodeCenter;
428    osg::Quat nodeRotation;
429    computeNodeCenterAndRotation(nodeCenter, nodeRotation);
430
431    unsigned int buttonMask = _ga_t1->getButtonMask();
432
433    if (buttonMask==GUIEventAdapter::LEFT_MOUSE_BUTTON)
434    {
435
436        if (_rotationMode==TRACKBALL)
437        {
438
439            // rotate camera.
440            osg::Vec3 axis;
441            double angle;
442
443            double px0 = _ga_t0->getXnormalized();
444            double py0 = _ga_t0->getYnormalized();
445
446            double px1 = _ga_t1->getXnormalized();
447            double py1 = _ga_t1->getYnormalized();
448
449
450            trackball(axis,angle,px1,py1,px0,py0);
451
452            osg::Quat new_rotate;
453            new_rotate.makeRotate(angle,axis);
454
455            _rotation = _rotation*new_rotate;
456        }
457        else
458        {
459            osg::Matrix rotation_matrix;
460            rotation_matrix.makeRotate(_rotation);
461
462            osg::Vec3d lookVector = -getUpVector(rotation_matrix);
463            osg::Vec3d sideVector = getSideVector(rotation_matrix);
464            osg::Vec3d upVector = getFrontVector(rotation_matrix);
465           
466            osg::Vec3d localUp(0.0f,0.0f,1.0f);
467
468            osg::Vec3d forwardVector = localUp^sideVector;
469            sideVector = forwardVector^localUp;
470
471            forwardVector.normalize();
472            sideVector.normalize();
473           
474            osg::Quat rotate_elevation;
475            rotate_elevation.makeRotate(dy,sideVector);
476
477            osg::Quat rotate_azim;
478            rotate_azim.makeRotate(-dx,localUp);
479           
480            _rotation = _rotation * rotate_elevation * rotate_azim;
481           
482        }
483       
484        return true;
485
486    }
487    else if (buttonMask==GUIEventAdapter::MIDDLE_MOUSE_BUTTON ||
488        buttonMask==(GUIEventAdapter::LEFT_MOUSE_BUTTON|GUIEventAdapter::RIGHT_MOUSE_BUTTON))
489    {
490        return true;
491    }
492    else if (buttonMask==GUIEventAdapter::RIGHT_MOUSE_BUTTON)
493    {
494
495        // zoom model.
496
497        double fd = _distance;
498        double scale = 1.0f+dy;
499        if (fd*scale>_minimumDistance)
500        {
501
502            _distance *= scale;
503
504        } else
505        {
506            _distance = _minimumDistance;
507        }
508       
509        return true;
510
511    }
512
513    return false;
514}
515
516bool NodeTrackerSpaceMouse::calcMovementSpaceMouse()
517{
518        // TRACKBALL mode is not possible, because it uses the mouseevent, which does not happen with Spacemouse.
519
520   double dTX, dTY, dTZ;        // Only TZ is used later.
521    _spaceMouse->getTranslations(dTX, dTY, dTZ);
522
523    double dRX, dRY, dRZ;
524    _spaceMouse->getRotations(dRX, dRY, dRZ);
525
526        //OSG_NOTIFY( osg::INFO ) << "Recieved Spacemousevalues: dRX:" << dRX << ", dRY:" << dRY << ", dRZ:" << dRZ << ", dTX:" << dTX << std::endl;
527
528        if(!_ah_init)
529        {
530                TZ=dTZ;
531                RX=dRX;
532                RY=dRY;
533                RZ=dRZ;
534                _ah_init=true;
535        }
536
537        if (_autohoming)        // The factors are required to allow macroscopic movements.
538        {
539                TZ=dTZ*5;
540                if (!_distanceDependendAV)
541                {
542                        RX=dRX*100;
543                        RY=dRY*100;
544                        RZ=dRZ*100;
545                }
546        }
547        else            // NOT Autohoming
548        {
549                TZ+=dTZ;
550                //OSG_NOTIFY( osg::INFO ) << "Stored Spacemousevalues: RX:" << RX << ", RY:" << RY << ", RZ:" << RZ << ", TX:" << TX << std::endl;
551                if (_distanceDependendAV)
552                {
553                        RX+=(dRX*800/TZ);
554                        RY+=(dRY*800/TZ);
555                        RZ+=(dRZ*800/TZ);
556                }
557                else
558                {
559                        RX+=dRX;
560                        RY+=dRY;
561                        RZ+=dRZ;
562                }
563        }
564
565    osg::Vec3d nodeCenter;
566    osg::Quat nodeRotation;
567    computeNodeCenterAndRotation(nodeCenter, nodeRotation);
568
569    // ROTATION PART
570    if (_rotationMode==TRACKBALL)
571    {
572                // return if less then two events have been added.
573                if (_ga_t0.get()==NULL || _ga_t1.get()==NULL) return false;
574
575        // rotate camera.
576        osg::Vec3 axis;
577        double angle;
578
579        double px0 = _ga_t0->getXnormalized();
580        double py0 = _ga_t0->getYnormalized();
581
582        double px1 = _ga_t1->getXnormalized();
583        double py1 = _ga_t1->getYnormalized();
584
585
586        trackball(axis,angle,px1,py1,px0,py0);
587
588        osg::Quat new_rotate;
589        new_rotate.makeRotate(angle,axis);
590
591        _rotation = _rotation*new_rotate;
592    }
593    else
594    {
595        osg::Matrix rotation_matrix;
596        rotation_matrix.makeRotate(_rotation);
597
598        osg::Vec3d lookVector = -getUpVector(rotation_matrix);
599        osg::Vec3d sideVector = getSideVector(rotation_matrix);
600        osg::Vec3d upVector = getFrontVector(rotation_matrix);
601       
602        osg::Vec3d localUp(0.0f,0.0f,1.0f);
603
604        osg::Vec3d forwardVector = localUp^sideVector;
605        sideVector = forwardVector^localUp;
606
607        forwardVector.normalize();
608        sideVector.normalize();
609       
610        osg::Quat rotate_elevation;
611                rotate_elevation.makeRotate(osg::DegreesToRadians(RX*90), sideVector);
612
613        osg::Quat rotate_azim;
614                rotate_azim.makeRotate(osg::DegreesToRadians(RY*90), localUp);
615       
616        _rotation = _rotation * rotate_elevation * rotate_azim;
617       
618    }
619
620    // TRANSLATION PART
621    double scale = 1.0f+TZ/100;
622        if (_distance*scale>_minimumDistance)
623    {
624
625        _distance *= scale;
626
627    } else
628    {
629                TZ = (_minimumDistance/_distance - 1)*100;      // Reset TZ to the value f(_minimalDistance, _distance, scale-function).
630        _distance = _minimumDistance;
631    }
632    return true;
633}
634
635void NodeTrackerSpaceMouse::clampOrientation()
636{
637}
638
639
640/*
641 * This size should really be based on the distance from the center of
642 * rotation to the point on the object underneath the mouse.  That
643 * point would then track the mouse as closely as possible.  This is a
644 * simple example, though, so that is left as an Exercise for the
645 * Programmer.
646 */
647const float TRACKBALLSIZE = 0.8f;
648
649/*
650 * Ok, simulate a track-ball.  Project the points onto the virtual
651 * trackball, then figure out the axis of rotation, which is the cross
652 * product of P1 P2 and O P1 (O is the center of the ball, 0,0,0)
653 * Note:  This is a deformed trackball-- is a trackball in the center,
654 * but is deformed into a hyperbolic sheet of rotation away from the
655 * center.  This particular function was chosen after trying out
656 * several variations.
657 *
658 * It is assumed that the arguments to this routine are in the range
659 * (-1.0 ... 1.0)
660 */
661void NodeTrackerSpaceMouse::trackball(osg::Vec3& axis,double & angle, double  p1x, double  p1y, double  p2x, double  p2y)
662{
663    /*
664     * First, figure out z-coordinates for projection of P1 and P2 to
665     * deformed sphere
666     */
667
668    osg::Matrix rotation_matrix(_rotation);
669
670
671    osg::Vec3d uv = osg::Vec3d(0.0,1.0,0.0)*rotation_matrix;
672    osg::Vec3d sv = osg::Vec3d(1.0,0.0,0.0)*rotation_matrix;
673    osg::Vec3d lv = osg::Vec3d(0.0,0.0,-1.0)*rotation_matrix;
674
675    osg::Vec3d p1 = sv*p1x+uv*p1y-lv*tb_project_to_sphere(TRACKBALLSIZE,p1x,p1y);
676    osg::Vec3d p2 = sv*p2x+uv*p2y-lv*tb_project_to_sphere(TRACKBALLSIZE,p2x,p2y);
677
678    /*
679     *  Now, we want the cross product of P1 and P2
680     */
681
682// Robert,
683//
684// This was the quick 'n' dirty  fix to get the trackball doing the right
685// thing after fixing the Quat rotations to be right-handed.  You may want
686// to do something more elegant.
687//   axis = p1^p2;
688axis = p2^p1;
689    axis.normalize();
690
691    /*
692     *  Figure out how much to rotate around that axis.
693     */
694    double t = (p2-p1).length() / (2.0*TRACKBALLSIZE);
695
696    /*
697     * Avoid problems with out-of-control values...
698     */
699    if (t > 1.0) t = 1.0;
700    if (t < -1.0) t = -1.0;
701    angle = inRadians(asin(t));
702
703}
704
705
706/*
707 * Project an x,y pair onto a sphere of radius r OR a hyperbolic sheet
708 * if we are away from the center of the sphere.
709 */
710double NodeTrackerSpaceMouse::tb_project_to_sphere(double  r, double  x, double  y)
711{
712    float d, t, z;
713
714    d = sqrt(x*x + y*y);
715                                 /* Inside sphere */
716    if (d < r * 0.70710678118654752440)
717    {
718        z = sqrt(r*r - d*d);
719    }                            /* On hyperbola */
720    else
721    {
722        t = r / 1.41421356237309504880;
723        z = t*t / d;
724    }
725    return z;
726}
Note: See TracBrowser for help on using the repository browser.