GraphLookupTable.cpp

Go to the documentation of this file.

// *****************************************************************
// Filename:    GraphLookupTable.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:        10.10.2008
// *****************************************************************
 
// *****************************************************************
// includes
// *****************************************************************
#include "GraphLookupTable.h"
#include "MathTools.h"
#include <cfloat>
 
// *****************************************************************
// implementation of CGraphLookupTable
// *****************************************************************
// construction / destruction
CGraphLookupTable::CGraphLookupTable(int nMaxZenithBins, int nMaxAzimuthBins)
{
    m_nMaxZenithBins = nMaxZenithBins;
    m_nMaxAzimuthBins = nMaxAzimuthBins;
 
    buildMemory();
}
 
CGraphLookupTable::~CGraphLookupTable()
{
    cleanMemory();
}
 
void CGraphLookupTable::buildLookupTable(CSphericalGraph* pGraph)
{
    reset();
 
    TNodeList* pNodes = pGraph->getNodes();
    int nSize = pNodes->size();
 
    for (int n = 0 ; n < nSize ; n++)
    {
        CSGNode* pNode = pNodes->at(n);
        addEntry(pNode->getPosition(), n);
    }
 
    m_pGraph = pGraph;
}
 
int CGraphLookupTable::getClosestNode(TSphereCoord position)
{
    bool bExact;
    return getClosestNode(position, bExact);
}
 
int CGraphLookupTable::getClosestNode(Eigen::Vector3d position)
{
    bool bExact;
    return getClosestNode(position, bExact);
}
 
int CGraphLookupTable::getClosestNode(Eigen::Vector3d position, bool& bExact)
{
    TSphereCoord coords;
    MathTools::convert(position, coords);
 
    return getClosestNode(coords, bExact);
}
 
 
int CGraphLookupTable::getClosestNode(TSphereCoord position, bool& bExact)
{
    int nIndex1, nIndex2;
    nIndex1 = getZenithBinIndex(position.fPhi);
    nIndex2 = getAzimuthBinIndex(position.fTheta, position.fPhi);
 
    std::list<int>::iterator iter = m_ppTable[nIndex1][nIndex2].begin();
 
    float fMinDistance = FLT_MAX;
    int nBestIndex = -1;
    float fDistance;
 
    while (iter != m_ppTable[nIndex1][nIndex2].end())
    {
        fDistance = MathTools::getDistanceOnArc(position, m_pGraph->getNode(*iter)->getPosition());
 
        if (fDistance < fMinDistance)
        {
            fMinDistance = fDistance;
            nBestIndex = *iter;
        }
 
        iter++;
    }
 
    // retrieve distance to bounding
    float fMinPhi, fMaxPhi, fMinTheta, fMaxTheta;
 
    float fTestValues[4];
    //  printf("Current position: phi %f, theta %f\n", position.fPhi, position.fTheta);
 
    getBinMinMaxValues(nIndex1, nIndex2, fMinPhi, fMaxPhi, fMinTheta, fMaxTheta);
    //  printf("B Phi: %f - %f\n", fMinPhi, fMaxPhi);
    //  printf("B Theta: %f - %f\n", fMinTheta, fMaxTheta);
 
    fTestValues[0] = std::fabs(fMinPhi - position.fPhi) * M_PI / 180.0f;
    fTestValues[1] = std::fabs(fMaxPhi - position.fPhi) * M_PI / 180.0f;
    fTestValues[2] = std::fabs(fMinTheta - position.fTheta) * M_PI / 180.0f;
    fTestValues[3] = std::fabs(fMaxTheta - position.fTheta) * M_PI / 180.0f;
    bExact = true;
 
    //  printf("AzimuthDistance (min,max,current): (%f,%f,%f)\n", fTestValues[2], fTestValues[3], fMinDistance);
    //  printf("ZenithDistance (min,max,current): (%f,%f,%f)\n", fTestValues[0], fTestValues[1], fMinDistance);
 
    for (float fTestValue : fTestValues)
    {
        if (fTestValue < fMinDistance)
        {
            bExact = false;
        }
    }
 
    return nBestIndex;
}
 
