Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

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

Last change on this file since 2736 was 2662, checked in by rgrieder, 16 years ago
Merged presentation branch back to trunk.
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	{
	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
[2430]	343	m_cachedSeparatingAxis = normalInB;
	344	m_cachedSeparatingDistance = distance;
	345
[1963]	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: