Grid.h

// This file is a part of the OpenSurgSim project.
// Copyright 2013, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef SURGSIM_DATASTRUCTURES_GRID_H
#define SURGSIM_DATASTRUCTURES_GRID_H

#include <array>
#include <unordered_map>

#include "SurgSim/Math/Vector.h"

namespace SurgSim
{

namespace DataStructures
{

template <size_t N>
struct powerOf3
{
    enum { value = 3 * powerOf3<N-1>::value };
};
template <>
struct powerOf3<0>
{
    enum { value = 1 };
};

template <typename T, size_t N>
class Grid
{
public:
    Grid(const Eigen::Matrix<double, N, 1>& cellSize, const Eigen::AlignedBox<double, N>& bounds);

    virtual ~Grid();

    void reset();

    template <class Derived>
    void addElement(const T element, const Eigen::MatrixBase<Derived>& position);

    const std::vector<T>& getNeighbors(const T& element);

    template <class Derived>
    const std::vector<T>& getNeighbors(const Eigen::MatrixBase<Derived>& position);

protected:
    typedef struct
    {
        std::vector<T> elements;
        std::vector<T> neighbors;
    } CellContent;

    typedef Eigen::Matrix<int, N, 1> NDId;

    class NDIdHash
    {
    public:
        size_t operator()(const NDId& nd) const;
    };

    std::unordered_map<NDId, CellContent, NDIdHash> m_activeCells;

    std::unordered_map<T, NDId> m_cellIds;

    Eigen::Matrix<double, N, 1> m_size;

    Eigen::AlignedBox<double, N> m_aabb;

    bool m_neighborsDirtyFlag;

private:
    void update();

    void getNeighborsCellIds(NDId cellId,
        std::array<NDId, powerOf3<N>::value>* cellsIds);

    bool isNdGreaterOrEqual(const NDId& a, const NDId& b);
};

};  // namespace DataStructures
};  // namespace SurgSim

#include "SurgSim/DataStructures/Grid-inl.h"

#endif  // SURGSIM_DATASTRUCTURES_GRID_H