virtual	void	processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold**	manifolds,int numManifolds, int islandId)
	{
		if (islandId<0)
		{
			///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
			m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
		} else
		{
				//also add all non-contact constraints/joints for this island
			btTypedConstraint** startConstraint = 0;
			int numCurConstraints = 0;
			int i;
			
			//find the first constraint for this island
			for (i=0;i<m_numConstraints;i++)
			{
				if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
				{
					startConstraint = &m_sortedConstraints[i];
					break;
				}
			}
			//count the number of constraints in this island
			for (;i<m_numConstraints;i++)
			{
				if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
				{
					numCurConstraints++;
				}
			}

			if (m_solverInfo->m_minimumSolverBatchSize<=1)
			{
				m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
			} else
			{
				
				for (i=0;i<numBodies;i++)
					m_bodies.push_back(bodies[i]);
				for (i=0;i<numManifolds;i++)
					m_manifolds.push_back(manifolds[i]);
				for (i=0;i<numCurConstraints;i++)
					m_constraints.push_back(startConstraint[i]);
				if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
				{
					processConstraints();
				} else
				{
					//printf("deferred\n");
				}
			}
		}
	}
 void PhysicsClientExample::enqueueCommand(const SharedMemoryCommand& orgCommand)
	{
		m_userCommandRequests.push_back(orgCommand);
		SharedMemoryCommand& cmd = m_userCommandRequests[m_userCommandRequests.size()-1];

		//b3Printf("User put command request %d on queue (queue length = %d)\n",cmd.m_type, m_userCommandRequests.size());
	}
void OpenGLExampleBrowser::registerFileImporter(const char* extension, CommonExampleInterface::CreateFunc*		createFunc)
{
    FileImporterByExtension fi;
    fi.m_extension = extension;
    fi.m_createFunc = createFunc;
    gFileImporterByExtension.push_back(fi);
}
Beispiel #4
0
void CreateRigidBody(XYZ position, XYZ rotation, XYZ size)
{
	//create a dynamic rigidbody

	btCollisionShape* colShape = new btBoxShape(btVector3(size.x / 2.0, size.y / 2.0, size.z / 2.0));
	//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
	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);

	startTransform.setOrigin(btVector3(position.x, position.y, position.z));
	
	btQuaternion rot;
	rot.setEuler(rotation.x,rotation.y,rotation.z);
	
	startTransform.setRotation(rot);

	//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
	btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
	boxBody = new btRigidBody(rbInfo);

	dynamicsWorld->addRigidBody(boxBody);
}
Beispiel #5
0
void buildBoneTree(akSkeleton* skel, btAlignedObjectArray<akMatrix4>& bind, Blender::Bone* bone, UTuint8 parent)
{
	//if(!(bone->flag & BONE_NO_DEFORM) || parent == AK_JOINT_NO_PARENT)
	{
		utHashedString jname = bone->name;
		
		float* bmat = (float*)bone->arm_mat; 
		akMatrix4 mat(akVector4(bmat[4*0+0], bmat[4*0+1], bmat[4*0+2], bmat[4*0+3]),
					  akVector4(bmat[4*1+0], bmat[4*1+1], bmat[4*1+2], bmat[4*1+3]),
					  akVector4(bmat[4*2+0], bmat[4*2+1], bmat[4*2+2], bmat[4*2+3]),
					  akVector4(bmat[4*3+0], bmat[4*3+1], bmat[4*3+2], bmat[4*3+3]));
		
		bind.push_back(mat);
		parent = skel->addJoint(jname, parent);
		if(bone->flag & BONE_NO_SCALE)
			skel->getJoint(parent)->m_inheritScale = false;
	}
	
	Blender::Bone* chi = static_cast<Blender::Bone*>(bone->childbase.first);
	while (chi)
	{
		// recurse
		buildBoneTree(skel, bind, chi, parent);
		chi = chi->next;
	}
}
Beispiel #6
0
btRigidBody* localCreateRigidBody(float mass, const btTransform& startTransform,btCollisionShape* shape)
{


	bool isDynamic = (mass != 0.f);

	btVector3 localInertia(0,0,0);
	if (isDynamic) shape->calculateLocalInertia(mass,localInertia);

	btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
	btRigidBody::btRigidBodyConstructionInfo cInfo(mass,myMotionState,shape,localInertia);

	box_body.push_back(new btRigidBody(cInfo));
	box_body[box_body.size()-1]->setContactProcessingThreshold(m_defaultContactProcessingThreshold);


	m_dynamicsWorld->addRigidBody(box_body[box_body.size()-1]);

	return box_body[box_body.size()-1];

/*

	btRigidBody* body = new btRigidBody(cInfo);
	body->setContactProcessingThreshold(m_defaultContactProcessingThreshold);
	m_dynamicsWorld->addRigidBody(body);


	return body;

*/

}
Beispiel #7
0
 void submitJob( IJob* job )
 {
     m_jobQueue.push_back( job );
     lockQueue();
     m_tailIndex++;
     m_queueIsEmpty = false;
     unlockQueue();
 }
/* ----------------------------------------------------------------------- 
 | build bullet box shape
 | 
 | : default create 125 (5x5x5) dynamic object
   ----------------------------------------------------------------------- */
bool
buildBoxShapeArray(Ogre::SceneManager* sceneMgr, btDynamicsWorld* dynamicsWorld, btAlignedObjectArray<btCollisionShape*>& collisionShapes,
                   const btVector3& array_size, btScalar scale)
{
    btTransform startTransform;
    startTransform.setIdentity();

    btScalar mass(1.f);	
    btVector3 localInertia(0,0,0);	
    btBoxShape* colShape = new btBoxShape(btVector3(scale, scale, scale));
    btAssert(colShape);    
    colShape->calculateLocalInertia(mass,localInertia);
    collisionShapes.push_back(colShape);

    float start_x = - array_size.getX()/2;
    float start_y = array_size.getY();
    float start_z = - array_size.getZ()/2;

    int index = 0;
    for (int k=0;k<array_size.getY();k++)
    {
	    for (int i=0;i<array_size.getX();i++)
	    {
            for(int j=0;j<array_size.getZ();j++)
		    {
			    startTransform.setOrigin(scale * btVector3(
								    btScalar(2.0*i + start_x),
								    btScalar(20+2.0*k + start_y),
								    btScalar(2.0*j + start_z)));

			    //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
			    btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
			    btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
			    btRigidBody* body = new btRigidBody(rbInfo);
                body->setContactProcessingThreshold(BT_LARGE_FLOAT);

                if (sceneMgr)
                {
                    Ogre::Entity* ent = sceneMgr->createEntity("ent_" + Ogre::StringConverter::toString(index++),"Barrel.mesh");

                    Ogre::SceneNode* node = sceneMgr->getRootSceneNode()->createChildSceneNode("node_box_" + Ogre::StringConverter::toString(index++));
                    node->attachObject(ent);
                    node->setPosition(startTransform.getOrigin().getX(), startTransform.getOrigin().getY(), startTransform.getOrigin().getZ());

                    const Ogre::AxisAlignedBox& aabb = ent->getBoundingBox();
                    const Ogre::Vector3& boxScale = (aabb.getMaximum() - aabb.getMinimum())/2.0f;
                    node->scale(scale/boxScale.x, scale/boxScale.y, scale/boxScale.z);

                    body->setUserPointer((void*)node);
                }

			    dynamicsWorld->addRigidBody(body);
		    }
	    }		
    }

    return true;
}
    void enqueueCommand(const SharedMemoryCommand& orgCommand)
	{
		m_userCommandRequests.push_back(orgCommand);
		SharedMemoryCommand& cmd = m_userCommandRequests[m_userCommandRequests.size()-1];
		cmd.m_sequenceNumber = m_sequenceNumberGenerator++;
		cmd.m_timeStamp = m_realtimeClock.getTimeMicroseconds();

		b3Printf("User put command request %d on queue (queue length = %d)\n",cmd.m_type, m_userCommandRequests.size());
	}
