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

void LoadMeshFromColladaAssimp(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances,btTransform& upAxisTrans, float& unitMeterScaling)
{
	upAxisTrans.setIdentity();
	unitMeterScaling=1;

	GLInstanceGraphicsShape* shape = 0;
	
	
	FILE* file = fopen(relativeFileName,"rb");
	if (file)
	{
		int size=0;
		if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET))
		{
			printf("Error: Cannot access file to determine size of %s\n", relativeFileName);
		} else
		{
			if (size)
			{
				printf("Open DAE file of %d bytes\n",size);
				
				Assimp::Importer importer;
				//importer.SetPropertyInteger(AI_CONFIG_PP_RVC_FLAGS, aiComponent_NORMALS | aiComponent_COLORS);
				importer.SetPropertyInteger(AI_CONFIG_PP_SBP_REMOVE, aiPrimitiveType_LINE | aiPrimitiveType_POINT);
			//	importer.SetPropertyInteger(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, 1);
				aiScene const* scene = importer.ReadFile(relativeFileName,
						aiProcess_JoinIdenticalVertices |
						//aiProcess_RemoveComponent |
						aiProcess_SortByPType |
						aiProcess_Triangulate);
				if (scene)
				{
					shape = &visualShapes.expand();
					shape->m_scaling[0] = 1;
					shape->m_scaling[1] = 1;
					shape->m_scaling[2] = 1;
					shape->m_scaling[3] = 1;
					int index = 0;
					shape->m_indices = new b3AlignedObjectArray<int>();
					shape->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();

					aiMatrix4x4 ident;
					addMeshParts(scene, scene->mRootNode, shape, ident);
					 shape->m_numIndices = shape->m_indices->size();
					shape->m_numvertices = shape->m_vertices->size();
					ColladaGraphicsInstance& instance = visualShapeInstances.expand();
					instance.m_shapeIndex = visualShapes.size()-1;
				}
			}
		}
		
	}
	
}

