00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
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
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
00110 const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
00111 const btConvexHullComputer::Edge* edge = firstEdge;
00112
00113 btVector3 edges[3];
00114 int numEdges = 0;
00115
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);
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
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
00198
00199 btAlignedObjectArray<GrahamVector3> orgpoints;
00200 btVector3 averageFaceNormal(0,0,0);
00201
00202 for (int i=0;i<coplanarFaceGroup.size();i++)
00203 {
00204
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
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;
00249 break;
00250 }
00251 }
00252 }
00253
00254
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;
00262
00263 for(int j = 0; j < tmpFaces.size(); j++) {
00264
00265 btFace& face = tmpFaces[j];
00266
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)
00275 continue;
00276
00277 for(int v = 0; v < face.m_indices.size(); v++) {
00278 if(face.m_indices[v] == orgpoints[i].m_orgIndex) {
00279
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
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
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