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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

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 }
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:835
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:276
btInternalVertexPair::equals
bool equals(const btInternalVertexPair &other) const
Definition: btConvexPolyhedron.cpp:55
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btInternalEdge::m_face1
short int m_face1
Definition: btConvexPolyhedron.cpp:69
btInternalVertexPair
Definition: btConvexPolyhedron.cpp:40
btConvexPolyhedron::mC
btVector3 mC
Definition: btConvexPolyhedron.h:53
btInternalEdge
Definition: btConvexPolyhedron.cpp:61
btAssert
#define btAssert(x)
Definition: btScalar.h:113
btConvexPolyhedron::m_uniqueEdges
btAlignedObjectArray< btVector3 > m_uniqueEdges
Definition: btConvexPolyhedron.h:48
IsAlmostZero
bool IsAlmostZero(const btVector3 &v)
Definition: btConvexPolyhedron.cpp:34
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btHashMap
The btHashMap template class implements a generic and lightweight hashmap.
Definition: btHashMap.h:220
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:297
btInternalVertexPair::btInternalVertexPair
btInternalVertexPair(short int v0, short int v1)
Definition: btConvexPolyhedron.cpp:42
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
btInternalVertexPair::getHash
int getHash() const
Definition: btConvexPolyhedron.cpp:51
btConvexPolyhedron::m_localCenter
btVector3 m_localCenter
Definition: btConvexPolyhedron.h:50
btInternalVertexPair::m_v0
short int m_v0
Definition: btConvexPolyhedron.cpp:49
btConvexPolyhedron::initialize
void initialize()
Definition: btConvexPolyhedron.cpp:101
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:149
btConvexPolyhedron::m_faces
btAlignedObjectArray< btFace > m_faces
Definition: btConvexPolyhedron.h:47
btSwap
void btSwap(T &a, T &b)
Definition: btScalar.h:585
btConvexPolyhedron::mE
btVector3 mE
Definition: btConvexPolyhedron.h:54
btConvexPolyhedron::btConvexPolyhedron
btConvexPolyhedron()
This file was written by Erwin Coumans Separating axis rest based on work from Pierre Terdiman...
Definition: btConvexPolyhedron.cpp:24
btConvexPolyhedron::m_radius
btScalar m_radius
Definition: btConvexPolyhedron.h:52
btHashMap::insert
void insert(const Key &key, const Value &value)
Definition: btHashMap.h:269
btConvexPolyhedron::~btConvexPolyhedron
virtual ~btConvexPolyhedron()
Definition: btConvexPolyhedron.cpp:28
btConvexPolyhedron::m_extents
btVector3 m_extents
Definition: btConvexPolyhedron.h:51
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btHashMap::find
const Value * find(const Key &key) const
Definition: btHashMap.h:402
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:220
btConvexPolyhedron::testContainment
bool testContainment() const
Definition: btConvexPolyhedron.cpp:75
btConvexPolyhedron::project
void project(const btTransform &trans, const btVector3 &dir, btScalar &minProj, btScalar &maxProj, btVector3 &witnesPtMin, btVector3 &witnesPtMax) const
Definition: btConvexPolyhedron.cpp:277
btHashMap.h
btInternalEdge::m_face0
short int m_face0
Definition: btConvexPolyhedron.cpp:68
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
btInternalEdge::btInternalEdge
btInternalEdge()
Definition: btConvexPolyhedron.cpp:63
btInternalVertexPair::m_v1
short int m_v1
Definition: btConvexPolyhedron.cpp:50
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:475
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:449
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579