* Adjustment: Update Bullet version to 3.24.

Engine/lib/bullet/examples/Importers/ImportBsp/BspConverter.cpp

@@ -0,0 +1,188 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BspConverter.h"
+#include "BspLoader.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btGeometryUtil.h"
+#include <stdio.h>
+#include <string.h>
+
+void BspConverter::convertBsp(BspLoader& bspLoader, float scaling)
+{
+	{
+		float playstartf[3] = {0, 0, 100};
+
+		if (bspLoader.findVectorByName(&playstartf[0], "info_player_start"))
+		{
+			printf("found playerstart\n");
+		}
+		else
+		{
+			if (bspLoader.findVectorByName(&playstartf[0], "info_player_deathmatch"))
+			{
+				printf("found deatchmatch start\n");
+			}
+		}
+
+		btVector3 playerStart(playstartf[0], playstartf[1], playstartf[2]);
+
+		playerStart[2] += 20.f;  //start a bit higher
+
+		playerStart *= scaling;
+
+		//progressBegin("Loading bsp");
+
+		for (int i = 0; i < bspLoader.m_numleafs; i++)
+		{
+			printf("Reading bspLeaf %i from total %i (%f procent)\n", i, bspLoader.m_numleafs, (100.f * (float)i / float(bspLoader.m_numleafs)));
+
+			bool isValidBrush = false;
+
+			BSPLeaf& leaf = bspLoader.m_dleafs[i];
+
+			for (int b = 0; b < leaf.numLeafBrushes; b++)
+			{
+				btAlignedObjectArray<btVector3> planeEquations;
+
+				int brushid = bspLoader.m_dleafbrushes[leaf.firstLeafBrush + b];
+
+				BSPBrush& brush = bspLoader.m_dbrushes[brushid];
+				if (brush.shaderNum != -1)
+				{
+					if (bspLoader.m_dshaders[brush.shaderNum].contentFlags & BSPCONTENTS_SOLID)
+					{
+						brush.shaderNum = -1;
+
+						for (int p = 0; p < brush.numSides; p++)
+						{
+							int sideid = brush.firstSide + p;
+							BSPBrushSide& brushside = bspLoader.m_dbrushsides[sideid];
+							int planeid = brushside.planeNum;
+							BSPPlane& plane = bspLoader.m_dplanes[planeid];
+							btVector3 planeEq;
+							planeEq.setValue(
+								plane.normal[0],
+								plane.normal[1],
+								plane.normal[2]);
+							planeEq[3] = scaling * -plane.dist;
+
+							planeEquations.push_back(planeEq);
+							isValidBrush = true;
+						}
+						if (isValidBrush)
+						{
+							btAlignedObjectArray<btVector3> vertices;
+							btGeometryUtil::getVerticesFromPlaneEquations(planeEquations, vertices);
+
+							bool isEntity = false;
+							btVector3 entityTarget(0.f, 0.f, 0.f);
+							addConvexVerticesCollider(vertices, isEntity, entityTarget);
+						}
+					}
+				}
+			}
+		}
+
+#define USE_ENTITIES
+#ifdef USE_ENTITIES
+
+		{
+			int i;
+			for (i = 0; i < bspLoader.m_num_entities; i++)
+			{
+				const BSPEntity& entity = bspLoader.m_entities[i];
+				const char* cl = bspLoader.getValueForKey(&entity, "classname");
+				if (!strcmp(cl, "trigger_push"))
+				{
+					btVector3 targetLocation(0.f, 0.f, 0.f);
+
+					cl = bspLoader.getValueForKey(&entity, "target");
+					if (strcmp(cl, ""))
+					{
+						//its not empty so ...
+
+						/*
+							//lookup the target position for the jumppad:
+							const BSPEntity* targetentity = bspLoader.getEntityByValue( "targetname" , cl );
+							if (targetentity)
+							{
+								if (bspLoader.getVectorForKey( targetentity , "origin",&targetLocation[0]))
+								{
+																	
+								}
+							}
+							*/
+
+						cl = bspLoader.getValueForKey(&entity, "model");
+						if (strcmp(cl, ""))
+						{
+							// add the model as a brush
+							if (cl[0] == '*')
+							{
+								int modelnr = atoi(&cl[1]);
+								if ((modelnr >= 0) && (modelnr < bspLoader.m_nummodels))
+								{
+									const BSPModel& model = bspLoader.m_dmodels[modelnr];
+									for (int n = 0; n < model.numBrushes; n++)
+									{
+										btAlignedObjectArray<btVector3> planeEquations;
+										bool isValidBrush = false;
+
+										//convert brush
+										const BSPBrush& brush = bspLoader.m_dbrushes[model.firstBrush + n];
+										{
+											for (int p = 0; p < brush.numSides; p++)
+											{
+												int sideid = brush.firstSide + p;
+												BSPBrushSide& brushside = bspLoader.m_dbrushsides[sideid];
+												int planeid = brushside.planeNum;
+												BSPPlane& plane = bspLoader.m_dplanes[planeid];
+												btVector3 planeEq;
+												planeEq.setValue(
+													plane.normal[0],
+													plane.normal[1],
+													plane.normal[2]);
+												planeEq[3] = scaling * -plane.dist;
+												planeEquations.push_back(planeEq);
+												isValidBrush = true;
+											}
+											if (isValidBrush)
+											{
+												btAlignedObjectArray<btVector3> vertices;
+												btGeometryUtil::getVerticesFromPlaneEquations(planeEquations, vertices);
+
+												bool isEntity = true;
+												addConvexVerticesCollider(vertices, isEntity, targetLocation);
+											}
+										}
+									}
+								}
+							}
+							else
+							{
+								printf("unsupported trigger_push model, md3 ?\n");
+							}
+						}
+					}
+				}
+			}
+		}
+
+#endif  //USE_ENTITIES
+
+		//progressEnd();
+	}
+}

Engine/lib/bullet/examples/Importers/ImportBsp/BspConverter.h

@@ -0,0 +1,36 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BSP_CONVERTER_H
+#define BSP_CONVERTER_H
+
+class BspLoader;
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+///BspConverter turns a loaded bsp level into convex parts (vertices)
+class BspConverter
+{
+public:
+	void convertBsp(BspLoader& bspLoader, float scaling);
+	virtual ~BspConverter()
+	{
+	}
+
+	///this callback is called for each brush that succesfully converted into vertices
+	virtual void addConvexVerticesCollider(btAlignedObjectArray<btVector3>& vertices, bool isEntity, const btVector3& entityTargetLocation) = 0;
+};
+
+#endif  //BSP_CONVERTER_H

Engine/lib/bullet/examples/Importers/ImportBsp/BspLoader.cpp

@@ -0,0 +1,719 @@
+/*
+===========================================================================
+Copyright (C) 1999-2005 Id Software, Inc.
+
+This file is part of Quake III Arena source code.
+
+Quake III Arena source code is free software; you can redistribute it
+and/or modify it under the terms of the GNU bteral Public License as
+published by the Free Software Foundation; either version 2 of the License,
+or (at your option) any later version.
+
+Quake III Arena source code is distributed in the hope that it will be
+useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU bteral Public License for more details.
+
+You should have received a copy of the GNU bteral Public License
+along with Foobar; if not, write to the Free Software
+Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+===========================================================================
+*/
+
+#include "BspLoader.h"
+#include <stdio.h>
+#include <string.h>
+
+typedef struct
+{
+	char filename[1024];
+	char *buffer, *script_p, *end_p;
+	int line;
+} BSPScript;
+
+#define MAX_INCLUDES 8
+BSPScript scriptstack[MAX_INCLUDES];
+BSPScript *script;
+int scriptline;
+
+char token[BSPMAXTOKEN];
+bool endofscript;
+bool tokenready;  // only true if UnGetToken was just called
+
+//
+//loadBSPFile
+//
+
+int extrasize = 100;
+
+BspLoader::BspLoader()
+	: m_num_entities(0)
+{
+	m_Endianness = getMachineEndianness();
+	if (m_Endianness == BSP_BIG_ENDIAN)
+	{
+		printf("Machine is BIG_ENDIAN\n");
+	}
+	else
+	{
+		printf("Machine is Little Endian\n");
+	}
+}
+
+bool BspLoader::loadBSPFile(void *memoryBuffer)
+{
+	BSPHeader *header = (BSPHeader *)memoryBuffer;
+
+	// load the file header
+	if (header)
+	{
+		// swap the header
+		swapBlock((int *)header, sizeof(*header));
+
+		int length = (header->lumps[BSPLUMP_SHADERS].filelen) / sizeof(BSPShader);
+		m_dshaders.resize(length + extrasize);
+		m_numShaders = copyLump(header, BSPLUMP_SHADERS, &m_dshaders[0], sizeof(BSPShader));
+
+		length = (header->lumps[LUMP_MODELS].filelen) / sizeof(BSPModel);
+		m_dmodels.resize(length + extrasize);
+		m_nummodels = copyLump(header, LUMP_MODELS, &m_dmodels[0], sizeof(BSPModel));
+
+		length = (header->lumps[BSPLUMP_PLANES].filelen) / sizeof(BSPPlane);
+		m_dplanes.resize(length + extrasize);
+		m_numplanes = copyLump(header, BSPLUMP_PLANES, &m_dplanes[0], sizeof(BSPPlane));
+
+		length = (header->lumps[BSPLUMP_LEAFS].filelen) / sizeof(BSPLeaf);
+		m_dleafs.resize(length + extrasize);
+		m_numleafs = copyLump(header, BSPLUMP_LEAFS, &m_dleafs[0], sizeof(BSPLeaf));
+
+		length = (header->lumps[BSPLUMP_NODES].filelen) / sizeof(BSPNode);
+		m_dnodes.resize(length + extrasize);
+		m_numnodes = copyLump(header, BSPLUMP_NODES, &m_dnodes[0], sizeof(BSPNode));
+
+		length = (header->lumps[BSPLUMP_LEAFSURFACES].filelen) / sizeof(m_dleafsurfaces[0]);
+		m_dleafsurfaces.resize(length + extrasize);
+		m_numleafsurfaces = copyLump(header, BSPLUMP_LEAFSURFACES, &m_dleafsurfaces[0], sizeof(m_dleafsurfaces[0]));
+
+		length = (header->lumps[BSPLUMP_LEAFBRUSHES].filelen) / sizeof(m_dleafbrushes[0]);
+		m_dleafbrushes.resize(length + extrasize);
+		m_numleafbrushes = copyLump(header, BSPLUMP_LEAFBRUSHES, &m_dleafbrushes[0], sizeof(m_dleafbrushes[0]));
+
+		length = (header->lumps[LUMP_BRUSHES].filelen) / sizeof(BSPBrush);
+		m_dbrushes.resize(length + extrasize);
+		m_numbrushes = copyLump(header, LUMP_BRUSHES, &m_dbrushes[0], sizeof(BSPBrush));
+
+		length = (header->lumps[LUMP_BRUSHSIDES].filelen) / sizeof(BSPBrushSide);
+		m_dbrushsides.resize(length + extrasize);
+		m_numbrushsides = copyLump(header, LUMP_BRUSHSIDES, &m_dbrushsides[0], sizeof(BSPBrushSide));
+
+		length = (header->lumps[LUMP_SURFACES].filelen) / sizeof(BSPSurface);
+		m_drawSurfaces.resize(length + extrasize);
+		m_numDrawSurfaces = copyLump(header, LUMP_SURFACES, &m_drawSurfaces[0], sizeof(BSPSurface));
+
+		length = (header->lumps[LUMP_DRAWINDEXES].filelen) / sizeof(m_drawIndexes[0]);
+		m_drawIndexes.resize(length + extrasize);
+		m_numDrawIndexes = copyLump(header, LUMP_DRAWINDEXES, &m_drawIndexes[0], sizeof(m_drawIndexes[0]));
+
+		length = (header->lumps[LUMP_VISIBILITY].filelen) / 1;
+		m_visBytes.resize(length + extrasize);
+		m_numVisBytes = copyLump(header, LUMP_VISIBILITY, &m_visBytes[0], 1);
+
+		length = (header->lumps[LUMP_LIGHTMAPS].filelen) / 1;
+		m_lightBytes.resize(length + extrasize);
+		m_numLightBytes = copyLump(header, LUMP_LIGHTMAPS, &m_lightBytes[0], 1);
+
+		length = (header->lumps[BSPLUMP_ENTITIES].filelen) / 1;
+		m_dentdata.resize(length + extrasize);
+		m_entdatasize = copyLump(header, BSPLUMP_ENTITIES, &m_dentdata[0], 1);
+
+		length = (header->lumps[LUMP_LIGHTGRID].filelen) / 1;
+		m_gridData.resize(length + extrasize);
+		m_numGridPoints = copyLump(header, LUMP_LIGHTGRID, &m_gridData[0], 8);
+
+		// swap everything
+		swapBSPFile();
+
+		return true;
+	}
+	return false;
+}
+
+const char *BspLoader::getValueForKey(const BSPEntity *ent, const char *key) const
+{
+	const BSPKeyValuePair *ep;
+
+	for (ep = ent->epairs; ep; ep = ep->next)
+	{
+		if (!strcmp(ep->key, key))
+		{
+			return ep->value;
+		}
+	}
+	return "";
+}
+
+float BspLoader::getFloatForKey(const BSPEntity *ent, const char *key)
+{
+	const char *k;
+
+	k = getValueForKey(ent, key);
+	return float(atof(k));
+}
+
+bool BspLoader::getVectorForKey(const BSPEntity *ent, const char *key, BSPVector3 vec)
+{
+	const char *k;
+	k = getValueForKey(ent, key);
+	if (strcmp(k, ""))
+	{
+		sscanf(k, "%f %f %f", &vec[0], &vec[1], &vec[2]);
+		return true;
+	}
+	return false;
+}
+
+/*
+==============
+parseFromMemory
+==============
+*/
+void BspLoader::parseFromMemory(char *buffer, int size)
+{
+	script = scriptstack;
+	script++;
+	if (script == &scriptstack[MAX_INCLUDES])
+	{
+		//printf("script file exceeded MAX_INCLUDES");
+	}
+	strcpy(script->filename, "memory buffer");
+
+	script->buffer = buffer;
+	script->line = 1;
+	script->script_p = script->buffer;
+	script->end_p = script->buffer + size;
+
+	endofscript = false;
+	tokenready = false;
+}
+
+bool BspLoader::isEndOfScript(bool crossline)
+{
+	if (!crossline)
+		//printf("Line %i is incomplete\n",scriptline);
+
+		if (!strcmp(script->filename, "memory buffer"))
+		{
+			endofscript = true;
+			return false;
+		}
+
+	//free (script->buffer);
+	if (script == scriptstack + 1)
+	{
+		endofscript = true;
+		return false;
+	}
+	script--;
+	scriptline = script->line;
+	//printf ("returning to %s\n", script->filename);
+	return getToken(crossline);
+}
+
+/*
+
+==============
+getToken
+==============
+*/
+bool BspLoader::getToken(bool crossline)
+{
+	char *token_p;
+
+	if (tokenready)  // is a token allready waiting?
+	{
+		tokenready = false;
+		return true;
+	}
+
+	if (script->script_p >= script->end_p)
+		return isEndOfScript(crossline);
+
+//
+// skip space
+//
+skipspace:
+	while (*script->script_p <= 32)
+	{
+		if (script->script_p >= script->end_p)
+			return isEndOfScript(crossline);
+		if (*script->script_p++ == '\n')
+		{
+			if (!crossline)
+			{
+				//printf("Line %i is incomplete\n",scriptline);
+			}
+			scriptline = script->line++;
+		}
+	}
+
+	if (script->script_p >= script->end_p)
+		return isEndOfScript(crossline);
+
+	// ; # // comments
+	if (*script->script_p == ';' || *script->script_p == '#' || (script->script_p[0] == '/' && script->script_p[1] == '/'))
+	{
+		if (!crossline)
+		{
+			//printf("Line %i is incomplete\n",scriptline);
+		}
+		while (*script->script_p++ != '\n')
+			if (script->script_p >= script->end_p)
+				return isEndOfScript(crossline);
+		scriptline = script->line++;
+		goto skipspace;
+	}
+
+	// /* */ comments
+	if (script->script_p[0] == '/' && script->script_p[1] == '*')
+	{
+		if (!crossline)
+		{
+			//printf("Line %i is incomplete\n",scriptline);
+		}
+		script->script_p += 2;
+		while (script->script_p[0] != '*' && script->script_p[1] != '/')
+		{
+			if (*script->script_p == '\n')
+			{
+				scriptline = script->line++;
+			}
+			script->script_p++;
+			if (script->script_p >= script->end_p)
+				return isEndOfScript(crossline);
+		}
+		script->script_p += 2;
+		goto skipspace;
+	}
+
+	//
+	// copy token
+	//
+	token_p = token;
+
+	if (*script->script_p == '"')
+	{
+		// quoted token
+		script->script_p++;
+		while (*script->script_p != '"')
+		{
+			*token_p++ = *script->script_p++;
+			if (script->script_p == script->end_p)
+				break;
+			if (token_p == &token[BSPMAXTOKEN])
+			{
+				//printf ("Token too large on line %i\n",scriptline);
+			}
+		}
+		script->script_p++;
+	}
+	else  // regular token
+		while (*script->script_p > 32 && *script->script_p != ';')
+		{
+			*token_p++ = *script->script_p++;
+			if (script->script_p == script->end_p)
+				break;
+			if (token_p == &token[BSPMAXTOKEN])
+			{
+				//printf ("Token too large on line %i\n",scriptline);
+			}
+		}
+
+	*token_p = 0;
+
+	if (!strcmp(token, "$include"))
+	{
+		//getToken (false);
+		//AddScriptToStack (token);
+		return false;  //getToken (crossline);
+	}
+
+	return true;
+}
+
+char *BspLoader::copystring(const char *s)
+{
+	char *b;
+	b = (char *)malloc(strlen(s) + 1);
+	strcpy(b, s);
+	return b;
+}
+
+void BspLoader::stripTrailing(char *e)
+{
+	char *s;
+
+	s = e + strlen(e) - 1;
+	while (s >= e && *s <= 32)
+	{
+		*s = 0;
+		s--;
+	}
+}
+/*
+=================
+parseEpair
+=================
+*/
+BSPKeyValuePair *BspLoader::parseEpair(void)
+{
+	BSPKeyValuePair *e;
+
+	e = (struct BSPPair *)malloc(sizeof(BSPKeyValuePair));
+	memset(e, 0, sizeof(BSPKeyValuePair));
+
+	if (strlen(token) >= BSPMAX_KEY - 1)
+	{
+		//printf ("ParseEpar: token too long");
+	}
+	e->key = copystring(token);
+	getToken(false);
+	if (strlen(token) >= BSPMAX_VALUE - 1)
+	{
+		//printf ("ParseEpar: token too long");
+	}
+	e->value = copystring(token);
+
+	// strip trailing spaces that sometimes get accidentally
+	// added in the editor
+	stripTrailing(e->key);
+	stripTrailing(e->value);
+
+	return e;
+}
+
+/*
+================
+parseEntity
+================
+*/
+bool BspLoader::parseEntity(void)
+{
+	BSPKeyValuePair *e;
+	BSPEntity *mapent;
+
+	if (!getToken(true))
+	{
+		return false;
+	}
+
+	if (strcmp(token, "{"))
+	{
+		//printf ("parseEntity: { not found");
+	}
+
+	BSPEntity bla;
+	bla.brushes = 0;
+	bla.epairs = 0;
+	bla.firstDrawSurf = 0;
+	bla.origin[0] = 0.f;
+	bla.origin[1] = 0.f;
+	bla.origin[2] = 0.f;
+	bla.patches = 0;
+
+	m_entities.push_back(bla);
+	mapent = &m_entities[m_entities.size() - 1];
+	m_num_entities++;
+
+	do
+	{
+		if (!getToken(true))
+		{
+			//printf("parseEntity: EOF without closing brace");
+		}
+		if (!strcmp(token, "}"))
+		{
+			break;
+		}
+		e = (struct BSPPair *)parseEpair();
+		e->next = mapent->epairs;
+		mapent->epairs = e;
+	} while (1);
+
+	return true;
+}
+
+/*
+================
+parseEntities
+
+Parses the dentdata string into entities
+================
+*/
+void BspLoader::parseEntities(void)
+{
+	m_num_entities = 0;
+	m_entities.clear();
+
+	parseFromMemory(&m_dentdata[0], m_entdatasize);
+
+	while (parseEntity())
+	{
+	}
+}
+
+int BspLoader::getMachineEndianness()
+{
+	long int i = 1;
+	const char *p = (const char *)&i;
+	if (p[0] == 1)  // Lowest address contains the least significant byte
+		return BSP_LITTLE_ENDIAN;
+	else
+		return BSP_BIG_ENDIAN;
+}
+
+short BspLoader::isLittleShort(short l)
+{
+	if (machineEndianness() == BSP_BIG_ENDIAN)
+	{
+		unsigned char b1, b2;
+
+		b1 = l & 255;
+		b2 = (l >> 8) & 255;
+
+		return (b1 << 8) + b2;
+	}
+	//little endian
+	return l;
+}
+
+short BspLoader::isBigShort(short l)
+{
+	if (machineEndianness() == BSP_BIG_ENDIAN)
+	{
+		return l;
+	}
+
+	unsigned char b1, b2;
+
+	b1 = l & 255;
+	b2 = (l >> 8) & 255;
+
+	return (b1 << 8) + b2;
+}
+
+int BspLoader::isLittleLong(int l)
+{
+	if (machineEndianness() == BSP_BIG_ENDIAN)
+	{
+		unsigned char b1, b2, b3, b4;
+
+		b1 = l & 255;
+		b2 = (l >> 8) & 255;
+		b3 = (l >> 16) & 255;
+		b4 = (l >> 24) & 255;
+
+		return ((int)b1 << 24) + ((int)b2 << 16) + ((int)b3 << 8) + b4;
+	}
+
+	//little endian
+	return l;
+}
+
+int BspLoader::isBigLong(int l)
+{
+	if (machineEndianness() == BSP_BIG_ENDIAN)
+	{
+		return l;
+	}
+
+	unsigned char b1, b2, b3, b4;
+
+	b1 = l & 255;
+	b2 = (l >> 8) & 255;
+	b3 = (l >> 16) & 255;
+	b4 = (l >> 24) & 255;
+
+	return ((int)b1 << 24) + ((int)b2 << 16) + ((int)b3 << 8) + b4;
+}
+
+float BspLoader::isLittleFloat(float l)
+{
+	if (machineEndianness() == BSP_BIG_ENDIAN)
+	{
+		union {
+			unsigned char b[4];
+			float f;
+		} in, out;
+
+		in.f = l;
+		out.b[0] = in.b[3];
+		out.b[1] = in.b[2];
+		out.b[2] = in.b[1];
+		out.b[3] = in.b[0];
+
+		return out.f;
+	}
+
+	//little endian
+	return l;
+}
+
+float BspLoader::isBigFloat(float l)
+{
+	if (machineEndianness() == BSP_BIG_ENDIAN)
+	{
+		return l;
+	}
+	//little endian
+	union {
+		unsigned char b[4];
+		float f;
+	} in, out;
+
+	in.f = l;
+	out.b[0] = in.b[3];
+	out.b[1] = in.b[2];
+	out.b[2] = in.b[1];
+	out.b[3] = in.b[0];
+
+	return out.f;
+}
+
+//
+// swapBlock
+// If all values are 32 bits, this can be used to swap everything
+//
+
+void BspLoader::swapBlock(int *block, int sizeOfBlock)
+{
+	int i;
+
+	sizeOfBlock >>= 2;
+	for (i = 0; i < sizeOfBlock; i++)
+	{
+		block[i] = isLittleLong(block[i]);
+	}
+}
+
+//
+// copyLump
+//
+
+int BspLoader::copyLump(BSPHeader *header, int lump, void *dest, int size)
+{
+	int length, ofs;
+
+	length = header->lumps[lump].filelen;
+	ofs = header->lumps[lump].fileofs;
+
+	//if ( length % size ) {
+	//	printf ("loadBSPFile: odd lump size");
+	//}
+
+	memcpy(dest, (unsigned char *)header + ofs, length);
+
+	return length / size;
+}
+
+//
+// swapBSPFile
+//
+
+void BspLoader::swapBSPFile(void)
+{
+	int i;
+
+	// models
+	swapBlock((int *)&m_dmodels[0], m_nummodels * sizeof(m_dmodels[0]));
+
+	// shaders (don't swap the name)
+	for (i = 0; i < m_numShaders; i++)
+	{
+		m_dshaders[i].contentFlags = isLittleLong(m_dshaders[i].contentFlags);
+		m_dshaders[i].surfaceFlags = isLittleLong(m_dshaders[i].surfaceFlags);
+	}
+
+	// planes
+	swapBlock((int *)&m_dplanes[0], m_numplanes * sizeof(m_dplanes[0]));
+
+	// nodes
+	swapBlock((int *)&m_dnodes[0], m_numnodes * sizeof(m_dnodes[0]));
+
+	// leafs
+	swapBlock((int *)&m_dleafs[0], m_numleafs * sizeof(m_dleafs[0]));
+
+	// leaffaces
+	swapBlock((int *)&m_dleafsurfaces[0], m_numleafsurfaces * sizeof(m_dleafsurfaces[0]));
+
+	// leafbrushes
+	swapBlock((int *)&m_dleafbrushes[0], m_numleafbrushes * sizeof(m_dleafbrushes[0]));
+
+	// brushes
+	swapBlock((int *)&m_dbrushes[0], m_numbrushes * sizeof(m_dbrushes[0]));
+
+	// brushsides
+	swapBlock((int *)&m_dbrushsides[0], m_numbrushsides * sizeof(m_dbrushsides[0]));
+
+	// vis
+	((int *)&m_visBytes)[0] = isLittleLong(((int *)&m_visBytes)[0]);
+	((int *)&m_visBytes)[1] = isLittleLong(((int *)&m_visBytes)[1]);
+
+	// drawindexes
+	swapBlock((int *)&m_drawIndexes[0], m_numDrawIndexes * sizeof(m_drawIndexes[0]));
+
+	// drawsurfs
+	swapBlock((int *)&m_drawSurfaces[0], m_numDrawSurfaces * sizeof(m_drawSurfaces[0]));
+}
+
+bool BspLoader::findVectorByName(float *outvec, const char *name)
+{
+	const char *cl;
+	BSPVector3 origin;
+
+	bool found = false;
+
+	parseEntities();
+
+	for (int i = 1; i < m_num_entities; i++)
+	{
+		cl = getValueForKey(&m_entities[i], "classname");
+		if (!strcmp(cl, "info_player_start"))
+		{
+			getVectorForKey(&m_entities[i], "origin", origin);
+			found = true;
+			break;
+		}
+		if (!strcmp(cl, "info_player_deathmatch"))
+		{
+			getVectorForKey(&m_entities[i], "origin", origin);
+			found = true;
+			break;
+		}
+	}
+
+	if (found)
+	{
+		outvec[0] = origin[0];
+		outvec[1] = origin[1];
+		outvec[2] = origin[2];
+	}
+	return found;
+}
+
+const BSPEntity *BspLoader::getEntityByValue(const char *name, const char *value)
+{
+	const BSPEntity *entity = NULL;
+
+	for (int i = 1; i < m_num_entities; i++)
+	{
+		const BSPEntity &ent = m_entities[i];
+
+		const char *cl = getValueForKey(&m_entities[i], name);
+		if (!strcmp(cl, value))
+		{
+			entity = &ent;
+			break;
+		}
+	}
+	return entity;
+}

Engine/lib/bullet/examples/Importers/ImportBsp/BspLoader.h

@@ -0,0 +1,288 @@
+/*
+===========================================================================
+Copyright (C) 1999-2005 Id Software, Inc.
+
+This file is part of Quake III Arena source code.
+
+Quake III Arena source code is free software; you can redistribute it
+and/or modify it under the terms of the GNU bteral Public License as
+published by the Free Software Foundation; either version 2 of the License,
+or (at your option) any later version.
+
+Quake III Arena source code is distributed in the hope that it will be
+useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU bteral Public License for more details.
+
+You should have received a copy of the GNU bteral Public License
+along with Foobar; if not, write to the Free Software
+Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+===========================================================================
+*/
+
+#ifndef BSP_LOADER_H
+#define BSP_LOADER_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+#define BSPMAXTOKEN 1024
+#define BSPMAX_KEY 32
+#define BSPMAX_VALUE 1024
+#define BSPCONTENTS_SOLID 1
+#define BSPCONTENTS_AREAPORTAL 0x8000
+#define BSPLUMP_ENTITIES 0
+#define BSPLUMP_SHADERS 1
+#define BSPLUMP_PLANES 2
+#define BSPLUMP_NODES 3
+#define BSPLUMP_LEAFS 4
+#define BSPLUMP_LEAFSURFACES 5
+#define BSPLUMP_LEAFBRUSHES 6
+#define LUMP_MODELS 7
+#define LUMP_BRUSHES 8
+#define LUMP_BRUSHSIDES 9
+#define LUMP_DRAWVERTS 10
+#define LUMP_DRAWINDEXES 11
+#define LUMP_SURFACES 13
+#define LUMP_LIGHTMAPS 14
+#define LUMP_LIGHTGRID 15
+#define LUMP_VISIBILITY 16
+#define HEADER_LUMPS 17
+#define MAX_QPATH 64
+
+typedef struct
+{
+	int fileofs, filelen;
+} BSPLump;
+
+typedef float BSPVector3[3];
+
+typedef struct
+{
+	int ident;
+	int version;
+
+	BSPLump lumps[HEADER_LUMPS];
+} BSPHeader;
+
+typedef struct
+{
+	float mins[3], maxs[3];
+	int firstSurface, numSurfaces;
+	int firstBrush, numBrushes;
+} BSPModel;
+
+typedef struct
+{
+	char shader[MAX_QPATH];
+	int surfaceFlags;
+	int contentFlags;
+} BSPShader;
+
+typedef struct
+{
+	float normal[3];
+	float dist;
+} BSPPlane;
+
+typedef struct
+{
+	int planeNum;
+	int children[2];
+	int mins[3];
+	int maxs[3];
+} BSPNode;
+
+typedef struct
+{
+	int cluster;
+	int area;
+
+	int mins[3];
+	int maxs[3];
+
+	int firstLeafSurface;
+	int numLeafSurfaces;
+
+	int firstLeafBrush;
+	int numLeafBrushes;
+} BSPLeaf;
+
+typedef struct
+{
+	int planeNum;
+	int shaderNum;
+} BSPBrushSide;
+
+typedef struct
+{
+	int firstSide;
+	int numSides;
+	int shaderNum;
+} BSPBrush;
+
+typedef struct BSPPair
+{
+	struct BSPPair *next;
+	char *key;
+	char *value;
+} BSPKeyValuePair;
+
+typedef struct
+{
+	BSPVector3 origin;
+	struct bspbrush_s *brushes;
+	struct parseMesh_s *patches;
+	int firstDrawSurf;
+	BSPKeyValuePair *epairs;
+} BSPEntity;
+
+typedef enum
+{
+	MST_BAD,
+	MST_PLANAR,
+	MST_PATCH,
+	MST_TRIANGLE_SOUP,
+	MST_FLARE
+} BSPMapSurface;
+
+typedef struct
+{
+	int shaderNum;
+	int fogNum;
+	int surfaceType;
+
+	int firstVert;
+	int numVerts;
+
+	int firstIndex;
+	int numIndexes;
+
+	int lightmapNum;
+	int lightmapX, lightmapY;
+	int lightmapWidth, lightmapHeight;
+
+	BSPVector3 lightmapOrigin;
+	BSPVector3 lightmapVecs[3];
+
+	int patchWidth;
+	int patchHeight;
+} BSPSurface;
+
+///GPL code from IdSofware to parse a Quake 3 BSP file
+///check that your platform define __BIG_ENDIAN__ correctly (in BspLoader.cpp)
+class BspLoader
+{
+	int m_Endianness;
+
+public:
+	BspLoader();
+
+	bool loadBSPFile(void *memoryBuffer);
+
+	const char *getValueForKey(const BSPEntity *ent, const char *key) const;
+
+	bool getVectorForKey(const BSPEntity *ent, const char *key, BSPVector3 vec);
+
+	float getFloatForKey(const BSPEntity *ent, const char *key);
+
+	void parseEntities(void);
+
+	bool findVectorByName(float *outvec, const char *name);
+
+	const BSPEntity *getEntityByValue(const char *name, const char *value);
+
+protected:
+	void parseFromMemory(char *buffer, int size);
+
+	bool isEndOfScript(bool crossline);
+
+	bool getToken(bool crossline);
+
+	char *copystring(const char *s);
+
+	void stripTrailing(char *e);
+
+	BSPKeyValuePair *parseEpair(void);
+
+	bool parseEntity(void);
+
+	short isLittleShort(short l);
+	int isLittleLong(int l);
+	float isLittleFloat(float l);
+
+	int isBigLong(int l);
+	short isBigShort(short l);
+	float isBigFloat(float l);
+
+	void swapBlock(int *block, int sizeOfBlock);
+
+	int copyLump(BSPHeader *header, int lump, void *dest, int size);
+
+	void swapBSPFile(void);
+
+public:  //easier for conversion
+	int m_num_entities;
+	btAlignedObjectArray<BSPEntity> m_entities;
+
+	int m_nummodels;
+	btAlignedObjectArray<BSPModel> m_dmodels;
+
+	int m_numShaders;
+	btAlignedObjectArray<BSPShader> m_dshaders;
+
+	int m_entdatasize;
+	btAlignedObjectArray<char> m_dentdata;
+
+	int m_numleafs;
+	btAlignedObjectArray<BSPLeaf> m_dleafs;
+
+	int m_numplanes;
+	btAlignedObjectArray<BSPPlane> m_dplanes;
+
+	int m_numnodes;
+	btAlignedObjectArray<BSPNode> m_dnodes;
+
+	int m_numleafsurfaces;
+	btAlignedObjectArray<int> m_dleafsurfaces;
+
+	int m_numleafbrushes;
+	btAlignedObjectArray<int> m_dleafbrushes;
+
+	int m_numbrushes;
+	btAlignedObjectArray<BSPBrush> m_dbrushes;
+
+	int m_numbrushsides;
+	btAlignedObjectArray<BSPBrushSide> m_dbrushsides;
+
+	int m_numLightBytes;
+	btAlignedObjectArray<unsigned char> m_lightBytes;
+
+	int m_numGridPoints;
+	btAlignedObjectArray<unsigned char> m_gridData;
+
+	int m_numVisBytes;
+	btAlignedObjectArray<unsigned char> m_visBytes;
+
+	int m_numDrawIndexes;
+	btAlignedObjectArray<int> m_drawIndexes;
+
+	int m_numDrawSurfaces;
+	btAlignedObjectArray<BSPSurface> m_drawSurfaces;
+
+	enum
+	{
+		BSP_LITTLE_ENDIAN = 0,
+		BSP_BIG_ENDIAN = 1
+	};
+
+	//returns machines big endian / little endian
+	//
+	int getMachineEndianness();
+
+	inline int machineEndianness()
+	{
+		return m_Endianness;
+	}
+};
+
+#endif  //BSP_LOADER_H

Engine/lib/bullet/examples/Importers/ImportBsp/ImportBspExample.cpp

@@ -0,0 +1,255 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "ImportBspExample.h"
+
+#include "btBulletDynamicsCommon.h"
+
+#include "LinearMath/btQuickprof.h"
+
+#define QUAKE_BSP_IMPORTING 1
+
+#ifdef QUAKE_BSP_IMPORTING
+#include "BspLoader.h"
+#include "BspConverter.h"
+#endif  //QUAKE_BSP_IMPORTING
+
+#include <stdio.h>  //printf debugging
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+
+///BspDemo shows the convex collision detection, by converting a Quake BSP file into convex objects and allowing interaction with boxes.
+class BspDemo : public CommonRigidBodyBase
+{
+public:
+	//keep the collision shapes, for deletion/cleanup
+
+	BspDemo(struct GUIHelperInterface* helper)
+		: CommonRigidBodyBase(helper)
+	{
+	}
+
+	virtual ~BspDemo();
+
+	virtual void initPhysics();
+
+	void initPhysics(const char* bspfilename);
+
+	virtual void resetCamera()
+	{
+		float dist = 43;
+		float pitch = -12;
+		float yaw = -175;
+		float targetPos[3] = {4, -25, -6};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+};
+
+#define CUBE_HALF_EXTENTS 1
+#define EXTRA_HEIGHT -20.f
+
+///BspToBulletConverter  extends the BspConverter to convert to Bullet datastructures
+class BspToBulletConverter : public BspConverter
+{
+	BspDemo* m_demoApp;
+
+public:
+	BspToBulletConverter(BspDemo* demoApp)
+		: m_demoApp(demoApp)
+	{
+	}
+
+	virtual void addConvexVerticesCollider(btAlignedObjectArray<btVector3>& vertices, bool isEntity, const btVector3& entityTargetLocation)
+	{
+		///perhaps we can do something special with entities (isEntity)
+		///like adding a collision Triggering (as example)
+
+		if (vertices.size() > 0)
+		{
+			float mass = 0.f;
+			btTransform startTransform;
+			//can use a shift
+			startTransform.setIdentity();
+			startTransform.setOrigin(btVector3(0, 0, -10.f));
+			//this create an internal copy of the vertices
+
+			btCollisionShape* shape = new btConvexHullShape(&(vertices[0].getX()), vertices.size());
+			m_demoApp->m_collisionShapes.push_back(shape);
+
+			//btRigidBody* body = m_demoApp->localCreateRigidBody(mass, startTransform,shape);
+			m_demoApp->createRigidBody(mass, startTransform, shape);
+		}
+	}
+};
+
+////////////////////////////////////
+
+BspDemo::~BspDemo()
+{
+	exitPhysics();  //will delete all default data
+}
+
+void BspDemo::initPhysics()
+{
+	const char* bspfilename = "BspDemo.bsp";
+
+	initPhysics(bspfilename);
+}
+
+void BspDemo::initPhysics(const char* bspfilename)
+{
+	int cameraUpAxis = 2;
+	m_guiHelper->setUpAxis(cameraUpAxis);
+	btVector3 grav(0, 0, 0);
+	grav[cameraUpAxis] = -10;
+	m_guiHelper->setUpAxis(cameraUpAxis);
+
+	//_cameraUp = btVector3(0,0,1);
+	//_forwardAxis = 1;
+
+	//etCameraDistance(22.f);
+
+	///Setup a Physics Simulation Environment
+
+	m_collisionConfiguration = new btDefaultCollisionConfiguration();
+	//	btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
+	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+	btVector3 worldMin(-1000, -1000, -1000);
+	btVector3 worldMax(1000, 1000, 1000);
+	m_broadphase = new btDbvtBroadphase();
+	//m_broadphase = new btAxisSweep3(worldMin,worldMax);
+	//btOverlappingPairCache* broadphase = new btSimpleBroadphase();
+	m_solver = new btSequentialImpulseConstraintSolver();
+	//ConstraintSolver* solver = new OdeConstraintSolver;
+	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+	m_dynamicsWorld->setGravity(grav);
+
+#ifdef QUAKE_BSP_IMPORTING
+
+	void* memoryBuffer = 0;
+
+	const char* filename = "BspDemo.bsp";
+
+	const char* prefix[] = {"./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"};
+	int numPrefixes = sizeof(prefix) / sizeof(const char*);
+	char relativeFileName[1024];
+	FILE* file = 0;
+
+	for (int i = 0; i < numPrefixes; i++)
+	{
+		sprintf(relativeFileName, "%s%s", prefix[i], filename);
+		file = fopen(relativeFileName, "r");
+		if (file)
+			break;
+	}
+
+	if (file)
+	{
+		BspLoader bspLoader;
+		int size = 0;
+		if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET))
+		{ /* File operations denied? ok, just close and return failure */
+			printf("Error: cannot get filesize from %s\n", bspfilename);
+		}
+		else
+		{
+			//how to detect file size?
+			memoryBuffer = malloc(size + 1);
+			fread(memoryBuffer, 1, size, file);
+			bspLoader.loadBSPFile(memoryBuffer);
+
+			BspToBulletConverter bsp2bullet(this);
+			float bspScaling = 0.1f;
+			bsp2bullet.convertBsp(bspLoader, bspScaling);
+		}
+		fclose(file);
+	}
+
+#endif
+
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+//some code that de-mangles the windows filename passed in as argument
+char cleaned_filename[512];
+char* getLastFileName()
+{
+	return cleaned_filename;
+}
+char* makeExeToBspFilename(const char* lpCmdLine)
+{
+	// We might get a windows-style path on the command line, this can mess up the DOM which expects
+	// all paths to be URI's.  This block of code does some conversion to try and make the input
+	// compliant without breaking the ability to accept a properly formatted URI.  Right now this only
+	// displays the first filename
+	const char* in = lpCmdLine;
+	char* out = cleaned_filename;
+	*out = '\0';
+	// If the first character is a ", skip it (filenames with spaces in them are quoted)
+	if (*in == '\"')
+	{
+		in++;
+	}
+	int i;
+	for (i = 0; i < 512; i++)
+	{
+		//if we get '.' we stop as well, unless it's the first character. Then we add .bsp as extension
+		// If we hit a null or a quote, stop copying.  This will get just the first filename.
+		if (i && (in[0] == '.') && (in[1] == 'e') && (in[2] == 'x') && (in[3] == 'e'))
+			break;
+
+		// If we hit a null or a quote, stop copying.  This will get just the first filename.
+		if (*in == '\0' || *in == '\"')
+			break;
+		// Copy while swapping backslashes for forward ones
+		if (*in == '\\')
+		{
+			*out = '/';
+		}
+		else
+		{
+			*out = *in;
+		}
+		in++;
+		out++;
+	}
+	*(out++) = '.';
+	*(out++) = 'b';
+	*(out++) = 's';
+	*(out++) = 'p';
+	*(out++) = 0;
+
+	return cleaned_filename;
+}
+
+CommonExampleInterface* ImportBspCreateFunc(struct CommonExampleOptions& options)
+{
+	BspDemo* demo = new BspDemo(options.m_guiHelper);
+
+	demo->initPhysics("BspDemo.bsp");
+	return demo;
+}
+/*
+static DemoApplication* Create()
+	{
+		BspDemo* demo = new BspDemo;
+		demo->myinit();
+		demo->initPhysics("BspDemo.bsp");
+		return demo;
+	}
+	*/

Engine/lib/bullet/examples/Importers/ImportBsp/ImportBspExample.h

@@ -0,0 +1,20 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#ifndef BSP_DEMO_H
+#define BSP_DEMO_H
+
+class CommonExampleInterface* ImportBspCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //BSP_DEMO_H

Engine/lib/bullet/examples/Importers/ImportBullet/SerializeSetup.cpp

@@ -0,0 +1,104 @@
+#include "SerializeSetup.h"
+#include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h"
+
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+
+class SerializeSetup : public CommonRigidBodyBase
+{
+	char m_fileName[1024];
+
+public:
+	SerializeSetup(struct GUIHelperInterface* helper, const char* fileName);
+	virtual ~SerializeSetup();
+
+	virtual void initPhysics();
+	virtual void stepSimulation(float deltaTime);
+	virtual void setFileName(const char* fileName)
+	{
+		memcpy(m_fileName, fileName, strlen(fileName) + 1);
+	}
+	virtual void resetCamera()
+	{
+		float dist = 9.5;
+		float pitch = -20;
+		float yaw = -2.8;
+		float targetPos[3] = {-0.2, -1.4, 3.5};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+};
+
+SerializeSetup::SerializeSetup(struct GUIHelperInterface* helper, const char* fileName)
+	: CommonRigidBodyBase(helper)
+{
+	if (fileName)
+	{
+		setFileName(fileName);
+	}
+	else
+	{
+		setFileName("spider.bullet");
+	}
+}
+SerializeSetup::~SerializeSetup()
+{
+}
+
+void SerializeSetup::initPhysics()
+{
+	this->createEmptyDynamicsWorld();
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+	m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints);
+	btBulletWorldImporter* importer = new btBulletWorldImporter(m_dynamicsWorld);
+
+	const char* prefix[] = {"", "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"};
+	int numPrefixes = sizeof(prefix) / sizeof(const char*);
+	char relativeFileName[1024];
+	FILE* f = 0;
+
+	for (int i = 0; !f && i < numPrefixes; i++)
+	{
+		sprintf(relativeFileName, "%s%s", prefix[i], m_fileName);
+		f = fopen(relativeFileName, "rb");
+		if (f)
+		{
+			break;
+		}
+	}
+	if (f)
+	{
+		fclose(f);
+	}
+
+	importer->loadFile(relativeFileName);
+
+	//for now, guess the up axis from gravity
+	if (m_dynamicsWorld->getGravity()[1] == 0.f)
+	{
+		m_guiHelper->setUpAxis(2);
+	}
+	else
+	{
+		m_guiHelper->setUpAxis(1);
+	}
+
+	//example code to export the dynamics world to a .bullet file
+
+	btDefaultSerializer* serializer = new btDefaultSerializer();
+	m_dynamicsWorld->serialize(serializer);
+
+	FILE* file = fopen("SerializeSetupTestFile.bullet", "wb");
+	fwrite(serializer->getBufferPointer(), serializer->getCurrentBufferSize(), 1, file);
+	fclose(file);
+
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void SerializeSetup::stepSimulation(float deltaTime)
+{
+	CommonRigidBodyBase::stepSimulation(deltaTime);
+}
+
+class CommonExampleInterface* SerializeBulletCreateFunc(struct CommonExampleOptions& options)
+{
+	return new SerializeSetup(options.m_guiHelper, options.m_fileName);
+}

Engine/lib/bullet/examples/Importers/ImportBullet/SerializeSetup.h

@@ -0,0 +1,6 @@
+#ifndef SERIALIZE_SETUP_H
+#define SERIALIZE_SETUP_H
+
+class CommonExampleInterface* SerializeBulletCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //SERIALIZE_SETUP_H

Engine/lib/bullet/examples/Importers/ImportColladaDemo/ColladaGraphicsInstance.h

@@ -0,0 +1,35 @@
+/*
+Bullet Collision Detection and Physics Library http://bulletphysics.org
+This file is Copyright (c) 2014 Google Inc. 
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+//original author: Erwin Coumans
+*/
+
+#ifndef COLLADA_GRAPHICS_INSTANCE_H
+#define COLLADA_GRAPHICS_INSTANCE_H
+
+#include "btMatrix4x4.h"
+
+struct ColladaGraphicsInstance
+{
+	ColladaGraphicsInstance()
+		: m_shapeIndex(-1)
+	{
+		m_worldTransform.setIdentity();
+	}
+	btMatrix4x4 m_worldTransform;
+	int m_shapeIndex;  //could be index into array of GLInstanceGraphicsShape
+	float m_color[4];
+};
+
+#endif  //COLLADA_GRAPHICS_INSTANCE_H

Engine/lib/bullet/examples/Importers/ImportColladaDemo/ImportColladaSetup.cpp

@@ -0,0 +1,192 @@
+/*
+Bullet Collision Detection and Physics Library http://bulletphysics.org
+This file is Copyright (c) 2014 Google Inc. 
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+//original author: Erwin Coumans
+*/
+
+#include "ImportColladaSetup.h"
+#include <vector>
+#include "../OpenGLWindow/GLInstancingRenderer.h"
+#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
+#include "btBulletDynamicsCommon.h"
+#include "../OpenGLWindow/SimpleOpenGL3App.h"
+#include "LoadMeshFromCollada.h"
+#include "Bullet3Common/b3FileUtils.h"
+#include "../../Utils/b3ResourcePath.h"
+#include "../../Utils/b3BulletDefaultFileIO.h"
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+
+class ImportColladaSetup : public CommonRigidBodyBase
+{
+public:
+	ImportColladaSetup(struct GUIHelperInterface* helper);
+	virtual ~ImportColladaSetup();
+
+	virtual void initPhysics();
+	virtual void resetCamera()
+	{
+		float dist = 16;
+		float pitch = -28;
+		float yaw = -140;
+		float targetPos[3] = {-4, -3, -3};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+};
+
+ImportColladaSetup::ImportColladaSetup(struct GUIHelperInterface* helper)
+	: CommonRigidBodyBase(helper)
+{
+}
+
+ImportColladaSetup::~ImportColladaSetup()
+{
+}
+
+static int ColladaGraphicsInstanceSortfnc(const ColladaGraphicsInstance& a, const ColladaGraphicsInstance& b)
+{
+	if (a.m_shapeIndex < b.m_shapeIndex) return +1;
+	if (a.m_shapeIndex > b.m_shapeIndex) return -1;
+	return 0;
+}
+
+void ImportColladaSetup::initPhysics()
+{
+	int upAxis = 1;
+	m_guiHelper->setUpAxis(upAxis);
+	this->createEmptyDynamicsWorld();
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+	m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
+
+	static int fileIndex = 0;
+
+	const char* fileNames[] = {
+		"duck.dae",
+		"seymourplane_triangulate.dae",
+	};
+	const char* fileName = fileNames[fileIndex];
+	int numFiles = sizeof(fileNames) / sizeof(const char*);
+
+	char relativeFileName[1024];
+
+	if (!b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024,0))
+		return;
+
+	btVector3 shift(0, 0, 0);
+	btVector3 scaling(1, 1, 1);
+	//	int index=10;
+
+	{
+		btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
+		btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
+
+		float unitMeterScaling(1);
+		btTransform upAxisTrans;
+		upAxisTrans.setIdentity();
+
+		btVector4 color(0, 0, 1,1);
+
+#ifdef COMPARE_WITH_ASSIMP
+		static int useAssimp = 0;
+		if (useAssimp)
+		{
+			LoadMeshFromColladaAssimp(relativeFileName, visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling);
+			fileIndex++;
+			if (fileIndex >= numFiles)
+			{
+				fileIndex = 0;
+			}
+			color.setValue(1, 0, 0);
+		}
+		else
+		{
+			LoadMeshFromCollada(relativeFileName, visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling);
+		}
+		useAssimp = 1 - useAssimp;
+#else
+		fileIndex++;
+		if (fileIndex >= numFiles)
+		{
+			fileIndex = 0;
+		}
+		b3BulletDefaultFileIO fileIO;
+		LoadMeshFromCollada(relativeFileName, visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, upAxis,&fileIO);
+#endif  // COMPARE_WITH_ASSIMP
+
+		//at the moment our graphics engine requires instances that share the same visual shape to be added right after registering the shape
+		//so perform a sort, just to be sure
+		visualShapeInstances.quickSort(ColladaGraphicsInstanceSortfnc);
+
+		for (int i = 0; i < visualShapeInstances.size(); i++)
+		{
+			ColladaGraphicsInstance* instance = &visualShapeInstances[i];
+			GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
+			btVector3 position(0, 0, 0);   // = scaling*btVector3(instance->m_pos[0],instance->m_pos[1],instance->m_pos[2]);
+			btQuaternion orn(0, 0, 0, 1);  //instance->m_orn[0],instance->m_orn[1],instance->m_orn[2],instance->m_orn[3]);
+
+			//sort the visualShapeInstances, then iterate etc
+			//void LoadMeshFromCollada(const char* relativeFileName,
+			//btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes,
+			//btAlignedObjectArray<GLInstanceGraphicsInstance> visualShapeInstances);
+
+			if (gfxShape)
+			{
+				//btTransform trans;
+				//trans.setIdentity();
+				//trans.setRotation(btQuaternion(btVector3(1,0,0),SIMD_HALF_PI));
+
+				b3AlignedObjectArray<GLInstanceVertex> verts;
+				verts.resize(gfxShape->m_vertices->size());
+
+				for (int i = 0; i < gfxShape->m_vertices->size(); i++)
+				{
+					verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
+					verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
+					verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
+					verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
+					verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
+					verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
+					verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
+					verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
+					verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
+				}
+
+				//compensate upAxisTrans and unitMeterScaling here
+				btMatrix4x4 upAxisMat;
+				upAxisMat.setPureRotation(upAxisTrans.getRotation());
+				btMatrix4x4 unitMeterScalingMat;
+				unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling));
+				btMatrix4x4 worldMat = unitMeterScalingMat * upAxisMat * instance->m_worldTransform;
+				//btMatrix4x4 worldMat = instance->m_worldTransform;
+				for (int v = 0; v < verts.size(); v++)
+				{
+					btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]);
+					pos = worldMat * pos;
+					verts[v].xyzw[0] = float(pos[0]);
+					verts[v].xyzw[1] = float(pos[1]);
+					verts[v].xyzw[2] = float(pos[2]);
+				}
+
+				int shapeId = m_guiHelper->getRenderInterface()->registerShape(&verts[0].xyzw[0], gfxShape->m_numvertices, &gfxShape->m_indices->at(0), gfxShape->m_numIndices);
+
+				//btVector3 instanceScaling(instance->m_scaling[0],instance->m_scaling[1],instance->m_scaling[2]);
+				m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId, position, orn, color, scaling);
+			}
+		}
+	}
+}
+
+class CommonExampleInterface* ImportColladaCreateFunc(struct CommonExampleOptions& options)
+{
+	return new ImportColladaSetup(options.m_guiHelper);
+}

Engine/lib/bullet/examples/Importers/ImportColladaDemo/ImportColladaSetup.h

@@ -0,0 +1,23 @@
+/*
+Bullet Collision Detection and Physics Library http://bulletphysics.org
+This file is Copyright (c) 2014 Google Inc. 
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+//original author: Erwin Coumans
+*/
+
+#ifndef IMPORT_COLLADA_SETUP_H
+#define IMPORT_COLLADA_SETUP_H
+
+class CommonExampleInterface* ImportColladaCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //IMPORT_COLLADA_SETUP_H

Engine/lib/bullet/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp

@@ -0,0 +1,772 @@
+/*
+Bullet Collision Detection and Physics Library http://bulletphysics.org
+This file is Copyright (c) 2014 Google Inc. 
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+//original author: Erwin Coumans
+*/
+
+#include "LoadMeshFromCollada.h"
+#include <stdio.h>  //fopen
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include <string>
+#include "../../ThirdPartyLibs/tinyxml2/tinyxml2.h"
+using namespace tinyxml2;
+
+
+#include "LinearMath/btHashMap.h"
+#include <assert.h>
+#include "btMatrix4x4.h"
+#include "../../CommonInterfaces/CommonFileIOInterface.h"
+
+#define MAX_VISUAL_SHAPES 512
+
+struct VertexSource
+{
+	std::string m_positionArrayId;
+	std::string m_normalArrayId;
+};
+
+struct TokenFloatArray
+{
+	btAlignedObjectArray<float>& m_values;
+	TokenFloatArray(btAlignedObjectArray<float>& floatArray)
+		: m_values(floatArray)
+	{
+	}
+	inline void add(const char* token)
+	{
+		float v = atof(token);
+		m_values.push_back(v);
+	}
+};
+struct TokenIntArray
+{
+	btAlignedObjectArray<int>& m_values;
+	TokenIntArray(btAlignedObjectArray<int>& intArray)
+		: m_values(intArray)
+	{
+	}
+	inline void add(const char* token)
+	{
+		float v = atoi(token);
+		m_values.push_back(v);
+	}
+};
+
+template <typename AddToken>
+void tokenize(const std::string& str, AddToken& tokenAdder, const std::string& delimiters = " \n")
+{
+	std::string::size_type pos, lastPos = 0;
+	while (true)
+	{
+		pos = str.find_first_of(delimiters, lastPos);
+		if (pos == std::string::npos)
+		{
+			pos = str.length();
+			if (pos != lastPos)
+			{
+				tokenAdder.add(str.data() + lastPos);
+			}
+			break;
+		}
+		else
+		{
+			if (pos != lastPos)
+			{
+				tokenAdder.add(str.data() + lastPos);
+			}
+		}
+		lastPos = pos + 1;
+	}
+}
+
+void readFloatArray(XMLElement* source, btAlignedObjectArray<float>& floatArray, int& componentStride)
+{
+	int numVals, stride;
+	XMLElement* array = source->FirstChildElement("float_array");
+	if (array)
+	{
+		componentStride = 1;
+		if (source->FirstChildElement("technique_common")->FirstChildElement("accessor")->QueryIntAttribute("stride", &stride) != XML_NO_ATTRIBUTE)
+		{
+			componentStride = stride;
+		}
+		array->QueryIntAttribute("count", &numVals);
+		TokenFloatArray adder(floatArray);
+		floatArray.reserve(numVals);
+		std::string txt = array->GetText();
+		tokenize(array->GetText(), adder);
+		assert(floatArray.size() == numVals);
+	}
+}
+
+btVector3 getVector3FromXmlText(const char* text)
+{
+	btVector3 vec(0, 0, 0);
+	btAlignedObjectArray<float> floatArray;
+	TokenFloatArray adder(floatArray);
+	floatArray.reserve(3);
+	tokenize(text, adder);
+	assert(floatArray.size() == 3);
+	if (floatArray.size() == 3)
+	{
+		vec.setValue(floatArray[0], floatArray[1], floatArray[2]);
+	}
+	return vec;
+}
+
+btVector4 getVector4FromXmlText(const char* text)
+{
+	btVector4 vec(0, 0, 0, 0);
+	btAlignedObjectArray<float> floatArray;
+	TokenFloatArray adder(floatArray);
+	floatArray.reserve(4);
+	tokenize(text, adder);
+	assert(floatArray.size() == 4);
+	if (floatArray.size() == 4)
+	{
+		vec.setValue(floatArray[0], floatArray[1], floatArray[2], floatArray[3]);
+	}
+	return vec;
+}
+
+void readLibraryGeometries(XMLDocument& doc, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btHashMap<btHashString, int>& name2Shape, float extraScaling)
+{
+	btHashMap<btHashString, XMLElement*> allSources;
+	btHashMap<btHashString, VertexSource> vertexSources;
+	for (XMLElement* geometry = doc.RootElement()->FirstChildElement("library_geometries")->FirstChildElement("geometry");
+		 geometry != NULL; geometry = geometry->NextSiblingElement("geometry"))
+	{
+		btAlignedObjectArray<btVector3> vertexPositions;
+		btAlignedObjectArray<btVector3> vertexNormals;
+		btAlignedObjectArray<int> indices;
+
+		const char* geometryName = geometry->Attribute("id");
+		for (XMLElement* mesh = geometry->FirstChildElement("mesh"); (mesh != NULL); mesh = mesh->NextSiblingElement("mesh"))
+		{
+			XMLElement* vertices2 = mesh->FirstChildElement("vertices");
+
+			for (XMLElement* source = mesh->FirstChildElement("source"); source != NULL; source = source->NextSiblingElement("source"))
+			{
+				const char* srcId = source->Attribute("id");
+				//				printf("source id=%s\n",srcId);
+				allSources.insert(srcId, source);
+			}
+			const char* vertexId = vertices2->Attribute("id");
+			//printf("vertices id=%s\n",vertexId);
+			VertexSource vs;
+			for (XMLElement* input = vertices2->FirstChildElement("input"); input != NULL; input = input->NextSiblingElement("input"))
+			{
+				const char* sem = input->Attribute("semantic");
+				std::string semName(sem);
+				//					printf("sem=%s\n",sem);
+				//		const char* src = input->Attribute("source");
+				//					printf("src=%s\n",src);
+				const char* srcIdRef = input->Attribute("source");
+				std::string source_name;
+				source_name = std::string(srcIdRef);
+				source_name = source_name.erase(0, 1);
+				if (semName == "POSITION")
+				{
+					vs.m_positionArrayId = source_name;
+				}
+				if (semName == "NORMAL")
+				{
+					vs.m_normalArrayId = source_name;
+				}
+			}
+			vertexSources.insert(vertexId, vs);
+
+			btAlignedObjectArray<XMLElement*> trianglesAndPolylists;
+
+			for (XMLElement* primitive = mesh->FirstChildElement("triangles"); primitive; primitive = primitive->NextSiblingElement("triangles"))
+			{
+				trianglesAndPolylists.push_back(primitive);
+			}
+			for (XMLElement* primitive = mesh->FirstChildElement("polylist"); primitive; primitive = primitive->NextSiblingElement("polylist"))
+			{
+				trianglesAndPolylists.push_back(primitive);
+			}
+
+			for (int i = 0; i < trianglesAndPolylists.size(); i++)
+			{
+				XMLElement* primitive = trianglesAndPolylists[i];
+				std::string positionSourceName;
+				std::string normalSourceName;
+				int primitiveCount;
+				primitive->QueryIntAttribute("count", &primitiveCount);
+				int indexStride = 1;
+				int posOffset = 0;
+				int normalOffset = 0;
+				int numIndices = 0;
+				{
+					for (XMLElement* input = primitive->FirstChildElement("input"); input != NULL; input = input->NextSiblingElement("input"))
+					{
+						const char* sem = input->Attribute("semantic");
+						std::string semName(sem);
+						int offset = atoi(input->Attribute("offset"));
+						if ((offset + 1) > indexStride)
+							indexStride = offset + 1;
+						//printf("sem=%s\n",sem);
+						//	const char* src = input->Attribute("source");
+
+						//printf("src=%s\n",src);
+						const char* srcIdRef = input->Attribute("source");
+						std::string source_name;
+						source_name = std::string(srcIdRef);
+						source_name = source_name.erase(0, 1);
+
+						if (semName == "VERTEX")
+						{
+							//now we have POSITION and possibly NORMAL too, using same index array (<p>)
+							VertexSource* vs = vertexSources[source_name.c_str()];
+							if (vs->m_positionArrayId.length())
+							{
+								positionSourceName = vs->m_positionArrayId;
+								posOffset = offset;
+							}
+							if (vs->m_normalArrayId.length())
+							{
+								normalSourceName = vs->m_normalArrayId;
+								normalOffset = offset;
+							}
+						}
+						if (semName == "NORMAL")
+						{
+							btAssert(normalSourceName.length() == 0);
+							normalSourceName = source_name;
+							normalOffset = offset;
+						}
+					}
+					numIndices = primitiveCount * 3;
+				}
+				btAlignedObjectArray<float> positionFloatArray;
+				int posStride = 1;
+				XMLElement** sourcePtr = allSources[positionSourceName.c_str()];
+				if (sourcePtr)
+				{
+					readFloatArray(*sourcePtr, positionFloatArray, posStride);
+				}
+				btAlignedObjectArray<float> normalFloatArray;
+				int normalStride = 1;
+				sourcePtr = allSources[normalSourceName.c_str()];
+				if (sourcePtr)
+				{
+					readFloatArray(*sourcePtr, normalFloatArray, normalStride);
+				}
+				btAlignedObjectArray<int> curIndices;
+				curIndices.reserve(numIndices * indexStride);
+				TokenIntArray adder(curIndices);
+				std::string txt = primitive->FirstChildElement("p")->GetText();
+				tokenize(txt, adder);
+				assert(curIndices.size() == numIndices * indexStride);
+				int indexOffset = vertexPositions.size();
+
+				for (int index = 0; index < numIndices; index++)
+				{
+					int posIndex = curIndices[index * indexStride + posOffset];
+					int normalIndex = curIndices[index * indexStride + normalOffset];
+					vertexPositions.push_back(btVector3(extraScaling * positionFloatArray[posIndex * 3 + 0],
+														extraScaling * positionFloatArray[posIndex * 3 + 1],
+														extraScaling * positionFloatArray[posIndex * 3 + 2]));
+
+					if (normalFloatArray.size() && (normalFloatArray.size() > normalIndex))
+					{
+						vertexNormals.push_back(btVector3(normalFloatArray[normalIndex * 3 + 0],
+														  normalFloatArray[normalIndex * 3 + 1],
+														  normalFloatArray[normalIndex * 3 + 2]));
+					}
+					else
+					{
+						//add a dummy normal of length zero, so it is easy to detect that it is an invalid normal
+						vertexNormals.push_back(btVector3(0, 0, 0));
+					}
+				}
+				int curNumIndices = indices.size();
+				indices.resize(curNumIndices + numIndices);
+				for (int index = 0; index < numIndices; index++)
+				{
+					indices[curNumIndices + index] = index + indexOffset;
+				}
+			}  //if(primitive != NULL)
+		}      //for each mesh
+
+		int shapeIndex = visualShapes.size();
+		if (shapeIndex < MAX_VISUAL_SHAPES)
+		{
+			GLInstanceGraphicsShape& visualShape = visualShapes.expand();
+			{
+				visualShape.m_vertices = new b3AlignedObjectArray<GLInstanceVertex>;
+				visualShape.m_indices = new b3AlignedObjectArray<int>;
+				int indexBase = 0;
+
+				btAssert(vertexNormals.size() == vertexPositions.size());
+				for (int v = 0; v < vertexPositions.size(); v++)
+				{
+					GLInstanceVertex vtx;
+					vtx.xyzw[0] = vertexPositions[v].x();
+					vtx.xyzw[1] = vertexPositions[v].y();
+					vtx.xyzw[2] = vertexPositions[v].z();
+					vtx.xyzw[3] = 1.f;
+					vtx.normal[0] = vertexNormals[v].x();
+					vtx.normal[1] = vertexNormals[v].y();
+					vtx.normal[2] = vertexNormals[v].z();
+					vtx.uv[0] = 0.5f;
+					vtx.uv[1] = 0.5f;
+					visualShape.m_vertices->push_back(vtx);
+				}
+
+				for (int index = 0; index < indices.size(); index++)
+				{
+					visualShape.m_indices->push_back(indices[index] + indexBase);
+				}
+
+				//b3Printf(" index_count =%dand vertexPositions.size=%d\n",indices.size(), vertexPositions.size());
+				indexBase = visualShape.m_vertices->size();
+				visualShape.m_numIndices = visualShape.m_indices->size();
+				visualShape.m_numvertices = visualShape.m_vertices->size();
+			}
+			//b3Printf("geometry name=%s\n",geometryName);
+			name2Shape.insert(geometryName, shapeIndex);
+		}
+		else
+		{
+			b3Warning("DAE exceeds number of visual shapes (%d/%d)", shapeIndex, MAX_VISUAL_SHAPES);
+		}
+
+	}  //for each geometry
+}
+
+void readNodeHierarchy(XMLElement* node, btHashMap<btHashString, int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, const btMatrix4x4& parentTransMat)
+{
+	btMatrix4x4 nodeTrans;
+	nodeTrans.setIdentity();
+
+	///todo(erwincoumans) we probably have to read the elements 'translate', 'scale', 'rotate' and 'matrix' in-order and accumulate them...
+	{
+		for (XMLElement* transElem = node->FirstChildElement("matrix"); transElem; transElem = node->NextSiblingElement("matrix"))
+		{
+			if (transElem->GetText())
+			{
+				btAlignedObjectArray<float> floatArray;
+				TokenFloatArray adder(floatArray);
+				tokenize(transElem->GetText(), adder);
+				if (floatArray.size() == 16)
+				{
+					btMatrix4x4 t(floatArray[0], floatArray[1], floatArray[2], floatArray[3],
+								  floatArray[4], floatArray[5], floatArray[6], floatArray[7],
+								  floatArray[8], floatArray[9], floatArray[10], floatArray[11],
+								  floatArray[12], floatArray[13], floatArray[14], floatArray[15]);
+
+					nodeTrans = nodeTrans * t;
+				}
+				else
+				{
+					b3Warning("Error: expected 16 elements in a <matrix> element, skipping\n");
+				}
+			}
+		}
+	}
+
+	{
+		for (XMLElement* transElem = node->FirstChildElement("translate"); transElem; transElem = node->NextSiblingElement("translate"))
+		{
+			if (transElem->GetText())
+			{
+				btVector3 pos = getVector3FromXmlText(transElem->GetText());
+				//nodePos+= unitScaling*parentScaling*pos;
+				btMatrix4x4 t;
+				t.setPureTranslation(pos);
+				nodeTrans = nodeTrans * t;
+			}
+		}
+	}
+	{
+		for (XMLElement* scaleElem = node->FirstChildElement("scale");
+			 scaleElem != NULL; scaleElem = node->NextSiblingElement("scale"))
+		{
+			if (scaleElem->GetText())
+			{
+				btVector3 scaling = getVector3FromXmlText(scaleElem->GetText());
+				btMatrix4x4 t;
+				t.setPureScaling(scaling);
+				nodeTrans = nodeTrans * t;
+			}
+		}
+	}
+	{
+		for (XMLElement* rotateElem = node->FirstChildElement("rotate");
+			 rotateElem != NULL; rotateElem = node->NextSiblingElement("rotate"))
+		{
+			if (rotateElem->GetText())
+			{
+				//accumulate orientation
+				btVector4 rotate = getVector4FromXmlText(rotateElem->GetText());
+				btQuaternion orn(btVector3(rotate), btRadians(rotate[3]));  //COLLADA DAE rotate is in degrees, convert to radians
+				btMatrix4x4 t;
+				t.setPureRotation(orn);
+				nodeTrans = nodeTrans * t;
+			}
+		}
+	}
+
+	nodeTrans = parentTransMat * nodeTrans;
+
+	for (XMLElement* instanceGeom = node->FirstChildElement("instance_geometry");
+		 instanceGeom != 0;
+		 instanceGeom = instanceGeom->NextSiblingElement("instance_geometry"))
+	{
+		const char* geomUrl = instanceGeom->Attribute("url");
+		//printf("node referring to geom %s\n", geomUrl);
+		geomUrl++;
+		int* shapeIndexPtr = name2Shape[geomUrl];
+		if (shapeIndexPtr)
+		{
+			//	int index = *shapeIndexPtr;
+			//printf("found geom with index %d\n", *shapeIndexPtr);
+			ColladaGraphicsInstance& instance = visualShapeInstances.expand();
+			instance.m_shapeIndex = *shapeIndexPtr;
+			instance.m_worldTransform = nodeTrans;
+		}
+		else
+		{
+			b3Warning("geom not found\n");
+		}
+	}
+
+	for (XMLElement* childNode = node->FirstChildElement("node");
+		 childNode != NULL; childNode = childNode->NextSiblingElement("node"))
+	{
+		readNodeHierarchy(childNode, name2Shape, visualShapeInstances, nodeTrans);
+	}
+}
+void readVisualSceneInstanceGeometries(XMLDocument& doc, btHashMap<btHashString, int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances)
+{
+	btHashMap<btHashString, XMLElement*> allVisualScenes;
+
+	XMLElement* libVisualScenes = doc.RootElement()->FirstChildElement("library_visual_scenes");
+	if (libVisualScenes == 0)
+		return;
+
+	{
+		for (XMLElement* scene = libVisualScenes->FirstChildElement("visual_scene");
+			 scene != NULL; scene = scene->NextSiblingElement("visual_scene"))
+		{
+			const char* sceneName = scene->Attribute("id");
+			allVisualScenes.insert(sceneName, scene);
+		}
+	}
+
+	XMLElement* scene = 0;
+	{
+		XMLElement* scenes = doc.RootElement()->FirstChildElement("scene");
+		if (scenes)
+		{
+			XMLElement* instanceSceneReference = scenes->FirstChildElement("instance_visual_scene");
+			if (instanceSceneReference)
+			{
+				const char* instanceSceneUrl = instanceSceneReference->Attribute("url");
+				XMLElement** sceneInstancePtr = allVisualScenes[instanceSceneUrl + 1];  //skip #
+				if (sceneInstancePtr)
+				{
+					scene = *sceneInstancePtr;
+				}
+			}
+		}
+	}
+
+	if (scene)
+	{
+		for (XMLElement* node = scene->FirstChildElement("node");
+			 node != NULL; node = node->NextSiblingElement("node"))
+		{
+			btMatrix4x4 identity;
+			identity.setIdentity();
+			btVector3 identScaling(1, 1, 1);
+			readNodeHierarchy(node, name2Shape, visualShapeInstances, identity);
+		}
+	}
+}
+
+void getUnitMeterScalingAndUpAxisTransform(XMLDocument& doc, btTransform& tr, float& unitMeterScaling, int clientUpAxis)
+{
+	///todo(erwincoumans) those up-axis transformations have been quickly coded without rigorous testing
+
+	XMLElement* unitMeter = doc.RootElement()->FirstChildElement("asset")->FirstChildElement("unit");
+	if (unitMeter)
+	{
+		const char* meterText = unitMeter->Attribute("meter");
+		//printf("meterText=%s\n", meterText);
+		unitMeterScaling = atof(meterText);
+	}
+
+	XMLElement* upAxisElem = doc.RootElement()->FirstChildElement("asset")->FirstChildElement("up_axis");
+	if (upAxisElem)
+	{
+		switch (clientUpAxis)
+		{
+			case 1:
+			{
+				std::string upAxisTxt = upAxisElem->GetText();
+				if (upAxisTxt == "X_UP")
+				{
+					btQuaternion x2y(btVector3(0, 0, 1), SIMD_HALF_PI);
+					tr.setRotation(x2y);
+				}
+				if (upAxisTxt == "Y_UP")
+				{
+					//assume Y_UP for now, to be compatible with assimp?
+					//client and COLLADA are both Z_UP so no transform needed (identity)
+				}
+				if (upAxisTxt == "Z_UP")
+				{
+					btQuaternion z2y(btVector3(1, 0, 0), -SIMD_HALF_PI);
+					tr.setRotation(z2y);
+				}
+				break;
+			}
+			case 2:
+			{
+				std::string upAxisTxt = upAxisElem->GetText();
+				if (upAxisTxt == "X_UP")
+				{
+					btQuaternion x2z(btVector3(0, 1, 0), -SIMD_HALF_PI);
+					tr.setRotation(x2z);
+				}
+				if (upAxisTxt == "Y_UP")
+				{
+					btQuaternion y2z(btVector3(1, 0, 0), SIMD_HALF_PI);
+					tr.setRotation(y2z);
+				}
+				if (upAxisTxt == "Z_UP")
+				{
+					//client and COLLADA are both Z_UP so no transform needed (identity)
+				}
+				break;
+			}
+			case 0:
+			default:
+			{
+				//we don't support X or other up axis
+				btAssert(0);
+			}
+		};
+	}
+}
+
+void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, btTransform& upAxisTransform, float& unitMeterScaling, int clientUpAxis, struct CommonFileIOInterface* fileIO)
+{
+	//	GLInstanceGraphicsShape* instance = 0;
+
+	//usually COLLADA files don't have that many visual geometries/shapes
+	visualShapes.reserve(MAX_VISUAL_SHAPES);
+
+	float extraScaling = 1;  //0.01;
+	btHashMap<btHashString, int> name2ShapeIndex;
+	
+	char filename[1024];
+	if (!fileIO->findResourcePath(relativeFileName, filename, 1024))
+	{
+		b3Warning("File not found: %s\n", filename);
+		return;
+	}
+
+	XMLDocument doc;
+	//doc.Parse((const char*)filedata, 0, TIXML_ENCODING_UTF8);
+	b3AlignedObjectArray<char> xmlString;
+	int fileHandle = fileIO->fileOpen(filename,"r");
+	if (fileHandle>=0)
+	{
+		int size = fileIO->getFileSize(fileHandle);
+		xmlString.resize(size);
+		int actual = fileIO->fileRead(fileHandle, &xmlString[0],size);
+		if (actual==size)
+		{
+		}
+		fileIO->fileClose(fileHandle);
+	}
+	if (xmlString.size()==0)
+		return;
+
+	if (doc.Parse(&xmlString[0], xmlString.size()) != XML_SUCCESS)
+	//if (doc.LoadFile(filename) != XML_SUCCESS)
+		return;
+
+	//We need units to be in meter, so apply a scaling using the asset/units meter
+	unitMeterScaling = 1;
+	upAxisTransform.setIdentity();
+
+	//Also we can optionally compensate all transforms using the asset/up_axis as well as unit meter scaling
+	getUnitMeterScalingAndUpAxisTransform(doc, upAxisTransform, unitMeterScaling, clientUpAxis);
+
+	btMatrix4x4 ident;
+	ident.setIdentity();
+
+	readLibraryGeometries(doc, visualShapes, name2ShapeIndex, extraScaling);
+
+	readVisualSceneInstanceGeometries(doc, name2ShapeIndex, visualShapeInstances);
+}
+
+#ifdef COMPARE_WITH_ASSIMP
+
+#include <assimp/Importer.hpp>
+#include <assimp/mesh.h>
+#include <assimp/postprocess.h>
+#include <assimp/scene.h>
+
+#include "assimp/ColladaLoader.h"
+//#	include "STLLoader.h"
+#include "assimp/SortByPTypeProcess.h"
+#include "assimp/LimitBoneWeightsProcess.h"
+#include "assimp/TriangulateProcess.h"
+#include "assimp/JoinVerticesProcess.h"
+#include "assimp/RemoveVCProcess.h"
+
+namespace Assimp
+{
+// ------------------------------------------------------------------------------------------------
+void GetImporterInstanceList(std::vector<BaseImporter*>& out)
+{
+	out.push_back(new ColladaLoader());
+}
+// ------------------------------------------------------------------------------------------------
+void GetPostProcessingStepInstanceList(std::vector<BaseProcess*>& out)
+{
+	out.push_back(new SortByPTypeProcess());
+	out.push_back(new LimitBoneWeightsProcess());
+	out.push_back(new TriangulateProcess());
+	out.push_back(new JoinVerticesProcess());
+	//out.push_back( new RemoveVCProcess());
+}
+
+}  // namespace Assimp
+
+static void addMeshParts(const aiScene* scene, const aiNode* node, GLInstanceGraphicsShape* outverts, const aiMatrix4x4& parentTr)
+{
+	aiMatrix4x4 const& nodeTrans(node->mTransformation);
+
+	aiMatrix4x4 trans;
+	trans = parentTr * nodeTrans;
+
+	for (size_t i = 0; i < node->mNumMeshes; ++i)
+	{
+		aiMesh const* mesh = scene->mMeshes[node->mMeshes[i]];
+		size_t num_vertices = mesh->mNumVertices;
+		if (mesh->mPrimitiveTypes == aiPrimitiveType_TRIANGLE)
+		{
+			int curVertexBase = outverts->m_vertices->size();
+
+			for (int v = 0; v < mesh->mNumVertices; v++)
+			{
+				GLInstanceVertex vtx;
+				aiVector3D vWorld = trans * mesh->mVertices[v];
+				vtx.xyzw[0] = vWorld.x;
+				vtx.xyzw[1] = vWorld.y;
+				vtx.xyzw[2] = vWorld.z;
+				vtx.xyzw[3] = 1;
+				if (mesh->HasNormals())
+				{
+					vtx.normal[0] = mesh->mNormals[v].x;
+					vtx.normal[1] = mesh->mNormals[v].y;
+					vtx.normal[2] = mesh->mNormals[v].z;
+				}
+				else
+				{
+					vtx.normal[0] = 0;
+					vtx.normal[1] = 0;
+					vtx.normal[2] = 1;
+				}
+				if (mesh->HasTextureCoords(0))
+				{
+					vtx.uv[0] = mesh->mTextureCoords[0][v].x;
+					vtx.uv[1] = mesh->mTextureCoords[0][v].y;
+				}
+				else
+				{
+					vtx.uv[0] = 0.5f;
+					vtx.uv[1] = 0.5f;
+				}
+				outverts->m_vertices->push_back(vtx);
+			}
+			for (int f = 0; f < mesh->mNumFaces; f++)
+			{
+				b3Assert(mesh->mFaces[f].mNumIndices == 3);
+				int i0 = mesh->mFaces[f].mIndices[0];
+				int i1 = mesh->mFaces[f].mIndices[1];
+				int i2 = mesh->mFaces[f].mIndices[2];
+				outverts->m_indices->push_back(i0 + curVertexBase);
+				outverts->m_indices->push_back(i1 + curVertexBase);
+				outverts->m_indices->push_back(i2 + curVertexBase);
+			}
+		}
+	}
+	for (size_t i = 0; i < node->mNumChildren; ++i)
+	{
+		addMeshParts(scene, node->mChildren[i], outverts, trans);
+	}
+}
+
+void LoadMeshFromColladaAssimp(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, btTransform& upAxisTrans, float& unitMeterScaling)
+{
+	upAxisTrans.setIdentity();
+	unitMeterScaling = 1;
+
+	GLInstanceGraphicsShape* shape = 0;
+
+	FILE* file = fopen(relativeFileName, "rb");
+	if (file)
+	{
+		int size = 0;
+		if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET))
+		{
+			b3Warning("Error: Cannot access file to determine size of %s\n", relativeFileName);
+		}
+		else
+		{
+			if (size)
+			{
+				//printf("Open DAE file of %d bytes\n",size);
+
+				Assimp::Importer importer;
+				//importer.SetPropertyInteger(AI_CONFIG_PP_RVC_FLAGS, aiComponent_NORMALS | aiComponent_COLORS);
+				importer.SetPropertyInteger(AI_CONFIG_PP_SBP_REMOVE, aiPrimitiveType_LINE | aiPrimitiveType_POINT);
+				//	importer.SetPropertyInteger(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, 1);
+				aiScene const* scene = importer.ReadFile(relativeFileName,
+														 aiProcess_JoinIdenticalVertices |
+															 //aiProcess_RemoveComponent |
+															 aiProcess_SortByPType |
+															 aiProcess_Triangulate);
+				if (scene)
+				{
+					shape = &visualShapes.expand();
+					shape->m_scaling[0] = 1;
+					shape->m_scaling[1] = 1;
+					shape->m_scaling[2] = 1;
+					shape->m_scaling[3] = 1;
+					int index = 0;
+					shape->m_indices = new b3AlignedObjectArray<int>();
+					shape->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
+
+					aiMatrix4x4 ident;
+					addMeshParts(scene, scene->mRootNode, shape, ident);
+					shape->m_numIndices = shape->m_indices->size();
+					shape->m_numvertices = shape->m_vertices->size();
+					ColladaGraphicsInstance& instance = visualShapeInstances.expand();
+					instance.m_shapeIndex = visualShapes.size() - 1;
+				}
+			}
+		}
+	}
+}
+
+#endif  //COMPARE_WITH_ASSIMP

Engine/lib/bullet/examples/Importers/ImportColladaDemo/LoadMeshFromCollada.h

@@ -0,0 +1,43 @@
+/*
+Bullet Collision Detection and Physics Library http://bulletphysics.org
+This file is Copyright (c) 2014 Google Inc. 
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+//original author: Erwin Coumans
+*/
+
+#ifndef LOAD_MESH_FROM_COLLADA_H
+#define LOAD_MESH_FROM_COLLADA_H
+
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btTransform.h"
+#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
+#include "ColladaGraphicsInstance.h"
+
+void LoadMeshFromCollada(const char* relativeFileName,
+						 btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes,
+						 btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances,
+						 btTransform& upAxisTrans,
+						 float& unitMeterScaling,
+						 int clientUpAxis,
+						 struct CommonFileIOInterface* fileIO);
+
+//#define COMPARE_WITH_ASSIMP
+#ifdef COMPARE_WITH_ASSIMP
+void LoadMeshFromColladaAssimp(const char* relativeFileName,
+							   btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes,
+							   btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances,
+							   btTransform& upAxisTrans,
+							   float& unitMeterScaling);
+#endif  //COMPARE_WITH_ASSIMP
+
+#endif  //LOAD_MESH_FROM_COLLADA_H

Engine/lib/bullet/examples/Importers/ImportColladaDemo/btMatrix4x4.h

@@ -0,0 +1,152 @@
+/*
+Bullet Collision Detection and Physics Library http://bulletphysics.org
+This file is Copyright (c) 2014 Google Inc. 
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+//original author: Erwin Coumans
+*/
+
+#ifndef MATRIX4x4_H
+#define MATRIX4x4_H
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btQuaternion.h"
+
+///This 4x4 matrix class is extremely limited, just created for the purpose of accumulating transform matrices in COLLADA .dae files
+ATTRIBUTE_ALIGNED16(class)
+btMatrix4x4
+{
+	btVector4 m_el[4];
+
+public:
+	btMatrix4x4()
+	{
+	}
+	btMatrix4x4(const btScalar& xx, const btScalar& xy, const btScalar& xz, const btScalar& xw,
+				const btScalar& yx, const btScalar& yy, const btScalar& yz, const btScalar& yw,
+				const btScalar& zx, const btScalar& zy, const btScalar& zz, const btScalar& zw,
+				const btScalar& wx, const btScalar& wy, const btScalar& wz, const btScalar& ww)
+	{
+		setValue(xx, xy, xz, xw,
+				 yx, yy, yz, yw,
+				 zx, zy, zz, zw,
+				 wx, wy, wz, ww);
+	}
+
+	~btMatrix4x4()
+	{
+	}
+
+	inline void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, const btScalar& xw,
+						 const btScalar& yx, const btScalar& yy, const btScalar& yz, const btScalar& yw,
+						 const btScalar& zx, const btScalar& zy, const btScalar& zz, const btScalar& zw,
+						 const btScalar& wx, const btScalar& wy, const btScalar& wz, const btScalar& ww)
+	{
+		m_el[0].setValue(xx, xy, xz, xw);
+		m_el[1].setValue(yx, yy, yz, yw);
+		m_el[2].setValue(zx, zy, zz, zw);
+		m_el[3].setValue(wx, wy, wz, ww);
+	}
+
+	inline void setIdentity()
+	{
+		m_el[0].setValue(1, 0, 0, 0);
+		m_el[1].setValue(0, 1, 0, 0);
+		m_el[2].setValue(0, 0, 1, 0);
+		m_el[3].setValue(0, 0, 0, 1);
+	}
+	inline void setPureRotation(const btQuaternion& orn)
+	{
+		setIdentity();
+
+		btMatrix3x3 m3(orn);
+		for (int i = 0; i < 3; i++)
+		{
+			for (int j = 0; j < 3; j++)
+			{
+				m_el[i][j] = m3[i][j];
+			}
+		}
+	}
+
+	inline void setPureScaling(const btVector3& scale)
+	{
+		m_el[0].setValue(scale[0], 0, 0, 0);
+		m_el[1].setValue(0, scale[1], 0, 0);
+		m_el[2].setValue(0, 0, scale[2], 0);
+		m_el[3].setValue(0, 0, 0, 1);
+	}
+
+	inline void setPureTranslation(const btVector3& pos)
+	{
+		m_el[0].setValue(1, 0, 0, pos[0]);
+		m_el[1].setValue(0, 1, 0, pos[1]);
+		m_el[2].setValue(0, 0, 1, pos[2]);
+		m_el[3].setValue(0, 0, 0, 1);
+	}
+	SIMD_FORCE_INLINE const btVector4& operator[](int i) const
+	{
+		btFullAssert(0 <= i && i < 3);
+		return m_el[i];
+	}
+
+	SIMD_FORCE_INLINE btScalar tdotx(const btVector4& v) const
+	{
+		return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z() + m_el[3].x() * v.w();
+	}
+	SIMD_FORCE_INLINE btScalar tdoty(const btVector4& v) const
+	{
+		return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z() + m_el[3].y() * v.w();
+	}
+	SIMD_FORCE_INLINE btScalar tdotz(const btVector4& v) const
+	{
+		return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z() + m_el[3].z() * v.w();
+	}
+	SIMD_FORCE_INLINE btScalar tdotw(const btVector4& v) const
+	{
+		return m_el[0].w() * v.x() + m_el[1].w() * v.y() + m_el[2].w() * v.z() + m_el[3].w() * v.w();
+	}
+
+	SIMD_FORCE_INLINE btMatrix4x4&
+	operator*=(const btMatrix4x4& m)
+	{
+		setValue(
+			m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]), m.tdotw(m_el[0]),
+			m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]), m.tdotw(m_el[1]),
+			m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]), m.tdotw(m_el[2]),
+			m.tdotx(m_el[3]), m.tdoty(m_el[3]), m.tdotz(m_el[3]), m.tdotw(m_el[3]));
+		return *this;
+	}
+};
+
+inline btScalar btDot4(const btVector4& v0, const btVector4& v1)
+{
+	return v0.x() * v1.x() + v0.y() * v1.y() + v0.z() * v1.z() + v0.w() * v1.w();
+}
+SIMD_FORCE_INLINE btVector3
+operator*(const btMatrix4x4& m, const btVector3& v1)
+{
+	btVector4 v(v1[0], v1[1], v1[2], 1);
+	return btVector3(btDot4(m[0], v), btDot4(m[1], v), btDot4(m[2], v));
+}
+
+SIMD_FORCE_INLINE btMatrix4x4
+operator*(const btMatrix4x4& m1, btMatrix4x4& m2)
+{
+	return btMatrix4x4(
+		m2.tdotx(m1[0]), m2.tdoty(m1[0]), m2.tdotz(m1[0]), m2.tdotw(m1[0]),
+		m2.tdotx(m1[1]), m2.tdoty(m1[1]), m2.tdotz(m1[1]), m2.tdotw(m1[1]),
+		m2.tdotx(m1[2]), m2.tdoty(m1[2]), m2.tdotz(m1[2]), m2.tdotw(m1[2]),
+		m2.tdotx(m1[3]), m2.tdoty(m1[3]), m2.tdotz(m1[3]), m2.tdotw(m1[3]));
+}
+
+#endif  //MATRIX4x4_H

Engine/lib/bullet/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h

@@ -0,0 +1,93 @@
+#ifndef BULLET_MJCF_IMPORTER_H
+#define BULLET_MJCF_IMPORTER_H
+
+#include "../ImportURDFDemo/URDFImporterInterface.h"
+#include "../ImportURDFDemo/UrdfRenderingInterface.h"
+
+struct MJCFErrorLogger
+{
+	virtual ~MJCFErrorLogger() {}
+	virtual void reportError(const char* error) = 0;
+	virtual void reportWarning(const char* warning) = 0;
+	virtual void printMessage(const char* msg) = 0;
+};
+
+struct MJCFURDFTexture
+{
+	int m_width;
+	int m_height;
+	unsigned char* textureData1;
+	bool m_isCached;
+};
+
+class BulletMJCFImporter : public URDFImporterInterface
+{
+	struct BulletMJCFImporterInternalData* m_data;
+
+	void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<struct GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MJCFURDFTexture>& texturesOut) const;
+
+public:
+	BulletMJCFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter,  struct CommonFileIOInterface* fileIO, int flags);
+	virtual ~BulletMJCFImporter();
+
+	virtual bool parseMJCFString(const char* xmlString, MJCFErrorLogger* logger);
+
+	virtual bool loadMJCF(const char* fileName, MJCFErrorLogger* logger, bool forceFixedBase = false);
+
+	virtual bool loadURDF(const char* fileName, bool forceFixedBase = false)
+	{
+		return false;
+	}
+
+	virtual bool loadSDF(const char* fileName, bool forceFixedBase = false) { return false; }
+
+	virtual const char* getPathPrefix();
+
+	///return >=0 for the root link index, -1 if there is no root link
+	virtual int getRootLinkIndex() const;
+
+	///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range)
+	virtual std::string getLinkName(int linkIndex) const;
+
+	virtual std::string getBodyName() const;
+
+	/// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise
+	virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const;
+	bool getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const;
+
+	//optional method to get collision group (type) and mask (affinity)
+	virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const;
+
+	///this API will likely change, don't override it!
+	virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo) const;
+
+	virtual std::string getJointName(int linkIndex) const;
+
+	//fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity.
+	virtual void getMassAndInertia(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
+
+	///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed
+	virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray<int>& childLinkIndices) const;
+
+	virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const;
+	virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const;
+
+	virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const;
+
+	virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
+
+	virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const;
+	virtual void setBodyUniqueId(int bodyId);
+	virtual int getBodyUniqueId() const;
+
+	virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;
+	virtual int getNumAllocatedCollisionShapes() const;
+	virtual class btCollisionShape* getAllocatedCollisionShape(int index);
+	virtual int getNumModels() const;
+	virtual void activateModel(int modelIndex);
+
+	virtual int getNumAllocatedMeshInterfaces() const;
+	virtual btStridingMeshInterface* getAllocatedMeshInterface(int index);
+};
+
+#endif  //BULLET_MJCF_IMPORTER_H

Engine/lib/bullet/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp

@@ -0,0 +1,373 @@
+
+#include "ImportMJCFSetup.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+//#define TEST_MULTIBODY_SERIALIZATION 1
+
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "Bullet3Common/b3FileUtils.h"
+
+#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include "../../Utils/b3ResourcePath.h"
+#include "../../Utils/b3BulletDefaultFileIO.h"
+#include "../CommonInterfaces/CommonMultiBodyBase.h"
+
+#include "../ImportURDFDemo/MyMultiBodyCreator.h"
+#include "BulletMJCFImporter.h"
+#include "../ImportURDFDemo/URDF2Bullet.h"
+
+class ImportMJCFSetup : public CommonMultiBodyBase
+{
+	char m_fileName[1024];
+
+	struct ImportMJCFInternalData* m_data;
+	bool m_useMultiBody;
+	btAlignedObjectArray<std::string*> m_nameMemory;
+	btScalar m_grav;
+	int m_upAxis;
+
+public:
+	ImportMJCFSetup(struct GUIHelperInterface* helper, int option, const char* fileName);
+	virtual ~ImportMJCFSetup();
+
+	virtual void initPhysics();
+	virtual void stepSimulation(float deltaTime);
+
+	void setFileName(const char* mjcfFileName);
+
+	virtual void resetCamera()
+	{
+		float dist = 3.5;
+		float pitch = -28;
+		float yaw = -136;
+		float targetPos[3] = {0.47, 0, -0.64};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+};
+
+static btAlignedObjectArray<std::string> gMCFJFileNameArray;
+
+#define MAX_NUM_MOTORS 1024
+
+struct ImportMJCFInternalData
+{
+	ImportMJCFInternalData()
+		: m_numMotors(0),
+		  m_mb(0)
+	{
+		for (int i = 0; i < MAX_NUM_MOTORS; i++)
+		{
+			m_jointMotors[i] = 0;
+			m_generic6DofJointMotors[i] = 0;
+		}
+	}
+
+	btScalar m_motorTargetPositions[MAX_NUM_MOTORS];
+	btMultiBodyJointMotor* m_jointMotors[MAX_NUM_MOTORS];
+	btGeneric6DofSpring2Constraint* m_generic6DofJointMotors[MAX_NUM_MOTORS];
+	int m_numMotors;
+	btMultiBody* m_mb;
+	btRigidBody* m_rb;
+};
+
+ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option, const char* fileName)
+	: CommonMultiBodyBase(helper),
+	  m_grav(-10),
+	  m_upAxis(2)
+{
+	m_data = new ImportMJCFInternalData;
+
+	m_useMultiBody = true;
+
+	static int count = 0;
+	if (fileName)
+	{
+		setFileName(fileName);
+	}
+	else
+	{
+		gMCFJFileNameArray.clear();
+
+		//load additional MJCF file names from file
+
+		FILE* f = fopen("mjcf_files.txt", "r");
+		if (f)
+		{
+			int result;
+			//warning: we don't avoid string buffer overflow in this basic example in fscanf
+			char fileName[1024];
+			do
+			{
+				result = fscanf(f, "%s", fileName);
+				b3Printf("mjcf_files.txt entry %s", fileName);
+				if (result == 1)
+				{
+					gMCFJFileNameArray.push_back(fileName);
+				}
+			} while (result == 1);
+
+			fclose(f);
+		}
+
+		if (gMCFJFileNameArray.size() == 0)
+		{
+			gMCFJFileNameArray.push_back("mjcf/humanoid.xml");
+
+			gMCFJFileNameArray.push_back("MPL/MPL.xml");
+
+			gMCFJFileNameArray.push_back("mjcf/inverted_pendulum.xml");
+			gMCFJFileNameArray.push_back("mjcf/ant.xml");
+			gMCFJFileNameArray.push_back("mjcf/hello_mjcf.xml");
+
+			gMCFJFileNameArray.push_back("mjcf/cylinder.xml");
+			gMCFJFileNameArray.push_back("mjcf/cylinder_fromtoX.xml");
+			gMCFJFileNameArray.push_back("mjcf/cylinder_fromtoY.xml");
+			gMCFJFileNameArray.push_back("mjcf/cylinder_fromtoZ.xml");
+
+			gMCFJFileNameArray.push_back("mjcf/capsule.xml");
+			gMCFJFileNameArray.push_back("mjcf/capsule_fromtoX.xml");
+			gMCFJFileNameArray.push_back("mjcf/capsule_fromtoY.xml");
+			gMCFJFileNameArray.push_back("mjcf/capsule_fromtoZ.xml");
+
+			gMCFJFileNameArray.push_back("mjcf/hopper.xml");
+			gMCFJFileNameArray.push_back("mjcf/swimmer.xml");
+			gMCFJFileNameArray.push_back("mjcf/reacher.xml");
+		}
+
+		int numFileNames = gMCFJFileNameArray.size();
+
+		if (count >= numFileNames)
+		{
+			count = 0;
+		}
+		sprintf(m_fileName, "%s", gMCFJFileNameArray[count++].c_str());
+	}
+}
+
+ImportMJCFSetup::~ImportMJCFSetup()
+{
+	for (int i = 0; i < m_nameMemory.size(); i++)
+	{
+		delete m_nameMemory[i];
+	}
+	m_nameMemory.clear();
+	delete m_data;
+}
+
+void ImportMJCFSetup::setFileName(const char* mjcfFileName)
+{
+	memcpy(m_fileName, mjcfFileName, strlen(mjcfFileName) + 1);
+}
+
+struct MyMJCFLogger : public MJCFErrorLogger
+{
+	virtual void reportError(const char* error)
+	{
+		b3Error(error);
+	}
+	virtual void reportWarning(const char* warning)
+	{
+		b3Warning(warning);
+	}
+	virtual void printMessage(const char* msg)
+	{
+		b3Printf(msg);
+	}
+};
+
+void ImportMJCFSetup::initPhysics()
+{
+	m_guiHelper->setUpAxis(m_upAxis);
+
+	createEmptyDynamicsWorld();
+
+	//MuJoCo uses a slightly different collision filter mode, use the FILTER_GROUPAMASKB_OR_GROUPBMASKA2
+	//@todo also use the modified collision filter for raycast and other collision related queries
+	m_filterCallback->m_filterMode = FILTER_GROUPAMASKB_OR_GROUPBMASKA2;
+
+	//m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+	m_dynamicsWorld->getDebugDrawer()->setDebugMode(
+		btIDebugDraw::DBG_DrawConstraints + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb);  //+btIDebugDraw::DBG_DrawConstraintLimits);
+
+	if (m_guiHelper->getParameterInterface())
+	{
+		SliderParams slider("Gravity", &m_grav);
+		slider.m_minVal = -10;
+		slider.m_maxVal = 10;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+	}
+
+	int flags = 0;
+	b3BulletDefaultFileIO fileIO;
+	BulletMJCFImporter importer(m_guiHelper, 0, &fileIO, flags);
+	MyMJCFLogger logger;
+	bool result = importer.loadMJCF(m_fileName, &logger);
+	if (result)
+	{
+		btTransform rootTrans;
+		rootTrans.setIdentity();
+
+		for (int m = 0; m < importer.getNumModels(); m++)
+		{
+			importer.activateModel(m);
+
+			// normally used with PhysicsServerCommandProcessor that allocates unique ids to multibodies,
+			// emulate this behavior here:
+			importer.setBodyUniqueId(m);
+
+			btMultiBody* mb = 0;
+
+			//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
+			//int rootLinkIndex = importer.getRootLinkIndex();
+			//b3Printf("urdf root link index = %d\n",rootLinkIndex);
+			MyMultiBodyCreator creation(m_guiHelper);
+
+			rootTrans.setIdentity();
+			importer.getRootTransformInWorld(rootTrans);
+
+			ConvertURDF2Bullet(importer, creation, rootTrans, m_dynamicsWorld, m_useMultiBody, importer.getPathPrefix(), CUF_USE_MJCF);
+
+			mb = creation.getBulletMultiBody();
+			if (mb)
+			{
+				std::string* name =
+					new std::string(importer.getLinkName(
+						importer.getRootLinkIndex()));
+				m_nameMemory.push_back(name);
+#ifdef TEST_MULTIBODY_SERIALIZATION
+				s->registerNameForPointer(name->c_str(), name->c_str());
+#endif  //TEST_MULTIBODY_SERIALIZATION
+				mb->setBaseName(name->c_str());
+				mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
+
+				//create motors for each btMultiBody joint
+				int numLinks = mb->getNumLinks();
+				for (int i = 0; i < numLinks; i++)
+				{
+					int mbLinkIndex = i;
+					int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];
+
+					std::string* jointName = new std::string(importer.getJointName(urdfLinkIndex));
+					std::string* linkName = new std::string(importer.getLinkName(urdfLinkIndex).c_str());
+#ifdef TEST_MULTIBODY_SERIALIZATION
+					s->registerNameForPointer(jointName->c_str(), jointName->c_str());
+					s->registerNameForPointer(linkName->c_str(), linkName->c_str());
+#endif  //TEST_MULTIBODY_SERIALIZATION
+					m_nameMemory.push_back(jointName);
+					m_nameMemory.push_back(linkName);
+					mb->getLinkCollider(i)->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
+
+					mb->getLink(i).m_linkName = linkName->c_str();
+					mb->getLink(i).m_jointName = jointName->c_str();
+					m_data->m_mb = mb;
+					if (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute || mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::ePrismatic)
+					{
+						if (m_data->m_numMotors < MAX_NUM_MOTORS)
+						{
+							char motorName[1024];
+							sprintf(motorName, "%s q ", jointName->c_str());
+							btScalar* motorPos = &m_data->m_motorTargetPositions[m_data->m_numMotors];
+							*motorPos = 0.f;
+							SliderParams slider(motorName, motorPos);
+							slider.m_minVal = -4;
+							slider.m_maxVal = 4;
+							slider.m_clampToIntegers = false;
+							slider.m_clampToNotches = false;
+							m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+							float maxMotorImpulse = 5.f;
+							btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb, mbLinkIndex, 0, 0, maxMotorImpulse);
+							motor->setErp(0.1);
+							//motor->setMaxAppliedImpulse(0);
+							m_data->m_jointMotors[m_data->m_numMotors] = motor;
+							m_dynamicsWorld->addMultiBodyConstraint(motor);
+							m_data->m_numMotors++;
+						}
+					}
+				}
+			}
+			else
+			{
+				// not multibody
+				if (1)
+				{
+					//create motors for each generic joint
+					int num6Dof = creation.getNum6DofConstraints();
+					for (int i = 0; i < num6Dof; i++)
+					{
+						btGeneric6DofSpring2Constraint* c = creation.get6DofConstraint(i);
+						if (c->getUserConstraintPtr())
+						{
+							GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)c->getUserConstraintPtr();
+							if ((jointInfo->m_urdfJointType == URDFRevoluteJoint) ||
+								(jointInfo->m_urdfJointType == URDFPrismaticJoint) ||
+								(jointInfo->m_urdfJointType == URDFContinuousJoint))
+							{
+								int urdfLinkIndex = jointInfo->m_urdfIndex;
+								std::string jointName = importer.getJointName(urdfLinkIndex);
+								char motorName[1024];
+								sprintf(motorName, "%s q'", jointName.c_str());
+								btScalar* motorVel = &m_data->m_motorTargetPositions[m_data->m_numMotors];
+
+								*motorVel = 0.f;
+								SliderParams slider(motorName, motorVel);
+								slider.m_minVal = -4;
+								slider.m_maxVal = 4;
+								m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+								m_data->m_generic6DofJointMotors[m_data->m_numMotors] = c;
+								bool motorOn = true;
+								c->enableMotor(jointInfo->m_jointAxisIndex, motorOn);
+								c->setMaxMotorForce(jointInfo->m_jointAxisIndex, 10000);
+								c->setTargetVelocity(jointInfo->m_jointAxisIndex, 0);
+
+								m_data->m_numMotors++;
+							}
+						}
+					}
+				}
+			}
+		}
+		m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+	}
+}
+
+void ImportMJCFSetup::stepSimulation(float deltaTime)
+{
+	if (m_dynamicsWorld)
+	{
+		btVector3 gravity(0, 0, -10);
+		gravity[m_upAxis] = m_grav;
+		m_dynamicsWorld->setGravity(gravity);
+
+		for (int i = 0; i < m_data->m_numMotors; i++)
+		{
+			if (m_data->m_jointMotors[i])
+			{
+				btScalar pos = m_data->m_motorTargetPositions[i];
+
+				int link = m_data->m_jointMotors[i]->getLinkA();
+				btScalar lowerLimit = m_data->m_mb->getLink(link).m_jointLowerLimit;
+				btScalar upperLimit = m_data->m_mb->getLink(link).m_jointUpperLimit;
+				if (lowerLimit < upperLimit)
+				{
+					btClamp(pos, lowerLimit, upperLimit);
+				}
+				m_data->m_jointMotors[i]->setPositionTarget(pos);
+			}
+			if (m_data->m_generic6DofJointMotors[i])
+			{
+				GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)m_data->m_generic6DofJointMotors[i]->getUserConstraintPtr();
+				m_data->m_generic6DofJointMotors[i]->setTargetVelocity(jointInfo->m_jointAxisIndex, m_data->m_motorTargetPositions[i]);
+			}
+		}
+
+		//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
+		m_dynamicsWorld->stepSimulation(deltaTime, 10, 1. / 240.);
+	}
+}
+
+class CommonExampleInterface* ImportMJCFCreateFunc(struct CommonExampleOptions& options)
+{
+	return new ImportMJCFSetup(options.m_guiHelper, options.m_option, options.m_fileName);
+}

Engine/lib/bullet/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.h

@@ -0,0 +1,6 @@
+#ifndef IMPORT_MJCF_SETUP_H
+#define IMPORT_MJCF_SETUP_H
+
+class CommonExampleInterface* ImportMJCFCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //IMPORT_MJCF_SETUP_H

Engine/lib/bullet/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp

@@ -0,0 +1,197 @@
+#include "b3ImportMeshUtility.h"
+
+#include <vector>
+#include "../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
+#include "LinearMath/btVector3.h"
+#include "../ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h"
+#include "../../Utils/b3ResourcePath.h"
+#include "Bullet3Common/b3FileUtils.h"
+#include "stb_image/stb_image.h"
+#include "../ImportObjDemo/LoadMeshFromObj.h"
+#include "Bullet3Common/b3HashMap.h"
+#include "../../CommonInterfaces/CommonFileIOInterface.h"
+
+struct CachedTextureResult
+{
+	std::string m_textureName;
+
+	int m_width;
+	int m_height;
+	unsigned char* m_pixels;
+	CachedTextureResult()
+		: m_width(0),
+		  m_height(0),
+		  m_pixels(0)
+	{
+	}
+};
+
+static b3HashMap<b3HashString, CachedTextureResult> gCachedTextureResults;
+struct CachedTextureManager
+{
+	CachedTextureManager()
+	{
+	}
+	virtual ~CachedTextureManager()
+	{
+		for (int i = 0; i < gCachedTextureResults.size(); i++)
+		{
+			CachedTextureResult* res = gCachedTextureResults.getAtIndex(i);
+			if (res)
+			{
+				free(res->m_pixels);
+			}
+		}
+	}
+};
+static CachedTextureManager sTexCacheMgr;
+
+bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData, struct CommonFileIOInterface* fileIO)
+{
+	B3_PROFILE("loadAndRegisterMeshFromFileInternal");
+	meshData.m_gfxShape = 0;
+	meshData.m_textureImage1 = 0;
+	meshData.m_textureHeight = 0;
+	meshData.m_textureWidth = 0;
+	meshData.m_flags = 0;
+	meshData.m_isCached = false;
+
+	char relativeFileName[1024];
+	if (fileIO->findResourcePath(fileName.c_str(), relativeFileName, 1024))
+	{
+		char pathPrefix[1024];
+
+		b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
+		btVector3 shift(0, 0, 0);
+
+		std::vector<bt_tinyobj::shape_t> shapes;
+		bt_tinyobj::attrib_t attribute;
+		{
+			B3_PROFILE("tinyobj::LoadObj");
+			std::string err = LoadFromCachedOrFromObj(attribute, shapes, relativeFileName, pathPrefix, fileIO);
+			//std::string err = tinyobj::LoadObj(shapes, relativeFileName, pathPrefix);
+		}
+
+		GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(attribute, shapes);
+		{
+			B3_PROFILE("Load Texture");
+			//int textureIndex = -1;
+			//try to load some texture
+			for (int i = 0; meshData.m_textureImage1 == 0 && i < shapes.size(); i++)
+			{
+				const bt_tinyobj::shape_t& shape = shapes[i];
+				meshData.m_rgbaColor[0] = shape.material.diffuse[0];
+				meshData.m_rgbaColor[1] = shape.material.diffuse[1];
+				meshData.m_rgbaColor[2] = shape.material.diffuse[2];
+				meshData.m_rgbaColor[3] = shape.material.transparency;
+				meshData.m_flags |= B3_IMPORT_MESH_HAS_RGBA_COLOR;
+
+				meshData.m_specularColor[0] = shape.material.specular[0];
+				meshData.m_specularColor[1] = shape.material.specular[1];
+				meshData.m_specularColor[2] = shape.material.specular[2];
+				meshData.m_specularColor[3] = 1;
+				meshData.m_flags |= B3_IMPORT_MESH_HAS_SPECULAR_COLOR;
+				
+				if (shape.material.diffuse_texname.length() > 0)
+				{
+					int width, height, n;
+					const char* filename = shape.material.diffuse_texname.c_str();
+					unsigned char* image = 0;
+
+					const char* prefix[] = {pathPrefix, "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"};
+					int numprefix = sizeof(prefix) / sizeof(const char*);
+
+					for (int i = 0; !image && i < numprefix; i++)
+					{
+						char relativeFileName[1024];
+						sprintf(relativeFileName, "%s%s", prefix[i], filename);
+						char relativeFileName2[1024];
+						if (fileIO->findResourcePath(relativeFileName, relativeFileName2, 1024))
+						{
+							if (b3IsFileCachingEnabled())
+							{
+								CachedTextureResult* texture = gCachedTextureResults[relativeFileName];
+								if (texture)
+								{
+									image = texture->m_pixels;
+									width = texture->m_width;
+									height = texture->m_height;
+									meshData.m_textureWidth = width;
+									meshData.m_textureHeight = height;
+									meshData.m_textureImage1 = image;
+									meshData.m_isCached = true;
+								}
+							}
+
+							if (image == 0)
+							{
+
+								b3AlignedObjectArray<char> buffer;
+								buffer.reserve(1024);
+								int fileId = fileIO->fileOpen(relativeFileName,"rb");
+								if (fileId>=0)
+								{
+									int size = fileIO->getFileSize(fileId);
+									if (size>0)
+									{
+										buffer.resize(size);
+										int actual = fileIO->fileRead(fileId,&buffer[0],size);
+										if (actual != size)
+										{
+											b3Warning("STL filesize mismatch!\n");
+											buffer.resize(0);
+										}
+									}
+									fileIO->fileClose(fileId);
+								}
+
+								if (buffer.size())
+								{
+									image = stbi_load_from_memory((const unsigned char*)&buffer[0], buffer.size(), &width, &height, &n, 3);
+								}
+								//image = stbi_load(relativeFileName, &width, &height, &n, 3);
+
+								meshData.m_textureImage1 = image;
+
+								if (image)
+								{
+									meshData.m_textureWidth = width;
+									meshData.m_textureHeight = height;
+
+									if (b3IsFileCachingEnabled())
+									{
+										CachedTextureResult result;
+										result.m_textureName = relativeFileName;
+										result.m_width = width;
+										result.m_height = height;
+										result.m_pixels = image;
+										meshData.m_isCached = true;
+										gCachedTextureResults.insert(relativeFileName, result);
+									}
+								}
+								else
+								{
+									b3Warning("Unsupported texture image format [%s]\n", relativeFileName);
+
+									break;
+								}
+							}
+						}
+						else
+						{
+							b3Warning("not found [%s]\n", relativeFileName);
+						}
+					}
+				}
+			}
+		}
+		meshData.m_gfxShape = gfxShape;
+		return true;
+	}
+	else
+	{
+		b3Warning("Cannot find %s\n", fileName.c_str());
+	}
+
+	return false;
+}

Engine/lib/bullet/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h

@@ -0,0 +1,42 @@
+#ifndef B3_IMPORT_MESH_UTILITY_H
+#define B3_IMPORT_MESH_UTILITY_H
+
+#include <string>
+
+enum b3ImportMeshDataFlags
+{
+	B3_IMPORT_MESH_HAS_RGBA_COLOR=1,
+	B3_IMPORT_MESH_HAS_SPECULAR_COLOR=2,
+};
+
+struct b3ImportMeshData
+{
+	struct GLInstanceGraphicsShape* m_gfxShape;
+
+	unsigned char* m_textureImage1;  //in 3 component 8-bit RGB data
+	bool m_isCached;
+	int m_textureWidth;
+	int m_textureHeight;
+	double m_rgbaColor[4];
+	double m_specularColor[4];
+	int m_flags;
+
+	b3ImportMeshData()
+		:m_gfxShape(0),
+		m_textureImage1(0),
+		m_isCached(false),
+		m_textureWidth(0),
+		m_textureHeight(0),
+		m_flags(0)
+	{
+	}
+
+};
+
+class b3ImportMeshUtility
+{
+public:
+	static bool loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData, struct CommonFileIOInterface* fileIO);
+};
+
+#endif  //B3_IMPORT_MESH_UTILITY_H

Engine/lib/bullet/examples/Importers/ImportObjDemo/ImportObjExample.cpp

@@ -0,0 +1,112 @@
+#include "ImportObjExample.h"
+#include <vector>
+#include "../OpenGLWindow/GLInstancingRenderer.h"
+#include "Wavefront/tiny_obj_loader.h"
+#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
+#include "btBulletDynamicsCommon.h"
+#include "../OpenGLWindow/SimpleOpenGL3App.h"
+#include "Wavefront2GLInstanceGraphicsShape.h"
+#include "../../Utils/b3ResourcePath.h"
+#include "../../Utils/b3BulletDefaultFileIO.h"
+#include "Bullet3Common/b3FileUtils.h"
+
+#include "stb_image/stb_image.h"
+
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../ImportMeshUtility/b3ImportMeshUtility.h"
+
+class ImportObjSetup : public CommonRigidBodyBase
+{
+	std::string m_fileName;
+
+public:
+	ImportObjSetup(struct GUIHelperInterface* helper, const char* fileName);
+	virtual ~ImportObjSetup();
+
+	virtual void initPhysics();
+
+	virtual void resetCamera()
+	{
+		float dist = 18;
+		float pitch = -46;
+		float yaw = 120;
+		float targetPos[3] = {-2, -2, -2};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+};
+
+ImportObjSetup::ImportObjSetup(struct GUIHelperInterface* helper, const char* fileName)
+	: CommonRigidBodyBase(helper)
+{
+	if (fileName)
+	{
+		m_fileName = fileName;
+	}
+	else
+	{
+		m_fileName = "cube.obj";  //"sponza_closed.obj";//sphere8.obj";
+	}
+}
+
+ImportObjSetup::~ImportObjSetup()
+{
+}
+
+int loadAndRegisterMeshFromFile2(const std::string& fileName, CommonRenderInterface* renderer)
+{
+	int shapeId = -1;
+
+	b3ImportMeshData meshData;
+	b3BulletDefaultFileIO fileIO;
+	if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData,&fileIO))
+	{
+		int textureIndex = -1;
+
+		if (meshData.m_textureImage1)
+		{
+			textureIndex = renderer->registerTexture(meshData.m_textureImage1, meshData.m_textureWidth, meshData.m_textureHeight);
+		}
+
+		shapeId = renderer->registerShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0],
+										  meshData.m_gfxShape->m_numvertices,
+										  &meshData.m_gfxShape->m_indices->at(0),
+										  meshData.m_gfxShape->m_numIndices,
+										  B3_GL_TRIANGLES,
+										  textureIndex);
+		delete meshData.m_gfxShape;
+		if (!meshData.m_isCached)
+		{
+			delete meshData.m_textureImage1;
+		}
+	}
+	return shapeId;
+}
+
+void ImportObjSetup::initPhysics()
+{
+	m_guiHelper->setUpAxis(2);
+	this->createEmptyDynamicsWorld();
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+	m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
+
+	btTransform trans;
+	trans.setIdentity();
+	trans.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI));
+	btVector3 position = trans.getOrigin();
+	btQuaternion orn = trans.getRotation();
+
+	btVector3 scaling(1, 1, 1);
+	btVector4 color(1, 1, 1,1);
+
+	int shapeId = loadAndRegisterMeshFromFile2(m_fileName, m_guiHelper->getRenderInterface());
+	if (shapeId >= 0)
+	{
+		//int id =
+		m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId, position, orn, color, scaling);
+	}
+}
+
+CommonExampleInterface* ImportObjCreateFunc(struct CommonExampleOptions& options)
+{
+	return new ImportObjSetup(options.m_guiHelper, options.m_fileName);
+}

Engine/lib/bullet/examples/Importers/ImportObjDemo/ImportObjExample.h

@@ -0,0 +1,6 @@
+#ifndef IMPORT_OBJ_EXAMPLE_H
+#define IMPORT_OBJ_EXAMPLE_H
+
+class CommonExampleInterface* ImportObjCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //IMPORT_OBJ_EXAMPLE_H

Engine/lib/bullet/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp

@@ -0,0 +1,77 @@
+#include "LoadMeshFromObj.h"
+
+#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
+#include <stdio.h>  //fopen
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include <string>
+#include <vector>
+#include "Wavefront2GLInstanceGraphicsShape.h"
+#include "Bullet3Common/b3HashMap.h"
+
+struct CachedObjResult
+{
+	std::string m_msg;
+	std::vector<bt_tinyobj::shape_t> m_shapes;
+	bt_tinyobj::attrib_t m_attribute;
+};
+
+static b3HashMap<b3HashString, CachedObjResult> gCachedObjResults;
+static int gEnableFileCaching = 1;
+
+int b3IsFileCachingEnabled()
+{
+	return gEnableFileCaching;
+}
+void b3EnableFileCaching(int enable)
+{
+	gEnableFileCaching = enable;
+	if (enable == 0)
+	{
+		gCachedObjResults.clear();
+	}
+}
+
+std::string LoadFromCachedOrFromObj(
+	bt_tinyobj::attrib_t& attribute,
+	std::vector<bt_tinyobj::shape_t>& shapes,  // [output]
+	const char* filename,
+	const char* mtl_basepath,
+	struct CommonFileIOInterface* fileIO)
+{
+	CachedObjResult* resultPtr = gCachedObjResults[filename];
+	if (resultPtr)
+	{
+		const CachedObjResult& result = *resultPtr;
+		shapes = result.m_shapes;
+		attribute = result.m_attribute;
+		return result.m_msg;
+	}
+
+	std::string err = bt_tinyobj::LoadObj(attribute, shapes, filename, mtl_basepath, fileIO);
+	CachedObjResult result;
+	result.m_msg = err;
+	result.m_shapes = shapes;
+	result.m_attribute = attribute;
+	if (gEnableFileCaching)
+	{
+		gCachedObjResults.insert(filename, result);
+	}
+	return err;
+}
+
+GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath, struct CommonFileIOInterface* fileIO)
+{
+	B3_PROFILE("LoadMeshFromObj");
+	std::vector<bt_tinyobj::shape_t> shapes;
+	bt_tinyobj::attrib_t attribute;
+	{
+		B3_PROFILE("bt_tinyobj::LoadObj2");
+		std::string err = LoadFromCachedOrFromObj(attribute, shapes, relativeFileName, materialPrefixPath, fileIO);
+	}
+
+	{
+		B3_PROFILE("btgCreateGraphicsShapeFromWavefrontObj");
+		GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(attribute, shapes);
+		return gfxShape;
+	}
+}

Engine/lib/bullet/examples/Importers/ImportObjDemo/LoadMeshFromObj.h

@@ -0,0 +1,20 @@
+#ifndef LOAD_MESH_FROM_OBJ_H
+#define LOAD_MESH_FROM_OBJ_H
+
+struct GLInstanceGraphicsShape;
+
+#include "../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
+
+int b3IsFileCachingEnabled();
+void b3EnableFileCaching(int enable);
+
+std::string LoadFromCachedOrFromObj(
+	bt_tinyobj::attrib_t& attribute,
+	std::vector<bt_tinyobj::shape_t>& shapes,  // [output]
+	const char* filename,
+	const char* mtl_basepath,
+	struct CommonFileIOInterface* fileIO);
+
+GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath,struct CommonFileIOInterface* fileIO);
+
+#endif  //LOAD_MESH_FROM_OBJ_H

Engine/lib/bullet/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp

@@ -0,0 +1,200 @@
+#include "Wavefront2GLInstanceGraphicsShape.h"
+
+#include "../../OpenGLWindow/GLInstancingRenderer.h"
+#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
+//#include "btBulletDynamicsCommon.h"
+#include "LinearMath/btVector3.h"
+#include "../../OpenGLWindow/SimpleOpenGL3App.h"
+#include "Wavefront2GLInstanceGraphicsShape.h"
+#include "../../OpenGLWindow/GLInstancingRenderer.h"
+#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
+
+GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const bt_tinyobj::attrib_t& attribute, std::vector<bt_tinyobj::shape_t>& shapes, bool flatShading)
+{
+	b3AlignedObjectArray<GLInstanceVertex>* vertices = new b3AlignedObjectArray<GLInstanceVertex>;
+	{
+		//		int numVertices = obj->vertexCount;
+		//	int numIndices = 0;
+		b3AlignedObjectArray<int>* indicesPtr = new b3AlignedObjectArray<int>;
+
+		for (int s = 0; s < (int)shapes.size(); s++)
+		{
+			bt_tinyobj::shape_t& shape = shapes[s];
+			int faceCount = shape.mesh.indices.size();
+
+			for (int f = 0; f < faceCount; f += 3)
+			{
+				//btVector3 normal(face.m_plane[0],face.m_plane[1],face.m_plane[2]);
+				if (1)
+				{
+					btVector3 normal(0, 1, 0);
+					int vtxBaseIndex = vertices->size();
+
+					if (f < 0 && f >= int(shape.mesh.indices.size()))
+					{
+						continue;
+					}
+
+					GLInstanceVertex vtx0;
+					bt_tinyobj::index_t v_0 = shape.mesh.indices[f];
+					vtx0.xyzw[0] = attribute.vertices[3 * v_0.vertex_index];
+					vtx0.xyzw[1] = attribute.vertices[3 * v_0.vertex_index + 1];
+					vtx0.xyzw[2] = attribute.vertices[3 * v_0.vertex_index + 2];
+					vtx0.xyzw[3] = 0.f;
+
+					if (attribute.texcoords.size())
+					{
+						int uv0Index = 2 * v_0.texcoord_index;
+						int uv1Index = 2 * v_0.texcoord_index + 1;
+						if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < int(attribute.texcoords.size()) && (uv1Index < attribute.texcoords.size())))
+						{
+							vtx0.uv[0] = attribute.texcoords[uv0Index];
+							vtx0.uv[1] = attribute.texcoords[uv1Index];
+						}
+						else
+						{
+							//	b3Warning("obj texture coordinate out-of-range!");
+							vtx0.uv[0] = 0;
+							vtx0.uv[1] = 0;
+						}
+					}
+					else
+					{
+						vtx0.uv[0] = 0.5;
+						vtx0.uv[1] = 0.5;
+					}
+
+					GLInstanceVertex vtx1;
+					bt_tinyobj::index_t v_1 = shape.mesh.indices[f + 1];
+					vtx1.xyzw[0] = attribute.vertices[3 * v_1.vertex_index];
+					vtx1.xyzw[1] = attribute.vertices[3 * v_1.vertex_index + 1];
+					vtx1.xyzw[2] = attribute.vertices[3 * v_1.vertex_index + 2];
+					vtx1.xyzw[3] = 0.f;
+
+					if (attribute.texcoords.size())
+					{
+						int uv0Index = 2 * v_1.texcoord_index;
+						int uv1Index = 2 * v_1.texcoord_index + 1;
+						if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < attribute.texcoords.size()) && (uv1Index < attribute.texcoords.size()))
+						{
+							vtx1.uv[0] = attribute.texcoords[uv0Index];
+							vtx1.uv[1] = attribute.texcoords[uv1Index];
+						}
+						else
+						{
+							//	b3Warning("obj texture coordinate out-of-range!");
+							vtx1.uv[0] = 0;
+							vtx1.uv[1] = 0;
+						}
+					}
+					else
+					{
+						vtx1.uv[0] = 0.5f;
+						vtx1.uv[1] = 0.5f;
+					}
+
+					GLInstanceVertex vtx2;
+					bt_tinyobj::index_t v_2 = shape.mesh.indices[f + 2];
+					vtx2.xyzw[0] = attribute.vertices[3 * v_2.vertex_index];
+					vtx2.xyzw[1] = attribute.vertices[3 * v_2.vertex_index + 1];
+					vtx2.xyzw[2] = attribute.vertices[3 * v_2.vertex_index + 2];
+					vtx2.xyzw[3] = 0.f;
+					if (attribute.texcoords.size())
+					{
+						int uv0Index = 2 * v_2.texcoord_index;
+						int uv1Index = 2 * v_2.texcoord_index + 1;
+
+						if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < attribute.texcoords.size()) && (uv1Index < attribute.texcoords.size()))
+						{
+							vtx2.uv[0] = attribute.texcoords[uv0Index];
+							vtx2.uv[1] = attribute.texcoords[uv1Index];
+						}
+						else
+						{
+							//b3Warning("obj texture coordinate out-of-range!");
+							vtx2.uv[0] = 0;
+							vtx2.uv[1] = 0;
+						}
+					}
+					else
+					{
+						vtx2.uv[0] = 0.5;
+						vtx2.uv[1] = 0.5;
+					}
+
+					btVector3 v0(vtx0.xyzw[0], vtx0.xyzw[1], vtx0.xyzw[2]);
+					btVector3 v1(vtx1.xyzw[0], vtx1.xyzw[1], vtx1.xyzw[2]);
+					btVector3 v2(vtx2.xyzw[0], vtx2.xyzw[1], vtx2.xyzw[2]);
+
+					unsigned int maxIndex = 0;
+					unsigned n0Index = shape.mesh.indices[f].normal_index;
+					unsigned n1Index = shape.mesh.indices[f + 1].normal_index;
+					unsigned n2Index = shape.mesh.indices[f + 2].normal_index;
+
+					maxIndex = b3Max(maxIndex, 3 * n0Index + 0);
+					maxIndex = b3Max(maxIndex, 3 * n0Index + 1);
+					maxIndex = b3Max(maxIndex, 3 * n0Index + 2);
+					maxIndex = b3Max(maxIndex, 3 * n1Index + 0);
+					maxIndex = b3Max(maxIndex, 3 * n1Index + 1);
+					maxIndex = b3Max(maxIndex, 3 * n1Index + 2);
+					maxIndex = b3Max(maxIndex, 3 * n2Index + 0);
+					maxIndex = b3Max(maxIndex, 3 * n2Index + 1);
+					maxIndex = b3Max(maxIndex, 3 * n2Index + 2);
+
+					bool hasNormals = (attribute.normals.size() && maxIndex < attribute.normals.size());
+
+					if (flatShading || !hasNormals)
+					{
+						normal = (v1 - v0).cross(v2 - v0);
+						btScalar len2 = normal.length2();
+						//skip degenerate triangles
+						if (len2 > SIMD_EPSILON)
+						{
+							normal.normalize();
+						}
+						else
+						{
+							normal.setValue(0, 0, 0);
+						}
+						vtx0.normal[0] = normal[0];
+						vtx0.normal[1] = normal[1];
+						vtx0.normal[2] = normal[2];
+						vtx1.normal[0] = normal[0];
+						vtx1.normal[1] = normal[1];
+						vtx1.normal[2] = normal[2];
+						vtx2.normal[0] = normal[0];
+						vtx2.normal[1] = normal[1];
+						vtx2.normal[2] = normal[2];
+					}
+					else
+					{
+						vtx0.normal[0] = attribute.normals[3 * n0Index+ 0];
+						vtx0.normal[1] = attribute.normals[3 * n0Index+ 1];
+						vtx0.normal[2] = attribute.normals[3 * n0Index+ 2];
+						vtx1.normal[0] = attribute.normals[3 * n1Index+ 0];
+						vtx1.normal[1] = attribute.normals[3 * n1Index+ 1];
+						vtx1.normal[2] = attribute.normals[3 * n1Index+ 2];
+						vtx2.normal[0] = attribute.normals[3 * n2Index+ 0];
+						vtx2.normal[1] = attribute.normals[3 * n2Index+ 1];
+						vtx2.normal[2] = attribute.normals[3 * n2Index+ 2];
+					}
+					vertices->push_back(vtx0);
+					vertices->push_back(vtx1);
+					vertices->push_back(vtx2);
+					indicesPtr->push_back(vtxBaseIndex);
+					indicesPtr->push_back(vtxBaseIndex + 1);
+					indicesPtr->push_back(vtxBaseIndex + 2);
+				}
+			}
+		}
+
+		GLInstanceGraphicsShape* gfxShape = new GLInstanceGraphicsShape;
+		gfxShape->m_vertices = vertices;
+		gfxShape->m_numvertices = vertices->size();
+		gfxShape->m_indices = indicesPtr;
+		gfxShape->m_numIndices = indicesPtr->size();
+		for (int i = 0; i < 4; i++)
+			gfxShape->m_scaling[i] = 1;  //bake the scaling into the vertices
+		return gfxShape;
+	}
+}

Engine/lib/bullet/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h

@@ -0,0 +1,9 @@
+#ifndef WAVEFRONT2GRAPHICS_H
+#define WAVEFRONT2GRAPHICS_H
+
+#include "../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
+#include <vector>
+
+struct GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const bt_tinyobj::attrib_t& attribute, std::vector<bt_tinyobj::shape_t>& shapes, bool flatShading = false);
+
+#endif  //WAVEFRONT2GRAPHICS_H

Engine/lib/bullet/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp

@@ -0,0 +1,301 @@
+
+#include "ImportSDFSetup.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "Bullet3Common/b3FileUtils.h"
+
+#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include "../../Utils/b3ResourcePath.h"
+
+#include "../ImportURDFDemo/BulletUrdfImporter.h"
+
+#include "../ImportURDFDemo/URDF2Bullet.h"
+
+//#include "urdf_samples.h"
+
+#include "../CommonInterfaces/CommonMultiBodyBase.h"
+
+#include "../ImportURDFDemo/MyMultiBodyCreator.h"
+
+class ImportSDFSetup : public CommonMultiBodyBase
+{
+	char m_fileName[1024];
+
+	struct ImportSDFInternalData* m_data;
+	bool m_useMultiBody;
+
+	//todo(erwincoumans) we need a name memory for each model
+	btAlignedObjectArray<std::string*> m_nameMemory;
+
+public:
+	ImportSDFSetup(struct GUIHelperInterface* helper, int option, const char* fileName);
+	virtual ~ImportSDFSetup();
+
+	virtual void initPhysics();
+	virtual void stepSimulation(float deltaTime);
+
+	void setFileName(const char* urdfFileName);
+
+	virtual void resetCamera()
+	{
+		float dist = 3.5;
+		float pitch = -28;
+		float yaw = -136;
+		float targetPos[3] = {0.47, 0, -0.64};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+};
+
+static btAlignedObjectArray<std::string> gFileNameArray;
+
+#define MAX_NUM_MOTORS 1024
+
+struct ImportSDFInternalData
+{
+	ImportSDFInternalData()
+		: m_numMotors(0)
+	{
+		for (int i = 0; i < MAX_NUM_MOTORS; i++)
+		{
+			m_jointMotors[i] = 0;
+			m_generic6DofJointMotors[i] = 0;
+		}
+	}
+
+	btScalar m_motorTargetVelocities[MAX_NUM_MOTORS];
+	btMultiBodyJointMotor* m_jointMotors[MAX_NUM_MOTORS];
+	btGeneric6DofSpring2Constraint* m_generic6DofJointMotors[MAX_NUM_MOTORS];
+	int m_numMotors;
+};
+
+ImportSDFSetup::ImportSDFSetup(struct GUIHelperInterface* helper, int option, const char* fileName)
+	: CommonMultiBodyBase(helper)
+{
+	m_data = new ImportSDFInternalData;
+	(void)option;
+
+	//	if (option==1)
+	//	{
+	m_useMultiBody = true;
+	//
+	//	} else
+	//	{
+	//		m_useMultiBody = false;
+	//	}
+
+	static int count = 0;
+	if (fileName)
+	{
+		setFileName(fileName);
+	}
+	else
+	{
+		gFileNameArray.clear();
+
+		//load additional urdf file names from file
+
+		FILE* f = fopen("sdf_files.txt", "r");
+		if (f)
+		{
+			int result;
+			//warning: we don't avoid string buffer overflow in this basic example in fscanf
+			char fileName[1024];
+			do
+			{
+				result = fscanf(f, "%s", fileName);
+				b3Printf("sdf_files.txt entry %s", fileName);
+				if (result == 1)
+				{
+					gFileNameArray.push_back(fileName);
+				}
+			} while (result == 1);
+
+			fclose(f);
+		}
+
+		if (gFileNameArray.size() == 0)
+		{
+			gFileNameArray.push_back("two_cubes.sdf");
+		}
+
+		int numFileNames = gFileNameArray.size();
+
+		if (count >= numFileNames)
+		{
+			count = 0;
+		}
+		sprintf(m_fileName, "%s", gFileNameArray[count++].c_str());
+	}
+}
+
+ImportSDFSetup::~ImportSDFSetup()
+{
+	for (int i = 0; i < m_nameMemory.size(); i++)
+	{
+		delete m_nameMemory[i];
+	}
+	m_nameMemory.clear();
+	delete m_data;
+}
+
+void ImportSDFSetup::setFileName(const char* urdfFileName)
+{
+	memcpy(m_fileName, urdfFileName, strlen(urdfFileName) + 1);
+}
+
+void ImportSDFSetup::initPhysics()
+{
+	int upAxis = 2;
+	m_guiHelper->setUpAxis(upAxis);
+
+	this->createEmptyDynamicsWorld();
+	//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+	m_dynamicsWorld->getDebugDrawer()->setDebugMode(
+		btIDebugDraw::DBG_DrawConstraints + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb);  //+btIDebugDraw::DBG_DrawConstraintLimits);
+
+	btVector3 gravity(0, 0, 0);
+	gravity[upAxis] = -9.8;
+
+	m_dynamicsWorld->setGravity(gravity);
+
+	BulletURDFImporter u2b(m_guiHelper, 0, 0, 1, 0);
+
+	bool loadOk = u2b.loadSDF(m_fileName);
+
+	if (loadOk)
+	{
+		//printTree(u2b,u2b.getRootLinkIndex());
+
+		//u2b.printTree();
+
+		btTransform rootTrans;
+		rootTrans.setIdentity();
+
+		for (int m = 0; m < u2b.getNumModels(); m++)
+		{
+			u2b.activateModel(m);
+
+			btMultiBody* mb = 0;
+
+			//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
+			//int rootLinkIndex = u2b.getRootLinkIndex();
+			//b3Printf("urdf root link index = %d\n",rootLinkIndex);
+			MyMultiBodyCreator creation(m_guiHelper);
+
+			u2b.getRootTransformInWorld(rootTrans);
+			ConvertURDF2Bullet(u2b, creation, rootTrans, m_dynamicsWorld, m_useMultiBody, u2b.getPathPrefix(), CUF_USE_SDF);
+			mb = creation.getBulletMultiBody();
+
+			if (m_useMultiBody && mb)
+			{
+				std::string* name = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
+				m_nameMemory.push_back(name);
+#ifdef TEST_MULTIBODY_SERIALIZATION
+				s->registerNameForPointer(name->c_str(), name->c_str());
+#endif  //TEST_MULTIBODY_SERIALIZATION
+				mb->setBaseName(name->c_str());
+				//create motors for each btMultiBody joint
+				int numLinks = mb->getNumLinks();
+				for (int i = 0; i < numLinks; i++)
+				{
+					int mbLinkIndex = i;
+					int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];
+
+					std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex));
+					std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
+#ifdef TEST_MULTIBODY_SERIALIZATION
+					s->registerNameForPointer(jointName->c_str(), jointName->c_str());
+					s->registerNameForPointer(linkName->c_str(), linkName->c_str());
+#endif  //TEST_MULTIBODY_SERIALIZATION
+					m_nameMemory.push_back(jointName);
+					m_nameMemory.push_back(linkName);
+
+					mb->getLink(i).m_linkName = linkName->c_str();
+					mb->getLink(i).m_jointName = jointName->c_str();
+
+					if (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute || mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::ePrismatic)
+					{
+						if (m_data->m_numMotors < MAX_NUM_MOTORS)
+						{
+							char motorName[1024];
+							sprintf(motorName, "%s q'", jointName->c_str());
+							btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
+							*motorVel = 0.f;
+							SliderParams slider(motorName, motorVel);
+							slider.m_minVal = -4;
+							slider.m_maxVal = 4;
+							m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+							float maxMotorImpulse = 10.1f;
+							btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb, mbLinkIndex, 0, 0, maxMotorImpulse);
+							//motor->setMaxAppliedImpulse(0);
+							m_data->m_jointMotors[m_data->m_numMotors] = motor;
+							m_dynamicsWorld->addMultiBodyConstraint(motor);
+							m_data->m_numMotors++;
+						}
+					}
+				}
+			}
+		}
+
+		for (int i = 0; i < m_dynamicsWorld->getNumMultiBodyConstraints(); i++)
+		{
+			m_dynamicsWorld->getMultiBodyConstraint(i)->finalizeMultiDof();
+		}
+
+		bool createGround = true;
+		if (createGround)
+		{
+			btVector3 groundHalfExtents(20, 20, 20);
+			groundHalfExtents[upAxis] = 1.f;
+			btBoxShape* box = new btBoxShape(groundHalfExtents);
+			box->initializePolyhedralFeatures();
+
+			m_guiHelper->createCollisionShapeGraphicsObject(box);
+			btTransform start;
+			start.setIdentity();
+			btVector3 groundOrigin(0, 0, 0);
+			groundOrigin[upAxis] = -2.5;
+			start.setOrigin(groundOrigin);
+			btRigidBody* body = createRigidBody(0, start, box);
+			//m_dynamicsWorld->removeRigidBody(body);
+			// m_dynamicsWorld->addRigidBody(body,2,1);
+			btVector3 color(0.5, 0.5, 0.5);
+			m_guiHelper->createRigidBodyGraphicsObject(body, color);
+		}
+
+		///this extra stepSimulation call makes sure that all the btMultibody transforms are properly propagates.
+		m_dynamicsWorld->stepSimulation(1. / 240., 0);  // 1., 10, 1. / 240.);
+	}
+}
+
+void ImportSDFSetup::stepSimulation(float deltaTime)
+{
+	if (m_dynamicsWorld)
+	{
+		for (int i = 0; i < m_data->m_numMotors; i++)
+		{
+			if (m_data->m_jointMotors[i])
+			{
+				m_data->m_jointMotors[i]->setVelocityTarget(m_data->m_motorTargetVelocities[i]);
+			}
+			if (m_data->m_generic6DofJointMotors[i])
+			{
+				GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)m_data->m_generic6DofJointMotors[i]->getUserConstraintPtr();
+				m_data->m_generic6DofJointMotors[i]->setTargetVelocity(jointInfo->m_jointAxisIndex, m_data->m_motorTargetVelocities[i]);
+				//jointInfo->
+			}
+		}
+
+		//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
+		m_dynamicsWorld->stepSimulation(deltaTime, 10, 1. / 240.);
+	}
+}
+
+class CommonExampleInterface* ImportSDFCreateFunc(struct CommonExampleOptions& options)
+{
+	return new ImportSDFSetup(options.m_guiHelper, options.m_option, options.m_fileName);
+}

Engine/lib/bullet/examples/Importers/ImportSDFDemo/ImportSDFSetup.h

@@ -0,0 +1,6 @@
+#ifndef IMPORT_SDF_SETUP_H
+#define IMPORT_SDF_SETUP_H
+
+class CommonExampleInterface* ImportSDFCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //IMPORT_SDF_SETUP_H

Engine/lib/bullet/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp

@@ -0,0 +1,90 @@
+#include "ImportSTLSetup.h"
+#include <vector>
+#include "../OpenGLWindow/GLInstancingRenderer.h"
+#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
+#include "btBulletDynamicsCommon.h"
+#include "../OpenGLWindow/SimpleOpenGL3App.h"
+#include "LoadMeshFromSTL.h"
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../../Utils/b3ResourcePath.h"
+#include "../../Utils/b3BulletDefaultFileIO.h"
+
+class ImportSTLSetup : public CommonRigidBodyBase
+{
+	const char* m_fileName;
+	btVector3 m_scaling;
+
+public:
+	ImportSTLSetup(struct GUIHelperInterface* helper, const char* fileName);
+	virtual ~ImportSTLSetup();
+
+	virtual void initPhysics();
+	virtual void resetCamera()
+	{
+		float dist = 3.5;
+		float pitch = -28;
+		float yaw = -136;
+		float targetPos[3] = {0.47, 0, -0.64};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+};
+
+ImportSTLSetup::ImportSTLSetup(struct GUIHelperInterface* helper, const char* fileName)
+	: CommonRigidBodyBase(helper),
+	  m_scaling(btVector3(10, 10, 10))
+{
+	if (fileName)
+	{
+		m_fileName = fileName;
+		m_scaling = btVector3(0.01, 0.01, 0.01);
+	}
+	else
+	{
+		m_fileName = "l_finger_tip.stl";
+	}
+}
+
+ImportSTLSetup::~ImportSTLSetup()
+{
+}
+
+void ImportSTLSetup::initPhysics()
+{
+	m_guiHelper->setUpAxis(2);
+	this->createEmptyDynamicsWorld();
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+	m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
+
+	char relativeFileName[1024];
+	if (!b3ResourcePath::findResourcePath(m_fileName, relativeFileName, 1024,0))
+	{
+		b3Warning("Cannot find file %s\n", m_fileName);
+		return;
+	}
+
+	btVector3 shift(0, 0, 0);
+	//	int index=10;
+
+	{
+		b3BulletDefaultFileIO fileIO;
+		GLInstanceGraphicsShape* gfxShape = LoadMeshFromSTL(relativeFileName,&fileIO);
+
+		btTransform trans;
+		trans.setIdentity();
+		trans.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI));
+
+		btVector3 position = trans.getOrigin();
+		btQuaternion orn = trans.getRotation();
+
+		btVector4 color(0, 0, 1,1);
+
+		int shapeId = m_guiHelper->getRenderInterface()->registerShape(&gfxShape->m_vertices->at(0).xyzw[0], gfxShape->m_numvertices, &gfxShape->m_indices->at(0), gfxShape->m_numIndices);
+
+		m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId, position, orn, color, m_scaling);
+	}
+}
+
+class CommonExampleInterface* ImportSTLCreateFunc(struct CommonExampleOptions& options)
+{
+	return new ImportSTLSetup(options.m_guiHelper, options.m_fileName);
+}

Engine/lib/bullet/examples/Importers/ImportSTLDemo/ImportSTLSetup.h

@@ -0,0 +1,6 @@
+#ifndef IMPORT_STL_SETUP_H
+#define IMPORT_STL_SETUP_H
+
+class CommonExampleInterface* ImportSTLCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //IMPORT_OBJ_SETUP_H

Engine/lib/bullet/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h

@@ -0,0 +1,107 @@
+#ifndef LOAD_MESH_FROM_STL_H
+#define LOAD_MESH_FROM_STL_H
+
+#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
+#include <stdio.h>  //fopen
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "../../CommonInterfaces/CommonFileIOInterface.h"
+#include <string.h> //memcpy
+struct MySTLTriangle
+{
+	float normal[3];
+	float vertex0[3];
+	float vertex1[3];
+	float vertex2[3];
+};
+
+static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName, struct CommonFileIOInterface* fileIO)
+{
+	GLInstanceGraphicsShape* shape = 0;
+
+	int fileHandle = fileIO->fileOpen(relativeFileName, "rb");
+	if (fileHandle>=0)
+	{
+		int size = 0;
+		size = fileIO->getFileSize(fileHandle);
+		{
+			if (size>=0)
+			{
+				//b3Warning("Open STL file of %d bytes\n",size);
+				char* memoryBuffer = new char[size + 1];
+				int actualBytesRead = fileIO->fileRead(fileHandle, memoryBuffer, size);
+				if (actualBytesRead != size)
+				{
+					b3Warning("Error reading from file %s", relativeFileName);
+				}
+				else
+				{
+					int numTriangles = *(int*)&memoryBuffer[80];
+
+					if (numTriangles)
+					{
+						{
+							//perform a sanity check instead of crashing on invalid triangles/STL files
+							int expectedBinaryFileSize = numTriangles * 50 + 84;
+							if (expectedBinaryFileSize != size)
+							{
+								delete[] memoryBuffer;
+								fileIO->fileClose(fileHandle);
+								return 0;
+							}
+						}
+						shape = new GLInstanceGraphicsShape;
+						//						b3AlignedObjectArray<GLInstanceVertex>*	m_vertices;
+						//						int				m_numvertices;
+						//						b3AlignedObjectArray<int>* 		m_indices;
+						//						int				m_numIndices;
+						//						float			m_scaling[4];
+						shape->m_scaling[0] = 1;
+						shape->m_scaling[1] = 1;
+						shape->m_scaling[2] = 1;
+						shape->m_scaling[3] = 1;
+						int index = 0;
+						shape->m_indices = new b3AlignedObjectArray<int>();
+						shape->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
+						for (int i = 0; i < numTriangles; i++)
+						{
+							char* curPtr = &memoryBuffer[84 + i * 50];
+							MySTLTriangle tmp;
+							memcpy(&tmp, curPtr, sizeof(MySTLTriangle));
+
+							GLInstanceVertex v0, v1, v2;
+							v0.uv[0] = v1.uv[0] = v2.uv[0] = 0.5;
+							v0.uv[1] = v1.uv[1] = v2.uv[1] = 0.5;
+							for (int v = 0; v < 3; v++)
+							{
+								v0.xyzw[v] = tmp.vertex0[v];
+								v1.xyzw[v] = tmp.vertex1[v];
+								v2.xyzw[v] = tmp.vertex2[v];
+								v0.normal[v] = v1.normal[v] = v2.normal[v] = tmp.normal[v];
+							}
+							v0.xyzw[3] = v1.xyzw[3] = v2.xyzw[3] = 0.f;
+
+							shape->m_vertices->push_back(v0);
+							shape->m_vertices->push_back(v1);
+							shape->m_vertices->push_back(v2);
+
+							shape->m_indices->push_back(index++);
+							shape->m_indices->push_back(index++);
+							shape->m_indices->push_back(index++);
+						}
+					}
+				}
+
+				delete[] memoryBuffer;
+			}
+		}
+		fileIO->fileClose(fileHandle);
+	}
+	if (shape)
+	{
+		shape->m_numIndices = shape->m_indices->size();
+		shape->m_numvertices = shape->m_vertices->size();
+	}
+	return shape;
+}
+
+#endif  //LOAD_MESH_FROM_STL_H

Engine/lib/bullet/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h

@@ -0,0 +1,99 @@
+#ifndef BULLET_URDF_IMPORTER_H
+#define BULLET_URDF_IMPORTER_H
+
+#include "URDFImporterInterface.h"
+#include "UrdfRenderingInterface.h"
+#include "UrdfParser.h"
+
+struct BulletURDFTexture
+{
+	int m_width;
+	int m_height;
+	unsigned char* textureData1;
+	bool m_isCached;
+};
+
+///BulletURDFImporter can deal with URDF and (soon) SDF files
+class BulletURDFImporter : public URDFImporterInterface
+{
+	struct BulletURDFInternalData* m_data;
+
+public:
+	BulletURDFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, struct CommonFileIOInterface* fileIO=0,double globalScaling=1, int flags=0);
+
+	virtual ~BulletURDFImporter();
+
+	virtual bool loadURDF(const char* fileName, bool forceFixedBase = false);
+
+	//warning: some quick test to load SDF: we 'activate' a model, so we can re-use URDF code path
+	virtual bool loadSDF(const char* fileName, bool forceFixedBase = false);
+	virtual int getNumModels() const;
+	virtual void activateModel(int modelIndex);
+	virtual void setBodyUniqueId(int bodyId);
+	virtual int getBodyUniqueId() const;
+	const char* getPathPrefix();
+
+	void printTree();  //for debugging
+
+	virtual int getRootLinkIndex() const;
+
+	virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const;
+
+	virtual std::string getBodyName() const;
+
+	virtual std::string getLinkName(int linkIndex) const;
+
+	virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const;
+
+	virtual bool getLinkColor2(int linkIndex, UrdfMaterialColor& matCol) const;
+
+	virtual void setLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const;
+
+	virtual bool getLinkContactInfo(int urdflinkIndex, URDFLinkContactInfo& contactInfo) const;
+
+	virtual bool getLinkAudioSource(int linkIndex, SDFAudioSource& audioSource) const;
+
+	virtual std::string getJointName(int linkIndex) const;
+
+	virtual void getMassAndInertia(int linkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
+	virtual void getMassAndInertia2(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame, int flags) const;
+
+	virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const;
+	virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const;
+	virtual bool getJointInfo3(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity, btScalar& twistLimit) const;
+
+	virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const;
+	virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld);
+
+	virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
+
+	virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const;
+
+	class btCollisionShape* convertURDFToCollisionShape(const struct UrdfCollision* collision, const char* urdfPathPrefix) const;
+
+	virtual int getUrdfFromCollisionShape(const btCollisionShape* collisionShape, UrdfCollision& collision) const;
+
+	///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation
+
+	virtual const struct UrdfModel* getUrdfModel() const;
+
+	virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;
+
+	virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const;
+
+	virtual int getNumAllocatedCollisionShapes() const;
+	virtual class btCollisionShape* getAllocatedCollisionShape(int index);
+
+	virtual int getNumAllocatedMeshInterfaces() const;
+	virtual class btStridingMeshInterface* getAllocatedMeshInterface(int index);
+
+	virtual int getNumAllocatedTextures() const;
+	virtual int getAllocatedTexture(int index) const;
+
+	virtual void setEnableTinyRenderer(bool enable);
+	void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const class btTransform& visualTransform, btAlignedObjectArray<struct GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<struct BulletURDFTexture>& texturesOut, struct b3ImportMeshData& meshData) const;
+	const struct UrdfDeformable& getDeformableModel() const;
+	const struct UrdfReducedDeformable& getReducedDeformableModel() const;
+};
+
+#endif  //BULLET_URDF_IMPORTER_H

Engine/lib/bullet/examples/Importers/ImportURDFDemo/ConvertRigidBodies2MultiBody.h

@@ -0,0 +1,13 @@
+#ifndef CONVERT_RIGIDBODIES_2_MULTIBODY_H
+#define CONVERT_RIGIDBODIES_2_MULTIBODY_H
+
+struct ConvertRigidBodies2MultiBody
+{
+	btAlignedObjectArray<btRigidBody*> m_bodies;
+	btAlignedObjectArray<btTypedConstraint*> m_constraints;
+
+	virtual void addRigidBody(btRigidBody* body);
+	virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies = false);
+	virtual btMultiBody* convertToMultiBody();
+};
+#endif  //CONVERT_RIGIDBODIES_2_MULTIBODY_H

Engine/lib/bullet/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp

@@ -0,0 +1,371 @@
+
+#include "ImportURDFSetup.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+//#define TEST_MULTIBODY_SERIALIZATION 1
+
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "Bullet3Common/b3FileUtils.h"
+
+#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include "../../Utils/b3ResourcePath.h"
+
+#include "BulletUrdfImporter.h"
+
+#include "URDF2Bullet.h"
+
+//#include "urdf_samples.h"
+
+#include "../CommonInterfaces/CommonMultiBodyBase.h"
+
+#include "MyMultiBodyCreator.h"
+
+class ImportUrdfSetup : public CommonMultiBodyBase
+{
+	char m_fileName[1024];
+
+	struct ImportUrdfInternalData* m_data;
+	bool m_useMultiBody;
+	btAlignedObjectArray<std::string*> m_nameMemory;
+	btScalar m_grav;
+	int m_upAxis;
+
+public:
+	ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName);
+	virtual ~ImportUrdfSetup();
+
+	virtual void initPhysics();
+	virtual void stepSimulation(float deltaTime);
+
+	void setFileName(const char* urdfFileName);
+
+	virtual void resetCamera()
+	{
+		float dist = 3.5;
+		float pitch = -28;
+		float yaw = -136;
+		float targetPos[3] = {0.47, 0, -0.64};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+};
+
+btAlignedObjectArray<std::string> gFileNameArray;
+
+#define MAX_NUM_MOTORS 1024
+
+struct ImportUrdfInternalData
+{
+	ImportUrdfInternalData()
+		: m_numMotors(0),
+		  m_mb(0)
+	{
+		for (int i = 0; i < MAX_NUM_MOTORS; i++)
+		{
+			m_jointMotors[i] = 0;
+			m_generic6DofJointMotors[i] = 0;
+		}
+	}
+
+	btScalar m_motorTargetVelocities[MAX_NUM_MOTORS];
+	btMultiBodyJointMotor* m_jointMotors[MAX_NUM_MOTORS];
+	btGeneric6DofSpring2Constraint* m_generic6DofJointMotors[MAX_NUM_MOTORS];
+	int m_numMotors;
+	btMultiBody* m_mb;
+	btRigidBody* m_rb;
+};
+
+ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName)
+	: CommonMultiBodyBase(helper),
+	  m_grav(-10),
+	  m_upAxis(2)
+{
+	m_data = new ImportUrdfInternalData;
+
+	if (option == 1)
+	{
+		m_useMultiBody = true;
+	}
+	else
+	{
+		m_useMultiBody = false;
+	}
+
+	static int count = 0;
+	if (fileName)
+	{
+		setFileName(fileName);
+	}
+	else
+	{
+		gFileNameArray.clear();
+
+		//load additional urdf file names from file
+
+		FILE* f = fopen("urdf_files.txt", "r");
+		if (f)
+		{
+			int result;
+			//warning: we don't avoid string buffer overflow in this basic example in fscanf
+			char fileName[1024];
+			do
+			{
+				result = fscanf(f, "%s", fileName);
+				b3Printf("urdf_files.txt entry %s", fileName);
+				if (result == 1)
+				{
+					gFileNameArray.push_back(fileName);
+				}
+			} while (result == 1);
+
+			fclose(f);
+		}
+
+		if (gFileNameArray.size() == 0)
+		{
+			gFileNameArray.push_back("r2d2.urdf");
+		}
+
+		int numFileNames = gFileNameArray.size();
+
+		if (count >= numFileNames)
+		{
+			count = 0;
+		}
+		sprintf(m_fileName, "%s", gFileNameArray[count++].c_str());
+	}
+}
+
+ImportUrdfSetup::~ImportUrdfSetup()
+{
+	for (int i = 0; i < m_nameMemory.size(); i++)
+	{
+		delete m_nameMemory[i];
+	}
+	m_nameMemory.clear();
+	delete m_data;
+}
+
+void ImportUrdfSetup::setFileName(const char* urdfFileName)
+{
+	memcpy(m_fileName, urdfFileName, strlen(urdfFileName) + 1);
+}
+
+void ImportUrdfSetup::initPhysics()
+{
+	m_guiHelper->setUpAxis(m_upAxis);
+
+	this->createEmptyDynamicsWorld();
+	//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+	m_dynamicsWorld->getDebugDrawer()->setDebugMode(
+		btIDebugDraw::DBG_DrawConstraints + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb);  //+btIDebugDraw::DBG_DrawConstraintLimits);
+
+	if (m_guiHelper->getParameterInterface())
+	{
+		SliderParams slider("Gravity", &m_grav);
+		slider.m_minVal = -10;
+		slider.m_maxVal = 10;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+	}
+
+	int flags = 0;
+	double globalScaling = 1;
+
+	BulletURDFImporter u2b(m_guiHelper, 0, 0, globalScaling, flags);
+
+	bool loadOk = u2b.loadURDF(m_fileName);
+
+#ifdef TEST_MULTIBODY_SERIALIZATION
+	//test to serialize a multibody to disk or shared memory, with base, link and joint names
+	btSerializer* s = new btDefaultSerializer;
+#endif  //TEST_MULTIBODY_SERIALIZATION
+
+	if (loadOk)
+	{
+		//printTree(u2b,u2b.getRootLinkIndex());
+
+		//u2b.printTree();
+
+		btTransform identityTrans;
+		identityTrans.setIdentity();
+
+		{
+			//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
+			//int rootLinkIndex = u2b.getRootLinkIndex();
+			//b3Printf("urdf root link index = %d\n",rootLinkIndex);
+			MyMultiBodyCreator creation(m_guiHelper);
+
+			ConvertURDF2Bullet(u2b, creation, identityTrans, m_dynamicsWorld, m_useMultiBody, u2b.getPathPrefix());
+			m_data->m_rb = creation.getRigidBody();
+			m_data->m_mb = creation.getBulletMultiBody();
+			btMultiBody* mb = m_data->m_mb;
+			for (int i = 0; i < u2b.getNumAllocatedCollisionShapes(); i++)
+			{
+				m_collisionShapes.push_back(u2b.getAllocatedCollisionShape(i));
+			}
+
+			if (m_useMultiBody && mb)
+			{
+				std::string* name = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
+				m_nameMemory.push_back(name);
+#ifdef TEST_MULTIBODY_SERIALIZATION
+				s->registerNameForPointer(name->c_str(), name->c_str());
+#endif  //TEST_MULTIBODY_SERIALIZATION
+				mb->setBaseName(name->c_str());
+				//create motors for each btMultiBody joint
+				int numLinks = mb->getNumLinks();
+				for (int i = 0; i < numLinks; i++)
+				{
+					int mbLinkIndex = i;
+					int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];
+
+					std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex));
+					std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
+#ifdef TEST_MULTIBODY_SERIALIZATION
+					s->registerNameForPointer(jointName->c_str(), jointName->c_str());
+					s->registerNameForPointer(linkName->c_str(), linkName->c_str());
+#endif  //TEST_MULTIBODY_SERIALIZATION
+					m_nameMemory.push_back(jointName);
+					m_nameMemory.push_back(linkName);
+
+					mb->getLink(i).m_linkName = linkName->c_str();
+					mb->getLink(i).m_jointName = jointName->c_str();
+
+					if (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute || mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::ePrismatic)
+					{
+						if (m_data->m_numMotors < MAX_NUM_MOTORS)
+						{
+							char motorName[1024];
+							sprintf(motorName, "%s q'", jointName->c_str());
+							btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
+							*motorVel = 0.f;
+							SliderParams slider(motorName, motorVel);
+							slider.m_minVal = -4;
+							slider.m_maxVal = 4;
+							m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+							float maxMotorImpulse = 10.1f;
+							btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb, mbLinkIndex, 0, 0, maxMotorImpulse);
+							//motor->setMaxAppliedImpulse(0);
+							m_data->m_jointMotors[m_data->m_numMotors] = motor;
+							m_dynamicsWorld->addMultiBodyConstraint(motor);
+							m_data->m_numMotors++;
+						}
+					}
+				}
+			}
+			else
+			{
+				if (1)
+				{
+					//create motors for each generic joint
+					int num6Dof = creation.getNum6DofConstraints();
+					for (int i = 0; i < num6Dof; i++)
+					{
+						btGeneric6DofSpring2Constraint* c = creation.get6DofConstraint(i);
+						if (c->getUserConstraintPtr())
+						{
+							GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)c->getUserConstraintPtr();
+							if ((jointInfo->m_urdfJointType == URDFRevoluteJoint) ||
+								(jointInfo->m_urdfJointType == URDFPrismaticJoint) ||
+								(jointInfo->m_urdfJointType == URDFContinuousJoint))
+							{
+								int urdfLinkIndex = jointInfo->m_urdfIndex;
+								std::string jointName = u2b.getJointName(urdfLinkIndex);
+								char motorName[1024];
+								sprintf(motorName, "%s q'", jointName.c_str());
+								btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
+
+								*motorVel = 0.f;
+								SliderParams slider(motorName, motorVel);
+								slider.m_minVal = -4;
+								slider.m_maxVal = 4;
+								m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+								m_data->m_generic6DofJointMotors[m_data->m_numMotors] = c;
+								bool motorOn = true;
+								c->enableMotor(jointInfo->m_jointAxisIndex, motorOn);
+								c->setMaxMotorForce(jointInfo->m_jointAxisIndex, 10000);
+								c->setTargetVelocity(jointInfo->m_jointAxisIndex, 0);
+
+								m_data->m_numMotors++;
+							}
+						}
+					}
+				}
+			}
+		}
+
+		//the btMultiBody support is work-in-progress :-)
+
+		for (int i = 0; i < m_dynamicsWorld->getNumMultiBodyConstraints(); i++)
+		{
+			m_dynamicsWorld->getMultiBodyConstraint(i)->finalizeMultiDof();
+		}
+
+		bool createGround = true;
+		if (createGround)
+		{
+			btVector3 groundHalfExtents(20, 20, 20);
+			groundHalfExtents[m_upAxis] = 1.f;
+			btBoxShape* box = new btBoxShape(groundHalfExtents);
+			m_collisionShapes.push_back(box);
+			box->initializePolyhedralFeatures();
+
+			m_guiHelper->createCollisionShapeGraphicsObject(box);
+			btTransform start;
+			start.setIdentity();
+			btVector3 groundOrigin(0, 0, 0);
+			groundOrigin[m_upAxis] = -2.5;
+			start.setOrigin(groundOrigin);
+			btRigidBody* body = createRigidBody(0, start, box);
+			//m_dynamicsWorld->removeRigidBody(body);
+			// m_dynamicsWorld->addRigidBody(body,2,1);
+			btVector3 color(0.5, 0.5, 0.5);
+			m_guiHelper->createRigidBodyGraphicsObject(body, color);
+		}
+	}
+
+#ifdef TEST_MULTIBODY_SERIALIZATION
+	m_dynamicsWorld->serialize(s);
+	b3ResourcePath p;
+	char resourcePath[1024];
+	if (p.findResourcePath("r2d2_multibody.bullet", resourcePath, 1024))
+	{
+		FILE* f = fopen(resourcePath, "wb");
+		fwrite(s->getBufferPointer(), s->getCurrentBufferSize(), 1, f);
+		fclose(f);
+	}
+#endif  //TEST_MULTIBODY_SERIALIZATION
+}
+
+void ImportUrdfSetup::stepSimulation(float deltaTime)
+{
+	if (m_dynamicsWorld)
+	{
+		btVector3 gravity(0, 0, 0);
+		gravity[m_upAxis] = m_grav;
+		m_dynamicsWorld->setGravity(gravity);
+
+		for (int i = 0; i < m_data->m_numMotors; i++)
+		{
+			if (m_data->m_jointMotors[i])
+			{
+				m_data->m_jointMotors[i]->setVelocityTarget(m_data->m_motorTargetVelocities[i]);
+			}
+			if (m_data->m_generic6DofJointMotors[i])
+			{
+				GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)m_data->m_generic6DofJointMotors[i]->getUserConstraintPtr();
+				m_data->m_generic6DofJointMotors[i]->setTargetVelocity(jointInfo->m_jointAxisIndex, m_data->m_motorTargetVelocities[i]);
+				//jointInfo->
+			}
+		}
+
+		//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
+		m_dynamicsWorld->stepSimulation(deltaTime, 10, 1. / 240.);
+	}
+}
+
+class CommonExampleInterface* ImportURDFCreateFunc(struct CommonExampleOptions& options)
+{
+	return new ImportUrdfSetup(options.m_guiHelper, options.m_option, options.m_fileName);
+}

Engine/lib/bullet/examples/Importers/ImportURDFDemo/ImportURDFSetup.h

@@ -0,0 +1,6 @@
+#ifndef IMPORT_URDF_SETUP_H
+#define IMPORT_URDF_SETUP_H
+
+class CommonExampleInterface* ImportURDFCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //IMPORT_URDF_SETUP_H

Engine/lib/bullet/examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h

@@ -0,0 +1,42 @@
+#ifndef MULTIBODY_CREATION_INTERFACE_H
+#define MULTIBODY_CREATION_INTERFACE_H
+
+#include "LinearMath/btTransform.h"
+
+class MultiBodyCreationInterface
+{
+public:
+	virtual ~MultiBodyCreationInterface() {}
+
+	virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) = 0;
+	virtual void createRigidBodyGraphicsInstance2(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, const btVector3& specularColor, int graphicsIndex)
+	{
+		createRigidBodyGraphicsInstance(linkIndex, body, colorRgba, graphicsIndex);
+	}
+
+	///optionally create some graphical representation from a collision object, usually for visual debugging purposes.
+	virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba) = 0;
+	virtual void createCollisionObjectGraphicsInstance2(int linkIndex, class btCollisionObject* col, const btVector4& colorRgba, const btVector3& specularColor)
+	{
+		createCollisionObjectGraphicsInstance(linkIndex, col, colorRgba);
+	}
+
+	virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints, btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep) = 0;
+
+	virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape) = 0;
+
+	virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder = 0) = 0;
+
+	virtual class btGeneric6DofSpring2Constraint* createPrismaticJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
+																	   const btVector3& jointAxisInJointSpace, btScalar jointLowerLimit, btScalar jointUpperLimit) = 0;
+	virtual class btGeneric6DofSpring2Constraint* createRevoluteJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
+																	  const btVector3& jointAxisInJointSpace, btScalar jointLowerLimit, btScalar jointUpperLimit) = 0;
+
+	virtual class btGeneric6DofSpring2Constraint* createFixedJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB) = 0;
+
+	virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body) = 0;
+
+	virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) = 0;
+};
+
+#endif  //MULTIBODY_CREATION_INTERFACE_H

Engine/lib/bullet/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp

@@ -0,0 +1,231 @@
+#include "MyMultiBodyCreator.h"
+
+#include "../../CommonInterfaces/CommonGUIHelperInterface.h"
+
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+
+#include "btBulletDynamicsCommon.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+#include "URDFJointTypes.h"
+
+MyMultiBodyCreator::MyMultiBodyCreator(GUIHelperInterface* guiHelper)
+	: m_bulletMultiBody(0),
+	  m_rigidBody(0),
+	  m_guiHelper(guiHelper)
+{
+}
+
+class btMultiBody* MyMultiBodyCreator::allocateMultiBody(int /* urdfLinkIndex */, int totalNumJoints, btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep)
+{
+	//	m_urdf2mbLink.resize(totalNumJoints+1,-2);
+	m_mb2urdfLink.resize(totalNumJoints + 1, -2);
+
+	m_bulletMultiBody = new btMultiBody(totalNumJoints, mass, localInertiaDiagonal, isFixedBase, canSleep);
+	//if (canSleep)
+	//	m_bulletMultiBody->goToSleep();
+	return m_bulletMultiBody;
+}
+
+class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape)
+{
+	btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
+	rbci.m_startWorldTransform = initialWorldTrans;
+	btScalar sleep_threshold = btScalar(0.22360679775);//sqrt(0.05) to be similar to btMultiBody (SLEEP_THRESHOLD)
+	rbci.m_angularSleepingThreshold = sleep_threshold;
+	rbci.m_linearSleepingThreshold = sleep_threshold;
+	
+	btRigidBody* body = new btRigidBody(rbci);
+	if (m_rigidBody == 0)
+	{
+		//only store the root of the multi body
+		m_rigidBody = body;
+	}
+	return body;
+}
+
+class btMultiBodyLinkCollider* MyMultiBodyCreator::allocateMultiBodyLinkCollider(int /*urdfLinkIndex*/, int mbLinkIndex, btMultiBody* multiBody)
+{
+	btMultiBodyLinkCollider* mbCol = new btMultiBodyLinkCollider(multiBody, mbLinkIndex);
+	return mbCol;
+}
+
+class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder)
+{
+	btGeneric6DofSpring2Constraint* c = new btGeneric6DofSpring2Constraint(rbA, rbB, offsetInA, offsetInB, (RotateOrder)rotateOrder);
+
+	return c;
+}
+
+class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createPrismaticJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
+																			   const btVector3& jointAxisInJointSpace, btScalar jointLowerLimit, btScalar jointUpperLimit)
+{
+	int rotateOrder = 0;
+	btGeneric6DofSpring2Constraint* dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA, rbB, offsetInA, offsetInB, rotateOrder);
+	//todo(erwincoumans) for now, we only support principle axis along X, Y or Z
+	int principleAxis = jointAxisInJointSpace.closestAxis();
+
+	GenericConstraintUserInfo* userInfo = new GenericConstraintUserInfo;
+	userInfo->m_jointAxisInJointSpace = jointAxisInJointSpace;
+	userInfo->m_jointAxisIndex = principleAxis;
+
+	userInfo->m_urdfJointType = URDFPrismaticJoint;
+	userInfo->m_lowerJointLimit = jointLowerLimit;
+	userInfo->m_upperJointLimit = jointUpperLimit;
+	userInfo->m_urdfIndex = urdfLinkIndex;
+	dof6->setUserConstraintPtr(userInfo);
+
+	switch (principleAxis)
+	{
+		case 0:
+		{
+			dof6->setLinearLowerLimit(btVector3(jointLowerLimit, 0, 0));
+			dof6->setLinearUpperLimit(btVector3(jointUpperLimit, 0, 0));
+			break;
+		}
+		case 1:
+		{
+			dof6->setLinearLowerLimit(btVector3(0, jointLowerLimit, 0));
+			dof6->setLinearUpperLimit(btVector3(0, jointUpperLimit, 0));
+			break;
+		}
+		case 2:
+		default:
+		{
+			dof6->setLinearLowerLimit(btVector3(0, 0, jointLowerLimit));
+			dof6->setLinearUpperLimit(btVector3(0, 0, jointUpperLimit));
+		}
+	};
+
+	dof6->setAngularLowerLimit(btVector3(0, 0, 0));
+	dof6->setAngularUpperLimit(btVector3(0, 0, 0));
+	m_6DofConstraints.push_back(dof6);
+	return dof6;
+}
+
+class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createRevoluteJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
+																			  const btVector3& jointAxisInJointSpace, btScalar jointLowerLimit, btScalar jointUpperLimit)
+{
+	btGeneric6DofSpring2Constraint* dof6 = 0;
+
+	//only handle principle axis at the moment,
+	//@todo(erwincoumans) orient the constraint for non-principal axis
+	int principleAxis = jointAxisInJointSpace.closestAxis();
+	switch (principleAxis)
+	{
+		case 0:
+		{
+			dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA, rbB, offsetInA, offsetInB, RO_ZYX);
+			dof6->setLinearLowerLimit(btVector3(0, 0, 0));
+			dof6->setLinearUpperLimit(btVector3(0, 0, 0));
+
+			dof6->setAngularLowerLimit(btVector3(jointLowerLimit, 0, 0));
+			dof6->setAngularUpperLimit(btVector3(jointUpperLimit, 0, 0));
+
+			break;
+		}
+		case 1:
+		{
+			dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA, rbB, offsetInA, offsetInB, RO_XZY);
+			dof6->setLinearLowerLimit(btVector3(0, 0, 0));
+			dof6->setLinearUpperLimit(btVector3(0, 0, 0));
+
+			dof6->setAngularLowerLimit(btVector3(0, jointLowerLimit, 0));
+			dof6->setAngularUpperLimit(btVector3(0, jointUpperLimit, 0));
+			break;
+		}
+		case 2:
+		default:
+		{
+			dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA, rbB, offsetInA, offsetInB, RO_XYZ);
+			dof6->setLinearLowerLimit(btVector3(0, 0, 0));
+			dof6->setLinearUpperLimit(btVector3(0, 0, 0));
+
+			dof6->setAngularLowerLimit(btVector3(0, 0, jointLowerLimit));
+			dof6->setAngularUpperLimit(btVector3(0, 0, jointUpperLimit));
+		}
+	};
+
+	GenericConstraintUserInfo* userInfo = new GenericConstraintUserInfo;
+	userInfo->m_jointAxisInJointSpace = jointAxisInJointSpace;
+	userInfo->m_jointAxisIndex = 3 + principleAxis;
+
+	if (jointLowerLimit > jointUpperLimit)
+	{
+		userInfo->m_urdfJointType = URDFContinuousJoint;
+	}
+	else
+	{
+		userInfo->m_urdfJointType = URDFRevoluteJoint;
+		userInfo->m_lowerJointLimit = jointLowerLimit;
+		userInfo->m_upperJointLimit = jointUpperLimit;
+	}
+	userInfo->m_urdfIndex = urdfLinkIndex;
+	dof6->setUserConstraintPtr(userInfo);
+	m_6DofConstraints.push_back(dof6);
+	return dof6;
+}
+
+class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createFixedJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB)
+{
+	btGeneric6DofSpring2Constraint* dof6 = allocateGeneric6DofSpring2Constraint(urdfLinkIndex, rbA, rbB, offsetInA, offsetInB);
+
+	GenericConstraintUserInfo* userInfo = new GenericConstraintUserInfo;
+	userInfo->m_urdfIndex = urdfLinkIndex;
+	userInfo->m_urdfJointType = URDFFixedJoint;
+
+	dof6->setUserConstraintPtr(userInfo);
+
+	dof6->setLinearLowerLimit(btVector3(0, 0, 0));
+	dof6->setLinearUpperLimit(btVector3(0, 0, 0));
+
+	dof6->setAngularLowerLimit(btVector3(0, 0, 0));
+	dof6->setAngularUpperLimit(btVector3(0, 0, 0));
+	m_6DofConstraints.push_back(dof6);
+	return dof6;
+}
+
+void MyMultiBodyCreator::addLinkMapping(int urdfLinkIndex, int mbLinkIndex)
+{
+	if (m_mb2urdfLink.size() < (mbLinkIndex + 1))
+	{
+		m_mb2urdfLink.resize((mbLinkIndex + 1), -2);
+	}
+	//    m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex;
+	m_mb2urdfLink[mbLinkIndex] = urdfLinkIndex;
+}
+
+void MyMultiBodyCreator::createRigidBodyGraphicsInstance(int linkIndex, btRigidBody* body, const btVector3& colorRgba, int graphicsIndex)
+{
+	m_guiHelper->createRigidBodyGraphicsObject(body, colorRgba);
+}
+
+void MyMultiBodyCreator::createRigidBodyGraphicsInstance2(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, const btVector3& specularColor, int graphicsIndex)
+{
+	m_guiHelper->createRigidBodyGraphicsObject(body, colorRgba);
+	int graphicsInstanceId = body->getUserIndex();
+	btVector3DoubleData speculard;
+	specularColor.serializeDouble(speculard);
+	m_guiHelper->changeSpecularColor(graphicsInstanceId, speculard.m_floats);
+}
+
+void MyMultiBodyCreator::createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* colObj, const btVector3& colorRgba)
+{
+	m_guiHelper->createCollisionObjectGraphicsObject(colObj, colorRgba);
+}
+
+void MyMultiBodyCreator::createCollisionObjectGraphicsInstance2(int linkIndex, class btCollisionObject* col, const btVector4& colorRgba, const btVector3& specularColor)
+{
+	createCollisionObjectGraphicsInstance(linkIndex, col, colorRgba);
+	int graphicsInstanceId = col->getUserIndex();
+	btVector3DoubleData speculard;
+	specularColor.serializeDouble(speculard);
+	m_guiHelper->changeSpecularColor(graphicsInstanceId, speculard.m_floats);
+}
+
+btMultiBody* MyMultiBodyCreator::getBulletMultiBody()
+{
+	return m_bulletMultiBody;
+}

Engine/lib/bullet/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h

@@ -0,0 +1,80 @@
+
+#ifndef MY_MULTIBODY_CREATOR
+#define MY_MULTIBODY_CREATOR
+
+#include "MultiBodyCreationInterface.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btHashMap.h"
+
+struct GUIHelperInterface;
+class btMultiBody;
+
+struct GenericConstraintUserInfo
+{
+	int m_urdfIndex;
+	int m_urdfJointType;
+	btVector3 m_jointAxisInJointSpace;
+	int m_jointAxisIndex;
+	btScalar m_lowerJointLimit;
+	btScalar m_upperJointLimit;
+};
+
+class MyMultiBodyCreator : public MultiBodyCreationInterface
+{
+protected:
+	btMultiBody* m_bulletMultiBody;
+	btRigidBody* m_rigidBody;
+
+	struct GUIHelperInterface* m_guiHelper;
+
+	btAlignedObjectArray<btGeneric6DofSpring2Constraint*> m_6DofConstraints;
+
+public:
+	btAlignedObjectArray<int> m_mb2urdfLink;
+
+	MyMultiBodyCreator(GUIHelperInterface* guiHelper);
+
+	virtual ~MyMultiBodyCreator() {}
+
+	virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex);
+	virtual void createRigidBodyGraphicsInstance2(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, const btVector3& specularColor, int graphicsIndex);
+
+	///optionally create some graphical representation from a collision object, usually for visual debugging purposes.
+	virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba);
+	virtual void createCollisionObjectGraphicsInstance2(int linkIndex, class btCollisionObject* col, const btVector4& colorRgba, const btVector3& specularColor);
+
+	virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints, btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep);
+
+	virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape);
+
+	virtual class btGeneric6DofSpring2Constraint* allocateGeneric6DofSpring2Constraint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB, int rotateOrder = 0);
+
+	virtual class btGeneric6DofSpring2Constraint* createPrismaticJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
+																	   const btVector3& jointAxisInJointSpace, btScalar jointLowerLimit, btScalar jointUpperLimit);
+	virtual class btGeneric6DofSpring2Constraint* createRevoluteJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB,
+																	  const btVector3& jointAxisInJointSpace, btScalar jointLowerLimit, btScalar jointUpperLimit);
+
+	virtual class btGeneric6DofSpring2Constraint* createFixedJoint(int urdfLinkIndex, btRigidBody& rbA /*parent*/, btRigidBody& rbB, const btTransform& offsetInA, const btTransform& offsetInB);
+
+	virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body);
+
+	virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex);
+
+	btMultiBody* getBulletMultiBody();
+	btRigidBody* getRigidBody()
+	{
+		return m_rigidBody;
+	}
+
+	int getNum6DofConstraints() const
+	{
+		return m_6DofConstraints.size();
+	}
+
+	btGeneric6DofSpring2Constraint* get6DofConstraint(int index)
+	{
+		return m_6DofConstraints[index];
+	}
+};
+
+#endif  //MY_MULTIBODY_CREATOR

Engine/lib/bullet/examples/Importers/ImportURDFDemo/SDFAudioTypes.h

@@ -0,0 +1,44 @@
+#ifndef SDF_AUDIO_TYPES_H
+#define SDF_AUDIO_TYPES_H
+
+#include <string>
+
+///See audio_source element in http://sdformat.org/spec?ver=1.6&elem=link
+struct SDFAudioSource
+{
+	enum
+	{
+		SDFAudioSourceValid = 1,
+		SDFAudioSourceLooping = 2,
+	};
+
+	int m_flags;  //repeat mode (0 = no repeat, 1 = loop forever)
+
+	std::string m_uri;  //media filename of the sound, .wav file
+	double m_pitch;     //1 = regular rate, -1 play in reverse
+	double m_gain;      //normalized volume in range [0..1] where 0 is silent, 1 is most loud
+
+	double m_attackRate;
+	double m_decayRate;
+	double m_sustainLevel;
+	double m_releaseRate;
+
+	double m_collisionForceThreshold;  //force that will trigger the audio, in Newton. If < 0, audio source is invalid
+
+	int m_userIndex;
+
+	SDFAudioSource()
+		: m_flags(0),
+		  m_pitch(1),
+		  m_gain(1),
+		  m_attackRate(0.0001),
+		  m_decayRate(0.00001),
+		  m_sustainLevel(0.5),
+		  m_releaseRate(0.0005),
+		  m_collisionForceThreshold(0.5),
+		  m_userIndex(-1)
+	{
+	}
+};
+
+#endif  //SDF_AUDIO_TYPES_H

Engine/lib/bullet/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -0,0 +1,897 @@
+
+#include <stdio.h>
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btThreads.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
+
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
+#include "BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.h"
+
+
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+#include "URDF2Bullet.h"
+#include "URDFImporterInterface.h"
+#include "MultiBodyCreationInterface.h"
+#include <string>
+#include "Bullet3Common/b3Logging.h"
+
+//static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter;
+//static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter);
+static bool enableConstraints = true;
+
+static btVector4 gGoogleyColors[4] =
+{
+		btVector4(60. / 256., 186. / 256., 84. / 256., 1),
+		btVector4(244. / 256., 194. / 256., 13. / 256., 1),
+		btVector4(219. / 256., 50. / 256., 54. / 256., 1),
+		btVector4(72. / 256., 133. / 256., 237. / 256., 1),
+};
+
+static btVector4 selectColor2()
+{
+#ifdef BT_THREADSAFE
+	static btSpinMutex sMutex;
+	sMutex.lock();
+#endif
+	static int curColor = 0;
+	btVector4 color = gGoogleyColors[curColor];
+	curColor++;
+	curColor &= 3;
+#ifdef BT_THREADSAFE
+	sMutex.unlock();
+#endif
+	return color;
+}
+
+struct URDF2BulletCachedData
+{
+	URDF2BulletCachedData()
+		: m_currentMultiBodyLinkIndex(-1),
+		  m_bulletMultiBody(0),
+		  m_totalNumJoints1(0)
+	{
+	}
+	//these arrays will be initialized in the 'InitURDF2BulletCache'
+
+	btAlignedObjectArray<int> m_urdfLinkParentIndices;
+	btAlignedObjectArray<int> m_urdfLinkIndices2BulletLinkIndices;
+	btAlignedObjectArray<class btRigidBody*> m_urdfLink2rigidBodies;
+	btAlignedObjectArray<btTransform> m_urdfLinkLocalInertialFrames;
+
+	int m_currentMultiBodyLinkIndex;
+
+	class btMultiBody* m_bulletMultiBody;
+
+	//this will be initialized in the constructor
+	int m_totalNumJoints1;
+	int getParentUrdfIndex(int linkIndex) const
+	{
+		return m_urdfLinkParentIndices[linkIndex];
+	}
+	int getMbIndexFromUrdfIndex(int urdfIndex) const
+	{
+		if (urdfIndex == -2)
+			return -2;
+		return m_urdfLinkIndices2BulletLinkIndices[urdfIndex];
+	}
+
+	void registerMultiBody(int urdfLinkIndex, class btMultiBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* compound, const btTransform& localInertialFrame)
+	{
+		m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
+	}
+
+	class btRigidBody* getRigidBodyFromLink(int urdfLinkIndex)
+	{
+		return m_urdfLink2rigidBodies[urdfLinkIndex];
+	}
+
+	void registerRigidBody(int urdfLinkIndex, class btRigidBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* compound, const btTransform& localInertialFrame)
+	{
+		btAssert(m_urdfLink2rigidBodies[urdfLinkIndex] == 0);
+
+		m_urdfLink2rigidBodies[urdfLinkIndex] = body;
+		m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
+	}
+};
+
+void ComputeTotalNumberOfJoints(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int linkIndex)
+{
+	btAlignedObjectArray<int> childIndices;
+	u2b.getLinkChildIndices(linkIndex, childIndices);
+	//b3Printf("link %s has %d children\n", u2b.getLinkName(linkIndex).c_str(),childIndices.size());
+	//for (int i=0;i<childIndices.size();i++)
+	//{
+	//    b3Printf("child %d has childIndex%d=%s\n",i,childIndices[i],u2b.getLinkName(childIndices[i]).c_str());
+	//}
+	cache.m_totalNumJoints1 += childIndices.size();
+	for (int i = 0; i < childIndices.size(); i++)
+	{
+		int childIndex = childIndices[i];
+		ComputeTotalNumberOfJoints(u2b, cache, childIndex);
+	}
+}
+
+
+void ComputeParentIndices(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int urdfParentIndex)
+{
+	cache.m_urdfLinkParentIndices[urdfLinkIndex] = urdfParentIndex;
+	cache.m_urdfLinkIndices2BulletLinkIndices[urdfLinkIndex] = cache.m_currentMultiBodyLinkIndex++;
+
+	btAlignedObjectArray<int> childIndices;
+	u2b.getLinkChildIndices(urdfLinkIndex, childIndices);
+	for (int i = 0; i < childIndices.size(); i++)
+	{
+		ComputeParentIndices(u2b, cache, childIndices[i], urdfLinkIndex);
+	}
+}
+
+void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int flags)
+{
+	//compute the number of links, and compute parent indices array (and possibly other cached data?)
+	cache.m_totalNumJoints1 = 0;
+
+	int rootLinkIndex = u2b.getRootLinkIndex();
+	if (rootLinkIndex >= 0)
+	{
+		ComputeTotalNumberOfJoints(u2b, cache, rootLinkIndex);
+		int numTotalLinksIncludingBase = 1 + cache.m_totalNumJoints1;
+
+		cache.m_urdfLinkParentIndices.resize(numTotalLinksIncludingBase);
+		cache.m_urdfLinkIndices2BulletLinkIndices.resize(numTotalLinksIncludingBase);
+		cache.m_urdfLink2rigidBodies.resize(numTotalLinksIncludingBase);
+		cache.m_urdfLinkLocalInertialFrames.resize(numTotalLinksIncludingBase);
+
+		cache.m_currentMultiBodyLinkIndex = -1;  //multi body base has 'link' index -1
+		
+		bool maintainLinkOrder  = (flags & CUF_MAINTAIN_LINK_ORDER)!=0;
+		if (maintainLinkOrder)
+		{
+			URDF2BulletCachedData cache2 = cache;
+
+			ComputeParentIndices(u2b, cache2, rootLinkIndex, -2);
+
+			for (int j=0;j<numTotalLinksIncludingBase;j++)
+			{
+				cache.m_urdfLinkParentIndices[j] = cache2.m_urdfLinkParentIndices[j];
+				cache.m_urdfLinkIndices2BulletLinkIndices[j] = j - 1;
+			}
+		}else
+		{
+			ComputeParentIndices(u2b, cache, rootLinkIndex, -2);
+		}
+
+	}
+}
+
+void processContactParameters(const URDFLinkContactInfo& contactInfo, btCollisionObject* col)
+{
+	if ((contactInfo.m_flags & URDF_CONTACT_HAS_LATERAL_FRICTION) != 0)
+	{
+		col->setFriction(contactInfo.m_lateralFriction);
+	}
+	if ((contactInfo.m_flags & URDF_CONTACT_HAS_RESTITUTION) != 0)
+	{
+		col->setRestitution(contactInfo.m_restitution);
+	}
+
+	if ((contactInfo.m_flags & URDF_CONTACT_HAS_ROLLING_FRICTION) != 0)
+	{
+		col->setRollingFriction(contactInfo.m_rollingFriction);
+	}
+	if ((contactInfo.m_flags & URDF_CONTACT_HAS_SPINNING_FRICTION) != 0)
+	{
+		col->setSpinningFriction(contactInfo.m_spinningFriction);
+	}
+	if ((contactInfo.m_flags & URDF_CONTACT_HAS_STIFFNESS_DAMPING) != 0)
+	{
+		col->setContactStiffnessAndDamping(contactInfo.m_contactStiffness, contactInfo.m_contactDamping);
+	}
+	if ((contactInfo.m_flags & URDF_CONTACT_HAS_FRICTION_ANCHOR) != 0)
+	{
+		col->setCollisionFlags(col->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
+	}
+}
+
+btScalar tmpUrdfScaling = 2;
+
+btTransform ConvertURDF2BulletInternal(
+	const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
+	URDF2BulletCachedData& cache, int urdfLinkIndex,
+	const btTransform& parentTransformInWorldSpace, 
+	
+#ifdef USE_DISCRETE_DYNAMICS_WORLD
+	btDiscreteDynamicsWorld* world1,
+#else
+	btMultiBodyDynamicsWorld* world1,
+#endif
+
+
+	bool createMultiBody, const char* pathPrefix,
+	int flags, UrdfVisualShapeCache* cachedLinkGraphicsShapesIn, UrdfVisualShapeCache* cachedLinkGraphicsShapesOut, bool recursive)
+{
+	B3_PROFILE("ConvertURDF2BulletInternal2");
+	//b3Printf("start converting/extracting data from URDF interface\n");
+
+	btTransform linkTransformInWorldSpace;
+	linkTransformInWorldSpace.setIdentity();
+
+	int mbLinkIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
+
+	int urdfParentIndex = cache.getParentUrdfIndex(urdfLinkIndex);
+	int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
+	btRigidBody* parentRigidBody = 0;
+
+	//b3Printf("mb link index = %d\n",mbLinkIndex);
+
+	btTransform parentLocalInertialFrame;
+	parentLocalInertialFrame.setIdentity();
+	btScalar parentMass(1);
+	btVector3 parentLocalInertiaDiagonal(1, 1, 1);
+
+	if (urdfParentIndex == -2)
+	{
+		//b3Printf("root link has no parent\n");
+	}
+	else
+	{
+		//b3Printf("urdf parent index = %d\n",urdfParentIndex);
+		//b3Printf("mb parent index = %d\n",mbParentIndex);
+		parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
+		u2b.getMassAndInertia2(urdfParentIndex, parentMass, parentLocalInertiaDiagonal, parentLocalInertialFrame, flags);
+	}
+
+	btScalar mass = 0;
+	btTransform localInertialFrame;
+	localInertialFrame.setIdentity();
+	btVector3 localInertiaDiagonal(0, 0, 0);
+	u2b.getMassAndInertia2(urdfLinkIndex, mass, localInertiaDiagonal, localInertialFrame, flags);
+
+	btTransform parent2joint;
+	parent2joint.setIdentity();
+
+	int jointType;
+	btVector3 jointAxisInJointSpace;
+	btScalar jointLowerLimit;
+	btScalar jointUpperLimit;
+	btScalar jointDamping;
+	btScalar jointFriction;
+	btScalar jointMaxForce;
+	btScalar jointMaxVelocity;
+	btScalar twistLimit;
+	bool hasParentJoint = u2b.getJointInfo3(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction, jointMaxForce, jointMaxVelocity, twistLimit);
+	std::string linkName = u2b.getLinkName(urdfLinkIndex);
+
+	if (flags & CUF_USE_SDF)
+	{
+		parent2joint = parentTransformInWorldSpace.inverse() * linkTransformInWorldSpace;
+	}
+	else
+	{
+		if (flags & CUF_USE_MJCF)
+		{
+			linkTransformInWorldSpace = parentTransformInWorldSpace * linkTransformInWorldSpace;
+		}
+		else
+		{
+			linkTransformInWorldSpace = parentTransformInWorldSpace * parent2joint;
+		}
+	}
+
+	btCompoundShape* tmpShape = u2b.convertLinkCollisionShapes(urdfLinkIndex, pathPrefix, localInertialFrame);
+	btCollisionShape* compoundShape = tmpShape;
+	if (tmpShape->getNumChildShapes() == 1 && tmpShape->getChildTransform(0) == btTransform::getIdentity())
+	{
+		compoundShape = tmpShape->getChildShape(0);
+	}
+
+	int graphicsIndex;
+	{
+		B3_PROFILE("convertLinkVisualShapes");
+		if (cachedLinkGraphicsShapesIn && cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices.size() > (mbLinkIndex + 1))
+		{
+			graphicsIndex = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices[mbLinkIndex + 1];
+			UrdfMaterialColor matColor = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkColors[mbLinkIndex + 1];
+			u2b.setLinkColor2(urdfLinkIndex, matColor);
+		}
+		else
+		{
+			graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex, pathPrefix, localInertialFrame);
+			if (cachedLinkGraphicsShapesOut)
+			{
+				cachedLinkGraphicsShapesOut->m_cachedUrdfLinkVisualShapeIndices.push_back(graphicsIndex);
+				UrdfMaterialColor matColor;
+				u2b.getLinkColor2(urdfLinkIndex, matColor);
+				cachedLinkGraphicsShapesOut->m_cachedUrdfLinkColors.push_back(matColor);
+			}
+		}
+	}
+
+	if (compoundShape)
+	{
+		UrdfMaterialColor matColor;
+		
+		btVector4 color2 = (flags & CUF_GOOGLEY_UNDEFINED_COLORS) ? selectColor2() : btVector4(1, 1, 1, 1);
+		btVector3 specular(0.5, 0.5, 0.5);
+		if (u2b.getLinkColor2(urdfLinkIndex, matColor))
+		{
+			color2 = matColor.m_rgbaColor;
+			specular = matColor.m_specularColor;
+		}
+
+		/*
+         if (visual->material.get())
+         {
+            color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
+         }
+         */
+		if (mass)
+		{
+			if (!(flags & CUF_USE_URDF_INERTIA))
+			{
+				compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
+				btAssert(localInertiaDiagonal[0] < 1e10);
+				btAssert(localInertiaDiagonal[1] < 1e10);
+				btAssert(localInertiaDiagonal[2] < 1e10);
+			}
+			URDFLinkContactInfo contactInfo;
+			u2b.getLinkContactInfo(urdfLinkIndex, contactInfo);
+			//temporary inertia scaling until we load inertia from URDF
+			if (contactInfo.m_flags & URDF_CONTACT_HAS_INERTIA_SCALING)
+			{
+				localInertiaDiagonal *= contactInfo.m_inertiaScaling;
+			}
+		}
+
+		btRigidBody* linkRigidBody = 0;
+		btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace * localInertialFrame;
+		bool canSleep = (flags & CUF_ENABLE_SLEEPING) != 0;
+
+		if (!createMultiBody)
+		{
+			btRigidBody* body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape);
+
+			if (!canSleep)
+			{
+				body->forceActivationState(DISABLE_DEACTIVATION);
+			}
+
+			linkRigidBody = body;
+
+			world1->addRigidBody(body);
+
+			compoundShape->setUserIndex(graphicsIndex);
+
+			URDFLinkContactInfo contactInfo;
+			u2b.getLinkContactInfo(urdfLinkIndex, contactInfo);
+
+			processContactParameters(contactInfo, body);
+			creation.createRigidBodyGraphicsInstance2(urdfLinkIndex, body, color2, specular, graphicsIndex);
+			cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
+
+			//untested: u2b.convertLinkVisualShapes2(linkIndex,urdfLinkIndex,pathPrefix,localInertialFrame,body);
+		}
+		else
+		{
+			if (cache.m_bulletMultiBody == 0)
+			{
+				bool isFixedBase = (mass == 0);  //todo: figure out when base is fixed
+				int totalNumJoints = cache.m_totalNumJoints1;
+				cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints, mass, localInertiaDiagonal, isFixedBase, canSleep);
+				if (flags & CUF_GLOBAL_VELOCITIES_MB)
+				{
+					cache.m_bulletMultiBody->useGlobalVelocities(true);
+				}
+				if (flags & CUF_USE_MJCF)
+				{
+					cache.m_bulletMultiBody->setBaseWorldTransform(linkTransformInWorldSpace);
+				}
+
+				cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
+			}
+		}
+
+		//create a joint if necessary
+		if (hasParentJoint)
+		{
+			btTransform offsetInA, offsetInB;
+			offsetInA = parentLocalInertialFrame.inverse() * parent2joint;
+			offsetInB = localInertialFrame.inverse();
+			btQuaternion parentRotToThis = offsetInB.getRotation() * offsetInA.inverse().getRotation();
+
+			bool disableParentCollision = true;
+
+			if (createMultiBody && cache.m_bulletMultiBody)
+			{
+				cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping;
+				cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction = jointFriction;
+				cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointLowerLimit = jointLowerLimit;
+				cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointUpperLimit = jointUpperLimit;
+				cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointMaxForce = jointMaxForce;
+				cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointMaxVelocity = jointMaxVelocity;
+			}
+
+			switch (jointType)
+			{
+				case URDFSphericalJoint:
+				{
+					if (createMultiBody)
+					{
+						creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
+						cache.m_bulletMultiBody->setupSpherical(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
+							parentRotToThis, offsetInA.getOrigin(), -offsetInB.getOrigin(),
+							disableParentCollision);
+
+
+						//create a spherical joint limit, swing_x,. swing_y and twist
+						//jointLowerLimit <= jointUpperLimit)
+						if (jointUpperLimit > 0 && jointLowerLimit> 0 && twistLimit > 0 && jointMaxForce>0)
+						{
+							btMultiBodySphericalJointLimit* con = new btMultiBodySphericalJointLimit(cache.m_bulletMultiBody, mbLinkIndex, 
+								jointLowerLimit,
+								jointUpperLimit,
+								twistLimit,
+								jointMaxForce);
+							world1->addMultiBodyConstraint(con);
+						}
+
+					}
+					else
+					{
+						btAssert(0);
+					}
+					break;
+				}
+				case URDFPlanarJoint:
+				{
+					
+					if (createMultiBody)
+					{
+#if 0
+						void setupPlanar(int i,  // 0 to num_links-1
+							btScalar mass,
+							const btVector3 &inertia,
+							int parent,
+							const btQuaternion &rotParentToThis,  // rotate points in parent frame to this frame, when q = 0
+							const btVector3 &rotationAxis,
+							const btVector3 &parentComToThisComOffset,  // vector from parent COM to this COM, in PARENT frame
+							bool disableParentCollision = false);
+#endif
+						creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
+						cache.m_bulletMultiBody->setupPlanar(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
+							parentRotToThis, quatRotate(offsetInB.getRotation(), jointAxisInJointSpace), offsetInA.getOrigin(),
+							disableParentCollision);
+					}
+					else
+					{
+#if 0
+						//b3Printf("Fixed joint\n");
+
+						btGeneric6DofSpring2Constraint* dof6 = 0;
+
+						//backward compatibility
+						if (flags & CUF_RESERVED)
+						{
+							dof6 = creation.createFixedJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
+						}
+						else
+						{
+							dof6 = creation.createFixedJoint(urdfLinkIndex, *linkRigidBody, *parentRigidBody, offsetInB, offsetInA);
+						}
+						if (enableConstraints)
+							world1->addConstraint(dof6, true);
+#endif
+					}
+					break;
+				}
+				case URDFFloatingJoint:
+				
+				case URDFFixedJoint:
+				{
+					if ((jointType == URDFFloatingJoint) || (jointType == URDFPlanarJoint))
+					{
+						printf("Warning: joint unsupported, creating a fixed joint instead.");
+					}
+					creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
+
+					if (createMultiBody)
+					{
+						//todo: adjust the center of mass transform and pivot axis properly
+						cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
+															parentRotToThis, offsetInA.getOrigin(), -offsetInB.getOrigin());
+					}
+					else
+					{
+						//b3Printf("Fixed joint\n");
+
+						btGeneric6DofSpring2Constraint* dof6 = 0;
+
+						//backward compatibility
+						if (flags & CUF_RESERVED)
+						{
+							dof6 = creation.createFixedJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
+						}
+						else
+						{
+							dof6 = creation.createFixedJoint(urdfLinkIndex, *linkRigidBody, *parentRigidBody, offsetInB, offsetInA);
+						}
+						if (enableConstraints)
+							world1->addConstraint(dof6, true);
+					}
+					break;
+				}
+				case URDFContinuousJoint:
+				case URDFRevoluteJoint:
+				{
+					creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
+					if (createMultiBody)
+					{
+#ifndef USE_DISCRETE_DYNAMICS_WORLD
+						cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
+															   parentRotToThis, quatRotate(offsetInB.getRotation(), 
+																   jointAxisInJointSpace), 
+																offsetInA.getOrigin(),
+															   -offsetInB.getOrigin(),
+															   disableParentCollision);
+
+						if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit)
+						{
+							//std::string name = u2b.getLinkName(urdfLinkIndex);
+							//printf("create btMultiBodyJointLimitConstraint for revolute link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit, jointUpperLimit);
+							btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit);
+							world1->addMultiBodyConstraint(con);
+						}
+#endif
+					}
+					else
+
+					{
+						btGeneric6DofSpring2Constraint* dof6 = 0;
+						if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit)
+						{
+							//backwards compatibility
+							if (flags & CUF_RESERVED)
+							{
+								dof6 = creation.createRevoluteJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit);
+							}
+							else
+							{
+								dof6 = creation.createRevoluteJoint(urdfLinkIndex, *linkRigidBody, *parentRigidBody, offsetInB, offsetInA, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit);
+							}
+						}
+						else
+						{
+							//disable joint limits
+							if (flags & CUF_RESERVED)
+							{
+								dof6 = creation.createRevoluteJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, 1, -1);
+							}
+							else
+							{
+								dof6 = creation.createRevoluteJoint(urdfLinkIndex, *linkRigidBody, *parentRigidBody, offsetInB, offsetInA, jointAxisInJointSpace, 1, -1);
+							}
+						}
+
+						if (enableConstraints)
+							world1->addConstraint(dof6, true);
+						//b3Printf("Revolute/Continuous joint\n");
+					}
+					break;
+				}
+				case URDFPrismaticJoint:
+				{
+					creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
+
+					if (createMultiBody)
+					{
+#ifndef USE_DISCRETE_DYNAMICS_WORLD
+						cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
+																parentRotToThis, quatRotate(offsetInB.getRotation(), jointAxisInJointSpace), offsetInA.getOrigin(),  //parent2joint.getOrigin(),
+																-offsetInB.getOrigin(),
+																disableParentCollision);
+
+						if (jointLowerLimit <= jointUpperLimit)
+						{
+							//std::string name = u2b.getLinkName(urdfLinkIndex);
+							//printf("create btMultiBodyJointLimitConstraint for prismatic link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit,jointUpperLimit);
+
+							btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit);
+							world1->addMultiBodyConstraint(con);
+						}
+						//printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit);
+#endif
+					}
+					else
+					{
+						btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit);
+
+						if (enableConstraints)
+							world1->addConstraint(dof6, true);
+
+						//b3Printf("Prismatic\n");
+					}
+					break;
+				}
+				default:
+				{
+					//b3Printf("Error: unsupported joint type in URDF (%d)\n", jointType);
+					btAssert(0);
+				}
+			}
+		}
+
+		if (createMultiBody)
+		{
+			//if (compoundShape->getNumChildShapes()>0)
+			{
+				btMultiBodyLinkCollider* col = creation.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody);
+
+				compoundShape->setUserIndex(graphicsIndex);
+
+				col->setCollisionShape(compoundShape);
+
+				if (compoundShape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
+				{
+					btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)compoundShape;
+					if (trimeshShape->getTriangleInfoMap())
+					{
+						col->setCollisionFlags(col->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
+					}
+				}
+
+				if (compoundShape->getShapeType() == TERRAIN_SHAPE_PROXYTYPE)
+				{
+					col->setCollisionFlags(col->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
+				}
+
+				btTransform tr;
+				tr.setIdentity();
+				tr = linkTransformInWorldSpace;
+				//if we don't set the initial pose of the btCollisionObject, the simulator will do this
+				//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
+
+				col->setWorldTransform(tr);
+
+				//base and fixed? -> static, otherwise flag as dynamic
+				bool isDynamic = (mbLinkIndex < 0 && cache.m_bulletMultiBody->hasFixedBase()) ? false : true;
+				int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
+				int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+
+				int colGroup = 0, colMask = 0;
+				int collisionFlags = u2b.getCollisionGroupAndMask(urdfLinkIndex, colGroup, colMask);
+				if (collisionFlags & URDF_HAS_COLLISION_GROUP)
+				{
+					collisionFilterGroup = colGroup;
+				}
+				if (collisionFlags & URDF_HAS_COLLISION_MASK)
+				{
+					collisionFilterMask = colMask;
+				}
+				world1->addCollisionObject(col, collisionFilterGroup, collisionFilterMask);
+
+				btVector4 color2 = (flags & CUF_GOOGLEY_UNDEFINED_COLORS) ? selectColor2() : btVector4(1, 1, 1, 1);
+				btVector3 specularColor(1, 1, 1);
+				UrdfMaterialColor matCol;
+				if (u2b.getLinkColor2(urdfLinkIndex, matCol))
+				{
+					color2 = matCol.m_rgbaColor;
+					specularColor = matCol.m_specularColor;
+				}
+				{
+					B3_PROFILE("createCollisionObjectGraphicsInstance2");
+					creation.createCollisionObjectGraphicsInstance2(urdfLinkIndex, col, color2, specularColor);
+				}
+				{
+					B3_PROFILE("convertLinkVisualShapes2");
+					u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, localInertialFrame, col, u2b.getBodyUniqueId());
+				}
+				URDFLinkContactInfo contactInfo;
+				u2b.getLinkContactInfo(urdfLinkIndex, contactInfo);
+
+				processContactParameters(contactInfo, col);
+
+				if (mbLinkIndex >= 0)  //???? double-check +/- 1
+				{
+					//if the base is static and all joints in the chain between this link and the base are fixed, 
+					//then this link is static too (doesn't merge islands)
+					if (cache.m_bulletMultiBody->getBaseMass() == 0)
+					{
+						bool allJointsFixed = true;
+						int testLinkIndex = mbLinkIndex;
+						do
+						{
+							if (cache.m_bulletMultiBody->getLink(testLinkIndex).m_jointType != btMultibodyLink::eFixed)
+							{
+								allJointsFixed = false;
+								break;
+							}
+							testLinkIndex = cache.m_bulletMultiBody->getLink(testLinkIndex).m_parent;
+						} while (testLinkIndex> 0);
+						if (allJointsFixed)
+						{
+							col->setCollisionFlags(col->getCollisionFlags() | btCollisionObject::CF_STATIC_OBJECT);
+						}
+
+					}
+					cache.m_bulletMultiBody->getLink(mbLinkIndex).m_collider = col;
+					if (flags & CUF_USE_SELF_COLLISION_INCLUDE_PARENT)
+					{
+						cache.m_bulletMultiBody->getLink(mbLinkIndex).m_flags &= ~BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
+					}
+					if (flags & CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
+					{
+						cache.m_bulletMultiBody->getLink(mbLinkIndex).m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION;
+					}
+				}
+				else
+				{
+					//					if (canSleep)
+					{
+						if (cache.m_bulletMultiBody->getBaseMass() == 0)
+						//&& cache.m_bulletMultiBody->getNumDofs()==0)
+						{
+							//col->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
+							col->setCollisionFlags(col->getCollisionFlags() | btCollisionObject::CF_STATIC_OBJECT);
+						}
+					}
+
+					cache.m_bulletMultiBody->setBaseCollider(col);
+				}
+			}
+		}
+		else
+		{
+			int mbLinkIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
+			//u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, localInertialFrame, col, u2b.getBodyUniqueId());
+			u2b.convertLinkVisualShapes2(-1, urdfLinkIndex, pathPrefix, localInertialFrame, linkRigidBody, u2b.getBodyUniqueId());
+		}
+	}
+
+	btAlignedObjectArray<int> urdfChildIndices;
+	u2b.getLinkChildIndices(urdfLinkIndex, urdfChildIndices);
+
+	int numChildren = urdfChildIndices.size();
+
+	if (recursive)
+	{
+		for (int i = 0; i < numChildren; i++)
+		{
+			int urdfChildLinkIndex = urdfChildIndices[i];
+
+			ConvertURDF2BulletInternal(u2b, creation, cache, urdfChildLinkIndex, linkTransformInWorldSpace, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapesIn, cachedLinkGraphicsShapesOut, recursive);
+		}
+	}
+	return linkTransformInWorldSpace;
+}
+
+struct childParentIndex
+{
+	int m_index;
+	int m_mbIndex;
+	int m_parentIndex;
+	int m_parentMBIndex;
+	
+};
+
+void GetAllIndices(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int parentIndex, btAlignedObjectArray<childParentIndex>& allIndices)
+{
+	childParentIndex cp;
+	cp.m_index = urdfLinkIndex;
+	int mbIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
+	cp.m_mbIndex = mbIndex;
+	cp.m_parentIndex = parentIndex;
+	int parentMbIndex = parentIndex>=0? cache.getMbIndexFromUrdfIndex(parentIndex) : -1;
+	cp.m_parentMBIndex = parentMbIndex;
+
+	allIndices.push_back(cp);
+	btAlignedObjectArray<int> urdfChildIndices;
+	u2b.getLinkChildIndices(urdfLinkIndex, urdfChildIndices);
+	int numChildren = urdfChildIndices.size();
+	for (int i = 0; i < numChildren; i++)
+	{
+		int urdfChildLinkIndex = urdfChildIndices[i];
+		GetAllIndices(u2b, cache, urdfChildLinkIndex, urdfLinkIndex, allIndices);
+	}
+}
+
+
+bool MyIntCompareFunc(childParentIndex a, childParentIndex b)
+{
+	return (a.m_index < b.m_index);
+}
+
+
+	
+
+
+void ConvertURDF2Bullet(
+	const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
+	const btTransform& rootTransformInWorldSpace,
+
+#ifdef USE_DISCRETE_DYNAMICS_WORLD
+	btDiscreteDynamicsWorld* world1,
+#else
+	btMultiBodyDynamicsWorld* world1,
+#endif
+	bool createMultiBody, const char* pathPrefix, int flags, UrdfVisualShapeCache* cachedLinkGraphicsShapes)
+{
+	URDF2BulletCachedData cache;
+	InitURDF2BulletCache(u2b, cache, flags);
+	int urdfLinkIndex = u2b.getRootLinkIndex();
+	int rootIndex = u2b.getRootLinkIndex();
+	B3_PROFILE("ConvertURDF2Bullet");
+
+	UrdfVisualShapeCache cachedLinkGraphicsShapesOut;
+
+
+	bool recursive = (flags & CUF_MAINTAIN_LINK_ORDER)==0;
+	if (recursive)
+	{
+		ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex, rootTransformInWorldSpace, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive);
+	}
+	else
+	{
+		
+		btAlignedObjectArray<btTransform> parentTransforms;
+		if (urdfLinkIndex >= parentTransforms.size())
+		{
+			parentTransforms.resize(urdfLinkIndex + 1);
+		}
+		parentTransforms[urdfLinkIndex] = rootTransformInWorldSpace;
+		btAlignedObjectArray<childParentIndex> allIndices;
+		
+		GetAllIndices(u2b, cache, urdfLinkIndex, -1, allIndices);
+		allIndices.quickSort(MyIntCompareFunc);
+
+		for (int i = 0; i < allIndices.size(); i++)
+		{
+			int urdfLinkIndex = allIndices[i].m_index;
+			int parentIndex = allIndices[i].m_parentIndex;
+			btTransform parentTr = parentIndex >= 0 ? parentTransforms[parentIndex] : rootTransformInWorldSpace;
+			btTransform tr = ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex, parentTr , world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive);
+			if ((urdfLinkIndex+1) >= parentTransforms.size())
+			{
+				parentTransforms.resize(urdfLinkIndex + 1);
+			}
+			parentTransforms[urdfLinkIndex] = tr;
+		}
+		
+
+
+	}
+	if (cachedLinkGraphicsShapes && cachedLinkGraphicsShapesOut.m_cachedUrdfLinkVisualShapeIndices.size() > cachedLinkGraphicsShapes->m_cachedUrdfLinkVisualShapeIndices.size())
+	{
+		*cachedLinkGraphicsShapes = cachedLinkGraphicsShapesOut;
+	}
+#ifndef USE_DISCRETE_DYNAMICS_WORLD
+	if (world1 && cache.m_bulletMultiBody)
+	{
+		B3_PROFILE("Post process");
+		btMultiBody* mb = cache.m_bulletMultiBody;
+
+		mb->setHasSelfCollision((flags & CUF_USE_SELF_COLLISION) != 0);
+
+		mb->finalizeMultiDof();
+
+		btTransform localInertialFrameRoot = cache.m_urdfLinkLocalInertialFrames[urdfLinkIndex];
+
+		if (flags & CUF_USE_MJCF)
+		{
+		}
+		else
+		{
+			mb->setBaseWorldTransform(rootTransformInWorldSpace * localInertialFrameRoot);
+		}
+		btAlignedObjectArray<btQuaternion> scratch_q;
+		btAlignedObjectArray<btVector3> scratch_m;
+		mb->forwardKinematics(scratch_q, scratch_m);
+		mb->updateCollisionObjectWorldTransforms(scratch_q, scratch_m);
+
+		world1->addMultiBody(mb);
+	}
+	#endif
+}

