OSG OSGEATH各种坐标的相互转化

平时总要在各种坐标之间转化，总是想不起怎么弄，记录一下，忘了可以在博客上查找。

一、屏幕坐标转世界坐标

osg::Vec3d ScreenToWorld(const osg::Vec3d screen)
{
	osg::Camera* camera = _global->Viewer->getCamera();
	osg::Matrix VPW = camera->getViewMatrix() * camera->getProjectionMatrix() * camera->getViewport()->computeWindowMatrix();
	osg::Matrix inverseVPW = osg::Matrix::inverse(VPW);
	osg::Vec3d world = screen * inverseVPW;
	return world;
}

bool EarthManipulator::screenToWorld(float x, float y, osg::View* theView, osg::Vec3d& out_coords) const
{
    osgViewer::View* view = dynamic_cast<osgViewer::View*>( theView );
    if ( !view || !view->getCamera() )
        return false;

    osg::ref_ptr<MapNode> mapNode;
    if ( !_mapNode.lock(mapNode) )
        return false;

    return mapNode->getTerrain()->getWorldCoordsUnderMouse(view, x, y, out_coords);
}

二、世界坐标转屏幕坐标

osg::Vec3d WorldToScreen(const osg::Vec3d world)
{
	osg::Camera* camera = _global->Viewer->getCamera();
	osg::Matrix VPW = camera->getViewMatrix() * camera->getProjectionMatrix() * camera->getViewport()->computeWindowMatrix();
	osg::Vec3d screen = world * VPW;
	return screen;
}

参照 IntersectionPicker::pick

bool
IntersectionPicker::pick( float x, float y, Hits& results ) const
{
    float local_x, local_y = 0.0;
    const osg::Camera* camera = _view->getCameraContainingPosition(x, y, local_x, local_y);
    if ( !camera )
        camera = _view->getCamera();

    osg::ref_ptr<osgEarth::PrimitiveIntersector> picker;

    double buffer_x = _buffer, buffer_y = _buffer;
    if ( camera->getViewport() )
    {
        double aspectRatio = camera->getViewport()->width()/camera->getViewport()->height();
        buffer_x *= aspectRatio;
        buffer_y /= aspectRatio;
    }

    osg::Matrix windowMatrix;

    if ( _root.valid() )
    {
        osg::Matrix modelMatrix;

        if (camera->getViewport())
        {
            windowMatrix = camera->getViewport()->computeWindowMatrix();
            modelMatrix.preMult( windowMatrix );
        }

        modelMatrix.preMult( camera->getProjectionMatrix() );
        modelMatrix.preMult( camera->getViewMatrix() );

        osg::NodePath prunedNodePath( _path.begin(), _path.end()-1 );
        modelMatrix.preMult( osg::computeWorldToLocal(prunedNodePath) );

        osg::Matrix modelInverse;
        modelInverse.invert(modelMatrix);

        osg::Vec3d startLocal(local_x, local_y, 0.0);
        osg::Vec3d startModel = startLocal * modelInverse;

        osg::Vec3d endLocal(local_x, local_y, 1.0);
        osg::Vec3d endModel = endLocal * modelInverse;

        osg::Vec3d bufferLocal(local_x + buffer_x, local_y + buffer_y, 0.0);
        osg::Vec3d bufferModel = bufferLocal * modelInverse;

        double buffer = osg::maximum((bufferModel - startModel).length(), 5.0);  //TODO: Setting a minimum of 4.0 may need revisited

        OE_DEBUG
            << "local_x:" << local_x << ", local_y:" << local_y
            << ", buffer_x:" << buffer_x << ", buffer_y:" << buffer_y
            << ", bm.x:" << bufferModel.x() << ", bm.y:" << bufferModel.y()
            << ", bm.z:" << bufferModel.z()
            << ", BUFFER: " << buffer
            << std::endl;

        picker = new osgEarth::PrimitiveIntersector(osgUtil::Intersector::MODEL, startModel, endModel, buffer);
    }
    else
    {
        picker = new osgEarth::PrimitiveIntersector(camera->getViewport() ? osgUtil::Intersector::WINDOW : osgUtil::Intersector::PROJECTION, local_x, local_y, _buffer);
    }

    picker->setIntersectionLimit( (osgUtil::Intersector::IntersectionLimit)_limit );
    osgUtil::IntersectionVisitor iv(picker.get());

    //picker->setIntersectionLimit( osgUtil::Intersector::LIMIT_ONE_PER_DRAWABLE );

    // in MODEL mode, we need to window and proj matrixes in order to support some of the 
    // features in osgEarth (like Annotation::OrthoNode).
    if ( _root.valid() )
    {
        iv.pushWindowMatrix( new osg::RefMatrix(windowMatrix) );
        iv.pushProjectionMatrix( new osg::RefMatrix(camera->getProjectionMatrix()) );
        iv.pushViewMatrix( new osg::RefMatrix(camera->getViewMatrix()) );
    }

    iv.setTraversalMask( _travMask );

    if ( _root.valid() )
        _path.back()->accept(iv);
    else
        const_cast<osg::Camera*>(camera)->accept(iv);

    if (picker->containsIntersections())
    {
        results = picker->getIntersections();
        return true;
    }
    else
    {
        results.clear();
        return false;
    }
}

