Grid.h

Go to the documentation of this file.

/*
 * Copyright (C) 2006-2011, SRI International (R)
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program 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 Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
 
#pragma once
 
#ifndef __OpenKarto_Grid_h__
#define __OpenKarto_Grid_h__
 
#include <OpenKarto/Object.h>
#include <OpenKarto/CoordinateConverter.h>
 
namespace karto
{
 
 
 
 
  class KARTO_EXPORT Functor
  {
  public:
    virtual void operator() (kt_int32u) {};
  }; // Functor
 
 
  template<typename T>
  class Grid : public Object
  {
    KARTO_RTTI();
 
  public:
    static Grid* CreateGrid(kt_int32s width, kt_int32s height, kt_double resolution)
    {
      Grid* pGrid = new Grid(width, height);
      
      pGrid->GetCoordinateConverter()->SetScale(1.0 / resolution);
      
      return pGrid;
    }
    
  protected:
    //@cond EXCLUDE
    virtual ~Grid()
    {
      delete [] m_pData;
      delete m_pCoordinateConverter;
    }
    //@endcond
 
  public:
    void Clear()
    {
      memset(m_pData, 0, GetDataSize() * sizeof(T));
    }
    
    Grid* Clone()
    {
      Grid* pGrid = CreateGrid(GetWidth(), GetHeight(), GetResolution());
      pGrid->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
      
      memcpy(pGrid->GetDataPointer(), GetDataPointer(), GetDataSize());
      
      return pGrid;
    }
    
    virtual void Resize(kt_int32s width, kt_int32s height)
    {
      m_Width = width;
      m_Height = height;
      m_WidthStep = math::AlignValue<kt_int32s>(width, 8);
      
      if (m_pData != NULL)
      {
        delete[] m_pData;
        m_pData = NULL;
      }
 
      try
      {
        m_pData = new T[GetDataSize()];
 
        if (m_pCoordinateConverter == NULL)
        {
          m_pCoordinateConverter = new CoordinateConverter();
        }
 
        m_pCoordinateConverter->SetSize(Size2<kt_int32s>(width, height));
      }
      catch (...)
      {
        m_pData = NULL;
 
        m_Width = 0;
        m_Height = 0;
        m_WidthStep = 0;
      }
      
      Clear();
    }
    
    inline kt_bool IsValidGridIndex(const Vector2i& rGrid) const
    {
      return (math::IsUpTo(rGrid.GetX(), GetWidth()) && math::IsUpTo(rGrid.GetY(), GetHeight()));
    }
    
    virtual kt_int32s GridIndex(const Vector2i& rGrid, kt_bool boundaryCheck = true) const
    {
      if (boundaryCheck == true)
      { 
        if (IsValidGridIndex(rGrid) == false)
        {
          StringBuilder errorMessage;
          errorMessage << "Index (" << rGrid.GetX() << ", " << rGrid.GetY() << ")" << " out of range.  Index must be between [0; " << GetWidth() << ") and [0; " << GetHeight() << ")";
          throw Exception(errorMessage.ToString());          
        }
      }
 
      kt_int32s index = rGrid.GetX() + (rGrid.GetY() * m_WidthStep);
      
      if (boundaryCheck == true)
      {
        assert(math::IsUpTo(index, GetDataSize()));
      }
      
      return index;
    }
 
    Vector2i IndexToGrid(kt_int32s index) const
    {
      Vector2i grid;
 
      grid.SetY(index / m_WidthStep);
      grid.SetX(index - grid.GetY() * m_WidthStep);
 
      return grid;
    }
    
    inline Vector2i WorldToGrid(const Vector2d& rWorld, kt_bool flipY = false) const
    {
      return GetCoordinateConverter()->WorldToGrid(rWorld, flipY);
    }
    
    inline Vector2d GridToWorld(const Vector2i& rGrid, kt_bool flipY = false) const
    {      
      return GetCoordinateConverter()->GridToWorld(rGrid, flipY);
    }    
 
    T* GetDataPointer(const Vector2i& rGrid)
    {
      kt_int32s index = GridIndex(rGrid, true);      
      return m_pData + index;
    }
 
    T* GetDataPointer(const Vector2i& rGrid) const
    {
      kt_int32s index = GridIndex(rGrid, true);      
      return m_pData + index;
    }
 
    inline kt_int32s GetWidth() const
    {
      return m_Width;
    };
 
    inline kt_int32s GetHeight() const
    {
      return m_Height;
    };
 
    inline const Size2<kt_int32s> GetSize() const
    {
      return Size2<kt_int32s>(GetWidth(), GetHeight());
    }
 
    inline kt_int32s GetWidthStep() const
    {
      return m_WidthStep;
    }
 
    inline T* GetDataPointer()
    {
      return m_pData;
    }
 
    inline T* GetDataPointer() const
    {
      return m_pData;
    }
 
    inline kt_int32s GetDataSize() const
    {
      return m_WidthStep * GetHeight();
    }
 
    inline T GetValue(const Vector2i& rGrid) const
    {
      kt_int32s index = GridIndex(rGrid);
      return m_pData[index];
    }
    
    inline void SetValue(const Vector2i& rGrid, T rValue) const
    {
      kt_int32s index = GridIndex(rGrid);
      m_pData[index] = rValue;
    }
 
    inline CoordinateConverter* GetCoordinateConverter() const
    {
      return m_pCoordinateConverter;
    }
 
    inline kt_double GetResolution() const
    {
      return GetCoordinateConverter()->GetResolution();
    }
    
    inline BoundingBox2 GetBoundingBox() const
    {
      return GetCoordinateConverter()->GetBoundingBox();
    }
    
    void TraceLine(kt_int32s x0, kt_int32s y0, kt_int32s x1, kt_int32s y1, Functor* f = NULL)
    {
      kt_bool steep = abs(y1 - y0) > abs(x1 - x0);
      if (steep)
      {
        math::Swap(x0, y0);
        math::Swap(x1, y1);
      }
      if (x0 > x1)
      {
        math::Swap(x0, x1);
        math::Swap(y0, y1);
      }
 
      kt_int32s deltaX = x1 - x0;
      kt_int32s deltaY = abs(y1 - y0);
      kt_int32s error = 0;
      kt_int32s ystep;
      kt_int32s y = y0;
 
      if (y0 < y1)
      {
        ystep = 1;
      }
      else
      {
        ystep = -1;
      }
 
      kt_int32s pointX;
      kt_int32s pointY;
      for (kt_int32s x = x0; x <= x1; x++)
      {
        if (steep)
        {
          pointX = y;
          pointY = x;
        }
        else
        {
          pointX = x;
          pointY = y;
        }
 
        error += deltaY;
 
        if (2 * error >= deltaX)
        {
          y += ystep;
          error -= deltaX;
        }
 
        Vector2i gridIndex(pointX, pointY);
        if (IsValidGridIndex(gridIndex))
        {
          kt_int32s index = GridIndex(gridIndex, false);
          T* pGridPointer = GetDataPointer();
          pGridPointer[index]++;
          
          if (f != NULL)
          {
            (*f)(index); 
          }
        }
      }
    }
 
  protected:
    Grid(kt_int32s width, kt_int32s height)
      : m_Width(0)
      , m_Height(0)
      , m_WidthStep(0)
      , m_pData(NULL)
 
      , m_pCoordinateConverter(NULL)
    {
      Resize(width, height);
    }
    
  private:
    Grid(const Grid&);
    const Grid& operator=(const Grid&);
 
  private:
    kt_int32s m_Width;       // width of grid
    kt_int32s m_Height;      // height of grid
    kt_int32s m_WidthStep;   // 8-byte aligned width of grid
    T* m_pData;              // grid data
 
    CoordinateConverter* m_pCoordinateConverter; // coordinate converter to convert between world coordinates and grid coordinates
  }; // Grid
 
  KARTO_TYPE(Grid<kt_int8u>);
 
  KARTO_TYPE(Grid<kt_int32u>);
 
  KARTO_TYPE(Grid<kt_float>);
 
  KARTO_TYPE(Grid<kt_double>);
 
 
}
 
#endif // __OpenKarto_Grid_h__