2 Bullet Continuous Collision Detection and Physics Library
\r
3 Copyright (c) 2011 Advanced Micro Devices, Inc. http://bulletphysics.org
\r
5 This software is provided 'as-is', without any express or implied warranty.
\r
6 In no event will the authors be held liable for any damages arising from the use of this software.
\r
7 Permission is granted to anyone to use this software for any purpose,
\r
8 including commercial applications, and to alter it and redistribute it freely,
\r
9 subject to the following restrictions:
\r
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.
\r
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
\r
13 3. This notice may not be removed or altered from any source distribution.
\r
17 ///This file was written by Erwin Coumans
\r
18 ///Separating axis rest based on work from Pierre Terdiman, see
\r
19 ///And contact clipping based on work from Simon Hobbs
\r
21 #include "btConvexPolyhedron.h"
\r
22 #include "LinearMath/btHashMap.h"
\r
24 btConvexPolyhedron::btConvexPolyhedron()
\r
28 btConvexPolyhedron::~btConvexPolyhedron()
\r
34 inline bool IsAlmostZero(const btVector3& v)
\r
36 if(fabsf(v.x())>1e-6 || fabsf(v.y())>1e-6 || fabsf(v.z())>1e-6) return false;
\r
40 struct btInternalVertexPair
\r
42 btInternalVertexPair(short int v0,short int v1)
\r
53 return m_v0+(m_v1<<16);
\r
55 bool equals(const btInternalVertexPair& other) const
\r
57 return m_v0==other.m_v0 && m_v1==other.m_v1;
\r
61 struct btInternalEdge
\r
74 #ifdef TEST_INTERNAL_OBJECTS
\r
75 bool btConvexPolyhedron::testContainment() const
\r
77 for(int p=0;p<8;p++)
\r
80 if(p==0) LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], m_extents[2]);
\r
81 else if(p==1) LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], -m_extents[2]);
\r
82 else if(p==2) LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], m_extents[2]);
\r
83 else if(p==3) LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], -m_extents[2]);
\r
84 else if(p==4) LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], m_extents[2]);
\r
85 else if(p==5) LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], -m_extents[2]);
\r
86 else if(p==6) LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], m_extents[2]);
\r
87 else if(p==7) LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], -m_extents[2]);
\r
89 for(int i=0;i<m_faces.size();i++)
\r
91 const btVector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
\r
92 const btScalar d = LocalPt.dot(Normal) + m_faces[i].m_plane[3];
\r
101 void btConvexPolyhedron::initialize()
\r
104 btHashMap<btInternalVertexPair,btInternalEdge> edges;
\r
106 btScalar TotalArea = 0.0f;
\r
108 m_localCenter.setValue(0, 0, 0);
\r
109 for(int i=0;i<m_faces.size();i++)
\r
111 int numVertices = m_faces[i].m_indices.size();
\r
112 int NbTris = numVertices;
\r
113 for(int j=0;j<NbTris;j++)
\r
115 int k = (j+1)%numVertices;
\r
116 btInternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]);
\r
117 btInternalEdge* edptr = edges.find(vp);
\r
118 btVector3 edge = m_vertices[vp.m_v1]-m_vertices[vp.m_v0];
\r
121 bool found = false;
\r
123 for (int p=0;p<m_uniqueEdges.size();p++)
\r
126 if (IsAlmostZero(m_uniqueEdges[p]-edge) ||
\r
127 IsAlmostZero(m_uniqueEdges[p]+edge))
\r
136 m_uniqueEdges.push_back(edge);
\r
141 btAssert(edptr->m_face0>=0);
\r
142 btAssert(edptr->m_face1<0);
\r
143 edptr->m_face1 = i;
\r
148 edges.insert(vp,ed);
\r
153 #ifdef USE_CONNECTED_FACES
\r
154 for(int i=0;i<m_faces.size();i++)
\r
156 int numVertices = m_faces[i].m_indices.size();
\r
157 m_faces[i].m_connectedFaces.resize(numVertices);
\r
159 for(int j=0;j<numVertices;j++)
\r
161 int k = (j+1)%numVertices;
\r
162 btInternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]);
\r
163 btInternalEdge* edptr = edges.find(vp);
\r
165 btAssert(edptr->m_face0>=0);
\r
166 btAssert(edptr->m_face1>=0);
\r
168 int connectedFace = (edptr->m_face0==i)?edptr->m_face1:edptr->m_face0;
\r
169 m_faces[i].m_connectedFaces[j] = connectedFace;
\r
172 #endif//USE_CONNECTED_FACES
\r
174 for(int i=0;i<m_faces.size();i++)
\r
176 int numVertices = m_faces[i].m_indices.size();
\r
177 int NbTris = numVertices-2;
\r
179 const btVector3& p0 = m_vertices[m_faces[i].m_indices[0]];
\r
180 for(int j=1;j<=NbTris;j++)
\r
182 int k = (j+1)%numVertices;
\r
183 const btVector3& p1 = m_vertices[m_faces[i].m_indices[j]];
\r
184 const btVector3& p2 = m_vertices[m_faces[i].m_indices[k]];
\r
185 btScalar Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f;
\r
186 btVector3 Center = (p0+p1+p2)/3.0f;
\r
187 m_localCenter += Area * Center;
\r
191 m_localCenter /= TotalArea;
\r
196 #ifdef TEST_INTERNAL_OBJECTS
\r
199 m_radius = FLT_MAX;
\r
200 for(int i=0;i<m_faces.size();i++)
\r
202 const btVector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
\r
203 const btScalar dist = btFabs(m_localCenter.dot(Normal) + m_faces[i].m_plane[3]);
\r
209 btScalar MinX = FLT_MAX;
\r
210 btScalar MinY = FLT_MAX;
\r
211 btScalar MinZ = FLT_MAX;
\r
212 btScalar MaxX = -FLT_MAX;
\r
213 btScalar MaxY = -FLT_MAX;
\r
214 btScalar MaxZ = -FLT_MAX;
\r
215 for(int i=0; i<m_vertices.size(); i++)
\r
217 const btVector3& pt = m_vertices[i];
\r
218 if(pt.x()<MinX) MinX = pt.x();
\r
219 if(pt.x()>MaxX) MaxX = pt.x();
\r
220 if(pt.y()<MinY) MinY = pt.y();
\r
221 if(pt.y()>MaxY) MaxY = pt.y();
\r
222 if(pt.z()<MinZ) MinZ = pt.z();
\r
223 if(pt.z()>MaxZ) MaxZ = pt.z();
\r
225 mC.setValue(MaxX+MinX, MaxY+MinY, MaxZ+MinZ);
\r
226 mE.setValue(MaxX-MinX, MaxY-MinY, MaxZ-MinZ);
\r
230 // const btScalar r = m_radius / sqrtf(2.0f);
\r
231 const btScalar r = m_radius / sqrtf(3.0f);
\r
232 const int LargestExtent = mE.maxAxis();
\r
233 const btScalar Step = (mE[LargestExtent]*0.5f - r)/1024.0f;
\r
234 m_extents[0] = m_extents[1] = m_extents[2] = r;
\r
235 m_extents[LargestExtent] = mE[LargestExtent]*0.5f;
\r
236 bool FoundBox = false;
\r
237 for(int j=0;j<1024;j++)
\r
239 if(testContainment())
\r
245 m_extents[LargestExtent] -= Step;
\r
249 m_extents[0] = m_extents[1] = m_extents[2] = r;
\r
254 const btScalar Step = (m_radius - r)/1024.0f;
\r
255 const int e0 = (1<<LargestExtent) & 3;
\r
256 const int e1 = (1<<e0) & 3;
\r
258 for(int j=0;j<1024;j++)
\r
260 const btScalar Saved0 = m_extents[e0];
\r
261 const btScalar Saved1 = m_extents[e1];
\r
262 m_extents[e0] += Step;
\r
263 m_extents[e1] += Step;
\r
265 if(!testContainment())
\r
267 m_extents[e0] = Saved0;
\r
268 m_extents[e1] = Saved1;
\r
278 void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const
\r
282 int numVerts = m_vertices.size();
\r
283 for(int i=0;i<numVerts;i++)
\r
285 btVector3 pt = trans * m_vertices[i];
\r
286 btScalar dp = pt.dot(dir);
\r
287 if(dp < min) min = dp;
\r
288 if(dp > max) max = dp;
\r
292 btScalar tmp = min;
\r