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

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

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