Page MenuHome
Differential D8762 Diff 28333 extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp

Changeset View

Standalone View

View Options

extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp

Show All 13 Lines
*/ */
#include "btConvexTriangleMeshShape.h" #include "btConvexTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btCollisionMargin.h" #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
#include "LinearMath/btQuaternion.h" #include "LinearMath/btQuaternion.h"
#include "BulletCollision/CollisionShapes/btStridingMeshInterface.h" #include "BulletCollision/CollisionShapes/btStridingMeshInterface.h"
btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface, bool calcAabb) btConvexTriangleMeshShape ::btConvexTriangleMeshShape(btStridingMeshInterface* meshInterface, bool calcAabb)
: btPolyhedralConvexAabbCachingShape(), m_stridingMesh(meshInterface) : btPolyhedralConvexAabbCachingShape(), m_stridingMesh(meshInterface)
{ {
m_shapeType = CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE; m_shapeType = CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE;
if ( calcAabb ) if (calcAabb)
recalcLocalAabb(); recalcLocalAabb();
} }
///It's not nice to have all this virtual function overhead, so perhaps we can also gather the points once ///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 ///but then we are duplicating
class LocalSupportVertexCallback: public btInternalTriangleIndexCallback class LocalSupportVertexCallback : public btInternalTriangleIndexCallback
{ {
btVector3 m_supportVertexLocal; btVector3 m_supportVertexLocal;
public:
public:
btScalar m_maxDot; btScalar m_maxDot;
btVector3 m_supportVecLocal; btVector3 m_supportVecLocal;
LocalSupportVertexCallback(const btVector3& supportVecLocal) LocalSupportVertexCallback(const btVector3& supportVecLocal)
: m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), : m_supportVertexLocal(btScalar(0.), btScalar(0.), btScalar(0.)),
m_maxDot(btScalar(-BT_LARGE_FLOAT)), m_maxDot(btScalar(-BT_LARGE_FLOAT)),
m_supportVecLocal(supportVecLocal) m_supportVecLocal(supportVecLocal)
{ {
} }
virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
{ {
(void)triangleIndex; (void)triangleIndex;
(void)partId; (void)partId;
for (int i=0;i<3;i++) for (int i = 0; i < 3; i++)
{ {
btScalar dot = m_supportVecLocal.dot(triangle[i]); btScalar dot = m_supportVecLocal.dot(triangle[i]);
if (dot > m_maxDot) if (dot > m_maxDot)
{ {
m_maxDot = dot; m_maxDot = dot;
m_supportVertexLocal = triangle[i]; m_supportVertexLocal = triangle[i];
} }
} }
} }
btVector3 GetSupportVertexLocal() btVector3 GetSupportVertexLocal()
{ {
return m_supportVertexLocal; return m_supportVertexLocal;
} }
}; };
btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const
{ {
btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); btVector3 supVec(btScalar(0.), btScalar(0.), btScalar(0.));
btVector3 vec = vec0; btVector3 vec = vec0;
btScalar lenSqr = vec.length2(); btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001)) if (lenSqr < btScalar(0.0001))
{ {
vec.setValue(1,0,0); vec.setValue(1, 0, 0);
} else }
else
{ {
btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
vec *= rlen; vec *= rlen;
} }
LocalSupportVertexCallback supportCallback(vec); LocalSupportVertexCallback supportCallback(vec);
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); m_stridingMesh->InternalProcessAllTriangles(&supportCallback, -aabbMax, aabbMax);
supVec = supportCallback.GetSupportVertexLocal(); supVec = supportCallback.GetSupportVertexLocal();
return supVec; return supVec;
} }
void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
{ {
//use 'w' component of supportVerticesOut? //use 'w' component of supportVerticesOut?
{ {
for (int i=0;i<numVectors;i++) for (int i = 0; i < numVectors; i++)
{ {
supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT); supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
} }
} }
///@todo: could do the batch inside the callback! ///@todo: could do the batch inside the callback!
for (int j=0;j<numVectors;j++) for (int j = 0; j < numVectors; j++)
{ {
const btVector3& vec = vectors[j]; const btVector3& vec = vectors[j];
LocalSupportVertexCallback supportCallback(vec); LocalSupportVertexCallback supportCallback(vec);
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); m_stridingMesh->InternalProcessAllTriangles(&supportCallback, -aabbMax, aabbMax);
supportVerticesOut[j] = supportCallback.GetSupportVertexLocal(); supportVerticesOut[j] = supportCallback.GetSupportVertexLocal();
} }
} }
btVector3 btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& vec)const btVector3 btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& vec) const
{ {
btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec);
if ( getMargin()!=btScalar(0.) ) if (getMargin() != btScalar(0.))
{ {
btVector3 vecnorm = vec; btVector3 vecnorm = vec;
if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON))
{ {
vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.));
} }
vecnorm.normalize(); vecnorm.normalize();
supVertex+= getMargin() * vecnorm; supVertex += getMargin() * vecnorm;
} }
return supVertex; return supVertex;
} }
//currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection //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 //Please note that you can debug-draw btConvexTriangleMeshShape with the Raytracer Demo
int btConvexTriangleMeshShape::getNumVertices() const int btConvexTriangleMeshShape::getNumVertices() const
{ {
//cache this? //cache this?
return 0; return 0;
} }
int btConvexTriangleMeshShape::getNumEdges() const int btConvexTriangleMeshShape::getNumEdges() const
{ {
return 0; return 0;
} }
void btConvexTriangleMeshShape::getEdge(int ,btVector3& ,btVector3& ) const void btConvexTriangleMeshShape::getEdge(int, btVector3&, btVector3&) const
{ {
btAssert(0); btAssert(0);
} }
void btConvexTriangleMeshShape::getVertex(int ,btVector3& ) const void btConvexTriangleMeshShape::getVertex(int, btVector3&) const
{ {
btAssert(0); btAssert(0);
} }
int btConvexTriangleMeshShape::getNumPlanes() const int btConvexTriangleMeshShape::getNumPlanes() const
{ {
return 0; return 0;
} }
void btConvexTriangleMeshShape::getPlane(btVector3& ,btVector3& ,int ) const void btConvexTriangleMeshShape::getPlane(btVector3&, btVector3&, int) const
{ {
btAssert(0); btAssert(0);
} }
//not yet //not yet
bool btConvexTriangleMeshShape::isInside(const btVector3& ,btScalar ) const bool btConvexTriangleMeshShape::isInside(const btVector3&, btScalar) const
{ {
btAssert(0); btAssert(0);
return false; return false;
} }
void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling) void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling)
{ {
m_stridingMesh->setScaling(scaling); m_stridingMesh->setScaling(scaling);
recalcLocalAabb(); recalcLocalAabb();
} }
const btVector3& btConvexTriangleMeshShape::getLocalScaling() const const btVector3& btConvexTriangleMeshShape::getLocalScaling() const
{ {
return m_stridingMesh->getScaling(); return m_stridingMesh->getScaling();
} }
void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const
{ {
class CenterCallback: public btInternalTriangleIndexCallback class CenterCallback : public btInternalTriangleIndexCallback
{ {
bool first; bool first;
btVector3 ref; btVector3 ref;
btVector3 sum; btVector3 sum;
btScalar volume; btScalar volume;
public: public:
CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0) CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0)
{ {
} }
virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
{ {
(void) triangleIndex; (void)triangleIndex;
(void) partId; (void)partId;
if (first) if (first)
{ {
ref = triangle[0]; ref = triangle[0];
first = false; first = false;
} }
else else
{ {
btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref)); 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)); sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref));
volume += vol; volume += vol;
} }
} }
btVector3 getCenter() btVector3 getCenter()
{ {
return (volume > 0) ? sum / volume : ref; return (volume > 0) ? sum / volume : ref;
} }
btScalar getVolume() btScalar getVolume()
{ {
return volume * btScalar(1. / 6); return volume * btScalar(1. / 6);
} }
}; };
class InertiaCallback: public btInternalTriangleIndexCallback class InertiaCallback : public btInternalTriangleIndexCallback
{ {
btMatrix3x3 sum; btMatrix3x3 sum;
btVector3 center; btVector3 center;
public: public:
InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center) InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center)
{ {
} }
virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
{ {
(void) triangleIndex; (void)triangleIndex;
(void) partId; (void)partId;
btMatrix3x3 i; btMatrix3x3 i;
btVector3 a = triangle[0] - center; btVector3 a = triangle[0] - center;
btVector3 b = triangle[1] - center; btVector3 b = triangle[1] - center;
btVector3 c = triangle[2] - center; btVector3 c = triangle[2] - center;
btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6); btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6);
for (int j = 0; j < 3; j++) for (int j = 0; j < 3; j++)
{ {
for (int k = 0; k <= j; k++) 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]) 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(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 i00 = -i[0][0];
btScalar i11 = -i[1][1]; btScalar i11 = -i[1][1];
btScalar i22 = -i[2][2]; btScalar i22 = -i[2][2];
i[0][0] = i11 + i22; i[0][0] = i11 + i22;
i[1][1] = i22 + i00; i[1][1] = i22 + i00;
i[2][2] = i00 + i11; i[2][2] = i00 + i11;
sum[0] += i[0]; sum[0] += i[0];
sum[1] += i[1]; sum[1] += i[1];
sum[2] += i[2]; sum[2] += i[2];
} }
btMatrix3x3& getInertia() btMatrix3x3& getInertia()
{ {
return sum; return sum;
} }
}; };
CenterCallback centerCallback; CenterCallback centerCallback;
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
m_stridingMesh->InternalProcessAllTriangles(&centerCallback, -aabbMax, aabbMax); m_stridingMesh->InternalProcessAllTriangles(&centerCallback, -aabbMax, aabbMax);
btVector3 center = centerCallback.getCenter(); btVector3 center = centerCallback.getCenter();
principal.setOrigin(center); principal.setOrigin(center);
volume = centerCallback.getVolume(); volume = centerCallback.getVolume();
InertiaCallback inertiaCallback(center); InertiaCallback inertiaCallback(center);
m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax); m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax);
btMatrix3x3& i = inertiaCallback.getInertia(); btMatrix3x3& i = inertiaCallback.getInertia();
i.diagonalize(principal.getBasis(), btScalar(0.00001), 20); i.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
inertia.setValue(i[0][0], i[1][1], i[2][2]); inertia.setValue(i[0][0], i[1][1], i[2][2]);
inertia /= volume; inertia /= volume;
} }