Bullet Collision Detection & Physics Library
btConvexPolyhedron.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2011 Advanced Micro Devices, Inc. http://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15
16
20
21#include "btConvexPolyhedron.h"
23
24
26{
27
28}
30{
31
32}
33
34
35inline 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
42{
43 btInternalVertexPair(short int v0,short int v1)
44 :m_v0(v0),
45 m_v1(v1)
46 {
47 if (m_v1>m_v0)
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
63{
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
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
103{
104
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) ||
129 {
130 found = true;
131 break;
132 }
133 }
134
135 if (!found)
136 {
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 {
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
278void 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}
bool IsAlmostZero(const btVector3 &v)
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:886
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btScalar btFabs(btScalar x)
Definition: btScalar.h:475
void btSwap(T &a, T &b)
Definition: btScalar.h:621
#define btAssert(x)
Definition: btScalar.h:131
int size() const
return the number of elements in the array
void push_back(const T &_Val)
void project(const btTransform &trans, const btVector3 &dir, btScalar &minProj, btScalar &maxProj, btVector3 &witnesPtMin, btVector3 &witnesPtMax) const
bool testContainment() const
btAlignedObjectArray< btVector3 > m_vertices
btAlignedObjectArray< btFace > m_faces
btConvexPolyhedron()
This file was written by Erwin Coumans Separating axis rest based on work from Pierre Terdiman,...
btAlignedObjectArray< btVector3 > m_uniqueEdges
The btHashMap template class implements a generic and lightweight hashmap.
Definition: btHashMap.h:226
void insert(const Key &key, const Value &value)
Definition: btHashMap.h:274
const Value * find(const Key &key) const
Definition: btHashMap.h:434
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
const btScalar & z() const
Return the z value.
Definition: btVector3.h:591
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
const btScalar & x() const
Return the x value.
Definition: btVector3.h:587
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:487
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:309
const btScalar & y() const
Return the y value.
Definition: btVector3.h:589
btInternalVertexPair(short int v0, short int v1)
bool equals(const btInternalVertexPair &other) const