From 0e0d3da2b5c1fd4266a57594558ff28be3b2e1c8 Mon Sep 17 00:00:00 2001
From: yunfeibai <byf1658@gmail.com>
Date: Mon, 8 Jan 2018 18:10:28 -0800
Subject: [PATCH] Add python binding and a pybullet example for loading
 softbody from obj.

---
 CMakeLists.txt                               |  6 +-
 examples/pybullet/examples/load_soft_body.py | 20 ++++++
 examples/pybullet/pybullet.c                 | 66 +++++++++++++++++++-
 3 files changed, 88 insertions(+), 4 deletions(-)
 create mode 100644 examples/pybullet/examples/load_soft_body.py

diff --git a/CMakeLists.txt b/CMakeLists.txt
index feadf83f3..add652125 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -26,7 +26,7 @@ SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -D_DEBUG")
 OPTION(USE_DOUBLE_PRECISION "Use double precision"	OFF)
 OPTION(USE_GRAPHICAL_BENCHMARK "Use Graphical Benchmark" ON)
 OPTION(BUILD_SHARED_LIBS "Use shared libraries" OFF)
-OPTION(USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD "Use btSoftMultiBodyDynamicsWorld" OFF)
+OPTION(USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD "Use btSoftMultiBodyDynamicsWorld" ON)
 
 OPTION(BULLET2_USE_THREAD_LOCKS "Build Bullet 2 libraries with mutex locking around certain operations (required for multi-threading)" OFF)
 IF (BULLET2_USE_THREAD_LOCKS)
@@ -217,8 +217,8 @@ ADD_DEFINITIONS( -DBT_USE_DOUBLE_PRECISION)
 SET( BULLET_DOUBLE_DEF "-DBT_USE_DOUBLE_PRECISION")
 ENDIF (USE_DOUBLE_PRECISION)
 
-IF (USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
-ADD_DEFINITIONS(-DUSE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
+IF (NOT USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
+ADD_DEFINITIONS(-DSKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
 ENDIF (USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
 
 IF(USE_GRAPHICAL_BENCHMARK)
diff --git a/examples/pybullet/examples/load_soft_body.py b/examples/pybullet/examples/load_soft_body.py
new file mode 100644
index 000000000..ac56772cb
--- /dev/null
+++ b/examples/pybullet/examples/load_soft_body.py
@@ -0,0 +1,20 @@
+import pybullet as p
+from time import sleep
+
+physicsClient = p.connect(p.GUI)
+
+p.setGravity(0,0,-10)
+planeId = p.loadURDF("plane.urdf")
+bunnyId = p.loadSoftBody("bunny.obj")
+
+useRealTimeSimulation = 1
+
+if (useRealTimeSimulation):
+	p.setRealTimeSimulation(1)
+
+while 1:
+	if (useRealTimeSimulation):
+		p.setGravity(0,0,-10)
+		sleep(0.01) # Time in seconds.
+	else:
+		p.stepSimulation()
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index 232913605..c94b696f4 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -1353,6 +1353,67 @@ static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keyw
 	return pylist;
 }
 
+#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+// Load a softbody from an obj file
+static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	int physicsClientId = 0;
+	int flags = 0;
+	
+	static char* kwlist[] = {"fileName", "scale", "mass", "collisionMargin", "physicsClientId", NULL};
+	
+	int bodyUniqueId= -1;
+	const char* fileName = "";
+	double scale = -1;
+	double mass = -1;
+	double collisionMargin = -1;
+	
+	b3PhysicsClientHandle sm = 0;
+	
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|dddi", kwlist, &fileName, &scale, &mass, &collisionMargin, &physicsClientId))
+	{
+		return NULL;
+	}
+	
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0)
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+	
+	if (strlen(fileName))
+	{
+		b3SharedMemoryStatusHandle statusHandle;
+		int statusType;
+		b3SharedMemoryCommandHandle command =
+		b3LoadSoftBodyCommandInit(sm, fileName);
+
+		if (scale>0)
+		{
+			b3LoadSoftBodySetScale(command,scale);
+		}
+		if (mass>0)
+		{
+			b3LoadSoftBodySetMass(command,mass);
+		}
+		if (collisionMargin>0)
+		{
+			b3LoadSoftBodySetCollisionMargin(command,collisionMargin);
+		}
+		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+		statusType = b3GetStatusType(statusHandle);
+		if (statusType != CMD_CLIENT_COMMAND_COMPLETED)
+		{
+			PyErr_SetString(SpamError, "Cannot load soft body.");
+			return NULL;
+		}
+		bodyUniqueId = b3GetStatusBodyIndex(statusHandle);
+	}
+	return PyLong_FromLong(bodyUniqueId);
+}
+#endif
+
 // Reset the simulation to remove all loaded objects
 static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args, PyObject* keywds)
 {
@@ -7861,7 +7922,10 @@ static PyMethodDef SpamMethods[] = {
 
 	{"loadSDF", (PyCFunction)pybullet_loadSDF, METH_VARARGS | METH_KEYWORDS,
 	 "Load multibodies from an SDF file."},
-
+#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+	{"loadSoftBody", (PyCFunction)pybullet_loadSoftBody, METH_VARARGS | METH_KEYWORDS,
+	 "Load a softbody from an obj file."},
+#endif
 	{"loadBullet", (PyCFunction)pybullet_loadBullet, METH_VARARGS | METH_KEYWORDS,
 	 "Load a world from a .bullet file."},