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 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 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);
}
}
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);
}
}