OIFwdKinematicsInterface.cpp

Go to the documentation of this file.

/*
 *  OIFwdKinematicsInterface.cpp
 *
 *
 *  Created by A. Bierbaumm on 28.03.08.
 *  Copyright 2008 IAIM Universitaet Karlsruhe.
 *  All rights reserved.
 *
 */
 
/** \file OIFwdKinematicsInterface.cpp
* The source file for OIFwdKinematicsInterface.
*/
 
/** \class OIFwdKinematicsInterface
* Defines the OIFwdKinematicsInterface class.
* To use this class you can either create a direct instance of this class
* or you can create a subclass and override the OIFwdKinematicsInterface::buildScenery()
* method.\n
* If you create a direct instance of the OIFwdKinematicsInterface your code usually would
* look like this:
\code
 
#include "OIFwdKinematicsInterface.h"
 
int main (int argc, char ** argv)
{
    OIFwdKinematicsInterface viewer(argc, argv, "FileToOpen.iv");
    // create an instance of the OIFwdKinematicsInterface
    viewer.run(); // start the visualisation/simulation
    return 0;
}
 
\endcode
*
* If you create a subclass of OIFwdKinematicsInterface you need to override the constructor
* and the OIFwdKinematicsInterface::buildScenery() method. If you create custom variables
* in the constructor or in OIFwdKinematicsInterface::buildScenery() you also need to create
* a destructor and free the memory of those variables.\n
* A short example:
\code
 
#include <iostream>
#include <ipsaclasses.h>
#include "OIFwdKinematicsInterface.h"
 
class MyOIFwdKinematicsInterface : public OIFwdKinematicsInterface
{
public:
    MyOIFwdKinematicsInterface(int argc, char ** argv, std::string filename = "")
    : MyOIFwdKinematicsInterface(argc, argv, filename) {}
protected:
    virtual bool buildScenery();
};
 
bool MyOIFwdKinematicsInterface::buildScenery()
{
    // omit this line if you do not want to read a scenegraph from a file
    if ( true == this->readSceneryFromFile() )
        return true;
    // create your own scenegraph
    root = new SoSeparator;
    // the remaining code
}
 
int
main (int argc, char ** argv)
{
    std::string sceneFilename = "";
    if ( 2 <= argc )
    {
        // this can be omitted if you do not want to read a file
        // the third parameter can also be left out in this case
        sceneFilename = argv[1];
        std::cout << "\n\nOpen file: " << sceneFilename << std::endl << std::endl;
    }
    MyOIFwdKinematicsInterface viewer(argc, argv, sceneFilename);
    viewer.run();
    return 0;
}
 
\endcode
*/
 
/** \var SoSeparator OIFwdKinematicsInterface::root
* This is the root node of the scenegraph which gets rendered.\n
* You can create a new node in the OIFwdKinematicsInterface::buildScenery() method
* because it gets referenced and unreferenced in the OIFwdKinematicsInterface::run()
* method.
*/
 
/** \var SO_WINDOW OIFwdKinematicsInterface::window
* Depending on the platfrom this is either a HWND on Windows or
* a QWidget on Linux/Mac OS X.
*/
 
/** \var SO_EXAMINER_VIEWER OIFwdKinematicsInterface::viewer
* Depending on the platfrom this is either an SoWinExaminerViewer on Windows or
* an SoQtExaminerViewer on Linux/Mac OS X.
*/
 
/** \var std::string OIFwdKinematicsInterface::sceneryFilename
* This is the filename of the inventor scenegraph which can be loaded
* by the OIFwdKinematicsInterface::readSceneryFromFile() method.
*/
 
#include "OIFwdKinematicsInterface.h"
 
#include <stdlib.h>
#include <iostream>
 
#include <Inventor/SoDB.h>
#include <Inventor/SoInput.h>
#include <Inventor/SoFullPath.h>
#include <Inventor/nodes/SoFile.h>
#include <Inventor/nodes/SoGroup.h>
#include <Inventor/nodes/SoNode.h>
#include <Inventor/nodes/SoTransform.h>
#include <Inventor/actions/SoSearchAction.h>
#include <Inventor/actions/SoGetMatrixAction.h>
 
#include <qwidget.h>
 
#include "../HandLocalisationConstants.h"
 
 
 
