Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/kicklib/src/external/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @ 8071

Last change on this file since 8071 was 8071, checked in by rgrieder, 14 years ago
Merged ois_update branch (before it was renamed to mac_osx) into kicklib branch.
Property svn:eol-style set to `native`
File size: 13.7 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	btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
48	:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
49	m_penetrationDepthSolver(penetrationDepthSolver),
50	m_simplexSolver(simplexSolver),
51	m_minkowskiA(objectA),
52	m_minkowskiB(objectB),
53	m_shapeTypeA(objectA->getShapeType()),
54	m_shapeTypeB(objectB->getShapeType()),
55	m_marginA(objectA->getMargin()),
56	m_marginB(objectB->getMargin()),
57	m_ignoreMargin(false),
58	m_lastUsedMethod(-1),
59	m_catchDegeneracies(1)
60	{
61	}
62	btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
63	:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
64	m_penetrationDepthSolver(penetrationDepthSolver),
65	m_simplexSolver(simplexSolver),
66	m_minkowskiA(objectA),
67	m_minkowskiB(objectB),
68	m_shapeTypeA(shapeTypeA),
69	m_shapeTypeB(shapeTypeB),
70	m_marginA(marginA),
71	m_marginB(marginB),
72	m_ignoreMargin(false),
73	m_lastUsedMethod(-1),
74	m_catchDegeneracies(1)
75	{
76	}
77
78	void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
79	{
80	(void)swapResults;
81
82	getClosestPointsNonVirtual(input,output,debugDraw);
83	}
84
85	#ifdef __SPU__
86	void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
87	#else
88	void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
89	#endif
90	{
91	m_cachedSeparatingDistance = 0.f;
92
93	btScalar distance=btScalar(0.);
94	btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
95	btVector3 pointOnA,pointOnB;
96	btTransform localTransA = input.m_transformA;
97	btTransform localTransB = input.m_transformB;
98	btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
99	localTransA.getOrigin() -= positionOffset;
100	localTransB.getOrigin() -= positionOffset;
101
102	bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d();
103
104	btScalar marginA = m_marginA;
105	btScalar marginB = m_marginB;
106
107	gNumGjkChecks++;
108
109	#ifdef DEBUG_SPU_COLLISION_DETECTION
110	spu_printf("inside gjk\n");
111	#endif
112	//for CCD we don't use margins
113	if (m_ignoreMargin)
114	{
115	marginA = btScalar(0.);
116	marginB = btScalar(0.);
117	#ifdef DEBUG_SPU_COLLISION_DETECTION
118	spu_printf("ignoring margin\n");
119	#endif
120	}
121
122	m_curIter = 0;
123	int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
124	m_cachedSeparatingAxis.setValue(0,1,0);
125
126	bool isValid = false;
127	bool checkSimplex = false;
128	bool checkPenetration = true;
129	m_degenerateSimplex = 0;
130
131	m_lastUsedMethod = -1;
132
133	{
134	btScalar squaredDistance = BT_LARGE_FLOAT;
135	btScalar delta = btScalar(0.);
136
137	btScalar margin = marginA + marginB;
138
139
140
141	m_simplexSolver->reset();
142
143	for ( ; ; )
144	//while (true)
145	{
146
147	btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
148	btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
149
150	#if 1
151
152	btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
153	btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
154
155	// btVector3 pInA = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData[0]);//, &featureIndexA);
156	// btVector3 qInB = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData[1]);//, &featureIndexB);
157
158	#else
159	#ifdef __SPU__
160	btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
161	btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
162	#else
163	btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
164	btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
165	#ifdef TEST_NON_VIRTUAL
166	btVector3 pInAv = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
167	btVector3 qInBv = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
168	btAssert((pInAv-pInA).length() < 0.0001);
169	btAssert((qInBv-qInB).length() < 0.0001);
170	#endif //
171	#endif //__SPU__
172	#endif
173
174
175	btVector3 pWorld = localTransA(pInA);
176	btVector3 qWorld = localTransB(qInB);
177
178	#ifdef DEBUG_SPU_COLLISION_DETECTION
179	spu_printf("got local supporting vertices\n");
180	#endif
181
182	if (check2d)
183	{
184	pWorld[2] = 0.f;
185	qWorld[2] = 0.f;
186	}
187
188	btVector3 w = pWorld - qWorld;
189	delta = m_cachedSeparatingAxis.dot(w);
190
191	// potential exit, they don't overlap
192	if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
193	{
194	m_degenerateSimplex = 10;
195	checkSimplex=true;
196	//checkPenetration = false;
197	break;
198	}
199
200	//exit 0: the new point is already in the simplex, or we didn't come any closer
201	if (m_simplexSolver->inSimplex(w))
202	{
203	m_degenerateSimplex = 1;
204	checkSimplex = true;
205	break;
206	}
207	// are we getting any closer ?
208	btScalar f0 = squaredDistance - delta;
209	btScalar f1 = squaredDistance * REL_ERROR2;
210
211	if (f0 <= f1)
212	{
213	if (f0 <= btScalar(0.))
214	{
215	m_degenerateSimplex = 2;
216	} else
217	{
218	m_degenerateSimplex = 11;
219	}
220	checkSimplex = true;
221	break;
222	}
223
224	#ifdef DEBUG_SPU_COLLISION_DETECTION
225	spu_printf("addVertex 1\n");
226	#endif
227	//add current vertex to simplex
228	m_simplexSolver->addVertex(w, pWorld, qWorld);
229	#ifdef DEBUG_SPU_COLLISION_DETECTION
230	spu_printf("addVertex 2\n");
231	#endif
232	btVector3 newCachedSeparatingAxis;
233
234	//calculate the closest point to the origin (update vector v)
235	if (!m_simplexSolver->closest(newCachedSeparatingAxis))
236	{
237	m_degenerateSimplex = 3;
238	checkSimplex = true;
239	break;
240	}
241
242	if(newCachedSeparatingAxis.length2()<REL_ERROR2)
243	{
244	m_cachedSeparatingAxis = newCachedSeparatingAxis;
245	m_degenerateSimplex = 6;
246	checkSimplex = true;
247	break;
248	}
249
250	btScalar previousSquaredDistance = squaredDistance;
251	squaredDistance = newCachedSeparatingAxis.length2();
252	#if 0
253	///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
254	if (squaredDistance>previousSquaredDistance)
255	{
256	m_degenerateSimplex = 7;
257	squaredDistance = previousSquaredDistance;
258	checkSimplex = false;
259	break;
260	}
261	#endif //
262
263	m_cachedSeparatingAxis = newCachedSeparatingAxis;
264
265	//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
266
267	//are we getting any closer ?
268	if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
269	{
270	m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
271	checkSimplex = true;
272	m_degenerateSimplex = 12;
273
274	break;
275	}
276
277	//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
278	if (m_curIter++ > gGjkMaxIter)
279	{
280	#if defined(DEBUG) \|\| defined (_DEBUG) \|\| defined (DEBUG_SPU_COLLISION_DETECTION)
281
282	printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
283	printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
284	m_cachedSeparatingAxis.getX(),
285	m_cachedSeparatingAxis.getY(),
286	m_cachedSeparatingAxis.getZ(),
287	squaredDistance,
288	m_minkowskiA->getShapeType(),
289	m_minkowskiB->getShapeType());
290
291	#endif
292	break;
293
294	}
295
296
297	bool check = (!m_simplexSolver->fullSimplex());
298	//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
299
300	if (!check)
301	{
302	//do we need this backup_closest here ?
303	m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
304	m_degenerateSimplex = 13;
305	break;
306	}
307	}
308
309	if (checkSimplex)
310	{
311	m_simplexSolver->compute_points(pointOnA, pointOnB);
312	normalInB = pointOnA-pointOnB;
313	btScalar lenSqr =m_cachedSeparatingAxis.length2();
314
315	//valid normal
316	if (lenSqr < 0.0001)
317	{
318	m_degenerateSimplex = 5;
319	}
320	if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
321	{
322	btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
323	normalInB *= rlen; //normalize
324	btScalar s = btSqrt(squaredDistance);
325
326	btAssert(s > btScalar(0.0));
327	pointOnA -= m_cachedSeparatingAxis * (marginA / s);
328	pointOnB += m_cachedSeparatingAxis * (marginB / s);
329	distance = ((btScalar(1.)/rlen) - margin);
330	isValid = true;
331
332	m_lastUsedMethod = 1;
333	} else
334	{
335	m_lastUsedMethod = 2;
336	}
337	}
338
339	bool catchDegeneratePenetrationCase =
340	(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
341
342	//if (checkPenetration && !isValid)
343	if (checkPenetration && (!isValid \|\| catchDegeneratePenetrationCase ))
344	{
345	//penetration case
346
347	//if there is no way to handle penetrations, bail out
348	if (m_penetrationDepthSolver)
349	{
350	// Penetration depth case.
351	btVector3 tmpPointOnA,tmpPointOnB;
352
353	gNumDeepPenetrationChecks++;
354	m_cachedSeparatingAxis.setZero();
355
356	bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
357	*m_simplexSolver,
358	m_minkowskiA,m_minkowskiB,
359	localTransA,localTransB,
360	m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
361	debugDraw,input.m_stackAlloc
362	);
363
364
365	if (isValid2)
366	{
367	btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
368	btScalar lenSqr = tmpNormalInB.length2();
369	if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
370	{
371	tmpNormalInB = m_cachedSeparatingAxis;
372	lenSqr = m_cachedSeparatingAxis.length2();
373	}
374
375	if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
376	{
377	tmpNormalInB /= btSqrt(lenSqr);
378	btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
379	//only replace valid penetrations when the result is deeper (check)
380	if (!isValid \|\| (distance2 < distance))
381	{
382	distance = distance2;
383	pointOnA = tmpPointOnA;
384	pointOnB = tmpPointOnB;
385	normalInB = tmpNormalInB;
386	isValid = true;
387	m_lastUsedMethod = 3;
388	} else
389	{
390	m_lastUsedMethod = 8;
391	}
392	} else
393	{
394	m_lastUsedMethod = 9;
395	}
396	} else
397
398	{
399	///this is another degenerate case, where the initial GJK calculation reports a degenerate case
400	///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
401	///reports a valid positive distance. Use the results of the second GJK instead of failing.
402	///thanks to Jacob.Langford for the reproduction case
403	///http://code.google.com/p/bullet/issues/detail?id=250
404
405
406	if (m_cachedSeparatingAxis.length2() > btScalar(0.))
407	{
408	btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin;
409	//only replace valid distances when the distance is less
410	if (!isValid \|\| (distance2 < distance))
411	{
412	distance = distance2;
413	pointOnA = tmpPointOnA;
414	pointOnB = tmpPointOnB;
415	pointOnA -= m_cachedSeparatingAxis * marginA ;
416	pointOnB += m_cachedSeparatingAxis * marginB ;
417	normalInB = m_cachedSeparatingAxis;
418	normalInB.normalize();
419	isValid = true;
420	m_lastUsedMethod = 6;
421	} else
422	{
423	m_lastUsedMethod = 5;
424	}
425	}
426	}
427
428	}
429
430	}
431	}
432
433
434
435	if (isValid && ((distance < 0) \|\| (distance*distance < input.m_maximumDistanceSquared)))
436	{
437	#if 0
438	///some debugging
439	// if (check2d)
440	{
441	printf("n = %2.3f,%2.3f,%2.3f. ",normalInB[0],normalInB[1],normalInB[2]);
442	printf("distance = %2.3f exit=%d deg=%d\n",distance,m_lastUsedMethod,m_degenerateSimplex);
443	}
444	#endif
445
446	m_cachedSeparatingAxis = normalInB;
447	m_cachedSeparatingDistance = distance;
448
449	output.addContactPoint(
450	normalInB,
451	pointOnB+positionOffset,
452	distance);
453
454	}
455
456
457	}
458
459
460
461
462

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

Download in other formats: