RobotViewer.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX 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 this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "RobotViewer.h"
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <Inventor/events/SoLocation2Event.h>
#include <Inventor/events/SoMouseButtonEvent.h>
#include <Inventor/nodes/SoDrawStyle.h>
#include <Inventor/nodes/SoLineSet.h>
#include <Inventor/nodes/SoMaterial.h>
#include <Inventor/nodes/SoPickStyle.h>
#include <Inventor/nodes/SoSelection.h>
#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/nodes/SoVertexProperty.h>
 
armarx::RobotViewer::RobotViewer(QWidget* widget) :
    SoQtExaminerViewer(widget),
    sceneRootNode(new SoSeparator),
    contentRootNode(new SoSelection),
    camera(new SoPerspectiveCamera)
{
    this->setBackgroundColor(SbColor(150 / 255.0f, 150 / 255.0f, 150 / 255.0f));
    this->setAccumulationBuffer(true);
    this->setHeadlight(true);
    this->setViewing(false);
    this->setDecoration(false);
#ifdef WIN32
#ifndef _DEBUG
    this->setAntialiasing(true, 4);
#endif
#endif
    this->setTransparencyType(SoGLRenderAction::SORTED_OBJECT_BLEND);
    this->setFeedbackVisibility(true);
 
    //Create scene root node
    sceneRootNode->ref();
    this->setSceneGraph(sceneRootNode);
 
    //Add camera to scene
    sceneRootNode->addChild(camera);
    this->setCamera(camera);
 
    //Give camera standard position
    camera->position.setValue(SbVec3f(10, -10, 5));
    camera->pointAt(SbVec3f(0, 0, 0), SbVec3f(0, 0, 1));
 
    //Add gridfloor root and make it unpickable
    SoSeparator* gridFloorRoot = new SoSeparator();
    SoPickStyle* unpickable = new SoPickStyle();
    unpickable->style = SoPickStyle::UNPICKABLE;
    SoPickStyle* pickable = new SoPickStyle();
    pickable->style = SoPickStyle::SHAPE;
 
    sceneRootNode->addChild(unpickable);
    sceneRootNode->addChild(gridFloorRoot);
    sceneRootNode->addChild(pickable);
 
    /*/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    Here we draw our grid floor. To change the way the grid floor is drawn change the following constants.
    GRIDSIZE:       How many lines to display for each direction of each axis.
                    E. g.: 2 would lead to 2 lines in negative and positive direction respectively, plus indicator for axis.
                    5 lines in sum.
    GRIDUNIT:       Factor for coin units, e.g. 2 means between 2 lines there is space for 2 coin units.
    GRIDSUBSIZE:    How many subunits to display between the larger lines. So 5 would lead to 3 lines between 2 large lines for 5
                    subunits.
    GRIDPATTERN:    Pattern for sublines. Bitpattern, where 1 is line and 0 is no line.
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////*/
    const int GRIDSIZE = 3;
    const int GRIDUNIT = 15;
    const int GRIDSUBSIZE = 10;
    const unsigned short GRIDPATTERN = 0xCCCC;
 
    //Red material for X axis
    SoMaterial* red = new SoMaterial;
    red->diffuseColor = SbColor(1, 0, 0);
    //Green material for Y axis
    SoMaterial* green = new SoMaterial;
    green->diffuseColor = SbColor(0, 1, 0);
    //Blue material for Z axis
    SoMaterial* blue = new SoMaterial;
    blue->diffuseColor = SbColor(0, 0, 1);
 
    //Small lines
    SoDrawStyle* thinStyle = new SoDrawStyle;
    thinStyle->lineWidth = 1;
    thinStyle->linePattern = GRIDPATTERN;
    //Larger lines
    SoDrawStyle* largeStyle = new SoDrawStyle;
    largeStyle->lineWidth = 2;
 
    //Our set of vertices for the grid lines
    SoVertexProperty* vertices = new SoVertexProperty;
    SoVertexProperty* subVertices = new SoVertexProperty;
    SoVertexProperty* xVertices = new SoVertexProperty;
    SoVertexProperty* yVertices = new SoVertexProperty;
    SoVertexProperty* zVertices = new SoVertexProperty;
 
    //Definition of which vertex belongs to which line and should be connected
    SoLineSet* lines = new SoLineSet;
    SoLineSet* subLines = new SoLineSet;
    SoLineSet* xLines = new SoLineSet;
    SoLineSet* yLines = new SoLineSet;
    SoLineSet* zLines = new SoLineSet;
 
    //Add 2 vertices for X axis
    xVertices->vertex.set1Value(0, GRIDSIZE * GRIDUNIT, 0, 0);
    xVertices->vertex.set1Value(1, -(GRIDSIZE * GRIDUNIT), 0, 0);
    //Connect them to a line by adding '2' to numVertices at the correct position
    xLines->numVertices.set1Value(0, 2);
 
    //Y axis
    yVertices->vertex.set1Value(0, 0, GRIDSIZE * GRIDUNIT, 0);
    yVertices->vertex.set1Value(1, 0, -(GRIDSIZE * GRIDUNIT), 0);
    yLines->numVertices.set1Value(0, 2);
 
    //Z axis
    zVertices->vertex.set1Value(0, 0, 0, GRIDSIZE * GRIDUNIT);
    zVertices->vertex.set1Value(1, 0, 0, -(GRIDSIZE * GRIDUNIT));
    zLines->numVertices.set1Value(0, 2);
 
    //Counters to keep track of vertex index
    int verticescounter = 0;
    int subverticescounter = 0;
 
    //Draw all lines parallel to the X and Y axis and all sublines, excepted axis itself
    for (int i = -(GRIDSIZE * GRIDUNIT); i < GRIDUNIT * (GRIDSIZE + 1); i += GRIDUNIT)
    {
        if (i != 0)
        {
            vertices->vertex.set1Value(verticescounter++, GRIDSIZE * GRIDUNIT, i, 0);
            vertices->vertex.set1Value(verticescounter++, -(GRIDSIZE * GRIDUNIT), i, 0);
            lines->numVertices.set1Value((verticescounter - 1) / 2, 2);
 
            vertices->vertex.set1Value(verticescounter++, i, GRIDSIZE * GRIDUNIT, 0);
            vertices->vertex.set1Value(verticescounter++, i, -(GRIDSIZE * GRIDUNIT), 0);
            lines->numVertices.set1Value((verticescounter - 1) / 2, 2);
        }
 
        //If this is not the last line to draw, draw sublines
        if (i < GRIDUNIT * GRIDSIZE)
        {
            float delta = (float)GRIDUNIT / (float)GRIDSUBSIZE;
 
            for (int n = 1; n < GRIDSUBSIZE; n++)
            {
                subVertices->vertex.set1Value(
                    subverticescounter++, GRIDSIZE * GRIDUNIT, (float)i + (float)n * delta, 0);
                subVertices->vertex.set1Value(
                    subverticescounter++, -(GRIDSIZE * GRIDUNIT), (float)i + (float)n * delta, 0);
                subLines->numVertices.set1Value((subverticescounter - 1) / 2, 2);
 
                subVertices->vertex.set1Value(
                    subverticescounter++, (float)i + (float)n * delta, GRIDSIZE * GRIDUNIT, 0);
                subVertices->vertex.set1Value(
                    subverticescounter++, (float)i + (float)n * delta, -(GRIDSIZE * GRIDUNIT), 0);
                subLines->numVertices.set1Value((subverticescounter - 1) / 2, 2);
            }
        }
    }
 
 
    lines->vertexProperty.setValue(vertices);
    subLines->vertexProperty.setValue(subVertices);
    xLines->vertexProperty.setValue(xVertices);
    yLines->vertexProperty.setValue(yVertices);
    zLines->vertexProperty.setValue(zVertices);
 
    gridFloorRoot->addChild(thinStyle);
    gridFloorRoot->addChild(subLines);
    gridFloorRoot->addChild(largeStyle);
    gridFloorRoot->addChild(lines);
    gridFloorRoot->addChild(red);
    gridFloorRoot->addChild(xLines);
    gridFloorRoot->addChild(green);
    gridFloorRoot->addChild(yLines);
    gridFloorRoot->addChild(blue);
    gridFloorRoot->addChild(zLines);
 
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////
    //Gridfloor done
 
    //Create content root node
    sceneRootNode->addChild(contentRootNode);
}
 
