* Adjustment: Update Bullet version to 3.24.

Engine/lib/bullet/examples/SharedMemory/plugins/b3PluginAPI.h

@@ -0,0 +1,39 @@
+#ifndef B3_PLUGIN_API_H
+#define B3_PLUGIN_API_H
+
+#ifdef _WIN32
+#define B3_SHARED_API __declspec(dllexport)
+#elif defined(__GNUC__)
+#define B3_SHARED_API __attribute__((visibility("default")))
+#else
+#define B3_SHARED_API
+#endif
+
+#if defined(_WIN32)
+#define B3_API_ENTRY
+#define B3_API_CALL __cdecl
+#define B3_CALLBACK __cdecl
+#else
+#define B3_API_ENTRY
+#define B3_API_CALL
+#define B3_CALLBACK
+#endif
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+	/* Plugin API */
+	typedef B3_API_ENTRY int(B3_API_CALL* PFN_INIT)(struct b3PluginContext* context);
+	typedef B3_API_ENTRY void(B3_API_CALL* PFN_EXIT)(struct b3PluginContext* context);
+	typedef B3_API_ENTRY int(B3_API_CALL* PFN_EXECUTE)(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
+	typedef B3_API_ENTRY int(B3_API_CALL* PFN_TICK)(struct b3PluginContext* context);
+
+	typedef B3_API_ENTRY struct UrdfRenderingInterface*(B3_API_CALL* PFN_GET_RENDER_INTERFACE)(struct b3PluginContext* context);
+	typedef B3_API_ENTRY struct b3PluginCollisionInterface*(B3_API_CALL* PFN_GET_COLLISION_INTERFACE)(struct b3PluginContext* context);
+	typedef B3_API_ENTRY struct CommonFileIOInterface*(B3_API_CALL* PFN_GET_FILEIO_INTERFACE)(struct b3PluginContext* context);
+#ifdef __cplusplus
+}
+#endif
+
+#endif  //B3_PLUGIN_API_H

Engine/lib/bullet/examples/SharedMemory/plugins/b3PluginCollisionInterface.h

@@ -0,0 +1,32 @@
+#ifndef B3_PLUGIN_COLLISION_INTERFACE_H
+#define B3_PLUGIN_COLLISION_INTERFACE_H
+
+enum b3PluginCollisionFilterModes
+{
+	B3_FILTER_GROUPAMASKB_AND_GROUPBMASKA = 0,
+	B3_FILTER_GROUPAMASKB_OR_GROUPBMASKA
+};
+
+struct b3PluginCollisionInterface
+{
+	virtual void setBroadphaseCollisionFilter(
+		int objectUniqueIdA, int objectUniqueIdB,
+		int linkIndexA, int linkIndexB,
+		bool enableCollision) = 0;
+
+	virtual void removeBroadphaseCollisionFilter(
+		int objectUniqueIdA, int objectUniqueIdB,
+		int linkIndexA, int linkIndexB) = 0;
+
+	virtual int getNumRules() const = 0;
+
+	virtual void resetAll() = 0;
+
+	virtual int needsBroadphaseCollision(int objectUniqueIdA, int linkIndexA,
+										 int collisionFilterGroupA, int collisionFilterMaskA,
+										 int objectUniqueIdB, int linkIndexB,
+										 int collisionFilterGroupB, int collisionFilterMaskB,
+										 int filterMode) = 0;
+};
+
+#endif  //B3_PLUGIN_COLLISION_INTERFACE_H

Engine/lib/bullet/examples/SharedMemory/plugins/b3PluginContext.h

@@ -0,0 +1,31 @@
+#ifndef B3_PLUGIN_CONTEXT_H
+#define B3_PLUGIN_CONTEXT_H
+
+#include "../PhysicsClientC_API.h"
+
+struct b3PluginContext
+{
+	b3PhysicsClientHandle m_physClient;
+
+	//plugin can modify the m_userPointer to store persistent object pointer (class or struct instance etc)
+	void* m_userPointer;
+
+	//plugin can provide additional return data for executePluginCommand.
+	//Lifetime of this m_returnData pointer is minimum of 
+	//next call to the next executePluginCommand or plugin termination.
+	b3UserDataValue* m_returnData;
+
+	const struct b3VRControllerEvent* m_vrControllerEvents;
+	int m_numVRControllerEvents;
+	const struct b3KeyboardEvent* m_keyEvents;
+	int m_numKeyEvents;
+	const struct b3MouseEvent* m_mouseEvents;
+	int m_numMouseEvents;
+	const struct b3Notification* m_notifications;
+	int m_numNotifications;
+
+	//only used for grpc/processClientCommands
+	class PhysicsCommandProcessorInterface* m_rpcCommandProcessorInterface;
+};
+
+#endif  //B3_PLUGIN_CONTEXT_H

Engine/lib/bullet/examples/SharedMemory/plugins/b3PluginFileIOInterface.h

@@ -0,0 +1 @@
+#error

Engine/lib/bullet/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp

@@ -0,0 +1,204 @@
+
+//tinyRendererPlugin implements the TinyRenderer as a plugin
+//it is statically linked when using preprocessor #define STATIC_LINK_VR_PLUGIN
+//otherwise you can dynamically load it using pybullet.loadPlugin
+
+#include "collisionFilterPlugin.h"
+#include "../../SharedMemoryPublic.h"
+#include "../b3PluginContext.h"
+#include <stdio.h>
+#include "Bullet3Common/b3HashMap.h"
+
+#include "../b3PluginCollisionInterface.h"
+
+struct b3CustomCollisionFilter
+{
+	int m_objectUniqueIdA;
+	int m_linkIndexA;
+	int m_objectUniqueIdB;
+	int m_linkIndexB;
+	bool m_enableCollision;
+
+	B3_FORCE_INLINE unsigned int getHash() const
+	{
+		int obA = (m_objectUniqueIdA & 0xff);
+		int obB = ((m_objectUniqueIdB & 0xf) << 8);
+		int linkA = ((m_linkIndexA & 0xff) << 16);
+		int linkB = ((m_linkIndexB & 0xff) << 24);
+		long long int key = obA + obB + linkA + linkB;
+		// Thomas Wang's hash
+		key += ~(key << 15);
+		key ^= (key >> 10);
+		key += (key << 3);
+		key ^= (key >> 6);
+		key += ~(key << 11);
+		key ^= (key >> 16);
+		return (int) key;
+	}
+	bool equals(const b3CustomCollisionFilter& other) const
+	{
+		return m_objectUniqueIdA == other.m_objectUniqueIdA &&
+			   m_objectUniqueIdB == other.m_objectUniqueIdB &&
+			   m_linkIndexA == other.m_linkIndexA &&
+			   m_linkIndexB == other.m_linkIndexB;
+	}
+};
+
+struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface
+{
+	b3HashMap<b3CustomCollisionFilter, b3CustomCollisionFilter> m_customCollisionFilters;
+
+	virtual void setBroadphaseCollisionFilter(
+		int objectUniqueIdA, int objectUniqueIdB,
+		int linkIndexA, int linkIndexB,
+		bool enableCollision)
+	{
+		b3CustomCollisionFilter keyValue;
+		keyValue.m_objectUniqueIdA = objectUniqueIdA;
+		keyValue.m_linkIndexA = linkIndexA;
+		keyValue.m_objectUniqueIdB = objectUniqueIdB;
+		keyValue.m_linkIndexB = linkIndexB;
+		keyValue.m_enableCollision = enableCollision;
+
+		if (objectUniqueIdA > objectUniqueIdB)
+		{
+			b3Swap(keyValue.m_objectUniqueIdA, keyValue.m_objectUniqueIdB);
+			b3Swap(keyValue.m_linkIndexA, keyValue.m_linkIndexB);
+		}
+		if (objectUniqueIdA == objectUniqueIdB)
+		{
+			if (keyValue.m_linkIndexA > keyValue.m_linkIndexB)
+			{
+				b3Swap(keyValue.m_linkIndexA, keyValue.m_linkIndexB);
+			}
+		}
+
+		m_customCollisionFilters.insert(keyValue, keyValue);
+	}
+
+	virtual void removeBroadphaseCollisionFilter(
+		int objectUniqueIdA, int objectUniqueIdB,
+		int linkIndexA, int linkIndexB)
+	{
+		b3CustomCollisionFilter keyValue;
+		keyValue.m_objectUniqueIdA = objectUniqueIdA;
+		keyValue.m_linkIndexA = linkIndexA;
+		keyValue.m_objectUniqueIdB = objectUniqueIdB;
+		keyValue.m_linkIndexB = linkIndexB;
+
+		if (objectUniqueIdA > objectUniqueIdB)
+		{
+			b3Swap(keyValue.m_objectUniqueIdA, keyValue.m_objectUniqueIdB);
+			b3Swap(keyValue.m_linkIndexA, keyValue.m_linkIndexB);
+		}
+		if (objectUniqueIdA == objectUniqueIdB)
+		{
+			if (keyValue.m_linkIndexA > keyValue.m_linkIndexB)
+			{
+				b3Swap(keyValue.m_linkIndexA, keyValue.m_linkIndexB);
+			}
+		}
+
+		m_customCollisionFilters.remove(keyValue);
+	}
+
+	virtual int getNumRules() const
+	{
+		return m_customCollisionFilters.size();
+	}
+
+	virtual void resetAll()
+	{
+		m_customCollisionFilters.clear();
+	}
+
+	virtual int needsBroadphaseCollision(int objectUniqueIdA, int linkIndexA,
+										 int collisionFilterGroupA, int collisionFilterMaskA,
+										 int objectUniqueIdB, int linkIndexB,
+										 int collisionFilterGroupB, int collisionFilterMaskB,
+										 int filterMode)
+	{
+		//check and apply any custom rules for those objects/links
+		b3CustomCollisionFilter keyValue;
+		keyValue.m_objectUniqueIdA = objectUniqueIdA;
+		keyValue.m_linkIndexA = linkIndexA;
+		keyValue.m_objectUniqueIdB = objectUniqueIdB;
+		keyValue.m_linkIndexB = linkIndexB;
+
+		if (objectUniqueIdA > objectUniqueIdB)
+		{
+			b3Swap(keyValue.m_objectUniqueIdA, keyValue.m_objectUniqueIdB);
+			b3Swap(keyValue.m_linkIndexA, keyValue.m_linkIndexB);
+		}
+		if (objectUniqueIdA == objectUniqueIdB)
+		{
+			if (keyValue.m_linkIndexA > keyValue.m_linkIndexB)
+			{
+				b3Swap(keyValue.m_linkIndexA, keyValue.m_linkIndexB);
+			}
+		}
+
+		b3CustomCollisionFilter* filter = m_customCollisionFilters.find(keyValue);
+		if (filter)
+		{
+			return filter->m_enableCollision;
+		}
+
+		//otherwise use the default fallback
+
+		if (filterMode == B3_FILTER_GROUPAMASKB_AND_GROUPBMASKA)
+		{
+			bool collides = (collisionFilterGroupA & collisionFilterMaskB) != 0;
+			collides = collides && (collisionFilterGroupB & collisionFilterMaskA);
+			return collides;
+		}
+
+		if (filterMode == B3_FILTER_GROUPAMASKB_OR_GROUPBMASKA)
+		{
+			bool collides = (collisionFilterGroupA & collisionFilterMaskB) != 0;
+			collides = collides || (collisionFilterGroupB & collisionFilterMaskA);
+			return collides;
+		}
+		return false;
+	}
+};
+
+struct CollisionFilterMyClass
+{
+	int m_testData;
+
+	DefaultPluginCollisionInterface m_collisionFilter;
+
+	CollisionFilterMyClass()
+		: m_testData(42)
+	{
+	}
+	virtual ~CollisionFilterMyClass()
+	{
+	}
+};
+
+B3_SHARED_API int initPlugin_collisionFilterPlugin(struct b3PluginContext* context)
+{
+	CollisionFilterMyClass* obj = new CollisionFilterMyClass();
+	context->m_userPointer = obj;
+	return SHARED_MEMORY_MAGIC_NUMBER;
+}
+
+B3_SHARED_API struct b3PluginCollisionInterface* getCollisionInterface_collisionFilterPlugin(struct b3PluginContext* context)
+{
+	CollisionFilterMyClass* obj = (CollisionFilterMyClass*)context->m_userPointer;
+	return &obj->m_collisionFilter;
+}
+
+B3_SHARED_API int executePluginCommand_collisionFilterPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
+{
+	return 0;
+}
+
+B3_SHARED_API void exitPlugin_collisionFilterPlugin(struct b3PluginContext* context)
+{
+	CollisionFilterMyClass* obj = (CollisionFilterMyClass*)context->m_userPointer;
+	delete obj;
+	context->m_userPointer = 0;
+}

Engine/lib/bullet/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.h

@@ -0,0 +1,23 @@
+#ifndef COLLISION_FILTER_PLUGIN_H
+#define COLLISION_FILTER_PLUGIN_H
+
+#include "../b3PluginAPI.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+	//the following 3 APIs are required
+	B3_SHARED_API int initPlugin_collisionFilterPlugin(struct b3PluginContext* context);
+	B3_SHARED_API void exitPlugin_collisionFilterPlugin(struct b3PluginContext* context);
+	B3_SHARED_API int executePluginCommand_collisionFilterPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
+
+	//all the APIs below are optional
+	B3_SHARED_API struct b3PluginCollisionInterface* getCollisionInterface_collisionFilterPlugin(struct b3PluginContext* context);
+
+#ifdef __cplusplus
+};
+#endif
+
+#endif  //#define COLLISION_FILTER_PLUGIN_H

Engine/lib/bullet/examples/SharedMemory/plugins/collisionFilterPlugin/premake4.lua

@@ -0,0 +1,42 @@
+		
+
+project ("pybullet_collisionFilterPlugin")
+		language "C++"
+		kind "SharedLib"
+		
+		includedirs {".","../../../../src", "../../../../examples",
+		"../../../ThirdPartyLibs"}
+		defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
+	hasCL = findOpenCL("clew")
+
+	links{"BulletFileLoader", "Bullet3Common", "LinearMath"}
+
+
+	if os.is("MacOSX") then
+--		targetextension {"so"}
+		links{"Cocoa.framework","Python"}
+	end
+
+
+		files {
+			"collisionFilterPlugin.cpp",
+			"../../PhysicsClient.cpp",
+			"../../PhysicsClient.h",
+			"../../PhysicsClientSharedMemory.cpp",
+			"../../PhysicsClientSharedMemory.h",
+			"../../PhysicsClientSharedMemory_C_API.cpp",
+			"../../PhysicsClientSharedMemory_C_API.h",
+			"../../PhysicsClientC_API.cpp",
+			"../../PhysicsClientC_API.h",
+			"../../Win32SharedMemory.cpp",
+			"../../Win32SharedMemory.h",
+			"../../PosixSharedMemory.cpp",
+			"../../PosixSharedMemory.h",
+			"../../../Utils/b3Clock.cpp",
+			"../../../Utils/b3Clock.h",
+			"../../../Utils/b3ResourcePath.cpp",
+			"../../../Utils/b3ResourcePath.h",
+				}
+	
+	
+	

Engine/lib/bullet/examples/SharedMemory/plugins/eglPlugin/eglRendererPlugin.cpp

@@ -0,0 +1,97 @@
+
+#ifdef EGL_ADD_PYTHON_INIT
+#if defined(__APPLE__) && (!defined(B3_NO_PYTHON_FRAMEWORK))
+#include <Python/Python.h>
+#else
+#include <Python.h>
+#endif
+#endif  //EGL_ADD_PYTHON_INIT
+
+//eglRenderer plugin
+
+//see Bullet/examples/pybullet/examples/eglRendererTest.py
+
+#include "eglRendererPlugin.h"
+#include "eglRendererVisualShapeConverter.h"
+
+#include "../../SharedMemoryPublic.h"
+#include "../b3PluginContext.h"
+#include <stdio.h>
+
+struct EGLRendererPluginClass
+{
+	EGLRendererVisualShapeConverter m_renderer;
+	EGLRendererPluginClass()
+	{
+	}
+	virtual ~EGLRendererPluginClass()
+	{
+	}
+};
+
+B3_SHARED_API int initPlugin_eglRendererPlugin(struct b3PluginContext* context)
+{
+	EGLRendererPluginClass* obj = new EGLRendererPluginClass();
+	context->m_userPointer = obj;
+	return SHARED_MEMORY_MAGIC_NUMBER;
+}
+
+B3_SHARED_API int executePluginCommand_eglRendererPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
+{
+	return -1;
+}
+
+B3_SHARED_API void exitPlugin_eglRendererPlugin(struct b3PluginContext* context)
+{
+	EGLRendererPluginClass* obj = (EGLRendererPluginClass*)context->m_userPointer;
+	delete obj;
+	context->m_userPointer = 0;
+}
+
+//all the APIs below are optional
+B3_SHARED_API struct UrdfRenderingInterface* getRenderInterface_eglRendererPlugin(struct b3PluginContext* context)
+{
+	EGLRendererPluginClass* obj = (EGLRendererPluginClass*)context->m_userPointer;
+	return &obj->m_renderer;
+}
+
+#ifdef EGL_ADD_PYTHON_INIT
+
+static PyMethodDef eglMethods[] = {
+	{NULL, NULL, 0, NULL} /* Sentinel */
+};
+
+#if PY_MAJOR_VERSION >= 3
+static struct PyModuleDef moduledef = {
+	PyModuleDef_HEAD_INIT, "eglRenderer", /* m_name */
+	"eglRenderer for PyBullet ",          /* m_doc */
+	-1,                                   /* m_size */
+	eglMethods,                           /* m_methods */
+	NULL,                                 /* m_reload */
+	NULL,                                 /* m_traverse */
+	NULL,                                 /* m_clear */
+	NULL,                                 /* m_free */
+};
+#endif
+
+PyMODINIT_FUNC
+#if PY_MAJOR_VERSION >= 3
+PyInit_eglRenderer(void)
+#else
+initeglRenderer(void)
+#endif
+{
+	PyObject* m;
+#if PY_MAJOR_VERSION >= 3
+	m = PyModule_Create(&moduledef);
+#else
+	m = Py_InitModule3("eglRenderer", eglMethods, "eglRenderer for PyBullet");
+#endif
+
+#if PY_MAJOR_VERSION >= 3
+	if (m == NULL) return m;
+#else
+	if (m == NULL) return;
+#endif
+}
+#endif  //EGL_ADD_PYTHON_INIT

Engine/lib/bullet/examples/SharedMemory/plugins/eglPlugin/eglRendererPlugin.h

@@ -0,0 +1,23 @@
+#ifndef EGL_RENDERER_PLUGIN_H
+#define EGL_RENDERER_PLUGIN_H
+
+#include "../b3PluginAPI.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+	//initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load
+	B3_SHARED_API int initPlugin_eglRendererPlugin(struct b3PluginContext* context);
+	B3_SHARED_API void exitPlugin_eglRendererPlugin(struct b3PluginContext* context);
+	B3_SHARED_API int executePluginCommand_eglRendererPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
+
+	//all the APIs below are optional
+	B3_SHARED_API struct UrdfRenderingInterface* getRenderInterface_eglRendererPlugin(struct b3PluginContext* context);
+
+#ifdef __cplusplus
+};
+#endif
+
+#endif  //#define EGL_RENDERER_PLUGIN_H

Engine/lib/bullet/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h

@@ -0,0 +1,75 @@
+#ifndef EGL_RENDERER_VISUAL_SHAPE_CONVERTER_H
+#define EGL_RENDERER_VISUAL_SHAPE_CONVERTER_H
+
+#include "../../../Importers/ImportURDFDemo/UrdfRenderingInterface.h"
+
+struct EGLRendererVisualShapeConverter : public UrdfRenderingInterface
+{
+	struct EGLRendererVisualShapeConverterInternalData* m_data;
+
+	EGLRendererVisualShapeConverter();
+
+	virtual ~EGLRendererVisualShapeConverter();
+
+	virtual int convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int orgGraphicsUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO);
+
+	virtual int getNumVisualShapes(int bodyUniqueId);
+
+	virtual int getVisualShapesData(int bodyUniqueId, int shapeIndex, struct b3VisualShapeData* shapeData);
+
+	virtual int registerShapeAndInstance(const b3VisualShapeData& visualShape, const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType, int textureId, int orgGraphicsUniqueId, int bodyUniqueId, int linkIndex);
+
+	virtual void updateShape(int shapeUniqueId, const btVector3* vertices, int numVertices, const btVector3* normals, int numNormals);
+
+	virtual void changeRGBAColor(int bodyUniqueId, int linkIndex, int shapeIndex, const double rgbaColor[4]);
+
+	virtual void changeInstanceFlags(int bodyUniqueId, int linkIndex, int shapeIndex, int flags);
+
+	virtual void changeShapeTexture(int objectUniqueId, int linkIndex, int shapeIndex, int textureUniqueId);
+
+	virtual void removeVisualShape(int shapeUid);
+
+	virtual void setUpAxis(int axis);
+
+	virtual void resetCamera(float camDist, float yaw, float pitch, float camPosX, float camPosY, float camPosZ);
+
+	virtual void clearBuffers(struct TGAColor& clearColor);
+
+	virtual void resetAll();
+
+	virtual void getWidthAndHeight(int& width, int& height);
+	virtual void setWidthAndHeight(int width, int height);
+	virtual void setLightDirection(float x, float y, float z);
+	virtual void setLightColor(float x, float y, float z);
+	virtual void setLightDistance(float dist);
+	virtual void setLightAmbientCoeff(float ambientCoeff);
+	virtual void setLightDiffuseCoeff(float diffuseCoeff);
+	virtual void setLightSpecularCoeff(float specularCoeff);
+	virtual void setShadow(bool hasShadow);
+	virtual void setFlags(int flags);
+
+	virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);
+	void copyCameraImageDataGL(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);
+
+	virtual void render();
+	virtual void render(const float viewMat[16], const float projMat[16]);
+
+	virtual int loadTextureFile(const char* filename, struct CommonFileIOInterface* fileIO);
+	virtual int registerTexture(unsigned char* texels, int width, int height);
+	virtual int registerTextureInternal(unsigned char* texels, int width, int height);
+	
+
+	virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16]);
+	virtual void setProjectiveTexture(bool useProjectiveTexture);
+
+	virtual void syncTransform(int shapeUid, const class btTransform& worldTransform, const class btVector3& localScaling);
+
+	virtual void mouseMoveCallback(float x, float y);
+	virtual void mouseButtonCallback(int button, int state, float x, float y);
+
+	virtual bool getCameraInfo(int* width, int* height, float viewMatrix[16], float projectionMatrix[16], float camUp[3], float camForward[3], float hor[3], float vert[3], float* yaw, float* pitch, float* camDist, float cameraTarget[3]) const;
+	
+
+};
+
+#endif  //EGL_RENDERER_VISUAL_SHAPE_CONVERTER_H

Engine/lib/bullet/examples/SharedMemory/plugins/eglPlugin/premake4.lua

@@ -0,0 +1,74 @@
+		
+
+project ("pybullet_eglRendererPlugin")
+		language "C++"
+		kind "SharedLib"
+		initEGL()
+		
+		includedirs {".","../../../../src", "../../../../examples",
+		"../../../ThirdPartyLibs", "../../../ThirdPartyLibs/glad"}
+		defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER", "STB_AGAIN"}
+	hasCL = findOpenCL("clew")
+
+	links{"BulletCollision", "Bullet3Common", "LinearMath"}
+
+	initOpenGL()
+
+	if os.is("Windows") then
+		files {"../../../OpenGLWindow/Win32OpenGLWindow.cpp",
+		"../../../OpenGLWindow/Win32Window.cpp",}
+		
+	end
+	if os.is("MacOSX") then
+--		targetextension {"so"}
+		links{"Cocoa.framework"}
+	end
+
+  if os.is("Linux") then
+	  files {"../../../OpenGLWindow/EGLOpenGLWindow.cpp"}
+
+  end
+
+		files {
+			"eglRendererPlugin.cpp",
+			"eglRendererPlugin.h",
+			"eglRendererVisualShapeConverter.cpp",
+			"eglRendererVisualShapeConverter.h",
+			"../../../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
+			"../../../Importers/ImportColladaDemo/LoadMeshFromCollada.h",
+			"../../../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+			"../../../Importers/ImportMeshUtility/b3ImportMeshUtility.h",
+			"../../../Importers/ImportObjDemo/LoadMeshFromObj.cpp",
+			"../../../Importers/ImportObjDemo/LoadMeshFromObj.h",
+			"../../../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
+			"../../../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h",
+			"../../../TinyRenderer/geometry.cpp",
+			"../../../TinyRenderer/model.cpp",
+			"../../../TinyRenderer/our_gl.cpp",
+			"../../../TinyRenderer/tgaimage.cpp",
+			"../../../TinyRenderer/TinyRenderer.cpp",
+			"../../../ThirdPartyLibs/glad/gl.c",
+			"../../../ThirdPartyLibs/glad/egl.c",
+			"../../../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
+			"../../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
+			"../../../ThirdPartyLibs/stb_image/stb_image.cpp",
+			"../../../ThirdPartyLibs/stb_image/stb_image.h",
+			"../../../ThirdPartyLibs/stb_image/stb_image_write.cpp",
+			"../../../ThirdPartyLibs/tinyxml2/tinyxml2.cpp",
+			"../../../ThirdPartyLibs/tinyxml2/tinyxml2.h",
+			"../../../OpenGLWindow/SimpleCamera.cpp",
+			"../../../OpenGLWindow/SimpleCamera.h",
+			"../../../OpenGLWindow/GLInstancingRenderer.cpp",
+			"../../../OpenGLWindow/GLInstancingRenderer.h",
+			"../../../OpenGLWindow/LoadShader.cpp",
+			"../../../OpenGLWindow/LoadShader.h",
+			"../../../OpenGLWindow/GLRenderToTexture.cpp",
+			"../../../OpenGLWindow/GLRenderToTexture.h",
+			"../../../Utils/b3Clock.cpp",
+			"../../../Utils/b3Clock.h",
+			"../../../Utils/b3ResourcePath.cpp",
+			"../../../Utils/b3ResourcePath.h",
+			}
+	
+	
+	

