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 // TODO: make this generic so I can reuse it for constraints also.
25 static inline void
unthreadHelper(cpArbiter * arb,cpBody * body)26 unthreadHelper(cpArbiter *arb, cpBody *body)
27 {
28 struct cpArbiterThread *thread = cpArbiterThreadForBody(arb, body);
29 cpArbiter *prev = thread->prev;
30 cpArbiter *next = thread->next;
31
32 if(prev){
33 cpArbiterThreadForBody(prev, body)->next = next;
34 } else if(body->arbiterList == arb) {
35 // IFF prev is NULL and body->arbiterList == arb, is arb at the head of the list.
36 // This function may be called for an arbiter that was never in a list.
37 // In that case, we need to protect it from wiping out the body->arbiterList pointer.
38 body->arbiterList = next;
39 }
40
41 if(next) cpArbiterThreadForBody(next, body)->prev = prev;
42
43 thread->prev = NULL;
44 thread->next = NULL;
45 }
46
47 void
cpArbiterUnthread(cpArbiter * arb)48 cpArbiterUnthread(cpArbiter *arb)
49 {
50 unthreadHelper(arb, arb->body_a);
51 unthreadHelper(arb, arb->body_b);
52 }
53
cpArbiterIsFirstContact(const cpArbiter * arb)54 cpBool cpArbiterIsFirstContact(const cpArbiter *arb)
55 {
56 return arb->state == CP_ARBITER_STATE_FIRST_COLLISION;
57 }
58
cpArbiterIsRemoval(const cpArbiter * arb)59 cpBool cpArbiterIsRemoval(const cpArbiter *arb)
60 {
61 return arb->state == CP_ARBITER_STATE_INVALIDATED;
62 }
63
cpArbiterGetCount(const cpArbiter * arb)64 int cpArbiterGetCount(const cpArbiter *arb)
65 {
66 // Return 0 contacts if we are in a separate callback.
67 return (arb->state < CP_ARBITER_STATE_CACHED ? arb->count : 0);
68 }
69
70 cpVect
cpArbiterGetNormal(const cpArbiter * arb)71 cpArbiterGetNormal(const cpArbiter *arb)
72 {
73 return cpvmult(arb->n, arb->swapped ? -1.0f : 1.0);
74 }
75
76 cpVect
cpArbiterGetPointA(const cpArbiter * arb,int i)77 cpArbiterGetPointA(const cpArbiter *arb, int i)
78 {
79 cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter");
80 return cpvadd(arb->body_a->p, arb->contacts[i].r1);
81 }
82
83 cpVect
cpArbiterGetPointB(const cpArbiter * arb,int i)84 cpArbiterGetPointB(const cpArbiter *arb, int i)
85 {
86 cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter");
87 return cpvadd(arb->body_b->p, arb->contacts[i].r2);
88 }
89
90 cpFloat
cpArbiterGetDepth(const cpArbiter * arb,int i)91 cpArbiterGetDepth(const cpArbiter *arb, int i)
92 {
93 cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter");
94
95 struct cpContact *con = &arb->contacts[i];
96 return cpvdot(cpvadd(cpvsub(con->r2, con->r1), cpvsub(arb->body_b->p, arb->body_a->p)), arb->n);
97 }
98
99 cpContactPointSet
cpArbiterGetContactPointSet(const cpArbiter * arb)100 cpArbiterGetContactPointSet(const cpArbiter *arb)
101 {
102 cpContactPointSet set;
103 set.count = cpArbiterGetCount(arb);
104
105 cpBool swapped = arb->swapped;
106 cpVect n = arb->n;
107 set.normal = (swapped ? cpvneg(n) : n);
108
109 for(int i=0; i<set.count; i++){
110 // Contact points are relative to body CoGs;
111 cpVect p1 = cpvadd(arb->body_a->p, arb->contacts[i].r1);
112 cpVect p2 = cpvadd(arb->body_b->p, arb->contacts[i].r2);
113
114 set.points[i].pointA = (swapped ? p2 : p1);
115 set.points[i].pointB = (swapped ? p1 : p2);
116 set.points[i].distance = cpvdot(cpvsub(p2, p1), n);
117 }
118
119 return set;
120 }
121
122 void
cpArbiterSetContactPointSet(cpArbiter * arb,cpContactPointSet * set)123 cpArbiterSetContactPointSet(cpArbiter *arb, cpContactPointSet *set)
124 {
125 int count = set->count;
126 cpAssertHard(count == arb->count, "The number of contact points cannot be changed.");
127
128 cpBool swapped = arb->swapped;
129 arb->n = (swapped ? cpvneg(set->normal) : set->normal);
130
131 for(int i=0; i<count; i++){
132 // Convert back to CoG relative offsets.
133 cpVect p1 = set->points[i].pointA;
134 cpVect p2 = set->points[i].pointB;
135
136 arb->contacts[i].r1 = cpvsub(swapped ? p2 : p1, arb->body_a->p);
137 arb->contacts[i].r2 = cpvsub(swapped ? p1 : p2, arb->body_b->p);
138 }
139 }
140
141 cpVect
cpArbiterTotalImpulse(const cpArbiter * arb)142 cpArbiterTotalImpulse(const cpArbiter *arb)
143 {
144 struct cpContact *contacts = arb->contacts;
145 cpVect n = arb->n;
146 cpVect sum = cpvzero;
147
148 for(int i=0, count=cpArbiterGetCount(arb); i<count; i++){
149 struct cpContact *con = &contacts[i];
150 sum = cpvadd(sum, cpvrotate(n, cpv(con->jnAcc, con->jtAcc)));
151 }
152
153 return (arb->swapped ? sum : cpvneg(sum));
154 return cpvzero;
155 }
156
157 cpFloat
cpArbiterTotalKE(const cpArbiter * arb)158 cpArbiterTotalKE(const cpArbiter *arb)
159 {
160 cpFloat eCoef = (1 - arb->e)/(1 + arb->e);
161 cpFloat sum = 0.0;
162
163 struct cpContact *contacts = arb->contacts;
164 for(int i=0, count=cpArbiterGetCount(arb); i<count; i++){
165 struct cpContact *con = &contacts[i];
166 cpFloat jnAcc = con->jnAcc;
167 cpFloat jtAcc = con->jtAcc;
168
169 sum += eCoef*jnAcc*jnAcc/con->nMass + jtAcc*jtAcc/con->tMass;
170 }
171
172 return sum;
173 }
174
175 cpBool
cpArbiterIgnore(cpArbiter * arb)176 cpArbiterIgnore(cpArbiter *arb)
177 {
178 arb->state = CP_ARBITER_STATE_IGNORE;
179 return cpFalse;
180 }
181
182 cpFloat
cpArbiterGetRestitution(const cpArbiter * arb)183 cpArbiterGetRestitution(const cpArbiter *arb)
184 {
185 return arb->e;
186 }
187
188 void
cpArbiterSetRestitution(cpArbiter * arb,cpFloat restitution)189 cpArbiterSetRestitution(cpArbiter *arb, cpFloat restitution)
190 {
191 arb->e = restitution;
192 }
193
194 cpFloat
cpArbiterGetFriction(const cpArbiter * arb)195 cpArbiterGetFriction(const cpArbiter *arb)
196 {
197 return arb->u;
198 }
199
200 void
cpArbiterSetFriction(cpArbiter * arb,cpFloat friction)201 cpArbiterSetFriction(cpArbiter *arb, cpFloat friction)
202 {
203 arb->u = friction;
204 }
205
206 cpVect
cpArbiterGetSurfaceVelocity(cpArbiter * arb)207 cpArbiterGetSurfaceVelocity(cpArbiter *arb)
208 {
209 return cpvmult(arb->surface_vr, arb->swapped ? -1.0f : 1.0);
210 }
211
212 void
cpArbiterSetSurfaceVelocity(cpArbiter * arb,cpVect vr)213 cpArbiterSetSurfaceVelocity(cpArbiter *arb, cpVect vr)
214 {
215 arb->surface_vr = cpvmult(vr, arb->swapped ? -1.0f : 1.0);
216 }
217
218 cpDataPointer
cpArbiterGetUserData(const cpArbiter * arb)219 cpArbiterGetUserData(const cpArbiter *arb)
220 {
221 return arb->data;
222 }
223
224 void
cpArbiterSetUserData(cpArbiter * arb,cpDataPointer userData)225 cpArbiterSetUserData(cpArbiter *arb, cpDataPointer userData)
226 {
227 arb->data = userData;
228 }
229
230 void
cpArbiterGetShapes(const cpArbiter * arb,cpShape ** a,cpShape ** b)231 cpArbiterGetShapes(const cpArbiter *arb, cpShape **a, cpShape **b)
232 {
233 if(arb->swapped){
234 (*a) = (cpShape *)arb->b, (*b) = (cpShape *)arb->a;
235 } else {
236 (*a) = (cpShape *)arb->a, (*b) = (cpShape *)arb->b;
237 }
238 }
239
cpArbiterGetBodies(const cpArbiter * arb,cpBody ** a,cpBody ** b)240 void cpArbiterGetBodies(const cpArbiter *arb, cpBody **a, cpBody **b)
241 {
242 CP_ARBITER_GET_SHAPES(arb, shape_a, shape_b);
243 (*a) = shape_a->body;
244 (*b) = shape_b->body;
245 }
246
247 cpBool
cpArbiterCallWildcardBeginA(cpArbiter * arb,cpSpace * space)248 cpArbiterCallWildcardBeginA(cpArbiter *arb, cpSpace *space)
249 {
250 cpCollisionHandler *handler = arb->handlerA;
251 return handler->beginFunc(arb, space, handler->userData);
252 }
253
254 cpBool
cpArbiterCallWildcardBeginB(cpArbiter * arb,cpSpace * space)255 cpArbiterCallWildcardBeginB(cpArbiter *arb, cpSpace *space)
256 {
257 cpCollisionHandler *handler = arb->handlerB;
258 arb->swapped = !arb->swapped;
259 cpBool retval = handler->beginFunc(arb, space, handler->userData);
260 arb->swapped = !arb->swapped;
261 return retval;
262 }
263
264 cpBool
cpArbiterCallWildcardPreSolveA(cpArbiter * arb,cpSpace * space)265 cpArbiterCallWildcardPreSolveA(cpArbiter *arb, cpSpace *space)
266 {
267 cpCollisionHandler *handler = arb->handlerA;
268 return handler->preSolveFunc(arb, space, handler->userData);
269 }
270
271 cpBool
cpArbiterCallWildcardPreSolveB(cpArbiter * arb,cpSpace * space)272 cpArbiterCallWildcardPreSolveB(cpArbiter *arb, cpSpace *space)
273 {
274 cpCollisionHandler *handler = arb->handlerB;
275 arb->swapped = !arb->swapped;
276 cpBool retval = handler->preSolveFunc(arb, space, handler->userData);
277 arb->swapped = !arb->swapped;
278 return retval;
279 }
280
281 void
cpArbiterCallWildcardPostSolveA(cpArbiter * arb,cpSpace * space)282 cpArbiterCallWildcardPostSolveA(cpArbiter *arb, cpSpace *space)
283 {
284 cpCollisionHandler *handler = arb->handlerA;
285 handler->postSolveFunc(arb, space, handler->userData);
286 }
287
288 void
cpArbiterCallWildcardPostSolveB(cpArbiter * arb,cpSpace * space)289 cpArbiterCallWildcardPostSolveB(cpArbiter *arb, cpSpace *space)
290 {
291 cpCollisionHandler *handler = arb->handlerB;
292 arb->swapped = !arb->swapped;
293 handler->postSolveFunc(arb, space, handler->userData);
294 arb->swapped = !arb->swapped;
295 }
296
297 void
cpArbiterCallWildcardSeparateA(cpArbiter * arb,cpSpace * space)298 cpArbiterCallWildcardSeparateA(cpArbiter *arb, cpSpace *space)
299 {
300 cpCollisionHandler *handler = arb->handlerA;
301 handler->separateFunc(arb, space, handler->userData);
302 }
303
304 void
cpArbiterCallWildcardSeparateB(cpArbiter * arb,cpSpace * space)305 cpArbiterCallWildcardSeparateB(cpArbiter *arb, cpSpace *space)
306 {
307 cpCollisionHandler *handler = arb->handlerB;
308 arb->swapped = !arb->swapped;
309 handler->separateFunc(arb, space, handler->userData);
310 arb->swapped = !arb->swapped;
311 }
312
313 cpArbiter*
cpArbiterInit(cpArbiter * arb,cpShape * a,cpShape * b)314 cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b)
315 {
316 arb->handler = NULL;
317 arb->swapped = cpFalse;
318
319 arb->handler = NULL;
320 arb->handlerA = NULL;
321 arb->handlerB = NULL;
322
323 arb->e = 0.0f;
324 arb->u = 0.0f;
325 arb->surface_vr = cpvzero;
326
327 arb->count = 0;
328 arb->contacts = NULL;
329
330 arb->a = a; arb->body_a = a->body;
331 arb->b = b; arb->body_b = b->body;
332
333 arb->thread_a.next = NULL;
334 arb->thread_b.next = NULL;
335 arb->thread_a.prev = NULL;
336 arb->thread_b.prev = NULL;
337
338 arb->stamp = 0;
339 arb->state = CP_ARBITER_STATE_FIRST_COLLISION;
340
341 arb->data = NULL;
342
343 return arb;
344 }
345
346 static inline cpCollisionHandler *
cpSpaceLookupHandler(cpSpace * space,cpCollisionType a,cpCollisionType b,cpCollisionHandler * defaultValue)347 cpSpaceLookupHandler(cpSpace *space, cpCollisionType a, cpCollisionType b, cpCollisionHandler *defaultValue)
348 {
349 cpCollisionType types[] = {a, b};
350 cpCollisionHandler *handler = (cpCollisionHandler *)cpHashSetFind(space->collisionHandlers, CP_HASH_PAIR(a, b), types);
351 return (handler ? handler : defaultValue);
352 }
353
354 void
cpArbiterUpdate(cpArbiter * arb,struct cpCollisionInfo * info,cpSpace * space)355 cpArbiterUpdate(cpArbiter *arb, struct cpCollisionInfo *info, cpSpace *space)
356 {
357 const cpShape *a = info->a, *b = info->b;
358
359 // For collisions between two similar primitive types, the order could have been swapped since the last frame.
360 arb->a = a; arb->body_a = a->body;
361 arb->b = b; arb->body_b = b->body;
362
363 // Iterate over the possible pairs to look for hash value matches.
364 for(int i=0; i<info->count; i++){
365 struct cpContact *con = &info->arr[i];
366
367 // r1 and r2 store absolute offsets at init time.
368 // Need to convert them to relative offsets.
369 con->r1 = cpvsub(con->r1, a->body->p);
370 con->r2 = cpvsub(con->r2, b->body->p);
371
372 // Cached impulses are not zeroed at init time.
373 con->jnAcc = con->jtAcc = 0.0f;
374
375 for(int j=0; j<arb->count; j++){
376 struct cpContact *old = &arb->contacts[j];
377
378 // This could trigger false positives, but is fairly unlikely nor serious if it does.
379 if(con->hash == old->hash){
380 // Copy the persistant contact information.
381 con->jnAcc = old->jnAcc;
382 con->jtAcc = old->jtAcc;
383 }
384 }
385 }
386
387 arb->contacts = info->arr;
388 arb->count = info->count;
389 arb->n = info->n;
390
391 arb->e = a->e * b->e;
392 arb->u = a->u * b->u;
393
394 cpVect surface_vr = cpvsub(b->surfaceV, a->surfaceV);
395 arb->surface_vr = cpvsub(surface_vr, cpvmult(info->n, cpvdot(surface_vr, info->n)));
396
397 cpCollisionType typeA = info->a->type, typeB = info->b->type;
398 cpCollisionHandler *defaultHandler = &space->defaultHandler;
399 cpCollisionHandler *handler = arb->handler = cpSpaceLookupHandler(space, typeA, typeB, defaultHandler);
400
401 // Check if the types match, but don't swap for a default handler which use the wildcard for type A.
402 cpBool swapped = arb->swapped = (typeA != handler->typeA && handler->typeA != CP_WILDCARD_COLLISION_TYPE);
403
404 if(handler != defaultHandler || space->usesWildcards){
405 // The order of the main handler swaps the wildcard handlers too. Uffda.
406 arb->handlerA = cpSpaceLookupHandler(space, (swapped ? typeB : typeA), CP_WILDCARD_COLLISION_TYPE, &cpCollisionHandlerDoNothing);
407 arb->handlerB = cpSpaceLookupHandler(space, (swapped ? typeA : typeB), CP_WILDCARD_COLLISION_TYPE, &cpCollisionHandlerDoNothing);
408 }
409
410 // mark it as new if it's been cached
411 if(arb->state == CP_ARBITER_STATE_CACHED) arb->state = CP_ARBITER_STATE_FIRST_COLLISION;
412 }
413
414 void
cpArbiterPreStep(cpArbiter * arb,cpFloat dt,cpFloat slop,cpFloat bias)415 cpArbiterPreStep(cpArbiter *arb, cpFloat dt, cpFloat slop, cpFloat bias)
416 {
417 cpBody *a = arb->body_a;
418 cpBody *b = arb->body_b;
419 cpVect n = arb->n;
420 cpVect body_delta = cpvsub(b->p, a->p);
421
422 for(int i=0; i<arb->count; i++){
423 struct cpContact *con = &arb->contacts[i];
424
425 // Calculate the mass normal and mass tangent.
426 con->nMass = 1.0f/k_scalar(a, b, con->r1, con->r2, n);
427 con->tMass = 1.0f/k_scalar(a, b, con->r1, con->r2, cpvperp(n));
428
429 // Calculate the target bias velocity.
430 cpFloat dist = cpvdot(cpvadd(cpvsub(con->r2, con->r1), body_delta), n);
431 con->bias = -bias*cpfmin(0.0f, dist + slop)/dt;
432 con->jBias = 0.0f;
433
434 // Calculate the target bounce velocity.
435 con->bounce = normal_relative_velocity(a, b, con->r1, con->r2, n)*arb->e;
436 }
437 }
438
439 void
cpArbiterApplyCachedImpulse(cpArbiter * arb,cpFloat dt_coef)440 cpArbiterApplyCachedImpulse(cpArbiter *arb, cpFloat dt_coef)
441 {
442 if(cpArbiterIsFirstContact(arb)) return;
443
444 cpBody *a = arb->body_a;
445 cpBody *b = arb->body_b;
446 cpVect n = arb->n;
447
448 for(int i=0; i<arb->count; i++){
449 struct cpContact *con = &arb->contacts[i];
450 cpVect j = cpvrotate(n, cpv(con->jnAcc, con->jtAcc));
451 apply_impulses(a, b, con->r1, con->r2, cpvmult(j, dt_coef));
452 }
453 }
454
455 // TODO: is it worth splitting velocity/position correction?
456
457 void
cpArbiterApplyImpulse(cpArbiter * arb)458 cpArbiterApplyImpulse(cpArbiter *arb)
459 {
460 cpBody *a = arb->body_a;
461 cpBody *b = arb->body_b;
462 cpVect n = arb->n;
463 cpVect surface_vr = arb->surface_vr;
464 cpFloat friction = arb->u;
465
466 for(int i=0; i<arb->count; i++){
467 struct cpContact *con = &arb->contacts[i];
468 cpFloat nMass = con->nMass;
469 cpVect r1 = con->r1;
470 cpVect r2 = con->r2;
471
472 cpVect vb1 = cpvadd(a->v_bias, cpvmult(cpvperp(r1), a->w_bias));
473 cpVect vb2 = cpvadd(b->v_bias, cpvmult(cpvperp(r2), b->w_bias));
474 cpVect vr = cpvadd(relative_velocity(a, b, r1, r2), surface_vr);
475
476 cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n);
477 cpFloat vrn = cpvdot(vr, n);
478 cpFloat vrt = cpvdot(vr, cpvperp(n));
479
480 cpFloat jbn = (con->bias - vbn)*nMass;
481 cpFloat jbnOld = con->jBias;
482 con->jBias = cpfmax(jbnOld + jbn, 0.0f);
483
484 cpFloat jn = -(con->bounce + vrn)*nMass;
485 cpFloat jnOld = con->jnAcc;
486 con->jnAcc = cpfmax(jnOld + jn, 0.0f);
487
488 cpFloat jtMax = friction*con->jnAcc;
489 cpFloat jt = -vrt*con->tMass;
490 cpFloat jtOld = con->jtAcc;
491 con->jtAcc = cpfclamp(jtOld + jt, -jtMax, jtMax);
492
493 apply_bias_impulses(a, b, r1, r2, cpvmult(n, con->jBias - jbnOld));
494 apply_impulses(a, b, r1, r2, cpvrotate(n, cpv(con->jnAcc - jnOld, con->jtAcc - jtOld)));
495 }
496 }
497