From a8d1c121fd9381852c053690fe7f312839081d45 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 16 Apr 2019 10:34:59 -0700 Subject: [PATCH] allow Z as up-axis for raycast acceleration in btHeightfieldTerrainShape --- examples/Heightfield/HeightfieldExample.cpp | 31 +++++-- .../CollisionDispatch/btCollisionWorld.cpp | 3 +- .../btHeightfieldTerrainShape.cpp | 85 ++++++++++++------- .../btHeightfieldTerrainShape.h | 6 +- 4 files changed, 81 insertions(+), 44 deletions(-) diff --git a/examples/Heightfield/HeightfieldExample.cpp b/examples/Heightfield/HeightfieldExample.cpp index a4bd06fe5..0e73066db 100644 --- a/examples/Heightfield/HeightfieldExample.cpp +++ b/examples/Heightfield/HeightfieldExample.cpp @@ -678,8 +678,9 @@ public: sum_ms = 0; } - btRaycastBar3(btScalar ray_length, btScalar z, btScalar max_y, struct GUIHelperInterface* guiHelper) + btRaycastBar3(btScalar ray_length, btScalar z, btScalar max_y, struct GUIHelperInterface* guiHelper, int upAxisIndex) { + m_guiHelper = guiHelper; frame_counter = 0; ms = 0; @@ -697,14 +698,24 @@ public: { btScalar alpha = dalpha * i; // rotate around by alpha degrees y - btQuaternion q(btVector3(0.0, 1.0, 0.0), alpha); + btVector3 upAxis(0, 0, 0); + upAxis[upAxisIndex] = 1; + + btQuaternion q(upAxis, 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); + if (upAxisIndex == 1) + { + source[i] = btVector3(min_x, max_y, z); + } + else + { + source[i] = btVector3(min_x, z, max_y); + } dest[i] = source[i] + direction[i]; - dest[i][1] = -1000; + dest[i][upAxisIndex] = -1000; normal[i] = btVector3(1.0, 0.0, 0.0); } } @@ -922,10 +933,12 @@ void HeightfieldExample::initPhysics() 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_upAxis = 2; // start with Y-axis as "up" + m_guiHelper->setUpAxis(m_upAxis); + raycastBar = btRaycastBar3(2500.0, 0, 2.0, m_guiHelper, m_upAxis); + // set up basic state + m_type = PHY_FLOAT;// SHORT; m_model = gHeightfieldType; @@ -970,7 +983,9 @@ void HeightfieldExample::resetPhysics(void) m_minHeight, m_maxHeight, m_upAxis, m_type, flipQuadEdges); btAssert(m_heightfieldShape && "null heightfield"); - + + if (m_upAxis == 2) + m_heightfieldShape->setFlipTriangleWinding(true); //buildAccelerator is optional, it may not support all features. m_heightfieldShape->buildAccelerator(); diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index eea18339d..71184f36a 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -415,8 +415,7 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans, co 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 + && collisionShape->getShapeType() == TERRAIN_SHAPE_PROXYTYPE ) { ///optimized version for btHeightfieldTerrainShape diff --git a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index 456418ef0..34ec2d8c4 100644 --- a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -71,6 +71,7 @@ void btHeightfieldTerrainShape::initialize( m_flipQuadEdges = flipQuadEdges; m_useDiamondSubdivision = false; m_useZigzagSubdivision = false; + m_flipTriangleWinding = false; m_upAxis = upAxis; m_localScaling.setValue(btScalar(1.), btScalar(1.), btScalar(1.)); @@ -335,30 +336,37 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback for (int x = startX; x < endX; x++) { btVector3 vertices[3]; + int indices[3] = { 0, 1, 2 }; + if (m_flipTriangleWinding) + { + indices[0] = 2; + indices[2] = 0; + } + if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j + x) & 1)) || (m_useZigzagSubdivision && !(j & 1))) { //first triangle - getVertex(x, j, vertices[0]); - getVertex(x, j + 1, vertices[1]); - getVertex(x + 1, j + 1, vertices[2]); + getVertex(x, j, vertices[indices[0]]); + getVertex(x, j + 1, vertices[indices[1]]); + getVertex(x + 1, j + 1, vertices[indices[2]]); callback->processTriangle(vertices, x, j); //second triangle // getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman - getVertex(x + 1, j + 1, vertices[1]); - getVertex(x + 1, j, vertices[2]); + getVertex(x + 1, j + 1, vertices[indices[1]]); + getVertex(x + 1, j, vertices[indices[2]]); callback->processTriangle(vertices, x, j); } else { //first triangle - getVertex(x, j, vertices[0]); - getVertex(x, j + 1, vertices[1]); - getVertex(x + 1, j, vertices[2]); + getVertex(x, j, vertices[indices[0]]); + getVertex(x, j + 1, vertices[indices[1]]); + getVertex(x + 1, j, vertices[indices[2]]); callback->processTriangle(vertices, x, j); //second triangle - getVertex(x + 1, j, vertices[0]); + getVertex(x + 1, j, vertices[indices[0]]); //getVertex(x,j+1,vertices[1]); - getVertex(x + 1, j + 1, vertices[2]); + getVertex(x + 1, j + 1, vertices[indices[2]]); callback->processTriangle(vertices, x, j); } } @@ -401,7 +409,7 @@ namespace /// 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) +void gridRaycast(Action_T& quadAction, const btVector3& beginPos, const btVector3& endPos, int indices[3]) { GridRaycastState rs; rs.maxDistance3d = beginPos.distance(endPos); @@ -410,9 +418,10 @@ void gridRaycast(Action_T& quadAction, const btVector3& beginPos, const btVector // Consider the ray is too small to hit anything return; } + - btScalar rayDirectionFlatX = endPos[0] - beginPos[0]; - btScalar rayDirectionFlatZ = endPos[2] - beginPos[2]; + btScalar rayDirectionFlatX = endPos[indices[0]] - beginPos[indices[0]]; + btScalar rayDirectionFlatZ = endPos[indices[2]] - beginPos[indices[2]]; rs.maxDistanceFlat = btSqrt(rayDirectionFlatX * rayDirectionFlatX + rayDirectionFlatZ * rayDirectionFlatZ); if (rs.maxDistanceFlat < 0.0001) @@ -444,11 +453,11 @@ void gridRaycast(Action_T& quadAction, const btVector3& beginPos, const btVector { if (xiStep == 1) { - paramCrossX = (ceil(beginPos[0]) - beginPos[0]) * paramDeltaX; + paramCrossX = (ceil(beginPos[indices[0]]) - beginPos[indices[0]]) * paramDeltaX; } else { - paramCrossX = (beginPos[0] - floor(beginPos[0])) * paramDeltaX; + paramCrossX = (beginPos[indices[0]] - floor(beginPos[indices[0]])) * paramDeltaX; } } else @@ -461,11 +470,11 @@ void gridRaycast(Action_T& quadAction, const btVector3& beginPos, const btVector { if (ziStep == 1) { - paramCrossZ = (ceil(beginPos[2]) - beginPos[2]) * paramDeltaZ; + paramCrossZ = (ceil(beginPos[indices[2]]) - beginPos[indices[2]]) * paramDeltaZ; } else { - paramCrossZ = (beginPos[2] - floor(beginPos[2])) * paramDeltaZ; + paramCrossZ = (beginPos[indices[2]] - floor(beginPos[indices[2]])) * paramDeltaZ; } } else @@ -473,8 +482,8 @@ void gridRaycast(Action_T& quadAction, const btVector3& beginPos, const btVector paramCrossZ = infinite; // Will never cross on Z } - rs.x = static_cast(floor(beginPos[0])); - rs.z = static_cast(floor(beginPos[2])); + rs.x = static_cast(floor(beginPos[indices[0]])); + rs.z = static_cast(floor(beginPos[indices[2]])); // Workaround cases where the ray starts at an integer position if (paramCrossX == 0.0) @@ -603,10 +612,12 @@ struct ProcessVBoundsAction btVector3 rayEnd; btVector3 rayDir; + int* m_indices; ProcessTrianglesAction processTriangles; - ProcessVBoundsAction(const btAlignedObjectArray& bnd) - : vbounds(bnd) + ProcessVBoundsAction(const btAlignedObjectArray& bnd, int* indices) + : vbounds(bnd), + m_indices(indices) { } void operator()(const GridRaycastState& rs) const @@ -634,11 +645,11 @@ struct ProcessVBoundsAction // 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) + if (enterPos[1] > chunk.max && exitPos[m_indices[1]] > chunk.max) { return; } - if (enterPos[1] < chunk.min && exitPos[1] < chunk.min) + if (enterPos[1] < chunk.min && exitPos[m_indices[1]] < chunk.min) { return; } @@ -651,7 +662,7 @@ struct ProcessVBoundsAction exitPos = rayEnd; } - gridRaycast(processTriangles, enterPos, exitPos); + gridRaycast(processTriangles, enterPos, exitPos, m_indices); // 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 } @@ -677,10 +688,16 @@ void btHeightfieldTerrainShape::performRaycast(btTriangleCallback* callback, con 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])); + int indices[3] = { 0, 1, 2 }; + if (m_upAxis == 2) + { + indices[1] = 2; + indices[2] = 1; + } + int iBeginX = static_cast(floor(beginPos[indices[0]])); + int iBeginZ = static_cast(floor(beginPos[indices[2]])); + int iEndX = static_cast(floor(endPos[indices[0]])); + int iEndZ = static_cast(floor(endPos[indices[2]])); if (iBeginX == iEndX && iBeginZ == iEndZ) { @@ -691,22 +708,24 @@ void btHeightfieldTerrainShape::performRaycast(btTriangleCallback* callback, con return; } + + if (m_vboundsGrid.size()==0) { // Process all quads intersecting the flat projection of the ray - gridRaycast(processTriangles, beginPos, endPos); + gridRaycast(processTriangles, beginPos, endPos, &indices[0]); } else { btVector3 rayDiff = endPos - beginPos; - btScalar flatDistance2 = rayDiff[0] * rayDiff[0] + rayDiff[2] * rayDiff[2]; + btScalar flatDistance2 = rayDiff[indices[0]] * rayDiff[indices[0]] + rayDiff[indices[2]] * rayDiff[indices[2]]; if (flatDistance2 < m_vboundsChunkSize * m_vboundsChunkSize) { // Don't use chunks, the ray is too short in the plane - gridRaycast(processTriangles, beginPos, endPos); + gridRaycast(processTriangles, beginPos, endPos, &indices[0]); } - ProcessVBoundsAction processVBounds(m_vboundsGrid); + ProcessVBoundsAction processVBounds(m_vboundsGrid, &indices[0]); processVBounds.width = m_vboundsGridWidth; processVBounds.length = m_vboundsGridLength; processVBounds.rayBegin = beginPos; @@ -715,7 +734,7 @@ void btHeightfieldTerrainShape::performRaycast(btTriangleCallback* callback, con 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); + gridRaycast(processVBounds, beginPos / m_vboundsChunkSize, endPos / m_vboundsChunkSize, indices); } } diff --git a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h index 7e71d4f65..43e1d25e3 100644 --- a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h +++ b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h @@ -103,7 +103,7 @@ protected: bool m_flipQuadEdges; bool m_useDiamondSubdivision; bool m_useZigzagSubdivision; - + bool m_flipTriangleWinding; int m_upAxis; btVector3 m_localScaling; @@ -158,6 +158,10 @@ public: ///could help compatibility with Ogre heightfields. See https://code.google.com/p/bullet/issues/detail?id=625 void setUseZigzagSubdivision(bool useZigzagSubdivision = true) { m_useZigzagSubdivision = useZigzagSubdivision; } + void setFlipTriangleWinding(bool flipTriangleWinding) + { + m_flipTriangleWinding = flipTriangleWinding; + } virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const;