dali-physics/third-party/chipmunk2d/src/cpSweep1D.c

   1 /* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
   2  *
   3  * Permission is hereby granted, free of charge, to any person obtaining a copy
   4  * of this software and associated documentation files (the "Software"), to deal
   5  * in the Software without restriction, including without limitation the rights
   6  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   7  * copies of the Software, and to permit persons to whom the Software is
   8  * furnished to do so, subject to the following conditions:
   9  *
  10  * The above copyright notice and this permission notice shall be included in
  11  * all copies or substantial portions of the Software.
  12  *
  13  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  14  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  15  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  16  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  17  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  18  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  19  * SOFTWARE.
  20  */
  21
  22 #include "chipmunk/chipmunk_private.h"
  23
  24 static inline cpSpatialIndexClass *Klass();
  25
  26 //MARK: Basic Structures
  27
  28 typedef struct Bounds {
  29         cpFloat min, max;
  30 } Bounds;
  31
  32 typedef struct TableCell {
  33         void *obj;
  34         Bounds bounds;
  35 } TableCell;
  36
  37 struct cpSweep1D
  38 {
  39         cpSpatialIndex spatialIndex;
  40
  41         int num;
  42         int max;
  43         TableCell *table;
  44 };
  45
  46 static inline cpBool
  47 BoundsOverlap(Bounds a, Bounds b)
  48 {
  49         return (a.min <= b.max && b.min <= a.max);
  50 }
  51
  52 static inline Bounds
  53 BBToBounds(cpSweep1D *sweep, cpBB bb)
  54 {
  55         Bounds bounds = {bb.l, bb.r};
  56         return bounds;
  57 }
  58
  59 static inline TableCell
  60 MakeTableCell(cpSweep1D *sweep, void *obj)
  61 {
  62         TableCell cell = {obj, BBToBounds(sweep, sweep->spatialIndex.bbfunc(obj))};
  63         return cell;
  64 }
  65
  66 //MARK: Memory Management Functions
  67
  68 cpSweep1D *
  69 cpSweep1DAlloc(void)
  70 {
  71         return (cpSweep1D *)cpcalloc(1, sizeof(cpSweep1D));
  72 }
  73
  74 static void
  75 ResizeTable(cpSweep1D *sweep, int size)
  76 {
  77         sweep->max = size;
  78         sweep->table = (TableCell *)cprealloc(sweep->table, size*sizeof(TableCell));
  79 }
  80
  81 cpSpatialIndex *
  82 cpSweep1DInit(cpSweep1D *sweep, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex)
  83 {
  84         cpSpatialIndexInit((cpSpatialIndex *)sweep, Klass(), bbfunc, staticIndex);
  85
  86         sweep->num = 0;
  87         ResizeTable(sweep, 32);
  88
  89         return (cpSpatialIndex *)sweep;
  90 }
  91
  92 cpSpatialIndex *
  93 cpSweep1DNew(cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex)
  94 {
  95         return cpSweep1DInit(cpSweep1DAlloc(), bbfunc, staticIndex);
  96 }
  97
  98 static void
  99 cpSweep1DDestroy(cpSweep1D *sweep)
 100 {
 101         cpfree(sweep->table);
 102         sweep->table = NULL;
 103 }
 104
 105 //MARK: Misc
 106
 107 static int
 108 cpSweep1DCount(cpSweep1D *sweep)
 109 {
 110         return sweep->num;
 111 }
 112
 113 static void
 114 cpSweep1DEach(cpSweep1D *sweep, cpSpatialIndexIteratorFunc func, void *data)
 115 {
 116         TableCell *table = sweep->table;
 117         for(int i=0, count=sweep->num; i<count; i++) func(table[i].obj, data);
 118 }
 119
 120 static int
 121 cpSweep1DContains(cpSweep1D *sweep, void *obj, cpHashValue hashid)
 122 {
 123         TableCell *table = sweep->table;
 124         for(int i=0, count=sweep->num; i<count; i++){
 125                 if(table[i].obj == obj) return cpTrue;
 126         }
 127
 128         return cpFalse;
 129 }
 130
 131 //MARK: Basic Operations
 132
 133 static void
 134 cpSweep1DInsert(cpSweep1D *sweep, void *obj, cpHashValue hashid)
 135 {
 136         if(sweep->num == sweep->max) ResizeTable(sweep, sweep->max*2);
 137
 138         sweep->table[sweep->num] = MakeTableCell(sweep, obj);
 139         sweep->num++;
 140 }
 141
 142 static void
 143 cpSweep1DRemove(cpSweep1D *sweep, void *obj, cpHashValue hashid)
 144 {
 145         TableCell *table = sweep->table;
 146         for(int i=0, count=sweep->num; i<count; i++){
 147                 if(table[i].obj == obj){
 148                         int num = --sweep->num;
 149
 150                         table[i] = table[num];
 151                         table[num].obj = NULL;
 152
 153                         return;
 154                 }
 155         }
 156 }
 157
 158 //MARK: Reindexing Functions
 159
 160 static void
 161 cpSweep1DReindexObject(cpSweep1D *sweep, void *obj, cpHashValue hashid)
 162 {
 163         // Nothing to do here
 164 }
 165
 166 static void
 167 cpSweep1DReindex(cpSweep1D *sweep)
 168 {
 169         // Nothing to do here
 170         // Could perform a sort, but queries are not accelerated anyway.
 171 }
 172
 173 //MARK: Query Functions
 174
 175 static void
 176 cpSweep1DQuery(cpSweep1D *sweep, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data)
 177 {
 178         // Implementing binary search here would allow you to find an upper limit
 179         // but not a lower limit. Probably not worth the hassle.
 180
 181         Bounds bounds = BBToBounds(sweep, bb);
 182
 183         TableCell *table = sweep->table;
 184         for(int i=0, count=sweep->num; i<count; i++){
 185                 TableCell cell = table[i];
 186                 if(BoundsOverlap(bounds, cell.bounds) && obj != cell.obj) func(obj, cell.obj, 0, data);
 187         }
 188 }
 189
 190 static void
 191 cpSweep1DSegmentQuery(cpSweep1D *sweep, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data)
 192 {
 193         cpBB bb = cpBBExpand(cpBBNew(a.x, a.y, a.x, a.y), b);
 194         Bounds bounds = BBToBounds(sweep, bb);
 195
 196         TableCell *table = sweep->table;
 197         for(int i=0, count=sweep->num; i<count; i++){
 198                 TableCell cell = table[i];
 199                 if(BoundsOverlap(bounds, cell.bounds)) func(obj, cell.obj, data);
 200         }
 201 }
 202
 203 //MARK: Reindex/Query
 204
 205 static int
 206 TableSort(TableCell *a, TableCell *b)
 207 {
 208         return (a->bounds.min < b->bounds.min ? -1 : (a->bounds.min > b->bounds.min ? 1 : 0));
 209 }
 210
 211 static void
 212 cpSweep1DReindexQuery(cpSweep1D *sweep, cpSpatialIndexQueryFunc func, void *data)
 213 {
 214         TableCell *table = sweep->table;
 215         int count = sweep->num;
 216
 217         // Update bounds and sort
 218         for(int i=0; i<count; i++) table[i] = MakeTableCell(sweep, table[i].obj);
 219         qsort(table, count, sizeof(TableCell), (int (*)(const void *, const void *))TableSort); // TODO: use insertion sort instead
 220
 221         for(int i=0; i<count; i++){
 222                 TableCell cell = table[i];
 223                 cpFloat max = cell.bounds.max;
 224
 225                 for(int j=i+1; table[j].bounds.min < max && j<count; j++){
 226                         func(cell.obj, table[j].obj, 0, data);
 227                 }
 228         }
 229
 230         // Reindex query is also responsible for colliding against the static index.
 231         // Fortunately there is a helper function for that.
 232         cpSpatialIndexCollideStatic((cpSpatialIndex *)sweep, sweep->spatialIndex.staticIndex, func, data);
 233 }
 234
 235 static cpSpatialIndexClass klass = {
 236         (cpSpatialIndexDestroyImpl)cpSweep1DDestroy,
 237
 238         (cpSpatialIndexCountImpl)cpSweep1DCount,
 239         (cpSpatialIndexEachImpl)cpSweep1DEach,
 240         (cpSpatialIndexContainsImpl)cpSweep1DContains,
 241
 242         (cpSpatialIndexInsertImpl)cpSweep1DInsert,
 243         (cpSpatialIndexRemoveImpl)cpSweep1DRemove,
 244
 245         (cpSpatialIndexReindexImpl)cpSweep1DReindex,
 246         (cpSpatialIndexReindexObjectImpl)cpSweep1DReindexObject,
 247         (cpSpatialIndexReindexQueryImpl)cpSweep1DReindexQuery,
 248
 249         (cpSpatialIndexQueryImpl)cpSweep1DQuery,
 250         (cpSpatialIndexSegmentQueryImpl)cpSweep1DSegmentQuery,
 251 };
 252
 253 static inline cpSpatialIndexClass *Klass(){return &klass;}
 254