Merge pull request #2362 from erwincoumans/master

pybullet: restore internal edge filtering (see internalEdge.py example)

6 files changed, 141 insertions, 14 deletions

diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
index 4f2a30b70..1a85c1928 100644
--- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
+++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
@@ -612,6 +612,11 @@ btTransform ConvertURDF2BulletInternal(
 					}
 				}
 
+				if (compoundShape->getShapeType() == TERRAIN_SHAPE_PROXYTYPE)
+				{
+					col->setCollisionFlags(col->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
+				}
+
 				btTransform tr;
 				tr.setIdentity();
 				tr = linkTransformInWorldSpace;
@@ -677,7 +682,7 @@ btTransform ConvertURDF2BulletInternal(
 						} while (testLinkIndex> 0);
 						if (allJointsFixed)
 						{
-							col->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);
+							col->setCollisionFlags(col->getCollisionFlags() | btCollisionObject::CF_STATIC_OBJECT);
 						}
 
 					}
@@ -699,7 +704,7 @@ btTransform ConvertURDF2BulletInternal(
 						//&& cache.m_bulletMultiBody->getNumDofs()==0)
 						{
 							//col->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
-							col->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);
+							col->setCollisionFlags(col->getCollisionFlags() | btCollisionObject::CF_STATIC_OBJECT);
 						}
 					}
 
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index cf2dc2328..351541cad 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -2805,6 +2805,14 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
 				delete trimesh->getTriangleInfoMap();
 			}
 		}
+		if (shape->getShapeType() == TERRAIN_SHAPE_PROXYTYPE)
+		{
+			btHeightfieldTerrainShape* terrain = (btHeightfieldTerrainShape*)shape;
+			if (terrain->getTriangleInfoMap())
+			{
+				delete terrain->getTriangleInfoMap();
+			}
+		}
 		delete shape;
 	}
 	for (int j = 0; j < m_data->m_heightfieldDatas.size(); j++)
@@ -4546,12 +4554,19 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 
 					bool flipQuadEdges = false;
 					int upAxis = 2;
-					btHeightfieldTerrainShape* heightfieldShape = worldImporter->createHeightfieldShape( width, height,
+					/*btHeightfieldTerrainShape* heightfieldShape = worldImporter->createHeightfieldShape( width, height,
 						heightfieldData,
 							gridHeightScale,
 							minHeight, maxHeight,
 							upAxis, int(scalarType), flipQuadEdges);
-					
+					*/
+					btHeightfieldTerrainShape* heightfieldShape = new btHeightfieldTerrainShape( width, height,
+					heightfieldData,
+					gridHeightScale,
+					minHeight, maxHeight,
+					upAxis, scalarType, flipQuadEdges);
+					m_data->m_collisionShapes.push_back(heightfieldShape);
+
 					heightfieldShape->setUserValue3(clientCmd.m_createUserShapeArgs.m_shapes[i].m_heightfieldTextureScaling);
 					shape = heightfieldShape;
 					if (upAxis == 2)
@@ -4565,6 +4580,9 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 						clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[2]);
 					
 					heightfieldShape->setLocalScaling(localScaling);
+
+					btTriangleInfoMap* triangleInfoMap = new btTriangleInfoMap();
+					btGenerateInternalEdgeInfo(heightfieldShape, triangleInfoMap);
 					
 					this->m_data->m_heightfieldDatas.push_back(heightfieldData);
 						
diff --git a/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp b/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp
index e74c83f9f..a4252c296 100644
--- a/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp
+++ b/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp
@@ -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;
 
diff --git a/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h b/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h
index 9d9cff040..cc6d11c24 100644
--- a/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h
+++ b/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h
@@ -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);
diff --git a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
index d4c2e1199..34e7926f1 100644
--- a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
+++ b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
@@ -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);
 			}
 		}
 	}
diff --git a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
index e0f25840b..7cf7e26b5 100644
--- a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
+++ b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
@@ -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
\ No newline at end of file