add pyproject.toml

see also https://packaging.python.org/en/latest/guides/modernize-setup-py-project/ and
https://github.com/pypa/pip/issues/6334

CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.4.3)
+cmake_minimum_required(VERSION 3.5)
 set(CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS true)
 cmake_policy(SET CMP0017 NEW)
 #this line has to appear before 'PROJECT' in order to be able to disable incremental linking

README.md

@@ -91,7 +91,7 @@ p.connect(p.SHARED_MEMORY) #or (p.TCP, "localhost", 6667) or (p.UDP, "192.168.86
 
 **Linux and Mac OSX gnu make**
 
-Make sure cmake is installed (sudo apt-get install cmake, brew install cmake, or https://cmake.org)
+Make sure gcc and cmake is installed (`sudo apt-get install build-essential` and `sudo apt-get install cmake` for Linux, `brew install cmake` for Mac, or https://cmake.org)
 
 In a terminal type:
 ```

VERSION

@@ -1 +1 @@
-3.24
+3.27

examples/ExampleBrowser/CMakeLists.txt

@@ -284,7 +284,7 @@ SET(BulletExampleBrowser_SRCS
 	../Benchmarks/BenchmarkDemo.cpp
 	../Benchmarks/BenchmarkDemo.h
 	../Benchmarks/landscapeData.h
-	../Benchmarks/TaruData
+	../Benchmarks/TaruData.h
 	../Raycast/RaytestDemo.cpp
 		../Importers/ImportBsp/BspConverter.h
  	../Importers/ImportBullet/SerializeSetup.cpp

examples/ExampleBrowser/OpenGLExampleBrowser.cpp

@@ -885,6 +885,20 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
 	sUseOpenGL2 = args.CheckCmdLineFlag("opengl2");
 	args.GetCmdLineArgument("render_device", gRenderDevice);
 	args.GetCmdLineArgument("window_backend", gWindowBackend);
+
+	int max_num_object_capacity = 128 * 1024;
+	int max_shape_capacity_in_bytes = 128 * 1024 * 1024;
+	if (args.CheckCmdLineFlag("max_num_object_capacity"))
+	{
+		args.GetCmdLineArgument("max_num_object_capacity", max_num_object_capacity);
+	}
+
+	if (args.CheckCmdLineFlag("max_shape_capacity_in_bytes"))
+	{
+		args.GetCmdLineArgument("max_shape_capacity_in_bytes", max_shape_capacity_in_bytes);
+	}
+
+
 #else
 	sUseOpenGL2 = true;
 #endif
@@ -914,7 +928,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
 	{
 		char title[1024];
 		sprintf(title, "%s using OpenGL3+ %s %s", appTitle, glContext, optMode);
-		simpleApp = new SimpleOpenGL3App(title, width, height, gAllowRetina, gWindowBackend, gRenderDevice);
+		simpleApp = new SimpleOpenGL3App(title, width, height, gAllowRetina, gWindowBackend, gRenderDevice, max_num_object_capacity, max_shape_capacity_in_bytes);
 		s_app = simpleApp;
 	}
 #endif

examples/ExtendedTutorials/CompoundBoxes.cpp

@@ -101,7 +101,7 @@ void CompoundBoxesExample::initPhysics()
 #else
 		// recompute the shift to make sure the compound shape is re-aligned
 		for (int i = 0; i < compoundShape->getNumChildShapes(); i++)
-			compound2->addChildShape(compoundShape->getChildTransform(i) * principal.inverse(),
+			compound2->addChildShape(principal.inverse() * compoundShape->getChildTransform(i),
 									 compoundShape->getChildShape(i));
 #endif
 		delete compoundShape;

examples/OpenGLWindow/EGLOpenGLWindow.cpp

@@ -248,8 +248,20 @@ void EGLOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
 		exit(EXIT_FAILURE);
 	}
 
+	EGLint egl_context_attribs[] = {
+		EGL_CONTEXT_MAJOR_VERSION,
+		3,
+		EGL_CONTEXT_MINOR_VERSION,
+		3,
+		EGL_CONTEXT_OPENGL_PROFILE_MASK,
+		EGL_CONTEXT_OPENGL_CORE_PROFILE_BIT,
+		EGL_CONTEXT_OPENGL_FORWARD_COMPATIBLE,
+		EGL_TRUE,
+		EGL_NONE,
+	};
+
 	m_data->egl_context = eglCreateContext(
-		m_data->egl_display, m_data->egl_config, EGL_NO_CONTEXT, NULL);
+		m_data->egl_display, m_data->egl_config, EGL_NO_CONTEXT, egl_context_attribs);
 	if (!m_data->egl_context)
 	{
 		fprintf(stderr, "Unable to create EGL context (eglError: %d)\n", eglGetError());

examples/SharedMemory/PhysicsClient.h

@@ -67,6 +67,8 @@ public:
 
 	virtual void getCachedMeshData(struct b3MeshData* meshData) = 0;
 
+	virtual void getCachedTetraMeshData(struct b3TetraMeshData* meshData) = 0;
+
 	virtual void getCachedVREvents(struct b3VREventsData* vrEventsData) = 0;
 
 	virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData) = 0;

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -1525,6 +1525,24 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3GetMeshDataCommandInit(b3PhysicsClie
 	return 0;
 }
 
+B3_SHARED_API b3SharedMemoryCommandHandle b3GetTetraMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+	if (cl)
+	{
+		struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+		b3Assert(command);
+		command->m_type = CMD_REQUEST_TETRA_MESH_DATA;
+		command->m_updateFlags = 0;
+		command->m_resetTetraMeshDataArgs.m_startingVertex = 0;
+		command->m_resetTetraMeshDataArgs.m_bodyUniqueId = bodyUniqueId;
+		return (b3SharedMemoryCommandHandle)command;
+	}
+	return 0;
+}
+
 B3_SHARED_API void b3GetMeshDataSetCollisionShapeIndex(b3SharedMemoryCommandHandle commandHandle, int shapeIndex)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
@@ -1549,6 +1567,18 @@ B3_SHARED_API void b3GetMeshDataSetFlags(b3SharedMemoryCommandHandle commandHand
 	}
 }
 
+B3_SHARED_API void b3GetTetraMeshDataSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command);
+	b3Assert(command->m_type == CMD_REQUEST_TETRA_MESH_DATA);
+	if (command->m_type == CMD_REQUEST_TETRA_MESH_DATA)
+	{
+		command->m_updateFlags = B3_TETRA_MESH_DATA_FLAGS;
+		command->m_requestMeshDataArgs.m_flags = flags;
+	}
+}
+
 B3_SHARED_API void b3GetMeshDataSimulationMesh(b3SharedMemoryCommandHandle commandHandle)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
@@ -1574,6 +1604,15 @@ B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3Mesh
 	}
 }
 
+B3_SHARED_API void b3GetTetraMeshData(b3PhysicsClientHandle physClient, struct b3TetraMeshData* meshData)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	if (cl)
+	{
+		cl->getCachedTetraMeshData(meshData);
+	}
+}
+
 B3_SHARED_API int b3CreateVisualShapeAddSphere(b3SharedMemoryCommandHandle commandHandle, double radius)
 {
 	return b3CreateCollisionShapeAddSphere(commandHandle, radius);

examples/SharedMemory/PhysicsClientC_API.h

@@ -533,12 +533,15 @@ extern "C"
 	B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveCollisionShapeCommand(b3PhysicsClientHandle physClient, int collisionShapeId);
 
 	B3_SHARED_API b3SharedMemoryCommandHandle b3GetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex);
+	B3_SHARED_API b3SharedMemoryCommandHandle b3GetTetraMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
 	B3_SHARED_API void b3GetMeshDataSimulationMesh(b3SharedMemoryCommandHandle commandHandle);
 	B3_SHARED_API void b3MeshDataSimulationMeshVelocity(b3SharedMemoryCommandHandle commandHandle);
 	B3_SHARED_API void b3GetMeshDataSetCollisionShapeIndex(b3SharedMemoryCommandHandle commandHandle, int shapeIndex);
 	B3_SHARED_API void b3GetMeshDataSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
+	B3_SHARED_API void b3GetTetraMeshDataSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
 	
 	B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3MeshData* meshData);
+	B3_SHARED_API void b3GetTetraMeshData(b3PhysicsClientHandle physClient, struct b3TetraMeshData* meshData);
 
 	B3_SHARED_API b3SharedMemoryCommandHandle b3ResetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int num_vertices, const double* vertices);
 

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -54,6 +54,7 @@ struct PhysicsClientSharedMemoryInternalData
 	btAlignedObjectArray<b3CollisionShapeData> m_cachedCollisionShapes;
 
 	b3MeshData m_cachedMeshData;
+	b3TetraMeshData m_cachedTetraMeshData;
 	btAlignedObjectArray<b3MeshVertex> m_cachedVertexPositions;
 
 	btAlignedObjectArray<b3VRControllerEvent> m_cachedVREvents;
@@ -1074,6 +1075,24 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
 				b3Warning("Request mesh data failed");
 				break;
 			}
+			case CMD_REQUEST_TETRA_MESH_DATA_COMPLETED:
+			{
+				m_data->m_cachedVertexPositions.resize(serverCmd.m_sendMeshDataArgs.m_startingVertex + serverCmd.m_sendMeshDataArgs.m_numVerticesCopied);
+				btVector3* verticesReceived = (btVector3*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
+				for (int i = 0; i < serverCmd.m_sendMeshDataArgs.m_numVerticesCopied; i++)
+				{
+					m_data->m_cachedVertexPositions[i + serverCmd.m_sendMeshDataArgs.m_startingVertex].x = verticesReceived[i].x();
+					m_data->m_cachedVertexPositions[i + serverCmd.m_sendMeshDataArgs.m_startingVertex].y = verticesReceived[i].y();
+					m_data->m_cachedVertexPositions[i + serverCmd.m_sendMeshDataArgs.m_startingVertex].z = verticesReceived[i].z();
+					m_data->m_cachedVertexPositions[i + serverCmd.m_sendMeshDataArgs.m_startingVertex].w = verticesReceived[i].w();
+				}
+				break;
+			}
+			case CMD_REQUEST_TETRA_MESH_DATA_FAILED:
+			{
+				b3Warning("Request tetra mesh data failed");
+				break;
+			}
 			case CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED:
 			{
 				break;
@@ -2088,6 +2107,15 @@ void PhysicsClientSharedMemory::getCachedMeshData(struct b3MeshData* meshData)
 	*meshData = m_data->m_cachedMeshData;
 }
 
+void PhysicsClientSharedMemory::getCachedTetraMeshData(struct b3TetraMeshData* meshData)
+{
+	m_data->m_cachedTetraMeshData.m_numVertices = m_data->m_cachedVertexPositions.size();
+	
+	m_data->m_cachedTetraMeshData.m_vertices = m_data->m_cachedTetraMeshData.m_numVertices ? &m_data->m_cachedVertexPositions[0] : 0;
+	
+	*meshData = m_data->m_cachedTetraMeshData;
+}
+
 const float* PhysicsClientSharedMemory::getDebugLinesFrom() const
 {
 	if (m_data->m_debugLinesFrom.size())

examples/SharedMemory/PhysicsClientSharedMemory.h

@@ -80,6 +80,8 @@ public:
 
 	virtual void getCachedMeshData(struct b3MeshData* meshData);
 
+	virtual void getCachedTetraMeshData(struct b3TetraMeshData* meshData);
+
 	virtual void getCachedVREvents(struct b3VREventsData* vrEventsData);
 
 	virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData);

examples/SharedMemory/PhysicsDirect.cpp

@@ -67,6 +67,7 @@ struct PhysicsDirectInternalData
 	btAlignedObjectArray<b3CollisionShapeData> m_cachedCollisionShapes;
 
 	b3MeshData m_cachedMeshData;
+	b3TetraMeshData m_cachedTetraMeshData;
 	btAlignedObjectArray<b3MeshVertex> m_cachedVertexPositions;
 
 	btAlignedObjectArray<b3VRControllerEvent> m_cachedVREvents;
@@ -99,6 +100,7 @@ struct PhysicsDirectInternalData
 		  m_timeOutInSeconds(1e30)
 	{
 		memset(&m_cachedMeshData.m_numVertices, 0, sizeof(b3MeshData));
+		memset(&m_cachedTetraMeshData.m_numVertices, 0, sizeof(b3TetraMeshData));
 		memset(&m_command, 0, sizeof(m_command));
 		memset(&m_serverStatus, 0, sizeof(m_serverStatus));
 		memset(m_bulletStreamDataServerToClient, 0, sizeof(m_bulletStreamDataServerToClient));
@@ -1691,6 +1693,16 @@ void PhysicsDirect::getCachedMeshData(struct b3MeshData* meshData)
 	*meshData = m_data->m_cachedMeshData;
 }
 
+void PhysicsDirect::getCachedTetraMeshData(struct b3TetraMeshData* meshData)
+{
+	m_data->m_cachedTetraMeshData.m_numVertices = m_data->m_cachedVertexPositions.size();
+
+	m_data->m_cachedTetraMeshData.m_vertices = m_data->m_cachedTetraMeshData.m_numVertices ? &m_data->m_cachedVertexPositions[0] : 0;
+
+	*meshData = m_data->m_cachedTetraMeshData;
+}
+
+
 void PhysicsDirect::getCachedContactPointInformation(struct b3ContactInformation* contactPointData)
 {
 	contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size();

examples/SharedMemory/PhysicsDirect.h

@@ -106,6 +106,8 @@ public:
 
 	virtual void getCachedMeshData(struct b3MeshData* meshData);
 
+	virtual void getCachedTetraMeshData(struct b3TetraMeshData* meshData);
+
 	virtual void getCachedVREvents(struct b3VREventsData* vrEventsData);
 
 	virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData);

examples/SharedMemory/PhysicsLoopBack.cpp

@@ -183,6 +183,11 @@ void PhysicsLoopBack::getCachedMeshData(struct b3MeshData* meshData)
 	return m_data->m_physicsClient->getCachedMeshData(meshData);
 }
 
+void PhysicsLoopBack::getCachedTetraMeshData(struct b3TetraMeshData* meshData)
+{
+	return m_data->m_physicsClient->getCachedTetraMeshData(meshData);
+}
+
 void PhysicsLoopBack::getCachedContactPointInformation(struct b3ContactInformation* contactPointData)
 {
 	return m_data->m_physicsClient->getCachedContactPointInformation(contactPointData);

examples/SharedMemory/PhysicsLoopBack.h

@@ -78,6 +78,8 @@ public:
 
 	virtual void getCachedMeshData(struct b3MeshData* meshData);
 
+	virtual void getCachedTetraMeshData(struct b3TetraMeshData* meshData);
+
 	virtual void getCachedVREvents(struct b3VREventsData* vrEventsData);
 
 	virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -2802,7 +2802,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld(int flags)
 	m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = 0.2;  //need to check if there are artifacts with frictionERP
 	m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
 	m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
-	if (flags & RESET_USE_REDUCED_DEFORMABLE_WORLD) 
+	if (flags & RESET_USE_REDUCED_DEFORMABLE_WORLD)
 	{
 		m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 128;
 	}
@@ -5677,7 +5677,8 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
 				separateRenderMesh = (psb->m_renderNodes.size() != 0);
 			}
             bool requestVelocity = clientCmd.m_updateFlags & B3_MESH_DATA_SIMULATION_MESH_VELOCITY;
-            
+
+			
 			int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size();
 			int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1;
 			int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex;
@@ -5704,6 +5705,8 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
                     }
 				}
 			}
+			
+
 			sizeInBytes = verticesCopied * sizeof(btVector3);
 			serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED;
 			serverStatusOut.m_sendMeshDataArgs.m_numVerticesCopied = verticesCopied;
@@ -5718,6 +5721,51 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
 	return hasStatus;
 }
 
+bool PhysicsServerCommandProcessor::processRequestTetraMeshDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
+{
+	bool hasStatus = true;
+	BT_PROFILE("CMD_REQUEST_TETRA_MESH_DATA");
+	serverStatusOut.m_type = CMD_REQUEST_TETRA_MESH_DATA_FAILED;
+	serverStatusOut.m_numDataStreamBytes = 0;
+	int sizeInBytes = 0;
+
+	InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_resetTetraMeshDataArgs.m_bodyUniqueId);
+	if (bodyHandle)
+	{
+		int totalBytesPerVertex = sizeof(btVector3);
+		btVector3* verticesOut = (btVector3*)bufferServerToClient;
+		const btCollisionShape* colShape = 0;
+
+		if (bodyHandle->m_softBody)
+		{
+			btSoftBody* psb = bodyHandle->m_softBody;
+			
+			int numTetra = psb->m_tetras.size();
+			int maxNumnumVertecies = bufferSizeInBytes / totalBytesPerVertex - 1;
+			int numVerticesRemaining = numTetra * 4;
+			int verticesCopied = btMin(maxNumnumVertecies, numVerticesRemaining);
+			for (int i = 0; i < verticesCopied; i += 4)
+			{
+				const btSoftBody::Tetra& n = psb->m_tetras[i/4];
+				verticesOut[i].setValue(n.m_n[0]->m_x.x(), n.m_n[0]->m_x.y(), n.m_n[0]->m_x.z());
+				verticesOut[i+1].setValue(n.m_n[1]->m_x.x(), n.m_n[1]->m_x.y(), n.m_n[1]->m_x.z());
+				verticesOut[i+2].setValue(n.m_n[2]->m_x.x(), n.m_n[2]->m_x.y(), n.m_n[2]->m_x.z());
+				verticesOut[i+3].setValue(n.m_n[3]->m_x.x(), n.m_n[3]->m_x.y(), n.m_n[3]->m_x.z());
+			}
+
+			sizeInBytes = verticesCopied * sizeof(btVector3);
+			serverStatusOut.m_type = CMD_REQUEST_TETRA_MESH_DATA_COMPLETED;
+			serverStatusOut.m_sendMeshDataArgs.m_numVerticesCopied = verticesCopied;
+			serverStatusOut.m_sendMeshDataArgs.m_startingVertex = clientCmd.m_requestMeshDataArgs.m_startingVertex;
+			serverStatusOut.m_sendMeshDataArgs.m_numVerticesRemaining = numVerticesRemaining - verticesCopied;
+		}
+	}
+
+	serverStatusOut.m_numDataStreamBytes = sizeInBytes;
+
+	return hasStatus;
+}
+
 bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
 {
 	bool hasStatus = true;
@@ -8187,7 +8235,7 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
 }
 
 bool RequestFiltered(const struct SharedMemoryCommand& clientCmd, int& linkIndexA, int& linkIndexB, int& objectIndexA, int& objectIndexB, bool& swap){
-   
+
     if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0)
     {
         if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexA)
@@ -8232,7 +8280,7 @@ bool RequestFiltered(const struct SharedMemoryCommand& clientCmd, int& linkIndex
     {
         return true;
     }
-    
+
     return false;
 }
 
@@ -8313,21 +8361,26 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c
 	{
 		return false;
 	}
-	int numSoftbodyContact = 0;
-	for (int i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
-	{
-		numSoftbodyContact += deformWorld->getSoftBodyArray()[i]->m_faceRigidContacts.size();
-	}
-	int num_contact_points = m_data->m_cachedContactPoints.size();
-	m_data->m_cachedContactPoints.reserve(num_contact_points + numSoftbodyContact);
 
 	for (int i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
 	{
 		btSoftBody* psb = deformWorld->getSoftBodyArray()[i];
+		btAlignedObjectArray<b3ContactPointData> distinctContactPoints;
+		btAlignedObjectArray<btSoftBody::Node*> nodesInContact;
 		for (int c = 0; c < psb->m_faceRigidContacts.size(); c++)
 		{
 			const btSoftBody::DeformableFaceRigidContact* contact = &psb->m_faceRigidContacts[c];
-			//convert rigidbody contact
+			// calculate normal and tangent impulse
+			btVector3 impulse = contact->m_cti.m_impulse;
+			btVector3 impulseNormal = impulse.dot(contact->m_cti.m_normal) * contact->m_cti.m_normal;
+			btVector3 impulseTangent = impulse - impulseNormal;
+			// get node in contact
+			int contactNodeIdx = contact->m_bary.maxAxis();
+			btSoftBody::Node* node = contact->m_face->m_n[contactNodeIdx];
+			// check if node is already in the list
+			int idx = nodesInContact.findLinearSearch2(node);
+
+			//apply the filter, if the user provides it
 			int linkIndexA = -1;
 			int linkIndexB = -1;
 			int objectIndexA = psb->getUserIndex2();
@@ -8344,8 +8397,6 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c
 				linkIndexB = mblB->m_link;
 				objectIndexB = mblB->m_multiBody->getUserIndex2();
 			}
-
-			//apply the filter, if the user provides it
 			bool swap = false;
 			if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0)
 			{
@@ -8391,37 +8442,87 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c
 			{
 				continue;
 			}
-			b3ContactPointData pt;
-			pt.m_bodyUniqueIdA = objectIndexA;
-			pt.m_bodyUniqueIdB = objectIndexB;
-			pt.m_contactDistance = contact->m_cti.m_offset;
-			pt.m_contactFlags = 0;
-			pt.m_linkIndexA = linkIndexA;
-			pt.m_linkIndexB = linkIndexB;
-			for (int j = 0; j < 3; j++)
+
+			if (idx < 0)
 			{
-				if (swap)
+				// add new node and contact point
+				nodesInContact.push_back(node);
+				b3ContactPointData pt;
+				pt.m_bodyUniqueIdA = objectIndexA;
+				pt.m_bodyUniqueIdB = objectIndexB;
+				pt.m_contactDistance = -contact->m_cti.m_offset;
+				pt.m_contactFlags = 0;
+				pt.m_linkIndexA = linkIndexA;
+				pt.m_linkIndexB = linkIndexB;
+				for (int j = 0; j < 3; j++)
 				{
-					pt.m_contactNormalOnBInWS[j] = -contact->m_cti.m_normal[j];
-					pt.m_positionOnAInWS[j] = contact->m_cti.m_normal[j];
-					pt.m_positionOnBInWS[j] = -contact->m_cti.m_normal[j];
+					if (swap)
+					{
+						pt.m_contactNormalOnBInWS[j] = -contact->m_cti.m_normal[j];
+						pt.m_positionOnAInWS[j] = node->m_x[j] - pt.m_contactDistance * pt.m_contactNormalOnBInWS[j]; // not really precise because of margins in btSoftBody.cpp:line 2912
+						// node is force application point, therefore node position is contact point (not contact->m_contactPoint, because not equal to node)
+						pt.m_positionOnBInWS[j] = node->m_x[j];
+					}
+					else
+					{
+						pt.m_contactNormalOnBInWS[j] = contact->m_cti.m_normal[j];
+						// node is force application point, therefore node position is contact point (not contact->m_contactPoint, because not equal to node)
+						pt.m_positionOnAInWS[j] = node->m_x[j];
+						pt.m_positionOnBInWS[j] = node->m_x[j] - pt.m_contactDistance * pt.m_contactNormalOnBInWS[j]; // not really precise because of margins in btSoftBody.cpp:line 2912
+					}
 				}
-				else
+				pt.m_normalForce = (impulseNormal / m_data->m_physicsDeltaTime).norm();
+				pt.m_linearFrictionForce1 = (impulseTangent.dot(contact->t1) * contact->t1 / m_data->m_physicsDeltaTime).norm();
+				pt.m_linearFrictionForce2 = (impulseTangent.dot(contact->t2) * contact->t2 / m_data->m_physicsDeltaTime).norm();
+				for (int j = 0; j < 3; j++)
 				{
-					pt.m_contactNormalOnBInWS[j] = contact->m_cti.m_normal[j];
-					pt.m_positionOnAInWS[j] = -contact->m_cti.m_normal[j];
-					pt.m_positionOnBInWS[j] = contact->m_cti.m_normal[j];
+					pt.m_linearFrictionDirection1[j] = contact->t1[j];
+					pt.m_linearFrictionDirection2[j] = contact->t2[j];
 				}
+				distinctContactPoints.push_back(pt);
 			}
-			pt.m_normalForce = 1;
-			pt.m_linearFrictionForce1 = 0;
-			pt.m_linearFrictionForce2 = 0;
-			for (int j = 0; j < 3; j++)
+			else
 			{
-				pt.m_linearFrictionDirection1[j] = 0;
-				pt.m_linearFrictionDirection2[j] = 0;
+				// add values to existing contact point
+				b3ContactPointData* pt = &distinctContactPoints[idx];
+				// current normal force of node
+				btVector3 normalForce = btVector3(btScalar(pt->m_contactNormalOnBInWS[0]),
+												  btScalar(pt->m_contactNormalOnBInWS[1]),
+												  btScalar(pt->m_contactNormalOnBInWS[2])) * pt->m_normalForce;
+				// add normal force of additional node contact
+				btScalar swapFactor = swap ? -1.0 : 1.0;
+				normalForce += swapFactor * contact->m_cti.m_normal * (impulseNormal / m_data->m_physicsDeltaTime).norm();
+				// get magnitude of normal force
+				pt->m_normalForce = normalForce.norm();
+				// get direction of normal force
+				if (!normalForce.fuzzyZero())
+				{
+					// normalize for unit vectors if above numerical threshold
+					normalForce.normalize();
+					for (int j = 0; j < 3; j++)
+					{
+						pt->m_contactNormalOnBInWS[j] = normalForce[j];
+					}
+				}
+
+				// add magnitudes of tangential forces in existing directions
+				btVector3 linearFrictionDirection1 = btVector3(btScalar(pt->m_linearFrictionDirection1[0]),
+															   btScalar(pt->m_linearFrictionDirection1[1]),
+															   btScalar(pt->m_linearFrictionDirection1[2]));
+				btVector3 linearFrictionDirection2 = btVector3(btScalar(pt->m_linearFrictionDirection2[0]),
+															   btScalar(pt->m_linearFrictionDirection2[1]),
+															   btScalar(pt->m_linearFrictionDirection2[2]));
+				pt->m_linearFrictionForce1 = (impulseTangent.dot(linearFrictionDirection1) * linearFrictionDirection1 / m_data->m_physicsDeltaTime).norm();
+				pt->m_linearFrictionForce2 = (impulseTangent.dot(linearFrictionDirection2) * linearFrictionDirection2 / m_data->m_physicsDeltaTime).norm();
 			}
-			m_data->m_cachedContactPoints.push_back(pt);
+		}
+
+		int num_contact_points = m_data->m_cachedContactPoints.size() + distinctContactPoints.size();
+		m_data->m_cachedContactPoints.reserve(num_contact_points);
+		// add points to contact points cache
+		for (int p = 0; p < distinctContactPoints.size(); p++)
+		{
+			m_data->m_cachedContactPoints.push_back(distinctContactPoints[p]);
 		}
 	}
 #endif
@@ -9929,7 +10030,7 @@ bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDe
 				softWorld->addSoftBody(rsb);
 			}
 		}
-		
+
 		*bodyUniqueId = m_data->m_bodyHandles.allocHandle();
 		InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(*bodyUniqueId);
 		bodyHandle->m_softBody = rsb;
@@ -9980,7 +10081,7 @@ bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDe
 			float scaling[4] = { 1,1,1,1 };
  			int instanceUid = m_data->m_guiHelper->registerGraphicsInstance(shapeUid1, position, orientation, color, scaling);
 			rsb->setUserIndex(instanceUid);
-			
+
 			if (m_data->m_enableTinyRenderer)
 			{
 				int texUid2 = m_data->m_pluginManager.getRenderInterface()->registerTexture(meshData.m_textureImage1, meshData.m_textureWidth, meshData.m_textureHeight);
@@ -10080,7 +10181,7 @@ bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDe
 				}
 			}
 		}
-		
+
 
 
 		btAlignedObjectArray<btVector3> vertices;
@@ -10093,7 +10194,7 @@ bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDe
 
 			for (int i = 0; i < rsb->m_faces.size(); i++)  // Foreach face
 			{
-				
+
 				for (int k = 0; k < 3; k++)  // Foreach vertex on a face
 				{
 					int currentIndex = i * 3 + k;
@@ -12259,9 +12360,10 @@ bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct S
 					q[4] = pos[1];
 					q[5] = pos[2];
 				}
+				int quatToEulerCorrection =  baseDofQ ? -1 : 0; // The conversion to Euler angles drops the DOF by one
 				for (int i = 0; i < num_dofs; i++)
 				{
-					q[i + baseDofQ] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i + baseDofQ];
+					q[i + baseDofQ + quatToEulerCorrection] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i + baseDofQ];
 				}
 				for (int i = 0; i < num_dofs + baseDofQdot; i++)
 				{
@@ -15067,6 +15169,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 			hasStatus = processRequestMeshDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
 			break;
 		}
+		case CMD_REQUEST_TETRA_MESH_DATA:
+		{
+			hasStatus = processRequestTetraMeshDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
+			break;
+		}
 		case CMD_RESET_MESH_DATA:
 		{
 			hasStatus = processResetMeshDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);

examples/SharedMemory/PhysicsServerCommandProcessor.h

@@ -32,6 +32,7 @@ protected:
 	bool processCreateCollisionShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processCreateVisualShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processRequestMeshDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
+	bool processRequestTetraMeshDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processResetMeshDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processCustomCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processUserDebugDrawCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);

examples/SharedMemory/PhysicsServerExample.cpp

@@ -2799,6 +2799,20 @@ void PhysicsServerExample::stepSimulation(float deltaTime)
 			}
 		}
 	}
+
+	for (int i = m_multiThreadedHelper->m_userDebugPoints.size() - 1; i >= 0; i--)
+	{
+		if (m_multiThreadedHelper->m_userDebugPoints[i].m_lifeTime)
+		{
+			m_multiThreadedHelper->m_userDebugPoints[i].m_lifeTime -= deltaTime;
+			if (m_multiThreadedHelper->m_userDebugPoints[i].m_lifeTime <= 0)
+			{
+				m_multiThreadedHelper->m_userDebugPoints.swap(i, m_multiThreadedHelper->m_userDebugPoints.size() - 1);
+				m_multiThreadedHelper->m_userDebugPoints.pop_back();
+			}
+		}
+	}
+
 	updateGraphics();
 
 

examples/SharedMemory/SharedMemoryCommands.h

@@ -1144,6 +1144,13 @@ struct b3RequestMeshDataArgs
 	int m_flags;
 };
 
+struct b3RequestTetraMeshDataArgs
+{
+	int m_bodyUniqueId;
+	int m_startingVertex;
+	int m_flags;
+};
+
 struct b3ResetMeshDataArgs
 {
 	int m_bodyUniqueId;
@@ -1219,8 +1226,9 @@ struct SharedMemoryCommand
 		struct UserDataRequestArgs m_removeUserDataRequestArgs;
 		struct b3CollisionFilterArgs m_collisionFilterArgs;
 		struct b3RequestMeshDataArgs m_requestMeshDataArgs;
+		struct b3RequestTetraMeshDataArgs m_requestTetraMeshDataArgs;
 		struct b3ResetMeshDataArgs m_resetMeshDataArgs;
-
+		struct b3RequestTetraMeshDataArgs m_resetTetraMeshDataArgs;
 	};
 };
 

examples/SharedMemory/SharedMemoryPublic.h

@@ -118,6 +118,8 @@ enum EnumSharedMemoryClientCommand
 	
 	CMD_PERFORM_COLLISION_DETECTION,
 	CMD_RESET_MESH_DATA,
+
+	CMD_REQUEST_TETRA_MESH_DATA,
 	//don't go beyond this command!
 	CMD_MAX_CLIENT_COMMANDS,
 };
@@ -244,6 +246,9 @@ enum EnumSharedMemoryServerStatus
 	CMD_PERFORM_COLLISION_DETECTION_COMPLETED,
 	CMD_RESET_MESH_DATA_COMPLETED,
 	CMD_RESET_MESH_DATA_FAILED,
+
+	CMD_REQUEST_TETRA_MESH_DATA_COMPLETED,
+	CMD_REQUEST_TETRA_MESH_DATA_FAILED,
 	//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
 	CMD_MAX_SERVER_COMMANDS
 };
@@ -481,6 +486,17 @@ struct b3MeshData
 	struct b3MeshVertex* m_vertices;
 };
 
+enum eTetraMeshDataEnum
+{
+	B3_TETRA_MESH_DATA_FLAGS=2,
+};
+
+struct b3TetraMeshData
+{
+	int m_numVertices;
+	struct b3MeshVertex* m_vertices;
+};
+
 struct b3OpenGLVisualizerCameraInfo
 {
 	int m_width;

examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_AntBulletEnv_v0_2017may.py

@@ -54,7 +54,7 @@ def main():
       frame += 1
       distance = 5
       yaw = 0
-      still_open = env.render("human")
+      still_open = env.render(mode="human")
       if still_open == False:
         return
       if not done: continue

examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HalfCheetahBulletEnv_v0_2017may.py

@@ -49,7 +49,7 @@ def main():
       obs, r, done, _ = env.step(a)
       score += r
       frame += 1
-      still_open = env.render("human")
+      still_open = env.render(mode="human")
       if still_open == False:
         return
       if not done: continue

examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidBulletEnv_v0_2017may.py

@@ -48,7 +48,7 @@ def main():
       obs, r, done, _ = env.step(a)
       score += r
       frame += 1
-      still_open = env.render("human")
+      still_open = env.render(mode="human")
       if still_open == False:
         return
       continue

examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_HumanoidFlagrunHarderBulletEnv_v1_2017jul.py

@@ -53,7 +53,7 @@ def demo_run():
       score += r
       frame += 1
 
-      still_open = env.render("human")
+      still_open = env.render(mode="human")
 
       if still_open == False:
         return

examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedDoublePendulumBulletEnv_v0_2017may.py

@@ -48,7 +48,7 @@ def main():
       obs, r, done, _ = env.step(a)
       score += r
       frame += 1
-      still_open = env.render("human")
+      still_open = env.render(mode="human")
       if still_open == False:
         return
       if not done: continue

examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedPendulumBulletEnv_v0_2017may.py

@@ -48,7 +48,7 @@ def main():
       obs, r, done, _ = env.step(a)
       score += r
       frame += 1
-      still_open = env.render("human")
+      still_open = env.render(mode="human")
       if still_open == False:
         return
       if not done: continue

examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_InvertedPendulumSwingupBulletEnv_v0_2017may.py

@@ -48,7 +48,7 @@ def main():
       obs, r, done, _ = env.step(a)
       score += r
       frame += 1
-      still_open = env.render("human")
+      still_open = env.render(mode="human")
       if still_open == False:
         return
       if not done: continue

examples/pybullet/gym/pybullet_envs/examples/enjoy_TF_Walker2DBulletEnv_v0_2017may.py

@@ -50,7 +50,7 @@ def main():
       score += r
       frame += 1
 
-      still_open = env.render("human")
+      still_open = env.render(mode="human")
       if still_open == False:
         return
       if not done: continue

examples/pybullet/gym/pybullet_envs/examples/testEnv.py

@@ -44,7 +44,6 @@ def test(args):
   print("action space:")
   sample = env.action_space.sample()
   action = sample * 0.0
-  action = [0,0]#sample * 0.0
   
   print("action=")
   print(action)

examples/pybullet/pybullet.c

@@ -9190,6 +9190,64 @@ static PyObject* pybullet_getMeshData(PyObject* self, PyObject* args, PyObject*
 }
 
 
+static PyObject* pybullet_getTetraMeshData(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	int bodyUniqueId = -1;
+	b3PhysicsClientHandle sm = 0;
+	b3SharedMemoryCommandHandle command;
+	b3SharedMemoryStatusHandle statusHandle;
+	struct b3TetraMeshData meshData;
+	int statusType;
+	int flags = -1;
+
+	int physicsClientId = 0;
+	static char* kwlist[] = {"bodyUniqueId",  "flags", "physicsClientId", NULL};
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|iii", kwlist, &bodyUniqueId,  &flags , &physicsClientId))
+	{
+		return NULL;
+	}
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0)
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+	command = b3GetTetraMeshDataCommandInit(sm, bodyUniqueId);
+	if (flags >= 0)
+	{
+		b3GetTetraMeshDataSetFlags(command, flags);
+	}
+
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	statusType = b3GetStatusType(statusHandle);
+	if (statusType == CMD_REQUEST_TETRA_MESH_DATA_COMPLETED)
+	{
+		int i;
+		PyObject* pyVertexData;
+		PyObject* pyListMeshData = PyTuple_New(2);
+		b3GetTetraMeshData(sm, &meshData);
+		PyTuple_SetItem(pyListMeshData, 0, PyInt_FromLong(meshData.m_numVertices));
+		pyVertexData = PyTuple_New(meshData.m_numVertices);
+		PyTuple_SetItem(pyListMeshData, 1, pyVertexData);
+
+		for (i = 0; i < meshData.m_numVertices; i++)
+		{
+			PyObject* pyListVertex = PyTuple_New(3);
+			PyTuple_SetItem(pyListVertex, 0, PyFloat_FromDouble(meshData.m_vertices[i].x));
+			PyTuple_SetItem(pyListVertex, 1, PyFloat_FromDouble(meshData.m_vertices[i].y));
+			PyTuple_SetItem(pyListVertex, 2, PyFloat_FromDouble(meshData.m_vertices[i].z));
+			PyTuple_SetItem(pyVertexData, i, pyListVertex);
+		}
+
+		return pyListMeshData;
+	}
+
+	PyErr_SetString(SpamError, "getTetraMeshData failed");
+	return NULL;
+}
+
+
+
 static PyObject* pybullet_resetMeshData(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	int bodyUniqueId = -1;
@@ -12644,6 +12702,9 @@ static PyMethodDef SpamMethods[] = {
 	{"getMeshData", (PyCFunction)pybullet_getMeshData, METH_VARARGS | METH_KEYWORDS,
 	 "Get mesh data. Returns vertices etc from the mesh."},
 
+	{"getTetraMeshData", (PyCFunction)pybullet_getTetraMeshData, METH_VARARGS | METH_KEYWORDS,
+	 "Get mesh data. Returns tetra from the mesh."},
+
 	{"resetMeshData", (PyCFunction)pybullet_resetMeshData, METH_VARARGS | METH_KEYWORDS,
 	 "Reset mesh data. Only implemented for deformable bodies."},
 

pyproject.toml

@@ -0,0 +1,5 @@
+# pyproject.toml
+[build-system]
+requires = ["setuptools >= 42.0.0"]
+build-backend = "setuptools.build_meta"
+

requirements.txt

@@ -0,0 +1,2 @@
+numpy
+

setup.py

@@ -505,7 +505,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS:
 
 setup(
     name='pybullet',
-    version='3.2.4',
+    version='3.2.7',
     description=
     'Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
     long_description=

src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h

@@ -47,8 +47,10 @@ class btSerializer;
 #define MAX_SUBTREE_SIZE_IN_BYTES 2048
 
 // 10 gives the potential for 1024 parts, with at most 2^21 (2097152) (minus one
+// 4 gives the potential for 16 parts, each with 2^27 triangles (134217728)
 // actually) triangles each (since the sign bit is reserved
-#define MAX_NUM_PARTS_IN_BITS 10
+//#define MAX_NUM_PARTS_IN_BITS 10
+#define MAX_NUM_PARTS_IN_BITS 4
 
 ///btQuantizedBvhNode is a compressed aabb node, 16 bytes.
 ///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).

src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp

@@ -103,7 +103,8 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle, int partId,
 
 	if (m_convexBodyWrap->getCollisionShape()->isConvex())
 	{
-#ifndef BT_DISABLE_CONVEX_CONCAVE_EARLY_OUT		
+#ifdef BT_ENABLE_CONVEX_CONCAVE_EARLY_OUT
+    //todo: check this issue https://github.com/bulletphysics/bullet3/issues/4263
 		//an early out optimisation if the object is separated from the triangle
 		//projected on the triangle normal)
 		{
@@ -139,7 +140,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle, int partId,
 			if (dist > contact_threshold)
 				return;
         }
-#endif //BT_DISABLE_CONVEX_CONCAVE_EARLY_OUT
+#endif //BT_ENABLE_CONVEX_CONCAVE_EARLY_OUT
 
 		btTriangleShape tm(triangle[0], triangle[1], triangle[2]);
 		tm.setMargin(m_collisionMarginTriangle);

src/BulletCollision/Gimpact/btBoxCollision.h

@@ -229,10 +229,12 @@ public:
 		m_min[0] = BT_MIN3(V1[0], V2[0], V3[0]);
 		m_min[1] = BT_MIN3(V1[1], V2[1], V3[1]);
 		m_min[2] = BT_MIN3(V1[2], V2[2], V3[2]);
+		m_min[3] = 0.f;
 
 		m_max[0] = BT_MAX3(V1[0], V2[0], V3[0]);
 		m_max[1] = BT_MAX3(V1[1], V2[1], V3[1]);
 		m_max[2] = BT_MAX3(V1[2], V2[2], V3[2]);
+		m_max[3] = 0.f;
 	}
 
 	btAABB(const btVector3 &V1,
@@ -243,10 +245,12 @@ public:
 		m_min[0] = BT_MIN3(V1[0], V2[0], V3[0]);
 		m_min[1] = BT_MIN3(V1[1], V2[1], V3[1]);
 		m_min[2] = BT_MIN3(V1[2], V2[2], V3[2]);
+		m_min[3] = 0.f;
 
 		m_max[0] = BT_MAX3(V1[0], V2[0], V3[0]);
 		m_max[1] = BT_MAX3(V1[1], V2[1], V3[1]);
 		m_max[2] = BT_MAX3(V1[2], V2[2], V3[2]);
+		m_max[3] = 0.f;
 
 		m_min[0] -= margin;
 		m_min[1] -= margin;
@@ -275,9 +279,11 @@ public:
 		m_min[0] = SIMD_INFINITY;
 		m_min[1] = SIMD_INFINITY;
 		m_min[2] = SIMD_INFINITY;
+		m_min[3] = 0.f;
 		m_max[0] = -SIMD_INFINITY;
 		m_max[1] = -SIMD_INFINITY;
 		m_max[2] = -SIMD_INFINITY;
+		m_max[3] = 0.f;
 	}
 
 	SIMD_FORCE_INLINE void increment_margin(btScalar margin)
@@ -295,10 +301,12 @@ public:
 		m_min[0] = other.m_min[0] - margin;
 		m_min[1] = other.m_min[1] - margin;
 		m_min[2] = other.m_min[2] - margin;
+		m_min[3] = 0.f;
 
 		m_max[0] = other.m_max[0] + margin;
 		m_max[1] = other.m_max[1] + margin;
 		m_max[2] = other.m_max[2] + margin;
+		m_max[3] = 0.f;
 	}
 
 	template <typename CLASS_POINT>
@@ -310,10 +318,12 @@ public:
 		m_min[0] = BT_MIN3(V1[0], V2[0], V3[0]);
 		m_min[1] = BT_MIN3(V1[1], V2[1], V3[1]);
 		m_min[2] = BT_MIN3(V1[2], V2[2], V3[2]);
+		m_min[3] = 0.f;
 
 		m_max[0] = BT_MAX3(V1[0], V2[0], V3[0]);
 		m_max[1] = BT_MAX3(V1[1], V2[1], V3[1]);
 		m_max[2] = BT_MAX3(V1[2], V2[2], V3[2]);
+		m_max[3] = 0.f;
 	}
 
 	template <typename CLASS_POINT>
@@ -325,10 +335,12 @@ public:
 		m_min[0] = BT_MIN3(V1[0], V2[0], V3[0]);
 		m_min[1] = BT_MIN3(V1[1], V2[1], V3[1]);
 		m_min[2] = BT_MIN3(V1[2], V2[2], V3[2]);
+		m_min[3] = 0.f;
 
 		m_max[0] = BT_MAX3(V1[0], V2[0], V3[0]);
 		m_max[1] = BT_MAX3(V1[1], V2[1], V3[1]);
 		m_max[2] = BT_MAX3(V1[2], V2[2], V3[2]);
+		m_max[3] = 0.f;
 
 		m_min[0] -= margin;
 		m_min[1] -= margin;

src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h

@@ -71,8 +71,8 @@ public:
 					const btVector3& normal,
 					btScalar distance) : m_localPointA(pointA),
 										 m_localPointB(pointB),
-                     m_positionWorldOnB(0,0,0),
-                     m_positionWorldOnA(0,0,0),
+										 m_positionWorldOnB(0,0,0),
+										 m_positionWorldOnA(0,0,0),
 										 m_normalWorldOnB(normal),
 										 m_distance1(distance),
 										 m_combinedFriction(btScalar(0.)),
@@ -95,8 +95,8 @@ public:
 										 m_contactERP(0.f),
 										 m_frictionCFM(0.f),
 										 m_lifeTime(0),
-            m_lateralFrictionDir1(0,0,0),
-            m_lateralFrictionDir2(0,0,0)
+										 m_lateralFrictionDir1(0,0,0),
+										 m_lateralFrictionDir2(0,0,0)
 	{
 	}
 

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -936,7 +936,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 			if (m_useGyroTerm)
 				zeroAccSpatFrc[i + 1].addAngular(spatVel[i + 1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular()));
 			//
-			zeroAccSpatFrc[i + 1].addLinear(m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear()));
+                        if (!isConstraintPass)
+      				zeroAccSpatFrc[i + 1].addLinear(m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear()));
 			//
 			//btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
 			////clamp parent's omega
@@ -983,7 +984,11 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 			//
 			hDof = spatInertia[i + 1] * m_links[i].m_axes[dof];
 			//
-			Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof] - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i + 1]) - spatCoriolisAcc[i].dot(hDof);
+			btScalar jointTorque = 0;
+			if (isConstraintPass) jointTorque = 0;
+			else jointTorque = m_links[i].m_jointTorque[dof];
+			Y[m_links[i].m_dofOffset + dof] = jointTorque - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i + 1]) - spatCoriolisAcc[i].dot(hDof);
+
 		}
 		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 		{

src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp

@@ -169,9 +169,12 @@ btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/)
 
 			/*the column becomes part of the basis*/
 			basis[pivotRowIndex] = pivotColIndexOld;
-
-			pivotRowIndex = findLexicographicMinimum(A, pivotColIndex);
-
+			bool isRayTermination = false;
+			pivotRowIndex = findLexicographicMinimum(A, pivotColIndex, z0Row, isRayTermination);
+			if (isRayTermination)
+			{
+				break; // ray termination
+			}
 			if (z0Row == pivotRowIndex)
 			{  //if z0 leaves the basis the solution is found --> one last elimination step is necessary
 				GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
@@ -217,79 +220,100 @@ btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/)
 	return solutionVector;
 }
 
-int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int& pivotColIndex)
+int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int& pivotColIndex, const int& z0Row, bool& isRayTermination)
 {
-	int RowIndex = 0;
+	isRayTermination = false;
+	btAlignedObjectArray<int> activeRows;
+
+        bool firstRow = true;
+	btScalar currentMin = 0.0;
+
 	int dim = A.rows();
-	btAlignedObjectArray<btVectorXu> Rows;
+
 	for (int row = 0; row < dim; row++)
 	{
-		btVectorXu vec(dim + 1);
-		vec.setZero();  //, INIT, 0.)
-		Rows.push_back(vec);
-		btScalar a = A(row, pivotColIndex);
-		if (a > 0)
-		{
-			Rows[row][0] = A(row, 2 * dim + 1) / a;
-			Rows[row][1] = A(row, 2 * dim) / a;
-			for (int j = 2; j < dim + 1; j++)
-				Rows[row][j] = A(row, j - 1) / a;
+		const btScalar denom = A(row, pivotColIndex);
 
-#ifdef BT_DEBUG_OSTREAM
-				//		if (DEBUGLEVEL) {
-				//	  cout << "Rows(" << row << ") = " << Rows[row] << endl;
-				// }
-#endif
+		if (denom > btMachEps())
+		{
+			const btScalar q = A(row, dim + dim + 1) / denom;
+			if (firstRow)
+			{
+				currentMin = q;
+				activeRows.push_back(row);
+				firstRow = false;
+			}
+			else if (fabs(currentMin - q) < btMachEps())
+			{
+				activeRows.push_back(row);
+			}
+			else if (currentMin > q)
+			{
+				currentMin = q;
+				activeRows.clear();
+				activeRows.push_back(row);
+			}
 		}
 	}
 
-	for (int i = 0; i < Rows.size(); i++)
+	if (activeRows.size() == 0)
 	{
-		if (Rows[i].nrm2() > 0.)
+		isRayTermination = true;
+		return 0;
+	}
+	else if (activeRows.size() == 1)
+	{
+		return activeRows[0];
+	}
+
+	// if there are multiple rows, check if they contain the row for z_0.
+	for (int i = 0; i < activeRows.size(); i++)
+	{
+		if (activeRows[i] == z0Row)
 		{
-			int j = 0;
-			for (; j < Rows.size(); j++)
-			{
-				if (i != j)
-				{
-					if (Rows[j].nrm2() > 0.)
-					{
-						btVectorXu test(dim + 1);
-						for (int ii = 0; ii < dim + 1; ii++)
-						{
-							test[ii] = Rows[j][ii] - Rows[i][ii];
-						}
-
-						//=Rows[j] - Rows[i]
-						if (!LexicographicPositive(test))
-							break;
-					}
-				}
-			}
-
-			if (j == Rows.size())
-			{
-				RowIndex += i;
-				break;
-			}
+			return z0Row;
 		}
 	}
 
-	return RowIndex;
-}
+	// look through the columns of the inverse of the basic matrix from left to right until the tie is broken.
+	for (int col = 0; col < dim ; col++)
+	{
+		btAlignedObjectArray<int> activeRowsCopy(activeRows);
+		activeRows.clear();
+		firstRow = true;
+		for (int i = 0; i<activeRowsCopy.size();i++)
+		{
+			const int row = activeRowsCopy[i];
 
-bool btLemkeAlgorithm::LexicographicPositive(const btVectorXu& v)
-{
-	int i = 0;
-	//  if (DEBUGLEVEL)
-	//  cout << "v " << v << endl;
+			// denom is positive here as an invariant.
+			const btScalar denom = A(row, pivotColIndex);
+			const btScalar ratio = A(row, col) / denom;
+			if (firstRow)
+			{
+				currentMin = ratio;
+				activeRows.push_back(row);
+				firstRow = false;
+			}
+			else if (fabs(currentMin - ratio) < btMachEps())
+			{
+				activeRows.push_back(row);
+			}
+			else if (currentMin > ratio)
+			{
+				currentMin = ratio;
+				activeRows.clear();
+				activeRows.push_back(row);
+			}
+		}
 
-	while (i < v.size() - 1 && fabs(v[i]) < btMachEps())
-		i++;
-	if (v[i] > 0)
-		return true;
-
-	return false;
+		if (activeRows.size() == 1)
+		{
+			return activeRows[0];
+		}
+	}
+	// must not reach here.
+	isRayTermination = true;
+	return 0;
 }
 
 void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis)

src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h

@@ -71,8 +71,7 @@ public:
 	}
 
 protected:
-	int findLexicographicMinimum(const btMatrixXu& A, const int& pivotColIndex);
-	bool LexicographicPositive(const btVectorXu& v);
+	int findLexicographicMinimum(const btMatrixXu& A, const int& pivotColIndex, const int& z0Row, bool& isRayTermination);
 	void GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis);
 	bool greaterZero(const btVectorXu& vector);
 	bool validBasis(const btAlignedObjectArray<int>& basis);

src/BulletSoftBody/btDeformableContactConstraint.cpp

@@ -268,7 +268,7 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv
 	{
 		dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
 	}
-	// dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
+	// dn is the normal component of velocity difference. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
 	btVector3 impulse = m_contact->m_c0 * (vr + m_total_normal_dv * infoGlobal.m_deformable_cfm + ((m_penetration > 0) ? m_penetration / infoGlobal.m_timeStep * cti.m_normal : btVector3(0, 0, 0)));
 	if (!infoGlobal.m_splitImpulse)
 	{
@@ -487,6 +487,9 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul
 	btVector3 dv = impulse * contact->m_c2;
 	btSoftBody::Face* face = contact->m_face;
 
+	// save applied impulse
+	contact->m_cti.m_impulse = impulse;
+
 	btVector3& v0 = face->m_n[0]->m_v;
 	btVector3& v1 = face->m_n[1]->m_v;
 	btVector3& v2 = face->m_n[2]->m_v;

src/BulletSoftBody/btSoftBody.cpp

@@ -2807,7 +2807,7 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr
 //
 // Compute barycentric coordinates (u, v, w) for
 // point p with respect to triangle (a, b, c)
-static void getBarycentric(const btVector3& p, btVector3& a, btVector3& b, btVector3& c, btVector3& bary)
+static void getBarycentric(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, btVector3& bary)
 {
 	btVector3 v0 = b - a, v1 = c - a, v2 = p - a;
 	btScalar d00 = v0.dot(v0);
@@ -2816,8 +2816,17 @@ static void getBarycentric(const btVector3& p, btVector3& a, btVector3& b, btVec
 	btScalar d20 = v2.dot(v0);
 	btScalar d21 = v2.dot(v1);
 	btScalar denom = d00 * d11 - d01 * d01;
-	bary.setY((d11 * d20 - d01 * d21) / denom);
-	bary.setZ((d00 * d21 - d01 * d20) / denom);
+	// In the case of a degenerate triangle, pick a vertex.
+	if (btFabs(denom) < SIMD_EPSILON) 
+	{
+		bary.setY(btScalar(0.0));
+		bary.setZ(btScalar(0.0));
+	} 
+	else 
+	{
+		bary.setY((d11 * d20 - d01 * d21) / denom);
+		bary.setZ((d00 * d21 - d01 * d20) / denom);
+  	}
 	bary.setX(btScalar(1) - bary.getY() - bary.getZ());
 }
 

src/BulletSoftBody/btSoftBody.h

@@ -223,10 +223,12 @@ public:
 	/* sCti is Softbody contact info	*/
 	struct sCti
 	{
-		const btCollisionObject* m_colObj; /* Rigid body			*/
-		btVector3 m_normal;                /* Outward normal		*/
-		btScalar m_offset;                 /* Offset from origin	*/
+		const btCollisionObject* m_colObj; /* Rigid body			        */
+		btVector3 m_normal;                /* Outward normal		        */
+		mutable btVector3 m_impulse;	   /* Applied impulse        	    */
+		btScalar m_offset;                 /* Offset from origin	        */
 		btVector3 m_bary;                  /* Barycentric weights for faces */
+		sCti() : m_impulse(0, 0, 0) {}
 	};
 
 	/* sMedium		*/
@@ -897,7 +899,7 @@ public:
 				   int node1) const;
 	bool checkLink(const Node* node0,
 				   const Node* node1) const;
-	/* Check for existring face												*/
+	/* Check for existing face												*/
 	bool checkFace(int node0,
 				   int node1,
 				   int node2) const;

src/BulletSoftBody/btSparseSDF.h

@@ -233,9 +233,9 @@ struct btSparseSdf
 			//int sz = sizeof(Cell);
 			if (ncells > m_clampCells)
 			{
-				static int numResets = 0;
-				numResets++;
-				//				printf("numResets=%d\n",numResets);
+				//static int numResets = 0;
+				//numResets++;
+				//printf("numResets=%d\n",numResets);
 				Reset();
 			}
 

src/LinearMath/btConvexHull.cpp

@@ -678,7 +678,9 @@ HullError HullLibrary::CreateConvexHull(const HullDesc &desc,  // describes the
 	if (vcount < 8) vcount = 8;
 
 	btAlignedObjectArray<btVector3> vertexSource;
-	vertexSource.resize(static_cast<int>(vcount));
+	btVector3 zero;
+	zero.setZero();
+	vertexSource.resize(static_cast<int>(vcount), zero);
 
 	btVector3 scale;
 

src/LinearMath/btScalar.h

@@ -25,7 +25,7 @@ subject to the following restrictions:
 #include <float.h>
 
 /* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
-#define BT_BULLET_VERSION 324
+#define BT_BULLET_VERSION 326
 
 inline int btGetVersion()
 {

src/LinearMath/btSerializer.h

@@ -481,7 +481,7 @@ public:
 
 		buffer[9] = '3';
 		buffer[10] = '2';
-		buffer[11] = '4';
+		buffer[11] = '6';
 	}
 
 	virtual void startSerialization()
@@ -512,7 +512,7 @@ public:
 			currentPtr += BT_HEADER_LENGTH;
 			for (int i = 0; i < m_chunkPtrs.size(); i++)
 			{
-				int curLength = sizeof(btChunk) + m_chunkPtrs[i]->m_length;
+				int curLength = (int)sizeof(btChunk) + m_chunkPtrs[i]->m_length;
 				memcpy(currentPtr, m_chunkPtrs[i], curLength);
 				btAlignedFree(m_chunkPtrs[i]);
 				currentPtr += curLength;

wheel.sh

@@ -0,0 +1,24 @@
+#!/bin/bash
+set -e -x
+
+# use this docker command
+# sudo docker run -it -v $(pwd):/io quay.io/pypa/manylinux2014_x86_64
+
+
+# Compile wheels
+for PYBIN in /opt/python/*/bin; do
+    "${PYBIN}/pip" install -r /io/dev/bullet3/requirements.txt
+    "${PYBIN}/pip" wheel /io/dev/bullet3 -w wheelhouse/
+done
+
+# Bundle external shared libraries into the wheels
+for whl in wheelhouse/*.whl; do
+    auditwheel repair "$whl"  -w /io/wheelhouse/
+done
+
+# Install packages and test
+for PYBIN in /opt/python/*/bin/; do
+    "${PYBIN}/pip" install python-manylinux-demo --no-index -f /io/wheelhouse
+    (cd "$HOME"; "${PYBIN}/nosetests" pymanylinuxdemo)
+done
+