← Back to branch summary

~ubuntu-branches/ubuntu/vivid/emscripten/vivid

« back to all changes in this revision

Viewing changes to tests/bullet/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp

  • Committer: Package Import Robot
  • Author(s): Sylvestre Ledru
  • Date: 2013-05-02 13:11:51 UTC
  • Revision ID: package-import@ubuntu.com-20130502131151-q8dvteqr1ef2x7xz
Tags: upstream-1.4.1~20130504~adb56cb
Import upstream version 1.4.1~20130504~adb56cb

Show diffs side-by-side

added added

removed removed

Lines of Context:
tests/bullet/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
 
 
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(&centerCallback, -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