Bullet Collision Detection & Physics Library: btConvexPolyhedron.cpp Source File

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
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 }
btConvexPolyhedron::m_vertices
btAlignedObjectArray< btVector3 > m_vertices
Definition: btConvexPolyhedron.h:46
length
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:886
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:274
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
btInternalEdge::m_face1
short int m_face1
Definition: btConvexPolyhedron.cpp:70
btInternalVertexPair
Definition: btConvexPolyhedron.cpp:41
btInternalVertexPair::equals
bool equals(const btInternalVertexPair &other) const
Definition: btConvexPolyhedron.cpp:56
btHashMap::find
const Value * find(const Key &key) const
Definition: btHashMap.h:434
btConvexPolyhedron::mC
btVector3 mC
Definition: btConvexPolyhedron.h:53
btInternalEdge
Definition: btConvexPolyhedron.cpp:62
btAssert
#define btAssert(x)
Definition: btScalar.h:131
btConvexPolyhedron::m_uniqueEdges
btAlignedObjectArray< btVector3 > m_uniqueEdges
Definition: btConvexPolyhedron.h:48
IsAlmostZero
bool IsAlmostZero(const btVector3 &v)
Definition: btConvexPolyhedron.cpp:35
btHashMap
The btHashMap template class implements a generic and lightweight hashmap.
Definition: btHashMap.h:225
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:309
btInternalVertexPair::btInternalVertexPair
btInternalVertexPair(short int v0, short int v1)
Definition: btConvexPolyhedron.cpp:43
btConvexPolyhedron::project
void project(const btTransform &trans, const btVector3 &dir, btScalar &minProj, btScalar &maxProj, btVector3 &witnesPtMin, btVector3 &witnesPtMax) const
Definition: btConvexPolyhedron.cpp:278
btConvexPolyhedron::testContainment
bool testContainment() const
Definition: btConvexPolyhedron.cpp:76
btConvexPolyhedron::m_localCenter
btVector3 m_localCenter
Definition: btConvexPolyhedron.h:50
btInternalVertexPair::m_v0
short int m_v0
Definition: btConvexPolyhedron.cpp:50
btConvexPolyhedron::initialize
void initialize()
Definition: btConvexPolyhedron.cpp:102
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:587
btConvexPolyhedron::m_faces
btAlignedObjectArray< btFace > m_faces
Definition: btConvexPolyhedron.h:47
btSwap
void btSwap(T &a, T &b)
Definition: btScalar.h:621
btConvexPolyhedron::mE
btVector3 mE
Definition: btConvexPolyhedron.h:54
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btConvexPolyhedron::btConvexPolyhedron
btConvexPolyhedron()
This file was written by Erwin Coumans Separating axis rest based on work from Pierre Terdiman...
Definition: btConvexPolyhedron.cpp:25
btConvexPolyhedron::m_radius
btScalar m_radius
Definition: btConvexPolyhedron.h:52
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:589
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:591
btHashMap::insert
void insert(const Key &key, const Value &value)
Definition: btHashMap.h:274
btConvexPolyhedron::~btConvexPolyhedron
virtual ~btConvexPolyhedron()
Definition: btConvexPolyhedron.cpp:29
btInternalVertexPair::getHash
int getHash() const
Definition: btConvexPolyhedron.cpp:52
btConvexPolyhedron::m_extents
btVector3 m_extents
Definition: btConvexPolyhedron.h:51
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:155
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btConvexPolyhedron.h
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:218
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:487
btHashMap.h
btInternalEdge::m_face0
short int m_face0
Definition: btConvexPolyhedron.cpp:69
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btInternalEdge::btInternalEdge
btInternalEdge()
Definition: btConvexPolyhedron.cpp:64
btInternalVertexPair::m_v1
short int m_v1
Definition: btConvexPolyhedron.cpp:51
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:475