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(glGetError()==GL_NO_ERROR);
-	assert(err==GL_NO_ERROR);
 	
-	err = glGetError();
-	assert(err==GL_NO_ERROR);
 	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, m_width,m_height,0,GL_RGBA,GL_UNSIGNED_BYTE,&m_imageData[0]);
 	glGenerateMipmap(GL_TEXTURE_2D);
 	
-	err = glGetError();
+	assert(glGetError()==GL_NO_ERROR);
-	assert(err==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;
+	//	Gwen::Controls::CheckBox* labeled = (Gwen::Controls::CheckBox* )pControl;
-		bool checked = labeled->IsChecked();
+//		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::Controls::Label* label = (Gwen::Controls::Label*) pControl;
-		Gwen::UnicodeString la = label->GetText();// node->GetButton()->GetName();// GetText();
+	//	Gwen::UnicodeString la = label->GetText();// node->GetButton()->GetName();// GetText();
-		Gwen::String laa = Gwen::Utility::UnicodeToString(la);
+	//	Gwen::String laa = Gwen::Utility::UnicodeToString(la);
-		const char* ha = laa.c_str();
+	//	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(glGetError()==GL_NO_ERROR);
-    assert(err==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];
+	//char nodeText[1024];
-	int curDemo = 0;
+	//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(glGetError()==GL_NO_ERROR);
-		assert(err==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 pos[]={0,0,0};
-	float orn[]={0,0,0,1};
+	//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);
+//	btVector3 offset(0,2,0);
-	RagDoll2* doll = new RagDoll2(m_dynamicsWorld,offset,m_glApp);
+	//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 minCoordViewableWindow = curAmount*contSize;
-			float maxCoordViewableWindow = minCoordViewableWindow+viewSize;
+			//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 minCoordViewableWindow = curAmount*contSize;
-			float maxCoordViewableWindow = minCoordViewableWindow+viewSize;
+			//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 minCoordViewableWindow = curAmount*contSize;
-		float maxCoordViewableWindow = minCoordViewableWindow+viewSize;
+//		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 minCoordViewableWindow = curAmount*contSize;
-		float maxCoordViewableWindow = minCoordViewableWindow+viewSize;
+	//	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(glGetError()==GL_NO_ERROR);
-    assert(err==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(glGetError()==GL_NO_ERROR);
-    assert(err==GL_NO_ERROR);
+  
-    
     
     //   glBindTexture(GL_TEXTURE_2D,m_texturehandle);
     
     
-    err = glGetError();
+       assert(glGetError()==GL_NO_ERROR);
-    assert(err==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(glGetError()==GL_NO_ERROR);
-    assert(err==GL_NO_ERROR);
+   
-	err = glGetError();
-    assert(err==GL_NO_ERROR);
-    
     glEnableVertexAttribArray(data->m_positionAttribute);
-	err = glGetError();
+    assert(glGetError()==GL_NO_ERROR);
-    assert(err==GL_NO_ERROR);
     
     glEnableVertexAttribArray(data->m_colourAttribute);
-	err = glGetError();
+    assert(glGetError()==GL_NO_ERROR);
-    assert(err==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(glGetError()==GL_NO_ERROR);
-    assert(err==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;
+    assert(glGetError()==GL_NO_ERROR);
-    
-    err = glGetError();
-    assert(err==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(glGetError()==GL_NO_ERROR);
-		assert(err==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(glGetError()==GL_NO_ERROR);
-	    assert(err==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(glGetError()==GL_NO_ERROR);
-			assert(err==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(glGetError()==GL_NO_ERROR);
-			assert(err==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(glGetError()==GL_NO_ERROR);
-				assert(err==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(glGetError()==GL_NO_ERROR);
-				assert(err==GL_NO_ERROR);
+	}
-			}
 		} else
 		{
 			//delete texture
@@ -196,13 +178,10 @@ void InternalOpenGL2RenderCallbacks::render(sth_texture* texture)
 
 	GLuint* texId = (GLuint*) texture->m_userData;
 
-    GLint err;
+    assert(glGetError()==GL_NO_ERROR);
-	err = glGetError();
-    assert(err==GL_NO_ERROR);
             
     glActiveTexture(GL_TEXTURE0);
-    err = glGetError();
+    assert(glGetError()==GL_NO_ERROR);
-    assert(err==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(glGetError()==GL_NO_ERROR);
-    assert(err==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(glGetError()==GL_NO_ERROR);
-    assert(err==GL_NO_ERROR);
             
     glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, s_indexBuffer);
             
     //glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
     int indexCount = texture->nverts;
-    err = glGetError();
+    assert(glGetError()==GL_NO_ERROR);
-    assert(err==GL_NO_ERROR);
             
     glDrawElements(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, 0);
-    err = glGetError();
+    assert(glGetError()==GL_NO_ERROR);
-    assert(err==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(glGetError()==GL_NO_ERROR);
-    assert(err==GL_NO_ERROR);
     
     
     
@@ -143,16 +142,14 @@ void GLPrimitiveRenderer::loadBufferData()
     
     glEnableVertexAttribArray(m_data->m_positionAttribute);
     glEnableVertexAttribArray(m_data->m_colourAttribute);
-	err = glGetError();
+	assert(glGetError()==GL_NO_ERROR);
-    assert(err==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(glGetError()==GL_NO_ERROR);
-    assert(err==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(glGetError()==GL_NO_ERROR);
-    assert(err==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;
+	assert(glGetError()==GL_NO_ERROR);
-    
-    err = glGetError();
-    assert(err==GL_NO_ERROR);
 	glActiveTexture(GL_TEXTURE0);
-	err = glGetError();
+	assert(glGetError()==GL_NO_ERROR);
-    assert(err==GL_NO_ERROR);
 	
 	 glBindTexture(GL_TEXTURE_2D,m_data->m_texturehandle);
-	err = glGetError();
+	assert(glGetError()==GL_NO_ERROR);
-    assert(err==GL_NO_ERROR);
-
 	drawTexturedRect(x0,y0,x1,y1,color,0,0,1,1);
-	err = glGetError();
+	assert(glGetError()==GL_NO_ERROR);
-    assert(err==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;
+    assert(glGetError()==GL_NO_ERROR);
-    
-    err = glGetError();
-    assert(err==GL_NO_ERROR);
    
     
     glUseProgram(m_data->m_shaderProg);
     
-    err = glGetError();
+    assert(glGetError()==GL_NO_ERROR);
-    assert(err==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(glGetError()==GL_NO_ERROR);
-    assert(err==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(glGetError()==GL_NO_ERROR);
-    assert(err==GL_NO_ERROR);
-	err = glGetError();
-    assert(err==GL_NO_ERROR);
     
     glEnableVertexAttribArray(m_data->m_positionAttribute);
-	err = glGetError();
+	assert(glGetError()==GL_NO_ERROR);
-    assert(err==GL_NO_ERROR);
     
     glEnableVertexAttribArray(m_data->m_colourAttribute);
-	err = glGetError();
+	assert(glGetError()==GL_NO_ERROR);
-    assert(err==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(glGetError()==GL_NO_ERROR);
-    assert(err==GL_NO_ERROR);
     
     glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_data->m_indexBuffer);
     
     //glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
     int indexCount = 6;
-    err = glGetError();
+    assert(glGetError()==GL_NO_ERROR);
-    assert(err==GL_NO_ERROR);
     
 	
     glDrawElements(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, 0);
-    err = glGetError();
+    assert(glGetError()==GL_NO_ERROR);
-    assert(err==GL_NO_ERROR);
 	
 	
     glBindVertexArray(0);
-    err = glGetError();
+    assert(glGetError()==GL_NO_ERROR);
-    assert(err==GL_NO_ERROR);
 
 	glBindBuffer(GL_ARRAY_BUFFER, 0);
-    err = glGetError();
+    assert(glGetError()==GL_NO_ERROR);
-    assert(err==GL_NO_ERROR);
 
 	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
-    err = glGetError();
+    assert(glGetError()==GL_NO_ERROR);
-    assert(err==GL_NO_ERROR);
 
 	//glDisableVertexAttribArray(m_data->m_textureAttribute);
-    err = glGetError();
+    assert(glGetError()==GL_NO_ERROR);
-    assert(err==GL_NO_ERROR);
 
 	glUseProgram(0);
    
-    err = glGetError();
+    assert(glGetError()==GL_NO_ERROR);
-    assert(err==GL_NO_ERROR);
 
 }
 

btgui/OpenGLWindow/GwenOpenGL3CoreRenderer.h

@@ -119,55 +119,37 @@ public:
 	{
 		m_yOffset=0;
 		glEnable(GL_BLEND);
-		GLint err = glGetError();
+	  assert(glGetError()==GL_NO_ERROR);
-		assert(err==GL_NO_ERROR);
+
 
 		glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
 		
-		err = glGetError();
+		assert(glGetError()==GL_NO_ERROR);
-		assert(err==GL_NO_ERROR);
 
-		err = glGetError();
+		assert(glGetError()==GL_NO_ERROR);
-		assert(err==GL_NO_ERROR);
         
 		glDisable(GL_DEPTH_TEST);
-		err = glGetError();
+		assert(glGetError()==GL_NO_ERROR);      
-		assert(err==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(glGetError()==GL_NO_ERROR);
-		assert(err==GL_NO_ERROR);
-
 			
-		err = glGetError();
-		assert(err==GL_NO_ERROR);
 
 		glDisable(GL_CULL_FACE);
 
 		glDisable(GL_DEPTH_TEST);
-		err = glGetError();
+		assert(glGetError()==GL_NO_ERROR);
-		assert(err==GL_NO_ERROR);
 
-		err = glGetError();
+		        
-		assert(err==GL_NO_ERROR);
-            
 		glEnable(GL_BLEND);
 
-            
+		assert(glGetError()==GL_NO_ERROR);        
-		err = glGetError();
-		assert(err==GL_NO_ERROR);
-            
 	}
 	virtual void End()
 	{
@@ -375,10 +357,7 @@ public:
 	glBindTexture(GL_TEXTURE_2D,texHandle);
 //		glDisable(GL_DEPTH_TEST);
 
-		GLint err;
+		assert(glGetError()==GL_NO_ERROR);
-		
-		err = glGetError();
-		assert(err==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(glGetError()==GL_NO_ERROR);
-		assert(err==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;
+  assert(glGetError()==GL_NO_ERROR);
-     err = glGetError();
-    assert(err==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(glGetError()==GL_NO_ERROR);
-	assert(err==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;
+			//btScalar hitDistance;
-			hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
+			//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 b1Axis1(0,0,0),b1Axis2(0,0,0),b1Axis3(0,0,0);
-	btVector3 b2Axis1,b2Axis2;
+	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 trACur = m_rbA.getCenterOfMassTransform();
-	btTransform trBCur = m_rbB.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
+Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
-
+
-This software is provided 'as-is', without any express or implied warranty.
+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.
+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, 
+Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
+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.
+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.
+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.
+3. This notice may not be removed or altered from any source distribution.
-*/
+*/
-
+
-
+
-#include "btFixedConstraint.h"
+#include "btFixedConstraint.h"
-#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
-#include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btTransformUtil.h"
-#include <new>
+#include <new>
-
+
-
+
-btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB)
+btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB)
-:btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB)
+:btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB)
-{
+{
-	m_frameInA = frameInA;
+	m_frameInA = frameInA;
-	m_frameInB = frameInB;
+	m_frameInB = frameInB;
-
+
-}
+}
-
+
-btFixedConstraint::~btFixedConstraint ()
+btFixedConstraint::~btFixedConstraint ()
-{
+{
-}
+}
-
+
-	
+	
-void btFixedConstraint::getInfo1 (btConstraintInfo1* info)
+void btFixedConstraint::getInfo1 (btConstraintInfo1* info)
-{
+{
-	info->m_numConstraintRows = 6;
+	info->m_numConstraintRows = 6;
-	info->nub = 0;
+	info->nub = 0;
-}
+}
-
+
-void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
+void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
-{
+{
-	//fix the 3 linear degrees of freedom
+	//fix the 3 linear degrees of freedom
-
+
-	const btTransform& transA = m_rbA.getCenterOfMassTransform();
+	const btTransform& transA = m_rbA.getCenterOfMassTransform();
-	const btTransform& transB = m_rbB.getCenterOfMassTransform();
+	const btTransform& transB = m_rbB.getCenterOfMassTransform();
-
+
-	const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin();
+	const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin();
-	const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis();
+	const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis();
-	const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin();
+	const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin();
-	const btMatrix3x3& worldOrnB = m_rbB.getCenterOfMassTransform().getBasis();
+	const btMatrix3x3& worldOrnB = m_rbB.getCenterOfMassTransform().getBasis();
-	
+	
-
+
-	info->m_J1linearAxis[0] = 1;
+	info->m_J1linearAxis[0] = 1;
-	info->m_J1linearAxis[info->rowskip+1] = 1;
+	info->m_J1linearAxis[info->rowskip+1] = 1;
-	info->m_J1linearAxis[2*info->rowskip+2] = 1;
+	info->m_J1linearAxis[2*info->rowskip+2] = 1;
-
+
-	btVector3 a1 = worldOrnA * m_frameInA.getOrigin();
+	btVector3 a1 = worldOrnA * m_frameInA.getOrigin();
-    {
+    {
-		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
+		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
-		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
+		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
-		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
-		btVector3 a1neg = -a1;
+		btVector3 a1neg = -a1;
-		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
-	}
+	}
-
+
-	if (info->m_J2linearAxis)
+	if (info->m_J2linearAxis)
-	{
+	{
-		info->m_J2linearAxis[0] = -1;
+		info->m_J2linearAxis[0] = -1;
-		info->m_J2linearAxis[info->rowskip+1] = -1;
+		info->m_J2linearAxis[info->rowskip+1] = -1;
-		info->m_J2linearAxis[2*info->rowskip+2] = -1;
+		info->m_J2linearAxis[2*info->rowskip+2] = -1;
-	}
+	}
-	
+	
-	btVector3 a2 = worldOrnB*m_frameInB.getOrigin();
+	btVector3 a2 = worldOrnB*m_frameInB.getOrigin();
-   {
+   {
-		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
+		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
-		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
+		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
-		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
-		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
-	}
+	}
-
+
-    // set right hand side for the linear dofs
+    // set right hand side for the linear dofs
-	btScalar k = info->fps * info->erp;
+	btScalar k = info->fps * info->erp;
-	
+	
-	btVector3 linearError = k*(a2+worldPosB-a1-worldPosA);
+	btVector3 linearError = k*(a2+worldPosB-a1-worldPosA);
-    int j;
+    int j;
-	for (j=0; j<3; j++)
+	for (j=0; j<3; j++)
-    {
+    {
-        info->m_constraintError[j*info->rowskip] = linearError[j];
+        info->m_constraintError[j*info->rowskip] = linearError[j];
-		//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
+		//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
-    }
+    }
-
+
-	btVector3 ivA = transA.getBasis() * m_frameInA.getBasis().getColumn(0);
+	btVector3 ivA = transA.getBasis() * m_frameInA.getBasis().getColumn(0);
-	btVector3 jvA = transA.getBasis() * m_frameInA.getBasis().getColumn(1);
+	btVector3 jvA = transA.getBasis() * m_frameInA.getBasis().getColumn(1);
-	btVector3 kvA = transA.getBasis() * m_frameInA.getBasis().getColumn(2);
+	btVector3 kvA = transA.getBasis() * m_frameInA.getBasis().getColumn(2);
-	btVector3 ivB = transB.getBasis() * m_frameInB.getBasis().getColumn(0);
+	btVector3 ivB = transB.getBasis() * m_frameInB.getBasis().getColumn(0);
-	btVector3 target;
+	btVector3 target;
-	btScalar x = ivB.dot(ivA);
+	//btScalar x = ivB.dot(ivA);//??
-	btScalar y = ivB.dot(jvA);
+	btScalar y = ivB.dot(jvA);
-	btScalar z = ivB.dot(kvA);
+	btScalar z = ivB.dot(kvA);
-	btVector3 swingAxis(0,0,0);
+	btVector3 swingAxis(0,0,0);
-	{ 
+	{ 
-		if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
+		if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
-		{
+		{
-			swingAxis = -ivB.cross(ivA);
+			swingAxis = -ivB.cross(ivA);
-		}
+		}
-	}
+	}
-	btVector3 vTwist(1,0,0);
+	btVector3 vTwist(1,0,0);
-
+
-	// compute rotation of A wrt B (in constraint space)
+	// compute rotation of A wrt B (in constraint space)
-	btQuaternion qA = transA.getRotation() * m_frameInA.getRotation();
+	btQuaternion qA = transA.getRotation() * m_frameInA.getRotation();
-	btQuaternion qB = transB.getRotation() * m_frameInB.getRotation();
+	btQuaternion qB = transB.getRotation() * m_frameInB.getRotation();
-	btQuaternion qAB = qB.inverse() * qA;
+	btQuaternion qAB = qB.inverse() * qA;
-	// split rotation into cone and twist
+	// split rotation into cone and twist
-	// (all this is done from B's perspective. Maybe I should be averaging axes...)
+	// (all this is done from B's perspective. Maybe I should be averaging axes...)
-	btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
+	btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
-	btQuaternion qABCone  = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
+	btQuaternion qABCone  = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
-	btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
+	btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
-
+
-	int row = 3;
+	int row = 3;
-    int srow = row * info->rowskip;
+    int srow = row * info->rowskip;
-	btVector3 ax1;
+	btVector3 ax1;
-	// angular limits
+	// angular limits
-	{
+	{
-		btScalar *J1 = info->m_J1angularAxis;
+		btScalar *J1 = info->m_J1angularAxis;
-		btScalar *J2 = info->m_J2angularAxis;
+		btScalar *J2 = info->m_J2angularAxis;
-		btTransform trA = transA*m_frameInA;
+		btTransform trA = transA*m_frameInA;
-		btVector3 twistAxis = trA.getBasis().getColumn(0);
+		btVector3 twistAxis = trA.getBasis().getColumn(0);
-
+
-		btVector3 p = trA.getBasis().getColumn(1);
+		btVector3 p = trA.getBasis().getColumn(1);
-		btVector3 q = trA.getBasis().getColumn(2);
+		btVector3 q = trA.getBasis().getColumn(2);
-		int srow1 = srow + info->rowskip;
+		int srow1 = srow + info->rowskip;
-		J1[srow+0] = p[0];
+		J1[srow+0] = p[0];
-		J1[srow+1] = p[1];
+		J1[srow+1] = p[1];
-		J1[srow+2] = p[2];
+		J1[srow+2] = p[2];
-		J1[srow1+0] = q[0];
+		J1[srow1+0] = q[0];
-		J1[srow1+1] = q[1];
+		J1[srow1+1] = q[1];
-		J1[srow1+2] = q[2];
+		J1[srow1+2] = q[2];
-		J2[srow+0] = -p[0];
+		J2[srow+0] = -p[0];
-		J2[srow+1] = -p[1];
+		J2[srow+1] = -p[1];
-		J2[srow+2] = -p[2];
+		J2[srow+2] = -p[2];
-		J2[srow1+0] = -q[0];
+		J2[srow1+0] = -q[0];
-		J2[srow1+1] = -q[1];
+		J2[srow1+1] = -q[1];
-		J2[srow1+2] = -q[2];
+		J2[srow1+2] = -q[2];
-		btScalar fact = info->fps;
+		btScalar fact = info->fps;
-		info->m_constraintError[srow] =   fact * swingAxis.dot(p);
+		info->m_constraintError[srow] =   fact * swingAxis.dot(p);
-		info->m_constraintError[srow1] =  fact * swingAxis.dot(q);
+		info->m_constraintError[srow1] =  fact * swingAxis.dot(q);
-		info->m_lowerLimit[srow] = -SIMD_INFINITY;
+		info->m_lowerLimit[srow] = -SIMD_INFINITY;
-		info->m_upperLimit[srow] = SIMD_INFINITY;
+		info->m_upperLimit[srow] = SIMD_INFINITY;
-		info->m_lowerLimit[srow1] = -SIMD_INFINITY;
+		info->m_lowerLimit[srow1] = -SIMD_INFINITY;
-		info->m_upperLimit[srow1] = SIMD_INFINITY;
+		info->m_upperLimit[srow1] = SIMD_INFINITY;
-		srow = srow1 + info->rowskip;
+		srow = srow1 + info->rowskip;
-
+
-		{
+		{
-			btQuaternion qMinTwist = qABTwist;
+			btQuaternion qMinTwist = qABTwist;
-			btScalar twistAngle = qABTwist.getAngle();
+			btScalar twistAngle = qABTwist.getAngle();
-
+
-			if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
+			if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
-			{
+			{
-				qMinTwist = -(qABTwist);
+				qMinTwist = -(qABTwist);
-				twistAngle = qMinTwist.getAngle();
+				twistAngle = qMinTwist.getAngle();
-			}
+			}
-
+
-			if (twistAngle > SIMD_EPSILON)
+			if (twistAngle > SIMD_EPSILON)
-			{
+			{
-				twistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
+				twistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
-				twistAxis.normalize();
+				twistAxis.normalize();
-				twistAxis = quatRotate(qB, -twistAxis);
+				twistAxis = quatRotate(qB, -twistAxis);
-			}
+			}
-			ax1 = twistAxis;
+			ax1 = twistAxis;
-			btScalar *J1 = info->m_J1angularAxis;
+			btScalar *J1 = info->m_J1angularAxis;
-			btScalar *J2 = info->m_J2angularAxis;
+			btScalar *J2 = info->m_J2angularAxis;
-			J1[srow+0] = ax1[0];
+			J1[srow+0] = ax1[0];
-			J1[srow+1] = ax1[1];
+			J1[srow+1] = ax1[1];
-			J1[srow+2] = ax1[2];
+			J1[srow+2] = ax1[2];
-			J2[srow+0] = -ax1[0];
+			J2[srow+0] = -ax1[0];
-			J2[srow+1] = -ax1[1];
+			J2[srow+1] = -ax1[1];
-			J2[srow+2] = -ax1[2];
+			J2[srow+2] = -ax1[2];
-			btScalar k = info->fps;
+			btScalar k = info->fps;
-			info->m_constraintError[srow] = k * twistAngle;
+			info->m_constraintError[srow] = k * twistAngle;
-			info->m_lowerLimit[srow] = -SIMD_INFINITY;
+			info->m_lowerLimit[srow] = -SIMD_INFINITY;
-			info->m_upperLimit[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
+Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
-
+
-This software is provided 'as-is', without any express or implied warranty.
+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.
+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, 
+Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
+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.
+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.
+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.
+3. This notice may not be removed or altered from any source distribution.
-*/
+*/
-
+
-#ifndef BT_FIXED_CONSTRAINT_H
+#ifndef BT_FIXED_CONSTRAINT_H
-#define BT_FIXED_CONSTRAINT_H
+#define BT_FIXED_CONSTRAINT_H
-
+
-#include "btTypedConstraint.h"
+#include "btTypedConstraint.h"
-
+
-ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint
+ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint
-{
+{
-
+
-	btTransform m_frameInA;
+	btTransform m_frameInA;
-	btTransform m_frameInB;
+	btTransform m_frameInB;
-public:
+public:
-	btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB);
+	btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB);
-	
+	
-	virtual ~btFixedConstraint();
+	virtual ~btFixedConstraint();
-
+
-	
+	
-	virtual void getInfo1 (btConstraintInfo1* info);
+	virtual void getInfo1 (btConstraintInfo1* info);
-
+
-	virtual void getInfo2 (btConstraintInfo2* info);
+	virtual void getInfo2 (btConstraintInfo2* info);
-
+
-	virtual	void	setParam(int num, btScalar value, int axis = -1)
+	virtual	void	setParam(int num, btScalar value, int axis = -1)
-	{
+	{
-		btAssert(0);
+		btAssert(0);
-	}
+	}
-	virtual	btScalar getParam(int num, int axis = -1) const
+	virtual	btScalar getParam(int num, int axis = -1) const
-	{
+	{
-		btAssert(0);
+		btAssert(0);
-		return 0.f;
+		return 0.f;
-	}
+	}
-
+
-};
+};
-
+
-#endif //BT_FIXED_CONSTRAINT_H
+#endif //BT_FIXED_CONSTRAINT_H

src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp

@@ -1,54 +1,54 @@
-/*
+/*
-Bullet Continuous Collision Detection and Physics Library
+Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2012 Advanced Micro Devices, Inc.  http://bulletphysics.org
+Copyright (c) 2012 Advanced Micro Devices, Inc.  http://bulletphysics.org
-
+
-This software is provided 'as-is', without any express or implied warranty.
+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.
+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, 
+Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
+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.
+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.
+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.
+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.
+/// Implemented by Erwin Coumans. The idea for the constraint comes from Dimitris Papavasiliou.
-
+
-#include "btGearConstraint.h"
+#include "btGearConstraint.h"
-
+
-btGearConstraint::btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio)
+btGearConstraint::btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio)
-:btTypedConstraint(GEAR_CONSTRAINT_TYPE,rbA,rbB),
+:btTypedConstraint(GEAR_CONSTRAINT_TYPE,rbA,rbB),
-m_axisInA(axisInA),
+m_axisInA(axisInA),
-m_axisInB(axisInB),
+m_axisInB(axisInB),
-m_ratio(ratio)
+m_ratio(ratio)
-{
+{
-}
+}
-
+
-btGearConstraint::~btGearConstraint ()
+btGearConstraint::~btGearConstraint ()
-{
+{
-}
+}
-
+
-void btGearConstraint::getInfo1 (btConstraintInfo1* info)
+void btGearConstraint::getInfo1 (btConstraintInfo1* info)
-{
+{
-	info->m_numConstraintRows = 1;
+	info->m_numConstraintRows = 1;
-	info->nub = 1;
+	info->nub = 1;
-}
+}
-
+
-void btGearConstraint::getInfo2 (btConstraintInfo2* info)
+void btGearConstraint::getInfo2 (btConstraintInfo2* info)
-{
+{
-	btVector3 globalAxisA, globalAxisB;
+	btVector3 globalAxisA, globalAxisB;
-
+
-	globalAxisA = m_rbA.getWorldTransform().getBasis()*this->m_axisInA;
+	globalAxisA = m_rbA.getWorldTransform().getBasis()*this->m_axisInA;
-	globalAxisB = m_rbB.getWorldTransform().getBasis()*this->m_axisInB;
+	globalAxisB = m_rbB.getWorldTransform().getBasis()*this->m_axisInB;
-
+
-	info->m_J1angularAxis[0] = globalAxisA[0];
+	info->m_J1angularAxis[0] = globalAxisA[0];
-	info->m_J1angularAxis[1] = globalAxisA[1];
+	info->m_J1angularAxis[1] = globalAxisA[1];
-	info->m_J1angularAxis[2] = globalAxisA[2];
+	info->m_J1angularAxis[2] = globalAxisA[2];
-
+
-	info->m_J2angularAxis[0] = m_ratio*globalAxisB[0];
+	info->m_J2angularAxis[0] = m_ratio*globalAxisB[0];
-	info->m_J2angularAxis[1] = m_ratio*globalAxisB[1];
+	info->m_J2angularAxis[1] = m_ratio*globalAxisB[1];
-	info->m_J2angularAxis[2] = m_ratio*globalAxisB[2];
+	info->m_J2angularAxis[2] = m_ratio*globalAxisB[2];
-
+
-}
+}
-
+

src/BulletDynamics/ConstraintSolver/btGearConstraint.h

@@ -1,79 +1,79 @@
-/*
+/*
-Bullet Continuous Collision Detection and Physics Library
+Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2012 Advanced Micro Devices, Inc.  http://bulletphysics.org
+Copyright (c) 2012 Advanced Micro Devices, Inc.  http://bulletphysics.org
-
+
-This software is provided 'as-is', without any express or implied warranty.
+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.
+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, 
+Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
+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.
+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.
+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.
+3. This notice may not be removed or altered from any source distribution.
-*/
+*/
-
+
-
+
-
+
-#ifndef BT_GEAR_CONSTRAINT_H
+#ifndef BT_GEAR_CONSTRAINT_H
-#define BT_GEAR_CONSTRAINT_H
+#define BT_GEAR_CONSTRAINT_H
-
+
-#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
-
+
-
+
-#ifdef BT_USE_DOUBLE_PRECISION
+#ifdef BT_USE_DOUBLE_PRECISION
-#define btGearConstraintData	btGearConstraintDoubleData
+#define btGearConstraintData	btGearConstraintDoubleData
-#define btGearConstraintDataName	"btGearConstraintDoubleData"
+#define btGearConstraintDataName	"btGearConstraintDoubleData"
-#else
+#else
-#define btGearConstraintData	btGearConstraintFloatData
+#define btGearConstraintData	btGearConstraintFloatData
-#define btGearConstraintDataName	"btGearConstraintFloatData"
+#define btGearConstraintDataName	"btGearConstraintFloatData"
-#endif //BT_USE_DOUBLE_PRECISION
+#endif //BT_USE_DOUBLE_PRECISION
-
+
-
+
-
+
-///The btGeatConstraint will couple the angular velocity for two bodies around given local axis and ratio.
+///The btGeatConstraint will couple the angular velocity for two bodies around given local axis and ratio.
-///See Bullet/Demos/ConstraintDemo for an example use.
+///See Bullet/Demos/ConstraintDemo for an example use.
-class btGearConstraint : public btTypedConstraint
+class btGearConstraint : public btTypedConstraint
-{
+{
-protected:
+protected:
-	btVector3	m_axisInA;
+	btVector3	m_axisInA;
-	btVector3	m_axisInB;
+	btVector3	m_axisInB;
-	bool		m_useFrameA;
+	bool		m_useFrameA;
-	btScalar	m_ratio;
+	btScalar	m_ratio;
-
+
-public:
+public:
-	btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio=1.f);
+	btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio=1.f);
-	virtual ~btGearConstraint ();
+	virtual ~btGearConstraint ();
-
+
-	///internal method used by the constraint solver, don't use them directly
+	///internal method used by the constraint solver, don't use them directly
-	virtual void getInfo1 (btConstraintInfo1* info);
+	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) 
+	void setAxisA(btVector3& axisA) 
-	{
+	{
-		m_axisInA = axisA;
+		m_axisInA = axisA;
 	}
-	void setAxisB(btVector3& axisB)
+	void setAxisB(btVector3& axisB)
-	{
+	{
-		m_axisInB = axisB;
+		m_axisInB = axisB;
 	}
-	void setRatio(btScalar ratio)
+	void setRatio(btScalar ratio)
-	{
+	{
-		m_ratio = ratio;
+		m_ratio = ratio;
 	}
-	const btVector3& getAxisA() const
+	const btVector3& getAxisA() const
-	{
+	{
-		return m_axisInA;
+		return m_axisInA;
 	}
-	const btVector3& getAxisB() const
+	const btVector3& getAxisB() const
-	{
+	{
-		return m_axisInB;
+		return m_axisInB;
 	}
-	btScalar getRatio() const
+	btScalar getRatio() const
-	{
+	{
-		return m_ratio;
+		return m_ratio;
 	}
 
 
@@ -84,69 +84,69 @@ public:
 		(void) axis;
 		btAssert(0);
 	}
-
+
-	///return the local value of parameter
+	///return the local value of parameter
-	virtual	btScalar getParam(int num, int axis = -1) const 
+	virtual	btScalar getParam(int num, int axis = -1) const 
-	{ 
+	{ 
-		(void) num;
+		(void) num;
-		(void) axis;
+		(void) axis;
-		btAssert(0);
+		btAssert(0);
-		return 0.f;
+		return 0.f;
-	}
+	}
-
+
-	virtual	int	calculateSerializeBufferSize() const;
+	virtual	int	calculateSerializeBufferSize() const;
-
+
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
-	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
+	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
-};
+};
-
+
-
+
-
+
-
+
-///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct btGearConstraintFloatData
+struct btGearConstraintFloatData
-{
+{
-	btTypedConstraintFloatData	m_typeConstraintData;
+	btTypedConstraintFloatData	m_typeConstraintData;
-
+
-	btVector3FloatData			m_axisInA;
+	btVector3FloatData			m_axisInA;
-	btVector3FloatData			m_axisInB;
+	btVector3FloatData			m_axisInB;
-
+
-	float							m_ratio;
+	float							m_ratio;
-	char							m_padding[4];
+	char							m_padding[4];
-};
+};
-
+
-struct btGearConstraintDoubleData
+struct btGearConstraintDoubleData
-{
+{
-	btTypedConstraintDoubleData	m_typeConstraintData;
+	btTypedConstraintDoubleData	m_typeConstraintData;
-
+
-	btVector3DoubleData			m_axisInA;
+	btVector3DoubleData			m_axisInA;
-	btVector3DoubleData			m_axisInB;
+	btVector3DoubleData			m_axisInB;
-
+
-	double						m_ratio;
+	double						m_ratio;
-};
+};
-
+
-SIMD_FORCE_INLINE	int	btGearConstraint::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE	int	btGearConstraint::calculateSerializeBufferSize() const
-{
+{
-	return sizeof(btGearConstraintData);
+	return sizeof(btGearConstraintData);
-}
+}
-
+
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
-SIMD_FORCE_INLINE	const char*	btGearConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+SIMD_FORCE_INLINE	const char*	btGearConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
-{
+{
-	btGearConstraintData* gear = (btGearConstraintData*)dataBuffer;
+	btGearConstraintData* gear = (btGearConstraintData*)dataBuffer;
-	btTypedConstraint::serialize(&gear->m_typeConstraintData,serializer);
+	btTypedConstraint::serialize(&gear->m_typeConstraintData,serializer);
-
+
-	m_axisInA.serialize( gear->m_axisInA );
+	m_axisInA.serialize( gear->m_axisInA );
-	m_axisInB.serialize( gear->m_axisInB );
+	m_axisInB.serialize( gear->m_axisInB );
-
+
-	gear->m_ratio = m_ratio;
+	gear->m_ratio = m_ratio;
-
+
-	return btGearConstraintDataName;
+	return btGearConstraintDataName;
-}
+}
-
+
-
+
-
+
-
+
-
+
-
+
-#endif //BT_GEAR_CONSTRAINT_H
+#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
+Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
-
+
-This software is provided 'as-is', without any express or implied warranty.
+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.
+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, 
+Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
+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.
+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.
+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.
+3. This notice may not be removed or altered from any source distribution.
-*/
+*/
-
+
-#include "btNNCGConstraintSolver.h"
+#include "btNNCGConstraintSolver.h"
-
+
-
+
-
+
-
+
-
+
-
+
-btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+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);
+	btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
-
+
-	m_pNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
+	m_pNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
-	m_pC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
+	m_pC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
-	m_pCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
+	m_pCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
-	m_pCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
+	m_pCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
-
+
-	m_deltafNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
+	m_deltafNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
-	m_deltafC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
+	m_deltafC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
-	m_deltafCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
+	m_deltafCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
-	m_deltafCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
+	m_deltafCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
-
+
-	return val;
+	return val;
-}
+}
-
+
-btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
+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 numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
-	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
+	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
-	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
+	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
-
+
-	if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
+	if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
-	{
+	{
-		if (1)			// uncomment this for a bit less random ((iteration & 7) == 0)
+		if (1)			// uncomment this for a bit less random ((iteration & 7) == 0)
-		{
+		{
-
+
-			for (int j=0; j<numNonContactPool; ++j) {
+			for (int j=0; j<numNonContactPool; ++j) {
-				int tmp = m_orderNonContactConstraintPool[j];
+				int tmp = m_orderNonContactConstraintPool[j];
-				int swapi = btRandInt2(j+1);
+				int swapi = btRandInt2(j+1);
-				m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
+				m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
-				m_orderNonContactConstraintPool[swapi] = tmp;
+				m_orderNonContactConstraintPool[swapi] = tmp;
-			}
+			}
-
+
-			//contact/friction constraints are not solved more than 
+			//contact/friction constraints are not solved more than 
-			if (iteration< infoGlobal.m_numIterations)
+			if (iteration< infoGlobal.m_numIterations)
-			{
+			{
-				for (int j=0; j<numConstraintPool; ++j) {
+				for (int j=0; j<numConstraintPool; ++j) {
-					int tmp = m_orderTmpConstraintPool[j];
+					int tmp = m_orderTmpConstraintPool[j];
-					int swapi = btRandInt2(j+1);
+					int swapi = btRandInt2(j+1);
-					m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
+					m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
-					m_orderTmpConstraintPool[swapi] = tmp;
+					m_orderTmpConstraintPool[swapi] = tmp;
-				}
+				}
-
+
-				for (int j=0; j<numFrictionPool; ++j) {
+				for (int j=0; j<numFrictionPool; ++j) {
-					int tmp = m_orderFrictionConstraintPool[j];
+					int tmp = m_orderFrictionConstraintPool[j];
-					int swapi = btRandInt2(j+1);
+					int swapi = btRandInt2(j+1);
-					m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
+					m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
-					m_orderFrictionConstraintPool[swapi] = tmp;
+					m_orderFrictionConstraintPool[swapi] = tmp;
-				}
+				}
-			}
+			}
-		}
+		}
-	}
+	}
-
+
-
+
-	btScalar deltaflengthsqr = 0;
+	btScalar deltaflengthsqr = 0;
-
+
-	if (infoGlobal.m_solverMode & SOLVER_SIMD)
+	if (infoGlobal.m_solverMode & SOLVER_SIMD)
-	{
+	{
-		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
-		{
+		{
-			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
-			if (iteration < constraint.m_overrideNumSolverIterations) 
+			if (iteration < constraint.m_overrideNumSolverIterations) 
-			{
+			{
-				btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+				btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
-				m_deltafNC[j] = deltaf;
+				m_deltafNC[j] = deltaf;
-				deltaflengthsqr += deltaf * deltaf;
+				deltaflengthsqr += deltaf * deltaf;
-			}
+			}
-		}
+		}
-	} else 
+	} else 
-	{
+	{
-		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
-		{
+		{
-			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
-			if (iteration < constraint.m_overrideNumSolverIterations) 
+			if (iteration < constraint.m_overrideNumSolverIterations) 
-			{
+			{
-				btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+				btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
-				m_deltafNC[j] = deltaf;
+				m_deltafNC[j] = deltaf;
-				deltaflengthsqr += deltaf * deltaf;
+				deltaflengthsqr += deltaf * deltaf;
-			}
+			}
-		}
+		}
-	}
+	}
-
+
-
+
-	if (m_onlyForNoneContact) 
+	if (m_onlyForNoneContact) 
-	{
+	{
-		if (iteration==0) 
+		if (iteration==0) 
-		{
+		{
-			for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
+			for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
-		} else {
+		} 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
+			// 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;
+			btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
-			if (beta>1) 
+			if (beta>1) 
-			{
+			{
-				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
+				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
-			} else 
+			} else 
-			{
+			{
-				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
-				{
+				{
-					btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+					btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
-					if (iteration < constraint.m_overrideNumSolverIterations) 
+					if (iteration < constraint.m_overrideNumSolverIterations) 
-					{
+					{
-						btScalar additionaldeltaimpulse = beta * m_pNC[j];
+						btScalar additionaldeltaimpulse = beta * m_pNC[j];
-						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
-						m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
+						m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
-						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
-						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
-						const btSolverConstraint& c = constraint;
+						const btSolverConstraint& c = constraint;
-						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
-						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
-					}
+					}
-				}
+				}
-			}
+			}
-		}
+		}
-		m_deltafLengthSqrPrev = deltaflengthsqr;
+		m_deltafLengthSqrPrev = deltaflengthsqr;
-	}
+	}
-
+
-
+
-
+
-	if (infoGlobal.m_solverMode & SOLVER_SIMD)
+	if (infoGlobal.m_solverMode & SOLVER_SIMD)
-	{
+	{
-
+
-		if (iteration< infoGlobal.m_numIterations)
+		if (iteration< infoGlobal.m_numIterations)
-		{
+		{
-			for (int j=0;j<numConstraints;j++)
+			for (int j=0;j<numConstraints;j++)
-			{
+			{
-				if (constraints[j]->isEnabled())
+				if (constraints[j]->isEnabled())
-				{
+				{
-					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
+					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
-					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
+					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
-					btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
+					btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
-					btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
+					btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
-					constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
+					constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
-				}
+				}
-			}
+			}
-
+
-			///solve all contact constraints using SIMD, if available
+			///solve all contact constraints using SIMD, if available
-			if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
+			if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
-			{
+			{
-				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-				int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
+				int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
-
+
-				for (int c=0;c<numPoolConstraints;c++)
+				for (int c=0;c<numPoolConstraints;c++)
-				{
+				{
-					btScalar totalImpulse =0;
+					btScalar totalImpulse =0;
-
+
-					{
+					{
-						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
+						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
-						btScalar deltaf = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);						
+						btScalar deltaf = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);						
-						m_deltafC[c] = deltaf;
+						m_deltafC[c] = deltaf;
-						deltaflengthsqr += deltaf*deltaf;
+						deltaflengthsqr += deltaf*deltaf;
-						totalImpulse = solveManifold.m_appliedImpulse;
+						totalImpulse = solveManifold.m_appliedImpulse;
-					}
+					}
-					bool applyFriction = true;
+					bool applyFriction = true;
-					if (applyFriction)
+					if (applyFriction)
-					{
+					{
-						{
+						{
-
+
-							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
+							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
-
+
-							if (totalImpulse>btScalar(0))
+							if (totalImpulse>btScalar(0))
-							{
+							{
-								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-								solveManifold.m_upperLimit = 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);
+								btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-								m_deltafCF[c*multiplier] = deltaf;
+								m_deltafCF[c*multiplier] = deltaf;
-								deltaflengthsqr += deltaf*deltaf;
+								deltaflengthsqr += deltaf*deltaf;
-							} else {
+							} else {
-								m_deltafCF[c*multiplier] = 0;
+								m_deltafCF[c*multiplier] = 0;
-							}
+							}
-						}
+						}
-
+
-						if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
+						if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
-						{
+						{
-
+
-							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
+							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
-
+
-							if (totalImpulse>btScalar(0))
+							if (totalImpulse>btScalar(0))
-							{
+							{
-								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-								solveManifold.m_upperLimit = 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);
+								btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-								m_deltafCF[c*multiplier+1] = deltaf;
+								m_deltafCF[c*multiplier+1] = deltaf;
-								deltaflengthsqr += deltaf*deltaf;
+								deltaflengthsqr += deltaf*deltaf;
-							} else {
+							} else {
-								m_deltafCF[c*multiplier+1] = 0;
+								m_deltafCF[c*multiplier+1] = 0;
-							}
+							}
-						}
+						}
-					}
+					}
-				}
+				}
-
+
-			}
+			}
-			else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
+			else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
-			{
+			{
-				//solve the friction constraints after all contact constraints, don't interleave them
+				//solve the friction constraints after all contact constraints, don't interleave them
-				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-				int j;
+				int j;
-
+
-				for (j=0;j<numPoolConstraints;j++)
+				for (j=0;j<numPoolConstraints;j++)
-				{
+				{
-					const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+					const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-					//resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					//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);
+					btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-					m_deltafC[j] = deltaf;
+					m_deltafC[j] = deltaf;
-					deltaflengthsqr += deltaf*deltaf;
+					deltaflengthsqr += deltaf*deltaf;
-				}
+				}
-
+
-
+
-
+
-				///solve all friction constraints, using SIMD, if available
+				///solve all friction constraints, using SIMD, if available
-
+
-				int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+				int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
-				for (j=0;j<numFrictionPoolConstraints;j++)
+				for (j=0;j<numFrictionPoolConstraints;j++)
-				{
+				{
-					btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+					btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
-					btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+					btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
-
+
-					if (totalImpulse>btScalar(0))
+					if (totalImpulse>btScalar(0))
-					{
+					{
-						solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+						solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-						solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+						solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
-
+
-						//resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						//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);
+						btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-						m_deltafCF[j] = deltaf;
+						m_deltafCF[j] = deltaf;
-						deltaflengthsqr += deltaf*deltaf;
+						deltaflengthsqr += deltaf*deltaf;
-					} else {
+					} else {
-						m_deltafCF[j] = 0;
+						m_deltafCF[j] = 0;
-					}
+					}
-				}
+				}
-
+
-
+
-				int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
+				int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
-				for (j=0;j<numRollingFrictionPoolConstraints;j++)
+				for (j=0;j<numRollingFrictionPoolConstraints;j++)
-				{
+				{
-
+
-					btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
+					btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
-					btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
+					btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
-					if (totalImpulse>btScalar(0))
+					if (totalImpulse>btScalar(0))
-					{
+					{
-						btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
+						btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
-						if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
+						if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
-							rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+							rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
-
+
-						rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+						rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
-						rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+						rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
-
+
-						btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+						btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
-						m_deltafCRF[j] = deltaf;
+						m_deltafCRF[j] = deltaf;
-						deltaflengthsqr += deltaf*deltaf;
+						deltaflengthsqr += deltaf*deltaf;
-					} else {
+					} else {
-						m_deltafCRF[j] = 0;
+						m_deltafCRF[j] = 0;
-					}
+					}
-				}
+				}
-
+
-
+
-			}
+			}
-		}
+		}
-
+
-
+
-
+
-	} else
+	} else
-	{
+	{
-
+
-		if (iteration< infoGlobal.m_numIterations)
+		if (iteration< infoGlobal.m_numIterations)
-		{
+		{
-			for (int j=0;j<numConstraints;j++)
+			for (int j=0;j<numConstraints;j++)
-			{
+			{
-				if (constraints[j]->isEnabled())
+				if (constraints[j]->isEnabled())
-				{
+				{
-					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
+					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
-					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
+					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
-					btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
+					btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
-					btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
+					btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
-					constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
+					constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
-				}
+				}
-			}
+			}
-			///solve all contact constraints
+			///solve all contact constraints
-			int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+			int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-			for (int j=0;j<numPoolConstraints;j++)
+			for (int j=0;j<numPoolConstraints;j++)
-			{
+			{
-				const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+				const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-				btScalar deltaf = resolveSingleConstraintRowLowerLimit(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;
+				m_deltafC[j] = deltaf;
-				deltaflengthsqr += deltaf*deltaf;
+				deltaflengthsqr += deltaf*deltaf;
-			}
+			}
-			///solve all friction constraints
+			///solve all friction constraints
-			int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+			int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
-			for (int j=0;j<numFrictionPoolConstraints;j++)
+			for (int j=0;j<numFrictionPoolConstraints;j++)
-			{
+			{
-				btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+				btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
-				btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+				btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
-
+
-				if (totalImpulse>btScalar(0))
+				if (totalImpulse>btScalar(0))
-				{
+				{
-					solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+					solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-					solveManifold.m_upperLimit = 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);
+					btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-					m_deltafCF[j] = deltaf;
+					m_deltafCF[j] = deltaf;
-					deltaflengthsqr += deltaf*deltaf;
+					deltaflengthsqr += deltaf*deltaf;
-				} else {
+				} else {
-					m_deltafCF[j] = 0;
+					m_deltafCF[j] = 0;
-				}
+				}
-			}
+			}
-
+
-			int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
+			int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
-			for (int j=0;j<numRollingFrictionPoolConstraints;j++)
+			for (int j=0;j<numRollingFrictionPoolConstraints;j++)
-			{
+			{
-				btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
+				btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
-				btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
+				btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
-				if (totalImpulse>btScalar(0))
+				if (totalImpulse>btScalar(0))
-				{
+				{
-					btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
+					btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
-					if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
+					if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
-						rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+						rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
-
+
-					rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+					rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
-					rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+					rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
-
+
-					btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+					btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
-					m_deltafCRF[j] = deltaf;
+					m_deltafCRF[j] = deltaf;
-					deltaflengthsqr += deltaf*deltaf;
+					deltaflengthsqr += deltaf*deltaf;
-				} else {
+				} else {
-					m_deltafCRF[j] = 0;
+					m_deltafCRF[j] = 0;
-				}
+				}
-			}
+			}
-		}
+		}
-	}
+	}
-
+
-
+
-
+
-
+
-	if (!m_onlyForNoneContact) 
+	if (!m_onlyForNoneContact) 
-	{
+	{
-		if (iteration==0) 
+		if (iteration==0) 
-		{
+		{
-			for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
+			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_tmpSolverContactConstraintPool.size();j++) m_pC[j] = m_deltafC[j];
-			for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = m_deltafCF[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 ) 
+			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];
+				for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = m_deltafCRF[j];
-			}
+			}
-		} else 
+		} 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
+			// 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;
+			btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
-			if (beta>1) {
+			if (beta>1) {
-				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
+				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_tmpSolverContactConstraintPool.size();j++) m_pC[j] = 0;
-				for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[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 ) {
+				if ( (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) {
-					for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = 0;
+					for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = 0;
-				}
+				}
-			} else {
+			} else {
-				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
-				{
+				{
-					btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+					btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
-					if (iteration < constraint.m_overrideNumSolverIterations) {
+					if (iteration < constraint.m_overrideNumSolverIterations) {
-						btScalar additionaldeltaimpulse = beta * m_pNC[j];
+						btScalar additionaldeltaimpulse = beta * m_pNC[j];
-						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
-						m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
+						m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
-						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
-						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
-						const btSolverConstraint& c = constraint;
+						const btSolverConstraint& c = constraint;
-						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
-						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
-					}
+					}
-				}
+				}
-				for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++)
+				for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++)
-				{
+				{
-					btSolverConstraint& constraint = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+					btSolverConstraint& constraint = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-					if (iteration< infoGlobal.m_numIterations) {
+					if (iteration< infoGlobal.m_numIterations) {
-						btScalar additionaldeltaimpulse = beta * m_pC[j];
+						btScalar additionaldeltaimpulse = beta * m_pC[j];
-						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
-						m_pC[j] = beta * m_pC[j] + m_deltafC[j];
+						m_pC[j] = beta * m_pC[j] + m_deltafC[j];
-						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
-						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
-						const btSolverConstraint& c = constraint;
+						const btSolverConstraint& c = constraint;
-						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
-						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
-					}
+					}
-				}
+				}
-				for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++)
+				for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++)
-				{
+				{
-					btSolverConstraint& constraint = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+					btSolverConstraint& constraint = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
-					if (iteration< infoGlobal.m_numIterations) {
+					if (iteration< infoGlobal.m_numIterations) {
-						btScalar additionaldeltaimpulse = beta * m_pCF[j];
+						btScalar additionaldeltaimpulse = beta * m_pCF[j];
-						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
-						m_pCF[j] = beta * m_pCF[j] + m_deltafCF[j];
+						m_pCF[j] = beta * m_pCF[j] + m_deltafCF[j];
-						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
-						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
-						const btSolverConstraint& c = constraint;
+						const btSolverConstraint& c = constraint;
-						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
-						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,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 ) {
+				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++)
+					for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++)
-					{
+					{
-						btSolverConstraint& constraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
+						btSolverConstraint& constraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
-						if (iteration< infoGlobal.m_numIterations) {
+						if (iteration< infoGlobal.m_numIterations) {
-							btScalar additionaldeltaimpulse = beta * m_pCRF[j];
+							btScalar additionaldeltaimpulse = beta * m_pCRF[j];
-							constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+							constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
-							m_pCRF[j] = beta * m_pCRF[j] + m_deltafCRF[j];
+							m_pCRF[j] = beta * m_pCRF[j] + m_deltafCRF[j];
-							btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+							btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
-							btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+							btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
-							const btSolverConstraint& c = constraint;
+							const btSolverConstraint& c = constraint;
-							body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+							body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
-							body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+							body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
-						}
+						}
-					}
+					}
-				}
+				}
-			}
+			}
-		}
+		}
-		m_deltafLengthSqrPrev = deltaflengthsqr;
+		m_deltafLengthSqrPrev = deltaflengthsqr;
-	}
+	}
-
+
-	return deltaflengthsqr;
+	return deltaflengthsqr;
-}
+}
-
+
-btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
+btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
-{
+{
-	m_pNC.resizeNoInitialize(0);
+	m_pNC.resizeNoInitialize(0);
-	m_pC.resizeNoInitialize(0);
+	m_pC.resizeNoInitialize(0);
-	m_pCF.resizeNoInitialize(0);
+	m_pCF.resizeNoInitialize(0);
-	m_pCRF.resizeNoInitialize(0);
+	m_pCRF.resizeNoInitialize(0);
-
+
-	m_deltafNC.resizeNoInitialize(0);
+	m_deltafNC.resizeNoInitialize(0);
-	m_deltafC.resizeNoInitialize(0);
+	m_deltafC.resizeNoInitialize(0);
-	m_deltafCF.resizeNoInitialize(0);
+	m_deltafCF.resizeNoInitialize(0);
-	m_deltafCRF.resizeNoInitialize(0);
+	m_deltafCRF.resizeNoInitialize(0);
-
+
-	return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
+	return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
-}
+}
-
+
-
+
-
+

src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h

@@ -1,64 +1,64 @@
-/*
+/*
-Bullet Continuous Collision Detection and Physics Library
+Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
-
+
-This software is provided 'as-is', without any express or implied warranty.
+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.
+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, 
+Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
+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.
+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.
+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.
+3. This notice may not be removed or altered from any source distribution.
-*/
+*/
-
+
-#ifndef BT_NNCG_CONSTRAINT_SOLVER_H
+#ifndef BT_NNCG_CONSTRAINT_SOLVER_H
-#define BT_NNCG_CONSTRAINT_SOLVER_H
+#define BT_NNCG_CONSTRAINT_SOLVER_H
-
+
-#include "btSequentialImpulseConstraintSolver.h"
+#include "btSequentialImpulseConstraintSolver.h"
-
+
-ATTRIBUTE_ALIGNED16(class) btNNCGConstraintSolver : public btSequentialImpulseConstraintSolver
+ATTRIBUTE_ALIGNED16(class) btNNCGConstraintSolver : public btSequentialImpulseConstraintSolver
-{
+{
-protected:
+protected:
-
+
-	btScalar m_deltafLengthSqrPrev;
+	btScalar m_deltafLengthSqrPrev;
-
+
-	btAlignedObjectArray<btScalar> m_pNC;  // p for None Contact constraints
+	btAlignedObjectArray<btScalar> m_pNC;  // p for None Contact constraints
-	btAlignedObjectArray<btScalar> m_pC;   // p for Contact constraints
+	btAlignedObjectArray<btScalar> m_pC;   // p for Contact constraints
-	btAlignedObjectArray<btScalar> m_pCF;  // p for ContactFriction constraints
+	btAlignedObjectArray<btScalar> m_pCF;  // p for ContactFriction constraints
-	btAlignedObjectArray<btScalar> m_pCRF; // p for ContactRollingFriction 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.
+	//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_deltafNC;  // deltaf for NoneContact constraints
-	btAlignedObjectArray<btScalar> m_deltafC;   // deltaf for Contact constraints
+	btAlignedObjectArray<btScalar> m_deltafC;   // deltaf for Contact constraints
-	btAlignedObjectArray<btScalar> m_deltafCF;  // deltaf for ContactFriction constraints
+	btAlignedObjectArray<btScalar> m_deltafCF;  // deltaf for ContactFriction constraints
-	btAlignedObjectArray<btScalar> m_deltafCRF; // deltaf for ContactRollingFriction constraints
+	btAlignedObjectArray<btScalar> m_deltafCRF; // deltaf for ContactRollingFriction constraints
-
+
-		
+		
-protected:
+protected:
-
+
-	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
+	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 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);
+	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
-
+
-public:
+public:
-
+
-	BT_DECLARE_ALIGNED_ALLOCATOR();
+	BT_DECLARE_ALIGNED_ALLOCATOR();
-
+
-	btNNCGConstraintSolver() : btSequentialImpulseConstraintSolver(), m_onlyForNoneContact(false) {}
+	btNNCGConstraintSolver() : btSequentialImpulseConstraintSolver(), m_onlyForNoneContact(false) {}
-
+
-	virtual btConstraintSolverType getSolverType() const
+	virtual btConstraintSolverType getSolverType() const
-	{
+	{
-		return BT_NNCG_SOLVER;
+		return BT_NNCG_SOLVER;
-	}
+	}
-
+
-	bool m_onlyForNoneContact;
+	bool m_onlyForNoneContact;
-};
+};
-
+
-
+
-
+
-
+
-#endif //BT_NNCG_CONSTRAINT_SOLVER_H
+#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_resolveSingleConstraintRowGeneric(gResolveSingleConstraintRowGeneric_scalar_reference),
+	 m_resolveSingleConstraintRowLowerLimit(gResolveSingleConstraintRowLowerLimit_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& pos1 = cp.getPositionWorldOnA();
-			const btVector3& pos2 = cp.getPositionWorldOnB();
+		//	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, 
+Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely, 
+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_hasSelfCollision(true),		
-		m_dofCount(0),
-		__posUpdated(false),		
 		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 "btMultiBodyConstraint.h"
-#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
-#include "btMultiBodyPoint2Point.h"				//for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
+#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),
+btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
-	m_linkA(linkA),
+	:m_bodyA(bodyA),
-	m_linkB(linkB),
+	m_bodyB(bodyB),
-	m_numRows(numRows),
+	m_linkA(linkA),
-	m_isUnilateral(isUnilateral),
+	m_linkB(linkB),
-	m_maxAppliedImpulse(100),
+	m_numRows(numRows),
-	m_jacSizeA(0),
+	m_jacSizeA(0),
-	m_jacSizeBoth(0)
+	m_jacSizeBoth(0),
-{
+	m_isUnilateral(isUnilateral),
-	
+	m_maxAppliedImpulse(100)
-	if(bodyA)
+{
-	{
+	
-		if(bodyA->isMultiDof())
+	if(bodyA)
-			m_jacSizeA = (6 + bodyA->getNumDofs());
+	{
-		else
+		if(bodyA->isMultiDof())
-			m_jacSizeA = (6 + bodyA->getNumLinks());
+			m_jacSizeA = (6 + bodyA->getNumDofs());
-	}
+		else
-
+			m_jacSizeA = (6 + bodyA->getNumLinks());
-	if(bodyB)
+	}
-	{
+
-		if(bodyB->isMultiDof())
+	if(bodyB)
-			m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumDofs();
+	{
-		else
+		if(bodyB->isMultiDof())
-			m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumLinks();
+			m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumDofs();
-	}
+		else
-	else
+			m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumLinks();
-		m_jacSizeBoth = m_jacSizeA;
+	}
-	
+	else
-	m_posOffset = ((1 + m_jacSizeBoth)*m_numRows);
+		m_jacSizeBoth = m_jacSizeA;
-	m_data.resize((2 + m_jacSizeBoth) * m_numRows);
+	
-}
+	m_posOffset = ((1 + m_jacSizeBoth)*m_numRows);
-
+	m_data.resize((2 + m_jacSizeBoth) * m_numRows);
-btMultiBodyConstraint::~btMultiBodyConstraint()
+}
-{
+
-}
+btMultiBodyConstraint::~btMultiBodyConstraint()
-
+{
-void	btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
+}
-{
+
-	for (int i = 0; i < ndof; ++i) 
+void	btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
-		data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
+{
-}
+	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,
+btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstraint& solverConstraint, 
-															const btVector3& contactNormalOnB,
+															btMultiBodyJacobianData& data,
-															const btVector3& posAworld, const btVector3& posBworld, 
+															btScalar* jacOrgA, btScalar* jacOrgB,
-															btScalar posError,
+															const btVector3& contactNormalOnB,
-															const btContactSolverInfo& infoGlobal,
+															const btVector3& posAworld, const btVector3& posBworld, 
-															btScalar lowerLimit, btScalar upperLimit,
+															btScalar posError,
-															btScalar relaxation,
+															const btContactSolverInfo& infoGlobal,
-															bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
+															btScalar lowerLimit, btScalar upperLimit,
-{
+															btScalar relaxation,
-	solverConstraint.m_multiBodyA = m_bodyA;
+															bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
-	solverConstraint.m_multiBodyB = m_bodyB;
+{
-	solverConstraint.m_linkA = m_linkA;
+	solverConstraint.m_multiBodyA = m_bodyA;
-	solverConstraint.m_linkB = m_linkB;	
+	solverConstraint.m_multiBodyB = m_bodyB;
-
+	solverConstraint.m_linkA = m_linkA;
-	btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
+	solverConstraint.m_linkB = m_linkB;	
-	btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
+
-
+	btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
-	btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
+	btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
-	btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
+
-
+	btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
-	btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
+	btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
-	btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
+
-
+	btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
-	btVector3 rel_pos1, rel_pos2;				//these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
+	btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
-	if (bodyA)
+
-		rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
+	btVector3 rel_pos1, rel_pos2;				//these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
-	if (bodyB)
+	if (bodyA)
-		rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
+		rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
-
+	if (bodyB)
-	if (multiBodyA)
+		rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
-	{
+
-		const int ndofA  = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
+	if (multiBodyA)
-
+	{
-		solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
+		const int ndofA  = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
-
+
-		if (solverConstraint.m_deltaVelAindex <0)
+		solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
-		{
+
-			solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
+		if (solverConstraint.m_deltaVelAindex <0)
-			multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
+		{
-			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
+			solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
-		} else
+			multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
-		{
+			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
-			btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+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();
+		//determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
-		data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
+		//resize..
-		//copy/determine
+		solverConstraint.m_jacAindex = data.m_jacobians.size();
-		if(jacOrgA)
+		data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
-		{
+		//copy/determine
-			for (int i=0;i<ndofA;i++)
+		if(jacOrgA)
-				data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i];
+		{
-		}
+			for (int i=0;i<ndofA;i++)
-		else
+				data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i];
-		{
+		}
-			btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
+		else
-			if(multiBodyA->isMultiDof())
+		{
-				multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+			btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
-			else
+			if(multiBodyA->isMultiDof())
-				multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+				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
+		//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)
-		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
+		//resize..
-		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);		//=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
-		//determine..
+		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
-		if(multiBodyA->isMultiDof())
+		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-			multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
+		//determine..
-		else
+		if(multiBodyA->isMultiDof())
-			multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
+			multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
-	}
+		else
-	else if(rb0)
+			multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
-	{
+	}
-		btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
+	else if(rb0)
-		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
+	{
-		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+		btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
-		solverConstraint.m_contactNormal1 = 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;
+	if (multiBodyB)
-
+	{
-		solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
+		const int ndofB  = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
-		if (solverConstraint.m_deltaVelBindex <0)
+
-		{
+		solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
-			solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
+		if (solverConstraint.m_deltaVelBindex <0)
-			multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
+		{
-			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
+			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();
+		//determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
-		data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
+		//resize..
-		//copy/determine..
+		solverConstraint.m_jacBindex = data.m_jacobians.size();
-		if(jacOrgB)
+		data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
-		{
+		//copy/determine..
-			for (int i=0;i<ndofB;i++)
+		if(jacOrgB)
-				data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
+		{
-		}
+			for (int i=0;i<ndofB;i++)
-		else
+				data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
-		{
+		}
-			if(multiBodyB->isMultiDof())
+		else
-				multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
+		{
-			else
+			if(multiBodyB->isMultiDof())
-				multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
+				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);
+		//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)
-		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
+		//resize..
-		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
-		//determine..
+		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
-		if(multiBodyB->isMultiDof())
+		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
-			multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
+		//determine..
-		else
+		if(multiBodyB->isMultiDof())
-			multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
+			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);		
+	else if(rb1)
-		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
+	{
-		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+		btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);		
-		solverConstraint.m_contactNormal2 = -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;
+		btVector3 vec;
-		btScalar* jacB = 0;
+		btScalar denom0 = 0.f;
-		btScalar* jacA = 0;
+		btScalar denom1 = 0.f;
-		btScalar* deltaVelA = 0;
+		btScalar* jacB = 0;
-		btScalar* deltaVelB = 0;
+		btScalar* jacA = 0;
-		int ndofA  = 0;
+		btScalar* deltaVelA = 0;
-		//determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
+		btScalar* deltaVelB = 0;
-		if (multiBodyA)
+		int ndofA  = 0;
-		{
+		//determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
-			ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
+		if (multiBodyA)
-			jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
+		{
-			deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+			ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
-			for (int i = 0; i < ndofA; ++i)
+			jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
-			{
+			deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-				btScalar j = jacA[i] ;
+			for (int i = 0; i < ndofA; ++i)
-				btScalar l = deltaVelA[i];
+			{
-				denom0 += j*l;
+				btScalar j = jacA[i] ;
-			}
+				btScalar l = deltaVelA[i];
-		}
+				denom0 += j*l;
-		else if(rb0)
+			}
-		{
+		}
-			vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+		else if(rb0)
-			denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
+		{
-		}
+			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;
+		if (multiBodyB)
-			jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
+		{
-			deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+			const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
-			for (int i = 0; i < ndofB; ++i)
+			jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
-			{
+			deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
-				btScalar j = jacB[i] ;
+			for (int i = 0; i < ndofB; ++i)
-				btScalar l = deltaVelB[i];
+			{
-				denom1 += j*l;
+				btScalar j = jacB[i] ;
-			}
+				btScalar l = deltaVelB[i];
-
+				denom1 += j*l;
-		}
+			}
-		else if(rb1)
+
-		{
+		}
-			vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
+		else if(rb1)
-			denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
+		{
-		}
+			vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
-		//determine the "effective mass" of the constrained multibodyB with respect to this 1D constraint (i.e. 1/A[i,i])
+			denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
-		if (multiBodyA && (multiBodyA==multiBodyB))
+		}
-		{
+		//determine the "effective mass" of the constrained multibodyB with respect to this 1D constraint (i.e. 1/A[i,i])
-			// ndof1 == ndof2 in this case
+		if (multiBodyA && (multiBodyA==multiBodyB))
-			for (int i = 0; i < ndofA; ++i) 
+		{
-			{
+			// ndof1 == ndof2 in this case
-				denom1 += jacB[i] * deltaVelA[i];
+			for (int i = 0; i < ndofA; ++i) 
-				denom1 += jacA[i] * deltaVelB[i];
+			{
-			}
+				denom1 += jacB[i] * deltaVelA[i];
-		}
+				denom1 += jacA[i] * deltaVelB[i];
-		//
+			}
-		btScalar d = denom0+denom1;
+		}
-		if (btFabs(d)>SIMD_EPSILON)
+		//
-		{
+		btScalar d = denom0+denom1;
-			 
+		if (btFabs(d)>SIMD_EPSILON)
-			solverConstraint.m_jacDiagABInv = relaxation/(d);
+		{
-		}
+			 
-		else
+			solverConstraint.m_jacDiagABInv = relaxation/(d);
-		{
+		}
-			solverConstraint.m_jacDiagABInv  = 1.f;
+		else
-		}		
+		{
-	}
+			solverConstraint.m_jacDiagABInv  = 1.f;
-
+		}		
-	
+	}
-	//compute rhs and remaining solverConstraint fields
+
-	btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
+	
-
+	//compute rhs and remaining solverConstraint fields
-	btScalar rel_vel = 0.f;
+	btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
-	int ndofA  = 0;
+
-	int ndofB  = 0;
+	btScalar rel_vel = 0.f;
-	{
+	int ndofA  = 0;
-		btVector3 vel1,vel2;
+	int ndofB  = 0;
-		if (multiBodyA)
+	{
-		{
+		btVector3 vel1,vel2;
-			ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
+		if (multiBodyA)
-			btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
+		{
-			for (int i = 0; i < ndofA ; ++i) 
+			ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
-				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
+			btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
-		}
+			for (int i = 0; i < ndofA ; ++i) 
-		else if(rb0)
+				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
-		{
+		}
-			rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
+		else if(rb0)
-		}
+		{
-		if (multiBodyB)
+			rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
-		{
+		}
-			ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
+		if (multiBodyB)
-			btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
+		{
-			for (int i = 0; i < ndofB ; ++i) 
+			ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
-				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
+			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);
+		else if(rb1)
-		}
+		{
-
+			rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
-		solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
+		}
-	}
+
-
+		solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
-
+	}
-	///warm starting (or zero if disabled)
+
-	/*
+
-	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+	///warm starting (or zero if disabled)
-	{
+	/*
-		solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
+	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-
+	{
-		if (solverConstraint.m_appliedImpulse)
+		solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
-		{
+
-			if (multiBodyA)
+		if (solverConstraint.m_appliedImpulse)
-			{
+		{
-				btScalar impulse = solverConstraint.m_appliedImpulse;
+			if (multiBodyA)
-				btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+			{
-				multiBodyA->applyDeltaVee(deltaV,impulse);
+				btScalar impulse = solverConstraint.m_appliedImpulse;
-				applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
+				btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-			} else
+				multiBodyA->applyDeltaVee(deltaV,impulse);
-			{
+				applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
-				if (rb0)
+			} else
-					bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
+			{
-			}
+				if (rb0)
-			if (multiBodyB)
+					bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
-			{
+			}
-				btScalar impulse = solverConstraint.m_appliedImpulse;
+			if (multiBodyB)
-				btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+			{
-				multiBodyB->applyDeltaVee(deltaV,impulse);
+				btScalar impulse = solverConstraint.m_appliedImpulse;
-				applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
+				btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
-			} else
+				multiBodyB->applyDeltaVee(deltaV,impulse);
-			{
+				applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
-				if (rb1)
+			} else
-					bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
+			{
-			}
+				if (rb1)
-		}
+					bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
-	} else
+			}
-	*/
+		}
-
+	} else
-	solverConstraint.m_appliedImpulse = 0.f;
+	*/
-	solverConstraint.m_appliedPushImpulse = 0.f;
+
-
+	solverConstraint.m_appliedImpulse = 0.f;
-	{	
+	solverConstraint.m_appliedPushImpulse = 0.f;
-
+
-		btScalar positionalError = 0.f;
+	{	
-		btScalar	velocityError = desiredVelocity - rel_vel;// * damping;
+
-					
+		btScalar positionalError = 0.f;
-
+		btScalar	velocityError = desiredVelocity - rel_vel;// * damping;
-		btScalar erp = infoGlobal.m_erp2;
+					
-		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+
-		{
+		btScalar erp = infoGlobal.m_erp2;
-			erp = infoGlobal.m_erp;
+		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
-		}
+		{
-
+			erp = infoGlobal.m_erp;
-		positionalError = -penetration * erp/infoGlobal.m_timeStep;
+		}
-
+
-		btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
+		positionalError = -penetration * erp/infoGlobal.m_timeStep;
-		btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+
-
+		btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
-		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+		btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
-		{
+
-			//combine position and velocity into rhs
+		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
-			solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+		{
-			solverConstraint.m_rhsPenetration = 0.f;
+			//combine position and velocity into rhs
-
+			solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
-		} else
+			solverConstraint.m_rhsPenetration = 0.f;
-		{
+
-			//split position and velocity into rhs and m_rhsPenetration
+		} else
-			solverConstraint.m_rhs = velocityImpulse;
+		{
-			solverConstraint.m_rhsPenetration = penetrationImpulse;
+			//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;
+		solverConstraint.m_cfm = 0.f;
-	}
+		solverConstraint.m_lowerLimit = lowerLimit;
-
+		solverConstraint.m_upperLimit = upperLimit;
-	return rel_vel;
+	}
-
+
-}
+	return rel_vel;
+
+}

src/BulletDynamics/Featherstone/btMultiBodyConstraint.h

@@ -1,160 +1,160 @@
-/*
+/*
-Bullet Continuous Collision Detection and Physics Library
+Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
-
+
-This software is provided 'as-is', without any express or implied warranty.
+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.
+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, 
+Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
+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.
+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.
+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.
+3. This notice may not be removed or altered from any source distribution.
-*/
+*/
-
+
-#ifndef BT_MULTIBODY_CONSTRAINT_H
+#ifndef BT_MULTIBODY_CONSTRAINT_H
-#define BT_MULTIBODY_CONSTRAINT_H
+#define BT_MULTIBODY_CONSTRAINT_H
-
+
-#include "LinearMath/btScalar.h"
+#include "LinearMath/btScalar.h"
-#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btAlignedObjectArray.h"
-#include "btMultiBody.h"
+#include "btMultiBody.h"
-
+
-class btMultiBody;
+class btMultiBody;
-struct btSolverInfo;
+struct btSolverInfo;
-
+
-#include "btMultiBodySolverConstraint.h"
+#include "btMultiBodySolverConstraint.h"
-
+
-struct btMultiBodyJacobianData
+struct btMultiBodyJacobianData
-{
+{
-	btAlignedObjectArray<btScalar>		m_jacobians;
+	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_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>		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<btScalar>		scratch_r;
-	btAlignedObjectArray<btVector3>		scratch_v;
+	btAlignedObjectArray<btVector3>		scratch_v;
-	btAlignedObjectArray<btMatrix3x3>	scratch_m;
+	btAlignedObjectArray<btMatrix3x3>	scratch_m;
-	btAlignedObjectArray<btSolverBody>*	m_solverBodyPool;
+	btAlignedObjectArray<btSolverBody>*	m_solverBodyPool;
-	int									m_fixedBodyId;
+	int									m_fixedBodyId;
-
+
-};
+};
-
+
-
+
-class btMultiBodyConstraint
+class btMultiBodyConstraint
-{
+{
-protected:
+protected:
-
+
-	btMultiBody*	m_bodyA;
+	btMultiBody*	m_bodyA;
-    btMultiBody*	m_bodyB;
+    btMultiBody*	m_bodyB;
-    int				m_linkA;
+    int				m_linkA;
-    int				m_linkB;
+    int				m_linkB;
-
+
-    int				m_numRows;
+    int				m_numRows;
-    int				m_jacSizeA;
+    int				m_jacSizeA;
-    int				m_jacSizeBoth;
+    int				m_jacSizeBoth;
-    int				m_posOffset;
+    int				m_posOffset;
-
+
-	bool			m_isUnilateral;
+	bool			m_isUnilateral;
-
+
-	btScalar		m_maxAppliedImpulse;
+	btScalar		m_maxAppliedImpulse;
-
+
-
+
-    // data block laid out as follows:
+    // data block laid out as follows:
-    // cached impulses. (one per row.)
+    // cached impulses. (one per row.)
-    // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc)
+    // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc)
-    // positions. (one per row.)
+    // positions. (one per row.)
-    btAlignedObjectArray<btScalar> m_data;
+    btAlignedObjectArray<btScalar> m_data;
-
+
-	void	applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof);
+	void	applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof);
-
+
-	btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, 
+	btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, 
-																btMultiBodyJacobianData& data,
+																btMultiBodyJacobianData& data,
-																btScalar* jacOrgA, btScalar* jacOrgB,
+																btScalar* jacOrgA, btScalar* jacOrgB,
-																const btVector3& contactNormalOnB,
+																const btVector3& contactNormalOnB,
-																const btVector3& posAworld, const btVector3& posBworld, 
+																const btVector3& posAworld, const btVector3& posBworld, 
-																btScalar posError,
+																btScalar posError,
-																const btContactSolverInfo& infoGlobal,
+																const btContactSolverInfo& infoGlobal,
-																btScalar lowerLimit, btScalar upperLimit,
+																btScalar lowerLimit, btScalar upperLimit,
-																btScalar relaxation = 1.f,
+																btScalar relaxation = 1.f,
-																bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0);
+																bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0);
-
+
-public:
+public:
-
+
-	btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral);
+	btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral);
-	virtual ~btMultiBodyConstraint();
+	virtual ~btMultiBodyConstraint();
-
+
-
+
-
+
-	virtual int getIslandIdA() const =0;
+	virtual int getIslandIdA() const =0;
-	virtual int getIslandIdB() const =0;
+	virtual int getIslandIdB() const =0;
-	
+	
-	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
-		btMultiBodyJacobianData& data,
+		btMultiBodyJacobianData& data,
-		const btContactSolverInfo& infoGlobal)=0;
+		const btContactSolverInfo& infoGlobal)=0;
-
+
-	int	getNumRows() const
+	int	getNumRows() const
-	{
+	{
-		return m_numRows;
+		return m_numRows;
-	}
+	}
-
+
-	btMultiBody*	getMultiBodyA()
+	btMultiBody*	getMultiBodyA()
-	{
+	{
-		return m_bodyA;
+		return m_bodyA;
-	}
+	}
-    btMultiBody*	getMultiBodyB()
+    btMultiBody*	getMultiBodyB()
-	{
+	{
-		return m_bodyB;
+		return m_bodyB;
-	}
+	}
-
+
-	// current constraint position
+	// current constraint position
-    // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
+    // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
-    // NOTE: ignored position for friction rows.
+    // NOTE: ignored position for friction rows.
-    btScalar getPosition(int row) const 
+    btScalar getPosition(int row) const 
-	{ 
+	{ 
-		return m_data[m_posOffset + row]; 
+		return m_data[m_posOffset + row]; 
-	}
+	}
-
+
-    void setPosition(int row, btScalar pos) 
+    void setPosition(int row, btScalar pos) 
-	{ 
+	{ 
-		m_data[m_posOffset + row] = pos; 
+		m_data[m_posOffset + row] = pos; 
-	}
+	}
-
+
-	
+	
-	bool isUnilateral() const
+	bool isUnilateral() const
-	{
+	{
-		return m_isUnilateral;
+		return m_isUnilateral;
-	}
+	}
-
+
-	// jacobian blocks.
+	// jacobian blocks.
-    // each of size 6 + num_links. (jacobian2 is null if no body2.)
+    // each of size 6 + num_links. (jacobian2 is null if no body2.)
-    // format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients.
+    // format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients.
-    btScalar* jacobianA(int row) 
+    btScalar* jacobianA(int row) 
-	{ 
+	{ 
-		return &m_data[m_numRows + row * m_jacSizeBoth]; 
+		return &m_data[m_numRows + row * m_jacSizeBoth]; 
-	}
+	}
-    const btScalar* jacobianA(int row) const 
+    const btScalar* jacobianA(int row) const 
-	{ 
+	{ 
-		return &m_data[m_numRows + (row * m_jacSizeBoth)]; 
+		return &m_data[m_numRows + (row * m_jacSizeBoth)]; 
-	}
+	}
-    btScalar* jacobianB(int row) 
+    btScalar* jacobianB(int row) 
-	{ 
+	{ 
-		return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; 
+		return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; 
-	}
+	}
-    const btScalar* jacobianB(int row) const 
+    const btScalar* jacobianB(int row) const 
-	{ 
+	{ 
-		return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; 
+		return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; 
-	}
+	}
-
+
-	btScalar	getMaxAppliedImpulse() const
+	btScalar	getMaxAppliedImpulse() const
-	{
+	{
-		return m_maxAppliedImpulse;
+		return m_maxAppliedImpulse;
-	}
+	}
-	void	setMaxAppliedImpulse(btScalar maxImp)
+	void	setMaxAppliedImpulse(btScalar maxImp)
-	{
+	{
-		m_maxAppliedImpulse = maxImp;
+		m_maxAppliedImpulse = maxImp;
-	}
+	}
-	
+	
-
+
-};
+};
-
+
-#endif //BT_MULTIBODY_CONSTRAINT_H
+#endif //BT_MULTIBODY_CONSTRAINT_H
-
+

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h

@@ -1,86 +1,86 @@
-/*
+/*
-Bullet Continuous Collision Detection and Physics Library
+Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
-
+
-This software is provided 'as-is', without any express or implied warranty.
+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.
+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, 
+Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
+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.
+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.
+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.
+3. This notice may not be removed or altered from any source distribution.
-*/
+*/
-
+
-#ifndef BT_MULTIBODY_CONSTRAINT_SOLVER_H
+#ifndef BT_MULTIBODY_CONSTRAINT_SOLVER_H
-#define BT_MULTIBODY_CONSTRAINT_SOLVER_H
+#define BT_MULTIBODY_CONSTRAINT_SOLVER_H
-
+
-#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
-#include "btMultiBodySolverConstraint.h"
+#include "btMultiBodySolverConstraint.h"
-
+
-
+
-class btMultiBody;
+class btMultiBody;
-
+
-#include "btMultiBodyConstraint.h"
+#include "btMultiBodyConstraint.h"
-
+
-
+
-
+
-ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver
+ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver
-{
+{
-
+
-protected:
+protected:
-
+
-	btMultiBodyConstraintArray			m_multiBodyNonContactConstraints;
+	btMultiBodyConstraintArray			m_multiBodyNonContactConstraints;
-
+
-	btMultiBodyConstraintArray			m_multiBodyNormalContactConstraints;
+	btMultiBodyConstraintArray			m_multiBodyNormalContactConstraints;
-	btMultiBodyConstraintArray			m_multiBodyFrictionContactConstraints;
+	btMultiBodyConstraintArray			m_multiBodyFrictionContactConstraints;
-
+
-	btMultiBodyJacobianData				m_data;
+	btMultiBodyJacobianData				m_data;
-	
+	
-	//temp storage for multi body constraints for a specific island/group called by 'solveGroup'
+	//temp storage for multi body constraints for a specific island/group called by 'solveGroup'
-	btMultiBodyConstraint**					m_tmpMultiBodyConstraints;
+	btMultiBodyConstraint**					m_tmpMultiBodyConstraints;
-	int										m_tmpNumMultiBodyConstraints;
+	int										m_tmpNumMultiBodyConstraints;
-
+
-	void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
+	void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
-	void resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c);
+	void resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c);
-
+
-	void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);
+	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);
+	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, 
+	void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, 
-																 btScalar* jacA,btScalar* jacB,
+																 btScalar* jacA,btScalar* jacB,
-																 btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
+																 btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
-																 const btContactSolverInfo& infoGlobal);
+																 const btContactSolverInfo& infoGlobal);
-
+
-	void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, 
+	void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, 
-																 const btVector3& contactNormal,
+																 const btVector3& contactNormal,
-																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
+																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
-																 btScalar& relaxation,
+																 btScalar& relaxation,
-																 bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
+																 bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
-
+
-	void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
+	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 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 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);
+	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);
+	void	applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof);
-
+
-public:
+public:
-
+
-	BT_DECLARE_ALIGNED_ALLOCATOR();
+	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)
+	///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 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 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);
+	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
+#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H
-
+

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

