Merge branch 'master' of github.com:erwincoumans/bullet3

6 files changed, 266 insertions, 138 deletions

diff --git a/examples/DeformableDemo/LoadDeformed.cpp b/examples/DeformableDemo/LoadDeformed.cpp
index 55c322779..283c428c0 100644
--- a/examples/DeformableDemo/LoadDeformed.cpp
+++ b/examples/DeformableDemo/LoadDeformed.cpp
@@ -26,6 +26,150 @@
 #include "../CommonInterfaces/CommonDeformableBodyBase.h"
 #include "../Utils/b3ResourcePath.h"
 #include "../Utils/b3BulletDefaultFileIO.h"
+#include <iostream>
+#include <iomanip> 
+#include <sstream>
+#include <string.h>
+
+struct CustomSoftBodyHelper : public btSoftBodyHelpers
+{
+	static std::string loadDeformableState(btAlignedObjectArray<btVector3>& qs, btAlignedObjectArray<btVector3>& vs, const char* filename, CommonFileIOInterface* fileIO);
+	
+};
+
+static inline bool isSpace(const char c)
+{
+	return (c == ' ') || (c == '\t');
+}
+static inline bool isNewLine(const char c)
+{
+	return (c == '\r') || (c == '\n') || (c == '\0');
+}
+static inline float parseFloat(const char*& token)
+{
+	token += strspn(token, " \t");
+	float f = (float)atof(token);
+	token += strcspn(token, " \t\r");
+	return f;
+}
+static inline void parseFloat3(
+	float& x, float& y, float& z,
+	const char*& token)
+{
+	x = parseFloat(token);
+	y = parseFloat(token);
+	z = parseFloat(token);
+}
+
+
+std::string CustomSoftBodyHelper::loadDeformableState(btAlignedObjectArray<btVector3>& qs, btAlignedObjectArray<btVector3>& vs, const char* filename, CommonFileIOInterface* fileIO)
+{
+	{
+		qs.clear();
+		vs.clear();
+		std::string tmp = filename;
+		std::stringstream err;
+#ifdef USE_STREAM
+		std::ifstream ifs(filename);
+		if (!ifs)
+		{
+			err << "Cannot open file [" << filename << "]" << std::endl;
+			return err.str();
+		}
+#else
+		int fileHandle = fileIO->fileOpen(filename, "r");
+		if (fileHandle < 0)
+		{
+			err << "Cannot open file [" << filename << "]" << std::endl;
+			return err.str();
+		}
+#endif
+
+		std::string name;
+
+		int maxchars = 8192;              // Alloc enough size.
+		std::vector<char> buf(maxchars);  // Alloc enough size.
+		std::string linebuf;
+		linebuf.reserve(maxchars);
+
+#ifdef USE_STREAM
+		while (ifs.peek() != -1)
+#else
+		char* line = 0;
+		do
+#endif
+		{
+			linebuf.resize(0);
+#ifdef USE_STREAM
+			safeGetline(ifs, linebuf);
+#else
+			char tmpBuf[1024];
+			line = fileIO->readLine(fileHandle, tmpBuf, 1024);
+			if (line)
+			{
+				linebuf = line;
+			}
+#endif
+			// Trim newline '\r\n' or '\r'
+			if (linebuf.size() > 0)
+			{
+				if (linebuf[linebuf.size() - 1] == '\n') linebuf.erase(linebuf.size() - 1);
+			}
+			if (linebuf.size() > 0)
+			{
+				if (linebuf[linebuf.size() - 1] == '\n') linebuf.erase(linebuf.size() - 1);
+			}
+
+			// Skip if empty line.
+			if (linebuf.empty())
+			{
+				continue;
+			}
+
+			// Skip leading space.
+			const char* token = linebuf.c_str();
+			token += strspn(token, " \t");
+
+			btAssert(token);
+			if (token[0] == '\0') continue;  // empty line
+
+			if (token[0] == '#') continue;  // comment line
+
+			// q
+			if (token[0] == 'q' && isSpace((token[1])))
+			{
+				token += 2;
+				float x, y, z;
+				parseFloat3(x, y, z, token);
+				qs.push_back(btVector3(x, y, z));
+				continue;
+			}
+
+			// v
+			if (token[0] == 'v' && isSpace((token[1])))
+			{
+				token += 3;
+				float x, y, z;
+				parseFloat3(x, y, z, token);
+				vs.push_back(btVector3(x, y, z));
+				continue;
+			}
+
+			// Ignore unknown command.
+		}
+#ifndef USE_STREAM
+		while (line)
+			;
+#endif
+
+		if (fileHandle >= 0)
+		{
+			fileIO->fileClose(fileHandle);
+		}
+		return err.str();
+	}
+}
+
 
 class LoadDeformed : public CommonDeformableBodyBase
 {
@@ -188,7 +332,7 @@ void LoadDeformed::addCloth(const btVector3& origin)
 	fileio.findResourcePath(filename, absolute_path, 1024);
 	btAlignedObjectArray<btVector3> qs;
 	btAlignedObjectArray<btVector3> vs;
-	btSoftBodyHelpers::loadDeformableState(qs, vs, absolute_path, &fileio);
+	CustomSoftBodyHelper::loadDeformableState(qs, vs, absolute_path, &fileio);
 	if (reset_frame > 0)
 		psb->updateState(qs, vs);
 	psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
diff --git a/examples/pybullet/examples/biped2d_pybullet.py b/examples/pybullet/examples/biped2d_pybullet.py
index 7ddf7b976..ad76f2178 100644
--- a/examples/pybullet/examples/biped2d_pybullet.py
+++ b/examples/pybullet/examples/biped2d_pybullet.py
@@ -2,18 +2,130 @@ import pybullet as p
 import pybullet_data
 import os
 import time
+
+import pybullet as p
+import pybullet_data as pd
+import math
+import time
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pd.getDataPath())
+
+textureId = -1
+
+useProgrammatic = 0
+useTerrainFromPNG = 1
+useDeepLocoCSV = 2
+updateHeightfield = False
+
+heightfieldSource = useProgrammatic
+import random
+random.seed(10)
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
+heightPerturbationRange = 0.05
+if heightfieldSource==useProgrammatic:
+  numHeightfieldRows = 256
+  numHeightfieldColumns = 256
+  heightfieldData = [0]*numHeightfieldRows*numHeightfieldColumns 
+  for j in range (int(numHeightfieldColumns/2)):
+    for i in range (int(numHeightfieldRows/2) ):
+      height = random.uniform(0,heightPerturbationRange)
+      heightfieldData[2*i+2*j*numHeightfieldRows]=height
+      heightfieldData[2*i+1+2*j*numHeightfieldRows]=height
+      heightfieldData[2*i+(2*j+1)*numHeightfieldRows]=height
+      heightfieldData[2*i+1+(2*j+1)*numHeightfieldRows]=height
+      
+      
+  terrainShape = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, meshScale=[.05,.05,1], heightfieldTextureScaling=(numHeightfieldRows-1)/2, heightfieldData=heightfieldData, numHeightfieldRows=numHeightfieldRows, numHeightfieldColumns=numHeightfieldColumns)
+  terrain  = p.createMultiBody(0, terrainShape)
+  p.resetBasePositionAndOrientation(terrain,[0,0,0], [0,0,0,1])
+
+if heightfieldSource==useDeepLocoCSV:
+  terrainShape = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, meshScale=[.5,.5,2.5],fileName = "heightmaps/ground0.txt", heightfieldTextureScaling=128)
+  terrain  = p.createMultiBody(0, terrainShape)
+  p.resetBasePositionAndOrientation(terrain,[0,0,0], [0,0,0,1])
+
+if heightfieldSource==useTerrainFromPNG:
+  terrainShape = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, meshScale=[.1,.1,24],fileName = "heightmaps/wm_height_out.png")
+  textureId = p.loadTexture("heightmaps/gimp_overlay_out.png")
+  terrain  = p.createMultiBody(0, terrainShape)
+  p.changeVisualShape(terrain, -1, textureUniqueId = textureId)
+ 
+ 
+p.changeVisualShape(terrain, -1, rgbaColor=[1,1,1,1])
+
+
+sphereRadius = 0.05
+colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
+colBoxId = p.createCollisionShape(p.GEOM_BOX,
+                                  halfExtents=[sphereRadius, sphereRadius, sphereRadius])
+
+mass = 1
+visualShapeId = -1
+
+link_Masses = [1]
+linkCollisionShapeIndices = [colBoxId]
+linkVisualShapeIndices = [-1]
+linkPositions = [[0, 0, 0.11]]
+linkOrientations = [[0, 0, 0, 1]]
+linkInertialFramePositions = [[0, 0, 0]]
+linkInertialFrameOrientations = [[0, 0, 0, 1]]
+indices = [0]
+jointTypes = [p.JOINT_REVOLUTE]
+axis = [[0, 0, 1]]
+
+for i in range(3):
+  for j in range(3):
+    for k in range(3):
+      basePosition = [
+          i * 5 * sphereRadius, j * 5 * sphereRadius, 1 + k * 5 * sphereRadius + 1
+      ]
+      baseOrientation = [0, 0, 0, 1]
+      if (k & 2):
+        sphereUid = p.createMultiBody(mass, colSphereId, visualShapeId, basePosition,
+                                      baseOrientation)
+      else:
+        sphereUid = p.createMultiBody(mass,
+                                      colBoxId,
+                                      visualShapeId,
+                                      basePosition,
+                                      baseOrientation,
+                                      linkMasses=link_Masses,
+                                      linkCollisionShapeIndices=linkCollisionShapeIndices,
+                                      linkVisualShapeIndices=linkVisualShapeIndices,
+                                      linkPositions=linkPositions,
+                                      linkOrientations=linkOrientations,
+                                      linkInertialFramePositions=linkInertialFramePositions,
+                                      linkInertialFrameOrientations=linkInertialFrameOrientations,
+                                      linkParentIndices=indices,
+                                      linkJointTypes=jointTypes,
+                                      linkJointAxis=axis)
+
+      
+      p.changeDynamics(sphereUid,
+                       -1,
+                       spinningFriction=0.001,
+                       rollingFriction=0.001,
+                       linearDamping=0.0)
+      for joint in range(p.getNumJoints(sphereUid)):
+        p.setJointMotorControl2(sphereUid, joint, p.VELOCITY_CONTROL, targetVelocity=1, force=10)
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
+
 GRAVITY = -9.8
 dt = 1e-3
 iters = 2000
 import pybullet_data
 
