add improved btGeneric6DofSpring2Constraint, thanks to Puhr Gabor and Tamas Umenhoffer!

improved the new demo testbed (work-in-progress)
add basic Lua demo, import URDF test, STL import, obj import

Demos/CommonParameterInterface.h

@@ -36,6 +36,8 @@ struct CommonParameterInterface
 	virtual void registerSliderFloatParameter(SliderParams& params)=0;
 	virtual void syncParameters()=0;
 	virtual void removeAllParameters()=0;
+    virtual void setSliderValue(int sliderIndex, double sliderValue)=0;
+
 };
 	
 #endif //PARAM_INTERFACE_H

Demos/SerializeDemo/SerializeSetup.cpp

@@ -0,0 +1,21 @@
+#include "SerializeSetup.h"
+#include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h"
+
+SerializeSetup::SerializeSetup()
+{
+    
+}
+SerializeSetup::~SerializeSetup()
+{
+    
+}
+    
+void SerializeSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
+{
+    this->createEmptyDynamicsWorld();
+    gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
+    m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
+    btBulletWorldImporter* importer = new btBulletWorldImporter(m_dynamicsWorld);
+    const char* filename = "testFile.bullet";
+    importer->loadFile(filename);
+}

Demos/SerializeDemo/SerializeSetup.h

@@ -0,0 +1,14 @@
+#ifndef SERIALIZE_SETUP_H
+#define SERIALIZE_SETUP_H
+#include "../../Demos/CommonRigidBodySetup.h"
+
+class SerializeSetup : public CommonRigidBodySetup
+{
+public:
+    SerializeSetup();
+    virtual ~SerializeSetup();
+    
+	virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge);
+};
+
+#endif //SERIALIZE_SETUP_H

Demos3/AllBullet2Demos/BulletDemoEntries.h

@@ -11,7 +11,7 @@
 #include "../bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.h"
 
 #include "../bullet2/RagdollDemo/RagdollDemo.h"
-#include "../bullet2/LuaDemo/LuaDemo.h"
+#include "../bullet2/LuaDemo/LuaPhysicsSetup.h"
 #include "../bullet2/ChainDemo/ChainDemo.h"
 #include "../../Demos/CcdPhysicsDemo/CcdPhysicsSetup.h"
 #include "../../Demos/ConstraintDemo/ConstraintPhysicsSetup.h"
@@ -19,6 +19,11 @@
 #include "../ImportObjDemo/ImportObjSetup.h"
 #include "../ImportSTLDemo/ImportSTLSetup.h"
 
+static BulletDemoInterface* LuaDemoCreateFunc(SimpleOpenGL3App* app)
+{
+       CommonPhysicsSetup* physicsSetup = new LuaPhysicsSetup(app);
+       return new BasicDemo(app, physicsSetup);
+}
 
 static BulletDemoInterface* MyCcdPhysicsDemoCreateFunc(SimpleOpenGL3App* app)
 {
@@ -73,6 +78,8 @@ static BulletDemoEntry allDemos[]=
 	{ 1, "CcdDemo", MyCcdPhysicsDemoCreateFunc },
 	{ 1, "Kinematic", MyKinematicObjectCreateFunc },
 	{ 1, "Constraints", MyConstraintCreateFunc },
+	{ 1, "LuaDemo",LuaDemoCreateFunc},
+
 	{0,"File Formats", 0},
 //@todo(erwincoumans)	{ 1, "bullet", MyImportSTLCreateFunc},
 	{ 1, "Wavefront Obj", MyImportObjCreateFunc},
@@ -95,7 +102,6 @@ static BulletDemoEntry allDemos[]=
 	{1,"MultiBody1",FeatherstoneDemo1::MyCreateFunc},
 //	{"MultiBody2",FeatherstoneDemo2::MyCreateFunc},
 	{1,"MultiDofDemo",MultiDofDemo::MyCreateFunc},
-//	{"LuaDemo",LuaDemo::MyCreateFunc}
 
 };
 

Demos3/AllBullet2Demos/GraphingTexture.cpp

@@ -0,0 +1,192 @@
+#include "GraphingTexture.h"
+#include "OpenGLWindow/OpenGLInclude.h"
+#include <assert.h>
+
+GraphingTexture::GraphingTexture()
+:m_textureId(0),
+m_width(0),
+m_height(0)
+{
+}
+
+GraphingTexture::~GraphingTexture()
+{
+	destroy();
+}
+
+bool GraphingTexture::destroy()
+{
+	//TODO(erwincoumans) release memory etc...
+	m_width = 0;
+	m_height=0;
+	glDeleteTextures(1,(GLuint*)&m_textureId);
+	m_textureId=0;
+}
+
+bool GraphingTexture::create(int texWidth, int texHeight)
+{
+	m_width = texWidth;
+	m_height = texHeight;
+	glActiveTexture(GL_TEXTURE0);
+	
+	m_imageData.resize(texWidth*texHeight*4);
+	for(int y=0;y<texHeight;++y)
+	{
+		//					const int	t=y>>5;
+		GLubyte*	pi=&m_imageData[y*texWidth*4];
+		for(int x=0;x<texWidth;++x)
+		{
+			if (x>=y)//x<2||y<2||x>253||y>253)
+			{
+				pi[0]=0;
+				pi[1]=0;
+				pi[2]=255;
+				pi[3]=255;
+			} else
+			{
+				pi[0]=255;
+				pi[1]=0;
+				pi[2]=0;
+				pi[3]=255;
+			}
+			
+			pi+=4;
+		}
+	}
+	
+	
+	glGenTextures(1,(GLuint*)&m_textureId);
+	
+	uploadImageData();
+}
+
+void GraphingTexture::uploadImageData()
+{
+	glBindTexture(GL_TEXTURE_2D,m_textureId);
+	GLint err = glGetError();
+	assert(err==GL_NO_ERROR);
+	
+	err = glGetError();
+	assert(err==GL_NO_ERROR);
+	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, m_width,m_height,0,GL_RGBA,GL_UNSIGNED_BYTE,&m_imageData[0]);
+	glGenerateMipmap(GL_TEXTURE_2D);
+	
+	err = glGetError();
+	assert(err==GL_NO_ERROR);
+	
+	
+}
+
+#if 0
+//shift the image one pixel
+for(int y=0;y<texHeight;++y)
+{
+	//					const int	t=y>>5;
+	for(int x=1;x<texWidth;++x)
+	{
+		GLubyte*	org=image+(x+y*texWidth)*4;
+		
+		GLubyte*	dst=image+(x-1+y*texWidth)*4;
+		
+		dst[0] = org[0];
+		dst[1] = org[1];
+		dst[2] = org[2];
+		dst[3] = org[3];
+	}
+}
+//render a new row at the right
+for(int y=0;y<texHeight;++y)
+{
+	GLubyte*	pi=image+(texWidth-1+y*texWidth)*4;
+	pi[0]=255;
+	pi[1]=255;
+	pi[2]=255;
+	pi[3]=255;
+	if (y==texHeight*0.5)
+	{
+		pi[0]=200;
+		pi[1]=200;
+		pi[2]=200;
+		pi[3]=255;
+	}
+	
+}
+
+{
+	static float timer = 0.f;
+	static int prevValue=0;
+	timer+= 0.01;
+	float value = 128+100*sinf(timer);
+	MyClamp(value,0.f,float(texHeight-1));
+	GLubyte*	org=image+(texWidth-1+(int)value*texWidth)*4;
+	org[0] = 0;
+	org[1] = 0;
+	org[2] = 0;
+	org[3] = 255;
+	
+	if (prevValue<value)
+	{
+		
+	} else
+	{
+		
+	}
+	
+}
+
+{
+	static float timer = 1.4f;
+	timer+= 0.04;
+	float value = 128+150*sinf(timer);
+	MyClamp(value,0.f,float(texHeight-1));
+	
+	GLubyte*	org=image+(texWidth-1+(int)value*texWidth)*4;
+	org[0] = 0;
+	org[1] = 255;
+	org[2] = 0;
+	org[3] = 255;
+}
+
+{
+	static float timer = 1.4f;
+	timer+= 0.02;
+	float value =256+400*sinf(timer);
+	MyClamp(value,0.f,float(texHeight-1));
+	static int prevValue = 0;
+	
+	GLubyte*	org=image+(texWidth-1+(int)value*texWidth)*4;
+	org[0] = 0;
+	org[1] = 0;
+	org[2] = 255;
+	org[3] = 255;
+	
+	if (prevValue<value)
+	{
+		for (int i=prevValue;i<value;i++)
+		{
+			GLubyte*	org=image+(texHeight-1+(int)i*texWidth)*4;
+			org[0] = 0;
+			org[1] = 0;
+			org[2] = 255;
+			org[3] = 255;
+		}
+	} else
+	{
+		for (int i=value;i<prevValue;i++)
+		{
+			GLubyte*	org=image+(texHeight-1+(int)i*texWidth)*4;
+			org[0] = 0;
+			org[1] = 0;
+			org[2] = 255;
+			org[3] = 255;
+		}
+	}
+	prevValue = value;
+}
+
+
+glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, texWidth,texHeight,0,GL_RGBA,GL_UNSIGNED_BYTE,image);
+glGenerateMipmap(GL_TEXTURE_2D);
+
+#endif
+

Demos3/AllBullet2Demos/GraphingTexture.h

@@ -0,0 +1,36 @@
+#ifndef GRAPHING_TEXTURE_H
+#define GRAPHING_TEXTURE_H
+#include "LinearMath/btAlignedObjectArray.h"
+
+struct GraphingTexture
+{
+	int m_textureId;
+	//assume rgba (8 bit per component, total of 32bit per pixel, for m_width*m_height pixels)
+	btAlignedObjectArray<unsigned char> m_imageData;
+	int m_width;
+	int m_height;
+	
+	GraphingTexture();
+	virtual ~GraphingTexture();
+	
+	bool create(int texWidth, int texHeight);
+	bool destroy();
+	
+	void setPixel(int x, int y, unsigned char red, unsigned char green, unsigned char blue, unsigned char alpha)
+	{
+		m_imageData[x*4+y*4*m_width+0] = red;
+		m_imageData[x*4+y*4*m_width+1] = green;
+		m_imageData[x*4+y*4*m_width+2] = blue;
+		m_imageData[x*4+y*4*m_width+3] = alpha;
+	}
+	
+	void uploadImageData();
+	
+	int getTextureId()
+	{
+		return m_textureId;
+	}
+};
+
+#endif //GRAPHING_TEXTURE_H
+

Demos3/AllBullet2Demos/GwenParameterInterface.cpp

@@ -2,6 +2,7 @@
 #include "../GpuDemos/gwenInternalData.h"
 
 
+
 template<typename T>
 struct MySliderEventHandler : public Gwen::Event::Handler
 {
@@ -76,6 +77,24 @@ GwenParameterInterface::~GwenParameterInterface()
 }
 
 
+void GwenParameterInterface::setSliderValue(int sliderIndex, double sliderValue)
+{
+    int sliderCapped = sliderValue+4;
+    sliderCapped /= 8;
+    sliderCapped *= 8;
+    
+    if (sliderIndex>=0 && sliderIndex<m_paramInternalData->m_sliders.size())
+    {
+        m_paramInternalData->m_sliders[sliderIndex]->GetRangeMin();
+        
+        m_paramInternalData->m_sliders[sliderIndex]->GetRangeMax();
+        float mappedValue =m_paramInternalData->m_sliders[sliderIndex]->GetRangeMin()+
+        (m_paramInternalData->m_sliders[sliderIndex]->GetRangeMax()-
+         m_paramInternalData->m_sliders[sliderIndex]->GetRangeMin())*sliderCapped/128.f;
+        printf("mappedValue = %f\n",mappedValue);
+        m_paramInternalData->m_sliders[sliderIndex]->SetValue(mappedValue);
+    }
+}
 
 #include <stdio.h>
 void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params)
@@ -95,7 +114,7 @@ void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params)
 	pSlider->SetPos( 10, m_gwenInternalData->m_curYposition );
 	pSlider->SetSize( 100, 20 );
 	pSlider->SetRange( params.m_minVal, params.m_maxVal);
-	pSlider->SetNotchCount(20);//float(params.m_maxVal-params.m_minVal)/100.f);
+	pSlider->SetNotchCount(128);//float(params.m_maxVal-params.m_minVal)/100.f);
 	pSlider->SetClampToNotches( params.m_clampToNotches );
 	pSlider->SetValue( *params.m_paramValuePointer);//dimensions[i] );
 	char labelName[1024];

Demos3/AllBullet2Demos/GwenParameterInterface.h

@@ -12,6 +12,7 @@ struct GwenParameterInterface : public CommonParameterInterface
 	GwenParameterInterface(struct GwenInternalData* gwenInternalData);
 	virtual ~GwenParameterInterface();
 	virtual void registerSliderFloatParameter(SliderParams& params);
+    virtual void setSliderValue(int sliderIndex, double sliderValue);
 	virtual void syncParameters();
 	virtual void removeAllParameters();
 

Demos3/AllBullet2Demos/GwenProfileWindow.cpp

