Bullet Collision Detection & Physics Library
btConvexPolyhedron.cpp
Go to the documentation of this file.
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 
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 }
Generated on Thu Oct 3 2013 18:25:06 for Bullet Collision Detection & Physics Library by   doxygen 1.8.4