StockObjects.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 "opengl-precomp.h"  // Precompiled header

#include <mrpt/io/CFileOutputStream.h>
#include <mrpt/opengl/CArrow.h>
#include <mrpt/opengl/CBox.h>
#include <mrpt/opengl/CCylinder.h>
#include <mrpt/opengl/CPolyhedron.h>
#include <mrpt/opengl/CSetOfObjects.h>
#include <mrpt/opengl/CSetOfTriangles.h>
#include <mrpt/opengl/CSimpleLine.h>
#include <mrpt/opengl/stock_objects.h>

using namespace mrpt::opengl;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace std;

/*---------------------------------------------------------------
                    RobotPioneer
  ---------------------------------------------------------------*/
CSetOfObjects::Ptr stock_objects::RobotPioneer()
{
    CSetOfObjects::Ptr ret = std::make_shared<CSetOfObjects>();

    ret->setName("theRobot");

    CSetOfTriangles::Ptr obj = std::make_shared<CSetOfTriangles>();
    obj->setLocalRepresentativePoint({0.0f, 0.0f, 0.15f});

    // Add triangles:
    mrpt::opengl::TTriangle t;

    t.r(0) = t.r(1) = t.r(2) = 255;
    t.g(0) = t.g(1) = t.g(2) = 0;
    t.b(0) = t.b(1) = t.b(2) = 0;
    t.a(0) = t.a(1) = t.a(2) = 255;

    t.vertex(0) = {0.10f, -0.10f, 0.20f};
    t.vertex(1) = {-0.20f, 0.10f, 0.25f};
    t.vertex(2) = {-0.20f, -0.10f, 0.25f};
    t.computeNormals();
    obj->insertTriangle(t);  // 0
    t.vertex(0) = {0.10f, -0.10f, 0.20f};
    t.vertex(1) = {0.10f, 0.10f, 0.20f};
    t.vertex(2) = {-0.20f, 0.10f, 0.25f};
    t.computeNormals();
    obj->insertTriangle(t);  // 1

    t.vertex(0) = {0.10f, -0.10f, 0.05f};
    t.vertex(1) = {0.10f, 0.10f, 0.20f};
    t.vertex(2) = {0.10f, -0.10f, 0.20f};
    t.computeNormals();
    obj->insertTriangle(t);  // 2
    t.vertex(0) = {0.10f, -0.10f, 0.05f};
    t.vertex(1) = {0.10f, 0.10f, 0.05f};
    t.vertex(2) = {0.10f, 0.10f, 0.20f};
    t.computeNormals();
    obj->insertTriangle(t);  // 3

    t.vertex(0) = {-0.20f, -0.10f, 0.05f};
    t.vertex(1) = {-0.20f, -0.10f, 0.25f};
    t.vertex(2) = {-0.20f, 0.10f, 0.25f};
    t.computeNormals();
    obj->insertTriangle(t);  // 2b
    t.vertex(0) = {-0.20f, -0.10f, 0.05f};
    t.vertex(1) = {-0.20f, 0.10f, 0.25f};
    t.vertex(2) = {-0.20f, 0.10f, 0.05f};
    t.computeNormals();
    obj->insertTriangle(t);  // 3b

    t.vertex(0) = {0.10f, -0.10f, 0.20f};
    t.vertex(1) = {-0.20f, -0.10f, 0.25f};
    t.vertex(2) = {-0.20f, -0.10f, 0.05f};
    t.computeNormals();
    obj->insertTriangle(t);  // 4

    t.vertex(0) = {0.10f, -0.10f, 0.20f};
    t.vertex(1) = {-0.20f, -0.10f, 0.05f};
    t.vertex(2) = {0.10f, -0.10f, 0.05f};
    t.computeNormals();
    obj->insertTriangle(t);  // 5

    t.vertex(0) = {0.10f, 0.10f, 0.20f};
    t.vertex(1) = {-0.20f, 0.10f, 0.05f};
    t.vertex(2) = {-0.20f, 0.10f, 0.25f};
    t.computeNormals();
    obj->insertTriangle(t);  // 6

    t.vertex(0) = {0.10f, 0.10f, 0.20f};
    t.vertex(1) = {0.10f, 0.10f, 0.05f};
    t.vertex(2) = {-0.20f, 0.10f, 0.05f};
    t.computeNormals();
    obj->insertTriangle(t);  // 7

    t.setColor(mrpt::img::TColorf(0.05f, 0.05f, 0.05f, 1));

    t.vertex(0) = {0.00f, 0.11f, 0.00f};
    t.vertex(1) = {0.00f, 0.11f, 0.10f};
    t.vertex(2) = {0.05f, 0.11f, 0.05f};
    t.computeNormals();
    obj->insertTriangle(t);  // 8
    t.vertex(0) = {0.00f, 0.11f, 0.00f};
    t.vertex(1) = {0.00f, 0.11f, 0.10f};
    t.vertex(2) = {-0.05f, 0.11f, 0.05f};
    t.computeNormals();
    obj->insertTriangle(t);  // 9

    t.vertex(0) = {0.00f, -0.11f, 0.00f};
    t.vertex(1) = {0.00f, -0.11f, 0.10f};
    t.vertex(2) = {0.05f, -0.11f, 0.05f};
    t.computeNormals();
    obj->insertTriangle(t);  // 10
    t.vertex(0) = {0.00f, -0.11f, 0.00f};
    t.vertex(1) = {0.00f, -0.11f, 0.10f};
    t.vertex(2) = {-0.05f, -0.11f, 0.05f};
    t.computeNormals();
    obj->insertTriangle(t);  // 11

    ret->insert(obj);

    return ret;
}

