| | 1 | #ifndef SPH_SPATIALMAPPING1D_H |
| | 2 | #define SPH_SPATIALMAPPING1D_H |
| | 3 | |
| | 4 | #include "_SPH_Base.h" |
| | 5 | |
| | 6 | #include <vector> |
| | 7 | #include <algorithm> |
| | 8 | |
| | 9 | namespace Squishy { namespace SPH { namespace DataStructures { |
| | 10 | |
| | 11 | /** |
| | 12 | * Points from the center of an axis-aligned box of length 2 toward its max point. |
| | 13 | */ |
| | 14 | #define ToMax Vector(1, 1, 1) |
| | 15 | |
| | 16 | // Reference: "Optimized Spatial Hashing for Collision Detection of Deformable Objects" |
| | 17 | // TODO: Timestamp |
| | 18 | template<class T> |
| | 19 | class SpatialMapping1D |
| | 20 | { |
| | 21 | public: |
| | 22 | typedef std::vector<T> Bucket; |
| | 23 | typedef typename Bucket::iterator BucketIterator; |
| | 24 | typedef typename Bucket::const_iterator BucketIteratorConst; |
| | 25 | |
| | 26 | typedef std::vector<Bucket*> BucketList; |
| | 27 | typedef typename BucketList::iterator BucketListIterator; |
| | 28 | |
| | 29 | protected: |
| | 30 | Real cellLength; |
| | 31 | int bucketCount; |
| | 32 | Bucket* buckets; |
| | 33 | |
| | 34 | public: |
| | 35 | SpatialMapping1D(Real cellLength, int size = 10) : cellLength(cellLength), bucketCount(size) |
| | 36 | { |
| | 37 | buckets = new std::vector<T>[size]; |
| | 38 | } |
| | 39 | |
| | 40 | virtual ~SpatialMapping1D() |
| | 41 | { |
| | 42 | delete [] buckets; |
| | 43 | } |
| | 44 | |
| | 45 | inline Real GetCellLength() const { return cellLength; } |
| | 46 | void SetCellLength(Real l) { cellLength = l; } |
| | 47 | |
| | 48 | Vec3i GetIndex(const Vector& pos) const |
| | 49 | { |
| | 50 | // negative floating point numbers are actually rounded up -> Make sure, we always round down: |
| | 51 | return Vec3i(glm::floor(pos / cellLength)); |
| | 52 | } |
| | 53 | |
| | 54 | Vector GetPositionMin(const Vec3i& idx) const |
| | 55 | { |
| | 56 | return Vector(idx) * cellLength; |
| | 57 | } |
| | 58 | |
| | 59 | virtual int MapIndex(const Vec3i& idx) const = 0; |
| | 60 | |
| | 61 | int MapPos(const Vector& pos) const |
| | 62 | { |
| | 63 | Vec3i idx = Vec3i(glm::floor(pos/cellLength)); |
| | 64 | return MapIndex(idx); |
| | 65 | } |
| | 66 | |
| | 67 | inline Vector GetPosition(T obj) const |
| | 68 | { |
| | 69 | return obj->GetPosition(); |
| | 70 | } |
| | 71 | |
| | 72 | inline int GetMap3To1D(T obj) const |
| | 73 | { |
| | 74 | return obj->hash; |
| | 75 | } |
| | 76 | |
| | 77 | inline void SetMap3To1D(T obj, int hash) const |
| | 78 | { |
| | 79 | obj->hash = hash; |
| | 80 | } |
| | 81 | |
| | 82 | inline BucketIterator GetBucketIndex(Bucket& bucket, T obj) const |
| | 83 | { |
| | 84 | //return obj->hashIndex; |
| | 85 | for (BucketIterator it = bucket.begin(); it != bucket.end(); ++it) |
| | 86 | { |
| | 87 | if ((*it)->index == obj->index) |
| | 88 | { |
| | 89 | return it; |
| | 90 | } |
| | 91 | } |
| | 92 | assert(0); |
| | 93 | return BucketIterator(); |
| | 94 | } |
| | 95 | |
| | 96 | /*inline void SetIndex(T obj, int index) const |
| | 97 | { |
| | 98 | obj->hashIndex = index; |
| | 99 | }*/ |
| | 100 | |
| | 101 | /** |
| | 102 | * Adds a point-sized object into the bucket at the given position |
| | 103 | */ |
| | 104 | void AddPoint(T obj) |
| | 105 | { |
| | 106 | Vector pos = GetPosition(obj); |
| | 107 | int hash = MapPos(pos); |
| | 108 | AddPoint(obj, hash); |
| | 109 | } |
| | 110 | |
| | 111 | /** |
| | 112 | * Adds a point-sized object into the bucket at the given position |
| | 113 | */ |
| | 114 | void AddPoint(T obj, int hash) |
| | 115 | { |
| | 116 | Bucket& bucket = buckets[hash]; |
| | 117 | SetMap3To1D(obj, hash); |
| | 118 | bucket.push_back(obj); |
| | 119 | } |
| | 120 | |
| | 121 | void RemovePoint(T obj, int hash) |
| | 122 | { |
| | 123 | Bucket& bucket = buckets[hash]; |
| | 124 | bucket.erase(GetBucketIndex(bucket, obj)); |
| | 125 | } |
| | 126 | |
| | 127 | ///** |
| | 128 | // * Adds a sphere-shaped object with given radius into all buckets around the object's position |
| | 129 | // */ |
| | 130 | //void AddSphere(T obj, Real radius) |
| | 131 | //{ |
| | 132 | // Vector pos = GetPosition(obj); |
| | 133 | |
| | 134 | // Vector minv = pos - radius * ToMax; |
| | 135 | // Vector maxv = pos + radius * ToMax; |
| | 136 | // Vec3i minIndex = GetIndex(Vector(minv)); |
| | 137 | // Vec3i maxIndex = GetIndex(Vector(maxv)); |
| | 138 | |
| | 139 | // Vector cellSize(cellLength); |
| | 140 | |
| | 141 | // Vec3i idx; |
| | 142 | // Vector current; |
| | 143 | // for (idx.x = minIndex.x; idx.x <= maxIndex.x; ++idx.x) |
| | 144 | // { |
| | 145 | // current.x = idx.x * cellLength; |
| | 146 | // for (idx.y = minIndex.y; idx.y <= maxIndex.y; ++idx.y) |
| | 147 | // { |
| | 148 | // current.y = idx.y * cellLength; |
| | 149 | // for (idx.z = minIndex.z; idx.z <= maxIndex.z; ++idx.z) |
| | 150 | // { |
| | 151 | // current.z = idx.z * cellLength; |
| | 152 | |
| | 153 | // // TODO: Verify that intersection test is correct |
| | 154 | // if (IntersectSphereAABB(current, current + cellSize, pos, radius)) |
| | 155 | // { |
| | 156 | // Bucket& bucket = buckets[Map3To1D(idx)]; |
| | 157 | // // TODO: Don't add objects twice to the same bucket |
| | 158 | // bucket.push_back(obj); |
| | 159 | // } |
| | 160 | // } |
| | 161 | // } |
| | 162 | // } |
| | 163 | //} |
| | 164 | |
| | 165 | const Bucket& GetBucket(const Vec3i& idx) const |
| | 166 | { |
| | 167 | return buckets[MapIndex(idx)]; |
| | 168 | } |
| | 169 | |
| | 170 | const Bucket& GetBucket(const Vector& pos) const |
| | 171 | { |
| | 172 | return buckets[MapPos(pos)]; |
| | 173 | } |
| | 174 | |
| | 175 | /** |
| | 176 | * Gets all buckets that are intersected by a sphere of given radius, centered at the given position. |
| | 177 | */ |
| | 178 | void GetBuckets(const Vector& pos, Real radius, BucketList& list) const |
| | 179 | { |
| | 180 | Vector min = pos - radius * ToMax; |
| | 181 | Vector max = pos + radius * ToMax; |
| | 182 | |
| | 183 | Vector current; |
| | 184 | Vector cellSize(cellLength); |
| | 185 | |
| | 186 | int l = (max.x - min.x) / cellLength; |
| | 187 | for (int i = 0; i < l; ++i) |
| | 188 | { |
| | 189 | current.x = min.x + cellLength * i; |
| | 190 | for (int j = 0; j < l; ++j) |
| | 191 | { |
| | 192 | current.y = min.y + cellLength * j; |
| | 193 | for (int k = 0; k < l; ++k) |
| | 194 | { |
| | 195 | current.z = min.z + cellLength * k; |
| | 196 | |
| | 197 | if (IntersectSphereAABB(current, current + cellSize, pos, radius)) |
| | 198 | { |
| | 199 | list.push_back(buckets[Map3To1D(current)]); |
| | 200 | } |
| | 201 | } |
| | 202 | } |
| | 203 | } |
| | 204 | } |
| | 205 | |
| | 206 | /** |
| | 207 | * Wether the cell with the given index contains any objects |
| | 208 | */ |
| | 209 | bool IsOccupied(const Vec3i& idx) const |
| | 210 | { |
| | 211 | const Bucket& bucket = GetBucket(idx); |
| | 212 | |
| | 213 | //return !bucket.empty(); |
| | 214 | // iterate over all particles in the bucket and see whether any of them are inside the box at the given index |
| | 215 | for (BucketIteratorConst it = bucket.begin(); it != bucket.end(); ++it) |
| | 216 | { |
| | 217 | if (IsInCell(*it, idx)) |
| | 218 | { |
| | 219 | return true; |
| | 220 | } |
| | 221 | } |
| | 222 | return false; |
| | 223 | } |
| | 224 | |
| | 225 | /** |
| | 226 | * |
| | 227 | */ |
| | 228 | bool IsInCell(T obj, const Vec3i& idx) const |
| | 229 | { |
| | 230 | Vector minv = GetPositionMin(idx); |
| | 231 | Vector pos = GetPosition(obj) - minv; |
| | 232 | |
| | 233 | return pos.x >= 0 && pos.x <= cellLength && |
| | 234 | pos.y >= 0 && pos.y <= cellLength && |
| | 235 | pos.z >= 0 && pos.z <= cellLength; |
| | 236 | } |
| | 237 | |
| | 238 | /** |
| | 239 | * Clears all buckets |
| | 240 | */ |
| | 241 | void Clear() |
| | 242 | { |
| | 243 | int i = 0; |
| | 244 | for (Bucket* b = buckets; i < bucketCount; ++i, ++b) |
| | 245 | { |
| | 246 | b->clear(); |
| | 247 | } |
| | 248 | } |
| | 249 | |
| | 250 | /** |
| | 251 | * Deletes all buckets and creates a new set of them |
| | 252 | */ |
| | 253 | void Reset(int newSize = 10) |
| | 254 | { |
| | 255 | delete [] buckets; |
| | 256 | buckets = new std::vector<T>[bucketCount = newSize]; |
| | 257 | } |
| | 258 | }; |
| | 259 | |
| | 260 | } } } |
| | 261 | |
| | 262 | #endif |