test.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/img/TColor.h>
#include <mrpt/math/TLine3D.h>
#include <mrpt/math/TObject3D.h>
#include <mrpt/math/geometry.h>
#include <mrpt/opengl/CAxis.h>
#include <mrpt/opengl/CBox.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CSphere.h>
#include <mrpt/opengl/CText.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/os.h>
#include <iostream>

using namespace std;
using namespace mrpt;
using namespace mrpt::gui;
using namespace mrpt::math;
using namespace mrpt::opengl;
using namespace mrpt::img;
using namespace mrpt::system;
// ------------------------------------------------------
//              TestDisplay3D
// ------------------------------------------------------
void TestDisplay3D()
{
    CDisplayWindow3D win("Example of 3D Scene Visualization - MRPT", 640, 480);

    COpenGLScene::Ptr& theScene = win.get3DSceneAndLock();

    // Add a clone viewport, using [0,1] factor X,Y,Width,Height coordinates:
    {
        COpenGLViewport::Ptr vi = theScene->createViewport("myClone");
        vi->setViewportPosition(0.7, 0.05, 0.28, 0.28);
        vi->setCloneView("main");
        vi->setTransparent(true);
        vi->getCamera().setAzimuthDegrees(45);
        vi->getCamera().setElevationDegrees(45);
        vi->getCamera().setZoomDistance(10);
    }

    // Another clone viewport, using absolute coordinates
    {
        COpenGLViewport::Ptr vi = theScene->createViewport("myClone2");
        vi->setViewportPosition(
            /*x px*/ -250, /*y px*/ -250, /*width px*/ 250,
            /*height px*/ 200);  // x,y negative means pixels from the
        // top/right, instead of from the bottom/left.
        vi->setCloneView("main");
        vi->setTransparent(false);
        vi->getCamera().setAzimuthDegrees(-95);
        vi->getCamera().setElevationDegrees(30);
        vi->getCamera().setZoomDistance(8);
    }

    // And another transparent viewport just to show 3D text:
    if (false)
    {
        mrpt::opengl::CText::Ptr txt1 = mrpt::opengl::CText::Create();
        COpenGLViewport::Ptr vi = theScene->createViewport("flat_viewport");
        vi->setViewportPosition(0, 0, 0.3, 0.3);
        vi->setTransparent(true);
        vi->setBorderSize(0);
        vi->getCamera().setAzimuthDegrees(0);
        vi->getCamera().setElevationDegrees(90);
        vi->getCamera().setZoomDistance(5);
        vi->getCamera().setOrthogonal(true);

        vi->insert(txt1);
    }

    // Modify the scene:
    // ------------------------------------------------------
    {
        opengl::CGridPlaneXY::Ptr obj =
            opengl::CGridPlaneXY::Create(-20, 20, -20, 20, 0, 1);
        obj->setColor(0.8f, 0.8f, 0.8f);
        theScene->insert(obj);
    }

    {
        opengl::CAxis::Ptr obj = opengl::CAxis::Create();
        obj->setFrequency(5);
        obj->enableTickMarks();
        obj->setAxisLimits(-10, -10, -10, 10, 10, 10);
        theScene->insert(obj);
    }

    {
        opengl::CBox::Ptr obj = opengl::CBox::Create();
        obj->setWireframe(false);
        obj->setColor(1, 0, 0);
        obj->setLineWidth(3.0);
        obj->setPose(TPose3D(10, 0, 0, 0.2, 0.3, 0.1));
        theScene->insert(obj);
    }

    {
        opengl::CSphere::Ptr obj = opengl::CSphere::Create();
        obj->setColor(0, 0, 1);
        obj->setRadius(0.3f);
        obj->setLocation(0, 0, 1);
        obj->setName("ball_1");
        theScene->insert(obj);
    }
    {
        opengl::CSphere::Ptr obj = opengl::CSphere::Create();
        obj->setColor(1, 0, 0);
        obj->setRadius(0.3f);
        obj->setLocation(-1, -1, 1);
        obj->setName("ball_2");
        theScene->insert(obj);
    }

    {
        opengl::CSphere::Ptr obj = opengl::CSphere::Create();
        obj->setColor(0, 1, 0);
        obj->setRadius(0.5);
        obj->setLocation(0, 0, 0);
        obj->setName("USER_MOUSE_PICK");
        theScene->insert(obj);
    }

    // IMPORTANT!!! IF NOT UNLOCKED, THE WINDOW WILL NOT BE UPDATED!
    win.unlockAccess3DScene();

    // Texts:
    mrpt::opengl::TFontParams fp;
    fp.color = TColorf(1, 1, 1);
    win.addTextMessage(0.01, 0.85, "This is a 2D message", 0 /*id*/, fp);

    win.setCameraElevationDeg(25.0f);
    // win.setCameraProjective(false);

    cout << endl;
    cout << "Control with mouse or keyboard. Valid keys:" << endl;
    cout << "  ESC                        -> Exit" << endl;
    cout << "  Left/right cursor arrow    -> Camera azimuth" << endl;
    cout << "  P                          -> Enable / disable 'place object' "
            "mode"
         << endl;
    cout << endl;

    bool end = false;
    bool placeMode = false;

    CTicTac timer;
    timer.Tic();

    while (!end && win.isOpen())
    {
        const double t = timer.Tac();

        // Move the scene:
        COpenGLScene::Ptr& theScene = win.get3DSceneAndLock();

        const double R1 = 8;
        const double W1 = 5.0, Q1 = 3.3;
        opengl::CRenderizable::Ptr obj1 = theScene->getByName("ball_1");
        obj1->setLocation(
            R1 * cos(W1 * t) * sin(Q1 * t), R1 * sin(W1 * t),
            R1 * cos(W1 * t) * cos(Q1 * t));

        const double R2 = 6;
        const double W2 = 1.3, Q2 = 7.2;
        opengl::CRenderizable::Ptr obj2 = theScene->getByName("ball_2");
        obj2->setLocation(
            R2 * cos(W2 * t) * sin(Q2 * t), R2 * sin(W2 * t),
            R2 * cos(W2 * t) * cos(Q2 * t));

        mrpt::opengl::TFontParams fp2;
        fp2.color = TColorf(.8f, .8f, .8f);
        fp2.vfont_name = "sans";
        fp2.vfont_scale = 14;

        win.addTextMessage(
            0.02, 0.02,  // X,Y<=1 means coordinates are factors over the entire
            // viewport area.
            format(
                "ball#1 pos: %.02f %.02f %.02f ", obj1->getPoseX(),
                obj1->getPoseY(), obj1->getPoseZ()),
            10 /*id*/, fp2);

        win.addTextMessage(
            5, -15,  // |X|,|Y|>1 means absolute coordinates, negative means
            // from the top instead of the bottom.
            format(
                "Time: %s",
                mrpt::system::dateTimeLocalToString(mrpt::system::now())
                    .c_str()),
            20 /* id */, fp2);

        // Show management of (x,y) mouse coordinates and 3D rays:
        // ------------------------------------------------------------
        int mouse_x, mouse_y;
        if (placeMode &&
            win.getLastMousePosition(
                mouse_x, mouse_y))  // See also: getLastMousePositionRay()
        {
            // Get the ray in 3D for the latest mouse (X,Y):
            mrpt::math::TLine3D ray;
            theScene->getViewport("main")->get3DRayForPixelCoord(
                mouse_x, mouse_y, ray);

            // Create a 3D plane, e.g. Z=0
            const mrpt::math::TPlane ground_plane(
                TPoint3D(0, 0, 0), TPoint3D(1, 0, 0), TPoint3D(0, 1, 0));

            // Intersection of the line with the plane:
            mrpt::math::TObject3D inters;
            mrpt::math::intersect(ray, ground_plane, inters);

            // Interpret the intersection as a point, if there is an
            // intersection:
            mrpt::math::TPoint3D inters_pt;
            if (inters.getPoint(inters_pt))
            {
                // Move an object to the position picked by the user:
                // printf("PT: %f %f %f\n",);
                theScene->getByName("USER_MOUSE_PICK")
                    ->setLocation(inters_pt.x, inters_pt.y, inters_pt.z);
            }
        }

        // IMPORTANT!!! IF NOT UNLOCKED, THE WINDOW WILL NOT BE UPDATED!
        win.unlockAccess3DScene();

        // Update window:
        win.forceRepaint();
        std::this_thread::sleep_for(20ms);

        if (mrpt::system::os::kbhit()) end = true;
        if (win.keyHit())
        {
            mrptKeyModifier kmods;
            int key = win.getPushedKey(&kmods);
            printf(
                "Key pushed: %c (%i) - modifiers: 0x%04X\n", char(key), key,
                kmods);

            if (key == MRPTK_ESCAPE) end = true;

            if (key == MRPTK_RIGHT)
                win.setCameraAzimuthDeg(win.getCameraAzimuthDeg() + 5);
            if (key == MRPTK_LEFT)
                win.setCameraAzimuthDeg(win.getCameraAzimuthDeg() - 5);

            if (key == 'p' || key == 'P')
            {
                placeMode = !placeMode;
                win.setCursorCross(placeMode);
            }
        }
    };
}

// ------------------------------------------------------
//                      MAIN
// ------------------------------------------------------
int main()
{
    try
    {
        TestDisplay3D();

        return 0;
    }
    catch (const std::exception& e)
    {
        std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
        return -1;
    }
    catch (...)
    {
        printf("Untyped exception!!");
        return -1;
    }
}