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
NearestPointQuery(struct PointQueryContext * context,cpShape * shape,cpCollisionID id,void * data)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
cpSpacePointQuery(cpSpace * space,cpVect point,cpFloat maxDistance,cpShapeFilter filter,cpSpacePointQueryFunc func,void * data)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
NearestPointQueryNearest(struct PointQueryContext * context,cpShape * shape,cpCollisionID id,cpPointQueryInfo * out)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 *
cpSpacePointQueryNearest(cpSpace * space,cpVect point,cpFloat maxDistance,cpShapeFilter filter,cpPointQueryInfo * out)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
SegmentQuery(struct SegmentQueryContext * context,cpShape * shape,void * data)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
cpSpaceSegmentQuery(cpSpace * space,cpVect start,cpVect end,cpFloat radius,cpShapeFilter filter,cpSpaceSegmentQueryFunc func,void * data)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
SegmentQueryFirst(struct SegmentQueryContext * context,cpShape * shape,cpSegmentQueryInfo * out)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 *
cpSpaceSegmentQueryFirst(cpSpace * space,cpVect start,cpVect end,cpFloat radius,cpShapeFilter filter,cpSegmentQueryInfo * out)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
BBQuery(struct BBQueryContext * context,cpShape * shape,cpCollisionID id,void * data)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
cpSpaceBBQuery(cpSpace * space,cpBB bb,cpShapeFilter filter,cpSpaceBBQueryFunc func,void * data)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
ShapeQuery(cpShape * a,cpShape * b,cpCollisionID id,struct ShapeQueryContext * context)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
cpSpaceShapeQuery(cpSpace * space,cpShape * shape,cpSpaceShapeQueryFunc func,void * data)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 }
247