Added some functionality to 3D View --Lionel

src/ui/map3D/Q3DWidget.cc

@@ -28,34 +28,35 @@ Q3DWidget::Q3DWidget(QWidget* parent)
   , _forceRedraw(false)
   , allow2DRotation(true)
   , limitCamera(false)
+  , lockCamera(false)
   , timerFunc(NULL)
   , timerFuncData(NULL)
 {
-	cameraPose.state = IDLE;
-	cameraPose.pan = 0.0f;
-	cameraPose.tilt = 180.0f;
-	cameraPose.distance = 10.0f;
-	cameraPose.xOffset = 0.0f;
-	cameraPose.yOffset = 0.0f;
-	cameraPose.zOffset = 0.0f;
-
-	cameraPose.xOffset2D = 0.0f;
-	cameraPose.yOffset2D = 0.0f;
-	cameraPose.rotation2D = 0.0f;
-	cameraPose.zoom = 1.0f;
-	cameraPose.warpX = 1.0f;
-	cameraPose.warpY = 1.0f;
-
-	cameraParams.zoomSensitivity = 0.05f;
-	cameraParams.rotateSensitivity = 0.5f;
-	cameraParams.moveSensitivity = 0.001f;
-	cameraParams.minZoomRange = 0.5f;
-	cameraParams.cameraFov = 30.0f;
-	cameraParams.minClipRange = 1.0f;
-	cameraParams.maxClipRange = 400.0f;
-	cameraParams.zoomSensitivity2D = 0.02f;
-	cameraParams.rotateSensitivity2D = 0.005f;
-	cameraParams.moveSensitivity2D = 1.0f;
+    cameraPose.state = IDLE;
+    cameraPose.pan = 0.0f;
+    cameraPose.tilt = 180.0f;
+    cameraPose.distance = 10.0f;
+    cameraPose.xOffset = 0.0f;
+    cameraPose.yOffset = 0.0f;
+    cameraPose.zOffset = 0.0f;
+
+    cameraPose.xOffset2D = 0.0f;
+    cameraPose.yOffset2D = 0.0f;
+    cameraPose.rotation2D = 0.0f;
+    cameraPose.zoom = 1.0f;
+    cameraPose.warpX = 1.0f;
+    cameraPose.warpY = 1.0f;
+
+    cameraParams.zoomSensitivity = 0.05f;
+    cameraParams.rotateSensitivity = 0.5f;
+    cameraParams.moveSensitivity = 0.001f;
+    cameraParams.minZoomRange = 0.5f;
+    cameraParams.cameraFov = 30.0f;
+    cameraParams.minClipRange = 1.0f;
+    cameraParams.maxClipRange = 400.0f;
+    cameraParams.zoomSensitivity2D = 0.02f;
+    cameraParams.rotateSensitivity2D = 0.005f;
+    cameraParams.moveSensitivity2D = 1.0f;
 }
 
 Q3DWidget::~Q3DWidget()
@@ -67,17 +68,17 @@ void
 Q3DWidget::initialize(int32_t windowX, int32_t windowY,
                       int32_t windowWidth, int32_t windowHeight, float fps)
 {
-	this->windowWidth = windowWidth;
-	this->windowHeight = windowHeight;
+    this->windowWidth = windowWidth;
+    this->windowHeight = windowHeight;
 
-	requestedFps = fps;
+    requestedFps = fps;
 
-	resize(windowWidth, windowHeight);
-	move(windowX, windowY);
+    resize(windowWidth, windowHeight);
+    move(windowX, windowY);
 
-	timer.start(static_cast<int>(floorf(1000.0f / requestedFps)), this);
+    timer.start(static_cast<int>(floorf(1000.0f / requestedFps)), this);
 
-	_is3D = true;
+    _is3D = true;
 }
 
 void
