git.maemo.org Git - blok/blob - Box2D/Source/Collision/b2BroadPhase.cpp

   1 /*
   2 * Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
   3 *
   4 * This software is provided 'as-is', without any express or implied
   5 * warranty.  In no event will the authors be held liable for any damages
   6 * arising from the use of this software.
   7 * Permission is granted to anyone to use this software for any purpose,
   8 * including commercial applications, and to alter it and redistribute it
   9 * freely, subject to the following restrictions:
  10 * 1. The origin of this software must not be misrepresented; you must not
  11 * claim that you wrote the original software. If you use this software
  12 * in a product, an acknowledgment in the product documentation would be
  13 * appreciated but is not required.
  14 * 2. Altered source versions must be plainly marked as such, and must not be
  15 * misrepresented as being the original software.
  16 * 3. This notice may not be removed or altered from any source distribution.
  17 */
  18
  19 #include "b2BroadPhase.h"
  20 #include <algorithm>
  21 #include <string.h>
  22
  23 // Notes:
  24 // - we use bound arrays instead of linked lists for cache coherence.
  25 // - we use quantized integral values for fast compares.
  26 // - we use short indices rather than pointers to save memory.
  27 // - we use a stabbing count for fast overlap queries (less than order N).
  28 // - we also use a time stamp on each proxy to speed up the registration of
  29 //   overlap query results.
  30 // - where possible, we compare bound indices instead of values to reduce
  31 //   cache misses (TODO_ERIN).
  32 // - no broadphase is perfect and neither is this one: it is not great for huge
  33 //   worlds (use a multi-SAP instead), it is not great for large objects.
  34
  35 bool b2BroadPhase::s_validate = false;
  36
  37 struct b2BoundValues
  38 {
  39         uint16 lowerValues[2];
  40         uint16 upperValues[2];
  41 };
  42
  43 static int32 BinarySearch(b2Bound* bounds, int32 count, uint16 value)
  44 {
  45         int32 low = 0;
  46         int32 high = count - 1;
  47         while (low <= high)
  48         {
  49                 int32 mid = (low + high) >> 1;
  50                 if (bounds[mid].value > value)
  51                 {
  52                         high = mid - 1;
  53                 }
  54                 else if (bounds[mid].value < value)
  55                 {
  56                         low = mid + 1;
  57                 }
  58                 else
  59                 {
  60                         return (uint16)mid;
  61                 }
  62         }
  63
  64         return low;
  65 }
  66
  67 b2BroadPhase::b2BroadPhase(const b2AABB& worldAABB, b2PairCallback* callback)
  68 {
  69         m_pairManager.Initialize(this, callback);
  70
  71         b2Assert(worldAABB.IsValid());
  72         m_worldAABB = worldAABB;
  73         m_proxyCount = 0;
  74
  75         b2Vec2 d = worldAABB.upperBound - worldAABB.lowerBound;
  76         m_quantizationFactor.x = float32(B2BROADPHASE_MAX) / d.x;
  77         m_quantizationFactor.y = float32(B2BROADPHASE_MAX) / d.y;
  78
  79         for (uint16 i = 0; i < b2_maxProxies - 1; ++i)
  80         {
  81                 m_proxyPool[i].SetNext(i + 1);
  82                 m_proxyPool[i].timeStamp = 0;
  83                 m_proxyPool[i].overlapCount = b2_invalid;
  84                 m_proxyPool[i].userData = NULL;
  85         }
  86         m_proxyPool[b2_maxProxies-1].SetNext(b2_nullProxy);
  87         m_proxyPool[b2_maxProxies-1].timeStamp = 0;
  88         m_proxyPool[b2_maxProxies-1].overlapCount = b2_invalid;
  89         m_proxyPool[b2_maxProxies-1].userData = NULL;
  90         m_freeProxy = 0;
  91
  92         m_timeStamp = 1;
  93         m_queryResultCount = 0;
  94 }
  95
  96 b2BroadPhase::~b2BroadPhase()
  97 {
  98 }
  99
 100 // This one is only used for validation.
 101 bool b2BroadPhase::TestOverlap(b2Proxy* p1, b2Proxy* p2)
 102 {
 103         for (int32 axis = 0; axis < 2; ++axis)
 104         {
 105                 b2Bound* bounds = m_bounds[axis];
 106
 107                 b2Assert(p1->lowerBounds[axis] < 2 * m_proxyCount);
 108                 b2Assert(p1->upperBounds[axis] < 2 * m_proxyCount);
 109                 b2Assert(p2->lowerBounds[axis] < 2 * m_proxyCount);
 110                 b2Assert(p2->upperBounds[axis] < 2 * m_proxyCount);
 111
 112                 if (bounds[p1->lowerBounds[axis]].value > bounds[p2->upperBounds[axis]].value)
 113                         return false;
 114
 115                 if (bounds[p1->upperBounds[axis]].value < bounds[p2->lowerBounds[axis]].value)
 116                         return false;
 117         }
 118
 119         return true;
 120 }
 121
 122 bool b2BroadPhase::TestOverlap(const b2BoundValues& b, b2Proxy* p)
 123 {
 124         for (int32 axis = 0; axis < 2; ++axis)
 125         {
 126                 b2Bound* bounds = m_bounds[axis];
 127
 128                 b2Assert(p->lowerBounds[axis] < 2 * m_proxyCount);
 129                 b2Assert(p->upperBounds[axis] < 2 * m_proxyCount);
 130
 131                 if (b.lowerValues[axis] > bounds[p->upperBounds[axis]].value)
 132                         return false;
 133
 134                 if (b.upperValues[axis] < bounds[p->lowerBounds[axis]].value)
 135                         return false;
 136         }
 137
 138         return true;
 139 }
 140
 141 void b2BroadPhase::ComputeBounds(uint16* lowerValues, uint16* upperValues, const b2AABB& aabb)
 142 {
 143         b2Assert(aabb.upperBound.x > aabb.lowerBound.x);
 144         b2Assert(aabb.upperBound.y > aabb.lowerBound.y);
 145
 146         b2Vec2 minVertex = b2Clamp(aabb.lowerBound, m_worldAABB.lowerBound, m_worldAABB.upperBound);
 147         b2Vec2 maxVertex = b2Clamp(aabb.upperBound, m_worldAABB.lowerBound, m_worldAABB.upperBound);
 148
 149         // Bump lower bounds downs and upper bounds up. This ensures correct sorting of
 150         // lower/upper bounds that would have equal values.
 151         // TODO_ERIN implement fast float to uint16 conversion.
 152         lowerValues[0] = (uint16)(m_quantizationFactor.x * (minVertex.x - m_worldAABB.lowerBound.x)) & (B2BROADPHASE_MAX - 1);
 153         upperValues[0] = (uint16)(m_quantizationFactor.x * (maxVertex.x - m_worldAABB.lowerBound.x)) | 1;
 154
 155         lowerValues[1] = (uint16)(m_quantizationFactor.y * (minVertex.y - m_worldAABB.lowerBound.y)) & (B2BROADPHASE_MAX - 1);
 156         upperValues[1] = (uint16)(m_quantizationFactor.y * (maxVertex.y - m_worldAABB.lowerBound.y)) | 1;
 157 }
 158
 159 void b2BroadPhase::IncrementTimeStamp()
 160 {
 161         if (m_timeStamp == B2BROADPHASE_MAX)
 162         {
 163                 for (uint16 i = 0; i < b2_maxProxies; ++i)
 164                 {
 165                         m_proxyPool[i].timeStamp = 0;
 166                 }
 167                 m_timeStamp = 1;
 168         }
 169         else
 170         {
 171                 ++m_timeStamp;
 172         }
 173 }
 174
 175 void b2BroadPhase::IncrementOverlapCount(int32 proxyId)
 176 {
 177         b2Proxy* proxy = m_proxyPool + proxyId;
 178         if (proxy->timeStamp < m_timeStamp)
 179         {
 180                 proxy->timeStamp = m_timeStamp;
 181                 proxy->overlapCount = 1;
 182         }
 183         else
 184         {
 185                 proxy->overlapCount = 2;
 186                 b2Assert(m_queryResultCount < b2_maxProxies);
 187                 m_queryResults[m_queryResultCount] = (uint16)proxyId;
 188                 ++m_queryResultCount;
 189         }
 190 }
 191
 192 void b2BroadPhase::Query(int32* lowerQueryOut, int32* upperQueryOut,
 193                                            uint16 lowerValue, uint16 upperValue,
 194                                            b2Bound* bounds, int32 boundCount, int32 axis)
 195 {
 196         int32 lowerQuery = BinarySearch(bounds, boundCount, lowerValue);
 197         int32 upperQuery = BinarySearch(bounds, boundCount, upperValue);
 198
 199         // Easy case: lowerQuery <= lowerIndex(i) < upperQuery
 200         // Solution: search query range for min bounds.
 201         for (int32 i = lowerQuery; i < upperQuery; ++i)
 202         {
 203                 if (bounds[i].IsLower())
 204                 {
 205                         IncrementOverlapCount(bounds[i].proxyId);
 206                 }
 207         }
 208
 209         // Hard case: lowerIndex(i) < lowerQuery < upperIndex(i)
 210         // Solution: use the stabbing count to search down the bound array.
 211         if (lowerQuery > 0)
 212         {
 213                 int32 i = lowerQuery - 1;
 214                 int32 s = bounds[i].stabbingCount;
 215
 216                 // Find the s overlaps.
 217                 while (s)
 218                 {
 219                         b2Assert(i >= 0);
 220
 221                         if (bounds[i].IsLower())
 222                         {
 223                                 b2Proxy* proxy = m_proxyPool + bounds[i].proxyId;
 224                                 if (lowerQuery <= proxy->upperBounds[axis])
 225                                 {
 226                                         IncrementOverlapCount(bounds[i].proxyId);
 227                                         --s;
 228                                 }
 229                         }
 230                         --i;
 231                 }
 232         }
 233
 234         *lowerQueryOut = lowerQuery;
 235         *upperQueryOut = upperQuery;
 236 }
 237
 238 uint16 b2BroadPhase::CreateProxy(const b2AABB& aabb, void* userData)
 239 {
 240         b2Assert(m_proxyCount < b2_maxProxies);
 241         b2Assert(m_freeProxy != b2_nullProxy);
 242
 243         uint16 proxyId = m_freeProxy;
 244         b2Proxy* proxy = m_proxyPool + proxyId;
 245         m_freeProxy = proxy->GetNext();
 246
 247         proxy->overlapCount = 0;
 248         proxy->userData = userData;
 249
 250         int32 boundCount = 2 * m_proxyCount;
 251
 252         uint16 lowerValues[2], upperValues[2];
 253         ComputeBounds(lowerValues, upperValues, aabb);
 254
 255         for (int32 axis = 0; axis < 2; ++axis)
 256         {
 257                 b2Bound* bounds = m_bounds[axis];
 258                 int32 lowerIndex, upperIndex;
 259                 Query(&lowerIndex, &upperIndex, lowerValues[axis], upperValues[axis], bounds, boundCount, axis);
 260
 261                 memmove(bounds + upperIndex + 2, bounds + upperIndex, (boundCount - upperIndex) * sizeof(b2Bound));
 262                 memmove(bounds + lowerIndex + 1, bounds + lowerIndex, (upperIndex - lowerIndex) * sizeof(b2Bound));
 263
 264                 // The upper index has increased because of the lower bound insertion.
 265                 ++upperIndex;
 266
 267                 // Copy in the new bounds.
 268                 bounds[lowerIndex].value = lowerValues[axis];
 269                 bounds[lowerIndex].proxyId = proxyId;
 270                 bounds[upperIndex].value = upperValues[axis];
 271                 bounds[upperIndex].proxyId = proxyId;
 272
 273                 bounds[lowerIndex].stabbingCount = lowerIndex == 0 ? 0 : bounds[lowerIndex-1].stabbingCount;
 274                 bounds[upperIndex].stabbingCount = bounds[upperIndex-1].stabbingCount;
 275
 276                 // Adjust the stabbing count between the new bounds.
 277                 for (int32 index = lowerIndex; index < upperIndex; ++index)
 278                 {
 279                         ++bounds[index].stabbingCount;
 280                 }
 281
 282                 // Adjust the all the affected bound indices.
 283                 for (int32 index = lowerIndex; index < boundCount + 2; ++index)
 284                 {
 285                         b2Proxy* proxy = m_proxyPool + bounds[index].proxyId;
 286                         if (bounds[index].IsLower())
 287                         {
 288                                 proxy->lowerBounds[axis] = (uint16)index;
 289                         }
 290                         else
 291                         {
 292                                 proxy->upperBounds[axis] = (uint16)index;
 293                         }
 294                 }
 295         }
 296
 297         ++m_proxyCount;
 298
 299         b2Assert(m_queryResultCount < b2_maxProxies);
 300
 301         // Create pairs if the AABB is in range.
 302         for (int32 i = 0; i < m_queryResultCount; ++i)
 303         {
 304                 b2Assert(m_queryResults[i] < b2_maxProxies);
 305                 b2Assert(m_proxyPool[m_queryResults[i]].IsValid());
 306
 307                 m_pairManager.AddBufferedPair(proxyId, m_queryResults[i]);
 308         }
 309
 310         m_pairManager.Commit();
 311
 312         if (s_validate)
 313         {
 314                 Validate();
 315         }
 316
 317         // Prepare for next query.
 318         m_queryResultCount = 0;
 319         IncrementTimeStamp();
 320
 321         return proxyId;
 322 }
 323
 324 void b2BroadPhase::DestroyProxy(int32 proxyId)
 325 {
 326         b2Assert(0 < m_proxyCount && m_proxyCount <= b2_maxProxies);
 327         b2Proxy* proxy = m_proxyPool + proxyId;
 328         b2Assert(proxy->IsValid());
 329
 330         int32 boundCount = 2 * m_proxyCount;
 331
 332         for (int32 axis = 0; axis < 2; ++axis)
 333         {
 334                 b2Bound* bounds = m_bounds[axis];
 335
 336                 int32 lowerIndex = proxy->lowerBounds[axis];
 337                 int32 upperIndex = proxy->upperBounds[axis];
 338                 uint16 lowerValue = bounds[lowerIndex].value;
 339                 uint16 upperValue = bounds[upperIndex].value;
 340
 341                 memmove(bounds + lowerIndex, bounds + lowerIndex + 1, (upperIndex - lowerIndex - 1) * sizeof(b2Bound));
 342                 memmove(bounds + upperIndex-1, bounds + upperIndex + 1, (boundCount - upperIndex - 1) * sizeof(b2Bound));
 343
 344                 // Fix bound indices.
 345                 for (int32 index = lowerIndex; index < boundCount - 2; ++index)
 346                 {
 347                         b2Proxy* proxy = m_proxyPool + bounds[index].proxyId;
 348                         if (bounds[index].IsLower())
 349                         {
 350                                 proxy->lowerBounds[axis] = (uint16)index;
 351                         }
 352                         else
 353                         {
 354                                 proxy->upperBounds[axis] = (uint16)index;
 355                         }
 356                 }
 357
 358                 // Fix stabbing count.
 359                 for (int32 index = lowerIndex; index < upperIndex - 1; ++index)
 360                 {
 361                         --bounds[index].stabbingCount;
 362                 }
 363
 364                 // Query for pairs to be removed. lowerIndex and upperIndex are not needed.
 365                 Query(&lowerIndex, &upperIndex, lowerValue, upperValue, bounds, boundCount - 2, axis);
 366         }
 367
 368         b2Assert(m_queryResultCount < b2_maxProxies);
 369
 370         for (int32 i = 0; i < m_queryResultCount; ++i)
 371         {
 372                 b2Assert(m_proxyPool[m_queryResults[i]].IsValid());
 373                 m_pairManager.RemoveBufferedPair(proxyId, m_queryResults[i]);
 374         }
 375
 376         m_pairManager.Commit();
 377
 378         // Prepare for next query.
 379         m_queryResultCount = 0;
 380         IncrementTimeStamp();
 381
 382         // Return the proxy to the pool.
 383         proxy->userData = NULL;
 384         proxy->overlapCount = b2_invalid;
 385         proxy->lowerBounds[0] = b2_invalid;
 386         proxy->lowerBounds[1] = b2_invalid;
 387         proxy->upperBounds[0] = b2_invalid;
 388         proxy->upperBounds[1] = b2_invalid;
 389
 390         proxy->SetNext(m_freeProxy);
 391         m_freeProxy = (uint16)proxyId;
 392         --m_proxyCount;
 393
 394         if (s_validate)
 395         {
 396                 Validate();
 397         }
 398 }
 399
 400 void b2BroadPhase::MoveProxy(int32 proxyId, const b2AABB& aabb)
 401 {
 402         if (proxyId == b2_nullProxy || b2_maxProxies <= proxyId)
 403         {
 404                 b2Assert(false);
 405                 return;
 406         }
 407
 408         if (aabb.IsValid() == false)
 409         {
 410                 b2Assert(false);
 411                 return;
 412         }
 413
 414         int32 boundCount = 2 * m_proxyCount;
 415
 416         b2Proxy* proxy = m_proxyPool + proxyId;
 417
 418         // Get new bound values
 419         b2BoundValues newValues;
 420         ComputeBounds(newValues.lowerValues, newValues.upperValues, aabb);
 421
 422         // Get old bound values
 423         b2BoundValues oldValues;
 424         for (int32 axis = 0; axis < 2; ++axis)
 425         {
 426                 oldValues.lowerValues[axis] = m_bounds[axis][proxy->lowerBounds[axis]].value;
 427                 oldValues.upperValues[axis] = m_bounds[axis][proxy->upperBounds[axis]].value;
 428         }
 429
 430         for (int32 axis = 0; axis < 2; ++axis)
 431         {
 432                 b2Bound* bounds = m_bounds[axis];
 433
 434                 int32 lowerIndex = proxy->lowerBounds[axis];
 435                 int32 upperIndex = proxy->upperBounds[axis];
 436
 437                 uint16 lowerValue = newValues.lowerValues[axis];
 438                 uint16 upperValue = newValues.upperValues[axis];
 439
 440                 int32 deltaLower = lowerValue - bounds[lowerIndex].value;
 441                 int32 deltaUpper = upperValue - bounds[upperIndex].value;
 442
 443                 bounds[lowerIndex].value = lowerValue;
 444                 bounds[upperIndex].value = upperValue;
 445
 446                 //
 447                 // Expanding adds overlaps
 448                 //
 449
 450                 // Should we move the lower bound down?
 451                 if (deltaLower < 0)
 452                 {
 453                         int32 index = lowerIndex;
 454                         while (index > 0 && lowerValue < bounds[index-1].value)
 455                         {
 456                                 b2Bound* bound = bounds + index;
 457                                 b2Bound* prevBound = bound - 1;
 458
 459                                 int32 prevProxyId = prevBound->proxyId;
 460                                 b2Proxy* prevProxy = m_proxyPool + prevBound->proxyId;
 461
 462                                 ++prevBound->stabbingCount;
 463
 464                                 if (prevBound->IsUpper() == true)
 465                                 {
 466                                         if (TestOverlap(newValues, prevProxy))
 467                                         {
 468                                                 m_pairManager.AddBufferedPair(proxyId, prevProxyId);
 469                                         }
 470
 471                                         ++prevProxy->upperBounds[axis];
 472                                         ++bound->stabbingCount;
 473                                 }
 474                                 else
 475                                 {
 476                                         ++prevProxy->lowerBounds[axis];
 477                                         --bound->stabbingCount;
 478                                 }
 479
 480                                 --proxy->lowerBounds[axis];
 481                                 b2Swap(*bound, *prevBound);
 482                                 --index;
 483                         }
 484                 }
 485
 486                 // Should we move the upper bound up?
 487                 if (deltaUpper > 0)
 488                 {
 489                         int32 index = upperIndex;
 490                         while (index < boundCount-1 && bounds[index+1].value <= upperValue)
 491                         {
 492                                 b2Bound* bound = bounds + index;
 493                                 b2Bound* nextBound = bound + 1;
 494                                 int32 nextProxyId = nextBound->proxyId;
 495                                 b2Proxy* nextProxy = m_proxyPool + nextProxyId;
 496
 497                                 ++nextBound->stabbingCount;
 498
 499                                 if (nextBound->IsLower() == true)
 500                                 {
 501                                         if (TestOverlap(newValues, nextProxy))
 502                                         {
 503                                                 m_pairManager.AddBufferedPair(proxyId, nextProxyId);
 504                                         }
 505
 506                                         --nextProxy->lowerBounds[axis];
 507                                         ++bound->stabbingCount;
 508                                 }
 509                                 else
 510                                 {
 511                                         --nextProxy->upperBounds[axis];
 512                                         --bound->stabbingCount;
 513                                 }
 514
 515                                 ++proxy->upperBounds[axis];
 516                                 b2Swap(*bound, *nextBound);
 517                                 ++index;
 518                         }
 519                 }
 520
 521                 //
 522                 // Shrinking removes overlaps
 523                 //
 524
 525                 // Should we move the lower bound up?
 526                 if (deltaLower > 0)
 527                 {
 528                         int32 index = lowerIndex;
 529                         while (index < boundCount-1 && bounds[index+1].value <= lowerValue)
 530                         {
 531                                 b2Bound* bound = bounds + index;
 532                                 b2Bound* nextBound = bound + 1;
 533
 534                                 int32 nextProxyId = nextBound->proxyId;
 535                                 b2Proxy* nextProxy = m_proxyPool + nextProxyId;
 536
 537                                 --nextBound->stabbingCount;
 538
 539                                 if (nextBound->IsUpper())
 540                                 {
 541                                         if (TestOverlap(oldValues, nextProxy))
 542                                         {
 543                                                 m_pairManager.RemoveBufferedPair(proxyId, nextProxyId);
 544                                         }
 545
 546                                         --nextProxy->upperBounds[axis];
 547                                         --bound->stabbingCount;
 548                                 }
 549                                 else
 550                                 {
 551                                         --nextProxy->lowerBounds[axis];
 552                                         ++bound->stabbingCount;
 553                                 }
 554
 555                                 ++proxy->lowerBounds[axis];
 556                                 b2Swap(*bound, *nextBound);
 557                                 ++index;
 558                         }
 559                 }
 560
 561                 // Should we move the upper bound down?
 562                 if (deltaUpper < 0)
 563                 {
 564                         int32 index = upperIndex;
 565                         while (index > 0 && upperValue < bounds[index-1].value)
 566                         {
 567                                 b2Bound* bound = bounds + index;
 568                                 b2Bound* prevBound = bound - 1;
 569
 570                                 int32 prevProxyId = prevBound->proxyId;
 571                                 b2Proxy* prevProxy = m_proxyPool + prevProxyId;
 572
 573                                 --prevBound->stabbingCount;
 574
 575                                 if (prevBound->IsLower() == true)
 576                                 {
 577                                         if (TestOverlap(oldValues, prevProxy))
 578                                         {
 579                                                 m_pairManager.RemoveBufferedPair(proxyId, prevProxyId);
 580                                         }
 581
 582                                         ++prevProxy->lowerBounds[axis];
 583                                         --bound->stabbingCount;
 584                                 }
 585                                 else
 586                                 {
 587                                         ++prevProxy->upperBounds[axis];
 588                                         ++bound->stabbingCount;
 589                                 }
 590
 591                                 --proxy->upperBounds[axis];
 592                                 b2Swap(*bound, *prevBound);
 593                                 --index;
 594                         }
 595                 }
 596         }
 597
 598         if (s_validate)
 599         {
 600                 Validate();
 601         }
 602 }
 603
 604 void b2BroadPhase::Commit()
 605 {
 606         m_pairManager.Commit();
 607 }
 608
 609 int32 b2BroadPhase::Query(const b2AABB& aabb, void** userData, int32 maxCount)
 610 {
 611         uint16 lowerValues[2];
 612         uint16 upperValues[2];
 613         ComputeBounds(lowerValues, upperValues, aabb);
 614
 615         int32 lowerIndex, upperIndex;
 616
 617         Query(&lowerIndex, &upperIndex, lowerValues[0], upperValues[0], m_bounds[0], 2*m_proxyCount, 0);
 618         Query(&lowerIndex, &upperIndex, lowerValues[1], upperValues[1], m_bounds[1], 2*m_proxyCount, 1);
 619
 620         b2Assert(m_queryResultCount < b2_maxProxies);
 621
 622         int32 count = 0;
 623         for (int32 i = 0; i < m_queryResultCount && count < maxCount; ++i, ++count)
 624         {
 625                 b2Assert(m_queryResults[i] < b2_maxProxies);
 626                 b2Proxy* proxy = m_proxyPool + m_queryResults[i];
 627                 b2Assert(proxy->IsValid());
 628                 userData[i] = proxy->userData;
 629         }
 630
 631         // Prepare for next query.
 632         m_queryResultCount = 0;
 633         IncrementTimeStamp();
 634
 635         return count;
 636 }
 637
 638 void b2BroadPhase::Validate()
 639 {
 640         for (int32 axis = 0; axis < 2; ++axis)
 641         {
 642                 b2Bound* bounds = m_bounds[axis];
 643
 644                 int32 boundCount = 2 * m_proxyCount;
 645                 uint16 stabbingCount = 0;
 646
 647                 for (int32 i = 0; i < boundCount; ++i)
 648                 {
 649                         b2Bound* bound = bounds + i;
 650                         b2Assert(i == 0 || bounds[i-1].value <= bound->value);
 651                         b2Assert(bound->proxyId != b2_nullProxy);
 652                         b2Assert(m_proxyPool[bound->proxyId].IsValid());
 653
 654                         if (bound->IsLower() == true)
 655                         {
 656                                 b2Assert(m_proxyPool[bound->proxyId].lowerBounds[axis] == i);
 657                                 ++stabbingCount;
 658                         }
 659                         else
 660                         {
 661                                 b2Assert(m_proxyPool[bound->proxyId].upperBounds[axis] == i);
 662                                 --stabbingCount;
 663                         }
 664
 665                         b2Assert(bound->stabbingCount == stabbingCount);
 666                 }
 667         }
 668 }