/*---------------------------------------------------------------
                    CornerXYZ
  ---------------------------------------------------------------*/
CSetOfObjects::Ptr stock_objects::CornerXYZ(float scale)
{
    CSetOfObjects::Ptr ret = std::make_shared<CSetOfObjects>();

    CArrow::Ptr obj = CArrow::Create(
        0, 0, 0, scale, 0, 0, 0.25f * scale, 0.02f * scale, 0.05f * scale);

    obj->setColor(1, 0, 0);
    ret->insert(obj);

    obj = CArrow::Create(
        0, 0, 0, 0, scale, 0, 0.25f * scale, 0.02f * scale, 0.05f * scale);
    obj->setColor(0, 1, 0);

    ret->insert(obj);

    obj = CArrow::Create(
        0, 0, 0, 0, 0, scale, 0.25f * scale, 0.02f * scale, 0.05f * scale);
    obj->setColor(0, 0, 1);

    ret->insert(obj);

    return ret;
}

/*---------------------------------------------------------------
                    RobotRhodon
  ---------------------------------------------------------------*/
CSetOfObjects::Ptr stock_objects::RobotRhodon()
{
    CSetOfObjects::Ptr ret = std::make_shared<CSetOfObjects>();
    float height = 0;

    vector<TPoint2D> level1;
    level1.emplace_back(0.31, 0);
    level1.emplace_back(0.22, 0.24);
    level1.emplace_back(-0.22, 0.24);
    level1.emplace_back(-0.31, 0);
    level1.emplace_back(-0.22, -0.24);
    level1.emplace_back(0.22, -0.24);

    CPolyhedron::Ptr obj1 =
        opengl::CPolyhedron::CreateCustomPrism(level1, 0.38);
    obj1->setLocation(0, 0, height);
    height += 0.38f;
    obj1->setColor(0.6f, 0.6f, 0.6f);
    ret->insert(obj1);

    vector<TPoint2D> level2;
    level2.emplace_back(0.16, 0.21);
    level2.emplace_back(-0.16, 0.21);
    level2.emplace_back(-0.16, -0.21);
    level2.emplace_back(0.16, -0.21);

    CPolyhedron::Ptr obj2 =
        opengl::CPolyhedron::CreateCustomPrism(level2, 0.35);
    obj2->setLocation(0, 0, height);
    height += 0.35f;
    obj2->setColor(0.2f, 0.2f, 0.2f);
    ret->insert(obj2);

    vector<TPoint2D> level3;
    level3.emplace_back(-0.12, 0.12);
    level3.emplace_back(-0.16, 0.12);
    level3.emplace_back(-0.16, -0.12);
    level3.emplace_back(-0.12, -0.12);

    CPolyhedron::Ptr obj3 = opengl::CPolyhedron::CreateCustomPrism(level3, 1);
    obj3->setLocation(0, 0, height);
    // height+=1;
    obj3->setColor(0.6f, 0.6f, 0.6f);
    ret->insert(obj3);

    opengl::CCylinder::Ptr obj4 =
        std::make_shared<opengl::CCylinder>(0.05f, 0.05f, 0.4f, 20);
    obj4->setLocation(0, 0, 0.73);
    obj4->setColor(0.0f, 0.0f, 0.9f);
    ret->insert(obj4);

    opengl::CCylinder::Ptr obj5 =
        std::make_shared<opengl::CCylinder>(0.05f, 0.05f, 0.4f, 20);
    obj5->setPose(CPose3D(0.32, 0, 0.89, 0, -1, 0));
    obj5->setColor(0.0f, 0.0f, 0.9f);
    ret->insert(obj5);

    return ret;
}