Engine/lib/bullet/examples/Importers/ImportURDFDemo/URDF2Bullet.h

@@ -0,0 +1,45 @@
+#ifndef _URDF2BULLET_H
+#define _URDF2BULLET_H
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btTransform.h"
+#include <string>
+#include "URDFJointTypes.h"  //for UrdfMaterialColor cache
+
+class btVector3;
+class btTransform;
+class btMultiBodyDynamicsWorld;
+class btDiscreteDynamicsWorld;
+class btTransform;
+
+class URDFImporterInterface;
+class MultiBodyCreationInterface;
+
+
+
+struct UrdfVisualShapeCache
+{
+	btAlignedObjectArray<UrdfMaterialColor> m_cachedUrdfLinkColors;
+	btAlignedObjectArray<int> m_cachedUrdfLinkVisualShapeIndices;
+};
+//#define USE_DISCRETE_DYNAMICS_WORLD
+#ifdef USE_DISCRETE_DYNAMICS_WORLD
+	void ConvertURDF2Bullet(const URDFImporterInterface& u2b,
+						MultiBodyCreationInterface& creationCallback,
+						const btTransform& rootTransformInWorldSpace,
+						btDiscreteDynamicsWorld* world,
+						bool createMultiBody,
+						const char* pathPrefix,
+						int flags = 0,
+						UrdfVisualShapeCache* cachedLinkGraphicsShapes = 0);
+
+#else
+void ConvertURDF2Bullet(const URDFImporterInterface& u2b,
+						MultiBodyCreationInterface& creationCallback,
+						const btTransform& rootTransformInWorldSpace,
+						btMultiBodyDynamicsWorld* world,
+						bool createMultiBody,
+						const char* pathPrefix,
+						int flags = 0,
+						UrdfVisualShapeCache* cachedLinkGraphicsShapes = 0);
+#endif
+#endif  //_URDF2BULLET_H

Engine/lib/bullet/examples/Importers/ImportURDFDemo/URDFImporterInterface.h

@@ -0,0 +1,110 @@
+#ifndef URDF_IMPORTER_INTERFACE_H
+#define URDF_IMPORTER_INTERFACE_H
+
+#include <string>
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btTransform.h"
+#include "URDFJointTypes.h"
+#include "SDFAudioTypes.h"
+
+class URDFImporterInterface
+{
+public:
+	virtual ~URDFImporterInterface() {}
+
+	virtual bool loadURDF(const char* fileName, bool forceFixedBase = false) = 0;
+
+	virtual bool loadSDF(const char* fileName, bool forceFixedBase = false) { return false; }
+
+	virtual const char* getPathPrefix() = 0;
+
+	///return >=0 for the root link index, -1 if there is no root link
+	virtual int getRootLinkIndex() const = 0;
+
+	///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range)
+	virtual std::string getLinkName(int linkIndex) const = 0;
+
+	//various derived class in internal source code break with new pure virtual methods, so provide some default implementation
+	virtual std::string getBodyName() const
+	{
+		return "";
+	}
+
+	/// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise
+	virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const { return false; }
+
+	virtual bool getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const { return false; }
+	virtual void setLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const {}
+
+	virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const { return 0; }
+	///this API will likely change, don't override it!
+	virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo) const { return false; }
+
+	virtual bool getLinkAudioSource(int linkIndex, SDFAudioSource& audioSource) const { return false; }
+
+	virtual std::string getJointName(int linkIndex) const = 0;
+
+	//fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity.
+	virtual void getMassAndInertia(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame) const = 0;
+	virtual void getMassAndInertia2(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame, int flags) const
+	{
+		getMassAndInertia(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrame);
+	}
+
+	///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed
+	virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray<int>& childLinkIndices) const = 0;
+
+	virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const = 0;
+
+	virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const
+	{
+		//backwards compatibility for custom file importers
+		jointMaxForce = 0;
+		jointMaxVelocity = 0;
+		return getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorld, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction);
+	};
+
+	virtual bool getJointInfo3(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity, btScalar& twistLimit) const
+	{
+		//backwards compatibility for custom file importers
+		twistLimit = 0;
+		return getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorld, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction, jointMaxForce, jointMaxVelocity);
+	};
+
+	virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const = 0;
+	virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld) {}
+
+	///quick hack: need to rethink the API/dependencies of this
+	virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const { return -1; }
+
+	virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const {}
+	virtual void setBodyUniqueId(int bodyId) {}
+	virtual int getBodyUniqueId() const { return 0; }
+
+	//default implementation for backward compatibility
+	virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const = 0;
+	virtual int getUrdfFromCollisionShape(const class btCollisionShape* collisionShape, struct UrdfCollision& collision) const
+	{
+		return 0;
+	}
+
+	virtual const struct UrdfLink* getUrdfLink(int urdfLinkIndex) const
+	{
+		return 0;
+	}
+
+	virtual const struct UrdfModel* getUrdfModel() const { return 0; };
+
+	virtual int getNumAllocatedCollisionShapes() const { return 0; }
+	virtual class btCollisionShape* getAllocatedCollisionShape(int /*index*/) { return 0; }
+	virtual int getNumModels() const { return 0; }
+	virtual void activateModel(int /*modelIndex*/) {}
+	virtual int getNumAllocatedMeshInterfaces() const { return 0; }
+
+	virtual int getNumAllocatedTextures() const { return 0; }
+	virtual int getAllocatedTexture(int index) const { return 0; }
+
+	virtual class btStridingMeshInterface* getAllocatedMeshInterface(int index) { return 0; }
+};
+
+#endif  //URDF_IMPORTER_INTERFACE_H

Engine/lib/bullet/examples/Importers/ImportURDFDemo/URDFJointTypes.h

@@ -0,0 +1,111 @@
+#ifndef URDF_JOINT_TYPES_H
+#define URDF_JOINT_TYPES_H
+
+#include "LinearMath/btScalar.h"
+#include "LinearMath/btVector3.h"
+
+enum UrdfJointTypes
+{
+	URDFRevoluteJoint = 1,
+	URDFPrismaticJoint,
+	URDFContinuousJoint,
+	URDFFloatingJoint,
+	URDFPlanarJoint,
+	URDFFixedJoint,
+	URDFSphericalJoint,
+
+};
+
+enum URDF_LinkContactFlags
+{
+	URDF_CONTACT_HAS_LATERAL_FRICTION = 1,
+	URDF_CONTACT_HAS_INERTIA_SCALING = 2,
+	URDF_CONTACT_HAS_CONTACT_CFM = 4,
+	URDF_CONTACT_HAS_CONTACT_ERP = 8,
+	URDF_CONTACT_HAS_STIFFNESS_DAMPING = 16,
+	URDF_CONTACT_HAS_ROLLING_FRICTION = 32,
+	URDF_CONTACT_HAS_SPINNING_FRICTION = 64,
+	URDF_CONTACT_HAS_RESTITUTION = 128,
+	URDF_CONTACT_HAS_FRICTION_ANCHOR = 256,
+
+};
+
+struct URDFLinkContactInfo
+{
+	btScalar m_lateralFriction;
+	btScalar m_rollingFriction;
+	btScalar m_spinningFriction;
+	btScalar m_restitution;
+	btScalar m_inertiaScaling;
+	btScalar m_contactCfm;
+	btScalar m_contactErp;
+	btScalar m_contactStiffness;
+	btScalar m_contactDamping;
+
+	int m_flags;
+
+	URDFLinkContactInfo()
+		: m_lateralFriction(0.5),
+		  m_rollingFriction(0),
+		  m_spinningFriction(0),
+		  m_restitution(0),
+		  m_inertiaScaling(1),
+		  m_contactCfm(0),
+		  m_contactErp(0),
+		  m_contactStiffness(1e4),
+		  m_contactDamping(1)
+	{
+		m_flags = URDF_CONTACT_HAS_LATERAL_FRICTION;
+	}
+};
+
+enum UrdfCollisionFlags
+{
+	URDF_FORCE_CONCAVE_TRIMESH = 1,
+	URDF_HAS_COLLISION_GROUP = 2,
+	URDF_HAS_COLLISION_MASK = 4,
+};
+
+struct UrdfMaterialColor
+{
+	btVector4 m_rgbaColor;
+	btVector3 m_specularColor;
+	UrdfMaterialColor()
+		: m_rgbaColor(0.8, 0.8, 0.8, 1),
+		  m_specularColor(0.4, 0.4, 0.4)
+	{
+	}
+};
+
+//manually sync with eURDF_Flags in SharedMemoryPublic.h!
+enum ConvertURDFFlags
+{
+	CUF_USE_SDF = 1,
+	// Use inertia values in URDF instead of recomputing them from collision shape.
+	CUF_USE_URDF_INERTIA = 2,
+	CUF_USE_MJCF = 4,
+	CUF_USE_SELF_COLLISION = 8,
+	CUF_USE_SELF_COLLISION_EXCLUDE_PARENT = 16,
+	CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS = 32,
+	CUF_RESERVED = 64,
+	CUF_USE_IMPLICIT_CYLINDER = 128,
+	CUF_GLOBAL_VELOCITIES_MB = 256,
+	CUF_MJCF_COLORS_FROM_FILE = 512,
+	CUF_ENABLE_CACHED_GRAPHICS_SHAPES = 1024,
+	CUF_ENABLE_SLEEPING = 2048,
+	CUF_INITIALIZE_SAT_FEATURES = 4096,
+	CUF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192,
+	CUF_PARSE_SENSORS = 16384,
+	CUF_USE_MATERIAL_COLORS_FROM_MTL = 32768,
+	CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 65536,
+	CUF_MAINTAIN_LINK_ORDER = 131072,
+	CUF_ENABLE_WAKEUP = 1 << 18, 
+	CUF_MERGE_FIXED_LINKS = 1 << 19,
+	CUF_IGNORE_VISUAL_SHAPES = 1 << 20,
+	CUF_IGNORE_COLLISION_SHAPES = 1 << 21,
+	CUF_PRINT_URDF_INFO = 1 << 22,
+	CUF_GOOGLEY_UNDEFINED_COLORS = 1 << 23,
+
+};
+
+#endif  //URDF_JOINT_TYPES_H

Engine/lib/bullet/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h

@@ -0,0 +1,112 @@
+
+#ifndef URDF_FIND_MESH_FILE_H
+#define URDF_FIND_MESH_FILE_H
+
+#include "../../CommonInterfaces/CommonFileIOInterface.h"
+#include "Bullet3Common/b3Logging.h"
+
+#include <string>
+#include <list>
+
+#include "UrdfParser.h"
+
+static bool UrdfFindMeshFile(
+	CommonFileIOInterface* fileIO,
+	const std::string& urdf_path, std::string fn,
+	const std::string& error_message_prefix,
+	std::string* out_found_filename, int* out_type)
+{
+	if (fn.size() <= 4)
+	{
+		b3Warning("%s: invalid mesh filename '%s'\n", error_message_prefix.c_str(), fn.c_str());
+		return false;
+	}
+
+	std::string ext;
+	std::string ext_ = fn.substr(fn.size() - 4);
+	for (std::string::iterator i = ext_.begin(); i != ext_.end(); ++i)
+	{
+		ext += char(tolower(*i));
+	}
+
+	if (ext == ".dae")
+	{
+		*out_type = UrdfGeometry::FILE_COLLADA;
+	}
+	else if (ext == ".stl")
+	{
+		*out_type = UrdfGeometry::FILE_STL;
+	}
+	else if (ext == ".obj")
+	{
+		*out_type = UrdfGeometry::FILE_OBJ;
+	}
+	else if (ext == ".cdf")
+	{
+		*out_type = UrdfGeometry::FILE_CDF;
+	}
+	else if (ext == ".vtk")
+	{
+		*out_type = UrdfGeometry::FILE_VTK;
+	}
+	else
+	{
+		b3Warning("%s: invalid mesh filename extension '%s'\n", error_message_prefix.c_str(), ext.c_str());
+		return false;
+	}
+
+	std::string drop_it_file = "file://";
+	std::string drop_it_pack = "package://";
+	std::string drop_it_model = "model://";
+	if (fn.substr(0, drop_it_file.length()) == drop_it_file)
+		fn = fn.substr(drop_it_file.length());
+	if (fn.substr(0, drop_it_pack.length()) == drop_it_pack)
+		fn = fn.substr(drop_it_pack.length());
+	else if (fn.substr(0, drop_it_model.length()) == drop_it_model)
+		fn = fn.substr(drop_it_model.length());
+
+	std::list<std::string> shorter;
+	shorter.push_back("../../");
+	shorter.push_back("../");
+	shorter.push_back("./");
+	int cnt = urdf_path.size();
+	for (int i = 0; i < cnt; ++i)
+	{
+		if (urdf_path[i] == '/' || urdf_path[i] == '\\')
+		{
+			shorter.push_back(urdf_path.substr(0, i) + "/");
+		}
+	}
+	shorter.push_back("");  // no prefix
+	shorter.reverse();
+
+	std::string existing_file;
+
+	for (std::list<std::string>::iterator x = shorter.begin(); x != shorter.end(); ++x)
+	{
+		std::string attempt = *x + fn;
+		int f = fileIO->fileOpen(attempt.c_str(), "rb");
+		if (f<0)
+		{
+			//b3Printf("%s: tried '%s'", error_message_prefix.c_str(), attempt.c_str());
+			continue;
+		}
+		fileIO->fileClose(f);
+		existing_file = attempt;
+		//b3Printf("%s: found '%s'", error_message_prefix.c_str(), attempt.c_str());
+		break;
+	}
+
+	if (existing_file.empty())
+	{
+		b3Warning("%s: cannot find '%s' in any directory in urdf path\n", error_message_prefix.c_str(), fn.c_str());
+		return false;
+	}
+	else
+	{
+		*out_found_filename = existing_file;
+		return true;
+	}
+}
+
+#endif //URDF_FIND_MESH_FILE_H

Engine/lib/bullet/examples/Importers/ImportURDFDemo/UrdfParser.h

@@ -0,0 +1,465 @@
+#ifndef URDF_PARSER_H
+#define URDF_PARSER_H
+
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btHashMap.h"
+#include "URDFJointTypes.h"
+#include "SDFAudioTypes.h"
+#include <string>
+
+#define btArray btAlignedObjectArray
+
+struct ErrorLogger
+{
+	virtual ~ErrorLogger() {}
+	virtual void reportError(const char* error) = 0;
+	virtual void reportWarning(const char* warning) = 0;
+	virtual void printMessage(const char* msg) = 0;
+};
+
+struct UrdfMaterial
+{
+	std::string m_name;
+	std::string m_textureFilename;
+	UrdfMaterialColor m_matColor;
+
+	UrdfMaterial()
+	{
+	}
+};
+
+struct UrdfInertia
+{
+	btTransform m_linkLocalFrame;
+	bool m_hasLinkLocalFrame;
+
+	double m_mass;
+	double m_ixx, m_ixy, m_ixz, m_iyy, m_iyz, m_izz;
+
+	UrdfInertia()
+	{
+		m_hasLinkLocalFrame = false;
+		m_linkLocalFrame.setIdentity();
+		m_mass = 0.f;
+		m_ixx = m_ixy = m_ixz = m_iyy = m_iyz = m_izz = 0.f;
+	}
+};
+
+enum UrdfGeomTypes
+{
+	URDF_GEOM_SPHERE = 2,
+	URDF_GEOM_BOX,
+	URDF_GEOM_CYLINDER,
+	URDF_GEOM_MESH,
+	URDF_GEOM_PLANE,
+	URDF_GEOM_CAPSULE,  //non-standard URDF
+	URDF_GEOM_CDF,      //signed-distance-field, non-standard URDF
+	URDF_GEOM_HEIGHTFIELD,   //heightfield, non-standard URDF
+	URDF_GEOM_UNKNOWN,
+};
+
+struct UrdfGeometry
+{
+	UrdfGeomTypes m_type;
+
+	double m_sphereRadius;
+
+	btVector3 m_boxSize;
+
+	double m_capsuleRadius;
+	double m_capsuleHeight;
+	int m_hasFromTo;
+	btVector3 m_capsuleFrom;
+	btVector3 m_capsuleTo;
+
+	btVector3 m_planeNormal;
+
+	enum
+	{
+		FILE_STL = 1,
+		FILE_COLLADA = 2,
+		FILE_OBJ = 3,
+		FILE_CDF = 4,
+		MEMORY_VERTICES = 5,
+	        FILE_VTK = 6,
+
+	};
+	int m_meshFileType;
+	std::string m_meshFileName;
+	btVector3 m_meshScale;
+
+	btArray<btVector3> m_vertices;
+	btArray<btVector3> m_uvs;
+	btArray<btVector3> m_normals;
+	btArray<int> m_indices;
+
+
+	UrdfMaterial m_localMaterial;
+	bool m_hasLocalMaterial;
+
+	UrdfGeometry()
+		: m_type(URDF_GEOM_UNKNOWN),
+		  m_sphereRadius(1),
+		  m_boxSize(1, 1, 1),
+		  m_capsuleRadius(1),
+		  m_capsuleHeight(1),
+		  m_hasFromTo(0),
+		  m_capsuleFrom(0, 1, 0),
+		  m_capsuleTo(1, 0, 0),
+		  m_planeNormal(0, 0, 1),
+		  m_meshFileType(0),
+		  m_meshScale(1, 1, 1),
+		  m_hasLocalMaterial(false)
+	{
+	}
+};
+
+
+struct UrdfShape
+{
+	std::string m_sourceFileLocation;
+	btTransform m_linkLocalFrame;
+	UrdfGeometry m_geometry;
+	std::string m_name;
+};
+
+struct UrdfVisual : UrdfShape
+{
+	std::string m_materialName;
+	// Maps user data keys to user data values.
+	btHashMap<btHashString, std::string> m_userData;
+};
+
+struct UrdfCollision : UrdfShape
+{
+	int m_flags;
+	int m_collisionGroup;
+	int m_collisionMask;
+	UrdfCollision()
+		: m_flags(0)
+	{
+	}
+};
+
+struct UrdfJoint;
+
+struct UrdfLink
+{
+	std::string m_name;
+	UrdfInertia m_inertia;
+	btTransform m_linkTransformInWorld;
+	btArray<UrdfVisual> m_visualArray;
+	btArray<UrdfCollision> m_collisionArray;
+	UrdfLink* m_parentLink;
+	UrdfJoint* m_parentJoint;
+
+	btArray<UrdfJoint*> m_childJoints;
+	btArray<UrdfLink*> m_childLinks;
+
+	int m_linkIndex;
+
+	URDFLinkContactInfo m_contactInfo;
+
+	SDFAudioSource m_audioSource;
+	// Maps user data keys to user data values.
+	btHashMap<btHashString, std::string> m_userData;
+
+	UrdfLink()
+		: m_parentLink(0),
+		  m_parentJoint(0),
+		  m_linkIndex(-2)
+	{
+	}
+};
+struct UrdfJoint
+{
+	std::string m_name;
+	UrdfJointTypes m_type;
+	btTransform m_parentLinkToJointTransform;
+	std::string m_parentLinkName;
+	std::string m_childLinkName;
+	btVector3 m_localJointAxis;
+
+	double m_lowerLimit;
+	double m_upperLimit;
+
+	double m_effortLimit;
+	double m_velocityLimit;
+
+	double m_jointDamping;
+	double m_jointFriction;
+	double m_twistLimit;
+	UrdfJoint()
+		: m_lowerLimit(0),
+		  m_upperLimit(-1),
+		  m_effortLimit(0),
+		  m_velocityLimit(0),
+		  m_jointDamping(0),
+		  m_jointFriction(0),
+		  m_twistLimit(-1)
+	{
+	}
+};
+
+struct SpringCoeffcients
+{
+	double elastic_stiffness;
+	double damping_stiffness;
+	double bending_stiffness;
+	int damp_all_directions;
+	int bending_stride;
+	SpringCoeffcients() : elastic_stiffness(0.),
+						  damping_stiffness(0.),
+						  bending_stiffness(0.),
+						  damp_all_directions(0),
+						  bending_stride(2) {}
+};
+
+struct LameCoefficients
+{
+	double mu;
+	double lambda;
+	double damping;
+	LameCoefficients() : mu(0.), lambda(0.), damping(0.) {}
+};
+
+struct UrdfDeformable
+{
+	std::string m_name;
+	double m_mass;
+	double m_collisionMargin;
+	double m_friction;
+	double m_repulsionStiffness;
+	double m_gravFactor;
+	bool m_cache_barycenter;
+
+	SpringCoeffcients m_springCoefficients;
+	LameCoefficients m_corotatedCoefficients;
+	LameCoefficients m_neohookeanCoefficients;
+
+	std::string m_visualFileName;
+	std::string m_simFileName;
+	btHashMap<btHashString, std::string> m_userData;
+
+	UrdfDeformable() : m_mass(1.), m_collisionMargin(0.02), m_friction(1.), m_repulsionStiffness(0.5), m_gravFactor(1.), m_cache_barycenter(false), m_visualFileName(""), m_simFileName("")
+	{
+	}
+};
+
+struct UrdfReducedDeformable
+{
+	std::string m_name;
+	int m_numModes;
+
+	double m_mass;
+	double m_stiffnessScale;
+	double m_erp;
+	double m_cfm;
+	double m_friction;
+	double m_collisionMargin;
+	double m_damping;
+
+	std::string m_visualFileName;
+	std::string m_simFileName;
+	btHashMap<btHashString, std::string> m_userData;
+
+	UrdfReducedDeformable() 
+		:	m_numModes(1),
+			m_mass(1),
+			m_stiffnessScale(100),
+			m_erp(0.2),					// generally, 0.2 is a good value for erp and cfm
+			m_cfm(0.2),
+			m_friction(0),
+			m_collisionMargin(0.02),
+			m_damping(0),
+			m_visualFileName(""),
+			m_simFileName("")
+	{}
+};
+
+struct UrdfModel
+{
+	std::string m_name;
+	std::string m_sourceFile;
+	btTransform m_rootTransformInWorld;
+	btHashMap<btHashString, UrdfMaterial*> m_materials;
+	btHashMap<btHashString, UrdfLink*> m_links;
+	btHashMap<btHashString, UrdfJoint*> m_joints;
+	UrdfDeformable m_deformable;
+	UrdfReducedDeformable m_reducedDeformable;
+	// Maps user data keys to user data values.
+	btHashMap<btHashString, std::string> m_userData;
+
+	btArray<UrdfLink*> m_rootLinks;
+	bool m_overrideFixedBase;
+
+	UrdfModel()
+		: m_overrideFixedBase(false)
+	{
+		m_rootTransformInWorld.setIdentity();
+	}
+
+	~UrdfModel()
+	{
+		for (int i = 0; i < m_materials.size(); i++)
+		{
+			UrdfMaterial** ptr = m_materials.getAtIndex(i);
+			if (ptr)
+			{
+				UrdfMaterial* t = *ptr;
+				delete t;
+			}
+		}
+		for (int i = 0; i < m_links.size(); i++)
+		{
+			UrdfLink** ptr = m_links.getAtIndex(i);
+			if (ptr)
+			{
+				UrdfLink* t = *ptr;
+				delete t;
+			}
+		}
+		for (int i = 0; i < m_joints.size(); i++)
+		{
+			UrdfJoint** ptr = m_joints.getAtIndex(i);
+			if (ptr)
+			{
+				UrdfJoint* t = *ptr;
+				delete t;
+			}
+		}
+	}
+};
+
+namespace tinyxml2
+{
+class XMLElement;
+};
+
+class UrdfParser
+{
+protected:
+	UrdfModel m_urdf2Model;
+	btAlignedObjectArray<UrdfModel*> m_sdfModels;
+	btAlignedObjectArray<UrdfModel*> m_tmpModels;
+
+	bool m_parseSDF;
+	int m_activeSdfModel;
+
+	btScalar m_urdfScaling;
+
+	struct CommonFileIOInterface* m_fileIO;
+
+	bool parseTransform(btTransform& tr, tinyxml2::XMLElement* xml, ErrorLogger* logger, bool parseSDF = false);
+	bool parseInertia(UrdfInertia& inertia, tinyxml2::XMLElement* config, ErrorLogger* logger);
+	bool parseGeometry(UrdfGeometry& geom, tinyxml2::XMLElement* g, ErrorLogger* logger);
+	bool parseVisual(UrdfModel& model, UrdfVisual& visual, tinyxml2::XMLElement* config, ErrorLogger* logger);
+	bool parseCollision(UrdfCollision& collision, tinyxml2::XMLElement* config, ErrorLogger* logger);
+	bool initTreeAndRoot(UrdfModel& model, ErrorLogger* logger);
+	bool parseMaterial(UrdfMaterial& material, tinyxml2::XMLElement* config, ErrorLogger* logger);
+	bool parseJointLimits(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
+	bool parseJointDynamics(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
+	bool parseJoint(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
+	bool parseLink(UrdfModel& model, UrdfLink& link, tinyxml2::XMLElement* config, ErrorLogger* logger);
+	bool parseSensor(UrdfModel& model, UrdfLink& link, UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
+  bool parseLameCoefficients(LameCoefficients& lameCoefficients, tinyxml2::XMLElement* config, ErrorLogger* logger);
+	bool parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config, ErrorLogger* logger);
+	bool parseReducedDeformable(UrdfModel& model, tinyxml2::XMLElement* config, ErrorLogger* logger);
+
+public:
+	UrdfParser(struct CommonFileIOInterface* fileIO);
+	virtual ~UrdfParser();
+
+	void setParseSDF(bool useSDF)
+	{
+		m_parseSDF = useSDF;
+	}
+	bool getParseSDF() const
+	{
+		return m_parseSDF;
+	}
+	void setGlobalScaling(btScalar scaling)
+	{
+		m_urdfScaling = scaling;
+	}
+
+	bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase, bool parseSensors);
+
+	bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase)
+	{
+		return loadUrdf(urdfText, logger, forceFixedBase, false);
+	}
+
+	bool loadSDF(const char* sdfText, ErrorLogger* logger);
+
+	int getNumModels() const
+	{
+		//user should have loaded an SDF when calling this method
+		if (m_parseSDF)
+		{
+			return m_sdfModels.size();
+		}
+		return 1;
+	}
+
+	void activateModel(int modelIndex);
+
+	UrdfModel& getModelByIndex(int index)
+	{
+		//user should have loaded an SDF when calling this method
+		btAssert(m_parseSDF);
+
+		return *m_sdfModels[index];
+	}
+
+	const UrdfModel& getModelByIndex(int index) const
+	{
+		//user should have loaded an SDF when calling this method
+		btAssert(m_parseSDF);
+
+		return *m_sdfModels[index];
+	}
+
+	const UrdfModel& getModel() const
+	{
+		if (m_parseSDF)
+		{
+			return *m_sdfModels[m_activeSdfModel];
+		}
+
+		return m_urdf2Model;
+	}
+
+	UrdfModel& getModel()
+	{
+		if (m_parseSDF)
+		{
+			return *m_sdfModels[m_activeSdfModel];
+		}
+		return m_urdf2Model;
+	}
+
+	const UrdfDeformable& getDeformable() const
+	{
+		return m_urdf2Model.m_deformable;
+	}
+
+	const UrdfReducedDeformable& getReducedDeformable() const
+	{
+		return m_urdf2Model.m_reducedDeformable;
+	}
+
+	bool mergeFixedLinks(UrdfModel& model, UrdfLink* link, ErrorLogger* logger, bool forceFixedBase, int level);
+	bool printTree(UrdfLink* link, ErrorLogger* logger, int level);
+	bool recreateModel(UrdfModel& model, UrdfLink* link, ErrorLogger* logger);
+
+	std::string sourceFileLocation(tinyxml2::XMLElement* e);
+
+	void setSourceFile(const std::string& sourceFile)
+	{
+		m_urdf2Model.m_sourceFile = sourceFile;
+	}
+};
+
+#endif

Engine/lib/bullet/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h

@@ -0,0 +1,115 @@
+#ifndef URDF_RENDERING_INTERFACE_H
+#define URDF_RENDERING_INTERFACE_H
+
+///UrdfLink and UrdfModel are the main URDF structures to define a robot
+struct UrdfLink;
+struct UrdfModel;
+///btTransform is a position and 3x3 matrix, as defined in Bullet/src/LinearMath/btTransform
+class btTransform;
+class btVector3;
+
+///UrdfRenderingInterface is a simple rendering interface, mainly for URDF-based robots.
+///There is an implementation in
+///bullet3\examples\SharedMemory\plugins\tinyRendererPlugin\TinyRendererVisualShapeConverter.cpp
+struct UrdfRenderingInterface
+{
+	virtual ~UrdfRenderingInterface() {}
+	///given a URDF link, convert all visual shapes into internal renderer (loading graphics meshes, textures etc)
+	///use the visualShapeUniqueId as a unique identifier to synchronize the world transform and to remove the visual shape.
+	virtual int convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int visualShapeUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO) = 0;
+
+	virtual int registerShapeAndInstance(const struct b3VisualShapeData& visualShape, const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType, int textureId, int orgGraphicsUniqueId, int bodyUniqueId, int linkIndex)=0;
+
+	virtual void updateShape(int shapeUniqueId, const btVector3* vertices, int numVertices, const btVector3* normals, int numNormals) = 0;
+
+	///remove a visual shapes, based on the shape unique id (shapeUid)
+	virtual void removeVisualShape(int collisionObjectUid) = 0;
+
+	///update the world transform + scaling of the visual shape, using the shapeUid
+	virtual void syncTransform(int collisionObjectUid, const class btTransform& worldTransform, const class btVector3& localScaling) = 0;
+
+	///return the number of visual shapes, for a particular body unique id
+	virtual int getNumVisualShapes(int bodyUniqueId) = 0;
+
+	///return the visual shape information, for a particular body unique id and 'shape index'
+	virtual int getVisualShapesData(int bodyUniqueId, int shapeIndex, struct b3VisualShapeData* shapeData) = 0;
+
+	///change the RGBA color for some visual shape.
+	virtual void changeRGBAColor(int bodyUniqueId, int linkIndex, int shapeIndex, const double rgbaColor[4]) = 0;
+
+	//change the instance flags, double-sided rendering
+	virtual void changeInstanceFlags(int bodyUniqueId, int linkIndex, int shapeIndex, int flags) = 0;
+
+	///select a given texture for some visual shape.
+	virtual void changeShapeTexture(int objectUniqueId, int linkIndex, int shapeIndex, int textureUniqueId) = 0;
+
+	///pick the up-axis, either Y (1) or Z (2)
+	virtual void setUpAxis(int axis) = 0;
+
+	///compute the view matrix based on those parameters
+	virtual void resetCamera(float camDist, float yaw, float pitch, float camPosX, float camPosY, float camPosZ) = 0;
+
+	///clear the render buffer with a particular color.
+	virtual void clearBuffers(struct TGAColor& clearColor) = 0;
+
+	///remove all visual shapes.
+	virtual void resetAll() = 0;
+
+	///return the frame buffer width and height for the renderer
+	virtual void getWidthAndHeight(int& width, int& height) = 0;
+
+	///set the frame buffer width and height for the renderer
+	virtual void setWidthAndHeight(int width, int height) = 0;
+
+	///set the light direction, in world coordinates
+	virtual void setLightDirection(float x, float y, float z) = 0;
+
+	///set the ambient light color, in world coordinates
+	virtual void setLightColor(float x, float y, float z) = 0;
+
+	///set the light distance
+	virtual void setLightDistance(float dist) = 0;
+
+	///set the light ambient coefficient
+	virtual void setLightAmbientCoeff(float ambientCoeff) = 0;
+
+	///set the light diffuse coefficient
+	virtual void setLightDiffuseCoeff(float diffuseCoeff) = 0;
+
+	///set the light specular coefficient
+	virtual void setLightSpecularCoeff(float specularCoeff) = 0;
+
+	///enable or disable rendering of shadows
+	virtual void setShadow(bool hasShadow) = 0;
+
+	///some undocumented flags for experimentation (todo: document)
+	virtual void setFlags(int flags) = 0;
+
+	///provide the image pixels as a part of a stream.
+	virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied) = 0;
+
+	///render an image, using some arbitraty view and projection matrix
+	virtual void render() = 0;
+
+	///render an image using the provided view and projection matrix
+	virtual void render(const float viewMat[16], const float projMat[16]) = 0;
+
+	///load a texture from file, in png or other popular/supported format
+	//virtual int loadTextureFile(const char* filename) = 0;
+	virtual int loadTextureFile(const char* filename, struct CommonFileIOInterface* fileIO)=0;
+	
+	///register a texture using an in-memory pixel buffer of a given width and height
+	virtual int registerTexture(unsigned char* texels, int width, int height) = 0;
+
+	virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16]) {}
+	virtual void setProjectiveTexture(bool useProjectiveTexture) {}
+
+
+	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
+	{
+		return false;
+	}
+
+};
+
+#endif  //LINK_VISUAL_SHAPES_CONVERTER_H

