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

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

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

void PanoDrawableLOD::setZoom(osg::Vec3 dir, float k)
{
    for(std::map<int,sph_model*>::iterator it = _pdi->modelMap.begin(); it!= _pdi->modelMap.end(); it++)
    {
	it->second->set_zoom(dir.x(),dir.y(),dir.z(),k);
    }
}

osg::Vec2 calcCoordReprojTrans(const osg::Vec3 &vert,const osg::Matrix &trans,const osg::Matrix &viewProj,const osg::Vec2 &size,const osg::Vec4 &ratio){
    osg::Vec4 v(vert.x(),vert.y(),vert.z(),1.0);
    v=v*trans;
    v=v*viewProj;
    v.x() /= v.w();
    v.y() /= v.w();
    v.z() /= v.w();
    v.w() /= v.w();
    //std::cout << "Pre shift " << v << std::endl;
    v.x() /= size.x();;
    v.y() /= size.y();


    v.x() -= (ratio.x()/size.x());
    v.y() -= (ratio.y()/size.y());
    //std::cout << "Post shift " << v << std::endl;


    //  std::cout << "PP shift " << v << std::endl;


    osg::Vec2 tc(v.x(),v.y());
    tc.x() *= ratio.z();
    tc.y() *=ratio.w();
    //tc.x()*=ratio.x();
    //tc.y()*=ratio.y();
    tc.x()/=(ratio.z());
    tc.y()/=(ratio.w());


    return tc;

}

bool HeightFieldLayer::getValue(unsigned int i, unsigned int j, osg::Vec3& value) const
{
    value.x() = _heightField->getHeight(i,j);
    value.y() = _defaultValue.y();
    value.z() = _defaultValue.z();
    return true;
}

    void addEdgePlane(const osg::Vec3& corner, const osg::Vec3& edge)
    {
        osg::Vec3 normal( edge.y(), -edge.x(), 0.0f);
        normal.normalize();

        add(osg::Plane(normal, corner));
    }

 void setMinMax(const osg::Vec3& pos)
 {
     _maxY = std::max(_maxY, (double) pos.y());
     _minY = std::min(_minY, (double) pos.y());
     _maxX = std::max(_maxX, (double) pos.x());
     _minX = std::min(_minX, (double) pos.x());
 }

double MultitouchNavigation::angleBetween3DVectors(osg::Vec3 v1, osg::Vec3 v2)
{
    // http://codered.sat.qc.ca/redmine/projects/spinframework/repository/revisions/b6245189c19a7c6ba4fdb126940321c41c44e228/raw/src/spin/osgUtil.cpp

    // normalize vectors (note: this must be done alone, not within any vector arithmetic. why?!)
    v1.normalize();
    v2.normalize();

    // Get the dot product of the vectors
    double dotProduct = v1 * v2;

    // for acos, the value has to be between -1.0 and 1.0, but due to numerical imprecisions it sometimes comes outside this range
    if (dotProduct > 1.0)
        dotProduct = 1.0;
    if (dotProduct < -1.0)
        dotProduct = -1.0;

    // Get the angle in radians between the 2 vectors (should this be -acos ? ie, negative?)
    double angle = acos(dotProduct);

    // Here we make sure that the angle is not a -1.#IND0000000 number, which means indefinite
    if (std::isnan(angle)) //__isnand(x)
        return 0;

    // Return the angle in radians
    return (angle);
}

osg::Quat makeQuat(const osg::Vec3& radians, ERotationOrder fbxRotOrder)
{
    fbxDouble3 degrees(
        osg::RadiansToDegrees(radians.x()),
        osg::RadiansToDegrees(radians.y()),
        osg::RadiansToDegrees(radians.z()));
    return makeQuat(degrees, fbxRotOrder);
}

osg::Vec3 Particle_Viewer::fixPosition(osg::Vec3 position)
{
    osg::Vec3 fixPosition;
    fixPosition.x()=position.x();
    fixPosition.y()=position.z();
    fixPosition.z()=position.y();
    return fixPosition;
}

float HfT_osg_Plugin01_ParametricSurface::abstand(osg::Vec3 startPickPos, int i)
{
    float x = (*mp_Points_Geom)[i].x() - startPickPos.x();
    float y = (*mp_Points_Geom)[i].y() - startPickPos.y();
    float z = (*mp_Points_Geom)[i].z() - startPickPos.z();

    float abst = (float)sqrt(x * x + y * y + z * z);
    return abst;
}

bool Uniform::getElement( unsigned int index, osg::Vec3& v3 ) const
{
    if( index>=getNumElements() || !isCompatibleType(FLOAT_VEC3) ) return false;
    unsigned int j = index * getTypeNumComponents(getType());
    v3.x() = (*_floatArray)[j];
    v3.y() = (*_floatArray)[j+1];
    v3.z() = (*_floatArray)[j+2];
    return true;
}