/*---------------------------------------------------------------
                    RobotGiraff
  ---------------------------------------------------------------*/
CSetOfObjects::Ptr stock_objects::RobotGiraff()
{
    CSetOfObjects::Ptr ret = std::make_shared<CSetOfObjects>();
    float height = 0;

    // Base
    vector<TPoint2D> level1;
    level1.emplace_back(0.31, 0);
    level1.emplace_back(0.22, 0.24);
    level1.emplace_back(-0.22, 0.24);
    level1.emplace_back(-0.31, 0);
    level1.emplace_back(-0.22, -0.24);
    level1.emplace_back(0.22, -0.24);

    CPolyhedron::Ptr obj1 =
        opengl::CPolyhedron::CreateCustomPrism(level1, 0.23);
    obj1->setLocation(0, 0, height);
    height += 0.23f;
    obj1->setColor(1.0f, 0.6f, 0.0f);
    ret->insert(obj1);

    // Electronic's cage
    vector<TPoint2D> level2;
    level2.emplace_back(0.13, 0.1);
    level2.emplace_back(-0.13, 0.1);
    level2.emplace_back(-0.13, -0.1);
    level2.emplace_back(0.13, -0.1);

    CPolyhedron::Ptr obj2 =
        opengl::CPolyhedron::CreateCustomPrism(level2, 0.45);
    obj2->setLocation(0, 0, height);
    height += 0.45f;
    obj2->setColor(1.0f, 0.6f, 0.2f);
    ret->insert(obj2);

    // Neck
    vector<TPoint2D> level3;
    level3.emplace_back(0.03, 0.03);
    level3.emplace_back(-0.03, 0.03);
    level3.emplace_back(-0.03, -0.03);
    level3.emplace_back(0.03, -0.03);

    CPolyhedron::Ptr obj3 =
        opengl::CPolyhedron::CreateCustomPrism(level3, 0.55);
    obj3->setLocation(0, 0, height);
    height += 0.55f;
    obj3->setColor(0.6f, 0.6f, 0.6f);
    ret->insert(obj3);

    // Screen
    vector<TPoint2D> level4;
    level4.emplace_back(0.03, 0.11);
    level4.emplace_back(-0.03, 0.11);
    level4.emplace_back(-0.03, -0.11);
    level4.emplace_back(0.03, -0.11);

    CPolyhedron::Ptr obj4 = opengl::CPolyhedron::CreateCustomPrism(level4, 0.4);
    obj4->setLocation(0, 0, height);
    obj4->setColor(1.0f, 0.6f, 0.0f);
    ret->insert(obj4);

    return ret;
}

CSetOfObjects::Ptr stock_objects::CornerXYZEye()
{
    CSetOfObjects::Ptr ret = std::make_shared<CSetOfObjects>();
    CPose3D rotation;

    CArrow::Ptr obj = CArrow::Create(0, 0, 0, 1.0, 0, 0, 0.25f, 0.02f, 0.05f);

    obj->setColor(1, 0, 0);

    ret->insert(obj);

    obj = CArrow::Create(0, 0, 0, 0, 1.0, 0, 0.25f, 0.02f, 0.05f);
    obj->setColor(0, 1, 0);

    ret->insert(obj);

    obj = CArrow::Create(0, 0, -1.0, 0, 0, 0, 0.25f, 0.02f, 0.05f);
    obj->setColor(0, 0, 1);

    ret->insert(obj);

    return ret;
}

/*---------------------------------------------------------------
                    BumblebeeCamera
  ---------------------------------------------------------------*/