@@ -1,56 +1,56 @@
-/*
+/*
-Bullet Continuous Collision Detection and Physics Library
+Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
-
+
-This software is provided 'as-is', without any express or implied warranty.
+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.
+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, 
+Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
+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.
+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.
+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.
+3. This notice may not be removed or altered from any source distribution.
-*/
+*/
-
+
-#ifndef BT_MULTIBODY_DYNAMICS_WORLD_H
+#ifndef BT_MULTIBODY_DYNAMICS_WORLD_H
-#define BT_MULTIBODY_DYNAMICS_WORLD_H
+#define BT_MULTIBODY_DYNAMICS_WORLD_H
-
+
-#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
+#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
-
+
-
+
-class btMultiBody;
+class btMultiBody;
-class btMultiBodyConstraint;
+class btMultiBodyConstraint;
-class btMultiBodyConstraintSolver;
+class btMultiBodyConstraintSolver;
-struct MultiBodyInplaceSolverIslandCallback;
+struct MultiBodyInplaceSolverIslandCallback;
-
+
-///The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet
+///The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet
-///This implementation is still preliminary/experimental.
+///This implementation is still preliminary/experimental.
-class btMultiBodyDynamicsWorld : public btDiscreteDynamicsWorld
+class btMultiBodyDynamicsWorld : public btDiscreteDynamicsWorld
-{
+{
-protected:
+protected:
-	btAlignedObjectArray<btMultiBody*> m_multiBodies;
+	btAlignedObjectArray<btMultiBody*> m_multiBodies;
-	btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
+	btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
-	btAlignedObjectArray<btMultiBodyConstraint*> m_sortedMultiBodyConstraints;
+	btAlignedObjectArray<btMultiBodyConstraint*> m_sortedMultiBodyConstraints;
-	btMultiBodyConstraintSolver*	m_multiBodyConstraintSolver;
+	btMultiBodyConstraintSolver*	m_multiBodyConstraintSolver;
-	MultiBodyInplaceSolverIslandCallback*	m_solverMultiBodyIslandCallback;
+	MultiBodyInplaceSolverIslandCallback*	m_solverMultiBodyIslandCallback;
-
+
-	virtual void	calculateSimulationIslands();
+	virtual void	calculateSimulationIslands();
-	virtual void	updateActivationState(btScalar timeStep);
+	virtual void	updateActivationState(btScalar timeStep);
-	virtual void	solveConstraints(btContactSolverInfo& solverInfo);
+	virtual void	solveConstraints(btContactSolverInfo& solverInfo);
-	virtual void	integrateTransforms(btScalar timeStep);
+	virtual void	integrateTransforms(btScalar timeStep);
-public:
+public:
-
+
-	btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+	btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
-	
+	
-	virtual ~btMultiBodyDynamicsWorld ();
+	virtual ~btMultiBodyDynamicsWorld ();
-
+
-	virtual void	addMultiBody(btMultiBody* body, short group= btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter);
+	virtual void	addMultiBody(btMultiBody* body, short group= btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter);
-
+
-	virtual void	removeMultiBody(btMultiBody* body);
+	virtual void	removeMultiBody(btMultiBody* body);
-
+
-	virtual void	addMultiBodyConstraint( btMultiBodyConstraint* constraint);
+	virtual void	addMultiBodyConstraint( btMultiBodyConstraint* constraint);
-
+
-	virtual void	removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
+	virtual void	removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
-};
+};
-#endif //BT_MULTIBODY_DYNAMICS_WORLD_H
+#endif //BT_MULTIBODY_DYNAMICS_WORLD_H

