dali-physics/third-party/chipmunk2d/include/chipmunk/chipmunk_private.h

   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 #ifndef CHIPMUNK_PRIVATE_H
  23 #define CHIPMUNK_PRIVATE_H
  24
  25 #include "chipmunk/chipmunk.h"
  26 #include "chipmunk/chipmunk_structs.h"
  27
  28 #define CP_HASH_COEF (3344921057ul)
  29 #define CP_HASH_PAIR(A, B) ((cpHashValue)(A)*CP_HASH_COEF ^ (cpHashValue)(B)*CP_HASH_COEF)
  30
  31 // TODO: Eww. Magic numbers.
  32 #define MAGIC_EPSILON 1e-5
  33
  34
  35 //MARK: cpArray
  36
  37 cpArray *cpArrayNew(int size);
  38
  39 void cpArrayFree(cpArray *arr);
  40
  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);
  45
  46 void cpArrayFreeEach(cpArray *arr, void (freeFunc)(void*));
  47
  48
  49 //MARK: cpHashSet
  50
  51 typedef cpBool (*cpHashSetEqlFunc)(const void *ptr, const void *elt);
  52 typedef void *(*cpHashSetTransFunc)(const void *ptr, void *data);
  53
  54 cpHashSet *cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc);
  55 void cpHashSetSetDefaultValue(cpHashSet *set, void *default_value);
  56
  57 void cpHashSetFree(cpHashSet *set);
  58
  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);
  63
  64 typedef void (*cpHashSetIteratorFunc)(void *elt, void *data);
  65 void cpHashSetEach(cpHashSet *set, cpHashSetIteratorFunc func, void *data);
  66
  67 typedef cpBool (*cpHashSetFilterFunc)(void *elt, void *data);
  68 void cpHashSetFilter(cpHashSet *set, cpHashSetFilterFunc func, void *data);
  69
  70
  71 //MARK: Bodies
  72
  73 void cpBodyAddShape(cpBody *body, cpShape *shape);
  74 void cpBodyRemoveShape(cpBody *body, cpShape *shape);
  75
  76 //void cpBodyAccumulateMassForShape(cpBody *body, cpShape *shape);
  77 void cpBodyAccumulateMassFromShapes(cpBody *body);
  78
  79 void cpBodyRemoveConstraint(cpBody *body, cpConstraint *constraint);
  80
  81
  82 //MARK: Spatial Index Functions
  83
  84 cpSpatialIndex *cpSpatialIndexInit(cpSpatialIndex *index, cpSpatialIndexClass *klass, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex);
  85
  86
  87 //MARK: Arbiters
  88
  89 cpArbiter* cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b);
  90
  91 static inline struct cpArbiterThread *
  92 cpArbiterThreadForBody(cpArbiter *arb, cpBody *body)
  93 {
  94         return (arb->body_a == body ? &arb->thread_a : &arb->thread_b);
  95 }
  96
  97 void cpArbiterUnthread(cpArbiter *arb);
  98
  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);
 103
 104
 105 //MARK: Shapes/Collisions
 106
 107 cpShape *cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body, struct cpShapeMassInfo massInfo);
 108
 109 static inline cpBool
 110 cpShapeActive(cpShape *shape)
 111 {
 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));
 115 }
 116
 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);
 119
 120 static inline void
 121 CircleSegmentQuery(cpShape *shape, cpVect center, cpFloat r1, cpVect a, cpVect b, cpFloat r2, cpSegmentQueryInfo *info)
 122 {
 123         cpVect da = cpvsub(a, center);
 124         cpVect db = cpvsub(b, center);
 125         cpFloat rsum = r1 + r2;
 126
 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);
 130
 131         if(det >= 0.0f){
 132                 cpFloat t = (-qb - cpfsqrt(det))/(qa);
 133                 if(0.0f<= t && t <= 1.0f){
 134                         cpVect n = cpvnormalize(cpvlerp(da, db, t));
 135
 136                         info->shape = shape;
 137                         info->point = cpvsub(cpvlerp(a, b, t), cpvmult(n, r2));
 138                         info->normal = n;
 139                         info->alpha = t;
 140                 }
 141         }
 142 }
 143
 144 static inline cpBool
 145 cpShapeFilterReject(cpShapeFilter a, cpShapeFilter b)
 146 {
 147         // Reject the collision if:
 148         return (
 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
 154         );
 155 }
 156
 157 void cpLoopIndexes(const cpVect *verts, int count, int *start, int *end);
 158
 159
 160 //MARK: Constraints
 161 // TODO naming conventions here
 162
 163 void cpConstraintInit(cpConstraint *constraint, const struct cpConstraintClass *klass, cpBody *a, cpBody *b);
 164
 165 static inline void
 166 cpConstraintActivateBodies(cpConstraint *constraint)
 167 {
 168         cpBody *a = constraint->a; cpBodyActivate(a);
 169         cpBody *b = constraint->b; cpBodyActivate(b);
 170 }
 171
 172 static inline cpVect
 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));
 176
 177         return cpvsub(v2_sum, v1_sum);
 178 }
 179
 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);
 183 }
 184
 185 static inline void
 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);
 189 }
 190
 191 static inline void
 192 apply_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j)
 193 {
 194         apply_impulse(a, cpvneg(j), r1);
 195         apply_impulse(b, j, r2);
 196 }
 197
 198 static inline void
 199 apply_bias_impulse(cpBody *body, cpVect j, cpVect r)
 200 {
 201         body->v_bias = cpvadd(body->v_bias, cpvmult(j, body->m_inv));
 202         body->w_bias += body->i_inv*cpvcross(r, j);
 203 }
 204
 205 static inline void
 206 apply_bias_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j)
 207 {
 208         apply_bias_impulse(a, cpvneg(j), r1);
 209         apply_bias_impulse(b, j, r2);
 210 }
 211
 212 static inline cpFloat
 213 k_scalar_body(cpBody *body, cpVect r, cpVect n)
 214 {
 215         cpFloat rcn = cpvcross(r, n);
 216         return body->m_inv + body->i_inv*rcn*rcn;
 217 }
 218
 219 static inline cpFloat
 220 k_scalar(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n)
 221 {
 222         cpFloat value = k_scalar_body(a, r1, n) + k_scalar_body(b, r2, n);
 223         cpAssertSoft(value != 0.0, "Unsolvable collision or constraint.");
 224
 225         return value;
 226 }
 227
 228 static inline cpMat2x2
 229 k_tensor(cpBody *a, cpBody *b, cpVect r1, cpVect r2)
 230 {
 231         cpFloat m_sum = a->m_inv + b->m_inv;
 232
 233         // start with Identity*m_sum
 234         cpFloat k11 = m_sum, k12 = 0.0f;
 235         cpFloat k21 = 0.0f,  k22 = m_sum;
 236
 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;
 244
 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;
 252
 253         // invert
 254         cpFloat det = k11*k22 - k12*k21;
 255         cpAssertSoft(det != 0.0, "Unsolvable constraint.");
 256
 257         cpFloat det_inv = 1.0f/det;
 258         return cpMat2x2New(
 259                  k22*det_inv, -k12*det_inv,
 260                 -k21*det_inv,  k11*det_inv
 261         );
 262 }
 263
 264 static inline cpFloat
 265 bias_coef(cpFloat errorBias, cpFloat dt)
 266 {
 267         return 1.0f - cpfpow(errorBias, dt);
 268 }
 269
 270
 271 //MARK: Spaces
 272
 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." \
 277         );
 278
 279 void cpSpaceSetStaticBody(cpSpace *space, cpBody *body);
 280
 281 extern cpCollisionHandler cpCollisionHandlerDoNothing;
 282
 283 void cpSpaceProcessComponents(cpSpace *space, cpFloat dt);
 284
 285 void cpSpacePushFreshContactBuffer(cpSpace *space);
 286 struct cpContact *cpContactBufferGetArray(cpSpace *space);
 287 void cpSpacePushContacts(cpSpace *space, int count);
 288
 289 cpPostStepCallback *cpSpaceGetPostStepCallback(cpSpace *space, void *key);
 290
 291 cpBool cpSpaceArbiterSetFilter(cpArbiter *arb, cpSpace *space);
 292 void cpSpaceFilterArbiters(cpSpace *space, cpBody *body, cpShape *filter);
 293
 294 void cpSpaceActivateBody(cpSpace *space, cpBody *body);
 295 void cpSpaceLock(cpSpace *space);
 296 void cpSpaceUnlock(cpSpace *space, cpBool runPostStep);
 297
 298 static inline void
 299 cpSpaceUncacheArbiter(cpSpace *space, cpArbiter *arb)
 300 {
 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);
 306 }
 307
 308 static inline cpArray *
 309 cpSpaceArrayForBodyType(cpSpace *space, cpBodyType type)
 310 {
 311         return (type == CP_BODY_TYPE_STATIC ? space->staticBodies : space->dynamicBodies);
 312 }
 313
 314 void cpShapeUpdateFunc(cpShape *shape, void *unused);
 315 cpCollisionID cpSpaceCollideShapes(cpShape *a, cpShape *b, cpCollisionID id, cpSpace *space);
 316
 317
 318 //MARK: Foreach loops
 319
 320 static inline cpConstraint *
 321 cpConstraintNext(cpConstraint *node, cpBody *body)
 322 {
 323         return (node->a == body ? node->next_a : node->next_b);
 324 }
 325
 326 #define CP_BODY_FOREACH_CONSTRAINT(bdy, var)\
 327         for(cpConstraint *var = bdy->constraintList; var; var = cpConstraintNext(var, bdy))
 328
 329 static inline cpArbiter *
 330 cpArbiterNext(cpArbiter *node, cpBody *body)
 331 {
 332         return (node->body_a == body ? node->thread_a.next : node->thread_b.next);
 333 }
 334
 335 #define CP_BODY_FOREACH_ARBITER(bdy, var)\
 336         for(cpArbiter *var = bdy->arbiterList; var; var = cpArbiterNext(var, bdy))
 337
 338 #define CP_BODY_FOREACH_SHAPE(body, var)\
 339         for(cpShape *var = body->shapeList; var; var = var->next)
 340
 341 #define CP_BODY_FOREACH_COMPONENT(root, var)\
 342         for(cpBody *var = root; var; var = var->sleeping.next)
 343
 344 #endif