PointHashGrid.h

Go to the documentation of this file.

#ifndef G3D_PointHashGrid_h
#define G3D_PointHashGrid_h

#include "G3D/platform.h"
#include "G3D/EqualsTrait.h"
#include "G3D/HashTrait.h"
#include "G3D/Vector3.h"
#include "G3D/Vector3int32.h"
#include "G3D/Array.h"
#include "G3D/Table.h"
#include "G3D/AABox.h"
#include "G3D/Sphere.h"
#include "G3D/SmallArray.h"

namespace G3D {

template<class Value,
 class PosFunc = PositionTrait<Value>, 
 class EqualsFunc = EqualsTrait<Value> >
class PointHashGrid {
private:

# define expectedCellSize (10)

# define ThisType PointHashGrid<Value, PosFunc, EqualsFunc>

 class Entry {
 public:
 Point3 position;
 Value value;
 };

 typedef SmallArray<Entry, expectedCellSize> Cell;
 typedef Table<Point3int32, Cell > CellTable;

 Vector3int32 m_offsetArray[3*3*3];

 int m_epoch;

 float m_cellWidth;

 float m_invCellWidth;

 AABox m_bounds;

 int m_size;

 CellTable m_data;

 MemoryManager::Ref m_memoryManager;

 PointHashGrid(const ThisType&);


 PointHashGrid& operator=(const ThisType&);


 bool find
 (const Value& v, 
 Point3int32& foundCellCoord, 
 Cell*& foundCell, 
 int& index) {
 
 Point3 pos;
 PosFunc::getPosition(v, pos);

 Point3int32 cellCoord;
 getCellCoord(pos, cellCoord);
 for (int i = 0; i < 27; ++i) {
 Point3int32 c = cellCoord + m_offsetArray[i];
 Cell* cell = m_data.getPointer(c);
 if (cell != NULL) {
 // The cell exists
 for (int j = 0; j < cell->size(); ++j) {
 if (EqualsFunc::equals((*cell)[j].value, v)) {
 foundCell = cell;
 index = j;
 foundCellCoord = c;
 return true;
 }
 }
 } 
 }

 // Not found
 return false; 
 }

public:

 inline void getCellCoord(const Point3& pos, Point3int32& cellCoord) const {
 for (int a = 0; a < 3; ++a) {
 cellCoord[a] = iFloor(pos[a] * m_invCellWidth);
 }
 }

protected:

 void initOffsetArray() {
 int i = 0;
 Point3int32 d;
 for (d.x = -1; d.x <= +1; ++d.x) {
 for (d.y = -1; d.y <= +1; ++d.y) {
 for (d.z = -1; d.z <= +1; ++d.z) {
 m_offsetArray[i] = d;
 ++i;
 }
 }
 }
 
 // Put (0, 0, 0) first, so that contains() is most likely to find
 // the value quickly.
 i = (1 * 3 + 1) * 3 + 1;
 debugAssert(m_offsetArray[i] == Vector3int32(0,0,0));
 Vector3int32 temp = m_offsetArray[0];
 m_offsetArray[0] = m_offsetArray[i];
 m_offsetArray[i] = temp;
 }

public:

 PointHashGrid(float radiusHint, const MemoryManager::Ref& m = MemoryManager::create()) : m_size(0), m_memoryManager(m) {
 initOffsetArray();
 m_data.clearAndSetMemoryManager(m_memoryManager);

 debugAssertM(radiusHint > 0, "Cell radius must be positive");
 m_cellWidth = radiusHint;
 m_invCellWidth = 1.0f / m_cellWidth;
 }

 float radiusHint() const {
 return m_cellWidth;
 }
 
 void clear(float radiusHint) {
 debugAssertM(radiusHint > 0, "Cell radius must be positive");
 clear();
 m_cellWidth = radiusHint;
 m_invCellWidth = 1.0f / m_cellWidth;
 }

 void clearAndSetRadiusHint(float radiusHint) {
 return clear(radiusHint);
 }
 
