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 <float.h>
23 #include <stdarg.h>
24
25 #include "chipmunk/chipmunk_private.h"
26
27 cpBody*
cpBodyAlloc(void)28 cpBodyAlloc(void)
29 {
30 return (cpBody *)cpcalloc(1, sizeof(cpBody));
31 }
32
33 cpBody *
cpBodyInit(cpBody * body,cpFloat mass,cpFloat moment)34 cpBodyInit(cpBody *body, cpFloat mass, cpFloat moment)
35 {
36 body->space = NULL;
37 body->shapeList = NULL;
38 body->arbiterList = NULL;
39 body->constraintList = NULL;
40
41 body->velocity_func = cpBodyUpdateVelocity;
42 body->position_func = cpBodyUpdatePosition;
43
44 body->sleeping.root = NULL;
45 body->sleeping.next = NULL;
46 body->sleeping.idleTime = 0.0f;
47
48 body->p = cpvzero;
49 body->v = cpvzero;
50 body->f = cpvzero;
51
52 body->w = 0.0f;
53 body->t = 0.0f;
54
55 body->v_bias = cpvzero;
56 body->w_bias = 0.0f;
57
58 body->userData = NULL;
59
60 // Setters must be called after full initialization so the sanity checks don't assert on garbage data.
61 cpBodySetMass(body, mass);
62 cpBodySetMoment(body, moment);
63 cpBodySetAngle(body, 0.0f);
64
65 return body;
66 }
67
68 cpBody*
cpBodyNew(cpFloat mass,cpFloat moment)69 cpBodyNew(cpFloat mass, cpFloat moment)
70 {
71 return cpBodyInit(cpBodyAlloc(), mass, moment);
72 }
73
74 cpBody*
cpBodyNewKinematic()75 cpBodyNewKinematic()
76 {
77 cpBody *body = cpBodyNew(0.0f, 0.0f);
78 cpBodySetType(body, CP_BODY_TYPE_KINEMATIC);
79
80 return body;
81 }
82
83 cpBody*
cpBodyNewStatic()84 cpBodyNewStatic()
85 {
86 cpBody *body = cpBodyNew(0.0f, 0.0f);
87 cpBodySetType(body, CP_BODY_TYPE_STATIC);
88
89 return body;
90 }
91
cpBodyDestroy(cpBody * body)92 void cpBodyDestroy(cpBody *body){}
93
94 void
cpBodyFree(cpBody * body)95 cpBodyFree(cpBody *body)
96 {
97 if(body){
98 cpBodyDestroy(body);
99 cpfree(body);
100 }
101 }
102
103 #ifdef NDEBUG
104 #define cpAssertSaneBody(body)
105 #else
cpv_assert_nan(cpVect v,char * message)106 static void cpv_assert_nan(cpVect v, char *message){cpAssertHard(v.x == v.x && v.y == v.y, message);}
cpv_assert_infinite(cpVect v,char * message)107 static void cpv_assert_infinite(cpVect v, char *message){cpAssertHard(cpfabs(v.x) != INFINITY && cpfabs(v.y) != INFINITY, message);}
cpv_assert_sane(cpVect v,char * message)108 static void cpv_assert_sane(cpVect v, char *message){cpv_assert_nan(v, message); cpv_assert_infinite(v, message);}
109
110 static void
cpBodySanityCheck(const cpBody * body)111 cpBodySanityCheck(const cpBody *body)
112 {
113 cpAssertHard(body->m == body->m && body->m_inv == body->m_inv, "Body's mass is NaN.");
114 cpAssertHard(body->i == body->i && body->i_inv == body->i_inv, "Body's moment is NaN.");
115 cpAssertHard(body->m >= 0.0f, "Body's mass is negative.");
116 cpAssertHard(body->i >= 0.0f, "Body's moment is negative.");
117
118 cpv_assert_sane(body->p, "Body's position is invalid.");
119 cpv_assert_sane(body->v, "Body's velocity is invalid.");
120 cpv_assert_sane(body->f, "Body's force is invalid.");
121
122 cpAssertHard(body->a == body->a && cpfabs(body->a) != INFINITY, "Body's angle is invalid.");
123 cpAssertHard(body->w == body->w && cpfabs(body->w) != INFINITY, "Body's angular velocity is invalid.");
124 cpAssertHard(body->t == body->t && cpfabs(body->t) != INFINITY, "Body's torque is invalid.");
125 }
126
127 #define cpAssertSaneBody(body) cpBodySanityCheck(body)
128 #endif
129
130 cpBool
cpBodyIsSleeping(const cpBody * body)131 cpBodyIsSleeping(const cpBody *body)
132 {
133 return (body->sleeping.root != ((cpBody*)0));
134 }
135
136 cpBodyType
cpBodyGetType(cpBody * body)137 cpBodyGetType(cpBody *body)
138 {
139 if(body->sleeping.idleTime == INFINITY){
140 return CP_BODY_TYPE_STATIC;
141 } else if(body->m == INFINITY){
142 return CP_BODY_TYPE_KINEMATIC;
143 } else {
144 return CP_BODY_TYPE_DYNAMIC;
145 }
146 }
147
148 void
cpBodySetType(cpBody * body,cpBodyType type)149 cpBodySetType(cpBody *body, cpBodyType type)
150 {
151 cpBodyType oldType = cpBodyGetType(body);
152 if(oldType == type) return;
153
154 // Static bodies have their idle timers set to infinity.
155 // Non-static bodies should have their idle timer reset.
156 body->sleeping.idleTime = (type == CP_BODY_TYPE_STATIC ? INFINITY : 0.0f);
157
158 if(type == CP_BODY_TYPE_DYNAMIC){
159 body->m = body->i = 0.0f;
160 body->m_inv = body->i_inv = INFINITY;
161
162 cpBodyAccumulateMassFromShapes(body);
163 } else {
164 body->m = body->i = INFINITY;
165 body->m_inv = body->i_inv = 0.0f;
166
167 body->v = cpvzero;
168 body->w = 0.0f;
169 }
170
171 // If the body is added to a space already, we'll need to update some space data structures.
172 cpSpace *space = cpBodyGetSpace(body);
173 if(space != NULL){
174 cpAssertSpaceUnlocked(space);
175
176 if(oldType == CP_BODY_TYPE_STATIC){
177 // TODO This is probably not necessary
178 // cpBodyActivateStatic(body, NULL);
179 } else {
180 cpBodyActivate(body);
181 }
182
183 // Move the bodies to the correct array.
184 cpArray *fromArray = cpSpaceArrayForBodyType(space, oldType);
185 cpArray *toArray = cpSpaceArrayForBodyType(space, type);
186 if(fromArray != toArray){
187 cpArrayDeleteObj(fromArray, body);
188 cpArrayPush(toArray, body);
189 }
190
191 // Move the body's shapes to the correct spatial index.
192 cpSpatialIndex *fromIndex = (oldType == CP_BODY_TYPE_STATIC ? space->staticShapes : space->dynamicShapes);
193 cpSpatialIndex *toIndex = (type == CP_BODY_TYPE_STATIC ? space->staticShapes : space->dynamicShapes);
194 if(fromIndex != toIndex){
195 CP_BODY_FOREACH_SHAPE(body, shape){
196 cpSpatialIndexRemove(fromIndex, shape, shape->hashid);
197 cpSpatialIndexInsert(toIndex, shape, shape->hashid);
198 }
199 }
200 }
201 }
202
203
204
205 // Should *only* be called when shapes with mass info are modified, added or removed.
206 void
cpBodyAccumulateMassFromShapes(cpBody * body)207 cpBodyAccumulateMassFromShapes(cpBody *body)
208 {
209 if(body == NULL || cpBodyGetType(body) != CP_BODY_TYPE_DYNAMIC) return;
210
211 // Reset the body's mass data.
212 body->m = body->i = 0.0f;
213 body->cog = cpvzero;
214
215 // Cache the position to realign it at the end.
216 cpVect pos = cpBodyGetPosition(body);
217
218 // Accumulate mass from shapes.
219 CP_BODY_FOREACH_SHAPE(body, shape){
220 struct cpShapeMassInfo *info = &shape->massInfo;
221 cpFloat m = info->m;
222
223 if(m > 0.0f){
224 cpFloat msum = body->m + m;
225
226 body->i += m*info->i + cpvdistsq(body->cog, info->cog)*(m*body->m)/msum;
227 body->cog = cpvlerp(body->cog, info->cog, m/msum);
228 body->m = msum;
229 }
230 }
231
232 // Recalculate the inverses.
233 body->m_inv = 1.0f/body->m;
234 body->i_inv = 1.0f/body->i;
235
236 // Realign the body since the CoG has probably moved.
237 cpBodySetPosition(body, pos);
238 cpAssertSaneBody(body);
239 }
240
241 cpSpace *
cpBodyGetSpace(const cpBody * body)242 cpBodyGetSpace(const cpBody *body)
243 {
244 return body->space;
245 }
246
247 cpFloat
cpBodyGetMass(const cpBody * body)248 cpBodyGetMass(const cpBody *body)
249 {
250 return body->m;
251 }
252
253 void
cpBodySetMass(cpBody * body,cpFloat mass)254 cpBodySetMass(cpBody *body, cpFloat mass)
255 {
256 cpAssertHard(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC, "You cannot set the mass of kinematic or static bodies.");
257 cpAssertHard(0.0f <= mass && mass < INFINITY, "Mass must be positive and finite.");
258
259 cpBodyActivate(body);
260 body->m = mass;
261 body->m_inv = 1.0f/mass;
262 cpAssertSaneBody(body);
263 }
264
265 cpFloat
cpBodyGetMoment(const cpBody * body)266 cpBodyGetMoment(const cpBody *body)
267 {
268 return body->i;
269 }
270
271 void
cpBodySetMoment(cpBody * body,cpFloat moment)272 cpBodySetMoment(cpBody *body, cpFloat moment)
273 {
274 cpAssertHard(moment >= 0.0f, "Moment of Inertia must be positive.");
275
276 cpBodyActivate(body);
277 body->i = moment;
278 body->i_inv = 1.0f/moment;
279 cpAssertSaneBody(body);
280 }
281
282 cpVect
cpBodyGetRotation(const cpBody * body)283 cpBodyGetRotation(const cpBody *body)
284 {
285 return cpv(body->transform.a, body->transform.b);
286 }
287
288 void
cpBodyAddShape(cpBody * body,cpShape * shape)289 cpBodyAddShape(cpBody *body, cpShape *shape)
290 {
291 cpShape *next = body->shapeList;
292 if(next) next->prev = shape;
293
294 shape->next = next;
295 body->shapeList = shape;
296
297 if(shape->massInfo.m > 0.0f){
298 cpBodyAccumulateMassFromShapes(body);
299 }
300 }
301
302 void
cpBodyRemoveShape(cpBody * body,cpShape * shape)303 cpBodyRemoveShape(cpBody *body, cpShape *shape)
304 {
305 cpShape *prev = shape->prev;
306 cpShape *next = shape->next;
307
308 if(prev){
309 prev->next = next;
310 } else {
311 body->shapeList = next;
312 }
313
314 if(next){
315 next->prev = prev;
316 }
317
318 shape->prev = NULL;
319 shape->next = NULL;
320
321 if(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC && shape->massInfo.m > 0.0f){
322 cpBodyAccumulateMassFromShapes(body);
323 }
324 }
325
326 static cpConstraint *
filterConstraints(cpConstraint * node,cpBody * body,cpConstraint * filter)327 filterConstraints(cpConstraint *node, cpBody *body, cpConstraint *filter)
328 {
329 if(node == filter){
330 return cpConstraintNext(node, body);
331 } else if(node->a == body){
332 node->next_a = filterConstraints(node->next_a, body, filter);
333 } else {
334 node->next_b = filterConstraints(node->next_b, body, filter);
335 }
336
337 return node;
338 }
339
340 void
cpBodyRemoveConstraint(cpBody * body,cpConstraint * constraint)341 cpBodyRemoveConstraint(cpBody *body, cpConstraint *constraint)
342 {
343 body->constraintList = filterConstraints(body->constraintList, body, constraint);
344 }
345
346 // 'p' is the position of the CoG
347 static void
SetTransform(cpBody * body,cpVect p,cpFloat a)348 SetTransform(cpBody *body, cpVect p, cpFloat a)
349 {
350 cpVect rot = cpvforangle(a);
351 cpVect c = body->cog;
352
353 body->transform = cpTransformNewTranspose(
354 rot.x, -rot.y, p.x - (c.x*rot.x - c.y*rot.y),
355 rot.y, rot.x, p.y - (c.x*rot.y + c.y*rot.x)
356 );
357 }
358
359 static inline cpFloat
SetAngle(cpBody * body,cpFloat a)360 SetAngle(cpBody *body, cpFloat a)
361 {
362 body->a = a;
363 cpAssertSaneBody(body);
364
365 return a;
366 }
367
368 cpVect
cpBodyGetPosition(const cpBody * body)369 cpBodyGetPosition(const cpBody *body)
370 {
371 return cpTransformPoint(body->transform, cpvzero);
372 }
373
374 void
cpBodySetPosition(cpBody * body,cpVect position)375 cpBodySetPosition(cpBody *body, cpVect position)
376 {
377 cpBodyActivate(body);
378 cpVect p = body->p = cpvadd(cpTransformVect(body->transform, body->cog), position);
379 cpAssertSaneBody(body);
380
381 SetTransform(body, p, body->a);
382 }
383
384 cpVect
cpBodyGetCenterOfGravity(const cpBody * body)385 cpBodyGetCenterOfGravity(const cpBody *body)
386 {
387 return body->cog;
388 }
389
390 void
cpBodySetCenterOfGravity(cpBody * body,cpVect cog)391 cpBodySetCenterOfGravity(cpBody *body, cpVect cog)
392 {
393 cpBodyActivate(body);
394 body->cog = cog;
395 cpAssertSaneBody(body);
396 }
397
398 cpVect
cpBodyGetVelocity(const cpBody * body)399 cpBodyGetVelocity(const cpBody *body)
400 {
401 return body->v;
402 }
403
404 void
cpBodySetVelocity(cpBody * body,cpVect velocity)405 cpBodySetVelocity(cpBody *body, cpVect velocity)
406 {
407 cpBodyActivate(body);
408 body->v = velocity;
409 cpAssertSaneBody(body);
410 }
411
412 cpVect
cpBodyGetForce(const cpBody * body)413 cpBodyGetForce(const cpBody *body)
414 {
415 return body->f;
416 }
417
418 void
cpBodySetForce(cpBody * body,cpVect force)419 cpBodySetForce(cpBody *body, cpVect force)
420 {
421 cpBodyActivate(body);
422 body->f = force;
423 cpAssertSaneBody(body);
424 }
425
426 cpFloat
cpBodyGetAngle(const cpBody * body)427 cpBodyGetAngle(const cpBody *body)
428 {
429 return body->a;
430 }
431
432 void
cpBodySetAngle(cpBody * body,cpFloat angle)433 cpBodySetAngle(cpBody *body, cpFloat angle)
434 {
435 cpBodyActivate(body);
436 SetAngle(body, angle);
437
438 SetTransform(body, body->p, angle);
439 }
440
441 cpFloat
cpBodyGetAngularVelocity(const cpBody * body)442 cpBodyGetAngularVelocity(const cpBody *body)
443 {
444 return body->w;
445 }
446
447 void
cpBodySetAngularVelocity(cpBody * body,cpFloat angularVelocity)448 cpBodySetAngularVelocity(cpBody *body, cpFloat angularVelocity)
449 {
450 cpBodyActivate(body);
451 body->w = angularVelocity;
452 cpAssertSaneBody(body);
453 }
454
455 cpFloat
cpBodyGetTorque(const cpBody * body)456 cpBodyGetTorque(const cpBody *body)
457 {
458 return body->t;
459 }
460
461 void
cpBodySetTorque(cpBody * body,cpFloat torque)462 cpBodySetTorque(cpBody *body, cpFloat torque)
463 {
464 cpBodyActivate(body);
465 body->t = torque;
466 cpAssertSaneBody(body);
467 }
468
469 cpDataPointer
cpBodyGetUserData(const cpBody * body)470 cpBodyGetUserData(const cpBody *body)
471 {
472 return body->userData;
473 }
474
475 void
cpBodySetUserData(cpBody * body,cpDataPointer userData)476 cpBodySetUserData(cpBody *body, cpDataPointer userData)
477 {
478 body->userData = userData;
479 }
480
481 void
cpBodySetVelocityUpdateFunc(cpBody * body,cpBodyVelocityFunc velocityFunc)482 cpBodySetVelocityUpdateFunc(cpBody *body, cpBodyVelocityFunc velocityFunc)
483 {
484 body->velocity_func = velocityFunc;
485 }
486
487 void
cpBodySetPositionUpdateFunc(cpBody * body,cpBodyPositionFunc positionFunc)488 cpBodySetPositionUpdateFunc(cpBody *body, cpBodyPositionFunc positionFunc)
489 {
490 body->position_func = positionFunc;
491 }
492
493 void
cpBodyUpdateVelocity(cpBody * body,cpVect gravity,cpFloat damping,cpFloat dt)494 cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt)
495 {
496 // Skip kinematic bodies.
497 if(cpBodyGetType(body) == CP_BODY_TYPE_KINEMATIC) return;
498
499 cpAssertSoft(body->m > 0.0f && body->i > 0.0f, "Body's mass and moment must be positive to simulate. (Mass: %f Moment: %f)", body->m, body->i);
500
501 body->v = cpvadd(cpvmult(body->v, damping), cpvmult(cpvadd(gravity, cpvmult(body->f, body->m_inv)), dt));
502 body->w = body->w*damping + body->t*body->i_inv*dt;
503
504 // Reset forces.
505 body->f = cpvzero;
506 body->t = 0.0f;
507
508 cpAssertSaneBody(body);
509 }
510
511 void
cpBodyUpdatePosition(cpBody * body,cpFloat dt)512 cpBodyUpdatePosition(cpBody *body, cpFloat dt)
513 {
514 cpVect p = body->p = cpvadd(body->p, cpvmult(cpvadd(body->v, body->v_bias), dt));
515 cpFloat a = SetAngle(body, body->a + (body->w + body->w_bias)*dt);
516 SetTransform(body, p, a);
517
518 body->v_bias = cpvzero;
519 body->w_bias = 0.0f;
520
521 cpAssertSaneBody(body);
522 }
523
524 cpVect
cpBodyLocalToWorld(const cpBody * body,const cpVect point)525 cpBodyLocalToWorld(const cpBody *body, const cpVect point)
526 {
527 return cpTransformPoint(body->transform, point);
528 }
529
530 cpVect
cpBodyWorldToLocal(const cpBody * body,const cpVect point)531 cpBodyWorldToLocal(const cpBody *body, const cpVect point)
532 {
533 return cpTransformPoint(cpTransformRigidInverse(body->transform), point);
534 }
535
536 void
cpBodyApplyForceAtWorldPoint(cpBody * body,cpVect force,cpVect point)537 cpBodyApplyForceAtWorldPoint(cpBody *body, cpVect force, cpVect point)
538 {
539 cpBodyActivate(body);
540 body->f = cpvadd(body->f, force);
541
542 cpVect r = cpvsub(point, cpTransformPoint(body->transform, body->cog));
543 body->t += cpvcross(r, force);
544 }
545
546 void
cpBodyApplyForceAtLocalPoint(cpBody * body,cpVect force,cpVect point)547 cpBodyApplyForceAtLocalPoint(cpBody *body, cpVect force, cpVect point)
548 {
549 cpBodyApplyForceAtWorldPoint(body, cpTransformVect(body->transform, force), cpTransformPoint(body->transform, point));
550 }
551
552 void
cpBodyApplyImpulseAtWorldPoint(cpBody * body,cpVect impulse,cpVect point)553 cpBodyApplyImpulseAtWorldPoint(cpBody *body, cpVect impulse, cpVect point)
554 {
555 cpBodyActivate(body);
556
557 cpVect r = cpvsub(point, cpTransformPoint(body->transform, body->cog));
558 apply_impulse(body, impulse, r);
559 }
560
561 void
cpBodyApplyImpulseAtLocalPoint(cpBody * body,cpVect impulse,cpVect point)562 cpBodyApplyImpulseAtLocalPoint(cpBody *body, cpVect impulse, cpVect point)
563 {
564 cpBodyApplyImpulseAtWorldPoint(body, cpTransformVect(body->transform, impulse), cpTransformPoint(body->transform, point));
565 }
566
567 cpVect
cpBodyGetVelocityAtLocalPoint(const cpBody * body,cpVect point)568 cpBodyGetVelocityAtLocalPoint(const cpBody *body, cpVect point)
569 {
570 cpVect r = cpTransformVect(body->transform, cpvsub(point, body->cog));
571 return cpvadd(body->v, cpvmult(cpvperp(r), body->w));
572 }
573
574 cpVect
cpBodyGetVelocityAtWorldPoint(const cpBody * body,cpVect point)575 cpBodyGetVelocityAtWorldPoint(const cpBody *body, cpVect point)
576 {
577 cpVect r = cpvsub(point, cpTransformPoint(body->transform, body->cog));
578 return cpvadd(body->v, cpvmult(cpvperp(r), body->w));
579 }
580
581 cpFloat
cpBodyKineticEnergy(const cpBody * body)582 cpBodyKineticEnergy(const cpBody *body)
583 {
584 // Need to do some fudging to avoid NaNs
585 cpFloat vsq = cpvdot(body->v, body->v);
586 cpFloat wsq = body->w*body->w;
587 return (vsq ? vsq*body->m : 0.0f) + (wsq ? wsq*body->i : 0.0f);
588 }
589
590 void
cpBodyEachShape(cpBody * body,cpBodyShapeIteratorFunc func,void * data)591 cpBodyEachShape(cpBody *body, cpBodyShapeIteratorFunc func, void *data)
592 {
593 cpShape *shape = body->shapeList;
594 while(shape){
595 cpShape *next = shape->next;
596 func(body, shape, data);
597 shape = next;
598 }
599 }
600
601 void
cpBodyEachConstraint(cpBody * body,cpBodyConstraintIteratorFunc func,void * data)602 cpBodyEachConstraint(cpBody *body, cpBodyConstraintIteratorFunc func, void *data)
603 {
604 cpConstraint *constraint = body->constraintList;
605 while(constraint){
606 cpConstraint *next = cpConstraintNext(constraint, body);
607 func(body, constraint, data);
608 constraint = next;
609 }
610 }
611
612 void
cpBodyEachArbiter(cpBody * body,cpBodyArbiterIteratorFunc func,void * data)613 cpBodyEachArbiter(cpBody *body, cpBodyArbiterIteratorFunc func, void *data)
614 {
615 cpArbiter *arb = body->arbiterList;
616 while(arb){
617 cpArbiter *next = cpArbiterNext(arb, body);
618
619 cpBool swapped = arb->swapped; {
620 arb->swapped = (body == arb->body_b);
621 func(body, arb, data);
622 } arb->swapped = swapped;
623
624 arb = next;
625 }
626 }
627