src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp

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

src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h

@@ -1,44 +1,44 @@
-/*
+/*
-Bullet Continuous Collision Detection and Physics Library
+Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
-
+
-This software is provided 'as-is', without any express or implied warranty.
+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.
+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, 
+Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
+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.
+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.
+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.
+3. This notice may not be removed or altered from any source distribution.
-*/
+*/
-
+
-#ifndef BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
+#ifndef BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
-#define BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
+#define BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
-
+
-#include "btMultiBodyConstraint.h"
+#include "btMultiBodyConstraint.h"
-struct btSolverInfo;
+struct btSolverInfo;
-
+
-class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint
+class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint
-{
+{
-protected:
+protected:
-
+
-	btScalar	m_lowerBound;
+	btScalar	m_lowerBound;
-	btScalar	m_upperBound;
+	btScalar	m_upperBound;
-public:
+public:
-
+
-	btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper);
+	btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper);
-	virtual ~btMultiBodyJointLimitConstraint();
+	virtual ~btMultiBodyJointLimitConstraint();
-
+
-	virtual int getIslandIdA() const;
+	virtual int getIslandIdA() const;
-	virtual int getIslandIdB() const;
+	virtual int getIslandIdB() const;
-
+
-	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
-		btMultiBodyJacobianData& data,
+		btMultiBodyJacobianData& data,
-		const btContactSolverInfo& infoGlobal);
+		const btContactSolverInfo& infoGlobal);
-	
+	
-	
+	
-};
+};
-
+
-#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
+#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
+Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
-
+
-This software is provided 'as-is', without any express or implied warranty.
+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.
+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, 
+Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
+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.
+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.
+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.
+3. This notice may not be removed or altered from any source distribution.
-*/
+*/
-
+
-#ifndef BT_FEATHERSTONE_LINK_COLLIDER_H
+#ifndef BT_FEATHERSTONE_LINK_COLLIDER_H
-#define BT_FEATHERSTONE_LINK_COLLIDER_H
+#define BT_FEATHERSTONE_LINK_COLLIDER_H
-
+
-#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
-
+
-#include "btMultiBody.h"
+#include "btMultiBody.h"
-
+
-class btMultiBodyLinkCollider : public btCollisionObject
+class btMultiBodyLinkCollider : public btCollisionObject
-{
+{
-//protected:
+//protected:
-public:
+public:
-
+
-	btMultiBody* m_multiBody;
+	btMultiBody* m_multiBody;
-	int m_link;
+	int m_link;
-
+
-
+
-	btMultiBodyLinkCollider (btMultiBody* multiBody,int link)
+	btMultiBodyLinkCollider (btMultiBody* multiBody,int link)
-		:m_multiBody(multiBody),
+		:m_multiBody(multiBody),
-		m_link(link)
+		m_link(link)
-	{
+	{
-		m_checkCollideWith =  true;
+		m_checkCollideWith =  true;
-		//we need to remove the 'CF_STATIC_OBJECT' flag, otherwise links/base doesn't merge islands
+		//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
+		//this means that some constraints might point to bodies that are not in the islands, causing crashes
-		//if (link>=0 || (multiBody && !multiBody->hasFixedBase()))
+		//if (link>=0 || (multiBody && !multiBody->hasFixedBase()))
-		{
+		{
-			m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
+			m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
-		}
+		}
-		// else
+		// else
-		//{
+		//{
-		//	m_collisionFlags |= (btCollisionObject::CF_STATIC_OBJECT);
+		//	m_collisionFlags |= (btCollisionObject::CF_STATIC_OBJECT);
-		//}
+		//}
-
+
-		m_internalType = CO_FEATHERSTONE_LINK;
+		m_internalType = CO_FEATHERSTONE_LINK;
-	}
+	}
-	static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj)
+	static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj)
-	{
+	{
-		if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
+		if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
-			return (btMultiBodyLinkCollider*)colObj;
+			return (btMultiBodyLinkCollider*)colObj;
-		return 0;
+		return 0;
-	}
+	}
-	static const btMultiBodyLinkCollider* upcast(const btCollisionObject* colObj)
+	static const btMultiBodyLinkCollider* upcast(const btCollisionObject* colObj)
-	{
+	{
-		if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
+		if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
-			return (btMultiBodyLinkCollider*)colObj;
+			return (btMultiBodyLinkCollider*)colObj;
-		return 0;
+		return 0;
-	}
+	}
-
+
-	virtual bool checkCollideWithOverride(const  btCollisionObject* co) const
+	virtual bool checkCollideWithOverride(const  btCollisionObject* co) const
-	{
+	{
-		const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co);
+		const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co);
-		if (!other)
+		if (!other)
-			return true;
+			return true;
-		if (other->m_multiBody != this->m_multiBody)
+		if (other->m_multiBody != this->m_multiBody)
-			return true;
+			return true;
-		if (!m_multiBody->hasSelfCollision())
+		if (!m_multiBody->hasSelfCollision())
-			return false;
+			return false;
-
+
-		//check if 'link' has collision disabled
+		//check if 'link' has collision disabled
-		if (m_link>=0)
+		if (m_link>=0)
-		{
+		{
-			const btMultibodyLink& link = m_multiBody->getLink(this->m_link);
+			const btMultibodyLink& link = m_multiBody->getLink(this->m_link);
-			if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.m_parent == other->m_link)
+			if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.m_parent == other->m_link)
-				return false;
+				return false;
-		}
+		}
-		
+		
-		if (other->m_link>=0)
+		if (other->m_link>=0)
-		{
+		{
-			const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link);
+			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)
+			if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.m_parent == this->m_link)
-				return false;
+				return false;
-		}
+		}
-		return true;
+		return true;
-	}
+	}
-};
+};
-
+
-#endif //BT_FEATHERSTONE_LINK_COLLIDER_H
+#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
+Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
-
+
-This software is provided 'as-is', without any express or implied warranty.
+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.
+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, 
+Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
+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.
+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.
+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.
+3. This notice may not be removed or altered from any source distribution.
-*/
+*/
-
+
-///This file was written by Erwin Coumans
+///This file was written by Erwin Coumans
-
+
-#ifndef BT_MULTIBODY_POINT2POINT_H
+#ifndef BT_MULTIBODY_POINT2POINT_H
-#define BT_MULTIBODY_POINT2POINT_H
+#define BT_MULTIBODY_POINT2POINT_H
-
+
-#include "btMultiBodyConstraint.h"
+#include "btMultiBodyConstraint.h"
-
+
-//#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
+//#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
-
+
-class btMultiBodyPoint2Point : public btMultiBodyConstraint
+class btMultiBodyPoint2Point : public btMultiBodyConstraint
-{
+{
-protected:
+protected:
-
+
-	btRigidBody*	m_rigidBodyA;
+	btRigidBody*	m_rigidBodyA;
-	btRigidBody*	m_rigidBodyB;
+	btRigidBody*	m_rigidBodyB;
-	btVector3		m_pivotInA;
+	btVector3		m_pivotInA;
-	btVector3		m_pivotInB;
+	btVector3		m_pivotInB;
-	
+	
-
+
-public:
+public:
-
+
-	btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB);
+	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);
+	btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB);
-
+
-	virtual ~btMultiBodyPoint2Point();
+	virtual ~btMultiBodyPoint2Point();
-
+
-	virtual int getIslandIdA() const;
+	virtual int getIslandIdA() const;
-	virtual int getIslandIdB() const;
+	virtual int getIslandIdB() const;
-
+
-	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
-		btMultiBodyJacobianData& data,
+		btMultiBodyJacobianData& data,
-		const btContactSolverInfo& infoGlobal);
+		const btContactSolverInfo& infoGlobal);
-
+
-	const btVector3& getPivotInB() const
+	const btVector3& getPivotInB() const
-	{
+	{
-		return m_pivotInB;
+		return m_pivotInB;
-	}
+	}
-
+
-	void setPivotInB(const btVector3& pivotInB)
+	void setPivotInB(const btVector3& pivotInB)
-	{
+	{
-		m_pivotInB = pivotInB;
+		m_pivotInB = pivotInB;
-	}
+	}
-
+
-	
+	
-};
+};
-
+
-#endif //BT_MULTIBODY_POINT2POINT_H
+#endif //BT_MULTIBODY_POINT2POINT_H

