From e96505d4c22780e4647490b873d94d0843d37b92 Mon Sep 17 00:00:00 2001 From: Marc Gilleron Date: Wed, 16 Jan 2019 20:57:21 +0000 Subject: [PATCH 1/6] Accelerated terrain raycast using "Bresenham" traversal and optional chunks --- .../CollisionDispatch/btCollisionWorld.cpp | 51 +- .../btHeightfieldTerrainShape.cpp | 457 ++++++++++++++++++ .../btHeightfieldTerrainShape.h | 21 +- 3 files changed, 490 insertions(+), 39 deletions(-) diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 782e9efaf..8b5a4c307 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -22,6 +22,7 @@ subject to the following restrictions: #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 @@ -423,44 +436,6 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans, co btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); - //ConvexCast::CastResult - - struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback - { - btCollisionWorld::RayResultCallback* m_resultCallback; - const btCollisionObject* m_collisionObject; - btConcaveShape* m_triangleMesh; - - btTransform m_colObjWorldTransform; - - BridgeTriangleRaycastCallback(const btVector3& from, const btVector3& to, - btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject, btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform) : //@BP Mod - btTriangleRaycastCallback(from, to, resultCallback->m_flags), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh), - m_colObjWorldTransform(colObjWorldTransform) - { - } - - virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex) - { - btCollisionWorld::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = partId; - shapeInfo.m_triangleIndex = triangleIndex; - - btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal; - - btCollisionWorld::LocalRayResult rayResult(m_collisionObject, - &shapeInfo, - hitNormalWorld, - hitFraction); - - bool normalInWorldSpace = true; - return m_resultCallback->addSingleResult(rayResult, normalInWorldSpace); - } - }; - BridgeTriangleRaycastCallback rcb(rayFromLocal, rayToLocal, &resultCallback, collisionObjectWrap->getCollisionObject(), concaveShape, colObjWorldTransform); rcb.m_hitFraction = resultCallback.m_closestHitFraction; diff --git a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index c85ce2498..66bf44fc9 100644 --- a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/src/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,453 @@ const btVector3& btHeightfieldTerrainShape::getLocalScaling() const { return m_localScaling; } + +namespace +{ + 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]; + + // TODO Since this is for raycasts, we could greatly benefit from an early exit on the first hit + + // 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/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h index 8a50a57e3..1f3c3d868 100644 --- a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h +++ b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h @@ -71,6 +71,13 @@ 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 /** @@ -155,6 +167,13 @@ public: 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"; } }; From 8f9ff5e6744412a359bf79ad98ab141bfb0d7e79 Mon Sep 17 00:00:00 2001 From: YungKC Date: Fri, 12 Apr 2019 17:42:26 -0700 Subject: [PATCH 2/6] Fix render issue in server render mode (e.g., docker) px is an 1D array, and cause a runtime error when trying to strip the color channels into RGB. I fixed this by first reshape the px array into am image compatible format first. --- examples/pybullet/gym/pybullet_envs/env_bases.py | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py index 861363348..aeef7fda5 100644 --- a/examples/pybullet/gym/pybullet_envs/env_bases.py +++ b/examples/pybullet/gym/pybullet_envs/env_bases.py @@ -96,6 +96,7 @@ class MJCFBaseBulletEnv(gym.Env): renderer=pybullet.ER_BULLET_HARDWARE_OPENGL ) rgb_array = np.array(px) + rgb_array = np.reshape(np.array(px), (self._render_height, self._render_width, -1)) rgb_array = rgb_array[:, :, :3] return rgb_array From 219970c09eeae823f7dc8a7488e58b33917db9b5 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sun, 14 Apr 2019 14:08:35 -0700 Subject: [PATCH 3/6] fix getCameraImage in VR server using renderer=pybullet.ER_BULLET_HARDWARE_OPENGL --- examples/StandaloneMain/hellovr_opengl_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index c786c737a..0f60543ba 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/examples/StandaloneMain/hellovr_opengl_main.cpp @@ -1792,6 +1792,7 @@ void CMainApplication::RenderStereoTargets() //m_app->drawGrid(gridUp); + m_app->m_instancingRenderer->setRenderFrameBuffer(0); glBindFramebuffer(GL_FRAMEBUFFER, 0); glDisable(GL_MULTISAMPLE); From e97a7d77af352225c86f5ae7fd6f106c4f295e42 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sun, 14 Apr 2019 18:20:20 -0700 Subject: [PATCH 4/6] only report solver analytics if enabled using setPhysicsEngineParameter(reportSolverAnalytics=1) --- examples/SharedMemory/PhysicsClientC_API.cpp | 11 ++++ examples/SharedMemory/PhysicsClientC_API.h | 3 +- .../PhysicsServerCommandProcessor.cpp | 7 ++- examples/SharedMemory/SharedMemoryCommands.h | 51 ++++++++++--------- examples/SharedMemory/SharedMemoryPublic.h | 3 +- examples/pybullet/pybullet.c | 13 +++-- .../ConstraintSolver/btContactSolverInfo.h | 4 +- .../Featherstone/btMultiBodyDynamicsWorld.cpp | 4 +- 8 files changed, 61 insertions(+), 35 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 93b84bf60..920a463b7 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -689,6 +689,17 @@ B3_SHARED_API int b3PhysicsParameterSetMinimumSolverIslandSize(b3SharedMemoryCom return 0; } +B3_SHARED_API int b3PhysicsParamSetSolverAnalytics(b3SharedMemoryCommandHandle commandHandle, int reportSolverAnalytics) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + command->m_physSimParamArgs.m_reportSolverAnalytics = reportSolverAnalytics; + command->m_updateFlags |= SIM_PARAM_REPORT_CONSTRAINT_SOLVER_ANALYTICS; + return 0; +} + + + B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 680e48ad4..290c8b36b 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -349,7 +349,8 @@ extern "C" B3_SHARED_API int b3PhysicsParameterSetEnableSAT(b3SharedMemoryCommandHandle commandHandle, int enableSAT); B3_SHARED_API int b3PhysicsParameterSetConstraintSolverType(b3SharedMemoryCommandHandle commandHandle, int constraintSolverType); B3_SHARED_API int b3PhysicsParameterSetMinimumSolverIslandSize(b3SharedMemoryCommandHandle commandHandle, int minimumSolverIslandSize); - + B3_SHARED_API int b3PhysicsParamSetSolverAnalytics(b3SharedMemoryCommandHandle commandHandle, int reportSolverAnalytics); + B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestPhysicsParamCommand(b3PhysicsClientHandle physClient); B3_SHARED_API int b3GetStatusPhysicsSimulationParameters(b3SharedMemoryStatusHandle statusHandle, struct b3PhysicsSimulationParameters* params); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 7beb0982d..b0c6697ec 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -7605,7 +7605,7 @@ bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct S } btScalar deltaTimeScaled = m_data->m_physicsDeltaTime * simTimeScalingFactor; - m_data->m_dynamicsWorld->getSolverInfo().m_reportSolverAnalytics = true; + int numSteps = 0; if (m_data->m_numSimulationSubSteps > 0) { @@ -8426,6 +8426,11 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st m_data->m_pluginManager.getFileIOInterface()->enableFileCaching(clientCmd.m_physSimParamArgs.m_enableFileCaching!=0); } + if (clientCmd.m_updateFlags & SIM_PARAM_REPORT_CONSTRAINT_SOLVER_ANALYTICS) + { + m_data->m_dynamicsWorld->getSolverInfo().m_reportSolverAnalytics = clientCmd.m_physSimParamArgs.m_reportSolverAnalytics; + } + SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; return hasStatus; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 443c4672e..1d70a9df0 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -456,31 +456,32 @@ enum EnumSimDesiredStateUpdateFlags enum EnumSimParamUpdateFlags { SIM_PARAM_UPDATE_DELTA_TIME = 1, - SIM_PARAM_UPDATE_GRAVITY = 2, - SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS = 4, - SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS = 8, - SIM_PARAM_UPDATE_REAL_TIME_SIMULATION = 16, - SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP = 32, - SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS = 64, - SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE = 128, - SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD = 256, - SIM_PARAM_UPDATE_COLLISION_FILTER_MODE = 512, - SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD = 1024, - SIM_PARAM_ENABLE_CONE_FRICTION = 2048, - SIM_PARAM_ENABLE_FILE_CACHING = 4096, - SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD = 8192, - SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP = 16384, - SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP = 32768, - SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS = 65536, - SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION = 131072, - SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE = 262144, - SIM_PARAM_UPDATE_DEFAULT_GLOBAL_CFM = 524288, - SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM = 1048576, - SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD = 2097152, - SIM_PARAM_UPDATE_CONTACT_SLOP = 4194304, - SIM_PARAM_ENABLE_SAT = 8388608, - SIM_PARAM_CONSTRAINT_SOLVER_TYPE = 16777216, - SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE = 33554432, + SIM_PARAM_UPDATE_GRAVITY = 1<<1, + SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS = 1<<2, + SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS = 1<<3, + SIM_PARAM_UPDATE_REAL_TIME_SIMULATION = 1<<4, + SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP = 1<<5, + SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS = 1<<6, + SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE = 1<<7, + SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD = 1<<8, + SIM_PARAM_UPDATE_COLLISION_FILTER_MODE = 1 << 9, + SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD = 1 << 10, + SIM_PARAM_ENABLE_CONE_FRICTION = 1 << 11, + SIM_PARAM_ENABLE_FILE_CACHING = 1 << 12, + SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD = 1 << 13, + SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP = 1 << 14, + SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP = 1 << 15, + SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS = 1 << 16, + SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION = 1 << 17, + SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE = 1 << 18, + SIM_PARAM_UPDATE_DEFAULT_GLOBAL_CFM = 1 << 19, + SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM = 1 << 20, + SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD = 1 << 21, + SIM_PARAM_UPDATE_CONTACT_SLOP = 1 << 22, + SIM_PARAM_ENABLE_SAT = 1 << 23, + SIM_PARAM_CONSTRAINT_SOLVER_TYPE = 1 << 24, + SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE = 1 << 25, + SIM_PARAM_REPORT_CONSTRAINT_SOLVER_ANALYTICS = 1 << 26, }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index d0ede5768..b2c2f6c68 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -903,7 +903,7 @@ struct b3PluginArguments struct b3PhysicsSimulationParameters { double m_deltaTime; - double m_simulationTimestamp; // Output only timestamp of simulation. + double m_simulationTimestamp; // user logging timestamp of simulation. double m_gravityAcceleration[3]; int m_numSimulationSubSteps; int m_numSolverIterations; @@ -929,6 +929,7 @@ struct b3PhysicsSimulationParameters int m_enableSAT; int m_constraintSolverType; int m_minimumSolverIslandSize; + int m_reportSolverAnalytics; }; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 9f59c79d8..b570e69bc 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1567,8 +1567,9 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar double globalCFM = -1; int minimumSolverIslandSize = -1; - + int reportSolverAnalytics = -1; int physicsClientId = 0; + static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", @@ -1592,10 +1593,12 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar "constraintSolverType", "globalCFM", "minimumSolverIslandSize", + "reportSolverAnalytics", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiidii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, - &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &constraintSolverType, &globalCFM, &minimumSolverIslandSize, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiidiii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, + &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &constraintSolverType, &globalCFM, &minimumSolverIslandSize, + &reportSolverAnalytics, &physicsClientId)) { return NULL; } @@ -1712,6 +1715,10 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar { b3PhysicsParamSetDefaultGlobalCFM(command, globalCFM); } + if (reportSolverAnalytics >= 0) + { + b3PhysicsParamSetSolverAnalytics(command, reportSolverAnalytics); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 0b6ac54c8..63d7c98e1 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -64,7 +64,7 @@ struct btContactSolverInfoData btScalar m_restitutionVelocityThreshold; bool m_jointFeedbackInWorldSpace; bool m_jointFeedbackInJointFrame; - bool m_reportSolverAnalytics; + int m_reportSolverAnalytics; }; struct btContactSolverInfo : public btContactSolverInfoData @@ -99,7 +99,7 @@ struct btContactSolverInfo : public btContactSolverInfoData m_restitutionVelocityThreshold = 0.2f; //if the relative velocity is below this threshold, there is zero restitution m_jointFeedbackInWorldSpace = false; m_jointFeedbackInJointFrame = false; - m_reportSolverAnalytics = false; + m_reportSolverAnalytics = 0; } }; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 28b569935..3bb056fbf 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -274,7 +274,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: { ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id m_solver->solveMultiBodyGroup(bodies, numBodies, manifolds, numManifolds, m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher); - if (m_solverInfo->m_reportSolverAnalytics) + if (m_solverInfo->m_reportSolverAnalytics&1) { m_solver->m_analyticsData.m_islandId = islandId; m_islandAnalyticsData.push_back(m_solver->m_analyticsData); @@ -363,7 +363,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size()); m_solver->solveMultiBodyGroup(bodies, m_bodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher); - if (m_bodies.size() && m_solverInfo->m_reportSolverAnalytics) + if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1)) { m_solver->m_analyticsData.m_islandId = islandId; m_islandAnalyticsData.push_back(m_solver->m_analyticsData); From f28fd91e4f1e6f10da6c03cd8b9567a3b5b4665f Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Mon, 15 Apr 2019 21:55:29 -0700 Subject: [PATCH 5/6] add raycast accelerator for btHeightfieldTerrainShape, thanks to Marc Zylann, see https://github.com/bulletphysics/bullet3/pull/2062 it can be disabled by setting the flag cb.m_flags |= btTriangleRaycastCallback::kF_DisableHeightfieldAccelerator; acceleration is disabled for z axis up. add btHeightfieldTerrainShape example to example browser --- examples/Benchmarks/BenchmarkDemo.cpp | 29 - .../CommonInterfaces/CommonRigidBodyBase.h | 11 +- examples/ExampleBrowser/CMakeLists.txt | 2 + examples/ExampleBrowser/ExampleEntries.cpp | 6 +- examples/ExampleBrowser/OpenGLGuiHelper.cpp | 58 +- examples/ExampleBrowser/premake4.lua | 1 + examples/Heightfield/HeightfieldExample.cpp | 1116 +++++++++++++++++ examples/Heightfield/HeightfieldExample.h | 21 + .../CollisionDispatch/btCollisionWorld.cpp | 16 + .../btHeightfieldTerrainShape.cpp | 451 +++++++ .../btHeightfieldTerrainShape.h | 28 +- .../NarrowPhaseCollision/btRaycastCallback.h | 1 + 12 files changed, 1702 insertions(+), 38 deletions(-) create mode 100644 examples/Heightfield/HeightfieldExample.cpp create mode 100644 examples/Heightfield/HeightfieldExample.h diff --git a/examples/Benchmarks/BenchmarkDemo.cpp b/examples/Benchmarks/BenchmarkDemo.cpp index 110a35eab..995275916 100644 --- a/examples/Benchmarks/BenchmarkDemo.cpp +++ b/examples/Benchmarks/BenchmarkDemo.cpp @@ -313,35 +313,6 @@ public: m_guiHelper->getRenderInterface()->drawLines(&points[0].m_floats[0], lineColor, points.size(), sizeof(btVector3FloatData), &indices[0], indices.size(), 1); } -#if 0 - glDisable (GL_LIGHTING); - glColor3f (0.0, 1.0, 0.0); - glBegin (GL_LINES); - int i; - - for (i = 0; i < NUMRAYS; i++) - { - glVertex3f (source[i][0], source[i][1], source[i][2]); - glVertex3f (hit[i][0], hit[i][1], hit[i][2]); - } - glEnd (); - glColor3f (1.0, 1.0, 1.0); - glBegin (GL_LINES); - for (i = 0; i < NUMRAYS; i++) - { - glVertex3f (hit[i][0], hit[i][1], hit[i][2]); - glVertex3f (hit[i][0] + normal[i][0], hit[i][1] + normal[i][1], hit[i][2] + normal[i][2]); - } - glEnd (); - glColor3f (0.0, 1.0, 1.0); - glBegin (GL_POINTS); - for ( i = 0; i < NUMRAYS; i++) - { - glVertex3f (hit[i][0], hit[i][1], hit[i][2]); - } - glEnd (); - glEnable (GL_LIGHTING); -#endif //USE_GRAPHICAL_BENCHMARK } }; diff --git a/examples/CommonInterfaces/CommonRigidBodyBase.h b/examples/CommonInterfaces/CommonRigidBodyBase.h index 09f8b57d3..fea34aec0 100644 --- a/examples/CommonInterfaces/CommonRigidBodyBase.h +++ b/examples/CommonInterfaces/CommonRigidBodyBase.h @@ -439,12 +439,15 @@ struct CommonRigidBodyBase : public CommonExampleInterface virtual void renderScene() { + if (m_dynamicsWorld) { - m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld); - } + { + m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld); + } - { - m_guiHelper->render(m_dynamicsWorld); + { + m_guiHelper->render(m_dynamicsWorld); + } } } }; diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index ceb6d9590..89531eb77 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -207,6 +207,8 @@ SET(BulletExampleBrowser_SRCS ../MultiThreadedDemo/MultiThreadedDemo.h ../MultiThreadedDemo/CommonRigidBodyMTBase.cpp ../MultiThreadedDemo/CommonRigidBodyMTBase.h + ../Heightfield/HeightfieldExample.cpp + ../Heightfield/HeightfieldExample.h ../BlockSolver/btBlockSolver.cpp ../BlockSolver/btBlockSolver.h ../BlockSolver/BlockSolverExample.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index f2e3901b7..c21fa8618 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -5,6 +5,7 @@ #include "../BlockSolver/RigidBodyBoxes.h" #include "LinearMath/btAlignedObjectArray.h" #include "EmptyExample.h" +#include "../Heightfield/HeightfieldExample.h" #include "../RenderingExamples/RenderInstancingDemo.h" #include "../RenderingExamples/CoordinateSystemDemo.h" #include "../RenderingExamples/RaytracerSetup.h" @@ -157,8 +158,8 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(1, "Stack MultiBody MLCP PGS", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_MLCP_PGS), ExampleEntry(1, "Stack MultiBody MLCP Dantzig", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_MLCP_DANTZIG), ExampleEntry(1, "Stack MultiBody Block", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_BLOCK), - ExampleEntry(1, "Stack RigidBody SI", "Create a stack of blocks, with heavy block at the top", RigidBodyBoxesCreateFunc, BLOCK_SOLVER_SI), - ExampleEntry(1, "Stack RigidBody Block", "Create a stack of blocks, with heavy block at the top", RigidBodyBoxesCreateFunc, BLOCK_SOLVER_BLOCK), + //ExampleEntry(1, "Stack RigidBody SI", "Create a stack of blocks, with heavy block at the top", RigidBodyBoxesCreateFunc, BLOCK_SOLVER_SI), + //ExampleEntry(1, "Stack RigidBody Block", "Create a stack of blocks, with heavy block at the top", RigidBodyBoxesCreateFunc, BLOCK_SOLVER_BLOCK), ExampleEntry(0, "Inverse Dynamics"), ExampleEntry(1, "Inverse Dynamics URDF", "Create a btMultiBody from URDF. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc, BT_ID_LOAD_URDF), @@ -234,6 +235,7 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(1, "Convex vs Mesh", "Benchmark the performance and stability of rigid bodies using convex hull collision shapes (btConvexHullShape), resting on a triangle mesh, btBvhTriangleMeshShape.", BenchmarkCreateFunc, 6), ExampleEntry(1, "Raycast", "Benchmark the performance of the btCollisionWorld::rayTest. Note that currently the rays are not rendered.", BenchmarkCreateFunc, 7), ExampleEntry(1, "Convex Pack", "Benchmark the performance of the convex hull primitive.", BenchmarkCreateFunc, 8), + ExampleEntry(1, "Heightfield", "Raycast against a btHeightfieldTerrainShape", HeightfieldExampleCreateFunc), //#endif ExampleEntry(0, "Importers"), diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index bb8671d48..93369d8a0 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -1,7 +1,7 @@ #include "OpenGLGuiHelper.h" #include "btBulletDynamicsCommon.h" - +#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h" #include "../CommonInterfaces/CommonGraphicsAppInterface.h" #include "../CommonInterfaces/CommonRenderInterface.h" #include "Bullet3Common/b3Scalar.h" @@ -255,6 +255,38 @@ void OpenGLGuiHelper::createRigidBodyGraphicsObject(btRigidBody* body, const btV createCollisionObjectGraphicsObject(body, color); } + +class MyTriangleCollector2 : public btTriangleCallback +{ +public: + btAlignedObjectArray* m_pVerticesOut; + btAlignedObjectArray* m_pIndicesOut; + + MyTriangleCollector2() + { + m_pVerticesOut = 0; + m_pIndicesOut = 0; + } + + virtual void processTriangle(btVector3* tris, int partId, int triangleIndex) + { + for (int k = 0; k < 3; k++) + { + GLInstanceVertex v; + v.xyzw[3] = 0; + v.uv[0] = v.uv[1] = 0.5f; + btVector3 normal = (tris[0] - tris[1]).cross(tris[0] - tris[2]); + normal.safeNormalize(); + for (int l = 0; l < 3; l++) + { + v.xyzw[l] = tris[k][l]; + v.normal[l] = normal[l]; + } + m_pIndicesOut->push_back(m_pVerticesOut->size()); + m_pVerticesOut->push_back(v); + } + } +}; void OpenGLGuiHelper::createCollisionObjectGraphicsObject(btCollisionObject* body, const btVector3& color) { if (body->getUserIndex() < 0) @@ -409,6 +441,30 @@ void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* colli //if (collisionShape->getShapeType()==BOX_SHAPE_PROXYTYPE) { } + + + if (collisionShape->getShapeType() == TERRAIN_SHAPE_PROXYTYPE) + { + const btHeightfieldTerrainShape* heightField = static_cast(collisionShape); + MyTriangleCollector2 col; + col.m_pVerticesOut = &gfxVertices; + col.m_pIndicesOut = &indices; + btVector3 aabbMin, aabbMax; + for (int k = 0; k < 3; k++) + { + aabbMin[k] = -BT_LARGE_FLOAT; + aabbMax[k] = BT_LARGE_FLOAT; + } + heightField->processAllTriangles(&col, aabbMin, aabbMax); + if (gfxVertices.size() && indices.size()) + { + int shapeId = m_data->m_glApp->m_renderer->registerShape(&gfxVertices[0].xyzw[0], gfxVertices.size(), &indices[0], indices.size()); + collisionShape->setUserIndex(shapeId); + } + return; + } + + if (collisionShape->getShapeType() == SOFTBODY_SHAPE_PROXYTYPE) { computeSoftBodyVertices(collisionShape, gfxVertices, indices); diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 067eb2c64..d23c782e5 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -169,6 +169,7 @@ project "App_BulletExampleBrowser" "../Collision/Internal/*", "../Benchmarks/*", "../MultiThreadedDemo/*", + "../Heightfield/HeightfieldExample.*", "../CommonInterfaces/*.h", "../ForkLift/ForkLiftDemo.*", "../Importers/**", diff --git a/examples/Heightfield/HeightfieldExample.cpp b/examples/Heightfield/HeightfieldExample.cpp new file mode 100644 index 000000000..1f65d17fd --- /dev/null +++ b/examples/Heightfield/HeightfieldExample.cpp @@ -0,0 +1,1116 @@ + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006,2008 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "HeightfieldExample.h" // always include our own header first! + +#include "btBulletDynamicsCommon.h" +#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h" +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../MultiThreadedDemo/CommonRigidBodyMTBase.h" +#include "../CommonInterfaces/CommonParameterInterface.h" +#include "../OpenGLWindow/GLInstanceGraphicsShape.h" + +// constants ------------------------------------------------------------------- +static const btScalar s_gravity = 9.8; // 9.8 m/s^2 + +static const int s_gridSize = 128 + 1; // must be (2^N) + 1 +static const btScalar s_gridSpacing = 0.5; + +static const btScalar s_gridHeightScale = 0.02; + +// the singularity at the center of the radial model means we need a lot of +// finely-spaced time steps to get the physics right. +// These numbers are probably too aggressive for a real game! +static const int s_requestedHz = 180; +static const float s_engineTimeStep = 1.0 / s_requestedHz; + +// delta phase: radians per second +static const btScalar s_deltaPhase = 0.25 * 2.0 * SIMD_PI; + +// what type of terrain is generated? +enum eTerrainModel { + eRadial = 0, // deterministic + eFractal = 1 // random +}; + + +typedef unsigned char byte_t; + + + +//////////////////////////////////////////////////////////////////////////////// +// +// static helper methods +// +// Only used within this file (helpers and terrain generation, etc) +// +//////////////////////////////////////////////////////////////////////////////// + +static const char * +getTerrainTypeName +( + eTerrainModel model +) +{ + switch (model) { + case eRadial: + return "Radial"; + + case eFractal: + return "Fractal"; + + default: + btAssert(!"bad terrain model type"); + } + + return NULL; +} + + + +static const char * +getDataTypeName +( + PHY_ScalarType type +) +{ + switch (type) { + case PHY_UCHAR: + return "UnsignedChar"; + + case PHY_SHORT: + return "Short"; + + case PHY_FLOAT: + return "Float"; + + default: + btAssert(!"bad heightfield data type"); + } + + return NULL; +} + + + +static const char * +getUpAxisName +( + int axis +) +{ + switch (axis) { + case 0: + return "X"; + + case 1: + return "Y"; + + case 2: + return "Z"; + + default: + btAssert(!"bad up axis"); + } + + return NULL; +} + + + +static btVector3 +getUpVector +( + int upAxis, + btScalar regularValue, + btScalar upValue +) +{ + btAssert(upAxis >= 0 && upAxis <= 2 && "bad up axis"); + + btVector3 v(regularValue, regularValue, regularValue); + v[upAxis] = upValue; + + return v; +} + + + +// TODO: it would probably cleaner to have a struct per data type, so +// you could lookup byte sizes, conversion functions, etc. +static int getByteSize +( + PHY_ScalarType type +) +{ + int size = 0; + + switch (type) { + case PHY_FLOAT: + size = sizeof(btScalar); + break; + + case PHY_UCHAR: + size = sizeof(unsigned char); + break; + + case PHY_SHORT: + size = sizeof(short); + break; + + default: + btAssert(!"Bad heightfield data type"); + } + + return size; +} + + + +static btScalar +convertToFloat +( + const byte_t * p, + PHY_ScalarType type +) +{ + btAssert(p); + + switch (type) { + case PHY_FLOAT: + { + btScalar * pf = (btScalar *)p; + return *pf; + } + + case PHY_UCHAR: + { + unsigned char * pu = (unsigned char *)p; + return ((*pu) * s_gridHeightScale); + } + + case PHY_SHORT: + { + short * ps = (short *)p; + return ((*ps) * s_gridHeightScale); + } + + default: + btAssert(!"bad type"); + } + + return 0; +} + + + +static btScalar +getGridHeight +( + byte_t * grid, + int i, + int j, + PHY_ScalarType type +) +{ + btAssert(grid); + btAssert(i >= 0 && i < s_gridSize); + btAssert(j >= 0 && j < s_gridSize); + + int bpe = getByteSize(type); + btAssert(bpe > 0 && "bad bytes per element"); + + int idx = (j * s_gridSize) + i; + long offset = ((long)bpe) * idx; + + byte_t * p = grid + offset; + + return convertToFloat(p, type); +} + + + +static void +convertFromFloat +( + byte_t * p, + btScalar value, + PHY_ScalarType type +) +{ + btAssert(p && "null"); + + switch (type) { + case PHY_FLOAT: + { + btScalar * pf = (btScalar *)p; + *pf = value; + } + break; + + case PHY_UCHAR: + { + unsigned char * pu = (unsigned char *)p; + *pu = (unsigned char)(value / s_gridHeightScale); + } + break; + + case PHY_SHORT: + { + short * ps = (short *)p; + *ps = (short)(value / s_gridHeightScale); + } + break; + + default: + btAssert(!"bad type"); + } +} + + + +// creates a radially-varying heightfield +static void +setRadial +( + byte_t * grid, + int bytesPerElement, + PHY_ScalarType type, + btScalar phase = 0.0 +) +{ + btAssert(grid); + btAssert(bytesPerElement > 0); + + // min/max + btScalar period = 0.5 / s_gridSpacing; + btScalar floor = 0.0; + btScalar min_r = 3.0 * btSqrt(s_gridSpacing); + btScalar magnitude = 5.0 * btSqrt(s_gridSpacing); + + // pick a base_phase such that phase = 0 results in max height + // (this way, if you create a heightfield with phase = 0, + // you can rely on the min/max heights that result) + btScalar base_phase = (0.5 * SIMD_PI) - (period * min_r); + phase += base_phase; + + // center of grid + btScalar cx = 0.5 * s_gridSize * s_gridSpacing; + btScalar cy = cx; // assume square grid + byte_t * p = grid; + for (int i = 0; i < s_gridSize; ++i) { + float x = i * s_gridSpacing; + for (int j = 0; j < s_gridSize; ++j) { + float y = j * s_gridSpacing; + + float dx = x - cx; + float dy = y - cy; + + float r = sqrt((dx * dx) + (dy * dy)); + + float z = period; + if (r < min_r) { + r = min_r; + } + z = (1.0 / r) * sin(period * r + phase); + if (z > period) { + z = period; + } + else if (z < -period) { + z = -period; + } + z = floor + magnitude * z; + + convertFromFloat(p, z, type); + p += bytesPerElement; + } + } +} + + + +static float +randomHeight +( + int step +) +{ + return (0.33 * s_gridSpacing * s_gridSize * step * (rand() - (0.5 * RAND_MAX))) / (1.0 * RAND_MAX * s_gridSize); +} + + + +static void +dumpGrid +( + const byte_t * grid, + int bytesPerElement, + PHY_ScalarType type, + int max +) +{ + //std::cerr << "Grid:\n"; + + char buffer[32]; + + for (int j = 0; j < max; ++j) { + for (int i = 0; i < max; ++i) { + long offset = j * s_gridSize + i; + float z = convertToFloat(grid + offset * bytesPerElement, type); + sprintf(buffer, "%6.2f", z); + //std::cerr << " " << buffer; + } + //std::cerr << "\n"; + } +} + + + +static void +updateHeight +( + byte_t * p, + btScalar new_val, + PHY_ScalarType type +) +{ + btScalar old_val = convertToFloat(p, type); + if (!old_val) { + convertFromFloat(p, new_val, type); + } +} + + + +// creates a random, fractal heightfield +static void +setFractal +( + byte_t * grid, + int bytesPerElement, + PHY_ScalarType type, + int step +) +{ + btAssert(grid); + btAssert(bytesPerElement > 0); + btAssert(step > 0); + btAssert(step < s_gridSize); + + int newStep = step / 2; + // std::cerr << "Computing grid with step = " << step << ": before\n"; + // dumpGrid(grid, bytesPerElement, type, step + 1); + + // special case: starting (must set four corners) + if (s_gridSize - 1 == step) { + // pick a non-zero (possibly negative) base elevation for testing + btScalar base = randomHeight(step / 2); + + convertFromFloat(grid, base, type); + convertFromFloat(grid + step * bytesPerElement, base, type); + convertFromFloat(grid + step * s_gridSize * bytesPerElement, base, type); + convertFromFloat(grid + (step * s_gridSize + step) * bytesPerElement, base, type); + } + + // determine elevation of each corner + btScalar c00 = convertToFloat(grid, type); + btScalar c01 = convertToFloat(grid + step * bytesPerElement, type); + btScalar c10 = convertToFloat(grid + (step * s_gridSize) * bytesPerElement, type); + btScalar c11 = convertToFloat(grid + (step * s_gridSize + step) * bytesPerElement, type); + + // set top middle + updateHeight(grid + newStep * bytesPerElement, 0.5 * (c00 + c01) + randomHeight(step), type); + + // set left middle + updateHeight(grid + (newStep * s_gridSize) * bytesPerElement, 0.5 * (c00 + c10) + randomHeight(step), type); + + // set right middle + updateHeight(grid + (newStep * s_gridSize + step) * bytesPerElement, 0.5 * (c01 + c11) + randomHeight(step), type); + + // set bottom middle + updateHeight(grid + (step * s_gridSize + newStep) * bytesPerElement, 0.5 * (c10 + c11) + randomHeight(step), type); + + // set middle + updateHeight(grid + (newStep * s_gridSize + newStep) * bytesPerElement, 0.25 * (c00 + c01 + c10 + c11) + randomHeight(step), type); + + // std::cerr << "Computing grid with step = " << step << ": after\n"; + // dumpGrid(grid, bytesPerElement, type, step + 1); + + // terminate? + if (newStep < 2) { + return; + } + + // recurse + setFractal(grid, bytesPerElement, type, newStep); + setFractal(grid + newStep * bytesPerElement, bytesPerElement, type, newStep); + setFractal(grid + (newStep * s_gridSize) * bytesPerElement, bytesPerElement, type, newStep); + setFractal(grid + ((newStep * s_gridSize) + newStep) * bytesPerElement, bytesPerElement, type, newStep); +} + + + +static byte_t * +getRawHeightfieldData +( + eTerrainModel model, + PHY_ScalarType type, + btScalar& minHeight, + btScalar& maxHeight +) +{ + // std::cerr << "\nRegenerating terrain\n"; + // std::cerr << " model = " << model << "\n"; + // std::cerr << " type = " << type << "\n"; + + long nElements = ((long)s_gridSize) * s_gridSize; + // std::cerr << " nElements = " << nElements << "\n"; + + int bytesPerElement = getByteSize(type); + // std::cerr << " bytesPerElement = " << bytesPerElement << "\n"; + btAssert(bytesPerElement > 0 && "bad bytes per element"); + + long nBytes = nElements * bytesPerElement; + // std::cerr << " nBytes = " << nBytes << "\n"; + byte_t * raw = new byte_t[nBytes]; + btAssert(raw && "out of memory"); + + // reseed randomization every 30 seconds + // srand(time(NULL) / 30); + + // populate based on model + switch (model) { + case eRadial: + setRadial(raw, bytesPerElement, type); + break; + + case eFractal: + for (int i = 0; i < nBytes; i++) + { + raw[i] = 0; + } + setFractal(raw, bytesPerElement, type, s_gridSize - 1); + break; + + default: + btAssert(!"bad model type"); + } + + if (0) { + // inside if(0) so it keeps compiling but isn't + // exercised and doesn't cause warnings + // std::cerr << "final grid:\n"; + dumpGrid(raw, bytesPerElement, type, s_gridSize - 1); + } + + // find min/max + for (int i = 0; i < s_gridSize; ++i) { + for (int j = 0; j < s_gridSize; ++j) { + btScalar z = getGridHeight(raw, i, j, type); + // std::cerr << "i=" << i << ", j=" << j << ": z=" << z << "\n"; + + // update min/max + if (!i && !j) { + minHeight = z; + maxHeight = z; + } + else { + if (z < minHeight) { + minHeight = z; + } + if (z > maxHeight) { + maxHeight = z; + } + } + } + } + + if (maxHeight < -minHeight) { + maxHeight = -minHeight; + } + if (minHeight > -maxHeight) { + minHeight = -maxHeight; + } + + // std::cerr << " minHeight = " << minHeight << "\n"; + // std::cerr << " maxHeight = " << maxHeight << "\n"; + + return raw; +} + + + +//////////////////////////////////////////////////////////////////////////////// +// +// TerrainDemo class +// +//////////////////////////////////////////////////////////////////////////////// + +/// class that demonstrates the btHeightfieldTerrainShape object +class HeightfieldExample : public CommonRigidBodyMTBase//CommonRigidBodyBase +{ +public: + // constructor, destructor --------------------------------------------- + HeightfieldExample(struct GUIHelperInterface* helper); + virtual ~HeightfieldExample(); + + virtual void initPhysics(); + + // public class methods ------------------------------------------------ + + void castRays(); + + void stepSimulation(float deltaTime); + + void resetCamera() + { + float dist = 15; + float pitch = -32; + float yaw = 35; + float targetPos[3] = { 0, 0, 0 }; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + +private: + // private helper methods ---------------------------------------------- + void resetPhysics(void); + void clearWorld(void); + + // private data members ------------------------------------------------ + int m_upAxis; + PHY_ScalarType m_type; + eTerrainModel m_model; + byte_t * m_rawHeightfieldData; + btScalar m_minHeight; + btScalar m_maxHeight; + float m_phase; // for dynamics + bool m_isDynamic; + btHeightfieldTerrainShape * m_heightfieldShape; +}; + + +#define HEIGHTFIELD_TYPE_COUNT 2 +eTerrainModel gHeightfieldType = eRadial; + +void setHeightfieldTypeComboBoxCallback(int combobox, const char* item, void* userPointer) +{ + const char** items = static_cast(userPointer); + for (int i = 0; i < HEIGHTFIELD_TYPE_COUNT; ++i) + { + if (strcmp(item, items[i]) == 0) + { + gHeightfieldType = static_cast(i); + break; + } + } +} + + + +HeightfieldExample::HeightfieldExample(struct GUIHelperInterface* helper) + : CommonRigidBodyMTBase(helper), + m_upAxis(1), + m_type(PHY_FLOAT), + m_model(eFractal), + m_rawHeightfieldData(NULL), + m_phase(0.0), + m_isDynamic(true), + m_heightfieldShape(0) +{ + { + // create a combo box for selecting the solver type + static const char* sHeightfieldTypeComboBoxItems[HEIGHTFIELD_TYPE_COUNT]; + for (int i = 0; i < HEIGHTFIELD_TYPE_COUNT; ++i) + { + eTerrainModel heightfieldType = static_cast(i); + sHeightfieldTypeComboBoxItems[i] = getTerrainTypeName(heightfieldType); + } + ComboBoxParams comboParams; + comboParams.m_userPointer = sHeightfieldTypeComboBoxItems; + comboParams.m_numItems = HEIGHTFIELD_TYPE_COUNT; + comboParams.m_startItem = gHeightfieldType; + comboParams.m_items = sHeightfieldTypeComboBoxItems; + comboParams.m_callback = setHeightfieldTypeComboBoxCallback; + m_guiHelper->getParameterInterface()->registerComboBox(comboParams); + } +} + + + +HeightfieldExample::~HeightfieldExample(void) +{ + clearWorld(); + + +} + + +class MyTriangleCollector3 : public btTriangleCallback +{ +public: + btAlignedObjectArray* m_pVerticesOut; + btAlignedObjectArray* m_pIndicesOut; + + MyTriangleCollector3() + { + m_pVerticesOut = 0; + m_pIndicesOut = 0; + } + + virtual void processTriangle(btVector3* tris, int partId, int triangleIndex) + { + for (int k = 0; k < 3; k++) + { + GLInstanceVertex v; + v.xyzw[3] = 0; + v.uv[0] = v.uv[1] = 0.5f; + btVector3 normal = (tris[0] - tris[1]).cross(tris[0] - tris[2]); + normal.safeNormalize(); + for (int l = 0; l < 3; l++) + { + v.xyzw[l] = tris[k][l]; + v.normal[l] = normal[l]; + } + m_pIndicesOut->push_back(m_pVerticesOut->size()); + m_pVerticesOut->push_back(v); + } + } +}; + + +#define NUMRAYS2 500 +#define USE_PARALLEL_RAYCASTS 1 + +class btRaycastBar3 +{ +public: + btVector3 source[NUMRAYS2]; + btVector3 dest[NUMRAYS2]; + btVector3 direction[NUMRAYS2]; + btVector3 hit[NUMRAYS2]; + btVector3 normal[NUMRAYS2]; + struct GUIHelperInterface* m_guiHelper; + + int frame_counter; + int ms; + int sum_ms; + int sum_ms_samples; + int min_ms; + int max_ms; + +#ifdef USE_BT_CLOCK + btClock frame_timer; +#endif //USE_BT_CLOCK + + btScalar dx; + btScalar min_x; + btScalar max_x; + btScalar max_y; + btScalar sign; + + btRaycastBar3() + { + m_guiHelper = 0; + ms = 0; + max_ms = 0; + min_ms = 9999; + sum_ms_samples = 0; + sum_ms = 0; + } + + btRaycastBar3(btScalar ray_length, btScalar z, btScalar max_y, struct GUIHelperInterface* guiHelper) + { + m_guiHelper = guiHelper; + frame_counter = 0; + ms = 0; + max_ms = 0; + min_ms = 9999; + sum_ms_samples = 0; + sum_ms = 0; + dx = 10.0; + min_x = 0; + max_x = 0; + this->max_y = max_y; + sign = 1.0; + btScalar dalpha = 2 * SIMD_2_PI / NUMRAYS2; + for (int i = 0; i < NUMRAYS2; i++) + { + btScalar alpha = dalpha * i; + // rotate around by alpha degrees y + btQuaternion q(btVector3(0.0, 1.0, 0.0), alpha); + direction[i] = btVector3(1.0, 0.0, 0.0); + direction[i] = quatRotate(q, direction[i]); + direction[i] = direction[i] * ray_length; + + source[i] = btVector3(min_x, max_y, z); + dest[i] = source[i] + direction[i]; + dest[i][1] = -1000; + normal[i] = btVector3(1.0, 0.0, 0.0); + } + } + + void move(btScalar dt) + { + if (dt > btScalar(1.0 / 60.0)) + dt = btScalar(1.0 / 60.0); + for (int i = 0; i < NUMRAYS2; i++) + { + source[i][0] += dx * dt * sign; + dest[i][0] += dx * dt * sign; + } + if (source[0][0] < min_x) + sign = 1.0; + else if (source[0][0] > max_x) + sign = -1.0; + } + + void castRays(btCollisionWorld* cw, int iBegin, int iEnd) + { + if (m_guiHelper==0) + return; + + for (int i = iBegin; i < iEnd; ++i) + { + btCollisionWorld::ClosestRayResultCallback cb(source[i], dest[i]); + + { + BT_PROFILE("cw->rayTest"); + //to disable raycast accelerator, uncomment next line + //cb.m_flags |= btTriangleRaycastCallback::kF_DisableHeightfieldAccelerator; + cw->rayTest(source[i], dest[i], cb); + } + if (cb.hasHit()) + { + hit[i] = cb.m_hitPointWorld; + normal[i] = cb.m_hitNormalWorld; + normal[i].normalize(); + } + else + { + hit[i] = dest[i]; + normal[i] = btVector3(1.0, 0.0, 0.0); + } + } + } + + struct CastRaysLoopBody : public btIParallelForBody + { + btCollisionWorld* mWorld; + btRaycastBar3* mRaycasts; + + CastRaysLoopBody(btCollisionWorld* cw, btRaycastBar3* rb) : mWorld(cw), mRaycasts(rb) {} + + void forLoop(int iBegin, int iEnd) const + { + mRaycasts->castRays(mWorld, iBegin, iEnd); + } + }; + + void cast(btCollisionWorld* cw, bool multiThreading = false) + { + BT_PROFILE("cast"); + +#ifdef USE_BT_CLOCK + frame_timer.reset(); +#endif //USE_BT_CLOCK + +#ifdef BATCH_RAYCASTER + if (!gBatchRaycaster) + return; + + gBatchRaycaster->clearRays(); + for (int i = 0; i < NUMRAYS; i++) + { + gBatchRaycaster->addRay(source[i], dest[i]); + } + gBatchRaycaster->performBatchRaycast(); + for (int i = 0; i < gBatchRaycaster->getNumRays(); i++) + { + const SpuRaycastTaskWorkUnitOut& out = (*gBatchRaycaster)[i]; + hit[i].setInterpolate3(source[i], dest[i], out.hitFraction); + normal[i] = out.hitNormal; + normal[i].normalize(); + } +#else +#if USE_PARALLEL_RAYCASTS + if (multiThreading) + { + CastRaysLoopBody rayLooper(cw, this); + int grainSize = 20; // number of raycasts per task + btParallelFor(0, NUMRAYS2, grainSize, rayLooper); + } + else +#endif // USE_PARALLEL_RAYCASTS + { + // single threaded + castRays(cw, 0, NUMRAYS2); + } +#ifdef USE_BT_CLOCK + ms += frame_timer.getTimeMilliseconds(); +#endif //USE_BT_CLOCK + frame_counter++; + if (frame_counter > 50) + { + min_ms = ms < min_ms ? ms : min_ms; + max_ms = ms > max_ms ? ms : max_ms; + sum_ms += ms; + sum_ms_samples++; + btScalar mean_ms = (btScalar)sum_ms / (btScalar)sum_ms_samples; + printf("%d rays in %d ms %d %d %f\n", NUMRAYS2 * frame_counter, ms, min_ms, max_ms, mean_ms); + ms = 0; + frame_counter = 0; + } +#endif + } + + void draw() + { + if (m_guiHelper) + { + btAlignedObjectArray indices; + btAlignedObjectArray points; + + float lineColor[4] = { 1, 0.4, .4, 1 }; + + for (int i = 0; i < NUMRAYS2; i++) + { + btVector3FloatData s, h; + for (int w = 0; w < 4; w++) + { + s.m_floats[w] = source[i][w]; + h.m_floats[w] = hit[i][w]; + } + + points.push_back(s); + points.push_back(h); + indices.push_back(indices.size()); + indices.push_back(indices.size()); + } + + m_guiHelper->getRenderInterface()->drawLines(&points[0].m_floats[0], lineColor, points.size(), sizeof(btVector3FloatData), &indices[0], indices.size(), 1); + } + + } +}; + +static btRaycastBar3 raycastBar; + +void HeightfieldExample::castRays() +{ +#ifdef BT_THREADSAFE + raycastBar.cast(m_dynamicsWorld, true); +#else + raycastBar.cast(m_dynamicsWorld, false); +#endif +} + +void HeightfieldExample::stepSimulation(float deltaTime) +{ + castRays(); + + raycastBar.draw(); + + // if dynamic and radial, go ahead and update the field + if (m_rawHeightfieldData && m_isDynamic && eRadial == m_model && m_heightfieldShape) + { + btAlignedObjectArray gfxVertices; + btAlignedObjectArray indices; + int strideInBytes = 9 * sizeof(float); + + m_phase += s_deltaPhase * deltaTime; + if (m_phase > 2.0 * SIMD_PI) { + m_phase -= 2.0 * SIMD_PI; + } + int bpe = getByteSize(m_type); + btAssert(bpe > 0 && "Bad bytes per element"); + setRadial(m_rawHeightfieldData, bpe, m_type, m_phase); + + MyTriangleCollector3 col; + col.m_pVerticesOut = &gfxVertices; + col.m_pIndicesOut = &indices; + btVector3 aabbMin, aabbMax; + for (int k = 0; k < 3; k++) + { + aabbMin[k] = -BT_LARGE_FLOAT; + aabbMax[k] = BT_LARGE_FLOAT; + } + m_heightfieldShape->processAllTriangles(&col, aabbMin, aabbMax); + if (gfxVertices.size() && indices.size()) + { + m_guiHelper->getRenderInterface()->updateShape(m_heightfieldShape->getUserIndex(), &gfxVertices[0].xyzw[0]); + } + } + + if (m_model != gHeightfieldType) + { + m_model = gHeightfieldType; + resetPhysics(); + } + CommonRigidBodyMTBase::stepSimulation(deltaTime); +} + +//////////////////////////////////////////////////////////////////////////////// +// +// TerrainDemo -- public class methods +// +//////////////////////////////////////////////////////////////////////////////// + +/// one-time class and physics initialization +void HeightfieldExample::initPhysics() +{ + // std::cerr << "initializing...\n"; + + createEmptyDynamicsWorld(); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + raycastBar = btRaycastBar3(2500.0, 0, 2.0, m_guiHelper); + // set up basic state + m_upAxis = 1; // start with Y-axis as "up" + + + m_type = PHY_FLOAT;// SHORT; + m_model = gHeightfieldType; + m_isDynamic = true; + + // set up the physics world + + // initialize axis- or type-dependent physics from here + this->resetPhysics(); + +} + + + + +static PHY_ScalarType nextType (PHY_ScalarType type) +{ + switch (type) + { + case PHY_FLOAT: + return PHY_SHORT; + break; + case PHY_SHORT: + return PHY_UCHAR; + break; + case PHY_UCHAR: + return PHY_FLOAT; + break; + } + btAssert (0); + return PHY_FLOAT; +} + + + +static void doPrint(int x, int& y, int dy, const char * text) +{ + //GLDebugDrawString(x, y, text); + y += dy; +} + + +//////////////////////////////////////////////////////////////////////////////// +// +// TerrainDemo -- private helper methods +// +//////////////////////////////////////////////////////////////////////////////// + +/// called whenever key terrain attribute is changed +void HeightfieldExample::resetPhysics(void) +{ + m_guiHelper->removeAllGraphicsInstances(); + + // remove old heightfield + clearWorld(); + + // reset gravity to point in appropriate direction + m_dynamicsWorld->setGravity(getUpVector(m_upAxis, 0.0, -s_gravity)); + + // get new heightfield of appropriate type + m_rawHeightfieldData = + getRawHeightfieldData(m_model, m_type, m_minHeight, m_maxHeight); + btAssert(m_rawHeightfieldData && "failed to create raw heightfield"); + + bool flipQuadEdges = false; + m_heightfieldShape = + new btHeightfieldTerrainShape(s_gridSize, s_gridSize, + m_rawHeightfieldData, + s_gridHeightScale, + m_minHeight, m_maxHeight, + m_upAxis, m_type, flipQuadEdges); + btAssert(m_heightfieldShape && "null heightfield"); + + //buildAccelerator is optional, it may not support all features. + m_heightfieldShape->buildAccelerator(); + + // scale the shape + btVector3 localScaling = getUpVector(m_upAxis, s_gridSpacing, 1.0); + m_heightfieldShape->setLocalScaling(localScaling); + + // stash this shape away + m_collisionShapes.push_back(m_heightfieldShape); + + // set origin to middle of heightfield + btTransform tr; + tr.setIdentity(); + tr.setOrigin(btVector3(0, 0, 0)); + + // create ground object + float mass = 0.0; + createRigidBody(mass, tr, m_heightfieldShape); + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + + +/// removes all objects and shapes from the world +void HeightfieldExample::clearWorld(void) +{ + if (m_dynamicsWorld) + { + //remove the rigidbodies from the dynamics world and delete them + int i; + for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) + { + btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) + { + delete body->getMotionState(); + } + m_dynamicsWorld->removeCollisionObject(obj); + delete obj; + } + + //delete collision shapes + for (int j = 0; j < m_collisionShapes.size(); j++) + { + btCollisionShape* shape = m_collisionShapes[j]; + delete shape; + } + m_collisionShapes.clear(); + + // delete raw heightfield data + delete m_rawHeightfieldData; + m_rawHeightfieldData = NULL; + } +} + +CommonExampleInterface* HeightfieldExampleCreateFunc(CommonExampleOptions& options) +{ + return new HeightfieldExample(options.m_guiHelper); +} + +B3_STANDALONE_EXAMPLE(HeightfieldExampleCreateFunc) + diff --git a/examples/Heightfield/HeightfieldExample.h b/examples/Heightfield/HeightfieldExample.h new file mode 100644 index 000000000..258e7f701 --- /dev/null +++ b/examples/Heightfield/HeightfieldExample.h @@ -0,0 +1,21 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2015 Google Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef HEIGHTFIELD_EXAMPLE_H +#define HEIGHTFIELD_EXAMPLE_H + +class CommonExampleInterface* HeightfieldExampleCreateFunc(struct CommonExampleOptions& options); + +#endif //HEIGHTFIELD_EXAMPLE_H \ No newline at end of file diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 782e9efaf..eea18339d 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -22,6 +22,7 @@ subject to the following restrictions: #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,21 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans, co rcb.m_hitFraction = resultCallback.m_closestHitFraction; triangleMesh->performRaycast(&rcb, rayFromLocalScaled, rayToLocalScaled); } + else if (((resultCallback.m_flags&btTriangleRaycastCallback::kF_DisableHeightfieldAccelerator)==0) + && collisionShape->getShapeType() == TERRAIN_SHAPE_PROXYTYPE && + (((btHeightfieldTerrainShape*)collisionShape)->getUpAxis()==1)//accelerator only supports Y axis at the moment + ) + { + ///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/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index c85ce2498..456418ef0 100644 --- a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/src/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_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,447 @@ const btVector3& btHeightfieldTerrainShape::getLocalScaling() const { return m_localScaling; } + +namespace +{ + 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]; + + // TODO Since this is for raycasts, we could greatly benefit from an early exit on the first hit + + // 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 btAlignedObjectArray& vbounds; + int width; + int length; + int chunkSize; + + btVector3 rayBegin; + btVector3 rayEnd; + btVector3 rayDir; + + ProcessTrianglesAction processTriangles; + + ProcessVBoundsAction(const btAlignedObjectArray& bnd) + : vbounds(bnd) + { + } + 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.size()==0) + { + // 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(m_vboundsGrid); + processVBounds.width = m_vboundsGridWidth; + processVBounds.length = m_vboundsGridLength; + 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; + } + + // This data structure is only reallocated if the required size changed + m_vboundsGrid.resize(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() +{ + m_vboundsGrid.clear(); +} \ No newline at end of file diff --git a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h index 8a50a57e3..7e71d4f65 100644 --- a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h +++ b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h @@ -17,6 +17,7 @@ subject to the following restrictions: #define BT_HEIGHTFIELD_TERRAIN_SHAPE_H #include "btConcaveShape.h" +#include "LinearMath/btAlignedObjectArray.h" ///btHeightfieldTerrainShape simulates a 2D heightfield terrain /** @@ -71,6 +72,13 @@ 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 +108,14 @@ protected: btVector3 m_localScaling; + // Accelerator + btAlignedObjectArray 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 /** @@ -155,8 +168,19 @@ public: 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(); + + int getUpAxis() const + { + return m_upAxis; + } //debugging virtual const char* getName() const { return "HEIGHTFIELD"; } }; -#endif //BT_HEIGHTFIELD_TERRAIN_SHAPE_H +#endif //BT_HEIGHTFIELD_TERRAIN_SHAPE_H \ No newline at end of file diff --git a/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h index 2b2dfabec..2d0df718a 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h +++ b/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h @@ -37,6 +37,7 @@ public: ///SubSimplexConvexCastRaytest is the default, even if kF_None is set. kF_UseSubSimplexConvexCastRaytest = 1 << 2, // Uses an approximate but faster ray versus convex intersection algorithm kF_UseGjkConvexCastRaytest = 1 << 3, + kF_DisableHeightfieldAccelerator = 1 << 4, //don't use the heightfield raycast accelerator. See https://github.com/bulletphysics/bullet3/pull/2062 kF_Terminator = 0xFFFFFFFF }; unsigned int m_flags; From ee9b14299c587510efd54c5cff42729c2e05b13d Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Mon, 15 Apr 2019 22:19:40 -0700 Subject: [PATCH 6/6] Revert "Accelerated terrain raycast with "Bresenham" and chunks" --- .../CollisionDispatch/btCollisionWorld.cpp | 51 +- .../btHeightfieldTerrainShape.cpp | 457 ------------------ .../btHeightfieldTerrainShape.h | 21 +- 3 files changed, 39 insertions(+), 490 deletions(-) diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 8b5a4c307..782e9efaf 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -22,7 +22,6 @@ subject to the following restrictions: #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" @@ -414,18 +413,6 @@ 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 @@ -436,6 +423,44 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans, co btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); + //ConvexCast::CastResult + + struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback + { + btCollisionWorld::RayResultCallback* m_resultCallback; + const btCollisionObject* m_collisionObject; + btConcaveShape* m_triangleMesh; + + btTransform m_colObjWorldTransform; + + BridgeTriangleRaycastCallback(const btVector3& from, const btVector3& to, + btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject, btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform) : //@BP Mod + btTriangleRaycastCallback(from, to, resultCallback->m_flags), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh), + m_colObjWorldTransform(colObjWorldTransform) + { + } + + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = partId; + shapeInfo.m_triangleIndex = triangleIndex; + + btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal; + + btCollisionWorld::LocalRayResult rayResult(m_collisionObject, + &shapeInfo, + hitNormalWorld, + hitFraction); + + bool normalInWorldSpace = true; + return m_resultCallback->addSingleResult(rayResult, normalInWorldSpace); + } + }; + BridgeTriangleRaycastCallback rcb(rayFromLocal, rayToLocal, &resultCallback, collisionObjectWrap->getCollisionObject(), concaveShape, colObjWorldTransform); rcb.m_hitFraction = resultCallback.m_closestHitFraction; diff --git a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index 66bf44fc9..c85ce2498 100644 --- a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -73,10 +73,6 @@ 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) @@ -112,7 +108,6 @@ void btHeightfieldTerrainShape::initialize( btHeightfieldTerrainShape::~btHeightfieldTerrainShape() { - clearAccelerator(); } void btHeightfieldTerrainShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const @@ -328,8 +323,6 @@ 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++) @@ -380,453 +373,3 @@ const btVector3& btHeightfieldTerrainShape::getLocalScaling() const { return m_localScaling; } - -namespace -{ - 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]; - - // TODO Since this is for raycasts, we could greatly benefit from an early exit on the first hit - - // 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/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h index 1f3c3d868..8a50a57e3 100644 --- a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h +++ b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h @@ -71,13 +71,6 @@ 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; @@ -107,14 +100,9 @@ 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 /** @@ -167,13 +155,6 @@ public: 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"; } };