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 #if defined (_WIN32) || defined (__i386__)
16 #define BT_USE_SSE_IN_API
17 #endif
18
19 #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
20 #include "btConvexPolyhedron.h"
21 #include "LinearMath/btConvexHullComputer.h"
22 #include <new>
23 #include "LinearMath/btGeometryUtil.h"
24 #include "LinearMath/btGrahamScan2dConvexHull.h"
25
26
btPolyhedralConvexShape()27 btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape(),
28 m_polyhedron(0)
29 {
30
31 }
32
~btPolyhedralConvexShape()33 btPolyhedralConvexShape::~btPolyhedralConvexShape()
34 {
35 if (m_polyhedron)
36 {
37 m_polyhedron->~btConvexPolyhedron();
38 btAlignedFree(m_polyhedron);
39 }
40 }
41
42
initializePolyhedralFeatures(int shiftVerticesByMargin)43 bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMargin)
44 {
45
46 if (m_polyhedron)
47 {
48 m_polyhedron->~btConvexPolyhedron();
49 btAlignedFree(m_polyhedron);
50 }
51
52 void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16);
53 m_polyhedron = new (mem) btConvexPolyhedron;
54
55 btAlignedObjectArray<btVector3> orgVertices;
56
57 for (int i=0;i<getNumVertices();i++)
58 {
59 btVector3& newVertex = orgVertices.expand();
60 getVertex(i,newVertex);
61 }
62
63 btConvexHullComputer conv;
64
65 if (shiftVerticesByMargin)
66 {
67 btAlignedObjectArray<btVector3> planeEquations;
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
79 btAlignedObjectArray<btVector3> tmpVertices;
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
92 btAlignedObjectArray<btVector3> faceNormals;
93 int numFaces = conv.faces.size();
94 faceNormals.resize(numFaces);
95 btConvexHullComputer* convexUtil = &conv;
96
97
98 btAlignedObjectArray<btFace> tmpFaces;
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;
170 btAlignedObjectArray<int> todoFaces;
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
202 btAlignedObjectArray<GrahamVector3> orgpoints;
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
239 btAlignedObjectArray<GrahamVector3> hull;
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
315 m_polyhedron->initialize();
316
317 return true;
318 }
319
320 #ifndef MIN
321 #define MIN(_a, _b) ((_a) < (_b) ? (_a) : (_b))
322 #endif
323
localGetSupportingVertexWithoutMargin(const btVector3 & vec0) const324 btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
325 {
326
327
328 btVector3 supVec(0,0,0);
329 #ifndef __SPU__
330 int i;
331 btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
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
batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 * vectors,btVector3 * supportVerticesOut,int numVectors) const367 void 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
calculateLocalInertia(btScalar mass,btVector3 & inertia) const404 void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
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
setLocalScaling(const btVector3 & scaling)431 void btPolyhedralConvexAabbCachingShape::setLocalScaling(const btVector3& scaling)
432 {
433 btConvexInternalShape::setLocalScaling(scaling);
434 recalcLocalAabb();
435 }
436
btPolyhedralConvexAabbCachingShape()437 btPolyhedralConvexAabbCachingShape::btPolyhedralConvexAabbCachingShape()
438 :btPolyhedralConvexShape(),
439 m_localAabbMin(1,1,1),
440 m_localAabbMax(-1,-1,-1),
441 m_isLocalAabbValid(false)
442 {
443 }
444
getAabb(const btTransform & trans,btVector3 & aabbMin,btVector3 & aabbMax) const445 void btPolyhedralConvexAabbCachingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
446 {
447 getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin());
448 }
449
recalcLocalAabb()450 void btPolyhedralConvexAabbCachingShape::recalcLocalAabb()
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.);
489 btVector3 tmp = localGetSupportingVertex(vec);
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
501