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
btConvexPolyhedron()24 btConvexPolyhedron::btConvexPolyhedron()
25 {
26
27 }
~btConvexPolyhedron()28 btConvexPolyhedron::~btConvexPolyhedron()
29 {
30
31 }
32
33
IsAlmostZero(const btVector3 & v)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 {
btInternalVertexPairbtInternalVertexPair42 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;
getHashbtInternalVertexPair51 int getHash() const
52 {
53 return m_v0+(m_v1<<16);
54 }
equalsbtInternalVertexPair55 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 {
btInternalEdgebtInternalEdge63 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
testContainment() const75 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
initialize()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
project(const btTransform & trans,const btVector3 & dir,btScalar & minProj,btScalar & maxProj,btVector3 & witnesPtMin,btVector3 & witnesPtMax) const277 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 }
303