From 7b28e86c7b9ab3c4bb8d479f843b9b5c2e1c9a06 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 20 Aug 2014 16:28:16 -0700 Subject: [PATCH] add improved btGeneric6DofSpring2Constraint, thanks to Puhr Gabor and Tamas Umenhoffer! improved the new demo testbed (work-in-progress) add basic Lua demo, import URDF test, STL import, obj import --- Demos/CommonParameterInterface.h | 2 + Demos/SerializeDemo/SerializeSetup.cpp | 21 + Demos/SerializeDemo/SerializeSetup.h | 14 + Demos3/AllBullet2Demos/BulletDemoEntries.h | 10 +- Demos3/AllBullet2Demos/GraphingTexture.cpp | 192 +++ Demos3/AllBullet2Demos/GraphingTexture.h | 36 + .../GwenParameterInterface.cpp | 21 +- .../AllBullet2Demos/GwenParameterInterface.h | 1 + Demos3/AllBullet2Demos/GwenProfileWindow.cpp | 291 ++++ Demos3/AllBullet2Demos/GwenProfileWindow.h | 12 + Demos3/AllBullet2Demos/GwenTextureWindow.cpp | 100 ++ Demos3/AllBullet2Demos/GwenTextureWindow.h | 32 + Demos3/AllBullet2Demos/main.cpp | 207 ++- Demos3/AllBullet2Demos/premake4.lua | 23 + Demos3/GpuDemos/gwenInternalData.h | 1 + Demos3/GpuDemos/gwenUserInterface.cpp | 95 +- Demos3/GpuDemos/gwenUserInterface.h | 2 + Demos3/ImportObjDemo/ImportObjSetup.cpp | 40 +- Demos3/ImportSTLDemo/ImportSTLSetup.cpp | 1 + Demos3/ImportURDFDemo/ImportURDFSetup.cpp | 1354 ++++++----------- Demos3/ImportURDFDemo/urdf_samples.h | 671 ++++++++ .../BasicDemo/Bullet2RigidBodyDemo.cpp | 1 + Demos3/bullet2/BasicDemo/MyDebugDrawer.h | 2 + .../BulletMultiBodyDemos.cpp | 5 +- Demos3/bullet2/LuaDemo/LuaPhysicsSetup.cpp | 467 ++++++ Demos3/bullet2/LuaDemo/LuaPhysicsSetup.h | 43 + btgui/Gwen/Texture.h | 3 + btgui/GwenOpenGLTest/UnitTest.cpp | 4 +- btgui/OpenGLWindow/GwenOpenGL3CoreRenderer.h | 99 +- build3/findOpenGLGlewGlut.lua | 2 + .../CollisionDispatch/btCollisionObject.cpp | 1 + .../CollisionDispatch/btCollisionObject.h | 9 +- .../btCompoundCollisionAlgorithm.cpp | 4 +- .../btGeneric6DofSpring2Constraint.cpp | 1113 ++++++++++++++ .../btGeneric6DofSpring2Constraint.h | 654 ++++++++ .../Dynamics/btDiscreteDynamicsWorld.cpp | 3 + .../Featherstone/btMultiBodyJointMotor.h | 4 + src/LinearMath/btIDebugDraw.h | 5 + 38 files changed, 4530 insertions(+), 1015 deletions(-) create mode 100644 Demos/SerializeDemo/SerializeSetup.cpp create mode 100644 Demos/SerializeDemo/SerializeSetup.h create mode 100644 Demos3/AllBullet2Demos/GraphingTexture.cpp create mode 100644 Demos3/AllBullet2Demos/GraphingTexture.h create mode 100644 Demos3/AllBullet2Demos/GwenProfileWindow.cpp create mode 100644 Demos3/AllBullet2Demos/GwenProfileWindow.h create mode 100644 Demos3/AllBullet2Demos/GwenTextureWindow.cpp create mode 100644 Demos3/AllBullet2Demos/GwenTextureWindow.h create mode 100644 Demos3/ImportURDFDemo/urdf_samples.h create mode 100644 Demos3/bullet2/LuaDemo/LuaPhysicsSetup.cpp create mode 100644 Demos3/bullet2/LuaDemo/LuaPhysicsSetup.h create mode 100644 src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp create mode 100644 src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h diff --git a/Demos/CommonParameterInterface.h b/Demos/CommonParameterInterface.h index afb78114b..55acc3e94 100644 --- a/Demos/CommonParameterInterface.h +++ b/Demos/CommonParameterInterface.h @@ -36,6 +36,8 @@ struct CommonParameterInterface virtual void registerSliderFloatParameter(SliderParams& params)=0; virtual void syncParameters()=0; virtual void removeAllParameters()=0; + virtual void setSliderValue(int sliderIndex, double sliderValue)=0; + }; #endif //PARAM_INTERFACE_H diff --git a/Demos/SerializeDemo/SerializeSetup.cpp b/Demos/SerializeDemo/SerializeSetup.cpp new file mode 100644 index 000000000..7cbdb4cc4 --- /dev/null +++ b/Demos/SerializeDemo/SerializeSetup.cpp @@ -0,0 +1,21 @@ +#include "SerializeSetup.h" +#include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h" + +SerializeSetup::SerializeSetup() +{ + +} +SerializeSetup::~SerializeSetup() +{ + +} + +void SerializeSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) +{ + this->createEmptyDynamicsWorld(); + gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld); + m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe); + btBulletWorldImporter* importer = new btBulletWorldImporter(m_dynamicsWorld); + const char* filename = "testFile.bullet"; + importer->loadFile(filename); +} \ No newline at end of file diff --git a/Demos/SerializeDemo/SerializeSetup.h b/Demos/SerializeDemo/SerializeSetup.h new file mode 100644 index 000000000..bb86e3a3f --- /dev/null +++ b/Demos/SerializeDemo/SerializeSetup.h @@ -0,0 +1,14 @@ +#ifndef SERIALIZE_SETUP_H +#define SERIALIZE_SETUP_H +#include "../../Demos/CommonRigidBodySetup.h" + +class SerializeSetup : public CommonRigidBodySetup +{ +public: + SerializeSetup(); + virtual ~SerializeSetup(); + + virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge); +}; + +#endif //SERIALIZE_SETUP_H diff --git a/Demos3/AllBullet2Demos/BulletDemoEntries.h b/Demos3/AllBullet2Demos/BulletDemoEntries.h index abf4ad32a..bf14d5bb5 100644 --- a/Demos3/AllBullet2Demos/BulletDemoEntries.h +++ b/Demos3/AllBullet2Demos/BulletDemoEntries.h @@ -11,7 +11,7 @@ #include "../bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.h" #include "../bullet2/RagdollDemo/RagdollDemo.h" -#include "../bullet2/LuaDemo/LuaDemo.h" +#include "../bullet2/LuaDemo/LuaPhysicsSetup.h" #include "../bullet2/ChainDemo/ChainDemo.h" #include "../../Demos/CcdPhysicsDemo/CcdPhysicsSetup.h" #include "../../Demos/ConstraintDemo/ConstraintPhysicsSetup.h" @@ -19,6 +19,11 @@ #include "../ImportObjDemo/ImportObjSetup.h" #include "../ImportSTLDemo/ImportSTLSetup.h" +static BulletDemoInterface* LuaDemoCreateFunc(SimpleOpenGL3App* app) +{ + CommonPhysicsSetup* physicsSetup = new LuaPhysicsSetup(app); + return new BasicDemo(app, physicsSetup); +} static BulletDemoInterface* MyCcdPhysicsDemoCreateFunc(SimpleOpenGL3App* app) { @@ -73,6 +78,8 @@ static BulletDemoEntry allDemos[]= { 1, "CcdDemo", MyCcdPhysicsDemoCreateFunc }, { 1, "Kinematic", MyKinematicObjectCreateFunc }, { 1, "Constraints", MyConstraintCreateFunc }, + { 1, "LuaDemo",LuaDemoCreateFunc}, + {0,"File Formats", 0}, //@todo(erwincoumans) { 1, "bullet", MyImportSTLCreateFunc}, { 1, "Wavefront Obj", MyImportObjCreateFunc}, @@ -95,7 +102,6 @@ static BulletDemoEntry allDemos[]= {1,"MultiBody1",FeatherstoneDemo1::MyCreateFunc}, // {"MultiBody2",FeatherstoneDemo2::MyCreateFunc}, {1,"MultiDofDemo",MultiDofDemo::MyCreateFunc}, -// {"LuaDemo",LuaDemo::MyCreateFunc} }; diff --git a/Demos3/AllBullet2Demos/GraphingTexture.cpp b/Demos3/AllBullet2Demos/GraphingTexture.cpp new file mode 100644 index 000000000..d81072298 --- /dev/null +++ b/Demos3/AllBullet2Demos/GraphingTexture.cpp @@ -0,0 +1,192 @@ +#include "GraphingTexture.h" +#include "OpenGLWindow/OpenGLInclude.h" +#include + +GraphingTexture::GraphingTexture() +:m_textureId(0), +m_width(0), +m_height(0) +{ +} + +GraphingTexture::~GraphingTexture() +{ + destroy(); +} + +bool GraphingTexture::destroy() +{ + //TODO(erwincoumans) release memory etc... + m_width = 0; + m_height=0; + glDeleteTextures(1,(GLuint*)&m_textureId); + m_textureId=0; +} + +bool GraphingTexture::create(int texWidth, int texHeight) +{ + m_width = texWidth; + m_height = texHeight; + glActiveTexture(GL_TEXTURE0); + + m_imageData.resize(texWidth*texHeight*4); + for(int y=0;y>5; + GLubyte* pi=&m_imageData[y*texWidth*4]; + for(int x=0;x=y)//x<2||y<2||x>253||y>253) + { + pi[0]=0; + pi[1]=0; + pi[2]=255; + pi[3]=255; + } else + { + pi[0]=255; + pi[1]=0; + pi[2]=0; + pi[3]=255; + } + + pi+=4; + } + } + + + glGenTextures(1,(GLuint*)&m_textureId); + + uploadImageData(); +} + +void GraphingTexture::uploadImageData() +{ + glBindTexture(GL_TEXTURE_2D,m_textureId); + GLint err = glGetError(); + assert(err==GL_NO_ERROR); + + err = glGetError(); + assert(err==GL_NO_ERROR); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, m_width,m_height,0,GL_RGBA,GL_UNSIGNED_BYTE,&m_imageData[0]); + glGenerateMipmap(GL_TEXTURE_2D); + + err = glGetError(); + assert(err==GL_NO_ERROR); + + +} + +#if 0 +//shift the image one pixel +for(int y=0;y>5; + for(int x=1;x m_imageData; + int m_width; + int m_height; + + GraphingTexture(); + virtual ~GraphingTexture(); + + bool create(int texWidth, int texHeight); + bool destroy(); + + void setPixel(int x, int y, unsigned char red, unsigned char green, unsigned char blue, unsigned char alpha) + { + m_imageData[x*4+y*4*m_width+0] = red; + m_imageData[x*4+y*4*m_width+1] = green; + m_imageData[x*4+y*4*m_width+2] = blue; + m_imageData[x*4+y*4*m_width+3] = alpha; + } + + void uploadImageData(); + + int getTextureId() + { + return m_textureId; + } +}; + +#endif //GRAPHING_TEXTURE_H + diff --git a/Demos3/AllBullet2Demos/GwenParameterInterface.cpp b/Demos3/AllBullet2Demos/GwenParameterInterface.cpp index 6df0a26d3..be4802174 100644 --- a/Demos3/AllBullet2Demos/GwenParameterInterface.cpp +++ b/Demos3/AllBullet2Demos/GwenParameterInterface.cpp @@ -2,6 +2,7 @@ #include "../GpuDemos/gwenInternalData.h" + template struct MySliderEventHandler : public Gwen::Event::Handler { @@ -76,6 +77,24 @@ GwenParameterInterface::~GwenParameterInterface() } +void GwenParameterInterface::setSliderValue(int sliderIndex, double sliderValue) +{ + int sliderCapped = sliderValue+4; + sliderCapped /= 8; + sliderCapped *= 8; + + if (sliderIndex>=0 && sliderIndexm_sliders.size()) + { + m_paramInternalData->m_sliders[sliderIndex]->GetRangeMin(); + + m_paramInternalData->m_sliders[sliderIndex]->GetRangeMax(); + float mappedValue =m_paramInternalData->m_sliders[sliderIndex]->GetRangeMin()+ + (m_paramInternalData->m_sliders[sliderIndex]->GetRangeMax()- + m_paramInternalData->m_sliders[sliderIndex]->GetRangeMin())*sliderCapped/128.f; + printf("mappedValue = %f\n",mappedValue); + m_paramInternalData->m_sliders[sliderIndex]->SetValue(mappedValue); + } +} #include void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params) @@ -95,7 +114,7 @@ void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params) pSlider->SetPos( 10, m_gwenInternalData->m_curYposition ); pSlider->SetSize( 100, 20 ); pSlider->SetRange( params.m_minVal, params.m_maxVal); - pSlider->SetNotchCount(20);//float(params.m_maxVal-params.m_minVal)/100.f); + pSlider->SetNotchCount(128);//float(params.m_maxVal-params.m_minVal)/100.f); pSlider->SetClampToNotches( params.m_clampToNotches ); pSlider->SetValue( *params.m_paramValuePointer);//dimensions[i] ); char labelName[1024]; diff --git a/Demos3/AllBullet2Demos/GwenParameterInterface.h b/Demos3/AllBullet2Demos/GwenParameterInterface.h index a36741dd9..08c36b38e 100644 --- a/Demos3/AllBullet2Demos/GwenParameterInterface.h +++ b/Demos3/AllBullet2Demos/GwenParameterInterface.h @@ -12,6 +12,7 @@ struct GwenParameterInterface : public CommonParameterInterface GwenParameterInterface(struct GwenInternalData* gwenInternalData); virtual ~GwenParameterInterface(); virtual void registerSliderFloatParameter(SliderParams& params); + virtual void setSliderValue(int sliderIndex, double sliderValue); virtual void syncParameters(); virtual void removeAllParameters(); diff --git a/Demos3/AllBullet2Demos/GwenProfileWindow.cpp b/Demos3/AllBullet2Demos/GwenProfileWindow.cpp new file mode 100644 index 000000000..86cae4d2e --- /dev/null +++ b/Demos3/AllBullet2Demos/GwenProfileWindow.cpp @@ -0,0 +1,291 @@ +#include "../GpuDemos/gwenUserInterface.h" +#include "../GpuDemos/gwenInternalData.h" +#include "LinearMath/btQuickprof.h" + + + + + +class MyProfileWindow : public Gwen::Controls::WindowControl +{ + + // Gwen::Controls::TabControl* m_TabControl; + //Gwen::Controls::ListBox* m_TextOutput; + unsigned int m_iFrames; + float m_fLastSecond; + + Gwen::Controls::TreeNode* m_node; + Gwen::Controls::TreeControl* m_ctrl; + + +protected: + + void onButtonA( Gwen::Controls::Base* pControl ) + { + // OpenTissue::glut::toggleIdle(); + } + + void SliderMoved(Gwen::Controls::Base* pControl ) + { + Gwen::Controls::Slider* pSlider = (Gwen::Controls::Slider*)pControl; + //this->m_app->scaleYoungModulus(pSlider->GetValue()); + // printf("Slider Value: %.2f", pSlider->GetValue() ); + } + + + void OnCheckChangedStiffnessWarping (Gwen::Controls::Base* pControl) + { + Gwen::Controls::CheckBox* labeled = (Gwen::Controls::CheckBox* )pControl; + bool checked = labeled->IsChecked(); + //m_app->m_stiffness_warp_on = checked; + } +public: + + + CProfileIterator* profIter; + + MyProfileWindow ( Gwen::Controls::Base* pParent) + : Gwen::Controls::WindowControl( pParent ), + profIter(0) + { + SetTitle( L"Time Profiler" ); + + SetSize( 450, 450 ); + this->SetPos(10,400); + + // this->Dock( Gwen::Pos::Bottom); + + + + { + m_ctrl = new Gwen::Controls::TreeControl( this ); + m_node = m_ctrl->AddNode( L"Total Parent Time" ); + + + //Gwen::Controls::TreeNode* pNode = ctrl->AddNode( L"Node Two" ); + //pNode->AddNode( L"Node Two Inside" ); + //pNode->AddNode( L"Eyes" ); + //pNode->AddNode( L"Brown" )->AddNode( L"Node Two Inside" )->AddNode( L"Eyes" )->AddNode( L"Brown" ); + //Gwen::Controls::TreeNode* node = ctrl->AddNode( L"Node Three" ); + + + + //m_ctrl->Dock(Gwen::Pos::Bottom); + + m_ctrl->ExpandAll(); + m_ctrl->SetKeyboardInputEnabled(true); + m_ctrl->SetBounds( this->GetInnerBounds().x,this->GetInnerBounds().y,this->GetInnerBounds().w,this->GetInnerBounds().h); + + } + + + + } + + + float dumpRecursive(CProfileIterator* profileIterator, Gwen::Controls::TreeNode* parentNode) + { + profileIterator->First(); + if (profileIterator->Is_Done()) + return 0.f; + + float accumulated_time=0,parent_time = profileIterator->Is_Root() ? CProfileManager::Get_Time_Since_Reset() : profileIterator->Get_Current_Parent_Total_Time(); + int i; + int frames_since_reset = CProfileManager::Get_Frame_Count_Since_Reset(); + + //printf("Profiling: %s (total running time: %.3f ms) ---\n", profileIterator->Get_Current_Parent_Name(), parent_time ); + float totalTime = 0.f; + + + int numChildren = 0; + Gwen::UnicodeString txt; + std::vector nodes; + + for (i = 0; !profileIterator->Is_Done(); i++,profileIterator->Next()) + { + numChildren++; + float current_total_time = profileIterator->Get_Current_Total_Time(); + accumulated_time += current_total_time; + double fraction = parent_time > SIMD_EPSILON ? (current_total_time / parent_time) * 100 : 0.f; + + Gwen::String name(profileIterator->Get_Current_Name()); +#ifdef _WIN32 + Gwen::UnicodeString uname = Gwen::Utility::StringToUnicode(name); + + txt = Gwen::Utility::Format(L"%s (%.2f %%) :: %.3f ms / frame (%d calls)",uname.c_str(), fraction,(current_total_time / (double)frames_since_reset),profileIterator->Get_Current_Total_Calls()); + +#else + txt = Gwen::Utility::Format(L"%s (%.2f %%) :: %.3f ms / frame (%d calls)",name.c_str(), fraction,(current_total_time / (double)frames_since_reset),profileIterator->Get_Current_Total_Calls()); + +#endif + + Gwen::Controls::TreeNode* childNode = (Gwen::Controls::TreeNode*)profileIterator->Get_Current_UserPointer(); + if (!childNode) + { + childNode = parentNode->AddNode(L""); + profileIterator->Set_Current_UserPointer(childNode); + } + childNode->SetText(txt); + nodes.push_back(childNode); + + totalTime += current_total_time; + //recurse into children + } + + for (i=0;iEnter_Child(i); + Gwen::Controls::TreeNode* curNode = nodes[i]; + + dumpRecursive(profileIterator, curNode); + + profileIterator->Enter_Parent(); + } + return accumulated_time; + + } + + void UpdateText(CProfileIterator* profileIterator, bool idle) + { + + static bool update=true; + + m_ctrl->SetBounds(0,0,this->GetInnerBounds().w,this->GetInnerBounds().h); + + // if (!update) + // return; + update=false; + + + static int test = 1; + test++; + + static double time_since_reset = 0.f; + if (!idle) + { + time_since_reset = CProfileManager::Get_Time_Since_Reset(); + } + + //Gwen::UnicodeString txt = Gwen::Utility::Format( L"FEM Settings %i fps", test ); + { + //recompute profiling data, and store profile strings + + char blockTime[128]; + + double totalTime = 0; + + int frames_since_reset = CProfileManager::Get_Frame_Count_Since_Reset(); + + profileIterator->First(); + + double parent_time = profileIterator->Is_Root() ? time_since_reset : profileIterator->Get_Current_Parent_Total_Time(); + + + Gwen::Controls::TreeNode* curParent = m_node; + + double accumulated_time = dumpRecursive(profileIterator,m_node); + + const char* name = profileIterator->Get_Current_Parent_Name(); +#ifdef _WIN32 + Gwen::UnicodeString uname = Gwen::Utility::StringToUnicode(name); + Gwen::UnicodeString txt = Gwen::Utility::Format( L"Profiling: %s total time: %.3f ms, unaccounted %.3f %% :: %.3f ms", uname.c_str(), parent_time , + parent_time > SIMD_EPSILON ? ((parent_time - accumulated_time) / parent_time) * 100 : 0.f, parent_time - accumulated_time); +#else + Gwen::UnicodeString txt = Gwen::Utility::Format( L"Profiling: %s total time: %.3f ms, unaccounted %.3f %% :: %.3f ms", name, parent_time , + parent_time > SIMD_EPSILON ? ((parent_time - accumulated_time) / parent_time) * 100 : 0.f, parent_time - accumulated_time); +#endif + //sprintf(blockTime,"--- Profiling: %s (total running time: %.3f ms) ---", profileIterator->Get_Current_Parent_Name(), parent_time ); + //displayProfileString(xOffset,yStart,blockTime); + m_node->SetText(txt); + + + //printf("%s (%.3f %%) :: %.3f ms\n", "Unaccounted:",); + + + } + + static int counter=10; + if (counter) + { + counter--; + m_ctrl->ExpandAll(); + } + + } + void PrintText( const Gwen::UnicodeString& str ) + { + + } + + void Render( Gwen::Skin::Base* skin ) + { + m_iFrames++; + + if ( m_fLastSecond < Gwen::Platform::GetTimeInSeconds() ) + { + SetTitle( Gwen::Utility::Format( L"Profiler %i fps", m_iFrames ) ); + + m_fLastSecond = Gwen::Platform::GetTimeInSeconds() + 1.0f; + m_iFrames = 0; + } + + Gwen::Controls::WindowControl::Render( skin ); + + } + + +}; + +class MyMenuItems : public Gwen::Controls::Base +{ + +public: + + class MyProfileWindow* m_profWindow; + MyMenuItems() :Gwen::Controls::Base(0) + { + } + + void MenuItemSelect(Gwen::Controls::Base* pControl) + { + if (m_profWindow->Hidden()) + { + m_profWindow->SetHidden(false); + } else + { + m_profWindow->SetHidden(true); + } + + } +}; + + +MyProfileWindow* setupProfileWindow(GwenInternalData* data) +{ + MyMenuItems* menuItems = new MyMenuItems; + MyProfileWindow* profWindow = new MyProfileWindow(data->pCanvas); + //profWindow->SetHidden(true); + profWindow->profIter = CProfileManager::Get_Iterator(); + data->m_viewMenu->GetMenu()->AddItem( L"Profiler", menuItems,(Gwen::Event::Handler::Function)&MyMenuItems::MenuItemSelect); + menuItems->m_profWindow = profWindow; + return profWindow; +} + + +void processProfileData( MyProfileWindow* profWindow, bool idle) +{ + if (profWindow) + { + + profWindow->UpdateText(profWindow->profIter, idle); + } +} + +void profileWindowSetVisible(MyProfileWindow* window, bool visible) +{ + window->SetHidden(!visible); +} +void destroyProfileWindow(MyProfileWindow* window) +{ + delete window; +} diff --git a/Demos3/AllBullet2Demos/GwenProfileWindow.h b/Demos3/AllBullet2Demos/GwenProfileWindow.h new file mode 100644 index 000000000..cb68386bf --- /dev/null +++ b/Demos3/AllBullet2Demos/GwenProfileWindow.h @@ -0,0 +1,12 @@ +#ifndef GWEN_PROFILE_WINDOW_H +#define GWEN_PROFILE_WINDOW_H + +struct MyProfileWindow* setupProfileWindow(struct GwenInternalData* data); + +void processProfileData(MyProfileWindow* window, bool idle); +void profileWindowSetVisible(MyProfileWindow* window, bool visible); +void destroyProfileWindow(MyProfileWindow* window); + +#endif//GWEN_PROFILE_WINDOW_H + + diff --git a/Demos3/AllBullet2Demos/GwenTextureWindow.cpp b/Demos3/AllBullet2Demos/GwenTextureWindow.cpp new file mode 100644 index 000000000..72701c579 --- /dev/null +++ b/Demos3/AllBullet2Demos/GwenTextureWindow.cpp @@ -0,0 +1,100 @@ +#include "GwenTextureWindow.h" +#include "../GpuDemos/gwenUserInterface.h" +#include "../GpuDemos/gwenInternalData.h" +#include "Gwen/Controls/ImagePanel.h" + + + +class MyGraphWindow : public Gwen::Controls::WindowControl +{ + Gwen::Controls::ImagePanel* m_imgPanel; + +public: + + class MyMenuItems2* m_menuItems; + + MyGraphWindow ( const MyGraphInput& input) + : Gwen::Controls::WindowControl( input.m_data->pCanvas ), + m_menuItems(0) + { + Gwen::UnicodeString str = Gwen::Utility::StringToUnicode(input.m_name); + SetTitle( str ); + + + SetPos(input.m_xPos,input.m_yPos); + SetSize( 12+input.m_width+2*input.m_borderWidth, 30+input.m_height+2*input.m_borderWidth ); + + m_imgPanel = new Gwen::Controls::ImagePanel( this ); + if (input.m_texName) + { + Gwen::UnicodeString texName = Gwen::Utility::StringToUnicode(input.m_texName); + m_imgPanel->SetImage( texName ); + } + m_imgPanel->SetBounds( input.m_borderWidth, input.m_borderWidth, + input.m_width, + input.m_height ); + // this->Dock( Gwen::Pos::Bottom); + } + virtual ~MyGraphWindow() + { + delete m_imgPanel; + } + + +}; + +class MyMenuItems2 : public Gwen::Controls::Base +{ + MyGraphWindow* m_graphWindow; + +public: + + Gwen::Controls::MenuItem* m_item; + + MyMenuItems2(MyGraphWindow* graphWindow) + :Gwen::Controls::Base(0), + m_graphWindow(graphWindow), + m_item(0) + { + } + + void MenuItemSelect(Gwen::Controls::Base* pControl) + { + if (m_graphWindow->Hidden()) + { + m_graphWindow->SetHidden(false); + //@TODO(erwincoumans) setCheck/SetCheckable drawing is broken, need to see what's wrong +// if (m_item) +// m_item->SetCheck(false); + + } else + { + m_graphWindow->SetHidden(true); +// if (m_item) +// m_item->SetCheck(true); + + } + + } +}; + +struct MyGraphWindow* setupTextureWindow(const MyGraphInput& input) +{ + MyGraphWindow* graphWindow = new MyGraphWindow(input); + MyMenuItems2* menuItems = new MyMenuItems2(graphWindow); + graphWindow->m_menuItems = menuItems; + + Gwen::UnicodeString str = Gwen::Utility::StringToUnicode(input.m_name); + menuItems->m_item = input.m_data->m_viewMenu->GetMenu()->AddItem( str, menuItems,(Gwen::Event::Handler::Function)&MyMenuItems2::MenuItemSelect); +// menuItems->m_item->SetCheckable(true); + + return graphWindow; + +} + +void destroyTextureWindow(MyGraphWindow* window) +{ + delete window->m_menuItems->m_item; + delete window->m_menuItems; + delete window; +} diff --git a/Demos3/AllBullet2Demos/GwenTextureWindow.h b/Demos3/AllBullet2Demos/GwenTextureWindow.h new file mode 100644 index 000000000..a1080d761 --- /dev/null +++ b/Demos3/AllBullet2Demos/GwenTextureWindow.h @@ -0,0 +1,32 @@ + + +#ifndef GWEN_TEXTURE_WINDOW_H +#define GWEN_TEXTURE_WINDOW_H + +struct MyGraphInput +{ + struct GwenInternalData* m_data; + int m_xPos; + int m_yPos; + int m_width; + int m_height; + int m_borderWidth; + const char* m_name; + const char* m_texName; + MyGraphInput(struct GwenInternalData* data) + :m_data(data), + m_xPos(0), + m_yPos(0), + m_width(400), + m_height(400), + m_borderWidth(0), + m_name("GraphWindow"), + m_texName(0) + { + } +}; +struct MyGraphWindow* setupTextureWindow(const MyGraphInput& input); +void destroyTextureWindow(MyGraphWindow* window); + + +#endif //GWEN_TEXTURE_WINDOW_H diff --git a/Demos3/AllBullet2Demos/main.cpp b/Demos3/AllBullet2Demos/main.cpp index 82cf37c84..0dad18905 100644 --- a/Demos3/AllBullet2Demos/main.cpp +++ b/Demos3/AllBullet2Demos/main.cpp @@ -1,3 +1,4 @@ + #include "../../btgui/OpenGLWindow/SimpleOpenGL3App.h" #include "Bullet3Common/b3Vector3.h" #include "assert.h" @@ -7,6 +8,10 @@ #include "BulletDemoEntries.h" #include "../../btgui/Timing/b3Clock.h" #include "GwenParameterInterface.h" +#include "GwenProfileWindow.h" +#include "GwenTextureWindow.h" +#include "GraphingTexture.h" + #define DEMO_SELECTION_COMBOBOX 13 const char* startFileName = "bulletDemo.txt"; @@ -16,13 +21,89 @@ static int sCurrentDemoIndex = 0; static int sCurrentHightlighted = 0; static BulletDemoInterface* sCurrentDemo = 0; static b3AlignedObjectArray allNames; -double motorA=0,motorB=0; - - bool drawGUI=true; extern bool useShadowMap; static bool wireframe=false; static bool pauseSimulation=false; +int midiBaseIndex = 176; + + +#ifdef B3_USE_MIDI +#include "../../btgui/MidiTest/RtMidi.h" +bool chooseMidiPort( RtMidiIn *rtmidi ) +{ + if (!rtmidi) + return false; + + std::string portName; + unsigned int i = 0, nPorts = rtmidi->getPortCount(); + if ( nPorts == 0 ) { + std::cout << "No input ports available!" << std::endl; + return false; + } + + if ( nPorts == 1 ) { + std::cout << "\nOpening " << rtmidi->getPortName() << std::endl; + } + else { + for ( i=0; igetPortName(i); + std::cout << " Input port #" << i << ": " << portName << '\n'; + } + + do { + std::cout << "\nChoose a port number: "; + std::cin >> i; + } while ( i >= nPorts ); + } + + // std::getline( std::cin, keyHit ); // used to clear out stdin + rtmidi->openPort( i ); + + return true; +} +void PairMidiCallback( double deltatime, std::vector< unsigned char > *message, void *userData ) +{ + unsigned int nBytes = message->size(); + if (nBytes==3) + { + //if ( message->at(1)==16) + { + printf("midi %d at %f = %d\n", message->at(1), deltatime, message->at(2)); + //test->SetValue(message->at(2)); +#if KORG_MIDI + if (message->at(0)>=midiBaseIndex && message->at(0)<(midiBaseIndex+8)) + { + int sliderIndex = message->at(0)-midiBaseIndex;//-16; + printf("sliderIndex = %d\n", sliderIndex); + float sliderValue = message->at(2); + printf("sliderValue = %f\n", sliderValue); + app->m_parameterInterface->setSliderValue(sliderIndex,sliderValue); + } +#else + //ICONTROLS + if (message->at(0)==176) + { + int sliderIndex = message->at(1)-32;//-16; + printf("sliderIndex = %d\n", sliderIndex); + float sliderValue = message->at(2); + printf("sliderValue = %f\n", sliderValue); + app->m_parameterInterface->setSliderValue(sliderIndex,sliderValue); + } + +#endif + + } + } +} + +#endif //B3_USE_MIDI + + + + + + void MyKeyboardCallback(int key, int state) { @@ -121,7 +202,7 @@ void selectDemo(int demoIndex) if (sCurrentDemo) { sCurrentDemo->initPhysics(); - } + } } } @@ -225,7 +306,25 @@ struct MyMenuItemHander :public Gwen::Event::Handler }; +#include "Bullet3Common/b3HashMap.h" +struct GL3TexLoader : public MyTextureLoader +{ + b3HashMap m_hashMap; + + virtual void LoadTexture( Gwen::Texture* pTexture ) + { + const char* n = pTexture->name.Get().c_str(); + GLint* texIdPtr = m_hashMap[n]; + if (texIdPtr) + { + pTexture->m_intData = *texIdPtr; + } + } + virtual void FreeTexture( Gwen::Texture* pTexture ) + { + } +}; extern float shadowMapWorldSize; @@ -239,6 +338,7 @@ int main(int argc, char* argv[]) int width = 1024; int height=768; + app = new SimpleOpenGL3App("AllBullet2Demos",width,height); app->m_instancingRenderer->setCameraDistance(13); app->m_instancingRenderer->setCameraPitch(0); @@ -246,7 +346,7 @@ int main(int argc, char* argv[]) app->m_window->setMouseMoveCallback(MyMouseMoveCallback); app->m_window->setMouseButtonCallback(MyMouseButtonCallback); app->m_window->setKeyboardCallback(MyKeyboardCallback); - + GLint err = glGetError(); assert(err==GL_NO_ERROR); @@ -257,6 +357,55 @@ int main(int argc, char* argv[]) // gui->getInternalData()->m_explorerPage Gwen::Controls::TreeControl* tree = gui->getInternalData()->m_explorerTreeCtrl; + GL3TexLoader* myTexLoader = new GL3TexLoader; + gui->getInternalData()->pRenderer->setTextureLoader(myTexLoader); + + + MyProfileWindow* profWindow = setupProfileWindow(gui->getInternalData()); + profileWindowSetVisible(profWindow,false); + + { + MyGraphInput input(gui->getInternalData()); + input.m_width=300; + input.m_height=300; + input.m_xPos = 0; + input.m_yPos = height-input.m_height; + input.m_name="Test Graph1"; + input.m_texName = "graph1"; + GraphingTexture* gt = new GraphingTexture; + gt->create(256,256); + int texId = gt->getTextureId(); + myTexLoader->m_hashMap.insert("graph1", texId); + MyGraphWindow* gw = setupTextureWindow(input); + } + if (1) + { + MyGraphInput input(gui->getInternalData()); + input.m_width=300; + input.m_height=300; + input.m_xPos = width-input.m_width; + input.m_yPos = height-input.m_height; + input.m_name="Test Graph2"; + input.m_texName = "graph2"; + GraphingTexture* gt = new GraphingTexture; + int texWidth = 512; + int texHeight = 512; + gt->create(texWidth,texHeight); + for (int i=0;isetPixel(i,j,0,0,0,255); + } + } + gt->uploadImageData(); + + int texId = gt->getTextureId(); + input.m_xPos = width-input.m_width; + myTexLoader->m_hashMap.insert("graph2", texId); + MyGraphWindow* gw = setupTextureWindow(input); + } + //destroyTextureWindow(gw); app->m_parameterInterface = new GwenParameterInterface(gui->getInternalData()); @@ -323,7 +472,7 @@ int main(int argc, char* argv[]) */ unsigned long int prevTimeInMicroseconds = clock.getTimeMicroseconds(); - + do { @@ -332,20 +481,24 @@ int main(int argc, char* argv[]) app->m_instancingRenderer->init(); DrawGridData dg; dg.upAxis = app->getUpAxis(); - - app->m_instancingRenderer->updateCamera(dg.upAxis); - app->drawGrid(dg); + { + BT_PROFILE("Update Camera"); + app->m_instancingRenderer->updateCamera(dg.upAxis); + } + { + BT_PROFILE("Draw Grid"); + app->drawGrid(dg); + } static int frameCount = 0; frameCount++; if (0) { - char bla[1024]; - - sprintf(bla,"Simple test frame %d", frameCount); - - app->drawText(bla,10,10); + BT_PROFILE("Draw frame counter"); + char bla[1024]; + sprintf(bla,"Frame %d", frameCount); + app->drawText(bla,10,10); } if (sCurrentDemo) @@ -361,18 +514,34 @@ int main(int argc, char* argv[]) sCurrentDemo->stepSimulation(deltaTimeInSeconds);//1./60.f); prevTimeInMicroseconds = curTimeInMicroseconds; } - sCurrentDemo->renderScene(); - sCurrentDemo->physicsDebugDraw(); + { + BT_PROFILE("Render Scene"); + sCurrentDemo->renderScene(); + } + { + sCurrentDemo->physicsDebugDraw(); + } } static int toggle = 1; if (1) { - gui->draw(app->m_instancingRenderer->getScreenWidth(),app->m_instancingRenderer->getScreenHeight()); + if (!pauseSimulation) + processProfileData(profWindow,false); + { + BT_PROFILE("Draw Gwen GUI"); + gui->draw(app->m_instancingRenderer->getScreenWidth(),app->m_instancingRenderer->getScreenHeight()); + } } toggle=1-toggle; - app->m_parameterInterface->syncParameters(); - app->swapBuffer(); + { + BT_PROFILE("Sync Parameters"); + app->m_parameterInterface->syncParameters(); + } + { + BT_PROFILE("Swap Buffers"); + app->swapBuffer(); + } } while (!app->m_window->requestedExit()); // selectDemo(0); diff --git a/Demos3/AllBullet2Demos/premake4.lua b/Demos3/AllBullet2Demos/premake4.lua index 8dd71a8a5..994fd4dbe 100644 --- a/Demos3/AllBullet2Demos/premake4.lua +++ b/Demos3/AllBullet2Demos/premake4.lua @@ -17,12 +17,35 @@ links{"gwen", "OpenGL_Window","OpenGL_TrueTypeFont","BulletSoftBody","BulletDynamics","BulletCollision","LinearMath","lua-5.2.3"} initOpenGL() initGlew() + + if _OPTIONS["midi"] then + if os.is("Windows") then + files {"../../btgui/MidiTest/RtMidi.cpp"} + links {"winmm"} + defines {"__WINDOWS_MM__", "WIN32","B3_USE_MIDI"} + end + + if os.is("Linux") then + defines {"__LINUX_ALSA__"} + links {"asound","pthread"} + end + + if os.is("MacOSX") then + files {"../../btgui/MidiTest/RtMidi.cpp"} + links{"CoreAudio.framework", "coreMIDI.framework", "Cocoa.framework"} + defines {"__MACOSX_CORE__","B3_USE_MIDI"} + end + end files { "**.cpp", "**.h", "../bullet2/BasicDemo/Bullet2RigidBodyDemo.cpp", "../bullet2/BasicDemo/Bullet2RigidBodyDemo.h", + "../bullet2/LuaDemo/LuaPhysicsSetup.cpp", + "../bullet2/LuaDemo/LuaPhysicsSetup.h", + "../DifferentialGearDemo/DifferentialGearSetup.cpp", + "../DifferentialGearDemo/DifferentialGearSetup.h", "../../Demos/BasicDemo/BasicDemoPhysicsSetup.cpp", "../../Demos/BasicDemo/BasicDemoPhysicsSetup.h", "../../Demos/CcdPhysicsDemo/CcdPhysicsSetup.cpp", diff --git a/Demos3/GpuDemos/gwenInternalData.h b/Demos3/GpuDemos/gwenInternalData.h index 11b6be228..97681b463 100644 --- a/Demos3/GpuDemos/gwenInternalData.h +++ b/Demos3/GpuDemos/gwenInternalData.h @@ -39,6 +39,7 @@ struct GwenInternalData Gwen::Controls::TabButton* m_demoPage; Gwen::Controls::TabButton* m_explorerPage; Gwen::Controls::TreeControl* m_explorerTreeCtrl; + Gwen::Controls::MenuItem* m_viewMenu; int m_curYposition; diff --git a/Demos3/GpuDemos/gwenUserInterface.cpp b/Demos3/GpuDemos/gwenUserInterface.cpp index 0646b59e6..4db1347aa 100644 --- a/Demos3/GpuDemos/gwenUserInterface.cpp +++ b/Demos3/GpuDemos/gwenUserInterface.cpp @@ -1,6 +1,11 @@ #include "gwenUserInterface.h" #include "gwenInternalData.h" +#include "Gwen/Controls/ImagePanel.h" + +class MyGraphWindow* graphWindow = 0; + + GwenUserInterface::GwenUserInterface() { @@ -9,7 +14,7 @@ GwenUserInterface::GwenUserInterface() m_data->m_comboBoxCallback = 0; } - + GwenUserInterface::~GwenUserInterface() { for (int i=0;im_handlers.size();i++) @@ -32,43 +37,39 @@ GwenUserInterface::~GwenUserInterface() } + + + class MyMenuItems : public Gwen::Controls::Base { public: - MyMenuItems() :Gwen::Controls::Base(0) - { - } - void myQuitApp( Gwen::Controls::Base* pControl ) - { - exit(0); - } + MyMenuItems() :Gwen::Controls::Base(0) + { + } + void myQuitApp( Gwen::Controls::Base* pControl ) + { + exit(0); + } }; struct MyTestMenuBar : public Gwen::Controls::MenuStrip { - + Gwen::Controls::MenuItem* m_fileMenu; + Gwen::Controls::MenuItem* m_viewMenu; MyTestMenuBar(Gwen::Controls::Base* pParent) :Gwen::Controls::MenuStrip(pParent) { // Gwen::Controls::MenuStrip* menu = new Gwen::Controls::MenuStrip( pParent ); { -MyMenuItems* menuItems = new MyMenuItems; + MyMenuItems* menuItems = new MyMenuItems; - Gwen::Controls::MenuItem* pRoot = AddItem( L"File" ); - pRoot->GetMenu()->AddItem(L"Quit",menuItems,(Gwen::Event::Handler::Function)&MyMenuItems::myQuitApp); - pRoot = AddItem( L"View" ); -// Gwen::Event::Handler* handler = GWEN_MCALL(&MyTestMenuBar::MenuItemSelect ); - pRoot->GetMenu()->AddItem( L"Profiler");//,,m_profileWindow,(Gwen::Event::Handler::Function)&MyProfileWindow::MenuItemSelect); - -/* pRoot->GetMenu()->AddItem( L"New", L"test16.png", GWEN_MCALL( ThisClass::MenuItemSelect ) ); - pRoot->GetMenu()->AddItem( L"Load", L"test16.png", GWEN_MCALL( ThisClass::MenuItemSelect ) ); - pRoot->GetMenu()->AddItem( L"Save", GWEN_MCALL( ThisClass::MenuItemSelect ) ); - pRoot->GetMenu()->AddItem( L"Save As..", GWEN_MCALL( ThisClass::MenuItemSelect ) ); - pRoot->GetMenu()->AddItem( L"Quit", GWEN_MCALL( ThisClass::MenuItemSelect ) ); - */ + m_fileMenu = AddItem( L"File" ); + m_fileMenu->GetMenu()->AddItem(L"Quit",menuItems,(Gwen::Event::Handler::Function)&MyMenuItems::myQuitApp); + m_viewMenu = AddItem( L"View" ); + } } @@ -94,11 +95,11 @@ struct MyComboBoxHander :public Gwen::Event::Handler void onSelect( Gwen::Controls::Base* pControl ) { Gwen::Controls::ComboBox* but = (Gwen::Controls::ComboBox*) pControl; - - - + + + Gwen::String str = Gwen::Utility::UnicodeToString( but->GetSelectedItem()->GetText()); - + if (m_data->m_comboBoxCallback) (*m_data->m_comboBoxCallback)(m_buttonId,str.c_str()); } @@ -137,7 +138,7 @@ void GwenUserInterface::setStatusBarMessage(const char* message, bool isLeft) } else { m_data->m_rightStatusBar->SetText( msg); - + } } @@ -155,6 +156,7 @@ void GwenUserInterface::init(int width, int height,struct sth_stash* stash,float m_data->pCanvas->SetBackgroundColor( Gwen::Color( 150, 170, 170, 255 ) ); MyTestMenuBar* menubar = new MyTestMenuBar(m_data->pCanvas); + m_data->m_viewMenu = menubar->m_viewMenu; Gwen::Controls::StatusBar* bar = new Gwen::Controls::StatusBar(m_data->pCanvas); m_data->m_rightStatusBar = new Gwen::Controls::Label( bar ); m_data->m_rightStatusBar->SetWidth(width/2); @@ -181,7 +183,7 @@ void GwenUserInterface::init(int width, int height,struct sth_stash* stash,float //windowLeft->SetSkin( Gwen::Controls::TabControl* tab = new Gwen::Controls::TabControl(windowRight); - + //tab->SetHeight(300); tab->SetWidth(140); tab->SetHeight(250); @@ -192,20 +194,20 @@ void GwenUserInterface::init(int width, int height,struct sth_stash* stash,float Gwen::UnicodeString str1(L"Params"); m_data->m_demoPage = tab->AddPage(str1); - - + + // Gwen::UnicodeString str2(L"OpenCL"); // tab->AddPage(str2); //Gwen::UnicodeString str3(L"page3"); // tab->AddPage(str3); - - - + + + //but->onPress.Add(handler, &MyHander::onButtonA); - - + + //box->Dock(Gwen::Pos::Left); /*Gwen::Controls::WindowControl* windowBottom = new Gwen::Controls::WindowControl(m_data->pCanvas); @@ -221,8 +223,8 @@ void GwenUserInterface::init(int width, int height,struct sth_stash* stash,float split->SetWidth(300); */ /* - - + + */ Gwen::Controls::ScrollControl* windowLeft = new Gwen::Controls::ScrollControl(m_data->pCanvas); @@ -257,8 +259,9 @@ void GwenUserInterface::init(int width, int height,struct sth_stash* stash,float ctrl->Focus(); ctrl->SetBounds(2, 10, 236, 400); + } - + b3ToggleButtonCallback GwenUserInterface::getToggleButtonCallback() { return m_data->m_toggleButtonCallback; @@ -274,13 +277,13 @@ void GwenUserInterface::registerToggleButton(int buttonId, const char* name) assert(m_data->m_demoPage); Gwen::Controls::Button* but = new Gwen::Controls::Button(m_data->m_demoPage->GetPage()); - + ///some heuristic to find the button location int ypos = m_data->m_curYposition; but->SetPos(10, ypos ); but->SetWidth( 100 ); //but->SetBounds( 200, 30, 300, 200 ); - + MyButtonHander* handler = new MyButtonHander(m_data, buttonId); m_data->m_handlers.push_back(handler); m_data->m_curYposition+=22; @@ -305,7 +308,7 @@ void GwenUserInterface::registerComboBox(int comboboxId, int numItems, const cha Gwen::Controls::ComboBox* combobox = new Gwen::Controls::ComboBox(m_data->m_demoPage->GetPage()); MyComboBoxHander* handler = new MyComboBoxHander(m_data, comboboxId); m_data->m_handlers.push_back(handler); - + combobox->onSelection.Add(handler,&MyComboBoxHander::onSelect); int ypos = m_data->m_curYposition; combobox->SetPos(10, ypos ); @@ -319,14 +322,14 @@ void GwenUserInterface::registerComboBox(int comboboxId, int numItems, const cha } m_data->m_curYposition+=22; - - + + } void GwenUserInterface::draw(int width, int height) { - + // printf("width = %d, height=%d\n", width,height); if (m_data->pCanvas) { @@ -356,7 +359,7 @@ bool GwenUserInterface::mouseMoveCallback( float x, float y) handled = m_data->pCanvas->InputMouseMoved(x,y,m_lastmousepos[0],m_lastmousepos[1]); } return handled; - + } #include "OpenGLWindow/b3gWindowInterface.h" @@ -392,7 +395,7 @@ bool GwenUserInterface::keyboardCallback(int bulletKey, int state) } }; bool bDown = (state == 1); - + return m_data->pCanvas->InputKey(key, bDown); } return false; diff --git a/Demos3/GpuDemos/gwenUserInterface.h b/Demos3/GpuDemos/gwenUserInterface.h index 75f24cb5e..5c7e3ee59 100644 --- a/Demos3/GpuDemos/gwenUserInterface.h +++ b/Demos3/GpuDemos/gwenUserInterface.h @@ -47,5 +47,7 @@ class GwenUserInterface }; + + #endif //_GWEN_USER_INTERFACE_H diff --git a/Demos3/ImportObjDemo/ImportObjSetup.cpp b/Demos3/ImportObjDemo/ImportObjSetup.cpp index 684a25dd2..294788740 100644 --- a/Demos3/ImportObjDemo/ImportObjSetup.cpp +++ b/Demos3/ImportObjDemo/ImportObjSetup.cpp @@ -44,9 +44,7 @@ static GLInstanceGraphicsShape* gCreateGraphicsShapeFromWavefrontObj(std::vector int vtxBaseIndex = vertices->size(); - indicesPtr->push_back(vtxBaseIndex); - indicesPtr->push_back(vtxBaseIndex+1); - indicesPtr->push_back(vtxBaseIndex+2); + GLInstanceVertex vtx0; vtx0.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f]*3+0]; @@ -79,19 +77,28 @@ static GLInstanceGraphicsShape* gCreateGraphicsShapeFromWavefrontObj(std::vector btVector3 v2(vtx2.xyzw[0],vtx2.xyzw[1],vtx2.xyzw[2]); normal = (v1-v0).cross(v2-v0); - normal.normalize(); - vtx0.normal[0] = normal[0]; - vtx0.normal[1] = normal[1]; - vtx0.normal[2] = normal[2]; - vtx1.normal[0] = normal[0]; - vtx1.normal[1] = normal[1]; - vtx1.normal[2] = normal[2]; - vtx2.normal[0] = normal[0]; - vtx2.normal[1] = normal[1]; - vtx2.normal[2] = normal[2]; - vertices->push_back(vtx0); - vertices->push_back(vtx1); - vertices->push_back(vtx2); + btScalar len2 = normal.length2(); + //skip degenerate triangles + if (len2 > SIMD_EPSILON) + { + normal.normalize(); + vtx0.normal[0] = normal[0]; + vtx0.normal[1] = normal[1]; + vtx0.normal[2] = normal[2]; + vtx1.normal[0] = normal[0]; + vtx1.normal[1] = normal[1]; + vtx1.normal[2] = normal[2]; + vtx2.normal[0] = normal[0]; + vtx2.normal[1] = normal[1]; + vtx2.normal[2] = normal[2]; + vertices->push_back(vtx0); + vertices->push_back(vtx1); + vertices->push_back(vtx2); + indicesPtr->push_back(vtxBaseIndex); + indicesPtr->push_back(vtxBaseIndex+1); + indicesPtr->push_back(vtxBaseIndex+2); + } + } } } @@ -113,6 +120,7 @@ static GLInstanceGraphicsShape* gCreateGraphicsShapeFromWavefrontObj(std::vector void ImportObjDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge) { + gfxBridge.setUpAxis(2); this->createEmptyDynamicsWorld(); gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld); m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe); diff --git a/Demos3/ImportSTLDemo/ImportSTLSetup.cpp b/Demos3/ImportSTLDemo/ImportSTLSetup.cpp index 1a6c9e4a4..ae20f3e58 100644 --- a/Demos3/ImportSTLDemo/ImportSTLSetup.cpp +++ b/Demos3/ImportSTLDemo/ImportSTLSetup.cpp @@ -111,6 +111,7 @@ GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName) void ImportSTLDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge) { + gfxBridge.setUpAxis(2); this->createEmptyDynamicsWorld(); gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld); m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe); diff --git a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp index 0665c13a2..b5d9218df 100644 --- a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp +++ b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp @@ -1,921 +1,433 @@ -#include "ImportURDFSetup.h" - -ImportUrdfDemo::ImportUrdfDemo() -{ - -} - -ImportUrdfDemo::~ImportUrdfDemo() -{ - -} - - - - -#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h" -#include -#include - -using namespace urdf; - -void printTree(my_shared_ptr link,int level = 0) -{ - level+=2; - int count = 0; - for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) - { - if (*child) - { - for(int j=0;jname << std::endl; - // first grandchild - printTree(*child,level); - } - else - { - for(int j=0;jname << " has a null child!" << *child << std::endl; - } - } - -} - - -#define MSTRINGIFY(A) #A - - -const char* urdf_char2 = MSTRINGIFY( - - - - - - - - - - - - - - - - - - - - - ); - -const char* urdf_char1 = MSTRINGIFY( - - - - - - - - - - - ); - -const char* urdf_char3 = MSTRINGIFY( - - - - - - - - - - - - - - - - - - - - - - - ); - -const char* urdf_char4 = MSTRINGIFY( - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -); - -const char* urdf_char_r2d2 = MSTRINGIFY( - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -); - -const char* urdf_char = MSTRINGIFY( - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -); - -#include "BulletCollision/CollisionShapes/btCylinderShape.h" - -void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btDiscreteDynamicsWorld* world) -{ - btCollisionShape* shape = 0; - - btTransform linkTransformInWorldSpace; - linkTransformInWorldSpace.setIdentity(); - - btScalar mass = 0; - btTransform inertialFrame; - inertialFrame.setIdentity(); - - if ((*link).inertial) - { - mass = (*link).inertial->mass; - inertialFrame.setOrigin(btVector3((*link).inertial->origin.position.x,(*link).inertial->origin.position.y,(*link).inertial->origin.position.z)); - inertialFrame.setRotation(btQuaternion((*link).inertial->origin.rotation.x,(*link).inertial->origin.rotation.y,(*link).inertial->origin.rotation.z,(*link).inertial->origin.rotation.w)); - } - - if ((*link).parent_joint) - { - btTransform p2j; - const urdf::Vector3 pos = (*link).parent_joint->parent_to_joint_origin_transform.position; - const urdf::Rotation orn = (*link).parent_joint->parent_to_joint_origin_transform.rotation; - btTransform parent2joint; - parent2joint.setOrigin(btVector3(pos.x,pos.y,pos.z)); - parent2joint.setRotation(btQuaternion(orn.x,orn.y,orn.z,orn.w)); - linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint; - } else - { - linkTransformInWorldSpace = parentTransformInWorldSpace; - } - - //btTransform linkCenterOfMass =inertialFrame*linkTransformInWorldSpace; - - { - printf("converting link %s",link->name.c_str()); - for (int v=0;vvisual_array.size();v++) - { - const Visual* visual = link->visual_array[v].get(); - - - switch (visual->geometry->type) - { - case Geometry::CYLINDER: - { - printf("processing a cylinder\n"); - urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get(); - btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.); - btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); - shape = cylZShape; - break; - } - case Geometry::BOX: - { - printf("processing a box\n"); - urdf::Box* box = (urdf::Box*)visual->geometry.get(); - btVector3 extents(box->dim.x,box->dim.y,box->dim.z); - btBoxShape* boxShape = new btBoxShape(extents*0.5f); - shape = boxShape; - break; - } - case Geometry::SPHERE: - { - printf("processing a sphere\n"); - urdf::Sphere* sphere = (urdf::Sphere*)visual->geometry.get(); - btScalar radius = sphere->radius; - btSphereShape* sphereShape = new btSphereShape(radius); - shape = sphereShape; - break; - - break; - } - case Geometry::MESH: - { - break; - } - default: - { - printf("Error: unknown visual geometry type\n"); - } - } - - - - if (shape) - { - gfxBridge.createCollisionShapeGraphicsObject(shape); - - btVector3 color(0,0,1); - if (visual->material.get()) - { - color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a); - } - - btVector3 localInertia(0,0,0); - if (mass) - { - shape->calculateLocalInertia(mass,localInertia); - } - btRigidBody::btRigidBodyConstructionInfo rbci(mass,0,shape,localInertia); - - - btVector3 visual_pos(visual->origin.position.x,visual->origin.position.y,visual->origin.position.z); - btQuaternion visual_orn(visual->origin.rotation.x,visual->origin.rotation.y,visual->origin.rotation.z,visual->origin.rotation.w); - btTransform visual_frame; - visual_frame.setOrigin(visual_pos); - visual_frame.setRotation(visual_orn); - - btTransform visualFrameInWorldSpace =linkTransformInWorldSpace*visual_frame; - rbci.m_startWorldTransform = visualFrameInWorldSpace;//linkCenterOfMass; - - - btRigidBody* body = new btRigidBody(rbci); - world->addRigidBody(body); - gfxBridge.createRigidBodyGraphicsObject(body,color); - } - } - } - - for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) - { - if (*child) - { - URDFvisual2BulletCollisionShape(*child,gfxBridge, linkTransformInWorldSpace, world); - - } - else - { - std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl; - } - } - - - - -} -void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge) -{ - - int upAxis = 2; - gfxBridge.setUpAxis(upAxis); - - this->createEmptyDynamicsWorld(); -/* btVector3 groundHalfExtents(50,50,50); - groundHalfExtents[upAxis]=1.f; - btBoxShape* box = new btBoxShape(groundHalfExtents); - gfxBridge.createCollisionShapeGraphicsObject(box); - btTransform start; start.setIdentity(); - btVector3 groundOrigin(0,0,0); - groundOrigin[upAxis]=-5; - start.setOrigin(groundOrigin); - btRigidBody* body = createRigidBody(0,start,box); - btVector3 color(0,1,0); - gfxBridge.createRigidBodyGraphicsObject(body,color); -*/ - btVector3 gravity(0,0,0); - gravity[upAxis]=-9.8; - - m_dynamicsWorld->setGravity(gravity); - int argc=0; - char* filename="somefile.urdf"; - - std::string xml_string; - - if (argc < 2){ - std::cerr << "No URDF file name provided, using a dummy test URDF" << std::endl; - - xml_string = std::string(urdf_char); - - } else - { - - - std::fstream xml_file(filename, std::fstream::in); - while ( xml_file.good() ) - { - std::string line; - std::getline( xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - } - - my_shared_ptr robot = parseURDF(xml_string); - if (!robot){ - std::cerr << "ERROR: Model Parsing the xml failed" << std::endl; - return ; - } - std::cout << "robot name is: " << robot->getName() << std::endl; - - // get info from parser - std::cout << "---------- Successfully Parsed XML ---------------" << std::endl; - // get root link - my_shared_ptr root_link=robot->getRoot(); - if (!root_link) return ; - - std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl; - - // print entire tree - printTree(root_link); - btTransform worldTrans; - worldTrans.setIdentity(); - - { - URDFvisual2BulletCollisionShape(root_link, gfxBridge, worldTrans,m_dynamicsWorld); - } - - -} + +#include "ImportURDFSetup.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" + +static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter; +static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter); +static bool enableConstraints = true;//false; + + +ImportUrdfDemo::ImportUrdfDemo() +{ + +} + +ImportUrdfDemo::~ImportUrdfDemo() +{ + +} + + + + +#include "urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h" + +#include "urdf_samples.h" + +//#include "BulletCollision/CollisionShapes/btCylinderShape.h" +//#define USE_BARREL_VERTICES +//#include "OpenGLWindow/ShapeData.h" + +#include +#include + +using namespace urdf; + +void printTree(my_shared_ptr link,int level = 0) +{ + level+=2; + int count = 0; + for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) + { + if (*child) + { + for(int j=0;jname << std::endl; + // first grandchild + printTree(*child,level); + } + else + { + for(int j=0;jname << " has a null child!" << *child << std::endl; + } + } + +} + + +struct URDF_LinkInformation +{ + const Link* m_thisLink; + + btTransform m_localInertialFrame; + btTransform m_localVisualFrame; + + btRigidBody* m_bulletRigidBody; + virtual ~URDF_LinkInformation() + { +printf("~\n"); + } +}; + +struct URDF_JointInformation +{ + +}; + + +struct URDF2BulletMappings +{ + btHashMap m_link2rigidbody; + btHashMap m_joint2Constraint; +}; + +void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btDiscreteDynamicsWorld* world, URDF2BulletMappings& mappings) +{ + btCollisionShape* shape = 0; + + btTransform linkTransformInWorldSpace; + linkTransformInWorldSpace.setIdentity(); + + btScalar mass = 1; + btTransform inertialFrame; + inertialFrame.setIdentity(); + const Link* parentLink = (*link).getParent(); + URDF_LinkInformation* pp = 0; + + { + URDF_LinkInformation** ppRigidBody = mappings.m_link2rigidbody.find(parentLink); + if (ppRigidBody) + { + pp = (*ppRigidBody); + btRigidBody* parentRigidBody = pp->m_bulletRigidBody; + btTransform tr = parentRigidBody->getWorldTransform(); + printf("rigidbody origin (COM) of link(%s) parent(%s): %f,%f,%f\n",(*link).name.c_str(), parentLink->name.c_str(), tr.getOrigin().x(), tr.getOrigin().y(), tr.getOrigin().z()); + } + } + if ((*link).inertial) + { + mass = (*link).inertial->mass; + inertialFrame.setOrigin(btVector3((*link).inertial->origin.position.x,(*link).inertial->origin.position.y,(*link).inertial->origin.position.z)); + inertialFrame.setRotation(btQuaternion((*link).inertial->origin.rotation.x,(*link).inertial->origin.rotation.y,(*link).inertial->origin.rotation.z,(*link).inertial->origin.rotation.w)); + } + + btTransform parent2joint; + + if ((*link).parent_joint) + { + btTransform p2j; + const urdf::Vector3 pos = (*link).parent_joint->parent_to_joint_origin_transform.position; + const urdf::Rotation orn = (*link).parent_joint->parent_to_joint_origin_transform.rotation; + + parent2joint.setOrigin(btVector3(pos.x,pos.y,pos.z)); + parent2joint.setRotation(btQuaternion(orn.x,orn.y,orn.z,orn.w)); + linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint; + } else + { + linkTransformInWorldSpace = parentTransformInWorldSpace; + } + + + { + printf("converting link %s",link->name.c_str()); + for (int v=0;vvisual_array.size();v++) + { + const Visual* visual = link->visual_array[v].get(); + + + switch (visual->geometry->type) + { + case Geometry::CYLINDER: + { + printf("processing a cylinder\n"); + urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get(); + + btAlignedObjectArray vertices; + //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); + int numSteps = 32; + for (int i=0;iradius*btSin(SIMD_2_PI*(float(i)/numSteps)),cyl->radius*btCos(SIMD_2_PI*(float(i)/numSteps)),cyl->length/2.); + vertices.push_back(vert); + vert[2] = -cyl->length/2.; + vertices.push_back(vert); + + } + btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); + cylZShape->initializePolyhedralFeatures(); + //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.); + //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); + cylZShape->setMargin(0.001); + + shape = cylZShape; + break; + } + case Geometry::BOX: + { + printf("processing a box\n"); + urdf::Box* box = (urdf::Box*)visual->geometry.get(); + btVector3 extents(box->dim.x,box->dim.y,box->dim.z); + btBoxShape* boxShape = new btBoxShape(extents*0.5f); + shape = boxShape; + break; + } + case Geometry::SPHERE: + { + printf("processing a sphere\n"); + urdf::Sphere* sphere = (urdf::Sphere*)visual->geometry.get(); + btScalar radius = sphere->radius*0.8; + btSphereShape* sphereShape = new btSphereShape(radius); + shape = sphereShape; + break; + + break; + } + case Geometry::MESH: + { + break; + } + default: + { + printf("Error: unknown visual geometry type\n"); + } + } + + + + if (shape) + { + gfxBridge.createCollisionShapeGraphicsObject(shape); + + btVector3 color(0,0,1); + if (visual->material.get()) + { + color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a); + } + + btVector3 localInertia(0,0,0); + if (mass) + { + shape->calculateLocalInertia(mass,localInertia); + } + btRigidBody::btRigidBodyConstructionInfo rbci(mass,0,shape,localInertia); + + + btVector3 visual_pos(visual->origin.position.x,visual->origin.position.y,visual->origin.position.z); + btQuaternion visual_orn(visual->origin.rotation.x,visual->origin.rotation.y,visual->origin.rotation.z,visual->origin.rotation.w); + btTransform visual_frame; + visual_frame.setOrigin(visual_pos); + visual_frame.setRotation(visual_orn); + + btTransform visualFrameInWorldSpace =linkTransformInWorldSpace*visual_frame; + rbci.m_startWorldTransform = visualFrameInWorldSpace;//linkCenterOfMass; + + + btRigidBody* body = new btRigidBody(rbci); + + world->addRigidBody(body,bodyCollisionFilterGroup,bodyCollisionFilterMask); + // body->setFriction(0); + + gfxBridge.createRigidBodyGraphicsObject(body,color); + URDF_LinkInformation* linkInfo = new URDF_LinkInformation; + linkInfo->m_bulletRigidBody = body; + linkInfo->m_localVisualFrame =visual_frame; + linkInfo->m_localInertialFrame =inertialFrame; + linkInfo->m_thisLink = link.get(); + const Link* p = link.get(); + mappings.m_link2rigidbody.insert(p, linkInfo); + + //create a joint if necessary + if ((*link).parent_joint) + { + btRigidBody* parentBody =pp->m_bulletRigidBody; + + const Joint* pj = (*link).parent_joint.get(); + btTransform offsetInA,offsetInB; + btTransform p2j; p2j.setIdentity(); + btVector3 p2jPos; p2jPos.setValue(pj->parent_to_joint_origin_transform.position.x, + pj->parent_to_joint_origin_transform.position.y, + pj->parent_to_joint_origin_transform.position.z); + btQuaternion p2jOrn;p2jOrn.setValue(pj->parent_to_joint_origin_transform.rotation.x, + pj->parent_to_joint_origin_transform.rotation.y, + pj->parent_to_joint_origin_transform.rotation.z, + pj->parent_to_joint_origin_transform.rotation.w); + + p2j.setOrigin(p2jPos); + p2j.setRotation(p2jOrn); + + offsetInA.setIdentity(); + + offsetInA = pp->m_localVisualFrame.inverse()*p2j; + offsetInB.setIdentity(); + offsetInB = visual_frame.inverse(); + + switch (pj->type) + { + case Joint::FIXED: + { + printf("Fixed joint\n"); + btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentBody, *body,offsetInA,offsetInB); +// btVector3 bulletAxis(pj->axis.x,pj->axis.y,pj->axis.z); + dof6->setLinearLowerLimit(btVector3(0,0,0)); + dof6->setLinearUpperLimit(btVector3(0,0,0)); + + dof6->setAngularLowerLimit(btVector3(0,0,0)); + dof6->setAngularUpperLimit(btVector3(0,0,0)); + + if (enableConstraints) + world->addConstraint(dof6,true); + +// btFixedConstraint* fixed = new btFixedConstraint(*parentBody, *body,offsetInA,offsetInB); + // world->addConstraint(fixed,true); + break; + } + case Joint::CONTINUOUS: + case Joint::REVOLUTE: + { + btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentBody, *body,offsetInA,offsetInB); +// btVector3 bulletAxis(pj->axis.x,pj->axis.y,pj->axis.z); + dof6->setLinearLowerLimit(btVector3(0,0,0)); + dof6->setLinearUpperLimit(btVector3(0,0,0)); + + dof6->setAngularLowerLimit(btVector3(0,0,1000)); + dof6->setAngularUpperLimit(btVector3(0,0,-1000)); + + if (enableConstraints) + world->addConstraint(dof6,true); + + printf("Revolute/Continuous joint\n"); + break; + } + case Joint::PRISMATIC: + { + btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentBody, *body,offsetInA,offsetInB); + + dof6->setLinearLowerLimit(btVector3(pj->limits->lower,0,0)); + dof6->setLinearUpperLimit(btVector3(pj->limits->upper,0,0)); + + dof6->setAngularLowerLimit(btVector3(0,0,0)); + dof6->setAngularUpperLimit(btVector3(0,0,0)); + + if (enableConstraints) + world->addConstraint(dof6,true); + + printf("Prismatic\n"); + break; + } + default: + { + printf("Error: unsupported joint type in URDF (%d)\n", pj->type); + } + } + + } + } + } + } + + for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) + { + if (*child) + { + URDFvisual2BulletCollisionShape(*child,gfxBridge, linkTransformInWorldSpace, world,mappings); + + } + else + { + std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl; + } + } + + + + +} +void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge) +{ + + int upAxis = 2; + gfxBridge.setUpAxis(2); + + this->createEmptyDynamicsWorld(); + gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld); + m_dynamicsWorld->getDebugDrawer()->setDebugMode( + //btIDebugDraw::DBG_DrawConstraints + +btIDebugDraw::DBG_DrawContactPoints + //+btIDebugDraw::DBG_DrawAabb + );//+btIDebugDraw::DBG_DrawConstraintLimits); + + + + btVector3 gravity(0,0,0); + gravity[upAxis]=-9.8; + + m_dynamicsWorld->setGravity(gravity); + int argc=0; + char* filename="somefile.urdf"; + + std::string xml_string; + + if (argc < 2){ + std::cerr << "No URDF file name provided, using a dummy test URDF" << std::endl; + + xml_string = std::string(urdf_char); + + } else + { + + + std::fstream xml_file(filename, std::fstream::in); + while ( xml_file.good() ) + { + std::string line; + std::getline( xml_file, line); + xml_string += (line + "\n"); + } + xml_file.close(); + } + + my_shared_ptr robot = parseURDF(xml_string); + if (!robot){ + std::cerr << "ERROR: Model Parsing the xml failed" << std::endl; + return ; + } + std::cout << "robot name is: " << robot->getName() << std::endl; + + // get info from parser + std::cout << "---------- Successfully Parsed XML ---------------" << std::endl; + // get root link + my_shared_ptr root_link=robot->getRoot(); + if (!root_link) return ; + + std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl; + + // print entire tree + printTree(root_link); + btTransform worldTrans; + worldTrans.setIdentity(); + + { + URDF2BulletMappings mappings; + URDFvisual2BulletCollisionShape(root_link, gfxBridge, worldTrans,m_dynamicsWorld,mappings); + } + + { + btVector3 groundHalfExtents(20,20,20); + groundHalfExtents[upAxis]=1.f; + btBoxShape* box = new btBoxShape(groundHalfExtents); + box->initializePolyhedralFeatures(); + + gfxBridge.createCollisionShapeGraphicsObject(box); + btTransform start; start.setIdentity(); + btVector3 groundOrigin(0,0,0); + groundOrigin[upAxis]=-1.5; + start.setOrigin(groundOrigin); + btRigidBody* body = createRigidBody(0,start,box); + btVector3 color(0.5,0.5,0.5); + gfxBridge.createRigidBodyGraphicsObject(body,color); + } + + +} diff --git a/Demos3/ImportURDFDemo/urdf_samples.h b/Demos3/ImportURDFDemo/urdf_samples.h new file mode 100644 index 000000000..7a5253273 --- /dev/null +++ b/Demos3/ImportURDFDemo/urdf_samples.h @@ -0,0 +1,671 @@ +#ifndef URDF_SAMPLES_H +#define URDF_SAMPLES_H + +#define MSTRINGIFY(A) #A + + +const char* urdf_char2 = MSTRINGIFY( + + + + + + + + + + + + + + + + + + + + + ); + +const char* urdf_char1 = MSTRINGIFY( + + + + + + + + + + + ); + +const char* urdf_char3 = MSTRINGIFY( + + + + + + + + + + + + + + + + + + + + + + + ); + +const char* urdf_char4 = MSTRINGIFY( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +); + +const char* urdf_char_r2d2 = MSTRINGIFY( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +); + +const char* urdf_char = MSTRINGIFY( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +); + +#endif //URDF_SAMPLES_H + diff --git a/Demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.cpp b/Demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.cpp index 1ca85714a..016737643 100644 --- a/Demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.cpp +++ b/Demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.cpp @@ -167,6 +167,7 @@ Bullet2RigidBodyDemo::Bullet2RigidBodyDemo(SimpleOpenGL3App* app, CommonPhysicsS void Bullet2RigidBodyDemo::initPhysics() { MyGraphicsPhysicsBridge glBridge(m_glApp); + glBridge.setUpAxis(1); m_physicsSetup->initPhysics(glBridge); m_glApp->m_instancingRenderer->writeTransforms(); diff --git a/Demos3/bullet2/BasicDemo/MyDebugDrawer.h b/Demos3/bullet2/BasicDemo/MyDebugDrawer.h index 54c4387e8..eb95e2265 100644 --- a/Demos3/bullet2/BasicDemo/MyDebugDrawer.h +++ b/Demos3/bullet2/BasicDemo/MyDebugDrawer.h @@ -60,7 +60,9 @@ public: virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color) { + drawLine(PointOnB,PointOnB+normalOnB,color); } + virtual void reportErrorWarning(const char* warningString) { diff --git a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp index 81b1dc718..75b6b9cf2 100644 --- a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp +++ b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp @@ -599,7 +599,10 @@ void FeatherstoneDemo1::renderScene() btVector3 pos = col->getWorldTransform().getOrigin(); btQuaternion orn = col->getWorldTransform().getRotation(); int index = col->getUserIndex(); - m_glApp->m_instancingRenderer->writeSingleInstanceTransformToCPU(pos,orn,index); + if (index>=0) + { + m_glApp->m_instancingRenderer->writeSingleInstanceTransformToCPU(pos,orn,index); + } } m_glApp->m_instancingRenderer->writeTransforms(); } diff --git a/Demos3/bullet2/LuaDemo/LuaPhysicsSetup.cpp b/Demos3/bullet2/LuaDemo/LuaPhysicsSetup.cpp new file mode 100644 index 000000000..ad87a0a50 --- /dev/null +++ b/Demos3/bullet2/LuaDemo/LuaPhysicsSetup.cpp @@ -0,0 +1,467 @@ +#include "LuaPhysicsSetup.h" + +#include "OpenGLWindow/SimpleOpenGL3App.h"//?? +#include "btBulletDynamicsCommon.h" +#include "LinearMath/btVector3.h" +#include + +#include "BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h" + +extern "C" { + #include "lua.h" + #include "lualib.h" +#include "lauxlib.h" +} + + +char* sLuaFileName = "init_physics.lua"; + +static const float scaling=0.35f; +static LuaPhysicsSetup* sLuaDemo = 0; + +static btVector4 colors[4] = +{ + btVector4(1,0,0,1), + btVector4(0,1,0,1), + btVector4(0,1,1,1), + btVector4(1,1,0,1), +}; + +LuaPhysicsSetup::LuaPhysicsSetup(class SimpleOpenGL3App* app) +:m_glApp(app), +m_config(0), +m_dispatcher(0), +m_bp(0), +m_solver(0), +m_dynamicsWorld(0) +{ + sLuaDemo = this; +} + +LuaPhysicsSetup::~LuaPhysicsSetup() +{ + sLuaDemo = 0; +} + + + +//todo: allow to create solver, broadphase, multiple worlds etc. +static int gCreateDefaultDynamicsWorld(lua_State *L) +{ + sLuaDemo->m_config = new btDefaultCollisionConfiguration; + sLuaDemo->m_dispatcher = new btCollisionDispatcher(sLuaDemo->m_config); + sLuaDemo->m_bp = new btDbvtBroadphase(); + sLuaDemo->m_solver = new btNNCGConstraintSolver(); + sLuaDemo->m_dynamicsWorld = new btDiscreteDynamicsWorld(sLuaDemo->m_dispatcher,sLuaDemo->m_bp,sLuaDemo->m_solver,sLuaDemo->m_config); + lua_pushlightuserdata (L, sLuaDemo->m_dynamicsWorld); + return 1; +} + + +static int gDeleteDynamicsWorld(lua_State *L) +{ + return 0; +} + +ATTRIBUTE_ALIGNED16(struct) CustomShapeData +{ + btVector3 m_localScaling; + int m_shapeIndex; + + +}; + + +ATTRIBUTE_ALIGNED16(struct) CustomRigidBodyData +{ + int m_graphicsInstanceIndex; +}; + +static int gCreateCubeShape(lua_State *L) +{ + int argc = lua_gettop(L); + if (argc==4) + { + btVector3 halfExtents(1,1,1); + if (!lua_isuserdata(L,1)) + { + std::cerr << "error: first argument to createCubeShape should be world"; + return 0; + } + //expect userdata = sLuaDemo->m_dynamicsWorld + halfExtents = btVector3(lua_tonumber(L,2),lua_tonumber(L,3),lua_tonumber(L,4)); + btCollisionShape* colShape = new btBoxShape(halfExtents); + + CustomShapeData* shapeData = new CustomShapeData(); + shapeData->m_shapeIndex = sLuaDemo->m_glApp->registerCubeShape(); + shapeData->m_localScaling = halfExtents; + + colShape->setUserPointer(shapeData); + lua_pushlightuserdata (L, colShape); + return 1; + } else + { + std::cerr << "Error: invalid number of arguments to createCubeShape, expected 4 (world,halfExtentsX,halfExtentsY,halfExtentsX) but got " << argc; + } + return 0; +} + +static int gCreateSphereShape(lua_State *L) +{ + int argc = lua_gettop(L); + if (argc==2) + { + btVector3 halfExtents(1,1,1); + if (!lua_isuserdata(L,1)) + { + std::cerr << "error: first argument to createSphereShape should be world"; + return 0; + } + //expect userdata = sLuaDemo->m_dynamicsWorld + btScalar radius = lua_tonumber(L,2); + btCollisionShape* colShape = new btSphereShape(radius); + + CustomShapeData* shapeData = new CustomShapeData(); + shapeData->m_shapeIndex = sLuaDemo->m_glApp->registerGraphicsSphereShape(radius,false,100,0.5); + shapeData->m_localScaling = halfExtents; + + colShape->setUserPointer(shapeData); + lua_pushlightuserdata (L, colShape); + return 1; + } else + { + std::cerr << "Error: invalid number of arguments to createSphereShape, expected 2 (world,radius) but got " << argc; + } + return 0; +} + +int luaL_returnlen(lua_State* L, int index) +{ + lua_len(L, index); + int len = lua_tointeger(L,-1); + lua_pop(L, 1); + return len; +} + +btVector3 getLuaVectorArg(lua_State* L, int index) +{ + btVector3 pos(0,0,0); + + int sz = luaL_returnlen(L, index); // get size of table + { + lua_rawgeti(L, index, 1); // push t[i] + pos[0] = lua_tonumber(L,-1); + lua_pop(L, 1); + lua_rawgeti(L, index, 2); // push t[i] + pos[1] = lua_tonumber(L,-1); + lua_pop(L, 1); + lua_rawgeti(L, index, 3); // push t[i] + pos[2] = lua_tonumber(L,-1); + lua_pop(L, 1); + } + return pos; +} + +btQuaternion getLuaQuaternionArg(lua_State* L, int index) +{ + btQuaternion orn(0,0,0,1); + + int sz = luaL_returnlen(L, index); // get size of table + { + lua_rawgeti(L, index, 1); // push t[i] + orn[0] = lua_tonumber(L,-1); + lua_pop(L, 1); + lua_rawgeti(L, index, 2); // push t[i] + orn[1] = lua_tonumber(L,-1); + lua_pop(L, 1); + lua_rawgeti(L, index, 3); // push t[i] + orn[2] = lua_tonumber(L,-1); + lua_pop(L, 1); + lua_rawgeti(L, index, 4); // push t[i] + orn[3] = lua_tonumber(L,-1); + lua_pop(L, 1); + } + return orn; +} + + + + +static int gCreateRigidBody (lua_State *L) +{ + int argc = lua_gettop(L); + if (argc==5) + { + + btTransform startTransform; + startTransform.setIdentity(); + + + if (!lua_isuserdata(L,1)) + { + std::cerr << "error: first argument to b3CreateRigidbody should be world"; + return 0; + } + btDiscreteDynamicsWorld* world = (btDiscreteDynamicsWorld*) lua_touserdata(L,1); + if (world != sLuaDemo->m_dynamicsWorld) + { + std::cerr << "error: first argument expected to be a world"; + return 0; + } + + if (!lua_isuserdata(L,2)) + { + std::cerr << "error: second argument to b3CreateRigidbody should be world"; + return 0; + } + + btScalar mass = lua_tonumber(L,3); + + luaL_checktype(L,4, LUA_TTABLE); + + btVector3 pos = getLuaVectorArg(L,4); + + btQuaternion orn = getLuaQuaternionArg(L,5); + + btCollisionShape* colShape = (btCollisionShape* )lua_touserdata(L,2); + //expect userdata = sLuaDemo->m_dynamicsWorld + + btVector3 inertia(0,0,0); + if (mass) + { + colShape->calculateLocalInertia(mass,inertia); + } + + + + btRigidBody* body = new btRigidBody(mass,0,colShape,inertia); + body->getWorldTransform().setOrigin(pos); + body->getWorldTransform().setRotation(orn); + + + CustomShapeData* shapeData = (CustomShapeData*)colShape->getUserPointer(); + if (shapeData) + { + CustomRigidBodyData* rbd = new CustomRigidBodyData; + static int curColor = 0; + btVector4 color = colors[curColor]; + curColor++; + curColor&=3; + + CustomShapeData* shapeData = (CustomShapeData*)body->getCollisionShape()->getUserPointer(); + if (shapeData) + { + + rbd ->m_graphicsInstanceIndex = sLuaDemo->m_glApp->m_instancingRenderer->registerGraphicsInstance(shapeData->m_shapeIndex,startTransform.getOrigin(),startTransform.getRotation(),color,shapeData->m_localScaling); + body->setUserIndex(rbd->m_graphicsInstanceIndex); + } + } + + world->addRigidBody(body); + lua_pushlightuserdata (L, body); + return 1; + } else + { + std::cerr << "Error: invalid number of arguments to createRigidBody, expected 5 (world,shape,mass,pos,orn) but got " << argc; + } + return 0; +} + +static int gSetBodyPosition(lua_State *L) +{ + int argc = lua_gettop(L); + if (argc==3) + { + if (!lua_isuserdata(L,1)) + { + std::cerr << "error: first argument needs to be a world"; + return 0; + } + if (!lua_isuserdata(L,2)) + { + std::cerr << "error: second argument needs to be a body"; + return 0; + } + btRigidBody* body = (btRigidBody*)lua_touserdata(L,2); + btVector3 pos = getLuaVectorArg(L,3); + + btTransform& tr = body ->getWorldTransform(); + tr.setOrigin(pos); + body->setWorldTransform(tr); + } else + { + std::cerr << "error: setBodyPosition expects 6 arguments like setBodyPosition(world,body,0,1,0)"; + } + return 0; +} + +static int gSetBodyOrientation(lua_State *L) +{ + int argc = lua_gettop(L); + if (argc==3) + { + if (!lua_isuserdata(L,1)) + { + std::cerr << "error: first argument needs to be a world"; + return 0; + } + if (!lua_isuserdata(L,2)) + { + std::cerr << "error: second argument needs to be a body"; + return 0; + } + btRigidBody* body = (btRigidBody*)lua_touserdata(L,2); + btQuaternion orn = getLuaQuaternionArg(L,3); + btTransform& tr = body ->getWorldTransform(); + tr.setRotation(orn); + body->setWorldTransform(tr); + } else + { + std::cerr << "error: setBodyOrientation expects 3 arguments like setBodyOrientation(world,body,orn)"; + } + return 0; +} + +//b3CreateConvexShape(world, points) + +//b3CreateHingeConstraint(world,bodyA,bodyB,...) + +static void report_errors(lua_State *L, int status) +{ + if ( status!=0 ) { + std::cerr << "-- " << lua_tostring(L, -1) << std::endl; + lua_pop(L, 1); // remove error message + } +} + + + +void LuaPhysicsSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) +{ + const char* prefix[]={"./","./data/","../data/","../../data/","../../../data/","../../../../data/"}; + int numPrefixes = sizeof(prefix)/sizeof(const char*); + char relativeFileName[1024]; + FILE* f=0; + int result = 0; + + for (int i=0;!f && im_instancingRenderer->writeTransforms(); +} + + +void LuaPhysicsSetup::exitPhysics() +{ + delete m_dynamicsWorld; + m_dynamicsWorld=0; + delete m_dispatcher; + m_dispatcher=0; + delete m_bp; + m_bp=0; + delete m_config; + m_config=0; +} + +void LuaPhysicsSetup::stepSimulation(float deltaTime) +{ + if (m_dynamicsWorld) + m_dynamicsWorld->stepSimulation(deltaTime); + +} + +void LuaPhysicsSetup::debugDraw() +{ + if (m_dynamicsWorld) + m_dynamicsWorld->debugDrawWorld(); + +} + +bool LuaPhysicsSetup::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) +{ +//btAssert(0); +return false; +} +bool LuaPhysicsSetup::movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) +{ +//btAssert(0); +return false; + +} +void LuaPhysicsSetup::removePickingConstraint() +{ +//btAssert(0); + +} + +void LuaPhysicsSetup::syncPhysicsToGraphics(GraphicsPhysicsBridge& gfxBridge) +{ + int numCollisionObjects = m_dynamicsWorld->getNumCollisionObjects(); + for (int i = 0; igetCollisionObjectArray()[i]; + btVector3 pos = colObj->getWorldTransform().getOrigin(); + btQuaternion orn = colObj->getWorldTransform().getRotation(); + int index = colObj->getUserIndex(); + if (index >= 0) + { + m_glApp->m_instancingRenderer->writeSingleInstanceTransformToCPU(pos, orn, index); + } + } + m_glApp->m_instancingRenderer->writeTransforms(); +} + +btRigidBody* LuaPhysicsSetup::createRigidBody(float mass, const btTransform& startTransform,btCollisionShape* shape, const btVector4& color) +{ +btAssert(0); +return 0; + +} + +btBoxShape* LuaPhysicsSetup::createBoxShape(const btVector3& halfExtents) +{ +btAssert(0); +return 0; +} diff --git a/Demos3/bullet2/LuaDemo/LuaPhysicsSetup.h b/Demos3/bullet2/LuaDemo/LuaPhysicsSetup.h new file mode 100644 index 000000000..cad0cbd34 --- /dev/null +++ b/Demos3/bullet2/LuaDemo/LuaPhysicsSetup.h @@ -0,0 +1,43 @@ +#ifndef _LUA_PHYSICS_SETUP_H +#define _LUA_PHYSICS_SETUP_H + +#include "../Demos/CommonPhysicsSetup.h" + +//we don't derive from CommonRigidBodySetup because we +//create and own our own dynamics world (one or more) +//at run-time +struct LuaPhysicsSetup : public CommonPhysicsSetup +{ + + LuaPhysicsSetup(class SimpleOpenGL3App* app); + virtual ~LuaPhysicsSetup(); + + class btDefaultCollisionConfiguration* m_config; + class btCollisionDispatcher* m_dispatcher; + class btDbvtBroadphase* m_bp; + class btNNCGConstraintSolver* m_solver; + class btDiscreteDynamicsWorld* m_dynamicsWorld; + class SimpleOpenGL3App* m_glApp; + + virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge); + + virtual void exitPhysics(); + + virtual void stepSimulation(float deltaTime); + + virtual void debugDraw(); + + virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld); + virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld); + virtual void removePickingConstraint(); + + virtual void syncPhysicsToGraphics(GraphicsPhysicsBridge& gfxBridge); + + virtual btRigidBody* createRigidBody(float mass, const btTransform& startTransform,btCollisionShape* shape, const btVector4& color=btVector4(1,0,0,1)); + + virtual btBoxShape* createBoxShape(const btVector3& halfExtents); + +}; + + +#endif //_LUA_PHYSICS_SETUP_H diff --git a/btgui/Gwen/Texture.h b/btgui/Gwen/Texture.h index 876cb7386..aac95d4c3 100644 --- a/btgui/Gwen/Texture.h +++ b/btgui/Gwen/Texture.h @@ -22,6 +22,8 @@ namespace Gwen { TextObject name; void* data; + int m_intData; + bool failed; int width; int height; @@ -29,6 +31,7 @@ namespace Gwen Texture() { data = NULL; + m_intData = 0; width = 4; height = 4; failed = false; diff --git a/btgui/GwenOpenGLTest/UnitTest.cpp b/btgui/GwenOpenGLTest/UnitTest.cpp index 677d32884..9886c6eb3 100644 --- a/btgui/GwenOpenGLTest/UnitTest.cpp +++ b/btgui/GwenOpenGLTest/UnitTest.cpp @@ -20,6 +20,7 @@ GWEN_CONTROL_CONSTRUCTOR( UnitTest ) SetSize( 600, 450 ); + m_TabControl = new Controls::TabControl( this ); m_TabControl->Dock( Pos::Fill ); m_TabControl->SetMargin( Margin( 2, 2, 2, 2 ) ); @@ -29,6 +30,8 @@ GWEN_CONTROL_CONSTRUCTOR( UnitTest ) m_TextOutput->SetHeight( 100 ); + ADD_UNIT_TEST( ImagePanel ); + //ADD_UNIT_TEST( MenuStrip ); Gwen::UnicodeString str1(L"testje"); @@ -121,7 +124,6 @@ GWEN_CONTROL_CONSTRUCTOR( UnitTest ) ADD_UNIT_TEST( Slider ); ADD_UNIT_TEST( ProgressBar ); ADD_UNIT_TEST( RadioButton2 ); - ADD_UNIT_TEST( ImagePanel ); ADD_UNIT_TEST( Label ); ADD_UNIT_TEST( Checkbox ); diff --git a/btgui/OpenGLWindow/GwenOpenGL3CoreRenderer.h b/btgui/OpenGLWindow/GwenOpenGL3CoreRenderer.h index cb3f06e20..630f20c38 100644 --- a/btgui/OpenGLWindow/GwenOpenGL3CoreRenderer.h +++ b/btgui/OpenGLWindow/GwenOpenGL3CoreRenderer.h @@ -7,9 +7,26 @@ #include "GLPrimitiveRenderer.h" struct sth_stash; #include "../OpenGLTrueTypeFont/fontstash.h" +#include "Gwen/Texture.h" + #include "TwFonts.h" static float extraSpacing = 0.;//6f; #include +#include + +template +inline void MyClamp(T& a, const T& lb, const T& ub) +{ + if (a < lb) + { + a = lb; + } + else if (ub < a) + { + a = ub; + } +} + static GLuint BindFont(const CTexFont *_Font) { @@ -32,7 +49,14 @@ static GLuint BindFont(const CTexFont *_Font) return TexID; } - +struct MyTextureLoader +{ + virtual ~MyTextureLoader() + { + } + virtual void LoadTexture( Gwen::Texture* pTexture ) = 0; + virtual void FreeTexture( Gwen::Texture* pTexture )=0; +}; class GwenOpenGL3CoreRenderer : public Gwen::Renderer::Base { @@ -48,6 +72,7 @@ class GwenOpenGL3CoreRenderer : public Gwen::Renderer::Base const CTexFont* m_currentFont; GLuint m_fontTextureId; + MyTextureLoader* m_textureLoader; public: GwenOpenGL3CoreRenderer (GLPrimitiveRenderer* primRender, sth_stash* font,float screenWidth, float screenHeight, float retinaScale) :m_primitiveRenderer(primRender), @@ -55,7 +80,8 @@ public: m_screenWidth(screenWidth), m_screenHeight(screenHeight), m_retinaScale(retinaScale), - m_useTrueTypeFont(false) + m_useTrueTypeFont(false), + m_textureLoader(0) { ///only enable true type fonts on Macbook Retina, it looks gorgeous if (retinaScale==2.0f) @@ -315,7 +341,76 @@ public: return Gwen::Renderer::Base::MeasureText(pFont,text); } + void setTextureLoader(MyTextureLoader* loader) + { + m_textureLoader = loader; + } + + virtual void LoadTexture( Gwen::Texture* pTexture ) + { + if (m_textureLoader) + m_textureLoader->LoadTexture(pTexture); + } + virtual void FreeTexture( Gwen::Texture* pTexture ) + { + if (m_textureLoader) + m_textureLoader->FreeTexture(pTexture); + } + + + virtual void DrawTexturedRect( Gwen::Texture* pTexture, Gwen::Rect rect, float u1=0.0f, float v1=0.0f, float u2=1.0f, float v2=1.0f ) + { + + Translate( rect ); + + //float eraseColor[4] = {0,0,0,0}; + //m_primitiveRenderer->drawRect(rect.x, rect.y+m_yOffset, rect.x+rect.w, rect.y+rect.h+m_yOffset, eraseColor); + + GLint texHandle = (GLint) pTexture->m_intData; + //if (!texHandle) + // return; + + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D,texHandle); +// glDisable(GL_DEPTH_TEST); + + GLint err; + + err = glGetError(); + assert(err==GL_NO_ERROR); + + +/* bool useFiltering = true; + if (useFiltering) + { + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); + } else + { + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + } + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT); +*/ + + //glEnable(GL_TEXTURE_2D); + + + +// glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE ); + static float add=0.0; + //add+=1./512.;//0.01; + float color[4]={1,1,1,1}; + + m_primitiveRenderer->drawTexturedRect(rect.x, rect.y+m_yOffset, rect.x+rect.w, rect.y+rect.h+m_yOffset, color,0+add,0,1+add,1,true); + + + err = glGetError(); + assert(err==GL_NO_ERROR); + + + } }; #endif //__GWEN_OPENGL3_CORE_RENDERER_H diff --git a/build3/findOpenGLGlewGlut.lua b/build3/findOpenGLGlewGlut.lua index bf0dfb42e..d2b495a00 100644 --- a/build3/findOpenGLGlewGlut.lua +++ b/build3/findOpenGLGlewGlut.lua @@ -14,6 +14,7 @@ configuration {"MacOSX"} links { "OpenGL.framework"} configuration {"not Windows", "not MacOSX"} + if os.is("Linux") then if not _OPTIONS["force_dlopen_opengl"] and (os.isdir("/usr/include") and os.isfile("/usr/include/GL/gl.h")) then links {"GL"} else @@ -21,6 +22,7 @@ defines {"GLEW_INIT_OPENGL11_FUNCTIONS=1"} links {"dl"} end + end configuration{} end diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index d09241000..6f88c6d18 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -35,6 +35,7 @@ btCollisionObject::btCollisionObject() m_restitution(btScalar(0.)), m_internalType(CO_COLLISION_OBJECT), m_userObjectPointer(0), + m_userIndex(-1), m_hitFraction(btScalar(1.)), m_ccdSweptSphereRadius(btScalar(0.)), m_ccdMotionThreshold(btScalar(0.)), diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 587786ce9..c68402418 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -92,11 +92,10 @@ protected: int m_internalType; ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer - union - { - void* m_userObjectPointer; - int m_userIndex; - }; + + void* m_userObjectPointer; + + int m_userIndex; ///time of impact calculation btScalar m_hitFraction; diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 286e6c31f..af8cede74 100644 --- a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -232,7 +232,9 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap m_compoundShapeRevision = compoundShape->getUpdateRevision(); } - + if (m_childCollisionAlgorithms.size()==0) + return; + const btDbvt* tree = compoundShape->getDynamicAabbTree(); //use a dynamic aabb tree to cull potential child-overlaps btCompoundLeafCallback callback(colObjWrap,otherObjWrap,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold); diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp new file mode 100644 index 000000000..696aafb72 --- /dev/null +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp @@ -0,0 +1,1113 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +2014 May: btGeneric6DofSpring2Constraint is created from the original (2.82.2712) btGeneric6DofConstraint by Gabor Puhr and Tamas Umenhoffer +Pros: +- Much more accurate and stable in a lot of situation. (Especially when a sleeping chain of RBs connected with 6dof2 is pulled) +- Stable and accurate spring with minimal energy loss that works with all of the solvers. (latter is not true for the original 6dof spring) +- Servo motor functionality +- Much more accurate bouncing. 0 really means zero bouncing (not true for the original 6odf) and there is only a minimal energy loss when the value is 1 (because of the solvers' precision) +- Rotation order for the Euler system can be set. (One axis' freedom is still limited to pi/2) + +Cons: +- It is slower than the original 6dof. There is no exact ratio, but half speed is a good estimation. (with PGS) +- At bouncing the correct velocity is calculated, but not the correct position. (it is because of the solver can correct position or velocity, but not both.) +*/ + +/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev +/// Added support for generic constraint solver through getInfo1/getInfo2 methods + +/* +2007-09-09 +btGeneric6DofConstraint Refactored by Francisco Le?n +email: projectileman@yahoo.com +http://gimpact.sf.net +*/ + + + +#include "btGeneric6DofSpring2Constraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btTransformUtil.h" +#include + + + +btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder) + : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, rbA, rbB) + , m_frameInA(frameInA) + , m_frameInB(frameInB) + , m_flags(0) + , m_rotateOrder(rotOrder) +{ + calculateTransforms(); +} + + +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) +{ + ///not providing rigidbody A means implicitly using worldspace for body A + m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB; + calculateTransforms(); +} + + +btScalar btGeneric6DofSpring2Constraint::btGetMatrixElem(const btMatrix3x3& mat, int index) +{ + int i = index%3; + int j = index/3; + return mat[i][j]; +} + +// MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html + +bool btGeneric6DofSpring2Constraint::matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) +{ + // rot = cy*cz -cy*sz sy + // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx + // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy + + btScalar fi = btGetMatrixElem(mat,2); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) + { + xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8)); + xyz[1] = btAsin(btGetMatrixElem(mat,2)); + xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); + return true; + } + else + { + // WARNING. Not unique. XA - ZA = -atan2(r10,r11) + xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + xyz[1] = -SIMD_HALF_PI; + xyz[2] = btScalar(0.0); + return false; + } + } + else + { + // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) + xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + xyz[1] = SIMD_HALF_PI; + xyz[2] = 0.0; + } + return false; +} + +bool btGeneric6DofSpring2Constraint::matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz) +{ + // rot = cy*cz -sz sy*cz + // cy*cx*sz+sx*sy cx*cz sy*cx*sz-cy*sx + // cy*sx*sz-cx*sy sx*cz sy*sx*sz+cx*cy + + btScalar fi = btGetMatrixElem(mat,1); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) + { + xyz[0] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,4)); + xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0)); + xyz[2] = btAsin(-btGetMatrixElem(mat,1)); + return true; + } + else + { + xyz[0] = -btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8)); + xyz[1] = btScalar(0.0); + xyz[2] = SIMD_HALF_PI; + return false; + } + } + else + { + xyz[0] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8)); + xyz[1] = 0.0; + xyz[2] = -SIMD_HALF_PI; + } + return false; +} + +bool btGeneric6DofSpring2Constraint::matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz) +{ + // rot = cy*cz+sy*sx*sz cz*sy*sx-cy*sz cx*sy + // cx*sz cx*cz -sx + // cy*sx*sz-cz*sy sy*sz+cy*cz*sx cy*cx + + btScalar fi = btGetMatrixElem(mat,5); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) + { + xyz[0] = btAsin(-btGetMatrixElem(mat,5)); + xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,8)); + xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + return true; + } + else + { + xyz[0] = SIMD_HALF_PI; + xyz[1] = -btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); + xyz[2] = btScalar(0.0); + return false; + } + } + else + { + xyz[0] = -SIMD_HALF_PI; + xyz[1] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); + xyz[2] = 0.0; + } + return false; +} + +bool btGeneric6DofSpring2Constraint::matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz) +{ + // rot = cy*cz sy*sx-cy*cx*sz cx*sy+cy*sz*sx + // sz cz*cx -cz*sx + // -cz*sy cy*sx+cx*sy*sz cy*cx-sy*sz*sx + + btScalar fi = btGetMatrixElem(mat,3); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) + { + xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,4)); + xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,0)); + xyz[2] = btAsin(btGetMatrixElem(mat,3)); + return true; + } + else + { + xyz[0] = btScalar(0.0); + xyz[1] = -btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8)); + xyz[2] = -SIMD_HALF_PI; + return false; + } + } + else + { + xyz[0] = btScalar(0.0); + xyz[1] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8)); + xyz[2] = SIMD_HALF_PI; + } + return false; +} + +bool btGeneric6DofSpring2Constraint::matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz) +{ + // rot = cz*cy-sz*sx*sy -cx*sz cz*sy+cy*sz*sx + // cy*sz+cz*sx*sy cz*cx sz*sy-cz*xy*sx + // -cx*sy sx cx*cy + + btScalar fi = btGetMatrixElem(mat,7); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) + { + xyz[0] = btAsin(btGetMatrixElem(mat,7)); + xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8)); + xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,4)); + return true; + } + else + { + xyz[0] = -SIMD_HALF_PI; + xyz[1] = btScalar(0.0); + xyz[2] = -btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0)); + return false; + } + } + else + { + xyz[0] = SIMD_HALF_PI; + xyz[1] = btScalar(0.0); + xyz[2] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0)); + } + return false; +} + +bool btGeneric6DofSpring2Constraint::matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz) +{ + // rot = cz*cy cz*sy*sx-cx*sz sz*sx+cz*cx*sy + // cy*sz cz*cx+sz*sy*sx cx*sz*sy-cz*sx + // -sy cy*sx cy*cx + + btScalar fi = btGetMatrixElem(mat,6); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) + { + xyz[0] = btAtan2(btGetMatrixElem(mat,7), btGetMatrixElem(mat,8)); + xyz[1] = btAsin(-btGetMatrixElem(mat,6)); + xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,0)); + return true; + } + else + { + xyz[0] = btScalar(0.0); + xyz[1] = SIMD_HALF_PI; + xyz[2] = -btAtan2(btGetMatrixElem(mat,1),btGetMatrixElem(mat,2)); + return false; + } + } + else + { + xyz[0] = btScalar(0.0); + xyz[1] = -SIMD_HALF_PI; + xyz[2] = btAtan2(-btGetMatrixElem(mat,1),-btGetMatrixElem(mat,2)); + } + return false; +} + +void btGeneric6DofSpring2Constraint::calculateAngleInfo() +{ + btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis(); + switch (m_rotateOrder) + { + case RO_XYZ : matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff); break; + case RO_XZY : matrixToEulerXZY(relative_frame,m_calculatedAxisAngleDiff); break; + case RO_YXZ : matrixToEulerYXZ(relative_frame,m_calculatedAxisAngleDiff); break; + case RO_YZX : matrixToEulerYZX(relative_frame,m_calculatedAxisAngleDiff); break; + case RO_ZXY : matrixToEulerZXY(relative_frame,m_calculatedAxisAngleDiff); break; + case RO_ZYX : matrixToEulerZYX(relative_frame,m_calculatedAxisAngleDiff); break; + default : btAssert(false); + } + // in euler angle mode we do not actually constrain the angular velocity + // along the axes axis[0] and axis[2] (although we do use axis[1]) : + // + // to get constrain w2-w1 along ...not + // ------ --------------------- ------ + // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] + // d(angle[1])/dt = 0 ax[1] + // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] + // + // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. + // to prove the result for angle[0], write the expression for angle[0] from + // GetInfo1 then take the derivative. to prove this for angle[2] it is + // easier to take the euler rate expression for d(angle[2])/dt with respect + // to the components of w and set that to 0. + switch (m_rotateOrder) + { + case RO_XYZ : + { + //Is this the "line of nodes" calculation choosing planes YZ (B coordinate system) and xy (A coordinate system)? (http://en.wikipedia.org/wiki/Euler_angles) + //The two planes are non-homologous, so this is a Tait–Bryan angle formalism and not a proper Euler + //Extrinsic rotations are equal to the reversed order intrinsic rotations so the above xyz extrinsic rotations (axes are fixed) are the same as the zy'x" intrinsic rotations (axes are refreshed after each rotation) + //that is why xy and YZ planes are chosen (this will describe a zy'x" intrinsic rotation) (see the figure on the left at http://en.wikipedia.org/wiki/Euler_angles under Tait–Bryan angles) + // x' = Nperp = N.cross(axis2) + // y' = N = axis2.cross(axis0) + // z' = z + // + // x" = X + // y" = y' + // z" = ?? + //in other words: + //first rotate around z + //second rotate around y'= z.cross(X) + //third rotate around x" = X + //Original XYZ extrinsic rotation order. + //Planes: xy and YZ normals: z, X. Plane intersection (N) is z.cross(X) + btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0); + btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2); + m_calculatedAxis[1] = axis2.cross(axis0); + m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2); + m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]); + break; + } + case RO_XZY : + { + //planes: xz,ZY normals: y, X + //first rotate around y + //second rotate around z'= y.cross(X) + //third rotate around x" = X + btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0); + btVector3 axis1 = m_calculatedTransformA.getBasis().getColumn(1); + m_calculatedAxis[2] = axis0.cross(axis1); + m_calculatedAxis[0] = axis1.cross(m_calculatedAxis[2]); + m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0); + break; + } + case RO_YXZ : + { + //planes: yx,XZ normals: z, Y + //first rotate around z + //second rotate around x'= z.cross(Y) + //third rotate around y" = Y + btVector3 axis1 = m_calculatedTransformB.getBasis().getColumn(1); + btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2); + m_calculatedAxis[0] = axis1.cross(axis2); + m_calculatedAxis[1] = axis2.cross(m_calculatedAxis[0]); + m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1); + break; + } + case RO_YZX : + { + //planes: yz,ZX normals: x, Y + //first rotate around x + //second rotate around z'= x.cross(Y) + //third rotate around y" = Y + btVector3 axis0 = m_calculatedTransformA.getBasis().getColumn(0); + btVector3 axis1 = m_calculatedTransformB.getBasis().getColumn(1); + m_calculatedAxis[2] = axis0.cross(axis1); + m_calculatedAxis[0] = axis1.cross(m_calculatedAxis[2]); + m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0); + break; + } + case RO_ZXY : + { + //planes: zx,XY normals: y, Z + //first rotate around y + //second rotate around x'= y.cross(Z) + //third rotate around z" = Z + btVector3 axis1 = m_calculatedTransformA.getBasis().getColumn(1); + btVector3 axis2 = m_calculatedTransformB.getBasis().getColumn(2); + m_calculatedAxis[0] = axis1.cross(axis2); + m_calculatedAxis[1] = axis2.cross(m_calculatedAxis[0]); + m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1); + break; + } + case RO_ZYX : + { + //planes: zy,YX normals: x, Z + //first rotate around x + //second rotate around y' = x.cross(Z) + //third rotate around z" = Z + btVector3 axis0 = m_calculatedTransformA.getBasis().getColumn(0); + btVector3 axis2 = m_calculatedTransformB.getBasis().getColumn(2); + m_calculatedAxis[1] = axis2.cross(axis0); + m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2); + m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]); + break; + } + default: + btAssert(false); + } + + m_calculatedAxis[0].normalize(); + m_calculatedAxis[1].normalize(); + m_calculatedAxis[2].normalize(); + +} + +void btGeneric6DofSpring2Constraint::calculateTransforms() +{ + calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); +} + +void btGeneric6DofSpring2Constraint::calculateTransforms(const btTransform& transA,const btTransform& transB) +{ + m_calculatedTransformA = transA * m_frameInA; + m_calculatedTransformB = transB * m_frameInB; + calculateLinearInfo(); + calculateAngleInfo(); + + btScalar miA = getRigidBodyA().getInvMass(); + btScalar miB = getRigidBodyB().getInvMass(); + m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); + btScalar miS = miA + miB; + if(miS > btScalar(0.f)) + { + m_factA = miB / miS; + } + else + { + m_factA = btScalar(0.5f); + } + m_factB = btScalar(1.0f) - m_factA; +} + + +void btGeneric6DofSpring2Constraint::testAngularLimitMotor(int axis_index) +{ + btScalar angle = m_calculatedAxisAngleDiff[axis_index]; + angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit); + m_angularLimits[axis_index].m_currentPosition = angle; + m_angularLimits[axis_index].testLimitValue(angle); +} + + +void btGeneric6DofSpring2Constraint::getInfo1 (btConstraintInfo1* info) +{ + //prepare constraint + calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + info->m_numConstraintRows = 0; + info->nub = 0; + int i; + //test linear limits + for(i = 0; i < 3; i++) + { + if (m_linearLimits.m_currentLimit[i]==4) info->m_numConstraintRows += 2; + else if (m_linearLimits.m_currentLimit[i]!=0) info->m_numConstraintRows += 1; + if (m_linearLimits.m_enableMotor[i] ) info->m_numConstraintRows += 1; + if (m_linearLimits.m_enableSpring[i]) info->m_numConstraintRows += 1; + } + //test angular limits + for (i=0;i<3 ;i++ ) + { + testAngularLimitMotor(i); + if (m_angularLimits[i].m_currentLimit==4) info->m_numConstraintRows += 2; + else if (m_angularLimits[i].m_currentLimit!=0) info->m_numConstraintRows += 1; + if (m_angularLimits[i].m_enableMotor ) info->m_numConstraintRows += 1; + if (m_angularLimits[i].m_enableSpring) info->m_numConstraintRows += 1; + } +} + + +void btGeneric6DofSpring2Constraint::getInfo2 (btConstraintInfo2* info) +{ + const btTransform& transA = m_rbA.getCenterOfMassTransform(); + const btTransform& transB = m_rbB.getCenterOfMassTransform(); + const btVector3& linVelA = m_rbA.getLinearVelocity(); + const btVector3& linVelB = m_rbB.getLinearVelocity(); + const btVector3& angVelA = m_rbA.getAngularVelocity(); + const btVector3& angVelB = m_rbB.getAngularVelocity(); + + // for stability better to solve angular limits first + int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB); + setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB); +} + + +int btGeneric6DofSpring2Constraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) +{ + //solve linear limits + btRotationalLimitMotor2 limot; + for (int i=0;i<3 ;i++ ) + { + if(m_linearLimits.m_currentLimit[i] || m_linearLimits.m_enableMotor[i] || m_linearLimits.m_enableSpring[i]) + { // re-use rotational motor code + limot.m_bounce = m_linearLimits.m_bounce[i]; + limot.m_currentLimit = m_linearLimits.m_currentLimit[i]; + limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i]; + limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i]; + limot.m_currentLimitErrorHi = m_linearLimits.m_currentLimitErrorHi[i]; + limot.m_enableMotor = m_linearLimits.m_enableMotor[i]; + limot.m_servoMotor = m_linearLimits.m_servoMotor[i]; + limot.m_servoTarget = m_linearLimits.m_servoTarget[i]; + limot.m_enableSpring = m_linearLimits.m_enableSpring[i]; + limot.m_springStiffness = m_linearLimits.m_springStiffness[i]; + limot.m_springDamping = m_linearLimits.m_springDamping[i]; + limot.m_equilibriumPoint = m_linearLimits.m_equilibriumPoint[i]; + limot.m_hiLimit = m_linearLimits.m_upperLimit[i]; + limot.m_loLimit = m_linearLimits.m_lowerLimit[i]; + limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i]; + limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i]; + btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i); + int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT2); + limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP2) ? m_linearLimits.m_stopCFM[i] : info->cfm[0]; + limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP2) ? m_linearLimits.m_stopERP[i] : info->erp; + limot.m_motorCFM = (flags & BT_6DOF_FLAGS_CFM_MOTO2) ? m_linearLimits.m_motorCFM[i] : info->cfm[0]; + limot.m_motorERP = (flags & BT_6DOF_FLAGS_ERP_MOTO2) ? m_linearLimits.m_motorERP[i] : info->erp; + + //rotAllowed is a bit of a magic from the original 6dof. The calculation of it here is something that imitates the original behavior as much as possible. + int indx1 = (i + 1) % 3; + int indx2 = (i + 2) % 3; + int rotAllowed = 1; // rotations around orthos to current axis (it is used only when one of the body is static) + #define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION 1.0e-3 + bool indx1Violated = m_angularLimits[indx1].m_currentLimit == 1 || + m_angularLimits[indx1].m_currentLimit == 2 || + ( m_angularLimits[indx1].m_currentLimit == 3 && ( m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ) || + ( m_angularLimits[indx1].m_currentLimit == 4 && ( m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ); + bool indx2Violated = m_angularLimits[indx2].m_currentLimit == 1 || + m_angularLimits[indx2].m_currentLimit == 2 || + ( m_angularLimits[indx2].m_currentLimit == 3 && ( m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ) || + ( m_angularLimits[indx2].m_currentLimit == 4 && ( m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ); + if( indx1Violated && indx2Violated ) + { + rotAllowed = 0; + } + row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed); + + } + } + return row; +} + + + +int btGeneric6DofSpring2Constraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) +{ + int row = row_offset; + + //order of rotational constraint rows + int cIdx[] = {0, 1, 2}; + switch(m_rotateOrder) + { + case RO_XYZ : cIdx[0] = 0; cIdx[1] = 1; cIdx[2] = 2; break; + case RO_XZY : cIdx[0] = 0; cIdx[1] = 2; cIdx[2] = 1; break; + case RO_YXZ : cIdx[0] = 1; cIdx[1] = 0; cIdx[2] = 2; break; + case RO_YZX : cIdx[0] = 1; cIdx[1] = 2; cIdx[2] = 0; break; + case RO_ZXY : cIdx[0] = 2; cIdx[1] = 0; cIdx[2] = 1; break; + case RO_ZYX : cIdx[0] = 2; cIdx[1] = 1; cIdx[2] = 0; break; + default : btAssert(false); + } + + for (int ii = 0; ii < 3 ; ii++ ) + { + int i = cIdx[ii]; + if(m_angularLimits[i].m_currentLimit || m_angularLimits[i].m_enableMotor || m_angularLimits[i].m_enableSpring) + { + btVector3 axis = getAxis(i); + int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT2); + if(!(flags & BT_6DOF_FLAGS_CFM_STOP2)) + { + m_angularLimits[i].m_stopCFM = info->cfm[0]; + } + if(!(flags & BT_6DOF_FLAGS_ERP_STOP2)) + { + m_angularLimits[i].m_stopERP = info->erp; + } + if(!(flags & BT_6DOF_FLAGS_CFM_MOTO2)) + { + m_angularLimits[i].m_motorCFM = info->cfm[0]; + } + if(!(flags & BT_6DOF_FLAGS_ERP_MOTO2)) + { + m_angularLimits[i].m_motorERP = info->erp; + } + row += get_limit_motor_info2(&m_angularLimits[i],transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1); + } + } + + return row; +} + + +void btGeneric6DofSpring2Constraint::setFrames(const btTransform& frameA, const btTransform& frameB) +{ + m_frameInA = frameA; + m_frameInB = frameB; + buildJacobian(); + calculateTransforms(); +} + + +void btGeneric6DofSpring2Constraint::calculateLinearInfo() +{ + m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin(); + m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff; + for(int i = 0; i < 3; i++) + { + m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i]; + m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]); + } +} + +void btGeneric6DofSpring2Constraint::calculateJacobi(btRotationalLimitMotor2 * limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2 *info, int srow, btVector3& ax1, int rotational, int rotAllowed) +{ + btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis; + btScalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis; + + 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]; + + if(!rotational) + { + btVector3 tmpA, tmpB, relA, relB; + // get vector from bodyB to frameB in WCS + relB = m_calculatedTransformB.getOrigin() - transB.getOrigin(); + // same for bodyA + relA = m_calculatedTransformA.getOrigin() - transA.getOrigin(); + tmpA = relA.cross(ax1); + tmpB = relB.cross(ax1); + if(m_hasStaticBody && (!rotAllowed)) + { + tmpA *= m_factA; + tmpB *= m_factB; + } + int i; + for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i]; + for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i]; + } +} + + +int btGeneric6DofSpring2Constraint::get_limit_motor_info2( + btRotationalLimitMotor2 * limot, + const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB, + btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed) +{ + int count = 0; + int srow = row * info->rowskip; + + if (limot->m_currentLimit==4) + { + btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1); + + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1); + if (rotational) { + if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) { + btScalar bounceerror = -limot->m_bounce* vel; + if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror; + } + } else { + if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) { + btScalar bounceerror = -limot->m_bounce* vel; + if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror; + } + } + info->m_lowerLimit[srow] = rotational ? 0 : -SIMD_INFINITY; + info->m_upperLimit[srow] = rotational ? SIMD_INFINITY : 0; + info->cfm[srow] = limot->m_stopCFM; + srow += info->rowskip; + ++count; + + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitErrorHi * (rotational ? -1 : 1); + if (rotational) { + if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) { + btScalar bounceerror = -limot->m_bounce* vel; + if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror; + } + } else { + if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) { + btScalar bounceerror = -limot->m_bounce* vel; + if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror; + } + } + info->m_lowerLimit[srow] = rotational ? -SIMD_INFINITY : 0; + info->m_upperLimit[srow] = rotational ? 0 : SIMD_INFINITY; + info->cfm[srow] = limot->m_stopCFM; + srow += info->rowskip; + ++count; + } else + if (limot->m_currentLimit==3) + { + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1); + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + info->cfm[srow] = limot->m_stopCFM; + srow += info->rowskip; + ++count; + } + + if (limot->m_enableMotor && !limot->m_servoMotor) + { + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity; + btScalar mot_fact = getMotorFactor(limot->m_currentPosition, + limot->m_loLimit, + limot->m_hiLimit, + tag_vel, + info->fps * limot->m_motorERP); + info->m_constraintError[srow] = mot_fact * limot->m_targetVelocity; + info->m_lowerLimit[srow] = -limot->m_maxMotorForce; + info->m_upperLimit[srow] = limot->m_maxMotorForce; + info->cfm[srow] = limot->m_motorCFM; + srow += info->rowskip; + ++count; + } + + if (limot->m_enableMotor && limot->m_servoMotor) + { + btScalar error = limot->m_currentPosition - limot->m_servoTarget; + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + btScalar targetvelocity = error<0 ? -limot->m_targetVelocity : limot->m_targetVelocity; + btScalar tag_vel = -targetvelocity; + btScalar mot_fact; + if(error != 0) + { + btScalar lowLimit; + btScalar hiLimit; + if(limot->m_loLimit > limot->m_hiLimit) + { + lowLimit = error > 0 ? limot->m_servoTarget : -SIMD_INFINITY; + hiLimit = error < 0 ? limot->m_servoTarget : SIMD_INFINITY; + } + else + { + lowLimit = error > 0 && limot->m_servoTarget>limot->m_loLimit ? limot->m_servoTarget : limot->m_loLimit; + hiLimit = error < 0 && limot->m_servoTargetm_hiLimit ? limot->m_servoTarget : limot->m_hiLimit; + } + mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP); + } + else + { + mot_fact = 0; + } + info->m_constraintError[srow] = mot_fact * targetvelocity * (rotational ? -1 : 1); + info->m_lowerLimit[srow] = -limot->m_maxMotorForce; + info->m_upperLimit[srow] = limot->m_maxMotorForce; + info->cfm[srow] = limot->m_motorCFM; + srow += info->rowskip; + ++count; + } + + if (limot->m_enableSpring) + { + btScalar error = limot->m_currentPosition - limot->m_equilibriumPoint; + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + + //btScalar cfm = 1.0 / ((1.0/info->fps)*limot->m_springStiffness+ limot->m_springDamping); + //if(cfm > 0.99999) + // cfm = 0.99999; + //btScalar erp = (1.0/info->fps)*limot->m_springStiffness / ((1.0/info->fps)*limot->m_springStiffness + limot->m_springDamping); + //info->m_constraintError[srow] = info->fps * erp * error * (rotational ? -1.0 : 1.0); + //info->m_lowerLimit[srow] = -SIMD_INFINITY; + //info->m_upperLimit[srow] = SIMD_INFINITY; + + btScalar dt = 1.0 / info->fps; + 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 cfm = 0.0; + btScalar mA = 1.0 / m_rbA.getInvMass(); + btScalar mB = 1.0 / m_rbB.getInvMass(); + btScalar m = mA > mB ? mB : mA; + btScalar angularfreq = sqrt(ks / m); + + + //limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency) + if( 0.25 < angularfreq * dt) + { + ks = 1.0 / dt / dt / 16.0 / m; + } + //avoid overdamping + if(kd * dt > m) + { + kd = m / dt; + } + btScalar fs = ks * error * dt; + btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt; + btScalar f = (fs+fd); + + info->m_constraintError[srow] = (vel + f * (rotational ? -1 : 1)) ; + + btScalar minf = f < fd ? f : fd; + btScalar maxf = f < fd ? fd : f; + if(!rotational) + { + info->m_lowerLimit[srow] = minf > 0 ? 0 : minf; + info->m_upperLimit[srow] = maxf < 0 ? 0 : maxf; + } + else + { + info->m_lowerLimit[srow] = -maxf > 0 ? 0 : -maxf; + info->m_upperLimit[srow] = -minf < 0 ? 0 : -minf; + } + + info->cfm[srow] = cfm; + srow += info->rowskip; + ++count; + } + + return count; +} + + +//override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). +//If no axis is provided, it uses the default axis for this constraint. +void btGeneric6DofSpring2Constraint::setParam(int num, btScalar value, int axis) +{ + if((axis >= 0) && (axis < 3)) + { + switch(num) + { + case BT_CONSTRAINT_STOP_ERP : + m_linearLimits.m_stopERP[axis] = value; + m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + case BT_CONSTRAINT_STOP_CFM : + m_linearLimits.m_stopCFM[axis] = value; + m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + case BT_CONSTRAINT_ERP : + m_linearLimits.m_motorERP[axis] = value; + m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + case BT_CONSTRAINT_CFM : + m_linearLimits.m_motorCFM[axis] = value; + m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + default : + btAssertConstrParams(0); + } + } + else if((axis >=3) && (axis < 6)) + { + switch(num) + { + case BT_CONSTRAINT_STOP_ERP : + m_angularLimits[axis - 3].m_stopERP = value; + m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + case BT_CONSTRAINT_STOP_CFM : + m_angularLimits[axis - 3].m_stopCFM = value; + m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + case BT_CONSTRAINT_ERP : + m_angularLimits[axis - 3].m_motorERP = value; + m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + case BT_CONSTRAINT_CFM : + m_angularLimits[axis - 3].m_motorCFM = value; + m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + default : + btAssertConstrParams(0); + } + } + else + { + btAssertConstrParams(0); + } +} + +//return the local value of parameter +btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const +{ + btScalar retVal = 0; + if((axis >= 0) && (axis < 3)) + { + switch(num) + { + case BT_CONSTRAINT_STOP_ERP : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_linearLimits.m_stopERP[axis]; + break; + case BT_CONSTRAINT_STOP_CFM : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_linearLimits.m_stopCFM[axis]; + break; + case BT_CONSTRAINT_ERP : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_linearLimits.m_motorERP[axis]; + break; + case BT_CONSTRAINT_CFM : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_linearLimits.m_motorCFM[axis]; + break; + default : + btAssertConstrParams(0); + } + } + else if((axis >=3) && (axis < 6)) + { + switch(num) + { + case BT_CONSTRAINT_STOP_ERP : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_angularLimits[axis - 3].m_stopERP; + break; + case BT_CONSTRAINT_STOP_CFM : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_angularLimits[axis - 3].m_stopCFM; + break; + case BT_CONSTRAINT_ERP : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_angularLimits[axis - 3].m_motorERP; + break; + case BT_CONSTRAINT_CFM : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_angularLimits[axis - 3].m_motorCFM; + break; + default : + btAssertConstrParams(0); + } + } + else + { + btAssertConstrParams(0); + } + return retVal; +} + + + +void btGeneric6DofSpring2Constraint::setAxis(const btVector3& axis1,const btVector3& axis2) +{ + btVector3 zAxis = axis1.normalized(); + btVector3 yAxis = axis2.normalized(); + btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system + + btTransform frameInW; + frameInW.setIdentity(); + frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], + xAxis[1], yAxis[1], zAxis[1], + xAxis[2], yAxis[2], zAxis[2]); + + // now get constraint frame in local coordinate systems + m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW; + m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW; + + calculateTransforms(); +} + +void btGeneric6DofSpring2Constraint::setBounce(int index, btScalar bounce) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_bounce[index] = bounce; + else + m_angularLimits[index - 3].m_bounce = bounce; +} + +void btGeneric6DofSpring2Constraint::enableMotor(int index, bool onOff) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_enableMotor[index] = onOff; + else + m_angularLimits[index - 3].m_enableMotor = onOff; +} + +void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_servoMotor[index] = onOff; + else + m_angularLimits[index - 3].m_servoMotor = onOff; +} + +void btGeneric6DofSpring2Constraint::setTargetVelocity(int index, btScalar velocity) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_targetVelocity[index] = velocity; + else + m_angularLimits[index - 3].m_targetVelocity = velocity; +} + +void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar target) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_servoTarget[index] = target; + else + m_angularLimits[index - 3].m_servoTarget = target; +} + +void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_maxMotorForce[index] = force; + else + m_angularLimits[index - 3].m_maxMotorForce = force; +} + +void btGeneric6DofSpring2Constraint::enableSpring(int index, bool onOff) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_enableSpring[index] = onOff; + else + m_angularLimits[index - 3] .m_enableSpring = onOff; +} + +void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_springStiffness[index] = stiffness; + else + m_angularLimits[index - 3] .m_springStiffness = stiffness; +} + +void btGeneric6DofSpring2Constraint::setDamping(int index, btScalar damping) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_springDamping[index] = damping; + else + m_angularLimits[index - 3] .m_springDamping = damping; +} + +void btGeneric6DofSpring2Constraint::setEquilibriumPoint() +{ + calculateTransforms(); + int i; + for( i = 0; i < 3; i++) + m_linearLimits.m_equilibriumPoint[i] = m_calculatedLinearDiff[i]; + for(i = 0; i < 3; i++) + m_angularLimits[i].m_equilibriumPoint = m_calculatedAxisAngleDiff[i]; +} + +void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index) +{ + btAssert((index >= 0) && (index < 6)); + calculateTransforms(); + if (index<3) + m_linearLimits.m_equilibriumPoint[index] = m_calculatedLinearDiff[index]; + else + m_angularLimits[index - 3] .m_equilibriumPoint = m_calculatedAxisAngleDiff[index - 3]; +} + +void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index, btScalar val) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_equilibriumPoint[index] = val; + else + m_angularLimits[index - 3] .m_equilibriumPoint = val; +} + + +//////////////////////////// btRotationalLimitMotor2 //////////////////////////////////// + +void btRotationalLimitMotor2::testLimitValue(btScalar test_value) +{ + //we can't normalize the angles here because we would lost the sign that we use later, but it doesn't seem to be a problem + if(m_loLimit > m_hiLimit) { + m_currentLimit = 0; + m_currentLimitError = btScalar(0.f); + } + else if(m_loLimit == m_hiLimit) { + m_currentLimitError = test_value - m_loLimit; + m_currentLimit = 3; + } else { + m_currentLimitError = test_value - m_loLimit; + m_currentLimitErrorHi = test_value - m_hiLimit; + m_currentLimit = 4; + } +} + +//////////////////////////// btTranslationalLimitMotor2 //////////////////////////////////// + +void btTranslationalLimitMotor2::testLimitValue(int limitIndex, btScalar test_value) +{ + btScalar loLimit = m_lowerLimit[limitIndex]; + btScalar hiLimit = m_upperLimit[limitIndex]; + if(loLimit > hiLimit) { + m_currentLimitError[limitIndex] = 0; + m_currentLimit[limitIndex] = 0; + } + else if(loLimit == hiLimit) { + m_currentLimitError[limitIndex] = test_value - loLimit; + m_currentLimit[limitIndex] = 3; + } else { + m_currentLimitError[limitIndex] = test_value - loLimit; + m_currentLimitErrorHi[limitIndex] = test_value - hiLimit; + m_currentLimit[limitIndex] = 4; + } +} + + diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h new file mode 100644 index 000000000..e55ee235a --- /dev/null +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h @@ -0,0 +1,654 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +2014 May: btGeneric6DofSpring2Constraint is created from the original (2.82.2712) btGeneric6DofConstraint by Gabor Puhr and Tamas Umenhoffer +Pros: +- Much more accurate and stable in a lot of situation. (Especially when a sleeping chain of RBs connected with 6dof2 is pulled) +- Stable and accurate spring with minimal energy loss that works with all of the solvers. (latter is not true for the original 6dof spring) +- Servo motor functionality +- Much more accurate bouncing. 0 really means zero bouncing (not true for the original 6odf) and there is only a minimal energy loss when the value is 1 (because of the solvers' precision) +- Rotation order for the Euler system can be set. (One axis' freedom is still limited to pi/2) + +Cons: +- It is slower than the original 6dof. There is no exact ratio, but half speed is a good estimation. +- At bouncing the correct velocity is calculated, but not the correct position. (it is because of the solver can correct position or velocity, but not both.) +*/ + +/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev +/// Added support for generic constraint solver through getInfo1/getInfo2 methods + +/* +2007-09-09 +btGeneric6DofConstraint Refactored by Francisco Le?n +email: projectileman@yahoo.com +http://gimpact.sf.net +*/ + + +#ifndef BT_GENERIC_6DOF_CONSTRAINT2_H +#define BT_GENERIC_6DOF_CONSTRAINT2_H + +#include "LinearMath/btVector3.h" +#include "btJacobianEntry.h" +#include "btTypedConstraint.h" + +class btRigidBody; + + +#ifdef BT_USE_DOUBLE_PRECISION +#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintDoubleData2 +#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintDoubleData2" +#else +#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintData +#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintData" +#endif //BT_USE_DOUBLE_PRECISION + +enum RotateOrder +{ + RO_XYZ, + RO_XZY, + RO_YXZ, + RO_YZX, + RO_ZXY, + RO_ZYX +}; + +class btRotationalLimitMotor2 +{ +public: +// upper < lower means free +// upper == lower means locked +// upper > lower means limited + btScalar m_loLimit; + btScalar m_hiLimit; + btScalar m_bounce; + btScalar m_stopERP; + btScalar m_stopCFM; + btScalar m_motorERP; + btScalar m_motorCFM; + bool m_enableMotor; + btScalar m_targetVelocity; + btScalar m_maxMotorForce; + bool m_servoMotor; + btScalar m_servoTarget; + bool m_enableSpring; + btScalar m_springStiffness; + btScalar m_springDamping; + btScalar m_equilibriumPoint; + + btScalar m_currentLimitError; + btScalar m_currentLimitErrorHi; + btScalar m_currentPosition; + int m_currentLimit; + + btRotationalLimitMotor2() + { + m_loLimit = 1.0f; + m_hiLimit = -1.0f; + m_bounce = 0.0f; + m_stopERP = 0.2f; + m_stopCFM = 0.f; + m_motorERP = 0.9f; + m_motorCFM = 0.f; + m_enableMotor = false; + m_targetVelocity = 0; + m_maxMotorForce = 0.1f; + m_servoMotor = false; + m_servoTarget = 0; + m_enableSpring = false; + m_springStiffness = 0; + m_springDamping = 0; + m_equilibriumPoint = 0; + + m_currentLimitError = 0; + m_currentLimitErrorHi = 0; + m_currentPosition = 0; + m_currentLimit = 0; + } + + btRotationalLimitMotor2(const btRotationalLimitMotor2 & limot) + { + m_loLimit = limot.m_loLimit; + m_hiLimit = limot.m_hiLimit; + m_bounce = limot.m_bounce; + m_stopERP = limot.m_stopERP; + m_stopCFM = limot.m_stopCFM; + m_motorERP = limot.m_motorERP; + m_motorCFM = limot.m_motorCFM; + m_enableMotor = limot.m_enableMotor; + m_targetVelocity = limot.m_targetVelocity; + m_maxMotorForce = limot.m_maxMotorForce; + m_servoMotor = limot.m_servoMotor; + m_servoTarget = limot.m_servoTarget; + m_enableSpring = limot.m_enableSpring; + m_springStiffness = limot.m_springStiffness; + m_springDamping = limot.m_springDamping; + m_equilibriumPoint = limot.m_equilibriumPoint; + + m_currentLimitError = limot.m_currentLimitError; + m_currentLimitErrorHi = limot.m_currentLimitErrorHi; + m_currentPosition = limot.m_currentPosition; + m_currentLimit = limot.m_currentLimit; + } + + + bool isLimited() + { + if(m_loLimit > m_hiLimit) return false; + return true; + } + + void testLimitValue(btScalar test_value); +}; + + + +class btTranslationalLimitMotor2 +{ +public: +// upper < lower means free +// upper == lower means locked +// upper > lower means limited + btVector3 m_lowerLimit; + btVector3 m_upperLimit; + btVector3 m_bounce; + btVector3 m_stopERP; + btVector3 m_stopCFM; + btVector3 m_motorERP; + btVector3 m_motorCFM; + bool m_enableMotor[3]; + bool m_servoMotor[3]; + bool m_enableSpring[3]; + btVector3 m_servoTarget; + btVector3 m_springStiffness; + btVector3 m_springDamping; + btVector3 m_equilibriumPoint; + btVector3 m_targetVelocity; + btVector3 m_maxMotorForce; + + btVector3 m_currentLimitError; + btVector3 m_currentLimitErrorHi; + btVector3 m_currentLinearDiff; + int m_currentLimit[3]; + + btTranslationalLimitMotor2() + { + m_lowerLimit .setValue(0.f , 0.f , 0.f ); + m_upperLimit .setValue(0.f , 0.f , 0.f ); + m_bounce .setValue(0.f , 0.f , 0.f ); + m_stopERP .setValue(0.2f, 0.2f, 0.2f); + m_stopCFM .setValue(0.f , 0.f , 0.f ); + m_motorERP .setValue(0.9f, 0.9f, 0.9f); + m_motorCFM .setValue(0.f , 0.f , 0.f ); + + m_currentLimitError .setValue(0.f , 0.f , 0.f ); + m_currentLimitErrorHi.setValue(0.f , 0.f , 0.f ); + m_currentLinearDiff .setValue(0.f , 0.f , 0.f ); + + for(int i=0; i < 3; i++) + { + m_enableMotor[i] = false; + m_servoMotor[i] = false; + m_enableSpring[i] = false; + m_servoTarget[i] = btScalar(0.f); + m_springStiffness[i] = btScalar(0.f); + m_springDamping[i] = btScalar(0.f); + m_equilibriumPoint[i] = btScalar(0.f); + m_targetVelocity[i] = btScalar(0.f); + m_maxMotorForce[i] = btScalar(0.f); + + m_currentLimit[i] = 0; + } + } + + btTranslationalLimitMotor2(const btTranslationalLimitMotor2 & other ) + { + m_lowerLimit = other.m_lowerLimit; + m_upperLimit = other.m_upperLimit; + m_bounce = other.m_bounce; + m_stopERP = other.m_stopERP; + m_stopCFM = other.m_stopCFM; + m_motorERP = other.m_motorERP; + m_motorCFM = other.m_motorCFM; + + m_currentLimitError = other.m_currentLimitError; + m_currentLimitErrorHi = other.m_currentLimitErrorHi; + m_currentLinearDiff = other.m_currentLinearDiff; + + for(int i=0; i < 3; i++) + { + m_enableMotor[i] = other.m_enableMotor[i]; + m_servoMotor[i] = other.m_servoMotor[i]; + m_enableSpring[i] = other.m_enableSpring[i]; + m_servoTarget[i] = other.m_servoTarget[i]; + m_springStiffness[i] = other.m_springStiffness[i]; + m_springDamping[i] = other.m_springDamping[i]; + m_equilibriumPoint[i] = other.m_equilibriumPoint[i]; + m_targetVelocity[i] = other.m_targetVelocity[i]; + m_maxMotorForce[i] = other.m_maxMotorForce[i]; + + m_currentLimit[i] = other.m_currentLimit[i]; + } + } + + inline bool isLimited(int limitIndex) + { + return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); + } + + void testLimitValue(int limitIndex, btScalar test_value); +}; + +enum bt6DofFlags2 +{ + BT_6DOF_FLAGS_CFM_STOP2 = 1, + BT_6DOF_FLAGS_ERP_STOP2 = 2, + BT_6DOF_FLAGS_CFM_MOTO2 = 4, + BT_6DOF_FLAGS_ERP_MOTO2 = 8 +}; +#define BT_6DOF_FLAGS_AXIS_SHIFT2 4 // bits per axis + + +ATTRIBUTE_ALIGNED16(class) btGeneric6DofSpring2Constraint : public btTypedConstraint +{ +protected: + + btTransform m_frameInA; + btTransform m_frameInB; + + btJacobianEntry m_jacLinear[3]; + btJacobianEntry m_jacAng[3]; + + btTranslationalLimitMotor2 m_linearLimits; + btRotationalLimitMotor2 m_angularLimits[3]; + + RotateOrder m_rotateOrder; + +protected: + + btTransform m_calculatedTransformA; + btTransform m_calculatedTransformB; + btVector3 m_calculatedAxisAngleDiff; + btVector3 m_calculatedAxis[3]; + btVector3 m_calculatedLinearDiff; + btScalar m_factA; + btScalar m_factB; + bool m_hasStaticBody; + int m_flags; + + btGeneric6DofSpring2Constraint& operator=(btGeneric6DofSpring2Constraint&) + { + btAssert(0); + return *this; + } + + int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); + int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); + + void calculateLinearInfo(); + void calculateAngleInfo(); + void testAngularLimitMotor(int axis_index); + + void calculateJacobi(btRotationalLimitMotor2* limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2* info, int srow, btVector3& ax1, int rotational, int rotAllowed); + int get_limit_motor_info2(btRotationalLimitMotor2* limot, + const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB, + btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed = false); + + static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index); + static bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz); + static bool matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz); + static bool matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz); + static bool matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz); + static bool matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz); + static bool matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz); + +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ); + btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ); + + virtual void buildJacobian() {} + virtual void getInfo1 (btConstraintInfo1* info); + virtual void getInfo2 (btConstraintInfo2* info); + virtual int calculateSerializeBufferSize() const; + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + + btRotationalLimitMotor2* getRotationalLimitMotor(int index) { return &m_angularLimits[index]; } + btTranslationalLimitMotor2* getTranslationalLimitMotor() { return &m_linearLimits; } + + // Calculates the global transform for the joint offset for body A an B, and also calculates the angle differences between the bodies. + void calculateTransforms(const btTransform& transA,const btTransform& transB); + void calculateTransforms(); + + // Gets the global transform of the offset for body A + const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; } + // Gets the global transform of the offset for body B + const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; } + + const btTransform & getFrameOffsetA() const { return m_frameInA; } + const btTransform & getFrameOffsetB() const { return m_frameInB; } + + btTransform & getFrameOffsetA() { return m_frameInA; } + btTransform & getFrameOffsetB() { return m_frameInB; } + + // Get the rotation axis in global coordinates ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously ) + btVector3 getAxis(int axis_index) const { return m_calculatedAxis[axis_index]; } + + // Get the relative Euler angle ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously ) + btScalar getAngle(int axis_index) const { return m_calculatedAxisAngleDiff[axis_index]; } + + // Get the relative position of the constraint pivot ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously ) + btScalar getRelativePivotPosition(int axis_index) const { return m_calculatedLinearDiff[axis_index]; } + + void setFrames(const btTransform & frameA, const btTransform & frameB); + + void setLinearLowerLimit(const btVector3& linearLower) { m_linearLimits.m_lowerLimit = linearLower; } + void getLinearLowerLimit(btVector3& linearLower) { linearLower = m_linearLimits.m_lowerLimit; } + void setLinearUpperLimit(const btVector3& linearUpper) { m_linearLimits.m_upperLimit = linearUpper; } + void getLinearUpperLimit(btVector3& linearUpper) { linearUpper = m_linearLimits.m_upperLimit; } + + void setAngularLowerLimit(const btVector3& angularLower) + { + for(int i = 0; i < 3; i++) + m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]); + } + + void setAngularLowerLimitReversed(const btVector3& angularLower) + { + for(int i = 0; i < 3; i++) + m_angularLimits[i].m_hiLimit = btNormalizeAngle(-angularLower[i]); + } + + void getAngularLowerLimit(btVector3& angularLower) + { + for(int i = 0; i < 3; i++) + angularLower[i] = m_angularLimits[i].m_loLimit; + } + + void getAngularLowerLimitReversed(btVector3& angularLower) + { + for(int i = 0; i < 3; i++) + angularLower[i] = -m_angularLimits[i].m_hiLimit; + } + + void setAngularUpperLimit(const btVector3& angularUpper) + { + for(int i = 0; i < 3; i++) + m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]); + } + + void setAngularUpperLimitReversed(const btVector3& angularUpper) + { + for(int i = 0; i < 3; i++) + m_angularLimits[i].m_loLimit = btNormalizeAngle(-angularUpper[i]); + } + + void getAngularUpperLimit(btVector3& angularUpper) + { + for(int i = 0; i < 3; i++) + angularUpper[i] = m_angularLimits[i].m_hiLimit; + } + + void getAngularUpperLimitReversed(btVector3& angularUpper) + { + for(int i = 0; i < 3; i++) + angularUpper[i] = -m_angularLimits[i].m_loLimit; + } + + //first 3 are linear, next 3 are angular + + void setLimit(int axis, btScalar lo, btScalar hi) + { + if(axis<3) + { + m_linearLimits.m_lowerLimit[axis] = lo; + m_linearLimits.m_upperLimit[axis] = hi; + } + else + { + lo = btNormalizeAngle(lo); + hi = btNormalizeAngle(hi); + m_angularLimits[axis-3].m_loLimit = lo; + m_angularLimits[axis-3].m_hiLimit = hi; + } + } + + void setLimitReversed(int axis, btScalar lo, btScalar hi) + { + if(axis<3) + { + m_linearLimits.m_lowerLimit[axis] = lo; + m_linearLimits.m_upperLimit[axis] = hi; + } + else + { + lo = btNormalizeAngle(lo); + hi = btNormalizeAngle(hi); + m_angularLimits[axis-3].m_hiLimit = -lo; + m_angularLimits[axis-3].m_loLimit = -hi; + } + } + + bool isLimited(int limitIndex) + { + if(limitIndex<3) + { + return m_linearLimits.isLimited(limitIndex); + } + return m_angularLimits[limitIndex-3].isLimited(); + } + + void setRotationOrder(RotateOrder order) { m_rotateOrder = order; } + RotateOrder getRotationOrder() { return m_rotateOrder; } + + void setAxis( const btVector3& axis1, const btVector3& axis2); + + void setBounce(int index, btScalar bounce); + + void enableMotor(int index, bool onOff); + void setServo(int index, bool onOff); // set the type of the motor (servo or not) (the motor has to be turned on for servo also) + void setTargetVelocity(int index, btScalar velocity); + void setServoTarget(int index, btScalar target); + void setMaxMotorForce(int index, btScalar force); + + void enableSpring(int index, bool onOff); + void setStiffness(int index, btScalar stiffness); + void setDamping(int index, btScalar damping); + void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF + void setEquilibriumPoint(int index); // set the current constraint position/orientation as an equilibrium point for given DOF + void setEquilibriumPoint(int index, btScalar val); + + //override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). + //If no axis is provided, it uses the default axis for this constraint. + virtual void setParam(int num, btScalar value, int axis = -1); + virtual btScalar getParam(int num, int axis = -1) const; +}; + + +struct btGeneric6DofSpring2ConstraintData +{ + btTypedConstraintData m_typeConstraintData; + btTransformFloatData m_rbAFrame; + btTransformFloatData m_rbBFrame; + + btVector3FloatData m_linearUpperLimit; + btVector3FloatData m_linearLowerLimit; + btVector3FloatData m_linearBounce; + btVector3FloatData m_linearStopERP; + btVector3FloatData m_linearStopCFM; + btVector3FloatData m_linearMotorERP; + btVector3FloatData m_linearMotorCFM; + btVector3FloatData m_linearTargetVelocity; + btVector3FloatData m_linearMaxMotorForce; + btVector3FloatData m_linearServoTarget; + btVector3FloatData m_linearSpringStiffness; + btVector3FloatData m_linearSpringDamping; + btVector3FloatData m_linearEquilibriumPoint; + char m_linearEnableMotor[4]; + char m_linearServoMotor[4]; + char m_linearEnableSpring[4]; + char m_padding1[4]; + + btVector3FloatData m_angularUpperLimit; + btVector3FloatData m_angularLowerLimit; + btVector3FloatData m_angularBounce; + btVector3FloatData m_angularStopERP; + btVector3FloatData m_angularStopCFM; + btVector3FloatData m_angularMotorERP; + btVector3FloatData m_angularMotorCFM; + btVector3FloatData m_angularTargetVelocity; + btVector3FloatData m_angularMaxMotorForce; + btVector3FloatData m_angularServoTarget; + btVector3FloatData m_angularSpringStiffness; + btVector3FloatData m_angularSpringDamping; + btVector3FloatData m_angularEquilibriumPoint; + char m_angularEnableMotor[4]; + char m_angularServoMotor[4]; + char m_angularEnableSpring[4]; + char m_padding2[4]; + + int m_rotateOrder; + char m_padding3[4]; +}; + +struct btGeneric6DofSpring2ConstraintDoubleData2 +{ + btTypedConstraintDoubleData m_typeConstraintData; + btTransformDoubleData m_rbAFrame; + btTransformDoubleData m_rbBFrame; + + btVector3DoubleData m_linearUpperLimit; + btVector3DoubleData m_linearLowerLimit; + btVector3DoubleData m_linearBounce; + btVector3DoubleData m_linearStopERP; + btVector3DoubleData m_linearStopCFM; + btVector3DoubleData m_linearMotorERP; + btVector3DoubleData m_linearMotorCFM; + btVector3DoubleData m_linearTargetVelocity; + btVector3DoubleData m_linearMaxMotorForce; + btVector3DoubleData m_linearServoTarget; + btVector3DoubleData m_linearSpringStiffness; + btVector3DoubleData m_linearSpringDamping; + btVector3DoubleData m_linearEquilibriumPoint; + char m_linearEnableMotor[4]; + char m_linearServoMotor[4]; + char m_linearEnableSpring[4]; + char m_padding1[4]; + + btVector3DoubleData m_angularUpperLimit; + btVector3DoubleData m_angularLowerLimit; + btVector3DoubleData m_angularBounce; + btVector3DoubleData m_angularStopERP; + btVector3DoubleData m_angularStopCFM; + btVector3DoubleData m_angularMotorERP; + btVector3DoubleData m_angularMotorCFM; + btVector3DoubleData m_angularTargetVelocity; + btVector3DoubleData m_angularMaxMotorForce; + btVector3DoubleData m_angularServoTarget; + btVector3DoubleData m_angularSpringStiffness; + btVector3DoubleData m_angularSpringDamping; + btVector3DoubleData m_angularEquilibriumPoint; + char m_angularEnableMotor[4]; + char m_angularServoMotor[4]; + char m_angularEnableSpring[4]; + char m_padding2[4]; + + int m_rotateOrder; + char m_padding3[4]; +}; + +SIMD_FORCE_INLINE int btGeneric6DofSpring2Constraint::calculateSerializeBufferSize() const +{ + return sizeof(btGeneric6DofSpring2ConstraintData2); +} + +SIMD_FORCE_INLINE const char* btGeneric6DofSpring2Constraint::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btGeneric6DofSpring2ConstraintData2* dof = (btGeneric6DofSpring2ConstraintData2*)dataBuffer; + btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer); + + m_frameInA.serialize(dof->m_rbAFrame); + m_frameInB.serialize(dof->m_rbBFrame); + + int i; + for (i=0;i<3;i++) + { + dof->m_angularLowerLimit.m_floats[i] = m_angularLimits[i].m_loLimit; + dof->m_angularUpperLimit.m_floats[i] = m_angularLimits[i].m_hiLimit; + dof->m_angularBounce.m_floats[i] = m_angularLimits[i].m_bounce; + dof->m_angularStopERP.m_floats[i] = m_angularLimits[i].m_stopERP; + dof->m_angularStopCFM.m_floats[i] = m_angularLimits[i].m_stopCFM; + dof->m_angularMotorERP.m_floats[i] = m_angularLimits[i].m_motorERP; + dof->m_angularMotorCFM.m_floats[i] = m_angularLimits[i].m_motorCFM; + dof->m_angularTargetVelocity.m_floats[i] = m_angularLimits[i].m_targetVelocity; + dof->m_angularMaxMotorForce.m_floats[i] = m_angularLimits[i].m_maxMotorForce; + dof->m_angularServoTarget.m_floats[i] = m_angularLimits[i].m_servoTarget; + dof->m_angularSpringStiffness.m_floats[i] = m_angularLimits[i].m_springStiffness; + dof->m_angularSpringDamping.m_floats[i] = m_angularLimits[i].m_springDamping; + dof->m_angularEquilibriumPoint.m_floats[i] = m_angularLimits[i].m_equilibriumPoint; + } + dof->m_angularLowerLimit.m_floats[3] = 0; + dof->m_angularUpperLimit.m_floats[3] = 0; + dof->m_angularBounce.m_floats[3] = 0; + dof->m_angularStopERP.m_floats[3] = 0; + dof->m_angularStopCFM.m_floats[3] = 0; + dof->m_angularMotorERP.m_floats[3] = 0; + dof->m_angularMotorCFM.m_floats[3] = 0; + dof->m_angularTargetVelocity.m_floats[3] = 0; + dof->m_angularMaxMotorForce.m_floats[3] = 0; + dof->m_angularServoTarget.m_floats[3] = 0; + dof->m_angularSpringStiffness.m_floats[3] = 0; + dof->m_angularSpringDamping.m_floats[3] = 0; + dof->m_angularEquilibriumPoint.m_floats[3] = 0; + for (i=0;i<4;i++) + { + dof->m_angularEnableMotor[i] = i < 3 ? ( m_angularLimits[i].m_enableMotor ? 1 : 0 ) : 0; + dof->m_angularServoMotor[i] = i < 3 ? ( m_angularLimits[i].m_servoMotor ? 1 : 0 ) : 0; + dof->m_angularEnableSpring[i] = i < 3 ? ( m_angularLimits[i].m_enableSpring ? 1 : 0 ) : 0; + } + + m_linearLimits.m_lowerLimit.serialize( dof->m_linearLowerLimit ); + m_linearLimits.m_upperLimit.serialize( dof->m_linearUpperLimit ); + m_linearLimits.m_bounce.serialize( dof->m_linearBounce ); + m_linearLimits.m_stopERP.serialize( dof->m_linearStopERP ); + m_linearLimits.m_stopCFM.serialize( dof->m_linearStopCFM ); + m_linearLimits.m_motorERP.serialize( dof->m_linearMotorERP ); + m_linearLimits.m_motorCFM.serialize( dof->m_linearMotorCFM ); + m_linearLimits.m_targetVelocity.serialize( dof->m_linearTargetVelocity ); + m_linearLimits.m_maxMotorForce.serialize( dof->m_linearMaxMotorForce ); + m_linearLimits.m_servoTarget.serialize( dof->m_linearServoTarget ); + m_linearLimits.m_springStiffness.serialize( dof->m_linearSpringStiffness ); + m_linearLimits.m_springDamping.serialize( dof->m_linearSpringDamping ); + m_linearLimits.m_equilibriumPoint.serialize( dof->m_linearEquilibriumPoint ); + for (i=0;i<4;i++) + { + dof->m_linearEnableMotor[i] = i < 3 ? ( m_linearLimits.m_enableMotor[i] ? 1 : 0 ) : 0; + dof->m_linearServoMotor[i] = i < 3 ? ( m_linearLimits.m_servoMotor[i] ? 1 : 0 ) : 0; + dof->m_linearEnableSpring[i] = i < 3 ? ( m_linearLimits.m_enableSpring[i] ? 1 : 0 ) : 0; + } + + dof->m_rotateOrder = m_rotateOrder; + + return btGeneric6DofSpring2ConstraintDataName; +} + + + + + +#endif //BT_GENERIC_6DOF_CONSTRAINT_H diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index df57f893c..a6a51662d 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -317,6 +317,9 @@ void btDiscreteDynamicsWorld::debugDrawWorld() } } } + if (getDebugDrawer()) + getDebugDrawer()->flushLines(); + } void btDiscreteDynamicsWorld::clearForces() diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h index e863c7cc0..478d81257 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h @@ -41,6 +41,10 @@ public: btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal); + virtual void setVelocityTarget(btScalar velTarget) + { + m_desiredVelocity = velTarget; + } }; diff --git a/src/LinearMath/btIDebugDraw.h b/src/LinearMath/btIDebugDraw.h index de97c3f87..c76df7554 100644 --- a/src/LinearMath/btIDebugDraw.h +++ b/src/LinearMath/btIDebugDraw.h @@ -438,6 +438,11 @@ class btIDebugDraw drawLine(transform*pt0,transform*pt1,color); drawLine(transform*pt2,transform*pt3,color); } + + virtual void flushLines() + { + + } };