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/*
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
19
20#include "btConvexPolyhedron.h"
21#include "LinearMath/btHashMap.h"
22
23btConvexPolyhedron::btConvexPolyhedron()
24{
25}
26btConvexPolyhedron::~btConvexPolyhedron()
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
36 struct btInternalVertexPair
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
57 struct btInternalEdge
58{
59 btInternalEdge()
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
71 bool btConvexPolyhedron::testContainment() const
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
105 void btConvexPolyhedron::initialize()
106{
107 btHashMap<btInternalVertexPair, btInternalEdge> edges;
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
177 void btConvexPolyhedron::initialize2()
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}
IsAlmostZero1
bool IsAlmostZero1(const btVector3 &v)
Definition: btConvexPolyhedron.cpp:30
btConvexPolyhedron.h
btHashMap.h
length
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:895
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:497
btSwap
void btSwap(T &a, T &b)
Definition: btScalar.h:643
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:142
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:257
btConvexPolyhedron::m_localCenter
btVector3 m_localCenter
Definition: btConvexPolyhedron.h:46
btConvexPolyhedron::project
void project(const btTransform &trans, const btVector3 &dir, btScalar &minProj, btScalar &maxProj, btVector3 &witnesPtMin, btVector3 &witnesPtMax) const
Definition: btConvexPolyhedron.cpp:277
btConvexPolyhedron::testContainment
bool testContainment() const
Definition: btConvexPolyhedron.cpp:71
btConvexPolyhedron::m_vertices
btAlignedObjectArray< btVector3 > m_vertices
Definition: btConvexPolyhedron.h:42
btConvexPolyhedron::initialize2
void initialize2()
Definition: btConvexPolyhedron.cpp:177
btConvexPolyhedron::~btConvexPolyhedron
virtual ~btConvexPolyhedron()
Definition: btConvexPolyhedron.cpp:26
btConvexPolyhedron::m_faces
btAlignedObjectArray< btFace > m_faces
Definition: btConvexPolyhedron.h:43
btConvexPolyhedron::mC
btVector3 mC
Definition: btConvexPolyhedron.h:49
btConvexPolyhedron::m_radius
btScalar m_radius
Definition: btConvexPolyhedron.h:48
btConvexPolyhedron::btConvexPolyhedron
btConvexPolyhedron()
This file was written by Erwin Coumans Separating axis rest based on work from Pierre Terdiman,...
Definition: btConvexPolyhedron.cpp:23
btConvexPolyhedron::mE
btVector3 mE
Definition: btConvexPolyhedron.h:50
btConvexPolyhedron::m_uniqueEdges
btAlignedObjectArray< btVector3 > m_uniqueEdges
Definition: btConvexPolyhedron.h:44
btConvexPolyhedron::m_extents
btVector3 m_extents
Definition: btConvexPolyhedron.h:47
btConvexPolyhedron::initialize
void initialize()
Definition: btConvexPolyhedron.cpp:105
btHashMap
The btHashMap template class implements a generic and lightweight hashmap.
Definition: btHashMap.h:220
btHashMap::insert
void insert(const Key &key, const Value &value)
Definition: btHashMap.h:264
btHashMap::find
const Value * find(const Key &key) const
Definition: btHashMap.h:424
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
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::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
btInternalEdge
Definition: btConvexPolyhedron.cpp:58
btInternalEdge::m_face1
short int m_face1
Definition: btConvexPolyhedron.cpp:65
btInternalEdge::btInternalEdge
btInternalEdge()
Definition: btConvexPolyhedron.cpp:59
btInternalEdge::m_face0
short int m_face0
Definition: btConvexPolyhedron.cpp:64
btInternalVertexPair
Definition: btConvexPolyhedron.cpp:37
btInternalVertexPair::m_v1
short int m_v1
Definition: btConvexPolyhedron.cpp:46
btInternalVertexPair::getHash
int getHash() const
Definition: btConvexPolyhedron.cpp:47
btInternalVertexPair::m_v0
short int m_v0
Definition: btConvexPolyhedron.cpp:45
btInternalVertexPair::btInternalVertexPair
btInternalVertexPair(short int v0, short int v1)
Definition: btConvexPolyhedron.cpp:38
btInternalVertexPair::equals
bool equals(const btInternalVertexPair &other) const
Definition: btConvexPolyhedron.cpp:51