-physicsClient = p.connect(p.GUI)
+#physicsClient = p.connect(p.GUI)
+
+
 p.setAdditionalSearchPath(pybullet_data.getDataPath())
-p.resetSimulation()
+#p.resetSimulation()
 #p.setRealTimeSimulation(True)
 p.setGravity(0, 0, GRAVITY)
 p.setTimeStep(dt)
-planeId = p.loadURDF("plane.urdf")
+#planeId = p.loadURDF("plane.urdf")
 cubeStartPos = [0, 0, 1.13]
 cubeStartOrientation = p.getQuaternionFromEuler([0., 0, 0])
 botId = p.loadURDF("biped/biped2d_pybullet.urdf", cubeStartPos, cubeStartOrientation)
diff --git a/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h b/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h
index 3b5150f6d..f5763f698 100644
--- a/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h
+++ b/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h
@@ -43,6 +43,9 @@ public:
 	void setTransformB(const btTransform& transB) { m_transB = transB; }
 
 	const btTransform& getTransformA() const { return m_transA; }
+	const btTransform& getTransformB() const { return m_transB; }
+
+	// keep this for backward compatibility
 	const btTransform& GetTransformB() const { return m_transB; }
 
 	virtual btScalar getMargin() const;
diff --git a/src/BulletInverseDynamics/IDMath.cpp b/src/BulletInverseDynamics/IDMath.cpp
index 2f120ed48..0c3f5c9d4 100644
--- a/src/BulletInverseDynamics/IDMath.cpp
+++ b/src/BulletInverseDynamics/IDMath.cpp
@@ -503,8 +503,8 @@ vec3 rpyFromMatrix(const mat33 &rot)
 {
 	vec3 rpy;
 	rpy(2) = BT_ID_ATAN2(-rot(1, 0), rot(0, 0));
-	rpy(1) = BT_ID_ATAN2(rot(2, 0), BT_ID_COS(rpy(2)) * rot(0, 0) - BT_ID_SIN(rpy(0)) * rot(1, 0));
 	rpy(0) = BT_ID_ATAN2(-rot(2, 0), rot(2, 2));
+	rpy(1) = BT_ID_ATAN2(rot(2, 0), BT_ID_COS(rpy(2)) * rot(0, 0) - BT_ID_SIN(rpy(0)) * rot(1, 0));
 	return rpy;
 }
 }  // namespace btInverseDynamics
diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp
index 35eb22ec1..fbb729830 100644
--- a/src/BulletSoftBody/btSoftBodyHelpers.cpp
+++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp
@@ -1488,29 +1488,6 @@ void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb)
 	fs.close();
 }
 
-static inline bool isSpace(const char c)
-{
-	return (c == ' ') || (c == '\t');
-}
-static inline bool isNewLine(const char c)
-{
-	return (c == '\r') || (c == '\n') || (c == '\0');
-}
-static inline float parseFloat(const char*& token)
-{
-	token += strspn(token, " \t");
-	float f = (float)atof(token);
-	token += strcspn(token, " \t\r");
-	return f;
-}
-static inline void parseFloat3(
-	float& x, float& y, float& z,
-	const char*& token)
-{
-	x = parseFloat(token);
-	y = parseFloat(token);
-	z = parseFloat(token);
-}
 
 void btSoftBodyHelpers::writeState(const char* file, const btSoftBody* psb)
 {
@@ -1542,114 +1519,6 @@ void btSoftBodyHelpers::writeState(const char* file, const btSoftBody* psb)
 	fs.close();
 }
 
-std::string btSoftBodyHelpers::loadDeformableState(btAlignedObjectArray<btVector3>& qs, btAlignedObjectArray<btVector3>& vs, const char* filename, CommonFileIOInterface* fileIO)
-{
-	{
-		qs.clear();
-		vs.clear();
-		std::string tmp = filename;
-		std::stringstream err;
-#ifdef USE_STREAM
-		std::ifstream ifs(filename);
-		if (!ifs)
-		{
-			err << "Cannot open file [" << filename << "]" << std::endl;
-			return err.str();
-		}
-#else
-		int fileHandle = fileIO->fileOpen(filename, "r");
-		if (fileHandle < 0)
-		{
-			err << "Cannot open file [" << filename << "]" << std::endl;
-			return err.str();
-		}
-#endif
-
-		std::string name;
-
-		int maxchars = 8192;              // Alloc enough size.
-		std::vector<char> buf(maxchars);  // Alloc enough size.
-		std::string linebuf;
-		linebuf.reserve(maxchars);
-
-#ifdef USE_STREAM
-		while (ifs.peek() != -1)
-#else
-		char* line = 0;
-		do
-#endif
-		{
-			linebuf.resize(0);
-#ifdef USE_STREAM
-			safeGetline(ifs, linebuf);
-#else
-			char tmpBuf[1024];
-			line = fileIO->readLine(fileHandle, tmpBuf, 1024);
-			if (line)
-			{
-				linebuf = line;
-			}
-#endif
-			// Trim newline '\r\n' or '\r'
-			if (linebuf.size() > 0)
-			{
-				if (linebuf[linebuf.size() - 1] == '\n') linebuf.erase(linebuf.size() - 1);
-			}
-			if (linebuf.size() > 0)
-			{
-				if (linebuf[linebuf.size() - 1] == '\n') linebuf.erase(linebuf.size() - 1);
-			}
-
-			// Skip if empty line.
-			if (linebuf.empty())
-			{
-				continue;
-			}
-
-			// Skip leading space.
-			const char* token = linebuf.c_str();
-			token += strspn(token, " \t");
-
-			btAssert(token);
-			if (token[0] == '\0') continue;  // empty line
-
-			if (token[0] == '#') continue;  // comment line
-
-			// q
-			if (token[0] == 'q' && isSpace((token[1])))
-			{
-				token += 2;
-				float x, y, z;
-				parseFloat3(x, y, z, token);
-				qs.push_back(btVector3(x, y, z));
-				continue;
-			}
-
-			// v
-			if (token[0] == 'v' && isSpace((token[1])))
-			{
-				token += 3;
-				float x, y, z;
-				parseFloat3(x, y, z, token);
-				vs.push_back(btVector3(x, y, z));
-				continue;
-			}
-
-			// Ignore unknown command.
-		}
-#ifndef USE_STREAM
-		while (line)
-			;
-#endif
-
-		if (fileHandle >= 0)
-		{
-			fileIO->fileClose(fileHandle);
-		}
-		return err.str();
-	}
-}
-
 void btSoftBodyHelpers::duplicateFaces(const char* filename, const btSoftBody* psb)
 {
 	std::ifstream fs_read;
diff --git a/src/BulletSoftBody/btSoftBodyHelpers.h b/src/BulletSoftBody/btSoftBodyHelpers.h
index d1fdf0450..e48d7b751 100644
--- a/src/BulletSoftBody/btSoftBodyHelpers.h
+++ b/src/BulletSoftBody/btSoftBodyHelpers.h
@@ -17,7 +17,6 @@ subject to the following restrictions:
 #define BT_SOFT_BODY_HELPERS_H
 
 #include "btSoftBody.h"
-#include "../../examples/CommonInterfaces/CommonFileIOInterface.h"
 #include <fstream>
 #include <string>
 //
@@ -149,7 +148,8 @@ struct btSoftBodyHelpers
 
 	static void writeState(const char* file, const btSoftBody* psb);
 
-	static std::string loadDeformableState(btAlignedObjectArray<btVector3>& qs, btAlignedObjectArray<btVector3>& vs, const char* filename, CommonFileIOInterface* fileIO);
+  //this code cannot be here, dependency on example code are not allowed
+	//static std::string loadDeformableState(btAlignedObjectArray<btVector3>& qs, btAlignedObjectArray<btVector3>& vs, const char* filename, CommonFileIOInterface* fileIO);
 
 	static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary);