Merge remote-tracking branch 'upstream/master' into multibody_mlcp_solver_v2

Extras/BulletRobotics/CMakeLists.txt

@@ -9,6 +9,7 @@ INCLUDE_DIRECTORIES(
 )
 
 SET(BulletRobotics_SRCS
+		../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp
 		../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
 		../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
 		../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp

Extras/BulletRobotics/premake4.lua

@@ -81,6 +81,7 @@ if not _OPTIONS["no-enet"] then
 
 
 		files {
+		"../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp",
 		"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
 		"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
 		"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp",

examples/ExampleBrowser/CMakeLists.txt

@@ -145,6 +145,8 @@ SET(BulletExampleBrowser_SRCS
 	../TinyRenderer/tgaimage.cpp
 	../TinyRenderer/our_gl.cpp
 	../TinyRenderer/TinyRenderer.cpp
+	../SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp
+	../SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.h
 	../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
 	../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
 	../SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp

examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp

@@ -158,7 +158,7 @@ void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTrans
 							btVector3 pos1 =parentTransform*hull->getVertexPointer()[index1];
 							btVector3 pos2 =parentTransform*hull->getVertexPointer()[index2];
 							triNormal = (pos1-pos0).cross(pos2-pos0);
-							triNormal.normalize();
+							triNormal.safeNormalize();
 
 							for (int v=0;v<3;v++)
 							{

examples/ExampleBrowser/premake4.lua

@@ -115,6 +115,7 @@ project "App_BulletExampleBrowser"
 		"../SharedMemory/PhysicsServerCommandProcessor.cpp",
 		"../SharedMemory/PhysicsServerCommandProcessor.h",
 		"../SharedMemory/b3PluginManager.cpp",		
+		"../SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp",
 		"../SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp",
 		"../SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp",
 		"../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",

examples/SharedMemory/CMakeLists.txt

@@ -1,5 +1,7 @@
 
 SET(SharedMemory_SRCS
+	plugins/collisionFilterPlugin/collisionFilterPlugin.cpp
+	plugins/collisionFilterPlugin/collisionFilterPlugin.h
 	plugins/pdControlPlugin/pdControlPlugin.cpp
 	plugins/pdControlPlugin/pdControlPlugin.h
 	b3RobotSimulatorClientAPI_NoDirect.cpp

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -46,6 +46,10 @@
 #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
 #include "LinearMath/TaskScheduler/btThreadSupportInterface.h"
 
+#ifndef SKIP_COLLISION_FILTER_PLUGIN
+#include "plugins/collisionFilterPlugin/collisionFilterPlugin.h"
+#endif
+
 #ifndef SKIP_STATIC_PD_CONTROL_PLUGIN
 #include "plugins/pdControlPlugin/pdControlPlugin.h"
 #endif //SKIP_STATIC_PD_CONTROL_PLUGIN
@@ -1625,6 +1629,8 @@ struct PhysicsServerCommandProcessorInternalData
 	btScalar m_oldPickingDist;
 	bool m_prevCanSleep;
 	int m_pdControlPlugin;
+	int m_collisionFilterPlugin;
+
 #ifdef B3_ENABLE_TINY_AUDIO
 	b3SoundEngine m_soundEngine;
 #endif
@@ -1662,6 +1668,7 @@ struct PhysicsServerCommandProcessorInternalData
 		m_pickedConstraint(0),
 		m_pickingMultiBodyPoint2Point(0),
 		m_pdControlPlugin(-1),
+		m_collisionFilterPlugin(-1),
 		m_threadPool(0)
 	{
 
@@ -1677,6 +1684,11 @@ struct PhysicsServerCommandProcessorInternalData
 		}
 #endif //SKIP_STATIC_PD_CONTROL_PLUGIN
 
+#ifndef SKIP_COLLISION_FILTER_PLUGIN
+	{
+		m_collisionFilterPlugin = m_pluginManager.registerStaticLinkedPlugin("collisionFilterPlugin", initPlugin_collisionFilterPlugin, exitPlugin_collisionFilterPlugin, executePluginCommand_collisionFilterPlugin, 0,0,0);
+	}
+#endif
 
 #ifndef SKIP_STATIC_TINYRENDERER_PLUGIN
 			int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("tinyRendererPlugin", initPlugin_tinyRendererPlugin, exitPlugin_tinyRendererPlugin, executePluginCommand_tinyRendererPlugin,0,0,getRenderInterface_tinyRendererPlugin);

examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp

@@ -8,22 +8,22 @@
 #include "../b3PluginContext.h"
 #include <stdio.h>
 
-struct MyClass
+struct CollisionFilterMyClass
 {
 	int m_testData;
 
-	MyClass()
+	CollisionFilterMyClass()
 		:m_testData(42)
 	{
 	}
-	virtual ~MyClass()
+	virtual ~CollisionFilterMyClass()
 	{
 	}
 };
 
-B3_SHARED_API int initPlugin_testPlugin(struct b3PluginContext* context)
+B3_SHARED_API int initPlugin_collisionFilterPlugin(struct b3PluginContext* context)
 {
-	MyClass* obj = new MyClass();
+	CollisionFilterMyClass* obj = new CollisionFilterMyClass();
 	context->m_userPointer = obj;
 
 	printf("hi!\n");
@@ -31,21 +31,21 @@ B3_SHARED_API int initPlugin_testPlugin(struct b3PluginContext* context)
 }
 
 
-B3_SHARED_API int preTickPluginCallback_testPlugin(struct b3PluginContext* context)
+B3_SHARED_API int preTickPluginCallback_collisionFilterPlugin(struct b3PluginContext* context)
 {
 	//apply pd control here, apply forces using the PD gains
 	return 0;
 }
 
 
-B3_SHARED_API int postTickPluginCallback_testPlugin(struct b3PluginContext* context)
+B3_SHARED_API int postTickPluginCallback_collisionFilterPlugin(struct b3PluginContext* context)
 {
-	MyClass* obj = (MyClass* )context->m_userPointer;
+	CollisionFilterMyClass* obj = (CollisionFilterMyClass* )context->m_userPointer;
 	obj->m_testData++;
 	return 0;
 }
 
-B3_SHARED_API int executePluginCommand_testPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
+B3_SHARED_API int executePluginCommand_collisionFilterPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
 {
 	//set the PD gains
 	printf("text argument:%s\n",arguments->m_text);
@@ -69,7 +69,7 @@ B3_SHARED_API int executePluginCommand_testPlugin(struct b3PluginContext* contex
 	}
 	printf("]\n");
 
-	MyClass* obj = (MyClass*) context->m_userPointer;
+	CollisionFilterMyClass* obj = (CollisionFilterMyClass*) context->m_userPointer;
 	
 	b3SharedMemoryStatusHandle statusHandle;
 	int statusType = -1;
@@ -88,9 +88,9 @@ B3_SHARED_API int executePluginCommand_testPlugin(struct b3PluginContext* contex
 }
 
 
-B3_SHARED_API void exitPlugin_testPlugin(struct b3PluginContext* context)
+B3_SHARED_API void exitPlugin_collisionFilterPlugin(struct b3PluginContext* context)
 {
-	MyClass* obj = (MyClass*) context->m_userPointer;
+	CollisionFilterMyClass* obj = (CollisionFilterMyClass*) context->m_userPointer;
 	delete obj;
 	context->m_userPointer = 0;
 

examples/SharedMemory/premake4.lua

@@ -60,6 +60,7 @@ myfiles =
 	"PhysicsServerCommandProcessor.h",
 	"b3PluginManager.cpp",
 	"b3PluginManager.h",
+	"plugins/collisionFilterPlugin/collisionFilterPlugin.cpp",
 	"plugins/pdControlPlugin/pdControlPlugin.cpp",
 	"plugins/pdControlPlugin/pdControlPlugin.h",
 	"../OpenGLWindow/SimpleCamera.cpp",
@@ -472,4 +473,3 @@ include "plugins/tinyRendererPlugin"
 include "plugins/pdControlPlugin"
 include "plugins/collisionFilterPlugin"
 
-

examples/SharedMemory/tcp/premake4.lua

@@ -93,6 +93,7 @@ myfiles =
 	"../PhysicsDirect.cpp",
 	"../PhysicsClientC_API.cpp",
 	"../PhysicsClient.cpp",
+	"../plugins/collisionFilterPlugin/collisionFilterPlugin.cpp",
 	"../plugins/pdControlPlugin/pdControlPlugin.cpp",
 	"../plugins/pdControlPlugin/pdControlPlugin.h",
 	"../b3RobotSimulatorClientAPI_NoDirect.cpp",

examples/SharedMemory/udp/premake4.lua

@@ -77,6 +77,7 @@ language "C++"
 
 myfiles = 
 {
+	"../plugins/collisionFilterPlugin/collisionFilterPlugin.cpp",
 	"../plugins/pdControlPlugin/pdControlPlugin.cpp",
 	"../plugins/pdControlPlugin/pdControlPlugin.h",
 	"../b3RobotSimulatorClientAPI_NoDirect.cpp",

examples/TwoJoint/CMakeLists.txt

@@ -10,6 +10,7 @@ INCLUDE_DIRECTORIES(
 
 SET(RobotSimulator_SRCS
 		TwoJointMain.cpp
+		../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp
 		../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
 		../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
 		../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp

examples/Utils/ChromeTraceUtil.cpp

@@ -4,6 +4,7 @@
 #include "LinearMath/btQuickprof.h"
 #include "LinearMath/btAlignedObjectArray.h"
 #include "Bullet3Common/b3Logging.h"
+#include <stdio.h>
 
 struct btTiming
 {
@@ -155,13 +156,13 @@ struct btTimings
 	int m_activeBuffer;
 	btAlignedObjectArray<btTiming> m_timings[1];
 };
-#ifndef BT_NO_PROFILE
+//#ifndef BT_NO_PROFILE
 btTimings gTimings[BT_QUICKPROF_MAX_THREAD_COUNT];
 #define MAX_NESTING 1024
 int gStackDepths[BT_QUICKPROF_MAX_THREAD_COUNT] = { 0 };
 const char* gFuncNames[BT_QUICKPROF_MAX_THREAD_COUNT][MAX_NESTING];
 unsigned long long int gStartTimes[BT_QUICKPROF_MAX_THREAD_COUNT][MAX_NESTING];
-#endif
+//#endif
 
 btClock clk;
 
@@ -276,4 +277,4 @@ void b3ChromeUtilsStopTimingsAndWriteJsonFile(const char* fileNamePrefix)
 void b3ChromeUtilsEnableProfiling()
 {
 	gProfileDisabled = false;
-}
+}

examples/pybullet/CMakeLists.txt

@@ -15,6 +15,7 @@ ENDIF()
 
 SET(pybullet_SRCS
 	pybullet.c
+		../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp
 		../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
 		../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
 		../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp

examples/pybullet/premake4.lua

@@ -155,6 +155,7 @@ if not _OPTIONS["no-enet"] then
 			"../../examples/MultiThreading/b3PosixThreadSupport.cpp",
 			"../../examples/MultiThreading/b3Win32ThreadSupport.cpp",
 			"../../examples/MultiThreading/b3ThreadSupportInterface.cpp",
+			"../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp",
 			"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
 			"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
 			}

examples/pybullet/pybullet.c

@@ -4,6 +4,7 @@
 #ifdef BT_ENABLE_ENET
 #include "../SharedMemory/PhysicsClientUDP_C_API.h"
 #endif  //BT_ENABLE_ENET
+#define PYBULLET_PI (3.1415926535897932384626433832795029)
 
 #ifdef BT_ENABLE_DART
 #include "../SharedMemory/dart/DARTPhysicsC_API.h"
@@ -8239,13 +8240,28 @@ static PyObject* pybullet_getEulerFromQuaternion(PyObject* self,
 		sqy = quat[1] * quat[1];
 		sqz = quat[2] * quat[2];
 		squ = quat[3] * quat[3];
-		rpy[0] = atan2(2 * (quat[1] * quat[2] + quat[3] * quat[0]),
-					   squ - sqx - sqy + sqz);
 		sarg = -2 * (quat[0] * quat[2] - quat[3] * quat[1]);
-		rpy[1] = sarg <= -1.0 ? -0.5 * 3.141592538
+		
-							  : (sarg >= 1.0 ? 0.5 * 3.141592538 : asin(sarg));
+		// If the pitch angle is PI/2 or -PI/2, we can only compute
-		rpy[2] = atan2(2 * (quat[0] * quat[1] + quat[3] * quat[2]),
+		// the sum roll + yaw.  However, any combination that gives
-					   squ + sqx - sqy - sqz);
+		// the right sum will produce the correct orientation, so we
+		// set rollX = 0 and compute yawZ.
+		if (sarg <= -0.99999)
+		{
+			rpy[0]  = 0;
+			rpy[1] = -0.5*PYBULLET_PI;
+			rpy[2]   = 2 * atan2(quat[0],-quat[1]);
+		} else if (sarg >= 0.99999)
+		{
+			rpy[0]  = 0;
+			rpy[1] = 0.5*PYBULLET_PI;
+			rpy[2]   = 2 * atan2(-quat[0], quat[1]);
+		} else
+		{
+			rpy[0] = atan2(2 * (quat[1] * quat[2] + quat[3] * quat[0]), squ - sqx - sqy + sqz);
+			rpy[1] = asin(sarg);
+			rpy[2] = atan2(2 * (quat[0] * quat[1] + quat[3] * quat[2]), squ + sqx - sqy - sqz);
+		}
 		{
 			PyObject* pylist;
 			int i;

setup.py

@@ -50,6 +50,7 @@ sources = ["examples/pybullet/pybullet.c"]\
 +["examples/TinyRenderer/our_gl.cpp"]\
 +["examples/TinyRenderer/TinyRenderer.cpp"]\
 +["examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp"]\
++["examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp"]\
 +["examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp"]\
 +["examples/SharedMemory/IKTrajectoryHelper.cpp"]\
 +["examples/SharedMemory/InProcessMemory.cpp"]\

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -781,7 +781,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
     // hhat is NOT stored for the base (but ahat is)    
 	btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
 	btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
-	// v_ptr += num_links * 2 + 2;  // Disabled since v_ptr is not used in the rest of the code
+	v_ptr += num_links * 2 + 2; 
 	//
     // Y_i, invD_i
     btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
@@ -967,9 +967,11 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 			//
 			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)
+			- spatCoriolisAcc[i].dot(hDof);
-			;
 
+		}
+ 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{	
 			btScalar *D_row = &D[dof * m_links[i].m_dofCount];
 			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
 			{

src/LinearMath/btQuaternion.h

@@ -173,10 +173,28 @@ public:
 		sqy = m_floats[1] * m_floats[1];
 		sqz = m_floats[2] * m_floats[2];
 		squ = m_floats[3] * m_floats[3];
-		rollX = btAtan2(2 * (m_floats[1] * m_floats[2] + m_floats[3] * m_floats[0]), squ - sqx - sqy + sqz);
+	    sarg = btScalar(-2.) * (m_floats[0] * m_floats[2] - m_floats[3] * m_floats[1]);
-		sarg = btScalar(-2.) * (m_floats[0] * m_floats[2] - m_floats[3] * m_floats[1]);
+		
-		pitchY = sarg <= btScalar(-1.0) ? btScalar(-0.5) * SIMD_PI: (sarg >= btScalar(1.0) ? btScalar(0.5) * SIMD_PI : btAsin(sarg));
+		// If the pitch angle is PI/2 or -PI/2, we can only compute
-		yawZ = btAtan2(2 * (m_floats[0] * m_floats[1] + m_floats[3] * m_floats[2]), squ + sqx - sqy - sqz);
+		// the sum roll + yaw.  However, any combination that gives
+		// the right sum will produce the correct orientation, so we
+		// set rollX = 0 and compute yawZ.
+		if (sarg <= -btScalar(0.99999))
+		{
+			pitchY = btScalar(-0.5)*SIMD_PI;
+			rollX  = 0;
+			yawZ   = btScalar(2) * btAtan2(m_floats[0],-m_floats[1]);
+		} else if (sarg >= btScalar(0.99999))
+		{
+			pitchY = btScalar(0.5)*SIMD_PI;
+			rollX  = 0;
+			yawZ   = btScalar(2) * btAtan2(-m_floats[0], m_floats[1]);
+		} else
+		{
+			pitchY = btAsin(sarg);
+			rollX = btAtan2(2 * (m_floats[1] * m_floats[2] + m_floats[3] * m_floats[0]), squ - sqx - sqy + sqz);
+			yawZ = btAtan2(2 * (m_floats[0] * m_floats[1] + m_floats[3] * m_floats[2]), squ + sqx - sqy - sqz);
+		}
 	}
 
   /**@brief Add two quaternions

src/LinearMath/btQuickprof.cpp

@@ -729,11 +729,9 @@ unsigned int btQuickprofGetCurrentThreadIndex2() {
 
 void	btEnterProfileZoneDefault(const char* name)
 {
-	CProfileManager::Start_Profile( name ); 
 }
 void	btLeaveProfileZoneDefault()
 {
-	CProfileManager::Stop_Profile(); 
 }
 
 

src/LinearMath/btQuickprof.h

@@ -70,11 +70,12 @@ void btSetCustomLeaveProfileZoneFunc(btLeaveProfileZoneFunc* leaveFunc);
 //#define BT_NO_PROFILE 1
 #endif //BT_NO_PROFILE
 
+const unsigned int BT_QUICKPROF_MAX_THREAD_COUNT = 64;
+
 #ifndef BT_NO_PROFILE
 //btQuickprofGetCurrentThreadIndex will return -1 if thread index cannot be determined, 
 //otherwise returns thread index in range [0..maxThreads]
 unsigned int btQuickprofGetCurrentThreadIndex2();
-const unsigned int BT_QUICKPROF_MAX_THREAD_COUNT = 64;
 
 #include <stdio.h>//@todo remove this, backwards compatibility
 

test/SharedMemory/CMakeLists.txt

@@ -29,6 +29,7 @@ ENDIF()
 	ADD_EXECUTABLE(Test_PhysicsClientServer
 		gtestwrap.cpp
 		../../examples/SharedMemory/PhysicsClient.cpp
+		../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp
 		../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
 		../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
 		../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp

test/SharedMemory/premake4.lua

@@ -172,6 +172,7 @@ project ("Test_PhysicsServerLoopBack")
 			"test.c",
 			"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp",
 			"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h",
+			"../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp",
 			"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
 			"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
 			"../../examples/SharedMemory/IKTrajectoryHelper.cpp",
@@ -264,6 +265,7 @@ end
 			"test.c",
 			"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp",
 			"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h",
+			"../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp",
 			"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
 			"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
 			"../../examples/SharedMemory/IKTrajectoryHelper.cpp",
@@ -372,6 +374,7 @@ project ("Test_PhysicsServerInProcessExampleBrowser")
 			"test.c",
 			"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp",
 			"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h",
+			"../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp",
 			"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
 			"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
 			"../../examples/SharedMemory/IKTrajectoryHelper.cpp",