Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/trunk/src/external/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @ 5738

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

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

Download in other formats: