dali-physics/third-party/chipmunk2d/src/cpSpaceQuery.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 //MARK: Nearest Point Query Functions
  25
  26 struct PointQueryContext {
  27         cpVect point;
  28         cpFloat maxDistance;
  29         cpShapeFilter filter;
  30         cpSpacePointQueryFunc func;
  31 };
  32
  33 static cpCollisionID
  34 NearestPointQuery(struct PointQueryContext *context, cpShape *shape, cpCollisionID id, void *data)
  35 {
  36         if(
  37                 !cpShapeFilterReject(shape->filter, context->filter)
  38         ){
  39                 cpPointQueryInfo info;
  40                 cpShapePointQuery(shape, context->point, &info);
  41
  42                 if(info.shape && info.distance < context->maxDistance) context->func(shape, info.point, info.distance, info.gradient, data);
  43         }
  44
  45         return id;
  46 }
  47
  48 void
  49 cpSpacePointQuery(cpSpace *space, cpVect point, cpFloat maxDistance, cpShapeFilter filter, cpSpacePointQueryFunc func, void *data)
  50 {
  51         struct PointQueryContext context = {point, maxDistance, filter, func};
  52         cpBB bb = cpBBNewForCircle(point, cpfmax(maxDistance, 0.0f));
  53
  54         cpSpaceLock(space); {
  55                 cpSpatialIndexQuery(space->dynamicShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQuery, data);
  56                 cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQuery, data);
  57         } cpSpaceUnlock(space, cpTrue);
  58 }
  59
  60 static cpCollisionID
  61 NearestPointQueryNearest(struct PointQueryContext *context, cpShape *shape, cpCollisionID id, cpPointQueryInfo *out)
  62 {
  63         if(
  64                 !cpShapeFilterReject(shape->filter, context->filter) && !shape->sensor
  65         ){
  66                 cpPointQueryInfo info;
  67                 cpShapePointQuery(shape, context->point, &info);
  68
  69                 if(info.distance < out->distance) (*out) = info;
  70         }
  71
  72         return id;
  73 }
  74
  75 cpShape *
  76 cpSpacePointQueryNearest(cpSpace *space, cpVect point, cpFloat maxDistance, cpShapeFilter filter, cpPointQueryInfo *out)
  77 {
  78         cpPointQueryInfo info = {NULL, cpvzero, maxDistance, cpvzero};
  79         if(out){
  80                 (*out) = info;
  81   } else {
  82                 out = &info;
  83         }
  84
  85         struct PointQueryContext context = {
  86                 point, maxDistance,
  87                 filter,
  88                 NULL
  89         };
  90
  91         cpBB bb = cpBBNewForCircle(point, cpfmax(maxDistance, 0.0f));
  92         cpSpatialIndexQuery(space->dynamicShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQueryNearest, out);
  93         cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQueryNearest, out);
  94
  95         return (cpShape *)out->shape;
  96 }
  97
  98
  99 //MARK: Segment Query Functions
 100
 101 struct SegmentQueryContext {
 102         cpVect start, end;
 103         cpFloat radius;
 104         cpShapeFilter filter;
 105         cpSpaceSegmentQueryFunc func;
 106 };
 107
 108 static cpFloat
 109 SegmentQuery(struct SegmentQueryContext *context, cpShape *shape, void *data)
 110 {
 111         cpSegmentQueryInfo info;
 112
 113         if(
 114                 !cpShapeFilterReject(shape->filter, context->filter) &&
 115                 cpShapeSegmentQuery(shape, context->start, context->end, context->radius, &info)
 116         ){
 117                 context->func(shape, info.point, info.normal, info.alpha, data);
 118         }
 119
 120         return 1.0f;
 121 }
 122
 123 void
 124 cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpFloat radius, cpShapeFilter filter, cpSpaceSegmentQueryFunc func, void *data)
 125 {
 126         struct SegmentQueryContext context = {
 127                 start, end,
 128                 radius,
 129                 filter,
 130                 func,
 131         };
 132
 133         cpSpaceLock(space); {
 134     cpSpatialIndexSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)SegmentQuery, data);
 135     cpSpatialIndexSegmentQuery(space->dynamicShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)SegmentQuery, data);
 136         } cpSpaceUnlock(space, cpTrue);
 137 }
 138
 139 static cpFloat
 140 SegmentQueryFirst(struct SegmentQueryContext *context, cpShape *shape, cpSegmentQueryInfo *out)
 141 {
 142         cpSegmentQueryInfo info;
 143
 144         if(
 145                 !cpShapeFilterReject(shape->filter, context->filter) && !shape->sensor &&
 146                 cpShapeSegmentQuery(shape, context->start, context->end, context->radius, &info) &&
 147                 info.alpha < out->alpha
 148         ){
 149                 (*out) = info;
 150         }
 151
 152         return out->alpha;
 153 }
 154
 155 cpShape *
 156 cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpFloat radius, cpShapeFilter filter, cpSegmentQueryInfo *out)
 157 {
 158         cpSegmentQueryInfo info = {NULL, end, cpvzero, 1.0f};
 159         if(out){
 160                 (*out) = info;
 161   } else {
 162                 out = &info;
 163         }
 164
 165         struct SegmentQueryContext context = {
 166                 start, end,
 167                 radius,
 168                 filter,
 169                 NULL
 170         };
 171
 172         cpSpatialIndexSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)SegmentQueryFirst, out);
 173         cpSpatialIndexSegmentQuery(space->dynamicShapes, &context, start, end, out->alpha, (cpSpatialIndexSegmentQueryFunc)SegmentQueryFirst, out);
 174
 175         return (cpShape *)out->shape;
 176 }
 177
 178 //MARK: BB Query Functions
 179
 180 struct BBQueryContext {
 181         cpBB bb;
 182         cpShapeFilter filter;
 183         cpSpaceBBQueryFunc func;
 184 };
 185
 186 static cpCollisionID
 187 BBQuery(struct BBQueryContext *context, cpShape *shape, cpCollisionID id, void *data)
 188 {
 189         if(
 190                 !cpShapeFilterReject(shape->filter, context->filter) &&
 191                 cpBBIntersects(context->bb, shape->bb)
 192         ){
 193                 context->func(shape, data);
 194         }
 195
 196         return id;
 197 }
 198
 199 void
 200 cpSpaceBBQuery(cpSpace *space, cpBB bb, cpShapeFilter filter, cpSpaceBBQueryFunc func, void *data)
 201 {
 202         struct BBQueryContext context = {bb, filter, func};
 203
 204         cpSpaceLock(space); {
 205     cpSpatialIndexQuery(space->dynamicShapes, &context, bb, (cpSpatialIndexQueryFunc)BBQuery, data);
 206     cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)BBQuery, data);
 207         } cpSpaceUnlock(space, cpTrue);
 208 }
 209
 210 //MARK: Shape Query Functions
 211
 212 struct ShapeQueryContext {
 213         cpSpaceShapeQueryFunc func;
 214         void *data;
 215         cpBool anyCollision;
 216 };
 217
 218 // Callback from the spatial hash.
 219 static cpCollisionID
 220 ShapeQuery(cpShape *a, cpShape *b, cpCollisionID id, struct ShapeQueryContext *context)
 221 {
 222         if(cpShapeFilterReject(a->filter, b->filter) || a == b) return id;
 223
 224         cpContactPointSet set = cpShapesCollide(a, b);
 225         if(set.count){
 226                 if(context->func) context->func(b, &set, context->data);
 227                 context->anyCollision = !(a->sensor || b->sensor);
 228         }
 229
 230         return id;
 231 }
 232
 233 cpBool
 234 cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, void *data)
 235 {
 236         cpBody *body = shape->body;
 237         cpBB bb = (body ? cpShapeUpdate(shape, body->transform) : shape->bb);
 238         struct ShapeQueryContext context = {func, data, cpFalse};
 239
 240         cpSpaceLock(space); {
 241     cpSpatialIndexQuery(space->dynamicShapes, shape, bb, (cpSpatialIndexQueryFunc)ShapeQuery, &context);
 242     cpSpatialIndexQuery(space->staticShapes, shape, bb, (cpSpatialIndexQueryFunc)ShapeQuery, &context);
 243         } cpSpaceUnlock(space, cpTrue);
 244
 245         return context.anyCollision;
 246 }