 PointHashGrid(const Array<Value>& init, float radiusHint = -1.0f, const MemoryManager::Ref& m = MemoryManager::create()) : m_size(0), m_memoryManager(m) { 
 initOffsetArray();
 m_data.clearAndSetMemoryManager(m_memoryManager);

 Point3 lo(Vector3::inf());
 Point3 hi(-lo);

 // Compute bounds
 Array<Entry> entry(init.size());
 for (int i = 0; i < entry.size(); ++i) {
 const Value& value = init[i];
 Point3 pos;

 entry[i].value = value;
 entry[i].hashCode = m_hashFunc(value);
 PosFunc::getPosition(value, entry[i].position);

 lo = lo.min(pos);
 hi = hi.max(pos);
 }

 m_bounds = AABox(lo, hi);

 if (radiusHint <= 0) {
 // Compute a good cell width based on the bounds.
 //
 // N numPerCell
 // ----- = ---------
 // volume r^3

 float numPerCell = 5;
 radiusHint = 
 (float)pow(numPerCell * m_bounds.volume() / init.size(), 1.0 / 3.0);

 if (radiusHint == 0) {
 // Volume must have been zero because all points were colocated.
 radiusHint = 0.1f;
 }
 }

 insert(init);
 }

 int size() const {
 return m_size;
 }

 const AABox& conservativeBoxBounds() const {
 return m_bounds;
 }

 void debugPrintStatistics() const {
 debugPrintf("Deepest bucket size = %d\n", (int)m_data.debugGetDeepestBucketSize());
 debugPrintf("Average bucket size = %g\n", m_data.debugGetAverageBucketSize());
 debugPrintf("Load factor = %g\n", m_data.debugGetLoad());
 }

 void insert(const Value& v) {
 Point3 pos;
 PosFunc::getPosition(v, pos);
 Point3int32 cellCoord;
 getCellCoord(pos, cellCoord);

 // See if the cell already exists
 Cell& cell = m_data.getCreate(cellCoord);

 if (cell.size() == 0) {
 // Use the same memory manager as for the whole class
 cell.clearAndSetMemoryManager(m_memoryManager);
 }

 Entry& entry = cell.next();
 entry.value = v;
 entry.position = pos;

 // Update the bounds
 if (size() == 0) {
 m_bounds = AABox(pos);
 } else {
 m_bounds.merge(pos);
 }

 ++m_size;
 ++m_epoch;
 }


 void insert(const Array<Value>& v) {
 for (int i = 0; i < v.size(); ++i) {
 insert(v[i]);
 }
 }


 bool remove(const Value& v, bool shrinkIfNecessary = true) {
 Cell* cell = NULL;
 int index = 0;
 Vector3int32 cellCoord;

 if (find(v, cellCoord, cell, index)) {
 cell->fastRemove(index, shrinkIfNecessary);
 --m_size;
 ++m_epoch;

 if ((cell->size() == 0) && shrinkIfNecessary) {
 // Remove the cell itself

 // Drop our pointer, which is about to dangle
 cell = NULL;
 const bool success = m_data.remove(cellCoord);
 (void)success;
 debugAssertM(success, "Data structure corrupt: "
 "tried to remove a cell that doesn't exist.");
 }

 return true;

 } else {
 return false;
 }
 }

 void remove(const Array<Value>& v, bool shrink = true) {
 for (int i = 0; i < v.size(); ++i) {
 remove(v[i], shrink);
 }
 }


 class Iterator {
 private:
 friend class ThisType;

 bool m_isEnd;

 const ThisType* m_grid;
 
 typename CellTable::Iterator m_tableIterator;

 int m_arrayIndex;

 const int m_epoch;

 Iterator() : m_isEnd(true), m_grid(NULL), m_tableIterator(CellTable().end()), 
 m_arrayIndex(0), m_epoch(0) {}

 Iterator(const ThisType* grid) : 
 m_isEnd(grid->size() == 0),
 m_grid(grid),
 m_tableIterator( grid->m_data.begin() ),
 m_arrayIndex(0),
 m_epoch(grid->m_epoch) { }

 private:

 const Value& value() const {
 debugAssert(! m_isEnd);
 debugAssertM(m_tableIterator->value.size() > m_arrayIndex, 
 "No more elements");
 return m_tableIterator->value[m_arrayIndex].value;
 }

 public:

 inline bool operator!=(const Iterator& other) const {
 if (other.m_isEnd && m_isEnd) {
 return false;
 } else {
 return (m_isEnd != other.m_isEnd) ||
 (m_tableIterator != other.m_tableIterator) || 
 (m_arrayIndex != other.m_arrayIndex);
 }
 }

 bool isValid() const {
 return ! m_isEnd;
 }

 bool hasMore() const {
 return ! m_isEnd;
 }

 bool operator==(const Iterator& other) const {
 return !(*this != other);
 }

 Iterator& operator++() {
 debugAssert(! m_isEnd);
 debugAssertM(m_epoch == m_grid->m_epoch, 
 "It is illegal to mutate the HashGrid "
 "while iterating through it.");

 ++m_arrayIndex;

 if (m_arrayIndex >= m_tableIterator->value.size()) {
 // Move on to the next cell
 ++m_tableIterator;
 m_arrayIndex = 0;

 // Check to see if we're at the end
 m_isEnd = (m_tableIterator == m_grid->m_data.end());
 }

 return *this;
 }

 Iterator operator++(int) {
 debugAssert(! m_isEnd);
 Iterator old = *this;
 ++(*this);
 return old;
 }

 const Value& operator*() const { return value(); }
 const Value* operator->() const { return &value(); }
 operator Value*() const { return &value(); }
 }; // Iterator


 Iterator begin() const {
 return Iterator(this);
 }

 const Iterator& end() const {
 static const Iterator it;
 return it;
 }


 // Forward declaration required by older gcc versions for friend declaration in BoxIterator
 class SphereIterator;
 class BoxIterator {
 private:
 friend class ThisType;
 friend class SphereIterator;

 bool m_isEnd;

 const ThisType* m_grid;

 Vector3int32 m_lo;

 Vector3int32 m_hi;
 
 bool m_exact;

 AABox m_box;

 Vector3int32 m_current;
 
 Cell* m_cell;

 int m_arrayIndex;

 const int m_epoch;
 

 void advanceCell() {
 do {
 ++m_current.x;
 if (m_current.x > m_hi.x) {
 m_current.x = m_lo.x;
 ++m_current.y;
 if (m_current.y > m_hi.y) {
 m_current.y = m_lo.y;
 ++m_current.z;
 if (m_current.z > m_hi.z) {
 m_isEnd = true;
 return;
 }
 }
 }

 // Pick up the new cell
 m_cell = m_grid->m_data.getPointer(m_current);
 // Keep advancing if the cell does not exist
 } while ((m_cell == NULL) || (m_cell->size() == 0));
 }

 void advance() {
 debugAssert(! m_isEnd);

 do {
 ++m_arrayIndex;
 bool inConstructor = (m_cell == NULL);
 if (inConstructor || m_arrayIndex >= m_cell->size()) {
 advanceCell();
 m_arrayIndex = 0;

 if (m_isEnd) {
 // Ran out of values
 return;
 }
 debugAssert(m_cell != NULL);
 }

 // Advance until we have a value that can be returned, either
 // because we don't care about exactness or because it is 
 // guaranteed to be within the box.
 } while (m_exact && ! m_box.contains(position()));
 }


 BoxIterator() : m_isEnd(true), m_grid(NULL), m_exact(true), m_current(0,0,0), m_cell(NULL), m_arrayIndex(0), m_epoch(0) {}

 BoxIterator(const ThisType* grid, bool exact, const AABox& box) : 
 m_isEnd(false),
 m_grid(grid),
 m_exact(exact),
 m_box(box),
 m_current(-1, 0 ,0),
 m_cell(NULL),
 m_arrayIndex(0),
 m_epoch(grid->m_epoch) { 

 m_grid->getCellCoord(box.low(), m_lo);
 m_grid->getCellCoord(box.high(), m_hi);
 
 // Get to the first value
 m_current = m_lo;
 // Back up one so that advancing takes us to the first
 --m_current.x;
 advance();
 }

 const Value& value() const {
 debugAssert(! m_isEnd);
 return (*m_cell)[m_arrayIndex].value;
 }

