Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

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

Last change on this file since 6518 was 5781, checked in by rgrieder, 16 years ago
Reverted trunk again. We might want to find a way to delete these revisions again (x3n's changes are still available as diff in the commit mails).
Property svn:eol-style set to `native`
File size: 10.4 KB

Rev	Line
[1963]	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)
[2430]	24	//#define TEST_NON_VIRTUAL 1
[1963]	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	{
[2430]	56	m_cachedSeparatingDistance = 0.f;
	57
[1963]	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
[2430]	67	#ifdef __SPU__
[1963]	68	btScalar marginA = m_minkowskiA->getMarginNonVirtual();
	69	btScalar marginB = m_minkowskiB->getMarginNonVirtual();
[2430]	70	#else
	71	btScalar marginA = m_minkowskiA->getMargin();
	72	btScalar marginB = m_minkowskiB->getMargin();
[1963]	73	#ifdef TEST_NON_VIRTUAL
[2430]	74	btScalar marginAv = m_minkowskiA->getMarginNonVirtual();
	75	btScalar marginBv = m_minkowskiB->getMarginNonVirtual();
[1963]	76	btAssert(marginA == marginAv);
	77	btAssert(marginB == marginBv);
	78	#endif //TEST_NON_VIRTUAL
[2430]	79	#endif
	80
[1963]	81
[2430]	82
[1963]	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
[2430]	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);
[1963]	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 //
[2430]	138	#endif //__SPU__
[1963]	139
[2430]	140	btVector3 pWorld = localTransA(pInA);
	141	btVector3 qWorld = localTransB(qInB);
[1963]	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	{
[2882]	153	checkSimplex=true;
	154	//checkPenetration = false;
[1963]	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
[2430]	344	m_cachedSeparatingAxis = normalInB;
	345	m_cachedSeparatingDistance = distance;
	346
[1963]	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: