Extras/RigidBodyGpuPipeline/opencl/gpu_rigidbody_pipeline/btConvexUtility.cpp

   1 /*\r
   2 Copyright (c) 2012 Advanced Micro Devices, Inc.  \r
   3 \r
   4 This software is provided 'as-is', without any express or implied warranty.\r
   5 In no event will the authors be held liable for any damages arising from the use of this software.\r
   6 Permission is granted to anyone to use this software for any purpose, \r
   7 including commercial applications, and to alter it and redistribute it freely, \r
   8 subject to the following restrictions:\r
   9 \r
  10 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\r
  11 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\r
  12 3. This notice may not be removed or altered from any source distribution.\r
  13 */\r
  14 //Originally written by Erwin Coumans\r
  15 \r
  16 \r
  17 #include "btConvexUtility.h"\r
  18 #include "LinearMath/btConvexHullComputer.h"\r
  19 #include "LinearMath/btGrahamScan2dConvexHull.h"\r
  20 #include "LinearMath/btQuaternion.h"\r
  21 \r
  22 bool    btConvexUtility::initializePolyhedralFeatures(const btAlignedObjectArray<btVector3>& orgVertices, bool mergeCoplanarTriangles)\r
  23 {\r
  24         \r
  25 \r
  26         btConvexHullComputer conv;\r
  27         conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f);\r
  28 \r
  29         btAlignedObjectArray<btVector3> faceNormals;\r
  30         int numFaces = conv.faces.size();\r
  31         faceNormals.resize(numFaces);\r
  32         btConvexHullComputer* convexUtil = &conv;\r
  33 \r
  34         \r
  35         btAlignedObjectArray<btFace>    tmpFaces;\r
  36         tmpFaces.resize(numFaces);\r
  37 \r
  38         int numVertices = convexUtil->vertices.size();\r
  39         m_vertices.resize(numVertices);\r
  40         for (int p=0;p<numVertices;p++)\r
  41         {\r
  42                 m_vertices[p] = convexUtil->vertices[p];\r
  43         }\r
  44 \r
  45 \r
  46         for (int i=0;i<numFaces;i++)\r
  47         {\r
  48                 int face = convexUtil->faces[i];\r
  49                 //printf("face=%d\n",face);\r
  50                 const btConvexHullComputer::Edge*  firstEdge = &convexUtil->edges[face];\r
  51                 const btConvexHullComputer::Edge*  edge = firstEdge;\r
  52 \r
  53                 btVector3 edges[3];\r
  54                 int numEdges = 0;\r
  55                 //compute face normals\r
  56 \r
  57                 btScalar maxCross2 = 0.f;\r
  58                 int chosenEdge = -1;\r
  59 \r
  60                 do\r
  61                 {\r
  62                         \r
  63                         int src = edge->getSourceVertex();\r
  64                         tmpFaces[i].m_indices.push_back(src);\r
  65                         int targ = edge->getTargetVertex();\r
  66                         btVector3 wa = convexUtil->vertices[src];\r
  67 \r
  68                         btVector3 wb = convexUtil->vertices[targ];\r
  69                         btVector3 newEdge = wb-wa;\r
  70                         newEdge.normalize();\r
  71                         if (numEdges<2)\r
  72                                 edges[numEdges++] = newEdge;\r
  73 \r
  74                         edge = edge->getNextEdgeOfFace();\r
  75                 } while (edge!=firstEdge);\r
  76 \r
  77                 btScalar planeEq = 1e30f;\r
  78 \r
  79                 \r
  80                 if (numEdges==2)\r
  81                 {\r
  82                         faceNormals[i] = edges[0].cross(edges[1]);\r
  83                         faceNormals[i].normalize();\r
  84                         tmpFaces[i].m_plane[0] = faceNormals[i].getX();\r
  85                         tmpFaces[i].m_plane[1] = faceNormals[i].getY();\r
  86                         tmpFaces[i].m_plane[2] = faceNormals[i].getZ();\r
  87                         tmpFaces[i].m_plane[3] = planeEq;\r
  88 \r
  89                 }\r
  90                 else\r
  91                 {\r
  92                         btAssert(0);//degenerate?\r
  93                         faceNormals[i].setZero();\r
  94                 }\r
  95 \r
  96                 for (int v=0;v<tmpFaces[i].m_indices.size();v++)\r
  97                 {\r
  98                         btScalar eq = m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]);\r
  99                         if (planeEq>eq)\r
 100                         {\r
 101                                 planeEq=eq;\r
 102                         }\r
 103                 }\r
 104                 tmpFaces[i].m_plane[3] = -planeEq;\r
 105         }\r
 106 \r
 107         //merge coplanar faces\r
 108 \r
 109         btScalar faceWeldThreshold= 0.999f;\r
 110         btAlignedObjectArray<int> todoFaces;\r
 111         for (int i=0;i<tmpFaces.size();i++)\r
 112                 todoFaces.push_back(i);\r
 113 \r
 114         while (todoFaces.size())\r
 115         {\r
 116                 btAlignedObjectArray<int> coplanarFaceGroup;\r
 117                 int refFace = todoFaces[todoFaces.size()-1];\r
 118 \r
 119                 coplanarFaceGroup.push_back(refFace);\r
 120                 btFace& faceA = tmpFaces[refFace];\r
 121                 todoFaces.pop_back();\r
 122 \r
 123                 btVector3 faceNormalA(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]);\r
 124                 for (int j=todoFaces.size()-1;j>=0;j--)\r
 125                 {\r
 126                         int i = todoFaces[j];\r
 127                         btFace& faceB = tmpFaces[i];\r
 128                         btVector3 faceNormalB(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]);\r
 129                         if (faceNormalA.dot(faceNormalB)>faceWeldThreshold)\r
 130                         {\r
 131                                 coplanarFaceGroup.push_back(i);\r
 132                                 todoFaces.remove(i);\r
 133                         }\r
 134                 }\r
 135 \r
 136 \r
 137                 bool did_merge = false;\r
 138                 if (mergeCoplanarTriangles && coplanarFaceGroup.size()>1)\r
 139                 {\r
 140                         //do the merge: use Graham Scan 2d convex hull\r
 141 \r
 142                         btAlignedObjectArray<GrahamVector2> orgpoints;\r
 143 \r
 144                         for (int i=0;i<coplanarFaceGroup.size();i++)\r
 145                         {\r
 146 \r
 147                                 btFace& face = tmpFaces[coplanarFaceGroup[i]];\r
 148                                 btVector3 faceNormal(face.m_plane[0],face.m_plane[1],face.m_plane[2]);\r
 149                                 btVector3 xyPlaneNormal(0,0,1);\r
 150 \r
 151                                 btQuaternion rotationArc = shortestArcQuat(faceNormal,xyPlaneNormal);\r
 152                                 \r
 153                                 for (int f=0;f<face.m_indices.size();f++)\r
 154                                 {\r
 155                                         int orgIndex = face.m_indices[f];\r
 156                                         btVector3 pt = m_vertices[orgIndex];\r
 157                                         btVector3 rotatedPt =  quatRotate(rotationArc,pt);\r
 158                                         rotatedPt.setZ(0);\r
 159                                         bool found = false;\r
 160 \r
 161                                         for (int i=0;i<orgpoints.size();i++)\r
 162                                         {\r
 163                                                 //if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001))\r
 164                                                 if (orgpoints[i].m_orgIndex == orgIndex)\r
 165                                                 {\r
 166                                                         found=true;\r
 167                                                         break;\r
 168                                                 }\r
 169                                         }\r
 170                                         if (!found)\r
 171                                                 orgpoints.push_back(GrahamVector2(rotatedPt,orgIndex));\r
 172                                 }\r
 173                         }\r
 174 \r
 175                         btFace combinedFace;\r
 176                         for (int i=0;i<4;i++)\r
 177                                 combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];\r
 178 \r
 179                         btAlignedObjectArray<GrahamVector2> hull;\r
 180                         GrahamScanConvexHull2D(orgpoints,hull);\r
 181 \r
 182                         for (int i=0;i<hull.size();i++)\r
 183                         {\r
 184                                 combinedFace.m_indices.push_back(hull[i].m_orgIndex);\r
 185                                 for(int k = 0; k < orgpoints.size(); k++) {\r
 186                                         if(orgpoints[k].m_orgIndex == hull[i].m_orgIndex) {\r
 187                                                 orgpoints[k].m_orgIndex = -1; // invalidate...\r
 188                                                 break;\r
 189                         }\r
 190                                 }\r
 191                         }\r
 192                         // are there rejected vertices?\r
 193                         bool reject_merge = false;\r
 194                         for(int i = 0; i < orgpoints.size(); i++) {\r
 195                                 if(orgpoints[i].m_orgIndex == -1)\r
 196                                         continue; // this is in the hull...\r
 197                                 // this vertex is rejected -- is anybody else using this vertex?\r
 198                                 for(int j = 0; j < tmpFaces.size(); j++) {\r
 199                                         btFace& face = tmpFaces[j];\r
 200                                         // is this a face of the current coplanar group?\r
 201                                         bool is_in_current_group = false;\r
 202                                         for(int k = 0; k < coplanarFaceGroup.size(); k++) {\r
 203                                                 if(coplanarFaceGroup[k] == j) {\r
 204                                                         is_in_current_group = true;\r
 205                                                         break;\r
 206                                                 }\r
 207                                         }\r
 208                                         if(is_in_current_group) // ignore this face...\r
 209                                                 continue;\r
 210                                         // does this face use this rejected vertex?\r
 211                                         for(int v = 0; v < face.m_indices.size(); v++) {\r
 212                                                 if(face.m_indices[v] == orgpoints[i].m_orgIndex) {\r
 213                                                         // this rejected vertex is used in another face -- reject merge\r
 214                                                         reject_merge = true;\r
 215                                                         break;\r
 216                                                 }\r
 217                                         }\r
 218                                         if(reject_merge)\r
 219                                                 break;\r
 220                                 }\r
 221                                 if(reject_merge)\r
 222                                         break;\r
 223                         }\r
 224                         if(!reject_merge) {\r
 225                                 // do this merge!\r
 226                                 did_merge = true;\r
 227                         m_faces.push_back(combinedFace);\r
 228                         }\r
 229                 }\r
 230                 if(!did_merge)\r
 231                 {\r
 232                         for (int i=0;i<coplanarFaceGroup.size();i++)\r
 233                         {\r
 234                                 m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);\r
 235                         }\r
 236                 }\r
 237 \r
 238         }\r
 239         return true;\r
 240 }\r