1 /* 2 Bullet Continuous Collision Detection and Physics Library 3 Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org 4 5 This software is provided 'as-is', without any express or implied warranty. 6 In no event will the authors be held liable for any damages arising from the use of this software. 7 Permission is granted to anyone to use this software for any purpose, 8 including commercial applications, and to alter it and redistribute it freely, 9 subject to the following restrictions: 10 11 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. 12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 13 3. This notice may not be removed or altered from any source distribution. 14 */ 15 16 #include "btConvexTriangleMeshShape.h" 17 #include "BulletCollision/CollisionShapes/btCollisionMargin.h" 18 19 #include "LinearMath/btQuaternion.h" 20 #include "BulletCollision/CollisionShapes/btStridingMeshInterface.h" 21 22 23 btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface, bool calcAabb) 24 : btPolyhedralConvexAabbCachingShape(), m_stridingMesh(meshInterface) 25 { 26 m_shapeType = CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE; 27 if ( calcAabb ) 28 recalcLocalAabb(); 29 } 30 31 32 33 34 ///It's not nice to have all this virtual function overhead, so perhaps we can also gather the points once 35 ///but then we are duplicating 36 class LocalSupportVertexCallback: public btInternalTriangleIndexCallback 37 { 38 39 btVector3 m_supportVertexLocal; 40 public: 41 42 btScalar m_maxDot; 43 btVector3 m_supportVecLocal; 44 45 LocalSupportVertexCallback(const btVector3& supportVecLocal) 46 : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), 47 m_maxDot(btScalar(-BT_LARGE_FLOAT)), 48 m_supportVecLocal(supportVecLocal) 49 { 50 } 51 52 virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) 53 { 54 (void)triangleIndex; 55 (void)partId; 56 57 for (int i=0;i<3;i++) 58 { 59 btScalar dot = m_supportVecLocal.dot(triangle[i]); 60 if (dot > m_maxDot) 61 { 62 m_maxDot = dot; 63 m_supportVertexLocal = triangle[i]; 64 } 65 } 66 } 67 68 btVector3 GetSupportVertexLocal() 69 { 70 return m_supportVertexLocal; 71 } 72 73 }; 74 75 76 77 78 79 btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const 80 { 81 btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); 82 83 btVector3 vec = vec0; 84 btScalar lenSqr = vec.length2(); 85 if (lenSqr < btScalar(0.0001)) 86 { 87 vec.setValue(1,0,0); 88 } else 89 { 90 btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); 91 vec *= rlen; 92 } 93 94 LocalSupportVertexCallback supportCallback(vec); 95 btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); 96 m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); 97 supVec = supportCallback.GetSupportVertexLocal(); 98 99 return supVec; 100 } 101 102 void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const 103 { 104 //use 'w' component of supportVerticesOut? 105 { 106 for (int i=0;i<numVectors;i++) 107 { 108 supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT); 109 } 110 } 111 112 ///@todo: could do the batch inside the callback! 113 114 115 for (int j=0;j<numVectors;j++) 116 { 117 const btVector3& vec = vectors[j]; 118 LocalSupportVertexCallback supportCallback(vec); 119 btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); 120 m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); 121 supportVerticesOut[j] = supportCallback.GetSupportVertexLocal(); 122 } 123 124 } 125 126 127 128 btVector3 btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& vec)const 129 { 130 btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); 131 132 if ( getMargin()!=btScalar(0.) ) 133 { 134 btVector3 vecnorm = vec; 135 if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) 136 { 137 vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); 138 } 139 vecnorm.normalize(); 140 supVertex+= getMargin() * vecnorm; 141 } 142 return supVertex; 143 } 144 145 146 147 148 149 150 151 152 153 //currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection 154 //Please note that you can debug-draw btConvexTriangleMeshShape with the Raytracer Demo 155 int btConvexTriangleMeshShape::getNumVertices() const 156 { 157 //cache this? 158 return 0; 159 160 } 161 162 int btConvexTriangleMeshShape::getNumEdges() const 163 { 164 return 0; 165 } 166 167 void btConvexTriangleMeshShape::getEdge(int ,btVector3& ,btVector3& ) const 168 { 169 btAssert(0); 170 } 171 172 void btConvexTriangleMeshShape::getVertex(int ,btVector3& ) const 173 { 174 btAssert(0); 175 } 176 177 int btConvexTriangleMeshShape::getNumPlanes() const 178 { 179 return 0; 180 } 181 182 void btConvexTriangleMeshShape::getPlane(btVector3& ,btVector3& ,int ) const 183 { 184 btAssert(0); 185 } 186 187 //not yet 188 bool btConvexTriangleMeshShape::isInside(const btVector3& ,btScalar ) const 189 { 190 btAssert(0); 191 return false; 192 } 193 194 195 196 void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling) 197 { 198 m_stridingMesh->setScaling(scaling); 199 200 recalcLocalAabb(); 201 202 } 203 204 205 const btVector3& btConvexTriangleMeshShape::getLocalScaling() const 206 { 207 return m_stridingMesh->getScaling(); 208 } 209 210 void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const 211 { 212 class CenterCallback: public btInternalTriangleIndexCallback 213 { 214 bool first; 215 btVector3 ref; 216 btVector3 sum; 217 btScalar volume; 218 219 public: 220 221 CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0) 222 { 223 } 224 225 virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) 226 { 227 (void) triangleIndex; 228 (void) partId; 229 if (first) 230 { 231 ref = triangle[0]; 232 first = false; 233 } 234 else 235 { 236 btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref)); 237 sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref)); 238 volume += vol; 239 } 240 } 241 242 btVector3 getCenter() 243 { 244 return (volume > 0) ? sum / volume : ref; 245 } 246 247 btScalar getVolume() 248 { 249 return volume * btScalar(1. / 6); 250 } 251 252 }; 253 254 class InertiaCallback: public btInternalTriangleIndexCallback 255 { 256 btMatrix3x3 sum; 257 btVector3 center; 258 259 public: 260 261 InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center) 262 { 263 } 264 265 virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) 266 { 267 (void) triangleIndex; 268 (void) partId; 269 btMatrix3x3 i; 270 btVector3 a = triangle[0] - center; 271 btVector3 b = triangle[1] - center; 272 btVector3 c = triangle[2] - center; 273 btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6); 274 for (int j = 0; j < 3; j++) 275 { 276 for (int k = 0; k <= j; k++) 277 { 278 i[j][k] = i[k][j] = volNeg * (btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k]) 279 + 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])); 280 } 281 } 282 btScalar i00 = -i[0][0]; 283 btScalar i11 = -i[1][1]; 284 btScalar i22 = -i[2][2]; 285 i[0][0] = i11 + i22; 286 i[1][1] = i22 + i00; 287 i[2][2] = i00 + i11; 288 sum[0] += i[0]; 289 sum[1] += i[1]; 290 sum[2] += i[2]; 291 } 292 293 btMatrix3x3& getInertia() 294 { 295 return sum; 296 } 297 298 }; 299 300 CenterCallback centerCallback; 301 btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); 302 m_stridingMesh->InternalProcessAllTriangles(¢erCallback, -aabbMax, aabbMax); 303 btVector3 center = centerCallback.getCenter(); 304 principal.setOrigin(center); 305 volume = centerCallback.getVolume(); 306 307 InertiaCallback inertiaCallback(center); 308 m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax); 309 310 btMatrix3x3& i = inertiaCallback.getInertia(); 311 i.diagonalize(principal.getBasis(), btScalar(0.00001), 20); 312 inertia.setValue(i[0][0], i[1][1], i[2][2]); 313 inertia /= volume; 314 } 315 316