armarx::RobotViewer::~RobotViewer()
{
    sceneRootNode->unref();
}
 
SoSelection*
armarx::RobotViewer::getRootNode()
{
    return this->contentRootNode;
}
 
void
armarx::RobotViewer::cameraViewAll()
{
    camera->viewAll(this->contentRootNode, SbViewportRegion());
}
 
//Override the default navigation behaviour of the SoQtExaminerViewer
SbBool
armarx::RobotViewer::processSoEvent(const SoEvent* const event)
{
    const SoType type(event->getTypeId());
 
    //Remapping mouse press events
    if (type.isDerivedFrom(SoMouseButtonEvent::getClassTypeId()))
    {
        SoMouseButtonEvent* const ev = (SoMouseButtonEvent*)event;
        const int button = ev->getButton();
        const SbBool press = ev->getState() == SoButtonEvent::DOWN ? TRUE : FALSE;
 
        //LEFT MOUSE BUTTON
        if (button == SoMouseButtonEvent::BUTTON1)
        {
            SoQtExaminerViewer::processSoEvent(ev);
        }
 
        //MIDDLE MOUSE BUTTON
        if (button == SoMouseButtonEvent::BUTTON3)
        {
            /*Middle mouse button now is used for all changes in camera perspective.
            To also display the cool little rotation cursor, viewing mode is set to on
            while button is down*/
 
            //Enable or disable viewing mode while BUTTON3 pressed
            if (press)
            {
                if (!this->isViewing())
                {
                    this->setViewing(true);
                }
            }
            else
            {
                if (this->isViewing())
                {
                    this->setViewing(false);
                }
            }
 
            //Remap BUTTON3 to BUTTON1
            ev->setButton(SoMouseButtonEvent::BUTTON1);
 
            //Make sure this REALLY only can happen when viewing mode is on.
            //Zooming can also turn it on and it leads to weird effects when doing both
            //(deselections in scene etc.) because SoQtExaminerViewer passes BUTTON1
            //events up to scenegraph when not in viewing mode.
            if (this->isViewing())
            {
                //ARMARX_WARNING << "PROCESS";
                return SoQtExaminerViewer::processSoEvent(ev);
                //ARMARX_WARNING << "PROCESS END";
            }
        }
 
        //MOUSE WHEEL UP AND DOWN
        if ((button == SoMouseButtonEvent::BUTTON4 || button == SoMouseButtonEvent::BUTTON5))
        {
            /*Zooming is allowed, so just use it. We have to temporarily turn viewing mode
            on to make SoQtExaminerViewer allow zooming.*/
 
            //Swap BUTTON4 and BUTTON5 because zooming out while scrolling up is just retarded
            ev->setButton(button == SoMouseButtonEvent::BUTTON4 ? SoMouseButtonEvent::BUTTON5
                                                                : SoMouseButtonEvent::BUTTON4);
 
            //Zooming is allowed, so just pass it and temporarily set viewing mode on, if it is not already
            //(otherwise coin gives us warning messages...)
            if (!this->isViewing())
            {
                if (!this->isViewing())
                {
                    this->setViewing(true);
                }
 
                SoQtExaminerViewer::processSoEvent(ev);
 
                if (this->isViewing())
                {
                    this->setViewing(false);
                }
            }
            else
            {
                SoQtExaminerViewer::processSoEvent(ev);
            }
 
            return TRUE;
        }
    }
 
    // Keyboard handling
    if (type.isDerivedFrom(SoKeyboardEvent::getClassTypeId()))
    {
        const SoKeyboardEvent* const ev = (const SoKeyboardEvent*)event;
 
        /*The escape key and super key (windows key) is used to switch between
        viewing modes. We need to disable this behaviour completely.*/
 
        //65513 seems to be the super key, which is not available in the enum of keys in coin....
        if (ev->getKey() == SoKeyboardEvent::ESCAPE || ev->getKey() == 65513 ||
            ev->getKey() == SoKeyboardEvent::LEFT_CONTROL)
        {
            return TRUE;
        }
        /*else if (ev->getKey() == SoKeyboardEvent::S && ev->getState() == SoButtonEvent::DOWN)
        {
            if (!this->isSeekMode())
            {
                if (!this->isViewing())
                {
                    this->setViewing(true);
                }
 
                SoQtExaminerViewer::processSoEvent(ev);
                this->setSeekTime(0.5);
                this->seekToPoint(ev->getPosition());
 
                if (this->isViewing())
                {
                    this->setViewing(false);
                }
            }
        }*/
        else
        {
            SoQtExaminerViewer::processSoEvent(ev);
        }
 
        SoQtExaminerViewer::processSoEvent(ev);
    }
 
    //Let all move events trough
    if (type.isDerivedFrom(SoLocation2Event::getClassTypeId()))
    {
        //ARMARX_WARNING << "EVENT START";
        return SoQtExaminerViewer::processSoEvent(event);
        //ARMARX_WARNING << "EVENT END";
    }
    //ARMARX_WARNING << "YOU SHALL NOT PASS!";
    //YOU SHALL NOT PASS!
    return TRUE;
 
    return SoQtExaminerViewer::processSoEvent(event);
}