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"); } } for(TiXmlElement* childNode = node->FirstChildElement("node"); childNode!= NULL; childNode = childNode->NextSiblingElement("node")) { readNodeHierarchy(childNode,name2Shape,visualShapeInstances, nodeTrans); } }
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 sce::PhysicsEffects::PfxCylinder cylinder(cylindershape->getHalfExtentsWithMargin()[0],cylindershape->getRadius()); sce::PhysicsEffects::PfxShape& shape = shapes.expand(); shape.reset(); shape.setCylinder(cylinder); break; } case CONVEX_HULL_SHAPE_PROXYTYPE: { btConvexHullShape* convexHullShape = (btConvexHullShape*)bulletShape; sce::PhysicsEffects::PfxConvexMesh* convex = new sce::PhysicsEffects::PfxConvexMesh(); convex->m_numVerts = convexHullShape->getNumPoints(); convex->m_numIndices = 0;//todo for ray intersection test support for (int i=0;i<convex->m_numVerts;i++) { convex->m_verts[i].setX(convexHullShape->getPoints()[i].getX()); convex->m_verts[i].setY(convexHullShape->getPoints()[i].getY()); convex->m_verts[i].setZ(convexHullShape->getPoints()[i].getZ()); } convex->updateAABB(); sce::PhysicsEffects::PfxShape& shape = shapes.expand(); shape.reset(); shape.setConvexMesh(convex); break; } case COMPOUND_SHAPE_PROXYTYPE: { btCompoundShape* compound = (btCompoundShape*) bulletShape; for (int s=0;s<compound->getNumChildShapes();s++) { convertShape(compound->getChildShape(s),shapes); sce::PhysicsEffects::PfxMatrix3 rotMat = getVmMatrix3(compound->getChildTransform(s).getBasis()); sce::PhysicsEffects::PfxVector3 translate = getVmVector3(compound->getChildTransform(s).getOrigin()); sce::PhysicsEffects::PfxTransform3 childtransform(rotMat,translate); shapes[shapes.size()-1].setOffsetTransform(childtransform); } break; } default: { btAssert(0); } }; }
void readLibraryGeometries(TiXmlDocument& doc, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btHashMap<btHashString,int>& name2Shape, float extraScaling) { btHashMap<btHashString,TiXmlElement* > allSources; btHashMap<btHashString,VertexSource> vertexSources; for(TiXmlElement* geometry = doc.RootElement()->FirstChildElement("library_geometries")->FirstChildElement("geometry"); geometry != NULL; geometry = geometry->NextSiblingElement("geometry")) { btAlignedObjectArray<btVector3> vertexPositions; btAlignedObjectArray<btVector3> vertexNormals; btAlignedObjectArray<int> indices; const char* geometryName = geometry->Attribute("id"); for (TiXmlElement* mesh = geometry->FirstChildElement("mesh");(mesh != NULL); mesh = mesh->NextSiblingElement("mesh")) { TiXmlElement* vertices2 = mesh->FirstChildElement("vertices"); for (TiXmlElement* source = mesh->FirstChildElement("source");source != NULL;source = source->NextSiblingElement("source")) { const char* srcId= source->Attribute("id"); // printf("source id=%s\n",srcId); allSources.insert(srcId,source); } const char* vertexId = vertices2->Attribute("id"); //printf("vertices id=%s\n",vertexId); VertexSource vs; for(TiXmlElement* input = vertices2->FirstChildElement("input");input != NULL;input = input->NextSiblingElement("input")) { const char* sem = input->Attribute("semantic"); std::string semName(sem); // printf("sem=%s\n",sem); const char* src = input->Attribute("source"); // printf("src=%s\n",src); const char* srcIdRef = input->Attribute("source"); std::string source_name; source_name = std::string(srcIdRef); source_name = source_name.erase(0, 1); if (semName=="POSITION") { vs.m_positionArrayId = source_name; } if (semName=="NORMAL") { vs.m_normalArrayId = source_name; } } vertexSources.insert(vertexId,vs); for (TiXmlElement* primitive = mesh->FirstChildElement("triangles"); primitive; primitive = primitive->NextSiblingElement("triangles")) { std::string positionSourceName; std::string normalSourceName; int primitiveCount; primitive->QueryIntAttribute("count", &primitiveCount); bool positionAndNormalInVertex=false; int indexStride=1; int posOffset = 0; int normalOffset = 0; int numIndices = 0; { for (TiXmlElement* input = primitive->FirstChildElement("input");input != NULL;input = input->NextSiblingElement("input")) { const char* sem = input->Attribute("semantic"); std::string semName(sem); int offset = atoi(input->Attribute("offset")); if ((offset+1)>indexStride) indexStride=offset+1; //printf("sem=%s\n",sem); const char* src = input->Attribute("source"); //printf("src=%s\n",src); const char* srcIdRef = input->Attribute("source"); std::string source_name; source_name = std::string(srcIdRef); source_name = source_name.erase(0, 1); if (semName=="VERTEX") { //now we have POSITION and possibly NORMAL too, using same index array (<p>) VertexSource* vs = vertexSources[source_name.c_str()]; if (vs->m_positionArrayId.length()) { positionSourceName = vs->m_positionArrayId; posOffset = offset; } if (vs->m_normalArrayId.length()) { normalSourceName = vs->m_normalArrayId; normalOffset = offset; positionAndNormalInVertex = true; } } 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 ¶m) { // 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 ); //btVector3 m_localPointA; //btVector3 m_localPointB; //btVector3 m_positionWorldOnB; //m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity //btVector3 m_positionWorldOnA; /* pfxSetupContactConstraint( cp.m_constraintRow[0], cp.m_constraintRow[1], cp.m_constraintRow[2], cp.m_distance, restitution, friction, pfxReadVector3(cp.m_constraintRow[0].m_normal), pfxReadVector3(cp.m_localPointA), pfxReadVector3(cp.m_localPointB), stateA, stateB, solverBodyA, solverBodyB, param.separateBias, param.timeStep ); */ } contact.setCompositeFriction(friction); } SCE_PFX_POP_MARKER(); return SCE_PFX_OK; }