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 
19 
20 #include "btConvexPolyhedron.h"
21 #include "LinearMath/btHashMap.h"
22 
24 {
25 }
27 {
28 }
29 
30 inline bool IsAlmostZero1(const btVector3& v)
31 {
32  if (btFabs(v.x()) > 1e-6 || btFabs(v.y()) > 1e-6 || btFabs(v.z()) > 1e-6) return false;
33  return true;
34 }
35 
37 {
38  btInternalVertexPair(short int v0, short int v1)
39  : m_v0(v0),
40  m_v1(v1)
41  {
42  if (m_v1 > m_v0)
43  btSwap(m_v0, m_v1);
44  }
45  short int m_v0;
46  short int m_v1;
47  int getHash() const
48  {
49  return m_v0 + (m_v1 << 16);
50  }
51  bool equals(const btInternalVertexPair& other) const
52  {
53  return m_v0 == other.m_v0 && m_v1 == other.m_v1;
54  }
55 };
56 
58 {
60  : m_face0(-1),
61  m_face1(-1)
62  {
63  }
64  short int m_face0;
65  short int m_face1;
66 };
67 
68 //
69 
70 #ifdef TEST_INTERNAL_OBJECTS
72 {
73  for (int p = 0; p < 8; p++)
74  {
75  btVector3 LocalPt;
76  if (p == 0)
77  LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], m_extents[2]);
78  else if (p == 1)
79  LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], -m_extents[2]);
80  else if (p == 2)
81  LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], m_extents[2]);
82  else if (p == 3)
83  LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], -m_extents[2]);
84  else if (p == 4)
85  LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], m_extents[2]);
86  else if (p == 5)
87  LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], -m_extents[2]);
88  else if (p == 6)
89  LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], m_extents[2]);
90  else if (p == 7)
91  LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], -m_extents[2]);
92 
93  for (int i = 0; i < m_faces.size(); i++)
94  {
95  const btVector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
96  const btScalar d = LocalPt.dot(Normal) + m_faces[i].m_plane[3];
97  if (d > 0.0f)
98  return false;
99  }
100  }
101  return true;
102 }
103 #endif
104 
106 {
108 
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  if (IsAlmostZero1(m_uniqueEdges[p] - edge) ||
126  IsAlmostZero1(m_uniqueEdges[p] + edge))
127  {
128  found = true;
129  break;
130  }
131  }
132 
133  if (!found)
134  {
135  m_uniqueEdges.push_back(edge);
136  }
137 
138  if (edptr)
139  {
140  btAssert(edptr->m_face0 >= 0);
141  btAssert(edptr->m_face1 < 0);
142  edptr->m_face1 = i;
143  }
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  initialize2();
175 }
176 
178 {
179  m_localCenter.setValue(0, 0, 0);
180  btScalar TotalArea = 0.0f;
181  for (int i = 0; i < m_faces.size(); i++)
182  {
183  int numVertices = m_faces[i].m_indices.size();
184  int NbTris = numVertices - 2;
185 
186  const btVector3& p0 = m_vertices[m_faces[i].m_indices[0]];
187  for (int j = 1; j <= NbTris; j++)
188  {
189  int k = (j + 1) % numVertices;
190  const btVector3& p1 = m_vertices[m_faces[i].m_indices[j]];
191  const btVector3& p2 = m_vertices[m_faces[i].m_indices[k]];
192  btScalar Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f;
193  btVector3 Center = (p0 + p1 + p2) / 3.0f;
194  m_localCenter += Area * Center;
195  TotalArea += Area;
196  }
197  }
198  m_localCenter /= TotalArea;
199 
200 #ifdef TEST_INTERNAL_OBJECTS
201  if (1)
202  {
203  m_radius = FLT_MAX;
204  for (int i = 0; i < m_faces.size(); i++)
205  {
206  const btVector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
207  const btScalar dist = btFabs(m_localCenter.dot(Normal) + m_faces[i].m_plane[3]);
208  if (dist < m_radius)
209  m_radius = dist;
210  }
211 
212  btScalar MinX = FLT_MAX;
213  btScalar MinY = FLT_MAX;
214  btScalar MinZ = FLT_MAX;
215  btScalar MaxX = -FLT_MAX;
216  btScalar MaxY = -FLT_MAX;
217  btScalar MaxZ = -FLT_MAX;
218  for (int i = 0; i < m_vertices.size(); i++)
219  {
220  const btVector3& pt = m_vertices[i];
221  if (pt.x() < MinX) MinX = pt.x();
222  if (pt.x() > MaxX) MaxX = pt.x();
223  if (pt.y() < MinY) MinY = pt.y();
224  if (pt.y() > MaxY) MaxY = pt.y();
225  if (pt.z() < MinZ) MinZ = pt.z();
226  if (pt.z() > MaxZ) MaxZ = pt.z();
227  }
228  mC.setValue(MaxX + MinX, MaxY + MinY, MaxZ + MinZ);
229  mE.setValue(MaxX - MinX, MaxY - MinY, MaxZ - MinZ);
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 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 }
btConvexPolyhedron::testContainment
bool testContainment() const
Definition: btConvexPolyhedron.cpp:71
btConvexPolyhedron::initialize2
void initialize2()
Definition: btConvexPolyhedron.cpp:177
btConvexPolyhedron::mE
btVector3 mE
Definition: btConvexPolyhedron.h:50
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btConvexPolyhedron::m_faces
btAlignedObjectArray< btFace > m_faces
Definition: btConvexPolyhedron.h:43
btInternalEdge::m_face0
short int m_face0
Definition: btConvexPolyhedron.cpp:64
btVector3::maxAxis
int maxAxis() const
Return the axis with the largest value Note return values are 0,1,2 for x, y, or z.
Definition: btVector3.h:477
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btConvexPolyhedron::m_radius
btScalar m_radius
Definition: btConvexPolyhedron.h:48
btInternalVertexPair
Definition: btConvexPolyhedron.cpp:36
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
btConvexPolyhedron::initialize
void initialize()
Definition: btConvexPolyhedron.cpp:105
btInternalEdge
Definition: btConvexPolyhedron.cpp:57
btConvexPolyhedron::m_vertices
btAlignedObjectArray< btVector3 > m_vertices
Definition: btConvexPolyhedron.h:42
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btHashMap
The btHashMap template class implements a generic and lightweight hashmap.
Definition: btHashMap.h:219
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:497
btConvexPolyhedron::mC
btVector3 mC
Definition: btConvexPolyhedron.h:49
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btInternalVertexPair::btInternalVertexPair
btInternalVertexPair(short int v0, short int v1)
Definition: btConvexPolyhedron.cpp:38
btConvexPolyhedron::m_localCenter
btVector3 m_localCenter
Definition: btConvexPolyhedron.h:46
btInternalVertexPair::getHash
int getHash() const
Definition: btConvexPolyhedron.cpp:47
btConvexPolyhedron::~btConvexPolyhedron
virtual ~btConvexPolyhedron()
Definition: btConvexPolyhedron.cpp:26
btConvexPolyhedron::project
void project(const btTransform &trans, const btVector3 &dir, btScalar &minProj, btScalar &maxProj, btVector3 &witnesPtMin, btVector3 &witnesPtMax) const
Definition: btConvexPolyhedron.cpp:277
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
btInternalVertexPair::m_v0
short int m_v0
Definition: btConvexPolyhedron.cpp:45
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btConvexPolyhedron.h
btSwap
void btSwap(T &a, T &b)
Definition: btScalar.h:643
btHashMap::find
const Value * find(const Key &key) const
Definition: btHashMap.h:424
btHashMap.h
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
btConvexPolyhedron::m_extents
btVector3 m_extents
Definition: btConvexPolyhedron.h:47
btInternalEdge::btInternalEdge
btInternalEdge()
Definition: btConvexPolyhedron.cpp:59
btInternalVertexPair::equals
bool equals(const btInternalVertexPair &other) const
Definition: btConvexPolyhedron.cpp:51
btHashMap::insert
void insert(const Key &key, const Value &value)
Definition: btHashMap.h:264
btConvexPolyhedron::btConvexPolyhedron
btConvexPolyhedron()
This file was written by Erwin Coumans Separating axis rest based on work from Pierre Terdiman,...
Definition: btConvexPolyhedron.cpp:23
btInternalVertexPair::m_v1
short int m_v1
Definition: btConvexPolyhedron.cpp:46
btInternalEdge::m_face1
short int m_face1
Definition: btConvexPolyhedron.cpp:65
IsAlmostZero1
bool IsAlmostZero1(const btVector3 &v)
Definition: btConvexPolyhedron.cpp:30
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:257
length
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:895
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
btConvexPolyhedron::m_uniqueEdges
btAlignedObjectArray< btVector3 > m_uniqueEdges
Definition: btConvexPolyhedron.h:44
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:142