void CoordinateFrameDemoPhysicsSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
{
createEmptyDynamicsWorld();
m_dynamicsWorld->setGravity(btVector3(0,0,0));
gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(m_dynamicsWorld->getDebugDrawer()->getDebugMode() + btIDebugDraw::DBG_DrawFrames);
btScalar sqr2 = btSqrt(2);
btVector3 tetraVerts[] = {
btVector3(1.f, 0.f, -1/sqr2),
btVector3(-1.f, 0.f, -1/sqr2),
btVector3(0, 1.f, 1/sqr2),
btVector3(0, -1.f, 1/sqr2),
};
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
btCompoundShape* hull = new btCompoundShape();
btConvexHullShape* childHull = new btConvexHullShape(&tetraVerts[0].getX(),sizeof(tetraVerts)/sizeof(btVector3),sizeof(btVector3));
childHull->initializePolyhedralFeatures();
btTransform childTrans;
childTrans.setIdentity();
childTrans.setOrigin(btVector3(2,0,0));
hull->addChildShape(childTrans,childHull);
gfxBridge.createCollisionShapeGraphicsObject(hull);
m_collisionShapes.push_back(hull);
/// 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)
hull->calculateLocalInertia(mass,localInertia);
startTransform.setOrigin(btVector3(0,0,0));
btRigidBody* body = createRigidBody(mass,startTransform,hull);
gfxBridge.createRigidBodyGraphicsObject(body, btVector3(1, 1, 0));
}
}
void MultiBodyVehicleSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
{
int upAxis = 1;
btVector4 colors[4] =
{
btVector4(1,0,0,1),
btVector4(0,1,0,1),
btVector4(0,1,1,1),
btVector4(1,1,0,1),
};
int curColor = 0;
gfxBridge.setUpAxis(upAxis);
this->createEmptyDynamicsWorld();
gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
//btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawWireframe
+btIDebugDraw::DBG_DrawContactPoints
+btIDebugDraw::DBG_DrawAabb
);//+btIDebugDraw::DBG_DrawConstraintLimits);
createMultiBodyVehicle();
if (1)
{
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);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
gfxBridge.createRigidBodyGraphicsObject(body,color);
}
}
void GyroscopicSetup::syncPhysicsToGraphics(GraphicsPhysicsBridge& gfxBridge)
{
CommonRigidBodySetup::syncPhysicsToGraphics(gfxBridge);
//render method names above objects
for (int i=0;i<m_dynamicsWorld->getNumCollisionObjects();i++)
{
btRigidBody* body = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[i]);
if (body && body->getInvMass()>0)
{
btTransform tr = body->getWorldTransform();
btVector3 pos = tr.getOrigin()+btVector3(0,0,2);
btScalar size=1;
gfxBridge.drawText3D(gyroNames[i],pos.x(),pos.y(),pos.z(),size);
}
}
}
void ImportSTLDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
{
gfxBridge.setUpAxis(2);
this->createEmptyDynamicsWorld();
gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
const char* fileName = "l_finger_tip.stl";
char relativeFileName[1024];
const char* prefix[]={"./data/","../data/","../../data/","../../../data/","../../../../data/"};
int prefixIndex=-1;
{
int numPrefixes = sizeof(prefix)/sizeof(char*);
for (int i=0;i<numPrefixes;i++)
{
FILE* f = 0;
sprintf(relativeFileName,"%s%s",prefix[i],fileName);
f = fopen(relativeFileName,"r");
if (f)
{
fclose(f);
prefixIndex = i;
break;
}
}
}
if (prefixIndex<0)
return;
btVector3 shift(0,0,0);
btVector3 scaling(10,10,10);
// int index=10;
{
GLInstanceGraphicsShape* gfxShape = LoadMeshFromSTL(relativeFileName);
btTransform trans;
trans.setIdentity();
trans.setRotation(btQuaternion(btVector3(1,0,0),SIMD_HALF_PI));
btVector3 position = trans.getOrigin();
btQuaternion orn = trans.getRotation();
btVector3 color(0,0,1);
int shapeId = m_app->m_instancingRenderer->registerShape(&gfxShape->m_vertices->at(0).xyzw[0], gfxShape->m_numvertices, &gfxShape->m_indices->at(0), gfxShape->m_numIndices);
// int id =
m_app->m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling);
/*
btTriangleMesh* trimeshData = new btTriangleMesh();
for (int i=0;i<gfxShape->m_numvertices;i++)
{
for (int j=0;j<3;j++)
gfxShape->m_vertices->at(i).xyzw[j] += shift[j];
}
for (int i=0;i<gfxShape->m_numIndices;i+=3)
{
int index0 = gfxShape->m_indices->at(i);
int index1 = gfxShape->m_indices->at(i+1);
int index2 = gfxShape->m_indices->at(i+2);
btVector3 v0(gfxShape->m_vertices->at(index0).xyzw[0],
gfxShape->m_vertices->at(index0).xyzw[1],
gfxShape->m_vertices->at(index0).xyzw[2]);
btVector3 v1(gfxShape->m_vertices->at(index1).xyzw[0],
gfxShape->m_vertices->at(index1).xyzw[1],
gfxShape->m_vertices->at(index1).xyzw[2]);
btVector3 v2(gfxShape->m_vertices->at(index2).xyzw[0],
gfxShape->m_vertices->at(index2).xyzw[1],
gfxShape->m_vertices->at(index2).xyzw[2]);
trimeshData->addTriangle(v0,v1,v2);
}
//btConvexHullShape* convexShape = new btConvexHullShape(&verts[0].x(),verts.size(),sizeof(btVector3));
btBvhTriangleMeshShape* shape = new btBvhTriangleMeshShape(trimeshData,true);//meshInterface);
btTransform startTrans;startTrans.setIdentity();
btRigidBody* body = this->createRigidBody(0,startTrans,shape);
//gfxBridge.createCollisionShapeGraphicsObject(shape);
btVector3 color(0,0,1);
*/
//gfxBridge.createRigidBodyGraphicsObject(body,color);
}
}
void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> 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;v<(int)link->visual_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<btVector3> vertices;
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
int numSteps = 32;
for (int i=0;i<numSteps;i++)
{
btVector3 vert(cyl->radius*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<my_shared_ptr<Link> >::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;
const char* someFileName="r2d2.urdf";
std::string xml_string;
const char* prefix[]={"./","./data/","../data/","../../data/","../../../data/","../../../../data/"};
int numPrefixes = sizeof(prefix)/sizeof(const char*);
char relativeFileName[1024];
FILE* f=0;
bool fileFound = false;
int result = 0;
for (int i=0;!f && i<numPrefixes;i++)
{
sprintf(relativeFileName,"%s%s",prefix[i],someFileName);
f = fopen(relativeFileName,"rb");
if (f)
{
fileFound = true;
break;
}
}
if (f)
{
fclose(f);
}
if (!fileFound){
std::cerr << "URDF file not found, using a dummy test URDF" << std::endl;
xml_string = std::string(urdf_char);
} else
{
std::fstream xml_file(relativeFileName, 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<ModelInterface> 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<const Link> 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();
if (1)
{
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);
}
}
int main(int argc, char* argv[])
{
b3gDefaultOpenGLWindow* window = new b3gDefaultOpenGLWindow();
//window->setKeyboardCallback(keyCallback);
b3gWindowConstructionInfo wci;
wci.m_openglVersion = 2;
wci.m_width = sWidth;
wci.m_height = sHeight;
// wci.m_resizeCallback = MyResizeCallback;
window->createWindow(wci);
window->setResizeCallback(MyResizeCallback);
window->setMouseButtonCallback(MyMouseButtonCallback);
window->setMouseMoveCallback(MyMouseMoveCallback);
window->setKeyboardCallback(MyKeyboardCallback);
// window->setWindowTitle("render test");
int majorGlVersion, minorGlVersion;
if (!sscanf((const char*)glGetString(GL_VERSION), "%d.%d", &majorGlVersion, &minorGlVersion)==2)
{
printf("Exit: Error cannot extract OpenGL version from GL_VERSION string\n");
exit(0);
}
if (majorGlVersion>=3 && wci.m_openglVersion>=3)
{
float retinaScale = 1.f;
#ifndef __APPLE__
#ifndef _WIN32
//we need glewExperimental on Linux
glewExperimental = GL_TRUE;
#endif // _WIN32
glewInit();
#endif
//we ned to call glGetError twice, because of some Ubuntu/Intel/OpenGL issue
GLuint err = glGetError();
err = glGetError();
btAssert(err==GL_NO_ERROR);
retinaScale = window->getRetinaScale();
//primRenderer = new GLPrimitiveRenderer(sWidth,sHeight);
//sth_stash* font = initFont(primRenderer );
//gwenRenderer = new GwenOpenGL3CoreRenderer(primRenderer,font,sWidth,sHeight,retinaScale);
} else
{
//OpenGL 2.x
/*gwenRenderer = new Gwen::Renderer::OpenGL_DebugFont();
skin.SetRender( gwenRenderer );
pCanvas = new Gwen::Controls::Canvas( &skin );
pCanvas->SetSize( sWidth, sHeight);
pCanvas->SetDrawBackground( true );
pCanvas->SetBackgroundColor( Gwen::Color( 150, 170, 170, 255 ) );
*/
}
// glClearColor(0.2,0.2,0.2,1);
BasicDemoPhysicsSetup physicsSetup;
GraphicsPhysicsBridge br;
physicsSetup.initPhysics(br);
struct MyAppie : public DemoApplication
{
virtual void initPhysics()
{
}
virtual void clientMoveAndDisplay()
{
}
virtual void swapBuffers()
{
}
virtual void updateModifierKeys()
{
m_modifierKeys = 0;
if (isAltPressed)
m_modifierKeys |= BT_ACTIVE_ALT;
if (isCtrlPressed)
m_modifierKeys |= BT_ACTIVE_CTRL;
if (isShiftPressed)
m_modifierKeys |= BT_ACTIVE_SHIFT;
}
};
{
MyAppie appie;
appie.setDynamicsWorld(physicsSetup.m_dynamicsWorld);
appie.reshape(sWidth,sHeight);
appie.setShadows(true);
gApp = &appie;
GLDebugDrawer draw;
physicsSetup.m_dynamicsWorld->setDebugDrawer(&draw);
btClock timer;
unsigned long prevTime = timer.getTimeMicroseconds();
do
{
unsigned long curTime = timer.getTimeMicroseconds();
if (!appie.isIdle())
{
physicsSetup.stepSimulation((curTime-prevTime)*(1./1000000.));
}
prevTime = curTime;
window->startRendering();
br.syncPhysicsToGraphics(physicsSetup.m_dynamicsWorld);
appie.renderme();
physicsSetup.m_dynamicsWorld->debugDrawWorld();
window->endRendering();
} while (!window->requestedExit());
}
window->closeWindow();
delete window;
physicsSetup.exitPhysics();
printf("hello\n");
}
void GyroscopicSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
{
gfxBridge.setUpAxis(2);
createEmptyDynamicsWorld();
m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
btVector3 positions[5] = {
btVector3( -10, 8,4),
btVector3( -5, 8,4),
btVector3( 0, 8,4),
btVector3( 5, 8,4),
btVector3( 10, 8,4),
};
for (int i = 0; i<5; i++)
{
btCylinderShapeZ* pin = new btCylinderShapeZ(btVector3(0.1,0.1, 0.2));
btBoxShape* box = new btBoxShape(btVector3(1,0.1,0.1));
box->setMargin(0.01);
pin->setMargin(0.01);
btCompoundShape* compound = new btCompoundShape();
compound->addChildShape(btTransform::getIdentity(), pin);
btTransform offsetBox(btMatrix3x3::getIdentity(),btVector3(0,0,0.2));
compound->addChildShape(offsetBox, box);
btScalar masses[2] = {0.3,0.1};
btVector3 localInertia;
btTransform principal;
compound->calculatePrincipalAxisTransform(masses,principal,localInertia);
btRigidBody* body = new btRigidBody(1, 0, compound, localInertia);
btTransform tr;
tr.setIdentity();
tr.setOrigin(positions[i]);
body->setCenterOfMassTransform(tr);
body->setAngularVelocity(btVector3(0, 0.1, 10));//51));
//body->setLinearVelocity(btVector3(3, 0, 0));
body->setFriction(btSqrt(1));
m_dynamicsWorld->addRigidBody(body);
body->setFlags(gyroflags[i]);
m_dynamicsWorld->getSolverInfo().m_maxGyroscopicForce = 10.f;
body->setDamping(0.0000f, 0.000f);
}
{
//btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(0.5)));
btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0, 0, 1), 0);
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, 0, 0));
btRigidBody* groundBody;
groundBody = createRigidBody(0, groundTransform, groundShape);
groundBody->setFriction(btSqrt(2));
}
gfxBridge.autogenerateGraphicsObjects(m_dynamicsWorld);
}
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;
const 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<ModelInterface> 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<const Link> 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);
}
}