@@ -0,0 +1,291 @@
+#include "../GpuDemos/gwenUserInterface.h"
+#include "../GpuDemos/gwenInternalData.h"
+#include "LinearMath/btQuickprof.h"
+
+
+
+
+
+class MyProfileWindow : public Gwen::Controls::WindowControl
+{
+	
+	//		Gwen::Controls::TabControl*	m_TabControl;
+	//Gwen::Controls::ListBox*	m_TextOutput;
+	unsigned int				m_iFrames;
+	float						m_fLastSecond;
+	
+	Gwen::Controls::TreeNode* m_node;
+	Gwen::Controls::TreeControl* m_ctrl;
+	
+	
+protected:
+	
+	void onButtonA( Gwen::Controls::Base* pControl )
+	{
+        //		OpenTissue::glut::toggleIdle();
+	}
+	
+	void SliderMoved(Gwen::Controls::Base* pControl )
+	{
+		Gwen::Controls::Slider* pSlider = (Gwen::Controls::Slider*)pControl;
+		//this->m_app->scaleYoungModulus(pSlider->GetValue());
+		//	printf("Slider Value: %.2f", pSlider->GetValue() );
+	}
+	
+	
+	void	OnCheckChangedStiffnessWarping (Gwen::Controls::Base* pControl)
+	{
+		Gwen::Controls::CheckBox* labeled = (Gwen::Controls::CheckBox* )pControl;
+		bool checked = labeled->IsChecked();
+		//m_app->m_stiffness_warp_on  = checked;
+	}
+public:
+	
+  
+	CProfileIterator* profIter;
+	
+	MyProfileWindow (	Gwen::Controls::Base* pParent)
+    : Gwen::Controls::WindowControl( pParent ),
+	profIter(0)
+	{
+		SetTitle( L"Time Profiler" );
+		
+		SetSize( 450, 450 );
+		this->SetPos(10,400);
+		
+        //		this->Dock( Gwen::Pos::Bottom);
+		
+		
+		
+		{
+			m_ctrl = new Gwen::Controls::TreeControl( this );
+			m_node = m_ctrl->AddNode( L"Total Parent Time" );
+			
+			
+			//Gwen::Controls::TreeNode* pNode = ctrl->AddNode( L"Node Two" );
+			//pNode->AddNode( L"Node Two Inside" );
+			//pNode->AddNode( L"Eyes" );
+			//pNode->AddNode( L"Brown" )->AddNode( L"Node Two Inside" )->AddNode( L"Eyes" )->AddNode( L"Brown" );
+			//Gwen::Controls::TreeNode* node = ctrl->AddNode( L"Node Three" );
+			
+			
+			
+			//m_ctrl->Dock(Gwen::Pos::Bottom);
+			
+			m_ctrl->ExpandAll();
+            m_ctrl->SetKeyboardInputEnabled(true);
+			m_ctrl->SetBounds( this->GetInnerBounds().x,this->GetInnerBounds().y,this->GetInnerBounds().w,this->GetInnerBounds().h);
+			
+		}
+		
+		
+		
+	}
+	
+	
+	float	dumpRecursive(CProfileIterator* profileIterator, Gwen::Controls::TreeNode* parentNode)
+	{
+		profileIterator->First();
+		if (profileIterator->Is_Done())
+			return 0.f;
+		
+		float accumulated_time=0,parent_time = profileIterator->Is_Root() ? CProfileManager::Get_Time_Since_Reset() : profileIterator->Get_Current_Parent_Total_Time();
+		int i;
+		int frames_since_reset = CProfileManager::Get_Frame_Count_Since_Reset();
+		
+		//printf("Profiling: %s (total running time: %.3f ms) ---\n",	profileIterator->Get_Current_Parent_Name(), parent_time );
+		float totalTime = 0.f;
+		
+		
+		int numChildren = 0;
+		Gwen::UnicodeString txt;
+		std::vector<Gwen::Controls::TreeNode*> nodes;
+		
+		for (i = 0; !profileIterator->Is_Done(); i++,profileIterator->Next())
+		{
+			numChildren++;
+			float current_total_time = profileIterator->Get_Current_Total_Time();
+			accumulated_time += current_total_time;
+			double fraction = parent_time > SIMD_EPSILON ? (current_total_time / parent_time) * 100 : 0.f;
+			
+			Gwen::String name(profileIterator->Get_Current_Name());
+#ifdef _WIN32
+			Gwen::UnicodeString uname = Gwen::Utility::StringToUnicode(name);
+			
+			txt = Gwen::Utility::Format(L"%s (%.2f %%) :: %.3f ms / frame (%d calls)",uname.c_str(), fraction,(current_total_time / (double)frames_since_reset),profileIterator->Get_Current_Total_Calls());
+			
+#else
+			txt = Gwen::Utility::Format(L"%s (%.2f %%) :: %.3f ms / frame (%d calls)",name.c_str(), fraction,(current_total_time / (double)frames_since_reset),profileIterator->Get_Current_Total_Calls());
+			
+#endif
+			
+			Gwen::Controls::TreeNode* childNode = (Gwen::Controls::TreeNode*)profileIterator->Get_Current_UserPointer();
+			if (!childNode)
+			{
+                childNode = parentNode->AddNode(L"");
+                profileIterator->Set_Current_UserPointer(childNode);
+			}
+			childNode->SetText(txt);
+			nodes.push_back(childNode);
+			
+			totalTime += current_total_time;
+			//recurse into children
+		}
+		
+		for (i=0;i<numChildren;i++)
+		{
+			profileIterator->Enter_Child(i);
+			Gwen::Controls::TreeNode* curNode = nodes[i];
+			
+			dumpRecursive(profileIterator, curNode);
+			
+			profileIterator->Enter_Parent();
+		}
+		return accumulated_time;
+		
+	}
+	
+	void	UpdateText(CProfileIterator*  profileIterator, bool idle)
+	{
+		
+		static bool update=true;
+		
+        m_ctrl->SetBounds(0,0,this->GetInnerBounds().w,this->GetInnerBounds().h);
+		
+        //		if (!update)
+        //			return;
+		update=false;
+		
+		
+		static int test = 1;
+		test++;
+		
+		static double time_since_reset = 0.f;
+		if (!idle)
+		{
+			time_since_reset = CProfileManager::Get_Time_Since_Reset();
+		}
+		
+		//Gwen::UnicodeString txt = Gwen::Utility::Format( L"FEM Settings  %i fps", test );
+		{
+            //recompute profiling data, and store profile strings
+			
+            char blockTime[128];
+			
+            double totalTime = 0;
+			
+            int frames_since_reset = CProfileManager::Get_Frame_Count_Since_Reset();
+			
+            profileIterator->First();
+			
+            double parent_time = profileIterator->Is_Root() ? time_since_reset : profileIterator->Get_Current_Parent_Total_Time();
+			
+			
+            Gwen::Controls::TreeNode* curParent = m_node;
+			
+            double accumulated_time = dumpRecursive(profileIterator,m_node);
+			
+            const char* name = profileIterator->Get_Current_Parent_Name();
+#ifdef _WIN32
+            Gwen::UnicodeString uname = Gwen::Utility::StringToUnicode(name);
+            Gwen::UnicodeString txt = Gwen::Utility::Format( L"Profiling: %s total time: %.3f ms, unaccounted %.3f %% :: %.3f ms", uname.c_str(), parent_time ,
+                                                            parent_time > SIMD_EPSILON ? ((parent_time - accumulated_time) / parent_time) * 100 : 0.f, parent_time - accumulated_time);
+#else
+            Gwen::UnicodeString txt = Gwen::Utility::Format( L"Profiling: %s total time: %.3f ms, unaccounted %.3f %% :: %.3f ms", name, parent_time ,
+                                                            parent_time > SIMD_EPSILON ? ((parent_time - accumulated_time) / parent_time) * 100 : 0.f, parent_time - accumulated_time);
+#endif
+            //sprintf(blockTime,"--- Profiling: %s (total running time: %.3f ms) ---",	profileIterator->Get_Current_Parent_Name(), parent_time );
+            //displayProfileString(xOffset,yStart,blockTime);
+            m_node->SetText(txt);
+			
+			
+			//printf("%s (%.3f %%) :: %.3f ms\n", "Unaccounted:",);
+			
+			
+		}
+		
+		static int counter=10;
+		if (counter)
+		{
+            counter--;
+			m_ctrl->ExpandAll();
+		}
+		
+	}
+	void PrintText( const Gwen::UnicodeString& str )
+	{
+		
+	}
+	
+	void Render( Gwen::Skin::Base* skin )
+	{
+		m_iFrames++;
+		
+		if ( m_fLastSecond < Gwen::Platform::GetTimeInSeconds() )
+		{
+			SetTitle( Gwen::Utility::Format( L"Profiler  %i fps", m_iFrames ) );
+			
+			m_fLastSecond = Gwen::Platform::GetTimeInSeconds() + 1.0f;
+			m_iFrames = 0;
+		}
+		
+		Gwen::Controls::WindowControl::Render( skin );
+		
+	}
+	
+	
+};
+
+class MyMenuItems :  public Gwen::Controls::Base
+{
+	
+public:
+	
+	class MyProfileWindow* m_profWindow;
+    MyMenuItems() :Gwen::Controls::Base(0)
+    {
+    }
+   
+    void MenuItemSelect(Gwen::Controls::Base* pControl)
+    {
+		if (m_profWindow->Hidden())
+		{
+			m_profWindow->SetHidden(false);
+		} else
+		{
+			m_profWindow->SetHidden(true);
+		}
+		
+    }
+};
+
+
+MyProfileWindow* setupProfileWindow(GwenInternalData* data)
+{
+	MyMenuItems* menuItems = new MyMenuItems;
+	MyProfileWindow* profWindow = new MyProfileWindow(data->pCanvas);
+	//profWindow->SetHidden(true);	
+	profWindow->profIter = CProfileManager::Get_Iterator();
+	data->m_viewMenu->GetMenu()->AddItem( L"Profiler", menuItems,(Gwen::Event::Handler::Function)&MyMenuItems::MenuItemSelect);
+	menuItems->m_profWindow = profWindow;
+	return profWindow;
+}
+
+
+void	processProfileData( MyProfileWindow* profWindow, bool idle)
+{
+	if (profWindow)
+	{
+		
+		profWindow->UpdateText(profWindow->profIter, idle);
+	}	
+}
+
+void profileWindowSetVisible(MyProfileWindow* window, bool visible)
+{
+	window->SetHidden(!visible);
+}
+void destroyProfileWindow(MyProfileWindow* window)
+{
+	delete window;
+}

Demos3/AllBullet2Demos/GwenProfileWindow.h

@@ -0,0 +1,12 @@
+#ifndef GWEN_PROFILE_WINDOW_H
+#define GWEN_PROFILE_WINDOW_H
+
+struct MyProfileWindow* setupProfileWindow(struct GwenInternalData* data);
+
+void processProfileData(MyProfileWindow* window, bool idle);
+void profileWindowSetVisible(MyProfileWindow* window, bool visible);
+void destroyProfileWindow(MyProfileWindow* window);
+
+#endif//GWEN_PROFILE_WINDOW_H
+
+

Demos3/AllBullet2Demos/GwenTextureWindow.cpp

@@ -0,0 +1,100 @@
+#include "GwenTextureWindow.h"
+#include "../GpuDemos/gwenUserInterface.h"
+#include "../GpuDemos/gwenInternalData.h"
+#include "Gwen/Controls/ImagePanel.h"
+
+
+
+class MyGraphWindow : public Gwen::Controls::WindowControl
+{
+	Gwen::Controls::ImagePanel* m_imgPanel;
+
+public:
+	
+	class MyMenuItems2* m_menuItems;
+	
+	MyGraphWindow (	const MyGraphInput& input)
+    : Gwen::Controls::WindowControl( input.m_data->pCanvas ),
+	m_menuItems(0)
+	{
+		Gwen::UnicodeString str = Gwen::Utility::StringToUnicode(input.m_name);
+		SetTitle( str );
+
+
+		SetPos(input.m_xPos,input.m_yPos);
+		SetSize( 12+input.m_width+2*input.m_borderWidth, 30+input.m_height+2*input.m_borderWidth );
+		
+		m_imgPanel = new Gwen::Controls::ImagePanel( this );
+		if (input.m_texName)
+		{
+			Gwen::UnicodeString texName = Gwen::Utility::StringToUnicode(input.m_texName);
+			m_imgPanel->SetImage( texName );
+		}
+		m_imgPanel->SetBounds( input.m_borderWidth, input.m_borderWidth, 
+							  input.m_width, 
+							  input.m_height );
+        //		this->Dock( Gwen::Pos::Bottom);
+	}
+	virtual ~MyGraphWindow()
+	{
+		delete m_imgPanel;
+	}
+
+
+};
+
+class MyMenuItems2 :  public Gwen::Controls::Base
+{
+	MyGraphWindow* m_graphWindow;
+	
+public:
+	
+	Gwen::Controls::MenuItem* m_item;
+	
+    MyMenuItems2(MyGraphWindow* graphWindow) 
+	:Gwen::Controls::Base(0),
+	m_graphWindow(graphWindow),
+	m_item(0)
+    {
+    }
+	
+    void MenuItemSelect(Gwen::Controls::Base* pControl)
+    {
+		if (m_graphWindow->Hidden())
+		{
+			m_graphWindow->SetHidden(false);
+			//@TODO(erwincoumans) setCheck/SetCheckable drawing is broken, need to see what's wrong
+//			if (m_item)
+//				m_item->SetCheck(false);
+			
+		} else
+		{
+			m_graphWindow->SetHidden(true);
+//			if (m_item)
+//				m_item->SetCheck(true);
+			
+		}
+		
+    }
+};
+
+struct MyGraphWindow* setupTextureWindow(const MyGraphInput& input)
+{
+	MyGraphWindow* graphWindow = new MyGraphWindow(input);
+	MyMenuItems2* menuItems = new MyMenuItems2(graphWindow);
+	graphWindow->m_menuItems = menuItems;
+	
+	Gwen::UnicodeString str = Gwen::Utility::StringToUnicode(input.m_name);
+	menuItems->m_item = input.m_data->m_viewMenu->GetMenu()->AddItem( str, menuItems,(Gwen::Event::Handler::Function)&MyMenuItems2::MenuItemSelect);
+//	menuItems->m_item->SetCheckable(true);
+
+	return graphWindow;
+	
+}
+
+void destroyTextureWindow(MyGraphWindow* window)
+{
+	delete window->m_menuItems->m_item;
+	delete window->m_menuItems;
+	delete window;
+}

Demos3/AllBullet2Demos/GwenTextureWindow.h

@@ -0,0 +1,32 @@
+
+
+#ifndef GWEN_TEXTURE_WINDOW_H
+#define GWEN_TEXTURE_WINDOW_H
+
+struct MyGraphInput
+{
+	struct GwenInternalData* m_data;
+	int m_xPos;
+	int m_yPos;
+	int m_width;
+	int m_height;
+	int m_borderWidth;
+	const char* m_name;
+	const char* m_texName;
+	MyGraphInput(struct GwenInternalData* data)
+	:m_data(data),
+	m_xPos(0),
+	m_yPos(0),
+	m_width(400),
+	m_height(400),
+	m_borderWidth(0),
+	m_name("GraphWindow"),
+	m_texName(0)
+	{
+	}
+};
+struct MyGraphWindow* setupTextureWindow(const MyGraphInput& input);
+void destroyTextureWindow(MyGraphWindow* window);
+
+
+#endif //GWEN_TEXTURE_WINDOW_H

Demos3/AllBullet2Demos/main.cpp

@@ -1,3 +1,4 @@
+
 #include "../../btgui/OpenGLWindow/SimpleOpenGL3App.h"
 #include "Bullet3Common/b3Vector3.h"
 #include "assert.h"
@@ -7,6 +8,10 @@
 #include "BulletDemoEntries.h"
 #include "../../btgui/Timing/b3Clock.h"
 #include "GwenParameterInterface.h"
+#include "GwenProfileWindow.h"
+#include "GwenTextureWindow.h"
+#include "GraphingTexture.h"
+
 
 #define DEMO_SELECTION_COMBOBOX 13
 const char* startFileName = "bulletDemo.txt";
@@ -16,13 +21,89 @@ static int sCurrentDemoIndex = 0;
 static int sCurrentHightlighted = 0;
 static BulletDemoInterface* sCurrentDemo = 0;
 static b3AlignedObjectArray<const char*> allNames;
-double motorA=0,motorB=0;
-
-
 bool drawGUI=true;
 extern bool useShadowMap;
 static bool wireframe=false;
 static bool pauseSimulation=false;
+int midiBaseIndex = 176;
+
+
+#ifdef B3_USE_MIDI
+#include "../../btgui/MidiTest/RtMidi.h"
+bool chooseMidiPort( RtMidiIn *rtmidi )
+{
+    if (!rtmidi)
+        return false;
+    
+    std::string portName;
+    unsigned int i = 0, nPorts = rtmidi->getPortCount();
+    if ( nPorts == 0 ) {
+        std::cout << "No input ports available!" << std::endl;
+        return false;
+    }
+    
+    if ( nPorts == 1 ) {
+        std::cout << "\nOpening " << rtmidi->getPortName() << std::endl;
+    }
+    else {
+        for ( i=0; i<nPorts; i++ ) {
+            portName = rtmidi->getPortName(i);
+            std::cout << "  Input port #" << i << ": " << portName << '\n';
+        }
+        
+        do {
+            std::cout << "\nChoose a port number: ";
+            std::cin >> i;
+        } while ( i >= nPorts );
+    }
+    
+    //  std::getline( std::cin, keyHit );  // used to clear out stdin
+    rtmidi->openPort( i );
+    
+    return true;
+}
+void PairMidiCallback( double deltatime, std::vector< unsigned char > *message, void *userData )
+{
+    unsigned int nBytes = message->size();
+    if (nBytes==3)
+    {
+        //if ( message->at(1)==16)
+        {
+            printf("midi %d at %f = %d\n", message->at(1), deltatime, message->at(2));
+            //test->SetValue(message->at(2));
+#if KORG_MIDI
+            if (message->at(0)>=midiBaseIndex && message->at(0)<(midiBaseIndex+8))
+            {
+                int sliderIndex = message->at(0)-midiBaseIndex;//-16;
+                printf("sliderIndex = %d\n", sliderIndex);
+                float sliderValue = message->at(2);
+                printf("sliderValue = %f\n", sliderValue);
+                app->m_parameterInterface->setSliderValue(sliderIndex,sliderValue);
+            }
+#else
+            //ICONTROLS
+            if (message->at(0)==176)
+            {
+                int sliderIndex = message->at(1)-32;//-16;
+                printf("sliderIndex = %d\n", sliderIndex);
+                float sliderValue = message->at(2);
+                printf("sliderValue = %f\n", sliderValue);
+                app->m_parameterInterface->setSliderValue(sliderIndex,sliderValue);
+            }
+
+#endif
+            
+        }
+    }
+}
+
+#endif //B3_USE_MIDI
+
+
+
+
+
+
 void MyKeyboardCallback(int key, int state)
 {
 
@@ -225,7 +306,25 @@ struct MyMenuItemHander :public Gwen::Event::Handler
 
 
 };
+#include "Bullet3Common/b3HashMap.h"
 
+struct GL3TexLoader : public MyTextureLoader
+{
+	b3HashMap<b3HashString,GLint> m_hashMap;
+	
+	virtual void LoadTexture( Gwen::Texture* pTexture )
+	{
+		const char* n = pTexture->name.Get().c_str();
+		GLint* texIdPtr = m_hashMap[n];
+		if (texIdPtr)
+		{
+			pTexture->m_intData = *texIdPtr;
+		}
+	}
+	virtual void FreeTexture( Gwen::Texture* pTexture )
+	{
+	}
+};
 
 
 extern float shadowMapWorldSize;
@@ -239,6 +338,7 @@ int main(int argc, char* argv[])
 	int width = 1024;
 	int height=768;
 
+	
 	app = new SimpleOpenGL3App("AllBullet2Demos",width,height);
 	app->m_instancingRenderer->setCameraDistance(13);
 	app->m_instancingRenderer->setCameraPitch(0);
@@ -257,6 +357,55 @@ int main(int argc, char* argv[])
 //	gui->getInternalData()->m_explorerPage
 	Gwen::Controls::TreeControl* tree = gui->getInternalData()->m_explorerTreeCtrl;
 
+	GL3TexLoader* myTexLoader = new GL3TexLoader;
+	gui->getInternalData()->pRenderer->setTextureLoader(myTexLoader);
+
+	
+	MyProfileWindow* profWindow = setupProfileWindow(gui->getInternalData());
+	profileWindowSetVisible(profWindow,false);
+
+	{
+		MyGraphInput input(gui->getInternalData());
+		input.m_width=300;
+		input.m_height=300;
+		input.m_xPos = 0;
+		input.m_yPos = height-input.m_height;
+		input.m_name="Test Graph1";
+		input.m_texName = "graph1";
+		GraphingTexture* gt = new GraphingTexture;
+		gt->create(256,256);
+		int texId = gt->getTextureId();
+		myTexLoader->m_hashMap.insert("graph1", texId);
+		MyGraphWindow* gw = setupTextureWindow(input);
+	}
+	if (1)
+	{
+		MyGraphInput input(gui->getInternalData());
+		input.m_width=300;
+		input.m_height=300;
+		input.m_xPos = width-input.m_width;
+		input.m_yPos = height-input.m_height;
+		input.m_name="Test Graph2";
+		input.m_texName = "graph2";
+		GraphingTexture* gt = new GraphingTexture;
+		int texWidth = 512;
+		int texHeight = 512;
+		gt->create(texWidth,texHeight);
+		for (int i=0;i<texWidth;i++)
+		{
+			for (int j=0;j<texHeight;j++)
+			{
+				gt->setPixel(i,j,0,0,0,255);
+			}
+		}
+		gt->uploadImageData();
+		
+		int texId = gt->getTextureId();
+		input.m_xPos = width-input.m_width;
+		myTexLoader->m_hashMap.insert("graph2", texId);
+		MyGraphWindow* gw = setupTextureWindow(input);
+	}
+	//destroyTextureWindow(gw);
 	
 	
 	app->m_parameterInterface = new GwenParameterInterface(gui->getInternalData());
@@ -333,18 +482,22 @@ int main(int argc, char* argv[])
         DrawGridData dg;
         dg.upAxis = app->getUpAxis();
 
+        {
+            BT_PROFILE("Update Camera");
             app->m_instancingRenderer->updateCamera(dg.upAxis);
+        }
+        {
+            BT_PROFILE("Draw Grid");
             app->drawGrid(dg);
-
+        }
 		static int frameCount = 0;
 		frameCount++;
 
 		if (0)
 		{
+            BT_PROFILE("Draw frame counter");
             char bla[1024];
-
+            sprintf(bla,"Frame %d", frameCount);
-		sprintf(bla,"Simple test frame %d", frameCount);
-
             app->drawText(bla,10,10);
 		}
 
@@ -361,18 +514,34 @@ int main(int argc, char* argv[])
 				sCurrentDemo->stepSimulation(deltaTimeInSeconds);//1./60.f);
 				prevTimeInMicroseconds = curTimeInMicroseconds;
 			}
+            {
+                BT_PROFILE("Render Scene");
                 sCurrentDemo->renderScene();
+            }
+            {
                 sCurrentDemo->physicsDebugDraw();
+            }
 		}
 
 		static int toggle = 1;
 		if (1)
 		{
+            if (!pauseSimulation)
+                processProfileData(profWindow,false);
+            {
+                BT_PROFILE("Draw Gwen GUI");
                 gui->draw(app->m_instancingRenderer->getScreenWidth(),app->m_instancingRenderer->getScreenHeight());
+            }
 		}
 		toggle=1-toggle;
+        {
+            BT_PROFILE("Sync Parameters");
             app->m_parameterInterface->syncParameters();
+        }
+        {
+            BT_PROFILE("Swap Buffers");
             app->swapBuffer();
+        }
 	} while (!app->m_window->requestedExit());
 
 //	selectDemo(0);

Demos3/AllBullet2Demos/premake4.lua

@@ -18,11 +18,34 @@
 		initOpenGL()
 		initGlew()
 
+     if _OPTIONS["midi"] then
+                 if os.is("Windows") then
+                         files {"../../btgui/MidiTest/RtMidi.cpp"}
+                         links {"winmm"}
+                         defines {"__WINDOWS_MM__", "WIN32","B3_USE_MIDI"}
+                 end
+
+                 if os.is("Linux") then
+                       defines {"__LINUX_ALSA__"}
+                       links {"asound","pthread"}
+                 end
+
+                 if os.is("MacOSX") then
+                         files {"../../btgui/MidiTest/RtMidi.cpp"}
+                         links{"CoreAudio.framework", "coreMIDI.framework", "Cocoa.framework"}
+                         defines {"__MACOSX_CORE__","B3_USE_MIDI"}
+                 end
+         end
+	
 		files {
 		"**.cpp",
 		"**.h",
 		"../bullet2/BasicDemo/Bullet2RigidBodyDemo.cpp",
 		"../bullet2/BasicDemo/Bullet2RigidBodyDemo.h",
+		"../bullet2/LuaDemo/LuaPhysicsSetup.cpp",
+		"../bullet2/LuaDemo/LuaPhysicsSetup.h",
+		"../DifferentialGearDemo/DifferentialGearSetup.cpp",
+		"../DifferentialGearDemo/DifferentialGearSetup.h",
 		"../../Demos/BasicDemo/BasicDemoPhysicsSetup.cpp",
 		"../../Demos/BasicDemo/BasicDemoPhysicsSetup.h",
 		"../../Demos/CcdPhysicsDemo/CcdPhysicsSetup.cpp",

Demos3/GpuDemos/gwenInternalData.h

@@ -39,6 +39,7 @@ struct GwenInternalData
 	Gwen::Controls::TabButton*		m_demoPage;
 	Gwen::Controls::TabButton*		m_explorerPage;
 	Gwen::Controls::TreeControl*	m_explorerTreeCtrl;
+	Gwen::Controls::MenuItem*		m_viewMenu;
 
 	int		m_curYposition;
 

Demos3/GpuDemos/gwenUserInterface.cpp

@@ -1,6 +1,11 @@
 
 #include "gwenUserInterface.h"
 #include "gwenInternalData.h"
+#include "Gwen/Controls/ImagePanel.h"
+
+class MyGraphWindow* graphWindow = 0;
+
+
 
 GwenUserInterface::GwenUserInterface()
 {
@@ -32,6 +37,9 @@ GwenUserInterface::~GwenUserInterface()
 
 }
 
+
+
+
 class MyMenuItems :  public Gwen::Controls::Base
 {
 public:
@@ -48,7 +56,8 @@ public:
 struct MyTestMenuBar : public Gwen::Controls::MenuStrip
 {
 
-
+	Gwen::Controls::MenuItem* m_fileMenu;
+	Gwen::Controls::MenuItem* m_viewMenu;
 
 	MyTestMenuBar(Gwen::Controls::Base* pParent)
 		:Gwen::Controls::MenuStrip(pParent)
@@ -57,18 +66,10 @@ struct MyTestMenuBar : public Gwen::Controls::MenuStrip
 		{
 			MyMenuItems* menuItems = new MyMenuItems;
 
-			Gwen::Controls::MenuItem* pRoot = AddItem( L"File" );
+			m_fileMenu = AddItem( L"File" );
-			pRoot->GetMenu()->AddItem(L"Quit",menuItems,(Gwen::Event::Handler::Function)&MyMenuItems::myQuitApp);
+			m_fileMenu->GetMenu()->AddItem(L"Quit",menuItems,(Gwen::Event::Handler::Function)&MyMenuItems::myQuitApp);
-			pRoot = AddItem( L"View" );
+			m_viewMenu = AddItem( L"View" );
-//			Gwen::Event::Handler* handler =	GWEN_MCALL(&MyTestMenuBar::MenuItemSelect );
-			pRoot->GetMenu()->AddItem( L"Profiler");//,,m_profileWindow,(Gwen::Event::Handler::Function)&MyProfileWindow::MenuItemSelect);
 			
-/*			pRoot->GetMenu()->AddItem( L"New", L"test16.png", GWEN_MCALL( ThisClass::MenuItemSelect ) );
-			pRoot->GetMenu()->AddItem( L"Load", L"test16.png", GWEN_MCALL( ThisClass::MenuItemSelect ) );
-			pRoot->GetMenu()->AddItem( L"Save", GWEN_MCALL( ThisClass::MenuItemSelect ) );
-			pRoot->GetMenu()->AddItem( L"Save As..", GWEN_MCALL( ThisClass::MenuItemSelect ) );
-			pRoot->GetMenu()->AddItem( L"Quit", GWEN_MCALL( ThisClass::MenuItemSelect ) );
-			*/
 		}
 	}
 
@@ -155,6 +156,7 @@ void	GwenUserInterface::init(int width, int height,struct sth_stash* stash,float
 	m_data->pCanvas->SetBackgroundColor( Gwen::Color( 150, 170, 170, 255 ) );
 
 	MyTestMenuBar* menubar = new MyTestMenuBar(m_data->pCanvas);
+	m_data->m_viewMenu = menubar->m_viewMenu;
 	Gwen::Controls::StatusBar* bar = new Gwen::Controls::StatusBar(m_data->pCanvas);
 	m_data->m_rightStatusBar = new Gwen::Controls::Label( bar );
 	m_data->m_rightStatusBar->SetWidth(width/2);
@@ -257,6 +259,7 @@ void	GwenUserInterface::init(int width, int height,struct sth_stash* stash,float
 	ctrl->Focus();
 	ctrl->SetBounds(2, 10, 236, 400);
 
+   
 }
 
 b3ToggleButtonCallback	GwenUserInterface::getToggleButtonCallback()

Demos3/GpuDemos/gwenUserInterface.h

@@ -47,5 +47,7 @@ class GwenUserInterface
 
 };
 
+
+
 #endif //_GWEN_USER_INTERFACE_H
 

Demos3/ImportObjDemo/ImportObjSetup.cpp

@@ -44,9 +44,7 @@ static GLInstanceGraphicsShape* gCreateGraphicsShapeFromWavefrontObj(std::vector
 					int vtxBaseIndex = vertices->size();
 					
 					
-					indicesPtr->push_back(vtxBaseIndex);
+					
-					indicesPtr->push_back(vtxBaseIndex+1);
-					indicesPtr->push_back(vtxBaseIndex+2);
 					
 					GLInstanceVertex vtx0;
 					vtx0.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f]*3+0];
@@ -79,6 +77,10 @@ static GLInstanceGraphicsShape* gCreateGraphicsShapeFromWavefrontObj(std::vector
 					btVector3 v2(vtx2.xyzw[0],vtx2.xyzw[1],vtx2.xyzw[2]);
 					
 					normal = (v1-v0).cross(v2-v0);
+                    btScalar len2 = normal.length2();
+                    //skip degenerate triangles
+                    if (len2 > SIMD_EPSILON)
+                    {
                         normal.normalize();
                         vtx0.normal[0] = normal[0];
                         vtx0.normal[1] = normal[1];
@@ -92,6 +94,11 @@ static GLInstanceGraphicsShape* gCreateGraphicsShapeFromWavefrontObj(std::vector
                         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);
+                    }
+                    
 				}
 			}
 		}
@@ -113,6 +120,7 @@ static GLInstanceGraphicsShape* gCreateGraphicsShapeFromWavefrontObj(std::vector
 
 void ImportObjDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 {
+	gfxBridge.setUpAxis(2);
 	this->createEmptyDynamicsWorld();
 	gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
 	m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);

Demos3/ImportSTLDemo/ImportSTLSetup.cpp

@@ -111,6 +111,7 @@ GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
 
 void ImportSTLDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 {
+	gfxBridge.setUpAxis(2);
 	this->createEmptyDynamicsWorld();
 	gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
 	m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);

Demos3/ImportURDFDemo/ImportURDFSetup.cpp

@@ -1,4 +1,11 @@
+
 #include "ImportURDFSetup.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+
+static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter;
+static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter);
+static bool enableConstraints = true;//false;
+
 
 ImportUrdfDemo::ImportUrdfDemo()
 {
@@ -14,6 +21,13 @@ ImportUrdfDemo::~ImportUrdfDemo()
 
 
 #include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h"
+
+#include "urdf_samples.h"
+
+//#include "BulletCollision/CollisionShapes/btCylinderShape.h"
+//#define USE_BARREL_VERTICES
+//#include "OpenGLWindow/ShapeData.h"
+
 #include <iostream>
 #include <fstream>
 
@@ -42,685 +56,55 @@ void printTree(my_shared_ptr<const Link> link,int level = 0)
 }
 
 
-#define MSTRINGIFY(A) #A
+struct URDF_LinkInformation
+{
+    const Link* m_thisLink;
 
+    btTransform m_localInertialFrame;
+    btTransform m_localVisualFrame;
 
-const char* urdf_char2 = MSTRINGIFY(
+    btRigidBody* m_bulletRigidBody;
-                                   <robot name="test_robot">
+	virtual ~URDF_LinkInformation()
-                                   <link name="link1" />
+	{
-                                   <link name="link2" />
+printf("~\n");
-                                   <link name="link3" />
+	}
-                                   <link name="link4" />
+};
 
-                                   <joint name="joint1" type="continuous">
+struct URDF_JointInformation
-                                   <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(
+struct URDF2BulletMappings
-                                   <?xml version="1.0"?>
+{
-                                   <robot name="myfirst">
+    btHashMap<btHashPtr /*to Link*/, URDF_LinkInformation*> m_link2rigidbody;
-                                   <link name="base_link">
+    btHashMap<btHashPtr /*to Joint*/, btTypedConstraint*> m_joint2Constraint;
-                                   <visual>
+};
-                                   <geometry>
-                                   <cylinder length="0.6" radius="0.2"/>
-                                   </geometry>
-                                   </visual>
-                                   </link>
-                                   </robot>
-                                   );
 
-const char* urdf_char3 = MSTRINGIFY(<?xml version="1.0"?>
+void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btDiscreteDynamicsWorld* world, URDF2BulletMappings& mappings)
-                                   <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="flexible">
-								   <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="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>
-								   </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>
-								   </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="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>
-								   </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>
-								   </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="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="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="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="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>
-								   </link>
-								   
-								   <joint name="tobox" type="fixed">
-								   <parent link="head"/>
-								   <child link="box"/>
-								   <origin xyz="0 0.1414 0.1414"/>
-								   </joint>
-								   
-								   </robot>
-
-);
-
-#include "BulletCollision/CollisionShapes/btCylinderShape.h"
-
-void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btDiscreteDynamicsWorld* world)
 {
     btCollisionShape* shape = 0;
 
 	btTransform linkTransformInWorldSpace;
 	linkTransformInWorldSpace.setIdentity();
 
-	btScalar mass = 0;
+	btScalar mass = 1;
 	btTransform inertialFrame;
 	inertialFrame.setIdentity();
+    const Link* parentLink = (*link).getParent();
+	URDF_LinkInformation* pp = 0;
 
+    {
+		URDF_LinkInformation** ppRigidBody = mappings.m_link2rigidbody.find(parentLink);
+		if (ppRigidBody)
+		{
+		pp = (*ppRigidBody);
+			btRigidBody* parentRigidBody = pp->m_bulletRigidBody;
+			btTransform tr = parentRigidBody->getWorldTransform();
+			printf("rigidbody origin (COM) of link(%s) parent(%s): %f,%f,%f\n",(*link).name.c_str(), parentLink->name.c_str(), tr.getOrigin().x(), tr.getOrigin().y(), tr.getOrigin().z());
+		}
+	}
 	if ((*link).inertial)
 	{
 		mass = (*link).inertial->mass;
@@ -728,12 +112,14 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 		inertialFrame.setRotation(btQuaternion((*link).inertial->origin.rotation.x,(*link).inertial->origin.rotation.y,(*link).inertial->origin.rotation.z,(*link).inertial->origin.rotation.w));
 	}
 
+    btTransform parent2joint;
+
 	if ((*link).parent_joint)
 	{
 		btTransform p2j;
 		const urdf::Vector3 pos = (*link).parent_joint->parent_to_joint_origin_transform.position;
 		const urdf::Rotation orn = (*link).parent_joint->parent_to_joint_origin_transform.rotation;
-		btTransform parent2joint;
+
 		parent2joint.setOrigin(btVector3(pos.x,pos.y,pos.z));
 		parent2joint.setRotation(btQuaternion(orn.x,orn.y,orn.z,orn.w));
 		linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
@@ -742,7 +128,6 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 		linkTransformInWorldSpace = parentTransformInWorldSpace;
 	}
 
-	//btTransform linkCenterOfMass =inertialFrame*linkTransformInWorldSpace;
 
     {
         printf("converting link %s",link->name.c_str());
@@ -757,8 +142,25 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
                 {
                     printf("processing a cylinder\n");
                     urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get();
-                    btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
+                    
-                    btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
+                    btAlignedObjectArray<btVector3> vertices;
+                    //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
+                    int numSteps = 32;
+                    for (int i=0;i<numSteps;i++)
+                    {
+                        
+                        btVector3 vert(cyl->radius*btSin(SIMD_2_PI*(float(i)/numSteps)),cyl->radius*btCos(SIMD_2_PI*(float(i)/numSteps)),cyl->length/2.);
+                        vertices.push_back(vert);
+                        vert[2] = -cyl->length/2.;
+                        vertices.push_back(vert);
+                        
+                    }
+                    btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
+                    cylZShape->initializePolyhedralFeatures();
+                    //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
+                    //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
+                    cylZShape->setMargin(0.001);
+                    
                     shape = cylZShape;
                     break;
                 }
@@ -775,7 +177,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
                 {
 					printf("processing a sphere\n");
                     urdf::Sphere* sphere = (urdf::Sphere*)visual->geometry.get();
-                    btScalar radius = sphere->radius;
+                    btScalar radius = sphere->radius*0.8;
 					btSphereShape* sphereShape = new btSphereShape(radius);
                     shape = sphereShape;
                     break;
@@ -823,8 +225,104 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 
 
                 btRigidBody* body = new btRigidBody(rbci);
-				world->addRigidBody(body);
+                
+				world->addRigidBody(body,bodyCollisionFilterGroup,bodyCollisionFilterMask);
+    //            body->setFriction(0);
+
                 gfxBridge.createRigidBodyGraphicsObject(body,color);
+                URDF_LinkInformation* linkInfo = new URDF_LinkInformation;
+                linkInfo->m_bulletRigidBody = body;
+                linkInfo->m_localVisualFrame =visual_frame;
+                linkInfo->m_localInertialFrame =inertialFrame;
+                linkInfo->m_thisLink = link.get();
+                const Link* p = link.get();
+                mappings.m_link2rigidbody.insert(p, linkInfo);
+
+                //create a joint if necessary
+                if ((*link).parent_joint)
+                {
+                    btRigidBody* parentBody =pp->m_bulletRigidBody;
+
+                    const Joint* pj = (*link).parent_joint.get();
+                    btTransform offsetInA,offsetInB;
+                    btTransform p2j; p2j.setIdentity();
+                    btVector3 p2jPos; p2jPos.setValue(pj->parent_to_joint_origin_transform.position.x,
+                                                      pj->parent_to_joint_origin_transform.position.y,
+                                                      pj->parent_to_joint_origin_transform.position.z);
+                    btQuaternion p2jOrn;p2jOrn.setValue(pj->parent_to_joint_origin_transform.rotation.x,
+                                                        pj->parent_to_joint_origin_transform.rotation.y,
+                                                        pj->parent_to_joint_origin_transform.rotation.z,
+                                                        pj->parent_to_joint_origin_transform.rotation.w);
+
+                    p2j.setOrigin(p2jPos);
+                    p2j.setRotation(p2jOrn);
+
+                    offsetInA.setIdentity();
+
+                    offsetInA = pp->m_localVisualFrame.inverse()*p2j;
+                    offsetInB.setIdentity();
+                    offsetInB = visual_frame.inverse();
+
+                    switch (pj->type)
+                    {
+                        case Joint::FIXED:
+                        {
+                            printf("Fixed joint\n");
+                            btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentBody, *body,offsetInA,offsetInB);
+//                            btVector3 bulletAxis(pj->axis.x,pj->axis.y,pj->axis.z);
+                          dof6->setLinearLowerLimit(btVector3(0,0,0));
+                            dof6->setLinearUpperLimit(btVector3(0,0,0));
+
+                            dof6->setAngularLowerLimit(btVector3(0,0,0));
+                           dof6->setAngularUpperLimit(btVector3(0,0,0));
+
+                            if (enableConstraints)
+                                world->addConstraint(dof6,true);
+
+//                            btFixedConstraint* fixed = new btFixedConstraint(*parentBody, *body,offsetInA,offsetInB);
+  //                          world->addConstraint(fixed,true);
+                            break;
+                        }
+                        case Joint::CONTINUOUS:
+                        case Joint::REVOLUTE:
+                        {
+                            btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentBody, *body,offsetInA,offsetInB);
+//                            btVector3 bulletAxis(pj->axis.x,pj->axis.y,pj->axis.z);
+                          dof6->setLinearLowerLimit(btVector3(0,0,0));
+                            dof6->setLinearUpperLimit(btVector3(0,0,0));
+
+                            dof6->setAngularLowerLimit(btVector3(0,0,1000));
+                           dof6->setAngularUpperLimit(btVector3(0,0,-1000));
+
+                            if (enableConstraints)
+                                world->addConstraint(dof6,true);
+
+                            printf("Revolute/Continuous joint\n");
+                            break;
+                        }
+                        case Joint::PRISMATIC:
+                        {
+                            btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentBody, *body,offsetInA,offsetInB);
+
+                            dof6->setLinearLowerLimit(btVector3(pj->limits->lower,0,0));
+                            dof6->setLinearUpperLimit(btVector3(pj->limits->upper,0,0));
+
+                            dof6->setAngularLowerLimit(btVector3(0,0,0));
+                            dof6->setAngularUpperLimit(btVector3(0,0,0));
+
+                            if (enableConstraints)
+                                world->addConstraint(dof6,true);
+
+                            printf("Prismatic\n");
+                            break;
+                        }
+                        default:
+                        {
+                            printf("Error: unsupported joint type in URDF (%d)\n", pj->type);
+                        }
+                    }
+
+                }
             }
         }
     }
