Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics_new/src/bullet/BulletSoftBody/btSparseSDF.h @ 2119

Last change on this file since 2119 was 2119, checked in by rgrieder, 16 years ago

Merged physics branch into physics_new branch.
  • Property svn:eol-style set to native
File size: 7.0 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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
24template <const int DWORDLEN>
25unsigned int HsiehHash(const void* pdata)
26        {
27        const unsigned short*   data=(const unsigned short*)pdata;
28        unsigned                                hash=DWORDLEN<<2,tmp;
29        for(int i=0;i<DWORDLEN;++i)
30                {
31                hash    +=      data[0];
32                tmp             =       (data[1]<<11)^hash;
33                hash    =       (hash<<16)^tmp;
34                data    +=      2;
35                hash    +=      hash>>11;
36                }
37        hash^=hash<<3;hash+=hash>>5;
38        hash^=hash<<4;hash+=hash>>17;
39        hash^=hash<<25;hash+=hash>>6;
40        return(hash);
41        }
42
43template <const int CELLSIZE>
44struct  btSparseSdf
45        {
46        //
47        // Inner types
48        //
49        struct IntFrac
50                {
51                int                                     b;
52                int                                     i;
53                btScalar                        f;
54                };
55        struct  Cell
56                {
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;
63                };
64        //
65        // Fields
66        //
67       
68        btAlignedObjectArray<Cell*>             cells; 
69        btScalar                                                voxelsz;
70        int                                                             puid;
71        int                                                             ncells;
72        int                                                             nprobes;
73        int                                                             nqueries;       
74       
75        //
76        // Methods
77        //
78       
79        //
80        void                                    Initialize(int hashsize=2383)
81                {
82                cells.resize(hashsize,0);
83                Reset();               
84                }
85        //
86        void                                    Reset()
87                {
88                for(int i=0,ni=cells.size();i<ni;++i)
89                        {
90                        Cell*   pc=cells[i];
91                        cells[i]=0;
92                        while(pc)
93                                {
94                                Cell*   pn=pc->next;
95                                delete pc;
96                                pc=pn;
97                                }
98                        }
99                voxelsz         =0.25;
100                puid            =0;
101                ncells          =0;
102                nprobes         =1;
103                nqueries        =1;
104                }
105        //
106        void                                    GarbageCollect(int lifetime=256)
107                {
108                const int life=puid-lifetime;
109                for(int i=0;i<cells.size();++i)
110                        {
111                        Cell*&  root=cells[i];
112                        Cell*   pp=0;
113                        Cell*   pc=root;
114                        while(pc)
115                                {
116                                Cell*   pn=pc->next;
117                                if(pc->puid<life)
118                                        {
119                                        if(pp) pp->next=pn; else root=pn;
120                                        delete pc;pc=pp;--ncells;
121                                        }
122                                pp=pc;pc=pn;
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   */ 
129                                /* else setup a priority list...                                                */ 
130                }
131        //
132        int                                             RemoveReferences(btCollisionShape* pcs)
133                {
134                int     refcount=0;
135                for(int i=0;i<cells.size();++i)
136                        {
137                        Cell*&  root=cells[i];
138                        Cell*   pp=0;
139                        Cell*   pc=root;
140                        while(pc)
141                                {
142                                Cell*   pn=pc->next;
143                                if(pc->pclient==pcs)
144                                        {
145                                        if(pp) pp->next=pn; else root=pn;
146                                        delete pc;pc=pp;++refcount;
147                                        }
148                                pp=pc;pc=pn;
149                                }
150                        }
151                return(refcount);
152                }
153        //
154        btScalar                                Evaluate(       const btVector3& x,
155                                                                                btCollisionShape* shape,
156                                                                                btVector3& normal,
157                                                                                btScalar margin)
158                {
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)
169                        {
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))
176                                { break; }
177                                else
178                                { c=c->next; }
179                        }
180                if(!c)
181                        {
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);
190                        }
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],
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]};
202                /* Normal       */ 
203                #if 1
204                const btScalar  gx[]={  d[1]-d[0],d[2]-d[3],
205                                                                d[5]-d[4],d[6]-d[7]};
206                const btScalar  gy[]={  d[3]-d[0],d[2]-d[1],
207                                                                d[7]-d[4],d[6]-d[5]};
208                const btScalar  gz[]={  d[4]-d[0],d[5]-d[1],
209                                                                d[7]-d[3],d[6]-d[2]};
210                normal.setX(Lerp(       Lerp(gx[0],gx[1],iy.f),
211                                                        Lerp(gx[2],gx[3],iy.f),iz.f));
212                normal.setY(Lerp(       Lerp(gy[0],gy[1],ix.f),
213                                                        Lerp(gy[2],gy[3],ix.f),iz.f));
214                normal.setZ(Lerp(       Lerp(gz[0],gz[1],ix.f),
215                                                        Lerp(gz[2],gz[3],ix.f),iy.f));
216                normal          =       normal.normalized();
217                #else
218                normal          =       btVector3(d[1]-d[0],d[3]-d[0],d[4]-d[0]).normalized();
219                #endif
220                /* Distance     */ 
221                const btScalar  d0=Lerp(Lerp(d[0],d[1],ix.f),
222                                                                Lerp(d[3],d[2],ix.f),iy.f);
223                const btScalar  d1=Lerp(Lerp(d[4],d[5],ix.f),
224                                                                Lerp(d[7],d[6],ix.f),iy.f);
225                return(Lerp(d0,d1,iz.f)-margin);
226                }
227        //
228        void                                    BuildCell(Cell& c)
229                {
230                const btVector3 org=btVector3(  (btScalar)c.c[0],
231                                                                                (btScalar)c.c[1],
232                                                                                (btScalar)c.c[2])       *
233                                                                                CELLSIZE*voxelsz;
234                for(int k=0;k<=CELLSIZE;++k)
235                        {
236                        const btScalar  z=voxelsz*k+org.z();
237                        for(int j=0;j<=CELLSIZE;++j)
238                                {
239                                const btScalar  y=voxelsz*j+org.y();
240                                for(int i=0;i<=CELLSIZE;++i)
241                                        {
242                                        const btScalar  x=voxelsz*i+org.x();
243                                        c.d[i][j][k]=DistanceToShape(   btVector3(x,y,z),
244                                                                                                        c.pclient);
245                                        }
246                                }
247                        }
248                }
249        //
250        static inline btScalar  DistanceToShape(const btVector3& x,
251                                                                                        btCollisionShape* shape)
252                {
253                btTransform     unit;
254                unit.setIdentity();
255                if(shape->isConvex())
256                        {
257                        btGjkEpaSolver2::sResults       res;
258                        btConvexShape*                          csh=static_cast<btConvexShape*>(shape);
259                        return(btGjkEpaSolver2::SignedDistance(x,0,csh,unit,res));
260                        }
261                return(0);
262                }
263        //
264        static inline IntFrac   Decompose(btScalar x)
265                {
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);
275                }
276        //
277        static inline btScalar  Lerp(btScalar a,btScalar b,btScalar t)
278                {
279                return(a+(b-a)*t);
280                }
281
282       
283
284        //
285        static inline unsigned int      Hash(int x,int y,int z,btCollisionShape* shape)
286                {
287                struct btS
288                { 
289                        int x,y,z;
290                        void* p;
291                };
292
293                btS myset;
294               
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);
299               
300
301                return result;
302                }
303};
304       
305
306#endif
Note: See TracBrowser for help on using the repository browser.