namespace visionx
{
    /**
    * The constructor of the OIFwdKinematicsInterface class.\n
    * It initializes OIFwdKinematicsInterface::root with NULL,
    * the OIFwdKinematicsInterface::sceneryFilename with the \param filename
    * and initializes the OIFwdKinematicsInterface::window variable.
    *
    * \param int argc the argument count passed into the main method
    * \param char **argv the argument array passed into the main method
    * \param std::string filename the file getting opened by OIFwdKinematicsInterface::readSceneryFromFile(). Default value is an empty string.
    */
    OIFwdKinematicsInterface::OIFwdKinematicsInterface(std::string filename /* = "" */)
        : root(NULL), argc(0), args(NULL), iQApplication(NULL), sceneryFilename(filename), window(NULL), viewer(NULL), iReady(false)
    {
 
    }
 
    /**
    * The virtual destructor of the OIFwdKinematicsInterface class.
    */
    OIFwdKinematicsInterface::~OIFwdKinematicsInterface()
    {
        if (NULL != viewer)
        {
            delete viewer;
        }
        if (NULL != window)
        {
            delete window;
        }
        if (NULL != iQApplication)
        {
            delete iQApplication;
        }
    }
 
    void OIFwdKinematicsInterface::update(void)
    {
        if (iReady)
        {
            //      assert(iQApplication != NULL);
            //      #ifdef QT_THREAD_SUPPORT
            //      iQApplication->lock();
            //      #endif
            //      iQApplication->processEvents();
            //      viewer->render();
            //      #ifdef QT_THREAD_SUPPORT
            //      iQApplication->unlock();
            //      #endif
 
            //*** Collision detection
#ifdef HAVE_COLLISION_DET
            //ida->apply(root);
#endif
 
            // meins
            m_pOffscreenRenderer->render(m_pDavidsRoot);
 
        }
    }
 
    void OIFwdKinematicsInterface::initviewer()
    {
        //  iQApplication = new QApplication(argc,args);
        //  window = new QWidget();
        //  SoQt::init(window);
 
        // meins
        SoDB::init();
        m_pViewportRegion = new SbViewportRegion();
        m_pViewportRegion->setWindowSize(DSHT_OI_RENDERSIZE_X, DSHT_OI_RENDERSIZE_Y);   //760, 570
        //m_pViewportRegion->setPixelsPerInch(25.4);    // =  1 px/mm
        m_pCamera = new SoPerspectiveCamera;
        m_pLight = new SoDirectionalLight;
        m_pDavidsRoot = new SoSeparator;
        m_pCamera->position.setValue(0, 0, 0);
        m_pCamera->orientation.setValue(SbRotation(SbVec3f(1, 0, 0), M_PI));
        float alpha = 2 * atan(0.5 * DSHT_OI_RENDERSIZE_Y / m_fFocalLengthY);
        m_pCamera->heightAngle.setValue(alpha); //640x480 -> 0.85047002;    alpha = 2 * atan(240/focallength)
        //m_pCamera->aspectRatio.setValue(1.00409544);
        m_pCamera->farDistance.setValue(2000);
        m_pLight->direction.setValue(0.01, 0.01, 1.0);
        //m_pLight->on.setValue(false);
 
#ifdef HAVE_COLLISION_DET
        ida = new SoIntersectionDetectionAction();
        ida->addIntersectionCallback(intersectionCB, NULL);
        ida->setIntersectionDetectionEpsilon(10.0f);
        SoInteraction::init();
#endif
 
        iReady = true;
 
        if (false == buildScenery())
        {
            return;
        }
        root->ref();
 
        // meins
        m_pDavidsRoot->addChild(m_pCamera);
        m_pDavidsRoot->addChild(m_pLight);
        m_pDavidsRoot->addChild(root);
        m_pDavidsRoot->ref();
        m_pOffscreenRenderer = new SoOffscreenRenderer(*m_pViewportRegion);
        m_pOffscreenRenderer->setBackgroundColor(*(new SbColor(255, 0, 0)));
        printf("\nbefore m_pOffscreenRenderer->render()\n\n");
        bool bResult = m_pOffscreenRenderer->render(m_pDavidsRoot);
        printf("\nafter m_pOffscreenRenderer->render() which returned %d\n\n", bResult);
 
 
 
#ifdef HAVE_COLLISION_DET
        ida = new SoIntersectionDetectionAction();
        ida->addIntersectionCallback(intersectionCB, NULL);
        ida->setIntersectionDetectionEpsilon(10.0f);
#endif
 
        /*   viewer = new SoQtExaminerViewer(window);
           viewer->setSceneGraph(root);
           viewer->setTitle("ForwardKinematicsViewer");
           viewer->viewAll();
           viewer->setFeedbackVisibility(true);
           viewer->setAutoRedraw(false);
           viewer->show();
           window->show();
        */
    }
 
#ifdef HAVE_COLLISION_DET
    SoIntersectionDetectionAction::Resp OIFwdKinematicsInterface::intersectionCB(void* closure,
            const SoIntersectingPrimitive* pr1,
            const SoIntersectingPrimitive* pr2)
    {
        SbName n1, n2;
        int l = pr1->path->getLength();
 
        for (int i = 0; i < l; i++)
        {
            n1 = pr1->path->getNodeFromTail(i)->getName();
            if (n1.getLength() > 0)
            {
                break;
            }
        }
        l = pr2->path->getLength();
        for (int i = 0; i < l; i++)
        {
            n2 = pr2->path->getNodeFromTail(i)->getName();
            if (n2.getLength() > 0)
            {
                break;
            }
        }
        if ((n1.getLength() > 0) || (n2.getLength() > 0))
        {
            printf("intersection hit: [%s], [%s]\n", n1.getString(), n2.getString());
        }
 
        return SoIntersectionDetectionAction::NEXT_PRIMITIVE;
    }
#endif
 
