btPolyhedralConvexShape.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
00017 #include "btConvexPolyhedron.h"
00018 #include "LinearMath/btConvexHullComputer.h"
00019 #include <new>
00020 #include "LinearMath/btGeometryUtil.h"
00021 #include "LinearMath/btGrahamScan2dConvexHull.h"
00022 
00023 
00024 btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape(),
00025 m_polyhedron(0)
00026 {
00027 
00028 }
00029 
00030 btPolyhedralConvexShape::~btPolyhedralConvexShape()
00031 {
00032         if (m_polyhedron)
00033         {
00034                 m_polyhedron->~btConvexPolyhedron();
00035                 btAlignedFree(m_polyhedron);
00036         }
00037 }
00038 
00039 
00040 bool    btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMargin)
00041 {
00042 
00043         if (m_polyhedron)
00044         {
00045                 m_polyhedron->~btConvexPolyhedron();
00046                 btAlignedFree(m_polyhedron);
00047         }
00048         
00049         void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16);
00050         m_polyhedron = new (mem) btConvexPolyhedron;
00051 
00052         btAlignedObjectArray<btVector3> orgVertices;
00053 
00054         for (int i=0;i<getNumVertices();i++)
00055         {
00056                 btVector3& newVertex = orgVertices.expand();
00057                 getVertex(i,newVertex);
00058         }
00059         
00060         btConvexHullComputer conv;
00061         
00062         if (shiftVerticesByMargin)
00063         {
00064                 btAlignedObjectArray<btVector3> planeEquations;
00065                 btGeometryUtil::getPlaneEquationsFromVertices(orgVertices,planeEquations);
00066 
00067                 btAlignedObjectArray<btVector3> shiftedPlaneEquations;
00068                 for (int p=0;p<planeEquations.size();p++)
00069                 {
00070                            btVector3 plane = planeEquations[p];
00071                 //         btScalar margin = getMargin();
00072                            plane[3] -= getMargin();
00073                            shiftedPlaneEquations.push_back(plane);
00074                 }
00075 
00076                 btAlignedObjectArray<btVector3> tmpVertices;
00077 
00078                 btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,tmpVertices);
00079         
00080                 conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f);
00081         } else
00082         {
00083                 
00084                 conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f);
00085         }
00086 
00087 
00088 
00089         btAlignedObjectArray<btVector3> faceNormals;
00090         int numFaces = conv.faces.size();
00091         faceNormals.resize(numFaces);
00092         btConvexHullComputer* convexUtil = &conv;
00093 
00094         
00095         btAlignedObjectArray<btFace>    tmpFaces;
00096         tmpFaces.resize(numFaces);
00097 
00098         int numVertices = convexUtil->vertices.size();
00099         m_polyhedron->m_vertices.resize(numVertices);
00100         for (int p=0;p<numVertices;p++)
00101         {
00102                 m_polyhedron->m_vertices[p] = convexUtil->vertices[p];
00103         }
00104 
00105 
00106         for (int i=0;i<numFaces;i++)
00107         {
00108                 int face = convexUtil->faces[i];
00109                 //printf("face=%d\n",face);
00110                 const btConvexHullComputer::Edge*  firstEdge = &convexUtil->edges[face];
00111                 const btConvexHullComputer::Edge*  edge = firstEdge;
00112 
00113                 btVector3 edges[3];
00114                 int numEdges = 0;
00115                 //compute face normals
00116 
00117                 do
00118                 {
00119                         
00120                         int src = edge->getSourceVertex();
00121                         tmpFaces[i].m_indices.push_back(src);
00122                         int targ = edge->getTargetVertex();
00123                         btVector3 wa = convexUtil->vertices[src];
00124 
00125                         btVector3 wb = convexUtil->vertices[targ];
00126                         btVector3 newEdge = wb-wa;
00127                         newEdge.normalize();
00128                         if (numEdges<2)
00129                                 edges[numEdges++] = newEdge;
00130 
00131                         edge = edge->getNextEdgeOfFace();
00132                 } while (edge!=firstEdge);
00133 
00134                 btScalar planeEq = 1e30f;
00135 
00136                 
00137                 if (numEdges==2)
00138                 {
00139                         faceNormals[i] = edges[0].cross(edges[1]);
00140                         faceNormals[i].normalize();
00141                         tmpFaces[i].m_plane[0] = faceNormals[i].getX();
00142                         tmpFaces[i].m_plane[1] = faceNormals[i].getY();
00143                         tmpFaces[i].m_plane[2] = faceNormals[i].getZ();
00144                         tmpFaces[i].m_plane[3] = planeEq;
00145 
00146                 }
00147                 else
00148                 {
00149                         btAssert(0);//degenerate?
00150                         faceNormals[i].setZero();
00151                 }
00152 
00153                 for (int v=0;v<tmpFaces[i].m_indices.size();v++)
00154                 {
00155                         btScalar eq = m_polyhedron->m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]);
00156                         if (planeEq>eq)
00157                         {
00158                                 planeEq=eq;
00159                         }
00160                 }
00161                 tmpFaces[i].m_plane[3] = -planeEq;
00162         }
00163 
00164         //merge coplanar faces and copy them to m_polyhedron
00165 
00166         btScalar faceWeldThreshold= 0.999f;
00167         btAlignedObjectArray<int> todoFaces;
00168         for (int i=0;i<tmpFaces.size();i++)
00169                 todoFaces.push_back(i);
00170 
00171         while (todoFaces.size())
00172         {
00173                 btAlignedObjectArray<int> coplanarFaceGroup;
00174                 int refFace = todoFaces[todoFaces.size()-1];
00175 
00176                 coplanarFaceGroup.push_back(refFace);
00177                 btFace& faceA = tmpFaces[refFace];
00178                 todoFaces.pop_back();
00179 
00180                 btVector3 faceNormalA(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]);
00181                 for (int j=todoFaces.size()-1;j>=0;j--)
00182                 {
00183                         int i = todoFaces[j];
00184                         btFace& faceB = tmpFaces[i];
00185                         btVector3 faceNormalB(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]);
00186                         if (faceNormalA.dot(faceNormalB)>faceWeldThreshold)
00187                         {
00188                                 coplanarFaceGroup.push_back(i);
00189                                 todoFaces.remove(i);
00190                         }
00191                 }
00192 
00193 
00194                 bool did_merge = false;
00195                 if (coplanarFaceGroup.size()>1)
00196                 {
00197                         //do the merge: use Graham Scan 2d convex hull
00198 
00199                         btAlignedObjectArray<GrahamVector3> orgpoints;
00200                         btVector3 averageFaceNormal(0,0,0);
00201 
00202                         for (int i=0;i<coplanarFaceGroup.size();i++)
00203                         {
00204 //                              m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
00205 
00206                                 btFace& face = tmpFaces[coplanarFaceGroup[i]];
00207                                 btVector3 faceNormal(face.m_plane[0],face.m_plane[1],face.m_plane[2]);
00208                                 averageFaceNormal+=faceNormal;
00209                                 for (int f=0;f<face.m_indices.size();f++)
00210                                 {
00211                                         int orgIndex = face.m_indices[f];
00212                                         btVector3 pt = m_polyhedron->m_vertices[orgIndex];
00213                                         
00214                                         bool found = false;
00215 
00216                                         for (int i=0;i<orgpoints.size();i++)
00217                                         {
00218                                                 //if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001))
00219                                                 if (orgpoints[i].m_orgIndex == orgIndex)
00220                                                 {
00221                                                         found=true;
00222                                                         break;
00223                                                 }
00224                                         }
00225                                         if (!found)
00226                                                 orgpoints.push_back(GrahamVector3(pt,orgIndex));
00227                                 }
00228                         }
00229 
00230                         
00231 
00232                         btFace combinedFace;
00233                         for (int i=0;i<4;i++)
00234                                 combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];
00235 
00236                         btAlignedObjectArray<GrahamVector3> hull;
00237 
00238                         averageFaceNormal.normalize();
00239                         GrahamScanConvexHull2D(orgpoints,hull,averageFaceNormal);
00240 
00241                         for (int i=0;i<hull.size();i++)
00242                         {
00243                                 combinedFace.m_indices.push_back(hull[i].m_orgIndex);
00244                                 for(int k = 0; k < orgpoints.size(); k++) 
00245                                 {
00246                                         if(orgpoints[k].m_orgIndex == hull[i].m_orgIndex) 
00247                                         {
00248                                                 orgpoints[k].m_orgIndex = -1; // invalidate...
00249                                                 break;
00250                                         }
00251                                 }
00252                         }
00253 
00254                         // are there rejected vertices?
00255                         bool reject_merge = false;
00256                         
00257 
00258 
00259                         for(int i = 0; i < orgpoints.size(); i++) {
00260                                 if(orgpoints[i].m_orgIndex == -1)
00261                                         continue; // this is in the hull...
00262                                 // this vertex is rejected -- is anybody else using this vertex?
00263                                 for(int j = 0; j < tmpFaces.size(); j++) {
00264                                         
00265                                         btFace& face = tmpFaces[j];
00266                                         // is this a face of the current coplanar group?
00267                                         bool is_in_current_group = false;
00268                                         for(int k = 0; k < coplanarFaceGroup.size(); k++) {
00269                                                 if(coplanarFaceGroup[k] == j) {
00270                                                         is_in_current_group = true;
00271                                                         break;
00272                                                 }
00273                                         }
00274                                         if(is_in_current_group) // ignore this face...
00275                                                 continue;
00276                                         // does this face use this rejected vertex?
00277                                         for(int v = 0; v < face.m_indices.size(); v++) {
00278                                                 if(face.m_indices[v] == orgpoints[i].m_orgIndex) {
00279                                                         // this rejected vertex is used in another face -- reject merge
00280                                                         reject_merge = true;
00281                                                         break;
00282                                                 }
00283                                         }
00284                                         if(reject_merge)
00285                                                 break;
00286                                 }
00287                                 if(reject_merge)
00288                                         break;
00289                         }
00290 
00291                         if (!reject_merge)
00292                         {
00293                                 // do this merge!
00294                                 did_merge = true;
00295                                 m_polyhedron->m_faces.push_back(combinedFace);
00296                         }
00297                 }
00298                 if(!did_merge)
00299                 {
00300                         for (int i=0;i<coplanarFaceGroup.size();i++)
00301                         {
00302                                 btFace face = tmpFaces[coplanarFaceGroup[i]];
00303                                 m_polyhedron->m_faces.push_back(face);
00304                         }
00305 
00306                 } 
00307 
00308 
00309 
00310         }
00311         
00312         m_polyhedron->initialize();
00313 
00314         return true;
00315 }
00316 
00317 #ifndef MIN
00318     #define MIN(_a, _b)     ((_a) < (_b) ? (_a) : (_b))
00319 #endif
00320 
00321 btVector3       btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
00322 {
00323 
00324 
00325         btVector3 supVec(0,0,0);
00326 #ifndef __SPU__
00327         int i;
00328         btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
00329 
00330         btVector3 vec = vec0;
00331         btScalar lenSqr = vec.length2();
00332         if (lenSqr < btScalar(0.0001))
00333         {
00334                 vec.setValue(1,0,0);
00335         } else
00336         {
00337                 btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
00338                 vec *= rlen;
00339         }
00340 
00341         btVector3 vtx;
00342         btScalar newDot;
00343 
00344     for( int k = 0; k < getNumVertices(); k += 128 )
00345     {
00346         btVector3 temp[128];
00347         int inner_count = MIN(getNumVertices() - k, 128);
00348         for( i = 0; i < inner_count; i++ )
00349             getVertex(i,temp[i]); 
00350         i = (int) vec.maxDot( temp, inner_count, newDot);
00351                 if (newDot > maxDot)
00352                 {
00353                         maxDot = newDot;
00354                         supVec = temp[i];
00355                 }        
00356     }
00357         
00358 #endif //__SPU__
00359         return supVec;
00360 }
00361 
00362 
00363 
00364 void    btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
00365 {
00366 #ifndef __SPU__
00367         int i;
00368 
00369         btVector3 vtx;
00370         btScalar newDot;
00371 
00372         for (i=0;i<numVectors;i++)
00373         {
00374                 supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
00375         }
00376 
00377         for (int j=0;j<numVectors;j++)
00378         {
00379         const btVector3& vec = vectors[j];
00380         
00381         for( int k = 0; k < getNumVertices(); k += 128 )
00382         {
00383             btVector3 temp[128];
00384             int inner_count = MIN(getNumVertices() - k, 128);
00385             for( i = 0; i < inner_count; i++ )
00386                 getVertex(i,temp[i]); 
00387             i = (int) vec.maxDot( temp, inner_count, newDot);
00388             if (newDot > supportVerticesOut[j][3])
00389             {
00390                                 supportVerticesOut[j] = temp[i];
00391                                 supportVerticesOut[j][3] = newDot;
00392             }        
00393         }
00394     }
00395 
00396 #endif //__SPU__
00397 }
00398 
00399 
00400 
00401 void    btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
00402 {
00403 #ifndef __SPU__
00404         //not yet, return box inertia
00405 
00406         btScalar margin = getMargin();
00407 
00408         btTransform ident;
00409         ident.setIdentity();
00410         btVector3 aabbMin,aabbMax;
00411         getAabb(ident,aabbMin,aabbMax);
00412         btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
00413 
00414         btScalar lx=btScalar(2.)*(halfExtents.x()+margin);
00415         btScalar ly=btScalar(2.)*(halfExtents.y()+margin);
00416         btScalar lz=btScalar(2.)*(halfExtents.z()+margin);
00417         const btScalar x2 = lx*lx;
00418         const btScalar y2 = ly*ly;
00419         const btScalar z2 = lz*lz;
00420         const btScalar scaledmass = mass * btScalar(0.08333333);
00421 
00422         inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
00423 #endif //__SPU__
00424 }
00425 
00426 
00427 
00428 void    btPolyhedralConvexAabbCachingShape::setLocalScaling(const btVector3& scaling)
00429 {
00430         btConvexInternalShape::setLocalScaling(scaling);
00431         recalcLocalAabb();
00432 }
00433 
00434 btPolyhedralConvexAabbCachingShape::btPolyhedralConvexAabbCachingShape()
00435 :btPolyhedralConvexShape(),
00436 m_localAabbMin(1,1,1),
00437 m_localAabbMax(-1,-1,-1),
00438 m_isLocalAabbValid(false)
00439 {
00440 }
00441 
00442 void btPolyhedralConvexAabbCachingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
00443 {
00444         getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin());
00445 }
00446 
00447 void    btPolyhedralConvexAabbCachingShape::recalcLocalAabb()
00448 {
00449         m_isLocalAabbValid = true;
00450         
00451         #if 1
00452         static const btVector3 _directions[] =
00453         {
00454                 btVector3( 1.,  0.,  0.),
00455                 btVector3( 0.,  1.,  0.),
00456                 btVector3( 0.,  0.,  1.),
00457                 btVector3( -1., 0.,  0.),
00458                 btVector3( 0., -1.,  0.),
00459                 btVector3( 0.,  0., -1.)
00460         };
00461         
00462         btVector3 _supporting[] =
00463         {
00464                 btVector3( 0., 0., 0.),
00465                 btVector3( 0., 0., 0.),
00466                 btVector3( 0., 0., 0.),
00467                 btVector3( 0., 0., 0.),
00468                 btVector3( 0., 0., 0.),
00469                 btVector3( 0., 0., 0.)
00470         };
00471         
00472         batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6);
00473         
00474         for ( int i = 0; i < 3; ++i )
00475         {
00476                 m_localAabbMax[i] = _supporting[i][i] + m_collisionMargin;
00477                 m_localAabbMin[i] = _supporting[i + 3][i] - m_collisionMargin;
00478         }
00479         
00480         #else
00481 
00482         for (int i=0;i<3;i++)
00483         {
00484                 btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
00485                 vec[i] = btScalar(1.);
00486                 btVector3 tmp = localGetSupportingVertex(vec);
00487                 m_localAabbMax[i] = tmp[i];
00488                 vec[i] = btScalar(-1.);
00489                 tmp = localGetSupportingVertex(vec);
00490                 m_localAabbMin[i] = tmp[i];
00491         }
00492         #endif
00493 }
00494 
00495 
00496 
00497