osg::Vec3d spherical_to_cartesian (osg::Vec3d & spher, double scale)
{
    osg::Vec3d rads(spher.x (),
                    spher.y () * DEGS_TO_RADS,
                    spher.z () * DEGS_TO_RADS);
    return spherical_to_cartesian_radians(rads, scale);
}

// gets the point of intersection between two lines represented by the line
// segments passed in (note the intersection point may not be on the finite
// segment). If the lines are parallel, returns a point in the middle
static bool
getLineIntersection( Segment& s0, Segment& s1, osg::Vec3d& output )
{
    osg::Vec3d& p1 = s0.p0;
    osg::Vec3d& p2 = s0.p1;
    osg::Vec3d& p3 = s1.p0;
    osg::Vec3d& p4 = s1.p1;

    double denom = (p4.y()-p3.y())*(p2.x()-p1.x()) - (p4.x()-p3.x())*(p2.y()-p1.y());
    if ( ::fabs(denom) >= 0.001 ) //denom != 0.0 )
    {
        double ua_num = (p4.x()-p3.x())*(p1.y()-p3.y()) - (p4.y()-p3.y())*(p1.x()-p3.x());
        double ub_num = (p2.x()-p1.x())*(p1.y()-p3.y()) - (p2.y()-p1.y())*(p1.x()-p3.x());

        double ua = ua_num/denom;
        double ub = ub_num/denom;

        double isect_x = p1.x() + ua*(p2.x()-p1.x());
        double isect_y = p1.y() + ua*(p2.y()-p1.y());
        output.set( isect_x, isect_y, p2.z() );
        return true;
    }
    else // colinear or parallel
    {
        output.set( p2 );
        return false;
    }
    //return true;
}

osg::Vec3d ConfigManager::getVec3d(std::string attributeX,
        std::string attributeY, std::string attributeZ, std::string path,
        osg::Vec3d def, bool * found)
{
    bool hasEntry = false;
    bool isFound;

    osg::Vec3d result;
    result.x() = getDouble(attributeX,path,def.x(),&isFound);
    if(isFound)
    {
        hasEntry = true;
    }
    result.y() = getDouble(attributeY,path,def.y(),&isFound);
    if(isFound)
    {
        hasEntry = true;
    }
    result.z() = getDouble(attributeZ,path,def.z(),&isFound);
    if(isFound)
    {
        hasEntry = true;
    }

    if(found)
    {
        *found = hasEntry;
    }
    return result;
}

void
GeoLocator::cropLocal( osg::Vec3d& local ) const
{
    // crop if necessary:
    local.x() = osg::clampBetween( _x0 + local.x()*(_x1 - _x0), 0.0, 1.0 );
    local.y() = osg::clampBetween( _y0 + local.y()*(_y1 - _y0), 0.0, 1.0 );
}

bool
SpatialReference::createLocalToWorld(const osg::Vec3d& xyz, osg::Matrixd& out_local2world ) const
{
    if ( (isProjected() || _is_plate_carre) && !isCube() )
    {
        osg::Vec3d world;
        if ( !transformToWorld( xyz, world ) )
            return false;
        out_local2world = osg::Matrix::translate(world);
    }
    else if ( isECEF() )
    {
        //out_local2world = ECEF::createLocalToWorld(xyz);
        _ellipsoid->computeLocalToWorldTransformFromXYZ(xyz.x(), xyz.y(), xyz.z(), out_local2world);
    }
    else
    {
        // convert MSL to HAE if necessary:
        osg::Vec3d geodetic;
        if ( !transform(xyz, getGeodeticSRS(), geodetic) )
            return false;

        // then to ECEF:
        osg::Vec3d ecef;
        if ( !transform(geodetic, getGeodeticSRS()->getECEF(), ecef) )
            return false;

        //out_local2world = ECEF::createLocalToWorld(ecef);        
        _ellipsoid->computeLocalToWorldTransformFromXYZ(ecef.x(), ecef.y(), ecef.z(), out_local2world);
    }
    return true;
}

