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(btStridingMeshInterface * meshInterface,bool calcAabb)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
LocalSupportVertexCallback(const btVector3 & supportVecLocal)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
internalProcessTriangleIndex(btVector3 * triangle,int partId,int triangleIndex)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
GetSupportVertexLocal()68 btVector3 GetSupportVertexLocal()
69 {
70 return m_supportVertexLocal;
71 }
72
73 };
74
75
76
77
78
localGetSupportingVertexWithoutMargin(const btVector3 & vec0) const79 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
batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 * vectors,btVector3 * supportVerticesOut,int numVectors) const102 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
localGetSupportingVertex(const btVector3 & vec) const128 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
getNumVertices() const155 int btConvexTriangleMeshShape::getNumVertices() const
156 {
157 //cache this?
158 return 0;
159
160 }
161
getNumEdges() const162 int btConvexTriangleMeshShape::getNumEdges() const
163 {
164 return 0;
165 }
166
getEdge(int,btVector3 &,btVector3 &) const167 void btConvexTriangleMeshShape::getEdge(int ,btVector3& ,btVector3& ) const
168 {
169 btAssert(0);
170 }
171
getVertex(int,btVector3 &) const172 void btConvexTriangleMeshShape::getVertex(int ,btVector3& ) const
173 {
174 btAssert(0);
175 }
176
getNumPlanes() const177 int btConvexTriangleMeshShape::getNumPlanes() const
178 {
179 return 0;
180 }
181
getPlane(btVector3 &,btVector3 &,int) const182 void btConvexTriangleMeshShape::getPlane(btVector3& ,btVector3& ,int ) const
183 {
184 btAssert(0);
185 }
186
187 //not yet
isInside(const btVector3 &,btScalar) const188 bool btConvexTriangleMeshShape::isInside(const btVector3& ,btScalar ) const
189 {
190 btAssert(0);
191 return false;
192 }
193
194
195
setLocalScaling(const btVector3 & scaling)196 void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling)
197 {
198 m_stridingMesh->setScaling(scaling);
199
200 recalcLocalAabb();
201
202 }
203
204
getLocalScaling() const205 const btVector3& btConvexTriangleMeshShape::getLocalScaling() const
206 {
207 return m_stridingMesh->getScaling();
208 }
209
calculatePrincipalAxisTransform(btTransform & principal,btVector3 & inertia,btScalar & volume) const210 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(¢erCallback, -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
316