diff --git a/adf_loader/version_1_0/adf_loader_1_0.cpp b/adf_loader/version_1_0/adf_loader_1_0.cpp index 700c7647a..80343bf82 100644 --- a/adf_loader/version_1_0/adf_loader_1_0.cpp +++ b/adf_loader/version_1_0/adf_loader_1_0.cpp @@ -211,6 +211,7 @@ bool ADFUtils::getVisualAttribsFromNode(YAML::Node *a_node, afVisualAttributes * YAML::Node& node = *a_node; YAML::Node meshNode = node["mesh"]; + YAML::Node meshRemoveDuplicatesNode = node["mesh remove duplicates"]; YAML::Node shapeNode = node["shape"]; YAML::Node compoundShapeNode = node["compound shape"]; YAML::Node geometryNode = node["geometry"]; @@ -258,6 +259,13 @@ bool ADFUtils::getVisualAttribsFromNode(YAML::Node *a_node, afVisualAttributes * attribs->m_meshFilepath = localPath / meshNode.as(); if (!attribs->m_meshFilepath.c_str().empty()){ attribs->m_geometryType = afGeometryType::MESH; + + if (meshRemoveDuplicatesNode.IsDefined()){ + if (meshRemoveDuplicatesNode.as() == true){ + attribs->m_meshRemoveDuplicates = afStatusFlag::TRUE;} + else{ + attribs->m_meshRemoveDuplicates = afStatusFlag::FALSE;} + } } else{ valid = false; diff --git a/ambf_framework/afAttributes.h b/ambf_framework/afAttributes.h index 66e387198..1acdce0ba 100644 --- a/ambf_framework/afAttributes.h +++ b/ambf_framework/afAttributes.h @@ -53,6 +53,12 @@ typedef unsigned int uint; namespace ambf { +enum class afStatusFlag{ + UNDEFINED, + TRUE, + FALSE, +}; + /// /// \brief The afKinematicAttributes struct /// @@ -362,9 +368,11 @@ struct afColorAttributes{ struct afVisualAttributes{ afVisualAttributes(){ m_visible = true; + m_meshRemoveDuplicates = afStatusFlag::UNDEFINED; } afPath m_meshFilepath; + afStatusFlag m_meshRemoveDuplicates; afGeometryType m_geometryType; vector m_primitiveShapes; afColorAttributes m_colorAttribs; diff --git a/ambf_framework/afFramework.cpp b/ambf_framework/afFramework.cpp index ff668a755..a07d64f3f 100644 --- a/ambf_framework/afFramework.cpp +++ b/ambf_framework/afFramework.cpp @@ -231,67 +231,68 @@ btCollisionShape *afShapeUtils::createCollisionShape(const afPrimitiveShapeAttri return collisionShape; } -btCollisionShape* afShapeUtils::createCollisionShape(const cMesh *a_collisionMesh, - double a_margin, - afCollisionMeshShapeType a_meshType) -{ - // create the collision shape - btCollisionShape* collisionShape; - switch (a_meshType) { - case afCollisionMeshShapeType::CONCAVE_MESH:{ - // bullet mesh - btTriangleMesh* bulletMesh = new btTriangleMesh(); +/// +/// \brief cMeshTObtTriangleMesh +/// \param a_mesh +/// \return +/// +btTriangleMesh* cMeshTObtTriangleMesh(const cMesh* a_mesh){ - // read number of triangles of the object - unsigned int numTriangles = a_collisionMesh->m_triangles->getNumElements(); + // bullet mesh + btTriangleMesh* bulletMesh = new btTriangleMesh(); - // add all triangles to Bullet model - for (unsigned int i=0; im_triangles->getVertexIndex0(i); - unsigned int vertexIndex1 = a_collisionMesh->m_triangles->getVertexIndex1(i); - unsigned int vertexIndex2 = a_collisionMesh->m_triangles->getVertexIndex2(i); + // read number of indices of the object + unsigned int numIndices = a_mesh->m_triangles->getNumElements(); - cVector3d vertex0 = a_collisionMesh->m_vertices->getLocalPos(vertexIndex0); - cVector3d vertex1 = a_collisionMesh->m_vertices->getLocalPos(vertexIndex1); - cVector3d vertex2 = a_collisionMesh->m_vertices->getLocalPos(vertexIndex2); + // add all indices to Bullet model + for (unsigned int i=0; im_triangles->getVertexIndex0(i); + unsigned int vertexIndex1 = a_mesh->m_triangles->getVertexIndex1(i); + unsigned int vertexIndex2 = a_mesh->m_triangles->getVertexIndex2(i); + bulletMesh->addTriangleIndices(vertexIndex0, vertexIndex1, vertexIndex2); + } - bulletMesh->addTriangle(btVector3(vertex0(0), vertex0(1), vertex0(2)), - btVector3(vertex1(0), vertex1(1), vertex1(2)), - btVector3(vertex2(0), vertex2(1), vertex2(2))); - } + unsigned int numVertices = a_mesh->m_vertices->getNumElements(); - // create mesh collision model - collisionShape = new btGImpactMeshShape(bulletMesh); - break; + for (unsigned int i=0; im_vertices->getLocalPos(i); + bulletMesh->findOrAddVertex(btVector3(vertex(0), vertex(1), vertex(2)), false); } - case afCollisionMeshShapeType::CONVEX_MESH:{ - - // bullet mesh - btTriangleMesh* bulletMesh = new btTriangleMesh(); - // read number of triangles of the object - unsigned int numTriangles = a_collisionMesh->m_triangles->getNumElements(); + return bulletMesh; +} - // add all triangles to Bullet model - for (unsigned int i=0; im_triangles->getVertexIndex0(i); - unsigned int vertexIndex1 = a_collisionMesh->m_triangles->getVertexIndex1(i); - unsigned int vertexIndex2 = a_collisionMesh->m_triangles->getVertexIndex2(i); - cVector3d vertex0 = a_collisionMesh->m_vertices->getLocalPos(vertexIndex0); - cVector3d vertex1 = a_collisionMesh->m_vertices->getLocalPos(vertexIndex1); - cVector3d vertex2 = a_collisionMesh->m_vertices->getLocalPos(vertexIndex2); +/// +/// \brief afShapeUtils::createCollisionShape +/// \param a_collisionMesh +/// \param a_margin +/// \param a_meshType +/// \return +/// +btCollisionShape* afShapeUtils::createCollisionShape(cMesh *a_collisionMesh, + double a_margin, + afCollisionMeshShapeType a_meshType) +{ + // create the collision shape + btCollisionShape* collisionShape; - bulletMesh->addTriangle(btVector3(vertex0(0), vertex0(1), vertex0(2)), - btVector3(vertex1(0), vertex1(1), vertex1(2)), - btVector3(vertex2(0), vertex2(1), vertex2(2))); - } + switch (a_meshType) { + case afCollisionMeshShapeType::CONCAVE_MESH: + case afCollisionMeshShapeType::CONVEX_MESH:{ + // bullet mesh + btTriangleMesh* bulletMesh = cMeshTObtTriangleMesh(a_collisionMesh); + if (a_meshType == afCollisionMeshShapeType::CONCAVE_MESH){ // create mesh collision model - collisionShape = new btConvexTriangleMeshShape(bulletMesh); + collisionShape = new btGImpactMeshShape(bulletMesh); + } + else{ + collisionShape = new btConvexTriangleMeshShape(bulletMesh); + } break; } case afCollisionMeshShapeType::CONVEX_HULL:{ @@ -312,7 +313,15 @@ btCollisionShape* afShapeUtils::createCollisionShape(const cMesh *a_collisionMes } -btCompoundShape *afShapeUtils::createCollisionShape(const cMultiMesh *a_collisionMultiMesh, +/// +/// \brief afShapeUtils::createCollisionShape +/// \param a_collisionMultiMesh +/// \param a_margin +/// \param m_inertialOffset +/// \param a_meshType +/// \return +/// +btCompoundShape *afShapeUtils::createCollisionShape(cMultiMesh *a_collisionMultiMesh, double a_margin, afTransform m_inertialOffset, afCollisionMeshShapeType a_meshType){ @@ -323,76 +332,28 @@ btCompoundShape *afShapeUtils::createCollisionShape(const cMultiMesh *a_collisio inverseInertialOffsetTransform << m_inertialOffset.getInverse(); switch (a_meshType) { - case afCollisionMeshShapeType::CONCAVE_MESH:{ - // create collision detector for each mesh - std::vector::iterator it; - for (it = a_collisionMultiMesh->m_meshes->begin(); it != a_collisionMultiMesh->m_meshes->end(); ++it) - { - cMesh* mesh = (*it); - - // bullet mesh - btTriangleMesh* bulletMesh = new btTriangleMesh(); - - // read number of triangles of the object - unsigned int numTriangles = mesh->m_triangles->getNumElements(); - - // add all triangles to Bullet model - for (unsigned int i=0; im_triangles->getVertexIndex0(i); - unsigned int vertexIndex1 = mesh->m_triangles->getVertexIndex1(i); - unsigned int vertexIndex2 = mesh->m_triangles->getVertexIndex2(i); - - cVector3d vertex0 = mesh->m_vertices->getLocalPos(vertexIndex0); - cVector3d vertex1 = mesh->m_vertices->getLocalPos(vertexIndex1); - cVector3d vertex2 = mesh->m_vertices->getLocalPos(vertexIndex2); - - bulletMesh->addTriangle(btVector3(vertex0(0), vertex0(1), vertex0(2)), - btVector3(vertex1(0), vertex1(1), vertex1(2)), - btVector3(vertex2(0), vertex2(1), vertex2(2))); - } - - // create mesh collision model - collisionShape = new btGImpactMeshShape(bulletMesh); - collisionShape->setMargin(a_margin); - ((btGImpactMeshShape*) collisionShape)->updateBound(); - compoundCollisionShape->addChildShape(inverseInertialOffsetTransform, collisionShape); - } - break; - } + case afCollisionMeshShapeType::CONCAVE_MESH: case afCollisionMeshShapeType::CONVEX_MESH:{ // create collision detector for each mesh std::vector::iterator it; for (it = a_collisionMultiMesh->m_meshes->begin(); it != a_collisionMultiMesh->m_meshes->end(); ++it) { cMesh* mesh = (*it); + btTriangleMesh* bulletMesh = cMeshTObtTriangleMesh(mesh); - // bullet mesh - btTriangleMesh* bulletMesh = new btTriangleMesh(); - - // read number of triangles of the object - unsigned int numTriangles = mesh->m_triangles->getNumElements(); - - // add all triangles to Bullet model - for (unsigned int i=0; im_triangles->getVertexIndex0(i); - unsigned int vertexIndex1 = mesh->m_triangles->getVertexIndex1(i); - unsigned int vertexIndex2 = mesh->m_triangles->getVertexIndex2(i); - - cVector3d vertex0 = mesh->m_vertices->getLocalPos(vertexIndex0); - cVector3d vertex1 = mesh->m_vertices->getLocalPos(vertexIndex1); - cVector3d vertex2 = mesh->m_vertices->getLocalPos(vertexIndex2); - - bulletMesh->addTriangle(btVector3(vertex0(0), vertex0(1), vertex0(2)), - btVector3(vertex1(0), vertex1(1), vertex1(2)), - btVector3(vertex2(0), vertex2(1), vertex2(2))); + if (a_meshType == afCollisionMeshShapeType::CONCAVE_MESH){ + // create mesh collision model + collisionShape = new btGImpactMeshShape(bulletMesh); + collisionShape->setMargin(a_margin); + ((btGImpactMeshShape*) collisionShape)->updateBound(); + compoundCollisionShape->addChildShape(inverseInertialOffsetTransform, collisionShape); + } + else{ + // create mesh collision model + collisionShape = new btConvexTriangleMeshShape(bulletMesh); + collisionShape->setMargin(a_margin); + compoundCollisionShape->addChildShape(inverseInertialOffsetTransform, collisionShape); } - - // create mesh collision model - collisionShape = new btConvexTriangleMeshShape(bulletMesh); - collisionShape->setMargin(a_margin); - compoundCollisionShape->addChildShape(inverseInertialOffsetTransform, collisionShape); } break; } @@ -420,16 +381,10 @@ btCompoundShape *afShapeUtils::createCollisionShape(const cMultiMesh *a_collisio for (it = a_collisionMultiMesh->m_meshes->begin(); it != a_collisionMultiMesh->m_meshes->end(); ++it) { cMesh* mesh = (*it); - std::vector filteredVtx; - std::vector filterTri; - std::vector vtxTree; - afMeshCleanup::computeUniqueVerticesandTrianglesSequential(mesh, &filteredVtx, &filterTri, &vtxTree, nullptr, false); - - int numVtx = filteredVtx.size() / 3; - for (uint i = 0 ; i < numVtx ; i++){ + for (uint i = 0 ; i < mesh->m_vertices->getNumElements() ; i++){ collisionShape = new btSphereShape(a_margin); btTransform lT; - btVector3 btPos(filteredVtx[i * 3 + 0], filteredVtx[i * 3 + 1], filteredVtx[i * 3 + 2]); + btVector3 btPos = to_btVector(mesh->m_vertices->getLocalPos(i)); lT.setOrigin(btPos); compoundCollisionShape->addChildShape(inverseInertialOffsetTransform * lT, collisionShape); } @@ -506,9 +461,18 @@ cMaterial afMaterialUtils::createFromAttribs(afColorAttributes *a_color) bool afVisualUtils::createFromAttribs(afVisualAttributes *attribs, cMultiMesh *mesh, string obj_name){ if (attribs->m_geometryType == afGeometryType::MESH){ if (mesh->loadFromFile(attribs->m_meshFilepath.c_str()) ){ - // mesh->scale(m_scale); + if (attribs->m_meshRemoveDuplicates == afStatusFlag::TRUE){ + mesh->removeDuplicateVertices(); + } + else if (attribs->m_meshRemoveDuplicates == afStatusFlag::UNDEFINED){ + for (auto m : *(mesh->m_meshes)){ + if (m->getNumVertices() > 10000){ + double wd = 0.; + m->removeDuplicateVertices(wd); + } + } + } mesh->setUseDisplayList(true); - // m_visualMesh->markForUpdate(false); } else{ cerr << "WARNING! OBJECT " @@ -2484,6 +2448,7 @@ bool afRigidBody::createFromAttribs(afRigidBodyAttributes *a_attribs) if (m_collisionGeometryType == afGeometryType::MESH){ if (m_collisionMesh->loadFromFile(m_collisionMeshFilePath.c_str()) ){ + m_collisionMesh->removeDuplicateVertices(); m_collisionMesh->scale(m_scale); // Override the inertial offset if it is required by attribs if (a_attribs->m_inertialAttribs.m_estimateInertialOffset){ @@ -2866,486 +2831,6 @@ void afMeshCleanup::updateMaxs(cVector3d &vMax, cVector3d &v){ } -/// -/// \brief afMeshCleanup::clearArrays -/// \param vtxChkBlock -/// \param vtxIdxBlock -/// \param blockSize -/// -void afMeshCleanup::clearArrays(bool *vtxChkBlock, int *vtxIdxBlock, int blockSize){ - int s = blockSize*blockSize*blockSize; - memset(vtxChkBlock, false, s*sizeof(bool)); // Initialize all the vtx check blocks to 0 - memset(vtxIdxBlock, -1, s*sizeof(int)); // Initialize all the vtx index blocks to -1 -} - - -/// -/// \brief afMeshCleanup::computeUniqueVerticesandTriangles -/// \param mesh -/// \param outputVertices -/// \param outputTriangles -/// \param a_vertexTrees -/// \param outputLines -/// \param print_debug_info -/// -void afMeshCleanup::computeUniqueVerticesandTriangles(const cMesh *mesh, std::vector *outputVertices, std::vector *outputTriangles, std::vector* a_vertexTrees, std::vector > *outputLines, bool print_debug_info) -{ - // read number of triangles of the object - int numTriangles = mesh->m_triangles->getNumElements(); - int numVertices = mesh->m_vertices->getNumElements(); - - if (print_debug_info){ - printf("# Triangles %d, # Vertices %d \n", numTriangles, numVertices); - } - - // The max number of vertices to check per block - int vtxBlockSize = 1000; - // Number of default blocks - int numBlocks = 1; - //Define bound for lowest value of vertices - cVector3d vMin(9999,9999,9999); - //Define bound for max value of vertices - cVector3d vMax(-9999,-9999,-9999); - // Length of the bounds (max - min) for each x,y,z - cVector3d vBounds; - - // Update the min and max value (x,y,z) of vertices to get bounds - for (int x = 0 ; x < numVertices ; x++){ - cVector3d v = mesh->m_vertices->getLocalPos(x); - updateMins(vMin, v); // Iterative search to get the min distance - updateMaxs(vMax, v); // Iterative search to get the max distance - } - // Update magnitude of bound - vBounds = vMax - vMin; - if (print_debug_info){ - printf("***************************************\n"); - printf("Vmin = [%f, %f, %f] \n", vMin.x(), vMin.y(), vMin.z()); - printf("Vmax = [%f, %f, %f] \n", vMax.x(), vMax.y(), vMax.z()); - printf("VBounds = [%f, %f, %f] \n", vBounds.x(), vBounds.y(), vBounds.z()); - printf("***************************************\n"); - } - // Place holder for count of repeat and duplicate vertices - int uniqueVtxCount = 0; - int duplicateVtxCount = 0; - - // If number of vertices is greater the vertices per block, increase no of blocks - // This is to prevent memory exhaustion - if (numVertices > vtxBlockSize){ - numBlocks = std::ceil((float)numVertices / (float)vtxBlockSize); - } - - if (print_debug_info){ - printf("Using %d blocks \n", numBlocks); - } - // Copy over the vertices to process without altering the original data - auto vtxArrCopy = mesh->m_vertices->copy(); - // This data-structure is to store the unaltered indices in the first row vertices referring to their - // original copy in the second row. The third row contains the indexes to the vertices after - // the unique vertices have been placed in the outputVertices array - // . E.g. if a vertex at idx 5 was a repeat of vtx at idx 3, orderedVtxList[5][0] = 5 ; orderedVtxList[5][1] = 3; - // and if the vertex was added to the array of unique vertices at Idx 2 then orderedVtxList[5][2] = 2; - int* orderedVtxList = new int [numVertices*3]; - memset(orderedVtxList, -1, numVertices*3*sizeof(int)); - -#define ordVtxIdx(a, b) (numVertices * b + a) - - // This forms a 3D block with all value initiazlied to false - // If we visit a specific 3D idx, it's set to true to know that we have been there - bool* vtxChkBlock = new bool[vtxBlockSize*vtxBlockSize*vtxBlockSize]; - - int vtxBlockSizeSquare = vtxBlockSize * vtxBlockSize; - -#define vtxChkIdx(a, b, c) (vtxBlockSizeSquare * c + vtxBlockSize * b + a) - // This forms a 3D block with all values init to -1 - // What ever 3D idx we visited we set the corresponding corrected idx value in this 3D block - int* vtxIdxBlock = new int[vtxBlockSize*vtxBlockSize*vtxBlockSize]; - // To reduce computational cost, if we have already checked a vertex, we can mark it - bool* vtxAlreadyChkd = new bool[numVertices]; - // Initialize all the vertex check index to false - memset(vtxAlreadyChkd, false, numVertices*sizeof(bool)); - // Upper a lower bound for block in x direction - int xblockLowerBound; - int xblockUpperBound; - // Upper a lower bound for block in y direction - int yblockLowerBound; - int yblockUpperBound; - // Upper a lower bound for block in z direction - int zblockLowerBound; - int zblockUpperBound; - - int vxKey; // X key to look up in the block - int vyKey; // Y key to look up in the block - int vzKey; // Z ket to look up in the block - - double xRes; // X Resolution - double yRes; // Y Resolution - double zRes; // X Resolution - - cVector3d vPos; // The position of a vertex - if(vBounds.x() == 0){ - xRes = 0; // If planar in x direction, set x res to 0 - } - else{ - - xRes = (double) (numVertices - 1) / vBounds.x(); - } - if(vBounds.y() == 0){ - yRes = 0; // If planar in y direction, set x res to 0 - } - else{ - - yRes = (double) (numVertices - 1) / vBounds.y(); - } - if(vBounds.z() == 0){ - zRes = 0; // If planar in z direction, set x res to 0 - } - else{ - zRes = (double) (numVertices - 1) / vBounds.z(); - } - - bool first_print = false; - - // Begin the loop to create a hash grid and check for unique vertices - for (int xblockNum = 0 ; xblockNum < numBlocks ; xblockNum ++){ - xblockLowerBound = xblockNum * vtxBlockSize; - xblockUpperBound = xblockLowerBound + vtxBlockSize; - // first_print = true; - for (int yblockNum = 0 ; yblockNum < numBlocks ; yblockNum ++){ - yblockLowerBound = yblockNum * vtxBlockSize; - yblockUpperBound = yblockLowerBound + vtxBlockSize; - first_print = true; - for (int zblockNum = 0 ; zblockNum < numBlocks ; zblockNum ++){ - zblockLowerBound = zblockNum * vtxBlockSize; - zblockUpperBound = zblockLowerBound + vtxBlockSize; - if (print_debug_info) { - if (first_print){ - printf("Block Num [%d, %d, %d] \n", xblockNum, yblockNum, zblockNum); - first_print = false; - } - } - // Clear the 3D idx and chk arrays to be reused for the new block - clearArrays(&vtxChkBlock[vtxChkIdx(0,0,0)], &vtxIdxBlock[vtxChkIdx(0,0,0)], vtxBlockSize); - for(int idx = 0; idx < numVertices ; idx++){ - if (!vtxAlreadyChkd[idx]){ - vPos = vtxArrCopy->getLocalPos(idx); - // Generate keys to parse the 3D idx and chk block - vxKey = xRes * (vPos.x() - vMin.x()); - vyKey = yRes * (vPos.y() - vMin.y()); - vzKey = zRes * (vPos.z() - vMin.z()); - // Check if the generated keys are in the bounds of the current block - if (vxKey >= xblockLowerBound){ - if (vyKey >= yblockLowerBound){ - if (vzKey >= zblockLowerBound){ - if (vxKey <= xblockUpperBound){ - if (vyKey <= yblockUpperBound){ - if (vzKey <= zblockUpperBound){ - // If the key lies inside the block, offset the value to the block bounds - vxKey -= xblockLowerBound; vyKey -= yblockLowerBound; vzKey -= zblockLowerBound; - // Mark that we already checked this vertex, so we don't have to check it again - vtxAlreadyChkd[idx] = true; - // Set the vertexIdx Pair value - orderedVtxList[ordVtxIdx(idx, 0)] = idx; - // Check if the key is already set in the chk block - if (vtxChkBlock[vtxChkIdx(vxKey,vyKey,vzKey)] == false){ - // Unique vertex, so mark it as such in the corresponding blocks - vtxChkBlock[vtxChkIdx(vxKey,vyKey,vzKey)] = true; - // Set the idx block to the original idx - vtxIdxBlock[vtxChkIdx(vxKey,vyKey,vzKey)] = idx; - orderedVtxList[ordVtxIdx(idx, 1)] = idx; - uniqueVtxCount ++; - } - else{ - // This is not a unique vertex, so get the original idx - // and set it in the corresponding blocks - orderedVtxList[ordVtxIdx(idx, 1)] = vtxIdxBlock[vtxChkIdx(vxKey,vyKey,vzKey)]; - duplicateVtxCount++; - } - } - } - } - } - } - } - } - } - } - } - } - - //Resize once to save on iterative push/pop time - outputVertices->resize(uniqueVtxCount*3); - outputTriangles->resize(numTriangles*3); - a_vertexTrees->resize(uniqueVtxCount); - - // In this loop we append the index of the newly resized array containing - // the unique vertices to the index of the original array of duplicated vertices. - // This is an example of the orderedVtxList might look like for usual run - // After above steps - // orderedVtxList[:][0] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11} - // orderedVtxList[:][1] = { 0, 1, 2, 1, 4, 2, 1, 7, 4, 7, 10, 4} - // orderedVtxList[:][1] = {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1} - // And we want: - // orderedVtxList[:][0] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11} - // orderedVtxList[:][1] = { 0, 1, 2, 1, 4, 2, 1, 7, 4, 7, 10, 4} - // orderedVtxList[:][1] = { 0, 1, 2, 1, 3, 2, 1, 4, 3, 4, 5, 3} - int vtxCounted = 0; - for (int aIdx = 0 ; aIdx < numVertices ; aIdx++){ - if (orderedVtxList[ordVtxIdx(aIdx,1)] == orderedVtxList[ordVtxIdx(aIdx,0)] && orderedVtxList[ordVtxIdx(aIdx,2)] == -1){ // A unique vertex - vPos = mesh->m_vertices->getLocalPos(aIdx); - (*outputVertices)[3*vtxCounted + 0] = vPos.x(); - (*outputVertices)[3*vtxCounted + 1] = vPos.y(); - (*outputVertices)[3*vtxCounted + 2] = vPos.z(); - - orderedVtxList[ordVtxIdx(aIdx,2)] = vtxCounted; // Record the index in queue where the unique vertex is added - (*a_vertexTrees)[vtxCounted].vertexIdx.push_back(aIdx); - vtxCounted++; // Increase the queue idx by 1 - } - else if(orderedVtxList[ordVtxIdx(aIdx,1)] < orderedVtxList[ordVtxIdx(aIdx,0)]){ // Not a unique vertex - int bIdx = orderedVtxList[ordVtxIdx(aIdx,1)]; - int cIdx = orderedVtxList[ordVtxIdx(bIdx,2)]; - if (orderedVtxList[ordVtxIdx(bIdx,1)] != orderedVtxList[ordVtxIdx(bIdx,0)] || cIdx == -1){ - // This shouldn't happend. This means that we haven't assigned the third row - // and row 1 is greater than row 2 - throw "Algorithm Failed for (b[i] < a[i]), a[b[i]] != b[b[i]] : %d and c[b[i]] != -1"; - } - orderedVtxList[ordVtxIdx(aIdx,2)] = cIdx; - (*a_vertexTrees)[cIdx].vertexIdx.push_back(aIdx); - } - else if(orderedVtxList[ordVtxIdx(aIdx,1)] > orderedVtxList[ordVtxIdx(aIdx,0)]){ - int bIdx = orderedVtxList[ordVtxIdx(aIdx,1)]; - if (orderedVtxList[ordVtxIdx(bIdx,1)] != orderedVtxList[ordVtxIdx(bIdx,0)]){ - throw "Algorithm Failed for (b[i] > a[i]), a[b[i]] != b[b[i]] : %d"; - } - if (orderedVtxList[ordVtxIdx(bIdx,2)] == -1){ - vPos = mesh->m_vertices->getLocalPos(bIdx); - vtxCounted++; - (*outputVertices)[3*vtxCounted + 0] = vPos.x(); - (*outputVertices)[3*vtxCounted + 1] = vPos.y(); - (*outputVertices)[3*vtxCounted + 2] = vPos.z(); - orderedVtxList[ordVtxIdx(bIdx,2)] = vtxCounted; - } - orderedVtxList[ordVtxIdx(aIdx,2)] = orderedVtxList[ordVtxIdx(bIdx,2)]; - } - } - - // This last loop iterates over the triangle idxes and assigns the re-idxd vertices from the - // third row of orderedVtxList - for (int i = 0 ; i < mesh->m_triangles->m_indices.size() ; i++){ - int triIdx = mesh->m_triangles->m_indices[i]; - if ( triIdx >= numVertices){ - std::cerr << "ERROR ! Triangle Vtx Index " << triIdx << " >= # Vertices " << numVertices << std::endl; - } - else{ - (*outputTriangles)[i] = orderedVtxList[ordVtxIdx(triIdx,2)]; - } - } - - // This last loop iterates over the lines and assigns the re-idxd vertices to the - // lines - if (outputLines){ - for (int i = 0 ; i < outputLines->size() ; i++){ - std::vector originalLine = (*outputLines)[i]; - std::vector reIndexedLine = originalLine; - for (int vtx = 0 ; vtx < originalLine.size() ; vtx++){ - int original_idx = originalLine[vtx]; - reIndexedLine[vtx] = orderedVtxList[ordVtxIdx(original_idx,2)]; - } - (*outputLines)[i].clear(); - (*outputLines)[i] = reIndexedLine; - } - } - - if (print_debug_info){ - printf("*** PARALLEL COMPUTE UNIQUE VERTICES AND TRIANGLE INDICES ***\n"); - - for (int i = 0 ; i < uniqueVtxCount; i ++){ - printf("Vertex %d = [%f, %f, %f] \n", i, (*outputVertices)[3*i + 0], (*outputVertices)[3*i + 1], (*outputVertices)[3*i + 2]); - } - - for (int i = 0 ; i < uniqueVtxCount; i ++){ - printf("%d) Children = [", i ); - for (int j = 0 ; j < (*a_vertexTrees)[i].vertexIdx.size(); j++){ - printf(" %d", (*a_vertexTrees)[i].vertexIdx[j]); - } - printf(" ]\n"); - } - - for (int i = 0 ; i < numTriangles; i ++){ - printf("Triangle %d = [%d, %d, %d] \n", i, (*outputTriangles)[3*i], (*outputTriangles)[3*i+1], (*outputTriangles)[3*i+2]); - } - - for (int i = 0 ; i < numVertices ; i++){ - printf("%d) v[0] = %d \t v[1] = %d \t v[2] = %d \n", i, orderedVtxList[ordVtxIdx(i,0)], orderedVtxList[ordVtxIdx(i,1)], orderedVtxList[ordVtxIdx(i,2)]); - } - } - - printf("Unique Vertices Found = %d, Duplicate Vertices Found = %d\n", uniqueVtxCount, duplicateVtxCount); - delete[] orderedVtxList; - delete[] vtxIdxBlock; - delete[] vtxChkBlock; - delete[] vtxAlreadyChkd; -} - - -/// -/// \brief afMeshCleanup::computeUniqueVerticesandTrianglesSequential -/// \param mesh -/// \param outputVertices -/// \param outputTriangles -/// \param a_vertexTrees -/// \param outputLines -/// \param print_debug_info -/// -void afMeshCleanup::computeUniqueVerticesandTrianglesSequential(const cMesh *mesh, std::vector *outputVertices, std::vector *outputTriangles, std::vector* a_vertexTrees , std::vector > *outputLines, bool print_debug_info) -{ - // read number of triangles of the object - int numTriangles = mesh->m_triangles->getNumElements(); - int numVertices = mesh->m_vertices->getNumElements(); - - // Place holder for count of repeat and duplicate vertices - int uniqueVtxCount = 0; - int duplicateVtxCount = 0; - - if (print_debug_info){ - printf("# Triangles %d, # Vertices %d \n", numTriangles, numVertices); - } - - int* orderedVtxList = new int[numVertices*3]; - -#define ordVtxIdx(a, b) (numVertices * b + a) - - orderedVtxList[ordVtxIdx(0,0)] = 0; - orderedVtxList[ordVtxIdx(0,1)] = 0; - orderedVtxList[ordVtxIdx(0,2)] = -1; - - cVector3d v1Pos, v2Pos; - for (int i = 0 ; i < numVertices ; i++){ - orderedVtxList[ordVtxIdx(i,0)] = i; - orderedVtxList[ordVtxIdx(i,1)] = -1; - orderedVtxList[ordVtxIdx(i,2)] = -1; - } - - for (int aIdx = 0 ; aIdx < numVertices - 1 ; aIdx++){ - if (orderedVtxList[ordVtxIdx(aIdx,1)] == -1){ - orderedVtxList[ordVtxIdx(aIdx,1)] = aIdx; - uniqueVtxCount++; - } - else{ - duplicateVtxCount++; - } - for (int bIdx = aIdx + 1 ; bIdx < numVertices ; bIdx++){ - v1Pos = mesh->m_vertices->getLocalPos(aIdx); - v2Pos = mesh->m_vertices->getLocalPos(bIdx); - - if (orderedVtxList[ordVtxIdx(bIdx,1)] == -1){ - if ( (v1Pos - v2Pos).length() == 0 ){ - orderedVtxList[ordVtxIdx(bIdx,1)] = aIdx; - } - } - } - } - - // Check if the last vtx index was assigned - if (orderedVtxList[ordVtxIdx(numVertices-1,1)] == -1){ - orderedVtxList[ordVtxIdx(numVertices-1,1)] = orderedVtxList[ordVtxIdx(numVertices-1,0)]; - uniqueVtxCount++; - } - - outputVertices->resize(uniqueVtxCount*3); - outputTriangles->resize(numTriangles*3); - a_vertexTrees->resize(uniqueVtxCount); - - // In this loop we append the index of the newly resized array containing - // the unique vertices to the index of the original array of duplicated vertices. - // This is an example of the orderedVtxList might look like for usual run - // After above steps - // orderedVtxList[:][0] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11} - // orderedVtxList[:][1] = { 0, 1, 2, 1, 4, 2, 1, 7, 4, 7, 10, 4} - // orderedVtxList[:][1] = {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1} - // And we want: - // orderedVtxList[:][0] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11} - // orderedVtxList[:][1] = { 0, 1, 2, 1, 4, 2, 1, 7, 4, 7, 10, 4} - // orderedVtxList[:][1] = { 0, 1, 2, 1, 3, 2, 1, 4, 3, 4, 5, 3} - int vtxCounted = 0; - cVector3d vPos; - for (int aIdx = 0 ; aIdx < numVertices ; aIdx++){ - if (orderedVtxList[ordVtxIdx(aIdx,1)] == orderedVtxList[ordVtxIdx(aIdx,0)] && orderedVtxList[ordVtxIdx(aIdx,2)] == -1){ // A unique vertex - vPos = mesh->m_vertices->getLocalPos(aIdx); - (*outputVertices)[3*vtxCounted + 0] = vPos.x(); - (*outputVertices)[3*vtxCounted + 1] = vPos.y(); - (*outputVertices)[3*vtxCounted + 2] = vPos.z(); - - orderedVtxList[ordVtxIdx(aIdx,2)] = vtxCounted; // Record the index in queue where the unique vertex is added - (*a_vertexTrees)[vtxCounted].vertexIdx.push_back(aIdx); - vtxCounted++; // Increase the queue idx by 1 - } - else if(orderedVtxList[ordVtxIdx(aIdx,1)] < orderedVtxList[ordVtxIdx(aIdx,0)]){ // Not a unique vertex - int bIdx = orderedVtxList[ordVtxIdx(aIdx,1)]; - int cIdx = orderedVtxList[ordVtxIdx(bIdx,2)]; - if (orderedVtxList[ordVtxIdx(bIdx,1)] != orderedVtxList[ordVtxIdx(bIdx,0)] || cIdx == -1){ - // This shouldn't happend. This means that we haven't assigned the third row - // and row 1 is greater than row 2 - throw "Algorithm Failed for (b[i] < a[i]), a[b[i]] != b[b[i]] : and c[b[i]] != -1"; - } - orderedVtxList[ordVtxIdx(aIdx,2)] = cIdx; - (*a_vertexTrees)[cIdx].vertexIdx.push_back(aIdx); - } - else if(orderedVtxList[ordVtxIdx(aIdx,1)] > orderedVtxList[ordVtxIdx(aIdx,0)]){ - int bIdx = orderedVtxList[ordVtxIdx(aIdx,1)]; - if (orderedVtxList[ordVtxIdx(bIdx,1)] != orderedVtxList[ordVtxIdx(bIdx,0)]){ - throw "Algorithm Failed for (b[i] > a[i]), a[b[i]] != b[b[i]] : %d"; - } - if (orderedVtxList[ordVtxIdx(bIdx,2)] == -1){ - vPos = mesh->m_vertices->getLocalPos(bIdx); - vtxCounted++; - (*outputVertices)[3*vtxCounted + 0] = vPos.x(); - (*outputVertices)[3*vtxCounted + 1] = vPos.y(); - (*outputVertices)[3*vtxCounted + 2] = vPos.z(); - orderedVtxList[ordVtxIdx(bIdx,2)] = vtxCounted; - } - orderedVtxList[ordVtxIdx(aIdx,2)] = orderedVtxList[ordVtxIdx(bIdx,2)]; - } - } - - // This last loop iterates over the triangle idxes and assigns the re-idxd vertices from the - // third row of orderedVtxList - for (int i = 0 ; i < mesh->m_triangles->m_indices.size() ; i++){ - int triIdx = mesh->m_triangles->m_indices[i]; if ( triIdx >= numVertices){ - std::cerr << "ERROR ! Triangle Vtx Index " << triIdx << " >= # Vertices " << numVertices << std::endl; - } - else{ - (*outputTriangles)[i] = orderedVtxList[ordVtxIdx(triIdx,2)]; - } - } - - // This last loop iterates over the lines and assigns the re-idxd vertices to the - // lines - if (outputLines){ - for (int i = 0 ; i < outputLines->size() ; i++){ - std::vector originalLine = (*outputLines)[i]; - std::vector reIndexedLine = originalLine; - for (int vtx = 0 ; vtx < originalLine.size() ; vtx++){ - reIndexedLine[vtx] = orderedVtxList[ordVtxIdx(originalLine[vtx],2)]; - } - (*outputLines)[i].clear(); - (*outputLines)[i] = reIndexedLine; - } - } - - if(print_debug_info){ - printf("*** SEQUENTIAL COMPUTE UNIQUE VERTICES AND TRIANGLE INDICES ***\n"); - - printf("# Unique Vertices = %d, # Duplicate Vertices = %d\n", uniqueVtxCount, duplicateVtxCount); - - for (int i = 0 ; i < numVertices ; i++){ - std::cerr << i << ")\t" << orderedVtxList[ordVtxIdx(i,0)] << " ,\t" << orderedVtxList[ordVtxIdx(i,1)] << " ,\t" << orderedVtxList[ordVtxIdx(i,2)] << std::endl; - } - } - - delete[] orderedVtxList; -} - - /// /// \brief afSoftBody::afSoftBody /// \param a_afWorld @@ -3354,6 +2839,11 @@ afSoftBody::afSoftBody(afWorldPtr a_afWorld, afModelPtr a_modelPtr): afInertialO } +/// +/// \brief afSoftBody::createFromAttribs +/// \param a_attribs +/// \return +/// bool afSoftBody::createFromAttribs(afSoftBodyAttributes *a_attribs) { storeAttributes(a_attribs); @@ -3372,7 +2862,6 @@ bool afSoftBody::createFromAttribs(afSoftBodyAttributes *a_attribs) if (afVisualUtils::createFromAttribs(&a_attribs->m_visualAttribs, m_visualMesh, m_name)){ m_visualMesh->scale(m_scale); - m_meshReductionSuccessful = false; } else { @@ -3384,10 +2873,10 @@ bool afSoftBody::createFromAttribs(afSoftBodyAttributes *a_attribs) } if (m_collisionMesh->loadFromFile(a_attribs->m_collisionAttribs.m_meshFilepath.c_str())){ + m_collisionMesh->removeDuplicateVertices(); m_collisionMesh->scale(m_scale); // Use the visual mesh for generating the softbody generateFromMesh(m_collisionMesh, a_attribs->m_collisionAttribs.m_margin); - cleanupMesh(m_visualMesh, m_afVertexTree, m_trianglesPtr); } else { @@ -3464,35 +2953,27 @@ bool afSoftBody::createFromAttribs(afSoftBodyAttributes *a_attribs) bool useOriginalIndexes = getVisualObject()->m_vtxIdxMap.size() > 0 ? true : false; for (uint i = 0 ; i < a_attribs->m_fixedNodes.size() ; i++){ + uint nodeIdx = a_attribs->m_fixedNodes[i]; - if ( nodeIdx < softBody->m_nodes.size()){ - if (useOriginalIndexes){ - // Find the node's original vertex index - map >::iterator nIt = getVisualObject()->m_vtxIdxMap.find(nodeIdx); - if ( nIt != getVisualObject()->m_vtxIdxMap.end()){ - if (nIt->second.size() > 0){ - int remappedIdx = nIt->second[0]; - int j = 0; - bool found = false; - while (j < m_afVertexTree.size() && !found){ - for (int k = 0 ; k < m_afVertexTree[j].vertexIdx.size() ; k++){ - if (remappedIdx == m_afVertexTree[j].vertexIdx[k]){ -// cerr << "Node Idx: " << nodeIdx -// << " | Original Vtx Idx: " << nIt->first -// << " | Remapped Vtx Idx: " << remappedIdx << endl; - softBody->setMass(j, 0); - found = true; - break; - } - } - j++; - } - } + if ( nodeIdx > softBody->m_nodes.size()){break;} + + if (useOriginalIndexes){ + // Find the node's original vertex index + map >::iterator nIt = getVisualObject()->m_vtxIdxMap.find(nodeIdx); + if ( nIt != getVisualObject()->m_vtxIdxMap.end()){ + if (nIt->second.size() == 0){break;} + int originalVtxIdx = nIt->second[0]; + unsigned int newIdx = m_collisionMesh->getMesh(0)->getNewVertexIndex(originalVtxIdx); + if (newIdx > 0){ + cerr << "INFO! Fixing Softbody Node. Original Node Idx: " << nodeIdx + << " | Old Vertex/Node Idx: " << originalVtxIdx + << " | New Vertex/Node Idx: " << newIdx << endl; + softBody->setMass(newIdx, 0); } } - else{ - softBody->setMass(nodeIdx, 0); - } + } + else{ + softBody->setMass(nodeIdx, 0); } } @@ -3539,23 +3020,12 @@ void afSoftBody::toggleSkeletalModelVisibility(){ void afSoftBody::updateSceneObjects(){ cMesh * mesh = m_visualMesh->getMesh(0); - if (m_meshReductionSuccessful){ - for (int i = 0 ; i < m_bulletSoftBody->m_nodes.size() ; i++){ - btVector3 nodePos = m_bulletSoftBody->m_nodes[i].m_x; - mesh->m_vertices->setLocalPos(i, nodePos[0], nodePos[1], nodePos[2]); - } - } - else{ - for (int i = 0 ; i < m_afVertexTree.size() ; i++){ - btVector3 nodePos = m_bulletSoftBody->m_nodes[i].m_x; - for (int j = 0 ; j < m_afVertexTree[i].vertexIdx.size() ; j++){ - int idx = m_afVertexTree[i].vertexIdx[j]; - mesh->m_vertices->setLocalPos(idx, nodePos[0], nodePos[1], nodePos[2]); - } - } + for (int i = 0 ; i < m_bulletSoftBody->m_nodes.size() ; i++){ + mesh->setVertexLocalPosForAllDuplicates(i, m_bulletSoftBody->m_nodes[i].m_x[0], m_bulletSoftBody->m_nodes[i].m_x[1], m_bulletSoftBody->m_nodes[i].m_x[2]); } mesh->computeAllNormals(); + mesh->markForUpdate(true); afBaseObject::updateSceneObjects(); } @@ -3605,66 +3075,42 @@ bool afSoftBody::cleanupMesh(cMultiMesh *multiMesh, std::vector &a reducedMesh->setShowEdges(false); multiMesh->m_meshes->clear(); multiMesh->addMesh(reducedMesh); - m_meshReductionSuccessful = true; } return valid; } + +/// +/// \brief afSoftBody::generateFromMesh +/// \param multiMesh +/// \param a_margin +/// \return +/// bool afSoftBody::generateFromMesh(cMultiMesh *multiMesh, const double a_margin) { // create compound shape m_bulletCollisionShape = new btCompoundShape();; - std::vector *v_meshes; - v_meshes = multiMesh->m_meshes; - // create collision detector for each mesh - std::vector::iterator it; - for (it = v_meshes->begin(); it < v_meshes->end(); it++) + for (int mi=0; mi < multiMesh->getNumMeshes() ; mi++) { - cMesh* mesh = (*it); - // read number of triangles of the object - int numTriangles = mesh->m_triangles->getNumElements(); - std::vector > polyLines = mesh->m_lines; - // computeUniqueVerticesandTriangles(mesh, &m_verticesPtr, &m_trianglesPtr, &m_afVertexTree, &polyLines, false); - afMeshCleanup::computeUniqueVerticesandTrianglesSequential(mesh, &m_verticesPtr, &m_trianglesPtr, &m_afVertexTree, &polyLines, false); - if (m_trianglesPtr.size() > 0){ - m_bulletSoftBody = createFromMesh(*m_afWorld->m_bulletSoftBodyWorldInfo, - m_verticesPtr.data(), m_verticesPtr.size() / 3, m_trianglesPtr.data(), numTriangles); - createLinksFromLines(m_bulletSoftBody, &polyLines, mesh); - } - else{ - m_bulletSoftBody = new btSoftBody(m_afWorld->m_bulletSoftBodyWorldInfo); - /* Default material */ - btSoftBody::Material* pm = m_bulletSoftBody->appendMaterial(); - pm->m_kLST = 1; - pm->m_kAST = 1; - pm->m_kVST = 1; - pm->m_flags = btSoftBody::fMaterial::Default; - if (m_bulletSoftBody){ - m_bulletSoftBody->m_nodes.resize(mesh->m_vertices->getNumElements()); - for(int nIdx = 0 ; nIdx < m_bulletSoftBody->m_nodes.size() ; nIdx++){ - btVector3 vPos = to_btVector(mesh->m_vertices->getLocalPos(nIdx)); - btSoftBody::Node& n = m_bulletSoftBody->m_nodes[nIdx]; - n.m_im = 1; - n.m_im = 1 / n.m_im; - n.m_x = vPos; - n.m_q = n.m_x; - n.m_n = btVector3(0, 0, 1); - n.m_leaf = m_bulletSoftBody->m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, 0.1), &n); - n.m_material = m_bulletSoftBody->m_materials[0]; - } - } - createLinksFromLines(m_bulletSoftBody, &mesh->m_lines, mesh); - } + cMesh* mesh = multiMesh->getMesh(mi); + m_bulletSoftBody = createFromMesh(m_afWorld->m_bulletSoftBodyWorldInfo, mesh, false); + createLinksFromLines(m_bulletSoftBody, &mesh->m_lines, mesh); m_bulletSoftBody->getCollisionShape()->setMargin(a_margin); } - return true; } +/// +/// \brief afSoftBody::createLinksFromLines +/// \param a_softBody +/// \param a_lines +/// \param a_mesh +/// \return +/// bool afSoftBody::createLinksFromLines(btSoftBody *a_softBody, std::vector< std::vector > *a_lines, cMesh* a_mesh){ if (a_softBody && a_lines){ for(int lIdx = 0 ; lIdx < a_lines->size() ; lIdx++){ @@ -3713,30 +3159,35 @@ bool afSoftBody::createLinksFromLines(btSoftBody *a_softBody, std::vector< std:: } -btSoftBody* afSoftBody::createFromMesh(btSoftBodyWorldInfo& worldInfo, const btScalar* vertices, int nNodes, - const unsigned int* triangles, int ntriangles, bool randomizeConstraints) -{ +/// +/// \brief afSoftBody::createFromMesh +/// \param worldInfo +/// \param a_mesh +/// \param randomizeConstraints +/// \return +/// +btSoftBody* afSoftBody::createFromMesh(btSoftBodyWorldInfo* worldInfo, cMesh *a_mesh, bool randomizeConstraints){ unsigned int maxidx = 0; int i, j, ni; - for (i = 0, ni = ntriangles * 3; i < ni; ++i) + for (i = 0, ni = a_mesh->getNumTriangles() * 3; i < ni; ++i) { - maxidx = btMax(triangles[i], maxidx); + maxidx = btMax(a_mesh->m_triangles->m_indices[i], maxidx); } ++maxidx; btAlignedObjectArray chks; btAlignedObjectArray vtx; chks.resize(maxidx * maxidx, false); - vtx.resize(nNodes); - for (i = 0, j = 0; i < nNodes * 3; ++j, i += 3) + vtx.resize(a_mesh->getNumVertices()); + for (i = 0; i < a_mesh->getNumVertices(); i++) { - vtx[j] = btVector3(vertices[i], vertices[i + 1], vertices[i + 2]); + vtx[i] = to_btVector(a_mesh->m_vertices->getLocalPos(i)); } - btSoftBody* psb = new btSoftBody(&worldInfo, vtx.size(), &vtx[0], 0); - for (i = 0, ni = ntriangles * 3; i < ni; i += 3) + btSoftBody* psb = new btSoftBody(worldInfo, vtx.size(), &vtx[0], 0); + for (i = 0, ni = a_mesh->getNumTriangles() * 3; i < ni; i += 3) { - const unsigned int idx[] = {triangles[i], triangles[i + 1], triangles[i + 2]}; -#define IDX(_x_, _y_) ((_y_)*maxidx + (_x_)) + const unsigned int idx[] = {a_mesh->m_triangles->m_indices[i], a_mesh->m_triangles->m_indices[i + 1], a_mesh->m_triangles->m_indices[i + 2]}; + #define IDX(_x_, _y_) ((_y_)*maxidx + (_x_)) for (int j = 2, k = 0; k < 3; j = k++) { if (!chks[IDX(idx[j], idx[k])]) @@ -3746,7 +3197,7 @@ btSoftBody* afSoftBody::createFromMesh(btSoftBodyWorldInfo& worldInfo, const btS psb->appendLink(idx[j], idx[k]); } } -#undef IDX + #undef IDX psb->appendFace(idx[0], idx[1], idx[2]); } @@ -3756,6 +3207,7 @@ btSoftBody* afSoftBody::createFromMesh(btSoftBodyWorldInfo& worldInfo, const btS } return (psb); + } @@ -8681,6 +8133,7 @@ bool afGhostObject::createFromAttribs(afGhostObjectAttributes *a_attribs) if (a_attribs->m_collisionAttribs.m_geometryType == afGeometryType::MESH){ if (m_collisionMesh->loadFromFile(a_attribs->m_collisionAttribs.m_meshFilepath.c_str()) ){ m_collisionMesh->scale(m_scale); + m_collisionMesh->removeDuplicateVertices(); m_bulletCollisionShape = afShapeUtils::createCollisionShape(m_collisionMesh, a_attribs->m_collisionAttribs.m_margin, afTransform(), diff --git a/ambf_framework/afFramework.h b/ambf_framework/afFramework.h index af00cb574..d980e86a5 100644 --- a/ambf_framework/afFramework.h +++ b/ambf_framework/afFramework.h @@ -174,9 +174,9 @@ class afShapeUtils{ static btCollisionShape* createCollisionShape(const afPrimitiveShapeAttributes* a_primitiveShape, double a_margin); - static btCollisionShape* createCollisionShape(const cMesh* a_collisionMesh, double a_margin, afCollisionMeshShapeType a_meshType); + static btCollisionShape* createCollisionShape(cMesh* a_collisionMesh, double a_margin, afCollisionMeshShapeType a_meshType); - static btCompoundShape* createCollisionShape(const cMultiMesh* a_collisionMesh, double a_margin, afTransform m_inertialOffset, afCollisionMeshShapeType a_meshType); + static btCompoundShape* createCollisionShape(cMultiMesh* a_collisionMesh, double a_margin, afTransform m_inertialOffset, afCollisionMeshShapeType a_meshType); static std::vector createRayAttribs(cMultiMesh* a_contourMesh, double a_range); }; @@ -1214,24 +1214,10 @@ class afSoftBody: public afInertialObject{ bool createLinksFromLines(btSoftBody* a_sb, std::vector< std::vector>* a_lines, cMesh* a_mesh); // Copied from btSoftBodyHelpers with few modifications - btSoftBody* createFromMesh(btSoftBodyWorldInfo& worldInfo, const btScalar* vertices, int nNodes, const unsigned int* triangles, int ntriangles, bool randomizeConstraints=true); + btSoftBody* createFromMesh(btSoftBodyWorldInfo* worldInfo, cMesh* a_mesh, bool randomizeConstraints=true); //! This method toggles the drawing of skeletal model. void toggleSkeletalModelVisibility(); - -private: - // Ptr to scalar vertex arrays of the sofy body - std::vector m_verticesPtr; - - // Ptr to Triangles arrays referring to vertices by indices - std::vector m_trianglesPtr; - - // Vertex Tree containing vtx idx's that are repeated for a given vtx - std::vector m_afVertexTree; - - // Boolean flag to indicate if we have been successful in reducing the mesh. - // A reduced mesh should speed up rendering. - bool m_meshReductionSuccessful; }; diff --git a/external/chai3d/src/graphics/CTriangleArray.h b/external/chai3d/src/graphics/CTriangleArray.h index bc031f2fa..370dbffd5 100644 --- a/external/chai3d/src/graphics/CTriangleArray.h +++ b/external/chai3d/src/graphics/CTriangleArray.h @@ -599,7 +599,7 @@ class cTriangleArray : public cGenericArray if (m_flagMarkForResize) { glBufferData(GL_ELEMENT_ARRAY_BUFFER, 3 * numtriangles * sizeof(unsigned int), &(m_indices[0]), GL_STATIC_DRAW); - m_flagMarkForResize = true; + m_flagMarkForResize = false; } // update data if needed diff --git a/external/chai3d/src/world/CMesh.cpp b/external/chai3d/src/world/CMesh.cpp index f2e019679..fdcdc07c2 100644 --- a/external/chai3d/src/world/CMesh.cpp +++ b/external/chai3d/src/world/CMesh.cpp @@ -124,6 +124,10 @@ cMesh::cMesh(cMaterialPtr a_material) { m_material = a_material; } + + m_duplicateVerticesFound = false; + + m_duplicateVerticesRemoved = false; } @@ -676,6 +680,52 @@ void cMesh::setVertexColor(const cColorf& a_color) } +//============================================================================== +/*! + Set the local pos of a vertex and all of it's duplicates + + \param a_idx index. + \param a_pos The position. +*/ +//============================================================================== +void cMesh::setVertexLocalPosForAllDuplicates(const unsigned int &a_idx, const cVector3d& a_pos){ + if (!m_duplicateVerticesFound){ + findDuplicateVertices(); + } + auto it = m_duplicateVertexIndexTree.find(a_idx); + + if (it != m_duplicateVertexIndexTree.end()){ + for (unsigned int i = 0 ; i < it->second.m_vertexIndices.size() ; i++){ + m_vertices->setLocalPos(it->second.m_vertexIndices[i], a_pos); + } + } +} + + +//============================================================================== +/*! + Set the local pos of a vertex and all of it's duplicates + + \param a_idx index. + \param a_x x. + \param a_y y. + \param a_z z. +*/ +//============================================================================== +void cMesh::setVertexLocalPosForAllDuplicates(const unsigned int &a_idx, const double &a_x, const double &a_y, const double &a_z){ + if (!m_duplicateVerticesFound){ + findDuplicateVertices(); + } + auto it = m_duplicateVertexIndexTree.find(a_idx); + + if (it != m_duplicateVertexIndexTree.end()){ + for (unsigned int i = 0 ; i < it->second.m_vertexIndices.size() ; i++){ + m_vertices->setLocalPos(it->second.m_vertexIndices[i], a_x, a_y, a_z); + } + } +} + + //============================================================================== /*! This method shifts all vertex positions by the specified amount. @@ -973,6 +1023,169 @@ void cMesh::clearAllEdges() } + +class afMeshWeldingSpecs{ +public: + afMeshWeldingSpecs(cVector3d a_minBounds, cVector3d a_maxBounds, double a_weldingThreshold){ + m_weldindThreshold = a_weldingThreshold; + m_minBounds = a_minBounds; + m_maxBounds = a_maxBounds; + cVector3d deltaBounds = m_maxBounds - m_minBounds; + m_deltaBoundsX = deltaBounds.x(); + m_deltaBoundsXY = m_deltaBoundsX * deltaBounds.y(); + m_deltaBoundsXYZ = m_deltaBoundsXY * deltaBounds.z(); + } + + double m_weldindThreshold; + cVector3d m_minBounds; + cVector3d m_maxBounds; + double m_deltaBoundsX; + double m_deltaBoundsXY; + double m_deltaBoundsXYZ; +}; + +class afTriVertex{ +public: + afTriVertex(const cVector3d& v, const uint &idx, const afMeshWeldingSpecs* a_weldingSpecs): m_weldingSpecs(a_weldingSpecs){ + m_x=v(0); m_y=v(1); m_z=v(2); m_idx=idx; + m_weldingSpecs = a_weldingSpecs; + computeHash(); + } + + bool operator==(const afTriVertex& rhs) const + { + if (this->m_x == rhs.m_x && this->m_y == rhs.m_y && this->m_z == rhs.m_z) return true; + else return false; + } + + void computeHash(){ + m_hash = (m_weldingSpecs->m_minBounds.x() + m_x) + + m_weldingSpecs->m_deltaBoundsX * (m_weldingSpecs->m_minBounds.y() + m_y) + + m_weldingSpecs->m_deltaBoundsXY * (m_weldingSpecs->m_minBounds.z() + m_z); + } + + const double& getHash() const{ + return m_hash; + + } + + bool operator<(const afTriVertex& rhs) const + { + return (getHash() < rhs.getHash()); + } + +public: + double m_x; + double m_y; + double m_z; + double m_hash; + const afMeshWeldingSpecs* m_weldingSpecs; + uint m_idx; +}; + + +//============================================================================== +/*! + This method removes all duplicate/repeated vertices. +*/ +//============================================================================== +bool cMesh::removeDuplicateVertices(double& a_weldingThreshold) +{ + if (m_duplicateVerticesRemoved){ + return true; + } + + bool res = findDuplicateVertices(a_weldingThreshold); + + if (res){ + cMesh* nMesh = new cMesh(); + unsigned int unique_vertex_count = m_duplicateVertexIndexTree.size(); + // nMesh->m_vertices->allocateData(nS, true, false, false, false, false, false); + nMesh->m_vertices->allocateData(unique_vertex_count, + m_vertices->getUseNormalData(), + m_vertices->getUseTexCoordData(), + m_vertices->getUseColorData(), + m_vertices->getUseTangentData(), + m_vertices->getUseBitangentData(), + m_vertices->getUseUserData()); + + for (auto it = m_duplicateVertexIndexTree.begin() ; it != m_duplicateVertexIndexTree.end() ; ++it){ + uint newIdx = it->first; + uint origIdx = it->second.m_vertexIndices[0]; + + nMesh->m_vertices->setLocalPos(newIdx, m_vertices->getLocalPos(origIdx)); + + if(m_vertices->getUseNormalData()) nMesh->m_vertices->setNormal(newIdx, m_vertices->getNormal(origIdx)); + if(m_vertices->getUseTexCoordData()) nMesh->m_vertices->setTexCoord(newIdx, m_vertices->getTexCoord(origIdx)); + if(m_vertices->getUseColorData()) nMesh->m_vertices->setColor(newIdx, m_vertices->getColor(origIdx)); + if(m_vertices->getUseTangentData()) nMesh->m_vertices->setTangent(newIdx, m_vertices->getTangent(origIdx)); + if(m_vertices->getUseBitangentData()) nMesh->m_vertices->setBitangent(newIdx, m_vertices->getBitangent(origIdx)); + if(m_vertices->getUseUserData()) nMesh->m_vertices->setUserData(newIdx, m_vertices->getUserData(origIdx)); + } + printf("INFO! Original/New vertex count [%u/%u]. Removed [%u] vertices \n", getNumVertices(), nMesh->getNumVertices(), getNumVertices() - nMesh->getNumVertices()); + m_vertices->clear(); + m_vertices = nMesh->m_vertices->copy(); + + vector recomputedIndices; + recomputedIndices.resize(m_triangles->m_indices.size()); + for (auto it = m_duplicateVertexIndexTree.begin() ; it != m_duplicateVertexIndexTree.end() ; ++it){ + for (int j = 0 ; j < it->second.m_elementIndices.size() ; j++){ + recomputedIndices[it->second.m_elementIndices[j]] = it->first; + } + it->second.m_vertexIndices.clear(); + it->second.m_vertexIndices.push_back(it->first); + } + + m_triangles->m_indices.clear(); + m_triangles->m_indices = recomputedIndices; + m_triangles->m_vertices = m_vertices; + computeAllNormals(); + m_duplicateVerticesRemoved = true; + } + return res; +} + + +//============================================================================== +/*! + This method finds duplicate vertices and computes a tree of unique vertices mapping their duplicates +*/ +//============================================================================== +bool cMesh::findDuplicateVertices(double a_weldingThreshold){ + if (m_duplicateVerticesFound){ + return true; + } + + set rMesh; + computeBoundaryBox(); + afMeshWeldingSpecs weldingSpecs(getBoundaryMin(), getBoundaryMax(), a_weldingThreshold); + m_originalToNewMapping.resize(m_vertices->getNumElements()); + + uint insIdx = 0; + for(int i = 0 ; i < m_triangles->m_indices.size() ; i++){ + uint oIdx = m_triangles->m_indices[i]; + cVector3d v = m_triangles->m_vertices->getLocalPos(oIdx); + auto insIt = rMesh.insert(afTriVertex(v, insIdx, &weldingSpecs)); + uint nIdx; + if (insIt.second){ + nIdx = insIdx; + insIdx++; + } + else{ + nIdx = insIt.first->m_idx; + } + m_duplicateVertexIndexTree[nIdx].m_elementIndices.push_back(i); + m_duplicateVertexIndexTree[nIdx].m_vertexIndices.push_back(oIdx); + m_originalToNewMapping[oIdx] = nIdx; + } + + bool res = m_duplicateVertexIndexTree.size() == m_vertices->getNumElements() ? 0 : 1; + + m_duplicateVerticesFound = true; + return res; +} + + //============================================================================== /*! This method sets the graphic properties for edge-rendering. @@ -1892,6 +2105,18 @@ void cEdge::set(cMesh* a_parent, } } +unsigned int cMesh::getNewVertexIndex(const unsigned int &a_idx) +{ + if (!m_duplicateVerticesFound){ + findDuplicateVertices(); + } + if (a_idx >= m_originalToNewMapping.size()){ + cerr << "ERROR! REQUESTED INDEX " << a_idx << " GREATER THAN THE SIZE OF ORIGINAL VERTEX LIST" << m_originalToNewMapping.size() << "\n"; + return -1; + } + return m_originalToNewMapping[a_idx]; +} + //------------------------------------------------------------------------------ #endif // DOXYGEN_SHOULD_SKIP_THIS //------------------------------------------------------------------------------ diff --git a/external/chai3d/src/world/CMesh.h b/external/chai3d/src/world/CMesh.h index 978041156..e482da068 100644 --- a/external/chai3d/src/world/CMesh.h +++ b/external/chai3d/src/world/CMesh.h @@ -59,6 +59,7 @@ //------------------------------------------------------------------------------ #include #include +#include //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ @@ -92,6 +93,12 @@ struct cEdge; //------------------------------------------------------------------------------ +struct cDuplicateVertexData{ +public: + std::vector m_elementIndices; // From array of elements representing triangle vertices + std::vector m_vertexIndices; // From array of vertex indices +}; + //============================================================================== /*! \class cMesh @@ -186,6 +193,12 @@ class cMesh : public cGenericObject //! This method sets the color of each vertex. void setVertexColor(const cColorf& a_color); + //! Set the local pos of a vertex and all of it's duplicates + void setVertexLocalPosForAllDuplicates(const unsigned int& a_idx, const cVector3d& a_pos); + + //! Set the local pos of a vertex and all of it's duplicates + void setVertexLocalPosForAllDuplicates(const unsigned int& a_idx, const double& a_x, const double& a_y, const double& a_z); + //-------------------------------------------------------------------------- // PUBLIC METHODS - TRIANGLES @@ -240,6 +253,12 @@ class cMesh : public cGenericObject //! This method clears all edges void clearAllEdges(); + //! This method removes duplicate vertices and updates triangles indices. + bool removeDuplicateVertices(double& a_weldingThreshold); + + //! Find duplicate vertices and record their indices. + bool findDuplicateVertices(double a_weldingThreshold=0.0); + //! This method enables or disables the rendering of edges. void setShowEdges(const bool a_showEdges) { m_showEdges = a_showEdges; } @@ -422,6 +441,30 @@ class cMesh : public cGenericObject //! Array of Edges. std::vector m_edges; + +public: + + unsigned int getNewVertexIndex(const unsigned int& a_idx); + + //! Tree of duplicate vertex indices, the key is the new index (after identifying duplicates) and + //! value (rhs) is the list of original indices of duplicate vertices. + //! E.g. Imagine two triangles <123 and <456 with a shared edge between with vertices 2,3 == 4,5 + //! Original Indices = [1, 2, 3, 4, 5, 6] + //! Duplicates = [1, 2, 3, 2, 3, 6] + //! Tree = {1: [1] + //! {2: [2, 4] + //! {3: [3, 5] + //! {4: [6]} + std::map m_duplicateVertexIndexTree; + + // Vector to main a relationship between old indices (with duplicates) and new indices (without dpulicates) + std::vector m_originalToNewMapping; + + //! Flag to check if duplicate vertices have been removed + bool m_duplicateVerticesFound; + + //! Flag to check if duplicate vertices have been computed + bool m_duplicateVerticesRemoved; }; diff --git a/external/chai3d/src/world/CMultiMesh.cpp b/external/chai3d/src/world/CMultiMesh.cpp index 0af6e716f..1f2b2f9cc 100644 --- a/external/chai3d/src/world/CMultiMesh.cpp +++ b/external/chai3d/src/world/CMultiMesh.cpp @@ -1661,6 +1661,24 @@ bool cMultiMesh::loadFromFile(string a_filename) return (result); } +bool cMultiMesh::findDuplicateVertices(double a_weldingThreshold) +{ + bool res = false; + for (int i=0; i < m_meshes->size() ; i++){ + res |= getMesh(i)->findDuplicateVertices(a_weldingThreshold); + }; + return res; +} + +bool cMultiMesh::removeDuplicateVertices(double a_weldingThreshold) +{ + bool res = false; + for (int i=0; i < m_meshes->size() ; i++){ + res |= getMesh(i)->removeDuplicateVertices(a_weldingThreshold); + }; + return res; +} + //============================================================================== /*! diff --git a/external/chai3d/src/world/CMultiMesh.h b/external/chai3d/src/world/CMultiMesh.h index 05b708814..9dd264f33 100644 --- a/external/chai3d/src/world/CMultiMesh.h +++ b/external/chai3d/src/world/CMultiMesh.h @@ -414,6 +414,12 @@ class cMultiMesh : public cGenericObject //! This method loads a 3D object from a file. virtual bool loadFromFile(std::string a_filename); + //! This method finds and remove duplicate vertices and updates mesh indices + virtual bool findDuplicateVertices(double a_weldingThreshold=0.0); + + //! This method finds and remove duplicate vertices and updates mesh indices + virtual bool removeDuplicateVertices(double a_weldingThreshold=0.0); + //! This method saves 3D object to a file. virtual bool saveToFile(std::string a_filename);