Engine/lib/bullet/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp

@@ -0,0 +1,700 @@
+#include "fileIOPlugin.h"
+#include "../../SharedMemoryPublic.h"
+#include "../b3PluginContext.h"
+#include <stdio.h>
+#include "../../../CommonInterfaces/CommonFileIOInterface.h"
+#include "../../../Utils/b3ResourcePath.h"
+#include "Bullet3Common/b3HashMap.h"
+#include <string.h> //memcpy/strlen
+#ifndef B3_EXCLUDE_DEFAULT_FILEIO
+#include "../../../Utils/b3BulletDefaultFileIO.h"
+#endif //B3_EXCLUDE_DEFAULT_FILEIO
+
+
+#ifdef B3_USE_ZIPFILE_FILEIO
+#include "zipFileIO.h"
+#endif //B3_USE_ZIPFILE_FILEIO
+
+
+#ifdef B3_USE_CNS_FILEIO
+#include "CNSFileIO.h"
+#endif //B3_USE_CNS_FILEIO
+
+#define B3_MAX_FILEIO_INTERFACES 1024
+
+struct WrapperFileHandle
+{
+	CommonFileIOInterface* childFileIO;
+	int m_childFileHandle;
+};
+
+struct InMemoryFile
+{
+	char* m_buffer;
+	int m_fileSize;
+};
+
+struct InMemoryFileAccessor
+{
+	InMemoryFile* m_file;
+	int m_curPos;
+};
+
+struct InMemoryFileIO : public CommonFileIOInterface
+{
+	b3HashMap<b3HashString,InMemoryFile*> m_fileCache;
+	InMemoryFileAccessor m_fileHandles[B3_MAX_FILEIO_INTERFACES];
+	int m_numAllocs;
+	int m_numFrees;
+
+	InMemoryFileIO()
+		:CommonFileIOInterface(eInMemoryFileIO,0)
+	{
+		m_numAllocs=0;
+		m_numFrees=0;
+
+		for (int i=0;i<B3_FILEIO_MAX_FILES;i++)
+		{
+			m_fileHandles[i].m_curPos = 0;
+			m_fileHandles[i].m_file = 0;
+		}
+	}
+
+	virtual ~InMemoryFileIO()
+	{
+		clearCache();
+		if (m_numAllocs != m_numFrees)
+		{
+			printf("Error: InMemoryFile::~InMemoryFileIO (numAllocs %d numFrees %d\n", m_numAllocs, m_numFrees);
+		}
+	}
+	void clearCache()
+	{
+		for (int i=0;i<m_fileCache.size();i++)
+		{
+			InMemoryFile** memPtr = m_fileCache.getAtIndex(i);
+			if (memPtr && *memPtr)
+			{
+				InMemoryFile* mem = *memPtr;
+				freeBuffer(mem->m_buffer);
+				m_numFrees++;
+				delete (mem);
+				m_numFrees++;
+			}
+		}
+		m_fileCache.clear();
+	}
+
+	char* allocateBuffer(int len)
+	{
+		char* buffer = 0;
+		if (len)
+		{
+			m_numAllocs++;
+			buffer = new char[len];
+		}
+		return buffer;
+	}
+
+	void freeBuffer(char* buffer)
+	{
+		delete[] buffer;
+	}
+
+	virtual int registerFile(const char* fileName, char* buffer, int len)
+	{
+		m_numAllocs++;
+		InMemoryFile* f = new InMemoryFile();
+		f->m_buffer = buffer;
+		f->m_fileSize = len;
+		b3HashString key(fileName);
+		m_fileCache.insert(key,f);
+		return 0;
+	}
+
+	void removeFileFromCache(const char* fileName)
+	{
+		InMemoryFile* f = getInMemoryFile(fileName);
+		if (f)
+		{
+			m_fileCache.remove(fileName);
+			freeBuffer(f->m_buffer);
+			delete (f);
+		}
+	}
+
+	InMemoryFile* getInMemoryFile(const char* fileName)
+	{
+		InMemoryFile** fPtr = m_fileCache[fileName];
+		if (fPtr && *fPtr)
+		{
+			return *fPtr;
+		}
+		return 0;
+	}
+
+	virtual int fileOpen(const char* fileName, const char* mode)
+	{
+		//search a free slot
+		int slot = -1;
+		for (int i=0;i<B3_FILEIO_MAX_FILES;i++)
+		{
+			if (m_fileHandles[i].m_file==0)
+			{
+				slot=i;
+				break;
+			}
+		}
+		if (slot>=0)
+		{
+			InMemoryFile* f = getInMemoryFile(fileName);
+			if (f)
+			{
+				m_fileHandles[slot].m_curPos = 0;
+				m_fileHandles[slot].m_file = f;
+			} else
+			{
+				slot=-1;
+			}
+		}
+		//printf("InMemoryFileIO fileOpen %s, %d\n", fileName, slot);
+		return slot;
+	}
+	virtual int fileRead(int fileHandle, char* destBuffer, int numBytes)
+	{
+		if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES)
+		{
+			InMemoryFileAccessor& f = m_fileHandles[fileHandle];
+			if (f.m_file)
+			{
+				//if (numBytes>1)
+				//	printf("curPos = %d\n", f.m_curPos);
+				if (f.m_curPos+numBytes <= f.m_file->m_fileSize)
+				{
+					memcpy(destBuffer,f.m_file->m_buffer+f.m_curPos,numBytes);
+					f.m_curPos+=numBytes;
+					//if (numBytes>1)
+					//	printf("read %d bytes, now curPos = %d\n", numBytes, f.m_curPos);
+					return numBytes;
+				} else
+				{
+					if (numBytes!=1)
+					{
+						printf("InMemoryFileIO::fileRead Attempt to read beyond end of file\n");
+					}
+				}
+				
+			}
+		}
+		return 0;
+	}
+
+	virtual int fileWrite(int fileHandle,const char* sourceBuffer, int numBytes)
+	{
+		return 0;
+	}
+	virtual void fileClose(int fileHandle)
+	{
+		if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES)
+		{
+			InMemoryFileAccessor& f = m_fileHandles[fileHandle];
+			if (f.m_file)
+			{
+				m_fileHandles[fileHandle].m_file = 0;
+				m_fileHandles[fileHandle].m_curPos = 0;
+				//printf("InMemoryFileIO fileClose %d\n", fileHandle);
+			}
+		}
+	}
+	virtual bool findResourcePath(const char* fileName,  char* resourcePathOut, int resourcePathMaxNumBytes)
+	{
+		InMemoryFile* f = getInMemoryFile(fileName);
+		int fileNameLen = strlen(fileName);
+		if (f && fileNameLen<(resourcePathMaxNumBytes-1))
+		{
+			memcpy(resourcePathOut, fileName, fileNameLen);
+			resourcePathOut[fileNameLen]=0;
+			return true;
+		} 
+		return false;
+	}
+	virtual char* readLine(int fileHandle, char* destBuffer, int numBytes)
+	{
+		int numRead = 0;
+		int endOfFile = 0;
+		if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES )
+		{
+			InMemoryFileAccessor& f = m_fileHandles[fileHandle];
+			if (f.m_file)
+			{
+				//return ::fgets(destBuffer, numBytes, m_fileHandles[fileHandle]);
+				char c = 0;
+				do
+				{
+					int bytesRead = fileRead(fileHandle,&c,1);
+					if (bytesRead != 1)
+					{
+						endOfFile = 1;
+						c=0;
+					}
+					if (c && c!='\n')
+					{
+						if (c!=13)
+						{
+							destBuffer[numRead++]=c;
+						} else
+						{
+							destBuffer[numRead++]=0;
+						}
+					}
+				} while (c != 0 && c != '\n' && numRead<(numBytes-1));
+			}
+		}
+		if (numRead==0 && endOfFile)
+		{
+			return 0;
+		}
+
+		if (numRead<numBytes)
+		{
+			if (numRead >=0)
+			{
+				destBuffer[numRead]=0;
+			}
+			return &destBuffer[0];
+		} else
+		{
+			if (endOfFile==0)
+			{
+				printf("InMemoryFileIO::readLine readLine warning: numRead=%d, numBytes=%d\n", numRead, numBytes);
+			}
+		}
+		return 0;
+	}
+	virtual int getFileSize(int fileHandle)
+	{
+		if (fileHandle>=0 && fileHandle < B3_FILEIO_MAX_FILES )
+		{
+			
+			InMemoryFileAccessor& f = m_fileHandles[fileHandle];
+			if (f.m_file)
+			{
+				return f.m_file->m_fileSize;
+			}
+		}
+		return 0;
+	}
+
+	virtual void enableFileCaching(bool enable)
+	{
+		(void)enable;
+	}
+};
+
+struct WrapperFileIO : public CommonFileIOInterface
+{
+	CommonFileIOInterface* m_availableFileIOInterfaces[B3_MAX_FILEIO_INTERFACES];
+	int m_numWrapperInterfaces;
+
+	WrapperFileHandle m_wrapperFileHandles[B3_MAX_FILEIO_INTERFACES];
+	InMemoryFileIO m_cachedFiles;
+	bool m_enableFileCaching;
+
+	WrapperFileIO()
+		:CommonFileIOInterface(0,0),
+		m_numWrapperInterfaces(0),
+		m_enableFileCaching(true)
+	{
+		for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
+		{
+			m_availableFileIOInterfaces[i]=0;
+			m_wrapperFileHandles[i].childFileIO=0;
+			m_wrapperFileHandles[i].m_childFileHandle=0;
+		}
+		//addFileIOInterface(&m_cachedFiles);
+	}
+
+	virtual ~WrapperFileIO()
+	{
+		for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
+		{
+			removeFileIOInterface(i);
+		}
+		m_cachedFiles.clearCache();
+	}
+
+	int addFileIOInterface(CommonFileIOInterface* fileIO)
+	{
+		int result = -1;
+		for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
+		{
+			if (m_availableFileIOInterfaces[i]==0)
+			{
+				m_availableFileIOInterfaces[i]=fileIO;
+				result = i;
+				break;
+			}
+		}
+		return result;
+	}
+
+	CommonFileIOInterface* getFileIOInterface(int fileIOIndex)
+	{
+		if (fileIOIndex>=0 && fileIOIndex<B3_MAX_FILEIO_INTERFACES)
+		{
+			return m_availableFileIOInterfaces[fileIOIndex];
+		}
+		return 0;
+	}
+	void removeFileIOInterface(int fileIOIndex)
+	{
+		if (fileIOIndex>=0 && fileIOIndex<B3_MAX_FILEIO_INTERFACES)
+		{
+			if (m_availableFileIOInterfaces[fileIOIndex])
+			{
+				delete m_availableFileIOInterfaces[fileIOIndex];
+				m_availableFileIOInterfaces[fileIOIndex]=0;
+			}
+		}
+	}
+
+	virtual int fileOpen(const char* fileName, const char* mode)
+	{
+		
+		//find an available wrapperFileHandle slot
+		int wrapperFileHandle=-1;
+		int slot = -1;
+		for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
+		{
+			if (m_wrapperFileHandles[i].childFileIO==0)
+			{
+				slot=i;
+				break;
+			}
+		}
+		if (slot>=0)
+		{
+			//first check the cache
+			int cacheHandle = m_cachedFiles.fileOpen(fileName, mode);
+			if (cacheHandle>=0)
+			{
+				m_cachedFiles.fileClose(cacheHandle);
+			}
+			if (cacheHandle<0)
+			{
+				for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
+				{
+					CommonFileIOInterface* childFileIO=m_availableFileIOInterfaces[i];
+					if (childFileIO)
+					{
+						int childHandle = childFileIO->fileOpen(fileName, mode);
+						if (childHandle>=0)
+						{
+							if (m_enableFileCaching)
+							{
+								int fileSize = childFileIO->getFileSize(childHandle);
+								char* buffer = 0;
+								if (fileSize)
+								{
+									buffer = m_cachedFiles.allocateBuffer(fileSize);
+									if (buffer)
+									{
+										int readBytes = childFileIO->fileRead(childHandle, buffer, fileSize);
+										if (readBytes!=fileSize)
+										{
+											if (readBytes<fileSize)
+												{
+												fileSize = readBytes;
+											} else
+											{
+												printf("WrapperFileIO error: reading more bytes (%d) then reported file size (%d) of file %s.\n", readBytes, fileSize, fileName);
+											}
+										}
+									} else
+									{
+										fileSize=0;
+									}
+								}
+
+								//potentially register a zero byte file, or files that only can be read partially
+							
+								m_cachedFiles.registerFile(fileName, buffer, fileSize);
+							}
+							
+							childFileIO->fileClose(childHandle);
+							break;
+						}
+					}
+				}
+			}
+			
+			{
+				int childHandle = m_cachedFiles.fileOpen(fileName, mode);
+				if (childHandle>=0)
+				{
+					wrapperFileHandle = slot;
+					m_wrapperFileHandles[slot].childFileIO = &m_cachedFiles;
+					m_wrapperFileHandles[slot].m_childFileHandle = childHandle;
+				} else
+				{
+					//figure out what wrapper interface to use
+					//use the first one that can open the file
+					for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
+					{
+						CommonFileIOInterface* childFileIO=m_availableFileIOInterfaces[i];
+						if (childFileIO)
+						{
+							int childHandle = childFileIO->fileOpen(fileName, mode);
+							if (childHandle>=0)
+							{
+								wrapperFileHandle = slot;
+								m_wrapperFileHandles[slot].childFileIO = childFileIO;
+								m_wrapperFileHandles[slot].m_childFileHandle = childHandle;
+								break;
+							}
+						}
+					}
+				}
+			}
+		}
+		return wrapperFileHandle;
+	}
+
+	virtual int fileRead(int fileHandle, char* destBuffer, int numBytes)
+	{
+		int fileReadResult=-1;
+		if (fileHandle>=0 && fileHandle<B3_MAX_FILEIO_INTERFACES)
+		{
+			if (m_wrapperFileHandles[fileHandle].childFileIO)
+			{
+				fileReadResult = m_wrapperFileHandles[fileHandle].childFileIO->fileRead(
+					m_wrapperFileHandles[fileHandle].m_childFileHandle, destBuffer, numBytes);
+			}
+		}
+		return fileReadResult;
+	}
+
+	virtual int fileWrite(int fileHandle,const char* sourceBuffer, int numBytes)
+	{
+		//todo
+		return -1;
+	}
+	virtual void fileClose(int fileHandle)
+	{
+		if (fileHandle>=0 && fileHandle<B3_MAX_FILEIO_INTERFACES)
+		{
+			if (m_wrapperFileHandles[fileHandle].childFileIO)
+			{
+				m_wrapperFileHandles[fileHandle].childFileIO->fileClose(
+					m_wrapperFileHandles[fileHandle].m_childFileHandle);
+				m_wrapperFileHandles[fileHandle].childFileIO = 0;
+				m_wrapperFileHandles[fileHandle].m_childFileHandle = -1;
+			}
+		}
+	}
+	virtual bool findResourcePath(const char* fileName,  char* resourcePathOut, int resourcePathMaxNumBytes)
+	{
+		if (m_cachedFiles.findResourcePath(fileName, resourcePathOut, resourcePathMaxNumBytes))
+			return true;
+
+		bool found = false;
+		for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
+		{
+			if (m_availableFileIOInterfaces[i])
+			{
+				found = m_availableFileIOInterfaces[i]->findResourcePath(fileName, resourcePathOut, resourcePathMaxNumBytes);
+			}
+			if (found)
+				break;
+		}
+		return found;
+	}
+	virtual char* readLine(int fileHandle, char* destBuffer, int numBytes)
+	{
+		char* result = 0;
+
+		if (fileHandle>=0 && fileHandle<B3_MAX_FILEIO_INTERFACES)
+		{
+			if (m_wrapperFileHandles[fileHandle].childFileIO)
+			{
+				result = m_wrapperFileHandles[fileHandle].childFileIO->readLine(
+					m_wrapperFileHandles[fileHandle].m_childFileHandle,
+					destBuffer, numBytes);
+			}
+		}
+		return result;
+	}
+	virtual int getFileSize(int fileHandle)
+	{
+		int numBytes = 0;
+
+		if (fileHandle>=0 && fileHandle<B3_MAX_FILEIO_INTERFACES)
+		{
+			if (m_wrapperFileHandles[fileHandle].childFileIO)
+			{
+				numBytes = m_wrapperFileHandles[fileHandle].childFileIO->getFileSize(
+					m_wrapperFileHandles[fileHandle].m_childFileHandle);
+			}
+		}
+		return numBytes;
+	}
+
+	virtual void enableFileCaching(bool enable)
+	{
+		m_enableFileCaching = enable;
+		if (!enable)
+		{
+			m_cachedFiles.clearCache();
+		}
+	}
+
+};
+
+
+struct FileIOClass
+{
+	int m_testData;
+
+	WrapperFileIO m_fileIO;
+
+	FileIOClass()
+		: m_testData(42),
+		m_fileIO()
+	{
+	}
+	virtual ~FileIOClass()
+	{
+	}
+};
+
+B3_SHARED_API int initPlugin_fileIOPlugin(struct b3PluginContext* context)
+{
+	FileIOClass* obj = new FileIOClass();
+	context->m_userPointer = obj;
+	
+#ifndef B3_EXCLUDE_DEFAULT_FILEIO
+	obj->m_fileIO.addFileIOInterface(new b3BulletDefaultFileIO());
+#endif //B3_EXCLUDE_DEFAULT_FILEIO
+	
+
+	return SHARED_MEMORY_MAGIC_NUMBER;
+}
+
+
+B3_SHARED_API int executePluginCommand_fileIOPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
+{
+	int result=-1;
+
+	FileIOClass* obj = (FileIOClass*)context->m_userPointer;
+
+	printf("text argument:%s\n", arguments->m_text);
+	printf("int args: [");
+	
+	//remove a fileIO type
+	if (arguments->m_numInts==1)
+	{
+		int fileIOIndex = arguments->m_ints[0];
+		obj->m_fileIO.removeFileIOInterface(fileIOIndex);
+	}
+
+	if (arguments->m_numInts==2)
+	{
+		int action = arguments->m_ints[0];
+		switch (action)
+		{
+			case eAddFileIOAction:
+			{
+				//if the fileIO already exists, skip this action
+				int fileIOType = arguments->m_ints[1];
+				bool alreadyExists = false;
+
+				for (int i=0;i<B3_MAX_FILEIO_INTERFACES;i++)
+				{
+					CommonFileIOInterface* fileIO = obj->m_fileIO.getFileIOInterface(i);
+					if (fileIO)
+					{
+						if (fileIO->m_fileIOType == fileIOType)
+						{
+							if (fileIO->m_pathPrefix && strcmp(fileIO->m_pathPrefix,arguments->m_text)==0)
+							{
+								result = i;
+								alreadyExists = true;
+								break;
+							}
+						}
+					}
+				}
+				
+
+				//create new fileIO interface
+				if (!alreadyExists)
+				{
+					switch (fileIOType)
+					{
+						case ePosixFileIO:
+						{
+	#ifdef B3_EXCLUDE_DEFAULT_FILEIO
+							printf("ePosixFileIO is not enabled in this build.\n");
+	#else
+							result = obj->m_fileIO.addFileIOInterface(new b3BulletDefaultFileIO(ePosixFileIO, arguments->m_text));
+	#endif
+							break;
+						}
+						case eZipFileIO:
+						{
+	#ifdef B3_USE_ZIPFILE_FILEIO
+							if (arguments->m_text[0])
+							{
+								result = obj->m_fileIO.addFileIOInterface(new ZipFileIO(eZipFileIO, arguments->m_text, &obj->m_fileIO));
+							}
+	#else
+							printf("eZipFileIO is not enabled in this build.\n");
+	#endif
+							break;
+						}
+						case eCNSFileIO:
+						{
+	#ifdef B3_USE_CNS_FILEIO
+							result = obj->m_fileIO.addFileIOInterface(new CNSFileIO(eCNSFileIO, arguments->m_text));
+	#else//B3_USE_CNS_FILEIO
+							printf("CNSFileIO is not enabled in this build.\n");
+	#endif //B3_USE_CNS_FILEIO
+							break;
+						}
+						default:
+						{
+						}
+					}//switch (fileIOType)
+				}//if (!alreadyExists)
+				break;
+			}
+			case eRemoveFileIOAction:
+
+			{
+				//remove fileIO interface
+				int fileIOIndex = arguments->m_ints[1];
+				obj->m_fileIO.removeFileIOInterface(fileIOIndex);
+				result = fileIOIndex;
+				break;
+			}
+			default:
+			{
+				printf("executePluginCommand_fileIOPlugin: unknown action\n");
+			}
+		}
+	}
+	return result;
+}
+
+B3_SHARED_API struct CommonFileIOInterface* getFileIOFunc_fileIOPlugin(struct b3PluginContext* context)
+{
+	FileIOClass* obj = (FileIOClass*)context->m_userPointer;
+	return &obj->m_fileIO;
+}
+
+B3_SHARED_API void exitPlugin_fileIOPlugin(struct b3PluginContext* context)
+{
+	FileIOClass* obj = (FileIOClass*)context->m_userPointer;
+	delete obj;
+	context->m_userPointer = 0;
+}

Engine/lib/bullet/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.h

@@ -0,0 +1,24 @@
+#ifndef FILE_IO_PLUGIN_H
+#define FILE_IO_PLUGIN_H
+
+#include "../b3PluginAPI.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+	//initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load
+	B3_SHARED_API int initPlugin_fileIOPlugin(struct b3PluginContext* context);
+	B3_SHARED_API void exitPlugin_fileIOPlugin(struct b3PluginContext* context);
+	B3_SHARED_API int executePluginCommand_fileIOPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
+
+	//all the APIs below are optional
+	B3_SHARED_API struct CommonFileIOInterface* getFileIOFunc_fileIOPlugin(struct b3PluginContext* context);
+	
+
+#ifdef __cplusplus
+};
+#endif
+
+#endif  //#define FILE_IO_PLUGIN_H

Engine/lib/bullet/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h

@@ -0,0 +1,305 @@
+
+#include "minizip/unzip.h"
+
+#define B3_ZIP_FILEIO_MAX_FILES   1024
+
+struct ZipFileIO : public CommonFileIOInterface
+{
+	std::string m_zipfileName;
+
+	unzFile	m_fileHandles[B3_ZIP_FILEIO_MAX_FILES ];
+	int m_numFileHandles;
+	unzFile m_zipfile;
+	voidpf m_stream;
+	unz_global_info m_global_info;
+	bool m_memoryFile;
+	b3AlignedObjectArray<char> m_buffer;
+	
+	ZipFileIO(int fileIOType, const char* zipfileName, CommonFileIOInterface* wrapperFileIO)
+		:CommonFileIOInterface(fileIOType,0),
+		m_zipfileName(zipfileName),
+		m_numFileHandles(0),
+		m_stream(0),
+		m_memoryFile(false)
+	{
+		m_pathPrefix = m_zipfileName.c_str();
+		for (int i=0;i<B3_ZIP_FILEIO_MAX_FILES ;i++)
+		{
+			m_fileHandles[i]=0;
+		}
+		m_zipfile = unzOpen(m_zipfileName.c_str());
+
+		if (m_zipfile == 0)
+		{
+			int fileIO = wrapperFileIO->fileOpen(m_zipfileName.c_str(), "rb");
+			if (fileIO >= 0)
+			{
+				int stream_size = wrapperFileIO->getFileSize(fileIO);
+				m_buffer.resize(stream_size);
+				int read_bytes = wrapperFileIO->fileRead(fileIO, &m_buffer[0], stream_size);
+				b3Assert(read_bytes == stream_size);
+				if (read_bytes != stream_size)
+				{
+					printf("Error: mismatch reading file %s, expected %d bytes, read %d\n", m_zipfileName.c_str(), stream_size, read_bytes);
+				}
+				zlib_filefunc_def api;  // callbacks for in-mem file
+				
+				m_stream = mem_simple_create_file(&api, &m_buffer[0], stream_size);
+
+				m_zipfile = unzAttach(m_stream, &api);
+				m_memoryFile = true;
+				wrapperFileIO->fileClose(fileIO);
+			}
+		}
+	}
+
+	
+		
+	static bool FileIOPluginFindFile(void* userPtr, const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen)
+	{
+		ZipFileIO* fileIo = (ZipFileIO*) userPtr;
+		return fileIo->findFile(orgFileName, relativeFileName, maxRelativeFileNameMaxLen);
+	}
+
+	void closeZipFile()
+	{
+		if (m_zipfile)
+		{
+			if (m_memoryFile)
+			{
+				unzDetach(&m_zipfile);
+			}
+			else
+			{
+				unzClose(m_zipfile);
+			}
+		}
+		m_zipfile = 0;
+	}
+
+	virtual ~ZipFileIO()
+	{
+		for (int i=0;i<B3_ZIP_FILEIO_MAX_FILES;i++)
+		{
+			fileClose(i);
+		}
+		closeZipFile();
+		if (m_stream)
+		{
+			mem_simple_destroy_file(m_stream);
+		}
+	}
+
+	
+	virtual int fileOpen(const char* fileName, const char* mode)
+	{
+		
+		//search a free slot
+		int slot = -1;
+		for (int i=0;i<B3_ZIP_FILEIO_MAX_FILES ;i++)
+		{
+			if (m_fileHandles[i]==0)
+			{
+				slot=i;
+				break;
+			}
+		}
+		if (slot>=0)
+		{
+			
+			if (m_zipfile == NULL)
+			{
+				printf("%s: not found\n", m_zipfileName.c_str());
+				slot = -1;
+			} else
+			{
+				
+				int result = 0;
+				result = unzGetGlobalInfo(m_zipfile, &m_global_info );
+				if (result != UNZ_OK)
+				{
+					printf("could not read file global info from %s\n", m_zipfileName.c_str());
+					slot = -1;
+				}
+			}
+			if (slot >=0)
+			{
+				int result = unzLocateFile(m_zipfile, fileName, 0);
+				if (result == UNZ_OK)
+				{
+					unz_file_info info;
+					result = unzGetCurrentFileInfo(m_zipfile, &info, NULL, 0, NULL, 0, NULL, 0);
+					if (result != UNZ_OK)
+					{
+						printf("unzGetCurrentFileInfo() != UNZ_OK (%d)\n", result);
+						slot=-1;
+					}
+					else
+					{
+						result = unzOpenCurrentFile(m_zipfile);
+						if (result == UNZ_OK)
+						{
+							printf("zipFile::fileOpen %s in mode %s in fileHandle %d\n", fileName, mode, slot);
+							m_fileHandles[slot] = m_zipfile;
+						} else
+						{
+							slot=-1;
+						}
+					}
+				} else
+				{
+					slot=-1;
+					
+				}
+			}
+		}
+		return slot;
+	}
+	virtual int fileRead(int fileHandle, char* destBuffer, int numBytes)
+	{
+		int result = -1;
+		if (fileHandle>=0 && fileHandle < B3_ZIP_FILEIO_MAX_FILES )
+		{
+			unzFile f = m_fileHandles[fileHandle];
+			if (f)
+			{
+				result = unzReadCurrentFile(f, destBuffer,numBytes);
+				//::fread(destBuffer, 1, numBytes, f);
+			}
+		}
+		return result;
+			
+	}
+	virtual int fileWrite(int fileHandle,const char* sourceBuffer, int numBytes)
+	{
+#if 0
+		if (fileHandle>=0 && fileHandle < B3_ZIP_FILEIO_MAX_FILES )
+		{
+			FILE* f = m_fileHandles[fileHandle];
+			if (f)
+			{
+				return ::fwrite(sourceBuffer, 1, numBytes,m_fileHandles[fileHandle]);
+			}
+		}
+#endif
+		return -1;
+	}
+	virtual void fileClose(int fileHandle)
+	{
+		if (fileHandle>=0 && fileHandle < B3_ZIP_FILEIO_MAX_FILES )
+		{
+			unzFile f = m_fileHandles[fileHandle];
+			if (f)
+			{
+				printf("zipFile::fileClose slot %d\n", fileHandle);
+				m_fileHandles[fileHandle]=0;
+			}
+		}
+	}
+
+	virtual bool findResourcePath(const char* fileName, char* relativeFileName, int relativeFileNameSizeInBytes)
+	{
+		return b3ResourcePath::findResourcePath(fileName, relativeFileName, relativeFileNameSizeInBytes, ZipFileIO::FileIOPluginFindFile, this);
+	}
+
+
+	virtual bool findFile(const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen)
+	{
+		int fileHandle = -1;
+		fileHandle = fileOpen(orgFileName, "rb");
+		if (fileHandle>=0)
+		{
+			//printf("original file found: [%s]\n", orgFileName);
+			sprintf(relativeFileName, "%s", orgFileName);
+			fileClose(fileHandle);
+			return true;
+		}
+
+		//printf("Trying various directories, relative to current working directory\n");
+		const char* prefix[] = {"./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"};
+		int numPrefixes = sizeof(prefix) / sizeof(const char*);
+
+		int f = 0;
+		bool fileFound = false;
+
+		for (int i = 0; !f && i < numPrefixes; i++)
+		{
+#ifdef _MSC_VER
+			sprintf_s(relativeFileName, maxRelativeFileNameMaxLen, "%s%s", prefix[i], orgFileName);
+#else
+			sprintf(relativeFileName, "%s%s", prefix[i], orgFileName);
+#endif
+			f = fileOpen(relativeFileName, "rb");
+			if (f>=0)
+			{
+				fileFound = true;
+				break;
+			}
+		}
+		if (f>=0)
+		{
+			fileClose(f);
+		}
+
+		return fileFound;
+	}
+	virtual char* readLine(int fileHandle, char* destBuffer, int numBytes)
+	{
+		int numRead = 0;
+				
+		if (fileHandle>=0 && fileHandle < B3_ZIP_FILEIO_MAX_FILES )
+		{
+			unzFile f = m_fileHandles[fileHandle];
+			if (f)
+			{
+				//return ::fgets(destBuffer, numBytes, m_fileHandles[fileHandle]);
+				char c = 0;
+				do
+				{
+					fileRead(fileHandle,&c,1);
+					if (c && c!='\n')
+					{
+						if (c!=13)
+						{
+							destBuffer[numRead++]=c;
+						} else
+						{
+							destBuffer[numRead++]=0;
+						}
+					}
+				} while (c != 0 && c != '\n' && numRead<(numBytes-1));
+			}
+		}
+		if (numRead<numBytes && numRead>0)
+		{
+			destBuffer[numRead]=0;
+			return &destBuffer[0];
+		}
+		return 0;
+	}
+	virtual int getFileSize(int fileHandle)
+	{
+		int size=0;
+
+		if (fileHandle>=0 && fileHandle < B3_ZIP_FILEIO_MAX_FILES )
+		{
+			unzFile f = m_fileHandles[fileHandle];
+			if (f)
+			{
+				unz_file_info info;
+				int result = unzGetCurrentFileInfo(f, &info, NULL, 0, NULL, 0, NULL, 0);
+				if (result == UNZ_OK)
+				{
+					size = info.uncompressed_size;
+				}
+			}
+		}
+		return size;
+	}
+
+	virtual void enableFileCaching(bool enable)
+	{
+		(void)enable;
+	}
+
+};

Engine/lib/bullet/examples/SharedMemory/plugins/grpcPlugin/grpcPlugin.cpp

@@ -0,0 +1,386 @@
+
+///grpcPlugin add a GRPC server to any PyBullet/BulletRobotics
+///physics server. You can connect using PyBullet connect.GRPC method
+
+#include "grpcPlugin.h"
+#include "SharedMemory/SharedMemoryPublic.h"
+#include "../b3PluginContext.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "SharedMemory/SharedMemoryCommands.h"
+#include "SharedMemory/PhysicsCommandProcessorInterface.h"
+
+#include <stdio.h>
+
+#include <mutex>
+#include <thread>
+
+#include <grpc++/grpc++.h>
+#include <grpc/support/log.h>
+#include "../../../Utils/b3Clock.h"
+#include "SharedMemory/grpc/proto/pybullet.grpc.pb.h"
+#include "SharedMemory/grpc/ConvertGRPCBullet.h"
+using grpc::Server;
+using grpc::ServerAsyncResponseWriter;
+using grpc::ServerBuilder;
+using grpc::ServerCompletionQueue;
+using grpc::ServerContext;
+using grpc::Status;
+using pybullet_grpc::PyBulletAPI;
+using pybullet_grpc::PyBulletCommand;
+using pybullet_grpc::PyBulletStatus;
+
+bool gVerboseNetworkMessagesServer4 = false;
+
+class ServerImpl final
+{
+public:
+	ServerImpl()
+	{
+	}
+	~ServerImpl()
+	{
+		Exit();
+	}
+
+	void Exit()
+	{
+		if (server_)
+		{
+			server_->Shutdown();
+			m_requestThreadCancelled = true;
+			m_requestThread->join();
+			delete m_requestThread;
+			// Always shutdown the completion queue after the server.
+			cq_->Shutdown();
+			server_ = 0;
+		}
+	}
+
+	void Init(PhysicsCommandProcessorInterface* comProc, const std::string& hostNamePort)
+	{
+		// Listen on the given address without any authentication mechanism.
+		m_builder.AddListeningPort(hostNamePort, grpc::InsecureServerCredentials());
+		// Register "service_" as the instance through which we'll communicate with
+		// clients. In this case it corresponds to an *asynchronous* service.
+		m_builder.RegisterService(&service_);
+		// Get hold of the completion queue used for the asynchronous communication
+		// with the gRPC runtime.
+		cq_ = m_builder.AddCompletionQueue();
+		// Finally assemble the server.
+		server_ = m_builder.BuildAndStart();
+		std::cout << "grpcPlugin Bullet Physics GRPC server listening on " << hostNamePort << std::endl;
+
+		//Start the thread to gather the requests.
+		m_requestThreadCancelled = false;
+		m_requestThread = new std::thread(&ServerImpl::GatherRequests, this);
+
+		// Proceed to the server's main loop.
+		InitRpcs(comProc);
+	}
+
+	// This can be run in multiple threads if needed.
+	bool HandleSingleRpc()
+	{
+		CallData::CallStatus status = CallData::CallStatus::CREATE;
+		std::lock_guard<std::mutex> guard(m_queueMutex);
+    if (!m_requestQueue.empty()) {
+      void* tag = m_requestQueue.front();
+      m_requestQueue.pop_front();
+      status = static_cast<CallData*>(tag)->Proceed();
+    }
+
+		return status == CallData::CallStatus::TERMINATE;
+	}
+
+private:
+	// Class encompasing the state and logic needed to serve a request.
+	class CallData
+	{
+	public:
+		// Take in the "service" instance (in this case representing an asynchronous
+		// server) and the completion queue "cq" used for asynchronous communication
+		// with the gRPC runtime.
+		CallData(PyBulletAPI::AsyncService* service, ServerCompletionQueue* cq, PhysicsCommandProcessorInterface* comProc)
+			: service_(service), cq_(cq), responder_(&ctx_), status_(CREATE), m_finished(false), m_comProc(comProc)
+		{
+			// Invoke the serving logic right away.
+			Proceed();
+		}
+
+		enum CallStatus
+		{
+			CREATE,
+			PROCESS,
+			FINISH,
+			TERMINATE
+		};
+
+		CallStatus Proceed()
+		{
+			if (status_ == CREATE)
+			{
+				// Make this instance progress to the PROCESS state.
+				status_ = PROCESS;
+
+				// As part of the initial CREATE state, we *request* that the system
+				// start processing SayHello requests. In this request, "this" acts are
+				// the tag uniquely identifying the request (so that different CallData
+				// instances can serve different requests concurrently), in this case
+				// the memory address of this CallData instance.
+
+				service_->RequestSubmitCommand(&ctx_, &m_command, &responder_, cq_, cq_,
+											   this);
+			}
+			else if (status_ == PROCESS)
+			{
+				// Spawn a new CallData instance to serve new clients while we process
+				// the one for this CallData. The instance will deallocate itself as
+				// part of its FINISH state.
+				new CallData(service_, cq_, m_comProc);
+				status_ = FINISH;
+
+				std::string replyString;
+				// The actual processing.
+
+				SharedMemoryStatus serverStatus;
+				b3AlignedObjectArray<char> buffer;
+				buffer.resize(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+				SharedMemoryCommand cmd;
+				SharedMemoryCommand* cmdPtr = 0;
+
+				m_status.set_statustype(CMD_UNKNOWN_COMMAND_FLUSHED);
+
+				if (m_command.has_checkversioncommand())
+				{
+					m_status.set_statustype(CMD_CLIENT_COMMAND_COMPLETED);
+					m_status.mutable_checkversionstatus()->set_serverversion(SHARED_MEMORY_MAGIC_NUMBER);
+				}
+				else
+				{
+					cmdPtr = convertGRPCToBulletCommand(m_command, cmd);
+
+					if (cmdPtr)
+					{
+						bool hasStatus = m_comProc->processCommand(*cmdPtr, serverStatus, &buffer[0], buffer.size());
+
+						double timeOutInSeconds = 10;
+						b3Clock clock;
+						double startTimeSeconds = clock.getTimeInSeconds();
+						double curTimeSeconds = clock.getTimeInSeconds();
+
+						while ((!hasStatus) && ((curTimeSeconds - startTimeSeconds) < timeOutInSeconds))
+						{
+							hasStatus = m_comProc->receiveStatus(serverStatus, &buffer[0], buffer.size());
+							curTimeSeconds = clock.getTimeInSeconds();
+						}
+
+						if (gVerboseNetworkMessagesServer4)
+						{
+							//printf("buffer.size = %d\n", buffer.size());
+							printf("serverStatus.m_numDataStreamBytes = %d\n", serverStatus.m_numDataStreamBytes);
+						}
+						if (hasStatus)
+						{
+							b3AlignedObjectArray<unsigned char> packetData;
+							unsigned char* statBytes = (unsigned char*)&serverStatus;
+
+							convertStatusToGRPC(serverStatus, &buffer[0], buffer.size(), m_status);
+						}
+					}
+
+					if (m_command.has_terminateservercommand())
+					{
+						status_ = TERMINATE;
+					}
+				}
+
+				// And we are done! Let the gRPC runtime know we've finished, using the
+				// memory address of this instance as the uniquely identifying tag for
+				// the event.
+
+				responder_.Finish(m_status, Status::OK, this);
+			}
+			else
+			{
+				GPR_ASSERT(status_ == FINISH);
+				// Once in the FINISH state, deallocate ourselves (CallData).
+				CallData::CallStatus tmpStatus = status_;
+				delete this;
+				return tmpStatus;
+			}
+			return status_;
+		}
+
+	private:
+		// The means of communication with the gRPC runtime for an asynchronous
+		// server.
+		PyBulletAPI::AsyncService* service_;
+		// The producer-consumer queue where for asynchronous server notifications.
+		ServerCompletionQueue* cq_;
+		// Context for the rpc, allowing to tweak aspects of it such as the use
+		// of compression, authentication, as well as to send metadata back to the
+		// client.
+		ServerContext ctx_;
+
+		// What we get from the client.
+		PyBulletCommand m_command;
+		// What we send back to the client.
+		PyBulletStatus m_status;
+
+		// The means to get back to the client.
+		ServerAsyncResponseWriter<PyBulletStatus> responder_;
+
+		// Let's implement a tiny state machine with the following states.
+
+		CallStatus status_;  // The current serving state.
+
+		bool m_finished;
+
+		PhysicsCommandProcessorInterface* m_comProc;  //physics server command processor
+	};
+
+	// This can be run in multiple threads if needed.
+	void InitRpcs(PhysicsCommandProcessorInterface* comProc)
+	{
+		// Spawn a new CallData instance to serve new clients.
+		new CallData(&service_, cq_.get(), comProc);
+	}
+
+	ServerBuilder m_builder;
+	std::unique_ptr<ServerCompletionQueue> cq_;
+	PyBulletAPI::AsyncService service_;
+	std::unique_ptr<Server> server_;
+
+	// Mutex to protect access to the request queue variables (m_requestQueue,
+	// m_requestThread, m_requestThreadCancelled).
+	std::mutex m_queueMutex;
+
+	// List of outstanding request tags.
+	std::list<void*> m_requestQueue;
+
+	// Whether or not the gathering thread is cancelled.
+	bool m_requestThreadCancelled;
+
+	// Thread to gather requests from the completion queue.
+	std::thread* m_requestThread;
+
+	void GatherRequests() {
+		void* tag;  // uniquely identifies a request.
+		bool ok;
+
+		while(!m_requestThreadCancelled) {
+			// Block waiting to read the next event from the completion queue. The
+			// event is uniquely identified by its tag, which in this case is the
+			// memory address of a CallData instance.
+			// The return value of Next should always be checked. This return value
+			// tells us whether there is any kind of event or cq_ is shutting down.
+			grpc::CompletionQueue::NextStatus nextStatus = cq_->AsyncNext(&tag, &ok, gpr_now(GPR_CLOCK_MONOTONIC));
+			if (nextStatus == grpc::CompletionQueue::NextStatus::GOT_EVENT)
+			{
+				GPR_ASSERT(ok);
+				std::lock_guard<std::mutex> guard(m_queueMutex);
+				m_requestQueue.push_back(tag);
+			}
+		}
+	}
+};
+
+struct grpcMyClass
+{
+	int m_testData;
+
+	ServerImpl m_grpcServer;
+	bool m_grpcInitialized;
+	bool m_grpcTerminated;
+
+	grpcMyClass()
+		: m_testData(42),
+		  m_grpcInitialized(false),
+		  m_grpcTerminated(false)
+	{
+	}
+	virtual ~grpcMyClass()
+	{
+	}
+};
+
+B3_SHARED_API int initPlugin_grpcPlugin(struct b3PluginContext* context)
+{
+	grpcMyClass* obj = new grpcMyClass();
+	context->m_userPointer = obj;
+
+	return SHARED_MEMORY_MAGIC_NUMBER;
+}
+
+B3_SHARED_API int preTickPluginCallback_grpcPlugin(struct b3PluginContext* context)
+{
+	//process grpc server messages
+	return 0;
+}
+
+B3_SHARED_API int processClientCommands_grpcPlugin(struct b3PluginContext* context)
+{
+	grpcMyClass* obj = (grpcMyClass*)context->m_userPointer;
+
+	if (obj->m_grpcInitialized && !obj->m_grpcTerminated)
+	{
+		obj->m_grpcTerminated = obj->m_grpcServer.HandleSingleRpc();
+	}
+
+	obj->m_testData++;
+	return 0;
+}
+
+B3_SHARED_API int postTickPluginCallback_grpcPlugin(struct b3PluginContext* context)
+{
+	grpcMyClass* obj = (grpcMyClass*)context->m_userPointer;
+	obj->m_testData++;
+	return 0;
+}
+
+B3_SHARED_API int executePluginCommand_grpcPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
+{
+	///3 cases:
+	/// 1: send a non-empty string to start the GRPC server
+	/// 2: send some integer n, to call n times to HandleSingleRpc
+	/// 3: send nothing to terminate the GRPC server
+
+	grpcMyClass* obj = (grpcMyClass*)context->m_userPointer;
+
+	if (strlen(arguments->m_text))
+	{
+		if (!obj->m_grpcInitialized && context->m_rpcCommandProcessorInterface)
+		{
+			obj->m_grpcServer.Init(context->m_rpcCommandProcessorInterface, arguments->m_text);
+		}
+		obj->m_grpcInitialized = true;
+	}
+	else
+	{
+		if (arguments->m_numInts > 0)
+		{
+			for (int i = 0; i < arguments->m_ints[0]; i++)
+			{
+				if (obj->m_grpcInitialized && !obj->m_grpcTerminated)
+				{
+					obj->m_grpcTerminated = obj->m_grpcServer.HandleSingleRpc();
+				}
+			}
+		}
+		else
+		{
+			obj->m_grpcServer.Exit();
+			obj->m_grpcInitialized = false;
+		}
+	}
+
+	return 0;
+}
+
+B3_SHARED_API void exitPlugin_grpcPlugin(struct b3PluginContext* context)
+{
+	grpcMyClass* obj = (grpcMyClass*)context->m_userPointer;
+	obj->m_grpcServer.Exit();
+	delete obj;
+	context->m_userPointer = 0;
+}

Engine/lib/bullet/examples/SharedMemory/plugins/grpcPlugin/grpcPlugin.h

@@ -0,0 +1,26 @@
+#ifndef GRPC_PLUGIN_H
+#define GRPC_PLUGIN_H
+
+#include "../b3PluginAPI.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+	//the following 3 APIs are required
+	B3_SHARED_API int initPlugin_grpcPlugin(struct b3PluginContext* context);
+	B3_SHARED_API void exitPlugin_grpcPlugin(struct b3PluginContext* context);
+	B3_SHARED_API int executePluginCommand_grpcPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
+
+	//all the APIs below are optional
+	B3_SHARED_API int preTickPluginCallback_grpcPlugin(struct b3PluginContext* context);
+	B3_SHARED_API int postTickPluginCallback_grpcPlugin(struct b3PluginContext* context);
+
+	B3_SHARED_API int processClientCommands_grpcPlugin(struct b3PluginContext* context);
+
+#ifdef __cplusplus
+};
+#endif
+
+#endif  //#define GRPC_PLUGIN_H

Engine/lib/bullet/examples/SharedMemory/plugins/grpcPlugin/premake4.lua

@@ -0,0 +1,43 @@
+		
+
+project ("pybullet_grpcPlugin")
+		language "C++"
+		kind "SharedLib"
+		
+		includedirs {".","../../../../src", "../../../../examples",
+		"../../../ThirdPartyLibs"}
+		defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
+	
+	initGRPC()
+
+	links{"BulletFileLoader", "Bullet3Common", "LinearMath"}
+
+
+	if os.is("MacOSX") then
+--		targetextension {"so"}
+		links{"Cocoa.framework"}
+	end
+
+
+		files {
+			"grpcPlugin.cpp",
+			"../../PhysicsClient.cpp",
+			"../../PhysicsClient.h",
+			"../../PhysicsClientSharedMemory.cpp",
+			"../../PhysicsClientSharedMemory.h",
+			"../../PhysicsClientSharedMemory_C_API.cpp",
+			"../../PhysicsClientSharedMemory_C_API.h",
+			"../../PhysicsClientC_API.cpp",
+			"../../PhysicsClientC_API.h",
+			"../../Win32SharedMemory.cpp",
+			"../../Win32SharedMemory.h",
+			"../../PosixSharedMemory.cpp",
+			"../../PosixSharedMemory.h",
+			"../../../Utils/b3Clock.cpp",
+			"../../../Utils/b3Clock.h",
+			"../../../Utils/b3ResourcePath.cpp",
+			"../../../Utils/b3ResourcePath.h",
+				}
+	
+	
+	

Engine/lib/bullet/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp

@@ -0,0 +1,169 @@
+
+#include "pdControlPlugin.h"
+#include "../../SharedMemoryPublic.h"
+#include "../b3PluginContext.h"
+#include <stdio.h>
+
+#include "LinearMath/btScalar.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+#include "../../b3RobotSimulatorClientAPI_NoDirect.h"
+#include "../../b3RobotSimulatorClientAPI_InternalData.h"
+
+struct MyPDControl
+{
+	int m_objectUniqueId;
+	int m_linkIndex;
+	btScalar m_desiredPosition;
+	btScalar m_desiredVelocity;
+	btScalar m_kd;
+	btScalar m_kp;
+	btScalar m_maxForce;
+};
+
+struct MyPDControlContainer
+{
+	int m_testData;
+	btAlignedObjectArray<MyPDControl> m_controllers;
+	b3RobotSimulatorClientAPI_NoDirect m_api;
+	MyPDControlContainer()
+		: m_testData(42)
+	{
+	}
+	virtual ~MyPDControlContainer()
+	{
+	}
+};
+
+B3_SHARED_API int initPlugin_pdControlPlugin(struct b3PluginContext* context)
+{
+	MyPDControlContainer* obj = new MyPDControlContainer();
+	b3RobotSimulatorClientAPI_InternalData data;
+	data.m_physicsClientHandle = context->m_physClient;
+	data.m_guiHelper = 0;
+	obj->m_api.setInternalData(&data);
+	context->m_userPointer = obj;
+
+	return SHARED_MEMORY_MAGIC_NUMBER;
+}
+
+B3_SHARED_API int preTickPluginCallback_pdControlPlugin(struct b3PluginContext* context)
+{
+	//apply pd control here, apply forces using the PD gains
+	MyPDControlContainer* obj = (MyPDControlContainer*)context->m_userPointer;
+
+	for (int i = 0; i < obj->m_controllers.size(); i++)
+	{
+		const MyPDControl& pdControl = obj->m_controllers[i];
+
+		int dof1 = 0;
+		b3JointSensorState actualState;
+		if (obj->m_api.getJointState(pdControl.m_objectUniqueId, pdControl.m_linkIndex, &actualState))
+		{
+			if (pdControl.m_maxForce > 0)
+			{
+				//compute torque
+				btScalar qActual = actualState.m_jointPosition;
+				btScalar qdActual = actualState.m_jointVelocity;
+
+				btScalar positionError = (pdControl.m_desiredPosition - qActual);
+				double desiredVelocity = 0;
+				btScalar velocityError = (pdControl.m_desiredVelocity - qdActual);
+
+				btScalar force = pdControl.m_kp * positionError + pdControl.m_kd * velocityError;
+
+				btClamp(force, -pdControl.m_maxForce, pdControl.m_maxForce);
+
+				//apply torque
+				b3RobotSimulatorJointMotorArgs args(CONTROL_MODE_TORQUE);
+				args.m_maxTorqueValue = force;
+				obj->m_api.setJointMotorControl(pdControl.m_objectUniqueId, pdControl.m_linkIndex, args);
+			}
+		}
+	}
+
+	return 0;
+}
+
+B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
+{
+	MyPDControlContainer* obj = (MyPDControlContainer*)context->m_userPointer;
+
+	if (arguments->m_numInts == 0)
+	{
+		obj->m_api.syncBodies();
+		return -1;
+	}
+
+	int numObj = obj->m_api.getNumBodies();
+	//printf("numObj = %d\n", numObj);
+
+	//protocol:
+	//first int is the type of command
+	//second int is body unique id
+	//third int is link index
+
+	if (arguments->m_numInts != 3)
+		return -1;
+
+	switch (arguments->m_ints[0])
+	{
+		case eSetPDControl:
+		{
+			if (arguments->m_numFloats < 5)
+				return -1;
+			MyPDControl controller;
+			controller.m_desiredPosition = arguments->m_floats[0];
+			controller.m_desiredVelocity = arguments->m_floats[1];
+			controller.m_kd = arguments->m_floats[2];
+			controller.m_kp = arguments->m_floats[3];
+			controller.m_maxForce = arguments->m_floats[4];
+			controller.m_objectUniqueId = arguments->m_ints[1];
+			controller.m_linkIndex = arguments->m_ints[2];
+			int foundIndex = -1;
+			for (int i = 0; i < obj->m_controllers.size(); i++)
+			{
+				if (obj->m_controllers[i].m_objectUniqueId == controller.m_objectUniqueId && obj->m_controllers[i].m_linkIndex == controller.m_linkIndex)
+				{
+					obj->m_controllers[i] = controller;
+					foundIndex = i;
+				}
+			}
+			if (foundIndex < 0)
+			{
+				obj->m_controllers.push_back(controller);
+			}
+			break;
+		}
+		case eRemovePDControl:
+		{
+			MyPDControl controller;
+			controller.m_objectUniqueId = arguments->m_ints[1];
+			controller.m_linkIndex = arguments->m_ints[2];
+
+			for (int i = 0; i < obj->m_controllers.size(); i++)
+			{
+				if (obj->m_controllers[i].m_objectUniqueId == controller.m_objectUniqueId && obj->m_controllers[i].m_linkIndex == controller.m_linkIndex)
+				{
+					obj->m_controllers.removeAtIndex(i);
+					break;
+				}
+			}
+			break;
+		}
+		default:
+		{
+			return -1;
+		}
+	}
+
+	int result = 42;
+	return result;
+}
+
+B3_SHARED_API void exitPlugin_pdControlPlugin(struct b3PluginContext* context)
+{
+	MyPDControlContainer* obj = (MyPDControlContainer*)context->m_userPointer;
+	delete obj;
+	context->m_userPointer = 0;
+}