void CGraphLookupTable::reset()
{
    float fZenith;
 
    for (int z = 0 ; z < getNumberZenithBins() ; z++)
    {
        fZenith = 180.0f / m_nMaxZenithBins * z;
 
        for (int a = 0 ; a < getNumberAzimuthBins(fZenith) ; a++)
        {
            m_ppTable[z][a].clear();
        }
    }
}
 
// *****************************************************************
// private methods
// *****************************************************************
void CGraphLookupTable::addEntry(TSphereCoord position, int nIndex)
{
    int nIndex1, nIndex2;
    nIndex1 = getZenithBinIndex(position.fPhi);
    nIndex2 = getAzimuthBinIndex(position.fTheta, position.fPhi);
 
    m_ppTable[nIndex1][nIndex2].push_back(nIndex);
}
 
void CGraphLookupTable::buildMemory()
{
    float fZenith;
 
    m_ppTable = new std::list<int>* [m_nMaxZenithBins];
 
    for (int z = 0 ; z < m_nMaxZenithBins ; z++)
    {
        fZenith = 180.0f / m_nMaxZenithBins * z;
        m_ppTable[z] = new std::list<int>[getNumberAzimuthBins(fZenith)];
    }
}
 
void CGraphLookupTable::cleanMemory()
{
    for (int z = 0 ; z < m_nMaxZenithBins ; z++)
    {
        delete [] m_ppTable[z];
    }
 
    delete [] m_ppTable;
}
 
int CGraphLookupTable::getNumberZenithBins()
{
    return m_nMaxZenithBins;
}
 
int CGraphLookupTable::getNumberAzimuthBins(float fZenith)
{
    int nZenithBin = getZenithBinIndex(fZenith);
 
    // 0 degree means 1 min bins
    // 90 degree means max bins
    int nMinBins = 1;
    float fNumberBins;
 
    if (getNumberZenithBins() > 1)
    {
        fNumberBins = sin(float(nZenithBin) / (getNumberZenithBins() - 1)  * M_PI) * (m_nMaxAzimuthBins - nMinBins) + nMinBins;
    }
    else
    {
        fNumberBins = 0.5f;
    }
 
    return int(fNumberBins + 0.5f);
}
 
int CGraphLookupTable::getAzimuthBinIndex(float fAzimuth, float fZenith)
{
    float fBinIndex = fAzimuth / 360.0f * (getNumberAzimuthBins(fZenith) - 1) + 0.5f;
    return int(fBinIndex);
}
 
int CGraphLookupTable::getZenithBinIndex(float fZenith)
{
    float fBinIndex = fZenith / 180.0f * (m_nMaxZenithBins - 1) + 0.5f;
    return int(fBinIndex);
}
 
void CGraphLookupTable::getBinMinMaxValues(int nZenithBinIndex, int nAzimuthBinIndex, float& fMinZenith, float& fMaxZenith, float& fMinAzimuth, float& fMaxAzimuth)
{
    if ((m_nMaxZenithBins - 1) == 0)
    {
        fMinZenith = -FLT_MAX;
        fMaxZenith = FLT_MAX;
    }
    else
    {
        fMinZenith = (nZenithBinIndex - 0.5f) * 180.0f / (m_nMaxZenithBins - 1);
        fMaxZenith = (nZenithBinIndex + 0.5f) * 180.0f / (m_nMaxZenithBins - 1);
    }
 
    if ((getNumberAzimuthBins((fMinZenith + fMaxZenith) / 2.0f) - 1) == 0)
    {
        fMinAzimuth = -FLT_MAX;
        fMaxAzimuth = FLT_MAX;
    }
    else
    {
        fMinAzimuth = (nAzimuthBinIndex - 0.5f) * 360.0f / (getNumberAzimuthBins((fMinZenith + fMaxZenith) / 2.0f) - 1);
        fMaxAzimuth = (nAzimuthBinIndex + 0.5f) * 360.0f / (getNumberAzimuthBins((fMinZenith + fMaxZenith) / 2.0f) - 1);
    }
}