src/jelly/World.cpp

   1 /*
   2  * Copyright (c) 2007 Walaber
   3  * Modified by Ketmar // Invisible Vector
   4  *
   5  * Permission is hereby granted, free of charge, to any person obtaining a copy
   6  * of this software and associated documentation files (the "Software"), to deal
   7  * in the Software without restriction, including without limitation the rights
   8  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   9  * copies of the Software, and to permit persons to whom the Software is
  10  * furnished to do so, subject to the following conditions:
  11  *
  12  * The above copyright notice and this permission notice shall be included in
  13  * all copies or substantial portions of the Software.
  14  *
  15  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  16  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  17  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  18  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  19  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  20  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  21  * THE SOFTWARE.
  22  */
  23 #include <stdio.h>
  24
  25 #include "World.h"
  26
  27
  28 namespace JellyPhysics {
  29
  30 World::World () {
  31   mMaterialCount = 1;
  32   mMaterialPairs = new MaterialPair[1];
  33   mDefaultMatPair.Friction = 0.3f;
  34   mDefaultMatPair.Elasticity = 0.8f;
  35   mDefaultMatPair.Collide = true;
  36   mMaterialPairs[0] = mDefaultMatPair;
  37   setWorldLimits(Vector2(-20,-20), Vector2(20,20));
  38   mPenetrationThreshold = 0.3f;
  39   mPenetrationCount = 0;
  40   mBodies = NULL;
  41   mBodyCount = 0;
  42   mBodyAlloted = 0;
  43 }
  44
  45
  46 World::~World () {
  47   while (mBodyCount > 0) delete mBodies[mBodyCount-1];
  48   if (mBodies != NULL) free(mBodies);
  49   delete[] mMaterialPairs;
  50 }
  51
  52
  53 void World::killing () {
  54   // clear up all "VoidMarker" elements in the list...
  55   if (mBodyCount > 0) {
  56     Body::BodyBoundary *bb = &mBodies[0]->mBoundStart;
  57     while (bb->prev) bb = bb->prev;
  58     while (bb) {
  59       if (bb->type == Body::BodyBoundary::VoidMarker) {
  60         // remove this one!
  61         _removeBoundary(bb);
  62         Body::BodyBoundary *theNext = bb->next;
  63         delete bb;
  64         bb = theNext;
  65         continue;
  66       }
  67       bb = bb->next;
  68     }
  69   }
  70 }
  71
  72
  73 void World::setWorldLimits (const Vector2 &min, const Vector2 &max) {
  74   mWorldLimits = AABB(min, max);
  75   mWorldSize = max-min;
  76   mWorldGridStep = mWorldSize/32;
  77   // update bitmasks for all bodies
  78   for (int f = mBodyCount-1; f >= 0; --f) updateBodyBitmask(mBodies[f]);
  79 }
  80
  81
  82 int World::addMaterial () {
  83   MaterialPair *old = new MaterialPair[mMaterialCount*mMaterialCount];
  84   for (int i = 0; i < mMaterialCount; ++i) {
  85     for (int j = 0; j < mMaterialCount; ++j) {
  86       old[(i*mMaterialCount)+j] = mMaterialPairs[(i*mMaterialCount)+j];
  87     }
  88   }
  89   ++mMaterialCount;
  90   delete[] mMaterialPairs;
  91   mMaterialPairs = new MaterialPair[mMaterialCount*mMaterialCount];
  92   for (int i = 0; i < mMaterialCount; ++i) {
  93     for (int j = 0; j < mMaterialCount; ++j) {
  94       if (i < mMaterialCount-1 && j < mMaterialCount-1) {
  95         mMaterialPairs[(i*mMaterialCount)+j] = old[(i*(mMaterialCount-1))+j];
  96       } else {
  97         mMaterialPairs[(i*mMaterialCount)+j] = mDefaultMatPair;
  98       }
  99     }
 100   }
 101 #ifdef _DEBUG
 102   printf("addMaterial - final results...\n");
 103   _logMaterialCollide();
 104 #endif
 105   return mMaterialCount-1;
 106 }
 107
 108
 109 void World::setMaterialPairCollide (int a, int b, bool collide) {
 110 #ifdef _DEBUG
 111   printf("setMaterialPairCollide: %d vs %d %s\n", a, b, (collide ? "ON" : "OFF"));
 112 #endif
 113   if (a >= 0 && a < mMaterialCount && b >= 0 && b < mMaterialCount) {
 114     mMaterialPairs[(a*mMaterialCount)+b].Collide = collide;
 115     mMaterialPairs[(b*mMaterialCount)+a].Collide = collide;
 116   }
 117 #ifdef _DEBUG
 118   _logMaterialCollide();
 119 #endif
 120 }
 121
 122
 123 void World::setMaterialPairData (int a, int b, float friction, float elasticity) {
 124 #ifdef _DEBUG
 125   printf("setMaterialPairData: %d vs %d : f:%f e:%f\n", a, b, friction, elasticity);
 126 #endif
 127   if (a >= 0 && a < mMaterialCount && b >= 0 && b < mMaterialCount) {
 128     mMaterialPairs[(a*mMaterialCount)+b].Friction = friction;
 129     mMaterialPairs[(b*mMaterialCount)+a].Elasticity = elasticity;
 130     mMaterialPairs[(a*mMaterialCount)+b].Friction = friction;
 131     mMaterialPairs[(b*mMaterialCount)+a].Elasticity = elasticity;
 132   }
 133 #ifdef _DEBUG
 134   _logMaterialCollide();
 135 #endif
 136 }
 137
 138
 139 void World::setMaterialPairFilterCallback (int a, int b, CollisionCallback *c) {
 140   if (a >= 0 && a < mMaterialCount && b >= 0 && b < mMaterialCount) {
 141     mMaterialPairs[(a*mMaterialCount)+b].Callback = c;
 142     mMaterialPairs[(b*mMaterialCount)+a].Callback = c;
 143   }
 144 }
 145
 146
 147 void World::addBody (Body *b) {
 148 #ifdef _DEBUG
 149   printf("addBody: %d\n", b);
 150 #endif
 151   // check for already existing
 152   for (int i = mBodyCount-1; i >= 0; --i) if (mBodies[i] == b) return;
 153   if (mBodyCount+1 > mBodyAlloted) {
 154     int newsz = ((mBodyCount+1)|0x7f)+1;
 155     mBodies = (Body **)realloc(mBodies, sizeof(mBodies[0])*newsz);
 156     mBodyAlloted = newsz;
 157   }
 158   mBodies[mBodyCount++] = b;
 159   if (mBodyCount > 1) _addBoundaryAfter(&b->mBoundStart, &mBodies[0]->mBoundStart);
 160   _addBoundaryAfter(&b->mBoundEnd, &b->mBoundStart);
 161 #ifdef _DEBUG
 162   _logBoundaries();
 163 #endif
 164 }
 165
 166
 167 void World::removeBody (Body *b) {
 168 #ifdef _DEBUG
 169   printf("removeBody: %d\n", b);
 170 #endif
 171   for (int i = mBodyCount-1; i >= 0; --i) {
 172     if (mBodies[i] == b) {
 173       for (int c = i+1; c < mBodyCount; ++c) mBodies[c-1] = mBodies[c];
 174       _removeBoundary(&b->mBoundStart);
 175       _removeBoundary(&b->mBoundEnd);
 176 #ifdef _DEBUG
 177       _logBoundaries();
 178 #endif
 179       break;
 180     }
 181   }
 182 }
 183
 184
 185 void World::getClosestPointMass (const Vector2 &pt, int *bodyID, int *pmID) {
 186   if (bodyID) *bodyID = -1;
 187   if (pmID) *pmID = -1;
 188   float closestD = 1000.0f;
 189   for (int i = mBodyCount-1; i >= 0; --i) {
 190     float dist = 0.0f;
 191     int pm = mBodies[i]->getClosestPointMass(pt, &dist);
 192     if (dist < closestD) {
 193       closestD = dist;
 194       if (bodyID) *bodyID = i;
 195       if (pmID) *pmID = pm;
 196     }
 197   }
 198 }
 199
 200
 201 Body *World::getBodyContaining (const Vector2 &pt) {
 202   for (int i = mBodyCount-1; i >= 0; --i) if (mBodies[i]->contains(pt)) return mBodies[i];
 203   return NULL;
 204 }
 205
 206
 207 void World::update (float elapsed) {
 208   mPenetrationCount = 0;
 209   // first, accumulate all forces acting on PointMasses
 210   for (int f = mBodyCount-1; f >= 0; --f) {
 211     Body *b = mBodies[f];
 212     if (b->getIsStatic() || b->getIgnoreMe()) continue;
 213     b->derivePositionAndAngle(elapsed);
 214     b->accumulateExternalForces();
 215     b->accumulateInternalForces();
 216   }
 217   // now integrate
 218   for (int f = mBodyCount-1; f >= 0; --f) {
 219     //if (b->getIsStatic()) continue;
 220     mBodies[f]->integrate(elapsed);
 221   }
 222   // update all bounding boxes, and then bitmasks
 223   for (int f = mBodyCount-1; f >= 0; --f) {
 224     Body *b = mBodies[f];
 225     if (b->getIsStatic() || b->getIgnoreMe()) continue;
 226     b->updateAABB(elapsed);
 227     updateBodyBitmask(b);
 228     b->updateEdgeInfo();
 229     b->updateBoundaryValues();
 230   }
 231   // sort body boundaries for broadphase collision checks
 232   sortBodyBoundaries();
 233   // now check for collision
 234   for (int i = 0; i < mBodyCount; ++i) {
 235     Body *bA = mBodies[i];
 236     if (bA->getIsStatic() || bA->getIgnoreMe()) continue;
 237     /*
 238      // OLD, BRUTE-FORCE COLLISION CHECKS USING BITMASKS ONLY FOR OPTIMIZATION
 239      for (int j = i+1; j < mBodies.size(); ++j) _goNarrowCheck(mBodies[i], mBodies[j]);
 240     */
 241     Body::BodyBoundary *bS = &bA->mBoundStart;
 242     Body::BodyBoundary *bE = &bA->mBoundEnd;
 243     Body::BodyBoundary *cur = bS->next;
 244     bool passedMyEnd = false;
 245     while (cur) {
 246       if (cur == bE) {
 247         passedMyEnd = true;
 248       } else if (cur->type == Body::BodyBoundary::Begin && !passedMyEnd) {
 249         // overlapping, do narrow-phase check on this body pair
 250         _goNarrowCheck(bA, cur->body);
 251       } else if (cur->type == Body::BodyBoundary::End) {
 252         // this is an end... the only situation in which we didn't already catch this body from its "begin",
 253         // is if the begin of this body starts before our own begin.
 254         if (cur->body->mBoundStart.value <= bS->value) {
 255           // overlapping, do narrow-phase check on this body pair
 256           _goNarrowCheck(bA, cur->body);
 257         }
 258       } else if (cur->type == Body::BodyBoundary::VoidMarker) {
 259         break;
 260       }
 261       cur = cur->next;
 262     }
 263   }
 264   //printf("\n\n");
 265   // now handle all collisions found during the update at once
 266   _handleCollisions();
 267   // now dampen velocities.
 268   for (int f = mBodyCount-1; f >= 0; --f) {
 269     //if (mBodies[f]->getIsStatic()) continue;
 270     mBodies[f]->dampenVelocity();
 271   }
 272 }
 273
 274
 275 void World::_goNarrowCheck (Body *bI, Body *bJ) {
 276   //printf("goNarrow %d vs. %d\n", bI, bJ);
 277   // grid-based early out
 278   if (/*((bI->mBitMaskX.mask & bJ->mBitMaskX.mask) == 0) && */ (bI->mBitMaskY.mask &bJ->mBitMaskY.mask) == 0) {
 279     //printf("update - no bitmask overlap.\n");
 280     return;
 281   }
 282   // early out - these bodies materials are set NOT to collide
 283   if (!mMaterialPairs[(bI->getMaterial()*mMaterialCount)+bJ->getMaterial()].Collide) {
 284     //printf("update - material early out: %d vs. %d\n", mBodies[i]->getMaterial(), mBodies[j]->getMaterial());
 285     return;
 286   }
 287   // broad-phase collision via AABB
 288   const AABB& boxA = bI->getAABB();
 289   const AABB& boxB = bJ->getAABB();
 290   // early out
 291   if (!boxA.intersects(boxB)) {
 292     //printf("update - no AABB overlap.\n");
 293     return;
 294   }
 295   // okay, the AABB's of these 2 are intersecting: now check for collision of A against B
 296   bodyCollide(bI, bJ, mCollisionList);
 297   // and the opposite case, B colliding with A
 298   bodyCollide(bJ, bI, mCollisionList);
 299 }
 300
 301
 302 void World::updateBodyBitmask (Body *body) {
 303   AABB box = body->getAABB();
 304   /*int minX = (int)floor((box.Min.X - mWorldLimits.Min.X)/mWorldGridStep.X);
 305   int maxX = (int)floor((box.Max.X - mWorldLimits.Min.X)/mWorldGridStep.X);
 306   if (minX < 0) { minX = 0; } else if (minX > 31) { minX = 31; }
 307   if (maxX < 0) { maxX = 0; } else if (maxX > 31) { maxX = 31; }
 308   */
 309   int minY = (int)floorf((box.Min.Y-mWorldLimits.Min.Y)/mWorldGridStep.Y);
 310   int maxY = (int)floorf((box.Max.Y-mWorldLimits.Min.Y)/mWorldGridStep.Y);
 311   if (minY < 0) { minY = 0; } else if (minY > 31) { minY = 31; }
 312   if (maxY < 0) { maxY = 0; } else if (maxY > 31) { maxY = 31; }
 313   /*
 314   body->mBitMaskX.clear();
 315   for (int i = minX; i <= maxX; ++i) body->mBitMaskX.setOn(i);
 316   */
 317   body->mBitMaskY.clear();
 318   for (int i = minY; i <= maxY; ++i) body->mBitMaskY.setOn(i);
 319   //Console.WriteLine("Body bitmask: minX{0} maxX{1} minY{2} maxY{3}", minX, maxX, minY, minY, maxY);
 320 }
 321
 322
 323 void World::sortBodyBoundaries () {
 324   // for every body in the list, update it's START end END.
 325   for (int f = 0; f < mBodyCount; ++f) {
 326     // start with START boundary
 327     _checkAndMoveBoundary(&mBodies[f]->mBoundStart);
 328     // and then END boundary.
 329     _checkAndMoveBoundary(&mBodies[f]->mBoundEnd);
 330   }
 331   // now go through and add / remove the "VOID" identifiers
 332   if (mBodyCount > 0) {
 333     Body::BodyBoundary *bb = &mBodies[0]->mBoundStart;
 334     while (bb->prev) bb = bb->prev;
 335     int stackCount = 0;
 336     while (bb) {
 337       //printf("bb: ");
 338       //bb->log();
 339       if (bb->type == Body::BodyBoundary::Begin) {
 340         ++stackCount;
 341         //printf(" begin, stack inced to %d.", stackCount);
 342       } else if (bb->type == Body::BodyBoundary::End) {
 343         --stackCount;
 344         //printf(" end, stack deced to %d.", stackCount);
 345       } else if (bb->type == Body::BodyBoundary::VoidMarker) {
 346         if (stackCount != 0) {
 347           // this void marker should not be here
 348           Body::BodyBoundary *v = bb;
 349           bb = bb->next;
 350           _removeBoundary(v);
 351           delete v;
 352           //printf(" VOID but stack != 0. deleted.\n");
 353           continue;
 354         } else {
 355           // OK as-is
 356           //printf(" VOID but stack is 0. continuing.\n");
 357           bb = bb->next;
 358           continue;
 359         }
 360       }
 361       //printf("  |  ");
 362       if (stackCount == 0) {
 363         if (bb->next) {
 364           if (bb->next->type == Body::BodyBoundary::VoidMarker) {
 365             bb = bb->next->next;
 366             //printf("next is VOID, so OK. skipping VOID...\n");
 367             continue;
 368           }
 369         } else {
 370           // no next, end of the array!
 371           //printf("end of the array!\n");
 372           break;
 373         }
 374         // add a new void marker here!
 375         Body::BodyBoundary *v = new Body::BodyBoundary(0, Body::BodyBoundary::VoidMarker, bb->value+0.000001f);
 376         _addBoundaryAfter(v, bb);
 377         bb = v->next;
 378         //printf("stack is 0, adding VOID!\n");
 379         continue;
 380       }
 381       //printf("\n");
 382       bb = bb->next;
 383     }
 384   }
 385   //printf("sortBodyBoundaries...\n");
 386   //_logBoundaries();
 387 }
 388
 389
 390 void World::_checkAndMoveBoundary (Body::BodyBoundary *bb) {
 391   //printf("checkAndMoveBoundary:");
 392   //bb->log();
 393   //printf("\n");
 394   // need to move backwards?
 395   if (bb->prev) {
 396     if (bb->value < bb->prev->value) {
 397       Body::BodyBoundary *beforeThis = bb->prev;
 398       while (true) {
 399         if (!beforeThis->prev) {
 400           break;
 401         } else {
 402           if (bb->value < beforeThis->prev->value) beforeThis = beforeThis->prev; else break;
 403         }
 404       }
 405       //printf("moving left: ");
 406       //bb->log();
 407       //printf(" to before:");
 408       //beforeThis->log();
 409       //printf("\n");
 410       _removeBoundary(bb);
 411       _addBoundaryBefore(bb, beforeThis);
 412       // moved, so we're done!
 413       return;
 414     }
 415   }
 416   if (bb->next) {
 417     if (bb->value > bb->next->value) {
 418       Body::BodyBoundary *afterThis = bb->next;
 419       while (true) {
 420         if (!afterThis->next) {
 421           break;
 422         } else {
 423           if (bb->value > afterThis->next->value) afterThis = afterThis->next; else break;
 424         }
 425       }
 426       //printf("moving right: ");
 427       //bb->log();
 428       //printf(" to after:");
 429       //afterThis->log();
 430       //printf("\n");
 431       _removeBoundary(bb);
 432       _addBoundaryAfter(bb, afterThis);
 433       return;
 434     }
 435   }
 436 }
 437
 438
 439 void World::bodyCollide (Body *bA, Body *bB, std::vector<BodyCollisionInfo> &infoList) {
 440   int bApmCount = bA->pointMassCount();
 441   int bBpmCount = bB->pointMassCount();
 442   AABB boxB = bB->getAABB();
 443   // check all PointMasses on bodyA for collision against bodyB.  if there is a collision, return detailed info.
 444   BodyCollisionInfo infoAway;
 445   BodyCollisionInfo infoSame;
 446   for (int i = 0; i < bApmCount; ++i) {
 447     Vector2 pt = bA->getPointMass(i)->Position;
 448     // early out - if this point is outside the bounding box for bodyB, skip it!
 449     if (!boxB.contains(pt)) {
 450       //printf("bodyCollide - bodyB AABB does not contain pt\n");
 451       continue;
 452     }
 453     // early out - if this point is not inside bodyB, skip it!
 454     if (!bB->contains(pt)) {
 455       //printf("bodyCollide - bodyB does not contain pt\n");
 456       continue;
 457     }
 458     int prevPt = (i > 0 ? i-1 : bApmCount-1);
 459     int nextPt = (i < bApmCount-1 ? i+1 : 0);
 460     Vector2 prev = bA->getPointMass(prevPt)->Position;
 461     Vector2 next = bA->getPointMass(nextPt)->Position;
 462     // now get the normal for this point. (NOT A UNIT VECTOR)
 463     Vector2 fromPrev = pt-prev;
 464     Vector2 toNext = next-pt;
 465     Vector2 ptNorm = fromPrev+toNext;
 466     ptNorm.makePerpendicular();
 467     // this point is inside the other body; now check if the edges on either side intersect with and edges on bodyB
 468     float closestAway = 100000.0f;
 469     float closestSame = 100000.0f;
 470
 471     infoAway.Clear();
 472     infoAway.bodyA = bA;
 473     infoAway.bodyApm = i;
 474     infoAway.bodyB = bB;
 475
 476     infoSame.Clear();
 477     infoSame.bodyA = bA;
 478     infoSame.bodyApm = i;
 479     infoSame.bodyB = bB;
 480
 481     bool found = false;
 482     int b1 = 0;
 483     int b2 = 1;
 484     for (int j = 0; j < bBpmCount; ++j) {
 485       Vector2 hitPt;
 486       Vector2 norm;
 487       float edgeD;
 488       b1 = j;
 489       b2 = (j < bBpmCount-1 ? j+1 : 0);
 490       //Vector2 pt1 = bB->getPointMass(b1)->Position;
 491       //Vector2 pt2 = bB->getPointMass(b2)->Position;
 492       // quick test of distance to each point on the edge, if both are greater than current mins, we can skip!
 493       //float distToA = (pt1-pt).lengthSquared();
 494       //float distToB = (pt2-pt).lengthSquared();
 495       /*if (distToA > closestAway && distToA > closestSame && distToB > closestAway && distToB > closestSame) {
 496         //printf("bodyCollide - not close enough\n");
 497         continue;
 498       }*/
 499       // test against this edge.
 500       float dist = bB->getClosestPointOnEdgeSquared(pt, j, &hitPt, &norm, &edgeD);
 501       //printf("bodyCollide - dist:%f\n", dist);
 502       // only perform the check if the normal for this edge is facing AWAY from the point normal.
 503       float dot = ptNorm.dotProduct(norm);
 504       if (dot <= 0.0f) {
 505         if (dist < closestAway) {
 506           closestAway = dist;
 507           infoAway.bodyBpmA = b1;
 508           infoAway.bodyBpmB = b2;
 509           infoAway.edgeD = edgeD;
 510           infoAway.hitPt = hitPt;
 511           infoAway.norm = norm;
 512           infoAway.penetration = dist;
 513           found = true;
 514           //printf("bodyCollide - set away.\n");
 515           infoAway.Log();
 516         }
 517       } else {
 518         if (dist < closestSame) {
 519           closestSame = dist;
 520           infoSame.bodyBpmA = b1;
 521           infoSame.bodyBpmB = b2;
 522           infoSame.edgeD = edgeD;
 523           infoSame.hitPt = hitPt;
 524           infoSame.norm = norm;
 525           infoSame.penetration = dist;
 526           //printf("bodyCollide - set same\n");
 527           infoSame.Log();
 528         }
 529       }
 530     }
 531     // we've checked all edges on BodyB.  add the collision info to the stack.
 532     if (found && closestAway > mPenetrationThreshold && closestSame < closestAway) {
 533       infoSame.penetration = sqrtf(infoSame.penetration);
 534       infoList.push_back(infoSame);
 535       //printf("bodyCollide - added same: penetration:%f\n", infoSame.penetration);
 536     } else {
 537       infoAway.penetration = sqrtf(infoAway.penetration);
 538       infoList.push_back(infoAway);
 539       //printf("bodyCollide - added away: penetration:%f\n", infoAway.penetration);
 540     }
 541   }
 542 }
 543
 544
 545 void World::_handleCollisions () {
 546   if (mCollisionList.size() == 0) return;
 547   //printf("handleCollisions - count %u\n", mCollisionList.size());
 548   // handle all collisions!
 549   for (unsigned int i = 0; i < mCollisionList.size(); ++i) {
 550     BodyCollisionInfo info = mCollisionList[i];
 551     PointMass *A = info.bodyA->getPointMass(info.bodyApm);
 552     PointMass *B1 = info.bodyB->getPointMass(info.bodyBpmA);
 553     PointMass *B2 = info.bodyB->getPointMass(info.bodyBpmB);
 554     // velocity changes as a result of collision.
 555     Vector2 bVel = (B1->Velocity+B2->Velocity)*0.5f;
 556     Vector2 relVel = A->Velocity-bVel;
 557     float relDot = relVel.dotProduct(info.norm);
 558     //printf("handleCollisions - relVel:[x:%f][y:%f] relDot:%f\n", relVel.X, relVel.Y, relDot);
 559     // collision filter!
 560     //if (!mMaterialPairs[info.bodyA.Material, info.bodyB.Material].CollisionFilter(info.bodyA, info.bodyApm, info.bodyB, info.bodyBpmA, info.bodyBpmB, info.hitPt, relDot)) continue;
 561     CollisionCallback *cf = mMaterialPairs[(info.bodyA->getMaterial()*mMaterialCount)+info.bodyB->getMaterial()].Callback;
 562     if (cf) {
 563       if (!cf->collisionFilter(info.bodyA, info.bodyApm, info.bodyB, info.bodyBpmA, info.bodyBpmB, info.hitPt, relDot)) {
 564         //printf("collision filtered!\n");
 565         continue;
 566       }
 567     }
 568     if (info.penetration > mPenetrationThreshold) {
 569       //Console.WriteLine("penetration above Penetration Threshold!!  penetration={0}  threshold={1} difference={2}", info.penetration, mPenetrationThreshold, info.penetration-mPenetrationThreshold);
 570       //printf("handleCollisions - penetration above threshold! threshold:%f penetration:%f diff:%f\n", mPenetrationThreshold, info.penetration, info.penetration - mPenetrationThreshold);
 571       ++mPenetrationCount;
 572       continue;
 573     }
 574     //printf("calculating... (ed=%f)\n", info.edgeD);
 575     float Amove, Bmove;
 576     float b1inf = 1.0f-info.edgeD;
 577     float b2inf = info.edgeD;
 578     float b2MassSum = (B1->isInfMass() || B2->isInfMass() ? 0.0f : B1->mass()+B2->mass());
 579     float massSum = A->mass()+b2MassSum;
 580     //printf(" A->Mass=%f; B1->Mass=%f; B2->Mass=%f; massSum=%f\n", A->mass(), B1->mass(), B2->mass(), massSum);
 581     if (A->isInfMass()) {
 582       Amove = 0.0f;
 583       Bmove = info.penetration+0.001f;
 584     } else if (b2MassSum == 0.0f) {
 585       Amove = info.penetration+0.001f;
 586       Bmove = 0.0f;
 587     } else {
 588       Amove = info.penetration*(b2MassSum/massSum);
 589       Bmove = info.penetration*(A->mass()/massSum);
 590     }
 591     float B1move = Bmove*b1inf;
 592     float B2move = Bmove*b2inf;
 593     //printf("handleCollisions - Amove:%f B1move:%f B2move:%f\n", Amove, B1move, B2move);
 594     if (!A->isInfMass() != 0.0f) A->Position += info.norm*Amove;
 595     if (!B1->isInfMass() != 0.0f) B1->Position -= info.norm*B1move;
 596     if (!B2->isInfMass() != 0.0f) B2->Position -= info.norm*B2move;
 597     float AinvMass = (A->isInfMass() ? 0.0f : 1.0f/A->mass());
 598     float BinvMass = (b2MassSum == 0.0f ? 0.0f : 1.0f/b2MassSum);
 599     float jDenom = AinvMass+BinvMass;
 600     float elas = 1.0f+mMaterialPairs[(info.bodyA->getMaterial()*mMaterialCount)+info.bodyB->getMaterial()].Elasticity;
 601     Vector2 numV = relVel*elas;
 602     float jNumerator = numV.dotProduct(info.norm);
 603     jNumerator = -jNumerator;
 604     float j = jNumerator/jDenom;
 605     Vector2 tangent = info.norm.getPerpendicular();
 606     float friction = mMaterialPairs[(info.bodyA->getMaterial()*mMaterialCount)+info.bodyB->getMaterial()].Friction;
 607     float fNumerator = relVel.dotProduct(tangent);
 608     fNumerator *= friction;
 609     float f = fNumerator/jDenom;
 610     // adjust velocity if relative velocity is moving toward each other
 611     if (relDot <= 0.0001f) {
 612       if (!A->isInfMass()) A->Velocity += (info.norm*(j/A->mass()))-(tangent*(f/A->mass()));
 613       if (b2MassSum != 0.0f) B1->Velocity -= (info.norm*(j/b2MassSum)*b1inf)-(tangent*(f/b2MassSum)*b1inf);
 614       if (b2MassSum != 0.0f) B2->Velocity -= (info.norm*(j/b2MassSum)*b2inf)-(tangent*(f/b2MassSum)*b2inf);
 615     }
 616   }
 617   mCollisionList.clear();
 618 }
 619
 620
 621 void World::_removeBoundary (Body::BodyBoundary *me) {
 622   if (me->prev) me->prev->next = me->next;
 623   if (me->next) me->next->prev = me->prev;
 624 }
 625
 626
 627 void World::_addBoundaryAfter (Body::BodyBoundary *me, Body::BodyBoundary *toAfterMe) {
 628   me->next = toAfterMe->next;
 629   toAfterMe->next = me;
 630   if (me->next) me->next->prev = me;
 631   me->prev = toAfterMe;
 632 }
 633
 634
 635 void World::_addBoundaryBefore (Body::BodyBoundary *me, Body::BodyBoundary *toBeforeMe) {
 636   me->prev = toBeforeMe->prev;
 637   toBeforeMe->prev = me;
 638   if (me->prev) me->prev->next = me;
 639   me->next = toBeforeMe;
 640 }
 641
 642
 643 void World::_logBoundaries () const {
 644   // first, find the "first" boundary in the list
 645   if (mBodyCount == 0) return;
 646   Body::BodyBoundary *bb = &mBodies[0]->mBoundStart;
 647   while (bb->prev) bb = bb->prev;
 648   while (bb) {
 649     bb->log();
 650     bb = bb->next;
 651   }
 652   printf("\n\n");
 653 }
 654
 655
 656 void World::_logMaterialCollide () const {
 657   for (int i = 0; i < mMaterialCount; ++i) printf("%s[%d]", (i == 0 ? "[ ]" : ""), i);
 658   printf("\n");
 659   for (int i = 0; i < mMaterialCount; ++i) {
 660     printf("[%d]",i);
 661     for (int j = 0; j < mMaterialCount; ++j) {
 662       printf("[%s]", (mMaterialPairs[(i*mMaterialCount)+j].Collide ? "X" : " "));
 663     }
 664     printf("\n");
 665   }
 666   printf("\n");
 667 }
 668
 669 }