Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/physics_merge/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @ 2442

Last change on this file since 2442 was 2442, checked in by rgrieder, 15 years ago
Finally merged physics stuff. Target is physics_merge because I'll have to do some testing first.
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	checkPenetration = false;
154	break;
155	}
156
157	//exit 0: the new point is already in the simplex, or we didn't come any closer
158	if (m_simplexSolver->inSimplex(w))
159	{
160	m_degenerateSimplex = 1;
161	checkSimplex = true;
162	break;
163	}
164	// are we getting any closer ?
165	btScalar f0 = squaredDistance - delta;
166	btScalar f1 = squaredDistance * REL_ERROR2;
167
168	if (f0 <= f1)
169	{
170	if (f0 <= btScalar(0.))
171	{
172	m_degenerateSimplex = 2;
173	}
174	checkSimplex = true;
175	break;
176	}
177
178	#ifdef DEBUG_SPU_COLLISION_DETECTION
179	spu_printf("addVertex 1\n");
180	#endif
181	//add current vertex to simplex
182	m_simplexSolver->addVertex(w, pWorld, qWorld);
183	#ifdef DEBUG_SPU_COLLISION_DETECTION
184	spu_printf("addVertex 2\n");
185	#endif
186	//calculate the closest point to the origin (update vector v)
187	if (!m_simplexSolver->closest(m_cachedSeparatingAxis))
188	{
189	m_degenerateSimplex = 3;
190	checkSimplex = true;
191	break;
192	}
193
194	if(m_cachedSeparatingAxis.length2()<REL_ERROR2)
195	{
196	m_degenerateSimplex = 6;
197	checkSimplex = true;
198	break;
199	}
200
201	btScalar previousSquaredDistance = squaredDistance;
202	squaredDistance = m_cachedSeparatingAxis.length2();
203
204	//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
205
206	//are we getting any closer ?
207	if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
208	{
209	m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
210	checkSimplex = true;
211	break;
212	}
213
214	//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
215	if (m_curIter++ > gGjkMaxIter)
216	{
217	#if defined(DEBUG) \|\| defined (_DEBUG) \|\| defined (DEBUG_SPU_COLLISION_DETECTION)
218
219	printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
220	printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
221	m_cachedSeparatingAxis.getX(),
222	m_cachedSeparatingAxis.getY(),
223	m_cachedSeparatingAxis.getZ(),
224	squaredDistance,
225	m_minkowskiA->getShapeType(),
226	m_minkowskiB->getShapeType());
227
228	#endif
229	break;
230
231	}
232
233
234	bool check = (!m_simplexSolver->fullSimplex());
235	//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
236
237	if (!check)
238	{
239	//do we need this backup_closest here ?
240	m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
241	break;
242	}
243	}
244
245	if (checkSimplex)
246	{
247	m_simplexSolver->compute_points(pointOnA, pointOnB);
248	normalInB = pointOnA-pointOnB;
249	btScalar lenSqr = m_cachedSeparatingAxis.length2();
250	//valid normal
251	if (lenSqr < 0.0001)
252	{
253	m_degenerateSimplex = 5;
254	}
255	if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
256	{
257	btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
258	normalInB *= rlen; //normalize
259	btScalar s = btSqrt(squaredDistance);
260
261	btAssert(s > btScalar(0.0));
262	pointOnA -= m_cachedSeparatingAxis * (marginA / s);
263	pointOnB += m_cachedSeparatingAxis * (marginB / s);
264	distance = ((btScalar(1.)/rlen) - margin);
265	isValid = true;
266
267	m_lastUsedMethod = 1;
268	} else
269	{
270	m_lastUsedMethod = 2;
271	}
272	}
273
274	bool catchDegeneratePenetrationCase =
275	(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
276
277	//if (checkPenetration && !isValid)
278	if (checkPenetration && (!isValid \|\| catchDegeneratePenetrationCase ))
279	{
280	//penetration case
281
282	//if there is no way to handle penetrations, bail out
283	if (m_penetrationDepthSolver)
284	{
285	// Penetration depth case.
286	btVector3 tmpPointOnA,tmpPointOnB;
287
288	gNumDeepPenetrationChecks++;
289
290	bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
291	*m_simplexSolver,
292	m_minkowskiA,m_minkowskiB,
293	localTransA,localTransB,
294	m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
295	debugDraw,input.m_stackAlloc
296	);
297
298	if (isValid2)
299	{
300	btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
301	btScalar lenSqr = tmpNormalInB.length2();
302	if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
303	{
304	tmpNormalInB /= btSqrt(lenSqr);
305	btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
306	//only replace valid penetrations when the result is deeper (check)
307	if (!isValid \|\| (distance2 < distance))
308	{
309	distance = distance2;
310	pointOnA = tmpPointOnA;
311	pointOnB = tmpPointOnB;
312	normalInB = tmpNormalInB;
313	isValid = true;
314	m_lastUsedMethod = 3;
315	} else
316	{
317
318	}
319	} else
320	{
321	//isValid = false;
322	m_lastUsedMethod = 4;
323	}
324	} else
325	{
326	m_lastUsedMethod = 5;
327	}
328
329	}
330	}
331	}
332
333	if (isValid)
334	{
335	#ifdef __SPU__
336	//spu_printf("distance\n");
337	#endif //__CELLOS_LV2__
338
339
340	#ifdef DEBUG_SPU_COLLISION_DETECTION
341	spu_printf("output 1\n");
342	#endif
343	m_cachedSeparatingAxis = normalInB;
344	m_cachedSeparatingDistance = distance;
345
346	output.addContactPoint(
347	normalInB,
348	pointOnB+positionOffset,
349	distance);
350
351	#ifdef DEBUG_SPU_COLLISION_DETECTION
352	spu_printf("output 2\n");
353	#endif
354	//printf("gjk add:%f",distance);
355	}
356
357
358	}
359
360
361
362
363

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

Download in other formats: