Merge pull request #218 from erwincoumans/master

remove a lot of warnings (more todo in demos and serialization code)

Demos3/AllBullet2Demos/BulletDemoEntries.h

@@ -126,9 +126,13 @@ static int loadCurrentDemoEntry(const char* startFileName)
 	{
 		int result;
 		result = fscanf(f,"%d",&currentEntry);
+		if (result)
+		{
+			return currentEntry;
+		}
 		fclose(f);
 	}
-	return currentEntry;
+	return 0;
 };
 
 #endif//BULLET_DEMO_ENTRIES_H

Demos3/AllBullet2Demos/GraphingTexture.cpp

@@ -14,7 +14,7 @@ GraphingTexture::~GraphingTexture()
 	destroy();
 }
 
-bool GraphingTexture::destroy()
+void GraphingTexture::destroy()
 {
 	//TODO(erwincoumans) release memory etc...
 	m_width = 0;
@@ -58,21 +58,18 @@ bool GraphingTexture::create(int texWidth, int texHeight)
 	glGenTextures(1,(GLuint*)&m_textureId);
 	
 	uploadImageData();
+	return true;
 }
 
 void GraphingTexture::uploadImageData()
 {
 	glBindTexture(GL_TEXTURE_2D,m_textureId);
-	GLint err = glGetError();
-	assert(err==GL_NO_ERROR);
+	assert(glGetError()==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);
+	assert(glGetError()==GL_NO_ERROR);
 	
 	
 }

Demos3/AllBullet2Demos/GraphingTexture.h

@@ -14,7 +14,7 @@ struct GraphingTexture
 	virtual ~GraphingTexture();
 	
 	bool create(int texWidth, int texHeight);
-	bool destroy();
+	void  destroy();
 	
 	void setPixel(int x, int y, unsigned char red, unsigned char green, unsigned char blue, unsigned char alpha)
 	{

Demos3/AllBullet2Demos/GwenParameterInterface.cpp

@@ -124,7 +124,7 @@ void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params)
 
 	pSlider->onValueChanged.Add( handler, &MySliderEventHandler<btScalar>::SliderMoved );
 	handler->SliderMoved(pSlider);
-	float v = pSlider->GetValue();
+//	float v = pSlider->GetValue();
 	m_gwenInternalData->m_curYposition+=22;
 }
 

Demos3/AllBullet2Demos/GwenProfileWindow.cpp

@@ -27,7 +27,7 @@ protected:
 	
 	void SliderMoved(Gwen::Controls::Base* pControl )
 	{
-		Gwen::Controls::Slider* pSlider = (Gwen::Controls::Slider*)pControl;
+	//	Gwen::Controls::Slider* pSlider = (Gwen::Controls::Slider*)pControl;
 		//this->m_app->scaleYoungModulus(pSlider->GetValue());
 		//	printf("Slider Value: %.2f", pSlider->GetValue() );
 	}
@@ -35,8 +35,8 @@ protected:
 	
 	void	OnCheckChangedStiffnessWarping (Gwen::Controls::Base* pControl)
 	{
-		Gwen::Controls::CheckBox* labeled = (Gwen::Controls::CheckBox* )pControl;
-		bool checked = labeled->IsChecked();
+	//	Gwen::Controls::CheckBox* labeled = (Gwen::Controls::CheckBox* )pControl;
+//		bool checked = labeled->IsChecked();
 		//m_app->m_stiffness_warp_on  = checked;
 	}
 public:
@@ -148,13 +148,13 @@ public:
 	void	UpdateText(CProfileIterator*  profileIterator, bool idle)
 	{
 		
-		static bool update=true;
+	//	static bool update=true;
 		
         m_ctrl->SetBounds(0,0,this->GetInnerBounds().w,this->GetInnerBounds().h);
 		
         //		if (!update)
         //			return;
-		update=false;
+	//	update=false;
 		
 		
 		static int test = 1;
@@ -170,18 +170,18 @@ public:
 		{
             //recompute profiling data, and store profile strings
 			
-            char blockTime[128];
+         //   char blockTime[128];
 			
-            double totalTime = 0;
+           // double totalTime = 0;
 			
-            int frames_since_reset = CProfileManager::Get_Frame_Count_Since_Reset();
+          //  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;
+         //   Gwen::Controls::TreeNode* curParent = m_node;
 			
             double accumulated_time = dumpRecursive(profileIterator,m_node);
 			

Demos3/AllBullet2Demos/main.cpp

@@ -238,13 +238,13 @@ struct MyMenuItemHander :public Gwen::Event::Handler
 
 	void onButtonA(Gwen::Controls::Base* pControl)
 	{
-		const Gwen::String& name = pControl->GetName();
+		//const Gwen::String& name = pControl->GetName();
 		Gwen::Controls::TreeNode* node = (Gwen::Controls::TreeNode*)pControl;
-		Gwen::Controls::Label* l = node->GetButton();
+	//	Gwen::Controls::Label* l = node->GetButton();
 
 		Gwen::UnicodeString la = node->GetButton()->GetText();// node->GetButton()->GetName();// GetText();
 		Gwen::String laa = Gwen::Utility::UnicodeToString(la);
-		const char* ha = laa.c_str();
+	//	const char* ha = laa.c_str();
 
 		//printf("selected %s\n", ha);
 		//int dep = but->IsDepressed();
@@ -257,7 +257,7 @@ struct MyMenuItemHander :public Gwen::Event::Handler
 		Gwen::Controls::Label* label = (Gwen::Controls::Label*) pControl;
 		Gwen::UnicodeString la = label->GetText();// node->GetButton()->GetName();// GetText();
 		Gwen::String laa = Gwen::Utility::UnicodeToString(la);
-		const char* ha = laa.c_str();
+		//const char* ha = laa.c_str();
 
 
 		selectDemo(sCurrentHightlighted);
@@ -265,10 +265,10 @@ struct MyMenuItemHander :public Gwen::Event::Handler
 	}
 	void onButtonC(Gwen::Controls::Base* pControl)
 	{
-		Gwen::Controls::Label* label = (Gwen::Controls::Label*) pControl;
-		Gwen::UnicodeString la = label->GetText();// node->GetButton()->GetName();// GetText();
-		Gwen::String laa = Gwen::Utility::UnicodeToString(la);
-		const char* ha = laa.c_str();
+//		Gwen::Controls::Label* label = (Gwen::Controls::Label*) pControl;
+	//	Gwen::UnicodeString la = label->GetText();// node->GetButton()->GetName();// GetText();
+	//	Gwen::String laa = Gwen::Utility::UnicodeToString(la);
+	//	const char* ha = laa.c_str();
 
 
 //		printf("onButtonC ! %s\n", ha);
@@ -334,7 +334,7 @@ int main(int argc, char* argv[])
 
 	b3Clock clock;
 
-	float dt = 1./120.f;
+	//float dt = 1./120.f;
 	int width = 1024;
 	int height=768;
 
@@ -348,8 +348,7 @@ int main(int argc, char* argv[])
 	app->m_window->setKeyboardCallback(MyKeyboardCallback);
 
 
-	GLint err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
 
 	sth_stash* fontstash=app->getFontStash();
 	gui = new GwenUserInterface;
@@ -376,7 +375,8 @@ int main(int argc, char* argv[])
 		gt->create(256,256);
 		int texId = gt->getTextureId();
 		myTexLoader->m_hashMap.insert("graph1", texId);
-		MyGraphWindow* gw = setupTextureWindow(input);
+		//MyGraphWindow* gw = 
+		setupTextureWindow(input);
 	}
 	if (1)
 	{
@@ -403,7 +403,8 @@ int main(int argc, char* argv[])
 		int texId = gt->getTextureId();
 		input.m_xPos = width-input.m_width;
 		myTexLoader->m_hashMap.insert("graph2", texId);
-		MyGraphWindow* gw = setupTextureWindow(input);
+		//MyGraphWindow* gw = 
+		setupTextureWindow(input);
 	}
 	//destroyTextureWindow(gw);
 	
@@ -414,8 +415,8 @@ int main(int argc, char* argv[])
 
 	int numDemos = sizeof(allDemos)/sizeof(BulletDemoEntry);
 
-	char nodeText[1024];
-	int curDemo = 0;
+	//char nodeText[1024];
+	//int curDemo = 0;
 	int selectedDemo = loadCurrentDemoEntry(startFileName);
 	Gwen::Controls::TreeNode* curNode = tree;
 	MyMenuItemHander* handler2 = new MyMenuItemHander(-1);
@@ -476,8 +477,7 @@ int main(int argc, char* argv[])
 	do
 	{
 
-		GLint err = glGetError();
-		assert(err==GL_NO_ERROR);
+		assert(glGetError()==GL_NO_ERROR);
 		app->m_instancingRenderer->init();
         DrawGridData dg;
         dg.upAxis = app->getUpAxis();

Demos3/GpuDemos/gwenUserInterface.cpp

@@ -120,7 +120,7 @@ struct MyButtonHander   :public Gwen::Event::Handler
 	void onButtonA( Gwen::Controls::Base* pControl )
 	{
 		Gwen::Controls::Button* but = (Gwen::Controls::Button*) pControl;
-		int dep = but->IsDepressed();
+//		int dep = but->IsDepressed();
 		int tog = but->GetToggleState();
 		if (m_data->m_toggleButtonCallback)
 			(*m_data->m_toggleButtonCallback)(m_buttonId,tog);

Demos3/ImportObjDemo/ImportObjSetup.cpp

@@ -28,7 +28,7 @@ static GLInstanceGraphicsShape* gCreateGraphicsShapeFromWavefrontObj(std::vector
 		//	int numIndices = 0;
 		b3AlignedObjectArray<int>* indicesPtr = new b3AlignedObjectArray<int>;
 		
-		for (int s=0;s<shapes.size();s++)
+		for (int s=0;s<(int)shapes.size();s++)
 		{
 			tinyobj::shape_t& shape = shapes[s];
 			int faceCount = shape.mesh.indices.size();
@@ -151,7 +151,7 @@ void ImportObjDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 	
 	btVector3 shift(0,0,0);
 	btVector3 scaling(1,1,1);
-	int index=10;
+//	int index=10;
 	
 	{
 		
@@ -172,7 +172,8 @@ void ImportObjDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 		int shapeId = m_app->m_instancingRenderer->registerShape(&gfxShape->m_vertices->at(0).xyzw[0], gfxShape->m_numvertices, &gfxShape->m_indices->at(0), gfxShape->m_numIndices);
 		
 		
-		int id = m_app->m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling);
+		//int id = 
+		m_app->m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling);
 
 		/*
 

Demos3/ImportSTLDemo/ImportSTLSetup.cpp

@@ -143,7 +143,7 @@ void ImportSTLDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 	
 	btVector3 shift(0,0,0);
 	btVector3 scaling(10,10,10);
-	int index=10;
+//	int index=10;
 	
 	{
 		
@@ -161,7 +161,8 @@ void ImportSTLDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 		int shapeId = m_app->m_instancingRenderer->registerShape(&gfxShape->m_vertices->at(0).xyzw[0], gfxShape->m_numvertices, &gfxShape->m_indices->at(0), gfxShape->m_numIndices);
 		
 		
-		int id = m_app->m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling);
+	//	int id = 
+		m_app->m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling);
 
 		/*
 

Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp

@@ -169,7 +169,7 @@ bool	Bullet2MultiBodyDemo::mouseMoveCallback(float x,float y)
 			//keep it at the same picking distance
 			btVector3 newRayTo = getRayTo(x,y);
 			btVector3 rayFrom;
-			btVector3 oldPivotInB = pickCon->getPivotInB();
+//			btVector3 oldPivotInB = pickCon->getPivotInB();
 			btVector3 newPivotB;
 			m_glApp->m_instancingRenderer->getCameraPosition(rayFrom);
 			btVector3 dir = newRayTo-rayFrom;
@@ -186,7 +186,7 @@ bool	Bullet2MultiBodyDemo::mouseMoveCallback(float x,float y)
 
 		btVector3 newRayTo = getRayTo(x,y);
 		btVector3 rayFrom;
-		btVector3 oldPivotInB = m_pickingMultiBodyPoint2Point->getPivotInB();
+	//	btVector3 oldPivotInB = m_pickingMultiBodyPoint2Point->getPivotInB();
 		btVector3 newPivotB;
 		btVector3 camPos;
 		m_glApp->m_instancingRenderer->getCameraPosition(camPos);
@@ -438,7 +438,7 @@ btMultiBody* FeatherstoneDemo1::createFeatherstoneMultiBody(class btMultiBodyDyn
 		btVector4 halfExtents(7.5,0.45,4.5,1);
     	{
 			
-			float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
+		//	float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
 			float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
 
 			
@@ -484,7 +484,7 @@ btMultiBody* FeatherstoneDemo1::createFeatherstoneMultiBody(class btMultiBodyDyn
 		{
 		
 			btVector3 posr = local_origin[i+1];
-			float pos[4]={posr.x(),posr.y(),posr.z(),1};
+			//float pos[4]={posr.x(),posr.y(),posr.z(),1};
 			
 			float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
 
@@ -531,8 +531,8 @@ void FeatherstoneDemo1::createGround()
 {
 	//create ground
 	int cubeShapeId = m_glApp->registerCubeShape();
-	float pos[]={0,0,0};
-	float orn[]={0,0,0,1};
+	//float pos[]={0,0,0};
+	//float orn[]={0,0,0,1};
 		
 
 	{
@@ -575,7 +575,8 @@ void	FeatherstoneDemo1::initPhysics()
 	settings.m_isFixedBase = false;
 	settings.m_basePosition.setValue(0,10,0);
 	settings.m_numLinks = 10;
-	btMultiBody* mb = createFeatherstoneMultiBody(m_dynamicsWorld,settings);
+	//btMultiBody* mb = 
+	createFeatherstoneMultiBody(m_dynamicsWorld,settings);
 
 
 	m_glApp->m_instancingRenderer->writeTransforms();
@@ -727,7 +728,7 @@ class RagDoll2
 		hull->buildHull(0.01);
 		
 		{
-			int strideInBytes = 9*sizeof(float);
+		//	int strideInBytes = 9*sizeof(float);
 			int numVertices = hull->numVertices();
 			int numIndices =hull->numIndices();
 			
@@ -988,7 +989,7 @@ public:
 	
 	virtual	~RagDoll2 ()
 	{
-		int i;
+		//int i;
 		/*
 		// Remove all constraints
 		for ( i = 0; i < JOINT_COUNT; ++i)
@@ -1027,8 +1028,8 @@ void	FeatherstoneDemo2::initPhysics()
 	settings.m_usePrismatic = true;
 	btMultiBody* mb = createFeatherstoneMultiBody(m_dynamicsWorld,settings);
 */
-	btVector3 offset(0,2,0);
-	RagDoll2* doll = new RagDoll2(m_dynamicsWorld,offset,m_glApp);
+//	btVector3 offset(0,2,0);
+	//RagDoll2* doll = new RagDoll2(m_dynamicsWorld,offset,m_glApp);
 
 	
 	m_glApp->m_instancingRenderer->writeTransforms();

Demos3/bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.cpp

@@ -307,7 +307,7 @@ void MultiDofDemo::addColliders_testMultiDof(btMultiBody *pMultiBody, btMultiBod
 	
 	{
 			
-		float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
+	//	float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
 		float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
 
 			
@@ -348,7 +348,7 @@ void MultiDofDemo::addColliders_testMultiDof(btMultiBody *pMultiBody, btMultiBod
 	{
 		
 		btVector3 posr = local_origin[i+1];
-		float pos[4]={posr.x(),posr.y(),posr.z(),1};
+	//	float pos[4]={posr.x(),posr.y(),posr.z(),1};
 			
 		float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
 

btgui/Gwen/Controls/CrossSplitter.cpp

@@ -112,7 +112,7 @@ void CrossSplitter::Layout( Skin::Base* /*skin*/ )
 	else
 	{
 		//This should probably use Fill docking instead
-		m_Sections[m_iZoomedSection]->SetBounds( 0, 0, Width(), Height() );
+		m_Sections[(int)m_iZoomedSection]->SetBounds( 0, 0, Width(), Height() );
 	}
 }
 

btgui/Gwen/Controls/Resizer.cpp

@@ -23,7 +23,7 @@ void Resizer::OnMouseMoved( int x, int y, int /*deltaX*/, int /*deltaY*/ )
 	if ( !m_pTarget ) return;
 	if ( !m_bDepressed ) return;
 
-	Gwen::Rect oldBounds = m_pTarget->GetBounds();
+//	Gwen::Rect oldBounds = m_pTarget->GetBounds();
 	Gwen::Rect pBounds = m_pTarget->GetBounds();
 
 	Gwen::Point pntMin = m_pTarget->GetMinimumSize();

btgui/Gwen/Controls/TreeControl.cpp

@@ -101,12 +101,12 @@ bool TreeControl::OnKeyUp( bool bDown )
 	if (bDown)
 	{
 		ForceUpdateScrollBars();
-		int maxIndex = 0;
+//		int maxIndex = 0;
 		int newIndex = 0;
 		int maxItem=0;
 		int curItem=-1;
 		iterate(ITERATE_ACTION_FIND_SELECTED_INDEX,&maxItem,&curItem);
-		maxIndex = maxItem;
+	//	maxIndex = maxItem;
 		int targetItem = curItem;
 		if (curItem>0)
 		{
@@ -121,15 +121,15 @@ bool TreeControl::OnKeyUp( bool bDown )
 				iterate(ITERATE_ACTION_DESELECT_INDEX,&maxItem,&deselectIndex);
 			}
 			curItem = newIndex;
-			float amount = float(newIndex)/float(maxIndex);
+		//	float amount = float(newIndex)/float(maxIndex);
 			float viewSize = m_ScrollControl->m_VerticalScrollBar->getViewableContentSize();
 			float contSize = m_ScrollControl->m_VerticalScrollBar->getContentSize();
 		
 			float curAmount = m_ScrollControl->m_VerticalScrollBar->GetScrolledAmount();
-			float minCoordViewableWindow = curAmount*contSize;
-			float maxCoordViewableWindow = minCoordViewableWindow+viewSize;
+		//	float minCoordViewableWindow = curAmount*contSize;
+			//float maxCoordViewableWindow = minCoordViewableWindow+viewSize;
 			float minCoordSelectedItem = curItem*16.f;
-			float maxCoordSelectedItem = (curItem+1)*16.f;
+	//		float maxCoordSelectedItem = (curItem+1)*16.f;
 			{
 				float newAmount = float(minCoordSelectedItem)/(contSize-viewSize);
 				if (newAmount<curAmount)
@@ -157,12 +157,12 @@ bool TreeControl::OnKeyDown( bool bDown )
 	if (bDown)
 	{
 		ForceUpdateScrollBars();
-		int maxIndex = 0;
+	//	int maxIndex = 0;
 		int newIndex = 0;
 		int maxItem=0;
 		int curItem=-1;
 		iterate(ITERATE_ACTION_FIND_SELECTED_INDEX,&maxItem,&curItem);
-		maxIndex = maxItem;
+	//	maxIndex = maxItem;
 		int targetItem = curItem;
 		if (curItem>=0)
 		{
@@ -177,15 +177,15 @@ bool TreeControl::OnKeyDown( bool bDown )
 				iterate(ITERATE_ACTION_DESELECT_INDEX,&maxItem,&deselectIndex);
 			}
 			curItem= newIndex;
-			float amount = (int)float(newIndex)/float(maxIndex);
+		//	float amount = (int)float(newIndex)/float(maxIndex);
 			float viewSize = m_ScrollControl->m_VerticalScrollBar->getViewableContentSize();
 			float contSize = m_ScrollControl->m_VerticalScrollBar->getContentSize();
 		
 			float curAmount = m_ScrollControl->m_VerticalScrollBar->GetScrolledAmount();
-			float minCoordViewableWindow = curAmount*contSize;
-			float maxCoordViewableWindow = minCoordViewableWindow+viewSize;
+		//	float minCoordViewableWindow = curAmount*contSize;
+			//float maxCoordViewableWindow = minCoordViewableWindow+viewSize;
 			float minCoordSelectedItem = curItem*16.f;
-			float maxCoordSelectedItem = (curItem+1)*16.f;
+			//float maxCoordSelectedItem = (curItem+1)*16.f;
 			{
 				float newAmount = float(minCoordSelectedItem)/(contSize-viewSize);
 				if (newAmount<curAmount)
@@ -219,15 +219,15 @@ bool TreeControl::OnKeyRight( bool bDown )
 		int maxItem=0;
 		int curItem=0;
 		iterate(ITERATE_ACTION_FIND_SELECTED_INDEX,&maxItem,&curItem);
-		float amount = float(curItem)/float(maxItem);
+	//	float amount = float(curItem)/float(maxItem);
 		float viewSize = m_ScrollControl->m_VerticalScrollBar->getViewableContentSize();
 		float contSize = m_ScrollControl->m_VerticalScrollBar->getContentSize();
 		
 		float curAmount = m_ScrollControl->m_VerticalScrollBar->GetScrolledAmount();
-		float minCoordViewableWindow = curAmount*contSize;
-		float maxCoordViewableWindow = minCoordViewableWindow+viewSize;
+	//	float minCoordViewableWindow = curAmount*contSize;
+//		float maxCoordViewableWindow = minCoordViewableWindow+viewSize;
 		float minCoordSelectedItem = curItem*16.f;
-		float maxCoordSelectedItem = (curItem+1)*16.f;
+	//	float maxCoordSelectedItem = (curItem+1)*16.f;
 		{
 			float newAmount = float(minCoordSelectedItem)/(contSize-viewSize);
 			if (newAmount<curAmount)
@@ -262,17 +262,17 @@ bool TreeControl::OnKeyLeft( bool bDown )
 		int maxItems=0;
 		int curItem=0;
 		iterate(ITERATE_ACTION_FIND_SELECTED_INDEX,&maxItems,&curItem);
-		float amount = float(curItem)/float(maxItems);
+	//	float amount = float(curItem)/float(maxItems);
 		
 	//	m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(amount,true);
 		float viewSize = m_ScrollControl->m_VerticalScrollBar->getViewableContentSize();
 		float contSize = m_ScrollControl->m_VerticalScrollBar->getContentSize();
 		
 		float curAmount = m_ScrollControl->m_VerticalScrollBar->GetScrolledAmount();
-		float minCoordViewableWindow = curAmount*contSize;
-		float maxCoordViewableWindow = minCoordViewableWindow+viewSize;
+	//	float minCoordViewableWindow = curAmount*contSize;
+	//	float maxCoordViewableWindow = minCoordViewableWindow+viewSize;
 		float minCoordSelectedItem = curItem*16.f;
-		float maxCoordSelectedItem = (curItem+1)*16.f;
+	//	float maxCoordSelectedItem = (curItem+1)*16.f;
 		
 		{
 			float newAmount = float(minCoordSelectedItem)/(contSize-viewSize);

btgui/Gwen/Controls/TreeNode.cpp

@@ -73,7 +73,7 @@ void TreeNode::Render( Skin::Base* skin )
 
 TreeNode* TreeNode::AddNode( const UnicodeString& strLabel )
 {
-	int sz = sizeof(TreeNode);
+//	int sz = sizeof(TreeNode);
 	TreeNode* node = new TreeNode( this );
 	node->SetText( strLabel );
 	node->Dock( Pos::Top );
@@ -231,7 +231,7 @@ void TreeNode::iterate(int action, int* curIndex, int* targetIndex)
 
 	Gwen::String name = Gwen::Utility::UnicodeToString(m_Title->GetText());
 	
-	int actualIndex = curIndex? *curIndex : -1;
+//	int actualIndex = curIndex? *curIndex : -1;
 	//printf("iterated over item %d with name = %s\n", actualIndex, name.c_str());
 
 	if (action==ITERATE_ACTION_SELECT)

btgui/Gwen/Renderers/OpenGL_DebugFont.cpp

@@ -164,7 +164,7 @@ namespace Gwen
 			glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
 			glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
 
-			GLenum format = GL_RGB;
+			//GLenum format = GL_RGB;
 			unsigned char* texdata = new unsigned char[256*256*4];
 			for (int i=0;i<256*256;i++)
 			{
@@ -316,11 +316,11 @@ namespace Gwen
 			Gwen::String converted_string = Gwen::Utility::UnicodeToString( text );
 
 			float yOffset=0.0f;
-			for ( int i=0; i<text.length(); i++ )
+			for ( int i=0; i<(int)text.length(); i++ )
 			{
-				wchar_t chr = text[i];
+			//	wchar_t chr = text[i];
 				char ch = converted_string[i];
-				float curSpacing = sGwenDebugFontSpacing[ch] * m_fLetterSpacing * fSize * m_fFontScale[0];
+				float curSpacing = sGwenDebugFontSpacing[(int)ch] * m_fLetterSpacing * fSize * m_fFontScale[0];
 				Gwen::Rect r( pos.x + yOffset, pos.y-fSize*0.2f, (fSize * m_fFontScale[0]), fSize * m_fFontScale[1] );
 
 				if ( m_pFontTexture )
@@ -390,10 +390,10 @@ namespace Gwen
 			Gwen::String converted_string = Gwen::Utility::UnicodeToString( text );
 			float spacing = 0.0f;
 
-			for ( int i=0; i<text.length(); i++ )
+			for ( int i=0; i<(int)text.length(); i++ )
 			{
 				char ch = converted_string[i];
-				spacing += sGwenDebugFontSpacing[ch];
+				spacing += sGwenDebugFontSpacing[(int)ch];
 			}
 
 			p.x = spacing*m_fLetterSpacing*fSize * m_fFontScale[0];

btgui/Gwen/Skins/Simple.h

@@ -78,7 +78,7 @@ namespace Gwen
 						m_Render->DrawLinedRect( control->GetRenderBounds() );
 					}
 
-					Gwen::Rect rect = control->GetRenderBounds();
+				//	Gwen::Rect rect = control->GetRenderBounds();
 					if ( bChecked )
 					{
 						m_Render->SetDrawColor( Color( 0, 0, 0, 255) );
@@ -490,7 +490,7 @@ namespace Gwen
 				virtual void DrawSlider( Gwen::Controls::Base* control, bool bIsHorizontal, int numNotches, int barSize)
 				{
 					Gwen::Rect rect = control->GetRenderBounds();
-					Gwen::Rect notchRect = rect;
+//					Gwen::Rect notchRect = rect;
 
 					if ( bIsHorizontal )
 					{

btgui/OpenGLTrueTypeFont/fontstash.cpp

@@ -219,7 +219,7 @@ int sth_add_font(struct sth_stash* stash, const char* path)
 	FILE* fp = 0;
 	int datasize;
 	unsigned char* data = NULL;
-	int idx;
+	int idx=0;
 
 	// Read in the font data.
 	fp = fopen(path, "rb");
@@ -231,10 +231,14 @@ int sth_add_font(struct sth_stash* stash, const char* path)
 	if (data == NULL) goto error;
 	int bytesRead;
 	bytesRead = fread(data, 1, datasize, fp);
+	if (bytesRead)
+	{
+		idx = sth_add_font_from_memory(stash, data);
+	}
 	fclose(fp);
 	fp = 0;
 
-	idx = sth_add_font_from_memory(stash, data);
+	
 	// Modify type of the loaded font.
 	if (idx)
 		stash->fonts->type = TTFONT_FILE;

btgui/OpenGLTrueTypeFont/opengl_fontstashcallbacks.cpp

@@ -37,8 +37,7 @@ InternalOpenGL2RenderCallbacks::~InternalOpenGL2RenderCallbacks()
 void InternalOpenGL2RenderCallbacks::display2() 
 {
     
-    GLint err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
    // glViewport(0,0,10,10);
     
 	//const float timeScale = 0.008f;
@@ -48,39 +47,31 @@ void InternalOpenGL2RenderCallbacks::display2()
     glBindBuffer(GL_ARRAY_BUFFER, s_vertexBuffer);
     glBindVertexArray(s_vertexArrayObject);
     
-    err = glGetError();
-    assert(err==GL_NO_ERROR);
-    
+      assert(glGetError()==GL_NO_ERROR);
+  
     
     //   glBindTexture(GL_TEXTURE_2D,m_texturehandle);
     
     
-    err = glGetError();
-    assert(err==GL_NO_ERROR);
-    
+       assert(glGetError()==GL_NO_ERROR);
+  
     vec2 p( 0.f,0.f);//?b?0.5f * sinf(timeValue), 0.5f * cosf(timeValue) );
     glUniform2fv(data->m_positionUniform, 1, (const GLfloat *)&p);
     
-    err = glGetError();
-    assert(err==GL_NO_ERROR);
-	err = glGetError();
-    assert(err==GL_NO_ERROR);
-    
+     assert(glGetError()==GL_NO_ERROR);
+   
     glEnableVertexAttribArray(data->m_positionAttribute);
-	err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
     
     glEnableVertexAttribArray(data->m_colourAttribute);
-	err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
     
 	glEnableVertexAttribArray(data->m_textureAttribute);
     
     glVertexAttribPointer(data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)0);
     glVertexAttribPointer(data->m_colourAttribute  , 4, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)sizeof(vec4));
     glVertexAttribPointer(data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)(sizeof(vec4)+sizeof(vec4)));
-	err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
 /*    
  
     glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_indexBuffer);
@@ -101,10 +92,7 @@ void InternalOpenGL2RenderCallbacks::display2()
 
 void InternalOpenGL2RenderCallbacks::updateTexture(sth_texture* texture, sth_glyph* glyph, int textureWidth, int textureHeight)
 {
-	GLint err;
-    
-    err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
 
 
 	if (glyph)
@@ -114,13 +102,11 @@ void InternalOpenGL2RenderCallbacks::updateTexture(sth_texture* texture, sth_gly
 		
 		glBindTexture(GL_TEXTURE_2D, *gltexture);
 		glPixelStorei(GL_UNPACK_ALIGNMENT,1);
-		err = glGetError();
-		assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
 
 		glTexImage2D(GL_TEXTURE_2D, 0, GL_RED, textureWidth, textureHeight, 0, GL_RED, GL_UNSIGNED_BYTE, texture->m_texels);
 	
-		GLenum err = glGetError();
-	    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
 
 	} else
 	{
@@ -132,8 +118,7 @@ void InternalOpenGL2RenderCallbacks::updateTexture(sth_texture* texture, sth_gly
 
 			//create new texture
 			glGenTextures(1, texId);
-			GLenum err = glGetError();
-			assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
 
 			
 	
@@ -143,8 +128,7 @@ void InternalOpenGL2RenderCallbacks::updateTexture(sth_texture* texture, sth_gly
 			glTexImage2D(GL_TEXTURE_2D, 0, GL_RED, textureWidth, textureHeight, 0, GL_RED, GL_UNSIGNED_BYTE, texture->m_texels);
 			glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
 			glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
-			err = glGetError();
-			assert(err==GL_NO_ERROR);
+	    assert(glGetError()==GL_NO_ERROR);
 
 			////////////////////////////
 			//create the other data
@@ -155,8 +139,7 @@ void InternalOpenGL2RenderCallbacks::updateTexture(sth_texture* texture, sth_gly
 				glGenBuffers(1, &s_vertexBuffer);
 				glBindBuffer(GL_ARRAY_BUFFER, s_vertexBuffer);
 				glBufferData(GL_ARRAY_BUFFER, VERT_COUNT * sizeof(Vertex), texture->newverts, GL_DYNAMIC_DRAW);
-				GLuint err = glGetError();
-				assert(err==GL_NO_ERROR);
+				assert(glGetError()==GL_NO_ERROR);
         
 				for (int i=0;i<INDEX_COUNT;i++)
 				{
@@ -167,9 +150,8 @@ void InternalOpenGL2RenderCallbacks::updateTexture(sth_texture* texture, sth_gly
 				glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, s_indexBuffer);
 				glBufferData(GL_ELEMENT_ARRAY_BUFFER,INDEX_COUNT*sizeof(int), s_indexData,GL_STATIC_DRAW);
         
-				err = glGetError();
-				assert(err==GL_NO_ERROR);
-			}
+		    assert(glGetError()==GL_NO_ERROR);
+	}
 		} else
 		{
 			//delete texture
@@ -196,13 +178,10 @@ void InternalOpenGL2RenderCallbacks::render(sth_texture* texture)
 
 	GLuint* texId = (GLuint*) texture->m_userData;
 
-    GLint err;
-	err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
             
     glActiveTexture(GL_TEXTURE0);
-    err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
 
 	glBindTexture(GL_TEXTURE_2D, *texId);
 	bool useFiltering = false;
@@ -215,25 +194,21 @@ void InternalOpenGL2RenderCallbacks::render(sth_texture* texture)
 		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
 		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
 	}
-	err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
      glBindBuffer(GL_ARRAY_BUFFER, s_vertexBuffer);
      glBindVertexArray(s_vertexArrayObject);
     glBufferData(GL_ARRAY_BUFFER, texture->nverts * sizeof(Vertex), &texture->newverts[0].position.p[0], GL_DYNAMIC_DRAW);
 
-    err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
             
     glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, s_indexBuffer);
             
     //glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
     int indexCount = texture->nverts;
-    err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
             
     glDrawElements(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, 0);
-    err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
 		
 	 glBindVertexArray(0);
 

btgui/OpenGLWindow/GLPrimitiveRenderer.cpp

@@ -131,9 +131,8 @@ void GLPrimitiveRenderer::loadBufferData()
     glGenBuffers(1, &m_data->m_vertexBuffer);
     glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vertexBuffer);
     glBufferData(GL_ARRAY_BUFFER, 4 * sizeof(Vertex), vertexData, GL_DYNAMIC_DRAW);
-    GLuint err;
-    err = glGetError();
-    assert(err==GL_NO_ERROR);
+    
+    assert(glGetError()==GL_NO_ERROR);
     
     
     
@@ -143,16 +142,14 @@ void GLPrimitiveRenderer::loadBufferData()
     
     glEnableVertexAttribArray(m_data->m_positionAttribute);
     glEnableVertexAttribArray(m_data->m_colourAttribute);
-	err = glGetError();
-    assert(err==GL_NO_ERROR);
+	assert(glGetError()==GL_NO_ERROR);
     
 	glEnableVertexAttribArray(m_data->m_textureAttribute);
     
     glVertexAttribPointer(m_data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)0);
     glVertexAttribPointer(m_data->m_colourAttribute  , 4, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)sizeof(vec4));
     glVertexAttribPointer(m_data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)(sizeof(vec4)+sizeof(vec4)));
-	err = glGetError();
-    assert(err==GL_NO_ERROR);
+	assert(glGetError()==GL_NO_ERROR);
     
     
     
@@ -192,8 +189,7 @@ void GLPrimitiveRenderer::loadBufferData()
     glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, 256,256,0,GL_RGB,GL_UNSIGNED_BYTE,image);
     glGenerateMipmap(GL_TEXTURE_2D);
     
-    err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
     
     delete[] image;
     
@@ -218,36 +214,25 @@ void GLPrimitiveRenderer::drawLine()
 
 void GLPrimitiveRenderer::drawRect(float x0, float y0, float x1, float y1, float color[4])
 {
-	GLint err;
-    
-    err = glGetError();
-    assert(err==GL_NO_ERROR);
+	assert(glGetError()==GL_NO_ERROR);
 	glActiveTexture(GL_TEXTURE0);
-	err = glGetError();
-    assert(err==GL_NO_ERROR);
+	assert(glGetError()==GL_NO_ERROR);
 	
 	 glBindTexture(GL_TEXTURE_2D,m_data->m_texturehandle);
-	err = glGetError();
-    assert(err==GL_NO_ERROR);
-
+	assert(glGetError()==GL_NO_ERROR);
 	drawTexturedRect(x0,y0,x1,y1,color,0,0,1,1);
-	err = glGetError();
-    assert(err==GL_NO_ERROR);
+	assert(glGetError()==GL_NO_ERROR);
 
 }
 
 void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA)
 {
-    GLint err;
-    
-    err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
    
     
     glUseProgram(m_data->m_shaderProg);
     
-    err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
     
     glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vertexBuffer);
     glBindVertexArray(m_data->m_vertexArrayObject);
@@ -277,8 +262,7 @@ void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y
     
     
     
-    err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
     
     vec2 p( 0.f,0.f);//?b?0.5f * sinf(timeValue), 0.5f * cosf(timeValue) );
 	if (useRGBA)
@@ -291,60 +275,47 @@ void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y
     
 	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
 
-    err = glGetError();
-    assert(err==GL_NO_ERROR);
-	err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
     
     glEnableVertexAttribArray(m_data->m_positionAttribute);
-	err = glGetError();
-    assert(err==GL_NO_ERROR);
+	assert(glGetError()==GL_NO_ERROR);
     
     glEnableVertexAttribArray(m_data->m_colourAttribute);
-	err = glGetError();
-    assert(err==GL_NO_ERROR);
+	assert(glGetError()==GL_NO_ERROR);
     
 	glEnableVertexAttribArray(m_data->m_textureAttribute);
     
     glVertexAttribPointer(m_data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)0);
     glVertexAttribPointer(m_data->m_colourAttribute  , 4, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)sizeof(vec4));
     glVertexAttribPointer(m_data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)(sizeof(vec4)+sizeof(vec4)));
-	err = glGetError();
-    assert(err==GL_NO_ERROR);
+	assert(glGetError()==GL_NO_ERROR);
     
     glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_data->m_indexBuffer);
     
     //glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
     int indexCount = 6;
-    err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
     
 	
     glDrawElements(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, 0);
-    err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
 	
 	
     glBindVertexArray(0);
-    err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
 
 	glBindBuffer(GL_ARRAY_BUFFER, 0);
-    err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
 
 	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
-    err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
 
 	//glDisableVertexAttribArray(m_data->m_textureAttribute);
-    err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
 
 	glUseProgram(0);
    
-    err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
 
 }
 

btgui/OpenGLWindow/GwenOpenGL3CoreRenderer.h

@@ -119,55 +119,37 @@ public:
 	{
 		m_yOffset=0;
 		glEnable(GL_BLEND);
-		GLint err = glGetError();
-		assert(err==GL_NO_ERROR);
+	  assert(glGetError()==GL_NO_ERROR);
+
 
 		glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
 		
-		err = glGetError();
-		assert(err==GL_NO_ERROR);
+		assert(glGetError()==GL_NO_ERROR);
 
-		err = glGetError();
-		assert(err==GL_NO_ERROR);
+		assert(glGetError()==GL_NO_ERROR);
         
 		glDisable(GL_DEPTH_TEST);
-		err = glGetError();
-		assert(err==GL_NO_ERROR);
-        
+		assert(glGetError()==GL_NO_ERROR);      
 		//glColor4ub(255,0,0,255);
 		
-		err = glGetError();
-		assert(err==GL_NO_ERROR);
-        
 		
-		err = glGetError();
-		assert(err==GL_NO_ERROR);
+		
 		glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
 
 	//	saveOpenGLState(width,height);//m_glutScreenWidth,m_glutScreenHeight);
 			
-		err = glGetError();
-		assert(err==GL_NO_ERROR);
-
+		assert(glGetError()==GL_NO_ERROR);
 			
-		err = glGetError();
-		assert(err==GL_NO_ERROR);
 
 		glDisable(GL_CULL_FACE);
 
 		glDisable(GL_DEPTH_TEST);
-		err = glGetError();
-		assert(err==GL_NO_ERROR);
+		assert(glGetError()==GL_NO_ERROR);
 
-		err = glGetError();
-		assert(err==GL_NO_ERROR);
-            
+		        
 		glEnable(GL_BLEND);
 
-            
-		err = glGetError();
-		assert(err==GL_NO_ERROR);
-            
+		assert(glGetError()==GL_NO_ERROR);        
 	}
 	virtual void End()
 	{
@@ -375,10 +357,7 @@ public:
 	glBindTexture(GL_TEXTURE_2D,texHandle);
 //		glDisable(GL_DEPTH_TEST);
 
-		GLint err;
-		
-		err = glGetError();
-		assert(err==GL_NO_ERROR);
+		assert(glGetError()==GL_NO_ERROR);
 		
 	
 /*	bool useFiltering = true;
@@ -406,8 +385,8 @@ public:
 	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);
+				assert(glGetError()==GL_NO_ERROR);
+
 		
 		
 	}

btgui/OpenGLWindow/LoadShader.cpp

@@ -18,9 +18,7 @@ void gltLoadShaderSrc(const char *szShaderSrc, GLuint shader)
 GLuint gltLoadShaderPair(const char *szVertexProg, const char *szFragmentProg)
 {
 
-    GLuint err;
-     err = glGetError();
-    assert(err==GL_NO_ERROR);
+  assert(glGetError()==GL_NO_ERROR);
 
 	// Temporary Shader objects
 	GLuint hVertexShader;
@@ -37,7 +35,7 @@ GLuint gltLoadShaderPair(const char *szVertexProg, const char *szFragmentProg)
 
 	// Compile them
 	glCompileShader(hVertexShader);
-    err = glGetError();
+  assert(glGetError()==GL_NO_ERROR);
 
 	glGetShaderiv(hVertexShader, GL_COMPILE_STATUS, &testVal);
 	if(testVal == GL_FALSE)
@@ -52,10 +50,10 @@ GLuint gltLoadShaderPair(const char *szVertexProg, const char *szFragmentProg)
 		return (GLuint)NULL;
 	}
 
-    assert(err==GL_NO_ERROR);
+  assert(glGetError()==GL_NO_ERROR);
 
     glCompileShader(hFragmentShader);
-    err = glGetError();
+    assert(glGetError()==GL_NO_ERROR);
 
     glGetShaderiv(hFragmentShader, GL_COMPILE_STATUS, &testVal);
 	if(testVal == GL_FALSE)
@@ -70,7 +68,7 @@ GLuint gltLoadShaderPair(const char *szVertexProg, const char *szFragmentProg)
 		return (GLuint)NULL;
 	}
 
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
 
 	// Check for errors
 

btgui/OpenGLWindow/SimpleOpenGL3App.cpp

@@ -430,19 +430,17 @@ static void writeTextureToFile(int textureWidth, int textureHeight, const char*
 {
 	int numComponents = 4;
 	//glPixelStorei(GL_PACK_ALIGNMENT,1);
-	GLuint err=glGetError();
-	assert(err==GL_NO_ERROR);
+	
+	b3Assert(glGetError()==GL_NO_ERROR);
 	//glReadBuffer(GL_BACK);//COLOR_ATTACHMENT0);
-	err=glGetError();
-	assert(err==GL_NO_ERROR);
+	
 	float* orgPixels = (float*)malloc(textureWidth*textureHeight*numComponents*4);
 	glReadPixels(0,0,textureWidth, textureHeight, GL_RGBA, GL_FLOAT, orgPixels);
 	//it is useful to have the actual float values for debugging purposes
 
 	//convert float->char
 	char* pixels = (char*)malloc(textureWidth*textureHeight*numComponents);
-	err=glGetError();
-	assert(err==GL_NO_ERROR);
+	assert(glGetError()==GL_NO_ERROR);
 
 	for (int j=0;j<textureHeight;j++)
 	{

src/BulletDynamics/Character/btKinematicCharacterController.cpp

@@ -381,8 +381,8 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
 		if (callback.hasHit())
 		{	
 			// we moved only a fraction
-			btScalar hitDistance;
-			hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
+			//btScalar hitDistance;
+			//hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
 
 //			m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
 

src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp

@@ -540,8 +540,8 @@ void btConeTwistConstraint::calcAngleInfo()
 	m_solveTwistLimit = false;
 	m_solveSwingLimit = false;
 
-	btVector3 b1Axis1,b1Axis2,b1Axis3;
-	btVector3 b2Axis1,b2Axis2;
+	btVector3 b1Axis1(0,0,0),b1Axis2(0,0,0),b1Axis3(0,0,0);
+	btVector3 b2Axis1(0,0,0),b2Axis2(0,0,0);
 
 	b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
 	b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
@@ -983,8 +983,8 @@ void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingA
 
 void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
 {
-	btTransform trACur = m_rbA.getCenterOfMassTransform();
-	btTransform trBCur = m_rbB.getCenterOfMassTransform();
+	//btTransform trACur = m_rbA.getCenterOfMassTransform();
+	//btTransform trBCur = m_rbB.getCenterOfMassTransform();
 //	btTransform trABCur = trBCur.inverse() * trACur;
 //	btQuaternion qABCur = trABCur.getRotation();
 //	btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);

src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp

@@ -155,8 +155,7 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
 		body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
 		body2.getLinearVelocity(),
 		body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); 
-	btScalar a;
-	a=jacDiagABInv;
+
 
 
 	rel_vel = normal.dot(vel);

src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp

@@ -1,186 +1,186 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-
-#include "btFixedConstraint.h"
-#include "BulletDynamics/Dynamics/btRigidBody.h"
-#include "LinearMath/btTransformUtil.h"
-#include <new>
-
-
-btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB)
-:btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB)
-{
-	m_frameInA = frameInA;
-	m_frameInB = frameInB;
-
-}
-
-btFixedConstraint::~btFixedConstraint ()
-{
-}
-
-	
-void btFixedConstraint::getInfo1 (btConstraintInfo1* info)
-{
-	info->m_numConstraintRows = 6;
-	info->nub = 0;
-}
-
-void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
-{
-	//fix the 3 linear degrees of freedom
-
-	const btTransform& transA = m_rbA.getCenterOfMassTransform();
-	const btTransform& transB = m_rbB.getCenterOfMassTransform();
-
-	const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin();
-	const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis();
-	const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin();
-	const btMatrix3x3& worldOrnB = m_rbB.getCenterOfMassTransform().getBasis();
-	
-
-	info->m_J1linearAxis[0] = 1;
-	info->m_J1linearAxis[info->rowskip+1] = 1;
-	info->m_J1linearAxis[2*info->rowskip+2] = 1;
-
-	btVector3 a1 = worldOrnA * m_frameInA.getOrigin();
-    {
-		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
-		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
-		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
-		btVector3 a1neg = -a1;
-		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
-	}
-
-	if (info->m_J2linearAxis)
-	{
-		info->m_J2linearAxis[0] = -1;
-		info->m_J2linearAxis[info->rowskip+1] = -1;
-		info->m_J2linearAxis[2*info->rowskip+2] = -1;
-	}
-	
-	btVector3 a2 = worldOrnB*m_frameInB.getOrigin();
-   {
-		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
-		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
-		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
-		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
-	}
-
-    // set right hand side for the linear dofs
-	btScalar k = info->fps * info->erp;
-	
-	btVector3 linearError = k*(a2+worldPosB-a1-worldPosA);
-    int j;
-	for (j=0; j<3; j++)
-    {
-        info->m_constraintError[j*info->rowskip] = linearError[j];
-		//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
-    }
-
-	btVector3 ivA = transA.getBasis() * m_frameInA.getBasis().getColumn(0);
-	btVector3 jvA = transA.getBasis() * m_frameInA.getBasis().getColumn(1);
-	btVector3 kvA = transA.getBasis() * m_frameInA.getBasis().getColumn(2);
-	btVector3 ivB = transB.getBasis() * m_frameInB.getBasis().getColumn(0);
-	btVector3 target;
-	btScalar x = ivB.dot(ivA);
-	btScalar y = ivB.dot(jvA);
-	btScalar z = ivB.dot(kvA);
-	btVector3 swingAxis(0,0,0);
-	{ 
-		if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
-		{
-			swingAxis = -ivB.cross(ivA);
-		}
-	}
-	btVector3 vTwist(1,0,0);
-
-	// compute rotation of A wrt B (in constraint space)
-	btQuaternion qA = transA.getRotation() * m_frameInA.getRotation();
-	btQuaternion qB = transB.getRotation() * m_frameInB.getRotation();
-	btQuaternion qAB = qB.inverse() * qA;
-	// split rotation into cone and twist
-	// (all this is done from B's perspective. Maybe I should be averaging axes...)
-	btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
-	btQuaternion qABCone  = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
-	btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
-
-	int row = 3;
-    int srow = row * info->rowskip;
-	btVector3 ax1;
-	// angular limits
-	{
-		btScalar *J1 = info->m_J1angularAxis;
-		btScalar *J2 = info->m_J2angularAxis;
-		btTransform trA = transA*m_frameInA;
-		btVector3 twistAxis = trA.getBasis().getColumn(0);
-
-		btVector3 p = trA.getBasis().getColumn(1);
-		btVector3 q = trA.getBasis().getColumn(2);
-		int srow1 = srow + info->rowskip;
-		J1[srow+0] = p[0];
-		J1[srow+1] = p[1];
-		J1[srow+2] = p[2];
-		J1[srow1+0] = q[0];
-		J1[srow1+1] = q[1];
-		J1[srow1+2] = q[2];
-		J2[srow+0] = -p[0];
-		J2[srow+1] = -p[1];
-		J2[srow+2] = -p[2];
-		J2[srow1+0] = -q[0];
-		J2[srow1+1] = -q[1];
-		J2[srow1+2] = -q[2];
-		btScalar fact = info->fps;
-		info->m_constraintError[srow] =   fact * swingAxis.dot(p);
-		info->m_constraintError[srow1] =  fact * swingAxis.dot(q);
-		info->m_lowerLimit[srow] = -SIMD_INFINITY;
-		info->m_upperLimit[srow] = SIMD_INFINITY;
-		info->m_lowerLimit[srow1] = -SIMD_INFINITY;
-		info->m_upperLimit[srow1] = SIMD_INFINITY;
-		srow = srow1 + info->rowskip;
-
-		{
-			btQuaternion qMinTwist = qABTwist;
-			btScalar twistAngle = qABTwist.getAngle();
-
-			if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
-			{
-				qMinTwist = -(qABTwist);
-				twistAngle = qMinTwist.getAngle();
-			}
-
-			if (twistAngle > SIMD_EPSILON)
-			{
-				twistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
-				twistAxis.normalize();
-				twistAxis = quatRotate(qB, -twistAxis);
-			}
-			ax1 = twistAxis;
-			btScalar *J1 = info->m_J1angularAxis;
-			btScalar *J2 = info->m_J2angularAxis;
-			J1[srow+0] = ax1[0];
-			J1[srow+1] = ax1[1];
-			J1[srow+2] = ax1[2];
-			J2[srow+0] = -ax1[0];
-			J2[srow+1] = -ax1[1];
-			J2[srow+2] = -ax1[2];
-			btScalar k = info->fps;
-			info->m_constraintError[srow] = k * twistAngle;
-			info->m_lowerLimit[srow] = -SIMD_INFINITY;
-			info->m_upperLimit[srow] = SIMD_INFINITY;
-		}
-	}
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btFixedConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+#include <new>
+
+
+btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB)
+:btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB)
+{
+	m_frameInA = frameInA;
+	m_frameInB = frameInB;
+
+}
+
+btFixedConstraint::~btFixedConstraint ()
+{
+}
+
+	
+void btFixedConstraint::getInfo1 (btConstraintInfo1* info)
+{
+	info->m_numConstraintRows = 6;
+	info->nub = 0;
+}
+
+void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
+{
+	//fix the 3 linear degrees of freedom
+
+	const btTransform& transA = m_rbA.getCenterOfMassTransform();
+	const btTransform& transB = m_rbB.getCenterOfMassTransform();
+
+	const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin();
+	const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis();
+	const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin();
+	const btMatrix3x3& worldOrnB = m_rbB.getCenterOfMassTransform().getBasis();
+	
+
+	info->m_J1linearAxis[0] = 1;
+	info->m_J1linearAxis[info->rowskip+1] = 1;
+	info->m_J1linearAxis[2*info->rowskip+2] = 1;
+
+	btVector3 a1 = worldOrnA * m_frameInA.getOrigin();
+    {
+		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
+		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
+		btVector3 a1neg = -a1;
+		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
+
+	if (info->m_J2linearAxis)
+	{
+		info->m_J2linearAxis[0] = -1;
+		info->m_J2linearAxis[info->rowskip+1] = -1;
+		info->m_J2linearAxis[2*info->rowskip+2] = -1;
+	}
+	
+	btVector3 a2 = worldOrnB*m_frameInB.getOrigin();
+   {
+		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
+		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
+		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
+
+    // set right hand side for the linear dofs
+	btScalar k = info->fps * info->erp;
+	
+	btVector3 linearError = k*(a2+worldPosB-a1-worldPosA);
+    int j;
+	for (j=0; j<3; j++)
+    {
+        info->m_constraintError[j*info->rowskip] = linearError[j];
+		//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
+    }
+
+	btVector3 ivA = transA.getBasis() * m_frameInA.getBasis().getColumn(0);
+	btVector3 jvA = transA.getBasis() * m_frameInA.getBasis().getColumn(1);
+	btVector3 kvA = transA.getBasis() * m_frameInA.getBasis().getColumn(2);
+	btVector3 ivB = transB.getBasis() * m_frameInB.getBasis().getColumn(0);
+	btVector3 target;
+	//btScalar x = ivB.dot(ivA);//??
+	btScalar y = ivB.dot(jvA);
+	btScalar z = ivB.dot(kvA);
+	btVector3 swingAxis(0,0,0);
+	{ 
+		if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
+		{
+			swingAxis = -ivB.cross(ivA);
+		}
+	}
+	btVector3 vTwist(1,0,0);
+
+	// compute rotation of A wrt B (in constraint space)
+	btQuaternion qA = transA.getRotation() * m_frameInA.getRotation();
+	btQuaternion qB = transB.getRotation() * m_frameInB.getRotation();
+	btQuaternion qAB = qB.inverse() * qA;
+	// split rotation into cone and twist
+	// (all this is done from B's perspective. Maybe I should be averaging axes...)
+	btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
+	btQuaternion qABCone  = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
+	btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
+
+	int row = 3;
+    int srow = row * info->rowskip;
+	btVector3 ax1;
+	// angular limits
+	{
+		btScalar *J1 = info->m_J1angularAxis;
+		btScalar *J2 = info->m_J2angularAxis;
+		btTransform trA = transA*m_frameInA;
+		btVector3 twistAxis = trA.getBasis().getColumn(0);
+
+		btVector3 p = trA.getBasis().getColumn(1);
+		btVector3 q = trA.getBasis().getColumn(2);
+		int srow1 = srow + info->rowskip;
+		J1[srow+0] = p[0];
+		J1[srow+1] = p[1];
+		J1[srow+2] = p[2];
+		J1[srow1+0] = q[0];
+		J1[srow1+1] = q[1];
+		J1[srow1+2] = q[2];
+		J2[srow+0] = -p[0];
+		J2[srow+1] = -p[1];
+		J2[srow+2] = -p[2];
+		J2[srow1+0] = -q[0];
+		J2[srow1+1] = -q[1];
+		J2[srow1+2] = -q[2];
+		btScalar fact = info->fps;
+		info->m_constraintError[srow] =   fact * swingAxis.dot(p);
+		info->m_constraintError[srow1] =  fact * swingAxis.dot(q);
+		info->m_lowerLimit[srow] = -SIMD_INFINITY;
+		info->m_upperLimit[srow] = SIMD_INFINITY;
+		info->m_lowerLimit[srow1] = -SIMD_INFINITY;
+		info->m_upperLimit[srow1] = SIMD_INFINITY;
+		srow = srow1 + info->rowskip;
+
+		{
+			btQuaternion qMinTwist = qABTwist;
+			btScalar twistAngle = qABTwist.getAngle();
+
+			if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
+			{
+				qMinTwist = -(qABTwist);
+				twistAngle = qMinTwist.getAngle();
+			}
+
+			if (twistAngle > SIMD_EPSILON)
+			{
+				twistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
+				twistAxis.normalize();
+				twistAxis = quatRotate(qB, -twistAxis);
+			}
+			ax1 = twistAxis;
+			btScalar *J1 = info->m_J1angularAxis;
+			btScalar *J2 = info->m_J2angularAxis;
+			J1[srow+0] = ax1[0];
+			J1[srow+1] = ax1[1];
+			J1[srow+2] = ax1[2];
+			J2[srow+0] = -ax1[0];
+			J2[srow+1] = -ax1[1];
+			J2[srow+2] = -ax1[2];
+			btScalar k = info->fps;
+			info->m_constraintError[srow] = k * twistAngle;
+			info->m_lowerLimit[srow] = -SIMD_INFINITY;
+			info->m_upperLimit[srow] = SIMD_INFINITY;
+		}
+	}
 }

src/BulletDynamics/ConstraintSolver/btFixedConstraint.h

@@ -1,48 +1,48 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef BT_FIXED_CONSTRAINT_H
-#define BT_FIXED_CONSTRAINT_H
-
-#include "btTypedConstraint.h"
-
-ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint
-{
-
-	btTransform m_frameInA;
-	btTransform m_frameInB;
-public:
-	btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB);
-	
-	virtual ~btFixedConstraint();
-
-	
-	virtual void getInfo1 (btConstraintInfo1* info);
-
-	virtual void getInfo2 (btConstraintInfo2* info);
-
-	virtual	void	setParam(int num, btScalar value, int axis = -1)
-	{
-		btAssert(0);
-	}
-	virtual	btScalar getParam(int num, int axis = -1) const
-	{
-		btAssert(0);
-		return 0.f;
-	}
-
-};
-
-#endif //BT_FIXED_CONSTRAINT_H
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_FIXED_CONSTRAINT_H
+#define BT_FIXED_CONSTRAINT_H
+
+#include "btTypedConstraint.h"
+
+ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint
+{
+
+	btTransform m_frameInA;
+	btTransform m_frameInB;
+public:
+	btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB);
+	
+	virtual ~btFixedConstraint();
+
+	
+	virtual void getInfo1 (btConstraintInfo1* info);
+
+	virtual void getInfo2 (btConstraintInfo2* info);
+
+	virtual	void	setParam(int num, btScalar value, int axis = -1)
+	{
+		btAssert(0);
+	}
+	virtual	btScalar getParam(int num, int axis = -1) const
+	{
+		btAssert(0);
+		return 0.f;
+	}
+
+};
+
+#endif //BT_FIXED_CONSTRAINT_H

src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp

@@ -1,54 +1,54 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2012 Advanced Micro Devices, Inc.  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-/// Implemented by Erwin Coumans. The idea for the constraint comes from Dimitris Papavasiliou.
-
-#include "btGearConstraint.h"
-
-btGearConstraint::btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio)
-:btTypedConstraint(GEAR_CONSTRAINT_TYPE,rbA,rbB),
-m_axisInA(axisInA),
-m_axisInB(axisInB),
-m_ratio(ratio)
-{
-}
-
-btGearConstraint::~btGearConstraint ()
-{
-}
-
-void btGearConstraint::getInfo1 (btConstraintInfo1* info)
-{
-	info->m_numConstraintRows = 1;
-	info->nub = 1;
-}
-
-void btGearConstraint::getInfo2 (btConstraintInfo2* info)
-{
-	btVector3 globalAxisA, globalAxisB;
-
-	globalAxisA = m_rbA.getWorldTransform().getBasis()*this->m_axisInA;
-	globalAxisB = m_rbB.getWorldTransform().getBasis()*this->m_axisInB;
-
-	info->m_J1angularAxis[0] = globalAxisA[0];
-	info->m_J1angularAxis[1] = globalAxisA[1];
-	info->m_J1angularAxis[2] = globalAxisA[2];
-
-	info->m_J2angularAxis[0] = m_ratio*globalAxisB[0];
-	info->m_J2angularAxis[1] = m_ratio*globalAxisB[1];
-	info->m_J2angularAxis[2] = m_ratio*globalAxisB[2];
-
-}
-
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2012 Advanced Micro Devices, Inc.  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/// Implemented by Erwin Coumans. The idea for the constraint comes from Dimitris Papavasiliou.
+
+#include "btGearConstraint.h"
+
+btGearConstraint::btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio)
+:btTypedConstraint(GEAR_CONSTRAINT_TYPE,rbA,rbB),
+m_axisInA(axisInA),
+m_axisInB(axisInB),
+m_ratio(ratio)
+{
+}
+
+btGearConstraint::~btGearConstraint ()
+{
+}
+
+void btGearConstraint::getInfo1 (btConstraintInfo1* info)
+{
+	info->m_numConstraintRows = 1;
+	info->nub = 1;
+}
+
+void btGearConstraint::getInfo2 (btConstraintInfo2* info)
+{
+	btVector3 globalAxisA, globalAxisB;
+
+	globalAxisA = m_rbA.getWorldTransform().getBasis()*this->m_axisInA;
+	globalAxisB = m_rbB.getWorldTransform().getBasis()*this->m_axisInB;
+
+	info->m_J1angularAxis[0] = globalAxisA[0];
+	info->m_J1angularAxis[1] = globalAxisA[1];
+	info->m_J1angularAxis[2] = globalAxisA[2];
+
+	info->m_J2angularAxis[0] = m_ratio*globalAxisB[0];
+	info->m_J2angularAxis[1] = m_ratio*globalAxisB[1];
+	info->m_J2angularAxis[2] = m_ratio*globalAxisB[2];
+
+}
+

src/BulletDynamics/ConstraintSolver/btGearConstraint.h

@@ -1,79 +1,79 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2012 Advanced Micro Devices, Inc.  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-
-
-#ifndef BT_GEAR_CONSTRAINT_H
-#define BT_GEAR_CONSTRAINT_H
-
-#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
-
-
-#ifdef BT_USE_DOUBLE_PRECISION
-#define btGearConstraintData	btGearConstraintDoubleData
-#define btGearConstraintDataName	"btGearConstraintDoubleData"
-#else
-#define btGearConstraintData	btGearConstraintFloatData
-#define btGearConstraintDataName	"btGearConstraintFloatData"
-#endif //BT_USE_DOUBLE_PRECISION
-
-
-
-///The btGeatConstraint will couple the angular velocity for two bodies around given local axis and ratio.
-///See Bullet/Demos/ConstraintDemo for an example use.
-class btGearConstraint : public btTypedConstraint
-{
-protected:
-	btVector3	m_axisInA;
-	btVector3	m_axisInB;
-	bool		m_useFrameA;
-	btScalar	m_ratio;
-
-public:
-	btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio=1.f);
-	virtual ~btGearConstraint ();
-
-	///internal method used by the constraint solver, don't use them directly
-	virtual void getInfo1 (btConstraintInfo1* info);
-
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2012 Advanced Micro Devices, Inc.  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef BT_GEAR_CONSTRAINT_H
+#define BT_GEAR_CONSTRAINT_H
+
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btGearConstraintData	btGearConstraintDoubleData
+#define btGearConstraintDataName	"btGearConstraintDoubleData"
+#else
+#define btGearConstraintData	btGearConstraintFloatData
+#define btGearConstraintDataName	"btGearConstraintFloatData"
+#endif //BT_USE_DOUBLE_PRECISION
+
+
+
+///The btGeatConstraint will couple the angular velocity for two bodies around given local axis and ratio.
+///See Bullet/Demos/ConstraintDemo for an example use.
+class btGearConstraint : public btTypedConstraint
+{
+protected:
+	btVector3	m_axisInA;
+	btVector3	m_axisInB;
+	bool		m_useFrameA;
+	btScalar	m_ratio;
+
+public:
+	btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio=1.f);
+	virtual ~btGearConstraint ();
+
+	///internal method used by the constraint solver, don't use them directly
+	virtual void getInfo1 (btConstraintInfo1* info);
+
 	///internal method used by the constraint solver, don't use them directly
 	virtual void getInfo2 (btConstraintInfo2* info);
 
-	void setAxisA(btVector3& axisA) 
-	{
-		m_axisInA = axisA;
+	void setAxisA(btVector3& axisA) 
+	{
+		m_axisInA = axisA;
 	}
-	void setAxisB(btVector3& axisB)
-	{
-		m_axisInB = axisB;
+	void setAxisB(btVector3& axisB)
+	{
+		m_axisInB = axisB;
 	}
-	void setRatio(btScalar ratio)
-	{
-		m_ratio = ratio;
+	void setRatio(btScalar ratio)
+	{
+		m_ratio = ratio;
 	}
-	const btVector3& getAxisA() const
-	{
-		return m_axisInA;
+	const btVector3& getAxisA() const
+	{
+		return m_axisInA;
 	}
-	const btVector3& getAxisB() const
-	{
-		return m_axisInB;
+	const btVector3& getAxisB() const
+	{
+		return m_axisInB;
 	}
-	btScalar getRatio() const
-	{
-		return m_ratio;
+	btScalar getRatio() const
+	{
+		return m_ratio;
 	}
 
 
@@ -84,69 +84,69 @@ public:
 		(void) axis;
 		btAssert(0);
 	}
-
-	///return the local value of parameter
-	virtual	btScalar getParam(int num, int axis = -1) const 
-	{ 
-		(void) num;
-		(void) axis;
-		btAssert(0);
-		return 0.f;
-	}
-
-	virtual	int	calculateSerializeBufferSize() const;
-
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
-};
-
-
-
-
-///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct btGearConstraintFloatData
-{
-	btTypedConstraintFloatData	m_typeConstraintData;
-
-	btVector3FloatData			m_axisInA;
-	btVector3FloatData			m_axisInB;
-
-	float							m_ratio;
-	char							m_padding[4];
-};
-
-struct btGearConstraintDoubleData
-{
-	btTypedConstraintDoubleData	m_typeConstraintData;
-
-	btVector3DoubleData			m_axisInA;
-	btVector3DoubleData			m_axisInB;
-
-	double						m_ratio;
-};
-
-SIMD_FORCE_INLINE	int	btGearConstraint::calculateSerializeBufferSize() const
-{
-	return sizeof(btGearConstraintData);
-}
-
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-SIMD_FORCE_INLINE	const char*	btGearConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
-{
-	btGearConstraintData* gear = (btGearConstraintData*)dataBuffer;
-	btTypedConstraint::serialize(&gear->m_typeConstraintData,serializer);
-
-	m_axisInA.serialize( gear->m_axisInA );
-	m_axisInB.serialize( gear->m_axisInB );
-
-	gear->m_ratio = m_ratio;
-
-	return btGearConstraintDataName;
-}
-
-
-
-
-
-
-#endif //BT_GEAR_CONSTRAINT_H
+
+	///return the local value of parameter
+	virtual	btScalar getParam(int num, int axis = -1) const 
+	{ 
+		(void) num;
+		(void) axis;
+		btAssert(0);
+		return 0.f;
+	}
+
+	virtual	int	calculateSerializeBufferSize() const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
+};
+
+
+
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct btGearConstraintFloatData
+{
+	btTypedConstraintFloatData	m_typeConstraintData;
+
+	btVector3FloatData			m_axisInA;
+	btVector3FloatData			m_axisInB;
+
+	float							m_ratio;
+	char							m_padding[4];
+};
+
+struct btGearConstraintDoubleData
+{
+	btTypedConstraintDoubleData	m_typeConstraintData;
+
+	btVector3DoubleData			m_axisInA;
+	btVector3DoubleData			m_axisInB;
+
+	double						m_ratio;
+};
+
+SIMD_FORCE_INLINE	int	btGearConstraint::calculateSerializeBufferSize() const
+{
+	return sizeof(btGearConstraintData);
+}
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE	const char*	btGearConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+	btGearConstraintData* gear = (btGearConstraintData*)dataBuffer;
+	btTypedConstraint::serialize(&gear->m_typeConstraintData,serializer);
+
+	m_axisInA.serialize( gear->m_axisInA );
+	m_axisInB.serialize( gear->m_axisInB );
+
+	gear->m_ratio = m_ratio;
+
+	return btGearConstraintDataName;
+}
+
+
+
+
+
+
+#endif //BT_GEAR_CONSTRAINT_H

src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp

@@ -50,8 +50,8 @@ btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbA,
 	: btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, rbA, rbB)
 	, m_frameInA(frameInA)
 	, m_frameInB(frameInB)
+	, m_rotateOrder(rotOrder)	
 	, m_flags(0)
-	, m_rotateOrder(rotOrder)
 {
 	calculateTransforms();
 }
@@ -60,8 +60,8 @@ btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbA,
 btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder)
 	: btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, getFixedBody(), rbB)
 	, m_frameInB(frameInB)
-	, m_flags(0)
 	, m_rotateOrder(rotOrder)
+	, m_flags(0)
 {
 	///not providing rigidbody A means implicitly using worldspace for body A
 	m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
@@ -776,7 +776,7 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 		btScalar kd = limot->m_springDamping;
 		btScalar ks = limot->m_springStiffness;
 		btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
-		btScalar erp = 0.1;
+//		btScalar erp = 0.1;
 		btScalar cfm = 0.0;
 		btScalar mA = 1.0 / m_rbA.getInvMass();
 		btScalar mB = 1.0 / m_rbB.getInvMass();

src/BulletDynamics/ConstraintSolver/btHingeConstraint.h

@@ -217,6 +217,14 @@ public:
 
 	}
 
+    bool hasLimit() const {
+#ifdef  _BT_USE_CENTER_LIMIT_
+        return m_limit.getHalfRange() > 0;
+#else
+        return m_lowerLimit <= m_upperLimit;
+#endif
+    }
+
 	btScalar	getLowerLimit() const
 	{
 #ifdef	_BT_USE_CENTER_LIMIT_

src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp

@@ -1,463 +1,463 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include "btNNCGConstraintSolver.h"
-
-
-
-
-
-
-btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
-{
-	btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
-
-	m_pNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
-	m_pC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
-	m_pCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
-	m_pCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
-
-	m_deltafNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
-	m_deltafC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
-	m_deltafCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
-	m_deltafCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
-
-	return val;
-}
-
-btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
-{
-
-	int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
-	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
-	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
-
-	if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
-	{
-		if (1)			// uncomment this for a bit less random ((iteration & 7) == 0)
-		{
-
-			for (int j=0; j<numNonContactPool; ++j) {
-				int tmp = m_orderNonContactConstraintPool[j];
-				int swapi = btRandInt2(j+1);
-				m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
-				m_orderNonContactConstraintPool[swapi] = tmp;
-			}
-
-			//contact/friction constraints are not solved more than 
-			if (iteration< infoGlobal.m_numIterations)
-			{
-				for (int j=0; j<numConstraintPool; ++j) {
-					int tmp = m_orderTmpConstraintPool[j];
-					int swapi = btRandInt2(j+1);
-					m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
-					m_orderTmpConstraintPool[swapi] = tmp;
-				}
-
-				for (int j=0; j<numFrictionPool; ++j) {
-					int tmp = m_orderFrictionConstraintPool[j];
-					int swapi = btRandInt2(j+1);
-					m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
-					m_orderFrictionConstraintPool[swapi] = tmp;
-				}
-			}
-		}
-	}
-
-
-	btScalar deltaflengthsqr = 0;
-
-	if (infoGlobal.m_solverMode & SOLVER_SIMD)
-	{
-		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
-		{
-			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
-			if (iteration < constraint.m_overrideNumSolverIterations) 
-			{
-				btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
-				m_deltafNC[j] = deltaf;
-				deltaflengthsqr += deltaf * deltaf;
-			}
-		}
-	} else 
-	{
-		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
-		{
-			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
-			if (iteration < constraint.m_overrideNumSolverIterations) 
-			{
-				btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
-				m_deltafNC[j] = deltaf;
-				deltaflengthsqr += deltaf * deltaf;
-			}
-		}
-	}
-
-
-	if (m_onlyForNoneContact) 
-	{
-		if (iteration==0) 
-		{
-			for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
-		} else {
-			// deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
-			btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
-			if (beta>1) 
-			{
-				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
-			} else 
-			{
-				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
-				{
-					btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
-					if (iteration < constraint.m_overrideNumSolverIterations) 
-					{
-						btScalar additionaldeltaimpulse = beta * m_pNC[j];
-						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
-						m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
-						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
-						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
-						const btSolverConstraint& c = constraint;
-						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
-						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
-					}
-				}
-			}
-		}
-		m_deltafLengthSqrPrev = deltaflengthsqr;
-	}
-
-
-
-	if (infoGlobal.m_solverMode & SOLVER_SIMD)
-	{
-
-		if (iteration< infoGlobal.m_numIterations)
-		{
-			for (int j=0;j<numConstraints;j++)
-			{
-				if (constraints[j]->isEnabled())
-				{
-					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
-					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
-					btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
-					btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
-					constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
-				}
-			}
-
-			///solve all contact constraints using SIMD, if available
-			if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
-			{
-				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-				int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
-
-				for (int c=0;c<numPoolConstraints;c++)
-				{
-					btScalar totalImpulse =0;
-
-					{
-						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
-						btScalar deltaf = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);						
-						m_deltafC[c] = deltaf;
-						deltaflengthsqr += deltaf*deltaf;
-						totalImpulse = solveManifold.m_appliedImpulse;
-					}
-					bool applyFriction = true;
-					if (applyFriction)
-					{
-						{
-
-							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
-
-							if (totalImpulse>btScalar(0))
-							{
-								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
-								btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-								m_deltafCF[c*multiplier] = deltaf;
-								deltaflengthsqr += deltaf*deltaf;
-							} else {
-								m_deltafCF[c*multiplier] = 0;
-							}
-						}
-
-						if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
-						{
-
-							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
-
-							if (totalImpulse>btScalar(0))
-							{
-								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
-								btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-								m_deltafCF[c*multiplier+1] = deltaf;
-								deltaflengthsqr += deltaf*deltaf;
-							} else {
-								m_deltafCF[c*multiplier+1] = 0;
-							}
-						}
-					}
-				}
-
-			}
-			else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
-			{
-				//solve the friction constraints after all contact constraints, don't interleave them
-				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-				int j;
-
-				for (j=0;j<numPoolConstraints;j++)
-				{
-					const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-					//resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-					btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-					m_deltafC[j] = deltaf;
-					deltaflengthsqr += deltaf*deltaf;
-				}
-
-
-
-				///solve all friction constraints, using SIMD, if available
-
-				int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
-				for (j=0;j<numFrictionPoolConstraints;j++)
-				{
-					btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
-					btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
-
-					if (totalImpulse>btScalar(0))
-					{
-						solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-						solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
-
-						//resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-						btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-						m_deltafCF[j] = deltaf;
-						deltaflengthsqr += deltaf*deltaf;
-					} else {
-						m_deltafCF[j] = 0;
-					}
-				}
-
-
-				int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
-				for (j=0;j<numRollingFrictionPoolConstraints;j++)
-				{
-
-					btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
-					btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
-					if (totalImpulse>btScalar(0))
-					{
-						btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
-						if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
-							rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
-
-						rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
-						rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
-
-						btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
-						m_deltafCRF[j] = deltaf;
-						deltaflengthsqr += deltaf*deltaf;
-					} else {
-						m_deltafCRF[j] = 0;
-					}
-				}
-
-
-			}
-		}
-
-
-
-	} else
-	{
-
-		if (iteration< infoGlobal.m_numIterations)
-		{
-			for (int j=0;j<numConstraints;j++)
-			{
-				if (constraints[j]->isEnabled())
-				{
-					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
-					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
-					btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
-					btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
-					constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
-				}
-			}
-			///solve all contact constraints
-			int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-			for (int j=0;j<numPoolConstraints;j++)
-			{
-				const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-				btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-				m_deltafC[j] = deltaf;
-				deltaflengthsqr += deltaf*deltaf;
-			}
-			///solve all friction constraints
-			int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
-			for (int j=0;j<numFrictionPoolConstraints;j++)
-			{
-				btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
-				btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
-
-				if (totalImpulse>btScalar(0))
-				{
-					solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-					solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
-
-					btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-					m_deltafCF[j] = deltaf;
-					deltaflengthsqr += deltaf*deltaf;
-				} else {
-					m_deltafCF[j] = 0;
-				}
-			}
-
-			int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
-			for (int j=0;j<numRollingFrictionPoolConstraints;j++)
-			{
-				btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
-				btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
-				if (totalImpulse>btScalar(0))
-				{
-					btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
-					if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
-						rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
-
-					rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
-					rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
-
-					btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
-					m_deltafCRF[j] = deltaf;
-					deltaflengthsqr += deltaf*deltaf;
-				} else {
-					m_deltafCRF[j] = 0;
-				}
-			}
-		}
-	}
-
-
-
-
-	if (!m_onlyForNoneContact) 
-	{
-		if (iteration==0) 
-		{
-			for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
-			for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = m_deltafC[j];
-			for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = m_deltafCF[j];
-			if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) 
-			{
-				for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = m_deltafCRF[j];
-			}
-		} else 
-		{
-			// deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
-			btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
-			if (beta>1) {
-				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
-				for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = 0;
-				for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = 0;
-				if ( (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) {
-					for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = 0;
-				}
-			} else {
-				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
-				{
-					btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
-					if (iteration < constraint.m_overrideNumSolverIterations) {
-						btScalar additionaldeltaimpulse = beta * m_pNC[j];
-						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
-						m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
-						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
-						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
-						const btSolverConstraint& c = constraint;
-						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
-						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
-					}
-				}
-				for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++)
-				{
-					btSolverConstraint& constraint = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-					if (iteration< infoGlobal.m_numIterations) {
-						btScalar additionaldeltaimpulse = beta * m_pC[j];
-						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
-						m_pC[j] = beta * m_pC[j] + m_deltafC[j];
-						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
-						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
-						const btSolverConstraint& c = constraint;
-						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
-						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
-					}
-				}
-				for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++)
-				{
-					btSolverConstraint& constraint = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
-					if (iteration< infoGlobal.m_numIterations) {
-						btScalar additionaldeltaimpulse = beta * m_pCF[j];
-						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
-						m_pCF[j] = beta * m_pCF[j] + m_deltafCF[j];
-						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
-						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
-						const btSolverConstraint& c = constraint;
-						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
-						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
-					}
-				}
-				if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) {
-					for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++)
-					{
-						btSolverConstraint& constraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
-						if (iteration< infoGlobal.m_numIterations) {
-							btScalar additionaldeltaimpulse = beta * m_pCRF[j];
-							constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
-							m_pCRF[j] = beta * m_pCRF[j] + m_deltafCRF[j];
-							btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
-							btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
-							const btSolverConstraint& c = constraint;
-							body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
-							body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
-						}
-					}
-				}
-			}
-		}
-		m_deltafLengthSqrPrev = deltaflengthsqr;
-	}
-
-	return deltaflengthsqr;
-}
-
-btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
-{
-	m_pNC.resizeNoInitialize(0);
-	m_pC.resizeNoInitialize(0);
-	m_pCF.resizeNoInitialize(0);
-	m_pCRF.resizeNoInitialize(0);
-
-	m_deltafNC.resizeNoInitialize(0);
-	m_deltafC.resizeNoInitialize(0);
-	m_deltafCF.resizeNoInitialize(0);
-	m_deltafCRF.resizeNoInitialize(0);
-
-	return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
-}
-
-
-
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btNNCGConstraintSolver.h"
+
+
+
+
+
+
+btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+{
+	btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
+
+	m_pNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
+	m_pC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
+	m_pCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
+	m_pCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
+
+	m_deltafNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
+	m_deltafC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
+	m_deltafCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
+	m_deltafCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
+
+	return val;
+}
+
+btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
+{
+
+	int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
+	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
+	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
+
+	if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
+	{
+		if (1)			// uncomment this for a bit less random ((iteration & 7) == 0)
+		{
+
+			for (int j=0; j<numNonContactPool; ++j) {
+				int tmp = m_orderNonContactConstraintPool[j];
+				int swapi = btRandInt2(j+1);
+				m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
+				m_orderNonContactConstraintPool[swapi] = tmp;
+			}
+
+			//contact/friction constraints are not solved more than 
+			if (iteration< infoGlobal.m_numIterations)
+			{
+				for (int j=0; j<numConstraintPool; ++j) {
+					int tmp = m_orderTmpConstraintPool[j];
+					int swapi = btRandInt2(j+1);
+					m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
+					m_orderTmpConstraintPool[swapi] = tmp;
+				}
+
+				for (int j=0; j<numFrictionPool; ++j) {
+					int tmp = m_orderFrictionConstraintPool[j];
+					int swapi = btRandInt2(j+1);
+					m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
+					m_orderFrictionConstraintPool[swapi] = tmp;
+				}
+			}
+		}
+	}
+
+
+	btScalar deltaflengthsqr = 0;
+
+	if (infoGlobal.m_solverMode & SOLVER_SIMD)
+	{
+		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+		{
+			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+			if (iteration < constraint.m_overrideNumSolverIterations) 
+			{
+				btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+				m_deltafNC[j] = deltaf;
+				deltaflengthsqr += deltaf * deltaf;
+			}
+		}
+	} else 
+	{
+		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+		{
+			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+			if (iteration < constraint.m_overrideNumSolverIterations) 
+			{
+				btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+				m_deltafNC[j] = deltaf;
+				deltaflengthsqr += deltaf * deltaf;
+			}
+		}
+	}
+
+
+	if (m_onlyForNoneContact) 
+	{
+		if (iteration==0) 
+		{
+			for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
+		} else {
+			// deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
+			btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
+			if (beta>1) 
+			{
+				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
+			} else 
+			{
+				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+				{
+					btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+					if (iteration < constraint.m_overrideNumSolverIterations) 
+					{
+						btScalar additionaldeltaimpulse = beta * m_pNC[j];
+						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+						m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
+						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+						const btSolverConstraint& c = constraint;
+						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+					}
+				}
+			}
+		}
+		m_deltafLengthSqrPrev = deltaflengthsqr;
+	}
+
+
+
+	if (infoGlobal.m_solverMode & SOLVER_SIMD)
+	{
+
+		if (iteration< infoGlobal.m_numIterations)
+		{
+			for (int j=0;j<numConstraints;j++)
+			{
+				if (constraints[j]->isEnabled())
+				{
+					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
+					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
+					btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
+					btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
+					constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
+				}
+			}
+
+			///solve all contact constraints using SIMD, if available
+			if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
+			{
+				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+				int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
+
+				for (int c=0;c<numPoolConstraints;c++)
+				{
+					btScalar totalImpulse =0;
+
+					{
+						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
+						btScalar deltaf = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);						
+						m_deltafC[c] = deltaf;
+						deltaflengthsqr += deltaf*deltaf;
+						totalImpulse = solveManifold.m_appliedImpulse;
+					}
+					bool applyFriction = true;
+					if (applyFriction)
+					{
+						{
+
+							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
+
+							if (totalImpulse>btScalar(0))
+							{
+								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+								btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+								m_deltafCF[c*multiplier] = deltaf;
+								deltaflengthsqr += deltaf*deltaf;
+							} else {
+								m_deltafCF[c*multiplier] = 0;
+							}
+						}
+
+						if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
+						{
+
+							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
+
+							if (totalImpulse>btScalar(0))
+							{
+								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+								btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+								m_deltafCF[c*multiplier+1] = deltaf;
+								deltaflengthsqr += deltaf*deltaf;
+							} else {
+								m_deltafCF[c*multiplier+1] = 0;
+							}
+						}
+					}
+				}
+
+			}
+			else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
+			{
+				//solve the friction constraints after all contact constraints, don't interleave them
+				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+				int j;
+
+				for (j=0;j<numPoolConstraints;j++)
+				{
+					const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+					//resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					m_deltafC[j] = deltaf;
+					deltaflengthsqr += deltaf*deltaf;
+				}
+
+
+
+				///solve all friction constraints, using SIMD, if available
+
+				int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+				for (j=0;j<numFrictionPoolConstraints;j++)
+				{
+					btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+					btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+
+					if (totalImpulse>btScalar(0))
+					{
+						solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+						solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+
+						//resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						m_deltafCF[j] = deltaf;
+						deltaflengthsqr += deltaf*deltaf;
+					} else {
+						m_deltafCF[j] = 0;
+					}
+				}
+
+
+				int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
+				for (j=0;j<numRollingFrictionPoolConstraints;j++)
+				{
+
+					btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
+					btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
+					if (totalImpulse>btScalar(0))
+					{
+						btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
+						if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
+							rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+
+						rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+						rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+
+						btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+						m_deltafCRF[j] = deltaf;
+						deltaflengthsqr += deltaf*deltaf;
+					} else {
+						m_deltafCRF[j] = 0;
+					}
+				}
+
+
+			}
+		}
+
+
+
+	} else
+	{
+
+		if (iteration< infoGlobal.m_numIterations)
+		{
+			for (int j=0;j<numConstraints;j++)
+			{
+				if (constraints[j]->isEnabled())
+				{
+					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
+					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
+					btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
+					btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
+					constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
+				}
+			}
+			///solve all contact constraints
+			int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+			for (int j=0;j<numPoolConstraints;j++)
+			{
+				const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+				btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+				m_deltafC[j] = deltaf;
+				deltaflengthsqr += deltaf*deltaf;
+			}
+			///solve all friction constraints
+			int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+			for (int j=0;j<numFrictionPoolConstraints;j++)
+			{
+				btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+				btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+
+				if (totalImpulse>btScalar(0))
+				{
+					solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+					solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+
+					btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					m_deltafCF[j] = deltaf;
+					deltaflengthsqr += deltaf*deltaf;
+				} else {
+					m_deltafCF[j] = 0;
+				}
+			}
+
+			int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
+			for (int j=0;j<numRollingFrictionPoolConstraints;j++)
+			{
+				btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
+				btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
+				if (totalImpulse>btScalar(0))
+				{
+					btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
+					if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
+						rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+
+					rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+					rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+
+					btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+					m_deltafCRF[j] = deltaf;
+					deltaflengthsqr += deltaf*deltaf;
+				} else {
+					m_deltafCRF[j] = 0;
+				}
+			}
+		}
+	}
+
+
+
+
+	if (!m_onlyForNoneContact) 
+	{
+		if (iteration==0) 
+		{
+			for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
+			for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = m_deltafC[j];
+			for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = m_deltafCF[j];
+			if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) 
+			{
+				for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = m_deltafCRF[j];
+			}
+		} else 
+		{
+			// deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
+			btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
+			if (beta>1) {
+				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
+				for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = 0;
+				for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = 0;
+				if ( (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) {
+					for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = 0;
+				}
+			} else {
+				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+				{
+					btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+					if (iteration < constraint.m_overrideNumSolverIterations) {
+						btScalar additionaldeltaimpulse = beta * m_pNC[j];
+						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+						m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
+						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+						const btSolverConstraint& c = constraint;
+						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+					}
+				}
+				for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++)
+				{
+					btSolverConstraint& constraint = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+					if (iteration< infoGlobal.m_numIterations) {
+						btScalar additionaldeltaimpulse = beta * m_pC[j];
+						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+						m_pC[j] = beta * m_pC[j] + m_deltafC[j];
+						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+						const btSolverConstraint& c = constraint;
+						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+					}
+				}
+				for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++)
+				{
+					btSolverConstraint& constraint = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+					if (iteration< infoGlobal.m_numIterations) {
+						btScalar additionaldeltaimpulse = beta * m_pCF[j];
+						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+						m_pCF[j] = beta * m_pCF[j] + m_deltafCF[j];
+						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+						const btSolverConstraint& c = constraint;
+						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+					}
+				}
+				if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) {
+					for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++)
+					{
+						btSolverConstraint& constraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
+						if (iteration< infoGlobal.m_numIterations) {
+							btScalar additionaldeltaimpulse = beta * m_pCRF[j];
+							constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+							m_pCRF[j] = beta * m_pCRF[j] + m_deltafCRF[j];
+							btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+							btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+							const btSolverConstraint& c = constraint;
+							body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+							body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+						}
+					}
+				}
+			}
+		}
+		m_deltafLengthSqrPrev = deltaflengthsqr;
+	}
+
+	return deltaflengthsqr;
+}
+
+btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
+{
+	m_pNC.resizeNoInitialize(0);
+	m_pC.resizeNoInitialize(0);
+	m_pCF.resizeNoInitialize(0);
+	m_pCRF.resizeNoInitialize(0);
+
+	m_deltafNC.resizeNoInitialize(0);
+	m_deltafC.resizeNoInitialize(0);
+	m_deltafCF.resizeNoInitialize(0);
+	m_deltafCRF.resizeNoInitialize(0);
+
+	return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
+}
+
+
+

src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h

@@ -1,64 +1,64 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef BT_NNCG_CONSTRAINT_SOLVER_H
-#define BT_NNCG_CONSTRAINT_SOLVER_H
-
-#include "btSequentialImpulseConstraintSolver.h"
-
-ATTRIBUTE_ALIGNED16(class) btNNCGConstraintSolver : public btSequentialImpulseConstraintSolver
-{
-protected:
-
-	btScalar m_deltafLengthSqrPrev;
-
-	btAlignedObjectArray<btScalar> m_pNC;  // p for None Contact constraints
-	btAlignedObjectArray<btScalar> m_pC;   // p for Contact constraints
-	btAlignedObjectArray<btScalar> m_pCF;  // p for ContactFriction constraints
-	btAlignedObjectArray<btScalar> m_pCRF; // p for ContactRollingFriction constraints
-
-	//These are recalculated in every iterations. We just keep these to prevent reallocation in each iteration.
-	btAlignedObjectArray<btScalar> m_deltafNC;  // deltaf for NoneContact constraints
-	btAlignedObjectArray<btScalar> m_deltafC;   // deltaf for Contact constraints
-	btAlignedObjectArray<btScalar> m_deltafCF;  // deltaf for ContactFriction constraints
-	btAlignedObjectArray<btScalar> m_deltafCRF; // deltaf for ContactRollingFriction constraints
-
-		
-protected:
-
-	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
-	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
-
-	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
-
-public:
-
-	BT_DECLARE_ALIGNED_ALLOCATOR();
-
-	btNNCGConstraintSolver() : btSequentialImpulseConstraintSolver(), m_onlyForNoneContact(false) {}
-
-	virtual btConstraintSolverType getSolverType() const
-	{
-		return BT_NNCG_SOLVER;
-	}
-
-	bool m_onlyForNoneContact;
-};
-
-
-
-
-#endif //BT_NNCG_CONSTRAINT_SOLVER_H
-
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_NNCG_CONSTRAINT_SOLVER_H
+#define BT_NNCG_CONSTRAINT_SOLVER_H
+
+#include "btSequentialImpulseConstraintSolver.h"
+
+ATTRIBUTE_ALIGNED16(class) btNNCGConstraintSolver : public btSequentialImpulseConstraintSolver
+{
+protected:
+
+	btScalar m_deltafLengthSqrPrev;
+
+	btAlignedObjectArray<btScalar> m_pNC;  // p for None Contact constraints
+	btAlignedObjectArray<btScalar> m_pC;   // p for Contact constraints
+	btAlignedObjectArray<btScalar> m_pCF;  // p for ContactFriction constraints
+	btAlignedObjectArray<btScalar> m_pCRF; // p for ContactRollingFriction constraints
+
+	//These are recalculated in every iterations. We just keep these to prevent reallocation in each iteration.
+	btAlignedObjectArray<btScalar> m_deltafNC;  // deltaf for NoneContact constraints
+	btAlignedObjectArray<btScalar> m_deltafC;   // deltaf for Contact constraints
+	btAlignedObjectArray<btScalar> m_deltafCF;  // deltaf for ContactFriction constraints
+	btAlignedObjectArray<btScalar> m_deltafCRF; // deltaf for ContactRollingFriction constraints
+
+		
+protected:
+
+	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
+	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+
+	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btNNCGConstraintSolver() : btSequentialImpulseConstraintSolver(), m_onlyForNoneContact(false) {}
+
+	virtual btConstraintSolverType getSolverType() const
+	{
+		return BT_NNCG_SOLVER;
+	}
+
+	bool m_onlyForNoneContact;
+};
+
+
+
+
+#endif //BT_NNCG_CONSTRAINT_SOLVER_H
+

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -364,9 +364,9 @@ void	btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri
 
 
  btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
-	 :m_btSeed2(0),
-	 m_resolveSingleConstraintRowGeneric(gResolveSingleConstraintRowGeneric_scalar_reference),
-	 m_resolveSingleConstraintRowLowerLimit(gResolveSingleConstraintRowLowerLimit_scalar_reference)
+	 : m_resolveSingleConstraintRowGeneric(gResolveSingleConstraintRowGeneric_scalar_reference),
+	 m_resolveSingleConstraintRowLowerLimit(gResolveSingleConstraintRowLowerLimit_scalar_reference),
+	 m_btSeed2(0)
  {
 
 #ifdef USE_SIMD
@@ -761,8 +761,8 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 																 const btVector3& rel_pos1, const btVector3& rel_pos2)
 {
 
-			const btVector3& pos1 = cp.getPositionWorldOnA();
-			const btVector3& pos2 = cp.getPositionWorldOnB();
+		//	const btVector3& pos1 = cp.getPositionWorldOnA();
+		//	const btVector3& pos2 = cp.getPositionWorldOnB();
 
 			btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
 			btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -4,8 +4,8 @@ Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+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.
@@ -58,7 +58,7 @@ int firstHit=startHit;
 SIMD_FORCE_INLINE	int	btGetConstraintIslandId(const btTypedConstraint* lhs)
 {
 	int islandId;
-	
+
 	const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
 	const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
 	islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
@@ -88,7 +88,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 	int						m_numConstraints;
 	btIDebugDraw*			m_debugDrawer;
 	btDispatcher*			m_dispatcher;
-	
+
 	btAlignedObjectArray<btCollisionObject*> m_bodies;
 	btAlignedObjectArray<btPersistentManifold*> m_manifolds;
 	btAlignedObjectArray<btTypedConstraint*> m_constraints;
@@ -127,7 +127,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 		m_constraints.resize (0);
 	}
 
-	
+
 	virtual	void	processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold**	manifolds,int numManifolds, int islandId)
 	{
 		if (islandId<0)
@@ -140,7 +140,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 			btTypedConstraint** startConstraint = 0;
 			int numCurConstraints = 0;
 			int i;
-			
+
 			//find the first constraint for this island
 			for (i=0;i<m_numConstraints;i++)
 			{
@@ -164,7 +164,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 				m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
 			} else
 			{
-				
+
 				for (i=0;i<numBodies;i++)
 					m_bodies.push_back(bodies[i]);
 				for (i=0;i<numManifolds;i++)
@@ -187,7 +187,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 		btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
 		btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
 		btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
-			
+
 		m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_dispatcher);
 		m_bodies.resize(0);
 		m_manifolds.resize(0);
@@ -206,10 +206,10 @@ m_solverIslandCallback ( NULL ),
 m_constraintSolver(constraintSolver),
 m_gravity(0,-10,0),
 m_localTime(0),
+m_fixedTimeStep(0),
 m_synchronizeAllMotionStates(false),
 m_applySpeculativeContactRestitution(false),
 m_profileTimings(0),
-m_fixedTimeStep(0),
 m_latencyMotionStateInterpolation(true)
 
 {
@@ -332,7 +332,7 @@ void	btDiscreteDynamicsWorld::clearForces()
 		//it might break backward compatibility (people applying forces on sleeping objects get never cleared and accumulate on wake-up
 		body->clearForces();
 	}
-}	
+}
 
 ///apply gravity, call this once per timestep
 void	btDiscreteDynamicsWorld::applyGravity()
@@ -448,7 +448,7 @@ int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
 
 		applyGravity();
 
-		
+
 
 		for (int i=0;i<clampedSimulationSteps;i++)
 		{
@@ -466,18 +466,18 @@ int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
 #ifndef BT_NO_PROFILE
 	CProfileManager::Increment_Frame_Counter();
 #endif //BT_NO_PROFILE
-	
+
 	return numSimulationSubSteps;
 }
 
 void	btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 {
-	
+
 	BT_PROFILE("internalSingleStepSimulation");
 
 	if(0 != m_internalPreTickCallback) {
 		(*m_internalPreTickCallback)(this, timeStep);
-	}	
+	}
 
 	///apply gravity, predict motion
 	predictUnconstraintMotion(timeStep);
@@ -490,20 +490,20 @@ void	btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 
 
     createPredictiveContacts(timeStep);
-    
+
 	///perform collision detection
 	performDiscreteCollisionDetection();
 
 	calculateSimulationIslands();
 
-	
+
 	getSolverInfo().m_timeStep = timeStep;
-	
+
 
 
 	///solve contact and other joint constraints
 	solveConstraints(getSolverInfo());
-	
+
 	///CallbackTriggers();
 
 	///integrate transforms
@@ -512,12 +512,12 @@ void	btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 
 	///update vehicle simulation
 	updateActions(timeStep);
-	
+
 	updateActivationState( timeStep );
 
 	if(0 != m_internalTickCallback) {
 		(*m_internalTickCallback)(this, timeStep);
-	}	
+	}
 }
 
 void	btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
@@ -609,14 +609,14 @@ void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short
 void	btDiscreteDynamicsWorld::updateActions(btScalar timeStep)
 {
 	BT_PROFILE("updateActions");
-	
+
 	for ( int i=0;i<m_actions.size();i++)
 	{
 		m_actions[i]->updateAction( this, timeStep);
 	}
 }
-	
-	
+
+
 void	btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
 {
 	BT_PROFILE("updateActivationState");
@@ -637,7 +637,7 @@ void	btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
 				{
 					if (body->getActivationState() == ACTIVE_TAG)
 						body->setActivationState( WANTS_DEACTIVATION );
-					if (body->getActivationState() == ISLAND_SLEEPING) 
+					if (body->getActivationState() == ISLAND_SLEEPING)
 					{
 						body->setAngularVelocity(btVector3(0,0,0));
 						body->setLinearVelocity(btVector3(0,0,0));
@@ -710,25 +710,25 @@ void	btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character)
 void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 {
 	BT_PROFILE("solveConstraints");
-	
+
 	m_sortedConstraints.resize( m_constraints.size());
-	int i; 
+	int i;
 	for (i=0;i<getNumConstraints();i++)
 	{
 		m_sortedConstraints[i] = m_constraints[i];
 	}
 
 //	btAssert(0);
-		
-	
+
+
 
 	m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
-	
+
 	btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
-	
+
 	m_solverIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),getDebugDrawer());
 	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
-	
+
 	/// solve all the constraints for this island
 	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverIslandCallback);
 
@@ -749,10 +749,10 @@ void	btDiscreteDynamicsWorld::calculateSimulationIslands()
         for (int i=0;i<this->m_predictiveManifolds.size();i++)
         {
             btPersistentManifold* manifold = m_predictiveManifolds[i];
-            
+
             const btCollisionObject* colObj0 = manifold->getBody0();
             const btCollisionObject* colObj1 = manifold->getBody1();
-            
+
             if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
                 ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
             {
@@ -760,7 +760,7 @@ void	btDiscreteDynamicsWorld::calculateSimulationIslands()
             }
         }
     }
-    
+
 	{
 		int i;
 		int numConstraints = int(m_constraints.size());
@@ -784,7 +784,7 @@ void	btDiscreteDynamicsWorld::calculateSimulationIslands()
 	//Store the island id in each body
 	getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
 
-	
+
 }
 
 
@@ -800,7 +800,7 @@ public:
 	btDispatcher* m_dispatcher;
 
 public:
-	btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : 
+	btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
 	  btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
 		m_me(me),
 		m_allowedPenetration(0.0f),
@@ -880,7 +880,7 @@ int gNumClampedCcdMotions=0;
 void	btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
 {
 	BT_PROFILE("createPredictiveContacts");
-	
+
 	{
 		BT_PROFILE("release predictive contact manifolds");
 
@@ -902,7 +902,7 @@ void	btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
 		{
 
 			body->predictIntegratedTransform(timeStep, predictedTrans);
-			
+
 			btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
 
 			if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
@@ -916,7 +916,7 @@ void	btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
 					{
 					public:
 
-						StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : 
+						StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
 						  btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
 						{
 						}
@@ -946,14 +946,14 @@ void	btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
 					convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
 					if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
 					{
-					
+
 						btVector3 distVec = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin())*sweepResults.m_closestHitFraction;
 						btScalar distance = distVec.dot(-sweepResults.m_hitNormalWorld);
 
-						
+
 						btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body,sweepResults.m_hitCollisionObject);
 						m_predictiveManifolds.push_back(manifold);
-						
+
 						btVector3 worldPointB = body->getWorldTransform().getOrigin()+distVec;
 						btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse()*worldPointB;
 
@@ -986,10 +986,10 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 		{
 
 			body->predictIntegratedTransform(timeStep, predictedTrans);
-			
+
 			btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
 
-			
+
 
 			if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
 			{
@@ -1002,7 +1002,7 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 					{
 					public:
 
-						StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : 
+						StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
 						  btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
 						{
 						}
@@ -1032,7 +1032,7 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 					convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
 					if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
 					{
-						
+
 						//printf("clamped integration to hit fraction = %f\n",fraction);
 						body->setHitFraction(sweepResults.m_closestHitFraction);
 						body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
@@ -1057,13 +1057,13 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 							printf("sm2=%f\n",sm2);
 						}
 #else
-						
+
 						//don't apply the collision response right now, it will happen next frame
 						//if you really need to, you can uncomment next 3 lines. Note that is uses zero restitution.
 						//btScalar appliedImpulse = 0.f;
 						//btScalar depth = 0.f;
 						//appliedImpulse = resolveSingleCollision(body,(btCollisionObject*)sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth);
-						
+
 
 #endif
 
@@ -1071,10 +1071,10 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 					}
 				}
 			}
-			
+
 
 			body->proceedToTransform( predictedTrans);
-		
+
 		}
 
 	}
@@ -1088,7 +1088,7 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 			btPersistentManifold* manifold = m_predictiveManifolds[i];
 			btRigidBody* body0 = btRigidBody::upcast((btCollisionObject*)manifold->getBody0());
 			btRigidBody* body1 = btRigidBody::upcast((btCollisionObject*)manifold->getBody1());
-			
+
 			for (int p=0;p<manifold->getNumContacts();p++)
 			{
 				const btManifoldPoint& pt = manifold->getContactPoint(p);
@@ -1098,11 +1098,11 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 				//if (pt.getDistance()>0 && combinedRestitution>0 && pt.m_appliedImpulse != 0.f)
 				{
 					btVector3 imp = -pt.m_normalWorldOnB * pt.m_appliedImpulse* combinedRestitution;
-				
+
 					const btVector3& pos1 = pt.getPositionWorldOnA();
 					const btVector3& pos2 = pt.getPositionWorldOnB();
 
-					btVector3 rel_pos0 = pos1 - body0->getWorldTransform().getOrigin(); 
+					btVector3 rel_pos0 = pos1 - body0->getWorldTransform().getOrigin();
 					btVector3 rel_pos1 = pos2 - body1->getWorldTransform().getOrigin();
 
 					if (body0)
@@ -1113,7 +1113,7 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 			}
 		}
 	}
-	
+
 }
 
 
@@ -1152,7 +1152,7 @@ void	btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
 
 
 
-	
+
 
 void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 {
@@ -1172,12 +1172,12 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 				btTransform tr;
 				tr.setIdentity();
 				btVector3 pivot = p2pC->getPivotInA();
-				pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot; 
+				pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;
 				tr.setOrigin(pivot);
 				getDebugDrawer()->drawTransform(tr, dbgDrawSize);
-				// that ideally should draw the same frame	
+				// that ideally should draw the same frame
 				pivot = p2pC->getPivotInB();
-				pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot; 
+				pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot;
 				tr.setOrigin(pivot);
 				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
 			}
@@ -1196,13 +1196,13 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 					break;
 				}
 				bool drawSect = true;
-				if(minAng > maxAng)
+				if(!pHinge->hasLimit())
 				{
 					minAng = btScalar(0.f);
 					maxAng = SIMD_2_PI;
 					drawSect = false;
 				}
-				if(drawLimits) 
+				if(drawLimits)
 				{
 					btVector3& center = tr.getOrigin();
 					btVector3 normal = tr.getBasis().getColumn(2);
@@ -1237,7 +1237,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 							getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0));
 
 						pPrev = pCur;
-					}						
+					}
 					btScalar tws = pCT->getTwistSpan();
 					btScalar twa = pCT->getTwistAngle();
 					bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f));
@@ -1265,7 +1265,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
 				tr = p6DOF->getCalculatedTransformB();
 				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
-				if(drawLimits) 
+				if(drawLimits)
 				{
 					tr = p6DOF->getCalculatedTransformA();
 					const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
@@ -1328,7 +1328,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 				}
 			}
 			break;
-		default : 
+		default :
 			break;
 	}
 	return;
@@ -1428,19 +1428,19 @@ void	btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serialize
 		worldInfo->m_solverInfo.m_globalCfm = getSolverInfo().m_globalCfm;
 		worldInfo->m_solverInfo.m_splitImpulsePenetrationThreshold = getSolverInfo().m_splitImpulsePenetrationThreshold;
 		worldInfo->m_solverInfo.m_splitImpulseTurnErp = getSolverInfo().m_splitImpulseTurnErp;
-		
+
 		worldInfo->m_solverInfo.m_linearSlop = getSolverInfo().m_linearSlop;
 		worldInfo->m_solverInfo.m_warmstartingFactor = getSolverInfo().m_warmstartingFactor;
 		worldInfo->m_solverInfo.m_maxGyroscopicForce = getSolverInfo().m_maxGyroscopicForce;
 		worldInfo->m_solverInfo.m_singleAxisRollingFrictionThreshold = getSolverInfo().m_singleAxisRollingFrictionThreshold;
-		
+
 		worldInfo->m_solverInfo.m_numIterations = getSolverInfo().m_numIterations;
 		worldInfo->m_solverInfo.m_solverMode = getSolverInfo().m_solverMode;
 		worldInfo->m_solverInfo.m_restingContactRestitutionThreshold = getSolverInfo().m_restingContactRestitutionThreshold;
 		worldInfo->m_solverInfo.m_minimumSolverBatchSize = getSolverInfo().m_minimumSolverBatchSize;
-		
+
 		worldInfo->m_solverInfo.m_splitImpulse = getSolverInfo().m_splitImpulse;
-	
+
 #ifdef BT_USE_DOUBLE_PRECISION
 		const char* structType = "btDynamicsWorldDoubleData";
 #else//BT_USE_DOUBLE_PRECISION

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -87,7 +87,10 @@ btMultiBody::btMultiBody(int n_links,
                      bool fixedBase,
                      bool canSleep,
 					 bool multiDof)
-    : m_baseQuat(0, 0, 0, 1),
+    : 
+    	m_baseCollider(0),
+    	m_basePos(0,0,0),
+    	m_baseQuat(0, 0, 0, 1),
       m_baseMass(mass),
       m_baseInertia(inertia),
     
@@ -95,18 +98,19 @@ btMultiBody::btMultiBody(int n_links,
 		m_awake(true),
 		m_canSleep(canSleep),
 		m_sleepTimer(0),
-		m_baseCollider(0),
+		
 		m_linearDamping(0.04f),
 		m_angularDamping(0.04f),
 		m_useGyroTerm(true),
-		m_maxAppliedImpulse(1000.f),
+			m_maxAppliedImpulse(1000.f),
 		m_maxCoordinateVelocity(100.f),
-		m_hasSelfCollision(true),		
-		m_dofCount(0),
-		__posUpdated(false),		
+			m_hasSelfCollision(true),		
 		m_isMultiDof(multiDof),
+		__posUpdated(false),
+			m_dofCount(0),
 		m_posVarCnt(0),
-		m_useRK4(false), m_useGlobalVelocities(false)
+		m_useRK4(false), 	
+		m_useGlobalVelocities(false)
 {
 	
 	if(!m_isMultiDof)
@@ -119,7 +123,7 @@ btMultiBody::btMultiBody(int n_links,
 	m_links.resize(n_links);
 	m_matrixBuf.resize(n_links + 1);
 
-	m_basePos.setValue(0, 0, 0);
+
     m_baseForce.setValue(0, 0, 0);
     m_baseTorque.setValue(0, 0, 0);
 }
@@ -1257,7 +1261,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
 			zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_baseInertia * spatVel[i+1].getAngular()));			
 		//		
 		zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()));
-		btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
+		//btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
 		////clamp parent's omega
 		//btScalar parOmegaMod = temp.length();
 		//btScalar parOmegaModMax = 1000;
@@ -2010,7 +2014,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
 	{		
 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 		{
-			btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1]);
+//??			btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1]);
 
 			Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof]
 											- SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1])											
@@ -2352,7 +2356,7 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output
     // 'Downward' loop.
     for (int i = num_links - 1; i >= 0; --i) 
 	{
-		btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
+//		btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
         Y[i] = - SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
         Y[i] += force[6 + i];  // add joint torque
         
@@ -2437,7 +2441,7 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output
 	/////////////////
 */
 
-	int dummy = 0;
+	//int dummy = 0;
 }
 
 void btMultiBody::stepPositions(btScalar dt)

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -510,8 +510,15 @@ public:
 	void useGlobalVelocities(bool use) { m_useGlobalVelocities = use; }
 	bool isUsingGlobalVelocities() const { return m_useGlobalVelocities; }
 
-	bool __posUpdated;
-
+	bool isPosUpdated() const
+	{
+		return __posUpdated;
+	}
+	void setPosUpdated(bool updated)
+	{
+		__posUpdated = updated;
+	}
+	
 private:
     btMultiBody(const btMultiBody &);  // not implemented
     void operator=(const btMultiBody &);  // not implemented
@@ -535,6 +542,7 @@ private:
 
 	void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
 	
+	
 private:
 
 	btMultiBodyLinkCollider* m_baseCollider;//can be NULL
@@ -550,9 +558,7 @@ private:
     
     btAlignedObjectArray<btMultibodyLink> m_links;    // array of m_links, excluding the base. index from 0 to num_links-1.
 	btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders;
-	int m_dofCount, m_posVarCnt;
 
-	bool m_useRK4, m_useGlobalVelocities;
     
     //
     // realBuf:
@@ -596,6 +602,9 @@ private:
 	btScalar	m_maxCoordinateVelocity;
 	bool		m_hasSelfCollision;
 	bool		m_isMultiDof;
+		bool __posUpdated;
+		int m_dofCount, m_posVarCnt;
+	bool m_useRK4, m_useGlobalVelocities;
 };
 
 #endif

src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp

@@ -1,364 +1,366 @@
-#include "btMultiBodyConstraint.h"
-#include "BulletDynamics/Dynamics/btRigidBody.h"
-#include "btMultiBodyPoint2Point.h"				//for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
-
-btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
-	:m_bodyA(bodyA),
-	m_bodyB(bodyB),
-	m_linkA(linkA),
-	m_linkB(linkB),
-	m_numRows(numRows),
-	m_isUnilateral(isUnilateral),
-	m_maxAppliedImpulse(100),
-	m_jacSizeA(0),
-	m_jacSizeBoth(0)
-{
-	
-	if(bodyA)
-	{
-		if(bodyA->isMultiDof())
-			m_jacSizeA = (6 + bodyA->getNumDofs());
-		else
-			m_jacSizeA = (6 + bodyA->getNumLinks());
-	}
-
-	if(bodyB)
-	{
-		if(bodyB->isMultiDof())
-			m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumDofs();
-		else
-			m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumLinks();
-	}
-	else
-		m_jacSizeBoth = m_jacSizeA;
-	
-	m_posOffset = ((1 + m_jacSizeBoth)*m_numRows);
-	m_data.resize((2 + m_jacSizeBoth) * m_numRows);
-}
-
-btMultiBodyConstraint::~btMultiBodyConstraint()
-{
-}
-
-void	btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
-{
-	for (int i = 0; i < ndof; ++i) 
-		data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
-}
-
-btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstraint& solverConstraint, 
-															btMultiBodyJacobianData& data,
-															btScalar* jacOrgA, btScalar* jacOrgB,
-															const btVector3& contactNormalOnB,
-															const btVector3& posAworld, const btVector3& posBworld, 
-															btScalar posError,
-															const btContactSolverInfo& infoGlobal,
-															btScalar lowerLimit, btScalar upperLimit,
-															btScalar relaxation,
-															bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
-{
-	solverConstraint.m_multiBodyA = m_bodyA;
-	solverConstraint.m_multiBodyB = m_bodyB;
-	solverConstraint.m_linkA = m_linkA;
-	solverConstraint.m_linkB = m_linkB;	
-
-	btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
-	btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
-
-	btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
-	btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
-
-	btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
-	btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
-
-	btVector3 rel_pos1, rel_pos2;				//these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
-	if (bodyA)
-		rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
-	if (bodyB)
-		rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
-
-	if (multiBodyA)
-	{
-		const int ndofA  = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
-
-		solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
-
-		if (solverConstraint.m_deltaVelAindex <0)
-		{
-			solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
-			multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
-			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
-		} else
-		{
-			btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
-		}
-
-		//determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
-		//resize..
-		solverConstraint.m_jacAindex = data.m_jacobians.size();
-		data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
-		//copy/determine
-		if(jacOrgA)
-		{
-			for (int i=0;i<ndofA;i++)
-				data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i];
-		}
-		else
-		{
-			btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
-			if(multiBodyA->isMultiDof())
-				multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
-			else
-				multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
-		}
-
-		//determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
-		//resize..
-		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);		//=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
-		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
-		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-		//determine..
-		if(multiBodyA->isMultiDof())
-			multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
-		else
-			multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
-	}
-	else if(rb0)
-	{
-		btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
-		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
-		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
-		solverConstraint.m_contactNormal1 = contactNormalOnB;
-	}
-
-	if (multiBodyB)
-	{
-		const int ndofB  = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
-
-		solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
-		if (solverConstraint.m_deltaVelBindex <0)
-		{
-			solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
-			multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
-			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
-		}
-
-		//determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
-		//resize..
-		solverConstraint.m_jacBindex = data.m_jacobians.size();
-		data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
-		//copy/determine..
-		if(jacOrgB)
-		{
-			for (int i=0;i<ndofB;i++)
-				data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
-		}
-		else
-		{
-			if(multiBodyB->isMultiDof())
-				multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
-			else
-				multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
-		}
-
-		//determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
-		//resize..
-		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
-		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
-		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
-		//determine..
-		if(multiBodyB->isMultiDof())
-			multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
-		else
-			multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
-
-	}
-	else if(rb1)
-	{
-		btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);		
-		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
-		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
-		solverConstraint.m_contactNormal2 = -contactNormalOnB;
-	}
-	{
-						
-		btVector3 vec;
-		btScalar denom0 = 0.f;
-		btScalar denom1 = 0.f;
-		btScalar* jacB = 0;
-		btScalar* jacA = 0;
-		btScalar* deltaVelA = 0;
-		btScalar* deltaVelB = 0;
-		int ndofA  = 0;
-		//determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
-		if (multiBodyA)
-		{
-			ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
-			jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
-			deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-			for (int i = 0; i < ndofA; ++i)
-			{
-				btScalar j = jacA[i] ;
-				btScalar l = deltaVelA[i];
-				denom0 += j*l;
-			}
-		}
-		else if(rb0)
-		{
-			vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
-			denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
-		}
-		//
-		if (multiBodyB)
-		{
-			const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
-			jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
-			deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
-			for (int i = 0; i < ndofB; ++i)
-			{
-				btScalar j = jacB[i] ;
-				btScalar l = deltaVelB[i];
-				denom1 += j*l;
-			}
-
-		}
-		else if(rb1)
-		{
-			vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
-			denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
-		}
-		//determine the "effective mass" of the constrained multibodyB with respect to this 1D constraint (i.e. 1/A[i,i])
-		if (multiBodyA && (multiBodyA==multiBodyB))
-		{
-			// ndof1 == ndof2 in this case
-			for (int i = 0; i < ndofA; ++i) 
-			{
-				denom1 += jacB[i] * deltaVelA[i];
-				denom1 += jacA[i] * deltaVelB[i];
-			}
-		}
-		//
-		btScalar d = denom0+denom1;
-		if (btFabs(d)>SIMD_EPSILON)
-		{
-			 
-			solverConstraint.m_jacDiagABInv = relaxation/(d);
-		}
-		else
-		{
-			solverConstraint.m_jacDiagABInv  = 1.f;
-		}		
-	}
-
-	
-	//compute rhs and remaining solverConstraint fields
-	btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
-
-	btScalar rel_vel = 0.f;
-	int ndofA  = 0;
-	int ndofB  = 0;
-	{
-		btVector3 vel1,vel2;
-		if (multiBodyA)
-		{
-			ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
-			btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
-			for (int i = 0; i < ndofA ; ++i) 
-				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
-		}
-		else if(rb0)
-		{
-			rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
-		}
-		if (multiBodyB)
-		{
-			ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
-			btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
-			for (int i = 0; i < ndofB ; ++i) 
-				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
-
-		}
-		else if(rb1)
-		{
-			rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
-		}
-
-		solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
-	}
-
-
-	///warm starting (or zero if disabled)
-	/*
-	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-	{
-		solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
-
-		if (solverConstraint.m_appliedImpulse)
-		{
-			if (multiBodyA)
-			{
-				btScalar impulse = solverConstraint.m_appliedImpulse;
-				btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-				multiBodyA->applyDeltaVee(deltaV,impulse);
-				applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
-			} else
-			{
-				if (rb0)
-					bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
-			}
-			if (multiBodyB)
-			{
-				btScalar impulse = solverConstraint.m_appliedImpulse;
-				btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
-				multiBodyB->applyDeltaVee(deltaV,impulse);
-				applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
-			} else
-			{
-				if (rb1)
-					bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
-			}
-		}
-	} else
-	*/
-
-	solverConstraint.m_appliedImpulse = 0.f;
-	solverConstraint.m_appliedPushImpulse = 0.f;
-
-	{	
-
-		btScalar positionalError = 0.f;
-		btScalar	velocityError = desiredVelocity - rel_vel;// * damping;
-					
-
-		btScalar erp = infoGlobal.m_erp2;
-		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
-		{
-			erp = infoGlobal.m_erp;
-		}
-
-		positionalError = -penetration * erp/infoGlobal.m_timeStep;
-
-		btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
-		btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
-
-		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
-		{
-			//combine position and velocity into rhs
-			solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
-			solverConstraint.m_rhsPenetration = 0.f;
-
-		} else
-		{
-			//split position and velocity into rhs and m_rhsPenetration
-			solverConstraint.m_rhs = velocityImpulse;
-			solverConstraint.m_rhsPenetration = penetrationImpulse;
-		}
-
-		solverConstraint.m_cfm = 0.f;
-		solverConstraint.m_lowerLimit = lowerLimit;
-		solverConstraint.m_upperLimit = upperLimit;
-	}
-
-	return rel_vel;
-
-}
+#include "btMultiBodyConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "btMultiBodyPoint2Point.h"				//for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
+
+
+	
+btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
+	:m_bodyA(bodyA),
+	m_bodyB(bodyB),
+	m_linkA(linkA),
+	m_linkB(linkB),
+	m_numRows(numRows),
+	m_jacSizeA(0),
+	m_jacSizeBoth(0),
+	m_isUnilateral(isUnilateral),
+	m_maxAppliedImpulse(100)
+{
+	
+	if(bodyA)
+	{
+		if(bodyA->isMultiDof())
+			m_jacSizeA = (6 + bodyA->getNumDofs());
+		else
+			m_jacSizeA = (6 + bodyA->getNumLinks());
+	}
+
+	if(bodyB)
+	{
+		if(bodyB->isMultiDof())
+			m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumDofs();
+		else
+			m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumLinks();
+	}
+	else
+		m_jacSizeBoth = m_jacSizeA;
+	
+	m_posOffset = ((1 + m_jacSizeBoth)*m_numRows);
+	m_data.resize((2 + m_jacSizeBoth) * m_numRows);
+}
+
+btMultiBodyConstraint::~btMultiBodyConstraint()
+{
+}
+
+void	btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
+{
+	for (int i = 0; i < ndof; ++i) 
+		data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
+}
+
+btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstraint& solverConstraint, 
+															btMultiBodyJacobianData& data,
+															btScalar* jacOrgA, btScalar* jacOrgB,
+															const btVector3& contactNormalOnB,
+															const btVector3& posAworld, const btVector3& posBworld, 
+															btScalar posError,
+															const btContactSolverInfo& infoGlobal,
+															btScalar lowerLimit, btScalar upperLimit,
+															btScalar relaxation,
+															bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
+{
+	solverConstraint.m_multiBodyA = m_bodyA;
+	solverConstraint.m_multiBodyB = m_bodyB;
+	solverConstraint.m_linkA = m_linkA;
+	solverConstraint.m_linkB = m_linkB;	
+
+	btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
+	btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
+
+	btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
+	btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
+
+	btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
+	btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
+
+	btVector3 rel_pos1, rel_pos2;				//these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
+	if (bodyA)
+		rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
+	if (bodyB)
+		rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
+
+	if (multiBodyA)
+	{
+		const int ndofA  = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
+
+		solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
+
+		if (solverConstraint.m_deltaVelAindex <0)
+		{
+			solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
+			multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
+			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
+		} else
+		{
+			btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
+		}
+
+		//determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
+		//resize..
+		solverConstraint.m_jacAindex = data.m_jacobians.size();
+		data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
+		//copy/determine
+		if(jacOrgA)
+		{
+			for (int i=0;i<ndofA;i++)
+				data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i];
+		}
+		else
+		{
+			btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
+			if(multiBodyA->isMultiDof())
+				multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+			else
+				multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+		}
+
+		//determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
+		//resize..
+		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);		//=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
+		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
+		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+		//determine..
+		if(multiBodyA->isMultiDof())
+			multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
+		else
+			multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
+	}
+	else if(rb0)
+	{
+		btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
+		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
+		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+		solverConstraint.m_contactNormal1 = contactNormalOnB;
+	}
+
+	if (multiBodyB)
+	{
+		const int ndofB  = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
+
+		solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
+		if (solverConstraint.m_deltaVelBindex <0)
+		{
+			solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
+			multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
+			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
+		}
+
+		//determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
+		//resize..
+		solverConstraint.m_jacBindex = data.m_jacobians.size();
+		data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
+		//copy/determine..
+		if(jacOrgB)
+		{
+			for (int i=0;i<ndofB;i++)
+				data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
+		}
+		else
+		{
+			if(multiBodyB->isMultiDof())
+				multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
+			else
+				multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
+		}
+
+		//determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
+		//resize..
+		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
+		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
+		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+		//determine..
+		if(multiBodyB->isMultiDof())
+			multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
+		else
+			multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
+
+	}
+	else if(rb1)
+	{
+		btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);		
+		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
+		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+		solverConstraint.m_contactNormal2 = -contactNormalOnB;
+	}
+	{
+						
+		btVector3 vec;
+		btScalar denom0 = 0.f;
+		btScalar denom1 = 0.f;
+		btScalar* jacB = 0;
+		btScalar* jacA = 0;
+		btScalar* deltaVelA = 0;
+		btScalar* deltaVelB = 0;
+		int ndofA  = 0;
+		//determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
+		if (multiBodyA)
+		{
+			ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
+			jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
+			deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+			for (int i = 0; i < ndofA; ++i)
+			{
+				btScalar j = jacA[i] ;
+				btScalar l = deltaVelA[i];
+				denom0 += j*l;
+			}
+		}
+		else if(rb0)
+		{
+			vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+			denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
+		}
+		//
+		if (multiBodyB)
+		{
+			const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
+			jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
+			deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+			for (int i = 0; i < ndofB; ++i)
+			{
+				btScalar j = jacB[i] ;
+				btScalar l = deltaVelB[i];
+				denom1 += j*l;
+			}
+
+		}
+		else if(rb1)
+		{
+			vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
+			denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
+		}
+		//determine the "effective mass" of the constrained multibodyB with respect to this 1D constraint (i.e. 1/A[i,i])
+		if (multiBodyA && (multiBodyA==multiBodyB))
+		{
+			// ndof1 == ndof2 in this case
+			for (int i = 0; i < ndofA; ++i) 
+			{
+				denom1 += jacB[i] * deltaVelA[i];
+				denom1 += jacA[i] * deltaVelB[i];
+			}
+		}
+		//
+		btScalar d = denom0+denom1;
+		if (btFabs(d)>SIMD_EPSILON)
+		{
+			 
+			solverConstraint.m_jacDiagABInv = relaxation/(d);
+		}
+		else
+		{
+			solverConstraint.m_jacDiagABInv  = 1.f;
+		}		
+	}
+
+	
+	//compute rhs and remaining solverConstraint fields
+	btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
+
+	btScalar rel_vel = 0.f;
+	int ndofA  = 0;
+	int ndofB  = 0;
+	{
+		btVector3 vel1,vel2;
+		if (multiBodyA)
+		{
+			ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
+			btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
+			for (int i = 0; i < ndofA ; ++i) 
+				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
+		}
+		else if(rb0)
+		{
+			rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
+		}
+		if (multiBodyB)
+		{
+			ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
+			btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
+			for (int i = 0; i < ndofB ; ++i) 
+				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
+
+		}
+		else if(rb1)
+		{
+			rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
+		}
+
+		solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
+	}
+
+
+	///warm starting (or zero if disabled)
+	/*
+	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+	{
+		solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
+
+		if (solverConstraint.m_appliedImpulse)
+		{
+			if (multiBodyA)
+			{
+				btScalar impulse = solverConstraint.m_appliedImpulse;
+				btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+				multiBodyA->applyDeltaVee(deltaV,impulse);
+				applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
+			} else
+			{
+				if (rb0)
+					bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
+			}
+			if (multiBodyB)
+			{
+				btScalar impulse = solverConstraint.m_appliedImpulse;
+				btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+				multiBodyB->applyDeltaVee(deltaV,impulse);
+				applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
+			} else
+			{
+				if (rb1)
+					bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
+			}
+		}
+	} else
+	*/
+
+	solverConstraint.m_appliedImpulse = 0.f;
+	solverConstraint.m_appliedPushImpulse = 0.f;
+
+	{	
+
+		btScalar positionalError = 0.f;
+		btScalar	velocityError = desiredVelocity - rel_vel;// * damping;
+					
+
+		btScalar erp = infoGlobal.m_erp2;
+		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+		{
+			erp = infoGlobal.m_erp;
+		}
+
+		positionalError = -penetration * erp/infoGlobal.m_timeStep;
+
+		btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
+		btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+
+		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+		{
+			//combine position and velocity into rhs
+			solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+			solverConstraint.m_rhsPenetration = 0.f;
+
+		} else
+		{
+			//split position and velocity into rhs and m_rhsPenetration
+			solverConstraint.m_rhs = velocityImpulse;
+			solverConstraint.m_rhsPenetration = penetrationImpulse;
+		}
+
+		solverConstraint.m_cfm = 0.f;
+		solverConstraint.m_lowerLimit = lowerLimit;
+		solverConstraint.m_upperLimit = upperLimit;
+	}
+
+	return rel_vel;
+
+}

src/BulletDynamics/Featherstone/btMultiBodyConstraint.h

@@ -1,160 +1,160 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef BT_MULTIBODY_CONSTRAINT_H
-#define BT_MULTIBODY_CONSTRAINT_H
-
-#include "LinearMath/btScalar.h"
-#include "LinearMath/btAlignedObjectArray.h"
-#include "btMultiBody.h"
-
-class btMultiBody;
-struct btSolverInfo;
-
-#include "btMultiBodySolverConstraint.h"
-
-struct btMultiBodyJacobianData
-{
-	btAlignedObjectArray<btScalar>		m_jacobians;
-	btAlignedObjectArray<btScalar>		m_deltaVelocitiesUnitImpulse;	//holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension
-	btAlignedObjectArray<btScalar>		m_deltaVelocities;				//holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI
-	btAlignedObjectArray<btScalar>		scratch_r;
-	btAlignedObjectArray<btVector3>		scratch_v;
-	btAlignedObjectArray<btMatrix3x3>	scratch_m;
-	btAlignedObjectArray<btSolverBody>*	m_solverBodyPool;
-	int									m_fixedBodyId;
-
-};
-
-
-class btMultiBodyConstraint
-{
-protected:
-
-	btMultiBody*	m_bodyA;
-    btMultiBody*	m_bodyB;
-    int				m_linkA;
-    int				m_linkB;
-
-    int				m_numRows;
-    int				m_jacSizeA;
-    int				m_jacSizeBoth;
-    int				m_posOffset;
-
-	bool			m_isUnilateral;
-
-	btScalar		m_maxAppliedImpulse;
-
-
-    // data block laid out as follows:
-    // cached impulses. (one per row.)
-    // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc)
-    // positions. (one per row.)
-    btAlignedObjectArray<btScalar> m_data;
-
-	void	applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof);
-
-	btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, 
-																btMultiBodyJacobianData& data,
-																btScalar* jacOrgA, btScalar* jacOrgB,
-																const btVector3& contactNormalOnB,
-																const btVector3& posAworld, const btVector3& posBworld, 
-																btScalar posError,
-																const btContactSolverInfo& infoGlobal,
-																btScalar lowerLimit, btScalar upperLimit,
-																btScalar relaxation = 1.f,
-																bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0);
-
-public:
-
-	btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral);
-	virtual ~btMultiBodyConstraint();
-
-
-
-	virtual int getIslandIdA() const =0;
-	virtual int getIslandIdB() const =0;
-	
-	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
-		btMultiBodyJacobianData& data,
-		const btContactSolverInfo& infoGlobal)=0;
-
-	int	getNumRows() const
-	{
-		return m_numRows;
-	}
-
-	btMultiBody*	getMultiBodyA()
-	{
-		return m_bodyA;
-	}
-    btMultiBody*	getMultiBodyB()
-	{
-		return m_bodyB;
-	}
-
-	// current constraint position
-    // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
-    // NOTE: ignored position for friction rows.
-    btScalar getPosition(int row) const 
-	{ 
-		return m_data[m_posOffset + row]; 
-	}
-
-    void setPosition(int row, btScalar pos) 
-	{ 
-		m_data[m_posOffset + row] = pos; 
-	}
-
-	
-	bool isUnilateral() const
-	{
-		return m_isUnilateral;
-	}
-
-	// jacobian blocks.
-    // each of size 6 + num_links. (jacobian2 is null if no body2.)
-    // format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients.
-    btScalar* jacobianA(int row) 
-	{ 
-		return &m_data[m_numRows + row * m_jacSizeBoth]; 
-	}
-    const btScalar* jacobianA(int row) const 
-	{ 
-		return &m_data[m_numRows + (row * m_jacSizeBoth)]; 
-	}
-    btScalar* jacobianB(int row) 
-	{ 
-		return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; 
-	}
-    const btScalar* jacobianB(int row) const 
-	{ 
-		return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; 
-	}
-
-	btScalar	getMaxAppliedImpulse() const
-	{
-		return m_maxAppliedImpulse;
-	}
-	void	setMaxAppliedImpulse(btScalar maxImp)
-	{
-		m_maxAppliedImpulse = maxImp;
-	}
-	
-
-};
-
-#endif //BT_MULTIBODY_CONSTRAINT_H
-
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_MULTIBODY_CONSTRAINT_H
+#define BT_MULTIBODY_CONSTRAINT_H
+
+#include "LinearMath/btScalar.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "btMultiBody.h"
+
+class btMultiBody;
+struct btSolverInfo;
+
+#include "btMultiBodySolverConstraint.h"
+
+struct btMultiBodyJacobianData
+{
+	btAlignedObjectArray<btScalar>		m_jacobians;
+	btAlignedObjectArray<btScalar>		m_deltaVelocitiesUnitImpulse;	//holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension
+	btAlignedObjectArray<btScalar>		m_deltaVelocities;				//holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI
+	btAlignedObjectArray<btScalar>		scratch_r;
+	btAlignedObjectArray<btVector3>		scratch_v;
+	btAlignedObjectArray<btMatrix3x3>	scratch_m;
+	btAlignedObjectArray<btSolverBody>*	m_solverBodyPool;
+	int									m_fixedBodyId;
+
+};
+
+
+class btMultiBodyConstraint
+{
+protected:
+
+	btMultiBody*	m_bodyA;
+    btMultiBody*	m_bodyB;
+    int				m_linkA;
+    int				m_linkB;
+
+    int				m_numRows;
+    int				m_jacSizeA;
+    int				m_jacSizeBoth;
+    int				m_posOffset;
+
+	bool			m_isUnilateral;
+
+	btScalar		m_maxAppliedImpulse;
+
+
+    // data block laid out as follows:
+    // cached impulses. (one per row.)
+    // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc)
+    // positions. (one per row.)
+    btAlignedObjectArray<btScalar> m_data;
+
+	void	applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof);
+
+	btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, 
+																btMultiBodyJacobianData& data,
+																btScalar* jacOrgA, btScalar* jacOrgB,
+																const btVector3& contactNormalOnB,
+																const btVector3& posAworld, const btVector3& posBworld, 
+																btScalar posError,
+																const btContactSolverInfo& infoGlobal,
+																btScalar lowerLimit, btScalar upperLimit,
+																btScalar relaxation = 1.f,
+																bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0);
+
+public:
+
+	btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral);
+	virtual ~btMultiBodyConstraint();
+
+
+
+	virtual int getIslandIdA() const =0;
+	virtual int getIslandIdB() const =0;
+	
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+		btMultiBodyJacobianData& data,
+		const btContactSolverInfo& infoGlobal)=0;
+
+	int	getNumRows() const
+	{
+		return m_numRows;
+	}
+
+	btMultiBody*	getMultiBodyA()
+	{
+		return m_bodyA;
+	}
+    btMultiBody*	getMultiBodyB()
+	{
+		return m_bodyB;
+	}
+
+	// current constraint position
+    // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
+    // NOTE: ignored position for friction rows.
+    btScalar getPosition(int row) const 
+	{ 
+		return m_data[m_posOffset + row]; 
+	}
+
+    void setPosition(int row, btScalar pos) 
+	{ 
+		m_data[m_posOffset + row] = pos; 
+	}
+
+	
+	bool isUnilateral() const
+	{
+		return m_isUnilateral;
+	}
+
+	// jacobian blocks.
+    // each of size 6 + num_links. (jacobian2 is null if no body2.)
+    // format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients.
+    btScalar* jacobianA(int row) 
+	{ 
+		return &m_data[m_numRows + row * m_jacSizeBoth]; 
+	}
+    const btScalar* jacobianA(int row) const 
+	{ 
+		return &m_data[m_numRows + (row * m_jacSizeBoth)]; 
+	}
+    btScalar* jacobianB(int row) 
+	{ 
+		return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; 
+	}
+    const btScalar* jacobianB(int row) const 
+	{ 
+		return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; 
+	}
+
+	btScalar	getMaxAppliedImpulse() const
+	{
+		return m_maxAppliedImpulse;
+	}
+	void	setMaxAppliedImpulse(btScalar maxImp)
+	{
+		m_maxAppliedImpulse = maxImp;
+	}
+	
+
+};
+
+#endif //BT_MULTIBODY_CONSTRAINT_H
+

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h

@@ -1,86 +1,86 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef BT_MULTIBODY_CONSTRAINT_SOLVER_H
-#define BT_MULTIBODY_CONSTRAINT_SOLVER_H
-
-#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
-#include "btMultiBodySolverConstraint.h"
-
-
-class btMultiBody;
-
-#include "btMultiBodyConstraint.h"
-
-
-
-ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver
-{
-
-protected:
-
-	btMultiBodyConstraintArray			m_multiBodyNonContactConstraints;
-
-	btMultiBodyConstraintArray			m_multiBodyNormalContactConstraints;
-	btMultiBodyConstraintArray			m_multiBodyFrictionContactConstraints;
-
-	btMultiBodyJacobianData				m_data;
-	
-	//temp storage for multi body constraints for a specific island/group called by 'solveGroup'
-	btMultiBodyConstraint**					m_tmpMultiBodyConstraints;
-	int										m_tmpNumMultiBodyConstraints;
-
-	void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
-	void resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c);
-
-	void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);
-	btMultiBodySolverConstraint&	addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
-
-
-	void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, 
-																 btScalar* jacA,btScalar* jacB,
-																 btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
-																 const btContactSolverInfo& infoGlobal);
-
-	void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, 
-																 const btVector3& contactNormal,
-																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
-																 btScalar& relaxation,
-																 bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
-
-	void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
-	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
-//	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
-
-	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
-	void	applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof);
-
-public:
-
-	BT_DECLARE_ALIGNED_ALLOCATOR();
-
-	///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints)
-	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
-	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
-	
-	virtual void solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
-};
-
-	
-	
-
-
-#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H
-
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_MULTIBODY_CONSTRAINT_SOLVER_H
+#define BT_MULTIBODY_CONSTRAINT_SOLVER_H
+
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "btMultiBodySolverConstraint.h"
+
+
+class btMultiBody;
+
+#include "btMultiBodyConstraint.h"
+
+
+
+ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver
+{
+
+protected:
+
+	btMultiBodyConstraintArray			m_multiBodyNonContactConstraints;
+
+	btMultiBodyConstraintArray			m_multiBodyNormalContactConstraints;
+	btMultiBodyConstraintArray			m_multiBodyFrictionContactConstraints;
+
+	btMultiBodyJacobianData				m_data;
+	
+	//temp storage for multi body constraints for a specific island/group called by 'solveGroup'
+	btMultiBodyConstraint**					m_tmpMultiBodyConstraints;
+	int										m_tmpNumMultiBodyConstraints;
+
+	void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
+	void resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c);
+
+	void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);
+	btMultiBodySolverConstraint&	addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
+
+
+	void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, 
+																 btScalar* jacA,btScalar* jacB,
+																 btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
+																 const btContactSolverInfo& infoGlobal);
+
+	void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, 
+																 const btVector3& contactNormal,
+																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
+																 btScalar& relaxation,
+																 bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
+
+	void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
+	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+//	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+
+	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+	void	applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof);
+
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints)
+	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
+	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
+	
+	virtual void solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
+};
+
+	
+	
+
+
+#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H
+

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

