dali-physics/third-party/bullet3/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp

   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 btConvexTriangleMeshShape ::btConvexTriangleMeshShape(btStridingMeshInterface* meshInterface, bool calcAabb)
  23         : btPolyhedralConvexAabbCachingShape(), m_stridingMesh(meshInterface)
  24 {
  25         m_shapeType = CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE;
  26         if (calcAabb)
  27                 recalcLocalAabb();
  28 }
  29
  30 ///It's not nice to have all this virtual function overhead, so perhaps we can also gather the points once
  31 ///but then we are duplicating
  32 class LocalSupportVertexCallback : public btInternalTriangleIndexCallback
  33 {
  34         btVector3 m_supportVertexLocal;
  35
  36 public:
  37         btScalar m_maxDot;
  38         btVector3 m_supportVecLocal;
  39
  40         LocalSupportVertexCallback(const btVector3& supportVecLocal)
  41                 : m_supportVertexLocal(btScalar(0.), btScalar(0.), btScalar(0.)),
  42                   m_maxDot(btScalar(-BT_LARGE_FLOAT)),
  43                   m_supportVecLocal(supportVecLocal)
  44         {
  45         }
  46
  47         virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
  48         {
  49                 (void)triangleIndex;
  50                 (void)partId;
  51
  52                 for (int i = 0; i < 3; i++)
  53                 {
  54                         btScalar dot = m_supportVecLocal.dot(triangle[i]);
  55                         if (dot > m_maxDot)
  56                         {
  57                                 m_maxDot = dot;
  58                                 m_supportVertexLocal = triangle[i];
  59                         }
  60                 }
  61         }
  62
  63         btVector3 GetSupportVertexLocal()
  64         {
  65                 return m_supportVertexLocal;
  66         }
  67 };
  68
  69 btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const
  70 {
  71         btVector3 supVec(btScalar(0.), btScalar(0.), btScalar(0.));
  72
  73         btVector3 vec = vec0;
  74         btScalar lenSqr = vec.length2();
  75         if (lenSqr < btScalar(0.0001))
  76         {
  77                 vec.setValue(1, 0, 0);
  78         }
  79         else
  80         {
  81                 btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
  82                 vec *= rlen;
  83         }
  84
  85         LocalSupportVertexCallback supportCallback(vec);
  86         btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
  87         m_stridingMesh->InternalProcessAllTriangles(&supportCallback, -aabbMax, aabbMax);
  88         supVec = supportCallback.GetSupportVertexLocal();
  89
  90         return supVec;
  91 }
  92
  93 void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
  94 {
  95         //use 'w' component of supportVerticesOut?
  96         {
  97                 for (int i = 0; i < numVectors; i++)
  98                 {
  99                         supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
 100                 }
 101         }
 102
 103         ///@todo: could do the batch inside the callback!
 104
 105         for (int j = 0; j < numVectors; j++)
 106         {
 107                 const btVector3& vec = vectors[j];
 108                 LocalSupportVertexCallback supportCallback(vec);
 109                 btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
 110                 m_stridingMesh->InternalProcessAllTriangles(&supportCallback, -aabbMax, aabbMax);
 111                 supportVerticesOut[j] = supportCallback.GetSupportVertexLocal();
 112         }
 113 }
 114
 115 btVector3 btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& vec) const
 116 {
 117         btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec);
 118
 119         if (getMargin() != btScalar(0.))
 120         {
 121                 btVector3 vecnorm = vec;
 122                 if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON))
 123                 {
 124                         vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.));
 125                 }
 126                 vecnorm.normalize();
 127                 supVertex += getMargin() * vecnorm;
 128         }
 129         return supVertex;
 130 }
 131
 132 //currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection
 133 //Please note that you can debug-draw btConvexTriangleMeshShape with the Raytracer Demo
 134 int btConvexTriangleMeshShape::getNumVertices() const
 135 {
 136         //cache this?
 137         return 0;
 138 }
 139
 140 int btConvexTriangleMeshShape::getNumEdges() const
 141 {
 142         return 0;
 143 }
 144
 145 void btConvexTriangleMeshShape::getEdge(int, btVector3&, btVector3&) const
 146 {
 147         btAssert(0);
 148 }
 149
 150 void btConvexTriangleMeshShape::getVertex(int, btVector3&) const
 151 {
 152         btAssert(0);
 153 }
 154
 155 int btConvexTriangleMeshShape::getNumPlanes() const
 156 {
 157         return 0;
 158 }
 159
 160 void btConvexTriangleMeshShape::getPlane(btVector3&, btVector3&, int) const
 161 {
 162         btAssert(0);
 163 }
 164
 165 //not yet
 166 bool btConvexTriangleMeshShape::isInside(const btVector3&, btScalar) const
 167 {
 168         btAssert(0);
 169         return false;
 170 }
 171
 172 void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling)
 173 {
 174         m_stridingMesh->setScaling(scaling);
 175
 176         recalcLocalAabb();
 177 }
 178
 179 const btVector3& btConvexTriangleMeshShape::getLocalScaling() const
 180 {
 181         return m_stridingMesh->getScaling();
 182 }
 183
 184 void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const
 185 {
 186         class CenterCallback : public btInternalTriangleIndexCallback
 187         {
 188                 bool first;
 189                 btVector3 ref;
 190                 btVector3 sum;
 191                 btScalar volume;
 192
 193         public:
 194                 CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0)
 195                 {
 196                 }
 197
 198                 virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
 199                 {
 200                         (void)triangleIndex;
 201                         (void)partId;
 202                         if (first)
 203                         {
 204                                 ref = triangle[0];
 205                                 first = false;
 206                         }
 207                         else
 208                         {
 209                                 btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref));
 210                                 sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref));
 211                                 volume += vol;
 212                         }
 213                 }
 214
 215                 btVector3 getCenter()
 216                 {
 217                         return (volume > 0) ? sum / volume : ref;
 218                 }
 219
 220                 btScalar getVolume()
 221                 {
 222                         return volume * btScalar(1. / 6);
 223                 }
 224         };
 225
 226         class InertiaCallback : public btInternalTriangleIndexCallback
 227         {
 228                 btMatrix3x3 sum;
 229                 btVector3 center;
 230
 231         public:
 232                 InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center)
 233                 {
 234                 }
 235
 236                 virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
 237                 {
 238                         (void)triangleIndex;
 239                         (void)partId;
 240                         btMatrix3x3 i;
 241                         btVector3 a = triangle[0] - center;
 242                         btVector3 b = triangle[1] - center;
 243                         btVector3 c = triangle[2] - center;
 244                         btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6);
 245                         for (int j = 0; j < 3; j++)
 246                         {
 247                                 for (int k = 0; k <= j; k++)
 248                                 {
 249                                         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]));
 250                                 }
 251                         }
 252                         btScalar i00 = -i[0][0];
 253                         btScalar i11 = -i[1][1];
 254                         btScalar i22 = -i[2][2];
 255                         i[0][0] = i11 + i22;
 256                         i[1][1] = i22 + i00;
 257                         i[2][2] = i00 + i11;
 258                         sum[0] += i[0];
 259                         sum[1] += i[1];
 260                         sum[2] += i[2];
 261                 }
 262
 263                 btMatrix3x3& getInertia()
 264                 {
 265                         return sum;
 266                 }
 267         };
 268
 269         CenterCallback centerCallback;
 270         btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
 271         m_stridingMesh->InternalProcessAllTriangles(&centerCallback, -aabbMax, aabbMax);
 272         btVector3 center = centerCallback.getCenter();
 273         principal.setOrigin(center);
 274         volume = centerCallback.getVolume();
 275
 276         InertiaCallback inertiaCallback(center);
 277         m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax);
 278
 279         btMatrix3x3& i = inertiaCallback.getInertia();
 280         i.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
 281         inertia.setValue(i[0][0], i[1][1], i[2][2]);
 282         inertia /= volume;
 283 }