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
27 m_polyhedron(0)
28{
29}
30
32{
33 if (m_polyhedron)
34 {
37 }
38}
39
41{
42 if (m_polyhedron)
43 {
45 }
46 else
47 {
48 void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron), 16);
50 }
51}
52
54{
55 if (m_polyhedron)
56 {
59 }
60
61 void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron), 16);
63
65
66 for (int i = 0; i < getNumVertices(); i++)
67 {
70 }
71
73
75 {
78
80 for (int p = 0; p < planeEquations.size(); p++)
81 {
83 // btScalar margin = getMargin();
84 plane[3] -= getMargin();
86 }
87
89
91
92 conv.compute(&tmpVertices[0].getX(), sizeof(btVector3), tmpVertices.size(), 0.f, 0.f);
93 }
94 else
95 {
96 conv.compute(&orgVertices[0].getX(), sizeof(btVector3), orgVertices.size(), 0.f, 0.f);
97 }
98
99#ifndef BT_RECONSTRUCT_FACES
100
101 int numVertices = conv.vertices.size();
102 m_polyhedron->m_vertices.resize(numVertices);
103 for (int p = 0; p < numVertices; p++)
104 {
105 m_polyhedron->m_vertices[p] = conv.vertices[p];
106 }
107
108 int v0, v1;
109 for (int j = 0; j < conv.faces.size(); j++)
110 {
111 btVector3 edges[3];
112 int numEdges = 0;
114 const btConvexHullComputer::Edge* edge = &conv.edges[conv.faces[j]];
115 v0 = edge->getSourceVertex();
116 int prevVertex = v0;
117 combinedFace.m_indices.push_back(v0);
118 v1 = edge->getTargetVertex();
119 while (v1 != v0)
120 {
121 btVector3 wa = conv.vertices[prevVertex];
122 btVector3 wb = conv.vertices[v1];
125 if (numEdges < 2)
126 edges[numEdges++] = newEdge;
127
128 //face->addIndex(v1);
129 combinedFace.m_indices.push_back(v1);
130 edge = edge->getNextEdgeOfFace();
131 prevVertex = v1;
132 int v01 = edge->getSourceVertex();
133 v1 = edge->getTargetVertex();
134 }
135
136 btAssert(combinedFace.m_indices.size() > 2);
137
138 btVector3 faceNormal = edges[0].cross(edges[1]);
140
141 btScalar planeEq = 1e30f;
142
143 for (int v = 0; v < combinedFace.m_indices.size(); v++)
144 {
146 if (planeEq > eq)
147 {
148 planeEq = eq;
149 }
150 }
151 combinedFace.m_plane[0] = faceNormal.getX();
152 combinedFace.m_plane[1] = faceNormal.getY();
153 combinedFace.m_plane[2] = faceNormal.getZ();
154 combinedFace.m_plane[3] = -planeEq;
155
157 }
158
159#else //BT_RECONSTRUCT_FACES
160
162 int numFaces = conv.faces.size();
163 faceNormals.resize(numFaces);
165
168
169 int numVertices = convexUtil->vertices.size();
170 m_polyhedron->m_vertices.resize(numVertices);
171 for (int p = 0; p < numVertices; p++)
172 {
173 m_polyhedron->m_vertices[p] = convexUtil->vertices[p];
174 }
175
176 for (int i = 0; i < numFaces; i++)
177 {
178 int face = convexUtil->faces[i];
179 //printf("face=%d\n",face);
180 const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
182
183 btVector3 edges[3];
184 int numEdges = 0;
185 //compute face normals
186
187 do
188 {
189 int src = edge->getSourceVertex();
190 tmpFaces[i].m_indices.push_back(src);
191 int targ = edge->getTargetVertex();
192 btVector3 wa = convexUtil->vertices[src];
193
194 btVector3 wb = convexUtil->vertices[targ];
197 if (numEdges < 2)
198 edges[numEdges++] = newEdge;
199
200 edge = edge->getNextEdgeOfFace();
201 } while (edge != firstEdge);
202
203 btScalar planeEq = 1e30f;
204
205 if (numEdges == 2)
206 {
207 faceNormals[i] = edges[0].cross(edges[1]);
209 tmpFaces[i].m_plane[0] = faceNormals[i].getX();
210 tmpFaces[i].m_plane[1] = faceNormals[i].getY();
211 tmpFaces[i].m_plane[2] = faceNormals[i].getZ();
212 tmpFaces[i].m_plane[3] = planeEq;
213 }
214 else
215 {
216 btAssert(0); //degenerate?
217 faceNormals[i].setZero();
218 }
219
220 for (int v = 0; v < tmpFaces[i].m_indices.size(); v++)
221 {
222 btScalar eq = m_polyhedron->m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]);
223 if (planeEq > eq)
224 {
225 planeEq = eq;
226 }
227 }
228 tmpFaces[i].m_plane[3] = -planeEq;
229 }
230
231 //merge coplanar faces and copy them to m_polyhedron
232
235 for (int i = 0; i < tmpFaces.size(); i++)
237
238 while (todoFaces.size())
239 {
241 int refFace = todoFaces[todoFaces.size() - 1];
242
245 todoFaces.pop_back();
246
247 btVector3 faceNormalA(faceA.m_plane[0], faceA.m_plane[1], faceA.m_plane[2]);
248 for (int j = todoFaces.size() - 1; j >= 0; j--)
249 {
250 int i = todoFaces[j];
251 btFace& faceB = tmpFaces[i];
252 btVector3 faceNormalB(faceB.m_plane[0], faceB.m_plane[1], faceB.m_plane[2]);
254 {
255 coplanarFaceGroup.push_back(i);
256 todoFaces.remove(i);
257 }
258 }
259
260 bool did_merge = false;
261 if (coplanarFaceGroup.size() > 1)
262 {
263 //do the merge: use Graham Scan 2d convex hull
264
267
268 for (int i = 0; i < coplanarFaceGroup.size(); i++)
269 {
270 // m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
271
273 btVector3 faceNormal(face.m_plane[0], face.m_plane[1], face.m_plane[2]);
275 for (int f = 0; f < face.m_indices.size(); f++)
276 {
277 int orgIndex = face.m_indices[f];
279
280 bool found = false;
281
282 for (int i = 0; i < orgpoints.size(); i++)
283 {
284 //if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001))
285 if (orgpoints[i].m_orgIndex == orgIndex)
286 {
287 found = true;
288 break;
289 }
290 }
291 if (!found)
293 }
294 }
295
297 for (int i = 0; i < 4; i++)
299
301
302 averageFaceNormal.normalize();
304
305 for (int i = 0; i < hull.size(); i++)
306 {
307 combinedFace.m_indices.push_back(hull[i].m_orgIndex);
308 for (int k = 0; k < orgpoints.size(); k++)
309 {
310 if (orgpoints[k].m_orgIndex == hull[i].m_orgIndex)
311 {
312 orgpoints[k].m_orgIndex = -1; // invalidate...
313 break;
314 }
315 }
316 }
317
318 // are there rejected vertices?
319 bool reject_merge = false;
320
321 for (int i = 0; i < orgpoints.size(); i++)
322 {
323 if (orgpoints[i].m_orgIndex == -1)
324 continue; // this is in the hull...
325 // this vertex is rejected -- is anybody else using this vertex?
326 for (int j = 0; j < tmpFaces.size(); j++)
327 {
328 btFace& face = tmpFaces[j];
329 // is this a face of the current coplanar group?
330 bool is_in_current_group = false;
331 for (int k = 0; k < coplanarFaceGroup.size(); k++)
332 {
333 if (coplanarFaceGroup[k] == j)
334 {
335 is_in_current_group = true;
336 break;
337 }
338 }
339 if (is_in_current_group) // ignore this face...
340 continue;
341 // does this face use this rejected vertex?
342 for (int v = 0; v < face.m_indices.size(); v++)
343 {
344 if (face.m_indices[v] == orgpoints[i].m_orgIndex)
345 {
346 // this rejected vertex is used in another face -- reject merge
347 reject_merge = true;
348 break;
349 }
350 }
351 if (reject_merge)
352 break;
353 }
354 if (reject_merge)
355 break;
356 }
357
358 if (!reject_merge)
359 {
360 // do this merge!
361 did_merge = true;
363 }
364 }
365 if (!did_merge)
366 {
367 for (int i = 0; i < coplanarFaceGroup.size(); i++)
368 {
371 }
372 }
373 }
374
375#endif //BT_RECONSTRUCT_FACES
376
378
379 return true;
380}
381
382#ifndef MIN
383#define MIN(_a, _b) ((_a) < (_b) ? (_a) : (_b))
384#endif
385
387{
388 btVector3 supVec(0, 0, 0);
389#ifndef __SPU__
390 int i;
392
395 if (lenSqr < btScalar(0.0001))
396 {
397 vec.setValue(1, 0, 0);
398 }
399 else
400 {
402 vec *= rlen;
403 }
404
407
408 for (int k = 0; k < getNumVertices(); k += 128)
409 {
410 btVector3 temp[128];
411 int inner_count = MIN(getNumVertices() - k, 128);
412 for (i = 0; i < inner_count; i++)
413 getVertex(i, temp[i]);
414 i = (int)vec.maxDot(temp, inner_count, newDot);
415 if (newDot > maxDot)
416 {
417 maxDot = newDot;
418 supVec = temp[i];
419 }
420 }
421
422#endif //__SPU__
423 return supVec;
424}
425
427{
428#ifndef __SPU__
429 int i;
430
433
434 for (i = 0; i < numVectors; i++)
435 {
437 }
438
439 for (int j = 0; j < numVectors; j++)
440 {
441 const btVector3& vec = vectors[j];
442
443 for (int k = 0; k < getNumVertices(); k += 128)
444 {
445 btVector3 temp[128];
446 int inner_count = MIN(getNumVertices() - k, 128);
447 for (i = 0; i < inner_count; i++)
448 getVertex(i, temp[i]);
449 i = (int)vec.maxDot(temp, inner_count, newDot);
450 if (newDot > supportVerticesOut[j][3])
451 {
452 supportVerticesOut[j] = temp[i];
454 }
455 }
456 }
457
458#endif //__SPU__
459}
460
462{
463#ifndef __SPU__
464 //not yet, return box inertia
465
466 btScalar margin = getMargin();
467
473
474 btScalar lx = btScalar(2.) * (halfExtents.x() + margin);
475 btScalar ly = btScalar(2.) * (halfExtents.y() + margin);
476 btScalar lz = btScalar(2.) * (halfExtents.z() + margin);
477 const btScalar x2 = lx * lx;
478 const btScalar y2 = ly * ly;
479 const btScalar z2 = lz * lz;
480 const btScalar scaledmass = mass * btScalar(0.08333333);
481
482 inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2));
483#endif //__SPU__
484}
485
487{
490}
491
494 m_localAabbMin(1, 1, 1),
495 m_localAabbMax(-1, -1, -1),
496 m_isLocalAabbValid(false)
497{
498}
499
501{
503}
504
506{
507 m_isLocalAabbValid = true;
508
509#if 1
510 static const btVector3 _directions[] =
511 {
512 btVector3(1., 0., 0.),
513 btVector3(0., 1., 0.),
514 btVector3(0., 0., 1.),
515 btVector3(-1., 0., 0.),
516 btVector3(0., -1., 0.),
517 btVector3(0., 0., -1.)};
518
520 {
521 btVector3(0., 0., 0.),
522 btVector3(0., 0., 0.),
523 btVector3(0., 0., 0.),
524 btVector3(0., 0., 0.),
525 btVector3(0., 0., 0.),
526 btVector3(0., 0., 0.)};
527
529
530 for (int i = 0; i < 3; ++i)
531 {
534 }
535
536#else
537
538 for (int i = 0; i < 3; i++)
539 {
540 btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.));
541 vec[i] = btScalar(1.);
543 m_localAabbMax[i] = tmp[i];
544 vec[i] = btScalar(-1.);
546 m_localAabbMin[i] = tmp[i];
547 }
548#endif
549}
#define btAlignedFree(ptr)
#define btAlignedAlloc(size, alignment)
void GrahamScanConvexHull2D(btAlignedObjectArray< GrahamVector3 > &originalPoints, btAlignedObjectArray< GrahamVector3 > &hull, const btVector3 &normalAxis)
const T & btMax(const T &a, const T &b)
Definition btMinMax.h:27
#define MIN(_a, _b)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
#define BT_LARGE_FLOAT
Definition btScalar.h:316
btScalar btSqrt(btScalar y)
Definition btScalar.h:466
#define btAssert(x)
Definition btScalar.h:153
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
void push_back(const T &_Val)
Convex hull implementation based on Preparata and Hong See http://code.google.com/p/bullet/issues/det...
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
virtual void setPolyhedralFeatures(btConvexPolyhedron &polyhedron)
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:30
void setIdentity()
Set this transformation to the identity.
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:82
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition btVector3.h:380
btScalar length2() const
Return the length of the vector squared.
Definition btVector3.h:251
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition btVector3.h:303
const btScalar & getX() const
Return the x value.
Definition btVector3.h:561
btAlignedObjectArray< int > m_indices
btScalar m_plane[4]