00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
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
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
00154
00155 int btConvexTriangleMeshShape::getNumVertices() const
00156 {
00157
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
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(¢erCallback, -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