void DataOutputStream::writeVec3(const osg::Vec3& v){

//	std::cout << "write " << _ostream->tellp() << std::endl;

    writeFloat(v.x());
    writeFloat(v.y());
    writeFloat(v.z());

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

void osgParticle::Particle::render(osg::RenderInfo& renderInfo, const osg::Vec3& xpos, const osg::Vec3& xrot) const
{
#if defined(OSG_GL_MATRICES_AVAILABLE)
    if (_drawable.valid())
    {
        bool requiresRotation = (xrot.x()!=0.0f || xrot.y()!=0.0f || xrot.z()!=0.0f);
        glColor4f(_current_color.x(),
                  _current_color.y(),
                  _current_color.z(),
                  _current_color.w() * _current_alpha);
        glPushMatrix();
        glTranslatef(xpos.x(), xpos.y(), xpos.z());
        if (requiresRotation)
        {
            osg::Quat rotation(xrot.x(), osg::X_AXIS, xrot.y(), osg::Y_AXIS, xrot.z(), osg::Z_AXIS);
#if defined(OSG_GLES1_AVAILABLE)
            glMultMatrixf(osg::Matrixf(rotation).ptr());
#else
            glMultMatrixd(osg::Matrixd(rotation).ptr());
#endif
        }
        _drawable->draw(renderInfo);
        glPopMatrix();
    }
#else
    OSG_NOTICE<<"Warning: Particle::render(..) not supported for user-defined shape."<<std::endl;
#endif
}

void VELOModel::addHit(osg::Vec3& coords){

	osg::ref_ptr<osg::MatrixTransform> t1 = new osg::MatrixTransform;
	t1->setMatrix( osg::Matrix::translate(
		coords.x() - HIT_RADIUS,
		coords.y(),
		coords.z()) );
	t1->addChild( _hitGeode.get() );

	hits->addChild( t1.get() );
}

inline osg::Vec3 ReaderWriterOBJ::transformNormal(const osg::Vec3& vec, const bool rotate) const
{
    if(rotate==true)
    {
        return osg::Vec3(vec.x(),-vec.z(),vec.y());
    }
    else
    {
        return vec;
    }
}

void ComputeBoundingBoxByRotation(osg::BoundingBox & InOut,osg::Vec3 BBCenter,float BBSize,osg::Matrixd Rotation)
{
	osg::BoundingBox bb;
	bb.set(BBCenter.x()-BBSize,BBCenter.y()-BBSize,BBCenter.z()-BBSize,BBCenter.x()+BBSize,BBCenter.y()+BBSize,BBCenter.z()+BBSize);
    osg::BoundingBox Tbb;
	for(unsigned int i=0;i<8;i++)
    {
        Tbb.expandBy(Rotation.preMult(bb.corner(i)));
    }
	InOut.set(Tbb.xMin(),Tbb.yMin(),Tbb.zMin(),Tbb.xMax(),Tbb.yMax(),Tbb.zMax());
}

void PanoManipulator::JumpTo(osg::Vec3 NewSpatialPos)
{
PartialVertexDEM JumpLoc;
JumpLoc.Lat = NewSpatialPos.y();
JumpLoc.Lon = NewSpatialPos.x();
JumpLoc.Elev = NewSpatialPos.z();
MasterScene.DegToCart(JumpLoc);

_eye[0] = JumpLoc.XYZ[0];
_eye[1] = JumpLoc.XYZ[1];
_eye[2] = JumpLoc.XYZ[2];
} // PanoManipulator::JumpTo

    double Frustum::getAngleBetween(const osg::Vec3 &a, const osg::Vec3 &b) {

        float dot = a * b;
        float mag = a.length() * b.length();
        
        double angle = acos(dot/mag);
        
		if (osg::isNaN(angle)) 
            return 0;
        else
            return angle;
    }

void VertexCollectionVisitor::addVertex(osg::Vec3 vertex)
{
  if (_geocentric)
  {
    double lat, lon, height;
    _ellipsoidModel->convertXYZToLatLongHeight(vertex.x(), vertex.y(), vertex.z(), lat, lon, height);
    _vertices->push_back(osg::Vec3d(osg::RadiansToDegrees(lon), osg::RadiansToDegrees(lat), height));
  }
  else
  {
    _vertices->push_back(vertex);
  }
}

void CameraFlight::navigate(osg::Matrix destMat, osg::Vec3 destVec)
{
    osg::Matrix objMat = SceneManager::instance()->getObjectTransform()->getMatrix();

    switch(_flightMode)
    {
	case INSTANT:{
	    cout<<"USING INSTANT"<<endl;
	    SceneManager::instance()->setObjectMatrix(destMat);
	    break;
	}
	case SATELLITE:
	    cout<<"USING SATELLITE"<<endl;

	    t = 0.0;
	    total = 0.0;
	
    	    objMat.decompose(trans2, rot2, scale2, so2);
	    a = (maxHeight - trans2[1])/25.0;

	    map->getProfile()->getSRS()->getEllipsoid()->convertLatLongHeightToXYZ(
					destVec.x(),destVec.y(),destVec.z(),toVec.x(),toVec.y(),toVec.z());

	    fromVec = origPlanetPoint;

	    fromVec.normalize();
	    toVec.normalize();

	    origAngle = acos((fromVec * toVec)/((fromVec.length() * toVec.length())));	
	    origAngle = RadiansToDegrees(origAngle);

	    angle = origAngle;

    	    if(origAngle <= 10) {
		maxHeight = 6.5e+9;
	    }

	    else {
		maxHeight = 2.0e+10;
	    }

	    flagRot = true;
	    break;
	case AIRPLANE:
	    cout<<"USING AIRPLANE"<<endl;
	    break;
	default:
	    cout<<"PICK THE ALGORYTHM!!!!"<<endl;
	    break;
    }
}