nevrax.org : docs

00001 
+00007 /* Copyright, 2001 Nevrax Ltd.
+00008  *
+00009  * This file is part of NEVRAX NEL.
+00010  * NEVRAX NEL is free software; you can redistribute it and/or modify
+00011  * it under the terms of the GNU General Public License as published by
+00012  * the Free Software Foundation; either version 2, or (at your option)
+00013  * any later version.
+00014 
+00015  * NEVRAX NEL is distributed in the hope that it will be useful, but
+00016  * WITHOUT ANY WARRANTY; without even the implied warranty of
+00017  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+00018  * General Public License for more details.
+00019 
+00020  * You should have received a copy of the GNU General Public License
+00021  * along with NEVRAX NEL; see the file COPYING. If not, write to the
+00022  * Free Software Foundation, Inc., 59 Temple Place - Suite 330, Boston,
+00023  * MA 02111-1307, USA.
+00024  */
+00025 
+00026 #include "stdpacs.h"
+00027 
+00028 #include "pacs/chain_quad.h"
+00029 
+00030 using   namespace std;
+00031 using   namespace NLMISC;
+00032 
+00033 
+00034 namespace NLPACS
+00035 {
+00036 
+00037 
+00038 // ***************************************************************************
+00039 const   float   CChainQuad::_QuadElementSize= 4;        // = 4 meters.
+00040 
+00041 
+00042 // ***************************************************************************
+00043 CChainQuad::CChainQuad()
+00044 {
+00045         _QuadData= NULL;
+00046         _QuadDataLen= 0;
+00047 }
+00048 // ***************************************************************************
+00049 CChainQuad::~CChainQuad()
+00050 {
+00051         delete [] _QuadData;
+00052         _QuadData= NULL;
+00053         _QuadDataLen= 0;
+00054 }
+00055 // ***************************************************************************
+00056 CChainQuad::CChainQuad(const CChainQuad &o)
+00057 {
+00058         _QuadData= NULL;
+00059         _QuadDataLen= 0;
+00060         *this= o;
+00061 }
+00062 // ***************************************************************************
+00063 CChainQuad      &CChainQuad::operator=(const CChainQuad &o)
+00064 {
+00065         // Alloc good quaddata.
+00066         _QuadDataLen= o._QuadDataLen;
+00067         delete [] _QuadData;
+00068         if(_QuadDataLen>0)
+00069         {
+00070                 _QuadData= (uint8*)new uint8[_QuadDataLen];
+00071                 // copy contents.
+00072                 memcpy(_QuadData, o._QuadData, _QuadDataLen);
+00073         }
+00074         else
+00075                 _QuadData= NULL;
+00076 
+00077         // copy infos.
+00078         _Width= o._Width;
+00079         _Height= o._Height;
+00080         _X= o._X;
+00081         _Y= o._Y;
+00082 
+00083         // copy good pointers.
+00084         _Quad.clear();
+00085         _Quad.resize(o._Quad.size(), NULL);
+00086         for(sint i=0; i<(sint)_Quad.size(); i++)
+00087         {
+00088                 if(o._Quad[i])
+00089                 {
+00090                         uint32  off= (uint32)(o._Quad[i]-o._QuadData);
+00091                         _Quad[i]= _QuadData+off;
+00092                 }
+00093         }
+00094 
+00095 
+00096         return *this;
+00097 }
+00098 
+00099 
+00100 
+00101 // ***************************************************************************
+00102 void                    CChainQuad::getGridBounds(sint32 &x0, sint32 &y0, sint32 &x1, sint32 &y1, const CVector &minP, const CVector &maxP) const
+00103 {
+00104         x0= (sint32)floor(minP.x / _QuadElementSize) - _X;
+00105         y0= (sint32)floor(minP.y / _QuadElementSize) - _Y;
+00106         x1= (sint32) ceil(maxP.x / _QuadElementSize) - _X;
+00107         y1= (sint32) ceil(maxP.y / _QuadElementSize) - _Y;
+00108         // Manage selection of a point exactly on a quad bound
+00109         if(x1-x0==0)
+00110                 x0--, x1++;
+00111         if(y1-y0==0)
+00112                 y0--, y1++;
+00113         // clamp
+00114         x0= max(x0, (sint32)0);
+00115         y0= max(y0, (sint32)0);
+00116         x1= min(x1, (sint32)_Width);
+00117         y1= min(y1, (sint32)_Height);
+00118 }
+00119 
+00120 
+00121 // ***************************************************************************
+00122 void                    CChainQuad::build(const std::vector<COrderedChain> &ochains)
+00123 {
+00124         vector< list<CEdgeChainEntry> > tempQuad;
+00125         sint    i,j;
+00126 
+00127         // first, clear any pr-build.
+00128         contReset(_Quad);
+00129         delete [] _QuadData;
+00130         _QuadData= NULL;
+00131         _QuadDataLen= 0;
+00132 
+00133 
+00134         // 0. Find BBox of the grid. Allocate grid.
+00135         //=========================================
+00136         bool            first=true;
+00137         CAABBox         chainquadBBox;
+00138         // run all chains.
+00139         for(i=0;i<(sint)ochains.size();i++)
+00140         {
+00141                 const std::vector<CVector2s>    &vertices= ochains[i].getVertices();
+00142 
+00143                 // run all vertices.
+00144                 for(j= 0; j<(sint)vertices.size();j++)
+00145                 {
+00146                         // enlarge bbox.
+00147                         if(first)
+00148                                 first= false, chainquadBBox.setCenter(vertices[j].unpack3f());
+00149                         else
+00150                                 chainquadBBox.extend(vertices[j].unpack3f());
+00151                 }
+00152         }
+00153 
+00154         // compute X,Y,Width, Height.
+00155         _X= (sint32)floor(chainquadBBox.getMin().x / _QuadElementSize);
+00156         _Y= (sint32)floor(chainquadBBox.getMin().y / _QuadElementSize);
+00157         _Width= (sint32)ceil(chainquadBBox.getMax().x / _QuadElementSize) - _X;
+00158         _Height= (sint32)ceil(chainquadBBox.getMax().y / _QuadElementSize) - _Y;
+00159 
+00160         tempQuad.resize(_Width*_Height);
+00161         _Quad.resize(_Width*_Height, NULL);
+00162 
+00163 
+00164         // 1. For each edge, add them to the quadgrid.
+00165         //=========================================
+00166         // run all chains.
+00167         for(i=0;i<(sint)ochains.size();i++)
+00168         {
+00169                 const std::vector<CVector2s>    &vertices= ochains[i].getVertices();
+00170 
+00171                 sint    numEdges= (sint)vertices.size()-1;
+00172 
+00173                 // run all edges.
+00174                 for(j= 0; j<numEdges; j++)
+00175                 {
+00176                         const CVector           p0= vertices[j].unpack3f();
+00177                         const CVector           p1= vertices[j+1].unpack3f();
+00178                         CVector         minP,maxP;
+00179                         minP.minof(p0, p1);
+00180                         maxP.maxof(p0, p1);
+00181                         // PrecisionPb: extend a little this edge. This is important for special case like borders on zones.
+00182                         if(minP.x-maxP.x==0)
+00183                                 minP.x-=0.001f, maxP.x+=0.001f;
+00184                         if(minP.y-maxP.y==0)
+00185                                 minP.y-=0.001f, maxP.y+=0.001f;
+00186 
+00187 
+00188                         // get bounding coordinate of this edge in the quadgrid.
+00189                         sint32  x0, y0, x1, y1;
+00190                         getGridBounds(x0, y0, x1, y1, minP, maxP);
+00191 
+00192                         // add this edge to all the quadnode it touch.
+00193                         for(sint y= y0; y<y1; y++)
+00194                         {
+00195                                 for(sint x= x0; x<x1; x++)
+00196                                 {
+00197                                         list<CEdgeChainEntry>   &quadNode= tempQuad[y*_Width+x];
+00198 
+00199                                         addEdgeToQuadNode(quadNode, i, j);
+00200                                 }
+00201                         }
+00202                 }
+00203         }
+00204 
+00205 
+00206         // 2. Mem optimisation: Use only 1 block for ALL quads of the grid.
+00207         //=========================================
+00208         sint    memSize= 0;
+00209         // run all quads.
+00210         for(i=0;i<(sint)tempQuad.size();i++)
+00211         {
+00212                 list<CEdgeChainEntry>   &quadNode= tempQuad[i];
+00213 
+00214                 if(!quadNode.empty())
+00215                 {
+00216                         // add an entry for Len.
+00217                         memSize+= sizeof(uint16);
+00218                         // add N entry of CEdgeChainEntry.
+00219                         memSize+= quadNode.size()*sizeof(CEdgeChainEntry);
+00220                 }
+00221         }
+00222 
+00223         // allocate.
+00224         _QuadData= (uint8*)new uint8[memSize];
+00225         _QuadDataLen= memSize;
+00226 
+00227 
+00228         // 3. Fill _QuadData with lists.
+00229         //=========================================
+00230         uint8   *ptr= _QuadData;
+00231         for(i=0;i<(sint)tempQuad.size();i++)
+00232         {
+00233                 list<CEdgeChainEntry>                   &srcQuadNode= tempQuad[i];
+00234                 list<CEdgeChainEntry>::iterator it;
+00235 
+00236                 if(!srcQuadNode.empty())
+00237                 {
+00238                         _Quad[i]= ptr;
+00239 
+00240                         // write len.
+00241                         uint16  len= srcQuadNode.size();
+00242                         *((uint16*)ptr)= len;
+00243                         ptr+= sizeof(uint16);
+00244 
+00245                         // add entries.
+00246                         it= srcQuadNode.begin();
+00247                         for(j=0; j<len; j++, it++)
+00248                         {
+00249                                 *((CEdgeChainEntry*)ptr)= *it;
+00250                                 ptr+= sizeof(CEdgeChainEntry);
+00251                         }
+00252                 }
+00253         }
+00254 
+00255 
+00256         // End.
+00257 }
+00258 
+00259 
+00260 // ***************************************************************************
+00261 void                    CChainQuad::addEdgeToQuadNode(list<CEdgeChainEntry> &quadNode, sint ochainId, sint edgeId)
+00262 {
+00263         // 0. try to find, insert an edge in an existing CEdgeChainEntry.
+00264         //=========================================
+00265         list<CEdgeChainEntry>::iterator         it;
+00266         for(it= quadNode.begin(); it!=quadNode.end();it++)
+00267         {
+00268                 if(it->OChainId==ochainId)
+00269                 {
+00270                         // selection is faster if we only manages a single start/end block.
+00271                         it->EdgeStart= min(it->EdgeStart, (uint16)edgeId);
+00272                         it->EdgeEnd= max(it->EdgeEnd, (uint16)(edgeId+1));
+00273                         return;
+00274                 }
+00275         }
+00276 
+00277 
+00278         // 1. else, create new one.
+00279         //=========================================
+00280         CEdgeChainEntry         entry;
+00281         entry.OChainId= ochainId;
+00282         entry.EdgeStart= edgeId;
+00283         entry.EdgeEnd= edgeId+1;
+00284         quadNode.push_back(entry);
+00285 }
+00286 
+00287 
+00288 // ***************************************************************************
+00289 sint                    CChainQuad::selectEdges(const NLMISC::CAABBox &bbox, CCollisionSurfaceTemp &cst) const
+00290 {
+00291         sint    nRes=0;
+00292         sint    i;
+00293         uint16  *ochainLUT= cst.OChainLUT;
+00294 
+00295         // start: no edge found.
+00296         cst.EdgeChainEntries.clear();
+00297 
+00298         // get bounding coordinate of this bbox in the quadgrid.
+00299         sint32  x0, y0, x1, y1;
+00300         getGridBounds(x0, y0, x1, y1, bbox.getMin(), bbox.getMax());
+00301 
+00302 
+00303         // run all intersected quads.
+00304         for(sint y= y0; y<y1; y++)
+00305         {
+00306                 for(sint x= x0; x<x1; x++)
+00307                 {
+00308                         uint8   *quadNode= _Quad[y*_Width+x];
+00309 
+00310                         // no edgechain entry??
+00311                         if(!quadNode)
+00312                                 continue;
+00313 
+00314                         // get edgechain entries
+00315                         sint    numEdgeChainEntries= *((uint16*)quadNode);
+00316                         quadNode+= sizeof(uint16);
+00317                         CEdgeChainEntry         *ptrEdgeChainEntry= (CEdgeChainEntry*)quadNode;
+00318 
+00319                         // For each one, add it to the result list.
+00320                         for(i=0;i<numEdgeChainEntries;i++)
+00321                         {
+00322                                 uint16  ochainId= ptrEdgeChainEntry[i].OChainId;
+00323 
+00324                                 // if ochain not yet inserted.
+00325                                 if(ochainLUT[ochainId]==0xFFFF)
+00326                                 {
+00327                                         // inc the list.
+00328                                         ochainLUT[ochainId]= nRes;
+00329                                         cst.EdgeChainEntries.push_back(ptrEdgeChainEntry[i]);
+00330                                         nRes++;
+00331                                 }
+00332                                 else
+00333                                 {
+00334                                         // extend the entry in the list.
+00335                                         uint16 id= ochainLUT[ochainId];
+00336                                         CEdgeChainEntry         &ece= cst.EdgeChainEntries[id];
+00337                                         ece.EdgeStart= min(ece.EdgeStart, ptrEdgeChainEntry[i].EdgeStart);
+00338                                         ece.EdgeEnd= max(ece.EdgeEnd, ptrEdgeChainEntry[i].EdgeEnd);
+00339                                 }
+00340                         }
+00341                 }
+00342         }
+00343 
+00344 
+00345         // reset LUT to 0xFFFF for all ochains selected.
+00346         for(i=0;i<nRes;i++)
+00347         {
+00348                 uint16  ochainId= cst.EdgeChainEntries[i].OChainId;
+00349                 ochainLUT[ochainId]= 0xFFFF;
+00350         }
+00351 
+00352 
+00353         return nRes;
+00354 }
+00355 
+00356 sint            CChainQuad::selectEdges(CVector start, CVector end, CCollisionSurfaceTemp &cst) const
+00357 {
+00358         sint    nRes=0;
+00359         sint    i;
+00360         uint16  *ochainLUT= cst.OChainLUT;
+00361 
+00362         // start: no edge found.
+00363         cst.EdgeChainEntries.clear();
+00364 
+00365         if (end.x < start.x)
+00366                 swap(start, end);
+00367 
+00368         float   minx = _X*_QuadElementSize,
+00369                         miny = _Y*_QuadElementSize,
+00370                         maxx = minx + _Width*_QuadElementSize,
+00371                         maxy = miny + _Height*_QuadElementSize;
+00372 
+00373         if (start.x > maxx || end.x < minx || start.y > maxy || end.y < miny)
+00374                 return nRes;
+00375 
+00376         if (start.x < minx)
+00377         {
+00378                 start.y = start.y+(end.y-start.y)*(minx-start.x)/(end.x-start.x);
+00379                 start.x = minx;
+00380         }
+00381 
+00382         if (start.y < miny)
+00383         {
+00384                 start.x = start.x+(end.x-start.x)*(miny-start.y)/(end.y-start.y);
+00385                 start.y = miny;
+00386         }
+00387 
+00388         if (end.x > maxx)
+00389         {
+00390                 end.y = start.y+(end.y-start.y)*(minx-start.x)/(end.x-start.x);
+00391                 end.x = maxx;
+00392         }
+00393 
+00394         if (end.y > maxy)
+00395         {
+00396                 end.x = start.x+(end.x-start.x)*(miny-start.y)/(end.y-start.y);
+00397                 end.y = maxy;
+00398         }
+00399 
+00400         sint32  x0, x1, ya, yb;
+00401         sint    x, y;
+00402         float   fx, fxa, fxb, fya, fyb;
+00403 
+00404         x0 = (sint32)floor(start.x / _QuadElementSize) - _X;
+00405         x1 = (sint32)ceil(end.x / _QuadElementSize) - _X;
+00406         fx = (x0+_X)*_QuadElementSize;
+00407 
+00408         for (x=x0; x<x1; ++x)
+00409         {
+00410                 fxa = (fx < start.x) ? start.x : fx;
+00411                 fxb = (fx+_QuadElementSize > end.x) ? end.x : fx+_QuadElementSize;
+00412 
+00413                 fya = start.y+(end.y-start.y)*(fxa-start.x)/(end.x-start.x);
+00414                 fyb = start.y+(end.y-start.y)*(fxb-start.x)/(end.x-start.x);
+00415 
+00416                 if (fya > fyb)
+00417                         swap (fya, fyb);
+00418 
+00419                 ya = (sint32)floor(fya / _QuadElementSize) - _Y;
+00420                 yb = (sint32)ceil(fyb / _QuadElementSize) - _Y;
+00421 
+00422                 fx += _QuadElementSize;
+00423 
+00424                 for (y=ya; y<yb; ++y)
+00425                 {
+00426                         uint8   *quadNode= _Quad[y*_Width+x];
+00427 
+00428                         // no edgechain entry??
+00429                         if(!quadNode)
+00430                                 continue;
+00431 
+00432                         // get edgechain entries
+00433                         sint    numEdgeChainEntries= *((uint16*)quadNode);
+00434                         quadNode+= sizeof(uint16);
+00435                         CEdgeChainEntry         *ptrEdgeChainEntry= (CEdgeChainEntry*)quadNode;
+00436 
+00437                         // For each one, add it to the result list.
+00438                         for(i=0;i<numEdgeChainEntries;i++)
+00439                         {
+00440                                 uint16  ochainId= ptrEdgeChainEntry[i].OChainId;
+00441 
+00442                                 // if ochain not yet inserted.
+00443                                 if(ochainLUT[ochainId]==0xFFFF)
+00444                                 {
+00445                                         // inc the list.
+00446                                         ochainLUT[ochainId]= nRes;
+00447                                         cst.EdgeChainEntries.push_back(ptrEdgeChainEntry[i]);
+00448                                         nRes++;
+00449                                 }
+00450                                 else
+00451                                 {
+00452                                         // extend the entry in the list.
+00453                                         uint16 id= ochainLUT[ochainId];
+00454                                         CEdgeChainEntry         &ece= cst.EdgeChainEntries[id];
+00455                                         ece.EdgeStart= min(ece.EdgeStart, ptrEdgeChainEntry[i].EdgeStart);
+00456                                         ece.EdgeEnd= max(ece.EdgeEnd, ptrEdgeChainEntry[i].EdgeEnd);
+00457                                 }
+00458                         }
+00459                 }
+00460         }
+00461 
+00462         // reset LUT to 0xFFFF for all ochains selected.
+00463         for(i=0;i<nRes;i++)
+00464         {
+00465                 uint16  ochainId= cst.EdgeChainEntries[i].OChainId;
+00466                 ochainLUT[ochainId]= 0xFFFF;
+00467         }
+00468 
+00469         return nRes;
+00470 }
+00471 
+00472 // ***************************************************************************
+00473 void            CChainQuad::serial(NLMISC::IStream &f)
+00474 {
+00475         /*
+00476         Version 0:
+00477                 - base version.
+00478         */
+00479         (void)f.serialVersion(0);
+00480         uint    i;
+00481 
+00482         // serial basics.
+00483         f.serial(_X, _Y, _Width, _Height, _QuadDataLen);
+00484 
+00485 
+00486         // serial _QuadData.
+00487         if(f.isReading())
+00488         {
+00489                 delete [] _QuadData;
+00490                 if(_QuadDataLen>0)
+00491                         _QuadData= (uint8*)new uint8[_QuadDataLen];
+00492                 else
+00493                         _QuadData= NULL;
+00494         }
+00495         // Since we have only uint16 (see CEdgeChainEntry), serial them in a single block.
+00496         uint16  *ptrQData= (uint16*)_QuadData;
+00497         for(i=0;i<_QuadDataLen/2; i++, ptrQData++)
+00498         {
+00499                 f.serial(*ptrQData);
+00500         }
+00501 
+00502 
+00503         // serial _Quad.
+00504         std::vector<uint32>             offsets;
+00505         uint32          len;
+00506         uint32          val;
+00507         if(f.isReading())
+00508         {
+00509                 // len/resize.
+00510                 f.serial(len);
+00511                 offsets.resize(len);
+00512                 contReset(_Quad);
+00513                 _Quad.resize(len);
+00514 
+00515                 // read offsets -> ptrs.
+00516                 for(i=0; i<len; i++)
+00517                 {
+00518                         f.serial(val);
+00519                         if(val== 0xFFFFFFFF)
+00520                                 _Quad[i]= NULL;
+00521                         else
+00522                                 _Quad[i]= _QuadData+val;
+00523                 }
+00524         }
+00525         else
+00526         {
+00527                 // len/resize.
+00528                 len= _Quad.size();
+00529                 f.serial(len);
+00530 
+00531                 // write offsets.
+00532                 for(i=0; i<len; i++)
+00533                 {
+00534                         uint8   *ptr= _Quad[i];
+00535                         if(ptr==NULL)
+00536                                 val= 0xFFFFFFFF;
+00537                         else
+00538                                 val= (uint32)(ptr-_QuadData);
+00539                         f.serial(val);
+00540                 }
+00541         }
+00542 
+00543 }
+00544 
+00545 
+00546 
+00547 } // NLPACS
+
chain_quad.cpp
