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

}
示例#2
0
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);
	}
}
示例#3
0
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);
}
示例#4
0
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);
    }


}
示例#5
0
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);
    }


}