Beispiel #10
0
void	btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut )
{
	const int numbrushes = planeEquations.size();
	// brute force:
	for (int i=0;i<numbrushes;i++)
	{
		const btVector3& N1 = planeEquations[i];
		

		for (int j=i+1;j<numbrushes;j++)
		{
			const btVector3& N2 = planeEquations[j];
				
			for (int k=j+1;k<numbrushes;k++)
			{

				const btVector3& N3 = planeEquations[k];

				btVector3 n2n3; n2n3 = N2.cross(N3);
				btVector3 n3n1; n3n1 = N3.cross(N1);
				btVector3 n1n2; n1n2 = N1.cross(N2);
				
				if ( ( n2n3.length2() > btScalar(0.0001) ) &&
					 ( n3n1.length2() > btScalar(0.0001) ) &&
					 ( n1n2.length2() > btScalar(0.0001) ) )
				{
					//point P out of 3 plane equations:

					//	d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )  
					//P =  -------------------------------------------------------------------------  
					//   N1 . ( N2 * N3 )  


					btScalar quotient = (N1.dot(n2n3));
					if (btFabs(quotient) > btScalar(0.000001))
					{
						quotient = btScalar(-1.) / quotient;
						n2n3 *= N1[3];
						n3n1 *= N2[3];
						n1n2 *= N3[3];
						btVector3 potentialVertex = n2n3;
						potentialVertex += n3n1;
						potentialVertex += n1n2;
						potentialVertex *= quotient;

						//check if inside, and replace supportingVertexOut if needed
						if (isPointInsidePlanes(planeEquations,potentialVertex,btScalar(0.01)))
						{
							verticesOut.push_back(potentialVertex);
						}
					}
				}
			}
		}
	}
}
/* ----------------------------------------------------------------------- 
 | the function describe build btRigidBody from Ogre mesh :   
 |
 | @prama in : Ogre entity (use as a single object)
 | @pamra out : return true if build success and add collision shape to dynamicsworld
   ----------------------------------------------------------------------- */
bool
buildRigidBodyFromOgreEntity(Ogre::Entity* ent,
                             btDynamicsWorld* dynamicsWorld, 
                             btAlignedObjectArray<btCollisionShape*>& collisionShapes,
                             void* &data)
{
    void *vertices, *indices;
    size_t vertexCount = 0, indexCount = 0;
    
    getVertexBuffer(ent, vertices, vertexCount, indices, indexCount);
       
    btScalar mass(1.0f);	
    btVector3 localInertia(0,0,0);
    
    data = new btTriangleMesh();
    btTriangleMesh* trimesh = static_cast<btTriangleMesh*>(data);
    btAssert(trimesh);

    Ogre::Vector3* vert = (Ogre::Vector3*)vertices;
    Ogre::ulong* index = (Ogre::ulong*)indices;

	for (size_t i=0 ; i<vertexCount ; i++)
	{
		int index0 = index[i*3];
		int index1 = index[i*3+1];
		int index2 = index[i*3+2];

        btVector3 vertex0(vert[index0].x, vert[index0].y, vert[index0].z);
        btVector3 vertex1(vert[index1].x, vert[index1].y, vert[index1].z);
        btVector3 vertex2(vert[index2].x, vert[index2].y, vert[index2].z);

		trimesh->addTriangle(vertex0,vertex1,vertex2);
	}

    delete [] vertices;
    delete [] indices;
    
    btCollisionShape* colShape = new btConvexTriangleMeshShape(trimesh);
    const btVector3& scale = colShape->getLocalScaling();
    colShape->calculateLocalInertia(mass,localInertia);
    collisionShapes.push_back(colShape);
    
    btTransform trans;
    trans.setIdentity();
    btDefaultMotionState* motionState = new btDefaultMotionState(trans);
    btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,motionState,colShape,localInertia);
    btRigidBody* body = new btRigidBody(rbInfo);
    dynamicsWorld->addRigidBody(body);
    
    // link RigidBody and SceneNode
    Ogre::SceneNode* node = ent->getParentSceneNode();
    body->setUserPointer((void*)node);
    
    return true;
}
void ROSURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const
{
    childLinkIndices.resize(0);
    int numChildren = m_data->m_links[linkIndex]->child_links.size();
        
    for (int i=0;i<numChildren;i++)
    {
        int childIndex =m_data->m_links[linkIndex]->child_links[i]->m_link_index;
        childLinkIndices.push_back(childIndex);
    }
}
Beispiel #13
0
    virtual void	drawLine(const btVector3& from1,const btVector3& to1,const btVector3& color1)
    {
        //float from[4] = {from1[0],from1[1],from1[2],from1[3]};
        //float to[4] = {to1[0],to1[1],to1[2],to1[3]};
        //float color[4] = {color1[0],color1[1],color1[2],color1[3]};
        //m_glApp->m_instancingRenderer->drawLine(from,to,color);
        if (m_currentLineColor!=color1 || m_linePoints.size() >= BT_LINE_BATCH_SIZE)
        {
            flushLines();
            m_currentLineColor = color1;
        }
        MyDebugVec3 from(from1);
        MyDebugVec3 to(to1);

        m_linePoints.push_back(from);
        m_linePoints.push_back(to);

        m_lineIndices.push_back(m_lineIndices.size());
        m_lineIndices.push_back(m_lineIndices.size());

    }
