4854f370f73b175dfe9e5362fa8bb21715ed50e5
[blender-staging.git] / extern / bullet2 / src / BulletCollision / CollisionShapes / btPolyhedralConvexShape.cpp
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
27 btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape(),
28 m_polyhedron(0)
29 {
30
31 }
32
33 btPolyhedralConvexShape::~btPolyhedralConvexShape()
34 {
35         if (m_polyhedron)
36         {
37                 m_polyhedron->~btConvexPolyhedron();
38                 btAlignedFree(m_polyhedron);
39         }
40 }
41
42
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
324 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
367 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
404 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
431 void    btPolyhedralConvexAabbCachingShape::setLocalScaling(const btVector3& scaling)
432 {
433         btConvexInternalShape::setLocalScaling(scaling);
434         recalcLocalAabb();
435 }
436
437 btPolyhedralConvexAabbCachingShape::btPolyhedralConvexAabbCachingShape()
438 :btPolyhedralConvexShape(),
439 m_localAabbMin(1,1,1),
440 m_localAabbMax(-1,-1,-1),
441 m_isLocalAabbValid(false)
442 {
443 }
444
445 void btPolyhedralConvexAabbCachingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
446 {
447         getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin());
448 }
449
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