osg::Matrixd ViewingCore::computeProjection() const
{
    if( !( _scene.valid() ) ) {
        osg::notify( osg::WARN ) << "ViewingCore::computeProjection: _scene == NULL." << std::endl;
        return( osg::Matrixd::identity() );
    }

    // TBD do we really want eyeToCenter to be a vector
    // to the *bound* center, or to the *view* center?
    const osg::BoundingSphere& bs = _scene->getBound();
    const osg::Vec3d eyeToCenter( bs._center - getEyePosition() );
    if( _ortho ) {
        double zNear = eyeToCenter.length() - bs._radius;
        double zFar = eyeToCenter.length() + bs._radius;

        const double xRange = _aspect * ( _orthoTop - _orthoBottom );
        const double right = xRange * .5;

        return( osg::Matrixd::ortho( -right, right, _orthoBottom, _orthoTop, zNear, zFar ) );
    } else {
        double zNear = eyeToCenter.length() - bs._radius;
        double zFar = zNear + ( bs._radius * 2. );
        if( zNear < 0. ) {
            zNear = zFar / 2000.; // Default z ratio.
        }
        return( osg::Matrixd::perspective( _fovy, _aspect, zNear, zFar ) );
    }
}

void DataOutputStream::writeVec3d(const osg::Vec3d& v){
    writeDouble(v.x());
    writeDouble(v.y());
    writeDouble(v.z());

    if (_verboseOutput) std::cout<<"read/writeVec3d() ["<<v<<"]"<<std::endl;
}

std::string toString(const osg::Vec3d &value)
{
    std::stringstream str;

    str << value.x() << " " << value.y() << " " << value.z();
    return str.str();
}

void ScreenBase::computeDefaultViewProj(osg::Vec3d eyePos, osg::Matrix & view,
        osg::Matrix & proj)
{
    //translate screen to origin
    osg::Matrix screenTrans;
    screenTrans.makeTranslate(-_myInfo->xyz);

    //rotate screen to xz
    osg::Matrix screenRot;
    screenRot.makeRotate(-_myInfo->h * M_PI / 180.0,osg::Vec3(0,0,1),
            -_myInfo->p * M_PI / 180.0,osg::Vec3(1,0,0),
            -_myInfo->r * M_PI / 180.0,osg::Vec3(0,1,0));

    eyePos = eyePos * screenTrans * screenRot;

    //make frustum
    float top, bottom, left, right;
    float screenDist = -eyePos.y();

    top = _near * (_myInfo->height / 2.0 - eyePos.z()) / screenDist;
    bottom = _near * (-_myInfo->height / 2.0 - eyePos.z()) / screenDist;
    right = _near * (_myInfo->width / 2.0 - eyePos.x()) / screenDist;
    left = _near * (-_myInfo->width / 2.0 - eyePos.x()) / screenDist;

    proj.makeFrustum(left,right,bottom,top,_near,_far);

    // move camera to origin
    osg::Matrix cameraTrans;
    cameraTrans.makeTranslate(-eyePos);

    //make view
    view = screenTrans * screenRot * cameraTrans
            * osg::Matrix::lookAt(osg::Vec3(0,0,0),osg::Vec3(0,1,0),
                    osg::Vec3(0,0,1));
}

bool
CubeFaceLocator::convertLocalToModel( const osg::Vec3d& local, osg::Vec3d& world ) const
{
#if ((OPENSCENEGRAPH_MAJOR_VERSION <= 2) && (OPENSCENEGRAPH_MINOR_VERSION < 8))
    // OSG 2.7 bug workaround: bug fix in Locator submitted by GW
    const_cast<CubeFaceLocator*>(this)->_inverse.invert( _transform );
#endif

    if ( _coordinateSystemType == GEOCENTRIC )
    {
        //Convert the NDC coordinate into face space
        osg::Vec3d faceCoord = local * _transform;

        double lat_deg, lon_deg;
        if ( !CubeUtils::faceCoordsToLatLon( faceCoord.x(), faceCoord.y(), _face, lat_deg, lon_deg ))
            return false;

        //OE_NOTICE << "LatLon=" << latLon <<  std::endl;

        // convert to geocentric:
        _ellipsoidModel->convertLatLongHeightToXYZ(
            osg::DegreesToRadians( lat_deg ),
            osg::DegreesToRadians( lon_deg ),
            local.z(),
            world.x(), world.y(), world.z() );

        return true;
    }    
    return true;
}

