4
|
1
|
|
2 // written in the D programming language
|
|
3
|
|
4 module chipmunkd.cpShape;
|
|
5
|
|
6 import chipmunkd.chipmunk;
|
|
7
|
|
8 import std.stdio;
|
|
9
|
|
10 struct cpSegmentQueryInfo {
|
|
11 cpShape *shape; // shape that was hit, null if no collision
|
|
12 cpFloat t; // Distance along query segment, will always be in the range [0, 1].
|
|
13 cpVect n; // normal of hit surface
|
|
14 }
|
|
15
|
|
16 // Enumeration of shape types.
|
|
17 enum cpShapeType{
|
|
18 CP_CIRCLE_SHAPE,
|
|
19 CP_SEGMENT_SHAPE,
|
|
20 CP_POLY_SHAPE,
|
|
21 CP_NUM_SHAPES
|
|
22 }
|
|
23
|
|
24 // Shape class. Holds function pointers and type data.
|
|
25 struct cpShapeClass {
|
|
26 cpShapeType type;
|
|
27
|
|
28 // Called by cpShapeCacheBB().
|
|
29 cpBB function(cpShape *shape, cpVect p, cpVect rot) cacheData;
|
|
30 // Called to by cpShapeDestroy().
|
|
31 void function(cpShape *shape) destroy;
|
|
32
|
|
33 // called by cpShapePointQuery().
|
|
34 cpBool function(cpShape *shape, cpVect p)pointQuery;
|
|
35
|
|
36 // called by cpShapeSegmentQuery()
|
|
37 void function(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info) segmentQuery;
|
|
38 }
|
|
39
|
|
40 // Basic shape struct that the others inherit from.
|
|
41 struct cpShape{
|
|
42 // The "class" of a shape as defined above
|
|
43 /+const+/ cpShapeClass *klass;
|
|
44
|
|
45 // cpBody that the shape is attached to.
|
|
46 cpBody* _body;
|
|
47
|
|
48 // Cached BBox for the shape.
|
|
49 cpBB bb;
|
|
50
|
|
51 // Sensors invoke callbacks, but do not generate collisions
|
|
52 cpBool sensor;
|
|
53
|
|
54 // *** Surface properties.
|
|
55
|
|
56 // Coefficient of restitution. (elasticity)
|
|
57 cpFloat e;
|
|
58 // Coefficient of friction.
|
|
59 cpFloat u;
|
|
60 // Surface velocity used when solving for friction.
|
|
61 cpVect surface_v;
|
|
62
|
|
63 // *** User Definable Fields
|
|
64
|
|
65 // User defined data pointer for the shape.
|
|
66 cpDataPointer data;
|
|
67
|
|
68 // User defined collision type for the shape.
|
|
69 cpCollisionType collision_type;
|
|
70 // User defined collision group for the shape.
|
|
71 cpGroup group;
|
|
72 // User defined layer bitmask for the shape.
|
|
73 cpLayers layers;
|
|
74
|
|
75 // *** Internally Used Fields
|
|
76
|
|
77 // Shapes form a linked list when added to space on a non-null body
|
|
78 cpShape* next;
|
|
79
|
|
80 // Unique id used as the hash value.
|
|
81 cpHashValue hashid;
|
|
82 }
|
|
83
|
|
84 //
|
|
85 //// Low level shape initialization func.
|
|
86 //cpShape* cpShapeInit(cpShape *shape, const struct cpShapeClass *klass, cpBody *body);
|
|
87 //
|
|
88 //// Basic destructor functions. (allocation functions are not shared)
|
|
89 //void cpShapeDestroy(cpShape *shape);
|
|
90 //void cpShapeFree(cpShape *shape);
|
|
91 //
|
|
92 //// Cache the BBox of the shape.
|
|
93 //cpBB cpShapeCacheBB(cpShape *shape);
|
|
94 //
|
|
95 //// Test if a point lies within a shape.
|
|
96 //cpBool cpShapePointQuery(cpShape *shape, cpVect p);
|
13
|
97
|
14
|
98 template CP_DefineShapeGetter(string _struct,string type,string member,string name)
|
|
99 {
|
|
100 enum CP_DefineShapeGetter =
|
|
101 type~" "~_struct~"Get"~name~"(cpShape *shape){"~
|
|
102 "assert(shape.klass == &"~_struct~"Class, \"shape is not a "~_struct~"\");"~
|
|
103 "return (cast("~_struct~"*)shape)."~member~";"~
|
|
104 "}";
|
|
105 }
|
4
|
106
|
|
107 // Circle shape structure.
|
|
108 struct cpCircleShape{
|
|
109 cpShape shape;
|
|
110
|
|
111 // Center in body space coordinates
|
|
112 cpVect c;
|
|
113 // Radius.
|
|
114 cpFloat r;
|
|
115
|
|
116 // Transformed center. (world space coordinates)
|
|
117 cpVect tc;
|
|
118 }
|
|
119
|
|
120 //// Basic allocation functions for cpCircleShape.
|
|
121 //cpCircleShape *cpCircleShapeAlloc(void);
|
|
122 //cpCircleShape *cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset);
|
|
123 //cpShape *cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset);
|
14
|
124
|
|
125 mixin(CP_DefineShapeGetter!("cpCircleShape", "cpVect", "c", "Offset"));
|
|
126 mixin(CP_DefineShapeGetter!("cpCircleShape", "cpFloat", "r", "Radius"));
|
4
|
127
|
|
128 // Segment shape structure.
|
|
129 struct cpSegmentShape{
|
|
130 cpShape shape;
|
|
131
|
|
132 // Endpoints and normal of the segment. (body space coordinates)
|
|
133 cpVect a, b, n;
|
|
134 // Radius of the segment. (Thickness)
|
|
135 cpFloat r;
|
|
136
|
|
137 // Transformed endpoints and normal. (world space coordinates)
|
|
138 cpVect ta, tb, tn;
|
|
139 }
|
|
140 //
|
|
141 //// Basic allocation functions for cpSegmentShape.
|
|
142 //cpSegmentShape* cpSegmentShapeAlloc(void);
|
|
143 //cpSegmentShape* cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat radius);
|
|
144 //cpShape* cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat radius);
|
14
|
145
|
|
146 mixin(CP_DefineShapeGetter!("cpSegmentShape", "cpVect", "a", "A"));
|
|
147 mixin(CP_DefineShapeGetter!("cpSegmentShape", "cpVect", "b", "B"));
|
|
148 mixin(CP_DefineShapeGetter!("cpSegmentShape", "cpVect", "n", "Normal"));
|
|
149 mixin(CP_DefineShapeGetter!("cpSegmentShape", "cpFloat", "r", "Radius"));
|
4
|
150 //
|
|
151 //// For determinism, you can reset the shape id counter.
|
|
152 //void cpResetShapeIdCounter(void);
|
|
153 //
|
|
154 //// Directed segment queries against individual shapes.
|
|
155 //void cpSegmentQueryInfoPrint(cpSegmentQueryInfo *info);
|
|
156 //
|
|
157 //cpBool cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info);
|
|
158 //
|
|
159 static cpVect
|
|
160 cpSegmentQueryHitPoint(const cpVect start, const cpVect end, const cpSegmentQueryInfo info)
|
|
161 {
|
|
162 return cpvlerp(start, end, info.t);
|
|
163 }
|
|
164
|
|
165 static cpFloat
|
|
166 cpSegmentQueryHitDist(const cpVect start, const cpVect end, const cpSegmentQueryInfo info)
|
|
167 {
|
|
168 return cpvdist(start, end)*info.t;
|
|
169 }
|
|
170
|
|
171 cpHashValue SHAPE_ID_COUNTER = 0;
|
|
172
|
|
173 void
|
|
174 cpResetShapeIdCounter()
|
|
175 {
|
|
176 SHAPE_ID_COUNTER = 0;
|
|
177 }
|
|
178
|
|
179 cpShape*
|
|
180 cpShapeInit(cpShape *shape, /+const+/ cpShapeClass *klass, cpBody *_body)
|
|
181 {
|
|
182 shape.klass = klass;
|
|
183
|
|
184 shape.hashid = SHAPE_ID_COUNTER;
|
|
185 SHAPE_ID_COUNTER++;
|
|
186
|
|
187 shape._body = _body;
|
|
188 shape.sensor = 0;
|
|
189
|
|
190 shape.e = 0.0f;
|
|
191 shape.u = 0.0f;
|
|
192 shape.surface_v = cpvzero;
|
|
193
|
|
194 shape.collision_type = 0;
|
|
195 shape.group = CP_NO_GROUP;
|
|
196 shape.layers = CP_ALL_LAYERS;
|
|
197
|
|
198 shape.data = null;
|
|
199 shape.next = null;
|
|
200
|
|
201 // cpShapeCacheBB(shape);
|
|
202
|
|
203 return shape;
|
|
204 }
|
|
205
|
|
206 void
|
|
207 cpShapeDestroy(cpShape *shape)
|
|
208 {
|
|
209 if(shape.klass.destroy) shape.klass.destroy(shape);
|
|
210 }
|
|
211
|
|
212 void
|
|
213 cpShapeFree(cpShape *shape)
|
|
214 {
|
|
215 if(shape){
|
|
216 cpShapeDestroy(shape);
|
|
217 cpfree(shape);
|
|
218 }
|
|
219 }
|
|
220
|
23
|
221 // TODO this function should really take a position and rotation explicitly and be renamed
|
4
|
222 cpBB
|
|
223 cpShapeCacheBB(cpShape *shape)
|
|
224 {
|
|
225 cpBody *_body = shape._body;
|
|
226
|
|
227 shape.bb = shape.klass.cacheData(shape, _body.p, _body.rot);
|
|
228 return shape.bb;
|
|
229 }
|
|
230
|
|
231 cpBool
|
|
232 cpShapePointQuery(cpShape *shape, cpVect p){
|
|
233 return shape.klass.pointQuery(shape, p);
|
|
234 }
|
|
235
|
|
236 cpBool
|
|
237 cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info){
|
|
238 cpSegmentQueryInfo blank = {null, 0.0f, cpvzero};
|
|
239 (*info) = blank;
|
|
240
|
|
241 shape.klass.segmentQuery(shape, a, b, info);
|
|
242 return (info.shape !is null);
|
|
243 }
|
|
244
|
|
245 void
|
|
246 cpSegmentQueryInfoPrint(cpSegmentQueryInfo *info)
|
|
247 {
|
13
|
248 writefln("Segment Query:");
|
|
249 writefln("\tt: %s", info.t);
|
4
|
250 // writefln("\tdist: %f\n", info.dist);
|
|
251 // writefln("\tpoint: %s\n", cpvstr(info.point));
|
13
|
252 writefln("\tn: %s", cpvstr(info.n));
|
4
|
253 }
|
|
254
|
|
255 cpCircleShape *
|
|
256 cpCircleShapeAlloc()
|
|
257 {
|
|
258 return cast(cpCircleShape *)cpcalloc(1, cpCircleShape.sizeof);
|
|
259 }
|
|
260
|
|
261 static cpBB
|
|
262 bbFromCircle(const cpVect c, const cpFloat r)
|
|
263 {
|
|
264 return cpBBNew(c.x-r, c.y-r, c.x+r, c.y+r);
|
|
265 }
|
|
266
|
|
267 static cpBB
|
|
268 cpCircleShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
|
|
269 {
|
|
270 cpCircleShape *circle = cast(cpCircleShape *)shape;
|
|
271
|
|
272 circle.tc = cpvadd(p, cpvrotate(circle.c, rot));
|
|
273 return bbFromCircle(circle.tc, circle.r);
|
|
274 }
|
|
275
|
|
276 static cpBool
|
|
277 cpCircleShapePointQuery(cpShape *shape, cpVect p){
|
|
278 cpCircleShape *circle = cast(cpCircleShape *)shape;
|
|
279 return cpvnear(circle.tc, p, circle.r);
|
|
280 }
|
|
281
|
|
282 static void
|
|
283 circleSegmentQuery(cpShape *shape, cpVect center, cpFloat r, cpVect a, cpVect b, cpSegmentQueryInfo *info)
|
|
284 {
|
|
285 // offset the line to be relative to the circle
|
|
286 a = cpvsub(a, center);
|
|
287 b = cpvsub(b, center);
|
|
288
|
|
289 cpFloat qa = cpvdot(a, a) - 2.0f*cpvdot(a, b) + cpvdot(b, b);
|
|
290 cpFloat qb = -2.0f*cpvdot(a, a) + 2.0f*cpvdot(a, b);
|
|
291 cpFloat qc = cpvdot(a, a) - r*r;
|
|
292
|
|
293 cpFloat det = qb*qb - 4.0f*qa*qc;
|
|
294
|
|
295 if(det >= 0.0f){
|
|
296 cpFloat t = (-qb - cpfsqrt(det))/(2.0f*qa);
|
|
297 if(0.0f<= t && t <= 1.0f){
|
|
298 info.shape = shape;
|
|
299 info.t = t;
|
|
300 info.n = cpvnormalize(cpvlerp(a, b, t));
|
|
301 }
|
|
302 }
|
|
303 }
|
|
304
|
|
305 static void
|
|
306 cpCircleShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
|
|
307 {
|
|
308 cpCircleShape *circle = cast(cpCircleShape *)shape;
|
|
309 circleSegmentQuery(shape, circle.tc, circle.r, a, b, info);
|
|
310 }
|
|
311
|
|
312 static /+const+/ cpShapeClass cpCircleShapeClass = {
|
|
313 cpShapeType.CP_CIRCLE_SHAPE,
|
|
314 &cpCircleShapeCacheData,
|
|
315 null,
|
|
316 &cpCircleShapePointQuery,
|
|
317 &cpCircleShapeSegmentQuery,
|
|
318 };
|
|
319
|
|
320 cpCircleShape *
|
|
321 cpCircleShapeInit(cpCircleShape *circle, cpBody *_body, cpFloat radius, cpVect offset)
|
|
322 {
|
|
323 circle.c = offset;
|
|
324 circle.r = radius;
|
|
325
|
|
326 cpShapeInit(cast(cpShape *)circle, &cpCircleShapeClass, _body);
|
|
327
|
|
328 return circle;
|
|
329 }
|
|
330
|
|
331 cpShape *
|
|
332 cpCircleShapeNew(cpBody *_body, cpFloat radius, cpVect offset)
|
|
333 {
|
|
334 return cast(cpShape *)cpCircleShapeInit(cpCircleShapeAlloc(), _body, radius, offset);
|
|
335 }
|
|
336
|
|
337 cpSegmentShape *
|
|
338 cpSegmentShapeAlloc()
|
|
339 {
|
|
340 return cast(cpSegmentShape *)cpcalloc(1, cpSegmentShape.sizeof);
|
|
341 }
|
|
342
|
|
343 static cpBB
|
|
344 cpSegmentShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
|
|
345 {
|
|
346 cpSegmentShape *seg = cast(cpSegmentShape *)shape;
|
|
347
|
|
348 seg.ta = cpvadd(p, cpvrotate(seg.a, rot));
|
|
349 seg.tb = cpvadd(p, cpvrotate(seg.b, rot));
|
|
350 seg.tn = cpvrotate(seg.n, rot);
|
|
351
|
|
352 cpFloat l,r,s,t;
|
|
353
|
|
354 if(seg.ta.x < seg.tb.x){
|
|
355 l = seg.ta.x;
|
|
356 r = seg.tb.x;
|
|
357 } else {
|
|
358 l = seg.tb.x;
|
|
359 r = seg.ta.x;
|
|
360 }
|
|
361
|
|
362 if(seg.ta.y < seg.tb.y){
|
|
363 s = seg.ta.y;
|
|
364 t = seg.tb.y;
|
|
365 } else {
|
|
366 s = seg.tb.y;
|
|
367 t = seg.ta.y;
|
|
368 }
|
|
369
|
|
370 cpFloat rad = seg.r;
|
|
371 return cpBBNew(l - rad, s - rad, r + rad, t + rad);
|
|
372 }
|
|
373
|
|
374 static cpBool
|
|
375 cpSegmentShapePointQuery(cpShape *shape, cpVect p){
|
|
376 if(!cpBBcontainsVect(shape.bb, p)) return cpFalse;
|
|
377
|
|
378 cpSegmentShape *seg = cast(cpSegmentShape *)shape;
|
|
379
|
|
380 // Calculate normal distance from segment.
|
|
381 cpFloat dn = cpvdot(seg.tn, p) - cpvdot(seg.ta, seg.tn);
|
|
382 cpFloat dist = cpfabs(dn) - seg.r;
|
|
383 if(dist > 0.0f) return cpFalse;
|
|
384
|
|
385 // Calculate tangential distance along segment.
|
|
386 cpFloat dt = -cpvcross(seg.tn, p);
|
|
387 cpFloat dtMin = -cpvcross(seg.tn, seg.ta);
|
|
388 cpFloat dtMax = -cpvcross(seg.tn, seg.tb);
|
|
389
|
|
390 // Decision tree to decide which feature of the segment to collide with.
|
|
391 if(dt <= dtMin){
|
|
392 if(dt < (dtMin - seg.r)){
|
|
393 return cpFalse;
|
|
394 } else {
|
|
395 return cpvlengthsq(cpvsub(seg.ta, p)) < (seg.r*seg.r);
|
|
396 }
|
|
397 } else {
|
|
398 if(dt < dtMax){
|
|
399 return cpTrue;
|
|
400 } else {
|
|
401 if(dt < (dtMax + seg.r)) {
|
|
402 return cpvlengthsq(cpvsub(seg.tb, p)) < (seg.r*seg.r);
|
|
403 } else {
|
|
404 return cpFalse;
|
|
405 }
|
|
406 }
|
|
407 }
|
|
408
|
|
409 return cpTrue;
|
|
410 }
|
|
411
|
|
412 static cpBool inUnitRange(cpFloat t){return (0.0f < t && t < 1.0f);}
|
|
413
|
|
414 static void
|
|
415 cpSegmentShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
|
|
416 {
|
|
417 // TODO this function could be optimized better.
|
|
418
|
|
419 cpSegmentShape *seg = cast(cpSegmentShape *)shape;
|
|
420 cpVect n = seg.tn;
|
|
421 // flip n if a is behind the axis
|
|
422 if(cpvdot(a, n) < cpvdot(seg.ta, n))
|
|
423 n = cpvneg(n);
|
|
424
|
|
425 cpFloat an = cpvdot(a, n);
|
|
426 cpFloat bn = cpvdot(b, n);
|
|
427
|
|
428 if(an != bn){
|
|
429 cpFloat d = cpvdot(seg.ta, n) + seg.r;
|
|
430 cpFloat t = (d - an)/(bn - an);
|
|
431
|
|
432 if(0.0f < t && t < 1.0f){
|
|
433 cpVect point = cpvlerp(a, b, t);
|
|
434 cpFloat dt = -cpvcross(seg.tn, point);
|
|
435 cpFloat dtMin = -cpvcross(seg.tn, seg.ta);
|
|
436 cpFloat dtMax = -cpvcross(seg.tn, seg.tb);
|
|
437
|
|
438 if(dtMin < dt && dt < dtMax){
|
|
439 info.shape = shape;
|
|
440 info.t = t;
|
|
441 info.n = n;
|
|
442
|
|
443 return; // don't continue on and check endcaps
|
|
444 }
|
|
445 }
|
|
446 }
|
|
447
|
|
448 if(seg.r) {
|
|
449 cpSegmentQueryInfo info1 = {null, 1.0f, cpvzero};
|
|
450 cpSegmentQueryInfo info2 = {null, 1.0f, cpvzero};
|
|
451 circleSegmentQuery(shape, seg.ta, seg.r, a, b, &info1);
|
|
452 circleSegmentQuery(shape, seg.tb, seg.r, a, b, &info2);
|
|
453
|
|
454 if(info1.t < info2.t){
|
|
455 (*info) = info1;
|
|
456 } else {
|
|
457 (*info) = info2;
|
|
458 }
|
|
459 }
|
|
460 }
|
|
461
|
|
462 static /+const+/ cpShapeClass cpSegmentShapeClass = {
|
|
463 cpShapeType.CP_SEGMENT_SHAPE,
|
|
464 &cpSegmentShapeCacheData,
|
|
465 null,
|
|
466 &cpSegmentShapePointQuery,
|
|
467 &cpSegmentShapeSegmentQuery,
|
|
468 };
|
|
469
|
|
470 cpSegmentShape *
|
|
471 cpSegmentShapeInit(cpSegmentShape *seg, cpBody *_body, cpVect a, cpVect b, cpFloat r)
|
|
472 {
|
|
473 seg.a = a;
|
|
474 seg.b = b;
|
|
475 seg.n = cpvperp(cpvnormalize(cpvsub(b, a)));
|
|
476
|
|
477 seg.r = r;
|
|
478
|
|
479 cpShapeInit(cast(cpShape *)seg, &cpSegmentShapeClass, _body);
|
|
480
|
|
481 return seg;
|
|
482 }
|
|
483
|
|
484 cpShape*
|
|
485 cpSegmentShapeNew(cpBody *_body, cpVect a, cpVect b, cpFloat r)
|
|
486 {
|
|
487 return cast(cpShape *)cpSegmentShapeInit(cpSegmentShapeAlloc(), _body, a, b, r);
|
|
488 }
|
|
489
|
|
490 // Unsafe API (chipmunk_unsafe.h)
|
|
491
|
|
492 void
|
|
493 cpCircleShapeSetRadius(cpShape *shape, cpFloat radius)
|
|
494 {
|
|
495 assert(shape.klass is &cpCircleShapeClass, "Shape is not a circle shape.");
|
|
496 cpCircleShape *circle = cast(cpCircleShape *)shape;
|
|
497
|
|
498 circle.r = radius;
|
|
499 }
|
|
500
|
|
501 void
|
|
502 cpCircleShapeSetOffset(cpShape *shape, cpVect offset)
|
|
503 {
|
|
504 assert(shape.klass is &cpCircleShapeClass, "Shape is not a circle shape.");
|
|
505 cpCircleShape *circle = cast(cpCircleShape *)shape;
|
|
506
|
|
507 circle.c = offset;
|
|
508 }
|
|
509
|
|
510 void
|
|
511 cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b)
|
|
512 {
|
|
513 assert(shape.klass is &cpSegmentShapeClass, "Shape is not a segment shape.");
|
|
514 cpSegmentShape *seg = cast(cpSegmentShape *)shape;
|
|
515
|
|
516 seg.a = a;
|
|
517 seg.b = b;
|
|
518 seg.n = cpvperp(cpvnormalize(cpvsub(b, a)));
|
|
519 }
|
|
520
|
|
521 void
|
|
522 cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius)
|
|
523 {
|
|
524 assert(shape.klass is &cpSegmentShapeClass, "Shape is not a segment shape.");
|
|
525 cpSegmentShape *seg = cast(cpSegmentShape *)shape;
|
|
526
|
|
527 seg.r = radius;
|
|
528 }
|