Beispiel #14
0
void			btDbvt::extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves)
{
	if(node->isinternal())
	{
		extractLeaves(node->childs[0],leaves);
		extractLeaves(node->childs[1],leaves);
	}
	else
	{
		leaves.push_back(node);
	}	
}
Beispiel #15
0
// TODO: this routine appears to be numerically instable ...
void getVerticesInsidePlanes(const btAlignedObjectArray<btVector3>& planes,
                             btAlignedObjectArray<btVector3>& verticesOut,
                             std::set<int>& planeIndicesOut)
{
    // Based on btGeometryUtil.cpp (Gino van den Bergen / Erwin Coumans)
    verticesOut.resize(0);
    planeIndicesOut.clear();
    const int numPlanes = planes.size();
    int i, j, k, l;
    for (i = 0; i < numPlanes; i++)
    {
        const btVector3& N1 = planes[i];
        for (j = i + 1; j < numPlanes; j++)
        {
            const btVector3& N2 = planes[j];
            btVector3 n1n2 = N1.cross(N2);
            if (n1n2.length2() > btScalar(0.0001))
            {
                for (k = j + 1; k < numPlanes; k++)
                {
                    const btVector3& N3 = planes[k];
                    btVector3 n2n3 = N2.cross(N3);
                    btVector3 n3n1 = N3.cross(N1);
                    if ((n2n3.length2() > btScalar(0.0001)) && (n3n1.length2() > btScalar(0.0001) ))
                    {
                        btScalar quotient = (N1.dot(n2n3));
                        if (btFabs(quotient) > btScalar(0.0001))
                        {
                            btVector3 potentialVertex = (n2n3 * N1[3] + n3n1 * N2[3] + n1n2 * N3[3]) * (btScalar(-1.) / quotient);
                            for (l = 0; l < numPlanes; l++)
                            {
                                const btVector3& NP = planes[l];
                                if (btScalar(NP.dot(potentialVertex))+btScalar(NP[3]) > btScalar(0.000001))
                                    break;
                            }
                            if (l == numPlanes)
                            {
                                // vertex (three plane intersection) inside all planes
                                verticesOut.push_back(potentialVertex);
                                planeIndicesOut.insert(i);
                                planeIndicesOut.insert(j);
                                planeIndicesOut.insert(k);
                            }
                        }
                    }
                }
            }
        }
    }
}
Beispiel #16
0
void BulletURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const
{
	childLinkIndices.resize(0);
	UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
	if (linkPtr)
	{
		const UrdfLink* link = *linkPtr;
		//int numChildren = m_data->m_urdfParser->getModel().m_links.getAtIndex(linkIndex)->
		
		for (int i=0;i<link->m_childLinks.size();i++)
		{
			int childIndex =link->m_childLinks[i]->m_linkIndex;
			childLinkIndices.push_back(childIndex);
		}
	}
}
Beispiel #17
0
void	btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut )
{
		const int numvertices = vertices.size();
	// brute force:
	for (int i=0;i<numvertices;i++)
	{
		const btVector3& N1 = vertices[i];
		

		for (int j=i+1;j<numvertices;j++)
		{
			const btVector3& N2 = vertices[j];
				
			for (int k=j+1;k<numvertices;k++)
			{

				const btVector3& N3 = vertices[k];

				btVector3 planeEquation,edge0,edge1;
				edge0 = N2-N1;
				edge1 = N3-N1;
				btScalar normalSign = btScalar(1.);
				for (int ww=0;ww<2;ww++)
				{
					planeEquation = normalSign * edge0.cross(edge1);
					if (planeEquation.length2() > btScalar(0.0001))
					{
						planeEquation.normalize();
						if (notExist(planeEquation,planeEquationsOut))
						{
							planeEquation[3] = -planeEquation.dot(N1);
							
								//check if inside, and replace supportingVertexOut if needed
								if (areVerticesBehindPlane(planeEquation,vertices,btScalar(0.01)))
								{
									planeEquationsOut.push_back(planeEquation);
								}
						}
					}
					normalSign = btScalar(-1.);
				}
			
			}
		}
	}

}
Beispiel #18
0
void	PhysicsClient::initPhysics()
{
	if (m_guiHelper && m_guiHelper->getParameterInterface())
	{
		bool isTrigger = false;
		
		createButton("Load URDF",CMD_LOAD_URDF,  isTrigger);
		createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION,  isTrigger);
		createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM,  isTrigger);
		createButton("Get State",CMD_REQUEST_ACTUAL_STATE,  isTrigger);
		createButton("Send Desired State",CMD_SEND_DESIRED_STATE,  isTrigger);
		createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger);
		
	} else
	{
		m_userCommandRequests.push_back(CMD_LOAD_URDF);
		m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
		//m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE);
		m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
		//m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK);
		m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE);
		//m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY);
		m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION);
		m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
		m_userCommandRequests.push_back(CMD_SHUTDOWN);
		
	}


    m_testBlock1 = (SharedMemoryExampleData*)m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
    if (m_testBlock1)
    {
     //   btAssert(m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER);
        if (m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
        {
            b3Error("Error: please start server before client\n");
            m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
            m_testBlock1 = 0;
        } else
		{
			b3Printf("Shared Memory status is OK\n");
		}
    } else
	{
		m_wantsTermination = true;
	}
}
bool
buildGroundShape(Ogre::SceneManager* sceneMgr, btDynamicsWorld* dynamicsWorld, btAlignedObjectArray<btCollisionShape*>& collisionShapes)
{
    btBoxShape* groundShape = new btBoxShape(btVector3(btScalar(150.0f),btScalar(1.0f),btScalar(150.0f)));
    //btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0), 1);
    //groundShape->setLocalScaling(btVector3(1,1,1));

    btDefaultMotionState* groundMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,-1,0)));
    btRigidBody::btRigidBodyConstructionInfo groundRigidBodyCI(0,groundMotionState,groundShape,btVector3(0,0,0));
    btRigidBody* groundRigidBody = new btRigidBody(groundRigidBodyCI);
    groundRigidBody->setCcdMotionThreshold(1.0f);
    groundRigidBody->setRestitution(0.0f);
    groundRigidBody->setFriction(1.0f);

    const btTransform& transform = groundRigidBody->getWorldTransform();
    btVector3 trans = transform.getOrigin();
    btQuaternion rot = transform.getRotation();

    dynamicsWorld->addRigidBody(groundRigidBody);

    collisionShapes.push_back(groundShape);

    // build plane if scene manager exist
    if (sceneMgr)
    {
        Ogre::Plane* plane = new Ogre::MovablePlane("Plane");
	    plane->d = 0;
	    plane->normal = Ogre::Vector3::UNIT_Y;
	
	    Ogre::MeshManager::getSingleton().createPlane("PlaneMesh", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, *plane,
		    256, 256, 1, 1, true, 1, 1, 1, Ogre::Vector3::UNIT_Z);

        Ogre::Entity* ent = sceneMgr->createEntity("PlaneEntity", "PlaneMesh");
        assert(ent);
        ent->setCastShadows(false);
        ent->setMaterialName("DefaultPlane");
	    
        Ogre::SceneNode* node = sceneMgr->getRootSceneNode()->createChildSceneNode("PlaneNode");
	    node->attachObject(ent);
	    node->setPosition(Ogre::Vector3::ZERO);
    } // End if

    return true;
}
Beispiel #20
0
    void body(int bodytype, vec3 origin,vec3 size, vec3 inertia,float mass) {
        btCollisionShape* aShape = NULL;
		pos[0] = origin.x; pos[1] = origin.y; pos[2] = origin.z;
        inertia.x = 1;
		inertia.y = 1;
		inertia.z = 1;
		mass = 1;

		switch (bodytype) {
            case BODYTYPE_BOX:
                aShape = new btBoxShape(btVector3(btScalar(size.x),btScalar(size.y),btScalar(size.z)));
                break;
			case BODYTYPE_CYLINDER:
				aShape = new btCylinderShape(btVector3(btScalar(size.x),btScalar(size.y),btScalar(size.z)));
                break;
			case BODYTYPE_SPHERE:
				aShape = new btSphereShape(btScalar(size.x));
		        break;
        	case BODYTYPE_CAPSULE:
				aShape = new btCapsuleShape(btScalar(size.x),btScalar(size.y));
                break;
			case BODYTYPE_CONE:
				aShape = new btConeShape(btScalar(size.x),btScalar(size.y));
		        break;
        }
        
        if (aShape) {
            btScalar Mass(mass);
            bool isDynamic = (Mass != 0.f);
            btVector3 localInertia(inertia.x,inertia.y,inertia.z);
            if (isDynamic)
                aShape->calculateLocalInertia(mass,localInertia);
            transform.setIdentity();
            transform.setOrigin(btVector3(origin.x,origin.y,origin.z));
			btDefaultMotionState* myMotionState = new btDefaultMotionState(transform);
			btRigidBody::btRigidBodyConstructionInfo rbInfo(Mass,myMotionState,aShape,localInertia);
			body1 = new btRigidBody(rbInfo);            
            collisionShapes.push_back(aShape);
			dynamicsWorld->addRigidBody(body1);
        }

        //return body1;
    }
Beispiel #21
0
void btGImpactCollisionAlgorithm::gimpact_vs_shape_find_pairs(
	btDispatcher* dispatcher,
	const btTransform& trans0,
	const btTransform& trans1,
	const btGImpactShapeInterface* shape0,
	const btCollisionShape* shape1,
	btAlignedObjectArray<int>& collided_primitives
)
{

	btAABB boxshape;


	if(shape0->hasBoxSet())
	{
		btTransform trans1to0 = trans0.inverse();
		trans1to0 *= trans1;

		shape1->getAabb(trans1to0,boxshape.m_min,boxshape.m_max);

		shape0->getBoxSet()->boxQuery(boxshape, collided_primitives);
	}
	else
	{
		shape1->getAabb(trans1,boxshape.m_min,boxshape.m_max);

		btAABB boxshape0;
		int i = shape0->getNumChildShapes();

		while(i--)
		{
			shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max);

			if(boxshape.has_collision(boxshape0))
			{
				collided_primitives.push_back(i);
			}
		}

	}

}
Beispiel #22
0
void GetAllIndices(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int parentIndex, btAlignedObjectArray<childParentIndex>& allIndices)
{
	childParentIndex cp;
	cp.m_index = urdfLinkIndex;
	int mbIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
	cp.m_mbIndex = mbIndex;
	cp.m_parentIndex = parentIndex;
	int parentMbIndex = parentIndex>=0? cache.getMbIndexFromUrdfIndex(parentIndex) : -1;
	cp.m_parentMBIndex = parentMbIndex;

	allIndices.push_back(cp);
	btAlignedObjectArray<int> urdfChildIndices;
	u2b.getLinkChildIndices(urdfLinkIndex, urdfChildIndices);
	int numChildren = urdfChildIndices.size();
	for (int i = 0; i < numChildren; i++)
	{
		int urdfChildLinkIndex = urdfChildIndices[i];
		GetAllIndices(u2b, cache, urdfChildLinkIndex, urdfLinkIndex, allIndices);
	}
}
Beispiel #23
0
/// called whenever key terrain attribute is changed
void TerrainDemo::resetPhysics(void)
{
	// remove old heightfield
	clearWorld();

	// reset gravity to point in appropriate direction
	m_dynamicsWorld->setGravity(getUpVector(m_upAxis, 0.0, -s_gravity));

	// get new heightfield of appropriate type
	m_rawHeightfieldData =
	    getRawHeightfieldData(m_model, m_type, m_minHeight, m_maxHeight);
	btAssert(m_rawHeightfieldData && "failed to create raw heightfield");

	bool flipQuadEdges = false;
	btHeightfieldTerrainShape * heightfieldShape =
	    new btHeightfieldTerrainShape(s_gridSize, s_gridSize,
					  m_rawHeightfieldData,
					  s_gridHeightScale,
					  m_minHeight, m_maxHeight,
					  m_upAxis, m_type, flipQuadEdges);
	btAssert(heightfieldShape && "null heightfield");

	// scale the shape
	btVector3 localScaling = getUpVector(m_upAxis, s_gridSpacing, 1.0);
	heightfieldShape->setLocalScaling(localScaling);

	// stash this shape away
	m_collisionShapes.push_back(heightfieldShape);

	// set origin to middle of heightfield
	btTransform tr;
	tr.setIdentity();
	tr.setOrigin(btVector3(0,-20,0));

	// create ground object
	float mass = 0.0;
	localCreateRigidBody(mass, tr, heightfieldShape);
}
Beispiel #24
0
void	Planar2D::initPhysics()
{

    m_guiHelper->setUpAxis(1);

    ///collision configuration contains default setup for memory, collision setup
    m_collisionConfiguration = new btDefaultCollisionConfiguration();
    //m_collisionConfiguration->setConvexConvexMultipointIterations();

    ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
    m_dispatcher = new	btCollisionDispatcher(m_collisionConfiguration);

    m_simplexSolver = new btVoronoiSimplexSolver();
    m_pdSolver = new btMinkowskiPenetrationDepthSolver();


    m_convexAlgo2d = new btConvex2dConvex2dAlgorithm::CreateFunc(m_simplexSolver,m_pdSolver);
    m_box2dbox2dAlgo = new btBox2dBox2dCollisionAlgorithm::CreateFunc();

    m_dispatcher->registerCollisionCreateFunc(CONVEX_2D_SHAPE_PROXYTYPE,CONVEX_2D_SHAPE_PROXYTYPE,m_convexAlgo2d);
    m_dispatcher->registerCollisionCreateFunc(BOX_2D_SHAPE_PROXYTYPE,CONVEX_2D_SHAPE_PROXYTYPE,m_convexAlgo2d);
    m_dispatcher->registerCollisionCreateFunc(CONVEX_2D_SHAPE_PROXYTYPE,BOX_2D_SHAPE_PROXYTYPE,m_convexAlgo2d);
    m_dispatcher->registerCollisionCreateFunc(BOX_2D_SHAPE_PROXYTYPE,BOX_2D_SHAPE_PROXYTYPE,m_box2dbox2dAlgo);

    m_broadphase = new btDbvtBroadphase();
    //m_broadphase = new btSimpleBroadphase();

    ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
    btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
    m_solver = sol;

    m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
    //m_dynamicsWorld->getSolverInfo().m_erp = 1.f;
    //m_dynamicsWorld->getSolverInfo().m_numIterations = 4;
    m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);



    m_dynamicsWorld->setGravity(btVector3(0,-10,0));

    ///create a few basic rigid bodies
    btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.),btScalar(50.),btScalar(150.)));
