Merge pull request #28928 from godotengine/revert-28328-revert-25543-optimize_bullet_heightfield_raycast2

Revert "Revert "Implemented terrain raycast acceleration""
This commit is contained in:
Rémi Verschelde 2019-05-16 10:38:31 +02:00 committed by GitHub
commit 4a25065089
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 463 additions and 5 deletions

View File

@ -148,7 +148,13 @@ btHeightfieldTerrainShape *ShapeBullet::create_shape_height_field(PoolVector<rea
const bool flipQuadEdges = false;
const void *heightsPtr = p_heights.read().ptr();
return bulletnew(btHeightfieldTerrainShape(p_width, p_depth, heightsPtr, ignoredHeightScale, p_min_height, p_max_height, YAxis, PHY_FLOAT, flipQuadEdges));
btHeightfieldTerrainShape *heightfield = bulletnew(btHeightfieldTerrainShape(p_width, p_depth, heightsPtr, ignoredHeightScale, p_min_height, p_max_height, YAxis, PHY_FLOAT, flipQuadEdges));
// The shape can be created without params when you do PhysicsServer.shape_create(PhysicsServer.SHAPE_HEIGHTMAP)
if (heightsPtr)
heightfield->buildAccelerator(16);
return heightfield;
}
btRayShape *ShapeBullet::create_shape_ray(real_t p_length, bool p_slips_on_slope) {

View File

@ -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

View File

@ -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 <typename Action_T>
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<int>(floor(beginPos[0]));
rs.z = static_cast<int>(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<int>(floor(beginPos[0]));
int iBeginZ = static_cast<int>(floor(beginPos[2]));
int iEndX = static_cast<int>(floor(endPos[0]));
int iEndZ = static_cast<int>(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;
}
}

View File

@ -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"; }