Engine/lib/bullet/examples/Importers/ImportURDFDemo/urdfLexicalCast.h

@@ -0,0 +1,13 @@
+#ifndef BOOST_REPLACEMENT_LEXICAL_CAST_H
+#define BOOST_REPLACEMENT_LEXICAL_CAST_H
+
+#include <stdlib.h>
+
+template <typename T>
+T urdfLexicalCast(const char* txt)
+{
+	double result = atof(txt);
+	return result;
+};
+
+#endif

Engine/lib/bullet/examples/Importers/ImportURDFDemo/urdfStringSplit.cpp

@@ -0,0 +1,256 @@
+
+
+#include <assert.h>
+//#include <stdbool.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "urdfStringSplit.h"
+
+void urdfStringSplit(btAlignedObjectArray<std::string> &pieces, const std::string &vector_str, const btAlignedObjectArray<std::string> &separators)
+{
+	assert(separators.size() == 1);
+	if (separators.size() == 1)
+	{
+		char **strArray = urdfStrSplit(vector_str.c_str(), separators[0].c_str());
+		int numSubStr = urdfStrArrayLen(strArray);
+		for (int i = 0; i < numSubStr; i++)
+			pieces.push_back(std::string(strArray[i]));
+		urdfStrArrayFree(strArray);
+	}
+}
+void urdfIsAnyOf(const char *seps, btAlignedObjectArray<std::string> &strArray)
+{
+	int numSeps = strlen(seps);
+	for (int i = 0; i < numSeps; i++)
+	{
+		char sep2[2] = {0, 0};
+
+		sep2[0] = seps[i];
+		strArray.push_back(sep2);
+	}
+}
+
+/* Append an item to a dynamically allocated array of strings. On failure,
+ return NULL, in which case the original array is intact. The item
+ string is dynamically copied. If the array is NULL, allocate a new
+ array. Otherwise, extend the array. Make sure the array is always
+ NULL-terminated. Input string might not be '\0'-terminated. */
+char **urdfStrArrayAppend(char **array, size_t nitems, const char *item,
+						  size_t itemlen)
+{
+	/* Make a dynamic copy of the item. */
+	char *copy;
+	if (item == NULL)
+		copy = NULL;
+	else
+	{
+		copy = (char *)malloc(itemlen + 1);
+		if (copy == NULL)
+			return NULL;
+		memcpy(copy, item, itemlen);
+		copy[itemlen] = '\0';
+	}
+
+	/* Extend array with one element. Except extend it by two elements,
+	 in case it did not yet exist. This might mean it is a teeny bit
+	 too big, but we don't care. */
+	array = (char **)realloc(array, (nitems + 2) * sizeof(array[0]));
+	if (array == NULL)
+	{
+		free(copy);
+		return NULL;
+	}
+
+	/* Add copy of item to array, and return it. */
+	array[nitems] = copy;
+	array[nitems + 1] = NULL;
+	return array;
+}
+
+/* Free a dynamic array of dynamic strings. */
+void urdfStrArrayFree(char **array)
+{
+	if (array == NULL)
+		return;
+	for (size_t i = 0; array[i] != NULL; ++i)
+		free(array[i]);
+	free(array);
+}
+
+/* Split a string into substrings. Return dynamic array of dynamically
+ allocated substrings, or NULL if there was an error. Caller is
+ expected to free the memory, for example with str_array_free. */
+char **urdfStrSplit(const char *input, const char *sep)
+{
+	size_t nitems = 0;
+	char **array = NULL;
+	const char *start = input;
+	const char *next = strstr(start, sep);
+	size_t seplen = strlen(sep);
+	const char *item;
+	size_t itemlen;
+
+	for (;;)
+	{
+		next = strstr(start, sep);
+		if (next == NULL)
+		{
+			/* Add the remaining string (or empty string, if input ends with
+			 separator. */
+			char **newstr = urdfStrArrayAppend(array, nitems, start, strlen(start));
+			if (newstr == NULL)
+			{
+				urdfStrArrayFree(array);
+				return NULL;
+			}
+			array = newstr;
+			++nitems;
+			break;
+		}
+		else if (next == input)
+		{
+			/* Input starts with separator. */
+			item = "";
+			itemlen = 0;
+		}
+		else
+		{
+			item = start;
+			itemlen = next - item;
+		}
+		char **newstr = urdfStrArrayAppend(array, nitems, item, itemlen);
+		if (newstr == NULL)
+		{
+			urdfStrArrayFree(array);
+			return NULL;
+		}
+		array = newstr;
+		++nitems;
+		start = next + seplen;
+	}
+
+	if (nitems == 0)
+	{
+		/* Input does not contain separator at all. */
+		assert(array == NULL);
+		array = urdfStrArrayAppend(array, nitems, input, strlen(input));
+	}
+
+	return array;
+}
+
+/* Return length of a NULL-delimited array of strings. */
+size_t urdfStrArrayLen(char **array)
+{
+	size_t len;
+
+	for (len = 0; array[len] != NULL; ++len)
+		continue;
+	return len;
+}
+
+#ifdef UNIT_TEST_STRING_SPLIT
+
+#define MAX_OUTPUT 20
+
+int main(void)
+{
+	struct
+	{
+		const char *input;
+		const char *sep;
+		char *output[MAX_OUTPUT];
+	} tab[] = {
+		/* Input is empty string. Output should be a list with an empty
+		 string. */
+		{
+			"",
+			"and",
+			{
+				"",
+				NULL,
+			},
+		},
+		/* Input is exactly the separator. Output should be two empty
+		 strings. */
+		{
+			"and",
+			"and",
+			{
+				"",
+				"",
+				NULL,
+			},
+		},
+		/* Input is non-empty, but does not have separator. Output should
+		 be the same string. */
+		{
+			"foo",
+			"and",
+			{
+				"foo",
+				NULL,
+			},
+		},
+		/* Input is non-empty, and does have separator. */
+		{
+			"foo bar 1 and foo bar 2",
+			" and ",
+			{
+				"foo bar 1",
+				"foo bar 2",
+				NULL,
+			},
+		},
+	};
+	const int tab_len = sizeof(tab) / sizeof(tab[0]);
+	bool errors;
+
+	errors = false;
+
+	for (int i = 0; i < tab_len; ++i)
+	{
+		printf("test %d\n", i);
+
+		char **output = str_split(tab[i].input, tab[i].sep);
+		if (output == NULL)
+		{
+			fprintf(stderr, "output is NULL\n");
+			errors = true;
+			break;
+		}
+		size_t num_output = str_array_len(output);
+		printf("num_output %lu\n", (unsigned long)num_output);
+
+		size_t num_correct = str_array_len(tab[i].output);
+		if (num_output != num_correct)
+		{
+			fprintf(stderr, "wrong number of outputs (%lu, not %lu)\n",
+					(unsigned long)num_output, (unsigned long)num_correct);
+			errors = true;
+		}
+		else
+		{
+			for (size_t j = 0; j < num_output; ++j)
+			{
+				if (strcmp(tab[i].output[j], output[j]) != 0)
+				{
+					fprintf(stderr, "output[%lu] is '%s' not '%s'\n",
+							(unsigned long)j, output[j], tab[i].output[j]);
+					errors = true;
+					break;
+				}
+			}
+		}
+
+		str_array_free(output);
+		printf("\n");
+	}
+
+	if (errors)
+		return EXIT_FAILURE;
+	return 0;
+}
+#endif  //UNIT_TEST_STRING_SPLIT

Engine/lib/bullet/examples/Importers/ImportURDFDemo/urdfStringSplit.h

@@ -0,0 +1,39 @@
+
+#ifndef STRING_SPLIT_H
+#define STRING_SPLIT_H
+
+#ifdef __cplusplus
+
+#include <cstring>
+#include <string>
+#include "LinearMath/btAlignedObjectArray.h"
+
+void urdfStringSplit(btAlignedObjectArray<std::string>& pieces, const std::string& vector_str, const btAlignedObjectArray<std::string>& separators);
+
+void urdfIsAnyOf(const char* seps, btAlignedObjectArray<std::string>& strArray);
+#endif
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+	///The string split C code is by Lars Wirzenius
+	///See http://stackoverflow.com/questions/2531605/how-to-split-a-string-with-a-delimiter-larger-than-one-single-char
+
+	/* Split a string into substrings. Return dynamic array of dynamically
+ allocated substrings, or NULL if there was an error. Caller is
+ expected to free the memory, for example with str_array_free. */
+	char** urdfStrSplit(const char* input, const char* sep);
+
+	/* Free a dynamic array of dynamic strings. */
+	void urdfStrArrayFree(char** array);
+
+	/* Return length of a NULL-delimited array of strings. */
+	size_t urdfStrArrayLen(char** array);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  //STRING_SPLIT_H

Engine/lib/bullet/examples/Importers/ImportURDFDemo/urdf_samples.h

@@ -0,0 +1,823 @@
+#ifndef URDF_SAMPLES_H
+#define URDF_SAMPLES_H
+
+#define MSTRINGIFY(A) #A
+
+const char* urdf_char2 = MSTRINGIFY(
+	<robot name = "test_robot">
+	<link name = "link1" />
+	<link name = "link2" />
+	<link name = "link3" />
+	<link name = "link4" />
+
+	<joint name = "joint1" type = "continuous">
+	<parent link = "link1" />
+	<child link = "link2" />
+	</ joint>
+
+	<joint name = "joint2" type = "continuous">
+	<parent link = "link1" />
+	<child link = "link3" />
+	</ joint>
+
+	<joint name = "joint3" type = "continuous">
+	<parent link = "link3" />
+	<child link = "link4" />
+	</ joint>
+	</ robot>);
+
+const char* urdf_char1 = MSTRINGIFY(
+                                   <?xml version="1.0"?>
+                                   <robot name="myfirst">
+                                   <link name="base_link">
+                                   <visual>
+                                   <geometry>
+                                   <cylinder length="0.6" radius="0.2"/>
+                                   </geometry>
+                                   </visual>
+                                   </link>
+                                   </robot>
+                                   );
+
+const char* urdf_char3 = MSTRINGIFY(<?xml version="1.0"?>
+                                   <robot name="multipleshapes">
+                                   <link name="base_link">
+                                   <visual>
+                                   <geometry>
+                                   <cylinder length="0.6" radius="0.2"/>
+                                   </geometry>
+                                   </visual>
+                                   </link>
+                                   
+                                   <link name="right_leg">
+                                   <visual>
+                                   <geometry>
+                                   <box size="0.6 .2 .1"/>
+                                   </geometry>
+                                   </visual>
+                                   </link>
+                                   
+                                   <joint name="base_to_right_leg" type="fixed">
+                                   <parent link="base_link"/>
+                                   <child link="right_leg"/>
+                                   </joint>
+                                   
+                                   </robot>);
+
+const char* urdf_char4 = MSTRINGIFY(
+
+								   <?xml version="1.0"?>
+								   <robot name="materials">
+								   <link name="base_link">
+								   <visual>
+								   <geometry>
+								   <cylinder length="0.6" radius="0.2"/>
+								   </geometry>
+								   <material name="blue">
+								   <color rgba="0 0 .8 1"/>
+								   </material>
+								   </visual>
+								   </link>
+								   
+								   <link name="right_leg">
+								   <visual>
+								   <geometry>
+								   <box size="0.6 .2 .1"/>
+								   </geometry>
+								   <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+								   <material name="white">
+								   <color rgba="1 1 1 1"/>
+								   </material>
+								   </visual>
+								   </link>
+								   
+								   <joint name="base_to_right_leg" type="fixed">
+								   <parent link="base_link"/>
+								   <child link="right_leg"/>
+								   <origin xyz="0.22 0 .25"/>
+								   </joint>
+								   
+								   <link name="left_leg">
+								   <visual>
+								   <geometry>
+								   <box size="0.6 .2 .1"/>
+								   </geometry>
+								   <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+								   <material name="white"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="base_to_left_leg" type="fixed">
+								   <parent link="base_link"/>
+								   <child link="left_leg"/>
+								   <origin xyz="-0.22 0 .25"/>
+								   </joint>
+								   
+								   <link name="head">
+								   <visual>
+								   <geometry>
+								   <sphere radius="0.2"/>
+								   </geometry>
+								   <material name="white"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="head_swivel" type="fixed">
+								   <parent link="base_link"/>
+								   <child link="head"/>
+								   <origin xyz="0 0 0.3"/>
+								   </joint>
+
+								   <link name="box">
+								   <visual>
+								   <geometry>
+								   <box size=".08 .08 .08"/>
+								   </geometry>
+								   <material name="blue"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="tobox" type="fixed">
+								   <parent link="head"/>
+								   <child link="box"/>
+								   <origin xyz="0 0.1414 0.1414"/>
+								   </joint>
+								   
+								   </robot>
+
+
+);
+
+const char* urdf_char_r2d2 = MSTRINGIFY(
+
+								   <?xml version="1.0"?>
+								   <robot name="visual">
+								   <link name="base_link">
+								   <visual>
+								   <geometry>
+								   <cylinder length="0.6" radius="0.2"/>
+								   </geometry>
+								   <material name="blue">
+								   <color rgba="0 0 .8 1"/>
+								   </material>
+								   </visual>
+								   </link>
+								   
+								   <link name="right_leg">
+								   <visual>
+								   <geometry>
+								   <box size="0.6 .2 .1"/>
+								   </geometry>
+								   <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+								   <material name="white">
+								   <color rgba="1 1 1 1"/>
+								   </material>
+								   </visual>
+								   </link>
+								   
+								   <joint name="base_to_right_leg" type="fixed">
+								   <parent link="base_link"/>
+								   <child link="right_leg"/>
+								   <origin xyz="0.22 0 .25"/>
+								   </joint>
+								   
+								   <link name="right_base">
+								   <visual>
+								   <geometry>
+								   <box size=".1 0.4 .1"/>
+								   </geometry>
+								   <material name="white"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="right_base_joint" type="fixed">
+								   <parent link="right_leg"/>
+								   <child link="right_base"/>
+								   <origin xyz="0 0 -0.6"/>
+								   </joint>
+								   
+								   <link name="right_front_wheel">
+								   <visual>
+								   <geometry>
+								   <cylinder length=".1" radius="0.035"/>
+								   </geometry>
+								   <material name="black">
+								   <color rgba="0 0 0 1"/>
+								   </material>
+								   </visual>
+								   </link>
+								   
+								   <joint name="right_front_wheel_joint" type="fixed">
+								   <parent link="right_base"/>
+								   <child link="right_front_wheel"/>
+								   <origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
+								   </joint>
+								   
+								   <link name="right_back_wheel">
+								   <visual>
+								   <geometry>
+								   <cylinder length=".1" radius="0.035"/>
+								   </geometry>
+								   <material name="black"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="right_back_wheel_joint" type="fixed">
+								   <parent link="right_base"/>
+								   <child link="right_back_wheel"/>
+								   <origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
+								   </joint>
+								   
+								   <link name="left_leg">
+								   <visual>
+								   <geometry>
+								   <box size="0.6 .2 .1"/>
+								   </geometry>
+								   <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+								   <material name="white"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="base_to_left_leg" type="fixed">
+								   <parent link="base_link"/>
+								   <child link="left_leg"/>
+								   <origin xyz="-0.22 0 .25"/>
+								   </joint>
+								   
+								   <link name="left_base">
+								   <visual>
+								   <geometry>
+								   <box size=".1 0.4 .1"/>
+								   </geometry>
+								   <material name="white"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="left_base_joint" type="fixed">
+								   <parent link="left_leg"/>
+								   <child link="left_base"/>
+								   <origin xyz="0 0 -0.6"/>
+								   </joint>
+								   
+								   <link name="left_front_wheel">
+								   <visual>
+								   <geometry>
+								   <cylinder length=".1" radius="0.035"/>
+								   </geometry>
+								   <material name="black"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="left_front_wheel_joint" type="fixed">
+								   <parent link="left_base"/>
+								   <child link="left_front_wheel"/>
+								   <origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
+								   </joint>
+								   
+								   <link name="left_back_wheel">
+								   <visual>
+								   <geometry>
+								   <cylinder length=".1" radius="0.035"/>
+								   </geometry>
+								   <material name="black"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="left_back_wheel_joint" type="fixed">
+								   <parent link="left_base"/>
+								   <child link="left_back_wheel"/>
+								   <origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
+								   </joint>
+								   
+								   <joint name="gripper_extension" type="fixed">
+								   <parent link="base_link"/>
+								   <child link="gripper_pole"/>
+								   <origin rpy="0 0 1.57075" xyz="0 0.19 .2"/>
+								   </joint>
+								   
+								   <link name="gripper_pole">
+								   <visual>
+								   <geometry>
+								   <cylinder length="0.2" radius=".01"/>
+								   </geometry>
+								   <origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
+								   <material name="Gray">
+								   <color rgba=".7 .7 .7 1"/>
+								   </material>
+								   </visual>
+								   </link>
+								   
+								   <joint name="left_gripper_joint" type="fixed">
+								   <origin rpy="0 0 0" xyz="0.2 0.01 0"/>
+								   <parent link="gripper_pole"/>
+								   <child link="left_gripper"/>
+								   </joint>
+								   
+								   <link name="left_gripper">
+								   <visual>
+								   <origin rpy="0.0 0 0" xyz="0 0 0"/>
+								   <geometry>
+								   <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
+								   </geometry>
+								   </visual>
+								   </link>
+								   
+								   <joint name="left_tip_joint" type="fixed">
+								   <parent link="left_gripper"/>
+								   <child link="left_tip"/>
+								   </joint>
+								   
+								   <link name="left_tip">
+								   <visual>
+								   <origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
+								   <geometry>
+								   <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
+								   </geometry>
+								   </visual>
+								   </link>
+								   
+								   <joint name="right_gripper_joint" type="fixed">
+								   <origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
+								   <parent link="gripper_pole"/>
+								   <child link="right_gripper"/>
+								   </joint>
+								   
+								   <link name="right_gripper">
+								   <visual>
+								   <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
+								   <geometry>
+								   <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
+								   </geometry>
+								   </visual>
+								   </link>
+								   
+								   <joint name="right_tip_joint" type="fixed">
+								   <parent link="right_gripper"/>
+								   <child link="right_tip"/>
+								   </joint>
+								   
+								   <link name="right_tip">
+								   <visual>
+								   <origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
+								   <geometry>
+								   <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
+								   </geometry>
+								   </visual>
+								   </link>
+								   
+								   <link name="head">
+								   <visual>
+								   <geometry>
+								   <sphere radius="0.2"/>
+								   </geometry>
+								   <material name="white"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="head_swivel" type="fixed">
+								   <parent link="base_link"/>
+								   <child link="head"/>
+								   <origin xyz="0 0 0.3"/>
+								   </joint>
+								   
+								   <link name="box">
+								   <visual>
+								   <geometry>
+								   <box size=".08 .08 .08"/>
+								   </geometry>
+								   <material name="blue"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="tobox" type="fixed">
+								   <parent link="head"/>
+								   <child link="box"/>
+								   <origin xyz="0 0.1414 0.1414"/>
+								   </joint>
+								   
+								   </robot>
+);
+
+const char* urdf_char = MSTRINGIFY(
+
+								<?xml version="1.0"?>
+<robot name="physics">
+  <link name="base_link">
+    <visual>
+      <geometry>
+        <cylinder length="0.6" radius="0.2"/>
+      </geometry>
+      <material name="blue">
+        <color rgba="0 0 .8 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <cylinder length="0.6" radius="0.2"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="10"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+
+  <link name="right_leg">
+    <visual>
+      <geometry>
+        <box size="0.6 .2 .1"/>
+      </geometry>
+      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.6 .2 .1"/>
+      </geometry>
+      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+    </collision>
+    <inertial>
+      <mass value="10"/>
+      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+
+  <joint name="base_to_right_leg" type="fixed">
+    <parent link="base_link"/>
+    <child link="right_leg"/>
+    <origin xyz="0.22 0 .25"/>
+  </joint>
+
+  <link name="right_base">
+    <visual>
+      <geometry>
+        <box size=".1 0.4 .1"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <geometry>
+        <box size=".1 0.4 .1"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="10"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+
+  <joint name="right_base_joint" type="fixed">
+    <parent link="right_leg"/>
+    <child link="right_base"/>
+    <origin xyz="0 0 -0.6"/>
+  </joint>
+
+  <link name="right_front_wheel">
+    <visual>
+      <geometry>
+        <cylinder length=".1" radius="0.035"/>
+      </geometry>
+      <material name="black">
+        <color rgba="0 0 0 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <cylinder length=".1" radius="0.035"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+
+  <joint name="right_front_wheel_joint" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="right_base"/>
+    <child link="right_front_wheel"/>
+    <origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
+    <limit effort="100" velocity="100"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+  </joint>
+
+  <link name="right_back_wheel">
+    <visual>
+      <geometry>
+        <cylinder length=".1" radius="0.035"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <geometry>
+        <cylinder length=".1" radius="0.035"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+
+  <joint name="right_back_wheel_joint" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="right_base"/>
+    <child link="right_back_wheel"/>
+    <origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
+    <limit effort="100" velocity="100"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+  </joint>
+
+  <link name="left_leg">
+    <visual>
+      <geometry>
+        <box size="0.6 .2 .1"/>
+      </geometry>
+      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.6 .2 .1"/>
+      </geometry>
+      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+    </collision>
+    <inertial>
+      <mass value="10"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+       <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+    </inertial>
+  </link>
+
+  <joint name="base_to_left_leg" type="fixed">
+    <parent link="base_link"/>
+    <child link="left_leg"/>
+    <origin xyz="-0.22 0 .25"/>
+  </joint>
+
+  <link name="left_base">
+    <visual>
+      <geometry>
+        <box size=".1 0.4 .1"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <geometry>
+        <box size=".1 0.4 .1"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="10"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+
+  <joint name="left_base_joint" type="fixed">
+    <parent link="left_leg"/>
+    <child link="left_base"/>
+    <origin xyz="0 0 -0.6"/>
+  </joint>
+
+  <link name="left_front_wheel">
+    <visual>
+      <geometry>
+        <cylinder length=".1" radius="0.035"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <geometry>
+        <cylinder length=".1" radius="0.035"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+
+  <joint name="left_front_wheel_joint" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="left_base"/>
+    <child link="left_front_wheel"/>
+    <origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
+    <limit effort="100" velocity="100"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+  </joint>
+
+  <link name="left_back_wheel">
+    <visual>
+      <geometry>
+        <cylinder length=".1" radius="0.035"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+    <collision>
+      <geometry>
+        <cylinder length=".1" radius="0.035"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+
+  <joint name="left_back_wheel_joint" type="continuous">
+    <axis xyz="0 0 1"/>
+    <parent link="left_base"/>
+    <child link="left_back_wheel"/>
+    <origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
+    <limit effort="100" velocity="100"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+  </joint>
+  <joint name="gripper_extension" type="prismatic">
+    <parent link="base_link"/>
+    <child link="gripper_pole"/>
+    <limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
+    <origin rpy="0 0 1.57075" xyz="0 0.19 .2"/>
+  </joint>
+
+  <link name="gripper_pole">
+    <visual>
+      <geometry>
+        <cylinder length="0.2" radius=".01"/>
+      </geometry>
+      <origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
+      <material name="Gray">
+        <color rgba=".7 .7 .7 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <cylinder length="0.2" radius=".01"/>
+      </geometry>
+      <origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
+    </collision>
+    <inertial>
+      <mass value="0.05"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+
+  <joint name="left_gripper_joint" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+    <origin rpy="0 0 0" xyz="0.2 0.01 0"/>
+    <parent link="gripper_pole"/>
+    <child link="left_gripper"/>
+  </joint>
+
+  <link name="left_gripper">
+    <visual>
+      <origin rpy="0.0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="l_finger.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="l_finger.stl"/>
+      </geometry>
+      <origin rpy="0.0 0 0" xyz="0 0 0"/>
+    </collision>
+    <inertial>
+      <mass value="0.05"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+
+  <joint name="left_tip_joint" type="fixed">
+    <parent link="left_gripper"/>
+    <child link="left_tip"/>
+  </joint>
+
+  <link name="left_tip">
+    <visual>
+      <origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
+      <geometry>
+        <mesh filename="l_finger_tip.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="l_finger_tip.stl"/>
+      </geometry>
+      <origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
+    </collision>
+    <inertial>
+      <mass value="0.05"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+
+  <joint name="right_gripper_joint" type="revolute">
+    <axis xyz="0 0 -1"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+    <origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
+    <parent link="gripper_pole"/>
+    <child link="right_gripper"/>
+  </joint>
+
+  <link name="right_gripper">
+    <visual>
+      <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="l_finger.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="l_finger.stl"/>
+      </geometry>
+      <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
+    </collision>
+    <inertial>
+      <mass value="0.05"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+
+  <joint name="right_tip_joint" type="fixed">
+    <parent link="right_gripper"/>
+    <child link="right_tip"/>
+  </joint>
+
+  <link name="right_tip">
+    <visual>
+      <origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
+      <geometry>
+        <mesh filename="l_finger_tip.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="l_finger_tip.stl"/>
+      </geometry>
+      <origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
+    </collision>
+    <inertial>
+      <mass value="0.05"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+
+  <link name="head">
+    <visual>
+      <geometry>
+        <sphere radius="0.2"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+    <collision>
+      <geometry>
+        <sphere radius="0.2"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="10"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+
+  <joint name="head_swivel" type="continuous">
+    <parent link="base_link"/>
+    <child link="head"/>
+    <axis xyz="0 0 1"/>
+    <origin xyz="0 0 0.3"/>
+  </joint>
+
+  <link name="box">
+    <visual>
+      <geometry>
+        <box size=".08 .08 .08"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <geometry>
+        <box size=".08 .08 .08"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+
+  <joint name="tobox" type="fixed">
+    <parent link="head"/>
+    <child link="box"/>
+    <origin xyz="0 0.1414 0.1414"/>
+  </joint>
+
+</robot>
+
+
+);
+
+#endif  //URDF_SAMPLES_H