@@ -1,56 +1,56 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef BT_MULTIBODY_DYNAMICS_WORLD_H
-#define BT_MULTIBODY_DYNAMICS_WORLD_H
-
-#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
-
-
-class btMultiBody;
-class btMultiBodyConstraint;
-class btMultiBodyConstraintSolver;
-struct MultiBodyInplaceSolverIslandCallback;
-
-///The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet
-///This implementation is still preliminary/experimental.
-class btMultiBodyDynamicsWorld : public btDiscreteDynamicsWorld
-{
-protected:
-	btAlignedObjectArray<btMultiBody*> m_multiBodies;
-	btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
-	btAlignedObjectArray<btMultiBodyConstraint*> m_sortedMultiBodyConstraints;
-	btMultiBodyConstraintSolver*	m_multiBodyConstraintSolver;
-	MultiBodyInplaceSolverIslandCallback*	m_solverMultiBodyIslandCallback;
-
-	virtual void	calculateSimulationIslands();
-	virtual void	updateActivationState(btScalar timeStep);
-	virtual void	solveConstraints(btContactSolverInfo& solverInfo);
-	virtual void	integrateTransforms(btScalar timeStep);
-public:
-
-	btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
-	
-	virtual ~btMultiBodyDynamicsWorld ();
-
-	virtual void	addMultiBody(btMultiBody* body, short group= btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter);
-
-	virtual void	removeMultiBody(btMultiBody* body);
-
-	virtual void	addMultiBodyConstraint( btMultiBodyConstraint* constraint);
-
-	virtual void	removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
-};
-#endif //BT_MULTIBODY_DYNAMICS_WORLD_H
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_MULTIBODY_DYNAMICS_WORLD_H
+#define BT_MULTIBODY_DYNAMICS_WORLD_H
+
+#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
+
+
+class btMultiBody;
+class btMultiBodyConstraint;
+class btMultiBodyConstraintSolver;
+struct MultiBodyInplaceSolverIslandCallback;
+
+///The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet
+///This implementation is still preliminary/experimental.
+class btMultiBodyDynamicsWorld : public btDiscreteDynamicsWorld
+{
+protected:
+	btAlignedObjectArray<btMultiBody*> m_multiBodies;
+	btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
+	btAlignedObjectArray<btMultiBodyConstraint*> m_sortedMultiBodyConstraints;
+	btMultiBodyConstraintSolver*	m_multiBodyConstraintSolver;
+	MultiBodyInplaceSolverIslandCallback*	m_solverMultiBodyIslandCallback;
+
+	virtual void	calculateSimulationIslands();
+	virtual void	updateActivationState(btScalar timeStep);
+	virtual void	solveConstraints(btContactSolverInfo& solverInfo);
+	virtual void	integrateTransforms(btScalar timeStep);
+public:
+
+	btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+	
+	virtual ~btMultiBodyDynamicsWorld ();
+
+	virtual void	addMultiBody(btMultiBody* body, short group= btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter);
+
+	virtual void	removeMultiBody(btMultiBody* body);
+
+	virtual void	addMultiBodyConstraint( btMultiBodyConstraint* constraint);
+
+	virtual void	removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
+};
+#endif //BT_MULTIBODY_DYNAMICS_WORLD_H