三、世界坐标转经纬度

inline void EllipsoidModel::convertXYZToLatLongHeight(double X, double Y, double Z,
                                      double& latitude, double& longitude, double& height) const
{
    // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif
    double p = sqrt(X*X + Y*Y);
    double theta = atan2(Z*_radiusEquator , (p*_radiusPolar));
    double eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar)/
                          (_radiusPolar*_radiusPolar);

    double sin_theta = sin(theta);
    double cos_theta = cos(theta);

    latitude = atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) /
                     (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) );
    longitude = atan2(Y,X);

    double sin_latitude = sin(latitude);
    double N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude);

    height = p/cos(latitude) - N;
}

四、经纬度转世界坐标

inline void EllipsoidModel::computeLocalToWorldTransformFromLatLongHeight(double latitude, double longitude, double height, osg::Matrixd& localToWorld) const
{
    double X, Y, Z;
    convertLatLongHeightToXYZ(latitude,longitude,height,X,Y,Z);
    computeLocalToWorldTransformFromXYZ(X,Y,Z,localToWorld);
}

inline void EllipsoidModel::computeLocalToWorldTransformFromXYZ(double X, double Y, double Z, osg::Matrixd& localToWorld) const
{
    localToWorld.makeTranslate(X,Y,Z);


    // normalize X,Y,Z
    double inverse_length = 1.0/sqrt(X*X + Y*Y + Z*Z);
    
    X *= inverse_length;
    Y *= inverse_length;
    Z *= inverse_length;

    double length_XY = sqrt(X*X + Y*Y);
    double inverse_length_XY = 1.0/length_XY;

    // Vx = |(-Y,X,0)|
    localToWorld(0,0) = -Y*inverse_length_XY;
    localToWorld(0,1) = X*inverse_length_XY;
    localToWorld(0,2) = 0.0;

    // Vy = /(-Z*X/(sqrt(X*X+Y*Y), -Z*Y/(sqrt(X*X+Y*Y),sqrt(X*X+Y*Y))| 
    double Vy_x = -Z*X*inverse_length_XY;
    double Vy_y = -Z*Y*inverse_length_XY;
    double Vy_z = length_XY;
    inverse_length = 1.0/sqrt(Vy_x*Vy_x + Vy_y*Vy_y + Vy_z*Vy_z);            
    localToWorld(1,0) = Vy_x*inverse_length;
    localToWorld(1,1) = Vy_y*inverse_length;
    localToWorld(1,2) = Vy_z*inverse_length;

    // Vz = (X,Y,Z)
    localToWorld(2,0) = X;
    localToWorld(2,1) = Y;
    localToWorld(2,2) = Z;
}