bool 
SpatialReference::transformFromWorld(const osg::Vec3d& world,
                                     osg::Vec3d&       output,
                                     double*           out_haeZ ) const
{
    if ( (isGeographic() && !isPlateCarre()) || isCube() ) //isGeographic() && !_is_plate_carre )
    {
        return transformFromECEF(world, output, out_haeZ);
    }
    else // isProjected || _is_plate_carre
    {
        output = world;

        if (out_haeZ)
            *out_haeZ = world.z();

        if ( _vdatum.valid() )
        {
            // get the geographic coords by converting x/y/hae -> lat/long/msl:
            osg::Vec3d lla;
            if (!transform(world, getGeographicSRS(), lla) )
                return false;

            output.z() = lla.z();
        }

        return true;
    }
}

// Returns angle between 2 vectors in degrees
double ViroManipulator::VecAngle(osg::Vec3d a, osg::Vec3d b){
	double ang;
	a.normalize();
	b.normalize();
	ang = RadiansToDegrees( acos(a * b) );
	return fabs( ang );
}

void LOSCreationDialog::getLOSPoint(LOSPoint point, osg::Vec3d& out_point, bool relative)
{
  double alt = 0.0;

  switch(point)
  {
    case P2P_START:
      alt = _ui.p1AltBox->value();
      if (!relative && _ui.p2pRelativeCheckBox->checkState() == Qt::Checked)
        alt += _p1BaseAlt;
      out_point.set(_ui.p1LonBox->value(), _ui.p1LatBox->value(), alt);
      break;
    case P2P_END:
      alt = _ui.p2AltBox->value();
      if (!relative && _ui.p2pRelativeCheckBox->checkState() == Qt::Checked)
        alt += _p2BaseAlt;
      out_point.set(_ui.p2LonBox->value(), _ui.p2LatBox->value(), alt);
      break;
    case RADIAL_CENTER:
      alt = _ui.radAltBox->value();
      if (!relative && _ui.radRelativeCheckBox->checkState() == Qt::Checked)
        alt += _radBaseAlt;
      out_point.set(_ui.radLonBox->value(), _ui.radLatBox->value(), alt);
      break;
  }
}

 inline double turbulence(module::Perlin& noise, const osg::Vec3d& v, double f )
 {
     double t = -0.5;
     for( ; f<getPixelsPerTile()/2; f *= 2 ) 
         t += abs(noise.GetValue(v.x(), v.y(), v.z())/f);
     return t;
 }

static bool getRelativeWorld(double x, double y, double relativeHeight, MapNode* mapNode, osg::Vec3d& world )
{
    GeoPoint mapPoint(mapNode->getMapSRS(), x, y);
    osg::Vec3d pos;
    mapNode->getMap()->toWorldPoint(mapPoint, pos);

    osg::Vec3d up(0,0,1);
    const osg::EllipsoidModel* em = mapNode->getMap()->getProfile()->getSRS()->getEllipsoid();
    if (em)
    {
        up = em->computeLocalUpVector( world.x(), world.y(), world.z());
    }    
    up.normalize();

    double segOffset = 50000;

    osg::Vec3d start = pos + (up * segOffset);
    osg::Vec3d end = pos - (up * segOffset);
    
    osgUtil::LineSegmentIntersector* i = new osgUtil::LineSegmentIntersector( start, end );
    
    osgUtil::IntersectionVisitor iv;    
    iv.setIntersector( i );
    mapNode->getTerrainEngine()->accept( iv );

    osgUtil::LineSegmentIntersector::Intersections& results = i->getIntersections();
    if ( !results.empty() )
    {
        const osgUtil::LineSegmentIntersector::Intersection& result = *results.begin();
        world = result.getWorldIntersectPoint();
        world += up * relativeHeight;
        return true;
    }
    return false;    
}

