Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/physics/src/bullet/BulletSoftBody/btSparseSDF.h @ 2258

Last change on this file since 2258 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: 7.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	///btSparseSdf implementation by Nathanael Presson
	16
	17	#ifndef _14F9D17F_EAE8_4aba_B41C_292DB2AA70F3_
	18	#define _14F9D17F_EAE8_4aba_B41C_292DB2AA70F3_
	19
	20	#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
	21	#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
	22
	23	// Modified Paul Hsieh hash
	24	template <const int DWORDLEN>
	25	unsigned int HsiehHash(const void* pdata)
[1972]	26	{
[1963]	27	const unsigned short* data=(const unsigned short*)pdata;
	28	unsigned hash=DWORDLEN<<2,tmp;
	29	for(int i=0;i<DWORDLEN;++i)
[1972]	30	{
[1963]	31	hash += data[0];
	32	tmp = (data[1]<<11)^hash;
	33	hash = (hash<<16)^tmp;
	34	data += 2;
	35	hash += hash>>11;
[1972]	36	}
[1963]	37	hash^=hash<<3;hash+=hash>>5;
	38	hash^=hash<<4;hash+=hash>>17;
	39	hash^=hash<<25;hash+=hash>>6;
	40	return(hash);
[1972]	41	}
[1963]	42
	43	template <const int CELLSIZE>
	44	struct btSparseSdf
[1972]	45	{
[1963]	46	//
	47	// Inner types
	48	//
	49	struct IntFrac
[1972]	50	{
[1963]	51	int b;
	52	int i;
	53	btScalar f;
[1972]	54	};
[1963]	55	struct Cell
[1972]	56	{
[1963]	57	btScalar d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1];
	58	int c[3];
	59	int puid;
	60	unsigned hash;
	61	btCollisionShape* pclient;
	62	Cell* next;
[1972]	63	};
[1963]	64	//
	65	// Fields
	66	//
[1972]	67
[1963]	68	btAlignedObjectArray<Cell*> cells;
	69	btScalar voxelsz;
	70	int puid;
	71	int ncells;
	72	int nprobes;
	73	int nqueries;
[1972]	74
[1963]	75	//
	76	// Methods
	77	//
[1972]	78
[1963]	79	//
	80	void Initialize(int hashsize=2383)
[1972]	81	{
[1963]	82	cells.resize(hashsize,0);
	83	Reset();
[1972]	84	}
[1963]	85	//
	86	void Reset()
[1972]	87	{
[1963]	88	for(int i=0,ni=cells.size();i<ni;++i)
[1972]	89	{
[1963]	90	Cell* pc=cells[i];
	91	cells[i]=0;
	92	while(pc)
[1972]	93	{
[1963]	94	Cell* pn=pc->next;
	95	delete pc;
	96	pc=pn;
[1972]	97	}
[1963]	98	}
	99	voxelsz =0.25;
	100	puid =0;
	101	ncells =0;
	102	nprobes =1;
	103	nqueries =1;
[1972]	104	}
[1963]	105	//
	106	void GarbageCollect(int lifetime=256)
[1972]	107	{
[1963]	108	const int life=puid-lifetime;
	109	for(int i=0;i<cells.size();++i)
[1972]	110	{
[1963]	111	Cell*& root=cells[i];
	112	Cell* pp=0;
	113	Cell* pc=root;
	114	while(pc)
[1972]	115	{
[1963]	116	Cell* pn=pc->next;
	117	if(pc->puid<life)
[1972]	118	{
[1963]	119	if(pp) pp->next=pn; else root=pn;
	120	delete pc;pc=pp;--ncells;
[1972]	121	}
	122	pp=pc;pc=pn;
[1963]	123	}
	124	}
	125	//printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries);
	126	nqueries=1;
	127	nprobes=1;
	128	++puid; /* TODO: Reset puid's when int range limit is reached */
[1972]	129	/* else setup a priority list... */
	130	}
[1963]	131	//
	132	int RemoveReferences(btCollisionShape* pcs)
[1972]	133	{
[1963]	134	int refcount=0;
	135	for(int i=0;i<cells.size();++i)
[1972]	136	{
[1963]	137	Cell*& root=cells[i];
	138	Cell* pp=0;
	139	Cell* pc=root;
	140	while(pc)
[1972]	141	{
[1963]	142	Cell* pn=pc->next;
	143	if(pc->pclient==pcs)
[1972]	144	{
[1963]	145	if(pp) pp->next=pn; else root=pn;
	146	delete pc;pc=pp;++refcount;
[1972]	147	}
	148	pp=pc;pc=pn;
[1963]	149	}
	150	}
[1972]	151	return(refcount);
[1963]	152	}
	153	//
	154	btScalar Evaluate( const btVector3& x,
[1972]	155	btCollisionShape* shape,
	156	btVector3& normal,
	157	btScalar margin)
	158	{
[1963]	159	/* Lookup cell */
	160	const btVector3 scx=x/voxelsz;
	161	const IntFrac ix=Decompose(scx.x());
	162	const IntFrac iy=Decompose(scx.y());
	163	const IntFrac iz=Decompose(scx.z());
	164	const unsigned h=Hash(ix.b,iy.b,iz.b,shape);
	165	Cell*& root=cells[static_cast<int>(h%cells.size())];
	166	Cell* c=root;
	167	++nqueries;
	168	while(c)
[1972]	169	{
[1963]	170	++nprobes;
	171	if( (c->hash==h) &&
	172	(c->c[0]==ix.b) &&
	173	(c->c[1]==iy.b) &&
	174	(c->c[2]==iz.b) &&
	175	(c->pclient==shape))
[1972]	176	{ break; }
	177	else
	178	{ c=c->next; }
	179	}
[1963]	180	if(!c)
[1972]	181	{
[1963]	182	++nprobes;
	183	++ncells;
	184	c=new Cell();
	185	c->next=root;root=c;
	186	c->pclient=shape;
	187	c->hash=h;
	188	c->c[0]=ix.b;c->c[1]=iy.b;c->c[2]=iz.b;
	189	BuildCell(*c);
[1972]	190	}
[1963]	191	c->puid=puid;
	192	/* Extract infos */
	193	const int o[]={ ix.i,iy.i,iz.i};
	194	const btScalar d[]={ c->d[o[0]+0][o[1]+0][o[2]+0],
[1972]	195	c->d[o[0]+1][o[1]+0][o[2]+0],
	196	c->d[o[0]+1][o[1]+1][o[2]+0],
	197	c->d[o[0]+0][o[1]+1][o[2]+0],
	198	c->d[o[0]+0][o[1]+0][o[2]+1],
	199	c->d[o[0]+1][o[1]+0][o[2]+1],
	200	c->d[o[0]+1][o[1]+1][o[2]+1],
	201	c->d[o[0]+0][o[1]+1][o[2]+1]};
[1963]	202	/* Normal */
[1972]	203	#if 1
[1963]	204	const btScalar gx[]={ d[1]-d[0],d[2]-d[3],
[1972]	205	d[5]-d[4],d[6]-d[7]};
[1963]	206	const btScalar gy[]={ d[3]-d[0],d[2]-d[1],
[1972]	207	d[7]-d[4],d[6]-d[5]};
[1963]	208	const btScalar gz[]={ d[4]-d[0],d[5]-d[1],
[1972]	209	d[7]-d[3],d[6]-d[2]};
[1963]	210	normal.setX(Lerp( Lerp(gx[0],gx[1],iy.f),
[1972]	211	Lerp(gx[2],gx[3],iy.f),iz.f));
[1963]	212	normal.setY(Lerp( Lerp(gy[0],gy[1],ix.f),
[1972]	213	Lerp(gy[2],gy[3],ix.f),iz.f));
[1963]	214	normal.setZ(Lerp( Lerp(gz[0],gz[1],ix.f),
[1972]	215	Lerp(gz[2],gz[3],ix.f),iy.f));
[1963]	216	normal = normal.normalized();
[1972]	217	#else
[1963]	218	normal = btVector3(d[1]-d[0],d[3]-d[0],d[4]-d[0]).normalized();
[1972]	219	#endif
[1963]	220	/* Distance */
	221	const btScalar d0=Lerp(Lerp(d[0],d[1],ix.f),
[1972]	222	Lerp(d[3],d[2],ix.f),iy.f);
[1963]	223	const btScalar d1=Lerp(Lerp(d[4],d[5],ix.f),
[1972]	224	Lerp(d[7],d[6],ix.f),iy.f);
[1963]	225	return(Lerp(d0,d1,iz.f)-margin);
[1972]	226	}
[1963]	227	//
	228	void BuildCell(Cell& c)
