btConvexTriangleMeshShape.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 "btConvexTriangleMeshShape.h"
00017 #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
00018 
00019 #include "LinearMath/btQuaternion.h"
00020 #include "BulletCollision/CollisionShapes/btStridingMeshInterface.h"
00021 
00022 
00023 btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface, bool calcAabb)
00024 : btPolyhedralConvexAabbCachingShape(), m_stridingMesh(meshInterface)
00025 {
00026         m_shapeType = CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE;
00027         if ( calcAabb )
00028                 recalcLocalAabb();
00029 }
00030 
00031 
00032 
00033 
00036 class LocalSupportVertexCallback: public btInternalTriangleIndexCallback
00037 {
00038 
00039         btVector3 m_supportVertexLocal;
00040 public:
00041 
00042         btScalar m_maxDot;
00043         btVector3 m_supportVecLocal;
00044 
00045         LocalSupportVertexCallback(const btVector3& supportVecLocal)
00046                 : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)),
00047                 m_maxDot(btScalar(-BT_LARGE_FLOAT)),
00048                 m_supportVecLocal(supportVecLocal)
00049         {
00050         }
00051 
00052         virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
00053         {
00054                 (void)triangleIndex;
00055                 (void)partId;
00056 
00057                 for (int i=0;i<3;i++)
00058                 {
00059                         btScalar dot = m_supportVecLocal.dot(triangle[i]);
00060                         if (dot > m_maxDot)
00061                         {
00062                                 m_maxDot = dot;
00063                                 m_supportVertexLocal = triangle[i];
00064                         }
00065                 }
00066         }
00067         
00068         btVector3       GetSupportVertexLocal()
00069         {
00070                 return m_supportVertexLocal;
00071         }
00072 
00073 };
00074 
00075 
00076 
00077 
00078 
00079 btVector3       btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
00080 {
00081         btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
00082 
00083         btVector3 vec = vec0;
00084         btScalar lenSqr = vec.length2();
00085         if (lenSqr < btScalar(0.0001))
00086         {
00087                 vec.setValue(1,0,0);
00088         } else
00089         {
00090                 btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
00091                 vec *= rlen;
00092         }
00093 
00094         LocalSupportVertexCallback      supportCallback(vec);
00095         btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
00096         m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
00097         supVec = supportCallback.GetSupportVertexLocal();
00098 
00099         return supVec;
00100 }
00101 
00102 void    btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
00103 {
00104         //use 'w' component of supportVerticesOut?
00105         {
00106                 for (int i=0;i<numVectors;i++)
00107                 {
00108                         supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
00109                 }
00110         }
00111         
00113 
00114 
00115         for (int j=0;j<numVectors;j++)
00116         {
00117                 const btVector3& vec = vectors[j];
00118                 LocalSupportVertexCallback      supportCallback(vec);
00119                 btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
00120                 m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
00121                 supportVerticesOut[j] = supportCallback.GetSupportVertexLocal();
00122         }
00123         
00124 }
00125         
00126 
00127 
00128 btVector3       btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& vec)const
00129 {
00130         btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec);
00131 
00132         if ( getMargin()!=btScalar(0.) )
00133         {
00134                 btVector3 vecnorm = vec;
00135                 if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
00136                 {
00137                         vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
00138                 } 
00139                 vecnorm.normalize();
00140                 supVertex+= getMargin() * vecnorm;
00141         }
00142         return supVertex;
00143 }
00144 
00145 
00146 
00147 
00148 
00149 
00150 
00151 
00152 
00153 //currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection
00154 //Please note that you can debug-draw btConvexTriangleMeshShape with the Raytracer Demo
00155 int     btConvexTriangleMeshShape::getNumVertices() const
00156 {
00157         //cache this?
00158         return 0;
00159         
00160 }
00161 
00162 int btConvexTriangleMeshShape::getNumEdges() const
00163 {
00164         return 0;
00165 }
00166 
00167 void btConvexTriangleMeshShape::getEdge(int ,btVector3& ,btVector3& ) const
00168 {
00169         btAssert(0);    
00170 }
00171 
00172 void btConvexTriangleMeshShape::getVertex(int ,btVector3& ) const
00173 {
00174         btAssert(0);
00175 }
00176 
00177 int     btConvexTriangleMeshShape::getNumPlanes() const
00178 {
00179         return 0;
00180 }
00181 
00182 void btConvexTriangleMeshShape::getPlane(btVector3& ,btVector3& ,int  ) const
00183 {
00184         btAssert(0);
00185 }
00186 
00187 //not yet
00188 bool btConvexTriangleMeshShape::isInside(const btVector3& ,btScalar ) const
00189 {
00190         btAssert(0);
00191         return false;
00192 }
00193 
00194 
00195 
00196 void    btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling)
00197 {
00198         m_stridingMesh->setScaling(scaling);
00199         
00200         recalcLocalAabb();
00201         
00202 }
00203 
00204 
00205 const btVector3& btConvexTriangleMeshShape::getLocalScaling() const
00206 {
00207         return m_stridingMesh->getScaling();
00208 }
00209 
00210 void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const
00211 {
00212    class CenterCallback: public btInternalTriangleIndexCallback
00213    {
00214       bool first;
00215       btVector3 ref;
00216       btVector3 sum;
00217       btScalar volume;
00218 
00219    public:
00220 
00221       CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0)
00222       {
00223       }
00224 
00225       virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
00226       {
00227          (void) triangleIndex;
00228          (void) partId;
00229          if (first)
00230          {
00231             ref = triangle[0];
00232             first = false;
00233          }
00234          else
00235          {
00236             btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref));
00237             sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref));
00238             volume += vol;
00239          }
00240       }
00241       
00242       btVector3 getCenter()
00243       {
00244          return (volume > 0) ? sum / volume : ref;
00245       }
00246 
00247       btScalar getVolume()
00248       {
00249          return volume * btScalar(1. / 6);
00250       }
00251 
00252    };
00253 
00254    class InertiaCallback: public btInternalTriangleIndexCallback
00255    {
00256       btMatrix3x3 sum;
00257       btVector3 center;
00258 
00259    public:
00260 
00261       InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center)
00262       {
00263       }
00264 
00265       virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
00266       {
00267          (void) triangleIndex;
00268          (void) partId;
00269          btMatrix3x3 i;
00270          btVector3 a = triangle[0] - center;
00271          btVector3 b = triangle[1] - center;
00272          btVector3 c = triangle[2] - center;
00273          btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6);
00274          for (int j = 0; j < 3; j++)
00275          {
00276             for (int k = 0; k <= j; k++)
00277             {
00278                i[j][k] = i[k][j] = volNeg * (btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k])
00279                   + btScalar(0.05) * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j]));
00280             }
00281          }
00282          btScalar i00 = -i[0][0];
00283          btScalar i11 = -i[1][1];
00284          btScalar i22 = -i[2][2];
00285          i[0][0] = i11 + i22; 
00286          i[1][1] = i22 + i00; 
00287          i[2][2] = i00 + i11;
00288          sum[0] += i[0];
00289          sum[1] += i[1];
00290          sum[2] += i[2];
00291       }
00292       
00293       btMatrix3x3& getInertia()
00294       {
00295          return sum;
00296       }
00297 
00298    };
00299 
00300    CenterCallback centerCallback;
00301    btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
00302    m_stridingMesh->InternalProcessAllTriangles(&centerCallback, -aabbMax, aabbMax);
00303    btVector3 center = centerCallback.getCenter();
00304    principal.setOrigin(center);
00305    volume = centerCallback.getVolume();
00306 
00307    InertiaCallback inertiaCallback(center);
00308    m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax);
00309 
00310    btMatrix3x3& i = inertiaCallback.getInertia();
00311    i.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
00312    inertia.setValue(i[0][0], i[1][1], i[2][2]);
00313    inertia /= volume;
00314 }
00315