physics library version update
[spacedolphin:spacedolphin.git] / lib / chipmunk / src / cpSpaceQuery.c
1 /* Copyright (c) 2007 Scott Lembcke
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_private.h"
23
24 //MARK: Point Query Functions
25
26 struct PointQueryContext {
27         cpVect point;
28         cpLayers layers;
29         cpGroup group;
30         cpSpacePointQueryFunc func;
31         void *data;
32 };
33
34 static void 
35 PointQuery(struct PointQueryContext *context, cpShape *shape, void *data)
36 {
37         if(
38                 !(shape->group && context->group == shape->group) && (context->layers&shape->layers) &&
39                 cpShapePointQuery(shape, context->point)
40         ){
41                 context->func(shape, context->data);
42         }
43 }
44
45 void
46 cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data)
47 {
48         struct PointQueryContext context = {point, layers, group, func, data};
49         cpBB bb = cpBBNewForCircle(point, 0.0f);
50         
51         cpSpaceLock(space); {
52     cpSpatialIndexQuery(space->activeShapes, &context, bb, (cpSpatialIndexQueryFunc)PointQuery, data);
53     cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)PointQuery, data);
54         } cpSpaceUnlock(space, cpTrue);
55 }
56
57 static void
58 PointQueryFirst(cpShape *shape, cpShape **outShape)
59 {
60         if(!shape->sensor) *outShape = shape;
61 }
62
63 cpShape *
64 cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, cpGroup group)
65 {
66         cpShape *shape = NULL;
67         cpSpacePointQuery(space, point, layers, group, (cpSpacePointQueryFunc)PointQueryFirst, &shape);
68         
69         return shape;
70 }
71
72 //MARK: Nearest Point Query Functions
73
74 struct NearestPointQueryContext {
75         cpVect point;
76         cpFloat maxDistance;
77         cpLayers layers;
78         cpGroup group;
79         cpSpaceNearestPointQueryFunc func;
80 };
81
82 static void 
83 NearestPointQuery(struct NearestPointQueryContext *context, cpShape *shape, void *data)
84 {
85         if(
86                 !(shape->group && context->group == shape->group) && (context->layers&shape->layers)
87         ){
88                 cpNearestPointQueryInfo info;
89                 cpShapeNearestPointQuery(shape, context->point, &info);
90                 
91                 if(info.shape && info.d < context->maxDistance) context->func(shape, info.d, info.p, data);
92         }
93 }
94
95 void
96 cpSpaceNearestPointQuery(cpSpace *space, cpVect point, cpFloat maxDistance, cpLayers layers, cpGroup group, cpSpaceNearestPointQueryFunc func, void *data)
97 {
98         struct NearestPointQueryContext context = {point, maxDistance, layers, group, func};
99         cpBB bb = cpBBNewForCircle(point, cpfmax(maxDistance, 0.0f));
100         
101         cpSpaceLock(space); {
102                 cpSpatialIndexQuery(space->activeShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQuery, data);
103                 cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQuery, data);
104         } cpSpaceUnlock(space, cpTrue);
105 }
106
107 static void
108 NearestPointQueryNearest(struct NearestPointQueryContext *context, cpShape *shape, cpNearestPointQueryInfo *out)
109 {
110         if(
111                 !(shape->group && context->group == shape->group) && (context->layers&shape->layers) && !shape->sensor
112         ){
113                 cpNearestPointQueryInfo info;
114                 cpShapeNearestPointQuery(shape, context->point, &info);
115                 
116                 if(info.d < out->d) (*out) = info;
117         }
118 }
119
120 cpShape *
121 cpSpaceNearestPointQueryNearest(cpSpace *space, cpVect point, cpFloat maxDistance, cpLayers layers, cpGroup group, cpNearestPointQueryInfo *out)
122 {
123         cpNearestPointQueryInfo info = {NULL, cpvzero, maxDistance};
124         if(out){
125                 (*out) = info;
126   } else {
127                 out = &info;
128         }
129         
130         struct NearestPointQueryContext context = {
131                 point, maxDistance,
132                 layers, group,
133                 NULL
134         };
135         
136         cpBB bb = cpBBNewForCircle(point, cpfmax(maxDistance, 0.0f));
137         cpSpatialIndexQuery(space->activeShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQueryNearest, out);
138         cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)NearestPointQueryNearest, out);
139         
140         return out->shape;
141 }
142
143
144 //MARK: Segment Query Functions
145
146 struct SegmentQueryContext {
147         cpVect start, end;
148         cpLayers layers;
149         cpGroup group;
150         cpSpaceSegmentQueryFunc func;
151 };
152
153 static cpFloat
154 SegmentQuery(struct SegmentQueryContext *context, cpShape *shape, void *data)
155 {
156         cpSegmentQueryInfo info;
157         
158         if(
159                 !(shape->group && context->group == shape->group) && (context->layers&shape->layers) &&
160                 cpShapeSegmentQuery(shape, context->start, context->end, &info)
161         ){
162                 context->func(shape, info.t, info.n, data);
163         }
164         
165         return 1.0f;
166 }
167
168 void
169 cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data)
170 {
171         struct SegmentQueryContext context = {
172                 start, end,
173                 layers, group,
174                 func,
175         };
176         
177         cpSpaceLock(space); {
178     cpSpatialIndexSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)SegmentQuery, data);
179     cpSpatialIndexSegmentQuery(space->activeShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)SegmentQuery, data);
180         } cpSpaceUnlock(space, cpTrue);
181 }
182
183 static cpFloat
184 SegmentQueryFirst(struct SegmentQueryContext *context, cpShape *shape, cpSegmentQueryInfo *out)
185 {
186         cpSegmentQueryInfo info;
187         
188         if(
189                 !(shape->group && context->group == shape->group) && (context->layers&shape->layers) &&
190                 !shape->sensor &&
191                 cpShapeSegmentQuery(shape, context->start, context->end, &info) &&
192                 info.t < out->t
193         ){
194                 (*out) = info;
195         }
196         
197         return out->t;
198 }
199
200 cpShape *
201 cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSegmentQueryInfo *out)
202 {
203         cpSegmentQueryInfo info = {NULL, 1.0f, cpvzero};
204         if(out){
205                 (*out) = info;
206   } else {
207                 out = &info;
208         }
209         
210         struct SegmentQueryContext context = {
211                 start, end,
212                 layers, group,
213                 NULL
214         };
215         
216         cpSpatialIndexSegmentQuery(space->staticShapes, &context, start, end, 1.0f, (cpSpatialIndexSegmentQueryFunc)SegmentQueryFirst, out);
217         cpSpatialIndexSegmentQuery(space->activeShapes, &context, start, end, out->t, (cpSpatialIndexSegmentQueryFunc)SegmentQueryFirst, out);
218         
219         return out->shape;
220 }
221
222 //MARK: BB Query Functions
223
224 struct BBQueryContext {
225         cpBB bb;
226         cpLayers layers;
227         cpGroup group;
228         cpSpaceBBQueryFunc func;
229 };
230
231 static void 
232 BBQuery(struct BBQueryContext *context, cpShape *shape, void *data)
233 {
234         if(
235                 !(shape->group && context->group == shape->group) && (context->layers&shape->layers) &&
236                 cpBBIntersects(context->bb, shape->bb)
237         ){
238                 context->func(shape, data);
239         }
240 }
241
242 void
243 cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data)
244 {
245         struct BBQueryContext context = {bb, layers, group, func};
246         
247         cpSpaceLock(space); {
248     cpSpatialIndexQuery(space->activeShapes, &context, bb, (cpSpatialIndexQueryFunc)BBQuery, data);
249     cpSpatialIndexQuery(space->staticShapes, &context, bb, (cpSpatialIndexQueryFunc)BBQuery, data);
250         } cpSpaceUnlock(space, cpTrue);
251 }
252
253 //MARK: Shape Query Functions
254
255 struct ShapeQueryContext {
256         cpSpaceShapeQueryFunc func;
257         void *data;
258         cpBool anyCollision;
259 };
260
261 // Callback from the spatial hash.
262 static void
263 ShapeQuery(cpShape *a, cpShape *b, struct ShapeQueryContext *context)
264 {
265         // Reject any of the simple cases
266         if(
267                 (a->group && a->group == b->group) ||
268                 !(a->layers & b->layers) ||
269                 a == b
270         ) return;
271         
272         cpContact contacts[CP_MAX_CONTACTS_PER_ARBITER];
273         int numContacts = 0;
274         
275         // Shape 'a' should have the lower shape type. (required by cpCollideShapes() )
276         if(a->klass->type <= b->klass->type){
277                 numContacts = cpCollideShapes(a, b, contacts);
278         } else {
279                 numContacts = cpCollideShapes(b, a, contacts);
280                 for(int i=0; i<numContacts; i++) contacts[i].n = cpvneg(contacts[i].n);
281         }
282         
283         if(numContacts){
284                 context->anyCollision = !(a->sensor || b->sensor);
285                 
286                 if(context->func){
287                         cpContactPointSet set;
288                         set.count = numContacts;
289                         
290                         for(int i=0; i<set.count; i++){
291                                 set.points[i].point = contacts[i].p;
292                                 set.points[i].normal = contacts[i].n;
293                                 set.points[i].dist = contacts[i].dist;
294                         }
295                         
296                         context->func(b, &set, context->data);
297                 }
298         }
299 }
300
301 cpBool
302 cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, void *data)
303 {
304         cpBody *body = shape->body;
305         cpBB bb = (body ? cpShapeUpdate(shape, body->p, body->rot) : shape->bb);
306         struct ShapeQueryContext context = {func, data, cpFalse};
307         
308         cpSpaceLock(space); {
309     cpSpatialIndexQuery(space->activeShapes, shape, bb, (cpSpatialIndexQueryFunc)ShapeQuery, &context);
310     cpSpatialIndexQuery(space->staticShapes, shape, bb, (cpSpatialIndexQueryFunc)ShapeQuery, &context);
311         } cpSpaceUnlock(space, cpTrue);
312         
313         return context.anyCollision;
314 }