C++ btAlignedObjectArray::push_back方法代码示例

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

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

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

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;

*/

}

/* ----------------------------------------------------------------------- 
 | 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());
	}

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

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

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

    }

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

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

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