dali-physics/third-party/bullet3/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages 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 freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 #include "btHeightfieldTerrainShape.h"
  17
  18 #include "LinearMath/btTransformUtil.h"
  19
  20 btHeightfieldTerrainShape::btHeightfieldTerrainShape(
  21         int heightStickWidth, int heightStickLength,
  22         const float* heightfieldData, btScalar minHeight, btScalar maxHeight,
  23         int upAxis, bool flipQuadEdges)
  24         : m_userValue3(0), m_triangleInfoMap(0)
  25 {
  26         initialize(heightStickWidth, heightStickLength, heightfieldData,
  27                            /*heightScale=*/1, minHeight, maxHeight, upAxis, PHY_FLOAT,
  28                            flipQuadEdges);
  29 }
  30
  31 btHeightfieldTerrainShape::btHeightfieldTerrainShape(
  32         int heightStickWidth, int heightStickLength, const double* heightfieldData,
  33         btScalar minHeight, btScalar maxHeight, int upAxis, bool flipQuadEdges)
  34         : m_userValue3(0), m_triangleInfoMap(0)
  35 {
  36         initialize(heightStickWidth, heightStickLength, heightfieldData,
  37                            /*heightScale=*/1, minHeight, maxHeight, upAxis, PHY_DOUBLE,
  38                            flipQuadEdges);
  39 }
  40
  41 btHeightfieldTerrainShape::btHeightfieldTerrainShape(
  42         int heightStickWidth, int heightStickLength, const short* heightfieldData, btScalar heightScale,
  43         btScalar minHeight, btScalar maxHeight, int upAxis, bool flipQuadEdges)
  44         : m_userValue3(0), m_triangleInfoMap(0)
  45 {
  46         initialize(heightStickWidth, heightStickLength, heightfieldData,
  47                            heightScale, minHeight, maxHeight, upAxis, PHY_SHORT,
  48                            flipQuadEdges);
  49 }
  50
  51 btHeightfieldTerrainShape::btHeightfieldTerrainShape(
  52         int heightStickWidth, int heightStickLength, const unsigned char* heightfieldData, btScalar heightScale,
  53         btScalar minHeight, btScalar maxHeight, int upAxis, bool flipQuadEdges)
  54         : m_userValue3(0), m_triangleInfoMap(0)
  55 {
  56         initialize(heightStickWidth, heightStickLength, heightfieldData,
  57                            heightScale, minHeight, maxHeight, upAxis, PHY_UCHAR,
  58                            flipQuadEdges);
  59 }
  60
  61 btHeightfieldTerrainShape::btHeightfieldTerrainShape(
  62         int heightStickWidth, int heightStickLength, const void* heightfieldData,
  63         btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis,
  64         PHY_ScalarType hdt, bool flipQuadEdges)
  65         :m_userValue3(0),
  66         m_triangleInfoMap(0)
  67 {
  68         // legacy constructor: Assumes PHY_FLOAT means btScalar.
  69 #ifdef BT_USE_DOUBLE_PRECISION
  70         if (hdt == PHY_FLOAT) hdt = PHY_DOUBLE;
  71 #endif
  72         initialize(heightStickWidth, heightStickLength, heightfieldData,
  73                            heightScale, minHeight, maxHeight, upAxis, hdt,
  74                            flipQuadEdges);
  75 }
  76
  77 btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar maxHeight, int upAxis, bool useFloatData, bool flipQuadEdges)
  78         :       m_userValue3(0),
  79         m_triangleInfoMap(0)
  80 {
  81         // legacy constructor: support only btScalar or unsigned char data,
  82         // and min height is zero.
  83         PHY_ScalarType hdt = (useFloatData) ? PHY_FLOAT : PHY_UCHAR;
  84 #ifdef BT_USE_DOUBLE_PRECISION
  85         if (hdt == PHY_FLOAT) hdt = PHY_DOUBLE;
  86 #endif
  87         btScalar minHeight = 0.0f;
  88
  89         // previously, height = uchar * maxHeight / 65535.
  90         // So to preserve legacy behavior, heightScale = maxHeight / 65535
  91         btScalar heightScale = maxHeight / 65535;
  92
  93         initialize(heightStickWidth, heightStickLength, heightfieldData,
  94                            heightScale, minHeight, maxHeight, upAxis, hdt,
  95                            flipQuadEdges);
  96 }
  97
  98 void btHeightfieldTerrainShape::initialize(
  99         int heightStickWidth, int heightStickLength, const void* heightfieldData,
 100         btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis,
 101         PHY_ScalarType hdt, bool flipQuadEdges)
 102 {
 103         // validation
 104         btAssert(heightStickWidth > 1);   // && "bad width");
 105         btAssert(heightStickLength > 1);  // && "bad length");
 106         btAssert(heightfieldData);        // && "null heightfield data");
 107         // btAssert(heightScale) -- do we care?  Trust caller here
 108         btAssert(minHeight <= maxHeight);                                    // && "bad min/max height");
 109         btAssert(upAxis >= 0 && upAxis < 3);                                 // && "bad upAxis--should be in range [0,2]");
 110         btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_DOUBLE || hdt != PHY_SHORT);  // && "Bad height data type enum");
 111
 112         // initialize member variables
 113         m_shapeType = TERRAIN_SHAPE_PROXYTYPE;
 114         m_heightStickWidth = heightStickWidth;
 115         m_heightStickLength = heightStickLength;
 116         m_minHeight = minHeight;
 117         m_maxHeight = maxHeight;
 118         m_width = (btScalar)(heightStickWidth - 1);
 119         m_length = (btScalar)(heightStickLength - 1);
 120         m_heightScale = heightScale;
 121         m_heightfieldDataUnknown = heightfieldData;
 122         m_heightDataType = hdt;
 123         m_flipQuadEdges = flipQuadEdges;
 124         m_useDiamondSubdivision = false;
 125         m_useZigzagSubdivision = false;
 126         m_flipTriangleWinding = false;
 127         m_upAxis = upAxis;
 128         m_localScaling.setValue(btScalar(1.), btScalar(1.), btScalar(1.));
 129
 130         m_vboundsChunkSize = 0;
 131         m_vboundsGridWidth = 0;
 132         m_vboundsGridLength = 0;
 133
 134         // determine min/max axis-aligned bounding box (aabb) values
 135         switch (m_upAxis)
 136         {
 137                 case 0:
 138                 {
 139                         m_localAabbMin.setValue(m_minHeight, 0, 0);
 140                         m_localAabbMax.setValue(m_maxHeight, m_width, m_length);
 141                         break;
 142                 }
 143                 case 1:
 144                 {
 145                         m_localAabbMin.setValue(0, m_minHeight, 0);
 146                         m_localAabbMax.setValue(m_width, m_maxHeight, m_length);
 147                         break;
 148                 };
 149                 case 2:
 150                 {
 151                         m_localAabbMin.setValue(0, 0, m_minHeight);
 152                         m_localAabbMax.setValue(m_width, m_length, m_maxHeight);
 153                         break;
 154                 }
 155                 default:
 156                 {
 157                         //need to get valid m_upAxis
 158                         btAssert(0);  // && "Bad m_upAxis");
 159                 }
 160         }
 161
 162         // remember origin (defined as exact middle of aabb)
 163         m_localOrigin = btScalar(0.5) * (m_localAabbMin + m_localAabbMax);
 164 }
 165
 166 btHeightfieldTerrainShape::~btHeightfieldTerrainShape()
 167 {
 168         clearAccelerator();
 169 }
 170
 171 void btHeightfieldTerrainShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
 172 {
 173         btVector3 halfExtents = (m_localAabbMax - m_localAabbMin) * m_localScaling * btScalar(0.5);
 174
 175         btVector3 localOrigin(0, 0, 0);
 176         localOrigin[m_upAxis] = (m_minHeight + m_maxHeight) * btScalar(0.5);
 177         localOrigin *= m_localScaling;
 178
 179         btMatrix3x3 abs_b = t.getBasis().absolute();
 180         btVector3 center = t.getOrigin();
 181         btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
 182         extent += btVector3(getMargin(), getMargin(), getMargin());
 183
 184         aabbMin = center - extent;
 185         aabbMax = center + extent;
 186 }
 187
 188 /// This returns the "raw" (user's initial) height, not the actual height.
 189 /// The actual height needs to be adjusted to be relative to the center
 190 ///   of the heightfield's AABB.
 191 btScalar
 192 btHeightfieldTerrainShape::getRawHeightFieldValue(int x, int y) const
 193 {
 194         btScalar val = 0.f;
 195         switch (m_heightDataType)
 196         {
 197                 case PHY_FLOAT:
 198                 {
 199                         val = m_heightfieldDataFloat[(y * m_heightStickWidth) + x];
 200                         break;
 201                 }
 202
 203                 case PHY_DOUBLE:
 204                 {
 205                         val = m_heightfieldDataDouble[(y * m_heightStickWidth) + x];
 206                         break;
 207                 }
 208
 209                 case PHY_UCHAR:
 210                 {
 211                         unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y * m_heightStickWidth) + x];
 212                         val = heightFieldValue * m_heightScale;
 213                         break;
 214                 }
 215
 216                 case PHY_SHORT:
 217                 {
 218                         short hfValue = m_heightfieldDataShort[(y * m_heightStickWidth) + x];
 219                         val = hfValue * m_heightScale;
 220                         break;
 221                 }
 222
 223                 default:
 224                 {
 225                         btAssert(!"Bad m_heightDataType");
 226                 }
 227         }
 228
 229         return val;
 230 }
 231
 232 /// this returns the vertex in bullet-local coordinates
 233 void btHeightfieldTerrainShape::getVertex(int x, int y, btVector3& vertex) const
 234 {
 235         btAssert(x >= 0);
 236         btAssert(y >= 0);
 237         btAssert(x < m_heightStickWidth);
 238         btAssert(y < m_heightStickLength);
 239
 240         btScalar height = getRawHeightFieldValue(x, y);
 241
 242         switch (m_upAxis)
 243         {
 244                 case 0:
 245                 {
 246                         vertex.setValue(
 247                                 height - m_localOrigin.getX(),
 248                                 (-m_width / btScalar(2.0)) + x,
 249                                 (-m_length / btScalar(2.0)) + y);
 250                         break;
 251                 }
 252                 case 1:
 253                 {
 254                         vertex.setValue(
 255                                 (-m_width / btScalar(2.0)) + x,
 256                                 height - m_localOrigin.getY(),
 257                                 (-m_length / btScalar(2.0)) + y);
 258                         break;
 259                 };
 260                 case 2:
 261                 {
 262                         vertex.setValue(
 263                                 (-m_width / btScalar(2.0)) + x,
 264                                 (-m_length / btScalar(2.0)) + y,
 265                                 height - m_localOrigin.getZ());
 266                         break;
 267                 }
 268                 default:
 269                 {
 270                         //need to get valid m_upAxis
 271                         btAssert(0);
 272                 }
 273         }
 274
 275         vertex *= m_localScaling;
 276 }
 277
 278 static inline int
 279 getQuantized(
 280         btScalar x)
 281 {
 282         if (x < 0.0)
 283         {
 284                 return (int)(x - 0.5);
 285         }
 286         return (int)(x + 0.5);
 287 }
 288
 289 // Equivalent to std::minmax({a, b, c}).
 290 // Performs at most 3 comparisons.
 291 static btHeightfieldTerrainShape::Range minmaxRange(btScalar a, btScalar b, btScalar c)
 292 {
 293         if (a > b)
 294         {
 295                 if (b > c)
 296                         return btHeightfieldTerrainShape::Range(c, a);
 297                 else if (a > c)
 298                         return btHeightfieldTerrainShape::Range(b, a);
 299                 else
 300                         return btHeightfieldTerrainShape::Range(b, c);
 301         }
 302         else
 303         {
 304                 if (a > c)
 305                         return btHeightfieldTerrainShape::Range(c, b);
 306                 else if (b > c)
 307                         return btHeightfieldTerrainShape::Range(a, b);
 308                 else
 309                         return btHeightfieldTerrainShape::Range(a, c);
 310         }
 311 }
 312
 313 /// given input vector, return quantized version
 314 /**
 315   This routine is basically determining the gridpoint indices for a given
 316   input vector, answering the question: "which gridpoint is closest to the
 317   provided point?".
 318
 319   "with clamp" means that we restrict the point to be in the heightfield's
 320   axis-aligned bounding box.
 321  */
 322 void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& point, int /*isMax*/) const
 323 {
 324         btVector3 clampedPoint(point);
 325         clampedPoint.setMax(m_localAabbMin);
 326         clampedPoint.setMin(m_localAabbMax);
 327
 328         out[0] = getQuantized(clampedPoint.getX());
 329         out[1] = getQuantized(clampedPoint.getY());
 330         out[2] = getQuantized(clampedPoint.getZ());
 331 }
 332
 333 /// process all triangles within the provided axis-aligned bounding box
 334 /**
 335   basic algorithm:
 336     - convert input aabb to local coordinates (scale down and shift for local origin)
 337     - convert input aabb to a range of heightfield grid points (quantize)
 338     - iterate over all triangles in that subset of the grid
 339  */
 340 void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const
 341 {
 342         // scale down the input aabb's so they are in local (non-scaled) coordinates
 343         btVector3 localAabbMin = aabbMin * btVector3(1.f / m_localScaling[0], 1.f / m_localScaling[1], 1.f / m_localScaling[2]);
 344         btVector3 localAabbMax = aabbMax * btVector3(1.f / m_localScaling[0], 1.f / m_localScaling[1], 1.f / m_localScaling[2]);
 345
 346         // account for local origin
 347         localAabbMin += m_localOrigin;
 348         localAabbMax += m_localOrigin;
 349
 350         //quantize the aabbMin and aabbMax, and adjust the start/end ranges
 351         int quantizedAabbMin[3];
 352         int quantizedAabbMax[3];
 353         quantizeWithClamp(quantizedAabbMin, localAabbMin, 0);
 354         quantizeWithClamp(quantizedAabbMax, localAabbMax, 1);
 355
 356         // expand the min/max quantized values
 357         // this is to catch the case where the input aabb falls between grid points!
 358         for (int i = 0; i < 3; ++i)
 359         {
 360                 quantizedAabbMin[i]--;
 361                 quantizedAabbMax[i]++;
 362         }
 363
 364         int startX = 0;
 365         int endX = m_heightStickWidth - 1;
 366         int startJ = 0;
 367         int endJ = m_heightStickLength - 1;
 368
 369         switch (m_upAxis)
 370         {
 371                 case 0:
 372                 {
 373                         if (quantizedAabbMin[1] > startX)
 374                                 startX = quantizedAabbMin[1];
 375                         if (quantizedAabbMax[1] < endX)
 376                                 endX = quantizedAabbMax[1];
 377                         if (quantizedAabbMin[2] > startJ)
 378                                 startJ = quantizedAabbMin[2];
 379                         if (quantizedAabbMax[2] < endJ)
 380                                 endJ = quantizedAabbMax[2];
 381                         break;
 382                 }
 383                 case 1:
 384                 {
 385                         if (quantizedAabbMin[0] > startX)
 386                                 startX = quantizedAabbMin[0];
 387                         if (quantizedAabbMax[0] < endX)
 388                                 endX = quantizedAabbMax[0];
 389                         if (quantizedAabbMin[2] > startJ)
 390                                 startJ = quantizedAabbMin[2];
 391                         if (quantizedAabbMax[2] < endJ)
 392                                 endJ = quantizedAabbMax[2];
 393                         break;
 394                 };
 395                 case 2:
 396                 {
 397                         if (quantizedAabbMin[0] > startX)
 398                                 startX = quantizedAabbMin[0];
 399                         if (quantizedAabbMax[0] < endX)
 400                                 endX = quantizedAabbMax[0];
 401                         if (quantizedAabbMin[1] > startJ)
 402                                 startJ = quantizedAabbMin[1];
 403                         if (quantizedAabbMax[1] < endJ)
 404                                 endJ = quantizedAabbMax[1];
 405                         break;
 406                 }
 407                 default:
 408                 {
 409                         //need to get valid m_upAxis
 410                         btAssert(0);
 411                 }
 412         }
 413
 414         // TODO If m_vboundsGrid is available, use it to determine if we really need to process this area
 415
 416         const Range aabbUpRange(aabbMin[m_upAxis], aabbMax[m_upAxis]);
 417         for (int j = startJ; j < endJ; j++)
 418         {
 419                 for (int x = startX; x < endX; x++)
 420                 {
 421                         btVector3 vertices[3];
 422                         int indices[3] = { 0, 1, 2 };
 423                         if (m_flipTriangleWinding)
 424                         {
 425                                 indices[0] = 2;
 426                                 indices[2] = 0;
 427                         }
 428
 429                         if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j + x) & 1)) || (m_useZigzagSubdivision && !(j & 1)))
 430                         {
 431                                 getVertex(x, j, vertices[indices[0]]);
 432                                 getVertex(x, j + 1, vertices[indices[1]]);
 433                                 getVertex(x + 1, j + 1, vertices[indices[2]]);
 434
 435                                 // Skip triangle processing if the triangle is out-of-AABB.
 436                                 Range upRange = minmaxRange(vertices[0][m_upAxis], vertices[1][m_upAxis], vertices[2][m_upAxis]);
 437
 438                                 if (upRange.overlaps(aabbUpRange))
 439                                         callback->processTriangle(vertices, 2 * x, j);
 440
 441                                 // already set: getVertex(x, j, vertices[indices[0]])
 442
 443                                 // equivalent to: getVertex(x + 1, j + 1, vertices[indices[1]]);
 444                                 vertices[indices[1]] = vertices[indices[2]];
 445
 446                                 getVertex(x + 1, j, vertices[indices[2]]);
 447                                 upRange.min = btMin(upRange.min, vertices[indices[2]][m_upAxis]);
 448                                 upRange.max = btMax(upRange.max, vertices[indices[2]][m_upAxis]);
 449
 450                                 if (upRange.overlaps(aabbUpRange))
 451                                         callback->processTriangle(vertices, 2 * x + 1, j);
 452                         }
 453                         else
 454                         {
 455                                 getVertex(x, j, vertices[indices[0]]);
 456                                 getVertex(x, j + 1, vertices[indices[1]]);
 457                                 getVertex(x + 1, j, vertices[indices[2]]);
 458
 459                                 // Skip triangle processing if the triangle is out-of-AABB.
 460                                 Range upRange = minmaxRange(vertices[0][m_upAxis], vertices[1][m_upAxis], vertices[2][m_upAxis]);
 461
 462                                 if (upRange.overlaps(aabbUpRange))
 463                                         callback->processTriangle(vertices, 2 * x, j);
 464
 465                                 // already set: getVertex(x, j + 1, vertices[indices[1]]);
 466
 467                                 // equivalent to: getVertex(x + 1, j, vertices[indices[0]]);
 468                                 vertices[indices[0]] = vertices[indices[2]];
 469
 470                                 getVertex(x + 1, j + 1, vertices[indices[2]]);
 471                                 upRange.min = btMin(upRange.min, vertices[indices[2]][m_upAxis]);
 472                                 upRange.max = btMax(upRange.max, vertices[indices[2]][m_upAxis]);
 473
 474                                 if (upRange.overlaps(aabbUpRange))
 475                                         callback->processTriangle(vertices, 2 * x + 1, j);
 476                         }
 477                 }
 478         }
 479 }
 480
 481 void btHeightfieldTerrainShape::calculateLocalInertia(btScalar, btVector3& inertia) const
 482 {
 483         //moving concave objects not supported
 484
 485         inertia.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
 486 }
 487
 488 void btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling)
 489 {
 490         m_localScaling = scaling;
 491 }
 492 const btVector3& btHeightfieldTerrainShape::getLocalScaling() const
 493 {
 494         return m_localScaling;
 495 }
 496
 497 namespace
 498 {
 499         struct GridRaycastState
 500         {
 501                 int x;  // Next quad coords
 502                 int z;
 503                 int prev_x;  // Previous quad coords
 504                 int prev_z;
 505                 btScalar param;      // Exit param for previous quad
 506                 btScalar prevParam;  // Enter param for previous quad
 507                 btScalar maxDistanceFlat;
 508                 btScalar maxDistance3d;
 509         };
 510 }
 511
 512 // TODO Does it really need to take 3D vectors?
 513 /// Iterates through a virtual 2D grid of unit-sized square cells,
 514 /// and executes an action on each cell intersecting the given segment, ordered from begin to end.
 515 /// Initially inspired by http://www.cse.yorku.ca/~amana/research/grid.pdf
 516 template <typename Action_T>
 517 void gridRaycast(Action_T& quadAction, const btVector3& beginPos, const btVector3& endPos, int indices[3])
 518 {
 519         GridRaycastState rs;
 520         rs.maxDistance3d = beginPos.distance(endPos);
 521         if (rs.maxDistance3d < 0.0001)
 522         {
 523                 // Consider the ray is too small to hit anything
 524                 return;
 525         }
 526
 527
 528         btScalar rayDirectionFlatX = endPos[indices[0]] - beginPos[indices[0]];
 529         btScalar rayDirectionFlatZ = endPos[indices[2]] - beginPos[indices[2]];
 530         rs.maxDistanceFlat = btSqrt(rayDirectionFlatX * rayDirectionFlatX + rayDirectionFlatZ * rayDirectionFlatZ);
 531
 532         if (rs.maxDistanceFlat < 0.0001)
 533         {
 534                 // Consider the ray vertical
 535                 rayDirectionFlatX = 0;
 536                 rayDirectionFlatZ = 0;
 537         }
 538         else
 539         {
 540                 rayDirectionFlatX /= rs.maxDistanceFlat;
 541                 rayDirectionFlatZ /= rs.maxDistanceFlat;
 542         }
 543
 544         const int xiStep = rayDirectionFlatX > 0 ? 1 : rayDirectionFlatX < 0 ? -1 : 0;
 545         const int ziStep = rayDirectionFlatZ > 0 ? 1 : rayDirectionFlatZ < 0 ? -1 : 0;
 546
 547         const float infinite = 9999999;
 548         const btScalar paramDeltaX = xiStep != 0 ? 1.f / btFabs(rayDirectionFlatX) : infinite;
 549         const btScalar paramDeltaZ = ziStep != 0 ? 1.f / btFabs(rayDirectionFlatZ) : infinite;
 550
 551         // pos = param * dir
 552         btScalar paramCrossX;  // At which value of `param` we will cross a x-axis lane?
 553         btScalar paramCrossZ;  // At which value of `param` we will cross a z-axis lane?
 554
 555         // paramCrossX and paramCrossZ are initialized as being the first cross
 556         // X initialization
 557         if (xiStep != 0)
 558         {
 559                 if (xiStep == 1)
 560                 {
 561                         paramCrossX = (ceil(beginPos[indices[0]]) - beginPos[indices[0]]) * paramDeltaX;
 562                 }
 563                 else
 564                 {
 565                         paramCrossX = (beginPos[indices[0]] - floor(beginPos[indices[0]])) * paramDeltaX;
 566                 }
 567         }
 568         else
 569         {
 570                 paramCrossX = infinite;  // Will never cross on X
 571         }
 572
 573         // Z initialization
 574         if (ziStep != 0)
 575         {
 576                 if (ziStep == 1)
 577                 {
 578                         paramCrossZ = (ceil(beginPos[indices[2]]) - beginPos[indices[2]]) * paramDeltaZ;
 579                 }
 580                 else
 581                 {
 582                         paramCrossZ = (beginPos[indices[2]] - floor(beginPos[indices[2]])) * paramDeltaZ;
 583                 }
 584         }
 585         else
 586         {
 587                 paramCrossZ = infinite;  // Will never cross on Z
 588         }
 589
 590         rs.x = static_cast<int>(floor(beginPos[indices[0]]));
 591         rs.z = static_cast<int>(floor(beginPos[indices[2]]));
 592
 593         // Workaround cases where the ray starts at an integer position
 594         if (paramCrossX == 0.0)
 595         {
 596                 paramCrossX += paramDeltaX;
 597                 // If going backwards, we should ignore the position we would get by the above flooring,
 598                 // because the ray is not heading in that direction
 599                 if (xiStep == -1)
 600                 {
 601                         rs.x -= 1;
 602                 }
 603         }
 604
 605         if (paramCrossZ == 0.0)
 606         {
 607                 paramCrossZ += paramDeltaZ;
 608                 if (ziStep == -1)
 609                         rs.z -= 1;
 610         }
 611
 612         rs.prev_x = rs.x;
 613         rs.prev_z = rs.z;
 614         rs.param = 0;
 615
 616         while (true)
 617         {
 618                 rs.prev_x = rs.x;
 619                 rs.prev_z = rs.z;
 620                 rs.prevParam = rs.param;
 621
 622                 if (paramCrossX < paramCrossZ)
 623                 {
 624                         // X lane
 625                         rs.x += xiStep;
 626                         // Assign before advancing the param,
 627                         // to be in sync with the initialization step
 628                         rs.param = paramCrossX;
 629                         paramCrossX += paramDeltaX;
 630                 }
 631                 else
 632                 {
 633                         // Z lane
 634                         rs.z += ziStep;
 635                         rs.param = paramCrossZ;
 636                         paramCrossZ += paramDeltaZ;
 637                 }
 638
 639                 if (rs.param > rs.maxDistanceFlat)
 640                 {
 641                         rs.param = rs.maxDistanceFlat;
 642                         quadAction(rs);
 643                         break;
 644                 }
 645                 else
 646                 {
 647                         quadAction(rs);
 648                 }
 649         }
 650 }
 651
 652 struct ProcessTrianglesAction
 653 {
 654         const btHeightfieldTerrainShape* shape;
 655         bool flipQuadEdges;
 656         bool useDiamondSubdivision;
 657         int width;
 658         int length;
 659         btTriangleCallback* callback;
 660
 661         void exec(int x, int z) const
 662         {
 663                 if (x < 0 || z < 0 || x >= width || z >= length)
 664                 {
 665                         return;
 666                 }
 667
 668                 btVector3 vertices[3];
 669
 670                 // TODO Since this is for raycasts, we could greatly benefit from an early exit on the first hit
 671
 672                 // Check quad
 673                 if (flipQuadEdges || (useDiamondSubdivision && (((z + x) & 1) > 0)))
 674                 {
 675                         // First triangle
 676                         shape->getVertex(x, z, vertices[0]);
 677                         shape->getVertex(x + 1, z, vertices[1]);
 678                         shape->getVertex(x + 1, z + 1, vertices[2]);
 679                         callback->processTriangle(vertices, x, z);
 680
 681                         // Second triangle
 682                         shape->getVertex(x, z, vertices[0]);
 683                         shape->getVertex(x + 1, z + 1, vertices[1]);
 684                         shape->getVertex(x, z + 1, vertices[2]);
 685                         callback->processTriangle(vertices, x, z);
 686                 }
 687                 else
 688                 {
 689                         // First triangle
 690                         shape->getVertex(x, z, vertices[0]);
 691                         shape->getVertex(x, z + 1, vertices[1]);
 692                         shape->getVertex(x + 1, z, vertices[2]);
 693                         callback->processTriangle(vertices, x, z);
 694
 695                         // Second triangle
 696                         shape->getVertex(x + 1, z, vertices[0]);
 697                         shape->getVertex(x, z + 1, vertices[1]);
 698                         shape->getVertex(x + 1, z + 1, vertices[2]);
 699                         callback->processTriangle(vertices, x, z);
 700                 }
 701         }
 702
 703         void operator()(const GridRaycastState& bs) const
 704         {
 705                 exec(bs.prev_x, bs.prev_z);
 706         }
 707 };
 708
 709 struct ProcessVBoundsAction
 710 {
 711         const btAlignedObjectArray<btHeightfieldTerrainShape::Range>& vbounds;
 712         int width;
 713         int length;
 714         int chunkSize;
 715
 716         btVector3 rayBegin;
 717         btVector3 rayEnd;
 718         btVector3 rayDir;
 719
 720         int* m_indices;
 721         ProcessTrianglesAction processTriangles;
 722
 723         ProcessVBoundsAction(const btAlignedObjectArray<btHeightfieldTerrainShape::Range>& bnd, int* indices)
 724                 : vbounds(bnd),
 725                 m_indices(indices)
 726         {
 727         }
 728         void operator()(const GridRaycastState& rs) const
 729         {
 730                 int x = rs.prev_x;
 731                 int z = rs.prev_z;
 732
 733                 if (x < 0 || z < 0 || x >= width || z >= length)
 734                 {
 735                         return;
 736                 }
 737
 738                 const btHeightfieldTerrainShape::Range chunk = vbounds[x + z * width];
 739
 740                 btVector3 enterPos;
 741                 btVector3 exitPos;
 742
 743                 if (rs.maxDistanceFlat > 0.0001)
 744                 {
 745                         btScalar flatTo3d = chunkSize * rs.maxDistance3d / rs.maxDistanceFlat;
 746                         btScalar enterParam3d = rs.prevParam * flatTo3d;
 747                         btScalar exitParam3d = rs.param * flatTo3d;
 748                         enterPos = rayBegin + rayDir * enterParam3d;
 749                         exitPos = rayBegin + rayDir * exitParam3d;
 750
 751                         // We did enter the flat projection of the AABB,
 752                         // but we have to check if we intersect it on the vertical axis
 753                         if (enterPos[1] > chunk.max && exitPos[m_indices[1]] > chunk.max)
 754                         {
 755                                 return;
 756                         }
 757                         if (enterPos[1] < chunk.min && exitPos[m_indices[1]] < chunk.min)
 758                         {
 759                                 return;
 760                         }
 761                 }
 762                 else
 763                 {
 764                         // Consider the ray vertical
 765                         // (though we shouldn't reach this often because there is an early check up-front)
 766                         enterPos = rayBegin;
 767                         exitPos = rayEnd;
 768                 }
 769
 770                 gridRaycast(processTriangles, enterPos, exitPos, m_indices);
 771                 // Note: it could be possible to have more than one grid at different levels,
 772                 // to do this there would be a branch using a pointer to another ProcessVBoundsAction
 773         }
 774 };
 775
 776 // TODO How do I interrupt the ray when there is a hit? `callback` does not return any result
 777 /// Performs a raycast using a hierarchical Bresenham algorithm.
 778 /// Does not allocate any memory by itself.
 779 void btHeightfieldTerrainShape::performRaycast(btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget) const
 780 {
 781         // Transform to cell-local
 782         btVector3 beginPos = raySource / m_localScaling;
 783         btVector3 endPos = rayTarget / m_localScaling;
 784         beginPos += m_localOrigin;
 785         endPos += m_localOrigin;
 786
 787         ProcessTrianglesAction processTriangles;
 788         processTriangles.shape = this;
 789         processTriangles.flipQuadEdges = m_flipQuadEdges;
 790         processTriangles.useDiamondSubdivision = m_useDiamondSubdivision;
 791         processTriangles.callback = callback;
 792         processTriangles.width = m_heightStickWidth - 1;
 793         processTriangles.length = m_heightStickLength - 1;
 794
 795         // TODO Transform vectors to account for m_upAxis
 796         int indices[3] = { 0, 1, 2 };
 797         if (m_upAxis == 2)
 798         {
 799                 indices[1] = 2;
 800                 indices[2] = 1;
 801         }
 802         int iBeginX = static_cast<int>(floor(beginPos[indices[0]]));
 803         int iBeginZ = static_cast<int>(floor(beginPos[indices[2]]));
 804         int iEndX = static_cast<int>(floor(endPos[indices[0]]));
 805         int iEndZ = static_cast<int>(floor(endPos[indices[2]]));
 806
 807         if (iBeginX == iEndX && iBeginZ == iEndZ)
 808         {
 809                 // The ray will never cross quads within the plane,
 810                 // so directly process triangles within one quad
 811                 // (typically, vertical rays should end up here)
 812                 processTriangles.exec(iBeginX, iEndZ);
 813                 return;
 814         }
 815
 816
 817
 818         if (m_vboundsGrid.size()==0)
 819         {
 820                 // Process all quads intersecting the flat projection of the ray
 821                 gridRaycast(processTriangles, beginPos, endPos, &indices[0]);
 822         }
 823         else
 824         {
 825                 btVector3 rayDiff = endPos - beginPos;
 826                 btScalar flatDistance2 = rayDiff[indices[0]] * rayDiff[indices[0]] + rayDiff[indices[2]] * rayDiff[indices[2]];
 827                 if (flatDistance2 < m_vboundsChunkSize * m_vboundsChunkSize)
 828                 {
 829                         // Don't use chunks, the ray is too short in the plane
 830                         gridRaycast(processTriangles, beginPos, endPos, &indices[0]);
 831                         return;
 832                 }
 833
 834                 ProcessVBoundsAction processVBounds(m_vboundsGrid, &indices[0]);
 835                 processVBounds.width = m_vboundsGridWidth;
 836                 processVBounds.length = m_vboundsGridLength;
 837                 processVBounds.rayBegin = beginPos;
 838                 processVBounds.rayEnd = endPos;
 839                 processVBounds.rayDir = rayDiff.normalized();
 840                 processVBounds.processTriangles = processTriangles;
 841                 processVBounds.chunkSize = m_vboundsChunkSize;
 842                 // The ray is long, run raycast on a higher-level grid
 843                 gridRaycast(processVBounds, beginPos / m_vboundsChunkSize, endPos / m_vboundsChunkSize, indices);
 844         }
 845 }
 846
 847 /// Builds a grid data structure storing the min and max heights of the terrain in chunks.
 848 /// if chunkSize is zero, that accelerator is removed.
 849 /// If you modify the heights, you need to rebuild this accelerator.
 850 void btHeightfieldTerrainShape::buildAccelerator(int chunkSize)
 851 {
 852         if (chunkSize <= 0)
 853         {
 854                 clearAccelerator();
 855                 return;
 856         }
 857
 858         m_vboundsChunkSize = chunkSize;
 859         int nChunksX = m_heightStickWidth / chunkSize;
 860         int nChunksZ = m_heightStickLength / chunkSize;
 861
 862         if (m_heightStickWidth % chunkSize > 0)
 863         {
 864                 ++nChunksX;  // In case terrain size isn't dividable by chunk size
 865         }
 866         if (m_heightStickLength % chunkSize > 0)
 867         {
 868                 ++nChunksZ;
 869         }
 870
 871         if (m_vboundsGridWidth != nChunksX || m_vboundsGridLength != nChunksZ)
 872         {
 873                 clearAccelerator();
 874                 m_vboundsGridWidth = nChunksX;
 875                 m_vboundsGridLength = nChunksZ;
 876         }
 877
 878         if (nChunksX == 0 || nChunksZ == 0)
 879         {
 880                 return;
 881         }
 882
 883         // This data structure is only reallocated if the required size changed
 884         m_vboundsGrid.resize(nChunksX * nChunksZ);
 885
 886         // Compute min and max height for all chunks
 887         for (int cz = 0; cz < nChunksZ; ++cz)
 888         {
 889                 int z0 = cz * chunkSize;
 890
 891                 for (int cx = 0; cx < nChunksX; ++cx)
 892                 {
 893                         int x0 = cx * chunkSize;
 894
 895                         Range r;
 896
 897                         r.min = getRawHeightFieldValue(x0, z0);
 898                         r.max = r.min;
 899
 900                         // Compute min and max height for this chunk.
 901                         // We have to include one extra cell to account for neighbors.
 902                         // Here is why:
 903                         // Say we have a flat terrain, and a plateau that fits a chunk perfectly.
 904                         //
 905                         //   Left        Right
 906                         // 0---0---0---1---1---1
 907                         // |   |   |   |   |   |
 908                         // 0---0---0---1---1---1
 909                         // |   |   |   |   |   |
 910                         // 0---0---0---1---1---1
 911                         //           x
 912                         //
 913                         // If the AABB for the Left chunk did not share vertices with the Right,
 914                         // then we would fail collision tests at x due to a gap.
 915                         //
 916                         for (int z = z0; z < z0 + chunkSize + 1; ++z)
 917                         {
 918                                 if (z >= m_heightStickLength)
 919                                 {
 920                                         continue;
 921                                 }
 922
 923                                 for (int x = x0; x < x0 + chunkSize + 1; ++x)
 924                                 {
 925                                         if (x >= m_heightStickWidth)
 926                                         {
 927                                                 continue;
 928                                         }
 929
 930                                         btScalar height = getRawHeightFieldValue(x, z);
 931
 932                                         if (height < r.min)
 933                                         {
 934                                                 r.min = height;
 935                                         }
 936                                         else if (height > r.max)
 937                                         {
 938                                                 r.max = height;
 939                                         }
 940                                 }
 941                         }
 942
 943                         m_vboundsGrid[cx + cz * nChunksX] = r;
 944                 }
 945         }
 946 }
 947
 948 void btHeightfieldTerrainShape::clearAccelerator()
 949 {
 950         m_vboundsGrid.clear();
 951 }