Context Navigation

btGjkPairDetector.cpp @ 7564

Last change on this file since 7564 was 7564, checked in by rgrieder, 14 years ago
Renamed all symbols called "check" because of macro collisions on OS X. Inserted something like an assert in btGjkPairDetector.cpp with the hope that I don't have to modify that 'check' there.
Property svn:eol-style set to `native`
File size: 10.5 KB

Line
1	/*
2	Bullet Continuous Collision Detection and Physics Library
3	Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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	#include "btGjkPairDetector.h"
17	#include "BulletCollision/CollisionShapes/btConvexShape.h"
18	#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
19	#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
20
21
22
23	#if defined(DEBUG) \|\| defined (_DEBUG)
24	//#define TEST_NON_VIRTUAL 1
25	#include <stdio.h> //for debug printf
26	#ifdef __SPU__
27	#include <spu_printf.h>
28	#define printf spu_printf
29	//#define DEBUG_SPU_COLLISION_DETECTION 1
30	#endif //__SPU__
31	#endif
32
33	//must be above the machine epsilon
34	#define REL_ERROR2 btScalar(1.0e-6)
35
36	//temp globals, to improve GJK/EPA/penetration calculations
37	int gNumDeepPenetrationChecks = 0;
38	int gNumGjkChecks = 0;
39
40	#ifdef check
41	struct CompilerError
42	{
43	void CompilerError() {}
44	};
45	#endif
46
47
48	btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
49	:m_cachedSeparatingAxis(btScalar(0.),btScalar(0.),btScalar(1.)),
50	m_penetrationDepthSolver(penetrationDepthSolver),
51	m_simplexSolver(simplexSolver),
52	m_minkowskiA(objectA),
53	m_minkowskiB(objectB),
54	m_ignoreMargin(false),
55	m_lastUsedMethod(-1),
56	m_catchDegeneracies(1)
57	{
58	}
59
60	void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
61	{
62	m_cachedSeparatingDistance = 0.f;
63
64	btScalar distance=btScalar(0.);
65	btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
66	btVector3 pointOnA,pointOnB;
67	btTransform localTransA = input.m_transformA;
68	btTransform localTransB = input.m_transformB;
69	btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
70	localTransA.getOrigin() -= positionOffset;
71	localTransB.getOrigin() -= positionOffset;
72
73	#ifdef __SPU__
74	btScalar marginA = m_minkowskiA->getMarginNonVirtual();
75	btScalar marginB = m_minkowskiB->getMarginNonVirtual();
76	#else
77	btScalar marginA = m_minkowskiA->getMargin();
78	btScalar marginB = m_minkowskiB->getMargin();
79	#ifdef TEST_NON_VIRTUAL
80	btScalar marginAv = m_minkowskiA->getMarginNonVirtual();
81	btScalar marginBv = m_minkowskiB->getMarginNonVirtual();
82	btAssert(marginA == marginAv);
83	btAssert(marginB == marginBv);
84	#endif //TEST_NON_VIRTUAL
85	#endif
86
87
88
89	gNumGjkChecks++;
90
91	#ifdef DEBUG_SPU_COLLISION_DETECTION
92	spu_printf("inside gjk\n");
93	#endif
94	//for CCD we don't use margins
95	if (m_ignoreMargin)
96	{
97	marginA = btScalar(0.);
98	marginB = btScalar(0.);
99	#ifdef DEBUG_SPU_COLLISION_DETECTION
100	spu_printf("ignoring margin\n");
101	#endif
102	}
103
104	m_curIter = 0;
105	int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
106	m_cachedSeparatingAxis.setValue(0,1,0);
107
108	bool isValid = false;
109	bool checkSimplex = false;
110	bool checkPenetration = true;
111	m_degenerateSimplex = 0;
112
113	m_lastUsedMethod = -1;
114
115	{
116	btScalar squaredDistance = SIMD_INFINITY;
117	btScalar delta = btScalar(0.);
118
119	btScalar margin = marginA + marginB;
120
121
122
123	m_simplexSolver->reset();
124
125	for ( ; ; )
126	//while (true)
127	{
128
129	btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
130	btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
131
132	#ifdef __SPU__
133	btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
134	btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
135	#else
136	btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
137	btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
138	#ifdef TEST_NON_VIRTUAL
139	btVector3 pInAv = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
140	btVector3 qInBv = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
141	btAssert((pInAv-pInA).length() < 0.0001);
142	btAssert((qInBv-qInB).length() < 0.0001);
143	#endif //
144	#endif //__SPU__
145
146	btVector3 pWorld = localTransA(pInA);
147	btVector3 qWorld = localTransB(qInB);
148
149	#ifdef DEBUG_SPU_COLLISION_DETECTION
150	spu_printf("got local supporting vertices\n");
151	#endif
152
153	btVector3 w = pWorld - qWorld;
154	delta = m_cachedSeparatingAxis.dot(w);
155
156	// potential exit, they don't overlap
157	if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
158	{
159	checkSimplex=true;
160	//checkPenetration = false;
161	break;
162	}
163
164	//exit 0: the new point is already in the simplex, or we didn't come any closer
165	if (m_simplexSolver->inSimplex(w))
166	{
167	m_degenerateSimplex = 1;
168	checkSimplex = true;
169	break;
170	}
171	// are we getting any closer ?
172	btScalar f0 = squaredDistance - delta;
173	btScalar f1 = squaredDistance * REL_ERROR2;
174
175	if (f0 <= f1)
176	{
177	if (f0 <= btScalar(0.))
178	{
179	m_degenerateSimplex = 2;
180	}
181	checkSimplex = true;
182	break;
183	}
184
185	#ifdef DEBUG_SPU_COLLISION_DETECTION
186	spu_printf("addVertex 1\n");
187	#endif
188	//add current vertex to simplex
189	m_simplexSolver->addVertex(w, pWorld, qWorld);
190	#ifdef DEBUG_SPU_COLLISION_DETECTION
191	spu_printf("addVertex 2\n");
192	#endif
193	//calculate the closest point to the origin (update vector v)
194	if (!m_simplexSolver->closest(m_cachedSeparatingAxis))
195	{
196	m_degenerateSimplex = 3;
197	checkSimplex = true;
198	break;
199	}
200
201	if(m_cachedSeparatingAxis.length2()<REL_ERROR2)
202	{
203	m_degenerateSimplex = 6;
204	checkSimplex = true;
205	break;
206	}
207
208	btScalar previousSquaredDistance = squaredDistance;
209	squaredDistance = m_cachedSeparatingAxis.length2();
210
211	//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
212
213	//are we getting any closer ?
214	if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
215	{
216	m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
217	checkSimplex = true;
218	break;
219	}
220
221	//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
222	if (m_curIter++ > gGjkMaxIter)
223	{
224	#if defined(DEBUG) \|\| defined (_DEBUG) \|\| defined (DEBUG_SPU_COLLISION_DETECTION)
225
226	printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
227	printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
228	m_cachedSeparatingAxis.getX(),
229	m_cachedSeparatingAxis.getY(),
230	m_cachedSeparatingAxis.getZ(),
231	squaredDistance,
232	m_minkowskiA->getShapeType(),
233	m_minkowskiB->getShapeType());
234
235	#endif
236	break;
237
238	}
239
240
241	bool check = (!m_simplexSolver->fullSimplex());
242	//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
243
244	if (!check)
245	{
246	//do we need this backup_closest here ?
247	m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
248	break;
249	}
250	}
251
252	if (checkSimplex)
253	{
254	m_simplexSolver->compute_points(pointOnA, pointOnB);
255	normalInB = pointOnA-pointOnB;
256	btScalar lenSqr = m_cachedSeparatingAxis.length2();
257	//valid normal
258	if (lenSqr < 0.0001)
259	{
260	m_degenerateSimplex = 5;
261	}
262	if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
263	{
264	btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
265	normalInB *= rlen; //normalize
266	btScalar s = btSqrt(squaredDistance);
267
268	btAssert(s > btScalar(0.0));
269	pointOnA -= m_cachedSeparatingAxis * (marginA / s);
270	pointOnB += m_cachedSeparatingAxis * (marginB / s);
271	distance = ((btScalar(1.)/rlen) - margin);
272	isValid = true;
273
274	m_lastUsedMethod = 1;
275	} else
276	{
277	m_lastUsedMethod = 2;
278	}
279	}
280
281	bool catchDegeneratePenetrationCase =
282	(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
283
284	//if (checkPenetration && !isValid)
285	if (checkPenetration && (!isValid \|\| catchDegeneratePenetrationCase ))
286	{
287	//penetration case
288
289	//if there is no way to handle penetrations, bail out
290	if (m_penetrationDepthSolver)
291	{
292	// Penetration depth case.
293	btVector3 tmpPointOnA,tmpPointOnB;
294
295	gNumDeepPenetrationChecks++;
296
297	bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
298	*m_simplexSolver,
299	m_minkowskiA,m_minkowskiB,
300	localTransA,localTransB,
301	m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
302	debugDraw,input.m_stackAlloc
303	);
304
305	if (isValid2)
306	{
307	btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
308	btScalar lenSqr = tmpNormalInB.length2();
309	if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
310	{
311	tmpNormalInB /= btSqrt(lenSqr);
312	btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
313	//only replace valid penetrations when the result is deeper (check)
314	if (!isValid \|\| (distance2 < distance))
315	{
316	distance = distance2;
317	pointOnA = tmpPointOnA;
318	pointOnB = tmpPointOnB;
319	normalInB = tmpNormalInB;
320	isValid = true;
321	m_lastUsedMethod = 3;
322	} else
323	{
324
325	}
326	} else
327	{
328	//isValid = false;
329	m_lastUsedMethod = 4;
330	}
331	} else
332	{
333	m_lastUsedMethod = 5;
334	}
335
336	}
337	}
338	}
339
340	if (isValid)
341	{
342	#ifdef __SPU__
343	//spu_printf("distance\n");
344	#endif //__CELLOS_LV2__
345
346
347	#ifdef DEBUG_SPU_COLLISION_DETECTION
348	spu_printf("output 1\n");
349	#endif
350	m_cachedSeparatingAxis = normalInB;
351	m_cachedSeparatingDistance = distance;
352
353	output.addContactPoint(
354	normalInB,
355	pointOnB+positionOffset,
356	distance);
357
358	#ifdef DEBUG_SPU_COLLISION_DETECTION
359	spu_printf("output 2\n");
360	#endif
361	//printf("gjk add:%f",distance);
362	}
363
364
365	}
366
367
368
369
370

Note: See TracBrowser for help on using the repository browser.

Context Navigation

source: code/branches/ois_update/src/external/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @ 7564

Download in other formats: