Bug Summary

File:src/external/bullet/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp
Location:line 248, column 6
Description:Value stored to 'numContacts' during its initialization is never read

Annotated Source Code

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
17///This file was written by Erwin Coumans
18///Separating axis rest based on work from Pierre Terdiman, see
19///And contact clipping based on work from Simon Hobbs
20
21
22#include "btPolyhedralContactClipping.h"
23#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
24
25#include <float.h> //for FLT_MAX
26
27
28// Clips a face to the back of a plane
29void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS)
30{
31
32 int ve;
33 btScalar ds, de;
34 int numVerts = pVtxIn.size();
35 if (numVerts < 2)
36 return;
37
38 btVector3 firstVertex=pVtxIn[pVtxIn.size()-1];
39 btVector3 endVertex = pVtxIn[0];
40
41 ds = planeNormalWS.dot(firstVertex)+planeEqWS;
42
43 for (ve = 0; ve < numVerts; ve++)
44 {
45 endVertex=pVtxIn[ve];
46
47 de = planeNormalWS.dot(endVertex)+planeEqWS;
48
49 if (ds<0)
50 {
51 if (de<0)
52 {
53 // Start < 0, end < 0, so output endVertex
54 ppVtxOut.push_back(endVertex);
55 }
56 else
57 {
58 // Start < 0, end >= 0, so output intersection
59 ppVtxOut.push_back( firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de))));
60 }
61 }
62 else
63 {
64 if (de<0)
65 {
66 // Start >= 0, end < 0 so output intersection and end
67 ppVtxOut.push_back(firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de))));
68 ppVtxOut.push_back(endVertex);
69 }
70 }
71 firstVertex = endVertex;
72 ds = de;
73 }
74}
75#include <stdio.h>
76
77
78static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btVector3& sep_axis, float& depth)
79{
80 float Min0,Max0;
81 float Min1,Max1;
82 hullA.project(transA,sep_axis, Min0, Max0);
83 hullB.project(transB, sep_axis, Min1, Max1);
84
85 if(Max0<Min1 || Max1<Min0)
86 return false;
87
88 float d0 = Max0 - Min1;
89 assert(d0>=0.0f)((d0>=0.0f) ? static_cast<void> (0) : __assert_fail (
"d0>=0.0f", "/home/jenkins/workspace/orxonox_qc_trunk_checks/src/external/bullet/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp"
, 89, __PRETTY_FUNCTION__));
90 float d1 = Max1 - Min0;
91 assert(d1>=0.0f)((d1>=0.0f) ? static_cast<void> (0) : __assert_fail (
"d1>=0.0f", "/home/jenkins/workspace/orxonox_qc_trunk_checks/src/external/bullet/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp"
, 91, __PRETTY_FUNCTION__));
92 depth = d0<d1 ? d0:d1;
93 return true;
94}
95
96
97
98static int gActualSATPairTests=0;
99
100inline bool IsAlmostZero(const btVector3& v)
101{
102 if(fabsf(v.x())>1e-6 || fabsf(v.y())>1e-6 || fabsf(v.z())>1e-6) return false;
103 return true;
104}
105
106
107bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep)
108{
109 gActualSATPairTests++;
110
111#ifdef TEST_INTERNAL_OBJECTS
112 const btVector3 c0 = transA * hullA.mLocalCenter;
113 const btVector3 c1 = transB * hullB.mLocalCenter;
114 const btVector3 DeltaC2 = c0 - c1;
115#endif
116
117 float dmin = FLT_MAX3.40282347e+38F;
118 int curPlaneTests=0;
119
120 int numFacesA = hullA.m_faces.size();
121 // Test normals from hullA
122 for(int i=0;i<numFacesA;i++)
123 {
124 const btVector3 Normal(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]);
125 const btVector3 faceANormalWS = transA.getBasis() * Normal;
126
127 curPlaneTests++;
128#ifdef TEST_INTERNAL_OBJECTS
129 gExpectedNbTests++;
130 if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, faceANormalWS, hullA, hullB, dmin))
131 continue;
132 gActualNbTests++;
133#endif
134
135 float d;
136 if(!TestSepAxis( hullA, hullB, transA,transB, faceANormalWS, d))
137 return false;
138
139 if(d<dmin)
140 {
141 dmin = d;
142 sep = faceANormalWS;
143 }
144 }
145
146 int numFacesB = hullB.m_faces.size();
147 // Test normals from hullB
148 for(int i=0;i<numFacesB;i++)
149 {
150 const btVector3 Normal(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]);
151 const btVector3 WorldNormal = transB.getBasis() * Normal;
152
153 curPlaneTests++;
154#ifdef TEST_INTERNAL_OBJECTS
155 gExpectedNbTests++;
156 if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin))
157 continue;
158 gActualNbTests++;
159#endif
160
161 float d;
162 if(!TestSepAxis(hullA, hullB,transA,transB, WorldNormal,d))
163 return false;
164
165 if(d<dmin)
166 {
167 dmin = d;
168 sep = WorldNormal;
169 }
170 }
171
172 btVector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
173
174 int curEdgeEdge = 0;
175 // Test edges
176 for(int e0=0;e0<hullA.m_uniqueEdges.size();e0++)
177 {
178 const btVector3 edge0 = hullA.m_uniqueEdges[e0];
179 const btVector3 WorldEdge0 = transA.getBasis() * edge0;
180 for(int e1=0;e1<hullB.m_uniqueEdges.size();e1++)
181 {
182 const btVector3 edge1 = hullB.m_uniqueEdges[e1];
183 const btVector3 WorldEdge1 = transB.getBasis() * edge1;
184
185 btVector3 Cross = WorldEdge0.cross(WorldEdge1);
186 curEdgeEdge++;
187 if(!IsAlmostZero(Cross))
188 {
189 Cross = Cross.normalize();
190
191#ifdef TEST_INTERNAL_OBJECTS
192 gExpectedNbTests++;
193 if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin))
194 continue;
195 gActualNbTests++;
196#endif
197
198 float dist;
199 if(!TestSepAxis( hullA, hullB, transA,transB, Cross, dist))
200 return false;
201
202 if(dist<dmin)
203 {
204 dmin = dist;
205 sep = Cross;
206 }
207 }
208 }
209
210 }
211
212 const btVector3 deltaC = transB.getOrigin() - transA.getOrigin();
213 if((deltaC.dot(sep))>0.0f)
214 sep = -sep;
215
216 return true;
217}
218
219void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
220{
221 btVertexArray worldVertsB2;
222 btVertexArray* pVtxIn = &worldVertsB1;
223 btVertexArray* pVtxOut = &worldVertsB2;
224 pVtxOut->reserve(pVtxIn->size());
225
226 int closestFaceA=-1;
227 {
228 btScalar dmin = FLT_MAX3.40282347e+38F;
229 for(int face=0;face<hullA.m_faces.size();face++)
230 {
231 const btVector3 Normal(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]);
232 const btVector3 faceANormalWS = transA.getBasis() * Normal;
233
234 btScalar d = faceANormalWS.dot(separatingNormal);
235 if (d < dmin)
236 {
237 dmin = d;
238 closestFaceA = face;
239 }
240 }
241 }
242 if (closestFaceA<0)
243 return;
244
245 const btFace& polyA = hullA.m_faces[closestFaceA];
246
247 // clip polygon to back of planes of all faces of hull A that are adjacent to witness face
248 int numContacts = pVtxIn->size();
Value stored to 'numContacts' during its initialization is never read
249 int numVerticesA = polyA.m_indices.size();
250 for(int e0=0;e0<numVerticesA;e0++)
251 {
252 /*const btVector3& a = hullA.m_vertices[polyA.m_indices[e0]];
253 const btVector3& b = hullA.m_vertices[polyA.m_indices[(e0+1)%numVerticesA]];
254 const btVector3 edge0 = a - b;
255 const btVector3 WorldEdge0 = transA.getBasis() * edge0;
256 */
257
258 int otherFace = polyA.m_connectedFaces[e0];
259 btVector3 localPlaneNormal (hullA.m_faces[otherFace].m_plane[0],hullA.m_faces[otherFace].m_plane[1],hullA.m_faces[otherFace].m_plane[2]);
260 btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3];
261
262 btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
263 btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
264 //clip face
265
266 clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
267 btSwap(pVtxIn,pVtxOut);
268 pVtxOut->resize(0);
269 }
270
271
272
273//#define ONLY_REPORT_DEEPEST_POINT
274
275 btVector3 point;
276
277
278 // only keep points that are behind the witness face
279 {
280 btVector3 localPlaneNormal (polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]);
281 btScalar localPlaneEq = polyA.m_plane[3];
282 btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
283 btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
284 for (int i=0;i<pVtxIn->size();i++)
285 {
286
287 btScalar depth = planeNormalWS.dot(pVtxIn->at(i))+planeEqWS;
288 if (depth <=maxDist && depth >=minDist)
289 {
290 btVector3 point = pVtxIn->at(i);
291#ifdef ONLY_REPORT_DEEPEST_POINT
292 curMaxDist = depth;
293#else
294#if 0
295 if (depth<-3)
296 {
297 printf("error in btPolyhedralContactClipping depth = %f\n", depth);
298 printf("likely wrong separatingNormal passed in\n");
299 }
300#endif
301 resultOut.addContactPoint(separatingNormal,point,depth);
302#endif
303 }
304 }
305 }
306#ifdef ONLY_REPORT_DEEPEST_POINT
307 if (curMaxDist<maxDist)
308 {
309 resultOut.addContactPoint(separatingNormal,point,curMaxDist);
310 }
311#endif //ONLY_REPORT_DEEPEST_POINT
312
313}
314
315void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
316{
317
318 btScalar curMaxDist=maxDist;
319 int closestFaceB=-1;
320
321 {
322 btScalar dmax = -FLT_MAX3.40282347e+38F;
323 for(int face=0;face<hullB.m_faces.size();face++)
324 {
325 const btVector3 Normal(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
326 const btVector3 WorldNormal = transB.getBasis() * Normal;
327
328 btScalar d = WorldNormal.dot(separatingNormal);
329 if (d > dmax)
330 {
331 dmax = d;
332 closestFaceB = face;
333 }
334 }
335 }
336
337
338
339 if (closestFaceB<0)
340 {
341 return;
342 }
343
344
345
346 // setup initial clip face (minimizing face from hull B)
347 btVertexArray worldVertsB1;
348 {
349 const btFace& polyB = hullB.m_faces[closestFaceB];
350 const int numVertices = polyB.m_indices.size();
351 for(int e0=0;e0<numVertices;e0++)
352 {
353 const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]];
354 worldVertsB1.push_back(transB*b);
355 }
356 }
357
358 clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, minDist, maxDist,resultOut);
359
360}