From af5883c6e8eeb4f5d4921d4eb88a601d8eaec0bc Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 22 Aug 2014 10:29:05 -0700 Subject: [PATCH] remove a lot of warnings (more todo in demos and serialization code) --- Demos3/AllBullet2Demos/BulletDemoEntries.h | 6 +- Demos3/AllBullet2Demos/GraphingTexture.cpp | 11 +- Demos3/AllBullet2Demos/GraphingTexture.h | 2 +- .../GwenParameterInterface.cpp | 2 +- Demos3/AllBullet2Demos/GwenProfileWindow.cpp | 18 +- Demos3/AllBullet2Demos/main.cpp | 34 +- Demos3/GpuDemos/gwenUserInterface.cpp | 2 +- Demos3/ImportObjDemo/ImportObjSetup.cpp | 7 +- Demos3/ImportSTLDemo/ImportSTLSetup.cpp | 5 +- .../BulletMultiBodyDemos.cpp | 23 +- .../MultiDofDemo.cpp | 4 +- btgui/Gwen/Controls/CrossSplitter.cpp | 2 +- btgui/Gwen/Controls/Resizer.cpp | 2 +- btgui/Gwen/Controls/TreeControl.cpp | 40 +- btgui/Gwen/Controls/TreeNode.cpp | 4 +- btgui/Gwen/Renderers/OpenGL_DebugFont.cpp | 12 +- btgui/Gwen/Skins/Simple.h | 4 +- btgui/OpenGLTrueTypeFont/fontstash.cpp | 8 +- .../opengl_fontstashcallbacks.cpp | 73 +- btgui/OpenGLWindow/GLPrimitiveRenderer.cpp | 75 +- btgui/OpenGLWindow/GwenOpenGL3CoreRenderer.h | 47 +- btgui/OpenGLWindow/LoadShader.cpp | 12 +- btgui/OpenGLWindow/SimpleOpenGL3App.cpp | 10 +- .../btKinematicCharacterController.cpp | 4 +- .../btConeTwistConstraint.cpp | 8 +- .../ConstraintSolver/btContactConstraint.cpp | 3 +- .../ConstraintSolver/btFixedConstraint.cpp | 370 ++-- .../ConstraintSolver/btFixedConstraint.h | 96 +- .../ConstraintSolver/btGearConstraint.cpp | 108 +- .../ConstraintSolver/btGearConstraint.h | 268 +-- .../btGeneric6DofSpring2Constraint.cpp | 6 +- .../ConstraintSolver/btHingeConstraint.h | 8 + .../btNNCGConstraintSolver.cpp | 926 ++++----- .../ConstraintSolver/btNNCGConstraintSolver.h | 128 +- .../btSequentialImpulseConstraintSolver.cpp | 10 +- .../Dynamics/btDiscreteDynamicsWorld.cpp | 134 +- .../Featherstone/btMultiBody.cpp | 28 +- src/BulletDynamics/Featherstone/btMultiBody.h | 17 +- .../Featherstone/btMultiBodyConstraint.cpp | 730 +++---- .../Featherstone/btMultiBodyConstraint.h | 320 +-- .../btMultiBodyConstraintSolver.cpp | 1813 +++++++++-------- .../btMultiBodyConstraintSolver.h | 172 +- .../Featherstone/btMultiBodyDynamicsWorld.cpp | 1506 +++++++------- .../Featherstone/btMultiBodyDynamicsWorld.h | 112 +- .../btMultiBodyJointLimitConstraint.cpp | 290 +-- .../btMultiBodyJointLimitConstraint.h | 88 +- .../Featherstone/btMultiBodyJointMotor.cpp | 2 +- .../Featherstone/btMultiBodyLink.h | 7 +- .../Featherstone/btMultiBodyLinkCollider.h | 184 +- .../Featherstone/btMultiBodyPoint2Point.cpp | 1 - .../Featherstone/btMultiBodyPoint2Point.h | 124 +- .../btMultiBodySolverConstraint.h | 170 +- .../MLCPSolvers/btDantzigLCP.cpp | 5 +- .../MLCPSolvers/btDantzigSolver.h | 224 +- .../MLCPSolvers/btLemkeSolver.h | 700 +++---- .../MLCPSolvers/btMLCPSolver.cpp | 1280 ++++++------ src/BulletDynamics/MLCPSolvers/btMLCPSolver.h | 186 +- .../MLCPSolvers/btMLCPSolverInterface.h | 66 +- src/BulletDynamics/MLCPSolvers/btPATHSolver.h | 302 +-- .../MLCPSolvers/btSolveProjectedGaussSeidel.h | 172 +- .../Vehicle/btRaycastVehicle.cpp | 5 +- src/LinearMath/btMatrixX.h | 6 +- 62 files changed, 5469 insertions(+), 5513 deletions(-) diff --git a/Demos3/AllBullet2Demos/BulletDemoEntries.h b/Demos3/AllBullet2Demos/BulletDemoEntries.h index d992acf9a..cdff06227 100644 --- a/Demos3/AllBullet2Demos/BulletDemoEntries.h +++ b/Demos3/AllBullet2Demos/BulletDemoEntries.h @@ -126,9 +126,13 @@ static int loadCurrentDemoEntry(const char* startFileName) { int result; result = fscanf(f,"%d",¤tEntry); + if (result) + { + return currentEntry; + } fclose(f); } - return currentEntry; + return 0; }; #endif//BULLET_DEMO_ENTRIES_H diff --git a/Demos3/AllBullet2Demos/GraphingTexture.cpp b/Demos3/AllBullet2Demos/GraphingTexture.cpp index d81072298..19eac1ff8 100644 --- a/Demos3/AllBullet2Demos/GraphingTexture.cpp +++ b/Demos3/AllBullet2Demos/GraphingTexture.cpp @@ -14,7 +14,7 @@ GraphingTexture::~GraphingTexture() destroy(); } -bool GraphingTexture::destroy() +void GraphingTexture::destroy() { //TODO(erwincoumans) release memory etc... m_width = 0; @@ -58,21 +58,18 @@ bool GraphingTexture::create(int texWidth, int texHeight) glGenTextures(1,(GLuint*)&m_textureId); uploadImageData(); + return true; } void GraphingTexture::uploadImageData() { glBindTexture(GL_TEXTURE_2D,m_textureId); - GLint err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); - err = glGetError(); - assert(err==GL_NO_ERROR); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, m_width,m_height,0,GL_RGBA,GL_UNSIGNED_BYTE,&m_imageData[0]); glGenerateMipmap(GL_TEXTURE_2D); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); } diff --git a/Demos3/AllBullet2Demos/GraphingTexture.h b/Demos3/AllBullet2Demos/GraphingTexture.h index 33d209e69..79c62dbed 100644 --- a/Demos3/AllBullet2Demos/GraphingTexture.h +++ b/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) { diff --git a/Demos3/AllBullet2Demos/GwenParameterInterface.cpp b/Demos3/AllBullet2Demos/GwenParameterInterface.cpp index be4802174..93ce77fff 100644 --- a/Demos3/AllBullet2Demos/GwenParameterInterface.cpp +++ b/Demos3/AllBullet2Demos/GwenParameterInterface.cpp @@ -124,7 +124,7 @@ void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params) pSlider->onValueChanged.Add( handler, &MySliderEventHandler::SliderMoved ); handler->SliderMoved(pSlider); - float v = pSlider->GetValue(); +// float v = pSlider->GetValue(); m_gwenInternalData->m_curYposition+=22; } diff --git a/Demos3/AllBullet2Demos/GwenProfileWindow.cpp b/Demos3/AllBullet2Demos/GwenProfileWindow.cpp index 86cae4d2e..5efb3e364 100644 --- a/Demos3/AllBullet2Demos/GwenProfileWindow.cpp +++ b/Demos3/AllBullet2Demos/GwenProfileWindow.cpp @@ -27,7 +27,7 @@ protected: void SliderMoved(Gwen::Controls::Base* pControl ) { - Gwen::Controls::Slider* pSlider = (Gwen::Controls::Slider*)pControl; + // Gwen::Controls::Slider* pSlider = (Gwen::Controls::Slider*)pControl; //this->m_app->scaleYoungModulus(pSlider->GetValue()); // printf("Slider Value: %.2f", pSlider->GetValue() ); } @@ -35,8 +35,8 @@ protected: void OnCheckChangedStiffnessWarping (Gwen::Controls::Base* pControl) { - Gwen::Controls::CheckBox* labeled = (Gwen::Controls::CheckBox* )pControl; - bool checked = labeled->IsChecked(); + // Gwen::Controls::CheckBox* labeled = (Gwen::Controls::CheckBox* )pControl; +// bool checked = labeled->IsChecked(); //m_app->m_stiffness_warp_on = checked; } public: @@ -148,13 +148,13 @@ public: void UpdateText(CProfileIterator* profileIterator, bool idle) { - static bool update=true; + // static bool update=true; m_ctrl->SetBounds(0,0,this->GetInnerBounds().w,this->GetInnerBounds().h); // if (!update) // return; - update=false; + // update=false; static int test = 1; @@ -170,18 +170,18 @@ public: { //recompute profiling data, and store profile strings - char blockTime[128]; + // char blockTime[128]; - double totalTime = 0; + // double totalTime = 0; - int frames_since_reset = CProfileManager::Get_Frame_Count_Since_Reset(); + // int frames_since_reset = CProfileManager::Get_Frame_Count_Since_Reset(); profileIterator->First(); double parent_time = profileIterator->Is_Root() ? time_since_reset : profileIterator->Get_Current_Parent_Total_Time(); - Gwen::Controls::TreeNode* curParent = m_node; + // Gwen::Controls::TreeNode* curParent = m_node; double accumulated_time = dumpRecursive(profileIterator,m_node); diff --git a/Demos3/AllBullet2Demos/main.cpp b/Demos3/AllBullet2Demos/main.cpp index 0dad18905..9728068db 100644 --- a/Demos3/AllBullet2Demos/main.cpp +++ b/Demos3/AllBullet2Demos/main.cpp @@ -238,13 +238,13 @@ struct MyMenuItemHander :public Gwen::Event::Handler void onButtonA(Gwen::Controls::Base* pControl) { - const Gwen::String& name = pControl->GetName(); + //const Gwen::String& name = pControl->GetName(); Gwen::Controls::TreeNode* node = (Gwen::Controls::TreeNode*)pControl; - Gwen::Controls::Label* l = node->GetButton(); + // Gwen::Controls::Label* l = node->GetButton(); Gwen::UnicodeString la = node->GetButton()->GetText();// node->GetButton()->GetName();// GetText(); Gwen::String laa = Gwen::Utility::UnicodeToString(la); - const char* ha = laa.c_str(); + // const char* ha = laa.c_str(); //printf("selected %s\n", ha); //int dep = but->IsDepressed(); @@ -257,7 +257,7 @@ struct MyMenuItemHander :public Gwen::Event::Handler Gwen::Controls::Label* label = (Gwen::Controls::Label*) pControl; Gwen::UnicodeString la = label->GetText();// node->GetButton()->GetName();// GetText(); Gwen::String laa = Gwen::Utility::UnicodeToString(la); - const char* ha = laa.c_str(); + //const char* ha = laa.c_str(); selectDemo(sCurrentHightlighted); @@ -265,10 +265,10 @@ struct MyMenuItemHander :public Gwen::Event::Handler } void onButtonC(Gwen::Controls::Base* pControl) { - Gwen::Controls::Label* label = (Gwen::Controls::Label*) pControl; - Gwen::UnicodeString la = label->GetText();// node->GetButton()->GetName();// GetText(); - Gwen::String laa = Gwen::Utility::UnicodeToString(la); - const char* ha = laa.c_str(); +// Gwen::Controls::Label* label = (Gwen::Controls::Label*) pControl; + // Gwen::UnicodeString la = label->GetText();// node->GetButton()->GetName();// GetText(); + // Gwen::String laa = Gwen::Utility::UnicodeToString(la); + // const char* ha = laa.c_str(); // printf("onButtonC ! %s\n", ha); @@ -334,7 +334,7 @@ int main(int argc, char* argv[]) b3Clock clock; - float dt = 1./120.f; + //float dt = 1./120.f; int width = 1024; int height=768; @@ -348,8 +348,7 @@ int main(int argc, char* argv[]) app->m_window->setKeyboardCallback(MyKeyboardCallback); - GLint err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); sth_stash* fontstash=app->getFontStash(); gui = new GwenUserInterface; @@ -376,7 +375,8 @@ int main(int argc, char* argv[]) gt->create(256,256); int texId = gt->getTextureId(); myTexLoader->m_hashMap.insert("graph1", texId); - MyGraphWindow* gw = setupTextureWindow(input); + //MyGraphWindow* gw = + setupTextureWindow(input); } if (1) { @@ -403,7 +403,8 @@ int main(int argc, char* argv[]) int texId = gt->getTextureId(); input.m_xPos = width-input.m_width; myTexLoader->m_hashMap.insert("graph2", texId); - MyGraphWindow* gw = setupTextureWindow(input); + //MyGraphWindow* gw = + setupTextureWindow(input); } //destroyTextureWindow(gw); @@ -414,8 +415,8 @@ int main(int argc, char* argv[]) int numDemos = sizeof(allDemos)/sizeof(BulletDemoEntry); - char nodeText[1024]; - int curDemo = 0; + //char nodeText[1024]; + //int curDemo = 0; int selectedDemo = loadCurrentDemoEntry(startFileName); Gwen::Controls::TreeNode* curNode = tree; MyMenuItemHander* handler2 = new MyMenuItemHander(-1); @@ -476,8 +477,7 @@ int main(int argc, char* argv[]) do { - GLint err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); app->m_instancingRenderer->init(); DrawGridData dg; dg.upAxis = app->getUpAxis(); diff --git a/Demos3/GpuDemos/gwenUserInterface.cpp b/Demos3/GpuDemos/gwenUserInterface.cpp index 4db1347aa..c7ce8f6c2 100644 --- a/Demos3/GpuDemos/gwenUserInterface.cpp +++ b/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); diff --git a/Demos3/ImportObjDemo/ImportObjSetup.cpp b/Demos3/ImportObjDemo/ImportObjSetup.cpp index 294788740..fb65a7b08 100644 --- a/Demos3/ImportObjDemo/ImportObjSetup.cpp +++ b/Demos3/ImportObjDemo/ImportObjSetup.cpp @@ -28,7 +28,7 @@ static GLInstanceGraphicsShape* gCreateGraphicsShapeFromWavefrontObj(std::vector // int numIndices = 0; b3AlignedObjectArray* indicesPtr = new b3AlignedObjectArray; - for (int s=0;sm_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); /* diff --git a/Demos3/ImportSTLDemo/ImportSTLSetup.cpp b/Demos3/ImportSTLDemo/ImportSTLSetup.cpp index ae20f3e58..e67526150 100644 --- a/Demos3/ImportSTLDemo/ImportSTLSetup.cpp +++ b/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); /* diff --git a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp index 75b6b9cf2..34db331d8 100644 --- a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp +++ b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp @@ -169,7 +169,7 @@ bool Bullet2MultiBodyDemo::mouseMoveCallback(float x,float y) //keep it at the same picking distance btVector3 newRayTo = getRayTo(x,y); btVector3 rayFrom; - btVector3 oldPivotInB = pickCon->getPivotInB(); +// btVector3 oldPivotInB = pickCon->getPivotInB(); btVector3 newPivotB; m_glApp->m_instancingRenderer->getCameraPosition(rayFrom); btVector3 dir = newRayTo-rayFrom; @@ -186,7 +186,7 @@ bool Bullet2MultiBodyDemo::mouseMoveCallback(float x,float y) btVector3 newRayTo = getRayTo(x,y); btVector3 rayFrom; - btVector3 oldPivotInB = m_pickingMultiBodyPoint2Point->getPivotInB(); + // btVector3 oldPivotInB = m_pickingMultiBodyPoint2Point->getPivotInB(); btVector3 newPivotB; btVector3 camPos; m_glApp->m_instancingRenderer->getCameraPosition(camPos); @@ -438,7 +438,7 @@ btMultiBody* FeatherstoneDemo1::createFeatherstoneMultiBody(class btMultiBodyDyn btVector4 halfExtents(7.5,0.45,4.5,1); { - float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; + // float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; @@ -484,7 +484,7 @@ btMultiBody* FeatherstoneDemo1::createFeatherstoneMultiBody(class btMultiBodyDyn { btVector3 posr = local_origin[i+1]; - float pos[4]={posr.x(),posr.y(),posr.z(),1}; + //float pos[4]={posr.x(),posr.y(),posr.z(),1}; float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; @@ -531,8 +531,8 @@ void FeatherstoneDemo1::createGround() { //create ground int cubeShapeId = m_glApp->registerCubeShape(); - float pos[]={0,0,0}; - float orn[]={0,0,0,1}; + //float pos[]={0,0,0}; + //float orn[]={0,0,0,1}; { @@ -575,7 +575,8 @@ void FeatherstoneDemo1::initPhysics() settings.m_isFixedBase = false; settings.m_basePosition.setValue(0,10,0); settings.m_numLinks = 10; - btMultiBody* mb = createFeatherstoneMultiBody(m_dynamicsWorld,settings); + //btMultiBody* mb = + createFeatherstoneMultiBody(m_dynamicsWorld,settings); m_glApp->m_instancingRenderer->writeTransforms(); @@ -727,7 +728,7 @@ class RagDoll2 hull->buildHull(0.01); { - int strideInBytes = 9*sizeof(float); + // int strideInBytes = 9*sizeof(float); int numVertices = hull->numVertices(); int numIndices =hull->numIndices(); @@ -988,7 +989,7 @@ public: virtual ~RagDoll2 () { - int i; + //int i; /* // Remove all constraints for ( i = 0; i < JOINT_COUNT; ++i) @@ -1027,8 +1028,8 @@ void FeatherstoneDemo2::initPhysics() settings.m_usePrismatic = true; btMultiBody* mb = createFeatherstoneMultiBody(m_dynamicsWorld,settings); */ - btVector3 offset(0,2,0); - RagDoll2* doll = new RagDoll2(m_dynamicsWorld,offset,m_glApp); +// btVector3 offset(0,2,0); + //RagDoll2* doll = new RagDoll2(m_dynamicsWorld,offset,m_glApp); m_glApp->m_instancingRenderer->writeTransforms(); diff --git a/Demos3/bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.cpp b/Demos3/bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.cpp index f31130428..0f76467bd 100644 --- a/Demos3/bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.cpp +++ b/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()}; diff --git a/btgui/Gwen/Controls/CrossSplitter.cpp b/btgui/Gwen/Controls/CrossSplitter.cpp index 2d52d61cc..f80c7255d 100644 --- a/btgui/Gwen/Controls/CrossSplitter.cpp +++ b/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() ); } } diff --git a/btgui/Gwen/Controls/Resizer.cpp b/btgui/Gwen/Controls/Resizer.cpp index c920d740e..b3506f849 100644 --- a/btgui/Gwen/Controls/Resizer.cpp +++ b/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(); diff --git a/btgui/Gwen/Controls/TreeControl.cpp b/btgui/Gwen/Controls/TreeControl.cpp index 972ec8456..5a6f71477 100644 --- a/btgui/Gwen/Controls/TreeControl.cpp +++ b/btgui/Gwen/Controls/TreeControl.cpp @@ -101,12 +101,12 @@ bool TreeControl::OnKeyUp( bool bDown ) if (bDown) { ForceUpdateScrollBars(); - int maxIndex = 0; +// int maxIndex = 0; int newIndex = 0; int maxItem=0; int curItem=-1; iterate(ITERATE_ACTION_FIND_SELECTED_INDEX,&maxItem,&curItem); - maxIndex = maxItem; + // maxIndex = maxItem; int targetItem = curItem; if (curItem>0) { @@ -121,15 +121,15 @@ bool TreeControl::OnKeyUp( bool bDown ) iterate(ITERATE_ACTION_DESELECT_INDEX,&maxItem,&deselectIndex); } curItem = newIndex; - float amount = float(newIndex)/float(maxIndex); + // float amount = float(newIndex)/float(maxIndex); float viewSize = m_ScrollControl->m_VerticalScrollBar->getViewableContentSize(); float contSize = m_ScrollControl->m_VerticalScrollBar->getContentSize(); float curAmount = m_ScrollControl->m_VerticalScrollBar->GetScrolledAmount(); - float minCoordViewableWindow = curAmount*contSize; - float maxCoordViewableWindow = minCoordViewableWindow+viewSize; + // float minCoordViewableWindow = curAmount*contSize; + //float maxCoordViewableWindow = minCoordViewableWindow+viewSize; float minCoordSelectedItem = curItem*16.f; - float maxCoordSelectedItem = (curItem+1)*16.f; + // float maxCoordSelectedItem = (curItem+1)*16.f; { float newAmount = float(minCoordSelectedItem)/(contSize-viewSize); if (newAmount=0) { @@ -177,15 +177,15 @@ bool TreeControl::OnKeyDown( bool bDown ) iterate(ITERATE_ACTION_DESELECT_INDEX,&maxItem,&deselectIndex); } curItem= newIndex; - float amount = (int)float(newIndex)/float(maxIndex); + // float amount = (int)float(newIndex)/float(maxIndex); float viewSize = m_ScrollControl->m_VerticalScrollBar->getViewableContentSize(); float contSize = m_ScrollControl->m_VerticalScrollBar->getContentSize(); float curAmount = m_ScrollControl->m_VerticalScrollBar->GetScrolledAmount(); - float minCoordViewableWindow = curAmount*contSize; - float maxCoordViewableWindow = minCoordViewableWindow+viewSize; + // float minCoordViewableWindow = curAmount*contSize; + //float maxCoordViewableWindow = minCoordViewableWindow+viewSize; float minCoordSelectedItem = curItem*16.f; - float maxCoordSelectedItem = (curItem+1)*16.f; + //float maxCoordSelectedItem = (curItem+1)*16.f; { float newAmount = float(minCoordSelectedItem)/(contSize-viewSize); if (newAmountm_VerticalScrollBar->getViewableContentSize(); float contSize = m_ScrollControl->m_VerticalScrollBar->getContentSize(); float curAmount = m_ScrollControl->m_VerticalScrollBar->GetScrolledAmount(); - float minCoordViewableWindow = curAmount*contSize; - float maxCoordViewableWindow = minCoordViewableWindow+viewSize; + // float minCoordViewableWindow = curAmount*contSize; +// float maxCoordViewableWindow = minCoordViewableWindow+viewSize; float minCoordSelectedItem = curItem*16.f; - float maxCoordSelectedItem = (curItem+1)*16.f; + // float maxCoordSelectedItem = (curItem+1)*16.f; { float newAmount = float(minCoordSelectedItem)/(contSize-viewSize); if (newAmountm_VerticalScrollBar->SetScrolledAmount(amount,true); float viewSize = m_ScrollControl->m_VerticalScrollBar->getViewableContentSize(); float contSize = m_ScrollControl->m_VerticalScrollBar->getContentSize(); float curAmount = m_ScrollControl->m_VerticalScrollBar->GetScrolledAmount(); - float minCoordViewableWindow = curAmount*contSize; - float maxCoordViewableWindow = minCoordViewableWindow+viewSize; + // float minCoordViewableWindow = curAmount*contSize; + // float maxCoordViewableWindow = minCoordViewableWindow+viewSize; float minCoordSelectedItem = curItem*16.f; - float maxCoordSelectedItem = (curItem+1)*16.f; + // float maxCoordSelectedItem = (curItem+1)*16.f; { float newAmount = float(minCoordSelectedItem)/(contSize-viewSize); diff --git a/btgui/Gwen/Controls/TreeNode.cpp b/btgui/Gwen/Controls/TreeNode.cpp index ba694567c..df3945b8c 100644 --- a/btgui/Gwen/Controls/TreeNode.cpp +++ b/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) diff --git a/btgui/Gwen/Renderers/OpenGL_DebugFont.cpp b/btgui/Gwen/Renderers/OpenGL_DebugFont.cpp index 078598791..c8c351b06 100644 --- a/btgui/Gwen/Renderers/OpenGL_DebugFont.cpp +++ b/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; iDrawLinedRect( 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 ) { diff --git a/btgui/OpenGLTrueTypeFont/fontstash.cpp b/btgui/OpenGLTrueTypeFont/fontstash.cpp index 25d8f4e69..56eaeb3a8 100644 --- a/btgui/OpenGLTrueTypeFont/fontstash.cpp +++ b/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; diff --git a/btgui/OpenGLTrueTypeFont/opengl_fontstashcallbacks.cpp b/btgui/OpenGLTrueTypeFont/opengl_fontstashcallbacks.cpp index d15485219..35250ce2e 100644 --- a/btgui/OpenGLTrueTypeFont/opengl_fontstashcallbacks.cpp +++ b/btgui/OpenGLTrueTypeFont/opengl_fontstashcallbacks.cpp @@ -37,8 +37,7 @@ InternalOpenGL2RenderCallbacks::~InternalOpenGL2RenderCallbacks() void InternalOpenGL2RenderCallbacks::display2() { - GLint err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); // glViewport(0,0,10,10); //const float timeScale = 0.008f; @@ -48,39 +47,31 @@ void InternalOpenGL2RenderCallbacks::display2() glBindBuffer(GL_ARRAY_BUFFER, s_vertexBuffer); glBindVertexArray(s_vertexArrayObject); - err = glGetError(); - assert(err==GL_NO_ERROR); - + assert(glGetError()==GL_NO_ERROR); + // glBindTexture(GL_TEXTURE_2D,m_texturehandle); - err = glGetError(); - assert(err==GL_NO_ERROR); - + assert(glGetError()==GL_NO_ERROR); + vec2 p( 0.f,0.f);//?b?0.5f * sinf(timeValue), 0.5f * cosf(timeValue) ); glUniform2fv(data->m_positionUniform, 1, (const GLfloat *)&p); - err = glGetError(); - assert(err==GL_NO_ERROR); - err = glGetError(); - assert(err==GL_NO_ERROR); - + assert(glGetError()==GL_NO_ERROR); + glEnableVertexAttribArray(data->m_positionAttribute); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glEnableVertexAttribArray(data->m_colourAttribute); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glEnableVertexAttribArray(data->m_textureAttribute); glVertexAttribPointer(data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)0); glVertexAttribPointer(data->m_colourAttribute , 4, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)sizeof(vec4)); glVertexAttribPointer(data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)(sizeof(vec4)+sizeof(vec4))); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); /* glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_indexBuffer); @@ -101,10 +92,7 @@ void InternalOpenGL2RenderCallbacks::display2() void InternalOpenGL2RenderCallbacks::updateTexture(sth_texture* texture, sth_glyph* glyph, int textureWidth, int textureHeight) { - GLint err; - - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); if (glyph) @@ -114,13 +102,11 @@ void InternalOpenGL2RenderCallbacks::updateTexture(sth_texture* texture, sth_gly glBindTexture(GL_TEXTURE_2D, *gltexture); glPixelStorei(GL_UNPACK_ALIGNMENT,1); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glTexImage2D(GL_TEXTURE_2D, 0, GL_RED, textureWidth, textureHeight, 0, GL_RED, GL_UNSIGNED_BYTE, texture->m_texels); - GLenum err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); } else { @@ -132,8 +118,7 @@ void InternalOpenGL2RenderCallbacks::updateTexture(sth_texture* texture, sth_gly //create new texture glGenTextures(1, texId); - GLenum err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); @@ -143,8 +128,7 @@ void InternalOpenGL2RenderCallbacks::updateTexture(sth_texture* texture, sth_gly glTexImage2D(GL_TEXTURE_2D, 0, GL_RED, textureWidth, textureHeight, 0, GL_RED, GL_UNSIGNED_BYTE, texture->m_texels); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); //////////////////////////// //create the other data @@ -155,8 +139,7 @@ void InternalOpenGL2RenderCallbacks::updateTexture(sth_texture* texture, sth_gly glGenBuffers(1, &s_vertexBuffer); glBindBuffer(GL_ARRAY_BUFFER, s_vertexBuffer); glBufferData(GL_ARRAY_BUFFER, VERT_COUNT * sizeof(Vertex), texture->newverts, GL_DYNAMIC_DRAW); - GLuint err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); for (int i=0;im_userData; - GLint err; - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glActiveTexture(GL_TEXTURE0); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glBindTexture(GL_TEXTURE_2D, *texId); bool useFiltering = false; @@ -215,25 +194,21 @@ void InternalOpenGL2RenderCallbacks::render(sth_texture* texture) glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); } - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glBindBuffer(GL_ARRAY_BUFFER, s_vertexBuffer); glBindVertexArray(s_vertexArrayObject); glBufferData(GL_ARRAY_BUFFER, texture->nverts * sizeof(Vertex), &texture->newverts[0].position.p[0], GL_DYNAMIC_DRAW); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, s_indexBuffer); //glDrawArrays(GL_TRIANGLE_FAN, 0, 4); int indexCount = texture->nverts; - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glDrawElements(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, 0); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glBindVertexArray(0); diff --git a/btgui/OpenGLWindow/GLPrimitiveRenderer.cpp b/btgui/OpenGLWindow/GLPrimitiveRenderer.cpp index f6c3cb47d..33e921216 100644 --- a/btgui/OpenGLWindow/GLPrimitiveRenderer.cpp +++ b/btgui/OpenGLWindow/GLPrimitiveRenderer.cpp @@ -131,9 +131,8 @@ void GLPrimitiveRenderer::loadBufferData() glGenBuffers(1, &m_data->m_vertexBuffer); glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vertexBuffer); glBufferData(GL_ARRAY_BUFFER, 4 * sizeof(Vertex), vertexData, GL_DYNAMIC_DRAW); - GLuint err; - err = glGetError(); - assert(err==GL_NO_ERROR); + + assert(glGetError()==GL_NO_ERROR); @@ -143,16 +142,14 @@ void GLPrimitiveRenderer::loadBufferData() glEnableVertexAttribArray(m_data->m_positionAttribute); glEnableVertexAttribArray(m_data->m_colourAttribute); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glEnableVertexAttribArray(m_data->m_textureAttribute); glVertexAttribPointer(m_data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)0); glVertexAttribPointer(m_data->m_colourAttribute , 4, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)sizeof(vec4)); glVertexAttribPointer(m_data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)(sizeof(vec4)+sizeof(vec4))); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); @@ -192,8 +189,7 @@ void GLPrimitiveRenderer::loadBufferData() glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, 256,256,0,GL_RGB,GL_UNSIGNED_BYTE,image); glGenerateMipmap(GL_TEXTURE_2D); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); delete[] image; @@ -218,36 +214,25 @@ void GLPrimitiveRenderer::drawLine() void GLPrimitiveRenderer::drawRect(float x0, float y0, float x1, float y1, float color[4]) { - GLint err; - - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glActiveTexture(GL_TEXTURE0); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glBindTexture(GL_TEXTURE_2D,m_data->m_texturehandle); - err = glGetError(); - assert(err==GL_NO_ERROR); - + assert(glGetError()==GL_NO_ERROR); drawTexturedRect(x0,y0,x1,y1,color,0,0,1,1); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); } void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA) { - GLint err; - - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glUseProgram(m_data->m_shaderProg); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vertexBuffer); glBindVertexArray(m_data->m_vertexArrayObject); @@ -277,8 +262,7 @@ void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); vec2 p( 0.f,0.f);//?b?0.5f * sinf(timeValue), 0.5f * cosf(timeValue) ); if (useRGBA) @@ -291,60 +275,47 @@ void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0); - err = glGetError(); - assert(err==GL_NO_ERROR); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glEnableVertexAttribArray(m_data->m_positionAttribute); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glEnableVertexAttribArray(m_data->m_colourAttribute); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glEnableVertexAttribArray(m_data->m_textureAttribute); glVertexAttribPointer(m_data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)0); glVertexAttribPointer(m_data->m_colourAttribute , 4, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)sizeof(vec4)); glVertexAttribPointer(m_data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)(sizeof(vec4)+sizeof(vec4))); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_data->m_indexBuffer); //glDrawArrays(GL_TRIANGLE_FAN, 0, 4); int indexCount = 6; - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glDrawElements(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, 0); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glBindVertexArray(0); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glBindBuffer(GL_ARRAY_BUFFER, 0); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); //glDisableVertexAttribArray(m_data->m_textureAttribute); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glUseProgram(0); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); } diff --git a/btgui/OpenGLWindow/GwenOpenGL3CoreRenderer.h b/btgui/OpenGLWindow/GwenOpenGL3CoreRenderer.h index 630f20c38..e4357bd7a 100644 --- a/btgui/OpenGLWindow/GwenOpenGL3CoreRenderer.h +++ b/btgui/OpenGLWindow/GwenOpenGL3CoreRenderer.h @@ -119,55 +119,37 @@ public: { m_yOffset=0; glEnable(GL_BLEND); - GLint err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glDisable(GL_DEPTH_TEST); - err = glGetError(); - assert(err==GL_NO_ERROR); - + assert(glGetError()==GL_NO_ERROR); //glColor4ub(255,0,0,255); - err = glGetError(); - assert(err==GL_NO_ERROR); - - err = glGetError(); - assert(err==GL_NO_ERROR); + glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA); // saveOpenGLState(width,height);//m_glutScreenWidth,m_glutScreenHeight); - err = glGetError(); - assert(err==GL_NO_ERROR); - + assert(glGetError()==GL_NO_ERROR); - err = glGetError(); - assert(err==GL_NO_ERROR); glDisable(GL_CULL_FACE); glDisable(GL_DEPTH_TEST); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); - err = glGetError(); - assert(err==GL_NO_ERROR); - + glEnable(GL_BLEND); - - err = glGetError(); - assert(err==GL_NO_ERROR); - + assert(glGetError()==GL_NO_ERROR); } virtual void End() { @@ -375,10 +357,7 @@ public: glBindTexture(GL_TEXTURE_2D,texHandle); // glDisable(GL_DEPTH_TEST); - GLint err; - - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); /* bool useFiltering = true; @@ -406,8 +385,8 @@ public: m_primitiveRenderer->drawTexturedRect(rect.x, rect.y+m_yOffset, rect.x+rect.w, rect.y+rect.h+m_yOffset, color,0+add,0,1+add,1,true); - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); + } diff --git a/btgui/OpenGLWindow/LoadShader.cpp b/btgui/OpenGLWindow/LoadShader.cpp index 4726504fc..e24fa7f12 100644 --- a/btgui/OpenGLWindow/LoadShader.cpp +++ b/btgui/OpenGLWindow/LoadShader.cpp @@ -18,9 +18,7 @@ void gltLoadShaderSrc(const char *szShaderSrc, GLuint shader) GLuint gltLoadShaderPair(const char *szVertexProg, const char *szFragmentProg) { - GLuint err; - err = glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); // Temporary Shader objects GLuint hVertexShader; @@ -37,7 +35,7 @@ GLuint gltLoadShaderPair(const char *szVertexProg, const char *szFragmentProg) // Compile them glCompileShader(hVertexShader); - err = glGetError(); + assert(glGetError()==GL_NO_ERROR); glGetShaderiv(hVertexShader, GL_COMPILE_STATUS, &testVal); if(testVal == GL_FALSE) @@ -52,10 +50,10 @@ GLuint gltLoadShaderPair(const char *szVertexProg, const char *szFragmentProg) return (GLuint)NULL; } - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); glCompileShader(hFragmentShader); - err = glGetError(); + assert(glGetError()==GL_NO_ERROR); glGetShaderiv(hFragmentShader, GL_COMPILE_STATUS, &testVal); if(testVal == GL_FALSE) @@ -70,7 +68,7 @@ GLuint gltLoadShaderPair(const char *szVertexProg, const char *szFragmentProg) return (GLuint)NULL; } - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); // Check for errors diff --git a/btgui/OpenGLWindow/SimpleOpenGL3App.cpp b/btgui/OpenGLWindow/SimpleOpenGL3App.cpp index b0b8c987c..e9c38d783 100644 --- a/btgui/OpenGLWindow/SimpleOpenGL3App.cpp +++ b/btgui/OpenGLWindow/SimpleOpenGL3App.cpp @@ -430,19 +430,17 @@ static void writeTextureToFile(int textureWidth, int textureHeight, const char* { int numComponents = 4; //glPixelStorei(GL_PACK_ALIGNMENT,1); - GLuint err=glGetError(); - assert(err==GL_NO_ERROR); + + b3Assert(glGetError()==GL_NO_ERROR); //glReadBuffer(GL_BACK);//COLOR_ATTACHMENT0); - err=glGetError(); - assert(err==GL_NO_ERROR); + float* orgPixels = (float*)malloc(textureWidth*textureHeight*numComponents*4); glReadPixels(0,0,textureWidth, textureHeight, GL_RGBA, GL_FLOAT, orgPixels); //it is useful to have the actual float values for debugging purposes //convert float->char char* pixels = (char*)malloc(textureWidth*textureHeight*numComponents); - err=glGetError(); - assert(err==GL_NO_ERROR); + assert(glGetError()==GL_NO_ERROR); for (int j=0;jm_rbAFrame.getBasis().getColumn(0); b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0); @@ -983,8 +983,8 @@ void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingA void btConeTwistConstraint::setMotorTarget(const btQuaternion &q) { - btTransform trACur = m_rbA.getCenterOfMassTransform(); - btTransform trBCur = m_rbB.getCenterOfMassTransform(); + //btTransform trACur = m_rbA.getCenterOfMassTransform(); + //btTransform trBCur = m_rbB.getCenterOfMassTransform(); // btTransform trABCur = trBCur.inverse() * trACur; // btQuaternion qABCur = trABCur.getRotation(); // btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame); diff --git a/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp index 9d60d9957..1098d0c96 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ b/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); diff --git a/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp index fd65bebe2..894059ad1 100644 --- a/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp @@ -1,186 +1,186 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btFixedConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" -#include - - -btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB) -:btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB) -{ - m_frameInA = frameInA; - m_frameInB = frameInB; - -} - -btFixedConstraint::~btFixedConstraint () -{ -} - - -void btFixedConstraint::getInfo1 (btConstraintInfo1* info) -{ - info->m_numConstraintRows = 6; - info->nub = 0; -} - -void btFixedConstraint::getInfo2 (btConstraintInfo2* info) -{ - //fix the 3 linear degrees of freedom - - const btTransform& transA = m_rbA.getCenterOfMassTransform(); - const btTransform& transB = m_rbB.getCenterOfMassTransform(); - - const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin(); - const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis(); - const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin(); - const btMatrix3x3& worldOrnB = m_rbB.getCenterOfMassTransform().getBasis(); - - - info->m_J1linearAxis[0] = 1; - info->m_J1linearAxis[info->rowskip+1] = 1; - info->m_J1linearAxis[2*info->rowskip+2] = 1; - - btVector3 a1 = worldOrnA * m_frameInA.getOrigin(); - { - btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); - btVector3 a1neg = -a1; - a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); - } - - if (info->m_J2linearAxis) - { - info->m_J2linearAxis[0] = -1; - info->m_J2linearAxis[info->rowskip+1] = -1; - info->m_J2linearAxis[2*info->rowskip+2] = -1; - } - - btVector3 a2 = worldOrnB*m_frameInB.getOrigin(); - { - btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); - a2.getSkewSymmetricMatrix(angular0,angular1,angular2); - } - - // set right hand side for the linear dofs - btScalar k = info->fps * info->erp; - - btVector3 linearError = k*(a2+worldPosB-a1-worldPosA); - int j; - for (j=0; j<3; j++) - { - info->m_constraintError[j*info->rowskip] = linearError[j]; - //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); - } - - btVector3 ivA = transA.getBasis() * m_frameInA.getBasis().getColumn(0); - btVector3 jvA = transA.getBasis() * m_frameInA.getBasis().getColumn(1); - btVector3 kvA = transA.getBasis() * m_frameInA.getBasis().getColumn(2); - btVector3 ivB = transB.getBasis() * m_frameInB.getBasis().getColumn(0); - btVector3 target; - btScalar x = ivB.dot(ivA); - btScalar y = ivB.dot(jvA); - btScalar z = ivB.dot(kvA); - btVector3 swingAxis(0,0,0); - { - if((!btFuzzyZero(y)) || (!(btFuzzyZero(z)))) - { - swingAxis = -ivB.cross(ivA); - } - } - btVector3 vTwist(1,0,0); - - // compute rotation of A wrt B (in constraint space) - btQuaternion qA = transA.getRotation() * m_frameInA.getRotation(); - btQuaternion qB = transB.getRotation() * m_frameInB.getRotation(); - btQuaternion qAB = qB.inverse() * qA; - // split rotation into cone and twist - // (all this is done from B's perspective. Maybe I should be averaging axes...) - btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize(); - btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize(); - btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize(); - - int row = 3; - int srow = row * info->rowskip; - btVector3 ax1; - // angular limits - { - btScalar *J1 = info->m_J1angularAxis; - btScalar *J2 = info->m_J2angularAxis; - btTransform trA = transA*m_frameInA; - btVector3 twistAxis = trA.getBasis().getColumn(0); - - btVector3 p = trA.getBasis().getColumn(1); - btVector3 q = trA.getBasis().getColumn(2); - int srow1 = srow + info->rowskip; - J1[srow+0] = p[0]; - J1[srow+1] = p[1]; - J1[srow+2] = p[2]; - J1[srow1+0] = q[0]; - J1[srow1+1] = q[1]; - J1[srow1+2] = q[2]; - J2[srow+0] = -p[0]; - J2[srow+1] = -p[1]; - J2[srow+2] = -p[2]; - J2[srow1+0] = -q[0]; - J2[srow1+1] = -q[1]; - J2[srow1+2] = -q[2]; - btScalar fact = info->fps; - info->m_constraintError[srow] = fact * swingAxis.dot(p); - info->m_constraintError[srow1] = fact * swingAxis.dot(q); - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - info->m_lowerLimit[srow1] = -SIMD_INFINITY; - info->m_upperLimit[srow1] = SIMD_INFINITY; - srow = srow1 + info->rowskip; - - { - btQuaternion qMinTwist = qABTwist; - btScalar twistAngle = qABTwist.getAngle(); - - if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate. - { - qMinTwist = -(qABTwist); - twistAngle = qMinTwist.getAngle(); - } - - if (twistAngle > SIMD_EPSILON) - { - twistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z()); - twistAxis.normalize(); - twistAxis = quatRotate(qB, -twistAxis); - } - ax1 = twistAxis; - btScalar *J1 = info->m_J1angularAxis; - btScalar *J2 = info->m_J2angularAxis; - J1[srow+0] = ax1[0]; - J1[srow+1] = ax1[1]; - J1[srow+2] = ax1[2]; - J2[srow+0] = -ax1[0]; - J2[srow+1] = -ax1[1]; - J2[srow+2] = -ax1[2]; - btScalar k = info->fps; - info->m_constraintError[srow] = k * twistAngle; - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - } +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "btFixedConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btTransformUtil.h" +#include + + +btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB) +:btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB) +{ + m_frameInA = frameInA; + m_frameInB = frameInB; + +} + +btFixedConstraint::~btFixedConstraint () +{ +} + + +void btFixedConstraint::getInfo1 (btConstraintInfo1* info) +{ + info->m_numConstraintRows = 6; + info->nub = 0; +} + +void btFixedConstraint::getInfo2 (btConstraintInfo2* info) +{ + //fix the 3 linear degrees of freedom + + const btTransform& transA = m_rbA.getCenterOfMassTransform(); + const btTransform& transB = m_rbB.getCenterOfMassTransform(); + + const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin(); + const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis(); + const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin(); + const btMatrix3x3& worldOrnB = m_rbB.getCenterOfMassTransform().getBasis(); + + + info->m_J1linearAxis[0] = 1; + info->m_J1linearAxis[info->rowskip+1] = 1; + info->m_J1linearAxis[2*info->rowskip+2] = 1; + + btVector3 a1 = worldOrnA * m_frameInA.getOrigin(); + { + btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); + btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); + btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); + btVector3 a1neg = -a1; + a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + + if (info->m_J2linearAxis) + { + info->m_J2linearAxis[0] = -1; + info->m_J2linearAxis[info->rowskip+1] = -1; + info->m_J2linearAxis[2*info->rowskip+2] = -1; + } + + btVector3 a2 = worldOrnB*m_frameInB.getOrigin(); + { + btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); + btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); + btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); + a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + + // set right hand side for the linear dofs + btScalar k = info->fps * info->erp; + + btVector3 linearError = k*(a2+worldPosB-a1-worldPosA); + int j; + for (j=0; j<3; j++) + { + info->m_constraintError[j*info->rowskip] = linearError[j]; + //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); + } + + btVector3 ivA = transA.getBasis() * m_frameInA.getBasis().getColumn(0); + btVector3 jvA = transA.getBasis() * m_frameInA.getBasis().getColumn(1); + btVector3 kvA = transA.getBasis() * m_frameInA.getBasis().getColumn(2); + btVector3 ivB = transB.getBasis() * m_frameInB.getBasis().getColumn(0); + btVector3 target; + //btScalar x = ivB.dot(ivA);//?? + btScalar y = ivB.dot(jvA); + btScalar z = ivB.dot(kvA); + btVector3 swingAxis(0,0,0); + { + if((!btFuzzyZero(y)) || (!(btFuzzyZero(z)))) + { + swingAxis = -ivB.cross(ivA); + } + } + btVector3 vTwist(1,0,0); + + // compute rotation of A wrt B (in constraint space) + btQuaternion qA = transA.getRotation() * m_frameInA.getRotation(); + btQuaternion qB = transB.getRotation() * m_frameInB.getRotation(); + btQuaternion qAB = qB.inverse() * qA; + // split rotation into cone and twist + // (all this is done from B's perspective. Maybe I should be averaging axes...) + btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize(); + btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize(); + btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize(); + + int row = 3; + int srow = row * info->rowskip; + btVector3 ax1; + // angular limits + { + btScalar *J1 = info->m_J1angularAxis; + btScalar *J2 = info->m_J2angularAxis; + btTransform trA = transA*m_frameInA; + btVector3 twistAxis = trA.getBasis().getColumn(0); + + btVector3 p = trA.getBasis().getColumn(1); + btVector3 q = trA.getBasis().getColumn(2); + int srow1 = srow + info->rowskip; + J1[srow+0] = p[0]; + J1[srow+1] = p[1]; + J1[srow+2] = p[2]; + J1[srow1+0] = q[0]; + J1[srow1+1] = q[1]; + J1[srow1+2] = q[2]; + J2[srow+0] = -p[0]; + J2[srow+1] = -p[1]; + J2[srow+2] = -p[2]; + J2[srow1+0] = -q[0]; + J2[srow1+1] = -q[1]; + J2[srow1+2] = -q[2]; + btScalar fact = info->fps; + info->m_constraintError[srow] = fact * swingAxis.dot(p); + info->m_constraintError[srow1] = fact * swingAxis.dot(q); + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + info->m_lowerLimit[srow1] = -SIMD_INFINITY; + info->m_upperLimit[srow1] = SIMD_INFINITY; + srow = srow1 + info->rowskip; + + { + btQuaternion qMinTwist = qABTwist; + btScalar twistAngle = qABTwist.getAngle(); + + if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate. + { + qMinTwist = -(qABTwist); + twistAngle = qMinTwist.getAngle(); + } + + if (twistAngle > SIMD_EPSILON) + { + twistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z()); + twistAxis.normalize(); + twistAxis = quatRotate(qB, -twistAxis); + } + ax1 = twistAxis; + btScalar *J1 = info->m_J1angularAxis; + btScalar *J2 = info->m_J2angularAxis; + J1[srow+0] = ax1[0]; + J1[srow+1] = ax1[1]; + J1[srow+2] = ax1[2]; + J2[srow+0] = -ax1[0]; + J2[srow+1] = -ax1[1]; + J2[srow+2] = -ax1[2]; + btScalar k = info->fps; + info->m_constraintError[srow] = k * twistAngle; + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + } } \ No newline at end of file diff --git a/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h b/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h index 386cf266b..e1c9430fd 100644 --- a/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h @@ -1,48 +1,48 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_FIXED_CONSTRAINT_H -#define BT_FIXED_CONSTRAINT_H - -#include "btTypedConstraint.h" - -ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint -{ - - btTransform m_frameInA; - btTransform m_frameInB; -public: - btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB); - - virtual ~btFixedConstraint(); - - - virtual void getInfo1 (btConstraintInfo1* info); - - virtual void getInfo2 (btConstraintInfo2* info); - - virtual void setParam(int num, btScalar value, int axis = -1) - { - btAssert(0); - } - virtual btScalar getParam(int num, int axis = -1) const - { - btAssert(0); - return 0.f; - } - -}; - -#endif //BT_FIXED_CONSTRAINT_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_FIXED_CONSTRAINT_H +#define BT_FIXED_CONSTRAINT_H + +#include "btTypedConstraint.h" + +ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint +{ + + btTransform m_frameInA; + btTransform m_frameInB; +public: + btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB); + + virtual ~btFixedConstraint(); + + + virtual void getInfo1 (btConstraintInfo1* info); + + virtual void getInfo2 (btConstraintInfo2* info); + + virtual void setParam(int num, btScalar value, int axis = -1) + { + btAssert(0); + } + virtual btScalar getParam(int num, int axis = -1) const + { + btAssert(0); + return 0.f; + } + +}; + +#endif //BT_FIXED_CONSTRAINT_H diff --git a/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp index 446e69de7..bcd457b67 100644 --- a/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp @@ -1,54 +1,54 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2012 Advanced Micro Devices, Inc. http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/// Implemented by Erwin Coumans. The idea for the constraint comes from Dimitris Papavasiliou. - -#include "btGearConstraint.h" - -btGearConstraint::btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio) -:btTypedConstraint(GEAR_CONSTRAINT_TYPE,rbA,rbB), -m_axisInA(axisInA), -m_axisInB(axisInB), -m_ratio(ratio) -{ -} - -btGearConstraint::~btGearConstraint () -{ -} - -void btGearConstraint::getInfo1 (btConstraintInfo1* info) -{ - info->m_numConstraintRows = 1; - info->nub = 1; -} - -void btGearConstraint::getInfo2 (btConstraintInfo2* info) -{ - btVector3 globalAxisA, globalAxisB; - - globalAxisA = m_rbA.getWorldTransform().getBasis()*this->m_axisInA; - globalAxisB = m_rbB.getWorldTransform().getBasis()*this->m_axisInB; - - info->m_J1angularAxis[0] = globalAxisA[0]; - info->m_J1angularAxis[1] = globalAxisA[1]; - info->m_J1angularAxis[2] = globalAxisA[2]; - - info->m_J2angularAxis[0] = m_ratio*globalAxisB[0]; - info->m_J2angularAxis[1] = m_ratio*globalAxisB[1]; - info->m_J2angularAxis[2] = m_ratio*globalAxisB[2]; - -} - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2012 Advanced Micro Devices, Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/// Implemented by Erwin Coumans. The idea for the constraint comes from Dimitris Papavasiliou. + +#include "btGearConstraint.h" + +btGearConstraint::btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio) +:btTypedConstraint(GEAR_CONSTRAINT_TYPE,rbA,rbB), +m_axisInA(axisInA), +m_axisInB(axisInB), +m_ratio(ratio) +{ +} + +btGearConstraint::~btGearConstraint () +{ +} + +void btGearConstraint::getInfo1 (btConstraintInfo1* info) +{ + info->m_numConstraintRows = 1; + info->nub = 1; +} + +void btGearConstraint::getInfo2 (btConstraintInfo2* info) +{ + btVector3 globalAxisA, globalAxisB; + + globalAxisA = m_rbA.getWorldTransform().getBasis()*this->m_axisInA; + globalAxisB = m_rbB.getWorldTransform().getBasis()*this->m_axisInB; + + info->m_J1angularAxis[0] = globalAxisA[0]; + info->m_J1angularAxis[1] = globalAxisA[1]; + info->m_J1angularAxis[2] = globalAxisA[2]; + + info->m_J2angularAxis[0] = m_ratio*globalAxisB[0]; + info->m_J2angularAxis[1] = m_ratio*globalAxisB[1]; + info->m_J2angularAxis[2] = m_ratio*globalAxisB[2]; + +} + diff --git a/src/BulletDynamics/ConstraintSolver/btGearConstraint.h b/src/BulletDynamics/ConstraintSolver/btGearConstraint.h index e3c5dff35..f9afcb912 100644 --- a/src/BulletDynamics/ConstraintSolver/btGearConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btGearConstraint.h @@ -1,79 +1,79 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2012 Advanced Micro Devices, Inc. http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#ifndef BT_GEAR_CONSTRAINT_H -#define BT_GEAR_CONSTRAINT_H - -#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" - - -#ifdef BT_USE_DOUBLE_PRECISION -#define btGearConstraintData btGearConstraintDoubleData -#define btGearConstraintDataName "btGearConstraintDoubleData" -#else -#define btGearConstraintData btGearConstraintFloatData -#define btGearConstraintDataName "btGearConstraintFloatData" -#endif //BT_USE_DOUBLE_PRECISION - - - -///The btGeatConstraint will couple the angular velocity for two bodies around given local axis and ratio. -///See Bullet/Demos/ConstraintDemo for an example use. -class btGearConstraint : public btTypedConstraint -{ -protected: - btVector3 m_axisInA; - btVector3 m_axisInB; - bool m_useFrameA; - btScalar m_ratio; - -public: - btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio=1.f); - virtual ~btGearConstraint (); - - ///internal method used by the constraint solver, don't use them directly - virtual void getInfo1 (btConstraintInfo1* info); - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2012 Advanced Micro Devices, Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#ifndef BT_GEAR_CONSTRAINT_H +#define BT_GEAR_CONSTRAINT_H + +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" + + +#ifdef BT_USE_DOUBLE_PRECISION +#define btGearConstraintData btGearConstraintDoubleData +#define btGearConstraintDataName "btGearConstraintDoubleData" +#else +#define btGearConstraintData btGearConstraintFloatData +#define btGearConstraintDataName "btGearConstraintFloatData" +#endif //BT_USE_DOUBLE_PRECISION + + + +///The btGeatConstraint will couple the angular velocity for two bodies around given local axis and ratio. +///See Bullet/Demos/ConstraintDemo for an example use. +class btGearConstraint : public btTypedConstraint +{ +protected: + btVector3 m_axisInA; + btVector3 m_axisInB; + bool m_useFrameA; + btScalar m_ratio; + +public: + btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio=1.f); + virtual ~btGearConstraint (); + + ///internal method used by the constraint solver, don't use them directly + virtual void getInfo1 (btConstraintInfo1* info); + ///internal method used by the constraint solver, don't use them directly virtual void getInfo2 (btConstraintInfo2* info); - void setAxisA(btVector3& axisA) - { - m_axisInA = axisA; + void setAxisA(btVector3& axisA) + { + m_axisInA = axisA; } - void setAxisB(btVector3& axisB) - { - m_axisInB = axisB; + void setAxisB(btVector3& axisB) + { + m_axisInB = axisB; } - void setRatio(btScalar ratio) - { - m_ratio = ratio; + void setRatio(btScalar ratio) + { + m_ratio = ratio; } - const btVector3& getAxisA() const - { - return m_axisInA; + const btVector3& getAxisA() const + { + return m_axisInA; } - const btVector3& getAxisB() const - { - return m_axisInB; + const btVector3& getAxisB() const + { + return m_axisInB; } - btScalar getRatio() const - { - return m_ratio; + btScalar getRatio() const + { + return m_ratio; } @@ -84,69 +84,69 @@ public: (void) axis; btAssert(0); } - - ///return the local value of parameter - virtual btScalar getParam(int num, int axis = -1) const - { - (void) num; - (void) axis; - btAssert(0); - return 0.f; - } - - virtual int calculateSerializeBufferSize() const; - - ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; -}; - - - - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btGearConstraintFloatData -{ - btTypedConstraintFloatData m_typeConstraintData; - - btVector3FloatData m_axisInA; - btVector3FloatData m_axisInB; - - float m_ratio; - char m_padding[4]; -}; - -struct btGearConstraintDoubleData -{ - btTypedConstraintDoubleData m_typeConstraintData; - - btVector3DoubleData m_axisInA; - btVector3DoubleData m_axisInB; - - double m_ratio; -}; - -SIMD_FORCE_INLINE int btGearConstraint::calculateSerializeBufferSize() const -{ - return sizeof(btGearConstraintData); -} - - ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btGearConstraint::serialize(void* dataBuffer, btSerializer* serializer) const -{ - btGearConstraintData* gear = (btGearConstraintData*)dataBuffer; - btTypedConstraint::serialize(&gear->m_typeConstraintData,serializer); - - m_axisInA.serialize( gear->m_axisInA ); - m_axisInB.serialize( gear->m_axisInB ); - - gear->m_ratio = m_ratio; - - return btGearConstraintDataName; -} - - - - - - -#endif //BT_GEAR_CONSTRAINT_H + + ///return the local value of parameter + virtual btScalar getParam(int num, int axis = -1) const + { + (void) num; + (void) axis; + btAssert(0); + return 0.f; + } + + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; +}; + + + + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btGearConstraintFloatData +{ + btTypedConstraintFloatData m_typeConstraintData; + + btVector3FloatData m_axisInA; + btVector3FloatData m_axisInB; + + float m_ratio; + char m_padding[4]; +}; + +struct btGearConstraintDoubleData +{ + btTypedConstraintDoubleData m_typeConstraintData; + + btVector3DoubleData m_axisInA; + btVector3DoubleData m_axisInB; + + double m_ratio; +}; + +SIMD_FORCE_INLINE int btGearConstraint::calculateSerializeBufferSize() const +{ + return sizeof(btGearConstraintData); +} + + ///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btGearConstraint::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btGearConstraintData* gear = (btGearConstraintData*)dataBuffer; + btTypedConstraint::serialize(&gear->m_typeConstraintData,serializer); + + m_axisInA.serialize( gear->m_axisInA ); + m_axisInB.serialize( gear->m_axisInB ); + + gear->m_ratio = m_ratio; + + return btGearConstraintDataName; +} + + + + + + +#endif //BT_GEAR_CONSTRAINT_H diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp index 696aafb72..ee57ca72b 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp +++ b/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(); diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 30d506c6e..e2aaee7ac 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/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_ diff --git a/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp index 11fbadef7..f110cd480 100644 --- a/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp @@ -1,463 +1,463 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btNNCGConstraintSolver.h" - - - - - - -btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) -{ - btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer); - - m_pNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size()); - m_pC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size()); - m_pCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size()); - m_pCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size()); - - m_deltafNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size()); - m_deltafC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size()); - m_deltafCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size()); - m_deltafCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size()); - - return val; -} - -btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/) -{ - - int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); - int numConstraintPool = m_tmpSolverContactConstraintPool.size(); - int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); - - if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) - { - if (1) // uncomment this for a bit less random ((iteration & 7) == 0) - { - - for (int j=0; j0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2; - if (beta>1) - { - for (int j=0;jisEnabled()) - { - int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep); - int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); - btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; - btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; - constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); - } - } - - ///solve all contact constraints using SIMD, if available - if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) - { - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1; - - for (int c=0;cbtScalar(0)) - { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - m_deltafCF[c*multiplier] = deltaf; - deltaflengthsqr += deltaf*deltaf; - } else { - m_deltafCF[c*multiplier] = 0; - } - } - - if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) - { - - btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]]; - - if (totalImpulse>btScalar(0)) - { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - m_deltafCF[c*multiplier+1] = deltaf; - deltaflengthsqr += deltaf*deltaf; - } else { - m_deltafCF[c*multiplier+1] = 0; - } - } - } - } - - } - else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS - { - //solve the friction constraints after all contact constraints, don't interleave them - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int j; - - for (j=0;jbtScalar(0)) - { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - - //resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - m_deltafCF[j] = deltaf; - deltaflengthsqr += deltaf*deltaf; - } else { - m_deltafCF[j] = 0; - } - } - - - int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); - for (j=0;jbtScalar(0)) - { - btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; - if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) - rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; - - rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; - rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; - - btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); - m_deltafCRF[j] = deltaf; - deltaflengthsqr += deltaf*deltaf; - } else { - m_deltafCRF[j] = 0; - } - } - - - } - } - - - - } else - { - - if (iteration< infoGlobal.m_numIterations) - { - for (int j=0;jisEnabled()) - { - int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep); - int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); - btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; - btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; - constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); - } - } - ///solve all contact constraints - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - for (int j=0;jbtScalar(0)) - { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - - btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - m_deltafCF[j] = deltaf; - deltaflengthsqr += deltaf*deltaf; - } else { - m_deltafCF[j] = 0; - } - } - - int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); - for (int j=0;jbtScalar(0)) - { - btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; - if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) - rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; - - rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; - rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; - - btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); - m_deltafCRF[j] = deltaf; - deltaflengthsqr += deltaf*deltaf; - } else { - m_deltafCRF[j] = 0; - } - } - } - } - - - - - if (!m_onlyForNoneContact) - { - if (iteration==0) - { - for (int j=0;j0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2; - if (beta>1) { - for (int j=0;j0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2; + if (beta>1) + { + for (int j=0;jisEnabled()) + { + int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep); + int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); + btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; + btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; + constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); + } + } + + ///solve all contact constraints using SIMD, if available + if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) + { + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1; + + for (int c=0;cbtScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + m_deltafCF[c*multiplier] = deltaf; + deltaflengthsqr += deltaf*deltaf; + } else { + m_deltafCF[c*multiplier] = 0; + } + } + + if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) + { + + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]]; + + if (totalImpulse>btScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + m_deltafCF[c*multiplier+1] = deltaf; + deltaflengthsqr += deltaf*deltaf; + } else { + m_deltafCF[c*multiplier+1] = 0; + } + } + } + } + + } + else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS + { + //solve the friction constraints after all contact constraints, don't interleave them + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + int j; + + for (j=0;jbtScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + + //resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + m_deltafCF[j] = deltaf; + deltaflengthsqr += deltaf*deltaf; + } else { + m_deltafCF[j] = 0; + } + } + + + int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); + for (j=0;jbtScalar(0)) + { + btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; + if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) + rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; + + rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; + rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; + + btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); + m_deltafCRF[j] = deltaf; + deltaflengthsqr += deltaf*deltaf; + } else { + m_deltafCRF[j] = 0; + } + } + + + } + } + + + + } else + { + + if (iteration< infoGlobal.m_numIterations) + { + for (int j=0;jisEnabled()) + { + int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep); + int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); + btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; + btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; + constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); + } + } + ///solve all contact constraints + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + for (int j=0;jbtScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + + btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + m_deltafCF[j] = deltaf; + deltaflengthsqr += deltaf*deltaf; + } else { + m_deltafCF[j] = 0; + } + } + + int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); + for (int j=0;jbtScalar(0)) + { + btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; + if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) + rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; + + rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; + rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; + + btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); + m_deltafCRF[j] = deltaf; + deltaflengthsqr += deltaf*deltaf; + } else { + m_deltafCRF[j] = 0; + } + } + } + } + + + + + if (!m_onlyForNoneContact) + { + if (iteration==0) + { + for (int j=0;j0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2; + if (beta>1) { + for (int j=0;j m_pNC; // p for None Contact constraints - btAlignedObjectArray m_pC; // p for Contact constraints - btAlignedObjectArray m_pCF; // p for ContactFriction constraints - btAlignedObjectArray m_pCRF; // p for ContactRollingFriction constraints - - //These are recalculated in every iterations. We just keep these to prevent reallocation in each iteration. - btAlignedObjectArray m_deltafNC; // deltaf for NoneContact constraints - btAlignedObjectArray m_deltafC; // deltaf for Contact constraints - btAlignedObjectArray m_deltafCF; // deltaf for ContactFriction constraints - btAlignedObjectArray m_deltafCRF; // deltaf for ContactRollingFriction constraints - - -protected: - - virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal); - virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - - virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btNNCGConstraintSolver() : btSequentialImpulseConstraintSolver(), m_onlyForNoneContact(false) {} - - virtual btConstraintSolverType getSolverType() const - { - return BT_NNCG_SOLVER; - } - - bool m_onlyForNoneContact; -}; - - - - -#endif //BT_NNCG_CONSTRAINT_SOLVER_H - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_NNCG_CONSTRAINT_SOLVER_H +#define BT_NNCG_CONSTRAINT_SOLVER_H + +#include "btSequentialImpulseConstraintSolver.h" + +ATTRIBUTE_ALIGNED16(class) btNNCGConstraintSolver : public btSequentialImpulseConstraintSolver +{ +protected: + + btScalar m_deltafLengthSqrPrev; + + btAlignedObjectArray m_pNC; // p for None Contact constraints + btAlignedObjectArray m_pC; // p for Contact constraints + btAlignedObjectArray m_pCF; // p for ContactFriction constraints + btAlignedObjectArray m_pCRF; // p for ContactRollingFriction constraints + + //These are recalculated in every iterations. We just keep these to prevent reallocation in each iteration. + btAlignedObjectArray m_deltafNC; // deltaf for NoneContact constraints + btAlignedObjectArray m_deltafC; // deltaf for Contact constraints + btAlignedObjectArray m_deltafCF; // deltaf for ContactFriction constraints + btAlignedObjectArray m_deltafCRF; // deltaf for ContactRollingFriction constraints + + +protected: + + virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal); + virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + + virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btNNCGConstraintSolver() : btSequentialImpulseConstraintSolver(), m_onlyForNoneContact(false) {} + + virtual btConstraintSolverType getSolverType() const + { + return BT_NNCG_SOLVER; + } + + bool m_onlyForNoneContact; +}; + + + + +#endif //BT_NNCG_CONSTRAINT_SOLVER_H + diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 8385639a1..fb06b76c3 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -364,9 +364,9 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() - :m_btSeed2(0), - m_resolveSingleConstraintRowGeneric(gResolveSingleConstraintRowGeneric_scalar_reference), - m_resolveSingleConstraintRowLowerLimit(gResolveSingleConstraintRowLowerLimit_scalar_reference) + : m_resolveSingleConstraintRowGeneric(gResolveSingleConstraintRowGeneric_scalar_reference), + m_resolveSingleConstraintRowLowerLimit(gResolveSingleConstraintRowLowerLimit_scalar_reference), + m_btSeed2(0) { #ifdef USE_SIMD @@ -761,8 +761,8 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra const btVector3& rel_pos1, const btVector3& rel_pos2) { - const btVector3& pos1 = cp.getPositionWorldOnA(); - const btVector3& pos2 = cp.getPositionWorldOnB(); + // const btVector3& pos1 = cp.getPositionWorldOnA(); + // const btVector3& pos2 = cp.getPositionWorldOnB(); btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index a6a51662d..409b64e96 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -4,8 +4,8 @@ Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. @@ -58,7 +58,7 @@ int firstHit=startHit; SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs) { int islandId; - + const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag(); @@ -88,7 +88,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal int m_numConstraints; btIDebugDraw* m_debugDrawer; btDispatcher* m_dispatcher; - + btAlignedObjectArray m_bodies; btAlignedObjectArray m_manifolds; btAlignedObjectArray 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;isolveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); } else { - + for (i=0;isolveGroup( 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;iupdateAction( 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;isetup(&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;im_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;pgetNumContacts();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 diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index fc12bfd6b..704099555 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -87,7 +87,10 @@ btMultiBody::btMultiBody(int n_links, bool fixedBase, bool canSleep, bool multiDof) - : m_baseQuat(0, 0, 0, 1), + : + m_baseCollider(0), + m_basePos(0,0,0), + m_baseQuat(0, 0, 0, 1), m_baseMass(mass), m_baseInertia(inertia), @@ -95,18 +98,19 @@ btMultiBody::btMultiBody(int n_links, m_awake(true), m_canSleep(canSleep), m_sleepTimer(0), - m_baseCollider(0), + m_linearDamping(0.04f), m_angularDamping(0.04f), m_useGyroTerm(true), - m_maxAppliedImpulse(1000.f), + m_maxAppliedImpulse(1000.f), m_maxCoordinateVelocity(100.f), - m_hasSelfCollision(true), - m_dofCount(0), - __posUpdated(false), + m_hasSelfCollision(true), m_isMultiDof(multiDof), + __posUpdated(false), + m_dofCount(0), m_posVarCnt(0), - m_useRK4(false), m_useGlobalVelocities(false) + m_useRK4(false), + m_useGlobalVelocities(false) { if(!m_isMultiDof) @@ -119,7 +123,7 @@ btMultiBody::btMultiBody(int n_links, m_links.resize(n_links); m_matrixBuf.resize(n_links + 1); - m_basePos.setValue(0, 0, 0); + m_baseForce.setValue(0, 0, 0); m_baseTorque.setValue(0, 0, 0); } @@ -1257,7 +1261,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_baseInertia * spatVel[i+1].getAngular())); // zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear())); - btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()); + //btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()); ////clamp parent's omega //btScalar parOmegaMod = temp.length(); //btScalar parOmegaModMax = 1000; @@ -2010,7 +2014,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar { for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) { - btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1]); +//?? btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1]); Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof] - SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1]) @@ -2352,7 +2356,7 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output // 'Downward' loop. for (int i = num_links - 1; i >= 0; --i) { - btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]); +// btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]); Y[i] = - SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]); Y[i] += force[6 + i]; // add joint torque @@ -2437,7 +2441,7 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output ///////////////// */ - int dummy = 0; + //int dummy = 0; } void btMultiBody::stepPositions(btScalar dt) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 85d6a2f3f..cd029c661 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/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 m_links; // array of m_links, excluding the base. index from 0 to num_links-1. btAlignedObjectArray 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 diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index 1bb518f94..3c71a08b9 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -1,364 +1,366 @@ -#include "btMultiBodyConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro) - -btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral) - :m_bodyA(bodyA), - m_bodyB(bodyB), - m_linkA(linkA), - m_linkB(linkB), - m_numRows(numRows), - m_isUnilateral(isUnilateral), - m_maxAppliedImpulse(100), - m_jacSizeA(0), - m_jacSizeBoth(0) -{ - - if(bodyA) - { - if(bodyA->isMultiDof()) - m_jacSizeA = (6 + bodyA->getNumDofs()); - else - m_jacSizeA = (6 + bodyA->getNumLinks()); - } - - if(bodyB) - { - if(bodyB->isMultiDof()) - m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumDofs(); - else - m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumLinks(); - } - else - m_jacSizeBoth = m_jacSizeA; - - m_posOffset = ((1 + m_jacSizeBoth)*m_numRows); - m_data.resize((2 + m_jacSizeBoth) * m_numRows); -} - -btMultiBodyConstraint::~btMultiBodyConstraint() -{ -} - -void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) -{ - for (int i = 0; i < ndof; ++i) - data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; -} - -btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint, - btMultiBodyJacobianData& data, - btScalar* jacOrgA, btScalar* jacOrgB, - const btVector3& contactNormalOnB, - const btVector3& posAworld, const btVector3& posBworld, - btScalar posError, - const btContactSolverInfo& infoGlobal, - btScalar lowerLimit, btScalar upperLimit, - btScalar relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) -{ - solverConstraint.m_multiBodyA = m_bodyA; - solverConstraint.m_multiBodyB = m_bodyB; - solverConstraint.m_linkA = m_linkA; - solverConstraint.m_linkB = m_linkB; - - btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; - btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; - - btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA); - btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB); - - btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; - btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; - - btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary - if (bodyA) - rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin(); - if (bodyB) - rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin(); - - if (multiBodyA) - { - const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; - - solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); - - if (solverConstraint.m_deltaVelAindex <0) - { - solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size(); - multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); - data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); - } else - { - btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); - } - - //determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom - //resize.. - solverConstraint.m_jacAindex = data.m_jacobians.size(); - data.m_jacobians.resize(data.m_jacobians.size()+ndofA); - //copy/determine - if(jacOrgA) - { - for (int i=0;iisMultiDof()) - multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); - else - multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); - } - - //determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint) - //resize.. - data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse - btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); - btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - //determine.. - if(multiBodyA->isMultiDof()) - multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); - else - multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); - } - else if(rb0) - { - btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB); - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); - solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = contactNormalOnB; - } - - if (multiBodyB) - { - const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; - - solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); - if (solverConstraint.m_deltaVelBindex <0) - { - solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size(); - multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); - data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); - } - - //determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom - //resize.. - solverConstraint.m_jacBindex = data.m_jacobians.size(); - data.m_jacobians.resize(data.m_jacobians.size()+ndofB); - //copy/determine.. - if(jacOrgB) - { - for (int i=0;iisMultiDof()) - multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); - else - multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); - } - - //determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint) - //resize.. - data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB); - btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); - btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - //determine.. - if(multiBodyB->isMultiDof()) - multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v); - else - multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v); - - } - else if(rb1) - { - btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); - solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - solverConstraint.m_contactNormal2 = -contactNormalOnB; - } - { - - btVector3 vec; - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - btScalar* jacB = 0; - btScalar* jacA = 0; - btScalar* deltaVelA = 0; - btScalar* deltaVelB = 0; - int ndofA = 0; - //determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i]) - if (multiBodyA) - { - ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; - jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; - deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA; ++i) - { - btScalar j = jacA[i] ; - btScalar l = deltaVelA[i]; - denom0 += j*l; - } - } - else if(rb0) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec); - } - // - if (multiBodyB) - { - const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; - jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; - deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB; ++i) - { - btScalar j = jacB[i] ; - btScalar l = deltaVelB[i]; - denom1 += j*l; - } - - } - else if(rb1) - { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec); - } - //determine the "effective mass" of the constrained multibodyB with respect to this 1D constraint (i.e. 1/A[i,i]) - if (multiBodyA && (multiBodyA==multiBodyB)) - { - // ndof1 == ndof2 in this case - for (int i = 0; i < ndofA; ++i) - { - denom1 += jacB[i] * deltaVelA[i]; - denom1 += jacA[i] * deltaVelB[i]; - } - } - // - btScalar d = denom0+denom1; - if (btFabs(d)>SIMD_EPSILON) - { - - solverConstraint.m_jacDiagABInv = relaxation/(d); - } - else - { - solverConstraint.m_jacDiagABInv = 1.f; - } - } - - - //compute rhs and remaining solverConstraint fields - btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop; - - btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; - { - btVector3 vel1,vel2; - if (multiBodyA) - { - ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; - btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA ; ++i) - rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; - } - else if(rb0) - { - rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); - } - if (multiBodyB) - { - ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; - btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB ; ++i) - rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; - - } - else if(rb1) - { - rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); - } - - solverConstraint.m_friction = 0.f;//cp.m_combinedFriction; - } - - - ///warm starting (or zero if disabled) - /* - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - - if (solverConstraint.m_appliedImpulse) - { - if (multiBodyA) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->applyDeltaVee(deltaV,impulse); - applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); - } else - { - if (rb0) - bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); - } - if (multiBodyB) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - multiBodyB->applyDeltaVee(deltaV,impulse); - applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); - } else - { - if (rb1) - bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); - } - } - } else - */ - - solverConstraint.m_appliedImpulse = 0.f; - solverConstraint.m_appliedPushImpulse = 0.f; - - { - - btScalar positionalError = 0.f; - btScalar velocityError = desiredVelocity - rel_vel;// * damping; - - - btScalar erp = infoGlobal.m_erp2; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - erp = infoGlobal.m_erp; - } - - positionalError = -penetration * erp/infoGlobal.m_timeStep; - - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; - solverConstraint.m_rhsPenetration = 0.f; - - } else - { - //split position and velocity into rhs and m_rhsPenetration - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = penetrationImpulse; - } - - solverConstraint.m_cfm = 0.f; - solverConstraint.m_lowerLimit = lowerLimit; - solverConstraint.m_upperLimit = upperLimit; - } - - return rel_vel; - -} +#include "btMultiBodyConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro) + + + +btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral) + :m_bodyA(bodyA), + m_bodyB(bodyB), + m_linkA(linkA), + m_linkB(linkB), + m_numRows(numRows), + m_jacSizeA(0), + m_jacSizeBoth(0), + m_isUnilateral(isUnilateral), + m_maxAppliedImpulse(100) +{ + + if(bodyA) + { + if(bodyA->isMultiDof()) + m_jacSizeA = (6 + bodyA->getNumDofs()); + else + m_jacSizeA = (6 + bodyA->getNumLinks()); + } + + if(bodyB) + { + if(bodyB->isMultiDof()) + m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumDofs(); + else + m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumLinks(); + } + else + m_jacSizeBoth = m_jacSizeA; + + m_posOffset = ((1 + m_jacSizeBoth)*m_numRows); + m_data.resize((2 + m_jacSizeBoth) * m_numRows); +} + +btMultiBodyConstraint::~btMultiBodyConstraint() +{ +} + +void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) +{ + for (int i = 0; i < ndof; ++i) + data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; +} + +btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint, + btMultiBodyJacobianData& data, + btScalar* jacOrgA, btScalar* jacOrgB, + const btVector3& contactNormalOnB, + const btVector3& posAworld, const btVector3& posBworld, + btScalar posError, + const btContactSolverInfo& infoGlobal, + btScalar lowerLimit, btScalar upperLimit, + btScalar relaxation, + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) +{ + solverConstraint.m_multiBodyA = m_bodyA; + solverConstraint.m_multiBodyB = m_bodyB; + solverConstraint.m_linkA = m_linkA; + solverConstraint.m_linkB = m_linkB; + + btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; + btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; + + btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA); + btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB); + + btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; + btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; + + btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary + if (bodyA) + rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin(); + if (bodyB) + rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin(); + + if (multiBodyA) + { + const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; + + solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); + + if (solverConstraint.m_deltaVelAindex <0) + { + solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size(); + multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); + } else + { + btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); + } + + //determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom + //resize.. + solverConstraint.m_jacAindex = data.m_jacobians.size(); + data.m_jacobians.resize(data.m_jacobians.size()+ndofA); + //copy/determine + if(jacOrgA) + { + for (int i=0;iisMultiDof()) + multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); + else + multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); + } + + //determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint) + //resize.. + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse + btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); + btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + //determine.. + if(multiBodyA->isMultiDof()) + multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); + else + multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); + } + else if(rb0) + { + btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB); + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = contactNormalOnB; + } + + if (multiBodyB) + { + const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; + + solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); + if (solverConstraint.m_deltaVelBindex <0) + { + solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size(); + multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); + } + + //determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom + //resize.. + solverConstraint.m_jacBindex = data.m_jacobians.size(); + data.m_jacobians.resize(data.m_jacobians.size()+ndofB); + //copy/determine.. + if(jacOrgB) + { + for (int i=0;iisMultiDof()) + multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); + else + multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); + } + + //determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint) + //resize.. + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB); + btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); + btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + //determine.. + if(multiBodyB->isMultiDof()) + multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v); + else + multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v); + + } + else if(rb1) + { + btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + solverConstraint.m_contactNormal2 = -contactNormalOnB; + } + { + + btVector3 vec; + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + btScalar* jacB = 0; + btScalar* jacA = 0; + btScalar* deltaVelA = 0; + btScalar* deltaVelB = 0; + int ndofA = 0; + //determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i]) + if (multiBodyA) + { + ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; + jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; + deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) + { + btScalar j = jacA[i] ; + btScalar l = deltaVelA[i]; + denom0 += j*l; + } + } + else if(rb0) + { + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec); + } + // + if (multiBodyB) + { + const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; + jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; + deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + { + btScalar j = jacB[i] ; + btScalar l = deltaVelB[i]; + denom1 += j*l; + } + + } + else if(rb1) + { + vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec); + } + //determine the "effective mass" of the constrained multibodyB with respect to this 1D constraint (i.e. 1/A[i,i]) + if (multiBodyA && (multiBodyA==multiBodyB)) + { + // ndof1 == ndof2 in this case + for (int i = 0; i < ndofA; ++i) + { + denom1 += jacB[i] * deltaVelA[i]; + denom1 += jacA[i] * deltaVelB[i]; + } + } + // + btScalar d = denom0+denom1; + if (btFabs(d)>SIMD_EPSILON) + { + + solverConstraint.m_jacDiagABInv = relaxation/(d); + } + else + { + solverConstraint.m_jacDiagABInv = 1.f; + } + } + + + //compute rhs and remaining solverConstraint fields + btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop; + + btScalar rel_vel = 0.f; + int ndofA = 0; + int ndofB = 0; + { + btVector3 vel1,vel2; + if (multiBodyA) + { + ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; + btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA ; ++i) + rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; + } + else if(rb0) + { + rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); + } + if (multiBodyB) + { + ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; + btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB ; ++i) + rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; + + } + else if(rb1) + { + rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); + } + + solverConstraint.m_friction = 0.f;//cp.m_combinedFriction; + } + + + ///warm starting (or zero if disabled) + /* + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; + + if (solverConstraint.m_appliedImpulse) + { + if (multiBodyA) + { + btScalar impulse = solverConstraint.m_appliedImpulse; + btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + multiBodyA->applyDeltaVee(deltaV,impulse); + applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); + } else + { + if (rb0) + bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); + } + if (multiBodyB) + { + btScalar impulse = solverConstraint.m_appliedImpulse; + btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + multiBodyB->applyDeltaVee(deltaV,impulse); + applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); + } else + { + if (rb1) + bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); + } + } + } else + */ + + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedPushImpulse = 0.f; + + { + + btScalar positionalError = 0.f; + btScalar velocityError = desiredVelocity - rel_vel;// * damping; + + + btScalar erp = infoGlobal.m_erp2; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + erp = infoGlobal.m_erp; + } + + positionalError = -penetration * erp/infoGlobal.m_timeStep; + + btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + //combine position and velocity into rhs + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_rhsPenetration = 0.f; + + } else + { + //split position and velocity into rhs and m_rhsPenetration + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = penetrationImpulse; + } + + solverConstraint.m_cfm = 0.f; + solverConstraint.m_lowerLimit = lowerLimit; + solverConstraint.m_upperLimit = upperLimit; + } + + return rel_vel; + +} diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index d01870485..454c9c380 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -1,160 +1,160 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MULTIBODY_CONSTRAINT_H -#define BT_MULTIBODY_CONSTRAINT_H - -#include "LinearMath/btScalar.h" -#include "LinearMath/btAlignedObjectArray.h" -#include "btMultiBody.h" - -class btMultiBody; -struct btSolverInfo; - -#include "btMultiBodySolverConstraint.h" - -struct btMultiBodyJacobianData -{ - btAlignedObjectArray m_jacobians; - btAlignedObjectArray m_deltaVelocitiesUnitImpulse; //holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension - btAlignedObjectArray m_deltaVelocities; //holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI - btAlignedObjectArray scratch_r; - btAlignedObjectArray scratch_v; - btAlignedObjectArray scratch_m; - btAlignedObjectArray* m_solverBodyPool; - int m_fixedBodyId; - -}; - - -class btMultiBodyConstraint -{ -protected: - - btMultiBody* m_bodyA; - btMultiBody* m_bodyB; - int m_linkA; - int m_linkB; - - int m_numRows; - int m_jacSizeA; - int m_jacSizeBoth; - int m_posOffset; - - bool m_isUnilateral; - - btScalar m_maxAppliedImpulse; - - - // data block laid out as follows: - // cached impulses. (one per row.) - // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc) - // positions. (one per row.) - btAlignedObjectArray m_data; - - void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof); - - btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, - btMultiBodyJacobianData& data, - btScalar* jacOrgA, btScalar* jacOrgB, - const btVector3& contactNormalOnB, - const btVector3& posAworld, const btVector3& posBworld, - btScalar posError, - const btContactSolverInfo& infoGlobal, - btScalar lowerLimit, btScalar upperLimit, - btScalar relaxation = 1.f, - bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0); - -public: - - btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral); - virtual ~btMultiBodyConstraint(); - - - - virtual int getIslandIdA() const =0; - virtual int getIslandIdB() const =0; - - virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal)=0; - - int getNumRows() const - { - return m_numRows; - } - - btMultiBody* getMultiBodyA() - { - return m_bodyA; - } - btMultiBody* getMultiBodyB() - { - return m_bodyB; - } - - // current constraint position - // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral - // NOTE: ignored position for friction rows. - btScalar getPosition(int row) const - { - return m_data[m_posOffset + row]; - } - - void setPosition(int row, btScalar pos) - { - m_data[m_posOffset + row] = pos; - } - - - bool isUnilateral() const - { - return m_isUnilateral; - } - - // jacobian blocks. - // each of size 6 + num_links. (jacobian2 is null if no body2.) - // format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients. - btScalar* jacobianA(int row) - { - return &m_data[m_numRows + row * m_jacSizeBoth]; - } - const btScalar* jacobianA(int row) const - { - return &m_data[m_numRows + (row * m_jacSizeBoth)]; - } - btScalar* jacobianB(int row) - { - return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; - } - const btScalar* jacobianB(int row) const - { - return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; - } - - btScalar getMaxAppliedImpulse() const - { - return m_maxAppliedImpulse; - } - void setMaxAppliedImpulse(btScalar maxImp) - { - m_maxAppliedImpulse = maxImp; - } - - -}; - -#endif //BT_MULTIBODY_CONSTRAINT_H - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_MULTIBODY_CONSTRAINT_H +#define BT_MULTIBODY_CONSTRAINT_H + +#include "LinearMath/btScalar.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "btMultiBody.h" + +class btMultiBody; +struct btSolverInfo; + +#include "btMultiBodySolverConstraint.h" + +struct btMultiBodyJacobianData +{ + btAlignedObjectArray m_jacobians; + btAlignedObjectArray m_deltaVelocitiesUnitImpulse; //holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension + btAlignedObjectArray m_deltaVelocities; //holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI + btAlignedObjectArray scratch_r; + btAlignedObjectArray scratch_v; + btAlignedObjectArray scratch_m; + btAlignedObjectArray* m_solverBodyPool; + int m_fixedBodyId; + +}; + + +class btMultiBodyConstraint +{ +protected: + + btMultiBody* m_bodyA; + btMultiBody* m_bodyB; + int m_linkA; + int m_linkB; + + int m_numRows; + int m_jacSizeA; + int m_jacSizeBoth; + int m_posOffset; + + bool m_isUnilateral; + + btScalar m_maxAppliedImpulse; + + + // data block laid out as follows: + // cached impulses. (one per row.) + // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc) + // positions. (one per row.) + btAlignedObjectArray m_data; + + void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof); + + btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, + btMultiBodyJacobianData& data, + btScalar* jacOrgA, btScalar* jacOrgB, + const btVector3& contactNormalOnB, + const btVector3& posAworld, const btVector3& posBworld, + btScalar posError, + const btContactSolverInfo& infoGlobal, + btScalar lowerLimit, btScalar upperLimit, + btScalar relaxation = 1.f, + bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0); + +public: + + btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral); + virtual ~btMultiBodyConstraint(); + + + + virtual int getIslandIdA() const =0; + virtual int getIslandIdB() const =0; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal)=0; + + int getNumRows() const + { + return m_numRows; + } + + btMultiBody* getMultiBodyA() + { + return m_bodyA; + } + btMultiBody* getMultiBodyB() + { + return m_bodyB; + } + + // current constraint position + // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral + // NOTE: ignored position for friction rows. + btScalar getPosition(int row) const + { + return m_data[m_posOffset + row]; + } + + void setPosition(int row, btScalar pos) + { + m_data[m_posOffset + row] = pos; + } + + + bool isUnilateral() const + { + return m_isUnilateral; + } + + // jacobian blocks. + // each of size 6 + num_links. (jacobian2 is null if no body2.) + // format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients. + btScalar* jacobianA(int row) + { + return &m_data[m_numRows + row * m_jacSizeBoth]; + } + const btScalar* jacobianA(int row) const + { + return &m_data[m_numRows + (row * m_jacSizeBoth)]; + } + btScalar* jacobianB(int row) + { + return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; + } + const btScalar* jacobianB(int row) const + { + return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA]; + } + + btScalar getMaxAppliedImpulse() const + { + return m_maxAppliedImpulse; + } + void setMaxAppliedImpulse(btScalar maxImp) + { + m_maxAppliedImpulse = maxImp; + } + + +}; + +#endif //BT_MULTIBODY_CONSTRAINT_H + diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 90a0e25a5..7f77376bf 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -1,906 +1,907 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btMultiBodyConstraintSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -#include "btMultiBodyLinkCollider.h" - -#include "BulletDynamics/ConstraintSolver/btSolverBody.h" -#include "btMultiBodyConstraint.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" - -#include "LinearMath/btQuickprof.h" - -btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) -{ - btScalar val = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); - - //solve featherstone non-contact constraints - - //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size()); - for (int j=0;j__posUpdated = false; - if(constraint.m_multiBodyB) - constraint.m_multiBodyB->__posUpdated = false; - } - - //solve featherstone normal contact - for (int j=0;j__posUpdated = false; - if(constraint.m_multiBodyB) - constraint.m_multiBodyB->__posUpdated = false; - } - - //solve featherstone frictional contact - - for (int j=0;jm_multiBodyFrictionContactConstraints.size();j++) - { - if (iteration < infoGlobal.m_numIterations) - { - btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j]; - btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; - //adjust friction limits here - if (totalImpulse>btScalar(0)) - { - frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); - frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; - resolveSingleConstraintRowGeneric(frictionConstraint); - - if(frictionConstraint.m_multiBodyA) - frictionConstraint.m_multiBodyA->__posUpdated = false; - if(frictionConstraint.m_multiBodyB) - frictionConstraint.m_multiBodyB->__posUpdated = false; - } - } - } - return val; -} - -btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) -{ - m_multiBodyNonContactConstraints.resize(0); - m_multiBodyNormalContactConstraints.resize(0); - m_multiBodyFrictionContactConstraints.resize(0); - m_data.m_jacobians.resize(0); - m_data.m_deltaVelocitiesUnitImpulse.resize(0); - m_data.m_deltaVelocities.resize(0); - - for (int i=0;im_multiBody->setCompanionId(-1); - } - } - - btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer); - - return val; -} - -void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) -{ - for (int i = 0; i < ndof; ++i) - m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; -} - -void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c) -{ - - btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - btScalar deltaVelADotn=0; - btScalar deltaVelBDotn=0; - btSolverBody* bodyA = 0; - btSolverBody* bodyB = 0; - int ndofA=0; - int ndofB=0; - - if (c.m_multiBodyA) - { - ndofA = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6; - for (int i = 0; i < ndofA; ++i) - deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i]; - } else if(c.m_solverBodyIdA >= 0) - { - bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA]; - deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); - } - - if (c.m_multiBodyB) - { - ndofB = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6; - for (int i = 0; i < ndofB; ++i) - deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i]; - } else if(c.m_solverBodyIdB >= 0) - { - bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB]; - deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); - } - - - deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom - deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv; - const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; - - if (sum < c.m_lowerLimit) - { - deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_lowerLimit; - } - else if (sum > c.m_upperLimit) - { - deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_upperLimit; - } - else - { - c.m_appliedImpulse = sum; - } - - if (c.m_multiBodyA) - { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); - //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations - //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - if(c.m_multiBodyA->isMultiDof()) - c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); - else - c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); - } else if(c.m_solverBodyIdA >= 0) - { - bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - - } - if (c.m_multiBodyB) - { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); - //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations - //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - if(c.m_multiBodyB->isMultiDof()) - c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); - else - c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); - } else if(c.m_solverBodyIdB >= 0) - { - bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse); - } - -} - - -void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c) -{ - - btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - btScalar deltaVelADotn=0; - btScalar deltaVelBDotn=0; - int ndofA=0; - int ndofB=0; - - if (c.m_multiBodyA) - { - ndofA = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6; - for (int i = 0; i < ndofA; ++i) - deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i]; - } - - if (c.m_multiBodyB) - { - ndofB = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6; - for (int i = 0; i < ndofB; ++i) - deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i]; - } - - - deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom - deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv; - const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; - - if (sum < c.m_lowerLimit) - { - deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_lowerLimit; - } - else if (sum > c.m_upperLimit) - { - deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_upperLimit; - } - else - { - c.m_appliedImpulse = sum; - } - - if (c.m_multiBodyA) - { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); - c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); - } - if (c.m_multiBodyB) - { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); - c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); - } -} - - -void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, - const btVector3& contactNormal, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) -{ - - BT_PROFILE("setupMultiBodyContactConstraint"); - btVector3 rel_pos1; - btVector3 rel_pos2; - - btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; - btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; - - const btVector3& pos1 = cp.getPositionWorldOnA(); - const btVector3& pos2 = cp.getPositionWorldOnB(); - - btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA]; - btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB]; - - btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; - btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; - - if (bodyA) - rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); - if (bodyB) - rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); - - relaxation = 1.f; - - if (multiBodyA) - { - const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; - - solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); - - if (solverConstraint.m_deltaVelAindex <0) - { - solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size(); - multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); - m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA); - } else - { - btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); - } - - solverConstraint.m_jacAindex = m_data.m_jacobians.size(); - m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA); - m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA); - btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); - - btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex]; - if(multiBodyA->isMultiDof()) - multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); - else - multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); - btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - if(multiBodyA->isMultiDof()) - multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); - else - multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); - } else - { - btVector3 torqueAxis0 = rel_pos1.cross(contactNormal); - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); - solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = contactNormal; - } - - if (multiBodyB) - { - const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; - - solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); - if (solverConstraint.m_deltaVelBindex <0) - { - solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size(); - multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); - m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB); - } - - solverConstraint.m_jacBindex = m_data.m_jacobians.size(); - - m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB); - m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB); - btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); - - if(multiBodyB->isMultiDof()) - multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); - else - multiBodyB->fillContactJacobian(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); - if(multiBodyB->isMultiDof()) - multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); - else - multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); - } else - { - btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); - solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - solverConstraint.m_contactNormal2 = -contactNormal; - } - - { - - btVector3 vec; - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - btScalar* jacB = 0; - btScalar* jacA = 0; - btScalar* lambdaA =0; - btScalar* lambdaB =0; - int ndofA = 0; - if (multiBodyA) - { - ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; - jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; - lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA; ++i) - { - btScalar j = jacA[i] ; - btScalar l =lambdaA[i]; - denom0 += j*l; - } - } else - { - if (rb0) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + contactNormal.dot(vec); - } - } - if (multiBodyB) - { - const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; - jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; - lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB; ++i) - { - btScalar j = jacB[i] ; - btScalar l =lambdaB[i]; - denom1 += j*l; - } - - } else - { - if (rb1) - { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->getInvMass() + contactNormal.dot(vec); - } - } - - if (multiBodyA && (multiBodyA==multiBodyB)) - { - // ndof1 == ndof2 in this case - for (int i = 0; i < ndofA; ++i) - { - denom1 += jacB[i] * lambdaA[i]; - denom1 += jacA[i] * lambdaB[i]; - } - } - - btScalar d = denom0+denom1; - if (btFabs(d)>SIMD_EPSILON) - { - - solverConstraint.m_jacDiagABInv = relaxation/(d); - } else - { - solverConstraint.m_jacDiagABInv = 1.f; - } - - } - - - //compute rhs and remaining solverConstraint fields - - - - btScalar restitution = 0.f; - btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop; - - btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; - { - - btVector3 vel1,vel2; - if (multiBodyA) - { - ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; - btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA ; ++i) - rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; - } else - { - if (rb0) - { - rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); - } - } - if (multiBodyB) - { - ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; - btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB ; ++i) - rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; - - } else - { - if (rb1) - { - rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); - } - } - - solverConstraint.m_friction = cp.m_combinedFriction; - - if(!isFriction) - { - restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); - if (restitution <= btScalar(0.)) - { - restitution = 0.f; - } - } - } - - - ///warm starting (or zero if disabled) - //disable warmstarting for btMultiBody, it has issues gaining energy (==explosion) - if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - - if (solverConstraint.m_appliedImpulse) - { - if (multiBodyA) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - if(multiBodyA->isMultiDof()) - multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse); - else - multiBodyA->applyDeltaVee(deltaV,impulse); - applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); - } else - { - if (rb0) - bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); - } - if (multiBodyB) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - if(multiBodyB->isMultiDof()) - multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse); - else - multiBodyB->applyDeltaVee(deltaV,impulse); - applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); - } else - { - if (rb1) - bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); - } - } - } else - { - solverConstraint.m_appliedImpulse = 0.f; - } - - solverConstraint.m_appliedPushImpulse = 0.f; - - { - - btScalar positionalError = 0.f; - btScalar velocityError = restitution - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction - - btScalar erp = infoGlobal.m_erp2; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - erp = infoGlobal.m_erp; - } - - if (penetration>0) - { - positionalError = 0; - velocityError -= penetration / infoGlobal.m_timeStep; - - } else - { - positionalError = -penetration * erp/infoGlobal.m_timeStep; - } - - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - - if(!isFriction) - { - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; - solverConstraint.m_rhsPenetration = 0.f; - - } else - { - //split position and velocity into rhs and m_rhsPenetration - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = penetrationImpulse; - } - - solverConstraint.m_lowerLimit = 0; - solverConstraint.m_upperLimit = 1e10f; - } - else - { - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = 0.f; - solverConstraint.m_lowerLimit = -solverConstraint.m_friction; - solverConstraint.m_upperLimit = solverConstraint.m_friction; - } - - solverConstraint.m_cfm = 0.f; //why not use cfmSlip? - } - -} - - - - -btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) -{ - BT_PROFILE("addMultiBodyFrictionConstraint"); - btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing(); - solverConstraint.m_frictionIndex = frictionIndex; - bool isFriction = true; - - const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); - const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); - - btMultiBody* mbA = fcA? fcA->m_multiBody : 0; - btMultiBody* mbB = fcB? fcB->m_multiBody : 0; - - int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); - int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); - - solverConstraint.m_solverBodyIdA = solverBodyIdA; - solverConstraint.m_solverBodyIdB = solverBodyIdB; - solverConstraint.m_multiBodyA = mbA; - if (mbA) - solverConstraint.m_linkA = fcA->m_link; - - solverConstraint.m_multiBodyB = mbB; - if (mbB) - solverConstraint.m_linkB = fcB->m_link; - - solverConstraint.m_originalContactPoint = &cp; - - setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip); - return solverConstraint; -} - -void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) -{ - const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); - const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); - - btMultiBody* mbA = fcA? fcA->m_multiBody : 0; - btMultiBody* mbB = fcB? fcB->m_multiBody : 0; - - btCollisionObject* colObj0=0,*colObj1=0; - - colObj0 = (btCollisionObject*)manifold->getBody0(); - colObj1 = (btCollisionObject*)manifold->getBody1(); - - int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); - int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); - - btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA]; - btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB]; - - - ///avoid collision response between two static objects -// if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero()))) - // return; - - int rollingFriction=1; - - for (int j=0;jgetNumContacts();j++) - { - - btManifoldPoint& cp = manifold->getContactPoint(j); - - if (cp.getDistance() <= manifold->getContactProcessingThreshold()) - { - - btScalar relaxation; - - int frictionIndex = m_multiBodyNormalContactConstraints.size(); - - btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing(); - - btRigidBody* rb0 = btRigidBody::upcast(colObj0); - btRigidBody* rb1 = btRigidBody::upcast(colObj1); - solverConstraint.m_solverBodyIdA = solverBodyIdA; - solverConstraint.m_solverBodyIdB = solverBodyIdB; - solverConstraint.m_multiBodyA = mbA; - if (mbA) - solverConstraint.m_linkA = fcA->m_link; - - solverConstraint.m_multiBodyB = mbB; - if (mbB) - solverConstraint.m_linkB = fcB->m_link; - - solverConstraint.m_originalContactPoint = &cp; - - bool isFriction = false; - setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction); - -// const btVector3& pos1 = cp.getPositionWorldOnA(); -// const btVector3& pos2 = cp.getPositionWorldOnB(); - - /////setup the friction constraints -#define ENABLE_FRICTION -#ifdef ENABLE_FRICTION - solverConstraint.m_frictionIndex = frictionIndex; -#if ROLLING_FRICTION - btVector3 angVelA(0,0,0),angVelB(0,0,0); - if (rb0) - angVelA = rb0->getAngularVelocity(); - if (rb1) - angVelB = rb1->getAngularVelocity(); - btVector3 relAngVel = angVelB-angVelA; - - if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) - { - //only a single rollingFriction per manifold - rollingFriction--; - if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold) - { - relAngVel.normalize(); - applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - if (relAngVel.length()>0.001) - addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } else - { - addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - btVector3 axis0,axis1; - btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1); - applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - if (axis0.length()>0.001) - addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - if (axis1.length()>0.001) - addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } - } -#endif //ROLLING_FRICTION - - ///Bullet has several options to set the friction directions - ///By default, each contact has only a single friction direction that is recomputed automatically very frame - ///based on the relative linear velocity. - ///If the relative velocity it zero, it will automatically compute a friction direction. - - ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS. - ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction. - /// - ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity. - /// - ///The user can manually override the friction directions for certain contacts using a contact callback, - ///and set the cp.m_lateralFrictionInitialized to true - ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) - ///this will give a conveyor belt effect - /// - if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) - {/* - cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; - btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); - if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) - { - cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel); - if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); - cp.m_lateralFrictionDir2.normalize();//?? - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } - - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } else - */ - { - btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); - - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); - - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); - } - - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) - { - cp.m_lateralFrictionInitialized = true; - } - } - - } else - { - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1); - - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2); - - //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); - //todo: - solverConstraint.m_appliedImpulse = 0.f; - solverConstraint.m_appliedPushImpulse = 0.f; - } - - -#endif //ENABLE_FRICTION - - } - } -} - -void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal) -{ - btPersistentManifold* manifold = 0; - - for (int i=0;igetBody0()); - const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); - if (!fcA && !fcB) - { - //the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case - convertContact(manifold,infoGlobal); - } else - { - convertMultiBodyContact(manifold,infoGlobal); - } - } - - //also convert the multibody constraints, if any - - - for (int i=0;icreateConstraintRows(m_multiBodyNonContactConstraints,m_data, infoGlobal); - } - -} - - - -btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher) -{ - return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); -} - -btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal) -{ - int numPoolConstraints = m_multiBodyNormalContactConstraints.size(); - int i,j; - - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - for (j=0;jm_appliedImpulse = solveManifold.m_appliedImpulse; - - pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solveManifold.m_frictionIndex].m_appliedImpulse; - //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solveManifold.m_frictionIndex+1].m_appliedImpulse; - } - //do a callback here? - } - } - - - numPoolConstraints = m_multiBodyNonContactConstraints.size(); - -#if 0 - //@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint - for (int i=0;igetJointFeedback(); - if (fb) - { - fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep; - fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep; - fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; - fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */ - - } - - constr->internalSetAppliedImpulse(c.m_appliedImpulse); - if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold()) - { - constr->setEnabled(false); - } - - } -#endif - - - return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal); -} - - -void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher) -{ - //printf("solveMultiBodyGroup start\n"); - m_tmpMultiBodyConstraints = multiBodyConstraints; - m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; - - btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); - - m_tmpMultiBodyConstraints = 0; - m_tmpNumMultiBodyConstraints = 0; - - -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btMultiBodyConstraintSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "btMultiBodyLinkCollider.h" + +#include "BulletDynamics/ConstraintSolver/btSolverBody.h" +#include "btMultiBodyConstraint.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" + +#include "LinearMath/btQuickprof.h" + +btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +{ + btScalar val = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); + + //solve featherstone non-contact constraints + + //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size()); + for (int j=0;jsetPosUpdated(false); + if(constraint.m_multiBodyB) + constraint.m_multiBodyB->setPosUpdated(false); + } + + //solve featherstone normal contact + for (int j=0;jsetPosUpdated(false); + if(constraint.m_multiBodyB) + constraint.m_multiBodyB->setPosUpdated(false); + } + + //solve featherstone frictional contact + + for (int j=0;jm_multiBodyFrictionContactConstraints.size();j++) + { + if (iteration < infoGlobal.m_numIterations) + { + btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j]; + btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; + //adjust friction limits here + if (totalImpulse>btScalar(0)) + { + frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); + frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; + resolveSingleConstraintRowGeneric(frictionConstraint); + + if(frictionConstraint.m_multiBodyA) + frictionConstraint.m_multiBodyA->setPosUpdated(false); + if(frictionConstraint.m_multiBodyB) + frictionConstraint.m_multiBodyB->setPosUpdated(false); + } + } + } + return val; +} + +btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +{ + m_multiBodyNonContactConstraints.resize(0); + m_multiBodyNormalContactConstraints.resize(0); + m_multiBodyFrictionContactConstraints.resize(0); + m_data.m_jacobians.resize(0); + m_data.m_deltaVelocitiesUnitImpulse.resize(0); + m_data.m_deltaVelocities.resize(0); + + for (int i=0;im_multiBody->setCompanionId(-1); + } + } + + btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer); + + return val; +} + +void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) +{ + for (int i = 0; i < ndof; ++i) + m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; +} + +void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c) +{ + + btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; + btScalar deltaVelADotn=0; + btScalar deltaVelBDotn=0; + btSolverBody* bodyA = 0; + btSolverBody* bodyB = 0; + int ndofA=0; + int ndofB=0; + + if (c.m_multiBodyA) + { + ndofA = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6; + for (int i = 0; i < ndofA; ++i) + deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i]; + } else if(c.m_solverBodyIdA >= 0) + { + bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA]; + deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); + } + + if (c.m_multiBodyB) + { + ndofB = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6; + for (int i = 0; i < ndofB; ++i) + deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i]; + } else if(c.m_solverBodyIdB >= 0) + { + bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB]; + deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); + } + + + deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom + deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv; + const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; + + if (sum < c.m_lowerLimit) + { + deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_lowerLimit; + } + else if (sum > c.m_upperLimit) + { + deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_upperLimit; + } + else + { + c.m_appliedImpulse = sum; + } + + if (c.m_multiBodyA) + { + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); + //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations + //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity + if(c.m_multiBodyA->isMultiDof()) + c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); + else + c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); + } else if(c.m_solverBodyIdA >= 0) + { + bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse); + + } + if (c.m_multiBodyB) + { + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); + //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations + //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity + if(c.m_multiBodyB->isMultiDof()) + c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); + else + c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); + } else if(c.m_solverBodyIdB >= 0) + { + bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + } + +} + + +void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c) +{ + + btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; + btScalar deltaVelADotn=0; + btScalar deltaVelBDotn=0; + int ndofA=0; + int ndofB=0; + + if (c.m_multiBodyA) + { + ndofA = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6; + for (int i = 0; i < ndofA; ++i) + deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i]; + } + + if (c.m_multiBodyB) + { + ndofB = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6; + for (int i = 0; i < ndofB; ++i) + deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i]; + } + + + deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom + deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv; + const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; + + if (sum < c.m_lowerLimit) + { + deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_lowerLimit; + } + else if (sum > c.m_upperLimit) + { + deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_upperLimit; + } + else + { + c.m_appliedImpulse = sum; + } + + if (c.m_multiBodyA) + { + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); + c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); + } + if (c.m_multiBodyB) + { + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); + c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); + } +} + + +void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, + const btVector3& contactNormal, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) +{ + + BT_PROFILE("setupMultiBodyContactConstraint"); + btVector3 rel_pos1; + btVector3 rel_pos2; + + btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; + btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; + + const btVector3& pos1 = cp.getPositionWorldOnA(); + const btVector3& pos2 = cp.getPositionWorldOnB(); + + btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA]; + btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB]; + + btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; + btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; + + if (bodyA) + rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + if (bodyB) + rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); + + relaxation = 1.f; + + if (multiBodyA) + { + const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; + + solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); + + if (solverConstraint.m_deltaVelAindex <0) + { + solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size(); + multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA); + } else + { + btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); + } + + solverConstraint.m_jacAindex = m_data.m_jacobians.size(); + m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA); + btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); + + btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex]; + if(multiBodyA->isMultiDof()) + multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + else + multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + if(multiBodyA->isMultiDof()) + multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); + else + multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); + } else + { + btVector3 torqueAxis0 = rel_pos1.cross(contactNormal); + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = contactNormal; + } + + if (multiBodyB) + { + const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; + + solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); + if (solverConstraint.m_deltaVelBindex <0) + { + solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size(); + multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB); + } + + solverConstraint.m_jacBindex = m_data.m_jacobians.size(); + + m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB); + btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); + + if(multiBodyB->isMultiDof()) + multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + else + multiBodyB->fillContactJacobian(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + if(multiBodyB->isMultiDof()) + multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); + else + multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); + } else + { + btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + solverConstraint.m_contactNormal2 = -contactNormal; + } + + { + + btVector3 vec; + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + btScalar* jacB = 0; + btScalar* jacA = 0; + btScalar* lambdaA =0; + btScalar* lambdaB =0; + int ndofA = 0; + if (multiBodyA) + { + ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; + jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; + lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) + { + btScalar j = jacA[i] ; + btScalar l =lambdaA[i]; + denom0 += j*l; + } + } else + { + if (rb0) + { + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = rb0->getInvMass() + contactNormal.dot(vec); + } + } + if (multiBodyB) + { + const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; + jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; + lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + { + btScalar j = jacB[i] ; + btScalar l =lambdaB[i]; + denom1 += j*l; + } + + } else + { + if (rb1) + { + vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = rb1->getInvMass() + contactNormal.dot(vec); + } + } + + if (multiBodyA && (multiBodyA==multiBodyB)) + { + // ndof1 == ndof2 in this case + for (int i = 0; i < ndofA; ++i) + { + denom1 += jacB[i] * lambdaA[i]; + denom1 += jacA[i] * lambdaB[i]; + } + } + + btScalar d = denom0+denom1; + if (btFabs(d)>SIMD_EPSILON) + { + + solverConstraint.m_jacDiagABInv = relaxation/(d); + } else + { + solverConstraint.m_jacDiagABInv = 1.f; + } + + } + + + //compute rhs and remaining solverConstraint fields + + + + btScalar restitution = 0.f; + btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop; + + btScalar rel_vel = 0.f; + int ndofA = 0; + int ndofB = 0; + { + + btVector3 vel1,vel2; + if (multiBodyA) + { + ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; + btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA ; ++i) + rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; + } else + { + if (rb0) + { + rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); + } + } + if (multiBodyB) + { + ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; + btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB ; ++i) + rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; + + } else + { + if (rb1) + { + rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); + } + } + + solverConstraint.m_friction = cp.m_combinedFriction; + + if(!isFriction) + { + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); + if (restitution <= btScalar(0.)) + { + restitution = 0.f; + } + } + } + + + ///warm starting (or zero if disabled) + //disable warmstarting for btMultiBody, it has issues gaining energy (==explosion) + if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; + + if (solverConstraint.m_appliedImpulse) + { + if (multiBodyA) + { + btScalar impulse = solverConstraint.m_appliedImpulse; + btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + if(multiBodyA->isMultiDof()) + multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse); + else + multiBodyA->applyDeltaVee(deltaV,impulse); + applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); + } else + { + if (rb0) + bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); + } + if (multiBodyB) + { + btScalar impulse = solverConstraint.m_appliedImpulse; + btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + if(multiBodyB->isMultiDof()) + multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse); + else + multiBodyB->applyDeltaVee(deltaV,impulse); + applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); + } else + { + if (rb1) + bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); + } + } + } else + { + solverConstraint.m_appliedImpulse = 0.f; + } + + solverConstraint.m_appliedPushImpulse = 0.f; + + { + + btScalar positionalError = 0.f; + btScalar velocityError = restitution - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction + + btScalar erp = infoGlobal.m_erp2; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + erp = infoGlobal.m_erp; + } + + if (penetration>0) + { + positionalError = 0; + velocityError -= penetration / infoGlobal.m_timeStep; + + } else + { + positionalError = -penetration * erp/infoGlobal.m_timeStep; + } + + btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + + if(!isFriction) + { + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + //combine position and velocity into rhs + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_rhsPenetration = 0.f; + + } else + { + //split position and velocity into rhs and m_rhsPenetration + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = penetrationImpulse; + } + + solverConstraint.m_lowerLimit = 0; + solverConstraint.m_upperLimit = 1e10f; + } + else + { + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = 0.f; + solverConstraint.m_lowerLimit = -solverConstraint.m_friction; + solverConstraint.m_upperLimit = solverConstraint.m_friction; + } + + solverConstraint.m_cfm = 0.f; //why not use cfmSlip? + } + +} + + + + +btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +{ + BT_PROFILE("addMultiBodyFrictionConstraint"); + btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing(); + solverConstraint.m_frictionIndex = frictionIndex; + bool isFriction = true; + + const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); + const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + + btMultiBody* mbA = fcA? fcA->m_multiBody : 0; + btMultiBody* mbB = fcB? fcB->m_multiBody : 0; + + int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); + int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_multiBodyA = mbA; + if (mbA) + solverConstraint.m_linkA = fcA->m_link; + + solverConstraint.m_multiBodyB = mbB; + if (mbB) + solverConstraint.m_linkB = fcB->m_link; + + solverConstraint.m_originalContactPoint = &cp; + + setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip); + return solverConstraint; +} + +void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) +{ + const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); + const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + + btMultiBody* mbA = fcA? fcA->m_multiBody : 0; + btMultiBody* mbB = fcB? fcB->m_multiBody : 0; + + btCollisionObject* colObj0=0,*colObj1=0; + + colObj0 = (btCollisionObject*)manifold->getBody0(); + colObj1 = (btCollisionObject*)manifold->getBody1(); + + int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); + int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); + +// btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA]; +// btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB]; + + + ///avoid collision response between two static objects +// if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero()))) + // return; + + + + for (int j=0;jgetNumContacts();j++) + { + + btManifoldPoint& cp = manifold->getContactPoint(j); + + if (cp.getDistance() <= manifold->getContactProcessingThreshold()) + { + + btScalar relaxation; + + int frictionIndex = m_multiBodyNormalContactConstraints.size(); + + btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing(); + + // btRigidBody* rb0 = btRigidBody::upcast(colObj0); + // btRigidBody* rb1 = btRigidBody::upcast(colObj1); + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_multiBodyA = mbA; + if (mbA) + solverConstraint.m_linkA = fcA->m_link; + + solverConstraint.m_multiBodyB = mbB; + if (mbB) + solverConstraint.m_linkB = fcB->m_link; + + solverConstraint.m_originalContactPoint = &cp; + + bool isFriction = false; + setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction); + +// const btVector3& pos1 = cp.getPositionWorldOnA(); +// const btVector3& pos2 = cp.getPositionWorldOnB(); + + /////setup the friction constraints +#define ENABLE_FRICTION +#ifdef ENABLE_FRICTION + solverConstraint.m_frictionIndex = frictionIndex; +#if ROLLING_FRICTION + int rollingFriction=1; + btVector3 angVelA(0,0,0),angVelB(0,0,0); + if (rb0) + angVelA = rb0->getAngularVelocity(); + if (rb1) + angVelB = rb1->getAngularVelocity(); + btVector3 relAngVel = angVelB-angVelA; + + if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) + { + //only a single rollingFriction per manifold + rollingFriction--; + if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold) + { + relAngVel.normalize(); + applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + if (relAngVel.length()>0.001) + addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + + } else + { + addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + btVector3 axis0,axis1; + btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1); + applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + if (axis0.length()>0.001) + addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + if (axis1.length()>0.001) + addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + + } + } +#endif //ROLLING_FRICTION + + ///Bullet has several options to set the friction directions + ///By default, each contact has only a single friction direction that is recomputed automatically very frame + ///based on the relative linear velocity. + ///If the relative velocity it zero, it will automatically compute a friction direction. + + ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS. + ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction. + /// + ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity. + /// + ///The user can manually override the friction directions for certain contacts using a contact callback, + ///and set the cp.m_lateralFrictionInitialized to true + ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) + ///this will give a conveyor belt effect + /// + if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) + {/* + cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; + btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); + if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) + { + cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel); + if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); + cp.m_lateralFrictionDir2.normalize();//?? + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + + } + + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + + } else + */ + { + btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); + + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); + } + + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) + { + cp.m_lateralFrictionInitialized = true; + } + } + + } else + { + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1); + + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2); + + //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); + //todo: + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedPushImpulse = 0.f; + } + + +#endif //ENABLE_FRICTION + + } + } +} + +void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal) +{ + //btPersistentManifold* manifold = 0; + + for (int i=0;igetBody0()); + const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + if (!fcA && !fcB) + { + //the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case + convertContact(manifold,infoGlobal); + } else + { + convertMultiBodyContact(manifold,infoGlobal); + } + } + + //also convert the multibody constraints, if any + + + for (int i=0;icreateConstraintRows(m_multiBodyNonContactConstraints,m_data, infoGlobal); + } + +} + + + +btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher) +{ + return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); +} + +btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal) +{ + int numPoolConstraints = m_multiBodyNormalContactConstraints.size(); + int j; + + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + for (j=0;jm_appliedImpulse = solveManifold.m_appliedImpulse; + + pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solveManifold.m_frictionIndex].m_appliedImpulse; + //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solveManifold.m_frictionIndex+1].m_appliedImpulse; + } + //do a callback here? + } + } + + + numPoolConstraints = m_multiBodyNonContactConstraints.size(); + +#if 0 + //@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint + for (int i=0;igetJointFeedback(); + if (fb) + { + fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep; + fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep; + fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; + fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */ + + } + + constr->internalSetAppliedImpulse(c.m_appliedImpulse); + if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold()) + { + constr->setEnabled(false); + } + + } +#endif + + + return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal); +} + + +void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher) +{ + //printf("solveMultiBodyGroup start\n"); + m_tmpMultiBodyConstraints = multiBodyConstraints; + m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; + + btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); + + m_tmpMultiBodyConstraints = 0; + m_tmpNumMultiBodyConstraints = 0; + + +} diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h index 800941c64..f11a007aa 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -1,86 +1,86 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MULTIBODY_CONSTRAINT_SOLVER_H -#define BT_MULTIBODY_CONSTRAINT_SOLVER_H - -#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" -#include "btMultiBodySolverConstraint.h" - - -class btMultiBody; - -#include "btMultiBodyConstraint.h" - - - -ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver -{ - -protected: - - btMultiBodyConstraintArray m_multiBodyNonContactConstraints; - - btMultiBodyConstraintArray m_multiBodyNormalContactConstraints; - btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints; - - btMultiBodyJacobianData m_data; - - //temp storage for multi body constraints for a specific island/group called by 'solveGroup' - btMultiBodyConstraint** m_tmpMultiBodyConstraints; - int m_tmpNumMultiBodyConstraints; - - void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c); - void resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c); - - void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal); - btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0); - - - void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, - btScalar* jacA,btScalar* jacB, - btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff, - const btContactSolverInfo& infoGlobal); - - void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, - const btVector3& contactNormal, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); - - void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); - virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); -// virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - - virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - void applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof); - -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - ///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints) - virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); - virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal); - - virtual void solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); -}; - - - - - -#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_MULTIBODY_CONSTRAINT_SOLVER_H +#define BT_MULTIBODY_CONSTRAINT_SOLVER_H + +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" +#include "btMultiBodySolverConstraint.h" + + +class btMultiBody; + +#include "btMultiBodyConstraint.h" + + + +ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver +{ + +protected: + + btMultiBodyConstraintArray m_multiBodyNonContactConstraints; + + btMultiBodyConstraintArray m_multiBodyNormalContactConstraints; + btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints; + + btMultiBodyJacobianData m_data; + + //temp storage for multi body constraints for a specific island/group called by 'solveGroup' + btMultiBodyConstraint** m_tmpMultiBodyConstraints; + int m_tmpNumMultiBodyConstraints; + + void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c); + void resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c); + + void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal); + btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0); + + + void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, + btScalar* jacA,btScalar* jacB, + btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff, + const btContactSolverInfo& infoGlobal); + + void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, + const btVector3& contactNormal, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); + + void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); + virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); +// virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + + virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + void applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof); + +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + ///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints) + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); + virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal); + + virtual void solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); +}; + + + + + +#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H + diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 58e48cd9a..7fa42073a 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -1,753 +1,753 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btMultiBodyDynamicsWorld.h" -#include "btMultiBodyConstraintSolver.h" -#include "btMultiBody.h" -#include "btMultiBodyLinkCollider.h" -#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" -#include "LinearMath/btQuickprof.h" -#include "btMultiBodyConstraint.h" - - - - -void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask) -{ - m_multiBodies.push_back(body); - -} - -void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body) -{ - m_multiBodies.remove(body); -} - -void btMultiBodyDynamicsWorld::calculateSimulationIslands() -{ - BT_PROFILE("calculateSimulationIslands"); - - getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); - - { - //merge islands based on speculative contact manifolds too - for (int i=0;im_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()))) - { - getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); - } - } - } - - { - int i; - int numConstraints = int(m_constraints.size()); - for (i=0;i< numConstraints ; i++ ) - { - btTypedConstraint* constraint = m_constraints[i]; - if (constraint->isEnabled()) - { - const btRigidBody* colObj0 = &constraint->getRigidBodyA(); - const btRigidBody* colObj1 = &constraint->getRigidBodyB(); - - if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && - ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) - { - getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); - } - } - } - } - - //merge islands linked by Featherstone link colliders - for (int i=0;igetBaseCollider(); - - for (int b=0;bgetNumLinks();b++) - { - btMultiBodyLinkCollider* cur = body->getLink(b).m_collider; - - if (((cur) && (!(cur)->isStaticOrKinematicObject())) && - ((prev) && (!(prev)->isStaticOrKinematicObject()))) - { - int tagPrev = prev->getIslandTag(); - int tagCur = cur->getIslandTag(); - getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur); - } - if (cur && !cur->isStaticOrKinematicObject()) - prev = cur; - - } - } - } - - //merge islands linked by multibody constraints - { - for (int i=0;im_multiBodyConstraints.size();i++) - { - btMultiBodyConstraint* c = m_multiBodyConstraints[i]; - int tagA = c->getIslandIdA(); - int tagB = c->getIslandIdB(); - if (tagA>=0 && tagB>=0) - getSimulationIslandManager()->getUnionFind().unite(tagA, tagB); - } - } - - //Store the island id in each body - getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); - -} - - -void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) -{ - BT_PROFILE("btMultiBodyDynamicsWorld::updateActivationState"); - - - - for ( int i=0;icheckMotionAndSleepIfRequired(timeStep); - if (!body->isAwake()) - { - btMultiBodyLinkCollider* col = body->getBaseCollider(); - if (col && col->getActivationState() == ACTIVE_TAG) - { - col->setActivationState( WANTS_DEACTIVATION); - col->setDeactivationTime(0.f); - } - for (int b=0;bgetNumLinks();b++) - { - btMultiBodyLinkCollider* col = body->getLink(b).m_collider; - if (col && col->getActivationState() == ACTIVE_TAG) - { - col->setActivationState( WANTS_DEACTIVATION); - col->setDeactivationTime(0.f); - } - } - } else - { - btMultiBodyLinkCollider* col = body->getBaseCollider(); - if (col && col->getActivationState() != DISABLE_DEACTIVATION) - col->setActivationState( ACTIVE_TAG ); - - for (int b=0;bgetNumLinks();b++) - { - btMultiBodyLinkCollider* col = body->getLink(b).m_collider; - if (col && col->getActivationState() != DISABLE_DEACTIVATION) - col->setActivationState( ACTIVE_TAG ); - } - } - - } - } - - btDiscreteDynamicsWorld::updateActivationState(timeStep); -} - - -SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs) -{ - int islandId; - - const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); - const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); - islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag(); - return islandId; - -} - - -class btSortConstraintOnIslandPredicate2 -{ - public: - - bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const - { - int rIslandId0,lIslandId0; - rIslandId0 = btGetConstraintIslandId2(rhs); - lIslandId0 = btGetConstraintIslandId2(lhs); - return lIslandId0 < rIslandId0; - } -}; - - - -SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs) -{ - int islandId; - - int islandTagA = lhs->getIslandIdA(); - int islandTagB = lhs->getIslandIdB(); - islandId= islandTagA>=0?islandTagA:islandTagB; - return islandId; - -} - - -class btSortMultiBodyConstraintOnIslandPredicate -{ - public: - - bool operator() ( const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs ) const - { - int rIslandId0,lIslandId0; - rIslandId0 = btGetMultiBodyConstraintIslandId(rhs); - lIslandId0 = btGetMultiBodyConstraintIslandId(lhs); - return lIslandId0 < rIslandId0; - } -}; - -struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback -{ - btContactSolverInfo* m_solverInfo; - btMultiBodyConstraintSolver* m_solver; - btMultiBodyConstraint** m_multiBodySortedConstraints; - int m_numMultiBodyConstraints; - - btTypedConstraint** m_sortedConstraints; - int m_numConstraints; - btIDebugDraw* m_debugDrawer; - btDispatcher* m_dispatcher; - - btAlignedObjectArray m_bodies; - btAlignedObjectArray m_manifolds; - btAlignedObjectArray m_constraints; - btAlignedObjectArray m_multiBodyConstraints; - - - MultiBodyInplaceSolverIslandCallback( btMultiBodyConstraintSolver* solver, - btDispatcher* dispatcher) - :m_solverInfo(NULL), - m_solver(solver), - m_multiBodySortedConstraints(NULL), - m_numConstraints(0), - m_debugDrawer(NULL), - m_dispatcher(dispatcher) - { - - } - - MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other) - { - btAssert(0); - (void)other; - return *this; - } - - SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer) - { - btAssert(solverInfo); - m_solverInfo = solverInfo; - - m_multiBodySortedConstraints = sortedMultiBodyConstraints; - m_numMultiBodyConstraints = numMultiBodyConstraints; - m_sortedConstraints = sortedConstraints; - m_numConstraints = numConstraints; - - m_debugDrawer = debugDrawer; - m_bodies.resize (0); - m_manifolds.resize (0); - m_constraints.resize (0); - m_multiBodyConstraints.resize(0); - } - - - virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) - { - if (islandId<0) - { - ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id - m_solver->solveMultiBodyGroup( bodies,numBodies,manifolds, numManifolds,m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); - } else - { - //also add all non-contact constraints/joints for this island - btTypedConstraint** startConstraint = 0; - btMultiBodyConstraint** startMultiBodyConstraint = 0; - - int numCurConstraints = 0; - int numCurMultiBodyConstraints = 0; - - int i; - - //find the first constraint for this island - - for (i=0;im_minimumSolverBatchSize<=1) - { - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); - } else - { - - for (i=0;im_solverInfo->m_minimumSolverBatchSize) - { - processConstraints(); - } else - { - //printf("deferred\n"); - } - } - } - } - void processConstraints() - { - - 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; - btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0; - - //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size()); - - m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher); - m_bodies.resize(0); - m_manifolds.resize(0); - m_constraints.resize(0); - m_multiBodyConstraints.resize(0); - } - -}; - - - -btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) - :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration), - m_multiBodyConstraintSolver(constraintSolver) -{ - //split impulse is not yet supported for Featherstone hierarchies - getSolverInfo().m_splitImpulse = false; - getSolverInfo().m_solverMode |=SOLVER_USE_2_FRICTION_DIRECTIONS; - m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver,dispatcher); -} - -btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld () -{ - delete m_solverMultiBodyIslandCallback; -} - -void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) -{ - - btAlignedObjectArray scratch_r; - btAlignedObjectArray scratch_v; - btAlignedObjectArray scratch_m; - - - BT_PROFILE("solveConstraints"); - - m_sortedConstraints.resize( m_constraints.size()); - int i; - for (i=0;isetup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer()); - m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); - - /// solve all the constraints for this island - m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback); - - - { - BT_PROFILE("btMultiBody addForce and stepVelocities"); - for (int i=0;im_multiBodies.size();i++) - { - btMultiBody* bod = m_multiBodies[i]; - - bool isSleeping = false; - - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b=0;bgetNumLinks();b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) - isSleeping = true; - } - - if (!isSleeping) - { - //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) - scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd) - scratch_v.resize(bod->getNumLinks()+1); - scratch_m.resize(bod->getNumLinks()+1); - - bod->clearForcesAndTorques(); - bod->addBaseForce(m_gravity * bod->getBaseMass()); - - for (int j = 0; j < bod->getNumLinks(); ++j) - { - bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); - } - - bool doNotUpdatePos = false; - - if(bod->isMultiDof()) - { - if(!bod->isUsingRK4Integration()) - bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); - else - { - // - int numDofs = bod->getNumDofs() + 6; - int numPosVars = bod->getNumPosVars() + 7; - btAlignedObjectArray scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs); - //convenience - btScalar *pMem = &scratch_r2[0]; - btScalar *scratch_q0 = pMem; pMem += numPosVars; - btScalar *scratch_qx = pMem; pMem += numPosVars; - btScalar *scratch_qd0 = pMem; pMem += numDofs; - btScalar *scratch_qd1 = pMem; pMem += numDofs; - btScalar *scratch_qd2 = pMem; pMem += numDofs; - btScalar *scratch_qd3 = pMem; pMem += numDofs; - btScalar *scratch_qdd0 = pMem; pMem += numDofs; - btScalar *scratch_qdd1 = pMem; pMem += numDofs; - btScalar *scratch_qdd2 = pMem; pMem += numDofs; - btScalar *scratch_qdd3 = pMem; pMem += numDofs; - btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]); - - ///// - //copy q0 to scratch_q0 and qd0 to scratch_qd0 - scratch_q0[0] = bod->getWorldToBaseRot().x(); - scratch_q0[1] = bod->getWorldToBaseRot().y(); - scratch_q0[2] = bod->getWorldToBaseRot().z(); - scratch_q0[3] = bod->getWorldToBaseRot().w(); - scratch_q0[4] = bod->getBasePos().x(); - scratch_q0[5] = bod->getBasePos().y(); - scratch_q0[6] = bod->getBasePos().z(); - // - for(int link = 0; link < bod->getNumLinks(); ++link) - { - for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof) - scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof]; - } - // - for(int dof = 0; dof < numDofs; ++dof) - scratch_qd0[dof] = bod->getVelocityVector()[dof]; - //// - struct - { - btMultiBody *bod; - btScalar *scratch_qx, *scratch_q0; - - void operator()() - { - for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof) - scratch_qx[dof] = scratch_q0[dof]; - } - } pResetQx = {bod, scratch_qx, scratch_q0}; - // - struct - { - void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size) - { - for(int i = 0; i < size; ++i) - pVal[i] = pCurVal[i] + dt * pDer[i]; - } - - } pEulerIntegrate; - // - struct - { - void operator()(btMultiBody *pBody, const btScalar *pData) - { - btScalar *pVel = const_cast(pBody->getVelocityVector()); - - for(int i = 0; i < pBody->getNumDofs() + 6; ++i) - pVel[i] = pData[i]; - - } - } pCopyToVelocityVector; - // - struct - { - void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size) - { - for(int i = 0; i < size; ++i) - pDst[i] = pSrc[start + i]; - } - } pCopy; - // - - btScalar h = solverInfo.m_timeStep; - #define output &scratch_r[bod->getNumDofs()] - //calc qdd0 from: q0 & qd0 - bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); - pCopy(output, scratch_qdd0, 0, numDofs); - //calc q1 = q0 + h/2 * qd0 - pResetQx(); - bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0); - //calc qd1 = qd0 + h/2 * qdd0 - pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs); - // - //calc qdd1 from: q1 & qd1 - pCopyToVelocityVector(bod, scratch_qd1); - bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); - pCopy(output, scratch_qdd1, 0, numDofs); - //calc q2 = q0 + h/2 * qd1 - pResetQx(); - bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1); - //calc qd2 = qd0 + h/2 * qdd1 - pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs); - // - //calc qdd2 from: q2 & qd2 - pCopyToVelocityVector(bod, scratch_qd2); - bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); - pCopy(output, scratch_qdd2, 0, numDofs); - //calc q3 = q0 + h * qd2 - pResetQx(); - bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2); - //calc qd3 = qd0 + h * qdd2 - pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs); - // - //calc qdd3 from: q3 & qd3 - pCopyToVelocityVector(bod, scratch_qd3); - bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); - pCopy(output, scratch_qdd3, 0, numDofs); - - // - //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3) - //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3) - btAlignedObjectArray delta_q; delta_q.resize(numDofs); - btAlignedObjectArray delta_qd; delta_qd.resize(numDofs); - for(int i = 0; i < numDofs; ++i) - { - delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]); - delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]); - //delta_q[i] = h*scratch_qd0[i]; - //delta_qd[i] = h*scratch_qdd0[i]; - } - // - pCopyToVelocityVector(bod, scratch_qd0); - bod->applyDeltaVeeMultiDof(&delta_qd[0], 1); - // - if(!doNotUpdatePos) - { - btScalar *pRealBuf = const_cast(bod->getVelocityVector()); - pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs(); - - for(int i = 0; i < numDofs; ++i) - pRealBuf[i] = delta_q[i]; - - //bod->stepPositionsMultiDof(1, 0, &delta_q[0]); - bod->__posUpdated = true; - } - - //ugly hack which resets the cached data to t0 (needed for constraint solver) - { - for(int link = 0; link < bod->getNumLinks(); ++link) - bod->getLink(link).updateCacheMultiDof(); - bod->stepVelocitiesMultiDof(0, scratch_r, scratch_v, scratch_m); - } - - } - } - else - bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); - } - } - } - - m_solverMultiBodyIslandCallback->processConstraints(); - - m_constraintSolver->allSolved(solverInfo, m_debugDrawer); - -} - -void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) -{ - btDiscreteDynamicsWorld::integrateTransforms(timeStep); - - { - BT_PROFILE("btMultiBody stepPositions"); - //integrate and update the Featherstone hierarchies - btAlignedObjectArray world_to_local; - btAlignedObjectArray local_origin; - - for (int b=0;bgetBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b=0;bgetNumLinks();b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) - isSleeping = true; - } - - - if (!isSleeping) - { - int nLinks = bod->getNumLinks(); - - ///base + num m_links - world_to_local.resize(nLinks+1); - local_origin.resize(nLinks+1); - - if(bod->isMultiDof()) - { - if(!bod->__posUpdated) - bod->stepPositionsMultiDof(timeStep); - else - { - btScalar *pRealBuf = const_cast(bod->getVelocityVector()); - pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs(); - - bod->stepPositionsMultiDof(1, 0, pRealBuf); - bod->__posUpdated = false; - } - } - else - bod->stepPositions(timeStep); - - world_to_local[0] = bod->getWorldToBaseRot(); - local_origin[0] = bod->getBasePos(); - - if (bod->getBaseCollider()) - { - btVector3 posr = local_origin[0]; - float pos[4]={posr.x(),posr.y(),posr.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()}; - btTransform tr; - tr.setIdentity(); - tr.setOrigin(posr); - tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); - - bod->getBaseCollider()->setWorldTransform(tr); - - } - - for (int k=0;kgetNumLinks();k++) - { - const int parent = bod->getParent(k); - world_to_local[k+1] = bod->getParentToLocalRot(k) * world_to_local[parent+1]; - local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , bod->getRVector(k))); - } - - - for (int m=0;mgetNumLinks();m++) - { - btMultiBodyLinkCollider* col = bod->getLink(m).m_collider; - if (col) - { - int link = col->m_link; - btAssert(link == m); - - int index = link+1; - - btVector3 posr = local_origin[index]; - float pos[4]={posr.x(),posr.y(),posr.z(),1}; - float quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()}; - btTransform tr; - tr.setIdentity(); - tr.setOrigin(posr); - tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); - - col->setWorldTransform(tr); - } - } - } else - { - bod->clearVelocities(); - } - } - } -} - - - -void btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint) -{ - m_multiBodyConstraints.push_back(constraint); -} - -void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint) -{ - m_multiBodyConstraints.remove(constraint); -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btMultiBodyDynamicsWorld.h" +#include "btMultiBodyConstraintSolver.h" +#include "btMultiBody.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" +#include "LinearMath/btQuickprof.h" +#include "btMultiBodyConstraint.h" + + + + +void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask) +{ + m_multiBodies.push_back(body); + +} + +void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body) +{ + m_multiBodies.remove(body); +} + +void btMultiBodyDynamicsWorld::calculateSimulationIslands() +{ + BT_PROFILE("calculateSimulationIslands"); + + getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); + + { + //merge islands based on speculative contact manifolds too + for (int i=0;im_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()))) + { + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); + } + } + } + + { + int i; + int numConstraints = int(m_constraints.size()); + for (i=0;i< numConstraints ; i++ ) + { + btTypedConstraint* constraint = m_constraints[i]; + if (constraint->isEnabled()) + { + const btRigidBody* colObj0 = &constraint->getRigidBodyA(); + const btRigidBody* colObj1 = &constraint->getRigidBodyB(); + + if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && + ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) + { + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); + } + } + } + } + + //merge islands linked by Featherstone link colliders + for (int i=0;igetBaseCollider(); + + for (int b=0;bgetNumLinks();b++) + { + btMultiBodyLinkCollider* cur = body->getLink(b).m_collider; + + if (((cur) && (!(cur)->isStaticOrKinematicObject())) && + ((prev) && (!(prev)->isStaticOrKinematicObject()))) + { + int tagPrev = prev->getIslandTag(); + int tagCur = cur->getIslandTag(); + getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur); + } + if (cur && !cur->isStaticOrKinematicObject()) + prev = cur; + + } + } + } + + //merge islands linked by multibody constraints + { + for (int i=0;im_multiBodyConstraints.size();i++) + { + btMultiBodyConstraint* c = m_multiBodyConstraints[i]; + int tagA = c->getIslandIdA(); + int tagB = c->getIslandIdB(); + if (tagA>=0 && tagB>=0) + getSimulationIslandManager()->getUnionFind().unite(tagA, tagB); + } + } + + //Store the island id in each body + getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); + +} + + +void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) +{ + BT_PROFILE("btMultiBodyDynamicsWorld::updateActivationState"); + + + + for ( int i=0;icheckMotionAndSleepIfRequired(timeStep); + if (!body->isAwake()) + { + btMultiBodyLinkCollider* col = body->getBaseCollider(); + if (col && col->getActivationState() == ACTIVE_TAG) + { + col->setActivationState( WANTS_DEACTIVATION); + col->setDeactivationTime(0.f); + } + for (int b=0;bgetNumLinks();b++) + { + btMultiBodyLinkCollider* col = body->getLink(b).m_collider; + if (col && col->getActivationState() == ACTIVE_TAG) + { + col->setActivationState( WANTS_DEACTIVATION); + col->setDeactivationTime(0.f); + } + } + } else + { + btMultiBodyLinkCollider* col = body->getBaseCollider(); + if (col && col->getActivationState() != DISABLE_DEACTIVATION) + col->setActivationState( ACTIVE_TAG ); + + for (int b=0;bgetNumLinks();b++) + { + btMultiBodyLinkCollider* col = body->getLink(b).m_collider; + if (col && col->getActivationState() != DISABLE_DEACTIVATION) + col->setActivationState( ACTIVE_TAG ); + } + } + + } + } + + btDiscreteDynamicsWorld::updateActivationState(timeStep); +} + + +SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs) +{ + int islandId; + + const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); + const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); + islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag(); + return islandId; + +} + + +class btSortConstraintOnIslandPredicate2 +{ + public: + + bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const + { + int rIslandId0,lIslandId0; + rIslandId0 = btGetConstraintIslandId2(rhs); + lIslandId0 = btGetConstraintIslandId2(lhs); + return lIslandId0 < rIslandId0; + } +}; + + + +SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs) +{ + int islandId; + + int islandTagA = lhs->getIslandIdA(); + int islandTagB = lhs->getIslandIdB(); + islandId= islandTagA>=0?islandTagA:islandTagB; + return islandId; + +} + + +class btSortMultiBodyConstraintOnIslandPredicate +{ + public: + + bool operator() ( const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs ) const + { + int rIslandId0,lIslandId0; + rIslandId0 = btGetMultiBodyConstraintIslandId(rhs); + lIslandId0 = btGetMultiBodyConstraintIslandId(lhs); + return lIslandId0 < rIslandId0; + } +}; + +struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback +{ + btContactSolverInfo* m_solverInfo; + btMultiBodyConstraintSolver* m_solver; + btMultiBodyConstraint** m_multiBodySortedConstraints; + int m_numMultiBodyConstraints; + + btTypedConstraint** m_sortedConstraints; + int m_numConstraints; + btIDebugDraw* m_debugDrawer; + btDispatcher* m_dispatcher; + + btAlignedObjectArray m_bodies; + btAlignedObjectArray m_manifolds; + btAlignedObjectArray m_constraints; + btAlignedObjectArray m_multiBodyConstraints; + + + MultiBodyInplaceSolverIslandCallback( btMultiBodyConstraintSolver* solver, + btDispatcher* dispatcher) + :m_solverInfo(NULL), + m_solver(solver), + m_multiBodySortedConstraints(NULL), + m_numConstraints(0), + m_debugDrawer(NULL), + m_dispatcher(dispatcher) + { + + } + + MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other) + { + btAssert(0); + (void)other; + return *this; + } + + SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer) + { + btAssert(solverInfo); + m_solverInfo = solverInfo; + + m_multiBodySortedConstraints = sortedMultiBodyConstraints; + m_numMultiBodyConstraints = numMultiBodyConstraints; + m_sortedConstraints = sortedConstraints; + m_numConstraints = numConstraints; + + m_debugDrawer = debugDrawer; + m_bodies.resize (0); + m_manifolds.resize (0); + m_constraints.resize (0); + m_multiBodyConstraints.resize(0); + } + + + virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) + { + if (islandId<0) + { + ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id + m_solver->solveMultiBodyGroup( bodies,numBodies,manifolds, numManifolds,m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); + } else + { + //also add all non-contact constraints/joints for this island + btTypedConstraint** startConstraint = 0; + btMultiBodyConstraint** startMultiBodyConstraint = 0; + + int numCurConstraints = 0; + int numCurMultiBodyConstraints = 0; + + int i; + + //find the first constraint for this island + + for (i=0;im_minimumSolverBatchSize<=1) + { + m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); + } else + { + + for (i=0;im_solverInfo->m_minimumSolverBatchSize) + { + processConstraints(); + } else + { + //printf("deferred\n"); + } + } + } + } + void processConstraints() + { + + 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; + btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0; + + //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size()); + + m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher); + m_bodies.resize(0); + m_manifolds.resize(0); + m_constraints.resize(0); + m_multiBodyConstraints.resize(0); + } + +}; + + + +btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) + :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration), + m_multiBodyConstraintSolver(constraintSolver) +{ + //split impulse is not yet supported for Featherstone hierarchies + getSolverInfo().m_splitImpulse = false; + getSolverInfo().m_solverMode |=SOLVER_USE_2_FRICTION_DIRECTIONS; + m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver,dispatcher); +} + +btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld () +{ + delete m_solverMultiBodyIslandCallback; +} + +void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) +{ + + btAlignedObjectArray scratch_r; + btAlignedObjectArray scratch_v; + btAlignedObjectArray scratch_m; + + + BT_PROFILE("solveConstraints"); + + m_sortedConstraints.resize( m_constraints.size()); + int i; + for (i=0;isetup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer()); + m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); + + /// solve all the constraints for this island + m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback); + + + { + BT_PROFILE("btMultiBody addForce and stepVelocities"); + for (int i=0;im_multiBodies.size();i++) + { + btMultiBody* bod = m_multiBodies[i]; + + bool isSleeping = false; + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b=0;bgetNumLinks();b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) + scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd) + scratch_v.resize(bod->getNumLinks()+1); + scratch_m.resize(bod->getNumLinks()+1); + + bod->clearForcesAndTorques(); + bod->addBaseForce(m_gravity * bod->getBaseMass()); + + for (int j = 0; j < bod->getNumLinks(); ++j) + { + bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); + } + + bool doNotUpdatePos = false; + + if(bod->isMultiDof()) + { + if(!bod->isUsingRK4Integration()) + bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); + else + { + // + int numDofs = bod->getNumDofs() + 6; + int numPosVars = bod->getNumPosVars() + 7; + btAlignedObjectArray scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs); + //convenience + btScalar *pMem = &scratch_r2[0]; + btScalar *scratch_q0 = pMem; pMem += numPosVars; + btScalar *scratch_qx = pMem; pMem += numPosVars; + btScalar *scratch_qd0 = pMem; pMem += numDofs; + btScalar *scratch_qd1 = pMem; pMem += numDofs; + btScalar *scratch_qd2 = pMem; pMem += numDofs; + btScalar *scratch_qd3 = pMem; pMem += numDofs; + btScalar *scratch_qdd0 = pMem; pMem += numDofs; + btScalar *scratch_qdd1 = pMem; pMem += numDofs; + btScalar *scratch_qdd2 = pMem; pMem += numDofs; + btScalar *scratch_qdd3 = pMem; pMem += numDofs; + btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]); + + ///// + //copy q0 to scratch_q0 and qd0 to scratch_qd0 + scratch_q0[0] = bod->getWorldToBaseRot().x(); + scratch_q0[1] = bod->getWorldToBaseRot().y(); + scratch_q0[2] = bod->getWorldToBaseRot().z(); + scratch_q0[3] = bod->getWorldToBaseRot().w(); + scratch_q0[4] = bod->getBasePos().x(); + scratch_q0[5] = bod->getBasePos().y(); + scratch_q0[6] = bod->getBasePos().z(); + // + for(int link = 0; link < bod->getNumLinks(); ++link) + { + for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof) + scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof]; + } + // + for(int dof = 0; dof < numDofs; ++dof) + scratch_qd0[dof] = bod->getVelocityVector()[dof]; + //// + struct + { + btMultiBody *bod; + btScalar *scratch_qx, *scratch_q0; + + void operator()() + { + for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof) + scratch_qx[dof] = scratch_q0[dof]; + } + } pResetQx = {bod, scratch_qx, scratch_q0}; + // + struct + { + void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size) + { + for(int i = 0; i < size; ++i) + pVal[i] = pCurVal[i] + dt * pDer[i]; + } + + } pEulerIntegrate; + // + struct + { + void operator()(btMultiBody *pBody, const btScalar *pData) + { + btScalar *pVel = const_cast(pBody->getVelocityVector()); + + for(int i = 0; i < pBody->getNumDofs() + 6; ++i) + pVel[i] = pData[i]; + + } + } pCopyToVelocityVector; + // + struct + { + void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size) + { + for(int i = 0; i < size; ++i) + pDst[i] = pSrc[start + i]; + } + } pCopy; + // + + btScalar h = solverInfo.m_timeStep; + #define output &scratch_r[bod->getNumDofs()] + //calc qdd0 from: q0 & qd0 + bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); + pCopy(output, scratch_qdd0, 0, numDofs); + //calc q1 = q0 + h/2 * qd0 + pResetQx(); + bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0); + //calc qd1 = qd0 + h/2 * qdd0 + pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs); + // + //calc qdd1 from: q1 & qd1 + pCopyToVelocityVector(bod, scratch_qd1); + bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); + pCopy(output, scratch_qdd1, 0, numDofs); + //calc q2 = q0 + h/2 * qd1 + pResetQx(); + bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1); + //calc qd2 = qd0 + h/2 * qdd1 + pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs); + // + //calc qdd2 from: q2 & qd2 + pCopyToVelocityVector(bod, scratch_qd2); + bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); + pCopy(output, scratch_qdd2, 0, numDofs); + //calc q3 = q0 + h * qd2 + pResetQx(); + bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2); + //calc qd3 = qd0 + h * qdd2 + pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs); + // + //calc qdd3 from: q3 & qd3 + pCopyToVelocityVector(bod, scratch_qd3); + bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); + pCopy(output, scratch_qdd3, 0, numDofs); + + // + //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3) + //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3) + btAlignedObjectArray delta_q; delta_q.resize(numDofs); + btAlignedObjectArray delta_qd; delta_qd.resize(numDofs); + for(int i = 0; i < numDofs; ++i) + { + delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]); + delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]); + //delta_q[i] = h*scratch_qd0[i]; + //delta_qd[i] = h*scratch_qdd0[i]; + } + // + pCopyToVelocityVector(bod, scratch_qd0); + bod->applyDeltaVeeMultiDof(&delta_qd[0], 1); + // + if(!doNotUpdatePos) + { + btScalar *pRealBuf = const_cast(bod->getVelocityVector()); + pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs(); + + for(int i = 0; i < numDofs; ++i) + pRealBuf[i] = delta_q[i]; + + //bod->stepPositionsMultiDof(1, 0, &delta_q[0]); + bod->setPosUpdated(true); + } + + //ugly hack which resets the cached data to t0 (needed for constraint solver) + { + for(int link = 0; link < bod->getNumLinks(); ++link) + bod->getLink(link).updateCacheMultiDof(); + bod->stepVelocitiesMultiDof(0, scratch_r, scratch_v, scratch_m); + } + + } + } + else + bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); + } + } + } + + m_solverMultiBodyIslandCallback->processConstraints(); + + m_constraintSolver->allSolved(solverInfo, m_debugDrawer); + +} + +void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) +{ + btDiscreteDynamicsWorld::integrateTransforms(timeStep); + + { + BT_PROFILE("btMultiBody stepPositions"); + //integrate and update the Featherstone hierarchies + btAlignedObjectArray world_to_local; + btAlignedObjectArray local_origin; + + for (int b=0;bgetBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b=0;bgetNumLinks();b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) + isSleeping = true; + } + + + if (!isSleeping) + { + int nLinks = bod->getNumLinks(); + + ///base + num m_links + world_to_local.resize(nLinks+1); + local_origin.resize(nLinks+1); + + if(bod->isMultiDof()) + { + if(!bod->isPosUpdated()) + bod->stepPositionsMultiDof(timeStep); + else + { + btScalar *pRealBuf = const_cast(bod->getVelocityVector()); + pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs(); + + bod->stepPositionsMultiDof(1, 0, pRealBuf); + bod->setPosUpdated(false); + } + } + else + bod->stepPositions(timeStep); + + world_to_local[0] = bod->getWorldToBaseRot(); + local_origin[0] = bod->getBasePos(); + + if (bod->getBaseCollider()) + { + btVector3 posr = local_origin[0]; + // float pos[4]={posr.x(),posr.y(),posr.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()}; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + + bod->getBaseCollider()->setWorldTransform(tr); + + } + + for (int k=0;kgetNumLinks();k++) + { + const int parent = bod->getParent(k); + world_to_local[k+1] = bod->getParentToLocalRot(k) * world_to_local[parent+1]; + local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , bod->getRVector(k))); + } + + + for (int m=0;mgetNumLinks();m++) + { + btMultiBodyLinkCollider* col = bod->getLink(m).m_collider; + if (col) + { + int link = col->m_link; + btAssert(link == m); + + int index = link+1; + + btVector3 posr = local_origin[index]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + float quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()}; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + + col->setWorldTransform(tr); + } + } + } else + { + bod->clearVelocities(); + } + } + } +} + + + +void btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint) +{ + m_multiBodyConstraints.push_back(constraint); +} + +void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint) +{ + m_multiBodyConstraints.remove(constraint); +} diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index a4d1a1161..ad57a346d 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -1,56 +1,56 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MULTIBODY_DYNAMICS_WORLD_H -#define BT_MULTIBODY_DYNAMICS_WORLD_H - -#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" - - -class btMultiBody; -class btMultiBodyConstraint; -class btMultiBodyConstraintSolver; -struct MultiBodyInplaceSolverIslandCallback; - -///The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet -///This implementation is still preliminary/experimental. -class btMultiBodyDynamicsWorld : public btDiscreteDynamicsWorld -{ -protected: - btAlignedObjectArray m_multiBodies; - btAlignedObjectArray m_multiBodyConstraints; - btAlignedObjectArray m_sortedMultiBodyConstraints; - btMultiBodyConstraintSolver* m_multiBodyConstraintSolver; - MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback; - - virtual void calculateSimulationIslands(); - virtual void updateActivationState(btScalar timeStep); - virtual void solveConstraints(btContactSolverInfo& solverInfo); - virtual void integrateTransforms(btScalar timeStep); -public: - - btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); - - virtual ~btMultiBodyDynamicsWorld (); - - virtual void addMultiBody(btMultiBody* body, short group= btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter); - - virtual void removeMultiBody(btMultiBody* body); - - virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint); - - virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint); -}; -#endif //BT_MULTIBODY_DYNAMICS_WORLD_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_MULTIBODY_DYNAMICS_WORLD_H +#define BT_MULTIBODY_DYNAMICS_WORLD_H + +#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" + + +class btMultiBody; +class btMultiBodyConstraint; +class btMultiBodyConstraintSolver; +struct MultiBodyInplaceSolverIslandCallback; + +///The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet +///This implementation is still preliminary/experimental. +class btMultiBodyDynamicsWorld : public btDiscreteDynamicsWorld +{ +protected: + btAlignedObjectArray m_multiBodies; + btAlignedObjectArray m_multiBodyConstraints; + btAlignedObjectArray m_sortedMultiBodyConstraints; + btMultiBodyConstraintSolver* m_multiBodyConstraintSolver; + MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback; + + virtual void calculateSimulationIslands(); + virtual void updateActivationState(btScalar timeStep); + virtual void solveConstraints(btContactSolverInfo& solverInfo); + virtual void integrateTransforms(btScalar timeStep); +public: + + btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); + + virtual ~btMultiBodyDynamicsWorld (); + + virtual void addMultiBody(btMultiBody* body, short group= btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter); + + virtual void removeMultiBody(btMultiBody* body); + + virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint); + + virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint); +}; +#endif //BT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp index b9658731b..778594b40 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -1,145 +1,145 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///This file was written by Erwin Coumans - -#include "btMultiBodyJointLimitConstraint.h" -#include "btMultiBody.h" -#include "btMultiBodyLinkCollider.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" - - -btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper) - //:btMultiBodyConstraint(body,0,link,-1,2,true), - :btMultiBodyConstraint(body,body,link,link,2,true), - m_lowerBound(lower), - m_upperBound(upper) -{ - // the data.m_jacobians never change, so may as well - // initialize them here - - // note: we rely on the fact that data.m_jacobians are - // always initialized to zero by the Constraint ctor - - unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset : link); - - // row 0: the lower bound - jacobianA(0)[offset] = 1; - // row 1: the upper bound - //jacobianA(1)[offset] = -1; - - jacobianB(1)[offset] = -1; -} -btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint() -{ -} - -int btMultiBodyJointLimitConstraint::getIslandIdA() const -{ - if(m_bodyA) - { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;igetNumLinks();i++) - { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); - } - } - return -1; -} - -int btMultiBodyJointLimitConstraint::getIslandIdB() const -{ - if(m_bodyB) - { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - - for (int i=0;igetNumLinks();i++) - { - col = m_bodyB->getLink(i).m_collider; - if (col) - return col->getIslandTag(); - } - } - return -1; -} - - -void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal) -{ - // only positions need to be updated -- data.m_jacobians and force - // directions were set in the ctor and never change. - - // row 0: the lower bound - setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent - - // row 1: the upper bound - setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA)); - - for (int row=0;row infoGlobal.m_splitImpulsePenetrationThreshold)) - { - erp = infoGlobal.m_erp; - } - if (penetration>0) - { - positionalError = 0; - velocityError = -penetration / infoGlobal.m_timeStep; - } else - { - positionalError = -penetration * erp/infoGlobal.m_timeStep; - } - - btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - //combine position and velocity into rhs - constraintRow.m_rhs = penetrationImpulse+velocityImpulse; - constraintRow.m_rhsPenetration = 0.f; - - } else - { - //split position and velocity into rhs and m_rhsPenetration - constraintRow.m_rhs = velocityImpulse; - constraintRow.m_rhsPenetration = penetrationImpulse; - } - } - } - -} - - - - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#include "btMultiBodyJointLimitConstraint.h" +#include "btMultiBody.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + + +btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper) + //:btMultiBodyConstraint(body,0,link,-1,2,true), + :btMultiBodyConstraint(body,body,link,link,2,true), + m_lowerBound(lower), + m_upperBound(upper) +{ + // the data.m_jacobians never change, so may as well + // initialize them here + + // note: we rely on the fact that data.m_jacobians are + // always initialized to zero by the Constraint ctor + + unsigned int offset = 6 + (body->isMultiDof() ? body->getLink(link).m_dofOffset : link); + + // row 0: the lower bound + jacobianA(0)[offset] = 1; + // row 1: the upper bound + //jacobianA(1)[offset] = -1; + + jacobianB(1)[offset] = -1; +} +btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint() +{ +} + +int btMultiBodyJointLimitConstraint::getIslandIdA() const +{ + if(m_bodyA) + { + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + for (int i=0;igetNumLinks();i++) + { + if (m_bodyA->getLink(i).m_collider) + return m_bodyA->getLink(i).m_collider->getIslandTag(); + } + } + return -1; +} + +int btMultiBodyJointLimitConstraint::getIslandIdB() const +{ + if(m_bodyB) + { + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); + if (col) + return col->getIslandTag(); + + for (int i=0;igetNumLinks();i++) + { + col = m_bodyB->getLink(i).m_collider; + if (col) + return col->getIslandTag(); + } + } + return -1; +} + + +void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal) +{ + // only positions need to be updated -- data.m_jacobians and force + // directions were set in the ctor and never change. + + // row 0: the lower bound + setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent + + // row 1: the upper bound + setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA)); + + for (int row=0;row infoGlobal.m_splitImpulsePenetrationThreshold)) + { + erp = infoGlobal.m_erp; + } + if (penetration>0) + { + positionalError = 0; + velocityError = -penetration / infoGlobal.m_timeStep; + } else + { + positionalError = -penetration * erp/infoGlobal.m_timeStep; + } + + btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + //combine position and velocity into rhs + constraintRow.m_rhs = penetrationImpulse+velocityImpulse; + constraintRow.m_rhsPenetration = 0.f; + + } else + { + //split position and velocity into rhs and m_rhsPenetration + constraintRow.m_rhs = velocityImpulse; + constraintRow.m_rhsPenetration = penetrationImpulse; + } + } + } + +} + + + + diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h index 563ef4b2e..0c7fc1708 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h @@ -1,44 +1,44 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H -#define BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H - -#include "btMultiBodyConstraint.h" -struct btSolverInfo; - -class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint -{ -protected: - - btScalar m_lowerBound; - btScalar m_upperBound; -public: - - btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper); - virtual ~btMultiBodyJointLimitConstraint(); - - virtual int getIslandIdA() const; - virtual int getIslandIdB() const; - - virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal); - - -}; - -#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H +#define BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H + +#include "btMultiBodyConstraint.h" +struct btSolverInfo; + +class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint +{ +protected: + + btScalar m_lowerBound; + btScalar m_upperBound; +public: + + btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper); + virtual ~btMultiBodyJointLimitConstraint(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + +}; + +#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H + diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index 18f49533b..29d505ebe 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/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); } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index 9e90c6f48..af95cd992 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/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() diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h b/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h index f5622e062..5080ea874 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h @@ -1,92 +1,92 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_FEATHERSTONE_LINK_COLLIDER_H -#define BT_FEATHERSTONE_LINK_COLLIDER_H - -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" - -#include "btMultiBody.h" - -class btMultiBodyLinkCollider : public btCollisionObject -{ -//protected: -public: - - btMultiBody* m_multiBody; - int m_link; - - - btMultiBodyLinkCollider (btMultiBody* multiBody,int link) - :m_multiBody(multiBody), - m_link(link) - { - m_checkCollideWith = true; - //we need to remove the 'CF_STATIC_OBJECT' flag, otherwise links/base doesn't merge islands - //this means that some constraints might point to bodies that are not in the islands, causing crashes - //if (link>=0 || (multiBody && !multiBody->hasFixedBase())) - { - m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT); - } - // else - //{ - // m_collisionFlags |= (btCollisionObject::CF_STATIC_OBJECT); - //} - - m_internalType = CO_FEATHERSTONE_LINK; - } - static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj) - { - if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK) - return (btMultiBodyLinkCollider*)colObj; - return 0; - } - static const btMultiBodyLinkCollider* upcast(const btCollisionObject* colObj) - { - if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK) - return (btMultiBodyLinkCollider*)colObj; - return 0; - } - - virtual bool checkCollideWithOverride(const btCollisionObject* co) const - { - const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co); - if (!other) - return true; - if (other->m_multiBody != this->m_multiBody) - return true; - if (!m_multiBody->hasSelfCollision()) - return false; - - //check if 'link' has collision disabled - if (m_link>=0) - { - const btMultibodyLink& link = m_multiBody->getLink(this->m_link); - if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.m_parent == other->m_link) - return false; - } - - if (other->m_link>=0) - { - const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link); - if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.m_parent == this->m_link) - return false; - } - return true; - } -}; - -#endif //BT_FEATHERSTONE_LINK_COLLIDER_H - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_FEATHERSTONE_LINK_COLLIDER_H +#define BT_FEATHERSTONE_LINK_COLLIDER_H + +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + +#include "btMultiBody.h" + +class btMultiBodyLinkCollider : public btCollisionObject +{ +//protected: +public: + + btMultiBody* m_multiBody; + int m_link; + + + btMultiBodyLinkCollider (btMultiBody* multiBody,int link) + :m_multiBody(multiBody), + m_link(link) + { + m_checkCollideWith = true; + //we need to remove the 'CF_STATIC_OBJECT' flag, otherwise links/base doesn't merge islands + //this means that some constraints might point to bodies that are not in the islands, causing crashes + //if (link>=0 || (multiBody && !multiBody->hasFixedBase())) + { + m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT); + } + // else + //{ + // m_collisionFlags |= (btCollisionObject::CF_STATIC_OBJECT); + //} + + m_internalType = CO_FEATHERSTONE_LINK; + } + static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj) + { + if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK) + return (btMultiBodyLinkCollider*)colObj; + return 0; + } + static const btMultiBodyLinkCollider* upcast(const btCollisionObject* colObj) + { + if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK) + return (btMultiBodyLinkCollider*)colObj; + return 0; + } + + virtual bool checkCollideWithOverride(const btCollisionObject* co) const + { + const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co); + if (!other) + return true; + if (other->m_multiBody != this->m_multiBody) + return true; + if (!m_multiBody->hasSelfCollision()) + return false; + + //check if 'link' has collision disabled + if (m_link>=0) + { + const btMultibodyLink& link = m_multiBody->getLink(this->m_link); + if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.m_parent == other->m_link) + return false; + } + + if (other->m_link>=0) + { + const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link); + if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.m_parent == this->m_link) + return false; + } + return true; + } +}; + +#endif //BT_FEATHERSTONE_LINK_COLLIDER_H + diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp index d2ebf29ef..d7dbf5230 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ b/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; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h index c9bfbc176..1295757a5 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h @@ -1,62 +1,62 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///This file was written by Erwin Coumans - -#ifndef BT_MULTIBODY_POINT2POINT_H -#define BT_MULTIBODY_POINT2POINT_H - -#include "btMultiBodyConstraint.h" - -//#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST - -class btMultiBodyPoint2Point : public btMultiBodyConstraint -{ -protected: - - btRigidBody* m_rigidBodyA; - btRigidBody* m_rigidBodyB; - btVector3 m_pivotInA; - btVector3 m_pivotInB; - - -public: - - btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB); - btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB); - - virtual ~btMultiBodyPoint2Point(); - - virtual int getIslandIdA() const; - virtual int getIslandIdB() const; - - virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal); - - const btVector3& getPivotInB() const - { - return m_pivotInB; - } - - void setPivotInB(const btVector3& pivotInB) - { - m_pivotInB = pivotInB; - } - - -}; - -#endif //BT_MULTIBODY_POINT2POINT_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#ifndef BT_MULTIBODY_POINT2POINT_H +#define BT_MULTIBODY_POINT2POINT_H + +#include "btMultiBodyConstraint.h" + +//#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST + +class btMultiBodyPoint2Point : public btMultiBodyConstraint +{ +protected: + + btRigidBody* m_rigidBodyA; + btRigidBody* m_rigidBodyB; + btVector3 m_pivotInA; + btVector3 m_pivotInB; + + +public: + + btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB); + btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB); + + virtual ~btMultiBodyPoint2Point(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + const btVector3& getPivotInB() const + { + return m_pivotInB; + } + + void setPivotInB(const btVector3& pivotInB) + { + m_pivotInB = pivotInB; + } + + +}; + +#endif //BT_MULTIBODY_POINT2POINT_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h index 45e31a176..7bc64569a 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h @@ -1,85 +1,85 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MULTIBODY_SOLVER_CONSTRAINT_H -#define BT_MULTIBODY_SOLVER_CONSTRAINT_H - -#include "LinearMath/btVector3.h" -#include "LinearMath/btAlignedObjectArray.h" - -class btMultiBody; -#include "BulletDynamics/ConstraintSolver/btSolverBody.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" - -///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. -ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint -{ - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_solverBodyIdB(-1), m_multiBodyA(0), m_multiBodyB(0), m_linkA(-1), m_linkB(-1) - {} - - int m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1 - int m_jacAindex; - int m_deltaVelBindex; - int m_jacBindex; - - btVector3 m_relpos1CrossNormal; - btVector3 m_contactNormal1; - btVector3 m_relpos2CrossNormal; - btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always - - - btVector3 m_angularComponentA; - btVector3 m_angularComponentB; - - mutable btSimdScalar m_appliedPushImpulse; - mutable btSimdScalar m_appliedImpulse; - - btScalar m_friction; - btScalar m_jacDiagABInv; - btScalar m_rhs; - btScalar m_cfm; - - btScalar m_lowerLimit; - btScalar m_upperLimit; - btScalar m_rhsPenetration; - union - { - void* m_originalContactPoint; - btScalar m_unusedPadding4; - }; - - int m_overrideNumSolverIterations; - int m_frictionIndex; - - int m_solverBodyIdA; - btMultiBody* m_multiBodyA; - int m_linkA; - - int m_solverBodyIdB; - btMultiBody* m_multiBodyB; - int m_linkB; - - enum btSolverConstraintType - { - BT_SOLVER_CONTACT_1D = 0, - BT_SOLVER_FRICTION_1D - }; -}; - -typedef btAlignedObjectArray btMultiBodyConstraintArray; - -#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_MULTIBODY_SOLVER_CONSTRAINT_H +#define BT_MULTIBODY_SOLVER_CONSTRAINT_H + +#include "LinearMath/btVector3.h" +#include "LinearMath/btAlignedObjectArray.h" + +class btMultiBody; +#include "BulletDynamics/ConstraintSolver/btSolverBody.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" + +///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. +ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint +{ + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1) + {} + + int m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1 + int m_jacAindex; + int m_deltaVelBindex; + int m_jacBindex; + + btVector3 m_relpos1CrossNormal; + btVector3 m_contactNormal1; + btVector3 m_relpos2CrossNormal; + btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always + + + btVector3 m_angularComponentA; + btVector3 m_angularComponentB; + + mutable btSimdScalar m_appliedPushImpulse; + mutable btSimdScalar m_appliedImpulse; + + btScalar m_friction; + btScalar m_jacDiagABInv; + btScalar m_rhs; + btScalar m_cfm; + + btScalar m_lowerLimit; + btScalar m_upperLimit; + btScalar m_rhsPenetration; + union + { + void* m_originalContactPoint; + btScalar m_unusedPadding4; + }; + + int m_overrideNumSolverIterations; + int m_frictionIndex; + + int m_solverBodyIdA; + btMultiBody* m_multiBodyA; + int m_linkA; + + int m_solverBodyIdB; + btMultiBody* m_multiBodyB; + int m_linkB; + + enum btSolverConstraintType + { + BT_SOLVER_CONTACT_1D = 0, + BT_SOLVER_FRICTION_1D + }; +}; + +typedef btAlignedObjectArray btMultiBodyConstraintArray; + +#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H diff --git a/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp b/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp index 3bf7b5c13..c2634dff5 100644 --- a/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp +++ b/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 */ diff --git a/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h b/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h index c9ff8a9f1..2a2f2d3d3 100644 --- a/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h +++ b/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h @@ -1,112 +1,112 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///original version written by Erwin Coumans, October 2013 - -#ifndef BT_DANTZIG_SOLVER_H -#define BT_DANTZIG_SOLVER_H - -#include "btMLCPSolverInterface.h" -#include "btDantzigLCP.h" - - -class btDantzigSolver : public btMLCPSolverInterface -{ -protected: - - btScalar m_acceptableUpperLimitSolution; - - btAlignedObjectArray m_tempBuffer; - - btAlignedObjectArray m_A; - btAlignedObjectArray m_b; - btAlignedObjectArray m_x; - btAlignedObjectArray m_lo; - btAlignedObjectArray m_hi; - btAlignedObjectArray m_dependencies; - btDantzigScratchMemory m_scratchMemory; -public: - - btDantzigSolver() - :m_acceptableUpperLimitSolution(btScalar(1000)) - { - } - - virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray& limitDependency, int numIterations, bool useSparsity = true) - { - bool result = true; - int n = b.rows(); - if (n) - { - int nub = 0; - btAlignedObjectArray ww; - ww.resize(n); - - - const btScalar* Aptr = A.getBufferPointer(); - m_A.resize(n*n); - for (int i=0;i= m_acceptableUpperLimitSolution) - { - return false; - } - - if (x[i] <= -m_acceptableUpperLimitSolution) - { - return false; - } - } - - for (int i=0;i m_tempBuffer; + + btAlignedObjectArray m_A; + btAlignedObjectArray m_b; + btAlignedObjectArray m_x; + btAlignedObjectArray m_lo; + btAlignedObjectArray m_hi; + btAlignedObjectArray m_dependencies; + btDantzigScratchMemory m_scratchMemory; +public: + + btDantzigSolver() + :m_acceptableUpperLimitSolution(btScalar(1000)) + { + } + + virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray& limitDependency, int numIterations, bool useSparsity = true) + { + bool result = true; + int n = b.rows(); + if (n) + { + int nub = 0; + btAlignedObjectArray ww; + ww.resize(n); + + + const btScalar* Aptr = A.getBufferPointer(); + m_A.resize(n*n); + for (int i=0;i= m_acceptableUpperLimitSolution) + { + return false; + } + + if (x[i] <= -m_acceptableUpperLimitSolution) + { + return false; + } + } + + for (int i=0;i& limitDependency, int numIterations, bool useSparsity = true) - { - - if (m_useLoHighBounds) - { - - BT_PROFILE("btLemkeSolver::solveMLCP"); - int n = A.rows(); - if (0==n) - return true; - - bool fail = false; - - btVectorXu solution(n); - btVectorXu q1; - q1.resize(n); - for (int row=0;rowm_maxValue) - { - if (x[i]> errorValueMax) - { - fail = true; - errorIndexMax = i; - errorValueMax = x[i]; - } - ////printf("x[i] = %f,",x[i]); - } - if (x[i]<-m_maxValue) - { - if (x[i]m_maxValue) - { - if (x[i]> errorValueMax) - { - fail = true; - errorIndexMax = i; - errorValueMax = x[i]; - } - ////printf("x[i] = %f,",x[i]); - } - if (x[i]<-m_maxValue) - { - if (x[i]& limitDependency, int numIterations, bool useSparsity = true) + { + + if (m_useLoHighBounds) + { + + BT_PROFILE("btLemkeSolver::solveMLCP"); + int n = A.rows(); + if (0==n) + return true; + + bool fail = false; + + btVectorXu solution(n); + btVectorXu q1; + q1.resize(n); + for (int row=0;rowm_maxValue) + { + if (x[i]> errorValueMax) + { + fail = true; + errorIndexMax = i; + errorValueMax = x[i]; + } + ////printf("x[i] = %f,",x[i]); + } + if (x[i]<-m_maxValue) + { + if (x[i]m_maxValue) + { + if (x[i]> errorValueMax) + { + fail = true; + errorIndexMax = i; + errorValueMax = x[i]; + } + ////printf("x[i] = %f,",x[i]); + } + if (x[i]<-m_maxValue) + { + if (x[i] limitDependenciesCopy = m_limitDependencies; -// printf("solve first LCP\n"); - result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations ); - if (result) - result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo,m_hi, limitDependenciesCopy,infoGlobal.m_numIterations ); - - } else - { - result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations ); - } - return result; -} - -struct btJointNode -{ - int jointIndex; // pointer to enclosing dxJoint object - int otherBodyIndex; // *other* body this joint is connected to - int nextJointNodeIndex;//-1 for null - int constraintRowIndex; -}; - - - -void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) -{ - int numContactRows = interleaveContactAndFriction ? 3 : 1; - - int numConstraintRows = m_allConstraintPtrArray.size(); - int n = numConstraintRows; - { - BT_PROFILE("init b (rhs)"); - m_b.resize(numConstraintRows); - m_bSplit.resize(numConstraintRows); - m_b.setZero(); - m_bSplit.setZero(); - for (int i=0;im_jacDiagABInv; - if (!btFuzzyZero(jacDiag)) - { - btScalar rhs = m_allConstraintPtrArray[i]->m_rhs; - btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration; - m_b[i]=rhs/jacDiag; - m_bSplit[i] = rhsPenetration/jacDiag; - } - - } - } - - btScalar* w = 0; - int nub = 0; - - m_lo.resize(numConstraintRows); - m_hi.resize(numConstraintRows); - - { - BT_PROFILE("init lo/ho"); - - for (int i=0;i=0) - { - m_lo[i] = -BT_INFINITY; - m_hi[i] = BT_INFINITY; - } else - { - m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit; - m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; - } - } - } - - // - int m=m_allConstraintPtrArray.size(); - - int numBodies = m_tmpSolverBodyPool.size(); - btAlignedObjectArray bodyJointNodeArray; - { - BT_PROFILE("bodyJointNodeArray.resize"); - bodyJointNodeArray.resize(numBodies,-1); - } - btAlignedObjectArray jointNodeArray; - { - BT_PROFILE("jointNodeArray.reserve"); - jointNodeArray.reserve(2*m_allConstraintPtrArray.size()); - } - - static btMatrixXu J3; - { - BT_PROFILE("J3.resize"); - J3.resize(2*m,8); - } - static btMatrixXu JinvM3; - { - BT_PROFILE("JinvM3.resize/setZero"); - - JinvM3.resize(2*m,8); - JinvM3.setZero(); - J3.setZero(); - } - int cur=0; - int rowOffset = 0; - static btAlignedObjectArray ofs; - { - BT_PROFILE("ofs resize"); - ofs.resize(0); - ofs.resizeNoInitialize(m_allConstraintPtrArray.size()); - } - { - BT_PROFILE("Compute J and JinvM"); - int c=0; - - int numRows = 0; - - for (int i=0;im_solverBodyIdA; - int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; - btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; - btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; - - numRows = im_contactNormal1 * orgBodyA->getInvMass(); - btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld(); - - for (int r=0;r<3;r++) - { - J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal1[r]); - J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal[r]); - JinvM3.setElem(cur,r,normalInvMass[r]); - JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]); - } - J3.setElem(cur,3,0); - JinvM3.setElem(cur,3,0); - J3.setElem(cur,7,0); - JinvM3.setElem(cur,7,0); - } - } else - { - cur += numRows; - } - if (orgBodyB) - { - - { - int slotB=-1; - //find free jointNode slot for sbA - slotB =jointNodeArray.size(); - jointNodeArray.expand();//NonInitializing(); - int prevSlot = bodyJointNodeArray[sbB]; - bodyJointNodeArray[sbB] = slotB; - jointNodeArray[slotB].nextJointNodeIndex = prevSlot; - jointNodeArray[slotB].jointIndex = c; - jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1; - jointNodeArray[slotB].constraintRowIndex = i; - } - - for (int row=0;rowm_contactNormal2*orgBodyB->getInvMass(); - btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld(); - - for (int r=0;r<3;r++) - { - J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal2[r]); - J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal[r]); - JinvM3.setElem(cur,r,normalInvMassB[r]); - JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]); - } - J3.setElem(cur,3,0); - JinvM3.setElem(cur,3,0); - J3.setElem(cur,7,0); - JinvM3.setElem(cur,7,0); - } - } - else - { - cur += numRows; - } - rowOffset+=numRows; - - } - - } - - - //compute JinvM = J*invM. - const btScalar* JinvM = JinvM3.getBufferPointer(); - - const btScalar* Jptr = J3.getBufferPointer(); - { - BT_PROFILE("m_A.resize"); - m_A.resize(n,n); - } - - { - BT_PROFILE("m_A.setZero"); - m_A.setZero(); - } - int c=0; - { - int numRows = 0; - BT_PROFILE("Compute A"); - for (int i=0;im_solverBodyIdA; - int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; - btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; - btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; - - numRows = i=0) - { - int j0 = jointNodeArray[startJointNodeA].jointIndex; - int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex; - if (j0m_solverBodyIdB == sbA) ? 8*numRowsOther : 0; - //printf("%d joint i %d and j0: %d: ",count++,i,j0); - m_A.multiplyAdd2_p8r ( JinvMrow, - Jptr + 2*8*(size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__,ofs[j0]); - } - startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex; - } - } - - { - int startJointNodeB = bodyJointNodeArray[sbB]; - while (startJointNodeB>=0) - { - int j1 = jointNodeArray[startJointNodeB].jointIndex; - int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex; - - if (j1m_solverBodyIdB == sbB) ? 8*numRowsOther : 0; - m_A.multiplyAdd2_p8r ( JinvMrow + 8*(size_t)numRows, - Jptr + 2*8*(size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__,ofs[j1]); - } - startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex; - } - } - } - - { - BT_PROFILE("compute diagonal"); - // compute diagonal blocks of m_A - - int row__ = 0; - int numJointRows = m_allConstraintPtrArray.size(); - - int jj=0; - for (;row__m_solverBodyIdA; - int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB; - btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; - btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; - - - const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows; - - const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__; - const btScalar *Jrow = Jptr + 2*8*(size_t)row__; - m_A.multiply2_p8r (JinvMrow, Jrow, infom, infom, row__,row__); - if (orgBodyB) - { - m_A.multiplyAdd2_p8r (JinvMrow + 8*(size_t)infom, Jrow + 8*(size_t)infom, infom, infom, row__,row__); - } - row__ += infom; - jj++; - } - } - } - - if (1) - { - // add cfm to the diagonal of m_A - for ( int i=0; im_tmpSolverBodyPool.size(); - int numConstraintRows = m_allConstraintPtrArray.size(); - - m_b.resize(numConstraintRows); - if (infoGlobal.m_splitImpulse) - m_bSplit.resize(numConstraintRows); - - m_bSplit.setZero(); - m_b.setZero(); - - for (int i=0;im_jacDiagABInv) - { - m_b[i]=m_allConstraintPtrArray[i]->m_rhs/m_allConstraintPtrArray[i]->m_jacDiagABInv; - if (infoGlobal.m_splitImpulse) - m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration/m_allConstraintPtrArray[i]->m_jacDiagABInv; - } - } - - static btMatrixXu Minv; - Minv.resize(6*numBodies,6*numBodies); - Minv.setZero(); - for (int i=0;igetInvInertiaTensorWorld()[r][c] : 0); - } - - static btMatrixXu J; - J.resize(numConstraintRows,6*numBodies); - J.setZero(); - - m_lo.resize(numConstraintRows); - m_hi.resize(numConstraintRows); - - for (int i=0;im_lowerLimit; - m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; - - int bodyIndex0 = m_allConstraintPtrArray[i]->m_solverBodyIdA; - int bodyIndex1 = m_allConstraintPtrArray[i]->m_solverBodyIdB; - if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody) - { - setElem(J,i,6*bodyIndex0+0,m_allConstraintPtrArray[i]->m_contactNormal1[0]); - setElem(J,i,6*bodyIndex0+1,m_allConstraintPtrArray[i]->m_contactNormal1[1]); - setElem(J,i,6*bodyIndex0+2,m_allConstraintPtrArray[i]->m_contactNormal1[2]); - setElem(J,i,6*bodyIndex0+3,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[0]); - setElem(J,i,6*bodyIndex0+4,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[1]); - setElem(J,i,6*bodyIndex0+5,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[2]); - } - if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody) - { - setElem(J,i,6*bodyIndex1+0,m_allConstraintPtrArray[i]->m_contactNormal2[0]); - setElem(J,i,6*bodyIndex1+1,m_allConstraintPtrArray[i]->m_contactNormal2[1]); - setElem(J,i,6*bodyIndex1+2,m_allConstraintPtrArray[i]->m_contactNormal2[2]); - setElem(J,i,6*bodyIndex1+3,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[0]); - setElem(J,i,6*bodyIndex1+4,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[1]); - setElem(J,i,6*bodyIndex1+5,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[2]); - } - } - - static btMatrixXu J_transpose; - J_transpose= J.transpose(); - - static btMatrixXu tmp; - - { - { - BT_PROFILE("J*Minv"); - tmp = J*Minv; - - } - { - BT_PROFILE("J*tmp"); - m_A = tmp*J_transpose; - } - } - - if (1) - { - // add cfm to the diagonal of m_A - for ( int i=0; i limitDependenciesCopy = m_limitDependencies; +// printf("solve first LCP\n"); + result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations ); + if (result) + result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo,m_hi, limitDependenciesCopy,infoGlobal.m_numIterations ); + + } else + { + result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations ); + } + return result; +} + +struct btJointNode +{ + int jointIndex; // pointer to enclosing dxJoint object + int otherBodyIndex; // *other* body this joint is connected to + int nextJointNodeIndex;//-1 for null + int constraintRowIndex; +}; + + + +void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) +{ + int numContactRows = interleaveContactAndFriction ? 3 : 1; + + int numConstraintRows = m_allConstraintPtrArray.size(); + int n = numConstraintRows; + { + BT_PROFILE("init b (rhs)"); + m_b.resize(numConstraintRows); + m_bSplit.resize(numConstraintRows); + m_b.setZero(); + m_bSplit.setZero(); + for (int i=0;im_jacDiagABInv; + if (!btFuzzyZero(jacDiag)) + { + btScalar rhs = m_allConstraintPtrArray[i]->m_rhs; + btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration; + m_b[i]=rhs/jacDiag; + m_bSplit[i] = rhsPenetration/jacDiag; + } + + } + } + +// btScalar* w = 0; +// int nub = 0; + + m_lo.resize(numConstraintRows); + m_hi.resize(numConstraintRows); + + { + BT_PROFILE("init lo/ho"); + + for (int i=0;i=0) + { + m_lo[i] = -BT_INFINITY; + m_hi[i] = BT_INFINITY; + } else + { + m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit; + m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; + } + } + } + + // + int m=m_allConstraintPtrArray.size(); + + int numBodies = m_tmpSolverBodyPool.size(); + btAlignedObjectArray bodyJointNodeArray; + { + BT_PROFILE("bodyJointNodeArray.resize"); + bodyJointNodeArray.resize(numBodies,-1); + } + btAlignedObjectArray jointNodeArray; + { + BT_PROFILE("jointNodeArray.reserve"); + jointNodeArray.reserve(2*m_allConstraintPtrArray.size()); + } + + static btMatrixXu J3; + { + BT_PROFILE("J3.resize"); + J3.resize(2*m,8); + } + static btMatrixXu JinvM3; + { + BT_PROFILE("JinvM3.resize/setZero"); + + JinvM3.resize(2*m,8); + JinvM3.setZero(); + J3.setZero(); + } + int cur=0; + int rowOffset = 0; + static btAlignedObjectArray ofs; + { + BT_PROFILE("ofs resize"); + ofs.resize(0); + ofs.resizeNoInitialize(m_allConstraintPtrArray.size()); + } + { + BT_PROFILE("Compute J and JinvM"); + int c=0; + + int numRows = 0; + + for (int i=0;im_solverBodyIdA; + int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; + btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + + numRows = im_contactNormal1 * orgBodyA->getInvMass(); + btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld(); + + for (int r=0;r<3;r++) + { + J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal1[r]); + J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal[r]); + JinvM3.setElem(cur,r,normalInvMass[r]); + JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]); + } + J3.setElem(cur,3,0); + JinvM3.setElem(cur,3,0); + J3.setElem(cur,7,0); + JinvM3.setElem(cur,7,0); + } + } else + { + cur += numRows; + } + if (orgBodyB) + { + + { + int slotB=-1; + //find free jointNode slot for sbA + slotB =jointNodeArray.size(); + jointNodeArray.expand();//NonInitializing(); + int prevSlot = bodyJointNodeArray[sbB]; + bodyJointNodeArray[sbB] = slotB; + jointNodeArray[slotB].nextJointNodeIndex = prevSlot; + jointNodeArray[slotB].jointIndex = c; + jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1; + jointNodeArray[slotB].constraintRowIndex = i; + } + + for (int row=0;rowm_contactNormal2*orgBodyB->getInvMass(); + btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld(); + + for (int r=0;r<3;r++) + { + J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal2[r]); + J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal[r]); + JinvM3.setElem(cur,r,normalInvMassB[r]); + JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]); + } + J3.setElem(cur,3,0); + JinvM3.setElem(cur,3,0); + J3.setElem(cur,7,0); + JinvM3.setElem(cur,7,0); + } + } + else + { + cur += numRows; + } + rowOffset+=numRows; + + } + + } + + + //compute JinvM = J*invM. + const btScalar* JinvM = JinvM3.getBufferPointer(); + + const btScalar* Jptr = J3.getBufferPointer(); + { + BT_PROFILE("m_A.resize"); + m_A.resize(n,n); + } + + { + BT_PROFILE("m_A.setZero"); + m_A.setZero(); + } + int c=0; + { + int numRows = 0; + BT_PROFILE("Compute A"); + for (int i=0;im_solverBodyIdA; + int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; + // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + // btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + + numRows = i=0) + { + int j0 = jointNodeArray[startJointNodeA].jointIndex; + int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex; + if (j0m_solverBodyIdB == sbA) ? 8*numRowsOther : 0; + //printf("%d joint i %d and j0: %d: ",count++,i,j0); + m_A.multiplyAdd2_p8r ( JinvMrow, + Jptr + 2*8*(size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__,ofs[j0]); + } + startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex; + } + } + + { + int startJointNodeB = bodyJointNodeArray[sbB]; + while (startJointNodeB>=0) + { + int j1 = jointNodeArray[startJointNodeB].jointIndex; + int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex; + + if (j1m_solverBodyIdB == sbB) ? 8*numRowsOther : 0; + m_A.multiplyAdd2_p8r ( JinvMrow + 8*(size_t)numRows, + Jptr + 2*8*(size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__,ofs[j1]); + } + startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex; + } + } + } + + { + BT_PROFILE("compute diagonal"); + // compute diagonal blocks of m_A + + int row__ = 0; + int numJointRows = m_allConstraintPtrArray.size(); + + int jj=0; + for (;row__m_solverBodyIdA; + int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB; + // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + + + const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows; + + const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__; + const btScalar *Jrow = Jptr + 2*8*(size_t)row__; + m_A.multiply2_p8r (JinvMrow, Jrow, infom, infom, row__,row__); + if (orgBodyB) + { + m_A.multiplyAdd2_p8r (JinvMrow + 8*(size_t)infom, Jrow + 8*(size_t)infom, infom, infom, row__,row__); + } + row__ += infom; + jj++; + } + } + } + + if (1) + { + // add cfm to the diagonal of m_A + for ( int i=0; im_tmpSolverBodyPool.size(); + int numConstraintRows = m_allConstraintPtrArray.size(); + + m_b.resize(numConstraintRows); + if (infoGlobal.m_splitImpulse) + m_bSplit.resize(numConstraintRows); + + m_bSplit.setZero(); + m_b.setZero(); + + for (int i=0;im_jacDiagABInv) + { + m_b[i]=m_allConstraintPtrArray[i]->m_rhs/m_allConstraintPtrArray[i]->m_jacDiagABInv; + if (infoGlobal.m_splitImpulse) + m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration/m_allConstraintPtrArray[i]->m_jacDiagABInv; + } + } + + static btMatrixXu Minv; + Minv.resize(6*numBodies,6*numBodies); + Minv.setZero(); + for (int i=0;igetInvInertiaTensorWorld()[r][c] : 0); + } + + static btMatrixXu J; + J.resize(numConstraintRows,6*numBodies); + J.setZero(); + + m_lo.resize(numConstraintRows); + m_hi.resize(numConstraintRows); + + for (int i=0;im_lowerLimit; + m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; + + int bodyIndex0 = m_allConstraintPtrArray[i]->m_solverBodyIdA; + int bodyIndex1 = m_allConstraintPtrArray[i]->m_solverBodyIdB; + if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody) + { + setElem(J,i,6*bodyIndex0+0,m_allConstraintPtrArray[i]->m_contactNormal1[0]); + setElem(J,i,6*bodyIndex0+1,m_allConstraintPtrArray[i]->m_contactNormal1[1]); + setElem(J,i,6*bodyIndex0+2,m_allConstraintPtrArray[i]->m_contactNormal1[2]); + setElem(J,i,6*bodyIndex0+3,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[0]); + setElem(J,i,6*bodyIndex0+4,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[1]); + setElem(J,i,6*bodyIndex0+5,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[2]); + } + if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody) + { + setElem(J,i,6*bodyIndex1+0,m_allConstraintPtrArray[i]->m_contactNormal2[0]); + setElem(J,i,6*bodyIndex1+1,m_allConstraintPtrArray[i]->m_contactNormal2[1]); + setElem(J,i,6*bodyIndex1+2,m_allConstraintPtrArray[i]->m_contactNormal2[2]); + setElem(J,i,6*bodyIndex1+3,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[0]); + setElem(J,i,6*bodyIndex1+4,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[1]); + setElem(J,i,6*bodyIndex1+5,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[2]); + } + } + + static btMatrixXu J_transpose; + J_transpose= J.transpose(); + + static btMatrixXu tmp; + + { + { + BT_PROFILE("J*Minv"); + tmp = J*Minv; + + } + { + BT_PROFILE("J*tmp"); + m_A = tmp*J_transpose; + } + } + + if (1) + { + // add cfm to the diagonal of m_A + for ( int i=0; i m_limitDependencies; - btAlignedObjectArray m_allConstraintPtrArray; - btMLCPSolverInterface* m_solver; - int m_fallback; - btScalar m_cfm; - - virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - - - virtual void createMLCP(const btContactSolverInfo& infoGlobal); - virtual void createMLCPFast(const btContactSolverInfo& infoGlobal); - - //return true is it solves the problem successfully - virtual bool solveMLCP(const btContactSolverInfo& infoGlobal); - -public: - - btMLCPSolver( btMLCPSolverInterface* solver); - virtual ~btMLCPSolver(); - - void setMLCPSolver(btMLCPSolverInterface* solver) - { - m_solver = solver; - } - - int getNumFallbacks() const - { - return m_fallback; - } - void setNumFallbacks(int num) - { - m_fallback = num; - } - - btScalar getCfm() const - { - return m_cfm; - } - void setCfm(btScalar cfm) - { - m_cfm = cfm; - } - - virtual btConstraintSolverType getSolverType() const - { - return BT_MLCP_SOLVER; - } - -}; - - -#endif //BT_MLCP_SOLVER_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///original version written by Erwin Coumans, October 2013 + +#ifndef BT_MLCP_SOLVER_H +#define BT_MLCP_SOLVER_H + +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" +#include "LinearMath/btMatrixX.h" +#include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h" + +class btMLCPSolver : public btSequentialImpulseConstraintSolver +{ + +protected: + + btMatrixXu m_A; + btVectorXu m_b; + btVectorXu m_x; + btVectorXu m_lo; + btVectorXu m_hi; + + ///when using 'split impulse' we solve two separate (M)LCPs + btVectorXu m_bSplit; + btVectorXu m_xSplit; + btVectorXu m_bSplit1; + btVectorXu m_xSplit2; + + btAlignedObjectArray m_limitDependencies; + btAlignedObjectArray m_allConstraintPtrArray; + btMLCPSolverInterface* m_solver; + int m_fallback; + btScalar m_cfm; + + virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + + + virtual void createMLCP(const btContactSolverInfo& infoGlobal); + virtual void createMLCPFast(const btContactSolverInfo& infoGlobal); + + //return true is it solves the problem successfully + virtual bool solveMLCP(const btContactSolverInfo& infoGlobal); + +public: + + btMLCPSolver( btMLCPSolverInterface* solver); + virtual ~btMLCPSolver(); + + void setMLCPSolver(btMLCPSolverInterface* solver) + { + m_solver = solver; + } + + int getNumFallbacks() const + { + return m_fallback; + } + void setNumFallbacks(int num) + { + m_fallback = num; + } + + btScalar getCfm() const + { + return m_cfm; + } + void setCfm(btScalar cfm) + { + m_cfm = cfm; + } + + virtual btConstraintSolverType getSolverType() const + { + return BT_MLCP_SOLVER; + } + +}; + + +#endif //BT_MLCP_SOLVER_H diff --git a/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h b/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h index 705ff47b5..25bb3f6d3 100644 --- a/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h +++ b/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h @@ -1,33 +1,33 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///original version written by Erwin Coumans, October 2013 - -#ifndef BT_MLCP_SOLVER_INTERFACE_H -#define BT_MLCP_SOLVER_INTERFACE_H - -#include "LinearMath/btMatrixX.h" - -class btMLCPSolverInterface -{ -public: - virtual ~btMLCPSolverInterface() - { - } - - //return true is it solves the problem successfully - virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray& limitDependency, int numIterations, bool useSparsity = true)=0; -}; - -#endif //BT_MLCP_SOLVER_INTERFACE_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///original version written by Erwin Coumans, October 2013 + +#ifndef BT_MLCP_SOLVER_INTERFACE_H +#define BT_MLCP_SOLVER_INTERFACE_H + +#include "LinearMath/btMatrixX.h" + +class btMLCPSolverInterface +{ +public: + virtual ~btMLCPSolverInterface() + { + } + + //return true is it solves the problem successfully + virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray& limitDependency, int numIterations, bool useSparsity = true)=0; +}; + +#endif //BT_MLCP_SOLVER_INTERFACE_H diff --git a/src/BulletDynamics/MLCPSolvers/btPATHSolver.h b/src/BulletDynamics/MLCPSolvers/btPATHSolver.h index ef045339a..9ec31a6d4 100644 --- a/src/BulletDynamics/MLCPSolvers/btPATHSolver.h +++ b/src/BulletDynamics/MLCPSolvers/btPATHSolver.h @@ -1,151 +1,151 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///original version written by Erwin Coumans, October 2013 - - -#ifndef BT_PATH_SOLVER_H -#define BT_PATH_SOLVER_H - -//#define BT_USE_PATH -#ifdef BT_USE_PATH - -extern "C" { -#include "PATH/SimpleLCP.h" -#include "PATH/License.h" -#include "PATH/Error_Interface.h" -}; - void __stdcall MyError(Void *data, Char *msg) -{ - printf("Path Error: %s\n",msg); -} - void __stdcall MyWarning(Void *data, Char *msg) -{ - printf("Path Warning: %s\n",msg); -} - -Error_Interface e; - - - -#include "btMLCPSolverInterface.h" -#include "Dantzig/lcp.h" - -class btPathSolver : public btMLCPSolverInterface -{ -public: - - btPathSolver() - { - License_SetString("2069810742&Courtesy_License&&&USR&2013&14_12_2011&1000&PATH&GEN&31_12_2013&0_0_0&0&0_0"); - e.error_data = 0; - e.warning = MyWarning; - e.error = MyError; - Error_SetInterface(&e); - } - - - virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray& limitDependency, int numIterations, bool useSparsity = true) - { - MCP_Termination status; - - - int numVariables = b.rows(); - if (0==numVariables) - return true; - - /* - variables - the number of variables in the problem - - m_nnz - the number of nonzeros in the M matrix - - m_i - a vector of size m_nnz containing the row indices for M - - m_j - a vector of size m_nnz containing the column indices for M - - m_ij - a vector of size m_nnz containing the data for M - - q - a vector of size variables - - lb - a vector of size variables containing the lower bounds on x - - ub - a vector of size variables containing the upper bounds on x - */ - btAlignedObjectArray values; - btAlignedObjectArray rowIndices; - btAlignedObjectArray colIndices; - - for (int i=0;i zResult; - zResult.resize(numVariables); - btAlignedObjectArray rhs; - btAlignedObjectArray upperBounds; - btAlignedObjectArray lowerBounds; - for (int i=0;i& limitDependency, int numIterations, bool useSparsity = true) + { + MCP_Termination status; + + + int numVariables = b.rows(); + if (0==numVariables) + return true; + + /* - variables - the number of variables in the problem + - m_nnz - the number of nonzeros in the M matrix + - m_i - a vector of size m_nnz containing the row indices for M + - m_j - a vector of size m_nnz containing the column indices for M + - m_ij - a vector of size m_nnz containing the data for M + - q - a vector of size variables + - lb - a vector of size variables containing the lower bounds on x + - ub - a vector of size variables containing the upper bounds on x + */ + btAlignedObjectArray values; + btAlignedObjectArray rowIndices; + btAlignedObjectArray colIndices; + + for (int i=0;i zResult; + zResult.resize(numVariables); + btAlignedObjectArray rhs; + btAlignedObjectArray upperBounds; + btAlignedObjectArray lowerBounds; + for (int i=0;i& limitDependency, int numIterations, bool useSparsity = true) - { - if (!A.rows()) - return true; - //the A matrix is sparse, so compute the non-zero elements - A.rowComputeNonZeroElements(); - - //A is a m-n matrix, m rows, n columns - btAssert(A.rows() == b.rows()); - - int i, j, numRows = A.rows(); - - float delta; - - for (int k = 0; k =0) - { - s = x[limitDependency[i]]; - if (s<0) - s=1; - } - - if (x[i]hi[i]*s) - x[i]=hi[i]*s; - } - } - return true; - } - -}; - -#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///original version written by Erwin Coumans, October 2013 + +#ifndef BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H +#define BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H + + +#include "btMLCPSolverInterface.h" + +///This solver is mainly for debug/learning purposes: it is functionally equivalent to the btSequentialImpulseConstraintSolver solver, but much slower (it builds the full LCP matrix) +class btSolveProjectedGaussSeidel : public btMLCPSolverInterface +{ +public: + virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray& limitDependency, int numIterations, bool useSparsity = true) + { + if (!A.rows()) + return true; + //the A matrix is sparse, so compute the non-zero elements + A.rowComputeNonZeroElements(); + + //A is a m-n matrix, m rows, n columns + btAssert(A.rows() == b.rows()); + + int i, j, numRows = A.rows(); + + float delta; + + for (int k = 0; k =0) + { + s = x[limitDependency[i]]; + if (s<0) + s=1; + } + + if (x[i]hi[i]*s) + x[i]=hi[i]*s; + } + } + return true; + } + +}; + +#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H diff --git a/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp index 77b475b96..a7b168846 100644 --- a/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp +++ b/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp @@ -296,8 +296,9 @@ void btRaycastVehicle::updateVehicle( btScalar step ) int i=0; for (i=0;i