Engine/lib/bullet/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h

@@ -0,0 +1,30 @@
+#ifndef PID_CONTROL_PLUGIN_H
+#define PID_CONTROL_PLUGIN_H
+
+#include "../b3PluginAPI.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+	//the following 3 APIs are required
+	B3_SHARED_API int initPlugin_pdControlPlugin(struct b3PluginContext* context);
+	B3_SHARED_API void exitPlugin_pdControlPlugin(struct b3PluginContext* context);
+	B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
+
+	///
+	enum PDControlCommandEnum
+	{
+		eSetPDControl = 1,
+		eRemovePDControl = 2,
+	};
+
+	//all the APIs below are optional
+	B3_SHARED_API int preTickPluginCallback_pdControlPlugin(struct b3PluginContext* context);
+
+#ifdef __cplusplus
+};
+#endif
+
+#endif  //#define PID_CONTROL_PLUGIN_H

Engine/lib/bullet/examples/SharedMemory/plugins/pdControlPlugin/premake4.lua

@@ -0,0 +1,44 @@
+		
+
+project ("pybullet_pdControlPlugin")
+		language "C++"
+		kind "SharedLib"
+		
+		includedirs {".","../../../../src", "../../../../examples",
+		"../../../ThirdPartyLibs"}
+		defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
+	hasCL = findOpenCL("clew")
+
+	links{"BulletFileLoader", "Bullet3Common", "LinearMath"}
+
+
+	if os.is("MacOSX") then
+--		targetextension {"so"}
+		links{"Cocoa.framework","Python"}
+	end
+
+
+		files {
+			"pdControlPlugin.cpp",
+			"../../b3RobotSimulatorClientAPI_NoDirect.cpp",
+			"../../b3RobotSimulatorClientAPI_NoDirect.h",
+			"../../PhysicsClient.cpp",
+			"../../PhysicsClient.h",
+			"../../PhysicsClientSharedMemory.cpp",
+			"../../PhysicsClientSharedMemory.h",
+			"../../PhysicsClientSharedMemory_C_API.cpp",
+			"../../PhysicsClientSharedMemory_C_API.h",
+			"../../PhysicsClientC_API.cpp",
+			"../../PhysicsClientC_API.h",
+			"../../Win32SharedMemory.cpp",
+			"../../Win32SharedMemory.h",
+			"../../PosixSharedMemory.cpp",
+			"../../PosixSharedMemory.h",
+			"../../../Utils/b3Clock.cpp",
+			"../../../Utils/b3Clock.h",
+			"../../../Utils/b3ResourcePath.cpp",
+			"../../../Utils/b3ResourcePath.h",
+				}
+	
+	
+	

Engine/lib/bullet/examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp

@@ -0,0 +1,406 @@
+#include "btBulletDynamicsCommon.h"
+
+//for inverse dynamics, DeepMimic implementation
+#include "RBDModel.h"
+#include "RBDUtil.h"
+#include "KinTree.h"
+
+//for BulletInverseDynamics
+//#include "BulletInverseDynamics/IDConfig.hpp"
+//#include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp"
+
+//#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
+//#include "BulletDynamics/MLCPSolvers/btLemkeSolver.h"
+//#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
+
+#include "BulletDynamics/Featherstone/btMultiBody.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLink.h"
+#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
+#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
+#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
+#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
+#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
+
+
+struct TempLink
+{
+	int m_parentIndex;
+	const btCollisionObject* m_collider;
+	double m_mass;
+	int m_jointType;
+	int m_dofOffset;
+	int m_dofCount;
+	btVector3 m_dVector;
+	btVector3 m_eVector;
+	btQuaternion m_zeroRotParentToThis;
+	btQuaternion m_this_to_body1;
+};
+
+bool btExtractJointBodyFromTempLinks(btAlignedObjectArray<TempLink>& links, Eigen::MatrixXd& bodyDefs, Eigen::MatrixXd& jointMat)
+{
+
+	bool result = true;
+
+	int num_joints = links.size();
+
+	btAlignedObjectArray<btVector3> bodyToLinkPositions;
+	btAlignedObjectArray<btQuaternion> bodyToLinkRotations;
+	btAlignedObjectArray<btQuaternion> dVectorRot;
+	bodyToLinkRotations.resize(num_joints);
+	bodyToLinkPositions.resize(num_joints);
+	dVectorRot.resize(num_joints);
+
+	jointMat.resize(num_joints, 19);
+	bodyDefs.resize(num_joints, 17);
+	for (int i = 0; i < num_joints * 19; i++)
+	{
+		jointMat(i) = SIMD_INFINITY;
+	}
+	for (int i = 0; i < num_joints * 17; i++)
+	{
+		bodyDefs(i) = SIMD_INFINITY;
+	}
+
+	for (int i = 0; i < num_joints * 17; i++)
+	{
+		bodyDefs(i) = SIMD_INFINITY;
+	}
+
+	btScalar unk = -12345;
+
+	int totalDofs = 0;
+
+	for (int j = 0; j < num_joints; ++j)
+	{
+
+		int i = j;
+		int parentIndex = links[j].m_parentIndex;
+
+		cShape::eShape shapeType = cShape::eShapeNull;
+		double param0 = 0, param1 = 0, param2 = 0;
+		if (links[j].m_collider)
+		{
+			const btCollisionShape* collisionShape = links[j].m_collider->getCollisionShape();
+			if (collisionShape->isCompound())
+			{
+				const btCompoundShape* compound = (const btCompoundShape*)collisionShape;
+				if (compound->getNumChildShapes() > 0)
+				{
+					collisionShape = compound->getChildShape(0);
+				}
+			}
+			
+			switch (collisionShape->getShapeType())
+			{
+			case BOX_SHAPE_PROXYTYPE:
+			{
+				shapeType = cShape::eShapeBox;
+				btBoxShape* box = (btBoxShape*)collisionShape;
+				param0 = box->getHalfExtentsWithMargin()[0] * 2;
+				param1 = box->getHalfExtentsWithMargin()[1] * 2;
+				param2 = box->getHalfExtentsWithMargin()[2] * 2;
+				
+				break;
+			}
+			case SPHERE_SHAPE_PROXYTYPE:
+			{
+				btSphereShape* sphere = (btSphereShape*)collisionShape;
+				param0 = sphere->getRadius() * 2;
+				param1 = sphere->getRadius() * 2;
+				param2 = sphere->getRadius() * 2;
+				shapeType = cShape::eShapeSphere;
+				break;
+			}
+			case CAPSULE_SHAPE_PROXYTYPE:
+			{
+				btCapsuleShape* caps = (btCapsuleShape*)collisionShape;
+				param0 = caps->getRadius() * 2;
+				param1 = caps->getHalfHeight() * 2;
+				param2 = caps->getRadius() * 2;
+				shapeType = cShape::eShapeCapsule;
+				break;
+			}
+			default:
+			{
+				//approximate by its box
+				btTransform identity;
+				identity.setIdentity();
+				btVector3 aabbMin, aabbMax;
+				collisionShape->getAabb(identity, aabbMin, aabbMax);
+				btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5);
+				btScalar margin = collisionShape->getMargin();
+				btScalar lx = btScalar(2.) * (halfExtents.x() + margin);
+				btScalar ly = btScalar(2.) * (halfExtents.y() + margin);
+				btScalar lz = btScalar(2.) * (halfExtents.z() + margin);
+				param0 = lx;
+				param1 = ly;
+				param2 = lz;
+				shapeType = cShape::eShapeBox;
+			}
+			}
+		}
+
+
+		btVector3 body_attach_pt1 = links[j].m_dVector;
+
+		//tQuaternion body_to_parent_body = parent_to_parent_body * this_to_parent * body_to_this;
+		//tQuaternion parent_to_parent_body = parent_body_to_parent.inverse();
+
+
+		bodyDefs(i, cKinTree::eBodyParam0) = param0;
+		bodyDefs(i, cKinTree::eBodyParam1) = param1;
+		bodyDefs(i, cKinTree::eBodyParam2) = param2;
+
+		bodyDefs(i, cKinTree::eBodyParamShape) = shapeType;
+		bodyDefs(i, cKinTree::eBodyParamMass) = links[j].m_mass;
+
+
+		bodyDefs(i, cKinTree::eBodyParamColGroup) = unk;
+		bodyDefs(i, cKinTree::eBodyParamEnableFallContact) = unk;
+
+		bodyDefs(i, cKinTree::eBodyColorR) = unk;
+		bodyDefs(i, cKinTree::eBodyColorG) = unk;
+		bodyDefs(i, cKinTree::eBodyColorB) = unk;
+		bodyDefs(i, cKinTree::eBodyColorA) = unk;
+
+		dVectorRot[j] = links[j].m_this_to_body1;
+
+		btVector3 body_attach_pt2 = quatRotate(links[j].m_this_to_body1.inverse(), body_attach_pt1);
+		bodyToLinkPositions[i] = body_attach_pt2;
+		bodyDefs(i, cKinTree::eBodyParamAttachX) = body_attach_pt2[0];
+		bodyDefs(i, cKinTree::eBodyParamAttachY) = body_attach_pt2[1];
+		bodyDefs(i, cKinTree::eBodyParamAttachZ) = body_attach_pt2[2];
+		btScalar bodyAttachThetaX = 0;
+		btScalar bodyAttachThetaY = 0;
+		btScalar bodyAttachThetaZ = 0;
+		btQuaternion body_to_this1 = links[j].m_this_to_body1.inverse();
+
+
+
+		body_to_this1.getEulerZYX(bodyAttachThetaZ, bodyAttachThetaY, bodyAttachThetaX);
+		bodyDefs(i, cKinTree::eBodyParamAttachThetaX) = bodyAttachThetaX;
+		bodyDefs(i, cKinTree::eBodyParamAttachThetaY) = bodyAttachThetaY;
+		bodyDefs(i, cKinTree::eBodyParamAttachThetaZ) = bodyAttachThetaZ;
+
+		jointMat(i, cKinTree::eJointDescType) = links[j].m_jointType;
+		jointMat(i, cKinTree::eJointDescParent) = parentIndex;
+
+
+		btVector3 jointAttachPointMy = links[j].m_eVector;
+		btVector3 jointAttachPointMyv0 = jointAttachPointMy;
+		btVector3 parentBodyAttachPtMy(0, 0, 0);
+		btQuaternion parentBodyToLink;
+		parentBodyToLink = btQuaternion::getIdentity();
+		btQuaternion linkToParentBody = btQuaternion::getIdentity();
+		int parent_joint = links[j].m_parentIndex;
+
+		if (parent_joint != gInvalidIdx)
+		{
+			parentBodyAttachPtMy = bodyToLinkPositions[parent_joint];
+			parentBodyToLink = bodyToLinkRotations[parent_joint];
+			linkToParentBody = parentBodyToLink.inverse();
+		}
+		parentBodyAttachPtMy = quatRotate(linkToParentBody, parentBodyAttachPtMy);
+		//bodyToLinkRotations
+		jointAttachPointMy += parentBodyAttachPtMy;
+		jointAttachPointMy = quatRotate(linkToParentBody.inverse(), jointAttachPointMy);
+
+		btVector3 parent_body_attach_pt1(0, 0, 0);
+		if (parentIndex >= 0)
+		{
+			parent_body_attach_pt1 = links[parentIndex].m_dVector;
+		}
+		btQuaternion myparent_body_to_body(0, 0, 0, 1);
+		btQuaternion mybody_to_parent_body(0, 0, 0, 1);
+		btQuaternion parent_body_to_body1 = links[i].m_zeroRotParentToThis;
+		btQuaternion body_to_parent_body1 = parent_body_to_body1.inverse();
+
+		bodyToLinkRotations[i] = body_to_this1;
+
+		jointMat(i, cKinTree::eJointDescAttachX) = jointAttachPointMy[0];
+		jointMat(i, cKinTree::eJointDescAttachY) = jointAttachPointMy[1];
+		jointMat(i, cKinTree::eJointDescAttachZ) = jointAttachPointMy[2];
+
+
+		btQuaternion parent2parent_body2(0, 0, 0, 1);
+
+		if (parent_joint >= 0)
+		{
+			//parent2parent_body2 = bulletMB->getLink(parent_joint).m_dVectorRot;
+			parent2parent_body2 = dVectorRot[parent_joint];
+		}
+		///btQuaternion this2bodyA = bulletMB->getLink(j).m_dVectorRot;
+		btQuaternion this2bodyA = dVectorRot[j];
+
+		btQuaternion parent_body_2_body = links[j].m_zeroRotParentToThis;
+		btQuaternion combined2 = parent_body_2_body.inverse();
+		btQuaternion recoverthis2parent = parent2parent_body2.inverse()*combined2*this2bodyA;// body2this.inverse();
+		btScalar eulZ, eulY, eulX;
+		recoverthis2parent.getEulerZYX(eulZ, eulY, eulX);
+
+
+		jointMat(i, cKinTree::eJointDescAttachThetaX) = eulX;
+		jointMat(i, cKinTree::eJointDescAttachThetaY) = eulY;
+		jointMat(i, cKinTree::eJointDescAttachThetaZ) = eulZ;
+
+
+		jointMat(i, cKinTree::eJointDescLimLow0) = unk;
+		jointMat(i, cKinTree::eJointDescLimLow1) = unk;
+		jointMat(i, cKinTree::eJointDescLimLow2) = unk;
+		jointMat(i, cKinTree::eJointDescLimHigh0) = unk;
+		jointMat(i, cKinTree::eJointDescLimHigh1) = unk;
+		jointMat(i, cKinTree::eJointDescLimHigh2) = unk;
+		jointMat(i, cKinTree::eJointDescTorqueLim) = unk;
+
+		jointMat(i, cKinTree::eJointDescForceLim) = unk;
+		jointMat(i, cKinTree::eJointDescIsEndEffector) = unk;
+		jointMat(i, cKinTree::eJointDescDiffWeight) = unk;
+		jointMat(i, cKinTree::eJointDescParamOffset) = totalDofs;
+		totalDofs += links[j].m_dofCount;
+
+	}
+	return result;
+}
+
+
+void btExtractJointBodyFromBullet(const btMultiBody* bulletMB, Eigen::MatrixXd& bodyDefs, Eigen::MatrixXd& jointMat)
+{
+	btAlignedObjectArray<TempLink> links;
+
+	int numBaseShapes = 0;
+	if (bulletMB->getBaseCollider())
+	{
+		switch (bulletMB->getBaseCollider()->getCollisionShape()->getShapeType())
+		{
+			case CAPSULE_SHAPE_PROXYTYPE:
+			case SPHERE_SHAPE_PROXYTYPE:
+			case BOX_SHAPE_PROXYTYPE:
+			{
+				numBaseShapes++;
+				break;
+			}
+			case COMPOUND_SHAPE_PROXYTYPE:
+			{
+				btCompoundShape* compound = (btCompoundShape*)bulletMB->getBaseCollider()->getCollisionShape();
+				numBaseShapes += compound->getNumChildShapes();
+				break;
+			}
+			default:
+			{
+			}
+		}
+
+	}
+
+	//links include the 'base' and its childlinks
+	int baseLink = numBaseShapes? 1 : 0;
+	links.resize(bulletMB->getNumLinks() + baseLink);
+	for (int i = 0; i < links.size(); i++)
+	{
+		memset(&links[i], 0xffffffff, sizeof(TempLink));
+	}
+
+	int totalDofs = 0;
+	if (numBaseShapes)
+	{
+		//links[0] is the root/base
+		links[0].m_parentIndex = -1;
+		links[0].m_collider = bulletMB->getBaseCollider();
+		links[0].m_mass = bulletMB->getBaseMass();
+		links[0].m_jointType = (bulletMB->hasFixedBase()) ? cKinTree::eJointTypeFixed : cKinTree::eJointTypeNone;
+		links[0].m_dofOffset = 0;
+		links[0].m_dofCount = 7;
+		links[0].m_dVector.setValue(0, 0, 0);
+		links[0].m_eVector.setValue(0, 0, 0);
+		links[0].m_zeroRotParentToThis = btQuaternion(0, 0, 0, 1);
+		links[0].m_this_to_body1 = btQuaternion(0, 0, 0, 1);
+		totalDofs = 7;
+	}
+	
+
+	for (int j = 0; j < bulletMB->getNumLinks(); ++j)
+	{
+		int parentIndex = bulletMB->getLink(j).m_parent;
+		links[j + baseLink].m_parentIndex = parentIndex + baseLink;
+		links[j + baseLink].m_collider = bulletMB->getLinkCollider(j);
+		links[j + baseLink].m_mass = bulletMB->getLink(j).m_mass;
+
+		int jointType = 0;
+		btQuaternion this_to_body1(0, 0, 0, 1);
+		int dofCount = 0;
+
+		if ((baseLink)==0 &&j == 0)//for 'root' either use fixed or none
+		{
+			dofCount = 7;
+			links[j].m_parentIndex = -1;
+			links[j].m_jointType = (bulletMB->hasFixedBase()) ? cKinTree::eJointTypeFixed : cKinTree::eJointTypeNone;
+			links[j].m_dofOffset = 0;
+			links[j].m_dofCount = dofCount;
+			
+			
+			links[j].m_zeroRotParentToThis = btQuaternion(0, 0, 0, 1);
+			//links[j].m_dVector.setValue(0, 0, 0);
+			links[j].m_dVector = bulletMB->getLink(j).m_dVector;
+			links[j].m_eVector.setValue(0, 0, 0);
+			//links[j].m_eVector = bulletMB->getLink(j).m_eVector;
+			links[j].m_zeroRotParentToThis = bulletMB->getLink(j).m_zeroRotParentToThis;
+
+			links[j].m_this_to_body1 = btQuaternion(0, 0, 0, 1);
+			totalDofs = 7;
+		}
+		else
+		{
+
+			switch (bulletMB->getLink(j).m_jointType)
+			{
+			case btMultibodyLink::eFixed:
+			{
+				jointType = cKinTree::eJointTypeFixed;
+				break;
+			}
+			case btMultibodyLink::ePrismatic:
+			{
+				dofCount = 1;
+				btVector3 refAxis(1, 0, 0);
+				btVector3 axis = bulletMB->getLink(j).getAxisTop(0);
+				this_to_body1 = shortestArcQuat(refAxis, btVector3(axis[0], axis[1], axis[2]));
+				jointType = cKinTree::eJointTypePrismatic;
+				break;
+			}
+			case btMultibodyLink::eSpherical:
+			{
+				dofCount = 4;//??
+				jointType = cKinTree::eJointTypeSpherical;
+				break;
+			}
+			case btMultibodyLink::eRevolute:
+			{
+				dofCount = 1;
+				btVector3 refAxis(0, 0, 1);
+				btVector3 axis = bulletMB->getLink(j).getAxisTop(0);
+				this_to_body1 = shortestArcQuat(refAxis, btVector3(axis[0], axis[1], axis[2]));
+				jointType = cKinTree::eJointTypeRevolute;
+				break;
+			}
+			default:
+			{
+			}
+			}
+			links[j + baseLink].m_jointType = jointType;
+			links[j + baseLink].m_dofOffset = totalDofs;
+			links[j + baseLink].m_dofCount = dofCount;
+			links[j + baseLink].m_dVector = bulletMB->getLink(j).m_dVector;
+			links[j + baseLink].m_eVector = bulletMB->getLink(j).m_eVector;
+			links[j + baseLink].m_zeroRotParentToThis = bulletMB->getLink(j).m_zeroRotParentToThis;
+			links[j + baseLink].m_this_to_body1 = this_to_body1;
+		}
+
+		totalDofs += dofCount;
+	}
+
+	btExtractJointBodyFromTempLinks(links, bodyDefs, jointMat);
+}

Engine/lib/bullet/examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.h

@@ -0,0 +1,9 @@
+#ifndef BULLET_CONVERSION_H
+#define BULLET_CONVERSION_H
+
+class btMultiBody;
+#include "MathUtil.h"
+void btExtractJointBodyFromBullet(const btMultiBody* bulletMB, Eigen::MatrixXd& bodyDefs, Eigen::MatrixXd& jointMat);
+
+
+#endif  //BULLET_CONVERSION_H

Engine/lib/bullet/examples/SharedMemory/plugins/stablePDPlugin/KinTree.h

@@ -0,0 +1,270 @@
+#pragma once
+
+#include <vector>
+#include <fstream>
+#include "Shape.h"
+#ifdef USE_JSON
+#include "json/json.h"
+#endif
+#include "MathUtil.h"
+
+class cKinTree
+{
+public:
+	// description of the joint tree representing an articulated figure
+	enum eJointType
+	{
+		eJointTypeRevolute,
+		eJointTypePlanar,
+		eJointTypePrismatic,
+		eJointTypeFixed,
+		eJointTypeSpherical,
+		eJointTypeNone,
+		eJointTypeMax
+	};
+
+	enum eJointDesc
+	{
+		eJointDescType,
+		eJointDescParent,
+		eJointDescAttachX,
+		eJointDescAttachY,
+		eJointDescAttachZ,
+		eJointDescAttachThetaX, // euler angles order rot(Z) * rot(Y) * rot(X)
+		eJointDescAttachThetaY,
+		eJointDescAttachThetaZ,
+		eJointDescLimLow0,
+		eJointDescLimLow1,
+		eJointDescLimLow2,
+		eJointDescLimHigh0,
+		eJointDescLimHigh1,
+		eJointDescLimHigh2,
+		eJointDescTorqueLim,
+		eJointDescForceLim,
+		eJointDescIsEndEffector,
+		eJointDescDiffWeight,
+		eJointDescParamOffset,
+		eJointDescMax
+	};
+	typedef Eigen::Matrix<double, 1, eJointDescMax> tJointDesc;
+
+	enum eBodyParam
+	{
+		eBodyParamShape,
+		eBodyParamMass,
+		eBodyParamColGroup, // 0 collides with nothing and 1 collides with everything
+		eBodyParamEnableFallContact,
+		eBodyParamAttachX,
+		eBodyParamAttachY,
+		eBodyParamAttachZ,
+		eBodyParamAttachThetaX, // Euler angles order XYZ
+		eBodyParamAttachThetaY,
+		eBodyParamAttachThetaZ,
+		eBodyParam0,
+		eBodyParam1,
+		eBodyParam2,
+		eBodyColorR,
+		eBodyColorG,
+		eBodyColorB,
+		eBodyColorA,
+		eBodyParamMax
+	};
+	typedef Eigen::Matrix<double, 1, eBodyParamMax> tBodyDef;
+
+	enum eDrawShape
+	{
+		eDrawShapeShape,
+		eDrawShapeParentJoint,
+		eDrawShapeAttachX,
+		eDrawShapeAttachY,
+		eDrawShapeAttachZ,
+		eDrawShapeAttachThetaX, // Euler angles order XYZ
+		eDrawShapeAttachThetaY,
+		eDrawShapeAttachThetaZ,
+		eDrawShapeParam0,
+		eDrawShapeParam1,
+		eDrawShapeParam2,
+		eDrawShapeColorR,
+		eDrawShapeColorG,
+		eDrawShapeColorB,
+		eDrawShapeColorA,
+		eDrawShapeMeshID,
+		eDrawShapeParamMax
+	};
+	typedef Eigen::Matrix<double, 1, eDrawShapeParamMax> tDrawShapeDef;
+
+	static const int gInvalidJointID;
+	static const int gPosDim;
+	static const int gRotDim;
+	static const int gRootDim;
+
+	static bool HasValidRoot(const Eigen::MatrixXd& joint_mat);
+	static tVector GetRootPos(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state);
+	static void SetRootPos(const Eigen::MatrixXd& joint_mat, const tVector& pos, Eigen::VectorXd& out_state);
+	static tQuaternion GetRootRot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state);
+	static void SetRootRot(const Eigen::MatrixXd& joint_mat, const tQuaternion& rot, Eigen::VectorXd& out_state);
+	static tVector GetRootVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel);
+	static void SetRootVel(const Eigen::MatrixXd& joint_mat, const tVector& vel, Eigen::VectorXd& out_vel);
+	static tVector GetRootAngVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel);
+	static void SetRootAngVel(const Eigen::MatrixXd& joint_mat, const tVector& ang_vel, Eigen::VectorXd& out_vel);
+
+	static tVector CalcJointWorldPos(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
+	static tVector LocalToWorldPos(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int parent_id, const tVector& attach_pt);
+	static tQuaternion CalcJointWorldRot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
+	static void CalcJointWorldTheta(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id, tVector& out_axis, double& out_theta);
+	
+	static tVector CalcJointWorldVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int joint_id);
+	static tVector CalcWorldVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int parent_id, const tVector& attach_pt);
+	static tVector CalcJointWorldAngularVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int joint_id);
+	static tVector CalcWorldAngularVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int parent_id, const tVector& attach_pt);
+
+	static int GetNumDof(const Eigen::MatrixXd& joint_mat);
+	static void ApplyStep(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& step, Eigen::VectorXd& out_pose);
+
+	static Eigen::VectorXi FindJointChain(const Eigen::MatrixXd& joint_mat, int joint_beg, int joint_end);
+	static bool IsAncestor(const Eigen::MatrixXd& joint_mat, int child_joint, int ancestor_joint, int& out_len);
+	static double CalcChainLength(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXi& chain);
+
+	static void CalcAABB(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, tVector& out_min, tVector& out_max);
+
+	static int GetParamOffset(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static int GetParamSize(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static int GetJointParamSize(eJointType joint_type);
+	static void GetJointParams(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int j, Eigen::VectorXd& out_params);
+	static void SetJointParams(const Eigen::MatrixXd& joint_mat, int j, const Eigen::VectorXd& params, Eigen::VectorXd& out_state);
+	static eJointType GetJointType(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static int GetParent(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static bool HasParent(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static bool IsRoot(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static bool IsJointActuated(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static double GetTorqueLimit(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static double GetForceLimit(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static bool IsEndEffector(const Eigen::MatrixXd& joint_mat, int joint_id);
+	
+	static tVector GetJointLimLow(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static tVector GetJointLimHigh(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static double GetJointDiffWeight(const Eigen::MatrixXd& joint_mat, int joint_id);
+	
+	static double CalcLinkLength(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static tVector GetAttachPt(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static tVector GetAttachTheta(const Eigen::MatrixXd& joint_mat, int joint_id);
+
+	// calculates the longest chain in the subtree of each joint
+	static void CalcMaxSubChainLengths(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_lengths);
+	static void CalcSubTreeMasses(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, Eigen::VectorXd& out_masses);
+	
+	static tMatrix BuildAttachTrans(const Eigen::MatrixXd& joint_mat, int joint_id);
+	static tMatrix ChildParentTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
+	static tMatrix ParentChildTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
+	static tMatrix JointWorldTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
+	static tMatrix WorldJointTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
+
+#ifdef USE_JSON
+	static bool Load(const Json::Value& root, Eigen::MatrixXd& out_joint_mat);
+	static bool ParseBodyDef(const Json::Value& root, tBodyDef& out_def);
+	static bool ParseDrawShapeDef(const Json::Value& root, tDrawShapeDef& out_def);
+	static std::string BuildJointMatJson(const Eigen::MatrixXd& joint_mat);
+	static std::string BuildJointJson(int id, const tJointDesc& joint_desc);
+	static bool ParseJoint(const Json::Value& root, tJointDesc& out_joint_desc);
+#endif
+
+	static int GetNumJoints(const Eigen::MatrixXd& joint_mat);
+	static int GetRoot(const Eigen::MatrixXd& joint_mat);
+	static void FindChildren(const Eigen::MatrixXd& joint_mat, int joint_id, Eigen::VectorXi& out_children);
+
+	static bool LoadBodyDefs(const std::string& char_file, Eigen::MatrixXd& out_body_defs);
+	
+	static bool LoadDrawShapeDefs(const std::string& char_file, Eigen::MatrixXd& out_draw_defs);
+	
+	static cShape::eShape GetBodyShape(const Eigen::MatrixXd& body_defs, int part_id);
+	static tVector GetBodyAttachPt(const Eigen::MatrixXd& body_defs, int part_id);
+	static tVector GetBodyAttachTheta(const Eigen::MatrixXd& body_defs, int part_id);
+	static void GetBodyRotation(const Eigen::MatrixXd& body_defs, int part_id, tVector& out_axis, double& out_theta);
+	static double GetBodyMass(const Eigen::MatrixXd& body_defs, int part_id);
+	static int GetBodyColGroup(const Eigen::MatrixXd& body_defs, int part_id);
+	static bool GetBodyEnableFallContact(const Eigen::MatrixXd& body_defs, int part_id);
+	static void SetBodyEnableFallContact(int part_id, bool enable, Eigen::MatrixXd& out_body_defs);
+	static tVector GetBodySize(const Eigen::MatrixXd& body_defs, int part_id);
+	static tVector GetBodyColor(const Eigen::MatrixXd& body_defs, int part_id);
+	static double CalcTotalMass(const Eigen::MatrixXd& body_defs);
+	static bool IsValidBody(const Eigen::MatrixXd& body_defs, int part_id);
+	static tVector GetBodyLocalCoM(const Eigen::MatrixXd& body_defs, int part_id);
+
+	static int GetDrawShapeParentJoint(const tDrawShapeDef& shape);
+	static tVector GetDrawShapeAttachPt(const tDrawShapeDef& shape);
+	static tVector GetDrawShapeAttachTheta(const tDrawShapeDef& shape);
+	static void GetDrawShapeRotation(const tDrawShapeDef& shape, tVector& out_axis, double& out_theta);
+	static tVector GetDrawShapeColor(const tDrawShapeDef& shape);
+	static int GetDrawShapeMeshID(const tDrawShapeDef& shape);
+
+	static tVector CalcBodyPartPos(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, int part_id);
+	static tVector CalcBodyPartVel(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int part_id);
+	static void CalcBodyPartRotation(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, int part_id, tVector& out_axis, double& out_theta);
+	static tMatrix BodyWorldTrans(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, int part_id);
+	static tMatrix BodyJointTrans(const Eigen::MatrixXd& body_defs, int part_id);
+
+	static tJointDesc BuildJointDesc(eJointType joint_type, int parent_id, const tVector& attach_pt);
+	static tJointDesc BuildJointDesc();
+	static tBodyDef BuildBodyDef();
+	static tDrawShapeDef BuildDrawShapeDef();
+
+	static void BuildDefaultPose(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose);
+	static void BuildDefaultVel(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_vel);
+
+	static void CalcPoseDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, Eigen::VectorXd& out_diff);
+	static tVector CalcRootPosDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1);
+	static tQuaternion CalcRootRotDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1);
+	static double CalcPoseErr(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1);
+	static double CalcRootPosErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1);
+	static double CalcRootRotErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1);
+
+	static void CalcVelDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1, Eigen::VectorXd& out_diff);
+	static tVector CalcRootVelDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1);
+	static tVector CalcRootAngVelDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1);
+	static double CalcVelErr(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1);
+	static double CalcRootVelErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1);
+	static double CalcRootAngVelErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1);
+
+	static void CalcVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, double dt, Eigen::VectorXd& out_vel);
+
+	static void PostProcessPose(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose);
+	static void LerpPoses(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, double lerp, Eigen::VectorXd& out_pose);
+	static void VelToPoseDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, const Eigen::VectorXd& vel, Eigen::VectorXd& out_pose_diff);
+
+	static double CalcHeading(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose);
+	static tQuaternion CalcHeadingRot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose);
+	static tMatrix BuildHeadingTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose);
+	static tMatrix BuildOriginTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose);
+	static void NormalizePoseHeading(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose);
+	static void NormalizePoseHeading(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose, Eigen::VectorXd& out_vel);
+
+	
+
+protected:
+	static bool ParseJointType(const std::string& type_str, eJointType& out_joint_type);
+	static void PostProcessJointMat(Eigen::MatrixXd& out_joint_mat);
+
+	static tMatrix ChildParentTransRoot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
+	static tMatrix ChildParentTransRevolute(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
+	static tMatrix ChildParentTransPlanar(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
+	static tMatrix ChildParentTransFixed(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
+	static tMatrix ChildParentTransPrismatic(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
+	static tMatrix ChildParentTransSpherical(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
+
+	static void BuildDefaultPoseRoot(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose);
+	static void BuildDefaultPoseRevolute(Eigen::VectorXd& out_pose);
+	static void BuildDefaultPosePrismatic(Eigen::VectorXd& out_pose);
+	static void BuildDefaultPosePlanar(Eigen::VectorXd& out_pose);
+	static void BuildDefaultPoseFixed(Eigen::VectorXd& out_pose);
+	static void BuildDefaultPoseSpherical(Eigen::VectorXd& out_pose);
+
+	static void BuildDefaultVelRoot(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose);
+	static void BuildDefaultVelRevolute(Eigen::VectorXd& out_pose);
+	static void BuildDefaultVelPrismatic(Eigen::VectorXd& out_pose);
+	static void BuildDefaultVelPlanar(Eigen::VectorXd& out_pose);
+	static void BuildDefaultVelFixed(Eigen::VectorXd& out_pose);
+	static void BuildDefaultVelSpherical(Eigen::VectorXd& out_pose);
+
+	static void CalcJointPoseDiff(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, Eigen::VectorXd& out_diff);
+	static void CalcJointVelDiff(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1, Eigen::VectorXd& out_diff);
+};

Engine/lib/bullet/examples/SharedMemory/plugins/stablePDPlugin/LICENSE.txt

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2018 Xue Bin Peng
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

Engine/lib/bullet/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp

@@ -0,0 +1,763 @@
+#include "MathUtil.h"
+#include <time.h>
+#define _USE_MATH_DEFINES
+#include <math.h>
+
+
+int cMathUtil::Clamp(int val, int min, int max)
+{
+	return std::max(min, std::min(val, max));
+}
+
+void cMathUtil::Clamp(const Eigen::VectorXd& min, const Eigen::VectorXd& max, Eigen::VectorXd& out_vec)
+{
+	out_vec = out_vec.cwiseMin(max).cwiseMax(min);
+}
+
+double cMathUtil::Clamp(double val, double min, double max)
+{
+	return std::max(min, std::min(val, max));
+}
+
+double cMathUtil::Saturate(double val)
+{
+	return Clamp(val, 0.0, 1.0);
+}
+
+double cMathUtil::Lerp(double t, double val0, double val1)
+{
+	return (1 - t) * val0 + t * val1;
+}
+
+double cMathUtil::NormalizeAngle(double theta)
+{
+	// normalizes theta to be between [-pi, pi]
+	double norm_theta = fmod(theta, 2 * M_PI);
+	if (norm_theta > M_PI)
+	{
+		norm_theta = -2 * M_PI + norm_theta;
+	}
+	else if (norm_theta < -M_PI)
+	{
+		norm_theta = 2 * M_PI + norm_theta;
+	}
+	return norm_theta;
+}
+
+
+double cMathUtil::SmoothStep(double t)
+{
+	double val = t * t * t * (t * (t * 6 - 15) + 10);
+	return val;
+}
+
+
+tMatrix cMathUtil::TranslateMat(const tVector& trans)
+{
+	tMatrix mat = tMatrix::Identity();
+	mat(0, 3) = trans[0];
+	mat(1, 3) = trans[1];
+	mat(2, 3) = trans[2];
+	return mat;
+}
+
+tMatrix cMathUtil::ScaleMat(double scale)
+{
+	return ScaleMat(tVector::Ones() * scale);
+}
+
+tMatrix cMathUtil::ScaleMat(const tVector& scale)
+{
+	tMatrix mat = tMatrix::Identity();
+	mat(0, 0) = scale[0];
+	mat(1, 1) = scale[1];
+	mat(2, 2) = scale[2];
+	return mat;
+}
+
+tMatrix cMathUtil::RotateMat(const tVector& euler)
+{
+	double x = euler[0];
+	double y = euler[1];
+	double z = euler[2];
+
+	double x_s = std::sin(x);
+	double x_c = std::cos(x);
+	double y_s = std::sin(y);
+	double y_c = std::cos(y);
+	double z_s = std::sin(z);
+	double z_c = std::cos(z);
+
+	tMatrix mat = tMatrix::Identity();
+	mat(0, 0) = y_c * z_c;
+	mat(1, 0) = y_c * z_s;
+	mat(2, 0) = -y_s;
+
+	mat(0, 1) = x_s * y_s * z_c - x_c * z_s;
+	mat(1, 1) = x_s * y_s * z_s + x_c * z_c;
+	mat(2, 1) = x_s * y_c;
+
+	mat(0, 2) = x_c * y_s * z_c + x_s * z_s;
+	mat(1, 2) = x_c * y_s * z_s - x_s * z_c;
+	mat(2, 2) = x_c * y_c;
+
+	return mat;
+}
+
+tMatrix cMathUtil::RotateMat(const tVector& axis, double theta)
+{
+	assert(std::abs(axis.squaredNorm() - 1) < 0.0001);
+	
+	double c = std::cos(theta);
+	double s = std::sin(theta);
+	double x = axis[0];
+	double y = axis[1];
+	double z = axis[2];
+
+	tMatrix mat;
+	mat <<	c + x * x * (1 - c),		x * y * (1 - c) - z * s,	x * z * (1 - c) + y * s,	0,
+			y * x * (1 - c) + z * s,	c + y * y * (1 - c),		y * z * (1 - c) - x * s,	0,
+			z * x * (1 - c) - y * s,	z * y * (1 - c) + x * s,	c + z * z * (1 - c),		0,
+			0,							0,							0,							1;
+
+	return mat;
+}
+
+tMatrix cMathUtil::RotateMat(const tQuaternion& q)
+{
+	tMatrix mat = tMatrix::Identity();
+
+	double sqw = q.w() * q.w();
+	double sqx = q.x()*  q.x();
+	double sqy = q.y() * q.y();
+	double sqz = q.z() * q.z();
+	double invs = 1 / (sqx + sqy + sqz + sqw);
+
+	mat(0, 0) = (sqx - sqy - sqz + sqw) * invs;
+	mat(1, 1) = (-sqx + sqy - sqz + sqw) * invs;
+	mat(2, 2) = (-sqx - sqy + sqz + sqw) * invs;
+
+	double tmp1 = q.x()*q.y();
+	double tmp2 = q.z()*q.w();
+	mat(1, 0) = 2.0 * (tmp1 + tmp2) * invs;
+	mat(0, 1) = 2.0 * (tmp1 - tmp2) * invs;
+
+	tmp1 = q.x()*q.z();
+	tmp2 = q.y()*q.w();
+	mat(2, 0) = 2.0 * (tmp1 - tmp2) * invs;
+	mat(0, 2) = 2.0 * (tmp1 + tmp2) * invs;
+
+	tmp1 = q.y()*q.z();
+	tmp2 = q.x()*q.w();
+	mat(2, 1) = 2.0 * (tmp1 + tmp2) * invs;
+	mat(1, 2) = 2.0 * (tmp1 - tmp2) * invs;
+
+	return mat;
+}
+
+tMatrix cMathUtil::CrossMat(const tVector& a)
+{
+	tMatrix m;
+	m << 0,		-a[2],	a[1],	0,
+		 a[2],	0,		-a[0],	0,
+		 -a[1],	a[0],	0,		0,
+		 0,		0,		0,		1;
+	return m;
+}
+
+tMatrix cMathUtil::InvRigidMat(const tMatrix& mat)
+{
+	tMatrix inv_mat = tMatrix::Zero();
+	inv_mat.block(0, 0, 3, 3) = mat.block(0, 0, 3, 3).transpose();
+	inv_mat.col(3) = -inv_mat * mat.col(3);
+	inv_mat(3, 3) = 1;
+	return inv_mat;
+}
+
+tVector cMathUtil::GetRigidTrans(const tMatrix& mat)
+{
+	return tVector(mat(0, 3), mat(1, 3), mat(2, 3), 0);
+}
+
+tVector cMathUtil::InvEuler(const tVector& euler)
+{
+	tMatrix inv_mat = cMathUtil::RotateMat(tVector(1, 0, 0, 0), -euler[0])
+					* cMathUtil::RotateMat(tVector(0, 1, 0, 0), -euler[1])
+					* cMathUtil::RotateMat(tVector(0, 0, 1, 0), -euler[2]);
+	tVector inv_euler = cMathUtil::RotMatToEuler(inv_mat);
+	return inv_euler;
+}
+
+void cMathUtil::RotMatToAxisAngle(const tMatrix& mat, tVector& out_axis, double& out_theta)
+{
+	double c = (mat(0, 0) + mat(1, 1) + mat(2, 2) - 1) * 0.5;
+	c = cMathUtil::Clamp(c, -1.0, 1.0);
+
+	out_theta = std::acos(c);
+	if (std::abs(out_theta) < 0.00001)
+	{
+		out_axis = tVector(0, 0, 1, 0);
+	}
+	else
+	{
+		double m21 = mat(2, 1) - mat(1, 2);
+		double m02 = mat(0, 2) - mat(2, 0);
+		double m10 = mat(1, 0) - mat(0, 1);
+		double denom = std::sqrt(m21 * m21 + m02 * m02 + m10 * m10);
+		out_axis[0] = m21 / denom;
+		out_axis[1] = m02 / denom;
+		out_axis[2] = m10 / denom;
+		out_axis[3] = 0;
+	}
+}
+
+tVector cMathUtil::RotMatToEuler(const tMatrix& mat)
+{
+	tVector euler;
+	euler[0] = std::atan2(mat(2, 1), mat(2, 2));
+	euler[1] = std::atan2(-mat(2, 0), std::sqrt(mat(2, 1) * mat(2, 1) + mat(2, 2) * mat(2, 2)));
+	euler[2] = std::atan2(mat(1, 0), mat(0, 0)); 
+	euler[3] = 0;
+	return euler;
+}
+
+tQuaternion cMathUtil::RotMatToQuaternion(const tMatrix& mat)
+{
+	double tr = mat(0, 0) + mat(1, 1) + mat(2, 2);
+	tQuaternion q;
+
+	if (tr > 0) {
+		double S = sqrt(tr + 1.0) * 2; // S=4*qw 
+		q.w() = 0.25 * S;
+		q.x() = (mat(2, 1) - mat(1, 2)) / S;
+		q.y() = (mat(0, 2) - mat(2, 0)) / S;
+		q.z() = (mat(1, 0) - mat(0, 1)) / S;
+	}
+	else if ((mat(0, 0) > mat(1, 1) && (mat(0, 0) > mat(2, 2)))) {
+		double S = sqrt(1.0 + mat(0, 0) - mat(1, 1) - mat(2, 2)) * 2; // S=4*qx 
+		q.w() = (mat(2, 1) - mat(1, 2)) / S;
+		q.x() = 0.25 * S;
+		q.y() = (mat(0, 1) + mat(1, 0)) / S;
+		q.z() = (mat(0, 2) + mat(2, 0)) / S;
+	}
+	else if (mat(1, 1) > mat(2, 2)) {
+		double S = sqrt(1.0 + mat(1, 1) - mat(0, 0) - mat(2, 2)) * 2; // S=4*qy
+		q.w() = (mat(0, 2) - mat(2, 0)) / S;
+		q.x() = (mat(0, 1) + mat(1, 0)) / S;
+		q.y() = 0.25 * S;
+		q.z() = (mat(1, 2) + mat(2, 1)) / S;
+	}
+	else {
+		double S = sqrt(1.0 + mat(2, 2) - mat(0, 0) - mat(1, 1)) * 2; // S=4*qz
+		q.w() = (mat(1, 0) - mat(0, 1)) / S;
+		q.x() = (mat(0, 2) + mat(2, 0)) / S;
+		q.y() = (mat(1, 2) + mat(2, 1)) / S;
+		q.z() = 0.25 * S;
+	}
+
+	return q;
+}
+
+void cMathUtil::EulerToAxisAngle(const tVector& euler, tVector& out_axis, double& out_theta)
+{
+	double x = euler[0];
+	double y = euler[1];
+	double z = euler[2];
+
+	double x_s = std::sin(x);
+	double x_c = std::cos(x);
+	double y_s = std::sin(y);
+	double y_c = std::cos(y);
+	double z_s = std::sin(z);
+	double z_c = std::cos(z);
+
+	double c = (y_c * z_c + x_s * y_s * z_s + x_c * z_c + x_c * y_c - 1) * 0.5;
+	c = Clamp(c, -1.0, 1.0);
+
+	out_theta = std::acos(c);
+	if (std::abs(out_theta) < 0.00001)
+	{
+		out_axis = tVector(0, 0, 1, 0);
+	}
+	else
+	{
+		double m21 = x_s * y_c - x_c * y_s * z_s + x_s * z_c;
+		double m02 = x_c * y_s * z_c + x_s * z_s + y_s;
+		double m10 = y_c * z_s - x_s * y_s * z_c + x_c * z_s;
+		double denom = std::sqrt(m21 * m21 + m02 * m02 + m10 * m10);
+		out_axis[0] = m21 / denom;
+		out_axis[1] = m02 / denom;
+		out_axis[2] = m10 / denom;
+		out_axis[3] = 0;
+	}
+}
+
+
+tMatrix cMathUtil::DirToRotMat(const tVector& dir, const tVector& up)
+{
+	tVector x = up.cross3(dir);
+	double x_norm = x.norm();
+	if (x_norm == 0)
+	{
+		x_norm = 1;
+		x = (dir.dot(up) >= 0) ? tVector(1, 0, 0, 0) : tVector(-1, 0, 0, 0);
+	}
+	x /= x_norm;
+
+	tVector y = dir.cross3(x).normalized();
+	tVector z = dir;
+
+	tMatrix mat = tMatrix::Identity();
+	mat.block(0, 0, 3, 1) = x.segment(0, 3);
+	mat.block(0, 1, 3, 1) = y.segment(0, 3);
+	mat.block(0, 2, 3, 1) = z.segment(0, 3);
+
+	return mat;
+}
+
+void cMathUtil::DeltaRot(const tVector& axis0, double theta0, const tVector& axis1, double theta1,
+							tVector& out_axis, double& out_theta)
+{
+	tMatrix R0 = RotateMat(axis0, theta0);
+	tMatrix R1 = RotateMat(axis1, theta1);
+	tMatrix M = DeltaRot(R0, R1);
+	RotMatToAxisAngle(M, out_axis, out_theta);
+}
+
+tMatrix cMathUtil::DeltaRot(const tMatrix& R0, const tMatrix& R1)
+{
+	return R1 * R0.transpose();
+}
+
+tQuaternion cMathUtil::EulerToQuaternion(const tVector& euler)
+{
+	tVector axis;
+	double theta;
+	EulerToAxisAngle(euler, axis, theta);
+	return AxisAngleToQuaternion(axis, theta);
+}
+
+
+tQuaternion cMathUtil::AxisAngleToQuaternion(const tVector& axis, double theta)
+{
+	// axis must be normalized
+	double c = std::cos(theta / 2);
+	double s = std::sin(theta / 2);
+	tQuaternion q;
+	q.w() = c;
+	q.x() = s * axis[0];
+	q.y() = s * axis[1];
+	q.z() = s * axis[2];
+	return q;
+}
+
+void cMathUtil::QuaternionToAxisAngle(const tQuaternion& q, tVector& out_axis, double& out_theta)
+{
+	out_theta = 0;
+	out_axis = tVector(0, 0, 1, 0);
+
+	tQuaternion q1 = q;
+	if (q1.w() > 1)
+	{
+		q1.normalize();
+	}
+
+	double sin_theta = std::sqrt(1 - q1.w() * q1.w());
+	if (sin_theta > 0.000001)
+	{
+		out_theta = 2 * std::acos(q1.w());
+		out_theta = cMathUtil::NormalizeAngle(out_theta);
+		out_axis = tVector(q1.x(), q1.y(), q1.z(), 0) / sin_theta;
+	}
+}
+
+tMatrix cMathUtil::BuildQuaternionDiffMat(const tQuaternion& q)
+{
+	tMatrix mat;
+	mat << -0.5 * q.x(), -0.5 * q.y(), -0.5 * q.z(), 0,
+		0.5 * q.w(), -0.5 * q.z(), 0.5 * q.y(), 0,
+		0.5 * q.z(), 0.5 * q.w(), -0.5 * q.x(), 0,
+		-0.5 * q.y(), 0.5 * q.x(), 0.5 * q.w(), 0;
+	return mat;
+}
+
+tVector cMathUtil::CalcQuaternionVel(const tQuaternion& q0, const tQuaternion& q1, double dt)
+{
+	tQuaternion q_diff = cMathUtil::QuatDiff(q0, q1);
+	tVector axis;
+	double theta;
+	QuaternionToAxisAngle(q_diff, axis, theta);
+	return (theta / dt) * axis;
+}
+
+tVector cMathUtil::CalcQuaternionVelRel(const tQuaternion& q0, const tQuaternion& q1, double dt)
+{
+	// calculate relative rotational velocity in the coordinate frame of q0
+	tQuaternion q_diff = q0.conjugate() * q1;
+	tVector axis;
+	double theta;
+	QuaternionToAxisAngle(q_diff, axis, theta);
+	return (theta / dt) * axis;
+}
+
+tQuaternion cMathUtil::VecToQuat(const tVector& v)
+{
+	return tQuaternion(v[0], v[1], v[2], v[3]);
+}
+
+tVector cMathUtil::QuatToVec(const tQuaternion& q)
+{
+	return tVector(q.w(), q.x(), q.y(), q.z());
+}
+
+tQuaternion cMathUtil::QuatDiff(const tQuaternion& q0, const tQuaternion& q1)
+{
+	return q1 * q0.conjugate();
+}
+
+double cMathUtil::QuatDiffTheta(const tQuaternion& q0, const tQuaternion& q1)
+{
+	tQuaternion dq = QuatDiff(q0, q1);
+	return QuatTheta(dq);
+}
+
+double cMathUtil::QuatTheta(const tQuaternion& dq)
+{
+	double theta = 0;
+	tQuaternion q1 = dq;
+	if (q1.w() > 1)
+	{
+		q1.normalize();
+	}
+
+	double sin_theta = std::sqrt(1 - q1.w() * q1.w());
+	if (sin_theta > 0.0001)
+	{
+		theta = 2 * std::acos(q1.w());
+		theta = cMathUtil::NormalizeAngle(theta);
+	}
+	return theta;
+}
+
+tQuaternion cMathUtil::VecDiffQuat(const tVector& v0, const tVector& v1)
+{
+	return tQuaternion::FromTwoVectors(v0.segment(0, 3), v1.segment(0, 3));
+}
+
+tVector cMathUtil::QuatRotVec(const tQuaternion& q, const tVector& dir)
+{
+	tVector rot_dir = tVector::Zero();
+	rot_dir.segment(0, 3)  = q * dir.segment(0, 3);
+	return rot_dir;
+}
+
+tQuaternion cMathUtil::MirrorQuaternion(const tQuaternion& q, eAxis axis)
+{
+	tQuaternion mirror_q;
+	mirror_q.w() = q.w();
+	mirror_q.x() = (axis == eAxisX) ? q.x() : -q.x();
+	mirror_q.y() = (axis == eAxisY) ? q.y() : -q.y();
+	mirror_q.z() = (axis == eAxisZ) ? q.z() : -q.z();
+	return mirror_q;
+}
+
+double cMathUtil::Sign(double val)
+{
+	return SignAux<double>(val);
+}
+
+int cMathUtil::Sign(int val)
+{
+	return SignAux<int>(val);
+}
+
+double cMathUtil::AddAverage(double avg0, int count0, double avg1, int count1)
+{
+	double total = count0 + count1;
+	return (count0 / total) * avg0 + (count1 / total) * avg1;
+}
+
+tVector cMathUtil::AddAverage(const tVector& avg0, int count0, const tVector& avg1, int count1)
+{
+	double total = count0 + count1;
+	return (count0 / total) * avg0 + (count1 / total) * avg1 ;
+}
+
+void cMathUtil::AddAverage(const Eigen::VectorXd& avg0, int count0, const Eigen::VectorXd& avg1, int count1, Eigen::VectorXd& out_result)
+{
+	double total = count0 + count1;
+	out_result = (count0 / total) * avg0 + (count1 / total) * avg1;
+}
+
+void cMathUtil::CalcSoftmax(const Eigen::VectorXd& vals, double temp, Eigen::VectorXd& out_prob)
+{
+	assert(out_prob.size() == vals.size());
+	int num_vals = static_cast<int>(vals.size());
+	double sum = 0;
+	double max_val = vals.maxCoeff();
+	for (int i = 0; i < num_vals; ++i)
+	{
+		double val = vals[i];
+		val = std::exp((val - max_val) / temp);
+		out_prob[i] = val;
+		sum += val;
+	}
+
+	out_prob /= sum;
+}
+
+double cMathUtil::EvalGaussian(const Eigen::VectorXd& mean, const Eigen::VectorXd& covar, const Eigen::VectorXd& sample)
+{
+	assert(mean.size() == covar.size());
+	assert(sample.size() == covar.size());
+
+	Eigen::VectorXd diff = sample - mean;
+	double exp_val = diff.dot(diff.cwiseQuotient(covar));
+	double likelihood = std::exp(-0.5 * exp_val);
+
+	double partition = CalcGaussianPartition(covar);
+	likelihood /= partition;
+	return likelihood;
+}
+
+double cMathUtil::EvalGaussian(double mean, double covar, double sample)
+{
+	double diff = sample - mean;
+	double exp_val = diff * diff / covar;
+	double norm = 1 / std::sqrt(2 * M_PI * covar);
+	double likelihood = norm * std::exp(-0.5 * exp_val);
+	return likelihood;
+}
+
+double cMathUtil::CalcGaussianPartition(const Eigen::VectorXd& covar)
+{
+	int data_size = static_cast<int>(covar.size());
+	double det = covar.prod();
+	double partition = std::sqrt(std::pow(2 * M_PI, data_size) * det);
+	return partition;
+}
+
+double cMathUtil::EvalGaussianLogp(const Eigen::VectorXd& mean, const Eigen::VectorXd& covar, const Eigen::VectorXd& sample)
+{
+	int data_size = static_cast<int>(covar.size());
+
+	Eigen::VectorXd diff = sample - mean;
+	double logp = -0.5 * diff.dot(diff.cwiseQuotient(covar));
+	double det = covar.prod();
+	logp += -0.5 * (data_size * std::log(2 * M_PI) + std::log(det));
+
+	return logp;
+}
+
+double cMathUtil::EvalGaussianLogp(double mean, double covar, double sample)
+{
+	double diff = sample - mean;
+	double logp = -0.5 * diff * diff / covar;
+	logp += -0.5 * (std::log(2 * M_PI) + std::log(covar));
+	return logp;
+}
+
+double cMathUtil::Sigmoid(double x)
+{
+	return Sigmoid(x, 1, 0);
+}
+
+double cMathUtil::Sigmoid(double x, double gamma, double bias)
+{
+	double exp = -gamma * (x + bias);
+	double val = 1 / (1 + std::exp(exp));
+	return val;
+}
+
+
+tVector cMathUtil::CalcBarycentric(const tVector& p, const tVector& a, const tVector& b, const tVector& c)
+{
+	tVector v0 = b - a;
+	tVector v1 = c - a;
+	tVector v2 = p - a;
+
+	double d00 = v0.dot(v0);
+	double d01 = v0.dot(v1);
+	double d11 = v1.dot(v1);
+	double d20 = v2.dot(v0);
+	double d21 = v2.dot(v1);
+	double denom = d00 * d11 - d01 * d01;
+	double v = (d11 * d20 - d01 * d21) / denom;
+	double w = (d00 * d21 - d01 * d20) / denom;
+	double u = 1.0f - v - w;
+
+	return tVector(u, v, w, 0);
+}
+
+bool cMathUtil::ContainsAABB(const tVector& pt, const tVector& aabb_min, const tVector& aabb_max)
+{
+	bool contains = pt[0] >= aabb_min[0] && pt[1] >= aabb_min[1] && pt[2] >= aabb_min[2]
+					&& pt[0] <= aabb_max[0] && pt[1] <= aabb_max[1] && pt[2] <= aabb_max[2];
+	return contains;
+}
+
+bool cMathUtil::ContainsAABB(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1)
+{
+	return ContainsAABB(aabb_min0, aabb_min1, aabb_max1) && ContainsAABB(aabb_max0, aabb_min1, aabb_max1);
+}
+
+bool cMathUtil::ContainsAABBXZ(const tVector& pt, const tVector& aabb_min, const tVector& aabb_max)
+{
+	bool contains = pt[0] >= aabb_min[0] && pt[2] >= aabb_min[2]
+		&& pt[0] <= aabb_max[0] && pt[2] <= aabb_max[2];
+	return contains;
+}
+
+bool cMathUtil::ContainsAABBXZ(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1)
+{
+	return ContainsAABBXZ(aabb_min0, aabb_min1, aabb_max1) && ContainsAABBXZ(aabb_max0, aabb_min1, aabb_max1);
+}
+
+void cMathUtil::CalcAABBIntersection(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1,
+									tVector& out_min, tVector& out_max)
+{
+	out_min = aabb_min0.cwiseMax(aabb_min1);
+	out_max = aabb_max0.cwiseMin(aabb_max1);
+	if (out_min[0] > out_max[0])
+	{
+		out_min[0] = 0;
+		out_max[0] = 0;
+	}
+	if (out_min[1] > out_max[1])
+	{
+		out_min[1] = 0;
+		out_max[1] = 0;
+	}
+	if (out_min[2] > out_max[2])
+	{
+		out_min[2] = 0;
+		out_max[2] = 0;
+	}
+}
+
+void cMathUtil::CalcAABBUnion(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1,
+								tVector& out_min, tVector& out_max)
+{
+	out_min = aabb_min0.cwiseMin(aabb_min1);
+	out_max = aabb_max0.cwiseMax(aabb_max1);
+}
+
+bool cMathUtil::IntersectAABB(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1)
+{
+	tVector center0 = 0.5 * (aabb_max0 + aabb_min0);
+	tVector center1 = 0.5 * (aabb_max1 + aabb_min1);
+	tVector size0 = aabb_max0 - aabb_min0;
+	tVector size1 = aabb_max1 - aabb_min1;
+	tVector test_len = 0.5 * (size0 + size1);
+	tVector delta = center1 - center0;
+	bool overlap = (std::abs(delta[0]) <= test_len[0]) && (std::abs(delta[1]) <= test_len[1]) && (std::abs(delta[2]) <= test_len[2]);
+	return overlap;
+}
+
+bool cMathUtil::IntersectAABBXZ(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1)
+{
+	tVector center0 = 0.5 * (aabb_max0 + aabb_min0);
+	tVector center1 = 0.5 * (aabb_max1 + aabb_min1);
+	tVector size0 = aabb_max0 - aabb_min0;
+	tVector size1 = aabb_max1 - aabb_min1;
+	tVector test_len = 0.5 * (size0 + size1);
+	tVector delta = center1 - center0;
+	bool overlap = (std::abs(delta[0]) <= test_len[0]) && (std::abs(delta[2]) <= test_len[2]);
+	return overlap;
+}
+
+bool cMathUtil::CheckNextInterval(double delta, double curr_val, double int_size)
+{
+	double pad = 0.001 * delta;
+	int curr_count = static_cast<int>(std::floor((curr_val + pad) / int_size));
+	int prev_count = static_cast<int>(std::floor((curr_val + pad - delta) / int_size));
+	bool new_action = (curr_count != prev_count);
+	return new_action;
+}
+
+
+
+void cMathUtil::QuatSwingTwistDecomposition(const tQuaternion& q, const tVector& dir, tQuaternion& out_swing, tQuaternion& out_twist)
+{
+	assert(std::abs(dir.norm() - 1) < 0.000001);
+	assert(std::abs(q.norm() - 1) < 0.000001);
+
+	tVector q_axis = tVector(q.x(), q.y(), q.z(), 0);
+	double p = q_axis.dot(dir);
+	tVector twist_axis = p * dir;
+	out_twist = tQuaternion(q.w(), twist_axis[0], twist_axis[1], twist_axis[2]);
+	out_twist.normalize();
+	out_swing = q * out_twist.conjugate();
+}
+
+tQuaternion cMathUtil::ProjectQuat(const tQuaternion& q, const tVector& dir)
+{
+	assert(std::abs(dir.norm() - 1) < 0.00001);
+	tVector ref_axis = tVector::Zero();
+	int min_idx = 0;
+	dir.cwiseAbs().minCoeff(&min_idx);
+	ref_axis[min_idx] = 1;
+
+	tVector rot_dir0 = dir.cross3(ref_axis);
+	tVector rot_dir1 = cMathUtil::QuatRotVec(q, rot_dir0);
+	rot_dir1 -= rot_dir1.dot(dir) * dir;
+
+	double dir1_norm = rot_dir1.norm();
+	tQuaternion p_rot = tQuaternion::Identity();
+	if (dir1_norm > 0.0001)
+	{
+		rot_dir1 /= dir1_norm;
+		p_rot = cMathUtil::VecDiffQuat(rot_dir0, rot_dir1);
+	}
+	return p_rot;
+}
+
+void cMathUtil::ButterworthFilter(double dt, double cutoff, Eigen::VectorXd& out_x)
+{
+	double sampling_rate = 1 / dt;
+	int n = static_cast<int>(out_x.size());
+
+	double wc = std::tan(cutoff * M_PI / sampling_rate);
+	double k1 = std::sqrt(2.0) * wc;
+	double k2 = wc * wc;
+	double a = k2 / (1 + k1 + k2);
+	double b = 2 * a;
+	double c = a;
+	double k3 = b / k2;
+	double d = -2 * a + k3;
+	double e = 1 - (2 * a) - k3;
+
+	double xm2 = out_x[0];
+	double xm1 = out_x[0];
+	double ym2 = out_x[0];
+	double ym1 = out_x[0];
+
+	for (int s = 0; s < n; ++s) 
+	{
+		double x = out_x[s];
+		double y = a * x + b * xm1 + c * xm2 + d * ym1 + e * ym2;
+
+		out_x[s] = y;
+		xm2 = xm1;
+		xm1 = x;
+		ym2 = ym1;
+		ym1 = y;
+	}
+
+	double yp2 = out_x[n - 1];
+	double yp1 = out_x[n - 1];
+	double zp2 = out_x[n - 1];
+	double zp1 = out_x[n - 1];
+
+	for (int t = n - 1; t >= 0; --t) 
+	{
+		double y = out_x[t];
+		double z = a * y + b * yp1 + c * yp2 + d * zp1 + e * zp2;
+
+		out_x[t] = z;
+		yp2 = yp1;
+		yp1 = y;
+		zp2 = zp1;
+		zp1 = z;
+	}
+}

Engine/lib/bullet/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h

@@ -0,0 +1,131 @@
+#pragma once
+
+
+#include "Eigen/Dense"
+#include "Eigen/StdVector"
+#include "Eigen/Geometry"
+
+#define _USE_MATH_DEFINES
+#include "math.h"
+
+const int gInvalidIdx = -1;
+
+// for convenience define standard vector for rendering
+typedef Eigen::Vector4d tVector;
+typedef Eigen::Vector4d tVector3;
+typedef Eigen::Matrix4d tMatrix;
+typedef Eigen::Matrix3d tMatrix3;
+typedef Eigen::Quaterniond tQuaternion;
+
+
+const double gRadiansToDegrees = 57.2957795;
+const double gDegreesToRadians = 1.0 / gRadiansToDegrees;
+const tVector gGravity = tVector(0, -9.8, 0, 0);
+const double gInchesToMeters = 0.0254;
+const double gFeetToMeters = 0.3048;
+
+class cMathUtil
+{
+public:
+	enum eAxis
+	{
+		eAxisX,
+		eAxisY,
+		eAxisZ,
+		eAxisMax
+	};
+
+	static int Clamp(int val, int min, int max);
+	static void Clamp(const Eigen::VectorXd& min, const Eigen::VectorXd& max, Eigen::VectorXd& out_vec);
+	static double Clamp(double val, double min, double max);
+	static double Saturate(double val);
+	static double Lerp(double t, double val0, double val1);
+
+	static double NormalizeAngle(double theta);
+
+	static double SmoothStep(double t);
+
+	// matrices
+	static tMatrix TranslateMat(const tVector& trans);
+	static tMatrix ScaleMat(double scale);
+	static tMatrix ScaleMat(const tVector& scale);
+	static tMatrix RotateMat(const tVector& euler); // euler angles order rot(Z) * rot(Y) * rot(X)
+	static tMatrix RotateMat(const tVector& axis, double theta);
+	static tMatrix RotateMat(const tQuaternion& q);
+	static tMatrix CrossMat(const tVector& a);
+	// inverts a transformation consisting only of rotations and translations
+	static tMatrix InvRigidMat(const tMatrix& mat);
+	static tVector GetRigidTrans(const tMatrix& mat);
+	static tVector InvEuler(const tVector& euler);
+	static void RotMatToAxisAngle(const tMatrix& mat, tVector& out_axis, double& out_theta);
+	static tVector RotMatToEuler(const tMatrix& mat);
+	static tQuaternion RotMatToQuaternion(const tMatrix& mat);
+	static void EulerToAxisAngle(const tVector& euler, tVector& out_axis, double& out_theta);
+	
+	static tMatrix DirToRotMat(const tVector& dir, const tVector& up);
+
+	static void DeltaRot(const tVector& axis0, double theta0, const tVector& axis1, double theta1,
+							tVector& out_axis, double& out_theta);
+	static tMatrix DeltaRot(const tMatrix& R0, const tMatrix& R1);
+
+	static tQuaternion EulerToQuaternion(const tVector& euler);
+	
+	static tQuaternion AxisAngleToQuaternion(const tVector& axis, double theta);
+	static void QuaternionToAxisAngle(const tQuaternion& q, tVector& out_axis, double& out_theta);
+	static tMatrix BuildQuaternionDiffMat(const tQuaternion& q);
+	static tVector CalcQuaternionVel(const tQuaternion& q0, const tQuaternion& q1, double dt);
+	static tVector CalcQuaternionVelRel(const tQuaternion& q0, const tQuaternion& q1, double dt);
+	static tQuaternion VecToQuat(const tVector& v);
+	static tVector QuatToVec(const tQuaternion& q);
+	static tQuaternion QuatDiff(const tQuaternion& q0, const tQuaternion& q1);
+	static double QuatDiffTheta(const tQuaternion& q0, const tQuaternion& q1);
+	static double QuatTheta(const tQuaternion& dq);
+	static tQuaternion VecDiffQuat(const tVector& v0, const tVector& v1);
+	static tVector QuatRotVec(const tQuaternion& q, const tVector& dir);
+	static tQuaternion MirrorQuaternion(const tQuaternion& q, eAxis axis);
+
+	static double Sign(double val);
+	static int Sign(int val);
+
+	static double AddAverage(double avg0, int count0, double avg1, int count1);
+	static tVector AddAverage(const tVector& avg0, int count0, const tVector& avg1, int count1);
+	static void AddAverage(const Eigen::VectorXd& avg0, int count0, const Eigen::VectorXd& avg1, int count1, Eigen::VectorXd& out_result);
+	static void CalcSoftmax(const Eigen::VectorXd& vals, double temp, Eigen::VectorXd& out_prob);
+	static double EvalGaussian(const Eigen::VectorXd& mean, const Eigen::VectorXd& covar, const Eigen::VectorXd& sample);
+	static double EvalGaussian(double mean, double covar, double sample);
+	static double CalcGaussianPartition(const Eigen::VectorXd& covar);
+	static double EvalGaussianLogp(double mean, double covar, double sample);
+	static double EvalGaussianLogp(const Eigen::VectorXd& mean, const Eigen::VectorXd& covar, const Eigen::VectorXd& sample);
+	static double Sigmoid(double x);
+	static double Sigmoid(double x, double gamma, double bias);
+
+	static tVector CalcBarycentric(const tVector& p, const tVector& a, const tVector& b, const tVector& c);
+
+	static bool ContainsAABB(const tVector& pt, const tVector& aabb_min, const tVector& aabb_max);
+	static bool ContainsAABB(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1);
+	static bool ContainsAABBXZ(const tVector& pt, const tVector& aabb_min, const tVector& aabb_max);
+	static bool ContainsAABBXZ(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1);
+	static void CalcAABBIntersection(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1,
+									tVector& out_min, tVector& out_max);
+	static void CalcAABBUnion(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1,
+								tVector& out_min, tVector& out_max);
+	static bool IntersectAABB(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1);
+	static bool IntersectAABBXZ(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1);
+
+	// check if curr_val and curr_val - delta belong to different intervals
+	static bool CheckNextInterval(double delta, double curr_val, double int_size);
+
+	
+	static void QuatSwingTwistDecomposition(const tQuaternion& q, const tVector& dir, tQuaternion& out_swing, tQuaternion& out_twist);
+	static tQuaternion ProjectQuat(const tQuaternion& q, const tVector& dir);
+
+	static void ButterworthFilter(double dt, double cutoff, Eigen::VectorXd& out_x);
+	
+private:
+	
+	template <typename T>
+	static T SignAux(T val)
+	{
+		return (T(0) < val) - (val < T(0));
+	}
+};

Engine/lib/bullet/examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp

@@ -0,0 +1,260 @@
+#include "RBDModel.h"
+#include "RBDUtil.h"
+#include "KinTree.h"
+#include "LinearMath/btQuickprof.h"
+cRBDModel::cRBDModel()
+{
+}
+
+cRBDModel::~cRBDModel()
+{
+}
+
+void cRBDModel::Init(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const tVector& gravity)
+{
+	assert(joint_mat.rows() == body_defs.rows());
+	mGravity = gravity;
+	mJointMat = joint_mat;
+	mBodyDefs = body_defs;
+
+	int num_dofs = GetNumDof();
+	int num_joints = GetNumJoints();
+	const int svs = gSpVecSize;
+
+	mPose = Eigen::VectorXd::Zero(num_dofs);
+	mVel = Eigen::VectorXd::Zero(num_dofs);
+
+	tMatrix trans_mat;
+	InitJointSubspaceArr();
+	mChildParentMatArr = Eigen::MatrixXd::Zero(num_joints * trans_mat.rows(), trans_mat.cols());
+	mSpWorldJointTransArr = Eigen::MatrixXd::Zero(num_joints * gSVTransRows, gSVTransCols);
+	mMassMat = Eigen::MatrixXd::Zero(num_dofs, num_dofs);
+	mBiasForce = Eigen::VectorXd::Zero(num_dofs);
+	mInertiaBuffer = Eigen::MatrixXd::Zero(num_joints * svs, svs);
+}
+
+void cRBDModel::Update(const Eigen::VectorXd& pose, const Eigen::VectorXd& vel)
+{
+	SetPose(pose);
+	SetVel(vel);
+
+	{
+		BT_PROFILE("rbdModel::UpdateJointSubspaceArr");
+		UpdateJointSubspaceArr();
+	}
+	{
+		BT_PROFILE("rbdModel::UpdateChildParentMatArr");
+		UpdateChildParentMatArr();
+	}
+	{
+		BT_PROFILE("rbdModel::UpdateSpWorldTrans");
+		UpdateSpWorldTrans();
+	}
+	{
+		BT_PROFILE("UpdateMassMat");
+		UpdateMassMat();
+	}
+	{
+		BT_PROFILE("UpdateBiasForce");
+		UpdateBiasForce();
+	}
+}
+
+int cRBDModel::GetNumDof() const
+{
+	return cKinTree::GetNumDof(mJointMat);
+}
+
+int cRBDModel::GetNumJoints() const
+{
+	return cKinTree::GetNumJoints(mJointMat);
+}
+
+void cRBDModel::SetGravity(const tVector& gravity)
+{
+	mGravity = gravity;
+}
+
+
+
+const tVector& cRBDModel::GetGravity() const
+{
+	return mGravity;
+}
+
+const Eigen::MatrixXd& cRBDModel::GetJointMat() const
+{
+	return mJointMat;
+}
+
+const Eigen::MatrixXd& cRBDModel::GetBodyDefs() const
+{
+	return mBodyDefs;
+}
+
+const Eigen::VectorXd& cRBDModel::GetPose() const
+{
+	return mPose;
+}
+
+const Eigen::VectorXd& cRBDModel::GetVel() const
+{
+	return mVel;
+}
+
+int cRBDModel::GetParent(int j) const
+{
+	return cKinTree::GetParent(mJointMat, j);
+}
+
+const Eigen::MatrixXd& cRBDModel::GetMassMat() const
+{
+	return mMassMat;
+}
+
+const Eigen::VectorXd& cRBDModel::GetBiasForce() const
+{
+	return mBiasForce;
+}
+
+Eigen::MatrixXd& cRBDModel::GetInertiaBuffer()
+{
+	return mInertiaBuffer;
+}
+
+tMatrix cRBDModel::GetChildParentMat(int j) const
+{
+	assert(j >= 0 && j < GetNumJoints());
+	tMatrix trans;
+	int r = static_cast<int>(trans.rows());
+	int c = static_cast<int>(trans.cols());
+	trans = mChildParentMatArr.block(j * r, 0, r, c);
+	return trans;
+}
+
+tMatrix cRBDModel::GetParentChildMat(int j) const
+{
+	tMatrix child_parent_trans = GetChildParentMat(j);
+	tMatrix parent_child_trans = cMathUtil::InvRigidMat(child_parent_trans);
+	return parent_child_trans;
+}
+
+cSpAlg::tSpTrans cRBDModel::GetSpChildParentTrans(int j) const
+{
+	tMatrix mat = GetChildParentMat(j);
+	return cSpAlg::MatToTrans(mat);
+}
+
+cSpAlg::tSpTrans cRBDModel::GetSpParentChildTrans(int j) const
+{
+	tMatrix mat = GetParentChildMat(j);
+	return cSpAlg::MatToTrans(mat);
+}
+
+tMatrix cRBDModel::GetWorldJointMat(int j) const
+{
+	cSpAlg::tSpTrans trans = GetSpWorldJointTrans(j);
+	return cSpAlg::TransToMat(trans);
+}
+
+tMatrix cRBDModel::GetJointWorldMat(int j) const
+{
+	cSpAlg::tSpTrans trans = GetSpJointWorldTrans(j);
+	return cSpAlg::TransToMat(trans);
+}
+
+cSpAlg::tSpTrans cRBDModel::GetSpWorldJointTrans(int j) const
+{
+	assert(j >= 0 && j < GetNumJoints());
+	cSpAlg::tSpTrans trans = cSpAlg::GetTrans(mSpWorldJointTransArr, j);
+	return trans;
+}
+
+cSpAlg::tSpTrans cRBDModel::GetSpJointWorldTrans(int j) const
+{
+	cSpAlg::tSpTrans world_joint_trans = GetSpWorldJointTrans(j);
+	return cSpAlg::InvTrans(world_joint_trans);
+}
+
+const Eigen::Block<const Eigen::MatrixXd> cRBDModel::GetJointSubspace(int j) const
+{
+	assert(j >= 0 && j < GetNumJoints());
+	int offset = cKinTree::GetParamOffset(mJointMat, j);
+	int dim = cKinTree::GetParamSize(mJointMat, j);
+	int r = static_cast<int>(mJointSubspaceArr.rows());
+	return mJointSubspaceArr.block(0, offset, r, dim);
+}
+
+tVector cRBDModel::CalcJointWorldPos(int j) const
+{
+	cSpAlg::tSpTrans world_joint_trans = GetSpWorldJointTrans(j);
+	tVector r = cSpAlg::GetRad(world_joint_trans);
+	return r;
+}
+
+void cRBDModel::SetPose(const Eigen::VectorXd& pose)
+{
+	mPose = pose;
+}
+
+void cRBDModel::SetVel(const Eigen::VectorXd& vel)
+{
+	mVel = vel;
+}
+
+void cRBDModel::InitJointSubspaceArr()
+{
+	int num_dofs = GetNumDof();
+	int num_joints = GetNumJoints();
+	mJointSubspaceArr = Eigen::MatrixXd(gSpVecSize, num_dofs);
+	for (int j = 0; j < num_joints; ++j)
+	{
+		int offset = cKinTree::GetParamOffset(mJointMat, j);
+		int dim = cKinTree::GetParamSize(mJointMat, j);
+		int r = static_cast<int>(mJointSubspaceArr.rows());
+		mJointSubspaceArr.block(0, offset, r, dim) = cRBDUtil::BuildJointSubspace(mJointMat, mPose, j);
+	}
+}
+
+void cRBDModel::UpdateJointSubspaceArr()
+{
+	int num_joints = GetNumJoints();
+	for (int j = 0; j < num_joints; ++j)
+	{
+		bool const_subspace = cRBDUtil::IsConstJointSubspace(mJointMat, j);
+		if (!const_subspace)
+		{
+			int offset = cKinTree::GetParamOffset(mJointMat, j);
+			int dim = cKinTree::GetParamSize(mJointMat, j);
+			int r = static_cast<int>(mJointSubspaceArr.rows());
+			mJointSubspaceArr.block(0, offset, r, dim) = cRBDUtil::BuildJointSubspace(mJointMat, mPose, j);
+		}
+	}
+}
+
+void cRBDModel::UpdateChildParentMatArr()
+{
+	int num_joints = GetNumJoints();
+	for (int j = 0; j < num_joints; ++j)
+	{
+		tMatrix child_parent_trans = cKinTree::ChildParentTrans(mJointMat, mPose, j);
+		int r = static_cast<int>(child_parent_trans.rows());
+		int c = static_cast<int>(child_parent_trans.cols());
+		mChildParentMatArr.block(j * r, 0, r, c) = child_parent_trans;
+	}
+}
+
+void cRBDModel::UpdateSpWorldTrans()
+{
+	cRBDUtil::CalcWorldJointTransforms(*this, mSpWorldJointTransArr);
+}
+
+void cRBDModel::UpdateMassMat()
+{
+	cRBDUtil::BuildMassMat(*this, mInertiaBuffer, mMassMat);
+}
+
+void cRBDModel::UpdateBiasForce()
+{
+	cRBDUtil::BuildBiasForce(*this, mBiasForce);
+}

Engine/lib/bullet/examples/SharedMemory/plugins/stablePDPlugin/RBDModel.h

@@ -0,0 +1,71 @@
+#pragma once
+
+#include "SpAlg.h"
+#include "MathUtil.h"
+
+// this class is mostly to help with efficiency by precomputing some useful
+// quantities for RBD calculations
+class cRBDModel
+{
+public:
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	cRBDModel();
+	virtual ~cRBDModel();
+
+	virtual void Init(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const tVector& gravity);
+	virtual void Update(const Eigen::VectorXd& pose, const Eigen::VectorXd& vel);
+
+	virtual int GetNumDof() const;
+	virtual int GetNumJoints() const;
+
+	virtual const tVector& GetGravity() const;
+	virtual void SetGravity(const tVector& gravity);
+
+	virtual const Eigen::MatrixXd& GetJointMat() const;
+	virtual const Eigen::MatrixXd& GetBodyDefs() const;
+	virtual const Eigen::VectorXd& GetPose() const;
+	virtual const Eigen::VectorXd& GetVel() const;
+	virtual int GetParent(int j) const;
+
+	virtual const Eigen::MatrixXd& GetMassMat() const;
+	virtual const Eigen::VectorXd& GetBiasForce() const;
+	virtual Eigen::MatrixXd& GetInertiaBuffer();
+
+	virtual tMatrix GetChildParentMat(int j) const;
+	virtual tMatrix GetParentChildMat(int j) const;
+	virtual cSpAlg::tSpTrans GetSpChildParentTrans(int j) const;
+	virtual cSpAlg::tSpTrans GetSpParentChildTrans(int j) const;
+
+	virtual tMatrix GetWorldJointMat(int j) const;
+	virtual tMatrix GetJointWorldMat(int j) const;
+	virtual cSpAlg::tSpTrans GetSpWorldJointTrans(int j) const;
+	virtual cSpAlg::tSpTrans GetSpJointWorldTrans(int j) const;
+
+	virtual const Eigen::Block<const Eigen::MatrixXd> GetJointSubspace(int j) const;
+	virtual tVector CalcJointWorldPos(int j) const;
+
+protected:
+	tVector mGravity;
+	Eigen::MatrixXd mJointMat;
+	Eigen::MatrixXd mBodyDefs;
+	Eigen::VectorXd mPose;
+	Eigen::VectorXd mVel;
+
+	Eigen::MatrixXd mJointSubspaceArr;
+	Eigen::MatrixXd mChildParentMatArr;
+	Eigen::MatrixXd mSpWorldJointTransArr;
+	Eigen::MatrixXd mMassMat;
+	Eigen::VectorXd mBiasForce;
+	Eigen::MatrixXd mInertiaBuffer;
+
+	virtual void SetPose(const Eigen::VectorXd& pose);
+	virtual void SetVel(const Eigen::VectorXd& vel);
+
+	virtual void InitJointSubspaceArr();
+
+	virtual void UpdateJointSubspaceArr();
+	virtual void UpdateChildParentMatArr();
+	virtual void UpdateSpWorldTrans();
+	virtual void UpdateMassMat();
+	virtual void UpdateBiasForce();
+};

Engine/lib/bullet/examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.h

@@ -0,0 +1,85 @@
+#pragma once
+
+#include <vector>
+#include <fstream>
+
+#include "RBDModel.h"
+#include "KinTree.h"
+
+class cRBDUtil
+{
+public:
+	static void SolveInvDyna(const cRBDModel& model, const Eigen::VectorXd& acc, Eigen::VectorXd& out_tau);
+
+	static void SolveForDyna(const cRBDModel& model, const Eigen::VectorXd& tau, Eigen::VectorXd& out_acc);
+	static void SolveForDyna(const cRBDModel& model, const Eigen::VectorXd& tau, const Eigen::VectorXd& total_force, Eigen::VectorXd& out_acc);
+
+	static void BuildMassMat(const cRBDModel& model, Eigen::MatrixXd& out_mass_mat);
+	static void BuildMassMat(const cRBDModel& model, Eigen::MatrixXd& inertia_buffer, Eigen::MatrixXd& out_mass_mat);
+	static void BuildBiasForce(const cRBDModel& model, Eigen::VectorXd& out_bias_force);
+
+	static void CalcGravityForce(const cRBDModel& model, Eigen::VectorXd& out_g_force);
+
+	static void BuildEndEffectorJacobian(const cRBDModel& model, int joint_id, Eigen::MatrixXd& out_J);
+	static void BuildEndEffectorJacobian(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int joint_id, Eigen::MatrixXd& out_J);
+	static Eigen::MatrixXd MultJacobianEndEff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel, const Eigen::MatrixXd& J, int joint_id);
+
+	static void BuildJacobian(const cRBDModel& model, Eigen::MatrixXd& out_J);
+	static Eigen::MatrixXd ExtractEndEffJacobian(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& J, int joint_id);
+
+	static void BuildCOMJacobian(const cRBDModel& model, Eigen::MatrixXd& out_J);
+	static void BuildCOMJacobian(const cRBDModel& model, const Eigen::MatrixXd& J, Eigen::MatrixXd& out_J);
+	static void BuildJacobianDot(const cRBDModel& model, Eigen::MatrixXd& out_J_dot);
+
+	static cSpAlg::tSpVec BuildCOMVelProdAcc(const cRBDModel& model);
+	static cSpAlg::tSpVec BuildCOMVelProdAccAux(const cRBDModel& model, const Eigen::MatrixXd& Jd);
+	static cSpAlg::tSpVec CalcVelProdAcc(const cRBDModel& model, const Eigen::MatrixXd& Jd, int joint_id);
+
+	static cSpAlg::tSpVec CalcJointWorldVel(const cRBDModel& model, int joint_id);
+	static cSpAlg::tSpVec CalcJointWorldVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int joint_id);
+	static cSpAlg::tSpVec CalcWorldVel(const cRBDModel& model, int joint_id);
+	static cSpAlg::tSpVec CalcWorldVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int joint_id);
+
+
+	static tVector CalcCoMPos(const cRBDModel& model);
+	static tVector CalcCoMVel(const cRBDModel& model);
+	static tVector CalcCoMVel(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& pose, const Eigen::VectorXd& vel);
+	static void CalcCoM(const cRBDModel& model, tVector& out_com, tVector& out_vel);
+	static void CalcCoM(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& pose, const Eigen::VectorXd& vel,
+						tVector& out_com, tVector& out_vel);
+
+	static cSpAlg::tSpTrans BuildParentChildTransform(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j);
+	static cSpAlg::tSpTrans BuildChildParentTransform(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j);
+	
+	// extract a cSpAlg::tSpTrans from a matrix presentating a stack of transforms
+	static void CalcWorldJointTransforms(const cRBDModel& model, Eigen::MatrixXd& out_trans_arr);
+	static bool IsConstJointSubspace(const Eigen::MatrixXd& joint_mat, int j);
+	static Eigen::MatrixXd BuildJointSubspace(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j);
+
+	static cSpAlg::tSpMat BuildMomentInertia(const Eigen::MatrixXd& body_defs, int part_id);
+
+protected:
+
+	static Eigen::MatrixXd BuildJointSubspaceRoot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose);
+	static Eigen::MatrixXd BuildJointSubspaceRevolute(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j);
+	static Eigen::MatrixXd BuildJointSubspacePrismatic(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j);
+	static Eigen::MatrixXd BuildJointSubspacePlanar(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j);
+	static Eigen::MatrixXd BuildJointSubspaceFixed(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j);
+	static Eigen::MatrixXd BuildJointSubspaceSpherical(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j);
+
+	static cSpAlg::tSpVec BuildCj(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q, const Eigen::VectorXd& q_dot, int j);
+	static cSpAlg::tSpVec BuildCjRoot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q, const Eigen::VectorXd& q_dot, int j);
+	static cSpAlg::tSpVec BuildCjRevolute(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j);
+	static cSpAlg::tSpVec BuildCjPrismatic(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j);
+	static cSpAlg::tSpVec BuildCjPlanar(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j);
+	static cSpAlg::tSpVec BuildCjFixed(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j);
+	static cSpAlg::tSpVec BuildCjSpherical(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j);
+
+	static cSpAlg::tSpMat BuildMomentInertiaBox(const Eigen::MatrixXd& body_defs, int part_id);
+	static cSpAlg::tSpMat BuildMomentInertiaCapsule(const Eigen::MatrixXd& body_defs, int part_id);
+	static cSpAlg::tSpMat BuildMomentInertiaSphere(const Eigen::MatrixXd& body_defs, int part_id);
+	static cSpAlg::tSpMat BuildMomentInertiaCylinder(const Eigen::MatrixXd& body_defs, int part_id);
+
+	// builds the spatial inertial matrix in the coordinate frame of the parent joint
+	static cSpAlg::tSpMat BuildInertiaSpatialMat(const Eigen::MatrixXd& body_defs, int part_id);
+};

