Commit c7dfc6e8 authored by pixhawk's avatar pixhawk

Merge branch 'dev' of pixhawk.ethz.ch:qgroundcontrol into dev

parents 6775f8f4 28f623fa
<map name="Map" type="projected" version="2">
<!--Add a high resolution inset of Zurich-->
<image name="zurich" driver="tms">
<url>../map/zurich/tms.xml</url>
</image>
</map>
......@@ -220,14 +220,10 @@ HEADERS += src/MG.h \
src/ui/RadioCalibration/AbstractCalibrator.h \
src/ui/map3D/Q3DWidget.h \
src/ui/map3D/PixhawkCheetahGeode.h \
src/ui/map3D/Texture.h \
src/ui/map3D/TextureCache.h \
src/ui/map3D/WebImage.h \
src/ui/map3D/WebImageCache.h \
src/ui/map3D/Imagery.h \
src/comm/QGCMAVLink.h \
src/ui/map3D/Pixhawk3DWidget.h \
src/ui/map3D/Q3DWidgetFactory.h
src/ui/map3D/Q3DWidgetFactory.h \
src/ui/map3D/GCManipulator.h
SOURCES += src/main.cc \
src/Core.cc \
src/uas/UASManager.cc \
......@@ -300,13 +296,9 @@ SOURCES += src/main.cc \
src/ui/WaypointGlobalView.cc \
src/ui/map3D/Q3DWidget.cc \
src/ui/map3D/PixhawkCheetahGeode.cc \
src/ui/map3D/Texture.cc \
src/ui/map3D/TextureCache.cc \
src/ui/map3D/WebImageCache.cc \
src/ui/map3D/WebImage.cc \
src/ui/map3D/Imagery.cc \
src/ui/map3D/Pixhawk3DWidget.cc \
src/ui/map3D/Q3DWidgetFactory.cc
src/ui/map3D/Q3DWidgetFactory.cc \
src/ui/map3D/GCManipulator.cc
RESOURCES = mavground.qrc
# Include RT-LAB Library
......
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL 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.
QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Definition of the class GCManipulator.
*
* @author Lionel Heng <hengli@student.ethz.ch>
*
*/
#include "GCManipulator.h"
GCManipulator::GCManipulator()
{
_moveSensitivity = 0.05f;
_zoomSensitivity = 1.0f;
_minZoomRange = 2.0f;
}
void
GCManipulator::setMinZoomRange(float minZoomRange)
{
_minZoomRange = minZoomRange;
}
void
GCManipulator::move(float dx, float dy, float dz)
{
_center += osg::Vec3(dx, dy, dz);
}
bool
GCManipulator::handle(const osgGA::GUIEventAdapter& ea,
osgGA::GUIActionAdapter& us)
{
using namespace osgGA;
switch (ea.getEventType())
{
case GUIEventAdapter::PUSH:
{
flushMouseEventStack();
addMouseEvent(ea);
if (calcMovement())
{
us.requestRedraw();
}
us.requestContinuousUpdate(false);
_thrown = false;
return true;
}
case GUIEventAdapter::RELEASE:
{
if (ea.getButtonMask() == 0)
{
if (isMouseMoving())
{
if (calcMovement())
{
us.requestRedraw();
us.requestContinuousUpdate(true);
_thrown = true;
}
}
else
{
flushMouseEventStack();
addMouseEvent(ea);
if (calcMovement())
{
us.requestRedraw();
}
us.requestContinuousUpdate(false);
_thrown = false;
}
}
else
{
flushMouseEventStack();
addMouseEvent(ea);
if (calcMovement())
{
us.requestRedraw();
}
us.requestContinuousUpdate(false);
_thrown = false;
}
return true;
}
case GUIEventAdapter::DRAG:
{
addMouseEvent(ea);
if (calcMovement())
{
us.requestRedraw();
}
us.requestContinuousUpdate(false);
_thrown = false;
return true;
}
case GUIEventAdapter::SCROLL:
{
// zoom model
float scale = 1.0f;
if (ea.getScrollingMotion() == GUIEventAdapter::SCROLL_UP)
{
scale -= _zoomSensitivity * 0.1f;
}
else
{
scale += _zoomSensitivity * 0.1f;
}
if (_distance * scale > _minZoomRange)
{
_distance *= scale;
}
return true;
}
case GUIEventAdapter::KEYDOWN:
// pan model
switch (ea.getKey())
{
case GUIEventAdapter::KEY_Space:
{
flushMouseEventStack();
_thrown = false;
home(ea,us);
us.requestRedraw();
us.requestContinuousUpdate(false);
return true;
}
case GUIEventAdapter::KEY_Left:
{
float scale = -_moveSensitivity * _distance;
osg::Matrix rotation_matrix;
rotation_matrix.makeRotate(_rotation);
osg::Vec3 dv(scale, 0.0f, 0.0f);
_center += dv * rotation_matrix;
return true;
}
case GUIEventAdapter::KEY_Right:
{
float scale = _moveSensitivity * _distance;
osg::Matrix rotation_matrix;
rotation_matrix.makeRotate(_rotation);
osg::Vec3 dv(scale, 0.0f, 0.0f);
_center += dv * rotation_matrix;
return true;
}
case GUIEventAdapter::KEY_Up:
{
float scale = _moveSensitivity * _distance;
osg::Matrix rotation_matrix;
rotation_matrix.makeRotate(_rotation);
osg::Vec3 dv(0.0f, scale, 0.0f);
_center += dv * rotation_matrix;
return true;
}
case GUIEventAdapter::KEY_Down:
{
float scale = -_moveSensitivity * _distance;
osg::Matrix rotation_matrix;
rotation_matrix.makeRotate(_rotation);
osg::Vec3 dv(0.0f, scale, 0.0f);
_center += dv * rotation_matrix;
return true;
}
return false;
}
case GUIEventAdapter::FRAME:
if (_thrown)
{
if (calcMovement())
{
us.requestRedraw();
}
}
return false;
default:
return false;
}
}
bool
GCManipulator::calcMovement()
{
using namespace osgGA;
// return if less then two events have been added.
if (_ga_t0.get() == NULL || _ga_t1.get() == NULL)
{
return false;
}
float dx = _ga_t0->getXnormalized() - _ga_t1->getXnormalized();
float dy = _ga_t0->getYnormalized() - _ga_t1->getYnormalized();
// return if there is no movement.
if (dx == 0.0f && dy == 0.0f)
{
return false;
}
unsigned int buttonMask = _ga_t1->getButtonMask();
if (buttonMask == GUIEventAdapter::LEFT_MOUSE_BUTTON)
{
// rotate camera
osg::Vec3 axis;
float angle;
float px0 = _ga_t0->getXnormalized();
float py0 = _ga_t0->getYnormalized();
float px1 = _ga_t1->getXnormalized();
float py1 = _ga_t1->getYnormalized();
trackball(axis, angle, px1, py1, px0, py0);
osg::Quat new_rotate;
new_rotate.makeRotate(angle, axis);
_rotation = _rotation * new_rotate;
return true;
}
else if (buttonMask == GUIEventAdapter::MIDDLE_MOUSE_BUTTON ||
buttonMask == (GUIEventAdapter::LEFT_MOUSE_BUTTON |
GUIEventAdapter::RIGHT_MOUSE_BUTTON))
{
// pan model
float scale = -_moveSensitivity * _distance;
osg::Matrix rotation_matrix;
rotation_matrix.makeRotate(_rotation);
osg::Vec3 dv(dx * scale, dy * scale, 0.0f);
_center += dv * rotation_matrix;
return true;
}
else if (buttonMask == GUIEventAdapter::RIGHT_MOUSE_BUTTON)
{
// zoom model
float scale = 1.0f + dy * _zoomSensitivity;
if (_distance * scale > _minZoomRange)
{
_distance *= scale;
}
return true;
}
return false;
}
......@@ -23,39 +23,39 @@ This file is part of the QGROUNDCONTROL project
/**
* @file
* @brief Definition of the class TextureCache.
* @brief Definition of the class GCManipulator.
*
* @author Lionel Heng <hengli@student.ethz.ch>
*
*/
#ifndef TEXTURECACHE_H
#define TEXTURECACHE_H
#ifndef GCMANIPULATOR_H
#define GCMANIPULATOR_H
#include <QVector>
#include <osgGA/TrackballManipulator>
#include "Texture.h"
#include "WebImageCache.h"
class TextureCache
class GCManipulator : public osgGA::TrackballManipulator
{
public:
explicit TextureCache(uint32_t cacheSize);
TexturePtr get(const QString& tileURL, bool useHeightModel = false);
GCManipulator();
void sync(void);
void setMinZoomRange(float minZoomRange);
private:
QPair<TexturePtr, int32_t> lookup(const QString& tileURL,
bool useHeightModel);
virtual void move(float dx, float dy, float dz);
bool requireSync(void) const;
/**
* @brief Handle events.
* @return True if event is handled; false otherwise.
*/
virtual bool handle(const osgGA::GUIEventAdapter& ea,
osgGA::GUIActionAdapter& us);
uint32_t cacheSize;
QVector<TexturePtr> textures;
protected:
bool calcMovement();
QScopedPointer<WebImageCache> imageCache;
float _moveSensitivity;
float _zoomSensitivity;
float _minZoomRange;
};
#endif // TEXTURECACHE_H
#endif // GCMANIPULATOR_H
This diff is collapsed.
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL 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.
QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Definition of the class Imagery.
*
* @author Lionel Heng <hengli@student.ethz.ch>
*
*/
#ifndef IMAGERY_H
#define IMAGERY_H
#include <inttypes.h>
#include "TextureCache.h"
class Imagery
{
public:
Imagery();
enum ImageryType
{
GOOGLE_MAP = 0,
GOOGLE_SATELLITE = 1,
SWISSTOPO_SATELLITE = 2,
SWISSTOPO_SATELLITE_3D = 3
};
void setImageryType(ImageryType type);
void setOffset(double xOffset, double yOffset);
void prefetch2D(double windowWidth, double windowHeight,
double zoom, double xOrigin, double yOrigin,
double viewXOffset, double viewYOffset,
const QString& utmZone);
void draw2D(double windowWidth, double windowHeight,
double zoom, double xOrigin, double yOrigin,
double viewXOffset, double viewYOffset,
const QString& utmZone);
void prefetch3D(double radius, double tileResolution,
double xOrigin, double yOrigin,
double viewXOffset, double viewYOffset,
const QString& utmZone, bool useHeightModel);
void draw3D(double radius, double tileResolution,
double xOrigin, double yOrigin,
double viewXOffset, double viewYOffset,
const QString& utmZone, bool useHeightModel);
bool update(void);
private:
void imageBounds(int32_t tileX, int32_t tileY, double tileResolution,
double& x1, double& y1, double& x2, double& y2,
double& x3, double& y3, double& x4, double& y4) const;
void tileBounds(double tileResolution,
double minUtmX, double minUtmY,
double maxUtmX, double maxUtmY, const QString& utmZone,
int32_t& minTileX, int32_t& minTileY,
int32_t& maxTileX, int32_t& maxTileY,
int32_t& zoomLevel) const;
double tileXToLongitude(int32_t tileX, int32_t numTiles) const;
double tileYToLatitude(int32_t tileY, int32_t numTiles) const;
int32_t longitudeToTileX(double longitude, int32_t numTiles) const;
int32_t latitudeToTileY(double latitude, int32_t numTiles) const;
void UTMtoTile(double northing, double easting, const QString& utmZone,
double tileResolution, int32_t& tileX, int32_t& tileY,
int32_t& zoomLevel) const;
QChar UTMLetterDesignator(double latitude) const;
void LLtoUTM(double latitude, double longitude,
double& utmNorthing, double& utmEasting,
QString& utmZone) const;
void UTMtoLL(double utmNorthing, double utmEasting, const QString& utmZone,
double& latitude, double& longitude) const;
QString getTileLocation(int32_t tileX, int32_t tileY, int32_t zoomLevel,
double tileResolution) const;
ImageryType currentImageryType;
QScopedPointer<TextureCache> textureCache;
double xOffset, yOffset;
};
#endif // IMAGERY_H
This diff is collapsed.
......@@ -52,17 +52,6 @@ public:
void buildLayout(void);
static void display(void* clientData);
void displayHandler(void);
static void mouse(Qt::MouseButton button, MouseState state,
int32_t x, int32_t y, void* clientData);
void mouseHandler(Qt::MouseButton button, MouseState state,
int32_t x, int32_t y);
static void timer(void* clientData);
void timerHandler(void);
double getTime(void) const;
public slots:
......@@ -72,17 +61,20 @@ private slots:
void showGrid(int state);
void showTrail(int state);
void showWaypoints(int state);
void recenterCamera(void);
void toggleLockCamera(int state);
void recenter(void);
void toggleFollowCamera(int state);
protected:
virtual void display(void);
virtual void mousePressEvent(QMouseEvent* event);
UASInterface* uas;
private:
osg::ref_ptr<osg::Geode> createGrid(void);
osg::ref_ptr<osg::Geode> createTrail(void);
osg::ref_ptr<osgEarth::MapNode> createMap(void);
osg::ref_ptr<osg::Group> createTarget(void);
osg::ref_ptr<osg::Node> createTarget(void);
osg::ref_ptr<osg::Group> createWaypoints(void);
void setupHUD(void);
......@@ -90,25 +82,21 @@ private:
void updateHUD(float robotX, float robotY, float robotZ,
float robotRoll, float robotPitch, float robotYaw);
void updateTrail(float robotX, float robotY, float robotZ);
void updateTarget(float robotX, float robotY, float robotZ);
void updateTarget(void);
void updateWaypoints(void);
void markTarget(void);
double lastRedrawTime;
bool displayGrid;
bool displayTrail;
bool displayTarget;
bool displayWaypoints;
bool lockCamera;
bool followCamera;
osg::ref_ptr<osg::Vec3Array> trailVertices;
QVarLengthArray<osg::Vec3, 10000> trail;
osg::Vec3 targetPosition;
osg::ref_ptr<osg::Geometry> hudBackgroundGeometry;
osg::ref_ptr<osgText::Text> statusText;
osg::ref_ptr<osg::Geode> gridNode;
......@@ -116,10 +104,13 @@ private:
osg::ref_ptr<osg::Geometry> trailGeometry;
osg::ref_ptr<osg::DrawArrays> trailDrawArrays;
osg::ref_ptr<osgEarth::MapNode> mapNode;
osg::ref_ptr<osg::Group> targetNode;
osg::ref_ptr<osg::Geode> targetNode;
osg::ref_ptr<osg::PositionAttitudeTransform> targetPosition;
osg::ref_ptr<osg::Group> waypointsNode;
QPushButton* targetButton;
float lastRobotX, lastRobotY, lastRobotZ;
};
#endif // PIXHAWK3DWIDGET_H
......@@ -23,7 +23,8 @@ This file is part of the QGROUNDCONTROL project
/**
* @file
* @brief Generates a geode which renders a Pixhawk Cheetah MAV.
* @brief Generates a OpenSceneGraph geode which renders a
* Pixhawk Cheetah MAV.
*
* @author Lionel Heng <hengli@student.ethz.ch>
*
......@@ -48,7 +49,7 @@ static sample_MATERIAL materials [1] = {
{{0.117647f,0.117647f,0.117647f}, {0.596078f,0.666667f,0.686275f}, {0.301176f,0.301176f,0.301176f}, {0.0f,0.0f,0.0f}, 1.0f,8.0f,-1} //Material #1
};
// 38747 Verticies
// 38747 Vertices
// 0 Texture Coordinates
// 22155 Normals
// 77848 Triangles
......@@ -59372,9 +59373,9 @@ PixhawkCheetahGeode::create(float red, float green, float blue)
for (int j = 0; j < 3; ++j)
{
int vi = face_indicies[i][j];
int ni = face_indicies[i][j + 3]; //Normal index
int ti = face_indicies[i][j + 6]; //Texture index
int vi = face_indicies[i][j]; // Vertice index
int ni = face_indicies[i][j + 3]; // Normal index
int ti = face_indicies[i][j + 6]; // Texture index
osgVertices->push_back(osg::Vec3(vertices[vi][0],
vertices[vi][1],
......@@ -23,7 +23,7 @@ This file is part of the QGROUNDCONTROL project
/**
* @file
* @brief Generates a geode which renders a Pixhawk Cheetah MAV.
* @brief Definition of the class PixhawkCheetahGeode.
*
* @author Lionel Heng <hengli@student.ethz.ch>
*
......@@ -35,22 +35,32 @@ This file is part of the QGROUNDCONTROL project
#include <osg/Geode>
/**
* @brief Creates a geode which renders a Pixhawk Cheetah MAV.
* @param red Red intensity of the MAV model
* @param green Green intensity of the MAV model
* @param blue Blue intensity of the MAV model
*
* @return The pointer to the geode.
* @brief The PixhawkCheetahGeode class.
* Generates an OpenSceneGraph geode which renders a Pixhawk Cheetah MAV.
**/
class PixhawkCheetahGeode
{
public:
/**
* Constructor.
*/
PixhawkCheetahGeode();
/**
* Get a single instance of the geode.
*/
static osg::ref_ptr<osg::Geode> instance(void);
private:
/**
* @brief Creates an OpenSceneGraph geode which renders a Pixhawk Cheetah MAV.
* @param red Red intensity of the MAV model.
* @param green Green intensity of the MAV model.
* @param blue Blue intensity of the MAV model.
*
* @return A smart pointer to the geode.
**/
static osg::ref_ptr<osg::Geode> create(float red, float green, float blue);
static osg::ref_ptr<osg::Geode> _instance;
......
......@@ -34,7 +34,6 @@ This file is part of the QGROUNDCONTROL project
#include <osg/Geometry>
#include <osg/LineWidth>
#include <osg/MatrixTransform>
#include <osgGA/NodeTrackerManipulator>
static const float KEY_ROTATE_AMOUNT = 5.0f;
static const float KEY_MOVE_AMOUNT = 10.0f;
......@@ -50,22 +49,9 @@ Q3DWidget::Q3DWidget(QWidget* parent)
, robotAttitude(new osg::PositionAttitudeTransform())
, hudGeode(new osg::Geode())
, hudProjectionMatrix(new osg::Projection)
, userDisplayFunc(NULL)
, userKeyboardFunc(NULL)
, userMouseFunc(NULL)
, userMotionFunc(NULL)
, userTimerFunc(NULL)
, userDisplayFuncData(NULL)
, userKeyboardFuncData(NULL)
, userMouseFuncData(NULL)
, userMotionFuncData(NULL)
, userTimerFuncData(NULL)
, lastMouseX(0)
, lastMouseY(0)
, _forceRedraw(false)
{
// set initial camera parameters
cameraParams.minZoomRange = 0.5f;
cameraParams.minZoomRange = 2.0f;
cameraParams.cameraFov = 30.0f;
cameraParams.minClipRange = 1.0f;
cameraParams.maxClipRange = 10000.0f;
......@@ -85,11 +71,14 @@ Q3DWidget::~Q3DWidget()
void
Q3DWidget::init(float fps)
{
getCamera()->setGraphicsContext(getGraphicsWindow());
getCamera()->setGraphicsContext(osgGW);
setLightingMode(osg::View::SKY_LIGHT);
// set up coordinate systems
// set up various maps
// allocentric - world map
// rolling - map aligned to the world axes and centered on the robot
// egocentric - vehicle-centric map
root->addChild(allocentricMap);
allocentricMap->addChild(robotPosition);
robotPosition->addChild(rollingMap);
......@@ -101,12 +90,14 @@ Q3DWidget::init(float fps)
// set up HUD
root->addChild(createHUD());
// set up robot
egocentricMap->addChild(createRobot());
osg::ref_ptr<osgGA::NodeTrackerManipulator> nodeTrackerManipulator(
new osgGA::NodeTrackerManipulator);
nodeTrackerManipulator->setTrackNode(rollingMap);
setCameraManipulator(nodeTrackerManipulator);
// set up camera control
cameraManipulator = new GCManipulator;
setCameraManipulator(cameraManipulator);
cameraManipulator->setMinZoomRange(cameraParams.minZoomRange);
cameraManipulator->setDistance(cameraParams.minZoomRange * 2.0);
connect(&timer, SIGNAL(timeout()), this, SLOT(redraw()));
timer.start(static_cast<int>(floorf(1000.0f / fps)));
......@@ -187,20 +178,18 @@ Q3DWidget::setCameraParams(float minZoomRange, float cameraFov,
cameraParams.cameraFov = cameraFov;
cameraParams.minClipRange = minClipRange;
cameraParams.maxClipRange = maxClipRange;
_forceRedraw = true;
}
void
Q3DWidget::forceRedraw(void)
Q3DWidget::moveCamera(float dx, float dy, float dz)
{
_forceRedraw = true;
cameraManipulator->move(dx, dy, dz);
}
void
Q3DWidget::recenter(void)
Q3DWidget::recenterCamera(float x, float y, float z)
{
cameraManipulator->setCenter(osg::Vec3d(x, y, z));
}
void
......@@ -214,66 +203,20 @@ Q3DWidget::setDisplayMode3D(void)
aspect,
cameraParams.minClipRange,
cameraParams.maxClipRange);
// getCamera()->setViewMatrixAsLookAt(osg::Vec3d(cameraX + cameraPose.xOffset,
// cameraY + cameraPose.yOffset,
// cameraZ + cameraPose.zOffset),
// osg::Vec3d(cameraPose.xOffset,
// cameraPose.yOffset,
// cameraPose.zOffset),
// osg::Vec3d(0.0, 0.0, 1.0));
}
void
Q3DWidget::setDisplayFunc(DisplayFunc func, void* clientData)
{
userDisplayFunc = func;
userDisplayFuncData = clientData;
}
void
Q3DWidget::setKeyboardFunc(KeyboardFunc func, void* clientData)
{
userKeyboardFunc = func;
userKeyboardFuncData = clientData;
}
void
Q3DWidget::setMouseFunc(MouseFunc func, void* clientData)
{
userMouseFunc = func;
userMouseFuncData = clientData;
}
void
Q3DWidget::setMotionFunc(MotionFunc func, void* clientData)
{
userMotionFunc = func;
userMotionFuncData = clientData;
}
void
Q3DWidget::addTimerFunc(uint32_t msecs, void(*func)(void *),
void* clientData)
{
userTimerFunc = func;
userTimerFuncData = clientData;
QTimer::singleShot(msecs, this, SLOT(userTimer()));
}
std::pair<double,double>
Q3DWidget::getGlobalCursorPosition(int32_t mouseX, int32_t mouseY,
double z)
Q3DWidget::getGlobalCursorPosition(int32_t cursorX, int32_t cursorY, double z)
{
osgUtil::LineSegmentIntersector::Intersections intersections;
// normalize cursor position to value between -1 and 1
double x = -1.0f + static_cast<double>(2 * mouseX)
double x = -1.0f + static_cast<double>(2 * cursorX)
/ static_cast<double>(width());
double y = -1.0f + static_cast<double>(2 * (height() - mouseY))
double y = -1.0f + static_cast<double>(2 * (height() - cursorY))
/ static_cast<double>(height());
// compute matrix which transforms screen coordinates to world coordinates
osg::Matrixd m = getCamera()->getViewMatrix()
* getCamera()->getProjectionMatrix();
osg::Matrixd invM = osg::Matrixd::inverse(m);
......@@ -295,20 +238,7 @@ Q3DWidget::getGlobalCursorPosition(int32_t mouseX, int32_t mouseY,
void
Q3DWidget::redraw(void)
{
if (_forceRedraw)
{
updateGL();
_forceRedraw = false;
}
}
void
Q3DWidget::userTimer(void)
{
if (userTimerFunc)
{
userTimerFunc(userTimerFuncData);
}
updateGL();
}
int
......@@ -323,31 +253,8 @@ Q3DWidget::getMouseY(void)
return mapFromGlobal(cursor().pos()).y();
}
int
Q3DWidget::getLastMouseX(void)
{
return lastMouseX;
}
int
Q3DWidget::getLastMouseY(void)
{
return lastMouseY;
}
osgViewer::GraphicsWindow*
Q3DWidget::getGraphicsWindow(void)
{
return osgGW.get();
}
const osgViewer::GraphicsWindow*
Q3DWidget::getGraphicsWindow(void) const
{
return osgGW.get();
}
void Q3DWidget::resizeGL(int width, int height)
void
Q3DWidget::resizeGL(int width, int height)
{
hudProjectionMatrix->setMatrix(osg::Matrix::ortho2D(0, width,
0, height));
......@@ -356,28 +263,34 @@ void Q3DWidget::resizeGL(int width, int height)
osgGW->resized(0 , 0, width, height);
}
void Q3DWidget::paintGL(void)
void
Q3DWidget::paintGL(void)
{
setDisplayMode3D();
getCamera()->setClearColor(osg::Vec4f(0.0f, 0.0f, 0.0f, 0.0f));
getCamera()->setClearMask(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
if (userDisplayFunc)
{
userDisplayFunc(userDisplayFuncData);
}
display();
frame();
}
void Q3DWidget::keyPressEvent(QKeyEvent* event)
void
Q3DWidget::display(void)
{
}
void
Q3DWidget::keyPressEvent(QKeyEvent* event)
{
QWidget::keyPressEvent(event);
if (event->isAccepted())
{
return;
}
if (event->text().isEmpty())
{
osgGW->getEventQueue()->keyPress(convertKey(event->key()));
......@@ -388,24 +301,10 @@ void Q3DWidget::keyPressEvent(QKeyEvent* event)
static_cast<osgGA::GUIEventAdapter::KeySymbol>(
*(event->text().toAscii().data())));
}
if (userKeyboardFunc)
{
if (event->text().isEmpty())
{
userKeyboardFunc(0, userKeyboardFuncData);
}
else
{
userKeyboardFunc(event->text().at(0).toAscii(),
userKeyboardFuncData);
}
}
forceRedraw();
}
void Q3DWidget::keyReleaseEvent(QKeyEvent* event)
void
Q3DWidget::keyReleaseEvent(QKeyEvent* event)
{
QWidget::keyReleaseEvent(event);
if (event->isAccepted())
......@@ -422,11 +321,10 @@ void Q3DWidget::keyReleaseEvent(QKeyEvent* event)
static_cast<osgGA::GUIEventAdapter::KeySymbol>(
*(event->text().toAscii().data())));
}
forceRedraw();
}
void Q3DWidget::mousePressEvent(QMouseEvent* event)
void
Q3DWidget::mousePressEvent(QMouseEvent* event)
{
int button = 0;
switch (event->button())
......@@ -447,17 +345,10 @@ void Q3DWidget::mousePressEvent(QMouseEvent* event)
{}
}
osgGW->getEventQueue()->mouseButtonPress(event->x(), event->y(), button);
if (userMouseFunc)
{
userMouseFunc(event->button(), MOUSE_STATE_DOWN, event->x(), event->y(),
userMouseFuncData);
}
forceRedraw();
}
void Q3DWidget::mouseReleaseEvent(QMouseEvent* event)
void
Q3DWidget::mouseReleaseEvent(QMouseEvent* event)
{
int button = 0;
switch (event->button())
......@@ -478,35 +369,20 @@ void Q3DWidget::mouseReleaseEvent(QMouseEvent* event)
{}
}
osgGW->getEventQueue()->mouseButtonRelease(event->x(), event->y(), button);
if (userMouseFunc)
{
userMouseFunc(event->button(), MOUSE_STATE_UP, event->x(), event->y(),
userMouseFuncData);
}
forceRedraw();
}
void Q3DWidget::mouseMoveEvent(QMouseEvent* event)
void
Q3DWidget::mouseMoveEvent(QMouseEvent* event)
{
osgGW->getEventQueue()->mouseMotion(event->x(), event->y());
if (userMotionFunc)
{
userMotionFunc(event->x(), event->y(), userMotionFuncData);
}
forceRedraw();
}
void Q3DWidget::wheelEvent(QWheelEvent* event)
void
Q3DWidget::wheelEvent(QWheelEvent* event)
{
osgGW->getEventQueue()->mouseScroll((event->delta() > 0) ?
osgGA::GUIEventAdapter::SCROLL_UP :
osgGA::GUIEventAdapter::SCROLL_DOWN);
forceRedraw();
}
float
......
......@@ -36,104 +36,226 @@ This file is part of the QGROUNDCONTROL project
#include <osg/LineSegment>
#include <osg/PositionAttitudeTransform>
#include <osgGA/TrackballManipulator>
#include <osgViewer/Viewer>
enum MouseState
{
MOUSE_STATE_UP = 0,
MOUSE_STATE_DOWN = 1
};
typedef void (*DisplayFunc)(void *);
typedef void (*KeyboardFunc)(char, void *);
typedef void (*MouseFunc)(Qt::MouseButton, MouseState, int, int, void *);
typedef void (*MotionFunc)(int, int, void *);
#include "GCManipulator.h"
/**
* @brief Definition of the class Q3DWidget.
* The Q3DWidget widget uses the OpenSceneGraph framework to render
* user-defined objects in 3D. The world coordinate system in OSG is defined:
* - x-axis points to the east
* - y-axis points to the north
* - z-axis points upwards
*/
class Q3DWidget : public QGLWidget, public osgViewer::Viewer
{
Q_OBJECT
public:
/**
* Constructor.
* @param parent Parent widget.
*/
Q3DWidget(QWidget* parent = 0);
/**
* Destructor.
*/
virtual ~Q3DWidget();
/**
* @brief Initializes the widget.
* @param fps Frames per second.
*/
void init(float fps);
void setCameraParams(float minZoomRange, float cameraFov,
float minClipRange,
float maxClipRange);
void forceRedraw(void);
void recenter(void);
/**
* @brief Sets the camera parameters.
* @param minZoomRange Minimum distance from the viewer to the camera.
* @param cameraFov Camera field of view.
* @param minClipRange Distance from the viewer to the near clipping range.
* @param maxClipRange Distance from the viewer to the far clipping range.
*/
void setCameraParams(float minZoomRange, float cameraFov,
float minClipRange, float maxClipRange);
/**
* @brief Moves the camera by [dx,dy,dz].
* @param dx Translation along the x-axis in meters.
* @param dy Translation along the y-axis in meters.
* @param dz Translation along the z-axis in meters.
*/
void moveCamera(float dx, float dy, float dz);
/**
* @brief Recenters the camera at (x,y,z).
*/
void recenterCamera(float x, float y, float z);
/**
* @brief Sets up 3D display mode.
*/
void setDisplayMode3D(void);
void setDisplayFunc(DisplayFunc func, void* clientData);
void setKeyboardFunc(KeyboardFunc func, void* clientData);
void setMouseFunc(MouseFunc func, void* clientData);
void setMotionFunc(MotionFunc func, void* clientData);
void addTimerFunc(uint msecs, void(*func)(void *),
void* clientData);
std::pair<double,double> getGlobalCursorPosition(int32_t mouseX,
int32_t mouseY,
/**
* @brief Gets the world 3D coordinates of the cursor.
* The function projects the 2D cursor position to a line in world space
* and returns the intersection of that line and the horizontal plane
* which contains the point (0, 0, z);
* @param cursorX x-coordinate of the cursor.
* @param cursorY y-coordinate of the cursor.
* @param z z-coordinate of the point in the plane.
* @return A pair of values containing the world 3D cursor coordinates.
*/
std::pair<double,double> getGlobalCursorPosition(int32_t cursorX,
int32_t cursorY,
double z);
protected slots:
/**
* @brief Updates the widget.
*/
void redraw(void);
void userTimer(void);
protected:
/**
* @brief Get base robot geode.
* @return Smart pointer to the geode.
*/
osg::ref_ptr<osg::Geode> createRobot(void);
/**
* @brief Get base HUD geode.
* @return Smart pointer to the geode.
*/
osg::ref_ptr<osg::Node> createHUD(void);
/**
* @brief Get screen x-coordinate of mouse cursor.
* @return screen x-coordinate of mouse cursor.
*/
int getMouseX(void);
int getMouseY(void);
int getLastMouseX(void);
int getLastMouseY(void);
osgViewer::GraphicsWindow* getGraphicsWindow(void);
const osgViewer::GraphicsWindow* getGraphicsWindow(void) const;
/**
* @brief Get screen y-coordinate of mouse cursor.
* @return screen y-coordinate of mouse cursor.
*/
int getMouseY(void);
/**
* @brief Handle widget resize event.
* @param width New width of widget.
* @param height New height of widget.
*/
virtual void resizeGL(int width, int height);
/**
* @brief Handle widget paint event.
*/
virtual void paintGL(void);
/**
* @brief This function is a container for user-defined rendering.
* All code to render objects should be in this function.
*/
virtual void display(void);
/**
* @brief Processes key press events.
* If this handler is reimplemented, it is very important that you call the
* base class implementation.
* @param event Key press event.
*/
virtual void keyPressEvent(QKeyEvent* event);
/**
* @brief Processes key release events.
* If this handler is reimplemented, it is very important that you call the
* base class implementation.
* @param event Key release event.
*/
virtual void keyReleaseEvent(QKeyEvent* event);
/**
* @brief Processes mouse press events.
* If this handler is reimplemented, it is very important that you call the
* base class implementation.
* @param event Mouse press event.
*/
virtual void mousePressEvent(QMouseEvent* event);
/**
* @brief Processes mouse release events.
* If this handler is reimplemented, it is very important that you call the
* base class implementation.
* @param event Mouse release event.
*/
virtual void mouseReleaseEvent(QMouseEvent* event);
/**
* @brief Processes mouse move events.
* If this handler is reimplemented, it is very important that you call the
* base class implementation.
* @param event Mouse move event.
*/
virtual void mouseMoveEvent(QMouseEvent* event);
/**
* @brief Processes mouse wheel events.
* If this handler is reimplemented, it is very important that you call the
* base class implementation.
* @param event Mouse wheel event.
*/
virtual void wheelEvent(QWheelEvent* event);
/**
* @brief Converts radians to degrees.
* @param angle Angle in radians.
* @return angle in degrees.
*/
float r2d(float angle);
/**
* @brief Converts degrees to radians.
* @param angle Angle in degrees.
* @return angle in radians.
*/
float d2r(float angle);
/**
* @brief Converts Qt-defined key to OSG-defined key.
* @param key Qt-defined key.
* @return OSG-defined key.
*/
osgGA::GUIEventAdapter::KeySymbol convertKey(int key) const;
/**
* @brief Computes intersection of line and plane.
* @param plane Vector which represents the plane.
* @param line Line representation.
* @return 3D point which lies at the intersection of the line and plane.
*/
bool getPlaneLineIntersection(const osg::Vec4d& plane,
const osg::LineSegment& line,
osg::Vec3d& isect);
osg::ref_ptr<osg::Group> root;
osg::ref_ptr<osg::Group> root; /**< Root node of scene graph. */
osg::ref_ptr<osg::Switch> allocentricMap;
osg::ref_ptr<osg::Switch> rollingMap;
osg::ref_ptr<osg::Switch> egocentricMap;
osg::ref_ptr<osg::PositionAttitudeTransform> robotPosition;
osg::ref_ptr<osg::PositionAttitudeTransform> robotAttitude;
osg::ref_ptr<osg::Geode> hudGeode;
osg::ref_ptr<osg::Projection> hudProjectionMatrix;
osg::ref_ptr<osg::Geode> hudGeode; /**< A geode which contains renderable HUD objects. */
osg::ref_ptr<osg::Projection> hudProjectionMatrix; /**< An orthographic projection matrix for HUD display. */
DisplayFunc userDisplayFunc;
KeyboardFunc userKeyboardFunc;
MouseFunc userMouseFunc;
MotionFunc userMotionFunc;
void (*userTimerFunc)(void *);
osg::ref_ptr<osgViewer::GraphicsWindowEmbedded> osgGW; /**< A class which manages OSG graphics windows and events. */
void* userDisplayFuncData;
void* userKeyboardFuncData;
void* userMouseFuncData;
void* userMotionFuncData;
void* userTimerFuncData;
osg::ref_ptr<GCManipulator> cameraManipulator; /**< Camera manipulator. */
osg::ref_ptr<osgViewer::GraphicsWindowEmbedded> osgGW;
QTimer timer;
QTimer timer; /**< Timer which draws graphics based on specified fps. */
struct CameraParams
{
......@@ -143,12 +265,7 @@ protected:
float maxClipRange;
};
CameraParams cameraParams;
int lastMouseX;
int lastMouseY;
bool _forceRedraw;
CameraParams cameraParams; /**< Struct representing camera parameters. */
};
#endif // Q3DWIDGET_H
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL 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.
QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Definition of the class Q3DWidgetFactory.
*
* @author Lionel Heng <hengli@student.ethz.ch>
*
*/
#ifndef Q3DWIDGETFACTORY_H
#define Q3DWIDGETFACTORY_H
......@@ -5,9 +36,20 @@
#include "Q3DWidget.h"
/**
* @brief A factory which creates project-specific 3D widgets without
* specifying the exact widget class.
*/
class Q3DWidgetFactory
{
public:
/**
* @brief Returns a Q3DWidget instance of the appropriate type.
* @param type String specifying the required type name.
* @return A smart pointer to the Q3DWidget instance.
*/
static QPointer<Q3DWidget> get(const std::string& type);
};
......
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL 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.
QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Definition of the class Texture.
*
* @author Lionel Heng <hengli@student.ethz.ch>
*
*/
#include "Texture.h"
Texture::Texture()
: _is3D(false)
{
}
const QString&
Texture::getSourceURL(void) const
{
return sourceURL;
}
void
Texture::setID(GLuint id)
{
this->id = id;
}
void
Texture::sync(const WebImagePtr& image)
{
state = static_cast<State>(image->getState());
if (image->getState() != WebImage::UNINITIALIZED &&
(sourceURL != image->getSourceURL() ||
_is3D != image->is3D()))
{
sourceURL = image->getSourceURL();
_is3D = image->is3D();
}
if (image->getState() == WebImage::READY && image->getSyncFlag())
{
image->setSyncFlag(false);
if (image->getWidth() != imageWidth ||
image->getHeight() != imageHeight)
{
imageWidth = image->getWidth();
textureWidth = 32;
while (textureWidth < imageWidth)
{
textureWidth *= 2;
}
imageHeight = image->getHeight();
textureHeight = 32;
while (textureHeight < imageHeight)
{
textureHeight *= 2;
}
maxU = static_cast<double>(imageWidth)
/ static_cast<double>(textureWidth);
maxV = static_cast<double>(imageHeight)
/ static_cast<double>(textureHeight);
glBindTexture(GL_TEXTURE_2D, id);
glTexImage2D(GL_TEXTURE_2D, 0, 3, textureWidth, textureHeight,
0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
}
glBindTexture(GL_TEXTURE_2D, id);
glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, imageWidth, imageHeight,
GL_RGBA, GL_UNSIGNED_BYTE, image->getImageData());
heightModel = image->getHeightModel();
}
}
void
Texture::draw(float x1, float y1, float x2, float y2,
bool smoothInterpolation) const
{
draw(x1, y1, x2, y1, x2, y2, x1, y2, smoothInterpolation);
}
void
Texture::draw(float x1, float y1, float x2, float y2,
float x3, float y3, float x4, float y4,
bool smoothInterpolation) const
{
if (state == REQUESTED)
{
glBegin(GL_LINE_LOOP);
glColor3f(0.0f, 0.0f, 1.0f);
glVertex2f(x1, y1);
glVertex2f(x2, y2);
glVertex2f(x3, y3);
glVertex2f(x4, y4);
glEnd();
return;
}
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D, id);
float dx, dy;
if (smoothInterpolation)
{
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
dx = 1.0f / (2.0f * textureWidth);
dy = 1.0f / (2.0f * textureHeight);
}
else
{
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
dx = 0.0f;
dy = 0.0f;
}
glColor3f(1.0f, 1.0f, 1.0f);
if (!_is3D)
{
glBegin(GL_QUADS);
glTexCoord2f(dx, maxV - dy);
glVertex3f(x1, y1, 0.0f);
glTexCoord2f(maxU - dx, maxV - dy);
glVertex3f(x2, y2, 0.0f);
glTexCoord2f(maxU - dx, dy);
glVertex3f(x3, y3, 0.0f);
glTexCoord2f(dx, dy);
glVertex3f(x4, y4, 0.0f);
glEnd();
}
else
{
float scaleX = 1.0f / static_cast<float>(heightModel.size() - 1);
for (int32_t i = 0; i < heightModel.size() - 1; ++i)
{
float scaleI = scaleX * static_cast<float>(i);
float scaleY =
1.0f / static_cast<float>(heightModel[i].size() - 1);
float x1i = x1 + scaleI * (x4 - x1);
float x1f = x2 + scaleI * (x3 - x2);
float x2i = x1i + scaleX * (x4 - x1);
float x2f = x1f + scaleX * (x3 - x2);
for (int32_t j = 0; j < heightModel[i].size() - 1; ++j)
{
float scaleJ = scaleY * static_cast<float>(j);
float y1i = y1 + scaleJ * (y2 - y1);
float y1f = y4 + scaleJ * (y3 - y4);
float y2i = y1i + scaleY * (y2 - y1);
float y2f = y1f + scaleY * (y3 - y4);
float nx1 = x1i + scaleJ * (x1f - x1i);
float nx2 = x1i + (scaleJ + scaleY) * (x1f - x1i);
float nx3 = x2i + (scaleJ + scaleY) * (x2f - x2i);
float nx4 = x2i + scaleJ * (x2f - x2i);
float ny1 = y1i + scaleI * (y1f - y1i);
float ny2 = y2i + scaleI * (y2f - y2i);
float ny3 = y2i + (scaleI + scaleX) * (y2f - y2i);
float ny4 = y1i + (scaleI + scaleX) * (y1f - y1i);
glBegin(GL_QUADS);
glTexCoord2f(dx + scaleJ * (maxU - dx * 2.0f),
dy + (1.0f - scaleI) * (maxV - dy * 2.0f));
glVertex3f(nx1, ny1, -static_cast<float>(heightModel[i][j]));
glTexCoord2f(dx + (scaleJ + scaleY) * (maxU - dx * 2.0f),
dy + (1.0f - scaleI) * (maxV - dy * 2.0f));
glVertex3f(nx2, ny2, -static_cast<float>(heightModel[i][j + 1]));
glTexCoord2f(dx + (scaleJ + scaleY) * (maxU - dx * 2.0f),
dy + (1.0f - scaleI - scaleX) * (maxV - dy * 2.0f));
glVertex3f(nx3, ny3, -static_cast<float>(heightModel[i + 1][j + 1]));
glTexCoord2f(dx + scaleJ * (maxU - dx * 2.0f),
dy + (1.0f - scaleI - scaleX) * (maxV - dy * 2.0f));
glVertex3f(nx4, ny4, -static_cast<float>(heightModel[i + 1][j]));
glEnd();
}
}
}
glDisable(GL_TEXTURE_2D);
}
bool
Texture::is3D(void) const
{
return _is3D;
}
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL 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.
QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Definition of the class Texture.
*
* @author Lionel Heng <hengli@student.ethz.ch>
*
*/
#ifndef TEXTURE_H
#define TEXTURE_H
#if (defined __APPLE__) & (defined __MACH__)
#include <OpenGL/gl.h>
#else
#include <GL/gl.h>
#endif
#include <inttypes.h>
#include <QSharedPointer>
#include "WebImage.h"
class Texture
{
public:
Texture();
const QString& getSourceURL(void) const;
void setID(GLuint id);
void sync(const WebImagePtr& image);
void draw(float x1, float y1, float x2, float y2,
bool smoothInterpolation) const;
void draw(float x1, float y1, float x2, float y2,
float x3, float y3, float x4, float y4,
bool smoothInterpolation) const;
bool is3D(void) const;
private:
enum State
{
UNINITIALIZED = 0,
REQUESTED = 1,
READY = 2
};
State state;
QString sourceURL;
GLuint id;
int32_t textureWidth;
int32_t textureHeight;
int32_t imageWidth;
int32_t imageHeight;
bool _is3D;
QVector< QVector<int32_t> > heightModel;
float maxU;
float maxV;
};
typedef QSharedPointer<Texture> TexturePtr;
#endif // TEXTURE_H
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL 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.
QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Definition of the class TextureCache.
*
* @author Lionel Heng <hengli@student.ethz.ch>
*
*/
#include "TextureCache.h"
TextureCache::TextureCache(uint32_t _cacheSize)
: cacheSize(_cacheSize)
, imageCache(new WebImageCache(0, cacheSize))
{
for (uint32_t i = 0; i < cacheSize; ++i)
{
TexturePtr t(new Texture);
GLuint id;
glGenTextures(1, &id);
t->setID(id);
glBindTexture(GL_TEXTURE_2D, id);
glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
textures.push_back(t);
}
}
TexturePtr
TextureCache::get(const QString& tileURL, bool useHeightModel)
{
QPair<TexturePtr, int32_t> p1 = lookup(tileURL, useHeightModel);
if (!p1.first.isNull())
{
return p1.first;
}
QPair<WebImagePtr, int32_t> p2 =
imageCache->lookup(tileURL, useHeightModel);
if (!p2.first.isNull())
{
textures[p2.second]->sync(p2.first);
p1 = lookup(tileURL, useHeightModel);
return p1.first;
}
return TexturePtr();
}
void
TextureCache::sync(void)
{
if (requireSync())
{
for (int32_t i = 0; i < textures.size(); ++i)
{
textures[i]->sync(imageCache->at(i));
}
}
}
QPair<TexturePtr, int32_t>
TextureCache::lookup(const QString& tileURL, bool useHeightModel)
{
for (int32_t i = 0; i < textures.size(); ++i)
{
if (textures[i]->getSourceURL() == tileURL &&
textures[i]->is3D() == useHeightModel)
{
return qMakePair(textures[i], i);
}
}
return qMakePair(TexturePtr(), -1);
}
bool
TextureCache::requireSync(void) const
{
for (uint32_t i = 0; i < cacheSize; ++i)
{
if (imageCache->at(i)->getSyncFlag())
{
return true;
}
}
return false;
}
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL 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.
QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Definition of the class WebImage.
*
* @author Lionel Heng <hengli@student.ethz.ch>
*
*/
#include "WebImage.h"
#include <QFile>
#include <QGLWidget>
WebImage::WebImage()
: state(WebImage::UNINITIALIZED)
, sourceURL("")
, image(0)
, lastReference(0)
, _is3D(false)
, syncFlag(false)
{
}
void
WebImage::clear(void)
{
image.reset();
sourceURL.clear();
state = WebImage::UNINITIALIZED;
lastReference = 0;
heightModel.clear();
}
WebImage::State
WebImage::getState(void) const
{
return state;
}
void
WebImage::setState(State state)
{
this->state = state;
}
const QString&
WebImage::getSourceURL(void) const
{
return sourceURL;
}
void
WebImage::setSourceURL(const QString& url)
{
sourceURL = url;
}
const uint8_t*
WebImage::getImageData(void) const
{
return image->scanLine(0);
}
const QVector< QVector<int32_t> >&
WebImage::getHeightModel(void) const
{
return heightModel;
}
bool
WebImage::setData(const QByteArray& data)
{
QImage tempImage;
if (tempImage.loadFromData(data))
{
if (image.isNull())
{
image.reset(new QImage);
}
*image = QGLWidget::convertToGLFormat(tempImage);
return true;
}
else
{
return false;
}
}
bool
WebImage::setData(const QString& filename)
{
QImage tempImage;
if (tempImage.load(filename))
{
if (image.isNull())
{
image.reset(new QImage);
}
*image = QGLWidget::convertToGLFormat(tempImage);
return true;
}
else
{
return false;
}
}
bool
WebImage::setData(const QString& imageFilename, const QString& heightFilename)
{
QFile heightFile(heightFilename);
QImage tempImage;
if (tempImage.load(imageFilename) && heightFile.open(QIODevice::ReadOnly))
{
if (image.isNull())
{
image.reset(new QImage);
}
*image = QGLWidget::convertToGLFormat(tempImage);
QDataStream heightDataStream(&heightFile);
// read in width and height values for height map
char header[8];
heightDataStream.readRawData(header, 8);
int32_t height = *(reinterpret_cast<int32_t *>(header));
int32_t width = *(reinterpret_cast<int32_t *>(header + 4));
char buffer[height * width * sizeof(int32_t)];
heightDataStream.readRawData(buffer, height * width * sizeof(int32_t));
heightModel.clear();
for (int32_t i = 0; i < height; ++i)
{
QVector<int32_t> scanline;
for (int32_t j = 0; j < width; ++j)
{
int32_t n = *(reinterpret_cast<int32_t *>(buffer
+ (i * height + j)
* sizeof(int32_t)));
scanline.push_back(n);
}
heightModel.push_back(scanline);
}
heightFile.close();
_is3D = true;
return true;
}
else
{
return false;
}
}
int32_t
WebImage::getWidth(void) const
{
return image->width();
}
int32_t
WebImage::getHeight(void) const
{
return image->height();
}
int32_t
WebImage::getByteCount(void) const
{
return image->byteCount();
}
bool
WebImage::is3D(void) const
{
return _is3D;
}
uint64_t
WebImage::getLastReference(void) const
{
return lastReference;
}
void
WebImage::setLastReference(uint64_t value)
{
lastReference = value;
}
bool
WebImage::getSyncFlag(void) const
{
return syncFlag;
}
void
WebImage::setSyncFlag(bool onoff)
{
syncFlag = onoff;
}
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL 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.
QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Definition of the class WebImage.
*
* @author Lionel Heng <hengli@student.ethz.ch>
*
*/
#ifndef WEBIMAGE_H
#define WEBIMAGE_H
#include <inttypes.h>
#include <QImage>
#include <QScopedPointer>
#include <QSharedPointer>
class WebImage
{
public:
WebImage();
void clear(void);
enum State
{
UNINITIALIZED = 0,
REQUESTED = 1,
READY = 2
};
State getState(void) const;
void setState(State state);
const QString& getSourceURL(void) const;
void setSourceURL(const QString& url);
const uint8_t* getImageData(void) const;
const QVector< QVector<int32_t> >& getHeightModel(void) const;
bool setData(const QByteArray& data);
bool setData(const QString& filename);
bool setData(const QString& imageFilename, const QString& heightFilename);
int32_t getWidth(void) const;
int32_t getHeight(void) const;
int32_t getByteCount(void) const;
bool is3D(void) const;
uint64_t getLastReference(void) const;
void setLastReference(uint64_t value);
bool getSyncFlag(void) const;
void setSyncFlag(bool onoff);
private:
State state;
QString sourceURL;
QScopedPointer<QImage> image;
QVector< QVector<int32_t> > heightModel;
uint64_t lastReference;
bool _is3D;
bool syncFlag;
};
typedef QSharedPointer<WebImage> WebImagePtr;
#endif // WEBIMAGE_H
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL 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.
QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Definition of the class WebImageCache.
*
* @author Lionel Heng <hengli@student.ethz.ch>
*
*/
#include "WebImageCache.h"
#include <QNetworkReply>
#include <QPixmap>
WebImageCache::WebImageCache(QObject* parent, uint32_t _cacheSize)
: QObject(parent)
, cacheSize(_cacheSize)
, currentReference(0)
, networkManager(new QNetworkAccessManager)
{
for (uint32_t i = 0; i < cacheSize; ++i)
{
WebImagePtr image(new WebImage);
webImages.push_back(image);
}
connect(networkManager.data(), SIGNAL(finished(QNetworkReply*)),
this, SLOT(downloadFinished(QNetworkReply*)));
}
QPair<WebImagePtr, int32_t>
WebImageCache::lookup(const QString& url, bool useHeightModel)
{
QPair<WebImagePtr, int32_t> cacheEntry;
for (int32_t i = 0; i < webImages.size(); ++i)
{
if (webImages[i]->getState() != WebImage::UNINITIALIZED &&
webImages[i]->getSourceURL() == url &&
webImages[i]->is3D() == useHeightModel)
{
cacheEntry.first = webImages[i];
cacheEntry.second = i;
break;
}
}
if (cacheEntry.first.isNull())
{
for (int32_t i = 0; i < webImages.size(); ++i)
{
// get uninitialized image
if (webImages[i]->getState() == WebImage::UNINITIALIZED)
{
cacheEntry.first = webImages[i];
cacheEntry.second = i;
break;
}
// get oldest image
else if (webImages[i]->getState() == WebImage::READY &&
(cacheEntry.first.isNull() ||
webImages[i]->getLastReference() <
cacheEntry.first->getLastReference()))
{
cacheEntry.first = webImages[i];
cacheEntry.second = i;
}
}
if (cacheEntry.first.isNull())
{
return qMakePair(WebImagePtr(), -1);
}
else
{
if (cacheEntry.first->getState() == WebImage::READY)
{
cacheEntry.first->clear();
}
cacheEntry.first->setSourceURL(url);
cacheEntry.first->setLastReference(currentReference);
++currentReference;
cacheEntry.first->setState(WebImage::REQUESTED);
if (url.left(4).compare("http") == 0)
{
networkManager->get(QNetworkRequest(QUrl(url)));
}
else
{
bool success;
if (useHeightModel)
{
QString heightURL = url;
heightURL.replace("color", "dom");
heightURL.replace(".jpg", ".txt");
success = cacheEntry.first->setData(url, heightURL);
}
else
{
success = cacheEntry.first->setData(url);
}
if (success)
{
cacheEntry.first->setSyncFlag(true);
cacheEntry.first->setState(WebImage::READY);
}
else
{
cacheEntry.first->setState(WebImage::UNINITIALIZED);
}
}
return cacheEntry;
}
}
else
{
if (cacheEntry.first->getState() == WebImage::READY)
{
cacheEntry.first->setLastReference(currentReference);
++currentReference;
return cacheEntry;
}
else
{
return qMakePair(WebImagePtr(), -1);
}
}
}
WebImagePtr
WebImageCache::at(int32_t index) const
{
return webImages[index];
}
void
WebImageCache::downloadFinished(QNetworkReply* reply)
{
reply->deleteLater();
if (reply->error() != QNetworkReply::NoError)
{
return;
}
QVariant attribute = reply->attribute(QNetworkRequest::RedirectionTargetAttribute);
if (attribute.isValid())
{
return;
}
WebImagePtr image;
foreach(image, webImages)
{
if (reply->url().toString() == image->getSourceURL())
{
image->setData(reply->readAll());
image->setSyncFlag(true);
image->setState(WebImage::READY);
return;
}
}
}
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL 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.
QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Definition of the class WebImageCache.
*
* @author Lionel Heng <hengli@student.ethz.ch>
*
*/
#ifndef WEBIMAGECACHE_H
#define WEBIMAGECACHE_H
#include <QNetworkAccessManager>
#include <QObject>
#include <QPair>
#include "WebImage.h"
class WebImageCache : public QObject
{
Q_OBJECT
public:
WebImageCache(QObject* parent, uint32_t cacheSize);
QPair<WebImagePtr, int32_t> lookup(const QString& url,
bool useHeightModel);
WebImagePtr at(int32_t index) const;
private Q_SLOTS:
void downloadFinished(QNetworkReply* reply);
private:
uint32_t cacheSize;
QVector<WebImagePtr> webImages;
uint64_t currentReference;
QScopedPointer<QNetworkAccessManager> networkManager;
};
#endif // WEBIMAGECACHE_H
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment