Skip to content

Q
qgroundcontrol
Commit 8b414e7d authored by Lionel Heng's avatar Lionel Heng
Browse files
Options

Added some functionality to 3D View --Lionel

parent 5e22d5a8
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 619 additions and 409 deletions
src/ui/map3D/Q3DWidget.cc
View file @ 8b414e7d
This diff is collapsed.
src/ui/map3D/Q3DWidget.h
View file @ 8b414e7d
......@@ -9,40 +9,40 @@
enum CameraState
{
IDLE = 0,
ROTATING = 1,
MOVING = 2,
ZOOMING = 3
IDLE = 0,
ROTATING = 1,
MOVING = 2,
ZOOMING = 3
};
struct CameraPose
{
CameraState state;
float pan, tilt, distance;
float xOffset, yOffset, zOffset;
float xOffset2D, yOffset2D, rotation2D, zoom, warpX, warpY;
CameraState state;
float pan, tilt, distance;
float xOffset, yOffset, zOffset;
float xOffset2D, yOffset2D, rotation2D, zoom, warpX, warpY;
};
struct CameraParams
{
float zoomSensitivity;
float rotateSensitivity;
float moveSensitivity;
float minZoomRange;
float cameraFov;
float minClipRange;
float maxClipRange;
float zoomSensitivity2D;
float rotateSensitivity2D;
float moveSensitivity2D;
float zoomSensitivity;
float rotateSensitivity;
float moveSensitivity;
float minZoomRange;
float cameraFov;
float minClipRange;
float maxClipRange;
float zoomSensitivity2D;
float rotateSensitivity2D;
float moveSensitivity2D;
};
enum MouseState
{
MOUSE_STATE_UP = 0,
MOUSE_STATE_DOWN = 1
MOUSE_STATE_UP = 0,
MOUSE_STATE_DOWN = 1
};
typedef void (*DisplayFunc)(void *);
......@@ -52,125 +52,128 @@ typedef void (*MotionFunc)(int32_t, int32_t, void *);
class Q3DWidget: public QGLWidget
{
Q_OBJECT
Q_OBJECT
public:
explicit Q3DWidget(QWidget* parent);
~Q3DWidget();
explicit Q3DWidget(QWidget* parent);
~Q3DWidget();
void initialize(int32_t windowX, int32_t windowY,
int32_t windowWidth, int32_t windowHeight, float fps);
void initialize(int32_t windowX, int32_t windowY,
int32_t windowWidth, int32_t windowHeight, float fps);
void setCameraParams(float zoomSensitivity, float rotateSensitivity,
float moveSensitivity, float minZoomRange,
float camera_fov, float minClipRange,
float maxClipRange);
void setCameraParams(float zoomSensitivity, float rotateSensitivity,
float moveSensitivity, float minZoomRange,
float cameraFov, float minClipRange,
float maxClipRange);
void setCameraLimit(bool onoff);
void setCameraLimit(bool onoff);
void setCameraLock(bool onoff);
void set2DCameraParams(float zoomSensitivity,
float rotateSensitivity,
float moveSensitivity);
void set2DCameraParams(float zoomSensitivity,
float rotateSensitivity,
float moveSensitivity);
void set3D(bool onoff);
bool is3D(void) const;
void set3D(bool onoff);
bool is3D(void) const;
void setInitialCameraPos(float pan, float tilt, float range,
float xOffset, float yOffset, float zOffset);
void setInitial2DCameraPos(float xOffset, float yOffset,
float rotation, float zoom);
void setCameraPose(const CameraPose& cameraPose);
CameraPose getCameraPose(void) const;
void setInitialCameraPos(float pan, float tilt, float range,
float xOffset, float yOffset, float zOffset);
void setInitial2DCameraPos(float xOffset, float yOffset,
float rotation, float zoom);
void setCameraPose(const CameraPose& cameraPose);
CameraPose getCameraPose(void) const;
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(uint32_t msecs, void(*func)(void *),
void* clientData);
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(uint32_t msecs, void(*func)(void *),
void* clientData);
void forceRedraw(void);
void forceRedraw(void);
void set2DWarping(float warpX, float warpY);
void set2DWarping(float warpX, float warpY);
void recenter(void);
void recenter2D(void);
void recenter(void);
void recenter2D(void);
void set2DRotation(bool onoff);
void set2DRotation(bool onoff);
void setDisplayMode2D(void);
void setDisplayMode2D(void);
std::pair<float,float> getPositionIn3DMode(int32_t mouseX,
int32_t mouseY);
std::pair<float,float> getPositionIn3DMode(int32_t mouseX,
int32_t mouseY);
std::pair<float,float> getPositionIn2DMode(int32_t mouseX,
int32_t mouseY);
std::pair<float,float> getPositionIn2DMode(int32_t mouseX,
int32_t mouseY);
int32_t getWindowWidth(void);
int32_t getWindowHeight(void);
int32_t getLastMouseX(void);
int32_t getLastMouseY(void);
int32_t getMouseX(void);
int32_t getMouseY(void);
int32_t getWindowWidth(void);
int32_t getWindowHeight(void);
int32_t getLastMouseX(void);
int32_t getLastMouseY(void);
int32_t getMouseX(void);
int32_t getMouseY(void);
private Q_SLOTS:
void userTimer(void);
void userTimer(void);
protected:
void rotateCamera(float dx, float dy);
void zoomCamera(float dy);
void moveCamera(float dx, float dy);
void rotateCamera2D(float dx);
void zoomCamera2D(float dx);
void moveCamera2D(float dx, float dy);
void switchTo3DMode(void);
void setDisplayMode3D(void);
float r2d(float angle);
float d2r(float angle);
private:
void rotateCamera(float dx, float dy);
void zoomCamera(float dy);
void moveCamera(float dx, float dy);
void rotateCamera2D(float dx);
void zoomCamera2D(float dx);
void moveCamera2D(float dx, float dy);
void switchTo3DMode(void);
void setDisplayMode3D(void);
float r2d(float angle);
float d2r(float angle);
// QGLWidget events
void initializeGL(void);
void paintGL(void);
void resizeGL(int32_t width, int32_t height);
// Qt events
void keyPressEvent(QKeyEvent* event);
void mousePressEvent(QMouseEvent* event);
void mouseReleaseEvent(QMouseEvent* event);
void mouseMoveEvent(QMouseEvent* event);
void wheelEvent(QWheelEvent *wheel);
void timerEvent(QTimerEvent* event);
void closeEvent(QCloseEvent* event);
DisplayFunc userDisplayFunc;
KeyboardFunc userKeyboardFunc;
MouseFunc userMouseFunc;
MotionFunc userMotionFunc;
void* userDisplayFuncData;
void* userKeyboardFuncData;
void* userMouseFuncData;
void* userMotionFuncData;
int32_t windowWidth, windowHeight;
float requestedFps;
CameraPose cameraPose;
int32_t lastMouseX, lastMouseY;
bool _is3D;
bool _forceRedraw;
bool allow2DRotation;
bool limitCamera;
CameraParams cameraParams;
QBasicTimer timer;
void (*timerFunc)(void *);
void* timerFuncData;
// QGLWidget events
void initializeGL(void);
void paintGL(void);
void resizeGL(int32_t width, int32_t height);
// Qt events
void keyPressEvent(QKeyEvent* event);
void mousePressEvent(QMouseEvent* event);
void mouseReleaseEvent(QMouseEvent* event);
void mouseMoveEvent(QMouseEvent* event);
void wheelEvent(QWheelEvent *wheel);
void timerEvent(QTimerEvent* event);
void closeEvent(QCloseEvent* event);
DisplayFunc userDisplayFunc;
KeyboardFunc userKeyboardFunc;
MouseFunc userMouseFunc;
MotionFunc userMotionFunc;
void* userDisplayFuncData;
void* userKeyboardFuncData;
void* userMouseFuncData;
void* userMotionFuncData;
int32_t windowWidth, windowHeight;
float requestedFps;
CameraPose cameraPose;
int32_t lastMouseX, lastMouseY;
bool _is3D;
bool _forceRedraw;
bool allow2DRotation;
bool limitCamera;
bool lockCamera;
CameraParams cameraParams;
QBasicTimer timer;
void (*timerFunc)(void *);
void* timerFuncData;
};
#endif
src/ui/map3D/QMap3DWidget.cc
View file @ 8b414e7d
#include "QMap3DWidget.h"
#include <FTGL/ftgl.h>
#include <QPushButton>
#include <QCheckBox>
#include <sys/time.h>
#include "CheetahModel.h"
......@@ -12,30 +12,19 @@ QMap3DWidget::QMap3DWidget(QWidget* parent)
: Q3DWidget(parent)
, uas(NULL)
, lastRedrawTime(0.0)
, displayGrid(false)
, displayTrail(false)
, lockCamera(false)
{
setFocusPolicy(Qt::StrongFocus);
initialize(10, 10, 1000, 900, 10.0f);
setCameraParams(0.05f, 0.5f, 0.001f, 0.5f, 30.0f, 0.01f, 400.0f);
setCameraParams(0.05f, 0.5f, 0.01f, 0.5f, 30.0f, 0.01f, 400.0f);
setDisplayFunc(display, this);
addTimerFunc(100, timer, this);
QPushButton* mapButton = new QPushButton(this);
mapButton->setText("Grid");
// display the MapControl in the application
QGridLayout* layout = new QGridLayout(this);
layout->setMargin(0);
layout->setSpacing(2);
//layout->addWidget(mc, 0, 0, 1, 2);
layout->addWidget(mapButton, 1, 0);
layout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 1);
layout->setRowStretch(0, 100);
layout->setRowStretch(1, 1);
layout->setColumnStretch(0, 1);
layout->setColumnStretch(1, 50);
setLayout(layout);
buildLayout();
font.reset(new FTTextureFont("images/Vera.ttf"));
......@@ -49,9 +38,44 @@ QMap3DWidget::~QMap3DWidget()
}
void
QMap3DWidget::init(void)
QMap3DWidget::buildLayout(void)
{
QCheckBox* gridCheckBox = new QCheckBox(this);
gridCheckBox->setText("Grid");
gridCheckBox->setChecked(displayGrid);
QCheckBox* trailCheckBox = new QCheckBox(this);
trailCheckBox->setText("Trail");
trailCheckBox->setChecked(displayTrail);
QPushButton* recenterButton = new QPushButton(this);
recenterButton->setText("Recenter Camera");
QCheckBox* lockCameraCheckBox = new QCheckBox(this);
lockCameraCheckBox->setText("Lock Camera");
lockCameraCheckBox->setChecked(lockCamera);
QGridLayout* layout = new QGridLayout(this);
layout->setMargin(0);
layout->setSpacing(2);
//layout->addWidget(mc, 0, 0, 1, 2);
layout->addWidget(gridCheckBox, 1, 0);
layout->addWidget(trailCheckBox, 1, 1);
layout->addWidget(recenterButton, 1, 2);
layout->addWidget(lockCameraCheckBox, 1, 3);
layout->setRowStretch(0, 100);
layout->setRowStretch(1, 1);
layout->setColumnStretch(0, 1);
layout->setColumnStretch(1, 50);
setLayout(layout);
connect(gridCheckBox, SIGNAL(stateChanged(int)),
this, SLOT(showGrid(int)));
connect(trailCheckBox, SIGNAL(stateChanged(int)),
this, SLOT(showTrail(int)));
connect(recenterButton, SIGNAL(clicked()), this, SLOT(recenterCamera()));
connect(lockCameraCheckBox, SIGNAL(stateChanged(int)),
this, SLOT(toggleLockCamera(int)));
}
void
......@@ -70,11 +94,20 @@ QMap3DWidget::displayHandler(void)
cheetahModel->init(1.0f, 1.0f, 1.0f);
}
if (uas == NULL)
float robotX = 0.0f, robotY = 0.0f, robotZ = 0.0f;
float robotRoll = 0.0f, robotPitch = 0.0f, robotYaw = 0.0f;
if (uas != NULL)
{
return;
robotX = uas->getLocalX();
robotY = uas->getLocalY();
robotZ = uas->getLocalZ();
robotRoll = uas->getRoll();
robotPitch = uas->getPitch();
robotYaw = uas->getYaw();
}
setCameraLock(lockCamera);
// turn on smooth lines
glEnable(GL_LINE_SMOOTH);
glHint(GL_LINE_SMOOTH_HINT, GL_NICEST);
......@@ -86,48 +119,44 @@ QMap3DWidget::displayHandler(void)
glClear(GL_COLOR_BUFFER_BIT);
// draw Cheetah model
glPushMatrix();
glRotatef(uas->getYaw(), 0.0f, 0.0f, 1.0f);
glRotatef(uas->getPitch(), 0.0f, 1.0f, 0.0f);
glRotatef(uas->getRoll(), 1.0f, 0.0f, 0.0f);
glLineWidth(3.0f);
glColor3f(0.0f, 1.0f, 0.0f);
glBegin(GL_LINES);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(0.3f, 0.0f, 0.0f);
glEnd();
drawPlatform(robotRoll, robotPitch, robotYaw);
cheetahModel->draw();
if (displayGrid)
{
drawGrid();
}
glPopMatrix();
if (displayTrail)
{
drawTrail(robotX, robotY, robotZ);
}
// switch to 2D
setDisplayMode2D();
// display pose information
glColor4f(0.0f, 0.0f, 0.0f, 0.5f);
glBegin(GL_POLYGON);
glVertex2f(0.0f, 0.0f);
glVertex2f(0.0f, 20.0f);
glVertex2f(getWindowWidth(), 20.0f);
glVertex2f(0.0f, 45.0f);
glVertex2f(getWindowWidth(), 45.0f);
glVertex2f(getWindowWidth(), 0.0f);
glEnd();
char buffer[6][255];
sprintf(buffer[0], "x = %.2f", uas->getLocalX());
sprintf(buffer[1], "y = %.2f", uas->getLocalY());
sprintf(buffer[2], "z = %.2f", uas->getLocalZ());
sprintf(buffer[3], "r = %.2f", uas->getRoll());
sprintf(buffer[4], "p = %.2f", uas->getPitch());
sprintf(buffer[5], "y = %.2f", uas->getYaw());
sprintf(buffer[0], "x = %.2f", robotX);
sprintf(buffer[1], "y = %.2f", robotY);
sprintf(buffer[2], "z = %.2f", robotZ);
sprintf(buffer[3], "r = %.2f", robotRoll);
sprintf(buffer[4], "p = %.2f", robotPitch);
sprintf(buffer[5], "y = %.2f", robotYaw);
font->FaceSize(10);
glColor3f(1.0f, 1.0f, 1.0f);
glPushMatrix();
glTranslatef(0.0f, 5.0f, 0.0f);
glTranslatef(0.0f, 30.0f, 0.0f);
for (int32_t i = 0; i < 6; ++i)
{
glTranslatef(60.0f, 0.0f, 0.0f);
......@@ -180,3 +209,149 @@ QMap3DWidget::getTime(void) const
return static_cast<double>(tv.tv_sec) +
static_cast<double>(tv.tv_usec) / 1000000.0;
}
void
QMap3DWidget::showGrid(int32_t state)
{
if (state == Qt::Checked)
{
displayGrid = true;
}
else
{
displayGrid = false;
}
}
void
QMap3DWidget::showTrail(int32_t state)
{
if (state == Qt::Checked)
{
if (!displayTrail)
{
trail.clear();
}
displayTrail = true;
}
else
{
displayTrail = false;
}
}
void
QMap3DWidget::recenterCamera(void)
{
recenter();
}
void
QMap3DWidget::toggleLockCamera(int32_t state)
{
if (state == Qt::Checked)
{
lockCamera = true;
}
else
{
lockCamera = false;
}
}
void
QMap3DWidget::drawPlatform(float roll, float pitch, float yaw)
{
glPushMatrix();
glRotatef(yaw, 0.0f, 0.0f, 1.0f);
glRotatef(pitch, 0.0f, 1.0f, 0.0f);
glRotatef(roll, 1.0f, 0.0f, 0.0f);
glLineWidth(3.0f);
glColor3f(0.0f, 1.0f, 0.0f);
glBegin(GL_LINES);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(0.3f, 0.0f, 0.0f);
glEnd();
cheetahModel->draw();
glPopMatrix();
}
void
QMap3DWidget::drawGrid(void)
{
float radius = 10.0f;
float resolution = 0.25f;
glPushMatrix();
// draw a 20m x 20m grid with 0.25m resolution
glColor3f(0.5f, 0.5f, 0.5f);
for (float i = -radius; i <= radius; i += resolution)
{
if (fabsf(i - roundf(i)) < 0.01f)
{
glLineWidth(2.0f);
}
else
{
glLineWidth(0.25f);
}
glBegin(GL_LINES);
glVertex3f(i, -radius, 0.0f);
glVertex3f(i, radius, 0.0f);
glVertex3f(-radius, i, 0.0f);
glVertex3f(radius, i, 0.0f);
glEnd();
}
glPopMatrix();
}
void
QMap3DWidget::drawTrail(float x, float y, float z)
{
bool addToTrail = false;
if (trail.size() > 0)
{
if (fabsf(x - trail[trail.size() - 1].x) > 0.01f ||
fabsf(y - trail[trail.size() - 1].y) > 0.01f ||
fabsf(z - trail[trail.size() - 1].z) > 0.01f)
{
addToTrail = true;
}
}
else
{
addToTrail = true;
}
if (addToTrail)
{
Pose3D p = {x, y, z};
if (trail.size() == trail.capacity())
{
memcpy(trail.data(), trail.data() + 1,
(trail.size() - 1) * sizeof(Pose3D));
trail[trail.size() - 1] = p;
}
else
{
trail.append(p);
}
}
glColor3f(1.0f, 0.0f, 0.0f);
glLineWidth(1.0f);
glBegin(GL_LINE_STRIP);
for (int32_t i = 0; i < trail.size(); ++i)
{
glVertex3f(trail[i].x - x, trail[i].y - y, trail[i].z - z);
}
glEnd();
}
src/ui/map3D/QMap3DWidget.h
View file @ 8b414e7d
......@@ -15,7 +15,7 @@ public:
explicit QMap3DWidget(QWidget* parent);
~QMap3DWidget();
void init(void);
void buildLayout(void);
static void display(void* clientData);
void displayHandler(void);
......@@ -28,12 +28,34 @@ public:
public slots:
void setActiveUAS(UASInterface* uas);
private slots:
void showGrid(int state);
void showTrail(int state);
void recenterCamera(void);
void toggleLockCamera(int state);
protected:
UASInterface* uas;
private:
void drawPlatform(float roll, float pitch, float yaw);
void drawGrid(void);
void drawTrail(float x, float y, float z);
double lastRedrawTime;
bool displayGrid;
bool displayTrail;
bool lockCamera;
typedef struct
{
float x;
float y;
float z;
} Pose3D;
QVarLengthArray<Pose3D, 10000> trail;
QScopedPointer<CheetahModel> cheetahModel;
QScopedPointer<FTTextureFont> font;
};
......
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