GraphMap.cpp

Go to the documentation of this file.

// *****************************************************************
// Filename:    GraphMap.cpp
// Copyright:   Kai Welke, Chair Prof. Dillmann (IAIM),
//              Institute for Computer Science and Engineering (CSE),
//              University of Karlsruhe. All rights reserved.
// Author:      Kai Welke
// Date:        9.08.2007
// *****************************************************************
 
// *****************************************************************
// includes
// *****************************************************************
#include "GraphMap.h"
 
#include <list>
 
#include "Base/DataStructures/Graph/GraphProcessor.h"
#include "Base/Math/MathTools.h"
#include "Helpers/helpers.h"
 
using namespace std;
 
// *****************************************************************
// implementation of CGraphMap
// *****************************************************************
// construction / destruction
CGraphMap::CGraphMap(CIntensityGraph* pGraph, bool bUseLookup)
{
    m_pGraph = pGraph;
    m_nNumberNodes = pGraph->getNodes()->size();
 
    m_nWidth = (int)sqrt(float(m_nNumberNodes));
    m_nHeight = m_nNumberNodes / m_nWidth + 1;
 
    m_bUseLookup = bUseLookup;
 
    // init lookup table
    if (m_bUseLookup)
    {
        m_pLookupTable = new CGraphPyramidLookupTable(100, 100);
        m_pLookupTable->buildLookupTable(pGraph);
    }
 
    // create float matrix
    m_pMap = new CFloatMatrix(m_nWidth, m_nHeight);
    m_bOwnMemory = true;
    toMap();
}
 
CGraphMap::~CGraphMap()
{
    if (m_bOwnMemory)
    {
        delete m_pMap;
    }
 
    if (m_bUseLookup)
    {
        delete m_pLookupTable;
    }
}
 
void
CGraphMap::setMap(CFloatMatrix* pMap, bool bUpdateGraph)
{
    if (m_bOwnMemory)
    {
        delete m_pMap;
    }
 
    m_bOwnMemory = false;
 
    m_pMap = pMap;
    m_nWidth = pMap->columns;
    m_nHeight = pMap->rows;
 
    if (bUpdateGraph)
    {
        toGraph();
    }
}
 
CFloatMatrix*
CGraphMap::getMap()
{
    return m_pMap;
}
 
int
CGraphMap::getWidth()
{
    return m_nWidth;
}
 
int
CGraphMap::getHeight()
{
    return m_nHeight;
}
 
// manipulation
void
CGraphMap::applyGaussian(EOperation nOperation, Vec3d position, float fAmplitude, float fVariance)
{
    TSphereCoord coord;
    MathTools::convert(position, coord);
 
    applyGaussian(nOperation, coord, fAmplitude, fVariance);
}
 
void
CGraphMap::applyGaussian(EOperation nOperation,
                         TSphereCoord coord,
                         float fAmplitude,
                         float fVariance)
{
    int nNodeID = 0;
 
    if (m_bUseLookup)
    {
        nNodeID = m_pLookupTable->getClosestNode(coord);
    }
    else
    {
        nNodeID = GraphProcessor::findClosestNode(m_pGraph, coord);
    }
 
    applyGaussian(nOperation, nNodeID, fAmplitude, fVariance);
}
 
void
CGraphMap::applyGaussian(EOperation nOperation, int nNodeID, float fAmplitude, float fVariance)
{
    list<int> nodes;
    vector<int> accomplished;
 
    // store initial node
    int nFirstNode = nNodeID;
    TSphereCoord coord = m_pGraph->getNodes()->at(nFirstNode)->getPosition();
    nodes.push_back(nFirstNode);
 
    // eat nodelist
    TSphereCoord cur_sc;
    Vec3d ref, cur;
    MathTools::convert(coord, ref);
    float fIntensity;
 
    while (nodes.size() > 0)
    {
        // retrieve first node
        int nCurrentNode = nodes.front();
        nodes.pop_front();
        accomplished.push_back(nCurrentNode);
 
        // calculate angle
        cur_sc = m_pGraph->getNodes()->at(nCurrentNode)->getPosition();
        MathTools::convert(cur_sc, cur);
        float fAngle = Math3d::Angle(ref, cur);
 
        // check if maximum angle
        if (fAngle > 2.0 * fVariance)
        {
            continue;
        }
 
        // evaluate gaussian at angle
        fIntensity = evaluateGaussian(fAngle, fAmplitude, fVariance);
 
        // update graph and map
        switch (nOperation)
        {
            case eSet:
                m_pMap->data[nCurrentNode] = fIntensity;
                break;
 
            case eAdd:
                m_pMap->data[nCurrentNode] += fIntensity;
                break;
 
            case eMultiply:
                m_pMap->data[nCurrentNode] *= fIntensity;
                break;
 
            case eMax:
                if (m_pMap->data[nCurrentNode] < fIntensity)
                {
                    m_pMap->data[nCurrentNode] = fIntensity;
                }
 
                break;
 
            default:
                printf("Illegal operation\n");
                break;
        }
 
        // add neighbours to nodelist
        vector<int>* pAdjacency = m_pGraph->getNodeAdjacency(nCurrentNode);
 
        // check for double nodes
        int nAdjNode;
 
        for (int a = 0; a < int(pAdjacency->size()); a++)
        {
            nAdjNode = pAdjacency->at(a);
 
            // check if we already processed this node
            bool bNewNode = true;
 
            for (int c = 0; c < int(accomplished.size()); c++)
                if (accomplished.at(c) == nAdjNode)
                {
                    bNewNode = false;
                }
 
            list<int>::iterator iter = nodes.begin();
 
            while (iter != nodes.end())
            {
                if (*iter == nAdjNode)
                {
                    bNewNode = false;
                }
 
                iter++;
            }
 
 
            if (bNewNode)
            {
                nodes.push_back(nAdjNode);
            }
        }
    }
}
 
void
CGraphMap::updateGraph()
{
    toGraph();
}
 
void
CGraphMap::updateMap()
{
    toMap();
}
 
float
CGraphMap::evaluateGaussian(float fDist, float fAmplitude, float fVariance)
{
    return exp(-(fDist / fVariance * 2.0f)) * fAmplitude;
}
 
void
CGraphMap::toGraph()
{
    float fIntensity;
 
    for (int n = 0; n < m_nNumberNodes; n++)
    {
        fIntensity = m_pMap->data[n];
        ((CIntensityNode*)m_pGraph->getNodes()->at(n))->setIntensity(fIntensity);
    }
}
 
void
CGraphMap::toMap()
{
    float fIntensity;
 
    for (int n = 0; n < m_nNumberNodes; n++)
    {
        fIntensity = ((CIntensityNode*)m_pGraph->getNodes()->at(n))->getIntensity();
        m_pMap->data[n] = fIntensity;
    }
}