 const Vector3& position() const {
 debugAssert(! m_isEnd);
 return (*m_cell)[m_arrayIndex].position;
 }

 // Intentionally unimplemented
 BoxIterator& operator=(const BoxIterator&);

 public:

 inline bool operator!=(const BoxIterator& other) const {
 if (other.m_isEnd && m_isEnd) {
 return false;
 } else {
 return (m_isEnd != other.m_isEnd) ||
 (m_cell != other.m_cell) || 
 (m_arrayIndex != other.m_arrayIndex);
 }
 }

 bool operator==(const BoxIterator& other) const {
 return !(*this != other);
 }

 BoxIterator& operator++() {
 debugAssert(! m_isEnd);
 debugAssertM(m_epoch == m_grid->m_epoch, 
 "It is illegal to mutate the HashGrid "
 "while iterating through it.");

 advance();

 return *this;
 }

 BoxIterator operator++(int) {
 Iterator old = *this;
 ++(*this);
 return old;
 }

 const Value& operator*() const { return value(); }
 const Value* operator->() const { return &value(); }
 operator Value*() const { return &value(); }

 bool hasMore() const {
 return ! m_isEnd;
 }

 bool isValid() const {
 return ! m_isEnd;
 }
 }; // BoxIterator

 BoxIterator beginBoxIntersection(const AABox& box, bool exact = true) const {
 return BoxIterator(this, exact, box);
 }

 const BoxIterator& endBoxIntersection() const {
 static const BoxIterator it;
 return it;
 }

 
 class SphereIterator {
 private:

 friend class ThisType;

 bool m_isEnd;
 Sphere m_sphere;
 BoxIterator m_boxIterator;

 SphereIterator() : m_isEnd(true) {}

 void advance() {
 if (! m_boxIterator.isValid()) {
 m_isEnd = true;
 return;
 }

 while (! m_sphere.contains(m_boxIterator.position())) {
 ++m_boxIterator;
 
 if (! m_boxIterator.isValid()) {
 m_isEnd = true;
 return;
 }
 } 
 }

 static AABox getBoundingBox(const Sphere& s) {
 AABox box;
 s.getBounds(box);
 return box;
 }

 SphereIterator(const ThisType* grid, const Sphere& sphere) : 
 m_isEnd(false),
 m_sphere(sphere),
 m_boxIterator(grid, false, getBoundingBox(sphere)) {
 
 // Find the first element that is actually in the sphere,
 // not just the box.
 advance();
 }

 const Value& value() const { 
 return *m_boxIterator;
 }

 // TODO: if the sphere is very big compared to radius, check each
 // cell's box to see if the cell itself is actually inside the sphere
 // before iterating through it, since there may be many boxes outside the sphere.

 // Intentionally unimplemented
 SphereIterator& operator=(const SphereIterator&);

 public:

 inline bool operator!=(const SphereIterator& other) const {
 if (other.m_isEnd && m_isEnd) {
 return false;
 } else {
 return 
 (m_isEnd != other.m_isEnd) ||
 (m_sphere != other.m_sphere) ||
 (m_boxIterator != other.m_boxIterator);
 }
 }

 bool operator==(const SphereIterator& other) const {
 return !(*this != other);
 }

 SphereIterator& operator++() {
 debugAssert(! m_isEnd);

 ++m_boxIterator;
 advance();

 return *this;
 }

 SphereIterator operator++(int) {
 Iterator old = *this;
 ++(*this);
 return old;
 }

 const Value& operator*() const { return value(); }
 const Value* operator->() const { return &value(); }
 operator Value*() const { return &value(); }

 bool G3D_DEPRECATED hasMore() const {
 return ! m_isEnd;
 }

 bool isValid() const {
 return ! m_isEnd;
 }
 }; // SphereIterator

 SphereIterator begin(const Sphere& sphere) const {
 return SphereIterator(this, sphere);
 }


 SphereIterator G3D_DEPRECATED beginSphereIntersection(const Sphere& sphere) const {
 return SphereIterator(this, sphere);
 }


