bullet: Update to current svn, r2636
[blender.git] / extern / bullet2 / src / BulletCollision / CollisionShapes / btConvexPolyhedron.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2011 Advanced Micro Devices, Inc.  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
16
17 ///This file was written by Erwin Coumans
18 ///Separating axis rest based on work from Pierre Terdiman, see
19 ///And contact clipping based on work from Simon Hobbs
20
21 #include "btConvexPolyhedron.h"
22 #include "LinearMath/btHashMap.h"
23
24 btConvexPolyhedron::btConvexPolyhedron()
25 {
26
27 }
28 btConvexPolyhedron::~btConvexPolyhedron()
29 {
30
31 }
32
33
34 inline bool IsAlmostZero(const btVector3& v)
35 {
36         if(fabsf(v.x())>1e-6 || fabsf(v.y())>1e-6 || fabsf(v.z())>1e-6) return false;
37         return true;
38 }
39
40 struct btInternalVertexPair
41 {
42         btInternalVertexPair(short int v0,short int v1)
43                 :m_v0(v0),
44                 m_v1(v1)
45         {
46                 if (m_v1>m_v0)
47                         btSwap(m_v0,m_v1);
48         }
49         short int m_v0;
50         short int m_v1;
51         int getHash() const
52         {
53                 return m_v0+(m_v1<<16);
54         }
55         bool equals(const btInternalVertexPair& other) const
56         {
57                 return m_v0==other.m_v0 && m_v1==other.m_v1;
58         }
59 };
60
61 struct btInternalEdge
62 {
63         btInternalEdge()
64                 :m_face0(-1),
65                 m_face1(-1)
66         {
67         }
68         short int m_face0;
69         short int m_face1;
70 };
71
72 //
73
74 #ifdef TEST_INTERNAL_OBJECTS
75 bool btConvexPolyhedron::testContainment() const
76 {
77         for(int p=0;p<8;p++)
78         {
79                 btVector3 LocalPt;
80                 if(p==0)                LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], m_extents[2]);
81                 else if(p==1)   LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], -m_extents[2]);
82                 else if(p==2)   LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], m_extents[2]);
83                 else if(p==3)   LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], -m_extents[2]);
84                 else if(p==4)   LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], m_extents[2]);
85                 else if(p==5)   LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], -m_extents[2]);
86                 else if(p==6)   LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], m_extents[2]);
87                 else if(p==7)   LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], -m_extents[2]);
88
89                 for(int i=0;i<m_faces.size();i++)
90                 {
91                         const btVector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
92                         const btScalar d = LocalPt.dot(Normal) + m_faces[i].m_plane[3];
93                         if(d>0.0f)
94                                 return false;
95                 }
96         }
97         return true;
98 }
99 #endif
100
101 void    btConvexPolyhedron::initialize()
102 {
103
104         btHashMap<btInternalVertexPair,btInternalEdge> edges;
105
106         btScalar TotalArea = 0.0f;
107         
108         m_localCenter.setValue(0, 0, 0);
109         for(int i=0;i<m_faces.size();i++)
110         {
111                 int numVertices = m_faces[i].m_indices.size();
112                 int NbTris = numVertices;
113                 for(int j=0;j<NbTris;j++)
114                 {
115                         int k = (j+1)%numVertices;
116                         btInternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]);
117                         btInternalEdge* edptr = edges.find(vp);
118                         btVector3 edge = m_vertices[vp.m_v1]-m_vertices[vp.m_v0];
119                         edge.normalize();
120
121                         bool found = false;
122
123                         for (int p=0;p<m_uniqueEdges.size();p++)
124                         {
125                                 
126                                 if (IsAlmostZero(m_uniqueEdges[p]-edge) || 
127                                         IsAlmostZero(m_uniqueEdges[p]+edge))
128                                 {
129                                         found = true;
130                                         break;
131                                 }
132                         }
133
134                         if (!found)
135                         {
136                                 m_uniqueEdges.push_back(edge);
137                         }
138
139                         if (edptr)
140                         {
141                                 btAssert(edptr->m_face0>=0);
142                                 btAssert(edptr->m_face1<0);
143                                 edptr->m_face1 = i;
144                         } else
145                         {
146                                 btInternalEdge ed;
147                                 ed.m_face0 = i;
148                                 edges.insert(vp,ed);
149                         }
150                 }
151         }
152
153 #ifdef USE_CONNECTED_FACES
154         for(int i=0;i<m_faces.size();i++)
155         {
156                 int numVertices = m_faces[i].m_indices.size();
157                 m_faces[i].m_connectedFaces.resize(numVertices);
158
159                 for(int j=0;j<numVertices;j++)
160                 {
161                         int k = (j+1)%numVertices;
162                         btInternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]);
163                         btInternalEdge* edptr = edges.find(vp);
164                         btAssert(edptr);
165                         btAssert(edptr->m_face0>=0);
166                         btAssert(edptr->m_face1>=0);
167
168                         int connectedFace = (edptr->m_face0==i)?edptr->m_face1:edptr->m_face0;
169                         m_faces[i].m_connectedFaces[j] = connectedFace;
170                 }
171         }
172 #endif//USE_CONNECTED_FACES
173
174         for(int i=0;i<m_faces.size();i++)
175         {
176                 int numVertices = m_faces[i].m_indices.size();
177                 int NbTris = numVertices-2;
178                 
179                 const btVector3& p0 = m_vertices[m_faces[i].m_indices[0]];
180                 for(int j=1;j<=NbTris;j++)
181                 {
182                         int k = (j+1)%numVertices;
183                         const btVector3& p1 = m_vertices[m_faces[i].m_indices[j]];
184                         const btVector3& p2 = m_vertices[m_faces[i].m_indices[k]];
185                         btScalar Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f;
186                         btVector3 Center = (p0+p1+p2)/3.0f;
187                         m_localCenter += Area * Center;
188                         TotalArea += Area;
189                 }
190         }
191         m_localCenter /= TotalArea;
192
193
194
195
196 #ifdef TEST_INTERNAL_OBJECTS
197         if(1)
198         {
199                 m_radius = FLT_MAX;
200                 for(int i=0;i<m_faces.size();i++)
201                 {
202                         const btVector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
203                         const btScalar dist = btFabs(m_localCenter.dot(Normal) + m_faces[i].m_plane[3]);
204                         if(dist<m_radius)
205                                 m_radius = dist;
206                 }
207
208         
209                 btScalar MinX = FLT_MAX;
210                 btScalar MinY = FLT_MAX;
211                 btScalar MinZ = FLT_MAX;
212                 btScalar MaxX = -FLT_MAX;
213                 btScalar MaxY = -FLT_MAX;
214                 btScalar MaxZ = -FLT_MAX;
215                 for(int i=0; i<m_vertices.size(); i++)
216                 {
217                         const btVector3& pt = m_vertices[i];
218                         if(pt.x()<MinX) MinX = pt.x();
219                         if(pt.x()>MaxX) MaxX = pt.x();
220                         if(pt.y()<MinY) MinY = pt.y();
221                         if(pt.y()>MaxY) MaxY = pt.y();
222                         if(pt.z()<MinZ) MinZ = pt.z();
223                         if(pt.z()>MaxZ) MaxZ = pt.z();
224                 }
225                 mC.setValue(MaxX+MinX, MaxY+MinY, MaxZ+MinZ);
226                 mE.setValue(MaxX-MinX, MaxY-MinY, MaxZ-MinZ);
227
228
229
230 //              const btScalar r = m_radius / sqrtf(2.0f);
231                 const btScalar r = m_radius / sqrtf(3.0f);
232                 const int LargestExtent = mE.maxAxis();
233                 const btScalar Step = (mE[LargestExtent]*0.5f - r)/1024.0f;
234                 m_extents[0] = m_extents[1] = m_extents[2] = r;
235                 m_extents[LargestExtent] = mE[LargestExtent]*0.5f;
236                 bool FoundBox = false;
237                 for(int j=0;j<1024;j++)
238                 {
239                         if(testContainment())
240                         {
241                                 FoundBox = true;
242                                 break;
243                         }
244
245                         m_extents[LargestExtent] -= Step;
246                 }
247                 if(!FoundBox)
248                 {
249                         m_extents[0] = m_extents[1] = m_extents[2] = r;
250                 }
251                 else
252                 {
253                         // Refine the box
254                         const btScalar Step = (m_radius - r)/1024.0f;
255                         const int e0 = (1<<LargestExtent) & 3;
256                         const int e1 = (1<<e0) & 3;
257
258                         for(int j=0;j<1024;j++)
259                         {
260                                 const btScalar Saved0 = m_extents[e0];
261                                 const btScalar Saved1 = m_extents[e1];
262                                 m_extents[e0] += Step;
263                                 m_extents[e1] += Step;
264
265                                 if(!testContainment())
266                                 {
267                                         m_extents[e0] = Saved0;
268                                         m_extents[e1] = Saved1;
269                                         break;
270                                 }
271                         }
272                 }
273         }
274 #endif
275 }
276
277 void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const
278 {
279         minProj = FLT_MAX;
280         maxProj = -FLT_MAX;
281         int numVerts = m_vertices.size();
282         for(int i=0;i<numVerts;i++)
283         {
284                 btVector3 pt = trans * m_vertices[i];
285                 btScalar dp = pt.dot(dir);
286                 if(dp < minProj)
287                 {
288                         minProj = dp;
289                         witnesPtMin = pt;
290                 }
291                 if(dp > maxProj)
292                 {
293                         maxProj = dp;
294                         witnesPtMax = pt;
295                 }
296         }
297         if(minProj>maxProj)
298         {
299                 btSwap(minProj,maxProj);
300                 btSwap(witnesPtMin,witnesPtMax);
301         }
302 }