CSetOfObjects::Ptr stock_objects::BumblebeeCamera()
{
    CSetOfObjects::Ptr camera = std::make_shared<opengl::CSetOfObjects>();

    CPolyhedron::Ptr rect = opengl::CPolyhedron::CreateCubicPrism(
        -0.02, 0.14, -0.02, 0.02, 0, -0.04);
    rect->setColor(1.0f, 0.8f, 0.0f);

    camera->insert(rect);

    CCylinder::Ptr lCam =
        std::make_shared<opengl::CCylinder>(0.01f, 0.01f, 0.003f, 10);
    lCam->setColor(1, 0, 0);

    CCylinder::Ptr rCam =
        std::make_shared<opengl::CCylinder>(0.01f, 0.01f, 0.003f, 10);
    rCam->setPose(CPose3D(0.12, 0, 0));
    rCam->setColor(0, 0, 0);

    camera->insert(lCam);
    camera->insert(rCam);

    return camera;
}

CSetOfObjects::Ptr stock_objects::CornerXYZSimple(float scale, float lineWidth)
{
    CSetOfObjects::Ptr ret = std::make_shared<CSetOfObjects>();

    // Using OpenGL shaders, it's more complicated to set line widths.
    // So, let's use cylinders of a diameter proportional to "scale":
    const float R = scale * 0.01f;
    const int nSlices = 6;

    {  // X:
        auto lin = CCylinder::Create(R, R, scale, nSlices);
        lin->setColor(1, 0, 0);
        lin->setPose(mrpt::poses::CPose3D::FromString("[0 0 0 0 90 0]"));
        ret->insert(lin);
    }
    {  // Y:
        auto lin = CCylinder::Create(R, R, scale, nSlices);
        lin->setColor(0, 1, 0);
        lin->setPose(mrpt::poses::CPose3D::FromString("[0 0 0 0 0 -90]"));
        ret->insert(lin);
    }
    {  // Z:
        auto lin = CCylinder::Create(R, R, scale, nSlices);
        lin->setColor(0, 0, 1);
        ret->insert(lin);
    }
    return ret;
}

CSetOfObjects::Ptr stock_objects::CornerXYSimple(float scale, float lineWidth)
{
    CSetOfObjects::Ptr ret = std::make_shared<CSetOfObjects>();

    // Using OpenGL shaders, it's more complicated to set line widths.
    // So, let's use cylinders of a diameter proportional to "scale":
    const float R = scale * 0.01f;
    const int nSlices = 6;

    {  // X:
        auto lin = CCylinder::Create(R, R, scale, nSlices);
        lin->setColor(1, 0, 0);
        lin->setPose(mrpt::poses::CPose3D::FromString("[0 0 0 0 90 0]"));
        ret->insert(lin);
    }
    {  // Y:
        auto lin = CCylinder::Create(R, R, scale, nSlices);
        lin->setColor(0, 1, 0);
        lin->setPose(mrpt::poses::CPose3D::FromString("[0 0 0 0 0 -90]"));
        ret->insert(lin);
    }
    return ret;
}

CSetOfObjects::Ptr stock_objects::Hokuyo_URG()
{
    CSetOfObjects::Ptr ret = std::make_shared<CSetOfObjects>();

    {
        CBox::Ptr base = std::make_shared<CBox>(
            TPoint3D(-0.025, -0.025, -0.0575), TPoint3D(0.025, 0.025, -0.0185));
        base->setColor(0.7f, 0.7f, 0.7f);
        ret->insert(base);
    }
    {
        CCylinder::Ptr cyl1 = std::make_shared<CCylinder>(0.02f, 0.02f, 0.01f);
        cyl1->setColor(0, 0, 0);
        cyl1->setLocation(0, 0, -0.014);
        ret->insert(cyl1);
    }
    {
        CCylinder::Ptr cyl2 =
            std::make_shared<CCylinder>(0.02f, 0.0175f, 0.01f);
        cyl2->setColor(0, 0, 0);
        cyl2->setLocation(0, 0, -0.004);
        ret->insert(cyl2);
    }
    {
        CCylinder::Ptr cyl3 =
            std::make_shared<CCylinder>(0.0175f, 0.0175f, 0.01f);
        cyl3->setColor(0, 0, 0);
        cyl3->setLocation(0, 0, 0.004);
        ret->insert(cyl3);
    }

    return ret;
}

