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