From 122fabac87a521dde2a1da0378e8c77962ea9ca4 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Fri, 12 Dec 2014 18:14:49 -0800 Subject: [PATCH] prepare to create coordinate frame demo, minor cleanup for create funcs in demo entries, fix r2d2.urdf inertia --- Demos3/AllBullet2Demos/BulletDemoEntries.h | 89 +++--- Demos3/AllBullet2Demos/main.cpp | 2 +- Demos3/ImportObjDemo/ImportObjSetup.cpp | 6 +- Demos3/ImportObjDemo/ImportObjSetup.h | 6 +- Demos3/ImportSTLDemo/ImportSTLSetup.cpp | 6 +- Demos3/ImportSTLDemo/ImportSTLSetup.h | 6 +- Demos3/ImportURDFDemo/ImportURDFSetup.cpp | 274 +++++++++++++++++- Demos3/ImportURDFDemo/ImportURDFSetup.h | 6 +- .../CoordinateFrameDemoPhysicsSetup.cpp | 90 ++++++ .../CoordinateFrameDemoPhysicsSetup.h | 28 ++ data/r2d2.urdf | 2 + 11 files changed, 433 insertions(+), 82 deletions(-) create mode 100644 Demos3/bullet2/CoordinateFramesDemo/CoordinateFrameDemoPhysicsSetup.cpp create mode 100644 Demos3/bullet2/CoordinateFramesDemo/CoordinateFrameDemoPhysicsSetup.h diff --git a/Demos3/AllBullet2Demos/BulletDemoEntries.h b/Demos3/AllBullet2Demos/BulletDemoEntries.h index e1fb78105..0966d93cd 100644 --- a/Demos3/AllBullet2Demos/BulletDemoEntries.h +++ b/Demos3/AllBullet2Demos/BulletDemoEntries.h @@ -4,6 +4,9 @@ #include "Bullet3AppSupport/BulletDemoInterface.h" #include "../bullet2/BasicDemo/BasicDemo.h" + +#include "../bullet2/CoordinateFramesDemo/CoordinateFrameDemoPhysicsSetup.h" + #include "../bullet2/BasicDemo/HingeDemo.h" #include "../bullet2/BasicDemo/HingeDemo.h" @@ -29,59 +32,33 @@ #include "../../Demos3/FiniteElementMethod/FiniteElementDemo.h" //#include "../../Demos3/bullet2/SoftDemo/SoftDemo.h" -static BulletDemoInterface* TestJointTorqueCreateFunc(CommonGraphicsApp* app) -{ - CommonPhysicsSetup* physicsSetup = new TestJointTorqueSetup(); - return new BasicDemo(app, physicsSetup); -} -static BulletDemoInterface* MultiBodyVehicleCreateFunc(CommonGraphicsApp* app) -{ - CommonPhysicsSetup* physicsSetup = new MultiBodyVehicleSetup(); - return new BasicDemo(app, physicsSetup); -} -static BulletDemoInterface* LuaDemoCreateFunc(CommonGraphicsApp* app) -{ - CommonPhysicsSetup* physicsSetup = new LuaPhysicsSetup(app); - return new BasicDemo(app, physicsSetup); + +#define MYCREATEFUNC(func) \ +static BulletDemoInterface* func##CreateFunc(CommonGraphicsApp* app)\ +{\ + CommonPhysicsSetup* physicsSetup = new func##Setup();\ + return new BasicDemo(app, physicsSetup);\ } -static BulletDemoInterface* MyCcdPhysicsDemoCreateFunc(CommonGraphicsApp* app) -{ - CommonPhysicsSetup* physicsSetup = new CcdPhysicsSetup(); - return new BasicDemo(app, physicsSetup); +#define MYCREATEFUNC2(func,setup) \ +static BulletDemoInterface* func(CommonGraphicsApp* app)\ +{\ + CommonPhysicsSetup* physicsSetup = new setup(app);\ + return new BasicDemo(app, physicsSetup);\ } -static BulletDemoInterface* MyKinematicObjectCreateFunc(CommonGraphicsApp* app) -{ - CommonPhysicsSetup* physicsSetup = new KinematicObjectSetup(); - return new BasicDemo(app, physicsSetup); -} -static BulletDemoInterface* MySerializeCreateFunc(CommonGraphicsApp* app) -{ - CommonPhysicsSetup* physicsSetup = new SerializeSetup(); - return new BasicDemo(app, physicsSetup); -} -static BulletDemoInterface* MyConstraintCreateFunc(CommonGraphicsApp* app) -{ - CommonPhysicsSetup* physicsSetup = new ConstraintPhysicsSetup(); - return new BasicDemo(app, physicsSetup); -} +MYCREATEFUNC(TestJointTorque); +MYCREATEFUNC(MultiBodyVehicle); +MYCREATEFUNC2(LuaDemoCreateFunc,LuaPhysicsSetup); +MYCREATEFUNC(Serialize); +MYCREATEFUNC(CcdPhysics); +MYCREATEFUNC(KinematicObject); +MYCREATEFUNC(ConstraintPhysics); +MYCREATEFUNC(ImportUrdf); +MYCREATEFUNC2(ImportObjCreateFunc,ImportObjSetup); +MYCREATEFUNC2(ImportSTLCreateFunc,ImportSTLSetup); +MYCREATEFUNC(CoordinateFrameDemoPhysics); -static BulletDemoInterface* MyImportUrdfCreateFunc(CommonGraphicsApp* app) -{ - CommonPhysicsSetup* physicsSetup = new ImportUrdfDemo(); - return new BasicDemo(app, physicsSetup); -} -static BulletDemoInterface* MyImportObjCreateFunc(CommonGraphicsApp* app) -{ - CommonPhysicsSetup* physicsSetup = new ImportObjDemo(app); - return new BasicDemo(app, physicsSetup); -} -static BulletDemoInterface* MyImportSTLCreateFunc(CommonGraphicsApp* app) -{ - CommonPhysicsSetup* physicsSetup = new ImportSTLDemo(app); - return new BasicDemo(app, physicsSetup); -} static BulletDemoInterface* MyImportColladaCreateFunc(CommonGraphicsApp* app) { @@ -92,7 +69,6 @@ static BulletDemoInterface* MyImportColladaCreateFunc(CommonGraphicsApp* app) - struct BulletDemoEntry { int m_menuLevel; @@ -107,21 +83,22 @@ static BulletDemoEntry allDemos[]= {0,"Basic Concepts",0}, {1,"Basis Frame", &CoordinateSystemDemo::CreateFunc}, {1,"SupportFunc", &MySupportFuncDemo::CreateFunc}, + {1,"Coordinate Frames", CoordinateFrameDemoPhysicsCreateFunc}, //{"emptydemo",EmptyBulletDemo::MyCreateFunc}, {0,"API Demos", 0}, {1,"BasicDemo",BasicDemo::MyCreateFunc}, - { 1, "CcdDemo", MyCcdPhysicsDemoCreateFunc }, - { 1, "Kinematic", MyKinematicObjectCreateFunc }, - { 1, "Constraints", MyConstraintCreateFunc }, + { 1, "CcdDemo", CcdPhysicsCreateFunc }, + { 1, "Kinematic", KinematicObjectCreateFunc }, + { 1, "Constraints", ConstraintPhysicsCreateFunc }, { 1, "LuaDemo",LuaDemoCreateFunc}, {0,"File Formats", 0}, - { 1, ".bullet",MySerializeCreateFunc}, - { 1, "Wavefront Obj", MyImportObjCreateFunc}, - { 1, "URDF", MyImportUrdfCreateFunc }, - { 1, "STL", MyImportSTLCreateFunc}, + { 1, ".bullet",SerializeCreateFunc}, + { 1, "Wavefront Obj", ImportObjCreateFunc}, + { 1, "URDF", ImportUrdfCreateFunc }, + { 1, "STL", ImportSTLCreateFunc}, { 1, "COLLADA", MyImportColladaCreateFunc}, {0,"Experiments", 0}, {1, "Finite Element Demo", FiniteElementDemo::CreateFunc}, diff --git a/Demos3/AllBullet2Demos/main.cpp b/Demos3/AllBullet2Demos/main.cpp index f8df8c3f1..ae2e396d3 100644 --- a/Demos3/AllBullet2Demos/main.cpp +++ b/Demos3/AllBullet2Demos/main.cpp @@ -321,7 +321,7 @@ void openURDFDemo(const char* filename) s_parameterInterface->removeAllParameters(); - ImportUrdfDemo* physicsSetup = new ImportUrdfDemo(); + ImportUrdfSetup* physicsSetup = new ImportUrdfSetup(); physicsSetup->setFileName(filename); sCurrentDemo = new BasicDemo(app, physicsSetup); diff --git a/Demos3/ImportObjDemo/ImportObjSetup.cpp b/Demos3/ImportObjDemo/ImportObjSetup.cpp index 13331a8ab..51f7f9d2b 100644 --- a/Demos3/ImportObjDemo/ImportObjSetup.cpp +++ b/Demos3/ImportObjDemo/ImportObjSetup.cpp @@ -7,13 +7,13 @@ #include "OpenGLWindow/SimpleOpenGL3App.h" #include "Wavefront2GLInstanceGraphicsShape.h" -ImportObjDemo::ImportObjDemo(CommonGraphicsApp* app) +ImportObjSetup::ImportObjSetup(CommonGraphicsApp* app) :m_app(app) { } -ImportObjDemo::~ImportObjDemo() +ImportObjSetup::~ImportObjSetup() { } @@ -24,7 +24,7 @@ ImportObjDemo::~ImportObjDemo() -void ImportObjDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge) +void ImportObjSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) { gfxBridge.setUpAxis(2); this->createEmptyDynamicsWorld(); diff --git a/Demos3/ImportObjDemo/ImportObjSetup.h b/Demos3/ImportObjDemo/ImportObjSetup.h index a2fc64caf..3a88523b7 100644 --- a/Demos3/ImportObjDemo/ImportObjSetup.h +++ b/Demos3/ImportObjDemo/ImportObjSetup.h @@ -4,12 +4,12 @@ #include "Bullet3AppSupport/CommonRigidBodySetup.h" -class ImportObjDemo : public CommonRigidBodySetup +class ImportObjSetup : public CommonRigidBodySetup { struct CommonGraphicsApp* m_app; public: - ImportObjDemo(CommonGraphicsApp* app); - virtual ~ImportObjDemo(); + ImportObjSetup(CommonGraphicsApp* app); + virtual ~ImportObjSetup(); virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge); }; diff --git a/Demos3/ImportSTLDemo/ImportSTLSetup.cpp b/Demos3/ImportSTLDemo/ImportSTLSetup.cpp index 4206be2f6..37a1a1373 100644 --- a/Demos3/ImportSTLDemo/ImportSTLSetup.cpp +++ b/Demos3/ImportSTLDemo/ImportSTLSetup.cpp @@ -6,20 +6,20 @@ #include "OpenGLWindow/SimpleOpenGL3App.h" #include "LoadMeshFromSTL.h" -ImportSTLDemo::ImportSTLDemo(CommonGraphicsApp* app) +ImportSTLSetup::ImportSTLSetup(CommonGraphicsApp* app) :m_app(app) { } -ImportSTLDemo::~ImportSTLDemo() +ImportSTLSetup::~ImportSTLSetup() { } -void ImportSTLDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge) +void ImportSTLSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) { gfxBridge.setUpAxis(2); this->createEmptyDynamicsWorld(); diff --git a/Demos3/ImportSTLDemo/ImportSTLSetup.h b/Demos3/ImportSTLDemo/ImportSTLSetup.h index 7b3da3941..40c9c2bdf 100644 --- a/Demos3/ImportSTLDemo/ImportSTLSetup.h +++ b/Demos3/ImportSTLDemo/ImportSTLSetup.h @@ -4,12 +4,12 @@ #include "Bullet3AppSupport/CommonRigidBodySetup.h" -class ImportSTLDemo : public CommonRigidBodySetup +class ImportSTLSetup : public CommonRigidBodySetup { struct CommonGraphicsApp* m_app; public: - ImportSTLDemo(CommonGraphicsApp* app); - virtual ~ImportSTLDemo(); + ImportSTLSetup(CommonGraphicsApp* app); + virtual ~ImportSTLSetup(); virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge); }; diff --git a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp index affa8c93e..82113e2e8 100644 --- a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp +++ b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp @@ -12,12 +12,12 @@ static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphasePr static bool enableConstraints = true;//false; -ImportUrdfDemo::ImportUrdfDemo() +ImportUrdfSetup::ImportUrdfSetup() { - sprintf(m_fileName,"r2d2.urdf"); + sprintf(m_fileName,"r2d2.urdf");//sphere2.urdf");// } -ImportUrdfDemo::~ImportUrdfDemo() +ImportUrdfSetup::~ImportUrdfSetup() { } @@ -41,7 +41,7 @@ btVector3 selectColor() return color; } -void ImportUrdfDemo::setFileName(const char* urdfFileName) +void ImportUrdfSetup::setFileName(const char* urdfFileName) { memcpy(m_fileName,urdfFileName,strlen(urdfFileName)+1); } @@ -544,7 +544,260 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr link, GraphicsPhy } -void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge) + + + +btMultiBody* URDF2BulletMultiBody(my_shared_ptr link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world, URDF2BulletMappings& mappings, const char* pathPrefix, btMultiBody* mb, int totalNumJoints) +{ + + btScalar mass = 0.f; + btTransform localInertialTransform; localInertialTransform.setIdentity(); + btVector3 localInertiaDiagonal(0,0,0); + + { + + if ((*link).inertial) + { + mass = (*link).inertial->mass; + btMatrix3x3 inertiaMat; + inertiaMat.setIdentity(); + inertiaMat.setValue( + (*link).inertial->ixx,(*link).inertial->ixy,(*link).inertial->ixz, + (*link).inertial->ixy,(*link).inertial->iyy,(*link).inertial->iyz, + (*link).inertial->ixz,(*link).inertial->iyz,(*link).inertial->izz); + + btScalar threshold = 0.00001f; + int maxSteps=20; + btMatrix3x3 inertia2PrincipalAxis; + inertiaMat.diagonalize(inertia2PrincipalAxis,threshold,maxSteps); + localInertiaDiagonal.setValue(inertiaMat[0][0],inertiaMat[1][1],inertiaMat[2][2]); + + btVector3 inertiaLocalCOM((*link).inertial->origin.position.x,(*link).inertial->origin.position.y,(*link).inertial->origin.position.z); + localInertialTransform.setOrigin(inertiaLocalCOM); + btQuaternion inertiaOrn((*link).inertial->origin.rotation.x,(*link).inertial->origin.rotation.y,(*link).inertial->origin.rotation.z,(*link).inertial->origin.rotation.w); + btMatrix3x3 inertiaOrnMat(inertiaOrn); + + if (mass > 0 && (localInertiaDiagonal[0]==0.f || localInertiaDiagonal[1] == 0.f + || localInertiaDiagonal[2] == 0.f)) + { + b3Warning("Error: inertia should not be zero if mass is positive\n"); + localInertiaDiagonal.setMax(btVector3(0.1,0.1,0.1)); + localInertialTransform.setIdentity();//.setBasis(inertiaOrnMat); + } + else + { + localInertialTransform.setBasis(inertiaOrnMat*inertia2PrincipalAxis); + } + } + } + btTransform linkTransformInWorldSpace; + int parentIndex = -1; + + const Link* parentLink = (*link).getParent(); + if (parentLink) + { + parentIndex = parentLink->m_link_index; + btAssert(parentIndex>=0); + } + int linkIndex = mappings.m_linkMasses.size(); + + btTransform parent2joint; + + if ((*link).parent_joint) + { + 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; + + btAssert(mb==0); + + bool multiDof = true; + bool canSleep = false; + bool isFixedBase = (mass==0);//todo: figure out when base is fixed + + mb = new btMultiBody(totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof); + + + } + + btAssert(mb); + + (*link).m_link_index = linkIndex; + + //compute this links center of mass transform, aligned with the principal axis of inertia + + + { + //btTransform rigidBodyFrameInWorldSpace =linkTransformInWorldSpace*inertialFrame; + + mappings.m_linkMasses.push_back(mass); + mappings.m_linkLocalDiagonalInertiaTensors.push_back(localInertiaDiagonal); + mappings.m_linkLocalInertiaTransforms.push_back(localInertialTransform); + + + + if ((*link).parent_joint) + { + btTransform offsetInA,offsetInB; + offsetInA.setIdentity(); + //offsetInA = mappings.m_linkLocalInertiaTransforms[parentIndex].inverse()*parent2joint; + offsetInA = parent2joint; + offsetInB.setIdentity(); + //offsetInB = localInertialTransform.inverse(); + + const Joint* pj = (*link).parent_joint.get(); + //btVector3 jointAxis(0,0,1);//pj->axis.x,pj->axis.y,pj->axis.z); + btVector3 jointAxis(pj->axis.x,pj->axis.y,pj->axis.z); + mappings.m_jointAxisArray.push_back(jointAxis); + mappings.m_jointOffsetInParent.push_back(offsetInA); + mappings.m_jointOffsetInChild.push_back(offsetInB); + mappings.m_jointTypeArray.push_back(pj->type); + + switch (pj->type) + { + case Joint::FIXED: + { + printf("Fixed joint\n"); + mb->setupFixed(linkIndex-1,mass,localInertiaDiagonal,parentIndex-1,offsetInA.getRotation(),offsetInA.getOrigin(),offsetInB.getOrigin()); + + break; + } + case Joint::CONTINUOUS: + case Joint::REVOLUTE: + { + printf("Revolute joint\n"); + mb->setupRevolute(linkIndex-1,mass,localInertiaDiagonal,parentIndex-1,offsetInA.getRotation(),jointAxis,offsetInA.getOrigin(),offsetInB.getOrigin(),true); + mb->finalizeMultiDof(); + //mb->setJointVel(linkIndex-1,1); + + break; + } + case Joint::PRISMATIC: + { + mb->setupPrismatic(linkIndex-1,mass,localInertiaDiagonal,parentIndex-1,offsetInA.getRotation(),jointAxis,offsetInB.getOrigin(),true); + printf("Prismatic joint\n"); + break; + } + default: + { + printf("Unknown joint\n"); + btAssert(0); + } + }; + + + + + } else + { + mappings.m_jointAxisArray.push_back(btVector3(0,0,0)); + btTransform ident; + ident.setIdentity(); + mappings.m_jointOffsetInParent.push_back(ident); + mappings.m_jointOffsetInChild.push_back(ident); + mappings.m_jointTypeArray.push_back(-1); + + + } + } + + //btCompoundShape* compoundShape = new btCompoundShape(); + btCollisionShape* shape = 0; + + for (int v=0;v<(int)link->collision_array.size();v++) + { + const Collision* visual = link->collision_array[v].get(); + + shape = convertVisualToCollisionShape(visual,pathPrefix); + + if (shape)//childShape) + { + gfxBridge.createCollisionShapeGraphicsObject(shape);//childShape); + + //btVector3 color = selectColor(); + /* + 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 childTransform; + childTransform.setIdentity();//TODO(erwincoumans): compute relative visual/inertial transform + // compoundShape->addChildShape(childTransform,childShape); + } + } + + if (shape)//compoundShape->getNumChildShapes()>0) + { + btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(mb, linkIndex-1); + col->setCollisionShape(shape); + + btTransform tr; + tr.setIdentity(); + tr = linkTransformInWorldSpace; + //if we don't set the initial pose of the btCollisionObject, the simulator will do this + //when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider + + //tr.setOrigin(local_origin[0]); + //tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + col->setWorldTransform(tr); + + bool isDynamic = true; + short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); + short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + + world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask); + + btVector3 color = selectColor();//(0.0,0.0,0.5); + gfxBridge.createCollisionObjectGraphicsObject(col,color); + btScalar friction = 0.5f; + + col->setFriction(friction); + + if (parentIndex>=0) + { + mb->getLink(linkIndex-1).m_collider=col; + } else + { + mb->setBaseCollider(col); + } + } + + for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) + { + if (*child) + { + URDF2BulletMultiBody(*child,gfxBridge, linkTransformInWorldSpace, world,mappings,pathPrefix,mb,totalNumJoints); + + } + else + { + std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl; + } + } + return mb; +} + + +void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) { int upAxis = 2; @@ -620,15 +873,16 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge) int numJoints = (*robot).m_numJoints; - if (1) + static bool useFeatherstone = false; + if (!useFeatherstone) { URDF2BulletMappings mappings; URDFvisual2BulletCollisionShape(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix); } //the btMultiBody support is work-in-progress :-) -#if 0 - if (0) +#if 1 + else { URDF2BulletMappings mappings; @@ -641,7 +895,7 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge) } #endif// - + useFeatherstone = !useFeatherstone; printf("numJoints/DOFS = %d\n", numJoints); if (0) @@ -681,7 +935,7 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge) } -void ImportUrdfDemo::stepSimulation(float deltaTime) +void ImportUrdfSetup::stepSimulation(float deltaTime) { if (m_dynamicsWorld) { diff --git a/Demos3/ImportURDFDemo/ImportURDFSetup.h b/Demos3/ImportURDFDemo/ImportURDFSetup.h index a9e0e4ef5..482a36915 100644 --- a/Demos3/ImportURDFDemo/ImportURDFSetup.h +++ b/Demos3/ImportURDFDemo/ImportURDFSetup.h @@ -4,13 +4,13 @@ #include "Bullet3AppSupport/CommonMultiBodySetup.h" -class ImportUrdfDemo : public CommonMultiBodySetup +class ImportUrdfSetup : public CommonMultiBodySetup { char m_fileName[1024]; public: - ImportUrdfDemo(); - virtual ~ImportUrdfDemo(); + ImportUrdfSetup(); + virtual ~ImportUrdfSetup(); virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge); virtual void stepSimulation(float deltaTime); diff --git a/Demos3/bullet2/CoordinateFramesDemo/CoordinateFrameDemoPhysicsSetup.cpp b/Demos3/bullet2/CoordinateFramesDemo/CoordinateFrameDemoPhysicsSetup.cpp new file mode 100644 index 000000000..f0c533ade --- /dev/null +++ b/Demos3/bullet2/CoordinateFramesDemo/CoordinateFrameDemoPhysicsSetup.cpp @@ -0,0 +1,90 @@ + + +#include "CoordinateFrameDemoPhysicsSetup.h" +#include "btBulletDynamicsCommon.h" +#define ARRAY_SIZE_Y 5 +#define ARRAY_SIZE_X 5 +#define ARRAY_SIZE_Z 5 + +void CoordinateFrameDemoPhysicsSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) +{ + createEmptyDynamicsWorld(); + gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld); + m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints); + + ///create a few basic rigid bodies + btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); + gfxBridge.createCollisionShapeGraphicsObject(groundShape); + + //groundShape->initializePolyhedralFeatures(); +// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0,-50,0)); + + { + btScalar mass(0.); + btRigidBody* body = createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1)); + gfxBridge.createRigidBodyGraphicsObject(body, btVector3(0, 1, 0)); + } + + + { + //create a few dynamic rigidbodies + // Re-using the same collision is better for memory usage and performance + + btBoxShape* colShape = createBoxShape(btVector3(1,1,1)); + gfxBridge.createCollisionShapeGraphicsObject(colShape); + + //btCollisionShape* colShape = new btSphereShape(btScalar(1.)); + m_collisionShapes.push_back(colShape); + + /// Create Dynamic Objects + btTransform startTransform; + startTransform.setIdentity(); + + btScalar mass(1.f); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0,0,0); + if (isDynamic) + colShape->calculateLocalInertia(mass,localInertia); + + + for (int k=0;k + @@ -145,6 +146,7 @@ +