dchip.cpSpaceQuery source code

1 /*
2  * Copyright (c) 2007-2013 Scott Lembcke and Howling Moon Software
3  *
4  * Permission is hereby granted, free of charge, to any person obtaining a copy
5  * of this software and associated documentation files (the "Software"), to deal
6  * in the Software without restriction, including without limitation the rights
7  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8  * copies of the Software, and to permit persons to whom the Software is
9  * furnished to do so, subject to the following conditions:
10  *
11  * The above copyright notice and this permission notice shall be included in
12  * all copies or substantial portions of the Software.
13  *
14  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
17  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
18  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
19  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
20  * SOFTWARE.
21  */
22 module dchip.cpSpaceQuery;
23 
24 import dchip.chipmunk_private;
25 import dchip.chipmunk_types;
26 import dchip.cpArbiter;
27 import dchip.cpBB;
28 import dchip.cpCollision;
29 import dchip.cpBody;
30 import dchip.cpSpace;
31 import dchip.cpSpaceStep;
32 import dchip.cpSpatialIndex;
33 import dchip.cpShape;
34 import dchip.cpVect;
35 import dchip.util;
36 
37 struct PointQueryContext
38 {
39     cpVect point;
40     cpLayers layers;
41     cpGroup group;
42     cpSpacePointQueryFunc func;
43     void* data;
44 }
45 
46 cpCollisionID PointQuery(PointQueryContext* context, cpShape* shape, cpCollisionID id, void* data)
47 {
48     if (
49         !(shape.group && context.group == shape.group) && (context.layers & shape.layers) &&
50         cpShapePointQuery(shape, context.point)
51         )
52     {
53         context.func(shape, context.data);
54     }
55 
56     return id;
57 }
58 
59 void cpSpacePointQuery(cpSpace* space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void* data)
60 {
61     PointQueryContext context = { point, layers, group, func, data };
62     cpBB bb = cpBBNewForCircle(point, 0.0f);
63 
64     cpSpaceLock(space);
65     {
66         cpSpatialIndexQuery(space.activeShapes, &context, bb, safeCast!cpSpatialIndexQueryFunc(&PointQuery), data);
67         cpSpatialIndexQuery(space.staticShapes, &context, bb, safeCast!cpSpatialIndexQueryFunc(&PointQuery), data);
68     }
69     cpSpaceUnlock(space, cpTrue);
70 }
71 
72 void PointQueryFirst(cpShape* shape, cpShape** outShape)
73 {
74     if (!shape.sensor)
75         *outShape = shape;
76 }
77 
78 cpShape* cpSpacePointQueryFirst(cpSpace* space, cpVect point, cpLayers layers, cpGroup group)
79 {
80     cpShape* shape = null;
81     cpSpacePointQuery(space, point, layers, group, safeCast!cpSpacePointQueryFunc(&PointQueryFirst), &shape);
82 
83     return shape;
84 }
85 
86 //MARK: Nearest Point Query Functions
87 
88 struct NearestPointQueryContext
89 {
90     cpVect point;
91     cpFloat maxDistance = 0;
92     cpLayers layers;
93     cpGroup group;
94     cpSpaceNearestPointQueryFunc func;
95 }
96 
97 cpCollisionID NearestPointQuery(NearestPointQueryContext* context, cpShape* shape, cpCollisionID id, void* data)
98 {
99     if (
100         !(shape.group && context.group == shape.group) && (context.layers & shape.layers)
101         )
102     {
103         cpNearestPointQueryInfo info;
104         cpShapeNearestPointQuery(shape, context.point, &info);
105 
106         if (info.shape && info.d < context.maxDistance)
107             context.func(shape, info.d, info.p, data);
108     }
109 
110     return id;
111 }
112 
113 void cpSpaceNearestPointQuery(cpSpace* space, cpVect point, cpFloat maxDistance, cpLayers layers, cpGroup group, cpSpaceNearestPointQueryFunc func, void* data)
114 {
115     NearestPointQueryContext context = { point, maxDistance, layers, group, func };
116     cpBB bb = cpBBNewForCircle(point, cpfmax(maxDistance, 0.0f));
117 
118     cpSpaceLock(space);
119     {
120         cpSpatialIndexQuery(space.activeShapes, &context, bb, cast(cpSpatialIndexQueryFunc)&NearestPointQuery, data);
121         cpSpatialIndexQuery(space.staticShapes, &context, bb, cast(cpSpatialIndexQueryFunc)&NearestPointQuery, data);
122     }
123     cpSpaceUnlock(space, cpTrue);
124 }
125 
126 cpCollisionID NearestPointQueryNearest(NearestPointQueryContext* context, cpShape* shape, cpCollisionID id, cpNearestPointQueryInfo* out_)
127 {
128     if (
129         !(shape.group && context.group == shape.group) && (context.layers & shape.layers) && !shape.sensor
130         )
131     {
132         cpNearestPointQueryInfo info;
133         cpShapeNearestPointQuery(shape, context.point, &info);
134 
135         if (info.d < out_.d)
136             (*out_) = info;
137     }
138 
139     return id;
140 }
141 
142 cpShape* cpSpaceNearestPointQueryNearest(cpSpace* space, cpVect point, cpFloat maxDistance, cpLayers layers, cpGroup group, cpNearestPointQueryInfo* out_)
143 {
144     cpNearestPointQueryInfo info = { null, cpvzero, maxDistance, cpvzero };
145 
146     if (out_)
147     {
148         (*out_) = info;
149     }
150     else
151     {
152         out_ = &info;
153     }
154 
155     NearestPointQueryContext context = {
156         point, maxDistance,
157         layers, group,
158         null
159     };
160 
161     cpBB bb = cpBBNewForCircle(point, cpfmax(maxDistance, 0.0f));
162     cpSpatialIndexQuery(space.activeShapes, &context, bb, safeCast!cpSpatialIndexQueryFunc(&NearestPointQueryNearest), out_);
163     cpSpatialIndexQuery(space.staticShapes, &context, bb, safeCast!cpSpatialIndexQueryFunc(&NearestPointQueryNearest), out_);
164 
165     return out_.shape;
166 }
167 
168 //MARK: Segment Query Functions
169 
170 struct SegmentQueryContext
171 {
172     cpVect start, end;
173     cpLayers layers;
174     cpGroup group;
175     cpSpaceSegmentQueryFunc func;
176 }
177 
178 cpFloat SegmentQuery(SegmentQueryContext* context, cpShape* shape, void* data)
179 {
180     cpSegmentQueryInfo info;
181 
182     if (
183         !(shape.group && context.group == shape.group) && (context.layers & shape.layers) &&
184         cpShapeSegmentQuery(shape, context.start, context.end, &info)
185         )
186     {
187         context.func(shape, info.t, info.n, data);
188     }
189 
190     return 1.0f;
191 }
192 
193 void cpSpaceSegmentQuery(cpSpace* space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void* data)
194 {
195     SegmentQueryContext context = {
196         start, end,
197         layers, group,
198         func,
199     };
200 
201     cpSpaceLock(space);
202     {
203         cpSpatialIndexSegmentQuery(space.staticShapes, &context, start, end, 1.0f, safeCast!cpSpatialIndexSegmentQueryFunc(&SegmentQuery), data);
204         cpSpatialIndexSegmentQuery(space.activeShapes, &context, start, end, 1.0f, safeCast!cpSpatialIndexSegmentQueryFunc(&SegmentQuery), data);
205     }
206     cpSpaceUnlock(space, cpTrue);
207 }
208 
209 cpFloat SegmentQueryFirst(SegmentQueryContext* context, cpShape* shape, cpSegmentQueryInfo* out_)
210 {
211     cpSegmentQueryInfo info;
212 
213     if (
214         !(shape.group && context.group == shape.group) && (context.layers & shape.layers) &&
215         !shape.sensor &&
216         cpShapeSegmentQuery(shape, context.start, context.end, &info) &&
217         info.t < out_.t
218         )
219     {
220         (*out_) = info;
221     }
222 
223     return out_.t;
224 }
225 
226 cpShape* cpSpaceSegmentQueryFirst(cpSpace* space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSegmentQueryInfo* out_)
227 {
228     cpSegmentQueryInfo info = { null, 1.0f, cpvzero };
229 
230     if (out_)
231     {
232         (*out_) = info;
233     }
234     else
235     {
236         out_ = &info;
237     }
238 
239     SegmentQueryContext context = {
240         start, end,
241         layers, group,
242         null
243     };
244 
245     cpSpatialIndexSegmentQuery(space.staticShapes, &context, start, end, 1.0f, safeCast!cpSpatialIndexSegmentQueryFunc(&SegmentQueryFirst), out_);
246     cpSpatialIndexSegmentQuery(space.activeShapes, &context, start, end, out_.t, safeCast!cpSpatialIndexSegmentQueryFunc(&SegmentQueryFirst), out_);
247 
248     return out_.shape;
249 }
250 
251 //MARK: BB Query Functions
252 
253 struct BBQueryContext
254 {
255     cpBB bb;
256     cpLayers layers;
257     cpGroup group;
258     cpSpaceBBQueryFunc func;
259 }
260 
261 cpCollisionID BBQuery(BBQueryContext* context, cpShape* shape, cpCollisionID id, void* data)
262 {
263     if (
264         !(shape.group && context.group == shape.group) && (context.layers & shape.layers) &&
265         cpBBIntersects(context.bb, shape.bb)
266         )
267     {
268         context.func(shape, data);
269     }
270 
271     return id;
272 }
273 
274 void cpSpaceBBQuery(cpSpace* space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void* data)
275 {
276     BBQueryContext context = { bb, layers, group, func };
277 
278     cpSpaceLock(space);
279     {
280         cpSpatialIndexQuery(space.activeShapes, &context, bb, safeCast!cpSpatialIndexQueryFunc(&BBQuery), data);
281         cpSpatialIndexQuery(space.staticShapes, &context, bb, safeCast!cpSpatialIndexQueryFunc(&BBQuery), data);
282     }
283     cpSpaceUnlock(space, cpTrue);
284 }
285 
286 //MARK: Shape Query Functions
287 
288 struct ShapeQueryContext
289 {
290     cpSpaceShapeQueryFunc func;
291     void* data;
292     cpBool anyCollision;
293 }
294 
295 // Callback from the spatial hash.
296 cpCollisionID ShapeQuery(cpShape* a, cpShape* b, cpCollisionID id, ShapeQueryContext* context)
297 {
298     // Reject any of the simple cases
299     if (
300         (a.group && a.group == b.group) ||
301         !(a.layers & b.layers) ||
302         a == b
303         )
304         return id;
305 
306     cpContact[CP_MAX_CONTACTS_PER_ARBITER] contacts;
307     int numContacts = 0;
308 
309     // Shape 'a' should have the lower shape type. (required by cpCollideShapes() )
310     if (a.klass.type <= b.klass.type)
311     {
312         numContacts = cpCollideShapes(a, b, &id, contacts.ptr);
313     }
314     else
315     {
316         numContacts = cpCollideShapes(b, a, &id, contacts.ptr);
317 
318         for (int i = 0; i < numContacts; i++)
319             contacts[i].n = cpvneg(contacts[i].n);
320     }
321 
322     if (numContacts)
323     {
324         context.anyCollision = !(a.sensor || b.sensor);
325 
326         if (context.func)
327         {
328             cpContactPointSet set;
329             set.count = numContacts;
330 
331             for (int i = 0; i < set.count; i++)
332             {
333                 set.points[i].point  = contacts[i].p;
334                 set.points[i].normal = contacts[i].n;
335                 set.points[i].dist   = contacts[i].dist;
336             }
337 
338             context.func(b, &set, context.data);
339         }
340     }
341 
342     return id;
343 }
344 
345 cpBool cpSpaceShapeQuery(cpSpace* space, cpShape* shape, cpSpaceShapeQueryFunc func, void* data)
346 {
347     cpBody* body_ = shape.body_;
348     cpBB bb      = (body_ ? cpShapeUpdate(shape, body_.p, body_.rot) : shape.bb);
349     ShapeQueryContext context = { func, data, cpFalse };
350 
351     cpSpaceLock(space);
352     {
353         cpSpatialIndexQuery(space.activeShapes, shape, bb, safeCast!cpSpatialIndexQueryFunc(&ShapeQuery), &context);
354         cpSpatialIndexQuery(space.staticShapes, shape, bb, safeCast!cpSpatialIndexQueryFunc(&ShapeQuery), &context);
355     }
356     cpSpaceUnlock(space, cpTrue);
357 
358     return context.anyCollision;
359 }