src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp

@@ -1,145 +1,145 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-///This file was written by Erwin Coumans
-
-#include "btMultiBodyJointLimitConstraint.h"
-#include "btMultiBody.h"
-#include "btMultiBodyLinkCollider.h"
-#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
-
-
-btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
-	//:btMultiBodyConstraint(body,0,link,-1,2,true),
-	:btMultiBodyConstraint(body,body,link,link,2,true),
-	m_lowerBound(lower),
-	m_upperBound(upper)
-{
-	// the data.m_jacobians never change, so may as well
-    // initialize them here
-        
-    // note: we rely on the fact that data.m_jacobians are
-    // always initialized to zero by the Constraint ctor
-
-	unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset : link);
-
-	// row 0: the lower bound
-	jacobianA(0)[offset] = 1;
-	// row 1: the upper bound
-	//jacobianA(1)[offset] = -1;
-
-	jacobianB(1)[offset] = -1;
-}
-btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
-{
-}
-
-int btMultiBodyJointLimitConstraint::getIslandIdA() const
-{
-	if(m_bodyA)
-	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
-		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
-		}
-	}
-	return -1;
-}
-
-int btMultiBodyJointLimitConstraint::getIslandIdB() const
-{
-	if(m_bodyB)
-	{
-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
-		{
-			col = m_bodyB->getLink(i).m_collider;
-			if (col)
-				return col->getIslandTag();
-		}
-	}
-	return -1;
-}
-
-
-void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
-		btMultiBodyJacobianData& data,
-		const btContactSolverInfo& infoGlobal)
-{
-    // only positions need to be updated -- data.m_jacobians and force
-    // directions were set in the ctor and never change.
-    
-    // row 0: the lower bound
-    setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound);			//multidof: this is joint-type dependent
-
-    // row 1: the upper bound
-    setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
-
-	for (int row=0;row<getNumRows();row++)
-	{
-		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
-		constraintRow.m_multiBodyA = m_bodyA;
-		constraintRow.m_multiBodyB = m_bodyB;
-		const btScalar posError = 0;						//why assume it's zero?
-		const btVector3 dummy(0, 0, 0);
-		
-		btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
-		{
-			btScalar penetration = getPosition(row);
-			btScalar positionalError = 0.f;
-			btScalar	velocityError =  - rel_vel;// * damping;
-			btScalar erp = infoGlobal.m_erp2;
-			if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
-			{
-				erp = infoGlobal.m_erp;
-			}
-			if (penetration>0)
-			{
-				positionalError = 0;
-				velocityError = -penetration / infoGlobal.m_timeStep;
-			} else
-			{
-				positionalError = -penetration * erp/infoGlobal.m_timeStep;
-			}
-
-			btScalar  penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
-			btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
-			if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
-			{
-				//combine position and velocity into rhs
-				constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
-				constraintRow.m_rhsPenetration = 0.f;
-
-			} else
-			{
-				//split position and velocity into rhs and m_rhsPenetration
-				constraintRow.m_rhs = velocityImpulse;
-				constraintRow.m_rhsPenetration = penetrationImpulse;
-			}
-		}
-	}
-
-}
-	
-	
-	
-
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodyJointLimitConstraint.h"
+#include "btMultiBody.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+
+btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
+	//:btMultiBodyConstraint(body,0,link,-1,2,true),
+	:btMultiBodyConstraint(body,body,link,link,2,true),
+	m_lowerBound(lower),
+	m_upperBound(upper)
+{
+	// the data.m_jacobians never change, so may as well
+    // initialize them here
+        
+    // note: we rely on the fact that data.m_jacobians are
+    // always initialized to zero by the Constraint ctor
+
+	unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset : link);
+
+	// row 0: the lower bound
+	jacobianA(0)[offset] = 1;
+	// row 1: the upper bound
+	//jacobianA(1)[offset] = -1;
+
+	jacobianB(1)[offset] = -1;
+}
+btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
+{
+}
+
+int btMultiBodyJointLimitConstraint::getIslandIdA() const
+{
+	if(m_bodyA)
+	{
+		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+		for (int i=0;i<m_bodyA->getNumLinks();i++)
+		{
+			if (m_bodyA->getLink(i).m_collider)
+				return m_bodyA->getLink(i).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodyJointLimitConstraint::getIslandIdB() const
+{
+	if(m_bodyB)
+	{
+		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+
+		for (int i=0;i<m_bodyB->getNumLinks();i++)
+		{
+			col = m_bodyB->getLink(i).m_collider;
+			if (col)
+				return col->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+
+void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+		btMultiBodyJacobianData& data,
+		const btContactSolverInfo& infoGlobal)
+{
+    // only positions need to be updated -- data.m_jacobians and force
+    // directions were set in the ctor and never change.
+    
+    // row 0: the lower bound
+    setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound);			//multidof: this is joint-type dependent
+
+    // row 1: the upper bound
+    setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
+
+	for (int row=0;row<getNumRows();row++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+		constraintRow.m_multiBodyA = m_bodyA;
+		constraintRow.m_multiBodyB = m_bodyB;
+		const btScalar posError = 0;						//why assume it's zero?
+		const btVector3 dummy(0, 0, 0);
+		
+		btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
+		{
+			btScalar penetration = getPosition(row);
+			btScalar positionalError = 0.f;
+			btScalar	velocityError =  - rel_vel;// * damping;
+			btScalar erp = infoGlobal.m_erp2;
+			if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+			{
+				erp = infoGlobal.m_erp;
+			}
+			if (penetration>0)
+			{
+				positionalError = 0;
+				velocityError = -penetration / infoGlobal.m_timeStep;
+			} else
+			{
+				positionalError = -penetration * erp/infoGlobal.m_timeStep;
+			}
+
+			btScalar  penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
+			btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
+			if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+			{
+				//combine position and velocity into rhs
+				constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
+				constraintRow.m_rhsPenetration = 0.f;
+
+			} else
+			{
+				//split position and velocity into rhs and m_rhsPenetration
+				constraintRow.m_rhs = velocityImpulse;
+				constraintRow.m_rhsPenetration = penetrationImpulse;
+			}
+		}
+	}
+
+}
+	
+	
+	
+

src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h

@@ -1,44 +1,44 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
-#define BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
-
-#include "btMultiBodyConstraint.h"
-struct btSolverInfo;
-
-class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint
-{
-protected:
-
-	btScalar	m_lowerBound;
-	btScalar	m_upperBound;
-public:
-
-	btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper);
-	virtual ~btMultiBodyJointLimitConstraint();
-
-	virtual int getIslandIdA() const;
-	virtual int getIslandIdB() const;
-
-	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
-		btMultiBodyJacobianData& data,
-		const btContactSolverInfo& infoGlobal);
-	
-	
-};
-
-#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
-
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
+#define BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
+
+#include "btMultiBodyConstraint.h"
+struct btSolverInfo;
+
+class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint
+{
+protected:
+
+	btScalar	m_lowerBound;
+	btScalar	m_upperBound;
+public:
+
+	btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper);
+	virtual ~btMultiBodyJointLimitConstraint();
+
+	virtual int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+		btMultiBodyJacobianData& data,
+		const btContactSolverInfo& infoGlobal);
+	
+	
+};
+
+#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
+

src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp

@@ -110,7 +110,7 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
 	{
 		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
 		
-		btScalar penetration = 0;
+		
 		fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
 	}
 

src/BulletDynamics/Featherstone/btMultiBodyLink.h

@@ -381,8 +381,7 @@ struct btMultibodyLink
 		eInvalid
 	};
 
-	eFeatherstoneJointType m_jointType;
-	int m_dofCount, m_posVarCount;				//redundant but handy
+	
 
 	// "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
     // for prismatic: m_axesTop[0] = zero;
@@ -429,6 +428,10 @@ struct btMultibodyLink
 
 	class btMultiBodyLinkCollider* m_collider;
 	int m_flags;
+	
+	
+	int m_dofCount, m_posVarCount;				//redundant but handy
+	eFeatherstoneJointType m_jointType;
 
     // ctor: set some sensible defaults
 	btMultibodyLink()

src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h

@@ -1,92 +1,92 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef BT_FEATHERSTONE_LINK_COLLIDER_H
-#define BT_FEATHERSTONE_LINK_COLLIDER_H
-
-#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
-
-#include "btMultiBody.h"
-
-class btMultiBodyLinkCollider : public btCollisionObject
-{
-//protected:
-public:
-
-	btMultiBody* m_multiBody;
-	int m_link;
-
-
-	btMultiBodyLinkCollider (btMultiBody* multiBody,int link)
-		:m_multiBody(multiBody),
-		m_link(link)
-	{
-		m_checkCollideWith =  true;
-		//we need to remove the 'CF_STATIC_OBJECT' flag, otherwise links/base doesn't merge islands
-		//this means that some constraints might point to bodies that are not in the islands, causing crashes
-		//if (link>=0 || (multiBody && !multiBody->hasFixedBase()))
-		{
-			m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
-		}
-		// else
-		//{
-		//	m_collisionFlags |= (btCollisionObject::CF_STATIC_OBJECT);
-		//}
-
-		m_internalType = CO_FEATHERSTONE_LINK;
-	}
-	static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj)
-	{
-		if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
-			return (btMultiBodyLinkCollider*)colObj;
-		return 0;
-	}
-	static const btMultiBodyLinkCollider* upcast(const btCollisionObject* colObj)
-	{
-		if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
-			return (btMultiBodyLinkCollider*)colObj;
-		return 0;
-	}
-
-	virtual bool checkCollideWithOverride(const  btCollisionObject* co) const
-	{
-		const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co);
-		if (!other)
-			return true;
-		if (other->m_multiBody != this->m_multiBody)
-			return true;
-		if (!m_multiBody->hasSelfCollision())
-			return false;
-
-		//check if 'link' has collision disabled
-		if (m_link>=0)
-		{
-			const btMultibodyLink& link = m_multiBody->getLink(this->m_link);
-			if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.m_parent == other->m_link)
-				return false;
-		}
-		
-		if (other->m_link>=0)
-		{
-			const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link);
-			if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.m_parent == this->m_link)
-				return false;
-		}
-		return true;
-	}
-};
-
-#endif //BT_FEATHERSTONE_LINK_COLLIDER_H
-
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_FEATHERSTONE_LINK_COLLIDER_H
+#define BT_FEATHERSTONE_LINK_COLLIDER_H
+
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+#include "btMultiBody.h"
+
+class btMultiBodyLinkCollider : public btCollisionObject
+{
+//protected:
+public:
+
+	btMultiBody* m_multiBody;
+	int m_link;
+
+
+	btMultiBodyLinkCollider (btMultiBody* multiBody,int link)
+		:m_multiBody(multiBody),
+		m_link(link)
+	{
+		m_checkCollideWith =  true;
+		//we need to remove the 'CF_STATIC_OBJECT' flag, otherwise links/base doesn't merge islands
+		//this means that some constraints might point to bodies that are not in the islands, causing crashes
+		//if (link>=0 || (multiBody && !multiBody->hasFixedBase()))
+		{
+			m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
+		}
+		// else
+		//{
+		//	m_collisionFlags |= (btCollisionObject::CF_STATIC_OBJECT);
+		//}
+
+		m_internalType = CO_FEATHERSTONE_LINK;
+	}
+	static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj)
+	{
+		if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
+			return (btMultiBodyLinkCollider*)colObj;
+		return 0;
+	}
+	static const btMultiBodyLinkCollider* upcast(const btCollisionObject* colObj)
+	{
+		if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
+			return (btMultiBodyLinkCollider*)colObj;
+		return 0;
+	}
+
+	virtual bool checkCollideWithOverride(const  btCollisionObject* co) const
+	{
+		const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co);
+		if (!other)
+			return true;
+		if (other->m_multiBody != this->m_multiBody)
+			return true;
+		if (!m_multiBody->hasSelfCollision())
+			return false;
+
+		//check if 'link' has collision disabled
+		if (m_link>=0)
+		{
+			const btMultibodyLink& link = m_multiBody->getLink(this->m_link);
+			if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.m_parent == other->m_link)
+				return false;
+		}
+		
+		if (other->m_link>=0)
+		{
+			const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link);
+			if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.m_parent == this->m_link)
+				return false;
+		}
+		return true;
+	}
+};
+
+#endif //BT_FEATHERSTONE_LINK_COLLIDER_H
+

