Moved here and cleaned the kdtree for points implemented by Gael
This commit is contained in:
parent
3a9c974fd5
commit
bc57fc36b4
|
|
@ -0,0 +1,336 @@
|
||||||
|
#ifndef KDTREE_H
|
||||||
|
#define KDTREE_H
|
||||||
|
|
||||||
|
#include "../../point3.h"
|
||||||
|
#include "../../box3.h"
|
||||||
|
#include "mlsutils.h"
|
||||||
|
#include "priorityqueue.h"
|
||||||
|
#include <vector>
|
||||||
|
#include <limits>
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
template<typename _DataType>
|
||||||
|
class ConstDataWrapper
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
typedef _DataType DataType;
|
||||||
|
inline ConstDataWrapper()
|
||||||
|
: mpData(0), mStride(0), mSize(0)
|
||||||
|
{}
|
||||||
|
inline ConstDataWrapper(const DataType* pData, int size, int stride = sizeof(DataType))
|
||||||
|
: mpData(reinterpret_cast<const unsigned char*>(pData)), mStride(stride), mSize(size)
|
||||||
|
{}
|
||||||
|
inline const DataType& operator[] (int i) const
|
||||||
|
{
|
||||||
|
return *reinterpret_cast<const DataType*>(mpData + i*mStride);
|
||||||
|
}
|
||||||
|
inline size_t size() const { return mSize; }
|
||||||
|
protected:
|
||||||
|
const unsigned char* mpData;
|
||||||
|
int mStride;
|
||||||
|
size_t mSize;
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This class allows to create a Kd-Tree thought to perform the k-nearest neighbour query
|
||||||
|
*/
|
||||||
|
template<typename _Scalar>
|
||||||
|
class KdTree
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
|
||||||
|
typedef _Scalar Scalar;
|
||||||
|
typedef vcg::Point3<Scalar> VectorType;
|
||||||
|
typedef vcg::Box3<Scalar> AxisAlignedBoxType;
|
||||||
|
|
||||||
|
struct Node
|
||||||
|
{
|
||||||
|
union {
|
||||||
|
//standard node
|
||||||
|
struct {
|
||||||
|
Scalar splitValue;
|
||||||
|
unsigned int firstChildId:24;
|
||||||
|
unsigned int dim:2;
|
||||||
|
unsigned int leaf:1;
|
||||||
|
};
|
||||||
|
//leaf
|
||||||
|
struct {
|
||||||
|
unsigned int start;
|
||||||
|
unsigned short size;
|
||||||
|
};
|
||||||
|
};
|
||||||
|
};
|
||||||
|
typedef std::vector<Node> NodeList;
|
||||||
|
|
||||||
|
// return the protected members which store the nodes and the points list
|
||||||
|
inline const NodeList& _getNodes(void) { return mNodes; }
|
||||||
|
inline const std::vector<VectorType>& _getPoints(void) { return mPoints; }
|
||||||
|
|
||||||
|
|
||||||
|
void setMaxNofNeighbors(unsigned int k);
|
||||||
|
inline int getNofFoundNeighbors(void) { return mNeighborQueue.getNofElements(); }
|
||||||
|
inline const VectorType& getNeighbor(int i) { return mPoints[ mNeighborQueue.getIndex(i) ]; }
|
||||||
|
inline unsigned int getNeighborId(int i) { return mIndices[mNeighborQueue.getIndex(i)]; }
|
||||||
|
inline float getNeighborSquaredDistance(int i) { return mNeighborQueue.getWeight(i); }
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
KdTree(const ConstDataWrapper<VectorType>& points, unsigned int nofPointsPerCell = 16, unsigned int maxDepth = 64);
|
||||||
|
|
||||||
|
~KdTree();
|
||||||
|
|
||||||
|
void doQueryK(const VectorType& p);
|
||||||
|
|
||||||
|
protected:
|
||||||
|
|
||||||
|
// element of the stack
|
||||||
|
struct QueryNode
|
||||||
|
{
|
||||||
|
QueryNode() {}
|
||||||
|
QueryNode(unsigned int id) : nodeId(id) {}
|
||||||
|
unsigned int nodeId; // id of the next node
|
||||||
|
Scalar sq; // squared distance to the next node
|
||||||
|
};
|
||||||
|
|
||||||
|
// used to build the tree: split the subset [start..end[ according to dim and splitValue,
|
||||||
|
// and returns the index of the first element of the second subset
|
||||||
|
unsigned int split(int start, int end, unsigned int dim, float splitValue);
|
||||||
|
|
||||||
|
void createTree(unsigned int nodeId, unsigned int start, unsigned int end, unsigned int level, unsigned int targetCellsize, unsigned int targetMaxDepth);
|
||||||
|
|
||||||
|
protected:
|
||||||
|
|
||||||
|
AxisAlignedBoxType mAABB; //BoundingBox
|
||||||
|
NodeList mNodes; //kd-tree nodes
|
||||||
|
std::vector<VectorType> mPoints; //points read from the input DataWrapper
|
||||||
|
std::vector<int> mIndices; //points indices
|
||||||
|
|
||||||
|
HeapMaxPriorityQueue<int,Scalar> mNeighborQueue; //used to perform the knn-query
|
||||||
|
QueryNode mNodeStack[64]; //used in the implementation of the knn-query
|
||||||
|
};
|
||||||
|
|
||||||
|
template<typename Scalar>
|
||||||
|
KdTree<Scalar>::KdTree(const ConstDataWrapper<VectorType>& points, unsigned int nofPointsPerCell, unsigned int maxDepth)
|
||||||
|
: mPoints(points.size()), mIndices(points.size())
|
||||||
|
{
|
||||||
|
// compute the AABB of the input
|
||||||
|
mPoints[0] = points[0];
|
||||||
|
mAABB.Set(mPoints[0]);
|
||||||
|
for (unsigned int i=1 ; i<mPoints.size() ; ++i)
|
||||||
|
{
|
||||||
|
mPoints[i] = points[i];
|
||||||
|
mIndices[i] = i;
|
||||||
|
mAABB.Add(mPoints[i]);
|
||||||
|
}
|
||||||
|
|
||||||
|
mNodes.reserve(4*mPoints.size()/nofPointsPerCell);
|
||||||
|
|
||||||
|
//first node inserted (no leaf). The others are made by the createTree function (recursively)
|
||||||
|
mNodes.resize(1);
|
||||||
|
mNodes.back().leaf = 0;
|
||||||
|
createTree(0, 0, mPoints.size(), 1, nofPointsPerCell, maxDepth);
|
||||||
|
}
|
||||||
|
|
||||||
|
template<typename Scalar>
|
||||||
|
KdTree<Scalar>::~KdTree()
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
template<typename Scalar>
|
||||||
|
void KdTree<Scalar>::setMaxNofNeighbors(unsigned int k)
|
||||||
|
{
|
||||||
|
mNeighborQueue.setMaxSize(k);
|
||||||
|
}
|
||||||
|
|
||||||
|
/** Performs the kNN query.
|
||||||
|
*
|
||||||
|
* This algorithm uses the simple distance to the split plane to prune nodes.
|
||||||
|
* A more elaborated approach consists to track the closest corner of the cell
|
||||||
|
* relatively to the current query point. This strategy allows to save about 5%
|
||||||
|
* of the leaves. However, in practice the slight overhead due to this tracking
|
||||||
|
* reduces the overall performance.
|
||||||
|
*
|
||||||
|
* This algorithm also use a simple stack while a priority queue using the squared
|
||||||
|
* distances to the cells as a priority values allows to save about 10% of the leaves.
|
||||||
|
* But, again, priority queue insertions and deletions are quite involved, and therefore
|
||||||
|
* a simple stack is by far much faster.
|
||||||
|
*
|
||||||
|
* The result of the query, the k-nearest neighbors, are internally stored into a stack, where the
|
||||||
|
* topmost element
|
||||||
|
*/
|
||||||
|
template<typename Scalar>
|
||||||
|
void KdTree<Scalar>::doQueryK(const VectorType& queryPoint)
|
||||||
|
{
|
||||||
|
mNeighborQueue.init();
|
||||||
|
mNeighborQueue.insert(0xffffffff, std::numeric_limits<Scalar>::max());
|
||||||
|
|
||||||
|
mNodeStack[0].nodeId = 0;
|
||||||
|
mNodeStack[0].sq = 0.f;
|
||||||
|
unsigned int count = 1;
|
||||||
|
|
||||||
|
while (count)
|
||||||
|
{
|
||||||
|
//we select the last node (AABB) inserted in the stack
|
||||||
|
QueryNode& qnode = mNodeStack[count-1];
|
||||||
|
|
||||||
|
//while going down the tree qnode.nodeId is the nearest sub-tree, otherwise,
|
||||||
|
//in backtracking, qnode.nodeId is the other sub-tree that will be visited iff
|
||||||
|
//the actual nearest node is further than the split distance.
|
||||||
|
Node& node = mNodes[qnode.nodeId];
|
||||||
|
|
||||||
|
//if the distance is less than the top of the max-heap, it could be one of the k-nearest neighbours
|
||||||
|
if (qnode.sq < mNeighborQueue.getTopWeight())
|
||||||
|
{
|
||||||
|
//when we arrive to a lef
|
||||||
|
if (node.leaf)
|
||||||
|
{
|
||||||
|
--count; //pop of the leaf
|
||||||
|
|
||||||
|
//end is the index of the last element of the leaf in mPoints
|
||||||
|
unsigned int end = node.start+node.size;
|
||||||
|
//adding the element of the leaf to the heap
|
||||||
|
for (unsigned int i=node.start ; i<end ; ++i)
|
||||||
|
mNeighborQueue.insert(i, vcg::SquaredNorm(queryPoint - mPoints[i]));
|
||||||
|
}
|
||||||
|
//otherwise, if we're not on a leaf
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// the new offset is the distance between the searched point and the actual split coordinate
|
||||||
|
float new_off = queryPoint[node.dim] - node.splitValue;
|
||||||
|
|
||||||
|
//left sub-tree
|
||||||
|
if (new_off < 0.)
|
||||||
|
{
|
||||||
|
mNodeStack[count].nodeId = node.firstChildId;
|
||||||
|
//in the father's nodeId we save the index of the other sub-tree (for backtracking)
|
||||||
|
qnode.nodeId = node.firstChildId+1;
|
||||||
|
}
|
||||||
|
//right sub-tree (same as above)
|
||||||
|
else
|
||||||
|
{
|
||||||
|
mNodeStack[count].nodeId = node.firstChildId+1;
|
||||||
|
qnode.nodeId = node.firstChildId;
|
||||||
|
}
|
||||||
|
//distance is inherited from the father (while descending the tree it's equal to 0)
|
||||||
|
mNodeStack[count].sq = qnode.sq;
|
||||||
|
//distance of the father is the squared distance from the split plane
|
||||||
|
qnode.sq = new_off*new_off;
|
||||||
|
++count;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// pop
|
||||||
|
--count;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Split the subarray between start and end in two part, one with the elements less than splitValue,
|
||||||
|
* the other with the elements greater or equal than splitValue. The elements are compared
|
||||||
|
* using the "dim" coordinate [0 = x, 1 = y, 2 = z].
|
||||||
|
*/
|
||||||
|
template<typename Scalar>
|
||||||
|
unsigned int KdTree<Scalar>::split(int start, int end, unsigned int dim, float splitValue)
|
||||||
|
{
|
||||||
|
int l(start), r(end-1);
|
||||||
|
for ( ; l<r ; ++l, --r)
|
||||||
|
{
|
||||||
|
while (l < end && mPoints[l][dim] < splitValue)
|
||||||
|
l++;
|
||||||
|
while (r >= start && mPoints[r][dim] >= splitValue)
|
||||||
|
r--;
|
||||||
|
if (l > r)
|
||||||
|
break;
|
||||||
|
std::swap(mPoints[l],mPoints[r]);
|
||||||
|
std::swap(mIndices[l],mIndices[r]);
|
||||||
|
}
|
||||||
|
//returns the index of the first element on the second part
|
||||||
|
return (mPoints[l][dim] < splitValue ? l+1 : l);
|
||||||
|
}
|
||||||
|
|
||||||
|
/** recursively builds the kdtree
|
||||||
|
*
|
||||||
|
* The heuristic is the following:
|
||||||
|
* - if the number of points in the node is lower than targetCellsize then make a leaf
|
||||||
|
* - else compute the AABB of the points of the node and split it at the middle of
|
||||||
|
* the largest AABB dimension.
|
||||||
|
*
|
||||||
|
* This strategy might look not optimal because it does not explicitly prune empty space,
|
||||||
|
* unlike more advanced SAH-like techniques used for RT. On the other hand it leads to a shorter tree,
|
||||||
|
* faster to traverse and our experience shown that in the special case of kNN queries,
|
||||||
|
* this strategy is indeed more efficient (and much faster to build). Moreover, for volume data
|
||||||
|
* (e.g., fluid simulation) pruning the empty space is useless.
|
||||||
|
*
|
||||||
|
* Actually, storing at each node the exact AABB (we therefore have a binary BVH) allows
|
||||||
|
* to prune only about 10% of the leaves, but the overhead of this pruning (ball/ABBB intersection)
|
||||||
|
* is more expensive than the gain it provides and the memory consumption is x4 higher !
|
||||||
|
*/
|
||||||
|
template<typename Scalar>
|
||||||
|
void KdTree<Scalar>::createTree(unsigned int nodeId, unsigned int start, unsigned int end, unsigned int level, unsigned int targetCellSize, unsigned int targetMaxDepth)
|
||||||
|
{
|
||||||
|
//select the first node
|
||||||
|
Node& node = mNodes[nodeId];
|
||||||
|
AxisAlignedBoxType aabb;
|
||||||
|
|
||||||
|
//putting all the points in the bounding box
|
||||||
|
aabb.Set(mPoints[start]);
|
||||||
|
for (unsigned int i=start+1 ; i<end ; ++i)
|
||||||
|
aabb.Add(mPoints[i]);
|
||||||
|
|
||||||
|
//bounding box diagonal
|
||||||
|
VectorType diag = aabb.max - aabb.min;
|
||||||
|
|
||||||
|
//the split "dim" is the dimension of the box with the biggest value
|
||||||
|
unsigned int dim = vcg::MaxCoeffId(diag);
|
||||||
|
node.dim = dim;
|
||||||
|
//we divide the bounding box in 2 partitions, considering the average of the "dim" dimension
|
||||||
|
node.splitValue = Scalar(0.5*(aabb.max[dim] + aabb.min[dim]));
|
||||||
|
|
||||||
|
//midId is the index of the first element in the second partition
|
||||||
|
unsigned int midId = split(start, end, dim, node.splitValue);
|
||||||
|
|
||||||
|
|
||||||
|
node.firstChildId = mNodes.size();
|
||||||
|
mNodes.resize(mNodes.size()+2);
|
||||||
|
|
||||||
|
{
|
||||||
|
// left child
|
||||||
|
unsigned int childId = mNodes[nodeId].firstChildId;
|
||||||
|
Node& child = mNodes[childId];
|
||||||
|
if (midId - start <= targetCellSize || level>=targetMaxDepth)
|
||||||
|
{
|
||||||
|
child.leaf = 1;
|
||||||
|
child.start = start;
|
||||||
|
child.size = midId - start;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
child.leaf = 0;
|
||||||
|
createTree(childId, start, midId, level+1, targetCellSize, targetMaxDepth);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
{
|
||||||
|
// right child
|
||||||
|
unsigned int childId = mNodes[nodeId].firstChildId+1;
|
||||||
|
Node& child = mNodes[childId];
|
||||||
|
if (end - midId <= targetCellSize || level>=targetMaxDepth)
|
||||||
|
{
|
||||||
|
child.leaf = 1;
|
||||||
|
child.start = midId;
|
||||||
|
child.size = end - midId;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
child.leaf = 0;
|
||||||
|
createTree(childId, midId, end, level+1, targetCellSize, targetMaxDepth);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
@ -0,0 +1,119 @@
|
||||||
|
/****************************************************************************
|
||||||
|
* MeshLab o o *
|
||||||
|
* A versatile mesh processing toolbox o o *
|
||||||
|
* _ O _ *
|
||||||
|
* Copyright(C) 2005 \/)\/ *
|
||||||
|
* Visual Computing Lab /\/| *
|
||||||
|
* ISTI - Italian National Research Council | *
|
||||||
|
* \ *
|
||||||
|
* All rights reserved. *
|
||||||
|
* *
|
||||||
|
* This program is free software; you can redistribute it and/or modify *
|
||||||
|
* it under the terms of the GNU General Public License as published by *
|
||||||
|
* the Free Software Foundation; either version 2 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 General Public License (http://www.gnu.org/licenses/gpl.txt) *
|
||||||
|
* for more details. *
|
||||||
|
* *
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#ifndef VCGADDONS_H
|
||||||
|
#define VCGADDONS_H
|
||||||
|
|
||||||
|
|
||||||
|
namespace vcg {
|
||||||
|
|
||||||
|
//template <typename Scalar>
|
||||||
|
//inline Point3<Scalar> CwiseMul(Point3<Scalar> const & p1, Point3<Scalar> const & p2)
|
||||||
|
//{
|
||||||
|
// return Point3<Scalar>(p1.X()*p2.X(), p1.Y()*p2.Y(), p1.Z()*p2.Z());
|
||||||
|
//}
|
||||||
|
|
||||||
|
//template <typename Scalar>
|
||||||
|
//inline Point3<Scalar> Min(Point3<Scalar> const & p1, Point3<Scalar> const & p2)
|
||||||
|
//{
|
||||||
|
// return Point3<Scalar>(std::min(p1.X(), p2.X()), std::min(p1.Y(), p2.Y()), std::min(p1.Z(), p2.Z()));
|
||||||
|
//}
|
||||||
|
|
||||||
|
//template <typename Scalar>
|
||||||
|
//inline Point3<Scalar> Max(Point3<Scalar> const & p1, Point3<Scalar> const & p2)
|
||||||
|
//{
|
||||||
|
// return Point3<Scalar>(std::max(p1.X(), p2.X()), std::max(p1.Y(), p2.Y()), std::max(p1.Z(), p2.Z()));
|
||||||
|
//}
|
||||||
|
|
||||||
|
template <typename Scalar>
|
||||||
|
inline Scalar MaxCoeff(Point3<Scalar> const & p)
|
||||||
|
{
|
||||||
|
return std::max(std::max(p.X(), p.Y()), p.Z());
|
||||||
|
}
|
||||||
|
|
||||||
|
//template <typename Scalar>
|
||||||
|
//inline Scalar MinCoeff(Point3<Scalar> const & p)
|
||||||
|
//{
|
||||||
|
// return std::min(std::min(p.X(), p.Y()), p.Z());
|
||||||
|
//}
|
||||||
|
|
||||||
|
template <typename Scalar>
|
||||||
|
inline Scalar Dot(Point3<Scalar> const & p1, Point3<Scalar> const & p2)
|
||||||
|
{
|
||||||
|
return p1.X() * p2.X() + p1.Y() * p2.Y() + p1.Z() * p2.Z();
|
||||||
|
}
|
||||||
|
|
||||||
|
//template <typename Scalar>
|
||||||
|
//inline Point3<Scalar> Cross(Point3<Scalar> const & p1, Point3<Scalar> const & p2)
|
||||||
|
//{
|
||||||
|
// return p1 ^ p2;
|
||||||
|
//}
|
||||||
|
|
||||||
|
//template <typename Scalar>
|
||||||
|
//inline Point3<Scalar> CwiseAdd(Point3<Scalar> const & p1, Scalar s)
|
||||||
|
//{
|
||||||
|
// return Point3<Scalar>(p1.X() + s, p1.Y() + s, p1.Z() + s);
|
||||||
|
//}
|
||||||
|
|
||||||
|
template <typename Scalar>
|
||||||
|
inline int MaxCoeffId(Point3<Scalar> const & p)
|
||||||
|
{
|
||||||
|
if (p.X()>p.Y())
|
||||||
|
return p.X()>p.Z() ? 0 : 2;
|
||||||
|
else
|
||||||
|
return p.Y()>p.Z() ? 1 : 2;
|
||||||
|
}
|
||||||
|
|
||||||
|
//template <typename Scalar>
|
||||||
|
//inline int MinCoeffId(Point3<Scalar> const & p)
|
||||||
|
//{
|
||||||
|
// if (p.X()<p.Y())
|
||||||
|
// return p.X()<p.Z() ? 0 : 2;
|
||||||
|
// else
|
||||||
|
// return p.Y()<p.Z() ? 1 : 2;
|
||||||
|
//}
|
||||||
|
|
||||||
|
//template <typename ToType, typename Scalar>
|
||||||
|
//inline Point3<ToType> Point3Cast(const Point3<Scalar>& p)
|
||||||
|
//{
|
||||||
|
// return Point3<ToType>(p.X(), p.Y(), p.Z());
|
||||||
|
//}
|
||||||
|
|
||||||
|
//template<class Scalar>
|
||||||
|
//Scalar Distance(const Point3<Scalar> &p, const Box3<Scalar> &bbox)
|
||||||
|
//{
|
||||||
|
// Scalar dist2 = 0.;
|
||||||
|
// Scalar aux;
|
||||||
|
// for (int k=0 ; k<3 ; ++k)
|
||||||
|
// {
|
||||||
|
// if ( (aux = (p[k]-bbox.min[k]))<0. )
|
||||||
|
// dist2 += aux*aux;
|
||||||
|
// else if ( (aux = (bbox.max[k]-p[k]))<0. )
|
||||||
|
// dist2 += aux*aux;
|
||||||
|
// }
|
||||||
|
// return sqrt(dist2);
|
||||||
|
//}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,122 @@
|
||||||
|
/****************************************************************************
|
||||||
|
* MeshLab o o *
|
||||||
|
* A versatile mesh processing toolbox o o *
|
||||||
|
* _ O _ *
|
||||||
|
* Copyright(C) 2005 \/)\/ *
|
||||||
|
* Visual Computing Lab /\/| *
|
||||||
|
* ISTI - Italian National Research Council | *
|
||||||
|
* \ *
|
||||||
|
* All rights reserved. *
|
||||||
|
* *
|
||||||
|
* This program is free software; you can redistribute it and/or modify *
|
||||||
|
* it under the terms of the GNU General Public License as published by *
|
||||||
|
* the Free Software Foundation; either version 2 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 General Public License (http://www.gnu.org/licenses/gpl.txt) *
|
||||||
|
* for more details. *
|
||||||
|
* *
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#ifndef _PriorityQueue_h_
|
||||||
|
#define _PriorityQueue_h_
|
||||||
|
|
||||||
|
/** Implements a bounded-size max priority queue using a heap
|
||||||
|
*/
|
||||||
|
template <typename Index, typename Weight>
|
||||||
|
class HeapMaxPriorityQueue
|
||||||
|
{
|
||||||
|
struct Element
|
||||||
|
{
|
||||||
|
Weight weight;
|
||||||
|
Index index;
|
||||||
|
};
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
HeapMaxPriorityQueue(void)
|
||||||
|
{
|
||||||
|
mElements = 0;
|
||||||
|
mMaxSize = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void setMaxSize(int maxSize)
|
||||||
|
{
|
||||||
|
if (mMaxSize!=maxSize)
|
||||||
|
{
|
||||||
|
mMaxSize = maxSize;
|
||||||
|
delete[] mElements;
|
||||||
|
mElements = new Element[mMaxSize];
|
||||||
|
mpOffsetedElements = (mElements-1);
|
||||||
|
}
|
||||||
|
init();
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void init() { mCount = 0; }
|
||||||
|
|
||||||
|
inline bool isFull() const { return mCount == mMaxSize; }
|
||||||
|
|
||||||
|
/** returns number of elements inserted in queue
|
||||||
|
*/
|
||||||
|
inline int getNofElements() const { return mCount; }
|
||||||
|
|
||||||
|
inline Weight getWeight(int i) const { return mElements[i].weight; }
|
||||||
|
inline Index getIndex(int i) const { return mElements[i].index; }
|
||||||
|
|
||||||
|
inline Weight getTopWeight() const { return mElements[0].weight; }
|
||||||
|
|
||||||
|
inline void insert(Index index, Weight weight)
|
||||||
|
{
|
||||||
|
if (mCount==mMaxSize)
|
||||||
|
{
|
||||||
|
if (weight<mElements[0].weight)
|
||||||
|
{
|
||||||
|
register int j, k;
|
||||||
|
j = 1;
|
||||||
|
k = 2;
|
||||||
|
while (k <= mMaxSize)
|
||||||
|
{
|
||||||
|
Element* z = &(mpOffsetedElements[k]);
|
||||||
|
if ((k < mMaxSize) && (z->weight < mpOffsetedElements[k+1].weight))
|
||||||
|
z = &(mpOffsetedElements[++k]);
|
||||||
|
|
||||||
|
if(weight >= z->weight)
|
||||||
|
break;
|
||||||
|
mpOffsetedElements[j] = *z;
|
||||||
|
j = k;
|
||||||
|
k = 2 * j;
|
||||||
|
}
|
||||||
|
mpOffsetedElements[j].weight = weight;
|
||||||
|
mpOffsetedElements[j].index = index;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
int i, j;
|
||||||
|
i = ++mCount;
|
||||||
|
while (i >= 2)
|
||||||
|
{
|
||||||
|
j = i >> 1;
|
||||||
|
Element& y = mpOffsetedElements[j];
|
||||||
|
if(weight <= y.weight)
|
||||||
|
break;
|
||||||
|
mpOffsetedElements[i] = y;
|
||||||
|
i = j;
|
||||||
|
}
|
||||||
|
mpOffsetedElements[i].index = index;
|
||||||
|
mpOffsetedElements[i].weight = weight;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
protected:
|
||||||
|
|
||||||
|
int mCount;
|
||||||
|
int mMaxSize;
|
||||||
|
Element* mElements;
|
||||||
|
Element* mpOffsetedElements;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
Loading…
Reference in New Issue