diff --git a/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp b/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp index 5e6857faa..de0b22c8d 100644 --- a/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp +++ b/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp @@ -953,6 +953,64 @@ void btWorldImporter::convertConstraintFloat(btTypedConstraintFloatData* constra constraint = gear; break; } + case D6_SPRING_2_CONSTRAINT_TYPE: + { + + btGeneric6DofSpring2ConstraintData* dofData = (btGeneric6DofSpring2ConstraintData*)constraintData; + + btGeneric6DofSpring2Constraint* dof = 0; + + if (rbA && rbB) + { + btTransform rbAFrame,rbBFrame; + rbAFrame.deSerializeFloat(dofData->m_rbAFrame); + rbBFrame.deSerializeFloat(dofData->m_rbBFrame); + dof = createGeneric6DofSpring2Constraint(*rbA,*rbB,rbAFrame,rbBFrame, dofData->m_rotateOrder); + } else + { + printf("Error in btWorldImporter::createGeneric6DofSpring2Constraint: requires rbA && rbB\n"); + } + + if (dof) + { + btVector3 angLowerLimit,angUpperLimit, linLowerLimit,linUpperlimit; + angLowerLimit.deSerializeFloat(dofData->m_angularLowerLimit); + angUpperLimit.deSerializeFloat(dofData->m_angularUpperLimit); + linLowerLimit.deSerializeFloat(dofData->m_linearLowerLimit); + linUpperlimit.deSerializeFloat(dofData->m_linearUpperLimit); + + angLowerLimit.setW(0.f); + dof->setAngularLowerLimit(angLowerLimit); + dof->setAngularUpperLimit(angUpperLimit); + dof->setLinearLowerLimit(linLowerLimit); + dof->setLinearUpperLimit(linUpperlimit); + + int i; + if (fileVersion>280) + { + //6-dof: 3 linear followed by 3 angular + for (i=0;i<3;i++) + { + dof->setStiffness(i,dofData->m_linearSpringStiffness.m_floats[i]); + dof->setEquilibriumPoint(i,dofData->m_linearEquilibriumPoint.m_floats[i]); + dof->enableSpring(i,dofData->m_linearEnableSpring[i]!=0); + dof->setDamping(i,dofData->m_linearSpringDamping.m_floats[i]); + } + for (i=0;i<3;i++) + { + dof->setStiffness(i+3,dofData->m_angularSpringStiffness.m_floats[i]); + dof->setEquilibriumPoint(i+3,dofData->m_angularEquilibriumPoint.m_floats[i]); + dof->enableSpring(i+3,dofData->m_angularEnableSpring[i]!=0); + dof->setDamping(i+3,dofData->m_angularSpringDamping.m_floats[i]); + } + + } + } + + constraint = dof; + break; + + } default: { printf("unknown constraint type\n"); @@ -1193,6 +1251,65 @@ void btWorldImporter::convertConstraintDouble(btTypedConstraintDoubleData* const constraint = gear; break; } + + case D6_SPRING_2_CONSTRAINT_TYPE: + { + + btGeneric6DofSpring2ConstraintDoubleData2* dofData = (btGeneric6DofSpring2ConstraintDoubleData2*)constraintData; + + btGeneric6DofSpring2Constraint* dof = 0; + + if (rbA && rbB) + { + btTransform rbAFrame,rbBFrame; + rbAFrame.deSerializeDouble(dofData->m_rbAFrame); + rbBFrame.deSerializeDouble(dofData->m_rbBFrame); + dof = createGeneric6DofSpring2Constraint(*rbA,*rbB,rbAFrame,rbBFrame, dofData->m_rotateOrder); + } else + { + printf("Error in btWorldImporter::createGeneric6DofSpring2Constraint: requires rbA && rbB\n"); + } + + if (dof) + { + btVector3 angLowerLimit,angUpperLimit, linLowerLimit,linUpperlimit; + angLowerLimit.deSerializeDouble(dofData->m_angularLowerLimit); + angUpperLimit.deSerializeDouble(dofData->m_angularUpperLimit); + linLowerLimit.deSerializeDouble(dofData->m_linearLowerLimit); + linUpperlimit.deSerializeDouble(dofData->m_linearUpperLimit); + + angLowerLimit.setW(0.f); + dof->setAngularLowerLimit(angLowerLimit); + dof->setAngularUpperLimit(angUpperLimit); + dof->setLinearLowerLimit(linLowerLimit); + dof->setLinearUpperLimit(linUpperlimit); + + int i; + if (fileVersion>280) + { + //6-dof: 3 linear followed by 3 angular + for (i=0;i<3;i++) + { + dof->setStiffness(i,dofData->m_linearSpringStiffness.m_floats[i]); + dof->setEquilibriumPoint(i,dofData->m_linearEquilibriumPoint.m_floats[i]); + dof->enableSpring(i,dofData->m_linearEnableSpring[i]!=0); + dof->setDamping(i,dofData->m_linearSpringDamping.m_floats[i]); + } + for (i=0;i<3;i++) + { + dof->setStiffness(i+3,dofData->m_angularSpringStiffness.m_floats[i]); + dof->setEquilibriumPoint(i+3,dofData->m_angularEquilibriumPoint.m_floats[i]); + dof->enableSpring(i+3,dofData->m_angularEnableSpring[i]!=0); + dof->setDamping(i+3,dofData->m_angularSpringDamping.m_floats[i]); + } + + } + } + + constraint = dof; + break; + + } default: { printf("unknown constraint type\n"); @@ -1711,6 +1828,15 @@ btGeneric6DofConstraint* btWorldImporter::createGeneric6DofConstraint(btRigidBod return dof; } +btGeneric6DofSpring2Constraint* btWorldImporter::createGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, int rotateOrder) +{ + btGeneric6DofSpring2Constraint* dof = new btGeneric6DofSpring2Constraint(rbA,rbB,frameInA,frameInB, (RotateOrder)rotateOrder); + m_allocatedConstraints.push_back(dof); + return dof; +} + + + btGeneric6DofSpringConstraint* btWorldImporter::createGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA) { btGeneric6DofSpringConstraint* dof = new btGeneric6DofSpringConstraint(rbA,rbB,frameInA,frameInB,useLinearReferenceFrameA); diff --git a/Extras/Serialize/BulletWorldImporter/btWorldImporter.h b/Extras/Serialize/BulletWorldImporter/btWorldImporter.h index bcd6405d4..ba1c38d12 100644 --- a/Extras/Serialize/BulletWorldImporter/btWorldImporter.h +++ b/Extras/Serialize/BulletWorldImporter/btWorldImporter.h @@ -42,6 +42,7 @@ class btHingeConstraint; class btConeTwistConstraint; class btGeneric6DofConstraint; class btGeneric6DofSpringConstraint; +class btGeneric6DofSpring2Constraint; class btSliderConstraint; class btGearConstraint; struct btContactSolverInfo; @@ -199,6 +200,8 @@ public: virtual btGeneric6DofConstraint* createGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); virtual btGeneric6DofConstraint* createGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB); virtual btGeneric6DofSpringConstraint* createGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); + virtual btGeneric6DofSpring2Constraint* createGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, int rotateOrder ); + virtual btSliderConstraint* createSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); virtual btSliderConstraint* createSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA); virtual btGearConstraint* createGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio); diff --git a/examples/CommonInterfaces/CommonRigidBodyBase.h b/examples/CommonInterfaces/CommonRigidBodyBase.h index a76de8f41..008314597 100644 --- a/examples/CommonInterfaces/CommonRigidBodyBase.h +++ b/examples/CommonInterfaces/CommonRigidBodyBase.h @@ -25,6 +25,7 @@ struct CommonRigidBodyBase : public CommonExampleInterface //data for picking objects class btRigidBody* m_pickedBody; class btTypedConstraint* m_pickedConstraint; + int m_savedState; btVector3 m_oldPickingPos; btVector3 m_hitPos; btScalar m_oldPickingDist; @@ -156,6 +157,19 @@ struct CommonRigidBodyBase : public CommonExampleInterface virtual bool keyboardCallback(int key, int state) { + if ((key==B3G_F3) && state && m_dynamicsWorld) + { + btDefaultSerializer* serializer = new btDefaultSerializer(); + m_dynamicsWorld->serialize(serializer); + + FILE* file = fopen("testFile.bullet","wb"); + fwrite(serializer->getBufferPointer(),serializer->getCurrentBufferSize(),1, file); + fclose(file); + //b3Printf("btDefaultSerializer wrote testFile.bullet"); + delete serializer; + return true; + + } return false;//don't handle this key } @@ -329,6 +343,7 @@ struct CommonRigidBodyBase : public CommonExampleInterface if (!(body->isStaticObject() || body->isKinematicObject())) { m_pickedBody = body; + m_savedState = m_pickedBody->getActivationState(); m_pickedBody->setActivationState(DISABLE_DEACTIVATION); //printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ()); btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; @@ -378,6 +393,8 @@ struct CommonRigidBodyBase : public CommonExampleInterface { if (m_pickedConstraint) { + m_pickedBody->forceActivationState(m_savedState); + m_pickedBody->activate(); m_dynamicsWorld->removeConstraint(m_pickedConstraint); delete m_pickedConstraint; m_pickedConstraint = 0; diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index b5d3e36e3..2da4d553b 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -33,9 +33,12 @@ #include "../OpenGLWindow/SimpleOpenGL2Renderer.h" #include "ExampleEntries.h" #include "OpenGLGuiHelper.h" +#include "Bullet3Common/b3FileUtils.h" + #include "LinearMath/btIDebugDraw.h" //quick test for file import, @todo(erwincoumans) make it more general and add other file formats #include "../Importers/ImportURDFDemo/ImportURDFSetup.h" +#include "../Importers/ImportBullet/SerializeSetup.h" static CommonGraphicsApp* s_app=0; @@ -191,7 +194,7 @@ static void MyMouseButtonCallback(int button, int state, float x, float y) #include -void openURDFDemo(const char* filename) +void openFileDemo(const char* filename) { if (sCurrentDemo) @@ -211,7 +214,21 @@ void openURDFDemo(const char* filename) CommonExampleOptions options(s_guiHelper,0); options.m_fileName = filename; - sCurrentDemo = ImportURDFCreateFunc(options); + char fullPath[1024]; + int fileType = 0; + sprintf(fullPath, "%s", filename); + b3FileUtils::toLower(fullPath); + if (strstr(fullPath, ".urdf")) + { + sCurrentDemo = ImportURDFCreateFunc(options); + } else + { + if (strstr(fullPath, ".bullet")) + { + sCurrentDemo = SerializeBulletCreateFunc(options); + } + } + //physicsSetup->setFileName(filename); @@ -471,7 +488,7 @@ void fileOpenCallback() { //todo(erwincoumans) check if it is actually URDF //printf("file open:%s\n", filename); - openURDFDemo(filename); + openFileDemo(filename); } } diff --git a/examples/Importers/ImportBullet/SerializeSetup.cpp b/examples/Importers/ImportBullet/SerializeSetup.cpp index fbc385722..ebaca15fe 100644 --- a/examples/Importers/ImportBullet/SerializeSetup.cpp +++ b/examples/Importers/ImportBullet/SerializeSetup.cpp @@ -6,13 +6,19 @@ class SerializeSetup : public CommonRigidBodyBase { + + char m_fileName[1024]; + public: - SerializeSetup(struct GUIHelperInterface* helper); + SerializeSetup(struct GUIHelperInterface* helper, const char* fileName); virtual ~SerializeSetup(); virtual void initPhysics(); virtual void stepSimulation(float deltaTime); - + virtual void setFileName(const char* fileName) + { + memcpy(m_fileName,fileName,strlen(fileName)+1); + } virtual void resetCamera() { float dist = 9.5; @@ -25,10 +31,16 @@ public: }; -SerializeSetup::SerializeSetup(struct GUIHelperInterface* helper) +SerializeSetup::SerializeSetup(struct GUIHelperInterface* helper, const char* fileName) :CommonRigidBodyBase(helper) { - + if (fileName) + { + setFileName(fileName); + } else + { + setFileName("spider.bullet"); + } } SerializeSetup::~SerializeSetup() { @@ -41,9 +53,9 @@ void SerializeSetup::initPhysics() m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints); btBulletWorldImporter* importer = new btBulletWorldImporter(m_dynamicsWorld); - const char* someFileName="spider.bullet"; + - const char* prefix[]={"./","./data/","../data/","../../data/","../../../data/","../../../../data/"}; + const char* prefix[]={"","./","./data/","../data/","../../data/","../../../data/","../../../../data/"}; int numPrefixes = sizeof(prefix)/sizeof(const char*); char relativeFileName[1024]; FILE* f=0; @@ -52,7 +64,7 @@ void SerializeSetup::initPhysics() for (int i=0;!f && iserialize(serializer); - FILE* file = fopen("testFile.bullet","wb"); + FILE* file = fopen("SerializeSetupTestFile.bullet","wb"); fwrite(serializer->getBufferPointer(),serializer->getCurrentBufferSize(),1, file); fclose(file); @@ -101,5 +113,5 @@ void SerializeSetup::stepSimulation(float deltaTime) class CommonExampleInterface* SerializeBulletCreateFunc(struct CommonExampleOptions& options) { - return new SerializeSetup(options.m_guiHelper); + return new SerializeSetup(options.m_guiHelper, options.m_fileName); } diff --git a/examples/OpenGLWindow/Win32OpenGLWindow.cpp b/examples/OpenGLWindow/Win32OpenGLWindow.cpp index d52f1545b..6efb95a9c 100644 --- a/examples/OpenGLWindow/Win32OpenGLWindow.cpp +++ b/examples/OpenGLWindow/Win32OpenGLWindow.cpp @@ -157,13 +157,13 @@ int Win32OpenGLWindow::fileOpenDialog(char* fileName, int maxFileNameLength) ofn.lpstrFile = bla; ofn.lpstrFile[0] = '\0'; ofn.nMaxFile = 1023; - ofn.lpstrFilter = L"URDF\0*.urdf\0"; + ofn.lpstrFilter = L"All Files\0*.*\0URDF\0*.urdf\0.bullet\0*.bullet\0"; #else ofn.lpstrFile = fileName; ofn.lpstrFile[0] = '\0'; ofn.nMaxFile = 1023; //ofn.lpstrFilter = "All\0*.*\0Text\0*.TXT\0"; - ofn.lpstrFilter = "URDF\0*.urdf\0"; + ofn.lpstrFilter = "All Files\0*.*\0URDF\0*.urdf\0.bullet\0*.bullet\0"; #endif diff --git a/examples/OpenGLWindow/Win32Window.cpp b/examples/OpenGLWindow/Win32Window.cpp index 5f3e9a2b3..cb5a8b595 100644 --- a/examples/OpenGLWindow/Win32Window.cpp +++ b/examples/OpenGLWindow/Win32Window.cpp @@ -64,10 +64,7 @@ int getAsciiCodeFromVirtualKeycode(int virtualKeyCode) { return virtualKeyCode; } - if (virtualKeyCode >= 'a' && virtualKeyCode <= 'z') - { - return virtualKeyCode; - } + if (virtualKeyCode >= 'A' && virtualKeyCode <= 'Z') { return virtualKeyCode+32;//todo: fix the ascii A vs a input