[1972]	229	{
[1963]	230	const btVector3 org=btVector3( (btScalar)c.c[0],
[1972]	231	(btScalar)c.c[1],
	232	(btScalar)c.c[2]) *
	233	CELLSIZE*voxelsz;
[1963]	234	for(int k=0;k<=CELLSIZE;++k)
[1972]	235	{
[1963]	236	const btScalar z=voxelsz*k+org.z();
	237	for(int j=0;j<=CELLSIZE;++j)
[1972]	238	{
[1963]	239	const btScalar y=voxelsz*j+org.y();
	240	for(int i=0;i<=CELLSIZE;++i)
[1972]	241	{
[1963]	242	const btScalar x=voxelsz*i+org.x();
	243	c.d[i][j][k]=DistanceToShape( btVector3(x,y,z),
[1972]	244	c.pclient);
	245	}
[1963]	246	}
	247	}
	248	}
	249	//
	250	static inline btScalar DistanceToShape(const btVector3& x,
[1972]	251	btCollisionShape* shape)
	252	{
[1963]	253	btTransform unit;
	254	unit.setIdentity();
	255	if(shape->isConvex())
[1972]	256	{
[1963]	257	btGjkEpaSolver2::sResults res;
	258	btConvexShape* csh=static_cast<btConvexShape*>(shape);
	259	return(btGjkEpaSolver2::SignedDistance(x,0,csh,unit,res));
[1972]	260	}
	261	return(0);
[1963]	262	}
	263	//
	264	static inline IntFrac Decompose(btScalar x)
[1972]	265	{
[1963]	266	/* That one need a lot of improvements... */
	267	/* Remove test, faster floor... */
	268	IntFrac r;
	269	x/=CELLSIZE;
	270	const int o=x<0?(int)(-x+1):0;
	271	x+=o;r.b=(int)x;
	272	const btScalar k=(x-r.b)*CELLSIZE;
	273	r.i=(int)k;r.f=k-r.i;r.b-=o;
	274	return(r);
[1972]	275	}
[1963]	276	//
	277	static inline btScalar Lerp(btScalar a,btScalar b,btScalar t)
[1972]	278	{
[1963]	279	return(a+(b-a)*t);
[1972]	280	}
[1963]	281
[1972]	282
[1963]	283
	284	//
	285	static inline unsigned int Hash(int x,int y,int z,btCollisionShape* shape)
[1972]	286	{
[1963]	287	struct btS
	288	{
	289	int x,y,z;
	290	void* p;
	291	};
	292
	293	btS myset;
[1972]	294
[1963]	295	myset.x=x;myset.y=y;myset.z=z;myset.p=shape;
	296	const void* ptr = &myset;
	297
	298	unsigned int result = HsiehHash<sizeof(btS)/4> (ptr);
[1972]	299
[1963]	300
	301	return result;
[1972]	302	}
[1963]	303	};
[1972]	304
[1963]	305
	306	#endif

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

Download in other formats: