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 }