Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp @ 11457

Last change on this file since 11457 was 8393, checked in by rgrieder, 14 years ago

Updated Bullet from v2.77 to v2.78.
(I'm not going to make a branch for that since the update from 2.74 to 2.77 hasn't been tested that much either).

You will HAVE to do a complete RECOMPILE! I tested with MSVC and MinGW and they both threw linker errors at me.

  • Property svn:eol-style set to native
File size: 8.0 KB
RevLine 
[1963]1/*
2Bullet Continuous Collision Detection and Physics Library
[8351]3Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
[1963]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
16#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
[8393]17#include "btConvexPolyhedron.h"
18#include "LinearMath/btConvexHullComputer.h"
19#include <new>
[1963]20
[8393]21btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape(),
22m_polyhedron(0)
[1963]23{
24
25}
26
[8393]27btPolyhedralConvexShape::~btPolyhedralConvexShape()
28{
29        if (m_polyhedron)
30        {
31                btAlignedFree(m_polyhedron);
32        }
33}
[1963]34
[8393]35bool    btPolyhedralConvexShape::initializePolyhedralFeatures()
36{
37        if (m_polyhedron)
38                btAlignedFree(m_polyhedron);
39       
40        void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16);
41        m_polyhedron = new (mem) btConvexPolyhedron;
42
43        btAlignedObjectArray<btVector3> tmpVertices;
44        for (int i=0;i<getNumVertices();i++)
45        {
46                btVector3& newVertex = tmpVertices.expand();
47                getVertex(i,newVertex);
48        }
49
50        btConvexHullComputer conv;
51        conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f);
52
53       
54
55        btAlignedObjectArray<btVector3> faceNormals;
56        int numFaces = conv.faces.size();
57        faceNormals.resize(numFaces);
58        btConvexHullComputer* convexUtil = &conv;
59
60       
61       
62        m_polyhedron->m_faces.resize(numFaces);
63        int numVertices = convexUtil->vertices.size();
64        m_polyhedron->m_vertices.resize(numVertices);
65        for (int p=0;p<numVertices;p++)
66        {
67                m_polyhedron->m_vertices[p] = convexUtil->vertices[p];
68        }
69
70        for (int i=0;i<numFaces;i++)
71        {
72                int face = convexUtil->faces[i];
73                //printf("face=%d\n",face);
74                const btConvexHullComputer::Edge*  firstEdge = &convexUtil->edges[face];
75                const btConvexHullComputer::Edge*  edge = firstEdge;
76
77                btVector3 edges[3];
78                int numEdges = 0;
79                //compute face normals
80
81                btScalar maxCross2 = 0.f;
82                int chosenEdge = -1;
83
84                do
85                {
86                       
87                        int src = edge->getSourceVertex();
88                        m_polyhedron->m_faces[i].m_indices.push_back(src);
89                        int targ = edge->getTargetVertex();
90                        btVector3 wa = convexUtil->vertices[src];
91
92                        btVector3 wb = convexUtil->vertices[targ];
93                        btVector3 newEdge = wb-wa;
94                        newEdge.normalize();
95                        if (numEdges<2)
96                                edges[numEdges++] = newEdge;
97
98                        edge = edge->getNextEdgeOfFace();
99                } while (edge!=firstEdge);
100
101                btScalar planeEq = 1e30f;
102
103               
104                if (numEdges==2)
105                {
106                        faceNormals[i] = edges[0].cross(edges[1]);
107                        faceNormals[i].normalize();
108                        m_polyhedron->m_faces[i].m_plane[0] = -faceNormals[i].getX();
109                        m_polyhedron->m_faces[i].m_plane[1] = -faceNormals[i].getY();
110                        m_polyhedron->m_faces[i].m_plane[2] = -faceNormals[i].getZ();
111                        m_polyhedron->m_faces[i].m_plane[3] = planeEq;
112
113                }
114                else
115                {
116                        btAssert(0);//degenerate?
117                        faceNormals[i].setZero();
118                }
119
120                for (int v=0;v<m_polyhedron->m_faces[i].m_indices.size();v++)
121                {
122                        btScalar eq = m_polyhedron->m_vertices[m_polyhedron->m_faces[i].m_indices[v]].dot(faceNormals[i]);
123                        if (planeEq>eq)
124                        {
125                                planeEq=eq;
126                        }
127                }
128                m_polyhedron->m_faces[i].m_plane[3] = planeEq;
129        }
130
131
132        if (m_polyhedron->m_faces.size() && conv.vertices.size())
133        {
134
135                for (int f=0;f<m_polyhedron->m_faces.size();f++)
136                {
137                       
138                        btVector3 planeNormal(m_polyhedron->m_faces[f].m_plane[0],m_polyhedron->m_faces[f].m_plane[1],m_polyhedron->m_faces[f].m_plane[2]);
139                        btScalar planeEq = m_polyhedron->m_faces[f].m_plane[3];
140
141                        btVector3 supVec = localGetSupportingVertex(-planeNormal);
142
143                        if (supVec.dot(planeNormal)<planeEq)
144                        {
145                                m_polyhedron->m_faces[f].m_plane[0] *= -1;
146                                m_polyhedron->m_faces[f].m_plane[1] *= -1;
147                                m_polyhedron->m_faces[f].m_plane[2] *= -1;
148                                m_polyhedron->m_faces[f].m_plane[3] *= -1;
149                                int numVerts = m_polyhedron->m_faces[f].m_indices.size();
150                                for (int v=0;v<numVerts/2;v++)
151                                {
152                                        btSwap(m_polyhedron->m_faces[f].m_indices[v],m_polyhedron->m_faces[f].m_indices[numVerts-1-v]);
153                                }
154                        }
155                }
156        }
157
158       
159
160        m_polyhedron->initialize();
161
162        return true;
163}
164
165
[1963]166btVector3       btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
167{
[8351]168
169
170        btVector3 supVec(0,0,0);
171#ifndef __SPU__
[1963]172        int i;
[8351]173        btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
[1963]174
175        btVector3 vec = vec0;
176        btScalar lenSqr = vec.length2();
177        if (lenSqr < btScalar(0.0001))
178        {
179                vec.setValue(1,0,0);
180        } else
181        {
182                btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
183                vec *= rlen;
184        }
185
186        btVector3 vtx;
187        btScalar newDot;
188
189        for (i=0;i<getNumVertices();i++)
190        {
191                getVertex(i,vtx);
192                newDot = vec.dot(vtx);
193                if (newDot > maxDot)
194                {
195                        maxDot = newDot;
196                        supVec = vtx;
197                }
198        }
199
[8351]200       
201#endif //__SPU__
[1963]202        return supVec;
203}
204
[8351]205
206
[1963]207void    btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
208{
[8351]209#ifndef __SPU__
[1963]210        int i;
211
212        btVector3 vtx;
213        btScalar newDot;
214
215        for (i=0;i<numVectors;i++)
216        {
[8351]217                supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
[1963]218        }
219
220        for (int j=0;j<numVectors;j++)
221        {
222       
223                const btVector3& vec = vectors[j];
224
225                for (i=0;i<getNumVertices();i++)
226                {
227                        getVertex(i,vtx);
228                        newDot = vec.dot(vtx);
229                        if (newDot > supportVerticesOut[j][3])
230                        {
231                                //WARNING: don't swap next lines, the w component would get overwritten!
232                                supportVerticesOut[j] = vtx;
233                                supportVerticesOut[j][3] = newDot;
234                        }
235                }
236        }
[8351]237#endif //__SPU__
[1963]238}
239
240
241
242void    btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
243{
[8351]244#ifndef __SPU__
[1963]245        //not yet, return box inertia
246
247        btScalar margin = getMargin();
248
249        btTransform ident;
250        ident.setIdentity();
251        btVector3 aabbMin,aabbMax;
252        getAabb(ident,aabbMin,aabbMax);
253        btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
254
255        btScalar lx=btScalar(2.)*(halfExtents.x()+margin);
256        btScalar ly=btScalar(2.)*(halfExtents.y()+margin);
257        btScalar lz=btScalar(2.)*(halfExtents.z()+margin);
258        const btScalar x2 = lx*lx;
259        const btScalar y2 = ly*ly;
260        const btScalar z2 = lz*lz;
261        const btScalar scaledmass = mass * btScalar(0.08333333);
262
263        inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
[8351]264#endif //__SPU__
[1963]265}
266
267
268
[8351]269void    btPolyhedralConvexAabbCachingShape::setLocalScaling(const btVector3& scaling)
[1963]270{
[8351]271        btConvexInternalShape::setLocalScaling(scaling);
272        recalcLocalAabb();
[1963]273}
274
[8351]275btPolyhedralConvexAabbCachingShape::btPolyhedralConvexAabbCachingShape()
276:btPolyhedralConvexShape(),
277m_localAabbMin(1,1,1),
278m_localAabbMax(-1,-1,-1),
279m_isLocalAabbValid(false)
280{
281}
[1963]282
[8351]283void btPolyhedralConvexAabbCachingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
[1963]284{
[8351]285        getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin());
[1963]286}
287
[8351]288void    btPolyhedralConvexAabbCachingShape::recalcLocalAabb()
[1963]289{
290        m_isLocalAabbValid = true;
291       
292        #if 1
293        static const btVector3 _directions[] =
294        {
295                btVector3( 1.,  0.,  0.),
296                btVector3( 0.,  1.,  0.),
297                btVector3( 0.,  0.,  1.),
298                btVector3( -1., 0.,  0.),
299                btVector3( 0., -1.,  0.),
300                btVector3( 0.,  0., -1.)
301        };
302       
303        btVector3 _supporting[] =
304        {
305                btVector3( 0., 0., 0.),
306                btVector3( 0., 0., 0.),
307                btVector3( 0., 0., 0.),
308                btVector3( 0., 0., 0.),
309                btVector3( 0., 0., 0.),
310                btVector3( 0., 0., 0.)
311        };
312       
313        batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6);
314       
315        for ( int i = 0; i < 3; ++i )
316        {
317                m_localAabbMax[i] = _supporting[i][i] + m_collisionMargin;
318                m_localAabbMin[i] = _supporting[i + 3][i] - m_collisionMargin;
319        }
320       
321        #else
322
323        for (int i=0;i<3;i++)
324        {
325                btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
326                vec[i] = btScalar(1.);
327                btVector3 tmp = localGetSupportingVertex(vec);
328                m_localAabbMax[i] = tmp[i]+m_collisionMargin;
329                vec[i] = btScalar(-1.);
330                tmp = localGetSupportingVertex(vec);
331                m_localAabbMin[i] = tmp[i]-m_collisionMargin;
332        }
333        #endif
334}
335
[8393]336
337
338
Note: See TracBrowser for help on using the repository browser.