Bullet Collision Detection & Physics Library
btPolyhedralConvexShape.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15#if defined (_WIN32) || defined (__i386__)
16#define BT_USE_SSE_IN_API
17#endif
18
20#include "btConvexPolyhedron.h"
22#include <new>
25
26
28m_polyhedron(0)
29{
30
31}
32
34{
35 if (m_polyhedron)
36 {
39 }
40}
41
42
44{
45
46 if (m_polyhedron)
47 {
50 }
51
52 void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16);
54
56
57 for (int i=0;i<getNumVertices();i++)
58 {
59 btVector3& newVertex = orgVertices.expand();
60 getVertex(i,newVertex);
61 }
62
64
65 if (shiftVerticesByMargin)
66 {
68 btGeometryUtil::getPlaneEquationsFromVertices(orgVertices,planeEquations);
69
70 btAlignedObjectArray<btVector3> shiftedPlaneEquations;
71 for (int p=0;p<planeEquations.size();p++)
72 {
73 btVector3 plane = planeEquations[p];
74 // btScalar margin = getMargin();
75 plane[3] -= getMargin();
76 shiftedPlaneEquations.push_back(plane);
77 }
78
80
81 btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,tmpVertices);
82
83 conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f);
84 } else
85 {
86
87 conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f);
88 }
89
90
91
93 int numFaces = conv.faces.size();
94 faceNormals.resize(numFaces);
95 btConvexHullComputer* convexUtil = &conv;
96
97
99 tmpFaces.resize(numFaces);
100
101 int numVertices = convexUtil->vertices.size();
102 m_polyhedron->m_vertices.resize(numVertices);
103 for (int p=0;p<numVertices;p++)
104 {
105 m_polyhedron->m_vertices[p] = convexUtil->vertices[p];
106 }
107
108
109 for (int i=0;i<numFaces;i++)
110 {
111 int face = convexUtil->faces[i];
112 //printf("face=%d\n",face);
113 const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
114 const btConvexHullComputer::Edge* edge = firstEdge;
115
116 btVector3 edges[3];
117 int numEdges = 0;
118 //compute face normals
119
120 do
121 {
122
123 int src = edge->getSourceVertex();
124 tmpFaces[i].m_indices.push_back(src);
125 int targ = edge->getTargetVertex();
126 btVector3 wa = convexUtil->vertices[src];
127
128 btVector3 wb = convexUtil->vertices[targ];
129 btVector3 newEdge = wb-wa;
130 newEdge.normalize();
131 if (numEdges<2)
132 edges[numEdges++] = newEdge;
133
134 edge = edge->getNextEdgeOfFace();
135 } while (edge!=firstEdge);
136
137 btScalar planeEq = 1e30f;
138
139
140 if (numEdges==2)
141 {
142 faceNormals[i] = edges[0].cross(edges[1]);
143 faceNormals[i].normalize();
144 tmpFaces[i].m_plane[0] = faceNormals[i].getX();
145 tmpFaces[i].m_plane[1] = faceNormals[i].getY();
146 tmpFaces[i].m_plane[2] = faceNormals[i].getZ();
147 tmpFaces[i].m_plane[3] = planeEq;
148
149 }
150 else
151 {
152 btAssert(0);//degenerate?
153 faceNormals[i].setZero();
154 }
155
156 for (int v=0;v<tmpFaces[i].m_indices.size();v++)
157 {
158 btScalar eq = m_polyhedron->m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]);
159 if (planeEq>eq)
160 {
161 planeEq=eq;
162 }
163 }
164 tmpFaces[i].m_plane[3] = -planeEq;
165 }
166
167 //merge coplanar faces and copy them to m_polyhedron
168
169 btScalar faceWeldThreshold= 0.999f;
171 for (int i=0;i<tmpFaces.size();i++)
172 todoFaces.push_back(i);
173
174 while (todoFaces.size())
175 {
176 btAlignedObjectArray<int> coplanarFaceGroup;
177 int refFace = todoFaces[todoFaces.size()-1];
178
179 coplanarFaceGroup.push_back(refFace);
180 btFace& faceA = tmpFaces[refFace];
181 todoFaces.pop_back();
182
183 btVector3 faceNormalA(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]);
184 for (int j=todoFaces.size()-1;j>=0;j--)
185 {
186 int i = todoFaces[j];
187 btFace& faceB = tmpFaces[i];
188 btVector3 faceNormalB(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]);
189 if (faceNormalA.dot(faceNormalB)>faceWeldThreshold)
190 {
191 coplanarFaceGroup.push_back(i);
192 todoFaces.remove(i);
193 }
194 }
195
196
197 bool did_merge = false;
198 if (coplanarFaceGroup.size()>1)
199 {
200 //do the merge: use Graham Scan 2d convex hull
201
203 btVector3 averageFaceNormal(0,0,0);
204
205 for (int i=0;i<coplanarFaceGroup.size();i++)
206 {
207// m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
208
209 btFace& face = tmpFaces[coplanarFaceGroup[i]];
210 btVector3 faceNormal(face.m_plane[0],face.m_plane[1],face.m_plane[2]);
211 averageFaceNormal+=faceNormal;
212 for (int f=0;f<face.m_indices.size();f++)
213 {
214 int orgIndex = face.m_indices[f];
215 btVector3 pt = m_polyhedron->m_vertices[orgIndex];
216
217 bool found = false;
218
219 for (int i=0;i<orgpoints.size();i++)
220 {
221 //if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001))
222 if (orgpoints[i].m_orgIndex == orgIndex)
223 {
224 found=true;
225 break;
226 }
227 }
228 if (!found)
229 orgpoints.push_back(GrahamVector3(pt,orgIndex));
230 }
231 }
232
233
234
235 btFace combinedFace;
236 for (int i=0;i<4;i++)
237 combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];
238
240
241 averageFaceNormal.normalize();
242 GrahamScanConvexHull2D(orgpoints,hull,averageFaceNormal);
243
244 for (int i=0;i<hull.size();i++)
245 {
246 combinedFace.m_indices.push_back(hull[i].m_orgIndex);
247 for(int k = 0; k < orgpoints.size(); k++)
248 {
249 if(orgpoints[k].m_orgIndex == hull[i].m_orgIndex)
250 {
251 orgpoints[k].m_orgIndex = -1; // invalidate...
252 break;
253 }
254 }
255 }
256
257 // are there rejected vertices?
258 bool reject_merge = false;
259
260
261
262 for(int i = 0; i < orgpoints.size(); i++) {
263 if(orgpoints[i].m_orgIndex == -1)
264 continue; // this is in the hull...
265 // this vertex is rejected -- is anybody else using this vertex?
266 for(int j = 0; j < tmpFaces.size(); j++) {
267
268 btFace& face = tmpFaces[j];
269 // is this a face of the current coplanar group?
270 bool is_in_current_group = false;
271 for(int k = 0; k < coplanarFaceGroup.size(); k++) {
272 if(coplanarFaceGroup[k] == j) {
273 is_in_current_group = true;
274 break;
275 }
276 }
277 if(is_in_current_group) // ignore this face...
278 continue;
279 // does this face use this rejected vertex?
280 for(int v = 0; v < face.m_indices.size(); v++) {
281 if(face.m_indices[v] == orgpoints[i].m_orgIndex) {
282 // this rejected vertex is used in another face -- reject merge
283 reject_merge = true;
284 break;
285 }
286 }
287 if(reject_merge)
288 break;
289 }
290 if(reject_merge)
291 break;
292 }
293
294 if (!reject_merge)
295 {
296 // do this merge!
297 did_merge = true;
298 m_polyhedron->m_faces.push_back(combinedFace);
299 }
300 }
301 if(!did_merge)
302 {
303 for (int i=0;i<coplanarFaceGroup.size();i++)
304 {
305 btFace face = tmpFaces[coplanarFaceGroup[i]];
306 m_polyhedron->m_faces.push_back(face);
307 }
308
309 }
310
311
312
313 }
314
316
317 return true;
318}
319
320#ifndef MIN
321 #define MIN(_a, _b) ((_a) < (_b) ? (_a) : (_b))
322#endif
323
325{
326
327
328 btVector3 supVec(0,0,0);
329#ifndef __SPU__
330 int i;
332
333 btVector3 vec = vec0;
334 btScalar lenSqr = vec.length2();
335 if (lenSqr < btScalar(0.0001))
336 {
337 vec.setValue(1,0,0);
338 } else
339 {
340 btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
341 vec *= rlen;
342 }
343
344 btVector3 vtx;
345 btScalar newDot;
346
347 for( int k = 0; k < getNumVertices(); k += 128 )
348 {
349 btVector3 temp[128];
350 int inner_count = MIN(getNumVertices() - k, 128);
351 for( i = 0; i < inner_count; i++ )
352 getVertex(i,temp[i]);
353 i = (int) vec.maxDot( temp, inner_count, newDot);
354 if (newDot > maxDot)
355 {
356 maxDot = newDot;
357 supVec = temp[i];
358 }
359 }
360
361#endif //__SPU__
362 return supVec;
363}
364
365
366
367void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
368{
369#ifndef __SPU__
370 int i;
371
372 btVector3 vtx;
373 btScalar newDot;
374
375 for (i=0;i<numVectors;i++)
376 {
377 supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
378 }
379
380 for (int j=0;j<numVectors;j++)
381 {
382 const btVector3& vec = vectors[j];
383
384 for( int k = 0; k < getNumVertices(); k += 128 )
385 {
386 btVector3 temp[128];
387 int inner_count = MIN(getNumVertices() - k, 128);
388 for( i = 0; i < inner_count; i++ )
389 getVertex(i,temp[i]);
390 i = (int) vec.maxDot( temp, inner_count, newDot);
391 if (newDot > supportVerticesOut[j][3])
392 {
393 supportVerticesOut[j] = temp[i];
394 supportVerticesOut[j][3] = newDot;
395 }
396 }
397 }
398
399#endif //__SPU__
400}
401
402
403
405{
406#ifndef __SPU__
407 //not yet, return box inertia
408
409 btScalar margin = getMargin();
410
411 btTransform ident;
412 ident.setIdentity();
413 btVector3 aabbMin,aabbMax;
414 getAabb(ident,aabbMin,aabbMax);
415 btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
416
417 btScalar lx=btScalar(2.)*(halfExtents.x()+margin);
418 btScalar ly=btScalar(2.)*(halfExtents.y()+margin);
419 btScalar lz=btScalar(2.)*(halfExtents.z()+margin);
420 const btScalar x2 = lx*lx;
421 const btScalar y2 = ly*ly;
422 const btScalar z2 = lz*lz;
423 const btScalar scaledmass = mass * btScalar(0.08333333);
424
425 inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
426#endif //__SPU__
427}
428
429
430
432{
435}
436
439m_localAabbMin(1,1,1),
440m_localAabbMax(-1,-1,-1),
441m_isLocalAabbValid(false)
442{
443}
444
446{
447 getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin());
448}
449
451{
452 m_isLocalAabbValid = true;
453
454 #if 1
455 static const btVector3 _directions[] =
456 {
457 btVector3( 1., 0., 0.),
458 btVector3( 0., 1., 0.),
459 btVector3( 0., 0., 1.),
460 btVector3( -1., 0., 0.),
461 btVector3( 0., -1., 0.),
462 btVector3( 0., 0., -1.)
463 };
464
465 btVector3 _supporting[] =
466 {
467 btVector3( 0., 0., 0.),
468 btVector3( 0., 0., 0.),
469 btVector3( 0., 0., 0.),
470 btVector3( 0., 0., 0.),
471 btVector3( 0., 0., 0.),
472 btVector3( 0., 0., 0.)
473 };
474
475 batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6);
476
477 for ( int i = 0; i < 3; ++i )
478 {
479 m_localAabbMax[i] = _supporting[i][i] + m_collisionMargin;
480 m_localAabbMin[i] = _supporting[i + 3][i] - m_collisionMargin;
481 }
482
483 #else
484
485 for (int i=0;i<3;i++)
486 {
487 btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
488 vec[i] = btScalar(1.);
490 m_localAabbMax[i] = tmp[i];
491 vec[i] = btScalar(-1.);
492 tmp = localGetSupportingVertex(vec);
493 m_localAabbMin[i] = tmp[i];
494 }
495 #endif
496}
497
498
499
500
#define btAlignedFree(ptr)
#define btAlignedAlloc(size, alignment)
void GrahamScanConvexHull2D(btAlignedObjectArray< GrahamVector3 > &originalPoints, btAlignedObjectArray< GrahamVector3 > &hull, const btVector3 &normalAxis)
#define MIN(_a, _b)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
#define BT_LARGE_FLOAT
Definition: btScalar.h:294
btScalar btSqrt(btScalar y)
Definition: btScalar.h:444
#define btAssert(x)
Definition: btScalar.h:131
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
void remove(const T &key)
T & expand(const T &fillValue=T())
void push_back(const T &_Val)
const Edge * getNextEdgeOfFace() const
Convex hull implementation based on Preparata and Hong See http://code.google.com/p/bullet/issues/det...
btScalar compute(const void *coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
btAlignedObjectArray< btVector3 > vertices
btAlignedObjectArray< int > faces
btAlignedObjectArray< Edge > edges
The btConvexInternalShape is an internal base class, shared by most convex shape implementations.
virtual btVector3 localGetSupportingVertex(const btVector3 &vec) const
void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const
getAabb's default implementation is brute force, expected derived classes to implement a fast dedicat...
virtual void setLocalScaling(const btVector3 &scaling)
virtual btScalar getMargin() const
btAlignedObjectArray< btVector3 > m_vertices
btAlignedObjectArray< btFace > m_faces
static void getVerticesFromPlaneEquations(const btAlignedObjectArray< btVector3 > &planeEquations, btAlignedObjectArray< btVector3 > &verticesOut)
static void getPlaneEquationsFromVertices(btAlignedObjectArray< btVector3 > &vertices, btAlignedObjectArray< btVector3 > &planeEquationsOut)
virtual void setLocalScaling(const btVector3 &scaling)
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const
getAabb's default implementation is brute force, expected derived classes to implement a fast dedicat...
void getNonvirtualAabb(const btTransform &trans, btVector3 &aabbMin, btVector3 &aabbMax, btScalar margin) const
The btPolyhedralConvexShape is an internal interface class for polyhedral convex shapes.
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3 &vec) const
virtual bool initializePolyhedralFeatures(int shiftVerticesByMargin=0)
optional method mainly used to generate multiple contact points by clipping polyhedral features (face...
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const
virtual void getVertex(int i, btVector3 &vtx) const =0
btConvexPolyhedron * m_polyhedron
virtual int getNumVertices() const =0
virtual void calculateLocalInertia(btScalar mass, btVector3 &inertia) const
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
const btScalar & z() const
Return the z value.
Definition: btVector3.h:591
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:389
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
long maxDot(const btVector3 *array, long array_count, btScalar &dotOut) const
returns index of maximum dot product between this and vectors in array[]
Definition: btVector3.h:1013
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
const btScalar & x() const
Return the x value.
Definition: btVector3.h:587
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:309
const btScalar & y() const
Return the y value.
Definition: btVector3.h:589
btAlignedObjectArray< int > m_indices
btScalar m_plane[4]