//	btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);

    m_collisionShapes.push_back(groundShape);

    btTransform groundTransform;
    groundTransform.setIdentity();
    groundTransform.setOrigin(btVector3(0,-43,0));

    //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
    {
        btScalar mass(0.);

        //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)
            groundShape->calculateLocalInertia(mass,localInertia);

        //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
        btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
        btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
        btRigidBody* body = new btRigidBody(rbInfo);

        //add the body to the dynamics world
        m_dynamicsWorld->addRigidBody(body);
    }


    {
        //create a few dynamic rigidbodies
        // Re-using the same collision is better for memory usage and performance

        btScalar u= btScalar(1*SCALING-0.04);
        btVector3 points[3] = {btVector3(0,u,0),btVector3(-u,-u,0),btVector3(u,-u,0)};
        btConvexShape* childShape0 = new btBoxShape(btVector3(btScalar(SCALING*1),btScalar(SCALING*1),btScalar(0.04)));
        btConvexShape* colShape= new btConvex2dShape(childShape0);
        //btCollisionShape* colShape = new btBox2dShape(btVector3(SCALING*1,SCALING*1,0.04));
        btConvexShape* childShape1 = new btConvexHullShape(&points[0].getX(),3);
        btConvexShape* colShape2= new btConvex2dShape(childShape1);
        btConvexShape* childShape2 = new btCylinderShapeZ(btVector3(btScalar(SCALING*1),btScalar(SCALING*1),btScalar(0.04)));
        btConvexShape* colShape3= new btConvex2dShape(childShape2);


        m_collisionShapes.push_back(colShape);
        m_collisionShapes.push_back(colShape2);
        m_collisionShapes.push_back(colShape3);

        m_collisionShapes.push_back(childShape0);
        m_collisionShapes.push_back(childShape1);
        m_collisionShapes.push_back(childShape2);


        //btUniformScalingShape* colShape = new btUniformScalingShape(convexColShape,1.f);
        colShape->setMargin(btScalar(0.03));
        //btCollisionShape* colShape = new btSphereShape(btScalar(1.));

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

//		float start_x = START_POS_X - ARRAY_SIZE_X/2;
//		float start_y = START_POS_Y;
//		float start_z = START_POS_Z - ARRAY_SIZE_Z/2;

        btVector3 x(-ARRAY_SIZE_X, 8.0f,-20.f);
        btVector3 y;
        btVector3 deltaX(SCALING*1, SCALING*2,0.f);
        btVector3 deltaY(SCALING*2, 0.0f,0.f);

        for (int i = 0; i < ARRAY_SIZE_X; ++i)
        {
            y = x;

            for (int  j = i; j < ARRAY_SIZE_Y; ++j)
            {
                startTransform.setOrigin(y-btVector3(-10,0,0));


                //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
                btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
                btRigidBody::btRigidBodyConstructionInfo rbInfo(0,0,0);
                switch (j%3)
                {
#if 1
                case 0:
                    rbInfo = btRigidBody::btRigidBodyConstructionInfo(mass,myMotionState,colShape,localInertia);
                    break;
                case 1:
                    rbInfo = btRigidBody::btRigidBodyConstructionInfo(mass,myMotionState,colShape3,localInertia);
                    break;
#endif
                default:
                    rbInfo = btRigidBody::btRigidBodyConstructionInfo(mass,myMotionState,colShape2,localInertia);
                }
                btRigidBody* body = new btRigidBody(rbInfo);
                //body->setContactProcessingThreshold(colShape->getContactBreakingThreshold());
                body->setActivationState(ISLAND_SLEEPING);
                body->setLinearFactor(btVector3(1,1,0));
                body->setAngularFactor(btVector3(0,0,1));

                m_dynamicsWorld->addRigidBody(body);
                body->setActivationState(ISLAND_SLEEPING);


                //	y += -0.8*deltaY;
                y += deltaY;
            }

            x += deltaX;
        }

    }

    m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
Beispiel #25
0
//////////////////////Bullet Physics code////////////////////////////////
void init_physics(){

	m_collisionConfiguration = new btFluidRigidCollisionConfiguration();

	//'standard' Bullet configuration
	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
	m_broadphase = new btDbvtBroadphase();
	m_solver = new btSequentialImpulseConstraintSolver();
	m_fluidSolverCPU = new btFluidSphSolverDefault();
	m_dynamicsWorld = new btFluidRigidDynamicsWorld(m_dispatcher, m_broadphase,
			m_solver, m_collisionConfiguration, m_fluidSolverCPU);

	m_fluidWorld = static_cast<btFluidRigidDynamicsWorld*>(m_dynamicsWorld);

	m_fluidWorld->setGravity(btVector3(0.0 ,-9.8, 0.0));
	m_fluidWorld->applyGravity();

	btCollisionShape* groundShape = new btBoxShape( btVector3(100.0, 0.0, 100.0) );
	m_collisionShapes.push_back(groundShape);


	plane = new Plane(m_fluidWorld, groundShape,  0, 0, 0);

	///////////////////// Demo_Drop/////////////////////////////
	const btScalar VOL_BOUND = 52.0f;
	btVector3 volMin(-VOL_BOUND, -10.0f, -VOL_BOUND);
	btVector3 volMax(VOL_BOUND, VOL_BOUND * 2.0f, VOL_BOUND);
	{
		btFluidSph* fluid;

		fluid = new btFluidSph(m_fluidWorld->getGlobalParameters(),
				MIN_FLUID_PARTICLES);
		btFluidSphParametersLocal FL = fluid->getLocalParameters();
		FL.setDefaultParameters();
		FL.m_aabbBoundaryMin = volMin;
		FL.m_aabbBoundaryMax = volMax;
		FL.m_particleRadius = 0.8f;
		FL.m_enableAabbBoundary = 1;
		FL.m_restDensity *= 3.0f;
		FL.m_sphParticleMass *= 2.0f;
		FL.m_stiffness /= 3.0f;

		fluid->setLocalParameters(FL);
		fluid->setGridCellSize(m_fluidWorld->getGlobalParameters() );
		m_fluids.push_back(fluid);

		// Also create an emitter
	//*
		{
			btFluidEmitter* emitter = new btFluidEmitter();
			m_emitter = emitter;

			emitter->m_fluid = fluid;

			const btScalar OFFSET(1.0);
			emitter->m_positions.push_back(btVector3(0, 0, 0));
			emitter->m_positions.push_back(btVector3(0, OFFSET, 0));
			emitter->m_positions.push_back(btVector3(0, -OFFSET, 0));
			emitter->m_positions.push_back(btVector3(OFFSET, 0, 0));
			emitter->m_positions.push_back(btVector3(-OFFSET, 0, 0));

			emitter->m_speed = 1.f;

			emitter->m_center.setValue(12.f, 2.f, 12.f);
			emitter->m_rotation.setEuler(90, -45, 0);
		//
			const btScalar INIT_BOUND = 50.0f;
			btVector3 initMin(-INIT_BOUND, 0.f, 0.f);
			btVector3 initMax(INIT_BOUND, 45.f, INIT_BOUND);
			emitter->addVolume(fluid, initMin, initMax, fluid->getEmitterSpacing(m_fluidWorld->getGlobalParameters()) * 0.87 );
		//
			m_fluidWorld->addSphEmitter(emitter);
		}
		//*/
//*/
		for (int i = 0; i < m_fluids.size(); ++i) {
			const bool ENABLE_CCD = true;
			if (ENABLE_CCD)
				m_fluids[i]->setCcdMotionThreshold(
						m_fluids[i]->getLocalParameters().m_particleRadius);

			m_fluidWorld->addFluidSph(m_fluids[i]);
		}

	}


}
Beispiel #26
0
ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName)
	:CommonMultiBodyBase(helper),
	m_grav(-10),
	m_upAxis(2)
{
	m_data = new ImportUrdfInternalData;

	if (option==1)
	{
		m_useMultiBody = true;
	} else
	{
		m_useMultiBody = false;
	}

	static int count = 0;
	if (fileName)
	{
		setFileName(fileName);
	} else
	{
		gFileNameArray.clear();
		


		//load additional urdf file names from file

		FILE* f = fopen("urdf_files.txt","r");
		if (f)
		{
			int result;
			//warning: we don't avoid string buffer overflow in this basic example in fscanf
			char fileName[1024];
			do
			{
				result = fscanf(f,"%s",fileName);
                b3Printf("urdf_files.txt entry %s",fileName);
				if (result==1)
				{
					gFileNameArray.push_back(fileName);
				}
			} while (result==1);

			fclose(f);
		}
		
		if (gFileNameArray.size()==0)
		{
			gFileNameArray.push_back("r2d2.urdf");

		}

		int numFileNames = gFileNameArray.size();

		if (count>=numFileNames)
		{
			count=0;
		}
		sprintf(m_fileName,"%s",gFileNameArray[count++].c_str());
	}
}
Beispiel #27
0
void ImportUrdfSetup::initPhysics()
{

	
	m_guiHelper->setUpAxis(m_upAxis);
	
	this->createEmptyDynamicsWorld();
	//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
    m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
    m_dynamicsWorld->getDebugDrawer()->setDebugMode(
    btIDebugDraw::DBG_DrawConstraints
    +btIDebugDraw::DBG_DrawContactPoints
    +btIDebugDraw::DBG_DrawAabb
        );//+btIDebugDraw::DBG_DrawConstraintLimits);


	if (m_guiHelper->getParameterInterface())
	{
		SliderParams slider("Gravity", &m_grav);
		slider.m_minVal = -10;
		slider.m_maxVal = 10;
		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
	}
	

	BulletURDFImporter u2b(m_guiHelper, 0);
	
	
	bool loadOk = u2b.loadURDF(m_fileName);

#ifdef TEST_MULTIBODY_SERIALIZATION	
	//test to serialize a multibody to disk or shared memory, with base, link and joint names
	btSerializer* s = new btDefaultSerializer;
#endif //TEST_MULTIBODY_SERIALIZATION

	if (loadOk)
	{
		//printTree(u2b,u2b.getRootLinkIndex());

		//u2b.printTree();

		btTransform identityTrans;
		identityTrans.setIdentity();


		{




			//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
			//int rootLinkIndex = u2b.getRootLinkIndex();
			//b3Printf("urdf root link index = %d\n",rootLinkIndex);
			MyMultiBodyCreator creation(m_guiHelper);

			ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix());
			m_data->m_rb = creation.getRigidBody();
			m_data->m_mb = creation.getBulletMultiBody();
			btMultiBody* mb = m_data->m_mb;
			for (int i = 0; i < u2b.getNumAllocatedCollisionShapes(); i++)
			{
				m_collisionShapes.push_back(u2b.getAllocatedCollisionShape(i));
			}

			if (m_useMultiBody && mb )
			{
				std::string*   name = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
				m_nameMemory.push_back(name);
#ifdef TEST_MULTIBODY_SERIALIZATION
				s->registerNameForPointer(name->c_str(),name->c_str());
#endif//TEST_MULTIBODY_SERIALIZATION
				mb->setBaseName(name->c_str());
				//create motors for each btMultiBody joint
				int numLinks = mb->getNumLinks();
				for (int i=0;i<numLinks;i++)
				{
					int mbLinkIndex = i;
					int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];

					std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex));
					std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
#ifdef TEST_MULTIBODY_SERIALIZATION					
					s->registerNameForPointer(jointName->c_str(),jointName->c_str());
					s->registerNameForPointer(linkName->c_str(),linkName->c_str());
#endif//TEST_MULTIBODY_SERIALIZATION
					m_nameMemory.push_back(jointName);
					m_nameMemory.push_back(linkName);

					mb->getLink(i).m_linkName = linkName->c_str();
					mb->getLink(i).m_jointName = jointName->c_str();
							
					if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute
					    ||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic
					)
					{
						if (m_data->m_numMotors<MAX_NUM_MOTORS)
						{
							
							char motorName[1024];
							sprintf(motorName,"%s q'", jointName->c_str());
							btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
							*motorVel = 0.f;
							SliderParams slider(motorName,motorVel);
							slider.m_minVal=-4;
							slider.m_maxVal=4;
							m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
							float maxMotorImpulse = 10.1f;
							btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse);
							//motor->setMaxAppliedImpulse(0);
							m_data->m_jointMotors[m_data->m_numMotors]=motor;
							m_dynamicsWorld->addMultiBodyConstraint(motor);
							m_data->m_numMotors++;
						}
					}

				}
			} else
			{
				if (1)
				{
					//create motors for each generic joint
					int num6Dof = creation.getNum6DofConstraints();
					for (int i=0;i<num6Dof;i++)
					{
						btGeneric6DofSpring2Constraint* c = creation.get6DofConstraint(i);
						if (c->getUserConstraintPtr())
						{
							GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)c->getUserConstraintPtr();
							if ((jointInfo->m_urdfJointType ==URDFRevoluteJoint) || 
								(jointInfo->m_urdfJointType ==URDFPrismaticJoint) ||
								(jointInfo->m_urdfJointType ==URDFContinuousJoint))
							{
								int urdfLinkIndex = jointInfo->m_urdfIndex;
								std::string jointName = u2b.getJointName(urdfLinkIndex);
								char motorName[1024];
								sprintf(motorName,"%s q'", jointName.c_str());
								btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];

								*motorVel = 0.f;
								SliderParams slider(motorName,motorVel);
								slider.m_minVal=-4;
								slider.m_maxVal=4;
								m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
								m_data->m_generic6DofJointMotors[m_data->m_numMotors]=c;
								bool motorOn = true;
								c->enableMotor(jointInfo->m_jointAxisIndex,motorOn);
								c->setMaxMotorForce(jointInfo->m_jointAxisIndex,10000);
								c->setTargetVelocity(jointInfo->m_jointAxisIndex,0);
								
								m_data->m_numMotors++;
							}
						}
					}
				}
				
			}
		}

		//the btMultiBody support is work-in-progress :-)

        for (int i=0;i<m_dynamicsWorld->getNumMultiBodyConstraints();i++)
        {
            m_dynamicsWorld->getMultiBodyConstraint(i)->finalizeMultiDof();
        }



		bool createGround=true;
		if (createGround)
		{
			btVector3 groundHalfExtents(20,20,20);
			groundHalfExtents[m_upAxis]=1.f;
			btBoxShape* box = new btBoxShape(groundHalfExtents);
			m_collisionShapes.push_back(box);
			box->initializePolyhedralFeatures();

			m_guiHelper->createCollisionShapeGraphicsObject(box);
			btTransform start; start.setIdentity();
			btVector3 groundOrigin(0,0,0);
			groundOrigin[m_upAxis]=-2.5;
			start.setOrigin(groundOrigin);
			btRigidBody* body =  createRigidBody(0,start,box);
			//m_dynamicsWorld->removeRigidBody(body);
		   // m_dynamicsWorld->addRigidBody(body,2,1);
			btVector3 color(0.5,0.5,0.5);
			m_guiHelper->createRigidBodyGraphicsObject(body,color);
		}

		
	}


#ifdef TEST_MULTIBODY_SERIALIZATION
	m_dynamicsWorld->serialize(s);
	b3ResourcePath p;
	char resourcePath[1024];
	if (p.findResourcePath("r2d2_multibody.bullet",resourcePath,1024))
	{
		FILE* f = fopen(resourcePath,"wb");
		fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);
		fclose(f);
	}
#endif//TEST_MULTIBODY_SERIALIZATION

}
Beispiel #28
0
void ExampleEntriesAll::registerExampleEntry(int menuLevel, const char* name,const char* description, CommonExampleInterface::CreateFunc* createFunc, int option)
{
	ExampleEntry e( menuLevel,name,description, createFunc, option);
	gAdditionalRegisteredExamples.push_back(e);
}
void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut)
{

	
	GLInstanceGraphicsShape* glmesh = 0;

	btConvexShape* convexColShape = 0;

	switch (visual->m_geometry.m_type)
	{
		case URDF_GEOM_CYLINDER:
		{
			btAlignedObjectArray<btVector3> vertices;
		
			//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
			int numSteps = 32;
			for (int i = 0; i<numSteps; i++)
			{

				btScalar cylRadius = visual->m_geometry.m_cylinderRadius;
				btScalar cylLength = visual->m_geometry.m_cylinderLength;
				
				btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.);
				vertices.push_back(vert);
				vert[2] = -cylLength / 2.;
				vertices.push_back(vert);
			}

			btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
			cylZShape->setMargin(0.001);
			convexColShape = cylZShape;
			break;
		}
		case URDF_GEOM_BOX:
		{
			
			btVector3 extents = visual->m_geometry.m_boxSize;
			
			btBoxShape* boxShape = new btBoxShape(extents*0.5f);
			//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
			convexColShape = boxShape;
			convexColShape->setMargin(0.001);
			break;
		}
		case URDF_GEOM_SPHERE:
		{
			btScalar radius = visual->m_geometry.m_sphereRadius;
			btSphereShape* sphereShape = new btSphereShape(radius);
			convexColShape = sphereShape;
			convexColShape->setMargin(0.001);
			break;

			break;
		}
		case URDF_GEOM_MESH:
		{
			if (visual->m_name.length())
			{
				//b3Printf("visual->name=%s\n", visual->m_name.c_str());
			}
			if (1)//visual->m_geometry)
			{
				if (visual->m_geometry.m_meshFileName.length())
				{
					const char* filename = visual->m_geometry.m_meshFileName.c_str();
					//b3Printf("mesh->filename=%s\n", filename);
					char fullPath[1024];
					int fileType = 0;
                    
                    char tmpPathPrefix[1024];
                    std::string xml_string;
                    int maxPathLen = 1024;
                    b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen);
                   
                    char visualPathPrefix[1024];
                    sprintf(visualPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix);
                    
                    
					sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
					b3FileUtils::toLower(fullPath);
					if (strstr(fullPath, ".dae"))
					{
						fileType = MY_FILE_COLLADA;
					}
					if (strstr(fullPath, ".stl"))
					{
						fileType = MY_FILE_STL;
					}
                    if (strstr(fullPath,".obj"))
                    {
                        fileType = MY_FILE_OBJ;
                    }


					sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
					FILE* f = fopen(fullPath, "rb");
					if (f)
					{
						fclose(f);
						


						switch (fileType)
						{
                            case MY_FILE_OBJ:
                            {
                                //glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
								b3ImportMeshData meshData;
								if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData))
								{
									
									if (meshData.m_textureImage)
									{
										MyTexture2 texData;
										texData.m_width = meshData.m_textureWidth;
										texData.m_height = meshData.m_textureHeight;
										texData.textureData = meshData.m_textureImage;
										texturesOut.push_back(texData);
									}
									glmesh = meshData.m_gfxShape;
								}

								
                                break;
                            }
                           
						case MY_FILE_STL:
						{
							glmesh = LoadMeshFromSTL(fullPath);
							break;
						}
						case MY_FILE_COLLADA:
						{

							btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
							btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
							btTransform upAxisTrans; upAxisTrans.setIdentity();
							float unitMeterScaling = 1;
							int upAxis = 2;

							LoadMeshFromCollada(fullPath,
								visualShapes,
								visualShapeInstances,
								upAxisTrans,
								unitMeterScaling,
												upAxis);

							glmesh = new GLInstanceGraphicsShape;
					//		int index = 0;
							glmesh->m_indices = new b3AlignedObjectArray<int>();
							glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();

							for (int i = 0; i<visualShapeInstances.size(); i++)
							{
								ColladaGraphicsInstance* instance = &visualShapeInstances[i];
								GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];

								b3AlignedObjectArray<GLInstanceVertex> verts;
								verts.resize(gfxShape->m_vertices->size());

								int baseIndex = glmesh->m_vertices->size();

								for (int i = 0; i<gfxShape->m_vertices->size(); i++)
								{
									verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
									verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
									verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
									verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
									verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
									verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
									verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
									verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
									verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];

								}

								int curNumIndices = glmesh->m_indices->size();
								int additionalIndices = gfxShape->m_indices->size();
								glmesh->m_indices->resize(curNumIndices + additionalIndices);
								for (int k = 0; k<additionalIndices; k++)
								{
									glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex;
								}

								//compensate upAxisTrans and unitMeterScaling here
								btMatrix4x4 upAxisMat;
								upAxisMat.setIdentity();
//								upAxisMat.setPureRotation(upAxisTrans.getRotation());
								btMatrix4x4 unitMeterScalingMat;
								unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling));
								btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform;
								//btMatrix4x4 worldMat = instance->m_worldTransform;
								int curNumVertices = glmesh->m_vertices->size();
								int additionalVertices = verts.size();
								glmesh->m_vertices->reserve(curNumVertices + additionalVertices);

								for (int v = 0; v<verts.size(); v++)
								{
									btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]);
									pos = worldMat*pos;
									verts[v].xyzw[0] = float(pos[0]);
									verts[v].xyzw[1] = float(pos[1]);
									verts[v].xyzw[2] = float(pos[2]);
									glmesh->m_vertices->push_back(verts[v]);
								}
							}
							glmesh->m_numIndices = glmesh->m_indices->size();
							glmesh->m_numvertices = glmesh->m_vertices->size();
							//glmesh = LoadMeshFromCollada(fullPath);

							break;
						}
						default:
						{
                            b3Warning("Error: unsupported file type for Visual mesh: %s\n", fullPath);
                            btAssert(0);
						}
						}


						if (glmesh && glmesh->m_vertices && (glmesh->m_numvertices>0))
						{
						    //apply the geometry scaling
						    for (int i=0;i<glmesh->m_vertices->size();i++)
                            {
                                glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0];
                                glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1];
                                glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2];
                            }
						    
						}
						else
						{
							b3Warning("issue extracting mesh from COLLADA/STL file %s\n", fullPath);
						}

					}
					else
					{
						b3Warning("mesh geometry not found %s\n", fullPath);
					}


				}
			}


			break;
		}
		default:
		{
			b3Warning("Error: unknown visual geometry type\n");
		}
	}

	//if we have a convex, tesselate into localVertices/localIndices
	if ((glmesh==0) && convexColShape)
	{
		btShapeHull* hull = new btShapeHull(convexColShape);
		hull->buildHull(0.0);
		{
			//	int strideInBytes = 9*sizeof(float);
			int numVertices = hull->numVertices();
			int numIndices = hull->numIndices();

			
			glmesh = new GLInstanceGraphicsShape;
		//	int index = 0;
			glmesh->m_indices = new b3AlignedObjectArray<int>();
			glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();


			for (int i = 0; i < numVertices; i++)
			{
				GLInstanceVertex vtx;
				btVector3 pos = hull->getVertexPointer()[i];
				vtx.xyzw[0] = pos.x();
				vtx.xyzw[1] = pos.y();
				vtx.xyzw[2] = pos.z();
				vtx.xyzw[3] = 1.f;
				pos.normalize();
				vtx.normal[0] = pos.x();
				vtx.normal[1] = pos.y();
				vtx.normal[2] = pos.z();
				vtx.uv[0] = 0.5f;
				vtx.uv[1] = 0.5f;
				glmesh->m_vertices->push_back(vtx);
			}

			btAlignedObjectArray<int> indices;
			for (int i = 0; i < numIndices; i++)
			{
				glmesh->m_indices->push_back(hull->getIndexPointer()[i]);
			}
			
			glmesh->m_numvertices = glmesh->m_vertices->size();
			glmesh->m_numIndices = glmesh->m_indices->size();
		}
        delete hull;
		delete convexColShape;
		convexColShape = 0;

	}
	
	if (glmesh && glmesh->m_numIndices>0 && glmesh->m_numvertices >0)
	{

		int baseIndex = verticesOut.size();



		for (int i = 0; i < glmesh->m_indices->size(); i++)
		{
			indicesOut.push_back(glmesh->m_indices->at(i) + baseIndex);
		}

		for (int i = 0; i < glmesh->m_vertices->size(); i++)
		{
			GLInstanceVertex& v = glmesh->m_vertices->at(i);
			btVector3 vert(v.xyzw[0],v.xyzw[1],v.xyzw[2]);
			btVector3 vt = visualTransform*vert;
			v.xyzw[0] = vt[0];
			v.xyzw[1] = vt[1];
			v.xyzw[2] = vt[2];
			btVector3 triNormal(v.normal[0],v.normal[1],v.normal[2]);
			triNormal = visualTransform.getBasis()*triNormal;
			v.normal[0] = triNormal[0];
			v.normal[1] = triNormal[1];
			v.normal[2] = triNormal[2];
			verticesOut.push_back(v);
		}
	}
    delete glmesh;
    
}
void initBullet(void)
{


#ifdef USE_GPU_SOLVER
	g_dx11Solver = new btDX11SoftBodySolver( g_pd3dDevice, DXUTGetD3D11DeviceContext() );
	g_solver = g_dx11Solver;
#ifdef USE_GPU_COPY
	g_softBodyOutput = new btSoftBodySolverOutputDXtoDX( g_pd3dDevice, DXUTGetD3D11DeviceContext() );
#else // #ifdef USE_GPU_COPY
	g_softBodyOutput = new btSoftBodySolverOutputDXtoCPU;
#endif // #ifdef USE_GPU_COPY
#else
#ifdef USE_SIMDAWARE_SOLVER
	g_dx11SIMDSolver = new btDX11SIMDAwareSoftBodySolver( g_pd3dDevice, DXUTGetD3D11DeviceContext() );
	g_solver = g_dx11SIMDSolver;
	g_softBodyOutput = new btSoftBodySolverOutputDXtoCPU;
#ifdef USE_GPU_COPY
	g_softBodyOutput = new btSoftBodySolverOutputDXtoDX( g_pd3dDevice, DXUTGetD3D11DeviceContext() );
#else // #ifdef USE_GPU_COPY
	g_softBodyOutput = new btSoftBodySolverOutputDXtoCPU;
#endif // #ifdef USE_GPU_COPY
#else
	g_cpuSolver = new btCPUSoftBodySolver;
	g_solver = g_cpuSolver;
	g_softBodyOutput = new btSoftBodySolverOutputCPUtoCPU;
	//g_defaultSolver = new btDefaultSoftBodySolver;
	//g_solver = g_defaultSolver;
#endif
#endif

	if (g_dx11SIMDSolver)
		g_dx11SIMDSolver->setEnableUpdateBounds(true);

	if (g_dx11Solver)
		g_dx11Solver->setEnableUpdateBounds(true);

	// Initialise CPU physics device
	//m_collisionConfiguration = new btDefaultCollisionConfiguration();
	m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
	m_dispatcher = new	btCollisionDispatcher(m_collisionConfiguration);
	m_broadphase = new btDbvtBroadphase();
	btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
	m_solver = sol;

	m_dynamicsWorld = new btSoftRigidDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration, g_solver);	

	m_dynamicsWorld->setGravity(btVector3(0,-10,0));	
	btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));	
	m_collisionShapes.push_back(groundShape);
	btTransform groundTransform;
	groundTransform.setIdentity();
	groundTransform.setOrigin(btVector3(0,-50,0));

	m_dynamicsWorld->getWorldInfo().air_density			=	(btScalar)1.2;
	m_dynamicsWorld->getWorldInfo().water_density		=	0;
	m_dynamicsWorld->getWorldInfo().water_offset		=	0;
	m_dynamicsWorld->getWorldInfo().water_normal		=	btVector3(0,0,0);
	m_dynamicsWorld->getWorldInfo().m_gravity.setValue(0,-10,0);
	
