Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/physics/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @ 2328

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

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

Download in other formats: