LibrePilot/ground/openpilotgcs/src/plugins/pfdqml/osgearth.cpp

/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
 * for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

#include "osgearth.h"

#include <QtCore/qfileinfo.h>
#include <QtDeclarative/qdeclarative.h>
#include <QtDeclarative/qdeclarativeview.h>
#include <QtDeclarative/qdeclarativeengine.h>
#include <QtGui/qpainter.h>
#include <QtGui/qvector3d.h>
#include <QtOpenGL/qglframebufferobject.h>

#include <osg/MatrixTransform>
#include <osg/AutoTransform>
#include <osg/Camera>
#include <osg/TexMat>
#include <osg/TextureRectangle>
#include <osg/Texture2D>
#include <osgViewer/ViewerEventHandlers>
#include <osgDB/ReadFile>
#include <osgEarthUtil/EarthManipulator>
#include <osgEarthUtil/ObjectPlacer>
#include <osgEarth/Map>

#include <QtCore/qtimer.h>

OsgEarthItem::OsgEarthItem(QDeclarativeItem *parent):
    QDeclarativeItem(parent),
    m_fbo(0),
    m_currentSize(640, 480),
    m_roll(0.0),
    m_pitch(0.0),
    m_yaw(0.0),
    m_latitude(-28.5),
    m_longitude(153.0),
    m_altitude(400.0),
    m_fieldOfView(90.0),
    m_sceneFile(QLatin1String("/usr/share/osgearth/maps/srtm.earth")),
    m_cameraDirty(false)
{
    setSize(m_currentSize);
    setFlag(ItemHasNoContents, false);
}

OsgEarthItem::~OsgEarthItem()
{
}

void OsgEarthItem::geometryChanged(const QRectF &newGeometry, const QRectF &oldGeometry)
{
    Q_UNUSED(oldGeometry);
    Q_UNUSED(newGeometry);

    //Dynamic gyometry changes are not supported yet,
    //terrain is rendered to fixed geompetry and scalled for now

    /*
    qDebug() << Q_FUNC_INFO << newGeometry;

    int w = qRound(newGeometry.width());
    int h = qRound(newGeometry.height());

    if (m_currentSize != QSize(w,h) && m_gw.get()) {
        m_currentSize = QSize(w,h);

        m_gw->getEventQueue()->windowResize(0,0,w,h);
        m_gw->resized(0,0,w,h);

        osg::Camera *camera = m_viewer->getCamera();
        camera->setViewport(new osg::Viewport(0,0,w,h));
        camera->setProjectionMatrixAsPerspective(m_fieldOfView, qreal(w)/h, 1.0f, 10000.0f);
    }
    */
}

void OsgEarthItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *style, QWidget *widget)
{
    Q_UNUSED(painter);
    Q_UNUSED(style);
    QGLWidget *glWidget = qobject_cast<QGLWidget*>(widget);

    if (!m_glWidget) {
        //make a shared gl widget to avoid
        //osg rendering to mess with qpainter state
        m_glWidget = new QGLWidget(widget, glWidget);
        m_glWidget.data()->setAttribute(Qt::WA_PaintOutsidePaintEvent);
    }

    if (!m_viewer.get())
        QMetaObject::invokeMethod(this, "initScene", Qt::QueuedConnection);

    if (glWidget && m_fbo)
        glWidget->drawTexture(boundingRect(), m_fbo->texture());
}

void OsgEarthItem::markCameraDirty()
{
    m_cameraDirty = true;
    QMetaObject::invokeMethod(this, "updateFBO", Qt::QueuedConnection);
}

void OsgEarthItem::updateFBO()
{
    if (!m_cameraDirty || !m_viewer.get() || m_glWidget.isNull())
        return;

    m_cameraDirty = false;
    m_glWidget.data()->makeCurrent();

    if (m_fbo && m_fbo->size() != m_currentSize) {
        delete m_fbo;
        m_fbo = 0;
    }

    if (!m_fbo) {
        m_fbo = new QGLFramebufferObject(m_currentSize, QGLFramebufferObject::CombinedDepthStencil);
        QPainter p(m_fbo);
        p.fillRect(0,0,m_currentSize.width(), m_currentSize.height(), Qt::gray);
    }

    //To find a camera view matrix, find placer matrixes for two points
    //onr at requested coords and another latitude shifted by 0.01 deg
    osgEarth::Util::ObjectPlacer placer(m_viewer->getSceneData());

    osg::Matrixd positionMatrix;
    placer.createPlacerMatrix(m_latitude, m_longitude, m_altitude, positionMatrix);
    osg::Matrixd positionMatrix2;
    placer.createPlacerMatrix(m_latitude+0.01, m_longitude, m_altitude, positionMatrix2);

    osg::Vec3d eye(0.0f, 0.0f, 0.0f);
    osg::Vec3d viewVector(0.0f, 0.0f, 0.0f);
    osg::Vec3d upVector(0.0f, 0.0f, 1.0f);

    eye = positionMatrix.preMult(eye);
    upVector = positionMatrix.preMult(upVector);
    upVector.normalize();
    viewVector = positionMatrix2.preMult(viewVector) - eye;
    viewVector.normalize();
    viewVector *= 10.0;

    //TODO: clarify the correct rotation order,
    //currently assuming yaw, pitch, roll
    osg::Quat q;
    q.makeRotate(-m_yaw*M_PI/180.0, upVector);
    upVector = q * upVector;
    viewVector = q * viewVector;

    osg::Vec3d side = viewVector ^ upVector;
    q.makeRotate(m_pitch*M_PI/180.0, side);
    upVector = q * upVector;
    viewVector = q * viewVector;

    q.makeRotate(m_roll*M_PI/180.0, viewVector);
    upVector = q * upVector;
    viewVector = q * viewVector;

    osg::Vec3d center = eye + viewVector;

//    qDebug() << "e " << eye.x() << eye.y() << eye.z();
//    qDebug() << "c " << center.x() << center.y() << center.z();
//    qDebug() << "up" << upVector.x() << upVector.y() << upVector.z();

    m_viewer->getCamera()->setViewMatrixAsLookAt(osg::Vec3d(eye.x(), eye.y(), eye.z()),
                                                 osg::Vec3d(center.x(), center.y(), center.z()),
                                                 osg::Vec3d(upVector.x(), upVector.y(), upVector.z()));

    {
        QPainter fboPainter(m_fbo);
        fboPainter.beginNativePainting();
        m_viewer->frame();
        fboPainter.endNativePainting();
    }
    m_glWidget.data()->doneCurrent();

    update();
}

void OsgEarthItem::setRoll(qreal arg)
{
    if (!qFuzzyCompare(m_roll, arg)) {
        m_roll = arg;
        markCameraDirty();
        emit rollChanged(arg);
    }
}

void OsgEarthItem::setPitch(qreal arg)
{
    if (!qFuzzyCompare(m_pitch, arg)) {
        m_pitch = arg;
        markCameraDirty();
        emit pitchChanged(arg);
    }
}

void OsgEarthItem::setYaw(qreal arg)
{
    if (!qFuzzyCompare(m_yaw, arg)) {
        m_yaw = arg;
        markCameraDirty();
        emit yawChanged(arg);
    }
}

void OsgEarthItem::setLatitude(double arg)
{
    //not sure qFuzzyCompare is accurate enough for geo coordinates
    if (m_latitude != arg) {
        m_latitude = arg;
        emit latitudeChanged(arg);
    }
}

void OsgEarthItem::setLongitude(double arg)
{
    if (m_longitude != arg) {
        m_longitude = arg;
        emit longitudeChanged(arg);
    }
}

void OsgEarthItem::setAltitude(double arg)
{
    if (!qFuzzyCompare(m_altitude,arg)) {
        m_altitude = arg;
        emit altitudeChanged(arg);
    }
}

//! Camera vertical field of view in degrees
void OsgEarthItem::setFieldOfView(qreal arg)
{
    if (!qFuzzyCompare(m_fieldOfView,arg)) {
        m_fieldOfView = arg;
        emit fieldOfViewChanged(arg);

        if (m_viewer.get()) {
            m_viewer->getCamera()->setProjectionMatrixAsPerspective(
                        m_fieldOfView,
                        qreal(m_currentSize.width())/m_currentSize.height(),
                        1.0f, 10000.0f);
        }

        markCameraDirty();
    }
}

void OsgEarthItem::setSceneFile(QString arg)
{
    if (m_sceneFile != arg) {
        m_sceneFile = arg;
        emit sceneFileChanged(arg);
    }
}


void OsgEarthItem::initScene()
{
    if (m_viewer.get())
        return;

    int w = m_currentSize.width();
    int h = m_currentSize.height();

    QString sceneFile = m_sceneFile;

    //try to resolve the relative scene file name:
    if (!QFileInfo(sceneFile).exists()) {
        QDeclarativeView *view = qobject_cast<QDeclarativeView*>(scene()->views().first());

        if (view) {
            QUrl baseUrl = view->engine()->baseUrl();
            sceneFile = baseUrl.resolved(sceneFile).toLocalFile();
        }
    }

    m_model = osgDB::readNodeFile(sceneFile.toStdString());

    m_gw = new osgViewer::GraphicsWindowEmbedded(0,0,w,h);

    m_viewer = new osgViewer::Viewer();
    m_viewer->setThreadingModel(osgViewer::Viewer::SingleThreaded);
    m_viewer->setSceneData(m_model);
    m_viewer->getDatabasePager()->setDoPreCompile(true);

    osg::Camera *camera = m_viewer->getCamera();
    camera->setViewport(new osg::Viewport(0,0,w,h));
    camera->setGraphicsContext(m_gw);
    camera->setClearMask(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);

    // configure the near/far so we don't clip things that are up close
    camera->setNearFarRatio(0.00002);
    camera->setProjectionMatrixAsPerspective(m_fieldOfView, qreal(w)/h, 1.0f, 10000.0f);

    markCameraDirty();
}