    bool OIFwdKinematicsInterface::getPath(std::string name, SoPath*& p, SoNode* r)
    {
        if (NULL == r)
        {
            r = root;
        }
        p = NULL;
 
        SoNode* lnode = SoNode::getByName(name.c_str());
        if (NULL == lnode)
        {
            return false;
        }
 
        SoSearchAction* searchaction = new SoSearchAction;
        searchaction->setNode(lnode);
        searchaction->apply(r);
 
        p = searchaction->getPath();
        if (NULL == p)
        {
            return false;
        }
 
        return true;
    }
 
    bool OIFwdKinematicsInterface::getTranslation(SbVec3f& translation, SoPath* p)
    {
        if (NULL == p)
        {
            std::cerr <<  "OI: getTranslation path is NULL! " << std::endl;
            return false;
        }
 
        // Apply the SoGetMatrixAction to get the full transformation
        // matrix from world space.
 
        //SoGetMatrixAction * getmatrixaction = new SoGetMatrixAction(viewer->getViewportRegion());
        SoGetMatrixAction* getmatrixaction = new SoGetMatrixAction(*m_pViewportRegion);
        getmatrixaction->apply(p);
 
        SbMatrix transformation = getmatrixaction->getMatrix();
 
        // And if you want to access the individual transformation
        // components of the matrix:
 
        //SbVec3f translation;
        SbRotation rotation;
        SbVec3f scalevector;
        SbRotation scaleorientation;
 
        transformation.getTransform(translation, rotation, scalevector, scaleorientation);
 
        return true;
    }
 
    bool OIFwdKinematicsInterface::setJointAngle(SoPath* p, float angle)
    {
        if (NULL == p)
        {
            std::cerr <<  "OI: setJointAngle path is NULL! " << std::endl;
            return false;
        }
 
        SoNode* n0 = p->getNodeFromTail(0);
        if (! n0->isOfType(SoTransform::getClassTypeId()))
        {
            printf("setJointAngle: node type is [%s] instead Transform!\n", n0->getClassTypeId().getName().getString());
            return false;
        }
        SoTransform* t = static_cast<SoTransform*>(n0);
        SbVec3f axis;
        float a0;
 
        //*** Keep angle in interval [-2Pi,2Pi] (required for coin!)
        if (abs(angle) > 2 * M_PI)
        {
            angle = angle - (floor(angle / (2 * M_PI)) * 2 * M_PI);
        }
        //*** Keep angle positive (required for coin!)
        if (angle <= 0.0f)                           // This is mandatory: Coin uses quaternions internally and can thus not
        {
            angle = 2 * M_PI + angle;    // reflect angles of 0deg without change of axis. Therefore set to 2Pi instead.
        }
 
        t->rotation.getValue(axis, a0);
        t->rotation.setValue(axis, angle);
        //t->rotation.getValue(axis,a0);             // just for control in debugger,if all is fine...
 
        return true;
    }
 