HUDHoverScaler::HUDHoverScaler(osgviz::Object* obj, const osg::Vec3d &size, const osg::Vec3d &scale, Type type, osg::Vec3d anchor_offset, HUD* hud):obj(obj),anchor_offset(anchor_offset),scale(scale),size(size),type(type),hud(hud){
    scaled = false;
    initial_scale = obj->getScale();
    if (scale.x() == 0 || scale.y() == 0 || scale.z() == 0){
        printf("%s scale cannot be 0\n",__PRETTY_FUNCTION__);
    }
}

void LazyCameraManipulator::calculateNextCameraPosition()
{ 
    // retrieve player position and direction of movement
    const Player *player = dynamic_cast<const Player *>(getTrackNode());
    const osg::Vec3d nodePosition = player->getPosition();
    const PlayerState *playerState = player->getPlayerState();

    const int newDirectionX = playerState->getDirectionX();

    // if this is the first run, follow node directly
    if(_firstRun)
    {
        _newCameraPosition.x() = nodePosition.x();
        _oldCameraPosition.x() = nodePosition.x();
        _oldNodePosition = nodePosition;
        _durationOfMovementX = 0;
        _firstRun = false;
        return;
    }

    // +++ step 1 +++
    // check if direction of movement has changed
    bool directionChanged = false;
    if(newDirectionX != _directionOfMovementX)
    {
        if(newDirectionX != 0)
        {
            _durationOfMovementX = 0;
            directionChanged = true;
        }
        _directionOfMovementX = newDirectionX;
    }
            
    float stepper = (_numSimulationSubSteps > 0) ? ((nodePosition.x() - _oldNodePosition.x()) / _numSimulationSubSteps) : 0.0f;
    
    for(int i=0; i<_numSimulationSubSteps; i++, _oldNodePosition.x() += stepper, _durationOfMovementX++)
    {
        if(_fadeOut)
            _distance += 10.0f * _numSimulationSubSteps;
        
        if(!directionChanged && fabs(_oldCameraPosition.x() - _oldNodePosition.x()) < 0.001 && _durationOfMovementX > 20)
        {
            // +++ step 2 +++
            // if direction has not changed, check if we were already following the node
            _newCameraPosition.x() = nodePosition.x();
        }
        else
        {
            // +++ step 3 +++
            // in any other case continue (or begin) approaching the node
            _newCameraPosition.x() = _oldCameraPosition.x() + (_oldNodePosition.x() - _oldCameraPosition.x()) * (_durationOfMovementX / MAX_FRAME_DELAY);
        }
    
        _oldCameraPosition = _newCameraPosition;
    }
    
    _oldNodePosition = nodePosition;
}

bool CameraMath::isInRect( osg::Vec3d point, double width, double height, double margin )
{
	if ( point.x() < margin || point.x() >  width - margin || point.y() < margin || point.y() > height - margin ) {
		return false;
	}
	else {
		return true;
	}
}

bool
TritonContext::intersect(const osg::Vec3d& start, const osg::Vec3d& dir, float& out_height, osg::Vec3f& out_normal) const
{
    ::Triton::Vector3 p(start.ptr());
    ::Triton::Vector3 d(dir.ptr());
    ::Triton::Vector3 normal;
    bool ok = _ocean->GetHeight(p, d, out_height, normal);
    out_normal.set(normal.x, normal.y, normal.z);
    return ok;
}

osg::Vec3d LineAnalysis::getWorldCoord(osg::Vec3d pos)
{
	osg::Vec3d world;
	m_pMap3D->getSRS()->getEllipsoid()->convertLatLongHeightToXYZ(
		osg::DegreesToRadians(pos.y()),
		osg::DegreesToRadians(pos.x()),
		pos.z(), world.x(), world.y(), world.z());

	return world;
}