#if 0
	{
		btScalar mass(0.);

		//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)
			groundShape->calculateLocalInertia(mass,localInertia);

		//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
		btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
		btRigidBody* body = new btRigidBody(rbInfo);

		//add the body to the dynamics world
		m_dynamicsWorld->addRigidBody(body);
	}
 
#endif
#if 0
	{		
		btScalar mass(0.);

		//btScalar mass(1.);

		//rigidbody is dynamic if and only if mass is non zero, otherwise static
		bool isDynamic = (mass != 0.f);
		
		btCollisionShape *capsuleShape = new btCapsuleShape(5, 10);
		capsuleShape->setMargin( 0.5 );
		
		

		my_capsule.set_collision_shape(capsuleShape);

		btVector3 localInertia(0,0,0);
		if (isDynamic)
			capsuleShape->calculateLocalInertia(mass,localInertia);

		m_collisionShapes.push_back(capsuleShape);
		btTransform capsuleTransform;
		capsuleTransform.setIdentity();
#ifdef TABLETEST
		capsuleTransform.setOrigin(btVector3(0, 10, -11));
		const btScalar pi = 3.141592654;
		capsuleTransform.setRotation(btQuaternion(0, 0, pi/2));
#else
		capsuleTransform.setOrigin(btVector3(0, 20, -10));
		
		const btScalar pi = 3.141592654;
		//capsuleTransform.setRotation(btQuaternion(0, 0, pi/2));
		capsuleTransform.setRotation(btQuaternion(0, 0, 0));
#endif
		btDefaultMotionState* myMotionState = new btDefaultMotionState(capsuleTransform);
		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,capsuleShape,localInertia);
		btRigidBody* body = new btRigidBody(rbInfo);
		body->setFriction( 0.8f );
		my_capsule.set_collision_object(body);

		m_dynamicsWorld->addRigidBody(body);
		//cap_1.collisionShape = body;
		capCollider = body;
	}
#endif


	createFlag( clothWidth, clothHeight, m_flags );

	// Create output buffer descriptions for ecah flag
	// These describe where the simulation should send output data to
	for( int flagIndex = 0; flagIndex < m_flags.size(); ++flagIndex )
	{		
		// In this case we have a DX11 output buffer with a vertex at index 0, 8, 16 and so on as well as a normal at 3, 11, 19 etc.
		// Copies will be performed GPU-side directly into the output buffer
#ifdef USE_GPU_COPY
		btDX11VertexBufferDescriptor *vertexBufferDescriptor = new btDX11VertexBufferDescriptor(DXUTGetD3D11DeviceContext(), cloths[flagIndex].pVB[0], cloths[flagIndex].g_pVB_UAV, 0, 8, 3, 8);
		cloths[flagIndex].m_vertexBufferDescriptor = vertexBufferDescriptor;
#else  // #ifdef USE_GPU_COPY
		btCPUVertexBufferDescriptor *vertexBufferDescriptor = new btCPUVertexBufferDescriptor(cloths[flagIndex].cpu_buffer, 0, 8, 3, 8);
		cloths[flagIndex].m_vertexBufferDescriptor = vertexBufferDescriptor;
#endif // #ifdef USE_GPU_COPY
	}

	g_solver->optimize( m_dynamicsWorld->getSoftBodyArray() );

}