Engine/lib/bullet/examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp

@@ -0,0 +1,38 @@
+#include "Shape.h"
+#include <assert.h>
+#include <stdio.h>
+
+bool cShape::ParseShape(const std::string& str, eShape& out_shape)
+{
+	bool succ = true;
+	if (str == "null")
+	{
+		out_shape = eShapeNull;  
+	}
+	else if (str == "box")
+	{
+		out_shape = eShapeBox;
+	}
+	else if (str == "capsule")
+	{
+		out_shape = eShapeCapsule;
+	}
+	else if (str == "sphere")
+	{
+		out_shape = eShapeSphere;
+	}
+	else if (str == "cylinder")
+	{
+		out_shape = eShapeCylinder;
+	}
+	else if (str == "plane")
+	{
+		out_shape = eShapePlane;
+	}
+	else
+	{
+		printf("Unsupported body shape %s\n", str.c_str());
+		assert(false);
+	}
+	return succ;
+}

Engine/lib/bullet/examples/SharedMemory/plugins/stablePDPlugin/Shape.h

@@ -0,0 +1,20 @@
+#pragma once
+
+#include <string>
+
+class cShape
+{
+public:
+	enum eShape
+	{
+		eShapeNull,
+		eShapeBox,
+		eShapeCapsule,
+		eShapeSphere,
+		eShapeCylinder,
+		eShapePlane,
+		eShapeMax,
+	};
+
+	static bool ParseShape(const std::string& str, cShape::eShape& out_shape);
+};