    /**
    * The virtual method OIFwdKinematicsInterface::buildScenery() is used to create
    * a scenegraph.\n
    * If you want to create your own scenegraph just override this method.
    * Otherwise it reads the scenegraph stored in the file OIFwdKinematicsInterface::sceneryFilename.
    * \return true if everything went well.
    * \return false if no scenery could be created.
    */
    bool OIFwdKinematicsInterface::buildScenery()
    {
        return this->readSceneryFromFile();
    }
 
    /**
    * This method tries to read the scenegraph from the file OIFwdKinematicsInterface::sceneryFilename
    * and stores it in OIFwdKinematicsInterface::root.
    *
    * \return true if everything went well and the scenegraph could be read.
    * \return false if the file couldn't be opened or a problem was encountered while reading the file.
    */
    bool OIFwdKinematicsInterface::readSceneryFromFile()
    {
        if (this->sceneryFilename.empty())
        {
            return false;
        }
 
        SoInput mySceneInput;
        if (!mySceneInput.openFile(this->sceneryFilename.c_str()))
        {
            std::cerr <<  "OI: Cannot open file " << this->sceneryFilename << std::endl;
            return false;
        }
 
 
        // Read the whole file into the database
        root = SoDB::readAll(&mySceneInput);
 
 
        if (NULL == root)
        {
            std::cerr <<  "OI: Problem reading file " << this->sceneryFilename << std::endl;
            return false;
        }
        mySceneInput.closeFile();
        expandFileNodes();
        return true;
    }
 
 
    //
    // This routine searches for and expands all SoFile nodes in the
    // given scene graph.  It does this by making all the children of a
    // SoFile node the children of its parent.
    // (Code for this function taken from ivcat)
    //
    void OIFwdKinematicsInterface::expandFileNodes()
    {
        //
        // Special case: if root is a file node, replace it with a group.
        //
        if (root->isOfType(SoFile::getClassTypeId()))
        {
            SoFile* f = (SoFile*)root;
            SoGroup* g = f->copyChildren();
            root->unref();
            root = dynamic_cast<SoSeparator*>(g);
            root->ref();
        }
 
        // Search for all file nodes
        SoSearchAction sa;
        sa.setType(SoFile::getClassTypeId());
        sa.setInterest(SoSearchAction::FIRST);
        sa.setSearchingAll(TRUE);
 
        root->ref(); // why do I need to ref here ?
        sa.apply(root);
 
        // We'll keep on searching until there are no more file nodes
        // left.  We don't search for all file nodes at once, because we
        // need to modify the scene graph, and so the paths returned may
        // be truncated (if there are several file nodes under a group, if
        // there are files within files, etc).  Dealing properly with that
        // is complicated-- it is easier (but slower) to just reapply the
        // search until it fails.
        // We need an SoFullPath here because we're searching node kit
        // contents.
        SoFullPath* p = (SoFullPath*) sa.getPath();
        while (p != NULL)
        {
            SoGroup* parent = (SoGroup*)p->getNodeFromTail(1);
            assert(parent != NULL);
 
            SoFile* file = (SoFile*)p->getTail();
 
            // If the filename includes a directory path, add the directory name
            // to the list of directories where to look for input files
            SbString filename = file->name.getValue();
 
            std::cout <<  "OI: expanding file " << filename.getString() << std::endl;
            //printf("OI: expanding file %s\n",filename.getString());
 
            int slashIndex = filename.find("/");
            if (-1 != slashIndex)
            {
                SbString searchPath = filename.getSubString(0, slashIndex - 1);
                SoInput::addDirectoryFirst(searchPath.getString());
            }
 
            int fileIndex = p->getIndexFromTail(0);
            assert(fileIndex != -1);
 
            // Now, add group of all children to file's parent's list of children,
            // right after the file node:
            SoGroup* fileGroup = file->copyChildren();
            fileGroup->ref();
            if (fileGroup != NULL)
            {
                parent->insertChild(fileGroup, fileIndex + 1);
            }
            else
            {
                // So we can at least see where the file node contents were
                // supposed to go.
                parent->insertChild(new SoGroup, fileIndex + 1);
            }
 
 
            // And expand the child node from the group.
            // Note that if the File node is multiply instanced,
            // the groups will not be instanced, but the children of the
            // groups will be.
            parent->removeChild(fileIndex);
 
            sa.apply(root);
            p = (SoFullPath*) sa.getPath();
        }
    }
 
    bool OIFwdKinematicsInterface::isInitialized()
    {
        return iReady;
    }
}