src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h

@@ -1,85 +1,85 @@
-/*
+/*
-Bullet Continuous Collision Detection and Physics Library
+Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
-
+
-This software is provided 'as-is', without any express or implied warranty.
+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.
+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, 
+Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
+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.
+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.
+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.
+3. This notice may not be removed or altered from any source distribution.
-*/
+*/
-
+
-#ifndef BT_MULTIBODY_SOLVER_CONSTRAINT_H
+#ifndef BT_MULTIBODY_SOLVER_CONSTRAINT_H
-#define BT_MULTIBODY_SOLVER_CONSTRAINT_H
+#define BT_MULTIBODY_SOLVER_CONSTRAINT_H
-
+
-#include "LinearMath/btVector3.h"
+#include "LinearMath/btVector3.h"
-#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btAlignedObjectArray.h"
-
+
-class btMultiBody;
+class btMultiBody;
-#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
+#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
-#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.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.
+///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
-ATTRIBUTE_ALIGNED16 (struct)	btMultiBodySolverConstraint
+ATTRIBUTE_ALIGNED16 (struct)	btMultiBodySolverConstraint
-{
+{
-	BT_DECLARE_ALIGNED_ALLOCATOR();
+	BT_DECLARE_ALIGNED_ALLOCATOR();
-
+
-	btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_solverBodyIdB(-1), m_multiBodyA(0), m_multiBodyB(0), m_linkA(-1), m_linkB(-1)
+	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_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1
-	int				m_jacAindex;
+	int				m_jacAindex;
-	int				m_deltaVelBindex;
+	int				m_deltaVelBindex;
-	int				m_jacBindex;
+	int				m_jacBindex;
-
+
-	btVector3		m_relpos1CrossNormal;
+	btVector3		m_relpos1CrossNormal;
-	btVector3		m_contactNormal1;	
+	btVector3		m_contactNormal1;	
-	btVector3		m_relpos2CrossNormal;
+	btVector3		m_relpos2CrossNormal;
-	btVector3		m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always
+	btVector3		m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always
-	
+	
-
+
-	btVector3		m_angularComponentA;
+	btVector3		m_angularComponentA;
-	btVector3		m_angularComponentB;
+	btVector3		m_angularComponentB;
-	
+	
-	mutable btSimdScalar	m_appliedPushImpulse;
+	mutable btSimdScalar	m_appliedPushImpulse;
-	mutable btSimdScalar	m_appliedImpulse;
+	mutable btSimdScalar	m_appliedImpulse;
-
+
-	btScalar	m_friction;
+	btScalar	m_friction;
-	btScalar	m_jacDiagABInv;
+	btScalar	m_jacDiagABInv;
-	btScalar		m_rhs;
+	btScalar		m_rhs;
-	btScalar		m_cfm;
+	btScalar		m_cfm;
-	
+	
-    btScalar		m_lowerLimit;
+    btScalar		m_lowerLimit;
-	btScalar		m_upperLimit;
+	btScalar		m_upperLimit;
-	btScalar		m_rhsPenetration;
+	btScalar		m_rhsPenetration;
-    union
+    union
-	{
+	{
-		void*		m_originalContactPoint;
+		void*		m_originalContactPoint;
-		btScalar	m_unusedPadding4;
+		btScalar	m_unusedPadding4;
-	};
+	};
-
+
-	int	m_overrideNumSolverIterations;
+	int	m_overrideNumSolverIterations;
-    int			m_frictionIndex;
+    int			m_frictionIndex;
-
+
-	int m_solverBodyIdA;
+	int m_solverBodyIdA;
-	btMultiBody* m_multiBodyA;
+	btMultiBody* m_multiBodyA;
-	int			m_linkA;
+	int			m_linkA;
-	
+	
-	int m_solverBodyIdB;
+	int m_solverBodyIdB;
-	btMultiBody* m_multiBodyB;
+	btMultiBody* m_multiBodyB;
-	int			m_linkB;
+	int			m_linkB;
-
+
-	enum		btSolverConstraintType
+	enum		btSolverConstraintType
-	{
+	{
-		BT_SOLVER_CONTACT_1D = 0,
+		BT_SOLVER_CONTACT_1D = 0,
-		BT_SOLVER_FRICTION_1D
+		BT_SOLVER_FRICTION_1D
-	};
+	};
-};
+};
-
+
-typedef btAlignedObjectArray<btMultiBodySolverConstraint>	btMultiBodyConstraintArray;
+typedef btAlignedObjectArray<btMultiBodySolverConstraint>	btMultiBodyConstraintArray;
-
+
-#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H
+#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
+Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
-
+
-This software is provided 'as-is', without any express or implied warranty.
+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.
+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, 
+Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
+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.
+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.
+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.
+3. This notice may not be removed or altered from any source distribution.
-*/
+*/
-///original version written by Erwin Coumans, October 2013
+///original version written by Erwin Coumans, October 2013
-
+
-#ifndef BT_DANTZIG_SOLVER_H
+#ifndef BT_DANTZIG_SOLVER_H
-#define BT_DANTZIG_SOLVER_H
+#define BT_DANTZIG_SOLVER_H
-
+
-#include "btMLCPSolverInterface.h"
+#include "btMLCPSolverInterface.h"
-#include "btDantzigLCP.h"
+#include "btDantzigLCP.h"
-
+
-
+
-class btDantzigSolver : public btMLCPSolverInterface
+class btDantzigSolver : public btMLCPSolverInterface
-{
+{
-protected:
+protected:
-
+
-	btScalar m_acceptableUpperLimitSolution;
+	btScalar m_acceptableUpperLimitSolution;
-
+
-	btAlignedObjectArray<char>	m_tempBuffer;
+	btAlignedObjectArray<char>	m_tempBuffer;
-
+
-	btAlignedObjectArray<btScalar> m_A;
+	btAlignedObjectArray<btScalar> m_A;
-	btAlignedObjectArray<btScalar> m_b;
+	btAlignedObjectArray<btScalar> m_b;
-	btAlignedObjectArray<btScalar> m_x;
+	btAlignedObjectArray<btScalar> m_x;
-	btAlignedObjectArray<btScalar> m_lo;
+	btAlignedObjectArray<btScalar> m_lo;
-	btAlignedObjectArray<btScalar> m_hi;
+	btAlignedObjectArray<btScalar> m_hi;
-	btAlignedObjectArray<int>	m_dependencies;
+	btAlignedObjectArray<int>	m_dependencies;
-	btDantzigScratchMemory m_scratchMemory;
+	btDantzigScratchMemory m_scratchMemory;
-public:
+public:
-
+
-	btDantzigSolver()
+	btDantzigSolver()
-		:m_acceptableUpperLimitSolution(btScalar(1000))
+		: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)
+	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;
+		bool result = true;
-		int n = b.rows();
+		int n = b.rows();
-		if (n)
+		if (n)
-		{
+		{
-			int nub = 0;
+			int nub = 0;
-			btAlignedObjectArray<btScalar> ww;
+			btAlignedObjectArray<btScalar> ww;
-			ww.resize(n);
+			ww.resize(n);
-	
+	
-
+
-			const btScalar* Aptr = A.getBufferPointer();
+			const btScalar* Aptr = A.getBufferPointer();
-			m_A.resize(n*n);
+			m_A.resize(n*n);
-			for (int i=0;i<n*n;i++)
+			for (int i=0;i<n*n;i++)
-			{
+			{
-				m_A[i] = Aptr[i];
+				m_A[i] = Aptr[i];
-
+
-			}
+			}
-
+
-			m_b.resize(n);
+			m_b.resize(n);
-			m_x.resize(n);
+			m_x.resize(n);
-			m_lo.resize(n);
+			m_lo.resize(n);
-			m_hi.resize(n);
+			m_hi.resize(n);
-			m_dependencies.resize(n);
+			m_dependencies.resize(n);
-			for (int i=0;i<n;i++)
+			for (int i=0;i<n;i++)
-			{
+			{
-				m_lo[i] = lo[i];
+				m_lo[i] = lo[i];
-				m_hi[i] = hi[i];
+				m_hi[i] = hi[i];
-				m_b[i] = b[i];
+				m_b[i] = b[i];
-				m_x[i] = x[i];
+				m_x[i] = x[i];
-				m_dependencies[i] = limitDependency[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);
+			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)
+			if (!result)
-				return result;
+				return result;
-
+
-//			printf("numAllocas = %d\n",numAllocas);
+//			printf("numAllocas = %d\n",numAllocas);
-			for (int i=0;i<n;i++)
+			for (int i=0;i<n;i++)
-			{
+			{
-				volatile btScalar xx = m_x[i];
+				volatile btScalar xx = m_x[i];
-				if (xx != m_x[i])
+				if (xx != m_x[i])
-					return false;
+					return false;
-				if (x[i] >= m_acceptableUpperLimitSolution)
+				if (x[i] >= m_acceptableUpperLimitSolution)
-				{
+				{
-					return false;
+					return false;
-				}
+				}
-
+
-				if (x[i] <= -m_acceptableUpperLimitSolution)
+				if (x[i] <= -m_acceptableUpperLimitSolution)
-				{
+				{
-					return false;
+					return false;
-				}
+				}
-			}
+			}
-
+
-			for (int i=0;i<n;i++)
+			for (int i=0;i<n;i++)
-			{
+			{
-				x[i] = m_x[i];
+				x[i] = m_x[i];
-			}
+			}
-			
+			
-		}
+		}
-
+
-		return result;
+		return result;
-	}
+	}
-};
+};
-
+
-#endif //BT_DANTZIG_SOLVER_H
+#endif //BT_DANTZIG_SOLVER_H

src/BulletDynamics/MLCPSolvers/btLemkeSolver.h

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

src/BulletDynamics/MLCPSolvers/btMLCPSolver.h

@@ -1,93 +1,93 @@
-/*
+/*
-Bullet Continuous Collision Detection and Physics Library
+Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
-
+
-This software is provided 'as-is', without any express or implied warranty.
+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.
+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, 
+Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
+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.
+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.
+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.
+3. This notice may not be removed or altered from any source distribution.
-*/
+*/
-///original version written by Erwin Coumans, October 2013
+///original version written by Erwin Coumans, October 2013
-
+
-#ifndef BT_MLCP_SOLVER_H
+#ifndef BT_MLCP_SOLVER_H
-#define BT_MLCP_SOLVER_H
+#define BT_MLCP_SOLVER_H
-
+
-#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
-#include "LinearMath/btMatrixX.h"
+#include "LinearMath/btMatrixX.h"
-#include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h"
+#include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h"
-
+
-class btMLCPSolver : public btSequentialImpulseConstraintSolver
+class btMLCPSolver : public btSequentialImpulseConstraintSolver
-{
+{
-
+
-protected:
+protected:
-	
+	
-	btMatrixXu m_A;
+	btMatrixXu m_A;
-	btVectorXu m_b;
+	btVectorXu m_b;
-	btVectorXu m_x;
+	btVectorXu m_x;
-	btVectorXu m_lo;
+	btVectorXu m_lo;
-	btVectorXu m_hi;
+	btVectorXu m_hi;
-	
+	
-	///when using 'split impulse' we solve two separate (M)LCPs
+	///when using 'split impulse' we solve two separate (M)LCPs
-	btVectorXu m_bSplit;
+	btVectorXu m_bSplit;
-	btVectorXu m_xSplit;
+	btVectorXu m_xSplit;
-	btVectorXu m_bSplit1;
+	btVectorXu m_bSplit1;
-	btVectorXu m_xSplit2;
+	btVectorXu m_xSplit2;
-
+
-	btAlignedObjectArray<int> m_limitDependencies;
+	btAlignedObjectArray<int> m_limitDependencies;
-	btAlignedObjectArray<btSolverConstraint*>	m_allConstraintPtrArray;
+	btAlignedObjectArray<btSolverConstraint*>	m_allConstraintPtrArray;
-	btMLCPSolverInterface* m_solver;
+	btMLCPSolverInterface* m_solver;
-	int m_fallback;
+	int m_fallback;
-	btScalar m_cfm;
+	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 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 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 createMLCP(const btContactSolverInfo& infoGlobal);
-	virtual void createMLCPFast(const btContactSolverInfo& infoGlobal);
+	virtual void createMLCPFast(const btContactSolverInfo& infoGlobal);
-
+
-	//return true is it solves the problem successfully
+	//return true is it solves the problem successfully
-	virtual bool solveMLCP(const btContactSolverInfo& infoGlobal);
+	virtual bool solveMLCP(const btContactSolverInfo& infoGlobal);
-
+
-public:
+public:
-
+
-	btMLCPSolver(	 btMLCPSolverInterface* solver);
+	btMLCPSolver(	 btMLCPSolverInterface* solver);
-	virtual ~btMLCPSolver();
+	virtual ~btMLCPSolver();
-
+
-	void setMLCPSolver(btMLCPSolverInterface* solver)
+	void setMLCPSolver(btMLCPSolverInterface* solver)
-	{
+	{
-		m_solver = solver;
+		m_solver = solver;
-	}
+	}
-
+
-	int getNumFallbacks() const
+	int getNumFallbacks() const
-	{
+	{
-		return m_fallback;
+		return m_fallback;
-	}
+	}
-	void setNumFallbacks(int num)
+	void setNumFallbacks(int num)
-	{
+	{
-		m_fallback = num;
+		m_fallback = num;
-	}
+	}
-
+
-	btScalar	getCfm() const
+	btScalar	getCfm() const
-	{
+	{
-		return m_cfm;
+		return m_cfm;
-	}
+	}
-	void setCfm(btScalar cfm)
+	void setCfm(btScalar cfm)
-	{
+	{
-		m_cfm = cfm;
+		m_cfm = cfm;
-	}
+	}
-
+
-	virtual btConstraintSolverType	getSolverType() const
+	virtual btConstraintSolverType	getSolverType() const
-	{
+	{
-		return BT_MLCP_SOLVER;
+		return BT_MLCP_SOLVER;
-	}
+	}
-
+
-};
+};
-
+
-
+
-#endif //BT_MLCP_SOLVER_H
+#endif //BT_MLCP_SOLVER_H

src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h

@@ -1,33 +1,33 @@
-/*
+/*
-Bullet Continuous Collision Detection and Physics Library
+Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
-
+
-This software is provided 'as-is', without any express or implied warranty.
+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.
+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, 
+Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
+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.
+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.
+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.
+3. This notice may not be removed or altered from any source distribution.
-*/
+*/
-///original version written by Erwin Coumans, October 2013
+///original version written by Erwin Coumans, October 2013
-
+
-#ifndef BT_MLCP_SOLVER_INTERFACE_H
+#ifndef BT_MLCP_SOLVER_INTERFACE_H
-#define BT_MLCP_SOLVER_INTERFACE_H
+#define BT_MLCP_SOLVER_INTERFACE_H
-
+
-#include "LinearMath/btMatrixX.h"
+#include "LinearMath/btMatrixX.h"
-
+
-class btMLCPSolverInterface
+class btMLCPSolverInterface
-{
+{
-public:
+public:
-	virtual ~btMLCPSolverInterface()
+	virtual ~btMLCPSolverInterface()
-	{
+	{
-	}
+	}
-
+
-	//return true is it solves the problem successfully
+	//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;
+	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
+#endif //BT_MLCP_SOLVER_INTERFACE_H

src/BulletDynamics/MLCPSolvers/btPATHSolver.h

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

src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h

@@ -1,86 +1,86 @@
-/*
+/*
-Bullet Continuous Collision Detection and Physics Library
+Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
-
+
-This software is provided 'as-is', without any express or implied warranty.
+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.
+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, 
+Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
+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.
+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.
+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.
+3. This notice may not be removed or altered from any source distribution.
-*/
+*/
-///original version written by Erwin Coumans, October 2013
+///original version written by Erwin Coumans, October 2013
-
+
-#ifndef BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
+#ifndef BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
-#define BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
+#define BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
-
+
-
+
-#include "btMLCPSolverInterface.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)
+///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
+class btSolveProjectedGaussSeidel : public btMLCPSolverInterface
-{
+{
-public:
+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)
+	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())
+		if (!A.rows())
-			return true;
+			return true;
-		//the A matrix is sparse, so compute the non-zero elements
+		//the A matrix is sparse, so compute the non-zero elements
-		A.rowComputeNonZeroElements();
+		A.rowComputeNonZeroElements();
-
+
-		//A is a m-n matrix, m rows, n columns
+		//A is a m-n matrix, m rows, n columns
-		btAssert(A.rows() == b.rows());
+		btAssert(A.rows() == b.rows());
-
+
-		int i, j, numRows = A.rows();
+		int i, j, numRows = A.rows();
-	
+	
-		float delta;
+		float delta;
-
+
-		for (int k = 0; k <numIterations; k++)
+		for (int k = 0; k <numIterations; k++)
-		{
+		{
-			for (i = 0; i <numRows; i++)
+			for (i = 0; i <numRows; i++)
-			{
+			{
-				delta = 0.0f;
+				delta = 0.0f;
-				if (useSparsity)
+				if (useSparsity)
-				{
+				{
-					for (int h=0;h<A.m_rowNonZeroElements1[i].size();h++)
+					for (int h=0;h<A.m_rowNonZeroElements1[i].size();h++)
-					{
+					{
-						int j = A.m_rowNonZeroElements1[i][h];
+						int j = A.m_rowNonZeroElements1[i][h];
-						if (j != i)//skip main diagonal
+						if (j != i)//skip main diagonal
-						{
+						{
-							delta += A(i,j) * x[j];
+							delta += A(i,j) * x[j];
-						}
+						}
-					}
+					}
-				} else
+				} else
-				{
+				{
-					for (j = 0; j <i; j++) 
+					for (j = 0; j <i; j++) 
-						delta += A(i,j) * x[j];
+						delta += A(i,j) * x[j];
-					for (j = i+1; j<numRows; j++) 
+					for (j = i+1; j<numRows; j++) 
-						delta += A(i,j) * x[j];
+						delta += A(i,j) * x[j];
-				}
+				}
-
+
-				float aDiag = A(i,i);
+				float aDiag = A(i,i);
-				x [i] = (b [i] - delta) / A(i,i);
+				x [i] = (b [i] - delta) / aDiag;
-				float s = 1.f;
+				float s = 1.f;
-
+
-				if (limitDependency[i]>=0)
+				if (limitDependency[i]>=0)
-				{
+				{
-					s = x[limitDependency[i]];
+					s = x[limitDependency[i]];
-					if (s<0)
+					if (s<0)
-						s=1;
+						s=1;
-				}
+				}
-			
+			
-				if (x[i]<lo[i]*s)
+				if (x[i]<lo[i]*s)
-					x[i]=lo[i]*s;
+					x[i]=lo[i]*s;
-				if (x[i]>hi[i]*s)
+				if (x[i]>hi[i]*s)
-					x[i]=hi[i]*s;
+					x[i]=hi[i]*s;
-			}
+			}
-		}
+		}
-		return true;
+		return true;
-	}
+	}
-
+
-};
+};
-
+
-#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
+#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; 
+		//btScalar depth; 
-		depth = rayCast( m_wheelInfo[i]);
+		//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;
+//					T dotProd2=0;
-					int waste=0,waste2=0;
+					//int waste=0,waste2=0;
 
 					{
-						bool useOtherCol = true;
+//						bool useOtherCol = true;
 						{
 							for (int v=0;v<rows();v++)
 							{