Engine/lib/bullet/examples/SharedMemory/plugins/stablePDPlugin/SpAlg.cpp

@@ -0,0 +1,353 @@
+#include "SpAlg.h"
+#include <iostream>
+
+
+cSpAlg::tSpVec cSpAlg::ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tVector& origin1)
+{
+	return ConvertCoordM(m0, origin0, origin1, tMatrix::Identity());
+}
+
+cSpAlg::tSpVec cSpAlg::ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tMatrix& R0,
+												const tVector& origin1, const tMatrix& R1)
+{
+	tMatrix R = R1 * R0.transpose();
+	return ConvertCoordM(m0, origin0, origin1, R);
+}
+
+
+cSpAlg::tSpVec cSpAlg::ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tVector& origin1, const tMatrix& R)
+{
+	tSpTrans X = BuildTrans(R, origin1 - origin0);
+	return ApplyTransM(X, m0);
+}
+
+cSpAlg::tSpVec cSpAlg::ConvertCoordF(const tSpVec& f0, const tVector& origin0, const tVector& origin1)
+{
+	return ConvertCoordF(f0, origin0, origin1, tMatrix::Identity());
+}
+
+cSpAlg::tSpVec cSpAlg::ConvertCoordF(const tSpVec& f0, const tVector& origin0, const tMatrix& R0,
+	const tVector& origin1, const tMatrix& R1)
+{
+	tMatrix R = R1 * R0.transpose();
+	return ConvertCoordF(f0, origin0, origin1, R);
+}
+
+cSpAlg::tSpVec cSpAlg::ConvertCoordF(const tSpVec& f0, const tVector& origin0,
+	const tVector& origin1, const tMatrix& R)
+{
+	tSpTrans X = BuildTrans(R, origin1 - origin0);
+	return ApplyTransF(X, f0);
+}
+
+
+cSpAlg::tSpVec cSpAlg::CrossM(const tSpVec& sv, const tSpVec& m)
+{
+	tVector sv_o = GetOmega(sv);
+	tVector sv_v = GetV(sv);
+	tVector m_o = GetOmega(m);
+	tVector m_v = GetV(m);
+
+	tVector o = sv_o.cross3(m_o);
+	tVector v = sv_v.cross3(m_o) + sv_o.cross3(m_v);
+
+	return BuildSV(o, v);
+}
+
+Eigen::MatrixXd cSpAlg::CrossMs(const tSpVec& sv, const Eigen::MatrixXd& ms)
+{
+	assert(ms.rows() == gSpVecSize);
+	Eigen::MatrixXd result(gSpVecSize, ms.cols());
+	for (int i = 0; i < ms.cols(); ++i)
+	{
+		const tSpVec& curr_m = ms.col(i);
+		result.col(i) = CrossM(sv, curr_m);
+	}
+	return result;
+}
+
+cSpAlg::tSpVec cSpAlg::CrossF(const tSpVec& sv, const tSpVec& f)
+{
+	tVector sv_o = GetOmega(sv);
+	tVector sv_v = GetV(sv);
+	tVector f_o = GetOmega(f);
+	tVector f_v = GetV(f);
+
+	tVector o = sv_o.cross3(f_o) + sv_v.cross3(f_v);
+	tVector v = sv_o.cross3(f_v);
+
+	return BuildSV(o, v);
+}
+
+Eigen::MatrixXd cSpAlg::CrossFs(const tSpVec& sv, const Eigen::MatrixXd& fs)
+{
+	assert(fs.rows() == gSpVecSize);
+	Eigen::MatrixXd result(gSpVecSize, fs.cols());
+	for (int i = 0; i < fs.cols(); ++i)
+	{
+		const tSpVec& curr_f = fs.col(i);
+		result.col(i) = CrossF(sv, curr_f);
+	}
+	return result;
+}
+
+cSpAlg::tSpVec cSpAlg::BuildSV(const tVector& v)
+{
+	return BuildSV(tVector::Zero(), v);
+}
+
+cSpAlg::tSpVec cSpAlg::BuildSV(const tVector& o, const tVector& v)
+{
+	tSpVec sv;
+	SetOmega(o, sv);
+	SetV(v, sv);
+	return sv;
+}
+
+tVector cSpAlg::GetOmega(const tSpVec& sv)
+{
+	tVector o = tVector::Zero();
+	o.block(0, 0, 3, 1) = sv.block(0, 0, 3, 1);
+	return o;
+}
+
+void cSpAlg::SetOmega(const tVector& o, tSpVec& out_sv)
+{
+	out_sv.block(0, 0, 3, 1) = o.block(0, 0, 3, 1);
+}
+
+tVector cSpAlg::GetV(const tSpVec& sv)
+{
+	tVector v = tVector::Zero();
+	v.block(0, 0, 3, 1) = sv.block(3, 0, 3, 1);
+	return v;
+}
+
+void cSpAlg::SetV(const tVector& v, tSpVec& out_sv)
+{
+	out_sv.block(3, 0, 3, 1) = v.block(0, 0, 3, 1);
+}
+
+cSpAlg::tSpTrans cSpAlg::BuildTrans()
+{
+	return BuildTrans(tMatrix::Identity(), tVector::Zero());
+}
+
+cSpAlg::tSpTrans cSpAlg::BuildTrans(const tMatrix& E, const tVector& r)
+{
+	tSpTrans X;
+	SetRad(r, X);
+	SetRot(E, X);
+	return X;
+}
+
+cSpAlg::tSpTrans cSpAlg::BuildTrans(const tVector& r)
+{
+	return BuildTrans(tMatrix::Identity(), r);
+}
+
+cSpAlg::tSpTrans cSpAlg::MatToTrans(const tMatrix& mat)
+{
+	tMatrix E = mat;
+	tVector r = mat.col(3);
+	r[3] = 0;
+	r = -E.transpose() * r;
+	return BuildTrans(E, r);
+}
+
+tMatrix cSpAlg::TransToMat(const tSpTrans& X)
+{
+	tMatrix E = GetRot(X);
+	tVector r = GetRad(X);
+	tMatrix m = E;
+	m.col(3) = -E * r;
+	m(3, 3) = 1;
+	return m;
+}
+
+cSpAlg::tSpMat cSpAlg::BuildSpatialMatM(const tSpTrans& X)
+{
+	tSpMat m = tSpMat::Zero();
+	tMatrix E = GetRot(X);
+	tVector r = GetRad(X);
+	tMatrix Er = E * cMathUtil::CrossMat(r);
+
+	m.block(0, 0, 3, 3) = E.block(0, 0, 3, 3);
+	m.block(3, 3, 3, 3) = E.block(0, 0, 3, 3);
+	m.block(3, 0, 3, 3) = -Er.block(0, 0, 3, 3);
+	return m;
+}
+
+cSpAlg::tSpMat cSpAlg::BuildSpatialMatF(const tSpTrans& X)
+{
+	tSpMat m = tSpMat::Zero();
+	tMatrix E = GetRot(X);
+	tVector r = GetRad(X);
+	tMatrix Er = E * cMathUtil::CrossMat(r);
+
+	m.block(0, 0, 3, 3) = E.block(0, 0, 3, 3);
+	m.block(3, 3, 3, 3) = E.block(0, 0, 3, 3);
+	m.block(0, 3, 3, 3) = -Er.block(0, 0, 3, 3);
+	return m;
+}
+
+
+cSpAlg::tSpTrans cSpAlg::InvTrans(const tSpTrans& X)
+{
+	tMatrix E = GetRot(X);
+	tVector r = GetRad(X);
+	tSpTrans inv_X = BuildTrans(E.transpose(), -E * r);
+	return inv_X;
+}
+
+tMatrix cSpAlg::GetRot(const tSpTrans& X)
+{
+	tMatrix E = tMatrix::Zero();
+	E.block(0, 0, 3, 3) = X.block(0, 0, 3, 3);
+	return E;
+}
+
+void cSpAlg::SetRot(const tMatrix& E, tSpTrans& out_X)
+{
+	out_X.block(0, 0, 3, 3) = E.block(0, 0, 3, 3);
+}
+
+tVector cSpAlg::GetRad(const tSpTrans& X)
+{
+	tVector r = tVector::Zero();
+	r.block(0, 0, 3, 1) = X.block(0, 3, 3, 1);
+	return r;
+}
+
+void cSpAlg::SetRad(const tVector& r, tSpTrans& out_X)
+{
+	out_X.block(0, 3, 3, 1) = r.block(0, 0, 3, 1);
+}
+
+cSpAlg::tSpVec cSpAlg::ApplyTransM(const tSpTrans& X, const tSpVec& sv)
+{
+	tMatrix E = GetRot(X);
+	tVector r = GetRad(X);
+	tVector o0 = GetOmega(sv);
+	tVector v0 = GetV(sv);
+
+	tVector o1 = E * o0;
+	tVector v1 = E * (v0 - r.cross3(o0));
+
+	tSpVec new_vec = BuildSV(o1, v1);
+	return new_vec;
+}
+
+cSpAlg::tSpVec cSpAlg::ApplyTransF(const tSpTrans& X, const tSpVec& sv)
+{
+	tMatrix E = GetRot(X);
+	tVector r = GetRad(X);
+	tVector o0 = GetOmega(sv);
+	tVector v0 = GetV(sv);
+
+	tVector o1 = E * (o0 - r.cross3(v0));
+	tVector v1 = E * v0;
+
+	tSpVec new_vec = BuildSV(o1, v1);
+	return new_vec;
+}
+
+Eigen::MatrixXd cSpAlg::ApplyTransM(const tSpTrans& X, const Eigen::MatrixXd& sm)
+{
+	assert(sm.rows() == gSpVecSize);
+	Eigen::MatrixXd result(gSpVecSize, sm.cols());
+	for (int i = 0; i < sm.cols(); ++i)
+	{
+		const tSpVec& curr_sv = sm.col(i);
+		result.col(i) = ApplyTransM(X, curr_sv);
+	}
+	return result;
+}
+
+Eigen::MatrixXd cSpAlg::ApplyTransF(const tSpTrans& X, const Eigen::MatrixXd& sm)
+{
+	assert(sm.rows() == gSpVecSize);
+	Eigen::MatrixXd result(gSpVecSize, sm.cols());
+	for (int i = 0; i < sm.cols(); ++i)
+	{
+		const tSpVec& curr_sv = sm.col(i);
+		result.col(i) = ApplyTransF(X, curr_sv);
+	}
+	return result;
+}
+
+cSpAlg::tSpVec cSpAlg::ApplyInvTransM(const tSpTrans& X, const tSpVec& sv)
+{
+	tMatrix E = GetRot(X);
+	tVector r = GetRad(X);
+	tVector o0 = GetOmega(sv);
+	tVector v0 = GetV(sv);
+
+	tVector o1 = E.transpose() * o0;
+	tVector v1 = E.transpose() * v0 + r.cross3(E.transpose() * o0);
+
+	tSpVec new_vec = BuildSV(o1, v1);
+	return new_vec;
+}
+
+cSpAlg::tSpVec cSpAlg::ApplyInvTransF(const tSpTrans& X, const tSpVec& sv)
+{
+	tMatrix E = GetRot(X);
+	tVector r = GetRad(X);
+	tVector o0 = GetOmega(sv);
+	tVector v0 = GetV(sv);
+
+	tVector o1 = E.transpose() * o0 + r.cross3(E.transpose() * v0);
+	tVector v1 = E.transpose() * v0;
+
+	tSpVec new_vec = BuildSV(o1, v1);
+	return new_vec;
+}
+
+Eigen::MatrixXd cSpAlg::ApplyInvTransM(const tSpTrans& X, const Eigen::MatrixXd& sm)
+{
+	assert(sm.rows() == gSpVecSize);
+	Eigen::MatrixXd result(sm.rows(), sm.cols());
+	for (int i = 0; i < sm.cols(); ++i)
+	{
+		const tSpVec& curr_sv = sm.col(i);
+		result.col(i) = ApplyInvTransM(X, curr_sv);
+	}
+	return result;
+}
+
+Eigen::MatrixXd cSpAlg::ApplyInvTransF(const tSpTrans& X, const Eigen::MatrixXd& sm)
+{
+	assert(sm.rows() == gSpVecSize);
+	Eigen::MatrixXd result(sm.rows(), sm.cols());
+	for (int i = 0; i < sm.cols(); ++i)
+	{
+		const tSpVec& curr_sv = sm.col(i);
+		result.col(i) = ApplyInvTransF(X, curr_sv);
+	}
+	return result;
+}
+
+cSpAlg::tSpTrans cSpAlg::CompTrans(const tSpTrans& X0, const tSpTrans& X1)
+{
+	tMatrix E0 = GetRot(X0);
+	tMatrix E1 = GetRot(X1);
+	tVector r0 = GetRad(X0);
+	tVector r1 = GetRad(X1);
+
+	tSpTrans X = BuildTrans(E0 * E1, r1 + E1.transpose() * r0);
+	return X;
+}
+
+cSpAlg::tSpTrans cSpAlg::GetTrans(const Eigen::MatrixXd& trans_arr, int j)
+{
+	assert(trans_arr.rows() >= gSVTransRows);
+	assert((trans_arr.rows() % gSVTransRows) == 0);
+	assert(trans_arr.cols() == gSVTransCols);
+
+	int row_idx = j * gSVTransRows;
+	assert(row_idx <= trans_arr.rows() - gSVTransRows);
+
+	tSpTrans X = trans_arr.block(row_idx, 0, gSVTransRows, gSVTransCols);
+	return X;
+}

Engine/lib/bullet/examples/SharedMemory/plugins/stablePDPlugin/SpAlg.h