 const SphereIterator& endSphereIntersection() const {
 static const SphereIterator it;
 return it;
 }

 
 void getIntersectingMembers(const Sphere& sphere, Array<Value>& result) const {
 for (SphereIterator it = beginSphereIntersection(sphere); it.isValid(); ++it) {
 result.append(*it);
 }
 }



 class CellIterator {
 private:
 friend class ThisType;

 bool m_isEnd;
 const ThisType* m_grid;
 typename CellTable::Iterator m_tableIterator;
 const int m_epoch;


 Cell& cell() {
 return m_tableIterator->value;
 }

 public:
 
 class CellObject {
 friend class CellIterator;
 private:
 const CellIterator* m_parent;

 CellObject() : m_parent(NULL) {}

 public:

 AABox bounds() const {
 const Vector3int32& k = m_parent->m_tableIterator->key;
 return AABox(Vector3(k) * m_parent->m_grid->m_cellWidth, 
 Vector3(k + Vector3int32(1, 1, 1)) * m_parent->m_grid->m_cellWidth);
 }

 int size() const {
 debugAssert(! m_parent->m_isEnd);
 return m_parent->m_tableIterator->value.size();
 }
 };

 private:
 CellObject m_indirection;

 CellIterator() : 
 m_isEnd(true),
 m_grid(NULL),
 m_tableIterator( CellTable().end() ),
 m_epoch(0) {}
 
 CellIterator(const ThisType* grid) : 
 m_isEnd(false),
 m_grid(grid),
 m_tableIterator( grid->m_data.begin()),
 m_epoch(grid->m_epoch) {
 m_indirection.m_parent = this;
 m_isEnd = ! m_tableIterator.isValid();
 }
 
 // Intentionally unimplemented
 CellIterator& operator=(const CellIterator&);

 public:

 const CellObject& operator*() const { return m_indirection; }
 const CellObject* operator->() const { return &m_indirection; }
 operator CellObject*() const { return &m_indirection; }

 inline bool operator!=(const CellIterator& other) const {
 // != is called more often than == during iteration
 return !(
 (m_isEnd && other.m_isEnd) ||
 ((m_isEnd == other.m_isEnd) && 
 (m_tableIterator != other.m_tableIterator)));
 }

 bool operator==(const CellIterator& other) const {
 return !(*this != other);
 }

 CellIterator& operator++() {
 debugAssertM(m_epoch == m_grid->m_epoch, 
 "It is illegal to mutate the HashGrid while "
 "iterating through it.");
 ++m_tableIterator;
 m_isEnd = ! m_tableIterator.isValid();
 return *this;
 }

 CellIterator operator++(int) {
 Iterator old = *this;
 ++(*this);
 return old;
 }

 bool hasMore() const {
 return ! m_isEnd;
 }

 bool isValid() const {
 return ! m_isEnd;
 }
 }; // CellIterator

 CellIterator beginCells() const {
 return CellIterator(this);
 }

 const CellIterator& endCells() const {
 static const CellIterator it;
 return it;
 }


 bool contains(const Value& v) const {
 Cell* cell = NULL;
 int index = 0;
 Vector3int32 cellCoord;
 return const_cast<ThisType*>(this)->find(v, cellCoord, cell, index);
 }

 void deleteAll() {
 for (Iterator it = begin(); it.isValid(); ++it) {
 delete *it;
 }
 clear();
 }

 void clearAndSetMemoryManager(const MemoryManager::Ref& m) {
 ++m_epoch;
 m_size = 0;
 m_bounds = AABox();

 m_data.clearAndSetMemoryManager(m);
 m_memoryManager = m;
 }

 void clear(bool shrink = true) {
 m_size = 0;
 m_bounds = AABox();
 if (! shrink) {
 // Remove all data
 for (CellIterator it = beginCells(); it.isValid(); ++it) {
 it.cell().clear(true);
 }
 } else {
 m_data.clear();
 }
 ++m_epoch;
 }

 int debugGetDeepestBucketSize() const {
 return (int)m_data.debugGetDeepestBucketSize();
 }

 float debugGetAverageBucketSize() const {
 return m_data.debugGetAverageBucketSize();
 }
#undef ThisType 
};

} // G3D
#endif