@@ -833,7 +331,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
     {
         if (*child)
         {
-            URDFvisual2BulletCollisionShape(*child,gfxBridge, linkTransformInWorldSpace, world);
+            URDFvisual2BulletCollisionShape(*child,gfxBridge, linkTransformInWorldSpace, world,mappings);
 
         }
         else
@@ -850,21 +348,18 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 {
 
 	int upAxis = 2;
-	gfxBridge.setUpAxis(upAxis);
+	gfxBridge.setUpAxis(2);
 
 	this->createEmptyDynamicsWorld();
-/*	btVector3 groundHalfExtents(50,50,50);
+    gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
-	groundHalfExtents[upAxis]=1.f;
+    m_dynamicsWorld->getDebugDrawer()->setDebugMode(
-	btBoxShape* box = new btBoxShape(groundHalfExtents);
+    //btIDebugDraw::DBG_DrawConstraints
-	gfxBridge.createCollisionShapeGraphicsObject(box);
+    +btIDebugDraw::DBG_DrawContactPoints
-	btTransform start; start.setIdentity();
+    //+btIDebugDraw::DBG_DrawAabb
-	btVector3 groundOrigin(0,0,0);
+        );//+btIDebugDraw::DBG_DrawConstraintLimits);
-	groundOrigin[upAxis]=-5;
+
-	start.setOrigin(groundOrigin);	
+
-	btRigidBody* body =  createRigidBody(0,start,box);
+    
-	btVector3 color(0,1,0);
-	gfxBridge.createRigidBodyGraphicsObject(body,color);
-*/
 	btVector3 gravity(0,0,0);
 	gravity[upAxis]=-9.8;
 
@@ -914,7 +409,24 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 	worldTrans.setIdentity();
 
     {
-        URDFvisual2BulletCollisionShape(root_link, gfxBridge, worldTrans,m_dynamicsWorld);
+        URDF2BulletMappings mappings;
+        URDFvisual2BulletCollisionShape(root_link, gfxBridge, worldTrans,m_dynamicsWorld,mappings);
+    }
+
+    {
+        btVector3 groundHalfExtents(20,20,20);
+        groundHalfExtents[upAxis]=1.f;
+        btBoxShape* box = new btBoxShape(groundHalfExtents);
+        box->initializePolyhedralFeatures();
+        
+        gfxBridge.createCollisionShapeGraphicsObject(box);
+        btTransform start; start.setIdentity();
+        btVector3 groundOrigin(0,0,0);
+        groundOrigin[upAxis]=-1.5;
+        start.setOrigin(groundOrigin);
+        btRigidBody* body =  createRigidBody(0,start,box);
+        btVector3 color(0.5,0.5,0.5);
+        gfxBridge.createRigidBodyGraphicsObject(body,color);
     }
     
 

Demos3/ImportURDFDemo/urdf_samples.h

@@ -0,0 +1,671 @@
+#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="flexible">
+								   <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="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>
+								   </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>
+								   </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="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>
+								   </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>
+								   </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="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="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="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="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>
+								   </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
+

Demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.cpp

@@ -167,6 +167,7 @@ Bullet2RigidBodyDemo::Bullet2RigidBodyDemo(SimpleOpenGL3App* app, CommonPhysicsS
 void Bullet2RigidBodyDemo::initPhysics()
 {
 	MyGraphicsPhysicsBridge glBridge(m_glApp);
+	glBridge.setUpAxis(1);
 	m_physicsSetup->initPhysics(glBridge);
 	m_glApp->m_instancingRenderer->writeTransforms();
 

Demos3/bullet2/BasicDemo/MyDebugDrawer.h

@@ -60,8 +60,10 @@ public:
 
 	virtual void	drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color)
 	{
+        drawLine(PointOnB,PointOnB+normalOnB,color);
 	}
      
+
 	virtual void	reportErrorWarning(const char* warningString)
 	{
 	}

Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp

@@ -599,7 +599,10 @@ void	FeatherstoneDemo1::renderScene()
 			btVector3 pos = col->getWorldTransform().getOrigin();
 			btQuaternion orn = col->getWorldTransform().getRotation();
 			int index = col->getUserIndex();
+            if (index>=0)
+            {
                 m_glApp->m_instancingRenderer->writeSingleInstanceTransformToCPU(pos,orn,index);
+            }
 		}
 		m_glApp->m_instancingRenderer->writeTransforms();
 	}

Demos3/bullet2/LuaDemo/LuaPhysicsSetup.cpp

@@ -0,0 +1,467 @@
+#include "LuaPhysicsSetup.h"
+
+#include "OpenGLWindow/SimpleOpenGL3App.h"//??
+#include "btBulletDynamicsCommon.h"
+#include "LinearMath/btVector3.h"
+#include <iostream>
+
+#include "BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h"
+
+extern "C" {
+	#include "lua.h"
+	#include "lualib.h"
+#include "lauxlib.h"
+}
+
+
+char* sLuaFileName = "init_physics.lua";
+
+static const float scaling=0.35f;
+static LuaPhysicsSetup* sLuaDemo = 0;
+
+static btVector4 colors[4] =
+{
+	btVector4(1,0,0,1),
+	btVector4(0,1,0,1),
+	btVector4(0,1,1,1),
+	btVector4(1,1,0,1),
+};
+
+LuaPhysicsSetup::LuaPhysicsSetup(class SimpleOpenGL3App* app)
+:m_glApp(app),
+m_config(0),
+m_dispatcher(0),
+m_bp(0),
+m_solver(0),
+m_dynamicsWorld(0)
+{
+    sLuaDemo = this;
+}
+
+LuaPhysicsSetup::~LuaPhysicsSetup()
+{
+    sLuaDemo = 0;
+}
+
+
+
+//todo: allow to create solver, broadphase, multiple worlds etc.
+static int gCreateDefaultDynamicsWorld(lua_State *L)
+{
+	sLuaDemo->m_config = new btDefaultCollisionConfiguration;
+	sLuaDemo->m_dispatcher = new btCollisionDispatcher(sLuaDemo->m_config);
+	sLuaDemo->m_bp = new btDbvtBroadphase();
+	sLuaDemo->m_solver = new btNNCGConstraintSolver();
+	sLuaDemo->m_dynamicsWorld = new btDiscreteDynamicsWorld(sLuaDemo->m_dispatcher,sLuaDemo->m_bp,sLuaDemo->m_solver,sLuaDemo->m_config);
+	lua_pushlightuserdata (L, sLuaDemo->m_dynamicsWorld);
+	return 1;
+}
+
+
+static int gDeleteDynamicsWorld(lua_State *L)
+{
+	return 0;
+}
+
+ATTRIBUTE_ALIGNED16(struct) CustomShapeData
+{
+	btVector3 m_localScaling;
+	int	m_shapeIndex;
+
+
+};
+
+
+ATTRIBUTE_ALIGNED16(struct) CustomRigidBodyData
+{
+	int m_graphicsInstanceIndex;
+};
+
+static int gCreateCubeShape(lua_State *L)
+{
+	int argc = lua_gettop(L);
+	if (argc==4)
+	{
+		btVector3 halfExtents(1,1,1);
+		if (!lua_isuserdata(L,1))
+		{
+			std::cerr << "error: first argument to createCubeShape should be world";
+			return 0;
+		}
+		//expect userdata = sLuaDemo->m_dynamicsWorld
+		halfExtents = btVector3(lua_tonumber(L,2),lua_tonumber(L,3),lua_tonumber(L,4));
+		btCollisionShape* colShape = new btBoxShape(halfExtents);
+
+		CustomShapeData* shapeData = new CustomShapeData();
+		shapeData->m_shapeIndex = sLuaDemo->m_glApp->registerCubeShape();
+		shapeData->m_localScaling = halfExtents;
+
+		colShape->setUserPointer(shapeData);
+		lua_pushlightuserdata (L, colShape);
+		return 1;
+	} else
+	{
+		std::cerr << "Error: invalid number of arguments to createCubeShape, expected 4 (world,halfExtentsX,halfExtentsY,halfExtentsX) but got " << argc;
+	}
+	return 0;
+}
+
+static int gCreateSphereShape(lua_State *L)
+{
+	int argc = lua_gettop(L);
+	if (argc==2)
+	{
+		btVector3 halfExtents(1,1,1);
+		if (!lua_isuserdata(L,1))
+		{
+			std::cerr << "error: first argument to createSphereShape should be world";
+			return 0;
+		}
+		//expect userdata = sLuaDemo->m_dynamicsWorld
+		btScalar radius = lua_tonumber(L,2);
+		btCollisionShape* colShape = new btSphereShape(radius);
+
+		CustomShapeData* shapeData = new CustomShapeData();
+		shapeData->m_shapeIndex = sLuaDemo->m_glApp->registerGraphicsSphereShape(radius,false,100,0.5);
+		shapeData->m_localScaling = halfExtents;
+
+		colShape->setUserPointer(shapeData);
+		lua_pushlightuserdata (L, colShape);
+		return 1;
+	} else
+	{
+		std::cerr << "Error: invalid number of arguments to createSphereShape, expected 2 (world,radius) but got " << argc;
+	}
+	return 0;
+}
+
+int luaL_returnlen(lua_State* L, int index)
+{
+  lua_len(L, index);
+  int len = lua_tointeger(L,-1);
+  lua_pop(L, 1);
+  return len;
+}
+
+btVector3 getLuaVectorArg(lua_State* L, int index)
+{
+	btVector3 pos(0,0,0);
+
+	int sz = luaL_returnlen(L, index);  // get size of table
+	{
+		lua_rawgeti(L, index, 1);  // push t[i]
+		pos[0] = lua_tonumber(L,-1);
+		lua_pop(L, 1);
+		lua_rawgeti(L, index, 2);  // push t[i]
+		pos[1] = lua_tonumber(L,-1);
+		lua_pop(L, 1);
+		lua_rawgeti(L, index, 3);  // push t[i]
+		pos[2] = lua_tonumber(L,-1);
+		lua_pop(L, 1);
+	}
+	return pos;
+}
+
+btQuaternion getLuaQuaternionArg(lua_State* L, int index)
+{
+	btQuaternion orn(0,0,0,1);
+
+	int sz = luaL_returnlen(L, index);  // get size of table
+	{
+		lua_rawgeti(L, index, 1);  // push t[i]
+		orn[0] = lua_tonumber(L,-1);
+		lua_pop(L, 1);
+		lua_rawgeti(L, index, 2);  // push t[i]
+		orn[1] = lua_tonumber(L,-1);
+		lua_pop(L, 1);
+		lua_rawgeti(L, index, 3);  // push t[i]
+		orn[2] = lua_tonumber(L,-1);
+		lua_pop(L, 1);
+		lua_rawgeti(L, index, 4);  // push t[i]
+		orn[3] = lua_tonumber(L,-1);
+		lua_pop(L, 1);
+	}
+	return orn;
+}
+
+
+
+
+static int gCreateRigidBody (lua_State *L)
+{
+	int argc = lua_gettop(L);
+	if (argc==5)
+	{
+
+		btTransform startTransform;
+		startTransform.setIdentity();
+
+
+		if (!lua_isuserdata(L,1))
+		{
+			std::cerr << "error: first argument to b3CreateRigidbody should be world";
+			return 0;
+		}
+		btDiscreteDynamicsWorld* world = (btDiscreteDynamicsWorld*) lua_touserdata(L,1);
+		if (world != sLuaDemo->m_dynamicsWorld)
+		{
+			std::cerr << "error: first argument expected to be a world";
+			return 0;
+		}
+
+		if (!lua_isuserdata(L,2))
+		{
+			std::cerr << "error: second argument to b3CreateRigidbody should be world";
+			return 0;
+		}
+
+		btScalar mass = lua_tonumber(L,3);
+
+		luaL_checktype(L,4, LUA_TTABLE);
+
+		btVector3 pos = getLuaVectorArg(L,4);
+
+		btQuaternion orn = getLuaQuaternionArg(L,5);
+
+		btCollisionShape* colShape = (btCollisionShape* )lua_touserdata(L,2);
+		//expect userdata = sLuaDemo->m_dynamicsWorld
+
+		btVector3 inertia(0,0,0);
+		if (mass)
+		{
+			colShape->calculateLocalInertia(mass,inertia);
+		}
+
+
+
+		btRigidBody* body = new btRigidBody(mass,0,colShape,inertia);
+		body->getWorldTransform().setOrigin(pos);
+		body->getWorldTransform().setRotation(orn);
+
+
+		CustomShapeData* shapeData = (CustomShapeData*)colShape->getUserPointer();
+		if (shapeData)
+		{
+			CustomRigidBodyData* rbd = new CustomRigidBodyData;
+			static int curColor = 0;
+			btVector4 color = colors[curColor];
+			curColor++;
+			curColor&=3;
+
+			CustomShapeData* shapeData = (CustomShapeData*)body->getCollisionShape()->getUserPointer();
+			if (shapeData)
+			{
+
+				rbd ->m_graphicsInstanceIndex = sLuaDemo->m_glApp->m_instancingRenderer->registerGraphicsInstance(shapeData->m_shapeIndex,startTransform.getOrigin(),startTransform.getRotation(),color,shapeData->m_localScaling);
+				body->setUserIndex(rbd->m_graphicsInstanceIndex);
+			}
+		}
+
+		world->addRigidBody(body);
+		lua_pushlightuserdata (L, body);
+		return 1;
+	} else
+	{
+		std::cerr << "Error: invalid number of arguments to createRigidBody, expected 5 (world,shape,mass,pos,orn) but got " << argc;
+	}
+	return 0;
+}
+
+static int gSetBodyPosition(lua_State *L)
+{
+	int argc = lua_gettop(L);
+	if (argc==3)
+	{
+		if (!lua_isuserdata(L,1))
+		{
+			std::cerr << "error: first argument needs to be a world";
+			return 0;
+		}
+		if (!lua_isuserdata(L,2))
+		{
+			std::cerr << "error: second argument needs to be a body";
+			return 0;
+		}
+		btRigidBody* body = (btRigidBody*)lua_touserdata(L,2);
+		btVector3 pos = getLuaVectorArg(L,3);
+
+		btTransform& tr = body ->getWorldTransform();
+		tr.setOrigin(pos);
+		body->setWorldTransform(tr);
+	} else
+	{
+		std::cerr << "error: setBodyPosition expects 6 arguments like setBodyPosition(world,body,0,1,0)";
+	}
+	return 0;
+}
+
+static int gSetBodyOrientation(lua_State *L)
+{
+	int argc = lua_gettop(L);
+	if (argc==3)
+	{
+		if (!lua_isuserdata(L,1))
+		{
+			std::cerr << "error: first argument needs to be a world";
+			return 0;
+		}
+		if (!lua_isuserdata(L,2))
+		{
+			std::cerr << "error: second argument needs to be a body";
+			return 0;
+		}
+		btRigidBody* body = (btRigidBody*)lua_touserdata(L,2);
+		btQuaternion orn = getLuaQuaternionArg(L,3);
+		btTransform& tr = body ->getWorldTransform();
+		tr.setRotation(orn);
+		body->setWorldTransform(tr);
+	} else
+	{
+		std::cerr << "error: setBodyOrientation expects 3 arguments like setBodyOrientation(world,body,orn)";
+	}
+	return 0;
+}
+
+//b3CreateConvexShape(world, points)
+
+//b3CreateHingeConstraint(world,bodyA,bodyB,...)
+
+static void report_errors(lua_State *L, int status)
+{
+  if ( status!=0 ) {
+    std::cerr << "-- " << lua_tostring(L, -1) << std::endl;
+    lua_pop(L, 1); // remove error message
+  }
+}
+
+
+
+void LuaPhysicsSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
+{
+	const char* prefix[]={"./","./data/","../data/","../../data/","../../../data/","../../../../data/"};
+	int numPrefixes = sizeof(prefix)/sizeof(const char*);
+	char relativeFileName[1024];
+	FILE* f=0;
+	int result = 0;
+
+	for (int i=0;!f && i<numPrefixes;i++)
+	{
+		sprintf(relativeFileName,"%s%s",prefix[i],sLuaFileName);
+		f = fopen(relativeFileName,"rb");
+	}
+	if (f)
+	{
+		fclose(f);
+
+		lua_State *L = luaL_newstate();
+
+		luaopen_io(L); // provides io.*
+		luaopen_base(L);
+		luaopen_table(L);
+		luaopen_string(L);
+		luaopen_math(L);
+		//luaopen_package(L);
+		luaL_openlibs(L);
+
+		 // make my_function() available to Lua programs
+		lua_register(L, "createDefaultDynamicsWorld", gCreateDefaultDynamicsWorld);
+		lua_register(L, "deleteDynamicsWorld", gDeleteDynamicsWorld);
+		lua_register(L, "createCubeShape", gCreateCubeShape);
+		lua_register(L, "createSphereShape", gCreateSphereShape);
+
+		lua_register(L, "createRigidBody", gCreateRigidBody);
+		lua_register(L, "setBodyPosition", gSetBodyPosition);
+		lua_register(L, "setBodyOrientation", gSetBodyOrientation);
+
+
+
+		int s = luaL_loadfile(L, relativeFileName);
+
+		if ( s==0 ) {
+		  // execute Lua program
+		  s = lua_pcall(L, 0, LUA_MULTRET, 0);
+		}
+
+		report_errors(L, s);
+		lua_close(L);
+	} else
+	{
+		b3Error("Cannot find Lua file%s\n",sLuaFileName);
+	}
+
+	m_glApp->m_instancingRenderer->writeTransforms();
+}
+
+
+void LuaPhysicsSetup::exitPhysics()
+{
+    delete m_dynamicsWorld;
+    m_dynamicsWorld=0;
+    delete m_dispatcher;
+    m_dispatcher=0;
+    delete m_bp;
+    m_bp=0;
+    delete m_config;
+    m_config=0;
+}
+
+void LuaPhysicsSetup::stepSimulation(float deltaTime)
+{
+ if (m_dynamicsWorld)
+    m_dynamicsWorld->stepSimulation(deltaTime);
+
+}
+
+void    LuaPhysicsSetup::debugDraw()
+{
+    if (m_dynamicsWorld)
+        m_dynamicsWorld->debugDrawWorld();
+
+}
+
+bool LuaPhysicsSetup::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
+{
+//btAssert(0);
+return false;
+}
+bool LuaPhysicsSetup::movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
+{
+//btAssert(0);
+return false;
+
+}
+void LuaPhysicsSetup::removePickingConstraint()
+{
+//btAssert(0);
+
+}
+
+void LuaPhysicsSetup::syncPhysicsToGraphics(GraphicsPhysicsBridge& gfxBridge)
+{
+    int numCollisionObjects = m_dynamicsWorld->getNumCollisionObjects();
+    for (int i = 0; i<numCollisionObjects; i++)
+    {
+        btCollisionObject* colObj = m_dynamicsWorld->getCollisionObjectArray()[i];
+        btVector3 pos = colObj->getWorldTransform().getOrigin();
+        btQuaternion orn = colObj->getWorldTransform().getRotation();
+        int index = colObj->getUserIndex();
+        if (index >= 0)
+        {
+            m_glApp->m_instancingRenderer->writeSingleInstanceTransformToCPU(pos, orn, index);
+        }
+    }
+    m_glApp->m_instancingRenderer->writeTransforms();
+}
+
+btRigidBody*	LuaPhysicsSetup::createRigidBody(float mass, const btTransform& startTransform,btCollisionShape* shape, const btVector4& color)
+{
+btAssert(0);
+return 0;
+
+}
+
+btBoxShape* LuaPhysicsSetup::createBoxShape(const btVector3& halfExtents)
+{
+btAssert(0);
+return 0;
+}

Demos3/bullet2/LuaDemo/LuaPhysicsSetup.h

@@ -0,0 +1,43 @@
+#ifndef _LUA_PHYSICS_SETUP_H
+#define _LUA_PHYSICS_SETUP_H
+
+#include "../Demos/CommonPhysicsSetup.h"
+
+//we don't derive from CommonRigidBodySetup because we
+//create and own our own dynamics world (one or more)
+//at run-time
+struct LuaPhysicsSetup : public CommonPhysicsSetup
+{
+
+    LuaPhysicsSetup(class SimpleOpenGL3App* app);
+    virtual ~LuaPhysicsSetup();
+
+	class btDefaultCollisionConfiguration* m_config;
+	class btCollisionDispatcher* m_dispatcher;
+	class btDbvtBroadphase* m_bp;
+	class btNNCGConstraintSolver* m_solver;
+	class btDiscreteDynamicsWorld* m_dynamicsWorld;
+    class SimpleOpenGL3App* m_glApp;
+
+	virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge);
+
+	virtual void exitPhysics();
+
+	virtual void stepSimulation(float deltaTime);
+
+    virtual void    debugDraw();
+
+	virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
+	virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
+	virtual void removePickingConstraint();
+
+	virtual void syncPhysicsToGraphics(GraphicsPhysicsBridge& gfxBridge);
+
+	virtual btRigidBody*	createRigidBody(float mass, const btTransform& startTransform,btCollisionShape* shape, const btVector4& color=btVector4(1,0,0,1));
+
+	virtual btBoxShape* createBoxShape(const btVector3& halfExtents);
+
+};
+
+
+#endif //_LUA_PHYSICS_SETUP_H

btgui/Gwen/Texture.h

@@ -22,6 +22,8 @@ namespace Gwen
 	{
 		TextObject	name;
 		void*	data;
+		int m_intData;
+		
 		bool	failed;
 		int		width;
 		int		height;
@@ -29,6 +31,7 @@ namespace Gwen
 		Texture()
 		{
 			data = NULL;
+			m_intData = 0;
 			width = 4;
 			height = 4;
 			failed = false;

btgui/GwenOpenGLTest/UnitTest.cpp

@@ -20,6 +20,7 @@ GWEN_CONTROL_CONSTRUCTOR( UnitTest )
 
 	SetSize( 600, 450 );
 
+
 	m_TabControl = new Controls::TabControl( this );
 	m_TabControl->Dock( Pos::Fill );
 	m_TabControl->SetMargin( Margin( 2, 2, 2, 2 ) );
@@ -29,6 +30,8 @@ GWEN_CONTROL_CONSTRUCTOR( UnitTest )
 	m_TextOutput->SetHeight( 100 );
 
 
+	ADD_UNIT_TEST( ImagePanel );
+
 	//ADD_UNIT_TEST( MenuStrip );
 
 	Gwen::UnicodeString str1(L"testje");
@@ -121,7 +124,6 @@ GWEN_CONTROL_CONSTRUCTOR( UnitTest )
 	ADD_UNIT_TEST( Slider );
 	ADD_UNIT_TEST( ProgressBar );
 	ADD_UNIT_TEST( RadioButton2 );
-	ADD_UNIT_TEST( ImagePanel );
 	
 	ADD_UNIT_TEST( Label );
 	ADD_UNIT_TEST( Checkbox );

btgui/OpenGLWindow/GwenOpenGL3CoreRenderer.h

@@ -7,9 +7,26 @@
 #include "GLPrimitiveRenderer.h"
 struct sth_stash;
 #include "../OpenGLTrueTypeFont/fontstash.h"
+#include "Gwen/Texture.h"
+
 #include "TwFonts.h"
 static float extraSpacing = 0.;//6f;
 #include <assert.h>
+#include <math.h>
+
+template <class T>
+inline void MyClamp(T& a, const T& lb, const T& ub) 
+{
+	if (a < lb) 
+	{
+		a = lb; 
+	}
+	else if (ub < a) 
+	{
+		a = ub;
+	}
+}
+
 
 static GLuint BindFont(const CTexFont *_Font)
 {
@@ -32,7 +49,14 @@ static GLuint BindFont(const CTexFont *_Font)
     return TexID;
 }
 
-
+struct MyTextureLoader
+{
+	virtual ~MyTextureLoader()
+	{
+	}
+	virtual void LoadTexture( Gwen::Texture* pTexture ) = 0;
+	virtual void FreeTexture( Gwen::Texture* pTexture )=0;
+};
 
 class GwenOpenGL3CoreRenderer : public Gwen::Renderer::Base
 {
@@ -48,6 +72,7 @@ class GwenOpenGL3CoreRenderer : public Gwen::Renderer::Base
 	const CTexFont* m_currentFont;
 	
 	GLuint	m_fontTextureId;
+	MyTextureLoader* m_textureLoader;
 public:
 	GwenOpenGL3CoreRenderer (GLPrimitiveRenderer* primRender, sth_stash* font,float screenWidth, float screenHeight, float retinaScale)
 		:m_primitiveRenderer(primRender),
@@ -55,7 +80,8 @@ public:
     m_screenWidth(screenWidth),
     m_screenHeight(screenHeight),
     m_retinaScale(retinaScale),
-	m_useTrueTypeFont(false)
+	m_useTrueTypeFont(false),
+	m_textureLoader(0)
 	{
 		///only enable true type fonts on Macbook Retina, it looks gorgeous
 		if (retinaScale==2.0f)
@@ -315,7 +341,76 @@ public:
 		return Gwen::Renderer::Base::MeasureText(pFont,text);
     }
 
+	void setTextureLoader(MyTextureLoader* loader)
+	{
+		m_textureLoader = loader;
+	}
 	
+	virtual void LoadTexture( Gwen::Texture* pTexture )
+	{
+		if (m_textureLoader)
+			m_textureLoader->LoadTexture(pTexture);
+	}
+	virtual void FreeTexture( Gwen::Texture* pTexture )
+	{
+		if (m_textureLoader)
+			m_textureLoader->FreeTexture(pTexture);
+
+	}
+	
+	
+	virtual void DrawTexturedRect( Gwen::Texture* pTexture, Gwen::Rect rect, float u1=0.0f, float v1=0.0f, float u2=1.0f, float v2=1.0f )
+	{
+
+		Translate( rect );
+
+		//float eraseColor[4] = {0,0,0,0};
+		//m_primitiveRenderer->drawRect(rect.x, rect.y+m_yOffset, rect.x+rect.w, rect.y+rect.h+m_yOffset, eraseColor);
+
+		GLint texHandle = (GLint) pTexture->m_intData;
+		//if (!texHandle)
+		//	return;
+		
+	glActiveTexture(GL_TEXTURE0);
+	glBindTexture(GL_TEXTURE_2D,texHandle);
+//		glDisable(GL_DEPTH_TEST);
+
+		GLint err;
+		
+		err = glGetError();
+		assert(err==GL_NO_ERROR);
+		
+	
+/*	bool useFiltering = true;
+	if (useFiltering)
+	{
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+	} else
+	{
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+	}
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
+*/
+	
+	//glEnable(GL_TEXTURE_2D);
+	
+		
+		
+//	glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE );
+	static float add=0.0;
+	//add+=1./512.;//0.01;
+	float color[4]={1,1,1,1};
+
+	m_primitiveRenderer->drawTexturedRect(rect.x, rect.y+m_yOffset, rect.x+rect.w, rect.y+rect.h+m_yOffset, color,0+add,0,1+add,1,true);
+
+		
+		err = glGetError();
+		assert(err==GL_NO_ERROR);
+		
+		
+	}
 			
 };
 #endif //__GWEN_OPENGL3_CORE_RENDERER_H

build3/findOpenGLGlewGlut.lua

@@ -14,6 +14,7 @@
 		configuration {"MacOSX"}
  			links { "OpenGL.framework"} 
 		configuration {"not Windows", "not MacOSX"}
+		if os.is("Linux") then	
 			if not  _OPTIONS["force_dlopen_opengl"] and (os.isdir("/usr/include") and os.isfile("/usr/include/GL/gl.h")) then
 				links {"GL"}
 			else
@@ -21,6 +22,7 @@
 				defines {"GLEW_INIT_OPENGL11_FUNCTIONS=1"}
 				links {"dl"}
 			end
+		end
 		configuration{}
 	end
 

src/BulletCollision/CollisionDispatch/btCollisionObject.cpp

@@ -35,6 +35,7 @@ btCollisionObject::btCollisionObject()
 		m_restitution(btScalar(0.)),
 		m_internalType(CO_COLLISION_OBJECT),
 		m_userObjectPointer(0),
+        m_userIndex(-1),
 		m_hitFraction(btScalar(1.)),
 		m_ccdSweptSphereRadius(btScalar(0.)),
 		m_ccdMotionThreshold(btScalar(0.)),

src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -92,11 +92,10 @@ protected:
 	int				m_internalType;
 
 	///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
-	union
+
-	{
     void*			m_userObjectPointer;
+    
     int	m_userIndex;
-	};
 
 	///time of impact calculation
 	btScalar		m_hitFraction; 

src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp

@@ -232,6 +232,8 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
 		m_compoundShapeRevision = compoundShape->getUpdateRevision();
 	}
 
+    if (m_childCollisionAlgorithms.size()==0)
+        return;
     
 	const btDbvt* tree = compoundShape->getDynamicAabbTree();
 	//use a dynamic aabb tree to cull potential child-overlaps

src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h

@@ -0,0 +1,654 @@
+/*
+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.
+*/
+
+/*
+2014 May: btGeneric6DofSpring2Constraint is created from the original (2.82.2712) btGeneric6DofConstraint by Gabor Puhr and Tamas Umenhoffer
+Pros:
+- Much more accurate and stable in a lot of situation. (Especially when a sleeping chain of RBs connected with 6dof2 is pulled)
+- Stable and accurate spring with minimal energy loss that works with all of the solvers. (latter is not true for the original 6dof spring)
+- Servo motor functionality
+- Much more accurate bouncing. 0 really means zero bouncing (not true for the original 6odf) and there is only a minimal energy loss when the value is 1 (because of the solvers' precision)
+- Rotation order for the Euler system can be set. (One axis' freedom is still limited to pi/2)
+
+Cons:
+- It is slower than the original 6dof. There is no exact ratio, but half speed is a good estimation.
+- At bouncing the correct velocity is calculated, but not the correct position. (it is because of the solver can correct position or velocity, but not both.)
+*/
+
+/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev
+/// Added support for generic constraint solver through getInfo1/getInfo2 methods
+
+/*
+2007-09-09
+btGeneric6DofConstraint Refactored by Francisco Le?n
+email: projectileman@yahoo.com
+http://gimpact.sf.net
+*/
+
+
+#ifndef BT_GENERIC_6DOF_CONSTRAINT2_H
+#define BT_GENERIC_6DOF_CONSTRAINT2_H
+
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+
+class btRigidBody;
+
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btGeneric6DofSpring2ConstraintData2		btGeneric6DofSpring2ConstraintDoubleData2
+#define btGeneric6DofSpring2ConstraintDataName	"btGeneric6DofSpring2ConstraintDoubleData2"
+#else
+#define btGeneric6DofSpring2ConstraintData2		btGeneric6DofSpring2ConstraintData
+#define btGeneric6DofSpring2ConstraintDataName	"btGeneric6DofSpring2ConstraintData"
+#endif //BT_USE_DOUBLE_PRECISION
+
+enum RotateOrder
+{
+	RO_XYZ,
+	RO_XZY,
+	RO_YXZ,
+	RO_YZX,
+	RO_ZXY,
+	RO_ZYX
+};
+
+class btRotationalLimitMotor2
+{
+public:
+// upper < lower means free
+// upper == lower means locked
+// upper > lower means limited
+	btScalar m_loLimit;
+	btScalar m_hiLimit;
+	btScalar m_bounce;
+	btScalar m_stopERP;
+	btScalar m_stopCFM;
+	btScalar m_motorERP;
+	btScalar m_motorCFM;
+	bool     m_enableMotor;
+	btScalar m_targetVelocity;
+	btScalar m_maxMotorForce;
+	bool     m_servoMotor;
+	btScalar m_servoTarget;
+	bool     m_enableSpring;
+	btScalar m_springStiffness;
+	btScalar m_springDamping;
+	btScalar m_equilibriumPoint;
+
+	btScalar m_currentLimitError;
+	btScalar m_currentLimitErrorHi;
+	btScalar m_currentPosition;
+	int      m_currentLimit;
+
+	btRotationalLimitMotor2()
+	{
+		m_loLimit             = 1.0f;
+		m_hiLimit             = -1.0f;
+		m_bounce              = 0.0f;
+		m_stopERP             = 0.2f;
+		m_stopCFM             = 0.f;
+		m_motorERP            = 0.9f;
+		m_motorCFM            = 0.f;
+		m_enableMotor         = false;
+		m_targetVelocity      = 0;
+		m_maxMotorForce       = 0.1f;
+		m_servoMotor          = false;
+		m_servoTarget         = 0;
+		m_enableSpring        = false;
+		m_springStiffness     = 0;
+		m_springDamping       = 0;
+		m_equilibriumPoint    = 0;
+
+		m_currentLimitError   = 0;
+		m_currentLimitErrorHi = 0;
+		m_currentPosition     = 0;
+		m_currentLimit        = 0;
+	}
+
+	btRotationalLimitMotor2(const btRotationalLimitMotor2 & limot)
+	{
+		m_loLimit             = limot.m_loLimit;
+		m_hiLimit             = limot.m_hiLimit;
+		m_bounce              = limot.m_bounce;
+		m_stopERP             = limot.m_stopERP;
+		m_stopCFM             = limot.m_stopCFM;
+		m_motorERP            = limot.m_motorERP;
+		m_motorCFM            = limot.m_motorCFM;
+		m_enableMotor         = limot.m_enableMotor;
+		m_targetVelocity      = limot.m_targetVelocity;
+		m_maxMotorForce       = limot.m_maxMotorForce;
+		m_servoMotor          = limot.m_servoMotor;
+		m_servoTarget         = limot.m_servoTarget;
+		m_enableSpring        = limot.m_enableSpring;
+		m_springStiffness     = limot.m_springStiffness;
+		m_springDamping       = limot.m_springDamping;
+		m_equilibriumPoint    = limot.m_equilibriumPoint;
+
+		m_currentLimitError   = limot.m_currentLimitError;
+		m_currentLimitErrorHi = limot.m_currentLimitErrorHi;
+		m_currentPosition     = limot.m_currentPosition;
+		m_currentLimit        = limot.m_currentLimit;
+	}
+
+
+	bool isLimited()
+	{
+		if(m_loLimit > m_hiLimit) return false;
+		return true;
+	}
+
+	void testLimitValue(btScalar test_value);
+};
+
+
+
+class btTranslationalLimitMotor2
+{
+public:
+// upper < lower means free
+// upper == lower means locked
+// upper > lower means limited
+	btVector3 m_lowerLimit;
+	btVector3 m_upperLimit;
+	btVector3 m_bounce;
+	btVector3 m_stopERP;
+	btVector3 m_stopCFM;
+	btVector3 m_motorERP;
+	btVector3 m_motorCFM;
+	bool      m_enableMotor[3];
+	bool      m_servoMotor[3];
+	bool      m_enableSpring[3];
+	btVector3 m_servoTarget;
+	btVector3 m_springStiffness;
+	btVector3 m_springDamping;
+	btVector3 m_equilibriumPoint;
+	btVector3 m_targetVelocity;
+	btVector3 m_maxMotorForce;
+
+	btVector3 m_currentLimitError;
+	btVector3 m_currentLimitErrorHi;
+	btVector3 m_currentLinearDiff;
+	int       m_currentLimit[3];
+
+	btTranslationalLimitMotor2()
+	{
+		m_lowerLimit         .setValue(0.f , 0.f , 0.f );
+		m_upperLimit         .setValue(0.f , 0.f , 0.f );
+		m_bounce             .setValue(0.f , 0.f , 0.f );
+		m_stopERP            .setValue(0.2f, 0.2f, 0.2f);
+		m_stopCFM            .setValue(0.f , 0.f , 0.f );
+		m_motorERP           .setValue(0.9f, 0.9f, 0.9f);
+		m_motorCFM           .setValue(0.f , 0.f , 0.f );
+
+		m_currentLimitError  .setValue(0.f , 0.f , 0.f );
+		m_currentLimitErrorHi.setValue(0.f , 0.f , 0.f );
+		m_currentLinearDiff  .setValue(0.f , 0.f , 0.f );
+
+		for(int i=0; i < 3; i++) 
+		{
+			m_enableMotor[i]      = false;
+			m_servoMotor[i]       = false;
+			m_enableSpring[i]     = false;
+			m_servoTarget[i]      = btScalar(0.f);
+			m_springStiffness[i]  = btScalar(0.f);
+			m_springDamping[i]    = btScalar(0.f);
+			m_equilibriumPoint[i] = btScalar(0.f);
+			m_targetVelocity[i]   = btScalar(0.f);
+			m_maxMotorForce[i]    = btScalar(0.f);
+			
+			m_currentLimit[i]     = 0;
+		}
+	}
+
+	btTranslationalLimitMotor2(const btTranslationalLimitMotor2 & other )
+	{
+		m_lowerLimit          = other.m_lowerLimit;
+		m_upperLimit          = other.m_upperLimit;
+		m_bounce              = other.m_bounce;
+		m_stopERP             = other.m_stopERP;
+		m_stopCFM             = other.m_stopCFM;
+		m_motorERP            = other.m_motorERP;
+		m_motorCFM            = other.m_motorCFM;
+		
+		m_currentLimitError   = other.m_currentLimitError;
+		m_currentLimitErrorHi = other.m_currentLimitErrorHi;
+		m_currentLinearDiff   = other.m_currentLinearDiff;
+
+		for(int i=0; i < 3; i++) 
+		{
+			m_enableMotor[i]      = other.m_enableMotor[i];
+			m_servoMotor[i]       = other.m_servoMotor[i];
+			m_enableSpring[i]     = other.m_enableSpring[i];
+			m_servoTarget[i]      = other.m_servoTarget[i];
+			m_springStiffness[i]  = other.m_springStiffness[i];
+			m_springDamping[i]    = other.m_springDamping[i];
+			m_equilibriumPoint[i] = other.m_equilibriumPoint[i];
+			m_targetVelocity[i]   = other.m_targetVelocity[i];
+			m_maxMotorForce[i]    = other.m_maxMotorForce[i];
+
+			m_currentLimit[i]     = other.m_currentLimit[i];
+		}
+	}
+
+	inline bool isLimited(int limitIndex)
+	{
+		return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
+	}
+
+	void testLimitValue(int limitIndex, btScalar test_value);
+};
+
+enum bt6DofFlags2
+{
+	BT_6DOF_FLAGS_CFM_STOP2 = 1,
+	BT_6DOF_FLAGS_ERP_STOP2 = 2,
+	BT_6DOF_FLAGS_CFM_MOTO2 = 4,
+	BT_6DOF_FLAGS_ERP_MOTO2 = 8
+};
+#define BT_6DOF_FLAGS_AXIS_SHIFT2 4 // bits per axis
+
+
+ATTRIBUTE_ALIGNED16(class) btGeneric6DofSpring2Constraint : public btTypedConstraint
+{
+protected:
+
+	btTransform m_frameInA;
+	btTransform m_frameInB;
+
+	btJacobianEntry m_jacLinear[3];
+	btJacobianEntry m_jacAng[3];
+
+	btTranslationalLimitMotor2 m_linearLimits;
+	btRotationalLimitMotor2 m_angularLimits[3];
+
+	RotateOrder m_rotateOrder;
+
+protected:
+
+	btTransform  m_calculatedTransformA;
+	btTransform  m_calculatedTransformB;
+	btVector3    m_calculatedAxisAngleDiff;
+	btVector3    m_calculatedAxis[3];
+	btVector3    m_calculatedLinearDiff;
+	btScalar     m_factA;
+	btScalar     m_factB;
+	bool         m_hasStaticBody;
+	int          m_flags;
+
+	btGeneric6DofSpring2Constraint&	operator=(btGeneric6DofSpring2Constraint&)
+	{
+		btAssert(0);
+		return *this;
+	}
+
+	int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
+	int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
+
+	void calculateLinearInfo();
+	void calculateAngleInfo();
+	void testAngularLimitMotor(int axis_index);
+
+	void calculateJacobi(btRotationalLimitMotor2* limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2* info, int srow, btVector3& ax1, int rotational, int rotAllowed);
+	int get_limit_motor_info2(btRotationalLimitMotor2* limot,
+		const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
+		btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
+
+	static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
+	static bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
+	static bool matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz);
+	static bool matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz);
+	static bool matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz);
+	static bool matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz);
+	static bool matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz);
+
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+    btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ);
+    btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ);
+
+	virtual void buildJacobian() {}
+	virtual void getInfo1 (btConstraintInfo1* info);
+	virtual void getInfo2 (btConstraintInfo2* info);
+	virtual int calculateSerializeBufferSize() const;
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
+
+	btRotationalLimitMotor2* getRotationalLimitMotor(int index) { return &m_angularLimits[index]; }
+	btTranslationalLimitMotor2* getTranslationalLimitMotor() { return &m_linearLimits; }
+
+	// Calculates the global transform for the joint offset for body A an B, and also calculates the angle differences between the bodies.
+	void calculateTransforms(const btTransform& transA,const btTransform& transB);
+	void calculateTransforms();
+
+	// Gets the global transform of the offset for body A
+	const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; }
+	// Gets the global transform of the offset for body B
+	const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; }
+
+	const btTransform & getFrameOffsetA() const { return m_frameInA; }
+	const btTransform & getFrameOffsetB() const { return m_frameInB; }
+
+	btTransform & getFrameOffsetA() { return m_frameInA; }
+	btTransform & getFrameOffsetB() { return m_frameInB; }
+
+	// Get the rotation axis in global coordinates ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
+	btVector3 getAxis(int axis_index) const { return m_calculatedAxis[axis_index]; }
+
+	// Get the relative Euler angle ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
+	btScalar getAngle(int axis_index) const { return m_calculatedAxisAngleDiff[axis_index]; }
+
+	// Get the relative position of the constraint pivot ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
+	btScalar getRelativePivotPosition(int axis_index) const { return m_calculatedLinearDiff[axis_index]; }
+
+	void setFrames(const btTransform & frameA, const btTransform & frameB);
+
+	void setLinearLowerLimit(const btVector3& linearLower) { m_linearLimits.m_lowerLimit = linearLower; }
+	void getLinearLowerLimit(btVector3& linearLower) { linearLower = m_linearLimits.m_lowerLimit; }
+	void setLinearUpperLimit(const btVector3& linearUpper) { m_linearLimits.m_upperLimit = linearUpper; }
+	void getLinearUpperLimit(btVector3& linearUpper) { linearUpper = m_linearLimits.m_upperLimit; }
+
+	void setAngularLowerLimit(const btVector3& angularLower)
+	{
+		for(int i = 0; i < 3; i++) 
+			m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
+	}
+
+	void setAngularLowerLimitReversed(const btVector3& angularLower)
+	{
+		for(int i = 0; i < 3; i++) 
+			m_angularLimits[i].m_hiLimit = btNormalizeAngle(-angularLower[i]);
+	}
+
+	void getAngularLowerLimit(btVector3& angularLower)
+	{
+		for(int i = 0; i < 3; i++) 
+			angularLower[i] = m_angularLimits[i].m_loLimit;
+	}
+
+	void getAngularLowerLimitReversed(btVector3& angularLower)
+	{
+		for(int i = 0; i < 3; i++)
+			angularLower[i] = -m_angularLimits[i].m_hiLimit;
+	}
+
+	void setAngularUpperLimit(const btVector3& angularUpper)
+	{
+		for(int i = 0; i < 3; i++)
+			m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
+	}
+
+	void setAngularUpperLimitReversed(const btVector3& angularUpper)
+	{
+		for(int i = 0; i < 3; i++)
+			m_angularLimits[i].m_loLimit = btNormalizeAngle(-angularUpper[i]);
+	}
+
+	void getAngularUpperLimit(btVector3& angularUpper)
+	{
+		for(int i = 0; i < 3; i++)
+			angularUpper[i] = m_angularLimits[i].m_hiLimit;
+	}
+
+	void getAngularUpperLimitReversed(btVector3& angularUpper)
+	{
+		for(int i = 0; i < 3; i++)
+			angularUpper[i] = -m_angularLimits[i].m_loLimit;
+	}
+
+	//first 3 are linear, next 3 are angular
+
+	void setLimit(int axis, btScalar lo, btScalar hi)
+	{
+		if(axis<3)
+		{
+			m_linearLimits.m_lowerLimit[axis] = lo;
+			m_linearLimits.m_upperLimit[axis] = hi;
+		}
+		else
+		{
+			lo = btNormalizeAngle(lo);
+			hi = btNormalizeAngle(hi);
+			m_angularLimits[axis-3].m_loLimit = lo;
+			m_angularLimits[axis-3].m_hiLimit = hi;
+		}
+	}
+
+	void setLimitReversed(int axis, btScalar lo, btScalar hi)
+	{
+		if(axis<3)
+		{
+			m_linearLimits.m_lowerLimit[axis] = lo;
+			m_linearLimits.m_upperLimit[axis] = hi;
+		}
+		else
+		{
+			lo = btNormalizeAngle(lo);
+			hi = btNormalizeAngle(hi);
+			m_angularLimits[axis-3].m_hiLimit = -lo;
+			m_angularLimits[axis-3].m_loLimit = -hi;
+		}
+	}
+
+	bool isLimited(int limitIndex)
+	{
+		if(limitIndex<3)
+		{
+			return m_linearLimits.isLimited(limitIndex);
+		}
+		return m_angularLimits[limitIndex-3].isLimited();
+	}
+
+	void setRotationOrder(RotateOrder order) { m_rotateOrder = order; }
+	RotateOrder getRotationOrder() { return m_rotateOrder; }
+
+	void setAxis( const btVector3& axis1, const btVector3& axis2);
+
+	void setBounce(int index, btScalar bounce);
+
+	void enableMotor(int index, bool onOff);
+	void setServo(int index, bool onOff); // set the type of the motor (servo or not) (the motor has to be turned on for servo also)
+	void setTargetVelocity(int index, btScalar velocity);
+	void setServoTarget(int index, btScalar target);
+	void setMaxMotorForce(int index, btScalar force);
+
+	void enableSpring(int index, bool onOff);
+	void setStiffness(int index, btScalar stiffness);
+	void setDamping(int index, btScalar damping);
+	void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
+	void setEquilibriumPoint(int index);  // set the current constraint position/orientation as an equilibrium point for given DOF
+	void setEquilibriumPoint(int index, btScalar val);
+
+	//override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	//If no axis is provided, it uses the default axis for this constraint.
+	virtual void setParam(int num, btScalar value, int axis = -1);
+	virtual btScalar getParam(int num, int axis = -1) const;
+};
+
+
+struct btGeneric6DofSpring2ConstraintData
+{
+	btTypedConstraintData m_typeConstraintData;
+	btTransformFloatData m_rbAFrame;
+	btTransformFloatData m_rbBFrame;
+
+	btVector3FloatData m_linearUpperLimit;
+	btVector3FloatData m_linearLowerLimit;
+	btVector3FloatData m_linearBounce;
+	btVector3FloatData m_linearStopERP;
+	btVector3FloatData m_linearStopCFM;
+	btVector3FloatData m_linearMotorERP;
+	btVector3FloatData m_linearMotorCFM;
+	btVector3FloatData m_linearTargetVelocity;
+	btVector3FloatData m_linearMaxMotorForce;
+	btVector3FloatData m_linearServoTarget;
+	btVector3FloatData m_linearSpringStiffness;
+	btVector3FloatData m_linearSpringDamping;
+	btVector3FloatData m_linearEquilibriumPoint;
+	char               m_linearEnableMotor[4];
+	char               m_linearServoMotor[4];
+	char               m_linearEnableSpring[4];
+	char               m_padding1[4];
+
+	btVector3FloatData m_angularUpperLimit;
+	btVector3FloatData m_angularLowerLimit;
+	btVector3FloatData m_angularBounce;
+	btVector3FloatData m_angularStopERP;
+	btVector3FloatData m_angularStopCFM;
+	btVector3FloatData m_angularMotorERP;
+	btVector3FloatData m_angularMotorCFM;
+	btVector3FloatData m_angularTargetVelocity;
+	btVector3FloatData m_angularMaxMotorForce;
+	btVector3FloatData m_angularServoTarget;
+	btVector3FloatData m_angularSpringStiffness;
+	btVector3FloatData m_angularSpringDamping;
+	btVector3FloatData m_angularEquilibriumPoint;
+	char               m_angularEnableMotor[4];
+	char               m_angularServoMotor[4];
+	char               m_angularEnableSpring[4];
+	char               m_padding2[4];
+
+	int                m_rotateOrder;
+	char               m_padding3[4];
+};
+
+struct btGeneric6DofSpring2ConstraintDoubleData2
+{
+	btTypedConstraintDoubleData m_typeConstraintData;
+	btTransformDoubleData m_rbAFrame;
+	btTransformDoubleData m_rbBFrame;
+
+	btVector3DoubleData m_linearUpperLimit;
+	btVector3DoubleData m_linearLowerLimit;
+	btVector3DoubleData m_linearBounce;
+	btVector3DoubleData m_linearStopERP;
+	btVector3DoubleData m_linearStopCFM;
+	btVector3DoubleData m_linearMotorERP;
+	btVector3DoubleData m_linearMotorCFM;
+	btVector3DoubleData m_linearTargetVelocity;
+	btVector3DoubleData m_linearMaxMotorForce;
+	btVector3DoubleData m_linearServoTarget;
+	btVector3DoubleData m_linearSpringStiffness;
+	btVector3DoubleData m_linearSpringDamping;
+	btVector3DoubleData m_linearEquilibriumPoint;
+	char                m_linearEnableMotor[4];
+	char                m_linearServoMotor[4];
+	char                m_linearEnableSpring[4];
+	char                m_padding1[4];
+
+	btVector3DoubleData m_angularUpperLimit;
+	btVector3DoubleData m_angularLowerLimit;
+	btVector3DoubleData m_angularBounce;
+	btVector3DoubleData m_angularStopERP;
+	btVector3DoubleData m_angularStopCFM;
+	btVector3DoubleData m_angularMotorERP;
+	btVector3DoubleData m_angularMotorCFM;
+	btVector3DoubleData m_angularTargetVelocity;
+	btVector3DoubleData m_angularMaxMotorForce;
+	btVector3DoubleData m_angularServoTarget;
+	btVector3DoubleData m_angularSpringStiffness;
+	btVector3DoubleData m_angularSpringDamping;
+	btVector3DoubleData m_angularEquilibriumPoint;
+	char                m_angularEnableMotor[4];
+	char                m_angularServoMotor[4];
+	char                m_angularEnableSpring[4];
+	char                m_padding2[4];
+
+	int                 m_rotateOrder;
+	char                m_padding3[4];
+};
+
+SIMD_FORCE_INLINE int btGeneric6DofSpring2Constraint::calculateSerializeBufferSize() const
+{
+	return sizeof(btGeneric6DofSpring2ConstraintData2);
+}
+
+SIMD_FORCE_INLINE const char* btGeneric6DofSpring2Constraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+	btGeneric6DofSpring2ConstraintData2* dof = (btGeneric6DofSpring2ConstraintData2*)dataBuffer;
+	btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
+
+	m_frameInA.serialize(dof->m_rbAFrame);
+	m_frameInB.serialize(dof->m_rbBFrame);
+
+	int i;
+	for (i=0;i<3;i++)
+	{
+		dof->m_angularLowerLimit.m_floats[i]       = m_angularLimits[i].m_loLimit;
+		dof->m_angularUpperLimit.m_floats[i]       = m_angularLimits[i].m_hiLimit;
+		dof->m_angularBounce.m_floats[i]           = m_angularLimits[i].m_bounce;
+		dof->m_angularStopERP.m_floats[i]          = m_angularLimits[i].m_stopERP;
+		dof->m_angularStopCFM.m_floats[i]          = m_angularLimits[i].m_stopCFM;
+		dof->m_angularMotorERP.m_floats[i]         = m_angularLimits[i].m_motorERP;
+		dof->m_angularMotorCFM.m_floats[i]         = m_angularLimits[i].m_motorCFM;
+		dof->m_angularTargetVelocity.m_floats[i]   = m_angularLimits[i].m_targetVelocity;
+		dof->m_angularMaxMotorForce.m_floats[i]    = m_angularLimits[i].m_maxMotorForce;
+		dof->m_angularServoTarget.m_floats[i]      = m_angularLimits[i].m_servoTarget;
+		dof->m_angularSpringStiffness.m_floats[i]  = m_angularLimits[i].m_springStiffness;
+		dof->m_angularSpringDamping.m_floats[i]    = m_angularLimits[i].m_springDamping;
+		dof->m_angularEquilibriumPoint.m_floats[i] = m_angularLimits[i].m_equilibriumPoint;
+	}
+	dof->m_angularLowerLimit.m_floats[3]       = 0;
+	dof->m_angularUpperLimit.m_floats[3]       = 0;
+	dof->m_angularBounce.m_floats[3]           = 0;
+	dof->m_angularStopERP.m_floats[3]          = 0;
+	dof->m_angularStopCFM.m_floats[3]          = 0;
+	dof->m_angularMotorERP.m_floats[3]         = 0;
+	dof->m_angularMotorCFM.m_floats[3]         = 0;
+	dof->m_angularTargetVelocity.m_floats[3]   = 0;
+	dof->m_angularMaxMotorForce.m_floats[3]    = 0;
+	dof->m_angularServoTarget.m_floats[3]      = 0;
+	dof->m_angularSpringStiffness.m_floats[3]  = 0;
+	dof->m_angularSpringDamping.m_floats[3]    = 0;
+	dof->m_angularEquilibriumPoint.m_floats[3] = 0;
+	for (i=0;i<4;i++)
+	{
+		dof->m_angularEnableMotor[i]  = i < 3 ? ( m_angularLimits[i].m_enableMotor ? 1 : 0 ) : 0;
+		dof->m_angularServoMotor[i]   = i < 3 ? ( m_angularLimits[i].m_servoMotor ? 1 : 0 ) : 0;
+		dof->m_angularEnableSpring[i] = i < 3 ? ( m_angularLimits[i].m_enableSpring ? 1 : 0 ) : 0;
+	}
+
+	m_linearLimits.m_lowerLimit.serialize( dof->m_linearLowerLimit );
+	m_linearLimits.m_upperLimit.serialize( dof->m_linearUpperLimit );
+	m_linearLimits.m_bounce.serialize( dof->m_linearBounce );
+	m_linearLimits.m_stopERP.serialize( dof->m_linearStopERP );
+	m_linearLimits.m_stopCFM.serialize( dof->m_linearStopCFM );
+	m_linearLimits.m_motorERP.serialize( dof->m_linearMotorERP );
+	m_linearLimits.m_motorCFM.serialize( dof->m_linearMotorCFM );
+	m_linearLimits.m_targetVelocity.serialize( dof->m_linearTargetVelocity );
+	m_linearLimits.m_maxMotorForce.serialize( dof->m_linearMaxMotorForce );
+	m_linearLimits.m_servoTarget.serialize( dof->m_linearServoTarget );
+	m_linearLimits.m_springStiffness.serialize( dof->m_linearSpringStiffness );
+	m_linearLimits.m_springDamping.serialize( dof->m_linearSpringDamping );
+	m_linearLimits.m_equilibriumPoint.serialize( dof->m_linearEquilibriumPoint );
+	for (i=0;i<4;i++)
+	{
+		dof->m_linearEnableMotor[i]  = i < 3 ? ( m_linearLimits.m_enableMotor[i] ? 1 : 0 ) : 0;
+		dof->m_linearServoMotor[i]   = i < 3 ? ( m_linearLimits.m_servoMotor[i] ? 1 : 0 ) : 0;
+		dof->m_linearEnableSpring[i] = i < 3 ? ( m_linearLimits.m_enableSpring[i] ? 1 : 0 ) : 0;
+	}
+
+	dof->m_rotateOrder = m_rotateOrder;
+
+	return btGeneric6DofSpring2ConstraintDataName;
+}
+
+
+
+
+
+#endif //BT_GENERIC_6DOF_CONSTRAINT_H

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -317,6 +317,9 @@ void	btDiscreteDynamicsWorld::debugDrawWorld()
 			}
 		}
 	}
+    if (getDebugDrawer())
+        getDebugDrawer()->flushLines();
+
 }
 
 void	btDiscreteDynamicsWorld::clearForces()

src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h

@@ -41,6 +41,10 @@ public:
 		btMultiBodyJacobianData& data,
 		const btContactSolverInfo& infoGlobal);
 	
+    virtual void setVelocityTarget(btScalar velTarget)
+    {
+        m_desiredVelocity = velTarget;
+    }
 	
 };
 

src/LinearMath/btIDebugDraw.h

@@ -438,6 +438,11 @@ class	btIDebugDraw
 		drawLine(transform*pt0,transform*pt1,color);
 		drawLine(transform*pt2,transform*pt3,color);
 	}
+    
+    virtual void flushLines()
+    {
+        
+    }
 };