diff --git a/modules/bullet/shape_bullet.cpp b/modules/bullet/shape_bullet.cpp index 1aba31f03d..717a1c120a 100644 --- a/modules/bullet/shape_bullet.cpp +++ b/modules/bullet/shape_bullet.cpp @@ -148,7 +148,13 @@ btHeightfieldTerrainShape *ShapeBullet::create_shape_height_field(PoolVectorbuildAccelerator(16); + + return heightfield; } btRayShape *ShapeBullet::create_shape_ray(real_t p_length, bool p_slips_on_slope) { diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 782e9efaf1..b30ce03164 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -19,9 +19,10 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btCollisionShape.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting -#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" //for raycasting -#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h" //for raycasting +#include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" //for raycasting +#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h" //for raycasting +#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h" //for raycasting #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" @@ -413,6 +414,18 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans, co rcb.m_hitFraction = resultCallback.m_closestHitFraction; triangleMesh->performRaycast(&rcb, rayFromLocalScaled, rayToLocalScaled); } + else if (collisionShape->getShapeType()==TERRAIN_SHAPE_PROXYTYPE) + { + ///optimized version for btHeightfieldTerrainShape + btHeightfieldTerrainShape* heightField = (btHeightfieldTerrainShape*)collisionShape; + btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); + btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); + + BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),heightField,colObjWorldTransform); + rcb.m_hitFraction = resultCallback.m_closestHitFraction; + heightField->performRaycast(&rcb, rayFromLocal, rayToLocal); + } else { //generic (slower) case diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index c85ce2498e..4adf27e6bb 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -73,6 +73,10 @@ void btHeightfieldTerrainShape::initialize( m_useZigzagSubdivision = false; m_upAxis = upAxis; m_localScaling.setValue(btScalar(1.), btScalar(1.), btScalar(1.)); + m_vboundsGrid = NULL; + m_vboundsChunkSize = 0; + m_vboundsGridWidth = 0; + m_vboundsGridLength = 0; // determine min/max axis-aligned bounding box (aabb) values switch (m_upAxis) @@ -108,6 +112,7 @@ void btHeightfieldTerrainShape::initialize( btHeightfieldTerrainShape::~btHeightfieldTerrainShape() { + clearAccelerator(); } void btHeightfieldTerrainShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const @@ -323,6 +328,8 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback } } + // TODO If m_vboundsGrid is available, use it to determine if we really need to process this area + for (int j = startJ; j < endJ; j++) { for (int x = startX; x < endX; x++) @@ -373,3 +380,416 @@ const btVector3& btHeightfieldTerrainShape::getLocalScaling() const { return m_localScaling; } + + + +struct GridRaycastState +{ + int x; // Next quad coords + int z; + int prev_x; // Previous quad coords + int prev_z; + btScalar param; // Exit param for previous quad + btScalar prevParam; // Enter param for previous quad + btScalar maxDistanceFlat; + btScalar maxDistance3d; +}; + + +// TODO Does it really need to take 3D vectors? +/// Iterates through a virtual 2D grid of unit-sized square cells, +/// and executes an action on each cell intersecting the given segment, ordered from begin to end. +/// Initially inspired by http://www.cse.yorku.ca/~amana/research/grid.pdf +template +void gridRaycast(Action_T &quadAction, const btVector3 &beginPos, const btVector3 &endPos) +{ + GridRaycastState rs; + rs.maxDistance3d = beginPos.distance(endPos); + if (rs.maxDistance3d < 0.0001) + // Consider the ray is too small to hit anything + return; + + btScalar rayDirectionFlatX = endPos[0] - beginPos[0]; + btScalar rayDirectionFlatZ = endPos[2] - beginPos[2]; + rs.maxDistanceFlat = btSqrt(rayDirectionFlatX * rayDirectionFlatX + rayDirectionFlatZ * rayDirectionFlatZ); + + if(rs.maxDistanceFlat < 0.0001) + { + // Consider the ray vertical + rayDirectionFlatX = 0; + rayDirectionFlatZ = 0; + } + else + { + rayDirectionFlatX /= rs.maxDistanceFlat; + rayDirectionFlatZ /= rs.maxDistanceFlat; + } + + const int xiStep = rayDirectionFlatX > 0 ? 1 : rayDirectionFlatX < 0 ? -1 : 0; + const int ziStep = rayDirectionFlatZ > 0 ? 1 : rayDirectionFlatZ < 0 ? -1 : 0; + + const float infinite = 9999999; + const btScalar paramDeltaX = xiStep != 0 ? 1.f / btFabs(rayDirectionFlatX) : infinite; + const btScalar paramDeltaZ = ziStep != 0 ? 1.f / btFabs(rayDirectionFlatZ) : infinite; + + // pos = param * dir + btScalar paramCrossX; // At which value of `param` we will cross a x-axis lane? + btScalar paramCrossZ; // At which value of `param` we will cross a z-axis lane? + + // paramCrossX and paramCrossZ are initialized as being the first cross + // X initialization + if (xiStep != 0) + { + if (xiStep == 1) + paramCrossX = (ceil(beginPos[0]) - beginPos[0]) * paramDeltaX; + else + paramCrossX = (beginPos[0] - floor(beginPos[0])) * paramDeltaX; + } + else + paramCrossX = infinite; // Will never cross on X + + // Z initialization + if (ziStep != 0) + { + if (ziStep == 1) + paramCrossZ = (ceil(beginPos[2]) - beginPos[2]) * paramDeltaZ; + else + paramCrossZ = (beginPos[2] - floor(beginPos[2])) * paramDeltaZ; + } + else + paramCrossZ = infinite; // Will never cross on Z + + rs.x = static_cast(floor(beginPos[0])); + rs.z = static_cast(floor(beginPos[2])); + + // Workaround cases where the ray starts at an integer position + if (paramCrossX == 0.0) + { + paramCrossX += paramDeltaX; + // If going backwards, we should ignore the position we would get by the above flooring, + // because the ray is not heading in that direction + if (xiStep == -1) + rs.x -= 1; + } + + if (paramCrossZ == 0.0) + { + paramCrossZ += paramDeltaZ; + if (ziStep == -1) + rs.z -= 1; + } + + rs.prev_x = rs.x; + rs.prev_z = rs.z; + rs.param = 0; + + while (true) + { + rs.prev_x = rs.x; + rs.prev_z = rs.z; + rs.prevParam = rs.param; + + if (paramCrossX < paramCrossZ) + { + // X lane + rs.x += xiStep; + // Assign before advancing the param, + // to be in sync with the initialization step + rs.param = paramCrossX; + paramCrossX += paramDeltaX; + } + else + { + // Z lane + rs.z += ziStep; + rs.param = paramCrossZ; + paramCrossZ += paramDeltaZ; + } + + if (rs.param > rs.maxDistanceFlat) + { + rs.param = rs.maxDistanceFlat; + quadAction(rs); + break; + } + else + quadAction(rs); + } +} + + +struct ProcessTrianglesAction +{ + const btHeightfieldTerrainShape *shape; + bool flipQuadEdges; + bool useDiamondSubdivision; + int width; + int length; + btTriangleCallback* callback; + + void exec(int x, int z) const + { + if(x < 0 || z < 0 || x >= width || z >= length) + return; + + btVector3 vertices[3]; + + // Check quad + if (flipQuadEdges || (useDiamondSubdivision && (((z + x) & 1) > 0))) + { + // First triangle + shape->getVertex(x, z, vertices[0]); + shape->getVertex(x + 1, z, vertices[1]); + shape->getVertex(x + 1, z + 1, vertices[2]); + callback->processTriangle(vertices, x, z); + + // Second triangle + shape->getVertex(x, z, vertices[0]); + shape->getVertex(x + 1, z + 1, vertices[1]); + shape->getVertex(x, z + 1, vertices[2]); + callback->processTriangle(vertices, x, z); + } + else + { + // First triangle + shape->getVertex(x, z, vertices[0]); + shape->getVertex(x, z + 1, vertices[1]); + shape->getVertex(x + 1, z, vertices[2]); + callback->processTriangle(vertices, x, z); + + // Second triangle + shape->getVertex(x + 1, z, vertices[0]); + shape->getVertex(x, z + 1, vertices[1]); + shape->getVertex(x + 1, z + 1, vertices[2]); + callback->processTriangle(vertices, x, z); + } + } + + void operator ()(const GridRaycastState &bs) const + { + exec(bs.prev_x, bs.prev_z); + } +}; + + +struct ProcessVBoundsAction +{ + const btHeightfieldTerrainShape::Range *vbounds; + int width; + int length; + int chunkSize; + + btVector3 rayBegin; + btVector3 rayEnd; + btVector3 rayDir; + + ProcessTrianglesAction processTriangles; + + void operator ()(const GridRaycastState &rs) const + { + int x = rs.prev_x; + int z = rs.prev_z; + + if(x < 0 || z < 0 || x >= width || z >= length) + return; + + const btHeightfieldTerrainShape::Range chunk = vbounds[x + z * width]; + + btVector3 enterPos; + btVector3 exitPos; + + if (rs.maxDistanceFlat > 0.0001) + { + btScalar flatTo3d = chunkSize * rs.maxDistance3d / rs.maxDistanceFlat; + btScalar enterParam3d = rs.prevParam * flatTo3d; + btScalar exitParam3d = rs.param * flatTo3d; + enterPos = rayBegin + rayDir * enterParam3d; + exitPos = rayBegin + rayDir * exitParam3d; + + // We did enter the flat projection of the AABB, + // but we have to check if we intersect it on the vertical axis + if (enterPos[1] > chunk.max && exitPos[1] > chunk.max) + return; + if (enterPos[1] < chunk.min && exitPos[1] < chunk.min) + return; + } + else + { + // Consider the ray vertical + // (though we shouldn't reach this often because there is an early check up-front) + enterPos = rayBegin; + exitPos = rayEnd; + } + + gridRaycast(processTriangles, enterPos, exitPos); + // Note: it could be possible to have more than one grid at different levels, + // to do this there would be a branch using a pointer to another ProcessVBoundsAction + } +}; + + +// TODO How do I interrupt the ray when there is a hit? `callback` does not return any result +/// Performs a raycast using a hierarchical Bresenham algorithm. +/// Does not allocate any memory by itself. +void btHeightfieldTerrainShape::performRaycast(btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget) const +{ + // Transform to cell-local + btVector3 beginPos = raySource / m_localScaling; + btVector3 endPos = rayTarget / m_localScaling; + beginPos += m_localOrigin; + endPos += m_localOrigin; + + ProcessTrianglesAction processTriangles; + processTriangles.shape = this; + processTriangles.flipQuadEdges = m_flipQuadEdges; + processTriangles.useDiamondSubdivision = m_useDiamondSubdivision; + processTriangles.callback = callback; + processTriangles.width = m_heightStickWidth - 1; + processTriangles.length = m_heightStickLength - 1; + + // TODO Transform vectors to account for m_upAxis + int iBeginX = static_cast(floor(beginPos[0])); + int iBeginZ = static_cast(floor(beginPos[2])); + int iEndX = static_cast(floor(endPos[0])); + int iEndZ = static_cast(floor(endPos[2])); + + if (iBeginX == iEndX && iBeginZ == iEndZ) + { + // The ray will never cross quads within the plane, + // so directly process triangles within one quad + // (typically, vertical rays should end up here) + processTriangles.exec(iBeginX, iEndZ); + return; + } + + if (m_vboundsGrid == NULL) + { + // Process all quads intersecting the flat projection of the ray + gridRaycast(processTriangles, beginPos, endPos); + } + else + { + btVector3 rayDiff = endPos - beginPos; + btScalar flatDistance2 = rayDiff[0] * rayDiff[0] + rayDiff[2] * rayDiff[2]; + if (flatDistance2 < m_vboundsChunkSize * m_vboundsChunkSize) + { + // Don't use chunks, the ray is too short in the plane + gridRaycast(processTriangles, beginPos, endPos); + } + + ProcessVBoundsAction processVBounds; + processVBounds.width = m_vboundsGridWidth; + processVBounds.length = m_vboundsGridLength; + processVBounds.vbounds = m_vboundsGrid; + processVBounds.rayBegin = beginPos; + processVBounds.rayEnd = endPos; + processVBounds.rayDir = rayDiff.normalized(); + processVBounds.processTriangles = processTriangles; + processVBounds.chunkSize = m_vboundsChunkSize; + // The ray is long, run raycast on a higher-level grid + gridRaycast(processVBounds, beginPos / m_vboundsChunkSize, endPos / m_vboundsChunkSize); + } +} + + +/// Builds a grid data structure storing the min and max heights of the terrain in chunks. +/// if chunkSize is zero, that accelerator is removed. +/// If you modify the heights, you need to rebuild this accelerator. +void btHeightfieldTerrainShape::buildAccelerator(int chunkSize) +{ + if (chunkSize <= 0) + { + clearAccelerator(); + return; + } + + m_vboundsChunkSize = chunkSize; + int nChunksX = m_heightStickWidth / chunkSize; + int nChunksZ = m_heightStickLength / chunkSize; + + if (m_heightStickWidth % chunkSize > 0) + ++nChunksX; // In case terrain size isn't dividable by chunk size + if (m_heightStickLength % chunkSize > 0) + ++nChunksZ; + + if(m_vboundsGridWidth != nChunksX || m_vboundsGridLength != nChunksZ) + { + clearAccelerator(); + m_vboundsGridWidth = nChunksX; + m_vboundsGridLength = nChunksZ; + } + + if (nChunksX == 0 || nChunksZ == 0) + return; + + // TODO What is the recommended way to allocate this? + // This data structure is only reallocated if the required size changed + if (m_vboundsGrid == NULL) + m_vboundsGrid = new Range[nChunksX * nChunksZ]; + + // Compute min and max height for all chunks + for (int cz = 0; cz < nChunksZ; ++cz) + { + int z0 = cz * chunkSize; + + for (int cx = 0; cx < nChunksX; ++cx) + { + int x0 = cx * chunkSize; + + Range r; + + r.min = getRawHeightFieldValue(x0, z0); + r.max = r.min; + + // Compute min and max height for this chunk. + // We have to include one extra cell to account for neighbors. + // Here is why: + // Say we have a flat terrain, and a plateau that fits a chunk perfectly. + // + // Left Right + // 0---0---0---1---1---1 + // | | | | | | + // 0---0---0---1---1---1 + // | | | | | | + // 0---0---0---1---1---1 + // x + // + // If the AABB for the Left chunk did not share vertices with the Right, + // then we would fail collision tests at x due to a gap. + // + for (int z = z0; z < z0 + chunkSize + 1; ++z) + { + if (z >= m_heightStickLength) + continue; + + for (int x = x0; x < x0 + chunkSize + 1; ++x) + { + if (x >= m_heightStickWidth) + continue; + + btScalar height = getRawHeightFieldValue(x, z); + + if (height < r.min) + r.min = height; + else if (height > r.max) + r.max = height; + } + } + + m_vboundsGrid[cx + cz * nChunksX] = r; + } + } +} + + +void btHeightfieldTerrainShape::clearAccelerator() +{ + if (m_vboundsGrid) + { + // TODO What is the recommended way to deallocate this? + delete[] m_vboundsGrid; + m_vboundsGrid = 0; + } +} + + diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h index 8a50a57e31..e23b548cb2 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h @@ -18,6 +18,7 @@ subject to the following restrictions: #include "btConcaveShape.h" + ///btHeightfieldTerrainShape simulates a 2D heightfield terrain /** The caller is responsible for maintaining the heightfield array; this @@ -71,6 +72,12 @@ subject to the following restrictions: ATTRIBUTE_ALIGNED16(class) btHeightfieldTerrainShape : public btConcaveShape { +public: + struct Range { + btScalar min; + btScalar max; + }; + protected: btVector3 m_localAabbMin; btVector3 m_localAabbMax; @@ -100,9 +107,14 @@ protected: btVector3 m_localScaling; + // Accelerator + Range *m_vboundsGrid; + int m_vboundsGridWidth; + int m_vboundsGridLength; + int m_vboundsChunkSize; + virtual btScalar getRawHeightFieldValue(int x, int y) const; void quantizeWithClamp(int* out, const btVector3& point, int isMax) const; - void getVertex(int x, int y, btVector3& vertex) const; /// protected initialization /** @@ -154,6 +166,13 @@ public: virtual void setLocalScaling(const btVector3& scaling); virtual const btVector3& getLocalScaling() const; + + void getVertex(int x,int y,btVector3& vertex) const; + + void performRaycast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget) const; + + void buildAccelerator(int chunkSize=16); + void clearAccelerator(); //debugging virtual const char* getName() const { return "HEIGHTFIELD"; }