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