* Adjustment: Initial CMake reworking.

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

@@ -0,0 +1,207 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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,39 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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,730 @@
+/*
+===========================================================================
+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,295 @@
+/*
+===========================================================================
+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,304 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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 = -175;
+		float yaw = 12;
+		float targetPos[3]={4,-25,-6};
+		m_guiHelper->resetCamera(dist,pitch,yaw,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,23 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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,114 @@
+#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 = -2.8;
+		float yaw = 20;
+		float targetPos[3]={-0.2,-1.4,3.5};
+		m_guiHelper->resetCamera(dist,pitch,yaw,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,7 @@
+#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,213 @@
+/*
+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 "../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 = -140;
+		float yaw = 28;
+		float targetPos[3]={-4,-3,-3};
+		m_guiHelper->resetCamera(dist,pitch,yaw,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))
+                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();
+
+		btVector3 color(0,0,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;
+		}
+		LoadMeshFromCollada(relativeFileName, visualShapes, visualShapeInstances,upAxisTrans,unitMeterScaling, upAxis);
+#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,25 @@
+/*
+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,753 @@
+/*
+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/tinyxml/tinyxml.h"
+
+#include "Bullet3Common/b3FileUtils.h"
+#include "LinearMath/btHashMap.h"
+#include <assert.h>
+#include "btMatrix4x4.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 = " ")
+{
+   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(TiXmlElement* source, btAlignedObjectArray<float>& floatArray, int& componentStride) 
+{
+	int numVals, stride;
+	TiXmlElement* array = source->FirstChildElement("float_array");
+	if(array) 
+	{
+		componentStride = 1;
+		if (source->FirstChildElement("technique_common")->FirstChildElement("accessor")->QueryIntAttribute("stride", &stride)!= TIXML_NO_ATTRIBUTE)
+		{
+			componentStride = stride;
+		}
+		array->QueryIntAttribute("count", &numVals);
+		TokenFloatArray adder(floatArray);
+		floatArray.reserve(numVals);
+		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(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btHashMap<btHashString,int>& name2Shape, float extraScaling) 
+{
+	btHashMap<btHashString,TiXmlElement* > allSources;
+	btHashMap<btHashString,VertexSource> vertexSources;
+	for(TiXmlElement* 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 (TiXmlElement* mesh = geometry->FirstChildElement("mesh");(mesh != NULL); mesh = mesh->NextSiblingElement("mesh")) 
+		{
+			TiXmlElement* vertices2 = mesh->FirstChildElement("vertices");
+			
+			for (TiXmlElement* 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(TiXmlElement* 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);
+
+			for (TiXmlElement* primitive = mesh->FirstChildElement("triangles"); primitive; primitive = primitive->NextSiblingElement("triangles"))
+			{
+				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 (TiXmlElement* 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;
+				TiXmlElement** 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);
+				tokenize(primitive->FirstChildElement("p")->GetText(),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(TiXmlElement* node,btHashMap<btHashString,int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances,  const btMatrix4x4& parentTransMat)
+{
+	const char* nodeName = node->Attribute("id");
+	//printf("processing node %s\n", nodeName);
+
+	
+	btMatrix4x4 nodeTrans;
+	nodeTrans.setIdentity();
+
+	///todo(erwincoumans) we probably have to read the elements 'translate', 'scale', 'rotate' and 'matrix' in-order and accumulate them...
+	{
+		for (TiXmlElement* 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 (TiXmlElement* 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(TiXmlElement* 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(TiXmlElement* 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 (TiXmlElement* 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(TiXmlElement* childNode = node->FirstChildElement("node");
+			childNode!= NULL; childNode = childNode->NextSiblingElement("node")) 
+	{
+		readNodeHierarchy(childNode,name2Shape,visualShapeInstances, nodeTrans);
+	}
+}
+void readVisualSceneInstanceGeometries(TiXmlDocument& doc, btHashMap<btHashString,int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances)
+{
+	btHashMap<btHashString,TiXmlElement* > allVisualScenes;
+
+	TiXmlElement* libVisualScenes = doc.RootElement()->FirstChildElement("library_visual_scenes");
+	if (libVisualScenes==0)
+		return;
+
+	{
+		for(TiXmlElement* scene = libVisualScenes->FirstChildElement("visual_scene");
+				scene != NULL; scene = scene->NextSiblingElement("visual_scene")) 
+		{
+			const char* sceneName = scene->Attribute("id");
+			allVisualScenes.insert(sceneName,scene);
+		}
+	}
+
+	TiXmlElement* scene = 0;
+	{
+		TiXmlElement* scenes = doc.RootElement()->FirstChildElement("scene");
+		if (scenes)
+		{
+			TiXmlElement* instanceSceneReference = scenes->FirstChildElement("instance_visual_scene");
+			if (instanceSceneReference)
+			{
+				const char* instanceSceneUrl = instanceSceneReference->Attribute("url");
+				TiXmlElement** sceneInstancePtr = allVisualScenes[instanceSceneUrl+1];//skip #
+				if (sceneInstancePtr)
+				{
+					scene = *sceneInstancePtr;
+				}
+			}
+		}
+	}
+
+	if (scene)
+	{
+		for(TiXmlElement* 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(TiXmlDocument& doc, btTransform& tr, float& unitMeterScaling, int clientUpAxis)
+{
+	///todo(erwincoumans) those up-axis transformations have been quickly coded without rigorous testing
+	
+	TiXmlElement* unitMeter = doc.RootElement()->FirstChildElement("asset")->FirstChildElement("unit");
+	if (unitMeter)
+	{
+		const char* meterText = unitMeter->Attribute("meter");
+	//printf("meterText=%s\n", meterText);
+		unitMeterScaling = atof(meterText);
+	}
+
+	TiXmlElement* 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)
+{
+
+//	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;
+	b3FileUtils f;
+	char filename[1024];
+	if (!f.findFile(relativeFileName,filename,1024))
+	{
+		b3Warning("File not found: %s\n", filename);
+		return;
+	}
+	 
+	TiXmlDocument doc(filename);
+	if (!doc.LoadFile())
+		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());
+	}
+	
+}
+
+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,45 @@
+/*
+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);
+
+//#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,157 @@
+/*
+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/ImportMeshUtility/b3ImportMeshUtility.cpp

@@ -0,0 +1,88 @@
+#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 "../../ThirdPartyLibs/stb_image/stb_image.h"
+
+
+bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData)
+{
+
+	meshData.m_gfxShape = 0;
+	meshData.m_textureImage = 0;
+	meshData.m_textureHeight = 0;
+	meshData.m_textureWidth = 0;
+
+
+	char relativeFileName[1024];
+    if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
+    {
+        char pathPrefix[1024];
+
+        b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
+		btVector3 shift(0,0,0);
+    	
+		std::vector<tinyobj::shape_t> shapes;
+		std::string err = tinyobj::LoadObj(shapes, relativeFileName, pathPrefix);
+		
+		GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(shapes);
+		
+		int textureIndex = -1;
+		//try to load some texture
+		for (int i=0;i<shapes.size();i++)
+		{
+			const tinyobj::shape_t& shape = shapes[i];
+			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 (b3ResourcePath::findResourcePath(relativeFileName, relativeFileName2, 1024))
+                    {
+                        image = stbi_load(relativeFileName, &width, &height, &n, 3);
+						meshData.m_textureImage = image;
+						if (image)
+						{
+							meshData.m_textureWidth = width;
+							meshData.m_textureHeight = height;
+						} else
+						{
+							meshData.m_textureWidth = 0;
+							meshData.m_textureHeight = 0;
+						}
+
+                    } 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,25 @@
+#ifndef B3_IMPORT_MESH_UTILITY_H
+#define B3_IMPORT_MESH_UTILITY_H
+
+#include <string>
+
+struct b3ImportMeshData
+{
+	struct GLInstanceGraphicsShape* m_gfxShape;
+
+	unsigned char* m_textureImage;//in 3 component 8-bit RGB data
+	int m_textureWidth;
+	int m_textureHeight;
+};
+
+class b3ImportMeshUtility
+{
+public:
+
+static bool loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData);
+
+};
+
+
+#endif //B3_IMPORT_MESH_UTILITY_H
+

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

@@ -0,0 +1,116 @@
+#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 "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 = 120;
+		float yaw = 46;
+		float targetPos[3]={-2,-2,-2};
+		m_guiHelper->resetCamera(dist,pitch,yaw,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;
+	if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData))
+	{
+		int textureIndex = -1;
+		
+		if (meshData.m_textureImage)
+		{
+			textureIndex = renderer->registerTexture(meshData.m_textureImage,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;
+		delete meshData.m_textureImage;
+	}
+	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);
+	btVector3 color(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,7 @@
+#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,17 @@
+#include "LoadMeshFromObj.h"
+#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
+#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
+#include <stdio.h> //fopen
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include <string>
+#include <vector>
+#include "Wavefront2GLInstanceGraphicsShape.h"
+
+GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath)
+{
+	std::vector<tinyobj::shape_t> shapes;
+	std::string err = tinyobj::LoadObj(shapes, relativeFileName, materialPrefixPath);
+		
+	GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(shapes);
+	return gfxShape;
+}

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

@@ -0,0 +1,11 @@
+#ifndef LOAD_MESH_FROM_OBJ_H
+#define LOAD_MESH_FROM_OBJ_H
+
+
+struct GLInstanceGraphicsShape;
+
+
+GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath);
+
+#endif //LOAD_MESH_FROM_OBJ_H
+

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

@@ -0,0 +1,197 @@
+#include "Wavefront2GLInstanceGraphicsShape.h"
+
+#include "../../OpenGLWindow/GLInstancingRenderer.h"
+#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
+#include "btBulletDynamicsCommon.h"
+#include "../../OpenGLWindow/SimpleOpenGL3App.h"
+#include "Wavefront2GLInstanceGraphicsShape.h"
+#include "../../OpenGLWindow/GLInstancingRenderer.h"
+#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
+
+GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<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++)
+		{
+			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>=shape.mesh.indices.size())
+					{
+						continue;
+					}
+					
+					GLInstanceVertex vtx0;
+					vtx0.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f]*3+0];
+					vtx0.xyzw[1] = shape.mesh.positions[shape.mesh.indices[f]*3+1];
+					vtx0.xyzw[2] = shape.mesh.positions[shape.mesh.indices[f]*3+2];
+					vtx0.xyzw[3] = 0.f;
+					
+
+					if (shape.mesh.texcoords.size() )
+					{
+						int uv0Index = shape.mesh.indices[f]*2+0;
+						int uv1Index = shape.mesh.indices[f]*2+1;
+						if (uv0Index>=0 && uv1Index>=0 && (uv0Index < shape.mesh.texcoords.size()) && (uv1Index < shape.mesh.texcoords.size()))
+						{
+							vtx0.uv[0] = shape.mesh.texcoords[uv0Index];
+							vtx0.uv[1] = shape.mesh.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;
+					vtx1.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f+1]*3+0];
+					vtx1.xyzw[1] = shape.mesh.positions[shape.mesh.indices[f+1]*3+1];
+					vtx1.xyzw[2] = shape.mesh.positions[shape.mesh.indices[f+1]*3+2];
+					vtx1.xyzw[3]= 0.f;
+
+					if (shape.mesh.texcoords.size())
+					{
+						int uv0Index = shape.mesh.indices[f+1]*2+0;
+						int uv1Index = shape.mesh.indices[f+1]*2+1;
+						if (uv0Index>=0 && uv1Index>=0 && (uv0Index < shape.mesh.texcoords.size()) && (uv1Index < shape.mesh.texcoords.size()))
+						{
+							vtx1.uv[0] = shape.mesh.texcoords[uv0Index];
+							vtx1.uv[1] = shape.mesh.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;
+					vtx2.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f+2]*3+0];
+					vtx2.xyzw[1] = shape.mesh.positions[shape.mesh.indices[f+2]*3+1];
+					vtx2.xyzw[2] = shape.mesh.positions[shape.mesh.indices[f+2]*3+2];
+					vtx2.xyzw[3] = 0.f;
+					if (shape.mesh.texcoords.size())
+					{
+						int uv0Index = shape.mesh.indices[f+2]*2+0;
+						int uv1Index = shape.mesh.indices[f+2]*2+1;
+						if (uv0Index>=0 && uv1Index>=0 && (uv0Index < shape.mesh.texcoords.size()) && (uv1Index < shape.mesh.texcoords.size()))
+						{
+							vtx2.uv[0] = shape.mesh.texcoords[uv0Index];
+							vtx2.uv[1] = shape.mesh.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;
+					maxIndex = b3Max(maxIndex,shape.mesh.indices[f]*3+0);
+					maxIndex = b3Max(maxIndex,shape.mesh.indices[f]*3+1);
+					maxIndex = b3Max(maxIndex,shape.mesh.indices[f]*3+2);
+					maxIndex = b3Max(maxIndex,shape.mesh.indices[f+1]*3+0);
+					maxIndex = b3Max(maxIndex,shape.mesh.indices[f+1]*3+1);
+					maxIndex = b3Max(maxIndex,shape.mesh.indices[f+1]*3+2);
+					maxIndex = b3Max(maxIndex,shape.mesh.indices[f+2]*3+0);
+					maxIndex = b3Max(maxIndex,shape.mesh.indices[f+2]*3+1);
+					maxIndex = b3Max(maxIndex,shape.mesh.indices[f+2]*3+2);
+					bool hasNormals = (shape.mesh.normals.size() && maxIndex<shape.mesh.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] = shape.mesh.normals[shape.mesh.indices[f]*3+0];
+                        vtx0.normal[1] = shape.mesh.normals[shape.mesh.indices[f]*3+1];
+                        vtx0.normal[2] = shape.mesh.normals[shape.mesh.indices[f]*3+2]; //shape.mesh.indices[f+1]*3+0
+                        vtx1.normal[0] = shape.mesh.normals[shape.mesh.indices[f+1]*3+0];
+                        vtx1.normal[1] = shape.mesh.normals[shape.mesh.indices[f+1]*3+1];
+                        vtx1.normal[2] = shape.mesh.normals[shape.mesh.indices[f+1]*3+2];
+                        vtx2.normal[0] = shape.mesh.normals[shape.mesh.indices[f+2]*3+0];
+                        vtx2.normal[1] = shape.mesh.normals[shape.mesh.indices[f+2]*3+1];
+                        vtx2.normal[2] = shape.mesh.normals[shape.mesh.indices[f+2]*3+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(std::vector<tinyobj::shape_t>& shapes, bool flatShading=false);
+
+#endif //WAVEFRONT2GRAPHICS_H

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

@@ -0,0 +1,356 @@
+
+#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 = -136;
+		float yaw = 28;
+		float targetPos[3]={0.47,0,-0.64};
+		m_guiHelper->resetCamera(dist,pitch,yaw,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;
+}
+
+static btVector4 colors[4] =
+{
+	btVector4(1,0,0,1),
+	btVector4(0,1,0,1),
+	btVector4(0,1,1,1),
+	btVector4(1,1,0,1),
+};
+
+
+static btVector3 selectColor()
+{
+
+	static int curColor = 0;
+	btVector4 color = colors[curColor];
+	curColor++;
+	curColor&=3;
+	return color;
+}
+
+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);
+    
+    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,8 @@
+#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,103 @@
+#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"
+
+
+
+
+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 = -136;
+		float yaw = 28;
+		float targetPos[3]={0.47,0,-0.64};
+		m_guiHelper->resetCamera(dist,pitch,yaw,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))
+        {
+                b3Warning("Cannot find file %s\n", m_fileName);
+                return;
+        }
+
+	
+	btVector3 shift(0,0,0);
+//	int index=10;
+	
+	{
+		
+		GLInstanceGraphicsShape* gfxShape = LoadMeshFromSTL(relativeFileName);
+		
+		btTransform trans;
+		trans.setIdentity();
+		trans.setRotation(btQuaternion(btVector3(1,0,0),SIMD_HALF_PI));
+		
+		btVector3 position = trans.getOrigin();
+		btQuaternion orn = trans.getRotation();
+		
+		btVector3 color(0,0,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,106 @@
+
+#ifndef LOAD_MESH_FROM_STL_H
+#define LOAD_MESH_FROM_STL_H
+
+#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
+#include <stdio.h> //fopen
+#include "Bullet3Common/b3AlignedObjectArray.h"
+
+struct MySTLTriangle
+{
+	float normal[3];
+	float vertex0[3];
+	float vertex1[3];
+	float vertex2[3];
+};
+
+static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
+{
+	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)
+			{
+				//b3Warning("Open STL file of %d bytes\n",size);
+				char* memoryBuffer = new char[size+1];
+				int actualBytesRead = fread(memoryBuffer,1,size,file);
+				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)
+							{
+								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;
+			}
+		}
+		fclose(file);
+	}
+	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,67 @@
+#ifndef BULLET_URDF_IMPORTER_H
+#define BULLET_URDF_IMPORTER_H 
+
+#include "URDFImporterInterface.h"
+
+#include "LinkVisualShapesConverter.h"
+
+
+///BulletURDFImporter can deal with URDF and (soon) SDF files
+class BulletURDFImporter : public URDFImporterInterface
+{
+    
+	struct BulletURDFInternalData* m_data;
+    
+
+public:
+
+	BulletURDFImporter(struct GUIHelperInterface* guiHelper, LinkVisualShapesConverter* customConverter);
+
+	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 getLinkName(int linkIndex) const;
+
+	virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const;
+
+	virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const;
+	
+    virtual std::string getJointName(int linkIndex) const;
+    
+    virtual void  getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) 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 getRootTransformInWorld(btTransform& rootTransformInWorld) const;
+
+    virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
+
+    virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const;
+
+    ///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation
+    
+	virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;
+    
+    virtual int getNumAllocatedCollisionShapes() const;
+    virtual class btCollisionShape* getAllocatedCollisionShape(int index);
+
+    
+};
+
+
+#endif //BULLET_URDF_IMPORTER_H

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

@@ -0,0 +1,14 @@
+#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,426 @@
+
+#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 = -136;
+		float yaw = 28;
+		float targetPos[3]={0.47,0,-0.64};
+		m_guiHelper->resetCamera(dist,pitch,yaw,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(0),
+	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;
+}
+
+static btVector4 colors[4] =
+{
+	btVector4(1,0,0,1),
+	btVector4(0,1,0,1),
+	btVector4(0,1,1,1),
+	btVector4(1,1,0,1),
+};
+
+
+static btVector3 selectColor()
+{
+
+	static int curColor = 0;
+	btVector4 color = colors[curColor];
+	curColor++;
+	curColor&=3;
+	return color;
+}
+
+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);
+	}
+	
+
+	BulletURDFImporter u2b(m_guiHelper, 0);
+	
+	
+	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,8 @@
+#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/LinkVisualShapesConverter.h

@@ -0,0 +1,9 @@
+#ifndef LINK_VISUAL_SHAPES_CONVERTER_H
+#define LINK_VISUAL_SHAPES_CONVERTER_H
+
+struct LinkVisualShapesConverter
+{
+	virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const class btTransform& localInertiaFrame, const struct UrdfModel& model, class btCollisionObject* colObj, int objectIndex)=0;
+};
+
+#endif //LINK_VISUAL_SHAPES_CONVERTER_H

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

@@ -0,0 +1,37 @@
+#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;
+  
+    ///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 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,210 @@
+#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);
+    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;
+    m_rigidBody = new btRigidBody(rbci);
+	m_rigidBody->forceActivationState(DISABLE_DEACTIVATION);
+	
+    return m_rigidBody;
+}
+    
+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->setAngularUpperLimit(btVector3(-1,0,0));
+            dof6->setAngularLowerLimit(btVector3(1,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->setAngularUpperLimit(btVector3(0,-1,0));
+            dof6->setAngularLowerLimit(btVector3(0,1,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->setAngularUpperLimit(btVector3(0,0,-1));
+            dof6->setAngularLowerLimit(btVector3(0,0,0));
+
+        }
+    };
+
+	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));
+
+	return dof6;
+}
+   
+
+
+void MyMultiBodyCreator::addLinkMapping(int urdfLinkIndex, int mbLinkIndex) 
+{
+//    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::createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* colObj, const btVector3& colorRgba) 
+{
+    m_guiHelper->createCollisionObjectGraphicsObject(colObj,colorRgba);
+}
+
+btMultiBody* MyMultiBodyCreator::getBulletMultiBody()
+{
+    return m_bulletMultiBody;
+}
+

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

@@ -0,0 +1,82 @@
+
+#ifndef MY_MULTIBODY_CREATOR
+#define MY_MULTIBODY_CREATOR
+
+#include "MultiBodyCreationInterface.h"
+#include "LinearMath/btAlignedObjectArray.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) ;
+  
+    ///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 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/URDF2Bullet.cpp

@@ -0,0 +1,492 @@
+#include <stdio.h>
+#include "LinearMath/btTransform.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+#include "URDF2Bullet.h"
+#include "URDFImporterInterface.h"
+#include "MultiBodyCreationInterface.h"
+#include <string>
+
+static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter;
+static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter);
+static bool enableConstraints = true;
+
+static btVector4 colors[4] =
+{
+    btVector4(1,0,0,1),
+    btVector4(0,1,0,1),
+    btVector4(0,1,1,1),
+    btVector4(1,1,0,1),
+};
+
+
+static btVector4 selectColor2()
+{
+
+    static int curColor = 0;
+    btVector4 color = colors[curColor];
+    curColor++;
+    curColor&=3;
+    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 btCompoundShape* 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 btCompoundShape* 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)
+{
+    //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
+        ComputeParentIndices(u2b,cache,rootLinkIndex,-2);
+    }
+
+}
+
+void ConvertURDF2BulletInternal(
+    const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
+    URDF2BulletCachedData& cache, int urdfLinkIndex,
+    const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,
+    bool createMultiBody, const char* pathPrefix,
+    int flags = 0)
+{
+    //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.getMassAndInertia(urdfParentIndex, parentMass,parentLocalInertiaDiagonal,parentLocalInertialFrame);
+
+    }
+
+    btScalar mass = 0;
+    btTransform localInertialFrame;
+    localInertialFrame.setIdentity();
+    btVector3 localInertiaDiagonal(0,0,0);
+    u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame);
+
+
+
+
+
+    btTransform parent2joint;
+    parent2joint.setIdentity();
+
+    int jointType;
+    btVector3 jointAxisInJointSpace;
+    btScalar jointLowerLimit;
+    btScalar jointUpperLimit;
+    btScalar jointDamping;
+    btScalar jointFriction;
+
+
+    bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction);
+    if (flags & CUF_USE_SDF)
+    {
+        parent2joint =parentTransformInWorldSpace.inverse()*linkTransformInWorldSpace;
+    }
+    else
+    {
+        linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
+    }
+    
+    
+
+    btCompoundShape* compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
+	
+	int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame);
+	
+	
+
+
+    if (compoundShape)
+    {
+
+
+        btVector4 color = selectColor2();
+		u2b.getLinkColor(urdfLinkIndex,color);
+        /*
+         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);
+            }
+            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;
+
+        if (!createMultiBody)
+        {
+            btRigidBody* body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape);
+            linkRigidBody = body;
+
+            world1->addRigidBody(body);
+
+            compoundShape->setUserIndex(graphicsIndex);
+
+            creation.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex);
+            cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
+            
+            //untested: u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,body);
+        } else
+        {
+            if (cache.m_bulletMultiBody==0)
+            {
+                
+                bool canSleep = false;
+                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);
+
+                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;
+            switch (jointType)
+            {
+                case URDFFixedJoint:
+                {
+                    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());
+                        creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
+                    } else
+                    {
+                        //b3Printf("Fixed joint\n");
+						
+						btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody,  offsetInB, offsetInA);
+                       
+                        if (enableConstraints)
+                            world1->addConstraint(dof6,true);
+                    }
+                    break;
+                }
+                case URDFContinuousJoint:
+                case URDFRevoluteJoint:
+                {
+                    if (createMultiBody)
+                    {
+                        cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
+                                                                  parentRotToThis, quatRotate(offsetInB.getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
+                                                                  -offsetInB.getOrigin(),
+                                                                  disableParentCollision);
+                        cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping;
+                        cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction= jointFriction;
+                        creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
+                        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);
+                        }
+                    } else
+                    {
+
+						btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit);
+
+						if (enableConstraints)
+                                    world1->addConstraint(dof6,true);
+                        //b3Printf("Revolute/Continuous joint\n");
+                    }
+                    break;
+                }
+                case URDFPrismaticJoint:
+                {
+                    if (createMultiBody)
+                    {
+                        cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
+                                                                   parentRotToThis, quatRotate(offsetInB.getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
+                                                                   -offsetInB.getOrigin(),
+                                                                   disableParentCollision);
+                        creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
+						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);
+
+                    } else
+                    {
+                        
+						btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,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);
+
+                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;
+                short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
+                short collisionFilterMask = isDynamic? 	short(btBroadphaseProxy::AllFilter) : 	short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+
+                world1->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
+
+                btVector4 color = selectColor2();//(0.0,0.0,0.5);
+				u2b.getLinkColor(urdfLinkIndex,color);
+                creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color);
+                
+                u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,col, u2b.getBodyUniqueId());
+
+				URDFLinkContactInfo contactInfo;
+				u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
+
+				if ((contactInfo.m_flags & URDF_CONTACT_HAS_LATERAL_FRICTION)!=0)
+				{
+					col->setFriction(contactInfo.m_lateralFriction);
+				}
+				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 (mbLinkIndex>=0) //???? double-check +/- 1
+                {
+                    cache.m_bulletMultiBody->getLink(mbLinkIndex).m_collider=col;
+                } else
+                {
+                    cache.m_bulletMultiBody->setBaseCollider(col);
+                }
+            }
+        } else
+        {
+            //u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,compoundShape);
+        }
+    }
+
+
+    btAlignedObjectArray<int> urdfChildIndices;
+    u2b.getLinkChildIndices(urdfLinkIndex,urdfChildIndices);
+
+    int numChildren = urdfChildIndices.size();
+
+    for (int i=0;i<numChildren;i++)
+    {
+        int urdfChildLinkIndex = urdfChildIndices[i];
+
+        ConvertURDF2BulletInternal(u2b,creation, cache,urdfChildLinkIndex,linkTransformInWorldSpace,world1,createMultiBody,pathPrefix,flags);
+    }
+
+}
+
+void ConvertURDF2Bullet(
+    const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
+    const btTransform& rootTransformInWorldSpace,
+    btMultiBodyDynamicsWorld* world1,
+    bool createMultiBody, const char* pathPrefix, int flags)
+{
+    URDF2BulletCachedData cache;
+
+    InitURDF2BulletCache(u2b,cache);
+    int urdfLinkIndex = u2b.getRootLinkIndex();
+    ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex,rootTransformInWorldSpace,world1,createMultiBody,pathPrefix,flags);
+
+	if (world1 && cache.m_bulletMultiBody)
+	{
+		btMultiBody* mb = cache.m_bulletMultiBody;
+		mb->setHasSelfCollision(false);
+		mb->finalizeMultiDof();
+
+		btTransform localInertialFrameRoot = cache.m_urdfLinkLocalInertialFrames[urdfLinkIndex];
+
+		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);
+	}
+}
+
+

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

@@ -0,0 +1,33 @@
+#ifndef _URDF2BULLET_H
+#define _URDF2BULLET_H
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btTransform.h"
+#include <string>
+class btVector3;
+class btTransform;
+class btMultiBodyDynamicsWorld;
+class btTransform;
+
+
+class URDFImporterInterface;
+class MultiBodyCreationInterface;
+
+
+
+enum ConvertURDFFlags {
+  CUF_USE_SDF = 1,
+  // Use inertia values in URDF instead of recomputing them from collision shape.
+  CUF_USE_URDF_INERTIA = 2
+};
+
+void ConvertURDF2Bullet(const URDFImporterInterface& u2b,
+			MultiBodyCreationInterface& creationCallback,
+			const btTransform& rootTransformInWorldSpace,
+			btMultiBodyDynamicsWorld* world,
+			bool createMultiBody,
+			const char* pathPrefix,
+            int flags = 0);
+
+
+#endif //_URDF2BULLET_H
+

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

@@ -0,0 +1,59 @@
+#ifndef URDF_IMPORTER_INTERFACE_H
+#define URDF_IMPORTER_INTERFACE_H
+
+#include <string>
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btTransform.h"
+#include "URDFJointTypes.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;
+	/// 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;}
+
+	///this API will likely change, don't override it!
+	virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) 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;
+    
+    ///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 getRootTransformInWorld(btTransform& rootTransformInWorld) const =0;
+    
+	///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, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const  { }
+    virtual void setBodyUniqueId(int bodyId) {}
+    virtual int getBodyUniqueId() const { return 0;}
+    
+	virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const  = 0;
+};
+
+#endif //URDF_IMPORTER_INTERFACE_H
+

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

@@ -0,0 +1,56 @@
+#ifndef URDF_JOINT_TYPES_H
+#define URDF_JOINT_TYPES_H
+
+
+enum UrdfJointTypes
+{
+		URDFRevoluteJoint=1,
+		URDFPrismaticJoint,
+		URDFContinuousJoint,
+		URDFFloatingJoint,
+		URDFPlanarJoint,
+		URDFFixedJoint,
+};
+#include "LinearMath/btScalar.h"
+
+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,
+
+};
+
+struct URDFLinkContactInfo
+{
+	btScalar m_lateralFriction;
+	btScalar m_rollingFriction;
+    btScalar m_spinningFriction;
+    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_inertiaScaling(1),
+		m_contactCfm(0),
+		m_contactErp(0),
+        m_contactStiffness(1e4),
+        m_contactDamping(1)
+	{
+		m_flags = URDF_CONTACT_HAS_LATERAL_FRICTION;
+	}
+};
+
+
+#endif //URDF_JOINT_TYPES_H

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

@@ -0,0 +1,260 @@
+#ifndef URDF_PARSER_H
+#define URDF_PARSER_H
+
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btHashMap.h"
+#include "URDFJointTypes.h"
+
+#define btArray btAlignedObjectArray
+#include <string>
+
+struct 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;
+	btVector4 m_rgbaColor;
+};
+
+struct UrdfInertia
+{
+	btTransform m_linkLocalFrame;
+	double m_mass;
+	double m_ixx,m_ixy,m_ixz,m_iyy,m_iyz,m_izz;
+	
+	UrdfInertia()
+	{
+		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,
+    
+};
+
+
+struct UrdfGeometry
+{
+	UrdfGeomTypes m_type;
+	
+	double m_sphereRadius;
+	
+	btVector3 m_boxSize;
+	
+	double m_cylinderRadius;
+	double m_cylinderLength;
+
+    btVector3 m_planeNormal;
+    
+	std::string m_meshFileName;
+	btVector3 m_meshScale;
+};
+
+struct UrdfVisual
+{
+	btTransform m_linkLocalFrame;
+	UrdfGeometry m_geometry;
+	std::string m_name;
+	std::string m_materialName;
+	bool m_hasLocalMaterial;
+	UrdfMaterial m_localMaterial;
+};
+
+enum UrdfCollisionFlags
+{
+	URDF_FORCE_CONCAVE_TRIMESH=1,
+};
+
+
+struct UrdfCollision
+{
+	btTransform m_linkLocalFrame;
+	UrdfGeometry m_geometry;
+	std::string m_name;
+	int m_flags;
+	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;
+
+	UrdfLink()
+		:m_parentLink(0),
+		m_parentJoint(0)
+	{
+	}
+	
+};
+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;
+	UrdfJoint()
+		:m_lowerLimit(0),
+		m_upperLimit(-1),
+		m_effortLimit(0),
+		m_velocityLimit(0),
+		m_jointDamping(0),
+		m_jointFriction(0)
+	{
+	}
+};
+
+struct UrdfModel
+{
+	std::string m_name;
+    btTransform m_rootTransformInWorld;
+	btHashMap<btHashString, UrdfMaterial*> m_materials;
+	btHashMap<btHashString, UrdfLink*> m_links;
+	btHashMap<btHashString, UrdfJoint*> m_joints;
+	
+	btArray<UrdfLink*> m_rootLinks;
+	bool m_overrideFixedBase;
+
+	UrdfModel()
+		:m_overrideFixedBase(false)
+	{
+		m_rootTransformInWorld.setIdentity();
+	}
+	
+};
+
+class UrdfParser
+{
+protected:
+    
+    UrdfModel m_urdf2Model;
+    btAlignedObjectArray<UrdfModel*> m_sdfModels;
+    btAlignedObjectArray<UrdfModel*> m_tmpModels;
+    
+    bool m_parseSDF;
+    int m_activeSdfModel;
+
+    
+    void cleanModel(UrdfModel* model);
+	bool parseInertia(UrdfInertia& inertia, class TiXmlElement* config, ErrorLogger* logger);
+	bool parseGeometry(UrdfGeometry& geom, class TiXmlElement* g, ErrorLogger* logger);
+	bool parseVisual(UrdfModel& model, UrdfVisual& visual, class TiXmlElement* config, ErrorLogger* logger);
+	bool parseCollision(UrdfCollision& collision, class TiXmlElement* config, ErrorLogger* logger);
+	bool initTreeAndRoot(UrdfModel& model, ErrorLogger* logger);
+	bool parseMaterial(UrdfMaterial& material, class TiXmlElement *config, ErrorLogger* logger);
+	bool parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger);
+    bool parseJointDynamics(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger);
+	bool parseJoint(UrdfJoint& link, TiXmlElement *config, ErrorLogger* logger);
+	bool parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *config, ErrorLogger* logger);
+	
+    
+public:
+	
+	UrdfParser();
+	virtual ~UrdfParser();
+	
+    void setParseSDF(bool useSDF)
+    {
+        m_parseSDF = useSDF;
+    }
+    bool getParseSDF() const
+    {
+        return m_parseSDF;
+    }
+    bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase);
+    bool loadSDF(const char* sdfText, ErrorLogger* logger);
+    
+    int getNumModels() const
+    {
+        //user should have loaded an SDF when calling this method
+        btAssert(m_parseSDF);
+        if (m_parseSDF)
+        {
+            return m_sdfModels.size();
+        }
+		return 0;
+    }
+    
+    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;
+	}
+};
+
+#endif
+

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

@@ -0,0 +1,17 @@
+#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,247 @@
+
+
+#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,30 @@
+
+#ifndef STRING_SPLIT_H
+#define STRING_SPLIT_H
+
+#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);
+
+///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);
+
+#endif //STRING_SPLIT_H
+

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

@@ -0,0 +1,825 @@
+#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
+