From 08b5e6d7669611704ef6862f02fec689e5d2a8be Mon Sep 17 00:00:00 2001
From: Erwin Coumans
 <erwincoumans@erwincoumans-macbookpro2.roam.corp.google.com>
Date: Mon, 30 Jul 2018 17:53:37 +0200
Subject: [PATCH] add collisionFilterPlugin to pybullet plugin, also apply
 quat->euler fix for singular cases

---
 examples/pybullet/premake4.lua |  1 +
 examples/pybullet/pybullet.c   | 28 ++++++++++++++++++++++------
 2 files changed, 23 insertions(+), 6 deletions(-)

diff --git a/examples/pybullet/premake4.lua b/examples/pybullet/premake4.lua
index 04380b9b0..60d1a1cf9 100644
--- a/examples/pybullet/premake4.lua
+++ b/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",
 			}
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index 836f06312..cae7bb816 100644
--- a/examples/pybullet/pybullet.c
+++ b/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));
-		rpy[2] = atan2(2 * (quat[0] * quat[1] + quat[3] * quat[2]),
-					   squ + sqx - sqy - sqz);
+		
+		// If the pitch angle is PI/2 or -PI/2, we can only compute
+		// 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 <= -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;