@@ -86,22 +87,28 @@ Q3DWidget::setCameraParams(float zoomSensitivity, float rotateSensitivity,
 							 float cameraFov, float minClipRange,
 							 float maxClipRange)
 {
-	cameraParams.zoomSensitivity = zoomSensitivity;
-	cameraParams.rotateSensitivity = rotateSensitivity;
-	cameraParams.moveSensitivity = moveSensitivity;
-	cameraParams.minZoomRange = minZoomRange;
-	cameraParams.cameraFov = cameraFov;
-	cameraParams.minClipRange = minClipRange;
-	cameraParams.maxClipRange = maxClipRange;
+    cameraParams.zoomSensitivity = zoomSensitivity;
+    cameraParams.rotateSensitivity = rotateSensitivity;
+    cameraParams.moveSensitivity = moveSensitivity;
+    cameraParams.minZoomRange = minZoomRange;
+    cameraParams.cameraFov = cameraFov;
+    cameraParams.minClipRange = minClipRange;
+    cameraParams.maxClipRange = maxClipRange;
 
-	limitCamera = true;
-	_forceRedraw = true;
+    limitCamera = true;
+    _forceRedraw = true;
 }
 
 void
 Q3DWidget::setCameraLimit(bool onoff)
 {
-	limitCamera = onoff;
+    limitCamera = onoff;
+}
+
+void
+Q3DWidget::setCameraLock(bool onoff)
+{
+    lockCamera = onoff;
 }
 
 void
@@ -109,422 +116,425 @@ Q3DWidget::set2DCameraParams(float zoomSensitivity2D,
                              float rotateSensitivity2D,
                              float moveSensitivity2D)
 {
-	cameraParams.zoomSensitivity2D = zoomSensitivity2D;
-	cameraParams.rotateSensitivity2D = rotateSensitivity2D;
-	cameraParams.moveSensitivity2D = moveSensitivity2D;
+    cameraParams.zoomSensitivity2D = zoomSensitivity2D;
+    cameraParams.rotateSensitivity2D = rotateSensitivity2D;
+    cameraParams.moveSensitivity2D = moveSensitivity2D;
 }
 
 void
 Q3DWidget::set3D(bool onoff)
 {
-	_is3D = onoff;
+    _is3D = onoff;
 }
 
 bool
 Q3DWidget::is3D(void) const
 {
-	return _is3D;
+    return _is3D;
 }
 
 void
 Q3DWidget::setInitialCameraPos(float pan, float tilt, float range,
 								 float xOffset, float yOffset, float zOffset)
 {
-	cameraPose.pan = pan;
-	cameraPose.tilt = tilt;
-	cameraPose.distance = range;
-	cameraPose.xOffset = xOffset;
-	cameraPose.yOffset = yOffset;
-	cameraPose.zOffset = zOffset;
+    cameraPose.pan = pan;
+    cameraPose.tilt = tilt;
+    cameraPose.distance = range;
+    cameraPose.xOffset = xOffset;
+    cameraPose.yOffset = yOffset;
+    cameraPose.zOffset = zOffset;
 }
 
 void
 Q3DWidget::setInitial2DCameraPos(float xOffset, float yOffset,
 								   float rotation, float zoom)
 {
-	cameraPose.xOffset2D = xOffset;
-	cameraPose.yOffset2D = yOffset;
-	cameraPose.rotation2D = rotation;
-	cameraPose.zoom = zoom;
+    cameraPose.xOffset2D = xOffset;
+    cameraPose.yOffset2D = yOffset;
+    cameraPose.rotation2D = rotation;
+    cameraPose.zoom = zoom;
 }
 
 void
 Q3DWidget::setCameraPose(const CameraPose& cameraPose)
 {
-	this->cameraPose = cameraPose;
+    this->cameraPose = cameraPose;
 }
 
 CameraPose
 Q3DWidget::getCameraPose(void) const
 {
-	return cameraPose;
+    return cameraPose;
 }
 
 void
 Q3DWidget::setDisplayFunc(DisplayFunc func, void* clientData)
 {
-	userDisplayFunc = func;
-	userDisplayFuncData = clientData;
+    userDisplayFunc = func;
+    userDisplayFuncData = clientData;
 }
 
 void
 Q3DWidget::setKeyboardFunc(KeyboardFunc func, void* clientData)
 {
-	userKeyboardFunc = func;
-	userKeyboardFuncData = clientData;
+    userKeyboardFunc = func;
+    userKeyboardFuncData = clientData;
 }
 
 void
 Q3DWidget::setMouseFunc(MouseFunc func, void* clientData)
 {
-	userMouseFunc = func;
-	userMouseFuncData = clientData;
+    userMouseFunc = func;
+    userMouseFuncData = clientData;
 }
 
 void
 Q3DWidget::setMotionFunc(MotionFunc func, void* clientData)
 {
-	userMotionFunc = func;
-	userMotionFuncData = clientData;
+    userMotionFunc = func;
+    userMotionFuncData = clientData;
 }
 
 void
 Q3DWidget::addTimerFunc(uint32_t msecs, void(*func)(void *),
                           void* clientData)
 {
-	timerFunc = func;
-	timerFuncData = clientData;
+    timerFunc = func;
+    timerFuncData = clientData;
 
-	QTimer::singleShot(msecs, this, SLOT(userTimer()));
+    QTimer::singleShot(msecs, this, SLOT(userTimer()));
 }
 
 void
 Q3DWidget::userTimer(void)
 {
-	if (timerFunc)
-	{
-		timerFunc(timerFuncData);
-	}
+    if (timerFunc)
+    {
+            timerFunc(timerFuncData);
+    }
 }
 
 void
 Q3DWidget::forceRedraw(void)
 {
-	_forceRedraw = true;
+    _forceRedraw = true;
 }
 
 void
 Q3DWidget::set2DWarping(float warpX, float warpY)
 {
-	cameraPose.warpX = warpX;
-	cameraPose.warpY = warpY;
+    cameraPose.warpX = warpX;
+    cameraPose.warpY = warpY;
 }
 
 void
 Q3DWidget::recenter(void)
 {
-	cameraPose.xOffset = 0.0f;
-	cameraPose.yOffset = 0.0f;
-	cameraPose.zOffset = 0.0f;
+    cameraPose.xOffset = 0.0f;
+    cameraPose.yOffset = 0.0f;
+    cameraPose.zOffset = 0.0f;
 }
 
 void
 Q3DWidget::recenter2D(void)
 {
-	cameraPose.xOffset2D = 0.0f;
-	cameraPose.yOffset2D = 0.0f;
+    cameraPose.xOffset2D = 0.0f;
+    cameraPose.yOffset2D = 0.0f;
 }
 
 void
 Q3DWidget::set2DRotation(bool onoff)
 {
-	allow2DRotation = onoff;
+    allow2DRotation = onoff;
 }
 
 void
 Q3DWidget::setDisplayMode2D(void)
 {
-        glDisable(GL_DEPTH_TEST);
-        glMatrixMode(GL_PROJECTION);
-        glLoadIdentity();
-        glOrtho(0.0, static_cast<GLfloat>(getWindowWidth()),
-                0.0, static_cast<GLfloat>(getWindowHeight()),
-                -10.0, 10.0);
-        glMatrixMode(GL_MODELVIEW);
-        glLoadIdentity();
+    glDisable(GL_DEPTH_TEST);
+    glMatrixMode(GL_PROJECTION);
+    glLoadIdentity();
+    glOrtho(0.0, static_cast<GLfloat>(getWindowWidth()),
+            0.0, static_cast<GLfloat>(getWindowHeight()),
+            -10.0, 10.0);
+    glMatrixMode(GL_MODELVIEW);
+    glLoadIdentity();
 }
 
 std::pair<float,float>
 Q3DWidget::getPositionIn3DMode(int32_t mouseX, int32_t mouseY)
 {
-	float cx = windowWidth / 2.0f;
-	float cy = windowHeight / 2.0f;
-	float pan = d2r(-90.0f - cameraPose.pan);
-	float tilt = d2r(90.0f - cameraPose.tilt);
-	float d = cameraPose.distance;
-	float f = cy / tanf(d2r(cameraParams.cameraFov / 2.0f));
+    float cx = windowWidth / 2.0f;
+    float cy = windowHeight / 2.0f;
+    float pan = d2r(-90.0f - cameraPose.pan);
+    float tilt = d2r(90.0f - cameraPose.tilt);
+    float d = cameraPose.distance;
+    float f = cy / tanf(d2r(cameraParams.cameraFov / 2.0f));
 
-	float px = (mouseX - cx) * cosf(tilt) * d / (cosf(tilt) * f + sinf(tilt)
-				* mouseY - sinf(tilt) * cy);
-	float py = -(mouseY - cy) * d / (cosf(tilt) * f + sinf(tilt) * mouseY
-				- sinf(tilt) * cy);
+    float px = (mouseX - cx) * cosf(tilt) * d / (cosf(tilt) * f + sinf(tilt)
+                            * mouseY - sinf(tilt) * cy);
+    float py = -(mouseY - cy) * d / (cosf(tilt) * f + sinf(tilt) * mouseY
+                            - sinf(tilt) * cy);
 
-	std::pair<float,float> scene_coords;
-	scene_coords.first = px * cosf(pan) + py * sinf(pan) + cameraPose.xOffset;
-	scene_coords.second = -px * sinf(pan) + py * cosf(pan) + cameraPose.yOffset;
+    std::pair<float,float> sceneCoords;
+    sceneCoords.first = px * cosf(pan) + py * sinf(pan) + cameraPose.xOffset;
+    sceneCoords.second = -px * sinf(pan) + py * cosf(pan) + cameraPose.yOffset;
 
-	return scene_coords;
+    return sceneCoords;
 }
 
 std::pair<float,float>
 Q3DWidget::getPositionIn2DMode(int32_t mouseX, int32_t mouseY)
 {
-        float dx = (mouseX - windowWidth / 2.0f) / cameraPose.zoom;
-        float dy = (windowHeight / 2.0f - mouseY) / cameraPose.zoom;
-	float ctheta = cosf(-cameraPose.rotation2D);
-	float stheta = sinf(-cameraPose.rotation2D);
+    float dx = (mouseX - windowWidth / 2.0f) / cameraPose.zoom;
+    float dy = (windowHeight / 2.0f - mouseY) / cameraPose.zoom;
+    float ctheta = cosf(-cameraPose.rotation2D);
+    float stheta = sinf(-cameraPose.rotation2D);
 
-	std::pair<float,float> coords;
-	coords.first = cameraPose.xOffset2D + ctheta * dx - stheta * dy;
-	coords.second = cameraPose.yOffset2D + stheta * dx + ctheta * dy;
+    std::pair<float,float> coords;
+    coords.first = cameraPose.xOffset2D + ctheta * dx - stheta * dy;
+    coords.second = cameraPose.yOffset2D + stheta * dx + ctheta * dy;
 
-	return coords;
+    return coords;
 }
 
 int
 Q3DWidget::getWindowWidth(void)
 {
-	return windowWidth;
+    return windowWidth;
 }
 
 int
 Q3DWidget::getWindowHeight(void)
 {
-	return windowHeight;
+    return windowHeight;
 }
 
 int
 Q3DWidget::getLastMouseX(void)
 {
-	return lastMouseX;
+    return lastMouseX;
 }
 
 int
 Q3DWidget::getLastMouseY(void)
 {
-	return lastMouseY;
+    return lastMouseY;
 }
 
 int
 Q3DWidget::getMouseX(void)
 {
-	return mapFromGlobal(cursor().pos()).x();
+    return mapFromGlobal(cursor().pos()).x();
 }
 
 int
 Q3DWidget::getMouseY(void)
 {
-	return mapFromGlobal(cursor().pos()).y();
+    return mapFromGlobal(cursor().pos()).y();
 }
 
 
 void
 Q3DWidget::rotateCamera(float dx, float dy)
 {
+    if (!lockCamera)
+    {
 	cameraPose.pan += dx * cameraParams.rotateSensitivity;
 	cameraPose.tilt += dy * cameraParams.rotateSensitivity;
 	if (limitCamera)
 	{
-		if (cameraPose.tilt < 180.5f)
-		{
-			cameraPose.tilt = 180.5f;
-		}
-		else if (cameraPose.tilt > 269.5f)
-		{
-			cameraPose.tilt = 269.5f;
-		}
+            if (cameraPose.tilt < 180.5f)
+            {
+                cameraPose.tilt = 180.5f;
+            }
+            else if (cameraPose.tilt > 269.5f)
+            {
+                cameraPose.tilt = 269.5f;
+            }
 	}
+    }
 }
 
 void
 Q3DWidget::zoomCamera(float dy)
 {
-	cameraPose.distance -=
-			dy * cameraParams.zoomSensitivity * cameraPose.distance;
-	if (cameraPose.distance < cameraParams.minZoomRange)
-	{
-		cameraPose.distance = cameraParams.minZoomRange;
-	}
+    cameraPose.distance -=
+            dy * cameraParams.zoomSensitivity * cameraPose.distance;
+    if (cameraPose.distance < cameraParams.minZoomRange)
+    {
+        cameraPose.distance = cameraParams.minZoomRange;
+    }
 }
 
 void
 Q3DWidget::moveCamera(float dx, float dy)
 {
-	cameraPose.xOffset +=
-			-dy * cosf(d2r(cameraPose.pan)) * cameraParams.moveSensitivity
-			* cameraPose.distance;
-	cameraPose.yOffset +=
-			-dy * sinf(d2r(cameraPose.pan)) * cameraParams.moveSensitivity
-			* cameraPose.distance;
-	cameraPose.xOffset += dx * cosf(d2r(cameraPose.pan - 90.0f))
-			* cameraParams.moveSensitivity * cameraPose.distance;
-	cameraPose.yOffset += dx * sinf(d2r(cameraPose.pan - 90.0f))
-			* cameraParams.moveSensitivity * cameraPose.distance;
+    cameraPose.xOffset +=
+            -dy * cosf(d2r(cameraPose.pan)) * cameraParams.moveSensitivity
+            * cameraPose.distance;
+    cameraPose.yOffset +=
+            -dy * sinf(d2r(cameraPose.pan)) * cameraParams.moveSensitivity
+            * cameraPose.distance;
+    cameraPose.xOffset += dx * cosf(d2r(cameraPose.pan - 90.0f))
+            * cameraParams.moveSensitivity * cameraPose.distance;
+    cameraPose.yOffset += dx * sinf(d2r(cameraPose.pan - 90.0f))
+            * cameraParams.moveSensitivity * cameraPose.distance;
 }
 
 void
 Q3DWidget::rotateCamera2D(float dx)
 {
-	if (allow2DRotation)
-	{
-		cameraPose.rotation2D += dx * cameraParams.rotateSensitivity2D;
-	}
+    if (allow2DRotation)
+    {
+        cameraPose.rotation2D += dx * cameraParams.rotateSensitivity2D;
+    }
 }
 
 void
 Q3DWidget::zoomCamera2D(float dx)
 {
-	cameraPose.zoom += dx * cameraParams.zoomSensitivity2D * cameraPose.zoom;
-	if (cameraPose.zoom > 1e7f)
-	{
-		cameraPose.zoom = 1e7f;
-	}
-	if (cameraPose.zoom < 1e-7f)
-	{
-		cameraPose.zoom = 1e-7f;
-	}
+    cameraPose.zoom += dx * cameraParams.zoomSensitivity2D * cameraPose.zoom;
+    if (cameraPose.zoom > 1e7f)
+    {
+        cameraPose.zoom = 1e7f;
+    }
+    if (cameraPose.zoom < 1e-7f)
+    {
+        cameraPose.zoom = 1e-7f;
+    }
 }
 
 void
 Q3DWidget::moveCamera2D(float dx, float dy)
 {
-	float scaledX = dx / cameraPose.zoom;
-	float scaledY = dy / cameraPose.zoom;
+    float scaledX = dx / cameraPose.zoom;
+    float scaledY = dy / cameraPose.zoom;
 
-	cameraPose.xOffset2D -= (scaledX * cosf(-cameraPose.rotation2D)
-			+ scaledY * sinf(-cameraPose.rotation2D)) / cameraPose.warpX
-			* cameraParams.moveSensitivity2D;
-	cameraPose.yOffset2D -= (scaledX * sinf(-cameraPose.rotation2D)
-			- scaledY * cosf(-cameraPose.rotation2D)) / cameraPose.warpY
-			* cameraParams.moveSensitivity2D;
+    cameraPose.xOffset2D -= (scaledX * cosf(-cameraPose.rotation2D)
+            + scaledY * sinf(-cameraPose.rotation2D)) / cameraPose.warpX
+            * cameraParams.moveSensitivity2D;
+    cameraPose.yOffset2D -= (scaledX * sinf(-cameraPose.rotation2D)
+            - scaledY * cosf(-cameraPose.rotation2D)) / cameraPose.warpY
+            * cameraParams.moveSensitivity2D;
 }
 
 void Q3DWidget::switchTo3DMode(void)
 {
-	// setup camera view
-	float cpan = d2r(cameraPose.pan);
-	float ctilt = d2r(cameraPose.tilt);
-	float cameraX = cameraPose.distance * cosf(cpan) * cosf(ctilt);
-	float cameraY = cameraPose.distance * sinf(cpan) * cosf(ctilt);
-	float cameraZ = cameraPose.distance * sinf(ctilt);
-        setDisplayMode3D();
-	glViewport(0, 0, static_cast<GLsizei>(windowWidth),
-			   static_cast<GLsizei>(windowHeight));
-	gluLookAt(cameraX + cameraPose.xOffset, cameraY + cameraPose.yOffset,
-			  cameraZ + cameraPose.zOffset, cameraPose.xOffset,
-			  cameraPose.yOffset, cameraPose.zOffset, 0.0, 0.0, 1.0);
+    // setup camera view
+    float cpan = d2r(cameraPose.pan);
+    float ctilt = d2r(cameraPose.tilt);
+    float cameraX = cameraPose.distance * cosf(cpan) * cosf(ctilt);
+    float cameraY = cameraPose.distance * sinf(cpan) * cosf(ctilt);
+    float cameraZ = cameraPose.distance * sinf(ctilt);
+    setDisplayMode3D();
+    glViewport(0, 0, static_cast<GLsizei>(windowWidth),
+               static_cast<GLsizei>(windowHeight));
+    gluLookAt(cameraX + cameraPose.xOffset, cameraY + cameraPose.yOffset,
+              cameraZ + cameraPose.zOffset, cameraPose.xOffset,
+              cameraPose.yOffset, cameraPose.zOffset, 0.0, 0.0, 1.0);
 }
 
 void
 Q3DWidget::setDisplayMode3D()
 {
-        float aspect = static_cast<float>(getWindowWidth()) /
-                       static_cast<float>(getWindowHeight());
+    float aspect = static_cast<float>(getWindowWidth()) /
+                   static_cast<float>(getWindowHeight());
 
-	glEnable(GL_DEPTH_TEST);
-	glMatrixMode(GL_PROJECTION);
-	glLoadIdentity();
-	gluPerspective(cameraParams.cameraFov, aspect,
-				   cameraParams.minClipRange, cameraParams.maxClipRange);
-	glMatrixMode(GL_MODELVIEW);
-	glLoadIdentity();
-	glScalef(-1.0f, -1.0f, 1.0f);
+    glEnable(GL_DEPTH_TEST);
+    glMatrixMode(GL_PROJECTION);
+    glLoadIdentity();
+    gluPerspective(cameraParams.cameraFov, aspect,
+                   cameraParams.minClipRange, cameraParams.maxClipRange);
+    glMatrixMode(GL_MODELVIEW);
+    glLoadIdentity();
+    glScalef(-1.0f, -1.0f, 1.0f);
 }
 
 float
 Q3DWidget::r2d(float angle)
 {
-	return angle * 57.295779513082320876f;
+    return angle * 57.295779513082320876f;
 }
 
 float
 Q3DWidget::d2r(float angle)
 {
-	return angle * 0.0174532925199432957692f;
+    return angle * 0.0174532925199432957692f;
 }
 
 void
 Q3DWidget::initializeGL(void)
 {
-	float lightAmbient[] = {0.0f, 0.0f, 0.0f, 0.0f};
-	float lightDiffuse[] = {1.0f, 1.0f, 1.0f, 1.0f};
-	float lightSpecular[] = {1.0f, 1.0f, 1.0f, 1.0f};
-	float lightPosition[] = {0.0f, 0.0f, 100.0f, 0.0f};
+    float lightAmbient[] = {0.0f, 0.0f, 0.0f, 0.0f};
+    float lightDiffuse[] = {1.0f, 1.0f, 1.0f, 1.0f};
+    float lightSpecular[] = {1.0f, 1.0f, 1.0f, 1.0f};
+    float lightPosition[] = {0.0f, 0.0f, 100.0f, 0.0f};
 
-	glEnable(GL_DEPTH_TEST);
-	glShadeModel(GL_SMOOTH);
-	glLightfv(GL_LIGHT0, GL_AMBIENT, lightAmbient);
-	glLightfv(GL_LIGHT0, GL_DIFFUSE, lightDiffuse);
-	glLightfv(GL_LIGHT0, GL_SPECULAR, lightSpecular);
-	glLightfv(GL_LIGHT0, GL_POSITION, lightPosition);
-	glEnable(GL_LIGHT0);
-	glDisable(GL_LIGHTING);
-	glEnable(GL_NORMALIZE);
+    glEnable(GL_DEPTH_TEST);
+    glShadeModel(GL_SMOOTH);
+    glLightfv(GL_LIGHT0, GL_AMBIENT, lightAmbient);
+    glLightfv(GL_LIGHT0, GL_DIFFUSE, lightDiffuse);
+    glLightfv(GL_LIGHT0, GL_SPECULAR, lightSpecular);
+    glLightfv(GL_LIGHT0, GL_POSITION, lightPosition);
+    glEnable(GL_LIGHT0);
+    glDisable(GL_LIGHTING);
+    glEnable(GL_NORMALIZE);
 }
 
 void
 Q3DWidget::paintGL(void)
 {
-	if (_is3D)
-	{
-		// setup camera view
-		switchTo3DMode();
-	}
-	else
-	{
-                setDisplayMode2D();
-		// do camera control
-		glTranslatef(static_cast<float>(windowWidth) / 2.0f,
-					 static_cast<float>(windowHeight) / 2.0f,
-					 0.0f);
-		glScalef(cameraPose.zoom, cameraPose.zoom, 1.0f);
-		glRotatef(r2d(cameraPose.rotation2D), 0.0f, 0.0f, 1.0f);
-		glScalef(cameraPose.warpX, cameraPose.warpY, 1.0f);
-		glTranslatef(-cameraPose.xOffset2D, -cameraPose.yOffset2D, 0.0f);
-	}
-
-	// turn on smooth lines
-	glEnable(GL_BLEND);
-	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
-	glEnable(GL_LINE_SMOOTH);
-
-        glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
-	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
-	glLineWidth(1.0f);
-
-	if (userDisplayFunc)
-	{
-		userDisplayFunc(userDisplayFuncData);
-	}
-	glFlush();
+    if (_is3D)
+    {
+        // setup camera view
+        switchTo3DMode();
+    }
+    else
+    {
+        setDisplayMode2D();
+        // do camera control
+        glTranslatef(static_cast<float>(windowWidth) / 2.0f,
+                                 static_cast<float>(windowHeight) / 2.0f,
+                                 0.0f);
+        glScalef(cameraPose.zoom, cameraPose.zoom, 1.0f);
+        glRotatef(r2d(cameraPose.rotation2D), 0.0f, 0.0f, 1.0f);
+        glScalef(cameraPose.warpX, cameraPose.warpY, 1.0f);
+        glTranslatef(-cameraPose.xOffset2D, -cameraPose.yOffset2D, 0.0f);
+    }
+
+    // turn on smooth lines
+    glEnable(GL_BLEND);
+    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+    glEnable(GL_LINE_SMOOTH);
+
+    glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
+    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+    glLineWidth(1.0f);
+
+    if (userDisplayFunc)
+    {
+        userDisplayFunc(userDisplayFuncData);
+    }
+    glFlush();
 }
 
 void
 Q3DWidget::resizeGL(int32_t width, int32_t height)
 {
-	glViewport(0, 0, width, height);
+    glViewport(0, 0, width, height);
 
-	windowWidth = width;
-	windowHeight = height;
+    windowWidth = width;
+    windowHeight = height;
 
-	if (_is3D)
-	{
-                setDisplayMode3D();
-	}
-	else
-	{
-                setDisplayMode2D();
-	}
+    if (_is3D)
+    {
+        setDisplayMode3D();
+    }
+    else
+    {
+        setDisplayMode2D();
+    }
 }
 
 void
@@ -832,5 +842,5 @@ Q3DWidget::timerEvent(QTimerEvent* event)
 void
 Q3DWidget::closeEvent(QCloseEvent *)
 {
-	// exit application
+    // exit application
 }

src/ui/map3D/Q3DWidget.h

@@ -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

 #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

@@ -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;
 };