1 /* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
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:
10 * The above copyright notice and this permission notice shall be included in
11 * all copies or substantial portions of the Software.
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
22 #ifndef CHIPMUNK_PRIVATE_H
23 #define CHIPMUNK_PRIVATE_H
25 #include "chipmunk/chipmunk.h"
26 #include "chipmunk/chipmunk_structs.h"
28 #define CP_HASH_COEF (3344921057ul)
29 #define CP_HASH_PAIR(A, B) ((cpHashValue)(A)*CP_HASH_COEF ^ (cpHashValue)(B)*CP_HASH_COEF)
31 // TODO: Eww. Magic numbers.
32 #define MAGIC_EPSILON 1e-5
37 cpArray *cpArrayNew(int size);
39 void cpArrayFree(cpArray *arr);
41 void cpArrayPush(cpArray *arr, void *object);
42 void *cpArrayPop(cpArray *arr);
43 void cpArrayDeleteObj(cpArray *arr, void *obj);
44 cpBool cpArrayContains(cpArray *arr, void *ptr);
46 void cpArrayFreeEach(cpArray *arr, void (freeFunc)(void*));
51 typedef cpBool (*cpHashSetEqlFunc)(const void *ptr, const void *elt);
52 typedef void *(*cpHashSetTransFunc)(const void *ptr, void *data);
54 cpHashSet *cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc);
55 void cpHashSetSetDefaultValue(cpHashSet *set, void *default_value);
57 void cpHashSetFree(cpHashSet *set);
59 int cpHashSetCount(cpHashSet *set);
60 const void *cpHashSetInsert(cpHashSet *set, cpHashValue hash, const void *ptr, cpHashSetTransFunc trans, void *data);
61 const void *cpHashSetRemove(cpHashSet *set, cpHashValue hash, const void *ptr);
62 const void *cpHashSetFind(cpHashSet *set, cpHashValue hash, const void *ptr);
64 typedef void (*cpHashSetIteratorFunc)(void *elt, void *data);
65 void cpHashSetEach(cpHashSet *set, cpHashSetIteratorFunc func, void *data);
67 typedef cpBool (*cpHashSetFilterFunc)(void *elt, void *data);
68 void cpHashSetFilter(cpHashSet *set, cpHashSetFilterFunc func, void *data);
73 void cpBodyAddShape(cpBody *body, cpShape *shape);
74 void cpBodyRemoveShape(cpBody *body, cpShape *shape);
76 //void cpBodyAccumulateMassForShape(cpBody *body, cpShape *shape);
77 void cpBodyAccumulateMassFromShapes(cpBody *body);
79 void cpBodyRemoveConstraint(cpBody *body, cpConstraint *constraint);
82 //MARK: Spatial Index Functions
84 cpSpatialIndex *cpSpatialIndexInit(cpSpatialIndex *index, cpSpatialIndexClass *klass, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex);
89 cpArbiter* cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b);
91 static inline struct cpArbiterThread *
92 cpArbiterThreadForBody(cpArbiter *arb, cpBody *body)
94 return (arb->body_a == body ? &arb->thread_a : &arb->thread_b);
97 void cpArbiterUnthread(cpArbiter *arb);
99 void cpArbiterUpdate(cpArbiter *arb, struct cpCollisionInfo *info, cpSpace *space);
100 void cpArbiterPreStep(cpArbiter *arb, cpFloat dt, cpFloat bias, cpFloat slop);
101 void cpArbiterApplyCachedImpulse(cpArbiter *arb, cpFloat dt_coef);
102 void cpArbiterApplyImpulse(cpArbiter *arb);
105 //MARK: Shapes/Collisions
107 cpShape *cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body, struct cpShapeMassInfo massInfo);
110 cpShapeActive(cpShape *shape)
112 // checks if the shape is added to a shape list.
113 // TODO could this just check the space now?
114 return (shape->prev || (shape->body && shape->body->shapeList == shape));
117 // Note: This function returns contact points with r1/r2 in absolute coordinates, not body relative.
118 struct cpCollisionInfo cpCollide(const cpShape *a, const cpShape *b, cpCollisionID id, struct cpContact *contacts);
121 CircleSegmentQuery(cpShape *shape, cpVect center, cpFloat r1, cpVect a, cpVect b, cpFloat r2, cpSegmentQueryInfo *info)
123 cpVect da = cpvsub(a, center);
124 cpVect db = cpvsub(b, center);
125 cpFloat rsum = r1 + r2;
127 cpFloat qa = cpvdot(da, da) - 2.0f*cpvdot(da, db) + cpvdot(db, db);
128 cpFloat qb = cpvdot(da, db) - cpvdot(da, da);
129 cpFloat det = qb*qb - qa*(cpvdot(da, da) - rsum*rsum);
132 cpFloat t = (-qb - cpfsqrt(det))/(qa);
133 if(0.0f<= t && t <= 1.0f){
134 cpVect n = cpvnormalize(cpvlerp(da, db, t));
137 info->point = cpvsub(cpvlerp(a, b, t), cpvmult(n, r2));
145 cpShapeFilterReject(cpShapeFilter a, cpShapeFilter b)
147 // Reject the collision if:
149 // They are in the same non-zero group.
150 (a.group != 0 && a.group == b.group) ||
151 // One of the category/mask combinations fails.
152 (a.categories & b.mask) == 0 ||
153 (b.categories & a.mask) == 0
157 void cpLoopIndexes(const cpVect *verts, int count, int *start, int *end);
161 // TODO naming conventions here
163 void cpConstraintInit(cpConstraint *constraint, const struct cpConstraintClass *klass, cpBody *a, cpBody *b);
166 cpConstraintActivateBodies(cpConstraint *constraint)
168 cpBody *a = constraint->a; cpBodyActivate(a);
169 cpBody *b = constraint->b; cpBodyActivate(b);
173 relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2){
174 cpVect v1_sum = cpvadd(a->v, cpvmult(cpvperp(r1), a->w));
175 cpVect v2_sum = cpvadd(b->v, cpvmult(cpvperp(r2), b->w));
177 return cpvsub(v2_sum, v1_sum);
180 static inline cpFloat
181 normal_relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n){
182 return cpvdot(relative_velocity(a, b, r1, r2), n);
186 apply_impulse(cpBody *body, cpVect j, cpVect r){
187 body->v = cpvadd(body->v, cpvmult(j, body->m_inv));
188 body->w += body->i_inv*cpvcross(r, j);
192 apply_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j)
194 apply_impulse(a, cpvneg(j), r1);
195 apply_impulse(b, j, r2);
199 apply_bias_impulse(cpBody *body, cpVect j, cpVect r)
201 body->v_bias = cpvadd(body->v_bias, cpvmult(j, body->m_inv));
202 body->w_bias += body->i_inv*cpvcross(r, j);
206 apply_bias_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j)
208 apply_bias_impulse(a, cpvneg(j), r1);
209 apply_bias_impulse(b, j, r2);
212 static inline cpFloat
213 k_scalar_body(cpBody *body, cpVect r, cpVect n)
215 cpFloat rcn = cpvcross(r, n);
216 return body->m_inv + body->i_inv*rcn*rcn;
219 static inline cpFloat
220 k_scalar(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n)
222 cpFloat value = k_scalar_body(a, r1, n) + k_scalar_body(b, r2, n);
223 cpAssertSoft(value != 0.0, "Unsolvable collision or constraint.");
228 static inline cpMat2x2
229 k_tensor(cpBody *a, cpBody *b, cpVect r1, cpVect r2)
231 cpFloat m_sum = a->m_inv + b->m_inv;
233 // start with Identity*m_sum
234 cpFloat k11 = m_sum, k12 = 0.0f;
235 cpFloat k21 = 0.0f, k22 = m_sum;
237 // add the influence from r1
238 cpFloat a_i_inv = a->i_inv;
239 cpFloat r1xsq = r1.x * r1.x * a_i_inv;
240 cpFloat r1ysq = r1.y * r1.y * a_i_inv;
241 cpFloat r1nxy = -r1.x * r1.y * a_i_inv;
242 k11 += r1ysq; k12 += r1nxy;
243 k21 += r1nxy; k22 += r1xsq;
245 // add the influnce from r2
246 cpFloat b_i_inv = b->i_inv;
247 cpFloat r2xsq = r2.x * r2.x * b_i_inv;
248 cpFloat r2ysq = r2.y * r2.y * b_i_inv;
249 cpFloat r2nxy = -r2.x * r2.y * b_i_inv;
250 k11 += r2ysq; k12 += r2nxy;
251 k21 += r2nxy; k22 += r2xsq;
254 cpFloat det = k11*k22 - k12*k21;
255 cpAssertSoft(det != 0.0, "Unsolvable constraint.");
257 cpFloat det_inv = 1.0f/det;
259 k22*det_inv, -k12*det_inv,
260 -k21*det_inv, k11*det_inv
264 static inline cpFloat
265 bias_coef(cpFloat errorBias, cpFloat dt)
267 return 1.0f - cpfpow(errorBias, dt);
273 #define cpAssertSpaceUnlocked(space) \
274 cpAssertHard(!space->locked, \
275 "This operation cannot be done safely during a call to cpSpaceStep() or during a query. " \
276 "Put these calls into a post-step callback." \
279 void cpSpaceSetStaticBody(cpSpace *space, cpBody *body);
281 extern cpCollisionHandler cpCollisionHandlerDoNothing;
283 void cpSpaceProcessComponents(cpSpace *space, cpFloat dt);
285 void cpSpacePushFreshContactBuffer(cpSpace *space);
286 struct cpContact *cpContactBufferGetArray(cpSpace *space);
287 void cpSpacePushContacts(cpSpace *space, int count);
289 cpPostStepCallback *cpSpaceGetPostStepCallback(cpSpace *space, void *key);
291 cpBool cpSpaceArbiterSetFilter(cpArbiter *arb, cpSpace *space);
292 void cpSpaceFilterArbiters(cpSpace *space, cpBody *body, cpShape *filter);
294 void cpSpaceActivateBody(cpSpace *space, cpBody *body);
295 void cpSpaceLock(cpSpace *space);
296 void cpSpaceUnlock(cpSpace *space, cpBool runPostStep);
299 cpSpaceUncacheArbiter(cpSpace *space, cpArbiter *arb)
301 const cpShape *a = arb->a, *b = arb->b;
302 const cpShape *shape_pair[] = {a, b};
303 cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)a, (cpHashValue)b);
304 cpHashSetRemove(space->cachedArbiters, arbHashID, shape_pair);
305 cpArrayDeleteObj(space->arbiters, arb);
308 static inline cpArray *
309 cpSpaceArrayForBodyType(cpSpace *space, cpBodyType type)
311 return (type == CP_BODY_TYPE_STATIC ? space->staticBodies : space->dynamicBodies);
314 void cpShapeUpdateFunc(cpShape *shape, void *unused);
315 cpCollisionID cpSpaceCollideShapes(cpShape *a, cpShape *b, cpCollisionID id, cpSpace *space);
318 //MARK: Foreach loops
320 static inline cpConstraint *
321 cpConstraintNext(cpConstraint *node, cpBody *body)
323 return (node->a == body ? node->next_a : node->next_b);
326 #define CP_BODY_FOREACH_CONSTRAINT(bdy, var)\
327 for(cpConstraint *var = bdy->constraintList; var; var = cpConstraintNext(var, bdy))
329 static inline cpArbiter *
330 cpArbiterNext(cpArbiter *node, cpBody *body)
332 return (node->body_a == body ? node->thread_a.next : node->thread_b.next);
335 #define CP_BODY_FOREACH_ARBITER(bdy, var)\
336 for(cpArbiter *var = bdy->arbiterList; var; var = cpArbiterNext(var, bdy))
338 #define CP_BODY_FOREACH_SHAPE(body, var)\
339 for(cpShape *var = body->shapeList; var; var = var->next)
341 #define CP_BODY_FOREACH_COMPONENT(root, var)\
342 for(cpBody *var = root; var; var = var->sleeping.next)