CSetOfObjects::Ptr stock_objects::Hokuyo_UTM()
{
    CSetOfObjects::Ptr ret = std::make_shared<CSetOfObjects>();

    {
        CBox::Ptr base = std::make_shared<CBox>(
            TPoint3D(-0.03, -0.03, -0.055), TPoint3D(0.03, 0.03, -0.014));
        base->setColor(0, 0, 0);
        ret->insert(base);
    }
    {
        CCylinder::Ptr cyl1 =
            std::make_shared<CCylinder>(0.028f, 0.024f, 0.028f);
        cyl1->setColor(0, 0, 0);
        cyl1->setPose(CPose3D(0, 0, -0.014));
        ret->insert(cyl1);
    }
    {
        CCylinder::Ptr cyl2 =
            std::make_shared<CCylinder>(0.028f, 0.028f, 0.01f);
        cyl2->setColor(1.0f, 69.0f / 255.0f, 0);
        cyl2->setLocation(0, 0, 0.014);
        ret->insert(cyl2);
    }
    {
        CCylinder::Ptr cyl3 =
            std::make_shared<CCylinder>(0.028f, 0.028f, 0.01f);
        cyl3->setColor(0, 0, 0);
        cyl3->setLocation(0, 0, 0.024);
        ret->insert(cyl3);
    }

    return ret;
}

CSetOfObjects::Ptr stock_objects::Househam_Sprayer()
{
    CSetOfObjects::Ptr ret = std::make_shared<CSetOfObjects>();

    {
        CBox::Ptr cabin = std::make_shared<CBox>(
            TPoint3D(0.878, 0.723, -0.12), TPoint3D(-0.258, -0.723, -1.690));
        cabin->setColor(0.7f, 0.7f, 0.7f);
        ret->insert(cabin);
    }
    {
        CBox::Ptr back = std::make_shared<CBox>(
            TPoint3D(-0.258, 0.723, -0.72), TPoint3D(-5.938, -0.723, -1.690));
        back->setColor(1, 1, 1);
        ret->insert(back);
    }
    {
        CBox::Ptr boomAxis = std::make_shared<CBox>(
            TPoint3D(-5.938, 0.723, -1.0), TPoint3D(-6.189, -0.723, -1.690));
        boomAxis->setColor(0, 0, 0);
        ret->insert(boomAxis);
    }
    {
        CBox::Ptr boom1 = std::make_shared<CBox>(
            TPoint3D(-5.938, 0.723, -1.0), TPoint3D(-6.189, 11.277, -1.620));
        boom1->setColor(0, 1, 0);
        ret->insert(boom1);
    }
    {
        CBox::Ptr boom2 = std::make_shared<CBox>(
            TPoint3D(-5.938, -0.723, -1.0), TPoint3D(-6.189, -11.277, -1.620));
        boom2->setColor(0, 1, 0);
        ret->insert(boom2);
    }
    {
        CCylinder::Ptr cyl1 =
            std::make_shared<CCylinder>(0.716f, 0.716f, 0.387f, 30);
        cyl1->setColor(0, 0, 0);
        cyl1->setPose(CPose3D(-0.710, 0.923, -2.480, 0, 0, 90.0_deg));
        ret->insert(cyl1);
    }
    {
        CCylinder::Ptr cyl2 =
            std::make_shared<CCylinder>(0.716f, 0.716f, 0.387f, 30);
        cyl2->setColor(0, 0, 0);
        cyl2->setPose(CPose3D(-3.937, 0.923, -2.480, 0, 0, 90.0_deg));
        ret->insert(cyl2);
    }
    {
        CCylinder::Ptr cyl1 =
            std::make_shared<CCylinder>(0.716f, 0.716f, 0.387f, 30);
        cyl1->setColor(0, 0, 0);
        cyl1->setPose(CPose3D(-0.710, -0.423, -2.480, 0, 0, 90.0_deg));
        ret->insert(cyl1);
    }
    {
        CCylinder::Ptr cyl2 =
            std::make_shared<CCylinder>(0.716f, 0.716f, 0.387f, 30);
        cyl2->setColor(0, 0, 0);
        cyl2->setPose(CPose3D(-3.937, -0.423, -2.480, 0, 0, 90.0_deg));
        ret->insert(cyl2);
    }
    return ret;
}