| /* |
| Bullet Continuous Collision Detection and Physics Library |
| Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org |
| |
| This software is provided 'as-is', without any express or implied warranty. |
| In no event will the authors be held liable for any damages arising from the use of this software. |
| Permission is granted to anyone to use this software for any purpose, |
| including commercial applications, and to alter it and redistribute it freely, |
| subject to the following restrictions: |
| |
| 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. |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. |
| 3. This notice may not be removed or altered from any source distribution. |
| */ |
| |
| #include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h" |
| #include "BulletCollision/CollisionShapes/btCollisionMargin.h" |
| |
| #include "LinearMath/btQuaternion.h" |
| #include "BulletCollision/CollisionShapes/btStridingMeshInterface.h" |
| |
| |
| btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface, bool calcAabb) |
| : btPolyhedralConvexAabbCachingShape(), m_stridingMesh(meshInterface) |
| { |
| m_shapeType = CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE; |
| if ( calcAabb ) |
| recalcLocalAabb(); |
| } |
| |
| |
| |
| |
| ///It's not nice to have all this virtual function overhead, so perhaps we can also gather the points once |
| ///but then we are duplicating |
| class LocalSupportVertexCallback: public btInternalTriangleIndexCallback |
| { |
| |
| btVector3 m_supportVertexLocal; |
| public: |
| |
| btScalar m_maxDot; |
| btVector3 m_supportVecLocal; |
| |
| LocalSupportVertexCallback(const btVector3& supportVecLocal) |
| : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), |
| m_maxDot(btScalar(-BT_LARGE_FLOAT)), |
| m_supportVecLocal(supportVecLocal) |
| { |
| } |
| |
| virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) |
| { |
| (void)triangleIndex; |
| (void)partId; |
| |
| for (int i=0;i<3;i++) |
| { |
| btScalar dot = m_supportVecLocal.dot(triangle[i]); |
| if (dot > m_maxDot) |
| { |
| m_maxDot = dot; |
| m_supportVertexLocal = triangle[i]; |
| } |
| } |
| } |
| |
| btVector3 GetSupportVertexLocal() |
| { |
| return m_supportVertexLocal; |
| } |
| |
| }; |
| |
| |
| |
| |
| |
| btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const |
| { |
| btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); |
| |
| btVector3 vec = vec0; |
| btScalar lenSqr = vec.length2(); |
| if (lenSqr < btScalar(0.0001)) |
| { |
| vec.setValue(1,0,0); |
| } else |
| { |
| btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); |
| vec *= rlen; |
| } |
| |
| LocalSupportVertexCallback supportCallback(vec); |
| btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); |
| m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); |
| supVec = supportCallback.GetSupportVertexLocal(); |
| |
| return supVec; |
| } |
| |
| void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const |
| { |
| //use 'w' component of supportVerticesOut? |
| { |
| for (int i=0;i<numVectors;i++) |
| { |
| supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT); |
| } |
| } |
| |
| ///@todo: could do the batch inside the callback! |
| |
| |
| for (int j=0;j<numVectors;j++) |
| { |
| const btVector3& vec = vectors[j]; |
| LocalSupportVertexCallback supportCallback(vec); |
| btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); |
| m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); |
| supportVerticesOut[j] = supportCallback.GetSupportVertexLocal(); |
| } |
| |
| } |
| |
| |
| |
| btVector3 btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& vec)const |
| { |
| btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); |
| |
| if ( getMargin()!=btScalar(0.) ) |
| { |
| btVector3 vecnorm = vec; |
| if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) |
| { |
| vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); |
| } |
| vecnorm.normalize(); |
| supVertex+= getMargin() * vecnorm; |
| } |
| return supVertex; |
| } |
| |
| |
| |
| |
| |
| |
| |
| |
| |
| //currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection |
| //Please note that you can debug-draw btConvexTriangleMeshShape with the Raytracer Demo |
| int btConvexTriangleMeshShape::getNumVertices() const |
| { |
| //cache this? |
| return 0; |
| |
| } |
| |
| int btConvexTriangleMeshShape::getNumEdges() const |
| { |
| return 0; |
| } |
| |
| void btConvexTriangleMeshShape::getEdge(int ,btVector3& ,btVector3& ) const |
| { |
| btAssert(0); |
| } |
| |
| void btConvexTriangleMeshShape::getVertex(int ,btVector3& ) const |
| { |
| btAssert(0); |
| } |
| |
| int btConvexTriangleMeshShape::getNumPlanes() const |
| { |
| return 0; |
| } |
| |
| void btConvexTriangleMeshShape::getPlane(btVector3& ,btVector3& ,int ) const |
| { |
| btAssert(0); |
| } |
| |
| //not yet |
| bool btConvexTriangleMeshShape::isInside(const btVector3& ,btScalar ) const |
| { |
| btAssert(0); |
| return false; |
| } |
| |
| |
| |
| void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling) |
| { |
| m_stridingMesh->setScaling(scaling); |
| |
| recalcLocalAabb(); |
| |
| } |
| |
| |
| const btVector3& btConvexTriangleMeshShape::getLocalScaling() const |
| { |
| return m_stridingMesh->getScaling(); |
| } |
| |
| void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const |
| { |
| class CenterCallback: public btInternalTriangleIndexCallback |
| { |
| bool first; |
| btVector3 ref; |
| btVector3 sum; |
| btScalar volume; |
| |
| public: |
| |
| CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0) |
| { |
| } |
| |
| virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) |
| { |
| (void) triangleIndex; |
| (void) partId; |
| if (first) |
| { |
| ref = triangle[0]; |
| first = false; |
| } |
| else |
| { |
| btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref)); |
| sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref)); |
| volume += vol; |
| } |
| } |
| |
| btVector3 getCenter() |
| { |
| return (volume > 0) ? sum / volume : ref; |
| } |
| |
| btScalar getVolume() |
| { |
| return volume * btScalar(1. / 6); |
| } |
| |
| }; |
| |
| class InertiaCallback: public btInternalTriangleIndexCallback |
| { |
| btMatrix3x3 sum; |
| btVector3 center; |
| |
| public: |
| |
| InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center) |
| { |
| } |
| |
| virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) |
| { |
| (void) triangleIndex; |
| (void) partId; |
| btMatrix3x3 i; |
| btVector3 a = triangle[0] - center; |
| btVector3 b = triangle[1] - center; |
| btVector3 c = triangle[2] - center; |
| btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6); |
| for (int j = 0; j < 3; j++) |
| { |
| for (int k = 0; k <= j; k++) |
| { |
| i[j][k] = i[k][j] = volNeg * (btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k]) |
| + 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])); |
| } |
| } |
| btScalar i00 = -i[0][0]; |
| btScalar i11 = -i[1][1]; |
| btScalar i22 = -i[2][2]; |
| i[0][0] = i11 + i22; |
| i[1][1] = i22 + i00; |
| i[2][2] = i00 + i11; |
| sum[0] += i[0]; |
| sum[1] += i[1]; |
| sum[2] += i[2]; |
| } |
| |
| btMatrix3x3& getInertia() |
| { |
| return sum; |
| } |
| |
| }; |
| |
| CenterCallback centerCallback; |
| btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); |
| m_stridingMesh->InternalProcessAllTriangles(¢erCallback, -aabbMax, aabbMax); |
| btVector3 center = centerCallback.getCenter(); |
| principal.setOrigin(center); |
| volume = centerCallback.getVolume(); |
| |
| InertiaCallback inertiaCallback(center); |
| m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax); |
| |
| btMatrix3x3& i = inertiaCallback.getInertia(); |
| i.diagonalize(principal.getBasis(), btScalar(0.00001), 20); |
| inertia.setValue(i[0][0], i[1][1], i[2][2]); |
| inertia /= volume; |
| } |
| |