Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

PyBullet: add support for internal edge filtering for height field terrains

Browse Source
This commit is contained in:
Erwin Coumans
2019-08-09 10:14:14 -07:00
parent 046e036519
commit 54111f7023
6 changed files with 139 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

93
src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp
View File

@@ -1,6 +1,8 @@
#include "btInternalEdgeUtility.h"
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h"
#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btTriangleShape.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
@@ -290,6 +292,39 @@ struct btConnectivityProcessor : public btTriangleCallback
}
}
};
struct b3ProcessAllTrianglesHeightfield: public btTriangleCallback
{
btHeightfieldTerrainShape* m_heightfieldShape;
btTriangleInfoMap* m_triangleInfoMap;
b3ProcessAllTrianglesHeightfield(btHeightfieldTerrainShape* heightFieldShape, btTriangleInfoMap* triangleInfoMap)
:m_heightfieldShape(heightFieldShape),
m_triangleInfoMap(triangleInfoMap)
{
}
virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
{
btConnectivityProcessor connectivityProcessor;
connectivityProcessor.m_partIdA = partId;
connectivityProcessor.m_triangleIndexA = triangleIndex;
connectivityProcessor.m_triangleVerticesA = triangle;
connectivityProcessor.m_triangleInfoMap = m_triangleInfoMap;
btVector3 aabbMin, aabbMax;
aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
aabbMin.setMin(triangle[0]);
aabbMax.setMax(triangle[0]);
aabbMin.setMin(triangle[1]);
aabbMax.setMax(triangle[1]);
aabbMin.setMin(triangle[2]);
aabbMax.setMax(triangle[2]);
m_heightfieldShape->processAllTriangles(&connectivityProcessor, aabbMin, aabbMax);
}
};
/////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////
@@ -361,6 +396,28 @@ void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape* trimeshShape, btTriangle
}
}
void btGenerateInternalEdgeInfo(btHeightfieldTerrainShape* heightfieldShape, btTriangleInfoMap* triangleInfoMap)
{
//the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there!
if (heightfieldShape->getTriangleInfoMap())
return;
heightfieldShape->setTriangleInfoMap(triangleInfoMap);
//get all the triangles of the heightfield
btVector3 aabbMin, aabbMax;
aabbMax.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
aabbMin.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
b3ProcessAllTrianglesHeightfield processHeightfield(heightfieldShape, triangleInfoMap);
heightfieldShape->processAllTriangles(&processHeightfield, aabbMin, aabbMax);
}
// Given a point and a line segment (defined by two points), compute the closest point
// in the line. Cap the point at the endpoints of the line segment.
void btNearestPointInLineSegment(const btVector3& point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint)
@@ -426,6 +483,32 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWr
if (colObj0Wrap->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE)
return;
btTriangleInfoMap* triangleInfoMapPtr = 0;
if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == TERRAIN_SHAPE_PROXYTYPE)
{
btHeightfieldTerrainShape* heightfield = (btHeightfieldTerrainShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
triangleInfoMapPtr = heightfield->getTriangleInfoMap();
//#define USE_HEIGHTFIELD_TRIANGLES
#ifdef USE_HEIGHTFIELD_TRIANGLES
btVector3 newNormal = btVector3(0, 0, 1);
const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0Wrap->getCollisionShape());
btVector3 tri_normal;
tri_shape->calcNormal(tri_normal);
newNormal = tri_normal;
// cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
cp.m_normalWorldOnB = newNormal;
// Reproject collision point along normal. (what about cp.m_distance1?)
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
return;
#endif
}
btBvhTriangleMeshShape* trimesh = 0;
if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE)
@@ -439,10 +522,12 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWr
trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
}
}
if (trimesh == 0)
return;
btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*)trimesh->getTriangleInfoMap();
if (trimesh)
{
triangleInfoMapPtr = (btTriangleInfoMap*)trimesh->getTriangleInfoMap();
}
if (!triangleInfoMapPtr)
return;

3
src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h
View File

@@ -15,6 +15,7 @@ class btCollisionObject;
struct btCollisionObjectWrapper;
class btManifoldPoint;
class btIDebugDraw;
class btHeightfieldTerrainShape;
enum btInternalEdgeAdjustFlags
{
@@ -26,6 +27,8 @@ enum btInternalEdgeAdjustFlags
///Call btGenerateInternalEdgeInfo to create triangle info, store in the shape 'userInfo'
void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape* trimeshShape, btTriangleInfoMap* triangleInfoMap);
void btGenerateInternalEdgeInfo(btHeightfieldTerrainShape* trimeshShape, btTriangleInfoMap* triangleInfoMap);
///Call the btFixMeshNormal to adjust the collision normal, using the triangle info map (generated using btGenerateInternalEdgeInfo)
///If this info map is missing, or the triangle is not store in this map, nothing will be done
void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* trimeshColObj0Wrap, const btCollisionObjectWrapper* otherColObj1Wrap, int partId0, int index0, int normalAdjustFlags = 0);

14
src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
View File

@@ -22,7 +22,8 @@ btHeightfieldTerrainShape::btHeightfieldTerrainShape(
btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis,
PHY_ScalarType hdt, bool flipQuadEdges)
:m_userIndex2(-1),
m_userValue3(0)
m_userValue3(0),
m_triangleInfoMap(0)
{
initialize(heightStickWidth, heightStickLength, heightfieldData,
heightScale, minHeight, maxHeight, upAxis, hdt,
@@ -31,7 +32,8 @@ btHeightfieldTerrainShape::btHeightfieldTerrainShape(
btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar maxHeight, int upAxis, bool useFloatData, bool flipQuadEdges)
:m_userIndex2(-1),
m_userValue3(0)
m_userValue3(0),
m_triangleInfoMap(0)
{
// legacy constructor: support only float or unsigned char,
// and min height is zero
@@ -353,12 +355,12 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback
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);
callback->processTriangle(vertices, 2 * x, j);
//second triangle
// getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman
getVertex(x + 1, j + 1, vertices[indices[1]]);
getVertex(x + 1, j, vertices[indices[2]]);
callback->processTriangle(vertices, x, j);
callback->processTriangle(vertices, 2 * x+1, j);
}
else
{
@@ -366,12 +368,12 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback
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);
callback->processTriangle(vertices, 2 * x, j);
//second triangle
getVertex(x + 1, j, vertices[indices[0]]);
//getVertex(x,j+1,vertices[1]);
getVertex(x + 1, j + 1, vertices[indices[2]]);
callback->processTriangle(vertices, x, j);
callback->processTriangle(vertices, 2 * x+1, j);
}
}
}

14
src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
View File

@@ -117,6 +117,8 @@ protected:
int m_userIndex2;
btScalar m_userValue3;
struct btTriangleInfoMap* m_triangleInfoMap;
virtual btScalar getRawHeightFieldValue(int x, int y) const;
void quantizeWithClamp(int* out, const btVector3& point, int isMax) const;
@@ -206,6 +208,18 @@ public:
{
return m_userValue3;
}
const struct btTriangleInfoMap* getTriangleInfoMap() const
{
return m_triangleInfoMap;
}
struct btTriangleInfoMap* getTriangleInfoMap()
{
return m_triangleInfoMap;
}
void setTriangleInfoMap(btTriangleInfoMap* map)
{
m_triangleInfoMap = map;
}
};
#endif //BT_HEIGHTFIELD_TERRAIN_SHAPE_H