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

6 files changed, 55 insertions, 36 deletions

diff --git a/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h b/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h
index 147d8c911..3050cadde 100644
--- a/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h
+++ b/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h
@@ -63,48 +63,35 @@ static bool UrdfFindMeshFile(
 		fn = fn.substr(drop_it_model.length());
 
 	std::list<std::string> shorter;
-	shorter.push_back("../..");
-	shorter.push_back("..");
-	shorter.push_back(".");
+	shorter.push_back("../../");
+	shorter.push_back("../");
+	shorter.push_back("./");
 	int cnt = urdf_path.size();
 	for (int i = 0; i < cnt; ++i)
 	{
 		if (urdf_path[i] == '/' || urdf_path[i] == '\\')
 		{
-			shorter.push_back(urdf_path.substr(0, i));
+			shorter.push_back(urdf_path.substr(0, i) + "/");
 		}
 	}
+	shorter.push_back("");  // no prefix
 	shorter.reverse();
 
 	std::string existing_file;
 
-	
-	
+	for (std::list<std::string>::iterator x = shorter.begin(); x != shorter.end(); ++x)
 	{
-		for (std::list<std::string>::iterator x = shorter.begin(); x != shorter.end(); ++x)
-		{
-			std::string attempt = *x + "/" + fn;
-			int f = fileIO->fileOpen(attempt.c_str(), "rb");
-			if (f<0)
-			{
-				//b3Printf("%s: tried '%s'", error_message_prefix.c_str(), attempt.c_str());
-				continue;
-			}
-			fileIO->fileClose(f);
-			existing_file = attempt;
-			//b3Printf("%s: found '%s'", error_message_prefix.c_str(), attempt.c_str());
-			break;
-		}
-	}
-	if (existing_file.empty())
-	{
-		std::string attempt = fn;
+		std::string attempt = *x + fn;
 		int f = fileIO->fileOpen(attempt.c_str(), "rb");
-		if (f>=0)
+		if (f<0)
 		{
-			existing_file = attempt;
-			fileIO->fileClose(f);
+			//b3Printf("%s: tried '%s'", error_message_prefix.c_str(), attempt.c_str());
+			continue;
 		}
+		fileIO->fileClose(f);
+		existing_file = attempt;
+		//b3Printf("%s: found '%s'", error_message_prefix.c_str(), attempt.c_str());
+		break;
 	}
 
 	if (existing_file.empty())
diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp
index 063959dfe..937ac28fb 100644
--- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp
+++ b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp
@@ -836,7 +836,7 @@ int  TinyRendererVisualShapeConverter::convertVisualShapes(
 			{
 				B3_PROFILE("convertURDFToVisualShape");
 				convertURDFToVisualShape(vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures, visualShape, fileIO, m_data->m_flags);
-				if ((vis->m_geometry.m_type == URDF_GEOM_PLANE) || (vis->m_geometry.m_type == URDF_GEOM_SPHERE))
+				if (vis->m_geometry.m_type == URDF_GEOM_PLANE)
 				{
 					int texWidth = 1024;
 					int texHeight = 1024;
diff --git a/examples/Utils/ChromeTraceUtil.cpp b/examples/Utils/ChromeTraceUtil.cpp
index a7eca7604..68a84effb 100644
--- a/examples/Utils/ChromeTraceUtil.cpp
+++ b/examples/Utils/ChromeTraceUtil.cpp
@@ -5,6 +5,7 @@
 #include "LinearMath/btAlignedObjectArray.h"
 #include "Bullet3Common/b3Logging.h"
 #include <stdio.h>
+#include <climits>
 
 struct btTiming
 {
@@ -111,16 +112,15 @@ struct btTimings
 			sprintf(newname, "%s%d", name, counter2++);
 
 #ifdef _WIN32
-
 			fprintf(gTimingFile, "{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%I64d.%s ,\"ph\":\"B\",\"name\":\"%s\",\"args\":{}},\n",
 					threadId, startTimeDiv1000, startTimeRem1000Str, newname);
 			fprintf(gTimingFile, "{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%I64d.%s ,\"ph\":\"E\",\"name\":\"%s\",\"args\":{}}",
 					threadId, endTimeDiv1000, endTimeRem1000Str, newname);
-
 #else
-			fprintf(gTimingFile, "{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%" PRIu64 ".%s ,\"ph\":\"B\",\"name\":\"%s\",\"args\":{}},\n",
+			// Note: on 64b build, PRIu64 resolves in 'lu' whereas timings ('ts') have to be printed as 'llu'.
+			fprintf(gTimingFile, "{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%llu.%s ,\"ph\":\"B\",\"name\":\"%s\",\"args\":{}},\n",
 					threadId, startTimeDiv1000, startTimeRem1000Str, newname);
-			fprintf(gTimingFile, "{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%" PRIu64 ".%s ,\"ph\":\"E\",\"name\":\"%s\",\"args\":{}}",
+			fprintf(gTimingFile, "{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%llu.%s ,\"ph\":\"E\",\"name\":\"%s\",\"args\":{}}",
 					threadId, endTimeDiv1000, endTimeRem1000Str, newname);
 #endif
 #endif
diff --git a/examples/pybullet/examples/constraint.py b/examples/pybullet/examples/constraint.py
index 70d4a6fe9..60b883a5c 100644
--- a/examples/pybullet/examples/constraint.py
+++ b/examples/pybullet/examples/constraint.py
@@ -13,7 +13,6 @@ p.setRealTimeSimulation(1)
 cid = p.createConstraint(cubeId, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0, 0, 1])
 print(cid)
 print(p.getConstraintUniqueId(0))
-prev = [0, 0, 1]
 a = -math.pi
 while 1:
   a = a + 0.01
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index f22366b65..d9c5beac7 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -6667,11 +6667,44 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject*
 		return NULL;
 	}
 
+	if (!rayFromObjList || !rayToObjList)
+	{
+		PyErr_SetString(SpamError, "rayFromPositions and rayToPositions must be not None.");
+		return NULL;
+	}
+
 	commandHandle = b3CreateRaycastBatchCommandInit(sm);
 	b3RaycastBatchSetNumThreads(commandHandle, numThreads);
 
-	if (rayFromObjList)
+
+	int raysAdded = 0;
+#ifdef PYBULLET_USE_NUMPY
+	// Faster approach if both inputs can be converted into ndarray.
+	if (PyArray_Check(rayFromObjList) && PyArray_Check(rayToObjList)) {
+		b3PushProfileTiming(sm, "extractPythonFromToNumpy");
+		PyArrayObject* rayFromPyArrayObj = (PyArrayObject*)PyArray_FROMANY(rayFromObjList, NPY_DOUBLE, 1, 2, NPY_ARRAY_CARRAY_RO);
+		PyArrayObject* rayToPyArrayObj = (PyArrayObject*)PyArray_FROMANY(rayToObjList, NPY_DOUBLE, 1, 2, NPY_ARRAY_CARRAY_RO);
+
+		// If there is error, this will fall back to default method and error messages will be reported there.
+		if (rayFromPyArrayObj && rayToPyArrayObj
+			&& PyArray_SAMESHAPE(rayFromPyArrayObj, rayToPyArrayObj)
+			&& PyArray_DIMS(rayFromPyArrayObj)[PyArray_NDIM(rayFromPyArrayObj) - 1] == 3)
+		{
+			int len = (PyArray_NDIM(rayFromPyArrayObj) == 2) ? PyArray_DIMS(rayFromPyArrayObj)[0] : 1;
+			if (len <= MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING)
+			{
+				b3RaycastBatchAddRays(sm, commandHandle, PyArray_DATA(rayFromPyArrayObj), PyArray_DATA(rayToPyArrayObj), len);
+				raysAdded = 1;
+			}
+		}
+		if (rayFromPyArrayObj) Py_DECREF(rayFromPyArrayObj);
+		if (rayToPyArrayObj) Py_DECREF(rayToPyArrayObj);
+		b3PopProfileTiming(sm);
+	}
+#endif
+	if (!raysAdded)
 	{
+		// go back to default method.
 		PyObject* seqRayFromObj = PySequence_Fast(rayFromObjList, "expected a sequence of rayFrom positions");
 		PyObject* seqRayToObj = PySequence_Fast(rayToObjList, "expected a sequence of 'rayTo' positions");
 
@@ -8020,7 +8053,7 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
 									 &bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB,
 									 &collisionShapeA, &collisionShapeB,
 									 &collisionShapePositionAObj, &collisionShapePositionBObj,
-									 &collisionShapeOrientationA, &collisionShapeOrientationBObj,
+									 &collisionShapeOrientationAObj, &collisionShapeOrientationBObj,
 									 &physicsClientId))
 		return NULL;
 
diff --git a/setup.py b/setup.py
index 746805c0d..3862c12b9 100644
--- a/setup.py
+++ b/setup.py
@@ -501,7 +501,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS:
 
 setup(
     name='pybullet',
-    version='2.8.5',
+    version='2.8.6',
     description=
     'Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
     long_description=