@@ -0,0 +1,70 @@
+#pragma once
+
+#include "MathUtil.h"
+
+#define gSpVecSize 6
+#define gSVTransRows 3
+#define gSVTransCols 4
+
+
+// spatial algebra util
+class cSpAlg
+{
+public:
+	
+
+	typedef Eigen::Matrix<double, gSVTransRows, gSVTransCols> tSpTrans;
+	typedef Eigen::Matrix<double, gSpVecSize, 1> tSpVec;
+	typedef Eigen::Matrix<double, gSpVecSize, gSpVecSize> tSpMat;
+
+	static tSpVec ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tVector& origin1);
+	// rows of R shoold be the basis for the coordinate frame centered ar origin
+	static tSpVec ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tMatrix& R0, const tVector& origin1, const tMatrix& R1);
+	// R is a rotation from coord frame 0 to coord frame 1
+	static tSpVec ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tVector& origin1, const tMatrix& R);
+
+	static tSpVec ConvertCoordF(const tSpVec& m0, const tVector& origin0, const tVector& origin1);
+	static tSpVec ConvertCoordF(const tSpVec& m0, const tVector& origin0, const tMatrix& R0, const tVector& origin1, const tMatrix& R1);
+	static tSpVec ConvertCoordF(const tSpVec& m0, const tVector& origin0, const tVector& origin1, const tMatrix& R);
+
+	static tSpVec CrossM(const tSpVec& sv, const tSpVec& m);
+	static Eigen::MatrixXd CrossMs(const tSpVec& sv, const Eigen::MatrixXd& ms);
+	static tSpVec CrossF(const tSpVec& sv, const tSpVec& f);
+	static Eigen::MatrixXd CrossFs(const tSpVec& sv, const Eigen::MatrixXd& fs);
+
+	// Spatial Vector methods
+	static tSpVec BuildSV(const tVector& v);
+	static tSpVec BuildSV(const tVector& o, const tVector& v);
+	static tVector GetOmega(const tSpVec& sv);
+	static void SetOmega(const tVector& o, tSpVec& out_sv);
+	static tVector GetV(const tSpVec& sv);
+	static void SetV(const tVector& v, tSpVec& out_sv);
+
+	// tSpTrans methods
+	static tSpTrans BuildTrans();
+	static tSpTrans BuildTrans(const tMatrix& E, const tVector& r);
+	static tSpTrans BuildTrans(const tVector& r);
+
+	static tSpTrans MatToTrans(const tMatrix& mat);
+	static tMatrix TransToMat(const tSpTrans& X);
+	static tSpMat BuildSpatialMatM(const tSpTrans& X);
+	static tSpMat BuildSpatialMatF(const tSpTrans& X);
+	static tSpTrans InvTrans(const tSpTrans& X);
+	static tMatrix GetRot(const tSpTrans& X);
+	static void SetRot(const tMatrix& E, tSpTrans& out_X);
+	static tVector GetRad(const tSpTrans& X);
+	static void SetRad(const tVector& r, tSpTrans& out_X);
+
+	static tSpVec ApplyTransM(const tSpTrans& X, const tSpVec& sv);
+	static tSpVec ApplyTransF(const tSpTrans& X, const tSpVec& sv);
+	static Eigen::MatrixXd ApplyTransM(const tSpTrans& X, const Eigen::MatrixXd& sm);
+	static Eigen::MatrixXd ApplyTransF(const tSpTrans& X, const Eigen::MatrixXd& sm);
+	static tSpVec ApplyInvTransM(const tSpTrans& X, const tSpVec& sv);
+	static tSpVec ApplyInvTransF(const tSpTrans& X, const tSpVec& sv);
+	static Eigen::MatrixXd ApplyInvTransM(const tSpTrans& X, const Eigen::MatrixXd& sm);
+	static Eigen::MatrixXd ApplyInvTransF(const tSpTrans& X, const Eigen::MatrixXd& sm);
+	static tSpTrans CompTrans(const tSpTrans& X0, const tSpTrans& X1);
+	
+	// extract a tSpTrans from a matrix presentating a stack of transforms
+	static tSpTrans GetTrans(const Eigen::MatrixXd& trans_arr, int j);
+};

Engine/lib/bullet/examples/SharedMemory/plugins/testPlugin/premake4.lua

@@ -0,0 +1,42 @@
+		
+
+project ("pybullet_testplugin")
+		language "C++"
+		kind "SharedLib"
+		
+		includedirs {".","../../../../src", "../../../../examples",
+		"../../../ThirdPartyLibs"}
+		defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
+	hasCL = findOpenCL("clew")
+
+	links{"BulletFileLoader", "Bullet3Common", "LinearMath"}
+
+
+	if os.is("MacOSX") then
+--		targetextension {"so"}
+		links{"Cocoa.framework","Python"}
+	end
+
+
+		files {
+			"testplugin.cpp",
+			"../../PhysicsClient.cpp",
+			"../../PhysicsClient.h",
+			"../../PhysicsClientSharedMemory.cpp",
+			"../../PhysicsClientSharedMemory.h",
+			"../../PhysicsClientSharedMemory_C_API.cpp",
+			"../../PhysicsClientSharedMemory_C_API.h",
+			"../../PhysicsClientC_API.cpp",
+			"../../PhysicsClientC_API.h",
+			"../../Win32SharedMemory.cpp",
+			"../../Win32SharedMemory.h",
+			"../../PosixSharedMemory.cpp",
+			"../../PosixSharedMemory.h",
+			"../../../Utils/b3Clock.cpp",
+			"../../../Utils/b3Clock.h",
+			"../../../Utils/b3ResourcePath.cpp",
+			"../../../Utils/b3ResourcePath.h",
+				}
+	
+	
+	

Engine/lib/bullet/examples/SharedMemory/plugins/testPlugin/testplugin.cpp

@@ -0,0 +1,101 @@
+
+//test plugin, can load a URDF file, example usage on a Windows machine:
+
+/*
+import pybullet as p
+p.connect(p.GUI)
+pluginUid = p.loadPlugin("E:/develop/bullet3/bin/pybullet_testplugin_vs2010_x64_debug.dll")
+commandUid = 0
+argument = "plane.urdf"
+p.executePluginCommand(pluginUid,commandUid,argument)
+p.unloadPlugin(pluginUid)
+*/
+
+#include "testplugin.h"
+#include "../../SharedMemoryPublic.h"
+#include "../b3PluginContext.h"
+#include <stdio.h>
+
+struct MyClass
+{
+	int m_testData;
+
+	MyClass()
+		: m_testData(42)
+	{
+	}
+	virtual ~MyClass()
+	{
+	}
+};
+
+B3_SHARED_API int initPlugin_testPlugin(struct b3PluginContext* context)
+{
+	MyClass* obj = new MyClass();
+	context->m_userPointer = obj;
+
+	printf("hi!\n");
+	return SHARED_MEMORY_MAGIC_NUMBER;
+}
+
+B3_SHARED_API int preTickPluginCallback_testPlugin(struct b3PluginContext* context)
+{
+	return 0;
+}
+
+B3_SHARED_API int postTickPluginCallback_testPlugin(struct b3PluginContext* context)
+{
+	MyClass* obj = (MyClass*)context->m_userPointer;
+	obj->m_testData++;
+	return 0;
+}
+
+B3_SHARED_API int executePluginCommand_testPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
+{
+	printf("text argument:%s\n", arguments->m_text);
+	printf("int args: [");
+	for (int i = 0; i < arguments->m_numInts; i++)
+	{
+		printf("%d", arguments->m_ints[i]);
+		if ((i + 1) < arguments->m_numInts)
+		{
+			printf(",");
+		}
+	}
+	printf("]\nfloat args: [");
+	for (int i = 0; i < arguments->m_numFloats; i++)
+	{
+		printf("%f", arguments->m_floats[i]);
+		if ((i + 1) < arguments->m_numFloats)
+		{
+			printf(",");
+		}
+	}
+	printf("]\n");
+
+	MyClass* obj = (MyClass*)context->m_userPointer;
+
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType = -1;
+	int bodyUniqueId = -1;
+
+	b3SharedMemoryCommandHandle command =
+		b3LoadUrdfCommandInit(context->m_physClient, arguments->m_text);
+
+	statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, command);
+	statusType = b3GetStatusType(statusHandle);
+	if (statusType == CMD_URDF_LOADING_COMPLETED)
+	{
+		bodyUniqueId = b3GetStatusBodyIndex(statusHandle);
+	}
+	return bodyUniqueId;
+}
+
+B3_SHARED_API void exitPlugin_testPlugin(struct b3PluginContext* context)
+{
+	MyClass* obj = (MyClass*)context->m_userPointer;
+	delete obj;
+	context->m_userPointer = 0;
+
+	printf("bye!\n");
+}

Engine/lib/bullet/examples/SharedMemory/plugins/testPlugin/testplugin.h

@@ -0,0 +1,24 @@
+#ifndef TEST_PLUGIN_H
+#define TEST_PLUGIN_H
+
+#include "../b3PluginAPI.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+	//initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load
+	B3_SHARED_API int initPlugin_testPlugin(struct b3PluginContext* context);
+	B3_SHARED_API void exitPlugin_testPlugin(struct b3PluginContext* context);
+	B3_SHARED_API int executePluginCommand_testPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
+
+	//all the APIs below are optional
+	B3_SHARED_API int preTickPluginCallback_testPlugin(struct b3PluginContext* context);
+	B3_SHARED_API int postTickPluginCallback_testPlugin(struct b3PluginContext* context);
+
+#ifdef __cplusplus
+};
+#endif
+
+#endif  //#define TEST_PLUGIN_H

Engine/lib/bullet/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h

@@ -0,0 +1,63 @@
+#ifndef TINY_RENDERER_VISUAL_SHAPE_CONVERTER_H
+#define TINY_RENDERER_VISUAL_SHAPE_CONVERTER_H
+
+#include "../../../Importers/ImportURDFDemo/UrdfRenderingInterface.h"
+
+struct TinyRendererVisualShapeConverter : public UrdfRenderingInterface
+{
+	struct TinyRendererVisualShapeConverterInternalData* m_data;
+
+	TinyRendererVisualShapeConverter();
+
+	virtual ~TinyRendererVisualShapeConverter();
+
+	virtual int convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int orgGraphicsUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO);
+
+	//returns a shapeUniqueId
+	virtual int registerShapeAndInstance(const b3VisualShapeData& visualShape, const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType, int textureId, int orgGraphicsUniqueId, int bodyUniqueId, int linkIndex);
+
+	virtual void updateShape(int shapeUniqueId, const btVector3* vertices, int numVertices, const btVector3* normals, int numNormals);
+
+	virtual void removeVisualShape(int shapeUniqueId);
+
+	virtual int getNumVisualShapes(int bodyUniqueId);
+
+	virtual int getVisualShapesData(int bodyUniqueId, int shapeIndex, struct b3VisualShapeData* shapeData);
+
+	virtual void changeRGBAColor(int bodyUniqueId, int linkIndex, int shapeIndex, const double rgbaColor[4]);
+
+	virtual void changeInstanceFlags(int bodyUniqueId, int linkIndex, int shapeIndex, int flags);
+
+	virtual void changeShapeTexture(int bodyUniqueId, int linkIndex, int shapeIndex, int textureUniqueId);
+
+	virtual void setUpAxis(int axis);
+
+	virtual void resetCamera(float camDist, float yaw, float pitch, float camPosX, float camPosY, float camPosZ);
+
+	virtual void clearBuffers(struct TGAColor& clearColor);
+
+	virtual void resetAll();
+
+	virtual void getWidthAndHeight(int& width, int& height);
+	virtual void setWidthAndHeight(int width, int height);
+	virtual void setLightDirection(float x, float y, float z);
+	virtual void setLightColor(float x, float y, float z);
+	virtual void setLightDistance(float dist);
+	virtual void setLightAmbientCoeff(float ambientCoeff);
+	virtual void setLightDiffuseCoeff(float diffuseCoeff);
+	virtual void setLightSpecularCoeff(float specularCoeff);
+	virtual void setShadow(bool hasShadow);
+	virtual void setFlags(int flags);
+
+	virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);
+
+	virtual void render();
+	virtual void render(const float viewMat[16], const float projMat[16]);
+
+	virtual int loadTextureFile(const char* filename, struct CommonFileIOInterface* fileIO);
+	virtual int registerTexture(unsigned char* texels, int width, int height);
+
+	virtual void syncTransform(int shapeUniqueId, const class btTransform& worldTransform, const class btVector3& localScaling);
+};
+
+#endif  //TINY_RENDERER_VISUAL_SHAPE_CONVERTER_H

Engine/lib/bullet/examples/SharedMemory/plugins/tinyRendererPlugin/premake4.lua

@@ -0,0 +1,53 @@
+		
+
+project ("pybullet_tinyRendererPlugin")
+		language "C++"
+		kind "SharedLib"
+		
+		includedirs {".","../../../../src", "../../../../examples",
+		"../../../ThirdPartyLibs"}
+		defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
+	hasCL = findOpenCL("clew")
+
+	links{"BulletCollision", "Bullet3Common", "LinearMath"}
+
+	if os.is("MacOSX") then
+--		targetextension {"so"}
+		links{"Cocoa.framework","Python"}
+	end
+
+
+		files {
+			"tinyRendererPlugin.cpp",
+			"tinyRendererPlugin.h",
+			"TinyRendererVisualShapeConverter.cpp",
+			"TinyRendererVisualShapeConverter.h",
+			"../../../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
+			"../../../Importers/ImportColladaDemo/LoadMeshFromCollada.h",
+			"../../../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+			"../../../Importers/ImportMeshUtility/b3ImportMeshUtility.h",
+			"../../../Importers/ImportObjDemo/LoadMeshFromObj.cpp",
+			"../../../Importers/ImportObjDemo/LoadMeshFromObj.h",
+			"../../../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
+			"../../../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h",
+			"../../../TinyRenderer/geometry.cpp",
+			"../../../TinyRenderer/model.cpp",
+			"../../../TinyRenderer/our_gl.cpp",
+			"../../../TinyRenderer/tgaimage.cpp",
+			"../../../TinyRenderer/TinyRenderer.cpp",
+			"../../../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",
+			"../../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h",
+			"../../../ThirdPartyLibs/stb_image/stb_image.cpp",
+			"../../../ThirdPartyLibs/stb_image/stb_image.h",
+			"../../../ThirdPartyLibs/tinyxml2/tinyxml2.cpp",
+			"../../../ThirdPartyLibs/tinyxml2/tinyxml2.h",
+			"../../../OpenGLWindow/SimpleCamera.cpp",
+			"../../../OpenGLWindow/SimpleCamera.h",
+			"../../../Utils/b3Clock.cpp",
+			"../../../Utils/b3Clock.h",
+			"../../../Utils/b3ResourcePath.cpp",
+			"../../../Utils/b3ResourcePath.h",
+			}
+	
+	
+	

Engine/lib/bullet/examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp

@@ -0,0 +1,77 @@
+
+//tinyRenderer plugin
+
+/*
+import pybullet as p
+p.connect(p.GUI)
+plugin = p.loadPlugin("e:/develop/bullet3/bin/pybullet_tinyRendererPlugin_vs2010_x64_debug.dll","_tinyRendererPlugin")
+print("plugin=",plugin)
+p.loadURDF("r2d2.urdf")
+while (1):
+	p.getCameraImage(320,200)
+*/
+
+#include "tinyRendererPlugin.h"
+#include "TinyRendererVisualShapeConverter.h"
+
+#include "../../SharedMemoryPublic.h"
+#include "../b3PluginContext.h"
+#include <stdio.h>
+
+struct MyRendererPluginClass
+{
+	TinyRendererVisualShapeConverter m_renderer;
+	b3UserDataValue* m_returnData;
+	MyRendererPluginClass()
+		:m_returnData(0)
+	{
+	}
+	virtual ~MyRendererPluginClass()
+	{
+		if (m_returnData)
+		{
+			delete[] m_returnData->m_data1;
+		}
+		delete m_returnData;
+	}
+};
+
+B3_SHARED_API int initPlugin_tinyRendererPlugin(struct b3PluginContext* context)
+{
+	MyRendererPluginClass* obj = new MyRendererPluginClass();
+	context->m_userPointer = obj;
+	return SHARED_MEMORY_MAGIC_NUMBER;
+}
+
+B3_SHARED_API int executePluginCommand_tinyRendererPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
+{
+	MyRendererPluginClass* obj = (MyRendererPluginClass*)context->m_userPointer;
+	if (obj->m_returnData==0)
+	{
+		obj->m_returnData = new b3UserDataValue();
+		obj->m_returnData->m_type = 1;
+		obj->m_returnData->m_length = 123;
+		char* data = new char[obj->m_returnData->m_length];
+		for (int i = 0; i < obj->m_returnData->m_length; i++)
+		{
+			data[i] = i;
+		}
+		obj->m_returnData->m_data1 = data;
+	}
+	context->m_returnData = obj->m_returnData;
+	return -1;
+}
+
+B3_SHARED_API void exitPlugin_tinyRendererPlugin(struct b3PluginContext* context)
+{
+	MyRendererPluginClass* obj = (MyRendererPluginClass*)context->m_userPointer;
+	delete obj;
+	context->m_userPointer = 0;
+}
+
+//all the APIs below are optional
+B3_SHARED_API struct UrdfRenderingInterface* getRenderInterface_tinyRendererPlugin(struct b3PluginContext* context)
+{
+	MyRendererPluginClass* obj = (MyRendererPluginClass*)context->m_userPointer;
+	return &obj->m_renderer;
+}

Engine/lib/bullet/examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.h

@@ -0,0 +1,23 @@
+#ifndef TINY_RENDERER_PLUGIN_H
+#define TINY_RENDERER_PLUGIN_H
+
+#include "../b3PluginAPI.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+	//initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load
+	B3_SHARED_API int initPlugin_tinyRendererPlugin(struct b3PluginContext* context);
+	B3_SHARED_API void exitPlugin_tinyRendererPlugin(struct b3PluginContext* context);
+	B3_SHARED_API int executePluginCommand_tinyRendererPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
+
+	//all the APIs below are optional
+	B3_SHARED_API struct UrdfRenderingInterface* getRenderInterface_tinyRendererPlugin(struct b3PluginContext* context);
+
+#ifdef __cplusplus
+};
+#endif
+
+#endif  //#define TEST_PLUGIN_H

Engine/lib/bullet/examples/SharedMemory/plugins/vrSyncPlugin/premake4.lua

@@ -0,0 +1,42 @@
+		
+
+project ("pybullet_vrSyncPlugin")
+		language "C++"
+		kind "SharedLib"
+		
+		includedirs {".","../../../../src", "../../../../examples",
+		"../../../ThirdPartyLibs"}
+		defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
+	hasCL = findOpenCL("clew")
+
+	links{"BulletFileLoader", "Bullet3Common", "LinearMath"}
+
+
+	if os.is("MacOSX") then
+--		targetextension {"so"}
+		links{"Cocoa.framework","Python"}
+	end
+
+
+		files {
+			"vrSyncPlugin.cpp",
+			"../../PhysicsClient.cpp",
+			"../../PhysicsClient.h",
+			"../../PhysicsClientSharedMemory.cpp",
+			"../../PhysicsClientSharedMemory.h",
+			"../../PhysicsClientSharedMemory_C_API.cpp",
+			"../../PhysicsClientSharedMemory_C_API.h",
+			"../../PhysicsClientC_API.cpp",
+			"../../PhysicsClientC_API.h",
+			"../../Win32SharedMemory.cpp",
+			"../../Win32SharedMemory.h",
+			"../../PosixSharedMemory.cpp",
+			"../../PosixSharedMemory.h",
+			"../../../Utils/b3Clock.cpp",
+			"../../../Utils/b3Clock.h",
+			"../../../Utils/b3ResourcePath.cpp",
+			"../../../Utils/b3ResourcePath.h",
+				}
+	
+	
+	

Engine/lib/bullet/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp

@@ -0,0 +1,194 @@
+
+//vrSyncPlugin plugin, will query the vr controller events
+//and set change the user constraint to match the pose
+
+//in Python you can load and configure the plugin like this:
+//plugin = p.loadPlugin("e:/develop/bullet3/bin/pybullet_vrSyncPlugin_vs2010_x64_release.dll")
+//could also be plugin = p.loadPlugin("vrSyncPlugin.so") on Mac/Linux
+//controllerId = 3
+
+#include "vrSyncPlugin.h"
+#include "../../SharedMemoryPublic.h"
+#include "../b3PluginContext.h"
+#include <stdio.h>
+
+struct MyClass
+{
+	int m_testData;
+
+	int m_controllerId;
+	int m_constraintId;
+	int m_constraintId2;
+	int m_gripperId;
+	float m_maxForce;
+	float m_maxForce2;
+	MyClass()
+		: m_testData(42),
+		  m_controllerId(-1),
+		  m_constraintId(-1),
+		  m_constraintId2(-1),
+		  m_gripperId(-1),
+		  m_maxForce(0),
+		  m_maxForce2(0)
+	{
+	}
+	virtual ~MyClass()
+	{
+	}
+};
+
+B3_SHARED_API int initPlugin_vrSyncPlugin(struct b3PluginContext* context)
+{
+	MyClass* obj = new MyClass();
+	context->m_userPointer = obj;
+	
+	return SHARED_MEMORY_MAGIC_NUMBER;
+}
+
+B3_SHARED_API int preTickPluginCallback_vrSyncPlugin(struct b3PluginContext* context)
+{
+	MyClass* obj = (MyClass*)context->m_userPointer;
+	if (obj && obj->m_controllerId >= 0)
+	{
+		{
+			int i = 0;
+			{
+				for (int n = 0; n < context->m_numVRControllerEvents; n++)
+				{
+					const b3VRControllerEvent& event = context->m_vrControllerEvents[n];
+					if (event.m_controllerId == obj->m_controllerId)
+					{
+						if (obj->m_constraintId >= 0)
+						{
+							struct b3UserConstraint constraintInfo;
+							if (b3GetUserConstraintInfo(context->m_physClient, obj->m_constraintId, &constraintInfo))
+							{
+								//this is basically equivalent to doing this in Python/pybullet:
+								//p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=...)
+								b3SharedMemoryCommandHandle commandHandle;
+								int userConstraintUniqueId = obj->m_constraintId;
+								commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, userConstraintUniqueId);
+								double pos[4] = {event.m_pos[0], event.m_pos[1], event.m_pos[2], 1};
+								b3InitChangeUserConstraintSetPivotInB(commandHandle, pos);
+								double orn[4] = {event.m_orn[0], event.m_orn[1], event.m_orn[2], event.m_orn[3]};
+								b3InitChangeUserConstraintSetFrameInB(commandHandle, orn);
+								b3InitChangeUserConstraintSetMaxForce(commandHandle, obj->m_maxForce);
+								b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
+							}
+						}
+						// apply the analogue button to close the constraint, using a gear constraint with position target
+						if (obj->m_constraintId2 >= 0)
+						{
+							struct b3UserConstraint constraintInfo;
+							if (b3GetUserConstraintInfo(context->m_physClient, obj->m_constraintId2, &constraintInfo))
+							{
+								//this block is similar to
+								//p.changeConstraint(c,gearRatio=1, erp=..., relativePositionTarget=relPosTarget, maxForce=...)
+								//printf("obj->m_constraintId2=%d\n", obj->m_constraintId2);
+								b3SharedMemoryCommandHandle commandHandle;
+								commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, obj->m_constraintId2);
+
+								//0 -> open, 1 = closed
+								double openPos = 1.;
+								double relPosTarget = openPos - (event.m_analogAxis * openPos);
+								b3InitChangeUserConstraintSetRelativePositionTarget(commandHandle, relPosTarget);
+								b3InitChangeUserConstraintSetERP(commandHandle, 1);
+								b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
+							}
+						}
+						//printf("event.m_analogAxis=%f\n", event.m_analogAxis);
+
+						// use the pr2_gripper motors to keep the gripper centered/symmetric around the center axis
+						if (obj->m_gripperId >= 0)
+						{
+							//this block is similar to
+							//b = p.getJointState(pr2_gripper,2)[0]
+							//print("b = " + str(b))
+							//p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3)
+
+							//printf("obj->m_gripperId=%d\n", obj->m_gripperId);
+							{
+								b3SharedMemoryCommandHandle cmd_handle =
+									b3RequestActualStateCommandInit(context->m_physClient, obj->m_gripperId);
+								b3SharedMemoryStatusHandle status_handle =
+									b3SubmitClientCommandAndWaitStatus(context->m_physClient, cmd_handle);
+
+								int status_type = b3GetStatusType(status_handle);
+								if (status_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
+								{
+									//printf("status_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED\n");
+
+									b3JointSensorState sensorState;
+									if (b3GetJointState(context->m_physClient, status_handle, 2, &sensorState))
+									{
+										b3SharedMemoryCommandHandle commandHandle;
+										double targetPosition = sensorState.m_jointPosition;
+										//printf("targetPosition =%f\n", targetPosition);
+										if (1)
+										{
+											b3JointInfo info;
+											b3GetJointInfo(context->m_physClient, obj->m_gripperId, 0, &info);
+											commandHandle = b3JointControlCommandInit2(context->m_physClient, obj->m_gripperId, CONTROL_MODE_POSITION_VELOCITY_PD);
+											double kp = .1;
+											double targetVelocity = 0;
+											double kd = .6;
+											b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition);
+											b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
+											b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
+											b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
+											b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, obj->m_maxForce2);
+											b3SubmitClientCommandAndWaitStatus(context->m_physClient, cmd_handle);
+										}
+									}
+									else
+									{
+										//printf("???\n");
+									}
+								}
+								else
+								{
+									//printf("no\n");
+								}
+							}
+						}
+					}
+				}
+			}
+		}
+	}
+
+	return 0;
+}
+
+B3_SHARED_API int executePluginCommand_vrSyncPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
+{
+	MyClass* obj = (MyClass*)context->m_userPointer;
+	if (arguments->m_numInts >= 4 && arguments->m_numFloats >= 2)
+	{
+		obj->m_constraintId = arguments->m_ints[1];
+		obj->m_constraintId2 = arguments->m_ints[2];
+		obj->m_gripperId = arguments->m_ints[3];
+		printf("obj->m_constraintId=%d\n", obj->m_constraintId);
+		obj->m_maxForce = arguments->m_floats[0];
+		obj->m_maxForce2 = arguments->m_floats[1];
+		printf("obj->m_maxForce = %f\n", obj->m_maxForce);
+		obj->m_controllerId = arguments->m_ints[0];
+		printf("obj->m_controllerId=%d\n", obj->m_controllerId);
+
+		b3SharedMemoryCommandHandle command = b3InitSyncBodyInfoCommand(context->m_physClient);
+		b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, command);
+		int statusType = b3GetStatusType(statusHandle);
+
+		if (statusType != CMD_SYNC_BODY_INFO_COMPLETED)
+		{
+		}
+	}
+	return 0;
+}
+
+B3_SHARED_API void exitPlugin_vrSyncPlugin(struct b3PluginContext* context)
+{
+	MyClass* obj = (MyClass*)context->m_userPointer;
+	delete obj;
+	context->m_userPointer = 0;
+}

Engine/lib/bullet/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.h

@@ -0,0 +1,23 @@
+#ifndef TEST_PLUGIN_H
+#define TEST_PLUGIN_H
+
+#include "../b3PluginAPI.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+	//initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load
+	B3_SHARED_API int initPlugin_vrSyncPlugin(struct b3PluginContext* context);
+	B3_SHARED_API void exitPlugin_vrSyncPlugin(struct b3PluginContext* context);
+	B3_SHARED_API int executePluginCommand_vrSyncPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
+
+	//optional APIs
+	B3_SHARED_API int preTickPluginCallback_vrSyncPlugin(struct b3PluginContext* context);
+
+#ifdef __cplusplus
+};
+#endif
+
+#endif  //#define TEST_PLUGIN_H