src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp

@@ -119,7 +119,6 @@ int numDim = BTMBP2PCONSTRAINT_DIM;
 		contactNormalOnB[i%3] = -1;
 #endif
 
-		btScalar penetration = 0;
 
 		 // Convert local points back to world
 		btVector3 pivotAworld = m_pivotInA;

src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h

@@ -1,62 +1,62 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-///This file was written by Erwin Coumans
-
-#ifndef BT_MULTIBODY_POINT2POINT_H
-#define BT_MULTIBODY_POINT2POINT_H
-
-#include "btMultiBodyConstraint.h"
-
-//#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
-
-class btMultiBodyPoint2Point : public btMultiBodyConstraint
-{
-protected:
-
-	btRigidBody*	m_rigidBodyA;
-	btRigidBody*	m_rigidBodyB;
-	btVector3		m_pivotInA;
-	btVector3		m_pivotInB;
-	
-
-public:
-
-	btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB);
-	btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB);
-
-	virtual ~btMultiBodyPoint2Point();
-
-	virtual int getIslandIdA() const;
-	virtual int getIslandIdB() const;
-
-	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
-		btMultiBodyJacobianData& data,
-		const btContactSolverInfo& infoGlobal);
-
-	const btVector3& getPivotInB() const
-	{
-		return m_pivotInB;
-	}
-
-	void setPivotInB(const btVector3& pivotInB)
-	{
-		m_pivotInB = pivotInB;
-	}
-
-	
-};
-
-#endif //BT_MULTIBODY_POINT2POINT_H
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#ifndef BT_MULTIBODY_POINT2POINT_H
+#define BT_MULTIBODY_POINT2POINT_H
+
+#include "btMultiBodyConstraint.h"
+
+//#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
+
+class btMultiBodyPoint2Point : public btMultiBodyConstraint
+{
+protected:
+
+	btRigidBody*	m_rigidBodyA;
+	btRigidBody*	m_rigidBodyB;
+	btVector3		m_pivotInA;
+	btVector3		m_pivotInB;
+	
+
+public:
+
+	btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB);
+	btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB);
+
+	virtual ~btMultiBodyPoint2Point();
+
+	virtual int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+		btMultiBodyJacobianData& data,
+		const btContactSolverInfo& infoGlobal);
+
+	const btVector3& getPivotInB() const
+	{
+		return m_pivotInB;
+	}
+
+	void setPivotInB(const btVector3& pivotInB)
+	{
+		m_pivotInB = pivotInB;
+	}
+
+	
+};
+
+#endif //BT_MULTIBODY_POINT2POINT_H

src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h

@@ -1,85 +1,85 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef BT_MULTIBODY_SOLVER_CONSTRAINT_H
-#define BT_MULTIBODY_SOLVER_CONSTRAINT_H
-
-#include "LinearMath/btVector3.h"
-#include "LinearMath/btAlignedObjectArray.h"
-
-class btMultiBody;
-#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
-#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
-
-///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
-ATTRIBUTE_ALIGNED16 (struct)	btMultiBodySolverConstraint
-{
-	BT_DECLARE_ALIGNED_ALLOCATOR();
-
-	btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_solverBodyIdB(-1), m_multiBodyA(0), m_multiBodyB(0), m_linkA(-1), m_linkB(-1)
-	{}
-
-	int				m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1
-	int				m_jacAindex;
-	int				m_deltaVelBindex;
-	int				m_jacBindex;
-
-	btVector3		m_relpos1CrossNormal;
-	btVector3		m_contactNormal1;	
-	btVector3		m_relpos2CrossNormal;
-	btVector3		m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always
-	
-
-	btVector3		m_angularComponentA;
-	btVector3		m_angularComponentB;
-	
-	mutable btSimdScalar	m_appliedPushImpulse;
-	mutable btSimdScalar	m_appliedImpulse;
-
-	btScalar	m_friction;
-	btScalar	m_jacDiagABInv;
-	btScalar		m_rhs;
-	btScalar		m_cfm;
-	
-    btScalar		m_lowerLimit;
-	btScalar		m_upperLimit;
-	btScalar		m_rhsPenetration;
-    union
-	{
-		void*		m_originalContactPoint;
-		btScalar	m_unusedPadding4;
-	};
-
-	int	m_overrideNumSolverIterations;
-    int			m_frictionIndex;
-
-	int m_solverBodyIdA;
-	btMultiBody* m_multiBodyA;
-	int			m_linkA;
-	
-	int m_solverBodyIdB;
-	btMultiBody* m_multiBodyB;
-	int			m_linkB;
-
-	enum		btSolverConstraintType
-	{
-		BT_SOLVER_CONTACT_1D = 0,
-		BT_SOLVER_FRICTION_1D
-	};
-};
-
-typedef btAlignedObjectArray<btMultiBodySolverConstraint>	btMultiBodyConstraintArray;
-
-#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_MULTIBODY_SOLVER_CONSTRAINT_H
+#define BT_MULTIBODY_SOLVER_CONSTRAINT_H
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+class btMultiBody;
+#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+
+///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
+ATTRIBUTE_ALIGNED16 (struct)	btMultiBodySolverConstraint
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1)
+	{}
+
+	int				m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1
+	int				m_jacAindex;
+	int				m_deltaVelBindex;
+	int				m_jacBindex;
+
+	btVector3		m_relpos1CrossNormal;
+	btVector3		m_contactNormal1;	
+	btVector3		m_relpos2CrossNormal;
+	btVector3		m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always
+	
+
+	btVector3		m_angularComponentA;
+	btVector3		m_angularComponentB;
+	
+	mutable btSimdScalar	m_appliedPushImpulse;
+	mutable btSimdScalar	m_appliedImpulse;
+
+	btScalar	m_friction;
+	btScalar	m_jacDiagABInv;
+	btScalar		m_rhs;
+	btScalar		m_cfm;
+	
+    btScalar		m_lowerLimit;
+	btScalar		m_upperLimit;
+	btScalar		m_rhsPenetration;
+    union
+	{
+		void*		m_originalContactPoint;
+		btScalar	m_unusedPadding4;
+	};
+
+	int	m_overrideNumSolverIterations;
+    int			m_frictionIndex;
+
+	int m_solverBodyIdA;
+	btMultiBody* m_multiBodyA;
+	int			m_linkA;
+	
+	int m_solverBodyIdB;
+	btMultiBody* m_multiBodyB;
+	int			m_linkB;
+
+	enum		btSolverConstraintType
+	{
+		BT_SOLVER_CONTACT_1D = 0,
+		BT_SOLVER_FRICTION_1D
+	};
+};
+
+typedef btAlignedObjectArray<btMultiBodySolverConstraint>	btMultiBodyConstraintArray;
+
+#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H

src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp

@@ -821,14 +821,15 @@ void btSolveL1T (const btScalar *L, btScalar *B, int n, int lskip1)
   /* declare variables - Z matrix, p and q vectors, etc */
   btScalar Z11,m11,Z21,m21,Z31,m31,Z41,m41,p1,q1,p2,p3,p4,*ex;
   const btScalar *ell;
-  int lskip2,lskip3,i,j;
+  int lskip2,i,j;
+//  int lskip3;
   /* special handling for L and B because we're solving L1 *transpose* */
   L = L + (n-1)*(lskip1+1);
   B = B + n-1;
   lskip1 = -lskip1;
   /* compute lskip values */
   lskip2 = 2*lskip1;
-  lskip3 = 3*lskip1;
+  //lskip3 = 3*lskip1;
   /* compute all 4 x 1 blocks of X */
   for (i=0; i <= n-4; i+=4) {
     /* compute all 4 x 1 block of X, from rows i..i+4-1 */

src/BulletDynamics/MLCPSolvers/btDantzigSolver.h

@@ -1,112 +1,112 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-///original version written by Erwin Coumans, October 2013
-
-#ifndef BT_DANTZIG_SOLVER_H
-#define BT_DANTZIG_SOLVER_H
-
-#include "btMLCPSolverInterface.h"
-#include "btDantzigLCP.h"
-
-
-class btDantzigSolver : public btMLCPSolverInterface
-{
-protected:
-
-	btScalar m_acceptableUpperLimitSolution;
-
-	btAlignedObjectArray<char>	m_tempBuffer;
-
-	btAlignedObjectArray<btScalar> m_A;
-	btAlignedObjectArray<btScalar> m_b;
-	btAlignedObjectArray<btScalar> m_x;
-	btAlignedObjectArray<btScalar> m_lo;
-	btAlignedObjectArray<btScalar> m_hi;
-	btAlignedObjectArray<int>	m_dependencies;
-	btDantzigScratchMemory m_scratchMemory;
-public:
-
-	btDantzigSolver()
-		:m_acceptableUpperLimitSolution(btScalar(1000))
-	{
-	}
-
-	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
-	{
-		bool result = true;
-		int n = b.rows();
-		if (n)
-		{
-			int nub = 0;
-			btAlignedObjectArray<btScalar> ww;
-			ww.resize(n);
-	
-
-			const btScalar* Aptr = A.getBufferPointer();
-			m_A.resize(n*n);
-			for (int i=0;i<n*n;i++)
-			{
-				m_A[i] = Aptr[i];
-
-			}
-
-			m_b.resize(n);
-			m_x.resize(n);
-			m_lo.resize(n);
-			m_hi.resize(n);
-			m_dependencies.resize(n);
-			for (int i=0;i<n;i++)
-			{
-				m_lo[i] = lo[i];
-				m_hi[i] = hi[i];
-				m_b[i] = b[i];
-				m_x[i] = x[i];
-				m_dependencies[i] = limitDependency[i];
-			}
-
-
-			result = btSolveDantzigLCP (n,&m_A[0],&m_x[0],&m_b[0],&ww[0],nub,&m_lo[0],&m_hi[0],&m_dependencies[0],m_scratchMemory);
-			if (!result)
-				return result;
-
-//			printf("numAllocas = %d\n",numAllocas);
-			for (int i=0;i<n;i++)
-			{
-				volatile btScalar xx = m_x[i];
-				if (xx != m_x[i])
-					return false;
-				if (x[i] >= m_acceptableUpperLimitSolution)
-				{
-					return false;
-				}
-
-				if (x[i] <= -m_acceptableUpperLimitSolution)
-				{
-					return false;
-				}
-			}
-
-			for (int i=0;i<n;i++)
-			{
-				x[i] = m_x[i];
-			}
-			
-		}
-
-		return result;
-	}
-};
-
-#endif //BT_DANTZIG_SOLVER_H
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///original version written by Erwin Coumans, October 2013
+
+#ifndef BT_DANTZIG_SOLVER_H
+#define BT_DANTZIG_SOLVER_H
+
+#include "btMLCPSolverInterface.h"
+#include "btDantzigLCP.h"
+
+
+class btDantzigSolver : public btMLCPSolverInterface
+{
+protected:
+
+	btScalar m_acceptableUpperLimitSolution;
+
+	btAlignedObjectArray<char>	m_tempBuffer;
+
+	btAlignedObjectArray<btScalar> m_A;
+	btAlignedObjectArray<btScalar> m_b;
+	btAlignedObjectArray<btScalar> m_x;
+	btAlignedObjectArray<btScalar> m_lo;
+	btAlignedObjectArray<btScalar> m_hi;
+	btAlignedObjectArray<int>	m_dependencies;
+	btDantzigScratchMemory m_scratchMemory;
+public:
+
+	btDantzigSolver()
+		:m_acceptableUpperLimitSolution(btScalar(1000))
+	{
+	}
+
+	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
+	{
+		bool result = true;
+		int n = b.rows();
+		if (n)
+		{
+			int nub = 0;
+			btAlignedObjectArray<btScalar> ww;
+			ww.resize(n);
+	
+
+			const btScalar* Aptr = A.getBufferPointer();
+			m_A.resize(n*n);
+			for (int i=0;i<n*n;i++)
+			{
+				m_A[i] = Aptr[i];
+
+			}
+
+			m_b.resize(n);
+			m_x.resize(n);
+			m_lo.resize(n);
+			m_hi.resize(n);
+			m_dependencies.resize(n);
+			for (int i=0;i<n;i++)
+			{
+				m_lo[i] = lo[i];
+				m_hi[i] = hi[i];
+				m_b[i] = b[i];
+				m_x[i] = x[i];
+				m_dependencies[i] = limitDependency[i];
+			}
+
+
+			result = btSolveDantzigLCP (n,&m_A[0],&m_x[0],&m_b[0],&ww[0],nub,&m_lo[0],&m_hi[0],&m_dependencies[0],m_scratchMemory);
+			if (!result)
+				return result;
+
+//			printf("numAllocas = %d\n",numAllocas);
+			for (int i=0;i<n;i++)
+			{
+				volatile btScalar xx = m_x[i];
+				if (xx != m_x[i])
+					return false;
+				if (x[i] >= m_acceptableUpperLimitSolution)
+				{
+					return false;
+				}
+
+				if (x[i] <= -m_acceptableUpperLimitSolution)
+				{
+					return false;
+				}
+			}
+
+			for (int i=0;i<n;i++)
+			{
+				x[i] = m_x[i];
+			}
+			
+		}
+
+		return result;
+	}
+};
+
+#endif //BT_DANTZIG_SOLVER_H

src/BulletDynamics/MLCPSolvers/btLemkeSolver.h

@@ -1,350 +1,350 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-///original version written by Erwin Coumans, October 2013
-
-#ifndef BT_LEMKE_SOLVER_H
-#define BT_LEMKE_SOLVER_H
-
-
-#include "btMLCPSolverInterface.h"
-#include "btLemkeAlgorithm.h"
-
-
-
-
-///The btLemkeSolver is based on "Fast Implementation of Lemke<6B>s Algorithm for Rigid Body Contact Simulation (John E. Lloyd) "
-///It is a slower but more accurate solver. Increase the m_maxLoops for better convergence, at the cost of more CPU time.
-///The original implementation of the btLemkeAlgorithm was done by Kilian Grundl from the MBSim team
-class btLemkeSolver : public btMLCPSolverInterface
-{
-protected:
-	
-public:
-
-	btScalar	m_maxValue;
-	int			m_debugLevel;
-	int			m_maxLoops;
-	bool		m_useLoHighBounds;
-	
-	
-
-	btLemkeSolver()
-		:m_maxValue(100000),
-		m_debugLevel(0),
-		m_maxLoops(1000),
-		m_useLoHighBounds(true)
-	{
-	}
-	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
-	{
-		
-		if (m_useLoHighBounds)
-		{
-
-		BT_PROFILE("btLemkeSolver::solveMLCP");
-		int n = A.rows();
-		if (0==n)
-			return true;
-		
-		bool fail = false;
-
-		btVectorXu solution(n);
-		btVectorXu q1;
-		q1.resize(n);
-		for (int row=0;row<n;row++)
-		{
-			q1[row] = -b[row];
-		}
-
-	//		cout << "A" << endl;
-	//		cout << A << endl;
-
-			/////////////////////////////////////
-
-			//slow matrix inversion, replace with LU decomposition
-			btMatrixXu A1;
-			btMatrixXu B(n,n);
-			{
-				BT_PROFILE("inverse(slow)");
-				A1.resize(A.rows(),A.cols());
-				for (int row=0;row<A.rows();row++)
-				{
-					for (int col=0;col<A.cols();col++)
-					{
-						A1.setElem(row,col,A(row,col));
-					}
-				}
-
-				btMatrixXu matrix;
-				matrix.resize(n,2*n);
-				for (int row=0;row<n;row++)
-				{
-					for (int col=0;col<n;col++)
-					{
-						matrix.setElem(row,col,A1(row,col));
-					}
-				}
-
-
-				btScalar ratio,a;
-				int i,j,k;
-				for(i = 0; i < n; i++){
-				for(j = n; j < 2*n; j++){
-					if(i==(j-n))
-						matrix.setElem(i,j,1.0);
-					else
-						matrix.setElem(i,j,0.0);
-				}
-			}
-			for(i = 0; i < n; i++){
-				for(j = 0; j < n; j++){
-					if(i!=j)
-					{
-						btScalar v = matrix(i,i);
-						if (btFuzzyZero(v))
-						{
-							a = 0.000001f;
-						}
-						ratio = matrix(j,i)/matrix(i,i);
-						for(k = 0; k < 2*n; k++){
-							matrix.addElem(j,k,- ratio * matrix(i,k));
-						}
-					}
-				}
-			}
-			for(i = 0; i < n; i++){
-				a = matrix(i,i);
-				if (btFuzzyZero(a))
-				{
-					a = 0.000001f;
-				}
-				btScalar invA = 1.f/a;
-				for(j = 0; j < 2*n; j++){
-					matrix.mulElem(i,j,invA);
-				}
-			}
-
-	
-
-	
-
-			for (int row=0;row<n;row++)
-				{
-					for (int col=0;col<n;col++)
-					{
-						B.setElem(row,col,matrix(row,n+col));
-					}
-				}
-			}
-
-		btMatrixXu b1(n,1);
-
-		btMatrixXu M(n*2,n*2);
-		for (int row=0;row<n;row++)
-		{
-			b1.setElem(row,0,-b[row]);
-			for (int col=0;col<n;col++)
-			{
-				btScalar v =B(row,col);
-				M.setElem(row,col,v);
-				M.setElem(n+row,n+col,v);
-				M.setElem(n+row,col,-v);
-				M.setElem(row,n+col,-v);
-
-			}
-		}
-
-		btMatrixXu Bb1 = B*b1;
-//		q = [ (-B*b1 - lo)'   (hi + B*b1)' ]'
-
-		btVectorXu qq;
-		qq.resize(n*2);
-		for (int row=0;row<n;row++)
-		{
-			qq[row] = -Bb1(row,0)-lo[row];
-			qq[n+row] = Bb1(row,0)+hi[row];
-		}
-
-		btVectorXu z1;
-
-		btMatrixXu y1;
-		y1.resize(n,1);
-		btLemkeAlgorithm lemke(M,qq,m_debugLevel);
-		{
-			BT_PROFILE("lemke.solve");
-			lemke.setSystem(M,qq);
-			z1  = lemke.solve(m_maxLoops);
-		}
-		for (int row=0;row<n;row++)
-		{
-			y1.setElem(row,0,z1[2*n+row]-z1[3*n+row]);
-		}
-		btMatrixXu y1_b1(n,1);
-		for (int i=0;i<n;i++)
-		{
-			y1_b1.setElem(i,0,y1(i,0)-b1(i,0));
-		}
-
-		btMatrixXu x1;
-
-		x1 = B*(y1_b1);
-			
-		for (int row=0;row<n;row++)
-		{
-			solution[row] = x1(row,0);//n];
-		}
-
-		int errorIndexMax = -1;
-		int errorIndexMin = -1;
-		float errorValueMax = -1e30;
-		float errorValueMin = 1e30;
-		
-		for (int i=0;i<n;i++)
-		{
-			x[i] = solution[i];
-			volatile btScalar check = x[i];
-			if (x[i] != check)
-			{
-				//printf("Lemke result is #NAN\n");
-				x.setZero();
-				return false;
-			}
-			
-			//this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver 
-			//we need to figure out why it happens, and fix it, or detect it properly)
-			if (x[i]>m_maxValue)
-			{
-				if (x[i]> errorValueMax)
-				{
-					fail = true;
-					errorIndexMax = i;
-					errorValueMax = x[i];
-				}
-				////printf("x[i] = %f,",x[i]);
-			}
-			if (x[i]<-m_maxValue)
-			{
-				if (x[i]<errorValueMin)
-				{
-					errorIndexMin = i;
-					errorValueMin = x[i];
-					fail = true;
-					//printf("x[i] = %f,",x[i]);
-				}
-			}
-		}
-		if (fail)
-		{
-			int m_errorCountTimes = 0;
-			if (errorIndexMin<0)
-				errorValueMin = 0.f;
-			if (errorIndexMax<0)
-				errorValueMax = 0.f;
-			m_errorCountTimes++;
-		//	printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
-			for (int i=0;i<n;i++)
-			{
-				x[i]=0.f;
-			}
-		}
-		return !fail;
-	} else
-		
-	{
-			int dimension = A.rows();
-		if (0==dimension)
-			return true;
-		
-//		printf("================ solving using Lemke/Newton/Fixpoint\n");
-
-		btVectorXu q;
-		q.resize(dimension);
-		for (int row=0;row<dimension;row++)
-		{
-			q[row] = -b[row];
-		}
-		
-		btLemkeAlgorithm lemke(A,q,m_debugLevel);
-		
-		
-		lemke.setSystem(A,q);
-		
-		btVectorXu solution = lemke.solve(m_maxLoops);
-		
-		//check solution
-		
-		bool fail = false;
-		int errorIndexMax = -1;
-		int errorIndexMin = -1;
-		float errorValueMax = -1e30;
-		float errorValueMin = 1e30;
-		
-		for (int i=0;i<dimension;i++)
-		{
-			x[i] = solution[i+dimension];
-			volatile btScalar check = x[i];
-			if (x[i] != check)
-			{
-				x.setZero();
-				return false;
-			}
-			
-			//this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver 
-			//we need to figure out why it happens, and fix it, or detect it properly)
-			if (x[i]>m_maxValue)
-			{
-				if (x[i]> errorValueMax)
-				{
-					fail = true;
-					errorIndexMax = i;
-					errorValueMax = x[i];
-				}
-				////printf("x[i] = %f,",x[i]);
-			}
-			if (x[i]<-m_maxValue)
-			{
-				if (x[i]<errorValueMin)
-				{
-					errorIndexMin = i;
-					errorValueMin = x[i];
-					fail = true;
-					//printf("x[i] = %f,",x[i]);
-				}
-			}
-		}
-		if (fail)
-		{
-			static int errorCountTimes = 0;
-			if (errorIndexMin<0)
-				errorValueMin = 0.f;
-			if (errorIndexMax<0)
-				errorValueMax = 0.f;
-			printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
-			for (int i=0;i<dimension;i++)
-			{
-				x[i]=0.f;
-			}
-		}
-
-
-		return !fail;
-	}
-	return true;
-
-	}
-
-};
-
-#endif //BT_LEMKE_SOLVER_H
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///original version written by Erwin Coumans, October 2013
+
+#ifndef BT_LEMKE_SOLVER_H
+#define BT_LEMKE_SOLVER_H
+
+
+#include "btMLCPSolverInterface.h"
+#include "btLemkeAlgorithm.h"
+
+
+
+
+///The btLemkeSolver is based on "Fast Implementation of Lemke<6B>s Algorithm for Rigid Body Contact Simulation (John E. Lloyd) "
+///It is a slower but more accurate solver. Increase the m_maxLoops for better convergence, at the cost of more CPU time.
+///The original implementation of the btLemkeAlgorithm was done by Kilian Grundl from the MBSim team
+class btLemkeSolver : public btMLCPSolverInterface
+{
+protected:
+	
+public:
+
+	btScalar	m_maxValue;
+	int			m_debugLevel;
+	int			m_maxLoops;
+	bool		m_useLoHighBounds;
+	
+	
+
+	btLemkeSolver()
+		:m_maxValue(100000),
+		m_debugLevel(0),
+		m_maxLoops(1000),
+		m_useLoHighBounds(true)
+	{
+	}
+	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
+	{
+		
+		if (m_useLoHighBounds)
+		{
+
+		BT_PROFILE("btLemkeSolver::solveMLCP");
+		int n = A.rows();
+		if (0==n)
+			return true;
+		
+		bool fail = false;
+
+		btVectorXu solution(n);
+		btVectorXu q1;
+		q1.resize(n);
+		for (int row=0;row<n;row++)
+		{
+			q1[row] = -b[row];
+		}
+
+	//		cout << "A" << endl;
+	//		cout << A << endl;
+
+			/////////////////////////////////////
+
+			//slow matrix inversion, replace with LU decomposition
+			btMatrixXu A1;
+			btMatrixXu B(n,n);
+			{
+				BT_PROFILE("inverse(slow)");
+				A1.resize(A.rows(),A.cols());
+				for (int row=0;row<A.rows();row++)
+				{
+					for (int col=0;col<A.cols();col++)
+					{
+						A1.setElem(row,col,A(row,col));
+					}
+				}
+
+				btMatrixXu matrix;
+				matrix.resize(n,2*n);
+				for (int row=0;row<n;row++)
+				{
+					for (int col=0;col<n;col++)
+					{
+						matrix.setElem(row,col,A1(row,col));
+					}
+				}
+
+
+				btScalar ratio,a;
+				int i,j,k;
+				for(i = 0; i < n; i++){
+				for(j = n; j < 2*n; j++){
+					if(i==(j-n))
+						matrix.setElem(i,j,1.0);
+					else
+						matrix.setElem(i,j,0.0);
+				}
+			}
+			for(i = 0; i < n; i++){
+				for(j = 0; j < n; j++){
+					if(i!=j)
+					{
+						btScalar v = matrix(i,i);
+						if (btFuzzyZero(v))
+						{
+							a = 0.000001f;
+						}
+						ratio = matrix(j,i)/matrix(i,i);
+						for(k = 0; k < 2*n; k++){
+							matrix.addElem(j,k,- ratio * matrix(i,k));
+						}
+					}
+				}
+			}
+			for(i = 0; i < n; i++){
+				a = matrix(i,i);
+				if (btFuzzyZero(a))
+				{
+					a = 0.000001f;
+				}
+				btScalar invA = 1.f/a;
+				for(j = 0; j < 2*n; j++){
+					matrix.mulElem(i,j,invA);
+				}
+			}
+
+	
+
+	
+
+			for (int row=0;row<n;row++)
+				{
+					for (int col=0;col<n;col++)
+					{
+						B.setElem(row,col,matrix(row,n+col));
+					}
+				}
+			}
+
+		btMatrixXu b1(n,1);
+
+		btMatrixXu M(n*2,n*2);
+		for (int row=0;row<n;row++)
+		{
+			b1.setElem(row,0,-b[row]);
+			for (int col=0;col<n;col++)
+			{
+				btScalar v =B(row,col);
+				M.setElem(row,col,v);
+				M.setElem(n+row,n+col,v);
+				M.setElem(n+row,col,-v);
+				M.setElem(row,n+col,-v);
+
+			}
+		}
+
+		btMatrixXu Bb1 = B*b1;
+//		q = [ (-B*b1 - lo)'   (hi + B*b1)' ]'
+
+		btVectorXu qq;
+		qq.resize(n*2);
+		for (int row=0;row<n;row++)
+		{
+			qq[row] = -Bb1(row,0)-lo[row];
+			qq[n+row] = Bb1(row,0)+hi[row];
+		}
+
+		btVectorXu z1;
+
+		btMatrixXu y1;
+		y1.resize(n,1);
+		btLemkeAlgorithm lemke(M,qq,m_debugLevel);
+		{
+			BT_PROFILE("lemke.solve");
+			lemke.setSystem(M,qq);
+			z1  = lemke.solve(m_maxLoops);
+		}
+		for (int row=0;row<n;row++)
+		{
+			y1.setElem(row,0,z1[2*n+row]-z1[3*n+row]);
+		}
+		btMatrixXu y1_b1(n,1);
+		for (int i=0;i<n;i++)
+		{
+			y1_b1.setElem(i,0,y1(i,0)-b1(i,0));
+		}
+
+		btMatrixXu x1;
+
+		x1 = B*(y1_b1);
+			
+		for (int row=0;row<n;row++)
+		{
+			solution[row] = x1(row,0);//n];
+		}
+
+		int errorIndexMax = -1;
+		int errorIndexMin = -1;
+		float errorValueMax = -1e30;
+		float errorValueMin = 1e30;
+		
+		for (int i=0;i<n;i++)
+		{
+			x[i] = solution[i];
+			volatile btScalar check = x[i];
+			if (x[i] != check)
+			{
+				//printf("Lemke result is #NAN\n");
+				x.setZero();
+				return false;
+			}
+			
+			//this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver 
+			//we need to figure out why it happens, and fix it, or detect it properly)
+			if (x[i]>m_maxValue)
+			{
+				if (x[i]> errorValueMax)
+				{
+					fail = true;
+					errorIndexMax = i;
+					errorValueMax = x[i];
+				}
+				////printf("x[i] = %f,",x[i]);
+			}
+			if (x[i]<-m_maxValue)
+			{
+				if (x[i]<errorValueMin)
+				{
+					errorIndexMin = i;
+					errorValueMin = x[i];
+					fail = true;
+					//printf("x[i] = %f,",x[i]);
+				}
+			}
+		}
+		if (fail)
+		{
+			int m_errorCountTimes = 0;
+			if (errorIndexMin<0)
+				errorValueMin = 0.f;
+			if (errorIndexMax<0)
+				errorValueMax = 0.f;
+			m_errorCountTimes++;
+		//	printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
+			for (int i=0;i<n;i++)
+			{
+				x[i]=0.f;
+			}
+		}
+		return !fail;
+	} else
+		
+	{
+			int dimension = A.rows();
+		if (0==dimension)
+			return true;
+		
+//		printf("================ solving using Lemke/Newton/Fixpoint\n");
+
+		btVectorXu q;
+		q.resize(dimension);
+		for (int row=0;row<dimension;row++)
+		{
+			q[row] = -b[row];
+		}
+		
+		btLemkeAlgorithm lemke(A,q,m_debugLevel);
+		
+		
+		lemke.setSystem(A,q);
+		
+		btVectorXu solution = lemke.solve(m_maxLoops);
+		
+		//check solution
+		
+		bool fail = false;
+		int errorIndexMax = -1;
+		int errorIndexMin = -1;
+		float errorValueMax = -1e30;
+		float errorValueMin = 1e30;
+		
+		for (int i=0;i<dimension;i++)
+		{
+			x[i] = solution[i+dimension];
+			volatile btScalar check = x[i];
+			if (x[i] != check)
+			{
+				x.setZero();
+				return false;
+			}
+			
+			//this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver 
+			//we need to figure out why it happens, and fix it, or detect it properly)
+			if (x[i]>m_maxValue)
+			{
+				if (x[i]> errorValueMax)
+				{
+					fail = true;
+					errorIndexMax = i;
+					errorValueMax = x[i];
+				}
+				////printf("x[i] = %f,",x[i]);
+			}
+			if (x[i]<-m_maxValue)
+			{
+				if (x[i]<errorValueMin)
+				{
+					errorIndexMin = i;
+					errorValueMin = x[i];
+					fail = true;
+					//printf("x[i] = %f,",x[i]);
+				}
+			}
+		}
+		if (fail)
+		{
+			static int errorCountTimes = 0;
+			if (errorIndexMin<0)
+				errorValueMin = 0.f;
+			if (errorIndexMax<0)
+				errorValueMax = 0.f;
+			printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
+			for (int i=0;i<dimension;i++)
+			{
+				x[i]=0.f;
+			}
+		}
+
+
+		return !fail;
+	}
+	return true;
+
+	}
+
+};
+
+#endif //BT_LEMKE_SOLVER_H

src/BulletDynamics/MLCPSolvers/btMLCPSolver.h

@@ -1,93 +1,93 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-///original version written by Erwin Coumans, October 2013
-
-#ifndef BT_MLCP_SOLVER_H
-#define BT_MLCP_SOLVER_H
-
-#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
-#include "LinearMath/btMatrixX.h"
-#include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h"
-
-class btMLCPSolver : public btSequentialImpulseConstraintSolver
-{
-
-protected:
-	
-	btMatrixXu m_A;
-	btVectorXu m_b;
-	btVectorXu m_x;
-	btVectorXu m_lo;
-	btVectorXu m_hi;
-	
-	///when using 'split impulse' we solve two separate (M)LCPs
-	btVectorXu m_bSplit;
-	btVectorXu m_xSplit;
-	btVectorXu m_bSplit1;
-	btVectorXu m_xSplit2;
-
-	btAlignedObjectArray<int> m_limitDependencies;
-	btAlignedObjectArray<btSolverConstraint*>	m_allConstraintPtrArray;
-	btMLCPSolverInterface* m_solver;
-	int m_fallback;
-	btScalar m_cfm;
-
-	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
-	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
-
-
-	virtual void createMLCP(const btContactSolverInfo& infoGlobal);
-	virtual void createMLCPFast(const btContactSolverInfo& infoGlobal);
-
-	//return true is it solves the problem successfully
-	virtual bool solveMLCP(const btContactSolverInfo& infoGlobal);
-
-public:
-
-	btMLCPSolver(	 btMLCPSolverInterface* solver);
-	virtual ~btMLCPSolver();
-
-	void setMLCPSolver(btMLCPSolverInterface* solver)
-	{
-		m_solver = solver;
-	}
-
-	int getNumFallbacks() const
-	{
-		return m_fallback;
-	}
-	void setNumFallbacks(int num)
-	{
-		m_fallback = num;
-	}
-
-	btScalar	getCfm() const
-	{
-		return m_cfm;
-	}
-	void setCfm(btScalar cfm)
-	{
-		m_cfm = cfm;
-	}
-
-	virtual btConstraintSolverType	getSolverType() const
-	{
-		return BT_MLCP_SOLVER;
-	}
-
-};
-
-
-#endif //BT_MLCP_SOLVER_H
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///original version written by Erwin Coumans, October 2013
+
+#ifndef BT_MLCP_SOLVER_H
+#define BT_MLCP_SOLVER_H
+
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "LinearMath/btMatrixX.h"
+#include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h"
+
+class btMLCPSolver : public btSequentialImpulseConstraintSolver
+{
+
+protected:
+	
+	btMatrixXu m_A;
+	btVectorXu m_b;
+	btVectorXu m_x;
+	btVectorXu m_lo;
+	btVectorXu m_hi;
+	
+	///when using 'split impulse' we solve two separate (M)LCPs
+	btVectorXu m_bSplit;
+	btVectorXu m_xSplit;
+	btVectorXu m_bSplit1;
+	btVectorXu m_xSplit2;
+
+	btAlignedObjectArray<int> m_limitDependencies;
+	btAlignedObjectArray<btSolverConstraint*>	m_allConstraintPtrArray;
+	btMLCPSolverInterface* m_solver;
+	int m_fallback;
+	btScalar m_cfm;
+
+	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+
+
+	virtual void createMLCP(const btContactSolverInfo& infoGlobal);
+	virtual void createMLCPFast(const btContactSolverInfo& infoGlobal);
+
+	//return true is it solves the problem successfully
+	virtual bool solveMLCP(const btContactSolverInfo& infoGlobal);
+
+public:
+
+	btMLCPSolver(	 btMLCPSolverInterface* solver);
+	virtual ~btMLCPSolver();
+
+	void setMLCPSolver(btMLCPSolverInterface* solver)
+	{
+		m_solver = solver;
+	}
+
+	int getNumFallbacks() const
+	{
+		return m_fallback;
+	}
+	void setNumFallbacks(int num)
+	{
+		m_fallback = num;
+	}
+
+	btScalar	getCfm() const
+	{
+		return m_cfm;
+	}
+	void setCfm(btScalar cfm)
+	{
+		m_cfm = cfm;
+	}
+
+	virtual btConstraintSolverType	getSolverType() const
+	{
+		return BT_MLCP_SOLVER;
+	}
+
+};
+
+
+#endif //BT_MLCP_SOLVER_H

src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h

@@ -1,33 +1,33 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-///original version written by Erwin Coumans, October 2013
-
-#ifndef BT_MLCP_SOLVER_INTERFACE_H
-#define BT_MLCP_SOLVER_INTERFACE_H
-
-#include "LinearMath/btMatrixX.h"
-
-class btMLCPSolverInterface
-{
-public:
-	virtual ~btMLCPSolverInterface()
-	{
-	}
-
-	//return true is it solves the problem successfully
-	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)=0;
-};
-
-#endif //BT_MLCP_SOLVER_INTERFACE_H
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///original version written by Erwin Coumans, October 2013
+
+#ifndef BT_MLCP_SOLVER_INTERFACE_H
+#define BT_MLCP_SOLVER_INTERFACE_H
+
+#include "LinearMath/btMatrixX.h"
+
+class btMLCPSolverInterface
+{
+public:
+	virtual ~btMLCPSolverInterface()
+	{
+	}
+
+	//return true is it solves the problem successfully
+	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)=0;
+};
+
+#endif //BT_MLCP_SOLVER_INTERFACE_H

src/BulletDynamics/MLCPSolvers/btPATHSolver.h

@@ -1,151 +1,151 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-///original version written by Erwin Coumans, October 2013
-
-
-#ifndef BT_PATH_SOLVER_H
-#define BT_PATH_SOLVER_H
-
-//#define BT_USE_PATH
-#ifdef BT_USE_PATH
-
-extern "C" {
-#include "PATH/SimpleLCP.h"
-#include "PATH/License.h"
-#include "PATH/Error_Interface.h"
-};
-  void __stdcall MyError(Void *data, Char *msg)
-{
-	printf("Path Error: %s\n",msg);
-}
-  void __stdcall MyWarning(Void *data, Char *msg)
-{
-	printf("Path Warning: %s\n",msg);
-}
-
-Error_Interface e;
-
-
-
-#include "btMLCPSolverInterface.h"
-#include "Dantzig/lcp.h"
-
-class btPathSolver : public btMLCPSolverInterface
-{
-public:
-
-	btPathSolver()
-	{
-		License_SetString("2069810742&Courtesy_License&&&USR&2013&14_12_2011&1000&PATH&GEN&31_12_2013&0_0_0&0&0_0");
-		e.error_data = 0;
-		e.warning = MyWarning;
-		e.error = MyError;
-		Error_SetInterface(&e);
-	}
-
-
-	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
-	{
-		MCP_Termination status;
-		
-
-		int numVariables = b.rows();
-		if (0==numVariables)
-			return true;
-
-			/*	 - variables - the number of variables in the problem
-			- m_nnz - the number of nonzeros in the M matrix
-			- m_i - a vector of size m_nnz containing the row indices for M
-			- m_j - a vector of size m_nnz containing the column indices for M
-			- m_ij - a vector of size m_nnz containing the data for M
-			- q - a vector of size variables
-			- lb - a vector of size variables containing the lower bounds on x
-			- ub - a vector of size variables containing the upper bounds on x
-			*/
-		btAlignedObjectArray<double> values;
-		btAlignedObjectArray<int> rowIndices;
-		btAlignedObjectArray<int> colIndices;
-
-		for (int i=0;i<A.rows();i++)
-		{
-			for (int j=0;j<A.cols();j++)
-			{
-				if (A(i,j)!=0.f)
-				{
-					//add 1, because Path starts at 1, instead of 0
-					rowIndices.push_back(i+1);
-					colIndices.push_back(j+1);
-					values.push_back(A(i,j));
-				}
-			}
-		}
-		int numNonZero = rowIndices.size();
-		btAlignedObjectArray<double> zResult;
-		zResult.resize(numVariables);
-		btAlignedObjectArray<double> rhs;
-		btAlignedObjectArray<double> upperBounds;
-		btAlignedObjectArray<double> lowerBounds;
-		for (int i=0;i<numVariables;i++)
-		{
-			upperBounds.push_back(hi[i]);
-			lowerBounds.push_back(lo[i]);
-			rhs.push_back(-b[i]);
-		}
-
-
-		SimpleLCP(numVariables,numNonZero,&rowIndices[0],&colIndices[0],&values[0],&rhs[0],&lowerBounds[0],&upperBounds[0], &status, &zResult[0]);
-
-		if (status != MCP_Solved)
-		{
-			static const char* gReturnMsgs[] = {
-				"Invalid return",
-				"MCP_Solved: The problem was solved",
-				"MCP_NoProgress: A stationary point was found",
-				"MCP_MajorIterationLimit: Major iteration limit met",
-				"MCP_MinorIterationLimit: Cumulative minor iteration limit met",
-				"MCP_TimeLimit: Ran out of time",
-				"MCP_UserInterrupt: Control-C, typically",
-				"MCP_BoundError: Problem has a bound error",
-				"MCP_DomainError: Could not find starting point",
-				"MCP_Infeasible: Problem has no solution",
-				"MCP_Error: An error occurred within the code",
-				"MCP_LicenseError: License could not be found",
-				"MCP_OK"
-			};
-
-			printf("ERROR: The PATH MCP solver failed: %s\n", gReturnMsgs[(unsigned int)status]);// << std::endl;
-			printf("using Projected Gauss Seidel fallback\n");
-			
-			return false;
-		} else
-		{
-			for (int i=0;i<numVariables;i++)
-			{
-				x[i] = zResult[i];
-				//check for #NAN
-				if (x[i] != zResult[i])
-					return false;
-			}
-			return true;
-
-		}
-
-	}
-};
-
-#endif //BT_USE_PATH
-
-
-#endif //BT_PATH_SOLVER_H
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///original version written by Erwin Coumans, October 2013
+
+
+#ifndef BT_PATH_SOLVER_H
+#define BT_PATH_SOLVER_H
+
+//#define BT_USE_PATH
+#ifdef BT_USE_PATH
+
+extern "C" {
+#include "PATH/SimpleLCP.h"
+#include "PATH/License.h"
+#include "PATH/Error_Interface.h"
+};
+  void __stdcall MyError(Void *data, Char *msg)
+{
+	printf("Path Error: %s\n",msg);
+}
+  void __stdcall MyWarning(Void *data, Char *msg)
+{
+	printf("Path Warning: %s\n",msg);
+}
+
+Error_Interface e;
+
+
+
+#include "btMLCPSolverInterface.h"
+#include "Dantzig/lcp.h"
+
+class btPathSolver : public btMLCPSolverInterface
+{
+public:
+
+	btPathSolver()
+	{
+		License_SetString("2069810742&Courtesy_License&&&USR&2013&14_12_2011&1000&PATH&GEN&31_12_2013&0_0_0&0&0_0");
+		e.error_data = 0;
+		e.warning = MyWarning;
+		e.error = MyError;
+		Error_SetInterface(&e);
+	}
+
+
+	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
+	{
+		MCP_Termination status;
+		
+
+		int numVariables = b.rows();
+		if (0==numVariables)
+			return true;
+
+			/*	 - variables - the number of variables in the problem
+			- m_nnz - the number of nonzeros in the M matrix
+			- m_i - a vector of size m_nnz containing the row indices for M
+			- m_j - a vector of size m_nnz containing the column indices for M
+			- m_ij - a vector of size m_nnz containing the data for M
+			- q - a vector of size variables
+			- lb - a vector of size variables containing the lower bounds on x
+			- ub - a vector of size variables containing the upper bounds on x
+			*/
+		btAlignedObjectArray<double> values;
+		btAlignedObjectArray<int> rowIndices;
+		btAlignedObjectArray<int> colIndices;
+
+		for (int i=0;i<A.rows();i++)
+		{
+			for (int j=0;j<A.cols();j++)
+			{
+				if (A(i,j)!=0.f)
+				{
+					//add 1, because Path starts at 1, instead of 0
+					rowIndices.push_back(i+1);
+					colIndices.push_back(j+1);
+					values.push_back(A(i,j));
+				}
+			}
+		}
+		int numNonZero = rowIndices.size();
+		btAlignedObjectArray<double> zResult;
+		zResult.resize(numVariables);
+		btAlignedObjectArray<double> rhs;
+		btAlignedObjectArray<double> upperBounds;
+		btAlignedObjectArray<double> lowerBounds;
+		for (int i=0;i<numVariables;i++)
+		{
+			upperBounds.push_back(hi[i]);
+			lowerBounds.push_back(lo[i]);
+			rhs.push_back(-b[i]);
+		}
+
+
+		SimpleLCP(numVariables,numNonZero,&rowIndices[0],&colIndices[0],&values[0],&rhs[0],&lowerBounds[0],&upperBounds[0], &status, &zResult[0]);
+
+		if (status != MCP_Solved)
+		{
+			static const char* gReturnMsgs[] = {
+				"Invalid return",
+				"MCP_Solved: The problem was solved",
+				"MCP_NoProgress: A stationary point was found",
+				"MCP_MajorIterationLimit: Major iteration limit met",
+				"MCP_MinorIterationLimit: Cumulative minor iteration limit met",
+				"MCP_TimeLimit: Ran out of time",
+				"MCP_UserInterrupt: Control-C, typically",
+				"MCP_BoundError: Problem has a bound error",
+				"MCP_DomainError: Could not find starting point",
+				"MCP_Infeasible: Problem has no solution",
+				"MCP_Error: An error occurred within the code",
+				"MCP_LicenseError: License could not be found",
+				"MCP_OK"
+			};
+
+			printf("ERROR: The PATH MCP solver failed: %s\n", gReturnMsgs[(unsigned int)status]);// << std::endl;
+			printf("using Projected Gauss Seidel fallback\n");
+			
+			return false;
+		} else
+		{
+			for (int i=0;i<numVariables;i++)
+			{
+				x[i] = zResult[i];
+				//check for #NAN
+				if (x[i] != zResult[i])
+					return false;
+			}
+			return true;
+
+		}
+
+	}
+};
+
+#endif //BT_USE_PATH
+
+
+#endif //BT_PATH_SOLVER_H

src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h

@@ -1,86 +1,86 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-///original version written by Erwin Coumans, October 2013
-
-#ifndef BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
-#define BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
-
-
-#include "btMLCPSolverInterface.h"
-
-///This solver is mainly for debug/learning purposes: it is functionally equivalent to the btSequentialImpulseConstraintSolver solver, but much slower (it builds the full LCP matrix)
-class btSolveProjectedGaussSeidel : public btMLCPSolverInterface
-{
-public:
-	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
-	{
-		if (!A.rows())
-			return true;
-		//the A matrix is sparse, so compute the non-zero elements
-		A.rowComputeNonZeroElements();
-
-		//A is a m-n matrix, m rows, n columns
-		btAssert(A.rows() == b.rows());
-
-		int i, j, numRows = A.rows();
-	
-		float delta;
-
-		for (int k = 0; k <numIterations; k++)
-		{
-			for (i = 0; i <numRows; i++)
-			{
-				delta = 0.0f;
-				if (useSparsity)
-				{
-					for (int h=0;h<A.m_rowNonZeroElements1[i].size();h++)
-					{
-						int j = A.m_rowNonZeroElements1[i][h];
-						if (j != i)//skip main diagonal
-						{
-							delta += A(i,j) * x[j];
-						}
-					}
-				} else
-				{
-					for (j = 0; j <i; j++) 
-						delta += A(i,j) * x[j];
-					for (j = i+1; j<numRows; j++) 
-						delta += A(i,j) * x[j];
-				}
-
-				float aDiag = A(i,i);
-				x [i] = (b [i] - delta) / A(i,i);
-				float s = 1.f;
-
-				if (limitDependency[i]>=0)
-				{
-					s = x[limitDependency[i]];
-					if (s<0)
-						s=1;
-				}
-			
-				if (x[i]<lo[i]*s)
-					x[i]=lo[i]*s;
-				if (x[i]>hi[i]*s)
-					x[i]=hi[i]*s;
-			}
-		}
-		return true;
-	}
-
-};
-
-#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+///original version written by Erwin Coumans, October 2013
+
+#ifndef BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
+#define BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
+
+
+#include "btMLCPSolverInterface.h"
+
+///This solver is mainly for debug/learning purposes: it is functionally equivalent to the btSequentialImpulseConstraintSolver solver, but much slower (it builds the full LCP matrix)
+class btSolveProjectedGaussSeidel : public btMLCPSolverInterface
+{
+public:
+	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
+	{
+		if (!A.rows())
+			return true;
+		//the A matrix is sparse, so compute the non-zero elements
+		A.rowComputeNonZeroElements();
+
+		//A is a m-n matrix, m rows, n columns
+		btAssert(A.rows() == b.rows());
+
+		int i, j, numRows = A.rows();
+	
+		float delta;
+
+		for (int k = 0; k <numIterations; k++)
+		{
+			for (i = 0; i <numRows; i++)
+			{
+				delta = 0.0f;
+				if (useSparsity)
+				{
+					for (int h=0;h<A.m_rowNonZeroElements1[i].size();h++)
+					{
+						int j = A.m_rowNonZeroElements1[i][h];
+						if (j != i)//skip main diagonal
+						{
+							delta += A(i,j) * x[j];
+						}
+					}
+				} else
+				{
+					for (j = 0; j <i; j++) 
+						delta += A(i,j) * x[j];
+					for (j = i+1; j<numRows; j++) 
+						delta += A(i,j) * x[j];
+				}
+
+				float aDiag = A(i,i);
+				x [i] = (b [i] - delta) / aDiag;
+				float s = 1.f;
+
+				if (limitDependency[i]>=0)
+				{
+					s = x[limitDependency[i]];
+					if (s<0)
+						s=1;
+				}
+			
+				if (x[i]<lo[i]*s)
+					x[i]=lo[i]*s;
+				if (x[i]>hi[i]*s)
+					x[i]=hi[i]*s;
+			}
+		}
+		return true;
+	}
+
+};
+
+#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H

src/BulletDynamics/Vehicle/btRaycastVehicle.cpp

@@ -296,8 +296,9 @@ void btRaycastVehicle::updateVehicle( btScalar step )
 	int i=0;
 	for (i=0;i<m_wheelInfo.size();i++)
 	{
-		btScalar depth; 
-		depth = rayCast( m_wheelInfo[i]);
+		//btScalar depth; 
+		//depth = 
+		rayCast( m_wheelInfo[i]);
 	}
 
 	updateSuspension(step);

src/LinearMath/btMatrixX.h

@@ -354,11 +354,11 @@ struct btMatrixX
 				for (int i=0; i < res.rows(); ++i)
 				{
 					T dotProd=0;
-					T dotProd2=0;
-					int waste=0,waste2=0;
+//					T dotProd2=0;
+					//int waste=0,waste2=0;
 
 					{
-						bool useOtherCol = true;
+//						bool useOtherCol = true;
 						{
 							for (int v=0;v<rows();v++)
 							{