void readNodeHierarchy(TiXmlElement* node,btHashMap<btHashString,int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances,  const btMatrix4x4& parentTransMat)
{
	const char* nodeName = node->Attribute("id");
	printf("processing node %s\n", nodeName);

	
	btMatrix4x4 nodeTrans;
	nodeTrans.setIdentity();

	///todo(erwincoumans) we probably have to read the elements 'translate', 'scale', 'rotate' and 'matrix' in-order and accumulate them...
	{
		for (TiXmlElement* transElem = node->FirstChildElement("matrix");transElem;transElem=node->NextSiblingElement("matrix"))
		{
			if (transElem->GetText())
			{
				btAlignedObjectArray<float> floatArray;
				TokenFloatArray adder(floatArray);
				tokenize(transElem->GetText(),adder);
				if (floatArray.size()==16)
				{
					btMatrix4x4 t(floatArray[0],floatArray[1],floatArray[2],floatArray[3],
									floatArray[4],floatArray[5],floatArray[6],floatArray[7],
									floatArray[8],floatArray[9],floatArray[10],floatArray[11],
									floatArray[12],floatArray[13],floatArray[14],floatArray[15]);

					nodeTrans = nodeTrans*t;
				} else
				{
					printf("Error: expected 16 elements in a <matrix> element, skipping\n");
				}
			}
		}
	}

	{
		for (TiXmlElement* transElem = node->FirstChildElement("translate");transElem;transElem=node->NextSiblingElement("translate"))
		{
			if (transElem->GetText())
			{
				btVector3 pos = getVector3FromXmlText(transElem->GetText());
				//nodePos+= unitScaling*parentScaling*pos;
				btMatrix4x4 t;
				t.setPureTranslation(pos);
				nodeTrans = nodeTrans*t;

			}
		}
	}
	{
		for(TiXmlElement* scaleElem = node->FirstChildElement("scale");
				scaleElem!= NULL; scaleElem= node->NextSiblingElement("scale")) 
		{
			if (scaleElem->GetText())
			{
				btVector3 scaling = getVector3FromXmlText(scaleElem->GetText());
				btMatrix4x4 t;
				t.setPureScaling(scaling);
				nodeTrans = nodeTrans*t;
			}
		}
	}
	{
		for(TiXmlElement* rotateElem = node->FirstChildElement("rotate");
				rotateElem!= NULL; rotateElem= node->NextSiblingElement("rotate")) 
		{
			if (rotateElem->GetText())
			{
				//accumulate orientation
				btVector4 rotate = getVector4FromXmlText(rotateElem->GetText());
				btQuaternion orn(btVector3(rotate),btRadians(rotate[3]));//COLLADA DAE rotate is in degrees, convert to radians
				btMatrix4x4 t;
				t.setPureRotation(orn);
				nodeTrans = nodeTrans*t;
			}
		}
	}
	
	nodeTrans = parentTransMat*nodeTrans;
	
	for (TiXmlElement* instanceGeom = node->FirstChildElement("instance_geometry");
				instanceGeom!=0;
				instanceGeom=instanceGeom->NextSiblingElement("instance_geometry"))
	{
		const char* geomUrl = instanceGeom->Attribute("url");
		printf("node referring to geom %s\n", geomUrl);
		geomUrl++;
		int* shapeIndexPtr = name2Shape[geomUrl];
		if (shapeIndexPtr)
		{
			int index = *shapeIndexPtr;
			printf("found geom with index %d\n", *shapeIndexPtr);
			ColladaGraphicsInstance& instance = visualShapeInstances.expand();
			instance.m_shapeIndex = *shapeIndexPtr;
			instance.m_worldTransform = nodeTrans;
		} else
		{
			printf("geom not found\n");
		}
	}

//.........这里部分代码省略.........

static void	convertShape(btCollisionShape* bulletShape, btAlignedObjectArray<sce::PhysicsEffects::PfxShape>& shapes)
{
	switch (bulletShape->getShapeType())
	{
	case BOX_SHAPE_PROXYTYPE:
		{
			btBoxShape* boxshape = (btBoxShape*)bulletShape;
			sce::PhysicsEffects::PfxBox box(boxshape->getHalfExtentsWithMargin().getX(),boxshape->getHalfExtentsWithMargin().getY(),boxshape->getHalfExtentsWithMargin().getZ());
			sce::PhysicsEffects::PfxShape& shape = shapes.expand();
			shape.reset();
			shape.setBox(box);
			break;
		}

	case TRIANGLE_MESH_SHAPE_PROXYTYPE:
		{
			btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*) bulletShape;

			int numSubParts = trimesh->getMeshInterface()->getNumSubParts();
			btAssert(numSubParts>0);
			
			for (int i=0;i<numSubParts;i++)
			{
				unsigned char* vertexBase=0;
				int numVerts = 0;
				PHY_ScalarType vertexType;
				int vertexStride=0;
				unsigned char* indexBase=0;
				int indexStride=0;
				int numFaces=0;
				PHY_ScalarType indexType;

				trimesh->getMeshInterface()->getLockedVertexIndexBase(&vertexBase,numVerts,vertexType,vertexStride,&indexBase,indexStride,numFaces,indexType,i);

				sce::PhysicsEffects::PfxCreateLargeTriMeshParam param;
				btAssert(param.flag&SCE_PFX_MESH_FLAG_16BIT_INDEX);
				unsigned short int* copyIndices = new unsigned short int[numFaces*3];
				switch (indexType)
				{
				case PHY_UCHAR:	
					{
						for (int p=0;p<numFaces;p++)
						{
							copyIndices[p*3]=indexBase[p*indexStride];
							copyIndices[p*3+1]=indexBase[p*indexStride+1];
							copyIndices[p*3+2]=indexBase[p*indexStride+2];
						}
						break;
					}
					//PHY_INTEGER:
					//PHY_SHORT:
				default:
					{
						btAssert(0);
					}
				};
				
				param.verts = (float*)vertexBase;
				param.numVerts = numVerts;
				param.vertexStrideBytes = vertexStride;

				param.triangles = copyIndices;
				param.numTriangles = numFaces;
				param.triangleStrideBytes = sizeof(unsigned short int)*3;
				
				sce::PhysicsEffects::PfxLargeTriMesh* largeMesh = new sce::PhysicsEffects::PfxLargeTriMesh();

				sce::PhysicsEffects::PfxInt32 ret = pfxCreateLargeTriMesh(*largeMesh,param);
				if(ret != SCE_PFX_OK) {
					SCE_PFX_PRINTF("Can't create large mesh.\n");
				}

				sce::PhysicsEffects::PfxShape& shape = shapes.expand();
				shape.reset();
				shape.setLargeTriMesh(largeMesh);
			}

			break;
		}
	case SPHERE_SHAPE_PROXYTYPE:
		{
			btSphereShape* sphereshape = (btSphereShape*)bulletShape;
			sce::PhysicsEffects::PfxSphere sphere(sphereshape->getRadius());
			sce::PhysicsEffects::PfxShape& shape = shapes.expand();
			shape.reset();
			shape.setSphere(sphere);
			break;
		}
	case CAPSULE_SHAPE_PROXYTYPE:
		{
			btCapsuleShape* capsuleshape= (btCapsuleShape*)bulletShape;//assume btCapsuleShapeX for now
			sce::PhysicsEffects::PfxCapsule capsule(capsuleshape->getHalfHeight(),capsuleshape->getRadius());
			sce::PhysicsEffects::PfxShape& shape = shapes.expand();
			shape.reset();
			shape.setCapsule(capsule);
			break;
		}
	case CYLINDER_SHAPE_PROXYTYPE:
		{
			btCylinderShape* cylindershape= (btCylinderShape*)bulletShape;//assume btCylinderShapeX for now
//.........这里部分代码省略.........

//.........这里部分代码省略.........
						}
						if (semName=="NORMAL")
						{
							btAssert(normalSourceName.length()==0);
							normalSourceName = source_name;
							normalOffset  = offset;
							positionAndNormalInVertex = false;
						}
					}
					numIndices = primitiveCount * 3; 
				}
				btAlignedObjectArray<float> positionFloatArray;
				int posStride=1;
				TiXmlElement** sourcePtr = allSources[positionSourceName.c_str()];
				if (sourcePtr)
				{
					readFloatArray(*sourcePtr,positionFloatArray, posStride);
				}
				btAlignedObjectArray<float> normalFloatArray;
				int normalStride=1;
				sourcePtr = allSources[normalSourceName.c_str()];
				if (sourcePtr)
				{
					readFloatArray(*sourcePtr,normalFloatArray,normalStride);
				}
				btAlignedObjectArray<int> curIndices;
				curIndices.reserve(numIndices*indexStride);
				TokenIntArray adder(curIndices);
				tokenize(primitive->FirstChildElement("p")->GetText(),adder);
				assert(curIndices.size() == numIndices*indexStride);
				int indexOffset = vertexPositions.size();

				for(int index=0; index<numIndices; index++) 
				{
					int posIndex = curIndices[index*indexStride+posOffset];
					int normalIndex = curIndices[index*indexStride+normalOffset];
					vertexPositions.push_back(btVector3(extraScaling*positionFloatArray[posIndex*3+0],
						extraScaling*positionFloatArray[posIndex*3+1],
						extraScaling*positionFloatArray[posIndex*3+2]));
							
					if (normalFloatArray.size() && (normalFloatArray.size()>normalIndex))
					{
						vertexNormals.push_back(btVector3(normalFloatArray[normalIndex*3+0],
															normalFloatArray[normalIndex*3+1],
															normalFloatArray[normalIndex*3+2]));
					} else
					{
						//add a dummy normal of length zero, so it is easy to detect that it is an invalid normal
						vertexNormals.push_back(btVector3(0,0,0));
					}
				}
				int curNumIndices = indices.size();
				indices.resize(curNumIndices+numIndices);
				for(int index=0; index<numIndices; index++) 
				{
					indices[curNumIndices+index] = index+indexOffset;
				}
			}//if(primitive != NULL) 
		}//for each mesh
		
		int shapeIndex = visualShapes.size();
		GLInstanceGraphicsShape& visualShape = visualShapes.expand();
		{
			visualShape.m_vertices = new b3AlignedObjectArray<GLInstanceVertex>;
			visualShape.m_indices = new b3AlignedObjectArray<int>;
			int indexBase = 0;

			btAssert(vertexNormals.size()==vertexPositions.size());
			for (int v=0;v<vertexPositions.size();v++)
			{
				GLInstanceVertex vtx;
				vtx.xyzw[0] = vertexPositions[v].x();
				vtx.xyzw[1] = vertexPositions[v].y();
				vtx.xyzw[2] = vertexPositions[v].z();
				vtx.xyzw[3] = 1.f;
				vtx.normal[0] = vertexNormals[v].x();
				vtx.normal[1] = vertexNormals[v].y();
				vtx.normal[2] = vertexNormals[v].z();
				vtx.uv[0] = 0.5f;
				vtx.uv[1] = 0.5f;
				visualShape.m_vertices->push_back(vtx);
			}

			for (int index=0;index<indices.size();index++)
			{
				visualShape.m_indices->push_back(indices[index]+indexBase);
			}
			
			
			printf(" index_count =%dand vertexPositions.size=%d\n",indices.size(), vertexPositions.size());
			indexBase=visualShape.m_vertices->size();
			visualShape.m_numIndices = visualShape.m_indices->size();
			visualShape.m_numvertices = visualShape.m_vertices->size();
		}
		printf("geometry name=%s\n",geometryName);
		name2Shape.insert(geometryName,shapeIndex);
		

	}//for each geometry
}

PfxInt32 BulletSetupContactConstraints(PfxSetupContactConstraintsParam &param)
{
//	PfxInt32 ret = pfxCheckParamOfSetupContactConstraints(param);
	//if(ret != SCE_PFX_OK) return ret;
	
	SCE_PFX_PUSH_MARKER("pfxSetupContactConstraints");

	PfxConstraintPair *contactPairs = param.contactPairs;
	PfxUInt32 numContactPairs = param.numContactPairs;
	PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;
	PfxRigidState *offsetRigidStates = param.offsetRigidStates;
	PfxRigidBody *offsetRigidBodies = param.offsetRigidBodies;
	PfxSolverBody *offsetSolverBodies = param.offsetSolverBodies;
	manifolds.resize(0);

	for(PfxUInt32 i=0;i<numContactPairs;i++) {
		PfxConstraintPair &pair = contactPairs[i];

//		if(!sce::PhysicsEffects::pfxCheckSolver(pair)) {
	//		continue;
		//}

		PfxUInt16 iA = pfxGetObjectIdA(pair);
		PfxUInt16 iB = pfxGetObjectIdB(pair);
		PfxUInt32 iConstraint = pfxGetConstraintId(pair);

		PfxContactManifold &contact = offsetContactManifolds[iConstraint];

		btPersistentManifold& manifold = manifolds.expand();
		memset(&manifold,0xff,sizeof(btPersistentManifold));

		manifold.m_body0 = &rbs[iA];
		manifold.m_body1 = &rbs[iB];
		manifold.m_cachedPoints = contact.getNumContacts();

		if (!contact.getNumContacts())
			continue;


		SCE_PFX_ALWAYS_ASSERT(iA==contact.getRigidBodyIdA());
		SCE_PFX_ALWAYS_ASSERT(iB==contact.getRigidBodyIdB());

		PfxRigidState &stateA = offsetRigidStates[iA];
		PfxRigidBody &bodyA = offsetRigidBodies[iA];
		PfxSolverBody &solverBodyA = offsetSolverBodies[iA];

		PfxRigidState &stateB = offsetRigidStates[iB];
		PfxRigidBody &bodyB = offsetRigidBodies[iB];
		PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
	
		contact.setInternalFlag(0);
		
		PfxFloat restitution = 0.5f * (bodyA.getRestitution() + bodyB.getRestitution());
		if(contact.getDuration() > 1) restitution = 0.0f;
		
		PfxFloat friction = sqrtf(bodyA.getFriction() * bodyB.getFriction());
		
		manifold.m_cachedPoints = contact.getNumContacts();
	

		manifold.m_contactProcessingThreshold = 0.01f;//SCE_PFX_CONTACT_THRESHOLD_NORMAL;
		manifold.m_contactBreakingThreshold = 0.01f;

		for(int j=0;j<contact.getNumContacts();j++) {
			PfxContactPoint &cp = contact.getContactPoint(j);

			PfxVector3 ptA = pfxReadVector3(cp.m_localPointA);
			manifold.m_pointCache[j].m_localPointA.setValue(ptA.getX(),ptA.getY(),ptA.getZ());
			PfxVector3 ptB = pfxReadVector3(cp.m_localPointB);
			manifold.m_pointCache[j].m_localPointB.setValue(ptB.getX(),ptB.getY(),ptB.getZ());
			
			manifold.m_pointCache[j].m_normalWorldOnB.setValue(
						cp.m_constraintRow[0].m_normal[0],
						cp.m_constraintRow[0].m_normal[1],
						cp.m_constraintRow[0].m_normal[2]);
			manifold.m_pointCache[j].m_distance1 = cp.m_distance1;
			manifold.m_pointCache[j].m_combinedFriction = friction;
			manifold.m_pointCache[j].m_combinedRestitution = restitution;
			manifold.m_pointCache[j].m_appliedImpulse = cp.m_constraintRow[0].m_accumImpulse;
			manifold.m_pointCache[j].m_lateralFrictionDir1.setValue(
						cp.m_constraintRow[1].m_normal[0],
						cp.m_constraintRow[1].m_normal[1],
						cp.m_constraintRow[1].m_normal[2]);
			manifold.m_pointCache[j].m_appliedImpulseLateral1 = cp.m_constraintRow[1].m_accumImpulse;

			manifold.m_pointCache[j].m_lateralFrictionDir2.setValue(
						cp.m_constraintRow[2].m_normal[0],
						cp.m_constraintRow[2].m_normal[1],
						cp.m_constraintRow[2].m_normal[2]);
			manifold.m_pointCache[j].m_appliedImpulseLateral2 = cp.m_constraintRow[2].m_accumImpulse;
			manifold.m_pointCache[j].m_lateralFrictionInitialized = true;
			manifold.m_pointCache[j].m_lifeTime = cp.m_duration;

			btTransform trA = manifold.m_body0->getWorldTransform();
			btTransform trB = manifold.m_body1->getWorldTransform();

			manifold.m_pointCache[j].m_positionWorldOnA = trA( manifold.m_pointCache[j].m_localPointA );
			manifold.m_pointCache[j].m_positionWorldOnB = trB( manifold.m_pointCache[j].m_localPointB );


//.........这里部分代码省略.........