Skip to content

Commit

Permalink
Update code style of spatial hashing collision manager
Browse files Browse the repository at this point in the history
  • Loading branch information
@jslee02
jslee02 committed Aug 12, 2016
1 parent d505e4c commit 6e97ef6
Show file tree
Hide file tree
Showing 9 changed files with 505 additions and 307 deletions.
142 changes: 67 additions & 75 deletions include/fcl/broadphase/broadphase_spatialhash.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,84 +35,33 @@

/** \author Jia Pan */

#ifndef FCL_BROAD_PHASE_SPATIAL_HASH_H
#define FCL_BROAD_PHASE_SPATIAL_HASH_H
#ifndef FCL_BROADPHASE_BROADPAHSESPATIALHASH_H
#define FCL_BROADPHASE_BROADPAHSESPATIALHASH_H

#include "fcl/broadphase/broadphase.h"
#include "fcl/broadphase/hash.h"
#include "fcl/BV/AABB.h"
#include <list>
#include <map>
#include "fcl/BV/AABB.h"
#include "fcl/broadphase/broadphase.h"
#include "fcl/broadphase/simple_hash_table.h"
#include "fcl/broadphase/spatial_hash.h"

namespace fcl
{

/// @brief Spatial hash function: hash an AABB to a set of integer values
template <typename S>
struct SpatialHash
{
SpatialHash(const AABB<S>& scene_limit_, S cell_size_) : cell_size(cell_size_),
scene_limit(scene_limit_)
{
width[0] = std::ceil(scene_limit.width() / cell_size);
width[1] = std::ceil(scene_limit.height() / cell_size);
width[2] = std::ceil(scene_limit.depth() / cell_size);
}

std::vector<unsigned int> operator() (const AABB<S>& aabb) const
{
int min_x = std::floor((aabb.min_[0] - scene_limit.min_[0]) / cell_size);
int max_x = std::ceil((aabb.max_[0] - scene_limit.min_[0]) / cell_size);
int min_y = std::floor((aabb.min_[1] - scene_limit.min_[1]) / cell_size);
int max_y = std::ceil((aabb.max_[1] - scene_limit.min_[1]) / cell_size);
int min_z = std::floor((aabb.min_[2] - scene_limit.min_[2]) / cell_size);
int max_z = std::ceil((aabb.max_[2] - scene_limit.min_[2]) / cell_size);

std::vector<unsigned int> keys((max_x - min_x) * (max_y - min_y) * (max_z - min_z));
int id = 0;
for(int x = min_x; x < max_x; ++x)
{
for(int y = min_y; y < max_y; ++y)
{
for(int z = min_z; z < max_z; ++z)
{
keys[id++] = x + y * width[0] + z * width[0] * width[1];
}
}
}
return keys;
}

private:

S cell_size;
AABB<S> scene_limit;
unsigned int width[3];
};

using SpatialHashf = SpatialHash<float>;
using SpatialHashd = SpatialHash<double>;

/// @brief spatial hashing collision mananger
template<typename S, typename HashTable = SimpleHashTable<AABB<S>, CollisionObject<S>*, SpatialHash<S>> >
template<typename S,
typename HashTable
= SimpleHashTable<AABB<S>, CollisionObject<S>*, SpatialHash<S>> >
class SpatialHashingCollisionManager : public BroadPhaseCollisionManager<S>
{
public:
SpatialHashingCollisionManager(
S cell_size,
const Vector3<S>& scene_min,
const Vector3<S>& scene_max,
unsigned int default_table_size = 1000)
: scene_limit(AABB<S>(scene_min, scene_max)),
hash_table(new HashTable(SpatialHash<S>(scene_limit, cell_size)))
{
hash_table->init(default_table_size);
}
unsigned int default_table_size = 1000);

~SpatialHashingCollisionManager()
{
delete hash_table;
}
~SpatialHashingCollisionManager();

/// @brief add one object to the manager
void registerObject(CollisionObject<S>* obj);
Expand Down Expand Up @@ -163,15 +112,7 @@ class SpatialHashingCollisionManager : public BroadPhaseCollisionManager<S>
size_t size() const;

/// @brief compute the bound for the environent
static void computeBound(std::vector<CollisionObject<S>*>& objs, Vector3<S>& l, Vector3<S>& u)
{
AABB<S> bound;
for(unsigned int i = 0; i < objs.size(); ++i)
bound += objs[i]->getAABB();

l = bound.min_;
u = bound.max_;
}
static void computeBound(std::vector<CollisionObject<S>*>& objs, Vector3<S>& l, Vector3<S>& u);

protected:

Expand All @@ -181,7 +122,6 @@ class SpatialHashingCollisionManager : public BroadPhaseCollisionManager<S>
/// @brief perform distance computation between one object and all the objects belonging ot the manager
bool distance_(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback, S& min_dist) const;


/// @brief all objects in the scene
std::list<CollisionObject<S>*> objs;

Expand Down Expand Up @@ -213,7 +153,28 @@ using SpatialHashingCollisionManagerd = SpatialHashingCollisionManager<double, H

//==============================================================================
template<typename S, typename HashTable>
void SpatialHashingCollisionManager<S, HashTable>::registerObject(CollisionObject<S>* obj)
SpatialHashingCollisionManager<S, HashTable>::SpatialHashingCollisionManager(
S cell_size,
const Vector3<S>& scene_min,
const Vector3<S>& scene_max,
unsigned int default_table_size)
: scene_limit(AABB<S>(scene_min, scene_max)),
hash_table(new HashTable(SpatialHash<S>(scene_limit, cell_size)))
{
hash_table->init(default_table_size);
}

//==============================================================================
template<typename S, typename HashTable>
SpatialHashingCollisionManager<S, HashTable>::~SpatialHashingCollisionManager()
{
delete hash_table;
}

//==============================================================================
template<typename S, typename HashTable>
void SpatialHashingCollisionManager<S, HashTable>::registerObject(
CollisionObject<S>* obj)
{
objs.push_back(obj);

Expand All @@ -233,6 +194,7 @@ void SpatialHashingCollisionManager<S, HashTable>::registerObject(CollisionObjec
obj_aabb_map[obj] = obj_aabb;
}

//==============================================================================
template<typename S, typename HashTable>
void SpatialHashingCollisionManager<S, HashTable>::unregisterObject(CollisionObject<S>* obj)
{
Expand All @@ -254,10 +216,14 @@ void SpatialHashingCollisionManager<S, HashTable>::unregisterObject(CollisionObj
obj_aabb_map.erase(obj);
}

//==============================================================================
template<typename S, typename HashTable>
void SpatialHashingCollisionManager<S, HashTable>::setup()
{}
{
// Do nothing
}

//==============================================================================
template<typename S, typename HashTable>
void SpatialHashingCollisionManager<S, HashTable>::update()
{
Expand All @@ -284,6 +250,7 @@ void SpatialHashingCollisionManager<S, HashTable>::update()
}
}

//==============================================================================
template<typename S, typename HashTable>
void SpatialHashingCollisionManager<S, HashTable>::update(CollisionObject<S>* updated_obj)
{
Expand Down Expand Up @@ -315,14 +282,15 @@ void SpatialHashingCollisionManager<S, HashTable>::update(CollisionObject<S>* up
obj_aabb_map[updated_obj] = new_aabb;
}

//==============================================================================
template<typename S, typename HashTable>
void SpatialHashingCollisionManager<S, HashTable>::update(const std::vector<CollisionObject<S>*>& updated_objs)
{
for(size_t i = 0; i < updated_objs.size(); ++i)
update(updated_objs[i]);
}


//==============================================================================
template<typename S, typename HashTable>
void SpatialHashingCollisionManager<S, HashTable>::clear()
{
Expand All @@ -332,20 +300,23 @@ void SpatialHashingCollisionManager<S, HashTable>::clear()
obj_aabb_map.clear();
}

//==============================================================================
template<typename S, typename HashTable>
void SpatialHashingCollisionManager<S, HashTable>::getObjects(std::vector<CollisionObject<S>*>& objs_) const
{
objs_.resize(objs.size());
std::copy(objs.begin(), objs.end(), objs_.begin());
}

//==============================================================================
template<typename S, typename HashTable>
void SpatialHashingCollisionManager<S, HashTable>::collide(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const
{
if(size() == 0) return;
collide_(obj, cdata, callback);
}

//==============================================================================
template<typename S, typename HashTable>
void SpatialHashingCollisionManager<S, HashTable>::distance(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback) const
{
Expand All @@ -354,6 +325,7 @@ void SpatialHashingCollisionManager<S, HashTable>::distance(CollisionObject<S>*
distance_(obj, cdata, callback, min_dist);
}

//==============================================================================
template<typename S, typename HashTable>
bool SpatialHashingCollisionManager<S, HashTable>::collide_(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const
{
Expand Down Expand Up @@ -393,6 +365,7 @@ bool SpatialHashingCollisionManager<S, HashTable>::collide_(CollisionObject<S>*
return false;
}

//==============================================================================
template<typename S, typename HashTable>
bool SpatialHashingCollisionManager<S, HashTable>::distance_(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback, S& min_dist) const
{
Expand Down Expand Up @@ -509,6 +482,7 @@ bool SpatialHashingCollisionManager<S, HashTable>::distance_(CollisionObject<S>*
return false;
}

//==============================================================================
template<typename S, typename HashTable>
void SpatialHashingCollisionManager<S, HashTable>::collide(void* cdata, CollisionCallBack<S> callback) const
{
Expand Down Expand Up @@ -547,6 +521,7 @@ void SpatialHashingCollisionManager<S, HashTable>::collide(void* cdata, Collisio
}
}

//==============================================================================
template<typename S, typename HashTable>
void SpatialHashingCollisionManager<S, HashTable>::distance(void* cdata, DistanceCallBack<S> callback) const
{
Expand All @@ -564,6 +539,7 @@ void SpatialHashingCollisionManager<S, HashTable>::distance(void* cdata, Distanc
this->tested_set.clear();
}

//==============================================================================
template<typename S, typename HashTable>
void SpatialHashingCollisionManager<S, HashTable>::collide(BroadPhaseCollisionManager<S>* other_manager_, void* cdata, CollisionCallBack<S> callback) const
{
Expand All @@ -589,6 +565,7 @@ void SpatialHashingCollisionManager<S, HashTable>::collide(BroadPhaseCollisionMa
}
}

//==============================================================================
template<typename S, typename HashTable>
void SpatialHashingCollisionManager<S, HashTable>::distance(BroadPhaseCollisionManager<S>* other_manager_, void* cdata, DistanceCallBack<S> callback) const
{
Expand Down Expand Up @@ -616,18 +593,33 @@ void SpatialHashingCollisionManager<S, HashTable>::distance(BroadPhaseCollisionM
}
}

//==============================================================================
template<typename S, typename HashTable>
bool SpatialHashingCollisionManager<S, HashTable>::empty() const
{
return objs.empty();
}

//==============================================================================
template<typename S, typename HashTable>
size_t SpatialHashingCollisionManager<S, HashTable>::size() const
{
return objs.size();
}

//==============================================================================
template<typename S, typename HashTable>
void SpatialHashingCollisionManager<S, HashTable>::computeBound(
std::vector<CollisionObject<S>*>& objs, Vector3<S>& l, Vector3<S>& u)
{
AABB<S> bound;
for(unsigned int i = 0; i < objs.size(); ++i)
bound += objs[i]->getAABB();

l = bound.min_;
u = bound.max_;
}

} // namespace fcl

#endif
43 changes: 0 additions & 43 deletions include/fcl/broadphase/broadphase_spatialhash.hxx
View file

This file was deleted.

0 comments on commit 6e97ef6

Please sign in to comment.