Bullet Collision Detection & Physics Library: btConvexTriangleMeshShape.cpp Source File

Bullet Collision Detection & Physics Library

btConvexTriangleMeshShape.cpp
Go to the documentation of this file.
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
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
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 }
CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE
@ CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE
Definition: btBroadphaseProxy.h:33
btCollisionMargin.h
btConvexTriangleMeshShape.h
btQuaternion.h
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
BT_LARGE_FLOAT
#define BT_LARGE_FLOAT
Definition: btScalar.h:316
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:497
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btAssert
#define btAssert(x)
Definition: btScalar.h:153
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition: btSoftBodyHelpers.cpp:94
btStridingMeshInterface.h
LocalSupportVertexCallback
It's not nice to have all this virtual function overhead, so perhaps we can also gather the points on...
Definition: btConvexTriangleMeshShape.cpp:33
LocalSupportVertexCallback::m_supportVertexLocal
btVector3 m_supportVertexLocal
Definition: btConvexTriangleMeshShape.cpp:34
LocalSupportVertexCallback::m_maxDot
btScalar m_maxDot
Definition: btConvexTriangleMeshShape.cpp:37
LocalSupportVertexCallback::GetSupportVertexLocal
btVector3 GetSupportVertexLocal()
Definition: btConvexTriangleMeshShape.cpp:63
LocalSupportVertexCallback::m_supportVecLocal
btVector3 m_supportVecLocal
Definition: btConvexTriangleMeshShape.cpp:38
LocalSupportVertexCallback::LocalSupportVertexCallback
LocalSupportVertexCallback(const btVector3 &supportVecLocal)
Definition: btConvexTriangleMeshShape.cpp:40
LocalSupportVertexCallback::internalProcessTriangleIndex
virtual void internalProcessTriangleIndex(btVector3 *triangle, int partId, int triangleIndex)
Definition: btConvexTriangleMeshShape.cpp:47
btCollisionShape::m_shapeType
int m_shapeType
Definition: btCollisionShape.h:30
btConvexInternalShape::getMargin
virtual btScalar getMargin() const
Definition: btConvexInternalShape.h:106
btConvexTriangleMeshShape::getNumVertices
virtual int getNumVertices() const
Definition: btConvexTriangleMeshShape.cpp:134
btConvexTriangleMeshShape::m_stridingMesh
class btStridingMeshInterface * m_stridingMesh
Definition: btConvexTriangleMeshShape.h:26
btConvexTriangleMeshShape::getLocalScaling
virtual const btVector3 & getLocalScaling() const
Definition: btConvexTriangleMeshShape.cpp:179
btConvexTriangleMeshShape::getNumPlanes
virtual int getNumPlanes() const
Definition: btConvexTriangleMeshShape.cpp:155
btConvexTriangleMeshShape::btConvexTriangleMeshShape
btConvexTriangleMeshShape(btStridingMeshInterface *meshInterface, bool calcAabb=true)
Definition: btConvexTriangleMeshShape.cpp:22
btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const
Definition: btConvexTriangleMeshShape.cpp:93
btConvexTriangleMeshShape::localGetSupportingVertex
virtual btVector3 localGetSupportingVertex(const btVector3 &vec) const
Definition: btConvexTriangleMeshShape.cpp:115
btConvexTriangleMeshShape::setLocalScaling
virtual void setLocalScaling(const btVector3 &scaling)
Definition: btConvexTriangleMeshShape.cpp:172
btConvexTriangleMeshShape::isInside
virtual bool isInside(const btVector3 &pt, btScalar tolerance) const
Definition: btConvexTriangleMeshShape.cpp:166
btConvexTriangleMeshShape::calculatePrincipalAxisTransform
void calculatePrincipalAxisTransform(btTransform &principal, btVector3 &inertia, btScalar &volume) const
computes the exact moment of inertia and the transform from the coordinate system defined by the prin...
Definition: btConvexTriangleMeshShape.cpp:184
btConvexTriangleMeshShape::getVertex
virtual void getVertex(int i, btVector3 &vtx) const
Definition: btConvexTriangleMeshShape.cpp:150
btConvexTriangleMeshShape::getNumEdges
virtual int getNumEdges() const
Definition: btConvexTriangleMeshShape.cpp:140
btConvexTriangleMeshShape::getEdge
virtual void getEdge(int i, btVector3 &pa, btVector3 &pb) const
Definition: btConvexTriangleMeshShape.cpp:145
btConvexTriangleMeshShape::getPlane
virtual void getPlane(btVector3 &planeNormal, btVector3 &planeSupport, int i) const
Definition: btConvexTriangleMeshShape.cpp:160
btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3 &vec) const
Definition: btConvexTriangleMeshShape.cpp:69
btInternalTriangleIndexCallback
Definition: btTriangleCallback.h:31
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
btMatrix3x3::diagonalize
void diagonalize(btMatrix3x3 &rot, btScalar threshold, int maxSteps)
diagonalizes this matrix by the Jacobi method.
Definition: btMatrix3x3.h:716
btPolyhedralConvexAabbCachingShape
The btPolyhedralConvexAabbCachingShape adds aabb caching to the btPolyhedralConvexShape.
Definition: btPolyhedralConvexShape.h:68
btPolyhedralConvexAabbCachingShape::recalcLocalAabb
void recalcLocalAabb()
Definition: btPolyhedralConvexShape.cpp:505
btStridingMeshInterface
The btStridingMeshInterface is the interface class for high performance generic access to triangle me...
Definition: btStridingMeshInterface.h:28
btStridingMeshInterface::getScaling
const btVector3 & getScaling() const
Definition: btStridingMeshInterface.h:80
btStridingMeshInterface::setScaling
void setScaling(const btVector3 &scaling)
Definition: btStridingMeshInterface.h:84
btStridingMeshInterface::InternalProcessAllTriangles
virtual void InternalProcessAllTriangles(btInternalTriangleIndexCallback *callback, const btVector3 &aabbMin, const btVector3 &aabbMax) const
Definition: btStridingMeshInterface.cpp:23
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
btTransform::setOrigin
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:146
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btVector3::triple
btScalar triple(const btVector3 &v1, const btVector3 &v2) const
Definition: btVector3.h:419
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251