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 "nel/misc/plane.h"
+00029 
+00030 #include "pacs/local_retriever.h"
+00031 #include "pacs/collision_desc.h"
+00032 #include "pacs/retriever_instance.h"
+00033 
+00034 #include "nel/misc/hierarchical_timer.h"
+00035 
+00036 using namespace std;
+00037 using namespace NLMISC;
+00038 
+00040 const float     NLPACS::CLocalRetriever::_TipThreshold = 0.1f;
+00041 const float     NLPACS::CLocalRetriever::_EdgeTipThreshold = 0.1f;
+00042 
+00044 const float     InsurePositionThreshold = 2.0e-2f;
+00045 
+00046 //static float  hybrid2dNorm(const CVector &v)
+00047 //{
+00048 //      return (float)(sqrt(sqr(v.x)+sqr(v.y))+fabs(v.z)*0.1);
+00049 //}
+00050 
+00051 
+00052 
+00053 
+00054 
+00055 NLPACS::CLocalRetriever::CLocalRetriever()
+00056 {
+00057         _Type = Landscape;
+00058         _FaceGrid.clear();
+00059 }
+00060 
+00061 
+00062 
+00063 const CVector   &NLPACS::CLocalRetriever::getStartVector(uint32 chain) const
+00064 {
+00065         const COrderedChain3f   &ochain = _FullOrderedChains[_Chains[chain].getSubChains().front()];
+00066         return (ochain.isForward()) ? ochain.getVertices().front() : ochain.getVertices().back();
+00067 }
+00068 
+00069 const CVector   &NLPACS::CLocalRetriever::getStopVector(uint32 chain) const
+00070 {
+00071         const COrderedChain3f   &ochain = _FullOrderedChains[_Chains[chain].getSubChains().back()];
+00072         return (ochain.isForward()) ? ochain.getVertices().back() : ochain.getVertices().front();
+00073 }
+00074 
+00075 
+00076 
+00077 
+00078 const CVector   &NLPACS::CLocalRetriever::getStartVector(uint32 chain, sint32 surface) const
+00079 {
+00080         bool                                    onLeft = _Chains[chain].getLeft() == surface;
+00081         const COrderedChain3f   &ochain = onLeft ? _FullOrderedChains[_Chains[chain].getSubChains().front()] :
+00082                                                                                            _FullOrderedChains[_Chains[chain].getSubChains().back()];
+00083 
+00084         if (ochain.isForward() && onLeft || !ochain.isForward() && !onLeft)
+00085                 return ochain.getVertices().front();
+00086         else
+00087                 return ochain.getVertices().back();
+00088 }
+00089 
+00090 const CVector   &NLPACS::CLocalRetriever::getStopVector(uint32 chain, sint32 surface) const
+00091 {
+00092         bool                                    onLeft = _Chains[chain].getLeft() == surface;
+00093         const COrderedChain3f   &ochain = onLeft ? _FullOrderedChains[_Chains[chain].getSubChains().back()] :
+00094                                                                                            _FullOrderedChains[_Chains[chain].getSubChains().front()];
+00095 
+00096         if (ochain.isForward() && onLeft || !ochain.isForward() && !onLeft)
+00097                 return ochain.getVertices().back();
+00098         else
+00099                 return ochain.getVertices().front();
+00100 }
+00101 
+00102 
+00103 
+00104 
+00105 uint16                  NLPACS::CLocalRetriever::getStartTip(uint32 chain, sint32 surface) const
+00106 {
+00107         return (_Chains[chain].getLeft() == surface) ? _Chains[chain].getStartTip() : _Chains[chain].getStopTip();
+00108 }
+00109 
+00110 uint16                  NLPACS::CLocalRetriever::getStopTip(uint32 chain, sint32 surface) const
+00111 {
+00112         return (_Chains[chain].getLeft() == surface) ? _Chains[chain].getStopTip() : _Chains[chain].getStartTip();
+00113 }
+00114 
+00115 
+00116 
+00117 
+00118 
+00119 void                    NLPACS::CLocalRetriever::setStartTip(uint32 chain, sint32 surface, uint16 startTip)
+00120 {
+00121         if (_Chains[chain].getLeft() == surface)
+00122                 _Chains[chain]._StartTip = startTip;
+00123         else
+00124                 _Chains[chain]._StopTip = startTip;
+00125 }
+00126 
+00127 void                    NLPACS::CLocalRetriever::setStopTip(uint32 chain, sint32 surface, uint16 stopTip)
+00128 {
+00129         if (_Chains[chain].getLeft() == surface)
+00130                 _Chains[chain]._StopTip = stopTip;
+00131         else
+00132                 _Chains[chain]._StartTip = stopTip;
+00133 }
+00134 
+00135 
+00136 
+00137 
+00138 uint32                  NLPACS::CLocalRetriever::getPreviousChain(uint32 chain, sint32 surface) const
+00139 {
+00140         uint                                                            loop;
+00141         uint                                                            loopIndex;
+00142 
+00143         if (_Chains[chain].getLeft() == surface)
+00144         {
+00145                 loop = _Chains[chain]._LeftLoop;
+00146                 loopIndex = _Chains[chain]._LeftLoopIndex;
+00147         }
+00148         else
+00149         {
+00150                 loop = _Chains[chain]._RightLoop;
+00151                 loopIndex = _Chains[chain]._RightLoopIndex;
+00152         }
+00153 
+00154         const CRetrievableSurface                       &surf = _Surfaces[surface];
+00155         const CRetrievableSurface::TLoop        &sLoop = surf._Loops[loop];
+00156         return surf._Chains[sLoop[(loopIndex+sLoop.size()-1)%sLoop.size()]].Chain;
+00157 }
+00158 
+00159 uint32                  NLPACS::CLocalRetriever::getNextChain(uint32 chain, sint32 surface) const
+00160 {
+00161         uint                                                            loop;
+00162         uint                                                            loopIndex;
+00163 
+00164         if (_Chains[chain].getLeft() == surface)
+00165         {
+00166                 loop = _Chains[chain]._LeftLoop;
+00167                 loopIndex = _Chains[chain]._LeftLoopIndex;
+00168         }
+00169         else
+00170         {
+00171                 loop = _Chains[chain]._RightLoop;
+00172                 loopIndex = _Chains[chain]._RightLoopIndex;
+00173         }
+00174 
+00175         const CRetrievableSurface                       &surf = _Surfaces[surface];
+00176         const CRetrievableSurface::TLoop        &sLoop = surf._Loops[loop];
+00177         return surf._Chains[sLoop[(loopIndex+1)%sLoop.size()]].Chain;
+00178 }
+00179 
+00180 
+00181 
+00182 
+00183 void    NLPACS::CLocalRetriever::unify()
+00184 {
+00185         uint    i, j;
+00186 
+00187         for (i=0; i<_Chains.size(); ++i)
+00188                 _Chains[i].unify(_OrderedChains);
+00189 
+00190         for (i=0; i<_Tips.size(); ++i)
+00191         {
+00192                 NLPACS::CLocalRetriever::CTip   &tip = _Tips[i];
+00193                 CVector2s ptip = tip.Point;
+00194 
+00195                 for (j=0; j<tip.Chains.size(); ++j)
+00196                 {
+00197                         if (tip.Chains[j].Start)
+00198                         {
+00199                                 if (_Chains[tip.Chains[j].Chain].getStartVector(_OrderedChains) != ptip)
+00200                                         nlwarning("chain %d is not stuck to tip %d", tip.Chains[j].Chain, i);
+00201                                 _Chains[tip.Chains[j].Chain].setStartVector(ptip, _OrderedChains);
+00202                         }
+00203                         else
+00204                         {
+00205                                 if (_Chains[tip.Chains[j].Chain].getStopVector(_OrderedChains) != ptip)
+00206                                         nlwarning("chain %d is not stuck to tip %d", tip.Chains[j].Chain, i);
+00207                                 _Chains[tip.Chains[j].Chain].setStopVector(ptip, _OrderedChains);
+00208                         }
+00209                 }
+00210         }
+00211 
+00212         _FullOrderedChains.resize(_OrderedChains.size());
+00213         for (i=0; i<_OrderedChains.size(); ++i)
+00214                 _FullOrderedChains[i].unpack(_OrderedChains[i]);
+00215 }
+00216 
+00217 
+00218 
+00219 
+00220 
+00221 
+00222 
+00223 void    NLPACS::CLocalRetriever::dumpSurface(uint surf, const CVector &vect) const
+00224 {
+00225         const CRetrievableSurface       &surface = _Surfaces[surf];
+00226 
+00227         nlinfo("dump surf %d", surf);
+00228         nlinfo("%d chains, %d loops", surface._Chains.size(), surface._Loops.size());
+00229         
+00230         uint    i, j, k;
+00231 
+00232         for (i=0; i<surface._Chains.size(); ++i)
+00233         {
+00234                 uint                    chainId = surface._Chains[i].Chain;
+00235                 const CChain    &chain = _Chains[chainId];
+00236                 nlinfo("-- chain %d[%d]: %d sub left=%d right=%d start=%d stop=%d", i, chainId, chain.getSubChains().size(), chain.getLeft(), chain.getRight(), chain.getStartTip(), chain.getStopTip());
+00237 
+00238                 for (j=0; j<chain.getSubChains().size(); ++j)
+00239                 {
+00240                         const COrderedChain3f   &ochain = _FullOrderedChains[chain.getSubChain(j)];
+00241                         const COrderedChain             &ochains = _OrderedChains[chain.getSubChain(j)];
+00242                         nlinfo("     subchain %d[%d]: fwd=%d parent=%d idx=%d", j, chain.getSubChain(j), ochain.isForward(), ochain.getParentId(), ochain.getIndexInParent());
+00243                         for (k=0; k<ochain.getVertices().size(); ++k)
+00244                                 nlinfo("       v[%d]=(%.3f,%.3f,%.3f) (%d,%d)", k, ochain.getVertices()[k].x+vect.x, ochain.getVertices()[k].y+vect.y, ochain.getVertices()[k].z+vect.z, ochains.getVertices()[k].x, ochains.getVertices()[k].y);
+00245                 }
+00246 
+00247         }
+00248 
+00249         for (i=0; i<surface._Loops.size(); ++i)
+00250         {
+00251                 const CRetrievableSurface::TLoop        &loop = surface._Loops[i];
+00252                 nlinfo("-- loop %d: %d chains length=%.2f", i, loop.size(), loop.Length);
+00253                 static char     wbuffer[256];
+00254                 static char     buffer[10240];
+00255                 sprintf(buffer, "    chains:");
+00256                 for (j=0; j<loop.size(); ++j)
+00257                 {
+00258                         sprintf(wbuffer, " %d[%d]", loop[j], surface._Chains[loop[j]].Chain);
+00259                         strcat(buffer, wbuffer);
+00260                 }
+00261                 nlinfo("%s", buffer);
+00262         }
+00263 }
+00264 
+00265 
+00266 float   NLPACS::CLocalRetriever::distanceToBorder(const ULocalPosition &pos) const
+00267 {
+00268         const CRetrievableSurface       &surf = _Surfaces[pos.Surface];
+00269         uint                                            i, j;
+00270         float                                           minDist = 1.0e10f, dist;
+00271 
+00272         for (i=0; i<surf._Chains.size(); ++i)
+00273         {
+00274                 const CChain    &chain = _Chains[surf._Chains[i].Chain];
+00275                 for (j=0; j<chain.getSubChains().size(); ++j)
+00276                 {
+00277                         dist = _OrderedChains[chain.getSubChain(j)].distance(pos.Estimation);
+00278                         if (dist < minDist)
+00279                         {
+00280                                 minDist = dist;
+00281                         }
+00282                 }
+00283         }
+00284 
+00285         return minDist;
+00286 }
+00287 
+00288 
+00289 
+00290 
+00291 
+00292 sint32  NLPACS::CLocalRetriever::addSurface(uint8 normalq, uint8 orientationq,
+00293                                                                                         uint8 mat, uint8 charact, uint8 level,
+00294                                                                                         bool isUnderWater, float waterHeight,
+00295                                                                                         const CVector &center,
+00296                                                                                         const NLPACS::CSurfaceQuadTree &quad)
+00297 {
+00298         // creates a new surface...
+00299         sint32  newId = _Surfaces.size();
+00300         _Surfaces.resize(newId+1);
+00301         CRetrievableSurface     &surf = _Surfaces.back();
+00302 
+00303         // ... and fills it
+00304         surf._NormalQuanta = normalq;
+00305         surf._OrientationQuanta = orientationq;
+00306         surf._Material = mat;
+00307         surf._Character = charact;
+00308         surf._Level = level;
+00309         surf._Quad = quad;
+00310         surf._Center = center;
+00311 
+00312         // WARNING!! MODIFY THESE IF QUANTAS VALUES CHANGE !!
+00313         surf._IsFloor = (surf._NormalQuanta <= 1);
+00314         surf._IsCeiling = (surf._NormalQuanta >= 3);
+00315 
+00316         surf._Flags = 0;
+00317         surf._Flags |= (surf._IsFloor) ? (1<<CRetrievableSurface::IsFloorBit) : 0;
+00318         surf._Flags |= (surf._IsCeiling) ? (1<<CRetrievableSurface::IsCeilingBit) : 0;
+00319         surf._Flags |= (!surf._IsFloor && !surf._IsCeiling) ? (1<<CRetrievableSurface::IsSlantBit) : 0;
+00320 
+00321         surf._Flags |= (isUnderWater) ? (1<<CRetrievableSurface::IsUnderWaterBit) : 0;
+00322         surf._WaterHeight = waterHeight;
+00323 
+00324         surf._Flags |= ((0xffffffff<<(CRetrievableSurface::NormalQuantasStartBit)) & CRetrievableSurface::NormalQuantasBitMask);
+00325 
+00326         return newId;
+00327 }
+00328 
+00329 sint32  NLPACS::CLocalRetriever::addChain(const vector<CVector> &verts,
+00330                                                                                   sint32 left, sint32 right)
+00331 {
+00332         vector<CVector> vertices = verts;
+00333         uint            i;
+00334 
+00335         if (vertices.size() < 2)
+00336         {
+00337                 nlwarning("in NLPACS::CLocalRetriever::addChain()");
+00338                 nlwarning("The chain has less than 2 vertices");
+00339                 return -1;
+00340         }
+00341 
+00342         // Remove doubled vertices due to CVector2s snapping
+00343         vector<CVector2s>       converts;
+00344 
+00345         for (i=0; i<vertices.size(); ++i)
+00346                 converts.push_back(CVector2s(vertices[i]));
+00347 
+00348         vector<CVector2s>::iterator     next2s = converts.begin(), it2s, prev2s;
+00349         prev2s = next2s; ++next2s;
+00350         it2s = next2s; ++next2s;
+00351 
+00352         vector<CVector>::iterator       it3f = vertices.begin();
+00353         CVector                                         prev3f = *it3f;
+00354         ++it3f;
+00355 
+00356 
+00357         for (; it2s != converts.end() && next2s != converts.end(); )
+00358         {
+00359                 // if the next point is equal to the previous
+00360                 if (*it2s == *prev2s || *it2s == *next2s)
+00361                 {
+00362                         // then remove the next point
+00363                         it2s = converts.erase(it2s);
+00364                         it3f = vertices.erase(it3f);
+00365 
+00366                         prev2s = it2s;
+00367                         --prev2s;
+00368                         next2s = it2s;
+00369                         ++next2s;
+00370                 }
+00371                 else
+00372                 {
+00373                         // else remember the next point, and step to the next...
+00374                         ++prev2s;
+00375                         ++it2s;
+00376                         ++next2s;
+00377                         ++it3f;
+00378                         prev3f = *it3f;
+00379                 }
+00380         }
+00381         
+00382         if (vertices.size() < 2)
+00383         {
+00384                 nlwarning("in NLPACS::CLocalRetriever::addChain()");
+00385                 nlwarning("The chain was snapped to a single point");
+00386                 return -1;
+00387         }
+00388 
+00389         sint32          newId = _Chains.size();
+00390         _Chains.resize(newId+1);
+00391         CChain          &chain = _Chains.back();
+00392 
+00393         if (left>(sint)_Surfaces.size())
+00394                 nlerror ("left surface id MUST be id<%d (id=%d)", _Surfaces.size(), left);
+00395         if (right>(sint)_Surfaces.size())
+00396                 nlerror ("right surface id MUST be id<%d (id=%d)", _Surfaces.size(), right);
+00397 
+00398         // checks if we can build the chain.
+00399         if (newId > 65535)
+00400                 nlerror("in NLPACS::CLocalRetriever::addChain(): reached the maximum number of chains");
+00401 
+00402         CRetrievableSurface     *leftSurface = (left>=0) ? &(_Surfaces[left]) : NULL;
+00403         CRetrievableSurface     *rightSurface = (right>=0) ? &(_Surfaces[right]) : NULL;
+00404 
+00405         // adds the chain and the link to the surface links vector.
+00406         if (leftSurface != NULL)
+00407                 leftSurface->_Chains.push_back(CRetrievableSurface::CSurfaceLink(newId, right));
+00408         if (rightSurface != NULL)
+00409                 rightSurface->_Chains.push_back(CRetrievableSurface::CSurfaceLink(newId, left));
+00410 
+00411         chain._StartTip = 0xffff;
+00412         chain._StopTip = 0xffff;
+00413 
+00414         // make the chain and its subchains.
+00415         chain.make(vertices, left, right, _OrderedChains, (uint16)newId, _FullOrderedChains);
+00416 
+00417         return newId;
+00418 }
+00419 
+00420 
+00421 
+00422 
+00423 void    NLPACS::CLocalRetriever::computeLoopsAndTips()
+00424 {
+00425         // for each surface,
+00426         // examine each chain tip to match another tip inside the surface tips
+00427         // if there is no matching tip, then creates a new one
+00428 
+00429         uint    i, j;
+00430 
+00431         for (i=0; i<_Surfaces.size(); ++i)
+00432         {
+00433                 CRetrievableSurface     &surface = _Surfaces[i];
+00434 
+00435                 vector<bool>            chainFlags;
+00436                 chainFlags.resize(surface._Chains.size());
+00437                 for (j=0; j<chainFlags.size(); ++j)
+00438                         chainFlags[j] = false;
+00439 
+00440                 uint    totalAdded = 0;
+00441 
+00442                 while (true)
+00443                 {
+00444                         for (j=0; j<chainFlags.size() && chainFlags[j]; ++j)
+00445                                 ;
+00446 
+00447                         if (j == chainFlags.size())
+00448                                 break;
+00449 
+00450                         uint32                                          loopId = surface._Loops.size();
+00451                         surface._Loops.push_back(CRetrievableSurface::TLoop());
+00452                         CRetrievableSurface::TLoop      &loop = surface._Loops.back();
+00453 
+00454                         CVector loopStart = getStartVector(surface._Chains[j].Chain, i);
+00455                         CVector currentEnd = getStopVector(surface._Chains[j].Chain, i);
+00456                         _Chains[surface._Chains[j].Chain].setLoopIndexes(i, loopId, loop.size());
+00457                         loop.push_back(j);
+00458                         chainFlags[j] = true;
+00459 
+00460                         float   loopCloseDistance;
+00461 
+00462                         while (true)
+00463                         {
+00464 //                              loopCloseDistance = hybrid2dNorm(loopStart-currentEnd);
+00465                                 loopCloseDistance = (loopStart-currentEnd).norm();
+00466 
+00467                                 // choose the best matching start vector
+00468                                 sint    bestChain = -1;
+00469                                 float   best = 1.0e10f;
+00470                                 CVector thisStart;
+00471                                 for (j=0; j<chainFlags.size(); ++j)
+00472                                 {
+00473                                         if (chainFlags[j])
+00474                                                 continue;
+00475                                         thisStart = getStartVector(surface._Chains[j].Chain, i);
+00476 //                                      float   d = hybrid2dNorm(thisStart-currentEnd);
+00477                                         float   d = (thisStart-currentEnd).norm();
+00478                                         if (d < best)
+00479                                         {
+00480                                                 best = d;
+00481                                                 bestChain = j;
+00482                                         }
+00483                                 }
+00484 
+00485                                 if ((bestChain == -1 || best > 3.0e-2f)&& loopCloseDistance > 3.0e-2f)
+00486                                 {
+00487                                         nlwarning("in NLPACS::CLocalRetriever::computeTips()");
+00488 
+00489                                         dumpSurface(i);
+00490 
+00491                                         for (j=0; j<surface._Chains.size(); ++j)
+00492                                         {
+00493                                                 CVector start = getStartVector(surface._Chains[j].Chain, i);
+00494                                                 CVector end = getStopVector(surface._Chains[j].Chain, i);
+00495                                                 nlinfo("surf=%d chain=%d", i, surface._Chains[j].Chain);
+00496                                                 nlinfo("start=(%f,%f,%f)", start.x, start.y, start.z);
+00497                                                 nlinfo("end=(%f,%f,%f)", end.x, end.y, end.z);
+00498                                         }
+00499                                         
+00500                                         nlwarning("bestChain=%d best=%f", bestChain, best);
+00501                                         nlwarning("loopCloseDistance=%f", loopCloseDistance);
+00502                                         nlerror("Couldn't close loop on surface=%d", i);
+00503                                 }
+00504                                 else if (best > 1.0e0f && loopCloseDistance < 3.0e-2f ||
+00505                                                  loopCloseDistance < 1.0e-3f)
+00506                                 {
+00507                                         break;
+00508                                 }
+00509 
+00510                                 currentEnd = getStopVector(surface._Chains[bestChain].Chain, i);
+00511                                 _Chains[surface._Chains[bestChain].Chain].setLoopIndexes(i, loopId, loop.size());
+00512                                 loop.push_back(bestChain);
+00513                                 chainFlags[bestChain] = true;
+00514                                 ++totalAdded;
+00515                         }
+00516                 }
+00517         }
+00518 
+00519         for (i=0; i<_Chains.size(); ++i)
+00520         {
+00521 /*              if (i == 431)
+00522                         nlstop;
+00523 */
+00524                 uint    whichTip;
+00525                 // for both tips (start and stop)
+00526                 for (whichTip=0; whichTip<=1; ++whichTip)
+00527                 {
+00528                         // get the tip id
+00529                         uint    thisTip = (whichTip) ? _Chains[i].getStopTip() : _Chains[i].getStartTip();
+00530 
+00531                         if (thisTip != 0xffff && thisTip >= _Tips.size())
+00532                         {
+00533                                 nlwarning("in NLPACS::CLocalRetriever::computeLoopsAndTips()");
+00534                                 nlerror("checked a tip that doesn't exist on chain %d (tipId=%d)", i, thisTip);
+00535                         }
+00536 
+00537                         // if it is unaffected yet creates an new tip and affect it to the common chains
+00538                         if (thisTip == 0xffff)
+00539                         {
+00540                                 uint    turn;
+00541                                 uint    tipId = _Tips.size();
+00542 /*
+00543                                 if (tipId == 310)
+00544                                         nlstop;
+00545 */
+00546                                 _Tips.resize(tipId+1);
+00547                                 CTip    &tip = _Tips[tipId];
+00548                                 tip.Point = (whichTip) ? getStopVector(i) : getStartVector(i);
+00549 
+00550                                 for (turn=0; turn<=1; ++turn)
+00551                                 {
+00552                                         uint    chain = i;
+00553 
+00554                                         //
+00555                                         if (whichTip)
+00556                                                 _Chains[chain]._StopTip = tipId;
+00557                                         else
+00558                                                 _Chains[chain]._StartTip = tipId;
+00559 
+00560                                         sint32  surf = (!turn && !whichTip || turn && whichTip) ? _Chains[chain].getLeft() : _Chains[chain].getRight();
+00561 
+00562                                         while (surf >= 0)
+00563                                         {
+00564 
+00565                                                 CChain  &nextChain = (turn) ? _Chains[chain = getNextChain(chain, surf)] : _Chains[chain = getPreviousChain(chain, surf)];
+00566                                                 bool    isForward = (nextChain.getLeft() == surf);      // tells if the left surf is the current surf
+00567                                                 bool    selectTip = isForward && !turn || !isForward && turn;
+00568                                                 uint16  &tipRef = selectTip ? nextChain._StopTip : nextChain._StartTip;
+00569                                                 surf = (isForward) ? nextChain.getRight() : nextChain.getLeft();
+00570 
+00571                                                 if (tipRef != 0xffff && tipRef != tipId)
+00572                                                 {
+00573                                                         nlwarning("in NLPACS::CLocalRetriever::computeLoopsAndTips()");
+00574                                                         nlerror("Trying to setup a already created tip (tipId=%d, previous=%d)", tipId, tipRef);
+00575                                                 }
+00576                                                 else if (tipRef != 0xffff)
+00577                                                 {
+00578                                                         break;
+00579                                                 }
+00580 
+00581                                                 tipRef = tipId;
+00582                                         }
+00583                                 }
+00584                         }
+00585                 }
+00586         }
+00587 
+00588         for (i=0; i<_Chains.size(); ++i)
+00589         {
+00590                 uint    startTip = _Chains[i].getStartTip(),
+00591                                 stopTip = _Chains[i].getStopTip();
+00592 
+00593 /*
+00594                 if (_Chains[i].getEdge() >= 0 && startTip == stopTip)
+00595                 {
+00596                         nlwarning("NLPACS::CLocalRetriever::computeLoopsAndTips(): chain %d on edge %d has same StartTip and StopTip", i, _Chains[i].getEdge(), startTip, stopTip);
+00597                 }
+00598 */
+00599 
+00600                 _Tips[startTip].Chains.push_back(CTip::CChainTip(i, true));
+00601                 _Tips[stopTip].Chains.push_back(CTip::CChainTip(i, false));
+00602         }
+00603 
+00604         for (i=0; i<_Surfaces.size(); ++i)
+00605         {
+00606                 for (j=0; j<_Surfaces[i]._Loops.size(); ++j)
+00607                 {
+00608                         _Surfaces[i]._Loops[j].Length = 0.0f;
+00609                         uint    k;
+00610 
+00611                         for (k=0; k<_Surfaces[i]._Loops[j].size(); ++k)
+00612                                 _Surfaces[i]._Loops[j].Length += _Chains[_Surfaces[i]._Chains[_Surfaces[i]._Loops[j][k]].Chain].getLength();
+00613                 }
+00614         }
+00615 }
+00616 
+00617 
+00618 //
+00619 void    NLPACS::CLocalRetriever::buildSurfacePolygons(uint32 surface, list<CPolygon> &polygons) const
+00620 {
+00621         const CRetrievableSurface       &surf = _Surfaces[surface];
+00622 
+00623         uint    i, j, k, l;
+00624 
+00625         for (i=0; i<surf._Loops.size(); ++i)
+00626         {
+00627                 polygons.push_back();
+00628                 CPolygon        &poly = polygons.back();
+00629 
+00630                 for (j=0; j<surf._Loops[i].size(); ++j)
+00631                 {
+00632                         const CChain    &chain = _Chains[surf._Loops[i][j]];
+00633                         bool                    chainforward = ((uint32)chain._Left == surface);
+00634 
+00635                         if (chainforward)
+00636                         {
+00637                                 for (k=0; k<chain._SubChains.size(); ++k)
+00638                                 {
+00639                                         const COrderedChain             &ochain = _OrderedChains[chain._SubChains[k]];
+00640                                         bool                                    ochainforward = ochain.isForward();
+00641 
+00642                                         if (ochainforward)
+00643                                         {
+00644                                                 for (l=0; l<ochain.getVertices().size()-1; ++l)
+00645                                                         poly.Vertices.push_back(ochain[l].unpack3f());
+00646                                         }
+00647                                         else
+00648                                         {
+00649                                                 for (l=ochain.getVertices().size()-1; l>0; --l)
+00650                                                         poly.Vertices.push_back(ochain[l].unpack3f());
+00651                                         }
+00652                                 }
+00653                         }
+00654                         else
+00655                         {
+00656                                 for (k=chain._SubChains.size(); (sint)k>0; --k)
+00657                                 {
+00658                                         const COrderedChain             &ochain = _OrderedChains[chain._SubChains[k]];
+00659                                         bool                                    ochainforward = ochain.isForward();
+00660 
+00661                                         if (ochainforward)
+00662                                         {
+00663                                                 for (l=ochain.getVertices().size()-1; (sint)l>0; --l)
+00664                                                         poly.Vertices.push_back(ochain[l].unpack3f());
+00665                                         }
+00666                                         else
+00667                                         {
+00668                                                 for (l=0; l<ochain.getVertices().size()-1; ++l)
+00669                                                         poly.Vertices.push_back(ochain[l].unpack3f());
+00670                                         }
+00671                                 }
+00672                         }
+00673                 }
+00674         }
+00675 }
+00676 
+00677 
+00678 // not implemented...
+00679 void    NLPACS::CLocalRetriever::sortTips()
+00680 {
+00681 }
+00682 
+00683 
+00684 
+00685 
+00686 void    NLPACS::CLocalRetriever::findBorderChains()
+00687 {
+00688         uint    chain;
+00689 
+00690         // for each chain, if it belongs to an edge of the
+00691         // local retriever, then adds it to the _BorderChains.
+00692         for (chain=0; chain<_Chains.size(); ++chain)
+00693                 if (_Chains[chain].isBorderChain())
+00694                 {
+00695                         sint32  index = _BorderChains.size();
+00696                         _BorderChains.push_back(chain);
+00697                         _Chains[chain].setBorderChainIndex(index);
+00698                 }
+00699 }
+00700 
+00701 void    NLPACS::CLocalRetriever::updateChainIds()
+00702 {
+00703         uint    surf, link;
+00704 
+00705         for (surf=0; surf<_Surfaces.size(); ++surf)
+00706         {
+00707                 CRetrievableSurface     &surface = _Surfaces[surf];
+00708 
+00709                 for (link=0; link<surface._Chains.size(); ++link)
+00710                 {
+00711                         sint32  chain = surface._Chains[link].Chain;
+00712 
+00713                         if (_Chains[chain]._Left == (sint32)surf)
+00714                                 surface._Chains[link].Surface = _Chains[chain]._Right;
+00715                         else if (_Chains[chain]._Right == (sint32)surf)
+00716                                 surface._Chains[link].Surface = _Chains[chain]._Left;
+00717                         else
+00718                         {
+00719                                 nlwarning("in NLPACS::CLocalRetriever::updateEdgesOnSurfaces()");
+00720                                 nlerror("Can't find back point to surface %d on chain %d", surf, chain);
+00721                         }
+00722                 }
+00723         }
+00724 }
+00725 
+00726 void    NLPACS::CLocalRetriever::computeTopologies()
+00727 {
+00728         nlinfo("compute topologies");
+00729 
+00730         // Find topologies out...
+00731         uint            character;
+00732         for (character=0; character<NumCreatureModels; ++character)
+00733         {
+00734                 // for each type of creature, flood fill surfaces...
+00735                 sint32  surface;
+00736                 uint    topology = 0;
+00737 
+00738                 for (surface=0; surface<(sint)_Surfaces.size(); ++surface)
+00739                 {
+00740                         if (_Surfaces[surface]._Topologies[character] == -1 &&
+00741                                 _Surfaces[surface]._Character == character)
+00742                         {
+00743                                 vector<sint32>  surfacesStack;
+00744                                 surfacesStack.push_back(surface);
+00745 
+00746                                 while (!surfacesStack.empty())
+00747                                 {
+00748                                         CRetrievableSurface     &current = _Surfaces[surfacesStack.back()];
+00749                                         surfacesStack.pop_back();
+00750                                         current._Topologies[character] = topology;
+00751 
+00752                                         uint    i;
+00753                                         for (i=0; i<current._Chains.size(); ++i)
+00754                                         {
+00755                                                 CChain  &chain = _Chains[current._Chains[i].Chain];
+00756                                                 sint32  link = (chain.getLeft() == surface) ? chain.getRight() : chain.getLeft();
+00757                                                 if (link>=0 && link<(sint)_Surfaces.size() &&
+00758                                                         _Surfaces[link]._Topologies[character] == -1 &&
+00759                                                         _Surfaces[link]._Character >= character)
+00760                                                 {
+00761                                                         surfacesStack.push_back(link);
+00762                                                         _Surfaces[link]._Topologies[character] = topology;
+00763                                                 }
+00764                                         }
+00765                                 }
+00766 
+00767                                 ++topology;
+00768                         }
+00769                 }
+00770 
+00771                 _Topologies[character].resize(topology);
+00772                 nlinfo("generated %d topologies for character %d", topology, character);
+00773         }
+00774 
+00775         uint            surface;
+00776         for (surface=0; surface<_Surfaces.size(); ++surface)
+00777         {
+00778                 CRetrievableSurface     &current = _Surfaces[surface];
+00779 
+00780                 for (character=0; character<NumCreatureModels; ++character)
+00781                         if (current._Topologies[character] >= 0)
+00782                                 _Topologies[character][current._Topologies[character]].push_back(surface);
+00783         }
+00784 }
+00785 
+00786 void    NLPACS::CLocalRetriever::translate(const NLMISC::CVector &translation)
+00787 {
+00788         uint    i;
+00789         for (i=0; i<_OrderedChains.size(); ++i)
+00790                 _OrderedChains[i].translate(translation);
+00791         for (i=0; i<_Surfaces.size(); ++i)
+00792                 _Surfaces[i].translate(translation);
+00793         for (i=0; i<_Tips.size(); ++i)
+00794                 _Tips[i].translate(translation);
+00795 }
+00796 
+00797 void    NLPACS::CLocalRetriever::serial(NLMISC::IStream &f)
+00798 {
+00799         /*
+00800         Version 0:
+00801                 - base version (with collision info).
+00802         Version 1:
+00803                 - interior vertices and faces, for interior ground snapping
+00804         Version 2:
+00805                 - face grid added.
+00806         Version 3:
+00807                 - identifier added.
+00808         */
+00809         sint    ver= f.serialVersion(3);
+00810 
+00811         uint    i;
+00812         f.serialCont(_Chains);
+00813         f.serialCont(_OrderedChains);
+00814         f.serialCont(_FullOrderedChains);
+00815         f.serialCont(_Surfaces);
+00816         f.serialCont(_Tips);
+00817         f.serialCont(_BorderChains);
+00818         for (i=0; i<NumCreatureModels; ++i)
+00819                 f.serialCont(_Topologies[i]);
+00820         f.serial(_ChainQuad);
+00821         f.serial(_BBox);
+00822         f.serialEnum(_Type);
+00823         f.serial(_ExteriorMesh);
+00824 
+00825         // a fix for old versions (with wrong _Type value)
+00826         if (_Type != CLocalRetriever::Interior) _Type = CLocalRetriever::Landscape;
+00827 
+00828         if (ver >= 1)
+00829         {
+00830                 f.serialCont(_InteriorVertices);
+00831                 f.serialCont(_InteriorFaces);
+00832 
+00833         }
+00834         if (ver >= 2)
+00835         {
+00836                 f.serial(_FaceGrid);
+00837         }
+00838         if (ver >= 3)
+00839         {
+00840                 f.serial(_Id);
+00841         }
+00842 }
+00843 
+00844 
+00845 
+00846 
+00847 
+00848 
+00849 
+00850 
+00851 bool    NLPACS::CLocalRetriever::insurePosition(NLPACS::ULocalPosition &local) const
+00852 {
+00853         if (local.Surface < 0 || local.Surface >= (sint)_Surfaces.size())
+00854         {
+00855                 nlwarning("PACS: can't insure position to inexistant surface %d", local.Surface);
+00856                 return false;
+00857         }
+00858 
+00859         // the surface
+00860         const NLPACS::CRetrievableSurface       &surface = _Surfaces[local.Surface];
+00861 
+00862         uint            i, j, k;
+00863         CVector2f       M = CVector2f(local.Estimation);
+00864         bool            moved = false;
+00865 
+00866         // for each chain and each subchain of the surface,
+00867         // check if point is located on the good side of the border (and far enough to avoid accuracy issues)
+00868         for (i=0; i<surface.getChains().size(); ++i)
+00869         {
+00870                 uint                                    ichain = surface.getChain(i).Chain;
+00871                 const NLPACS::CChain    &chain = _Chains[ichain];
+00872 
+00873                 for (j=0; j<chain.getSubChains().size(); ++j)
+00874                 {
+00875                         uint                                            iochain = chain.getSubChain(j);
+00876                         const NLPACS::COrderedChain     &ochain = _OrderedChains[iochain];
+00877 
+00878                         uint                                            isAtLeft = ((chain.getLeft() == local.Surface) ? 1 : 0);
+00879                         uint                                            isForward = (ochain.isForward() ? 1 : 0);
+00880                         bool                                            shouldBeUpper = !((isAtLeft ^ isForward) != 0); // shouldBeAtLeft for vertical segment
+00881 
+00882                         for (k=0; (sint)k<(sint)(ochain.getVertices().size()-1); ++k)
+00883                         {
+00884                                 CVector2f       A = ochain[k].unpack();
+00885                                 CVector2f       B = ochain[k+1].unpack();
+00886                                 CVector2f       AB = B-A;
+00887 
+00888                                 float           lambda = ((M-A)*AB)/AB.sqrnorm();
+00889 
+00890                                 if (lambda<0.0f || lambda>1.0f)
+00891                                         continue;
+00892 
+00893                                 CVector2f       n = (shouldBeUpper ? CVector2f(-AB.y, AB.x) : CVector2f(AB.y, -AB.x)).normed();
+00894                                 float           d = (M-A)*n;
+00895 
+00896                                 // if point is too close of the border or on the wrong side
+00897                                 // move it far enough
+00898                                 if (d < InsurePositionThreshold && d > -InsurePositionThreshold)
+00899                                 {
+00900                                         M += (InsurePositionThreshold*1.1f-d)*n;
+00901                                         moved = true;
+00902                                 }
+00903                         }
+00904                 }
+00905         }
+00906 
+00907         NLPACS::CRetrieverInstance::snapVector(M);
+00908 
+00909         local.Estimation.x = M.x;
+00910         local.Estimation.y = M.y;
+00911 
+00912         {
+00913                 float   fx1024 = local.Estimation.x * 1024.0f;
+00914                 float   fy1024 = local.Estimation.x * 1024.0f;
+00915                 sint32  ix1024 = (sint32)floor(fx1024);
+00916                 sint32  iy1024 = (sint32)floor(fy1024);
+00917 
+00918                 nlassert ((float)ix1024 == fx1024);
+00919                 nlassert ((float)iy1024 == fy1024);
+00920         }
+00921 
+00922         return moved;
+00923 }
+00924 
+00925 
+00926 void    NLPACS::CLocalRetriever::retrievePosition(CVector estimated, /*std::vector<uint8> &retrieveTable,*/ CCollisionSurfaceTemp &cst) const
+00927 {
+00928         CAABBox                 box;
+00929         const double    BorderThreshold = 2.0e-2f;
+00930         box.setMinMax(CVector(estimated.x-(float)BorderThreshold, _BBox.getMin().y, 0.0f), CVector(estimated.x+(float)BorderThreshold, _BBox.getMax().y, 0.0f));
+00931         uint                    numEdges = _ChainQuad.selectEdges(box, cst);
+00932 
+00933         uint                    ochain, i;
+00934         CVector2s               estim = CVector2s(estimated);
+00935 
+00936         cst.PossibleSurfaces.clear();
+00937 
+00938         // WARNING!!
+00939         // cst.SurfaceLUT is assumed to be 0 filled !!
+00940 
+00941 //      nldebug("estim=(%d,%d)", estim.x, estim.y);
+00942 
+00943         // for each ordered chain, checks if the estimated position is between the min and max.
+00944         for (i=0; i<numEdges; ++i)
+00945         {
+00946                 ochain = cst.EdgeChainEntries[i].OChainId;
+00947 
+00948                 const COrderedChain     &sub = _OrderedChains[ochain];
+00949                 const CVector2s &min = sub.getMin(),
+00950                                                 &max = sub.getMax();
+00951 
+00952                 // checks the position against the min and max of the chain
+00953                 if (estim.x < min.x || estim.x > max.x)
+00954                         continue;
+00955 
+00956                 bool    isUpper;
+00957                 bool    isOnBorder = false;
+00958 
+00959                 sint32  left = _Chains[sub.getParentId()].getLeft(),
+00960                                 right = _Chains[sub.getParentId()].getRight();
+00961 
+00962                 if (estim.y < min.y)
+00963                 {
+00964                         if (estim.x == max.x)
+00965                                 continue;
+00966                         isUpper = false;
+00967 //                      nlinfo("Box: min(%d,%d) max(%d,%d) forward=%d left=%d right=%d upper=false", min.x, min.y, max.x, max.y, sub.isForward(), left, right);
+00968                 }
+00969                 else if (estim.y > max.y)
+00970                 {
+00971                         if (estim.x == max.x)
+00972                                 continue;
+00973                         isUpper = true;
+00974 //                      nlinfo("Box: min(%d,%d) max(%d,%d) forward=%d left=%d right=%d upper=true", min.x, min.y, max.x, max.y, sub.isForward(), left, right);
+00975                 }
+00976                 else
+00977                 {
+00978                         const vector<CVector2s> &vertices = sub.getVertices();
+00979                         uint                                    start = 0, stop = vertices.size()-1;
+00980 
+00982 
+00983                         // then finds the smallest segment of the chain that includes the estimated position.
+00984                         while (stop-start > 1)
+00985                         {
+00986                                 uint    mid = (start+stop)/2;
+00987 
+00988                                 if (vertices[mid].x > estim.x)
+00989                                         stop = mid;
+00990                                 else
+00991                                         start = mid;
+00992                         }
+00993 
+00994                         // if a vertical edge
+00995                         if (vertices[start].x == vertices[stop].x)
+00996                         {
+00997                                 // look for maximal bounds
+00998                                 while (start > 0 && vertices[start].x == vertices[start-1].x)
+00999                                         --start;
+01000 
+01001                                 while (stop < vertices.size()-1 && vertices[stop].x == vertices[stop+1].x)
+01002                                         ++stop;
+01003 
+01004                                 // if upper or lower the bounds, do nothing
+01005                                 if (estim.y > vertices[start].y && estim.y > vertices[stop].y ||
+01006                                         estim.y < vertices[start].y && estim.y < vertices[stop].y)
+01007                                         continue;
+01008 
+01009                                 isOnBorder = true;
+01010                                 cst.incSurface(left);
+01011                                 cst.incSurface(right);
+01012                                 if (left >= 0)  cst.SurfaceLUT[left].FoundCloseEdge = true;
+01013                                 if (right >= 0) cst.SurfaceLUT[right].FoundCloseEdge = true;
+01014                                 continue;
+01015 //                              nlinfo("Edge: start(%d,%d) stop(%d,%d) forward=%d left=%d right=%d border=true", vertices[start].x, vertices[start].y, vertices[stop].x, vertices[stop].y, sub.isForward(), left, right);
+01016                         }
+01017                         else if (vertices[stop].x == estim.x)
+01018                         {
+01019                                 // if 
+01020                                 continue;
+01021                         }
+01022 
+01023                         // and then checks if the estimated position is up or down the chain.
+01024 
+01025                         // first trivial case (up both tips)
+01026                         if (estim.y > vertices[start].y && estim.y > vertices[stop].y)
+01027                         {
+01028                                 isUpper = true;
+01029 //                              nlinfo("Edge: start(%d,%d) stop(%d,%d) forward=%d left=%d right=%d upper=true", vertices[start].x, vertices[start].y, vertices[stop].x, vertices[stop].y, sub.isForward(), left, right);
+01030                         }
+01031                         // second trivial case (down both tips)
+01032                         else if (estim.y < vertices[start].y && estim.y < vertices[stop].y)
+01033                         {
+01034                                 isUpper = false;
+01035 //                              nlinfo("Edge: start(%d,%d) stop(%d,%d) forward=%d left=%d right=%d upper=false", vertices[start].x, vertices[start].y, vertices[stop].x, vertices[stop].y, sub.isForward(), left, right);
+01036                         }
+01037                         // full test...
+01038                         else
+01039                         {
+01040                                 const CVector2s &vstart = vertices[start],
+01041                                                                 &vstop = vertices[stop];
+01042 
+01043                                 sint16                  intersect = vstart.y + (vstop.y-vstart.y)*(estim.x-vstart.x)/(vstop.x-vstart.x);
+01044 
+01045                                 isUpper = estim.y > intersect;
+01046                                 isOnBorder = (fabs(estim.y - intersect)<BorderThreshold*Vector2sAccuracy);
+01047 //                              nlinfo("Edge: start(%d,%d) stop(%d,%d) forward=%d left=%d right=%d upper=%s border=%s", vertices[start].x, vertices[start].y, vertices[stop].x, vertices[stop].y, sub.isForward(), left, right, isUpper ? "true":"false", isOnBorder ? "true":"false");
+01048                         }
+01049                 }
+01050 
+01051                 if (isOnBorder)
+01052                 {
+01053                         cst.incSurface(left);
+01054                         cst.incSurface(right);
+01055                         if (left >= 0)  cst.SurfaceLUT[left].FoundCloseEdge = true;
+01056                         if (right >= 0) cst.SurfaceLUT[right].FoundCloseEdge = true;
+01057                         continue;
+01058                 }
+01059 
+01060                 // Depending on the chain is forward, up the position, increase/decrease the surface table...
+01061                 if (sub.isForward())
+01062                 {
+01063                         if (isUpper)
+01064                         {
+01065                                 cst.incSurface(left);
+01066                                 cst.decSurface(right);
+01067                         }
+01068                         else
+01069                         {
+01070                                 cst.decSurface(left);
+01071                                 cst.incSurface(right);
+01072                         }
+01073                 }
+01074                 else
+01075                 {
+01076                         if (isUpper)
+01077                         {
+01078                                 cst.decSurface(left);
+01079                                 cst.incSurface(right);
+01080                         }
+01081                         else
+01082                         {
+01083                                 cst.incSurface(left);
+01084                                 cst.decSurface(right);
+01085                         }
+01086                 }
+01087         }
+01088 }
+01089 
+01090 
+01091 void    NLPACS::CLocalRetriever::initFaceGrid()
+01092 {
+01093         CFaceGrid::CFaceGridBuild       fgb;
+01094         fgb.init(64, 4.0f);
+01095 
+01096         uint    i;
+01097         for (i=0; i<_InteriorFaces.size(); ++i)
+01098         {
+01099                 CAABBox                 box;
+01100                 CInteriorFace   &f = _InteriorFaces[i];
+01101                 box.setCenter(_InteriorVertices[f.Verts[0]]);
+01102                 box.extend(_InteriorVertices[f.Verts[1]]);
+01103                 box.extend(_InteriorVertices[f.Verts[2]]);
+01104 
+01105                 fgb.insert(box.getMin(), box.getMax(), i);
+01106         }
+01107 
+01108         _FaceGrid.create(fgb);
+01109 }
+01110 
+01111 void    NLPACS::CLocalRetriever::snapToInteriorGround(NLPACS::ULocalPosition &position, bool &snapped) const
+01112 {
+01113         // first preselect faces around the (x, y) position (CQuadGrid ?)
+01114         vector<uint32>  selection;
+01115         _FaceGrid.select(position.Estimation, selection);
+01116 
+01117         // from the preselect faces, look for the only face that belongs to the surface
+01118         // and that contains the position
+01119         CVector                                         pos = position.Estimation;
+01120         CVector                                         posh = pos+CVector(0.0f, 0.0f, 1.0f);
+01121         CVector2f                                       pos2d = position.Estimation;
+01122         float                                           bestDist = 1.0e10f;
+01123         CVector                                         best;
+01124         vector<uint32>::iterator        it;
+01125         snapped = false;
+01126         for (it=selection.begin(); it!=selection.end(); ++it)
+01127         {
+01128                 const CInteriorFace     &f = _InteriorFaces[*it];
+01129                 if (f.Surface == (uint32)position.Surface)
+01130                 {
+01131                         CVector v[3];
+01132                         v[0] = _InteriorVertices[f.Verts[0]];
+01133                         v[1] = _InteriorVertices[f.Verts[1]];
+01134                         v[2] = _InteriorVertices[f.Verts[2]];
+01135 
+01136                         CVector2f       n;
+01137                         float           c;                      // 2D cartesian coefficients of line in plane X/Y.
+01138                         // Line p0-p1.
+01139                         n = CVector2f(-(v[1].y-v[0].y), (v[1].x-v[0].x)).normed();
+01140                         c = -(v[0].x*n.x + v[0].y*n.y);
+01141                         if (n*pos2d + c < -1.0f/Vector2sAccuracy)       continue;
+01142                         // Line p1-p2.
+01143                         n = CVector2f(-(v[2].y-v[1].y), (v[2].x-v[1].x)).normed();
+01144                         c = -(v[1].x*n.x + v[1].y*n.y);
+01145                         if (n*pos2d + c < -1.0f/Vector2sAccuracy)       continue;
+01146                         //  Line p2-p0.
+01147                         n = CVector2f(-(v[0].y-v[2].y), (v[0].x-v[2].x)).normed();
+01148                         c = -(v[2].x*n.x + v[2].y*n.y);
+01149                         if (n*pos2d + c < -1.0f/Vector2sAccuracy)       continue;
+01150 
+01151                         CPlane  p;
+01152                         p.make(v[0], v[1], v[2]);
+01153 
+01154                         CVector i = p.intersect(pos, posh);
+01155 
+01156                         float   d = (float)fabs(pos.z-i.z);
+01157 
+01158                         if (d < bestDist)
+01159                         {
+01160                                 bestDist = d;
+01161                                 best = i;
+01162                         }
+01163                 }
+01164         }
+01165 
+01166         // and computes the real position on this face
+01167         if (bestDist < 50.0f)
+01168         {
+01169                 snapped = true;
+01170                 position.Estimation = best;
+01171         }
+01172 }
+01173 
+01174 float   NLPACS::CLocalRetriever::getHeight(const NLPACS::ULocalPosition &position) const
+01175 {
+01176         if (_Type == Interior)
+01177         {
+01178                 // first preselect faces around the (x, y) position (CQuadGrid ?)
+01179                 vector<uint32>  selection;
+01180                 _FaceGrid.select(position.Estimation, selection);
+01181 
+01182                 // from the preselect faces, look for the only face that belongs to the surface
+01183                 // and that contains the position
+01184                 CVector pos = position.Estimation;
+01185                 CVector posh = pos+CVector(0.0f, 0.0f, 1.0f);
+01186                 float   bestDist = 1.0e10f;
+01187                 CVector best;
+01188                 vector<uint32>::iterator        it;
+01189                 for (it=selection.begin(); it!=selection.end(); ++it)
+01190                 {
+01191                         const CInteriorFace     &f = _InteriorFaces[*it];
+01192                         if (f.Surface == (uint32)position.Surface)
+01193                         {
+01194                                 CVector v[3];
+01195                                 v[0] = _InteriorVertices[f.Verts[0]];
+01196                                 v[1] = _InteriorVertices[f.Verts[1]];
+01197                                 v[2] = _InteriorVertices[f.Verts[2]];
+01198 
+01199                                 float           a,b,c;          // 2D cartesian coefficients of line in plane X/Y.
+01200                                 // Line p0-p1.
+01201                                 a = -(v[1].y-v[0].y);
+01202                                 b =  (v[1].x-v[0].x);
+01203                                 c = -(v[0].x*a + v[0].y*b);
+01204                                 if (a*pos.x + b*pos.y + c < 0)  continue;
+01205                                 // Line p1-p2.
+01206                                 a = -(v[2].y-v[1].y);
+01207                                 b =  (v[2].x-v[1].x);
+01208                                 c = -(v[1].x*a + v[1].y*b);
+01209                                 if (a*pos.x + b*pos.y + c < 0)  continue;
+01210                                 //  Line p2-p0.
+01211                                 a = -(v[0].y-v[2].y);
+01212                                 b =  (v[0].x-v[2].x);
+01213                                 c = -(v[2].x*a + v[2].y*b);
+01214                                 if (a*pos.x + b*pos.y + c < 0)  continue;
+01215 
+01216                                 CPlane  p;
+01217                                 p.make(v[0], v[1], v[2]);
+01218 
+01219                                 CVector i = p.intersect(pos, posh);
+01220 
+01221                                 float   d = (float)fabs(pos.z-i.z);
+01222 
+01223                                 if (d < bestDist)
+01224                                 {
+01225                                         bestDist = d;
+01226                                         best = i;
+01227                                 }
+01228                         }
+01229                 }
+01230 
+01231                 // and computes the real position on this face
+01232                 return (bestDist < 50.0f) ?     best.z : position.Estimation.z;
+01233         }
+01234         else
+01235         {
+01236 
+01237                 // find quad leaf.
+01238                 const CQuadLeaf *leaf = _Surfaces[position.Surface].getQuadTree().getLeaf(position.Estimation);
+01239 
+01240                 // if there is no acceptable leaf, just give up
+01241                 if (leaf == NULL)
+01242                 {
+01243                         //nlinfo("COL: quadtree: don't find the quadLeaf!");
+01244                         return position.Estimation.z;
+01245                 }
+01246                 else
+01247                 {
+01248                         // else return mean height.
+01249                         float   meanHeight = (leaf->getMinHeight()+leaf->getMaxHeight())*0.5f;
+01250                         return meanHeight;
+01251                 }
+01252 
+01253 //              return _Surfaces[position.Surface].getQuadTree().getInterpZ(position.Estimation);
+01254         }
+01255 }
+01256 
+01257 
+01258 
+01259 #ifdef NL_OS_WINDOWS
+01260 #pragma optimize( "", off )
+01261 #endif // NL_OS_WINDOWS
+01262 
+01263 void    NLPACS::CLocalRetriever::findPath(const NLPACS::CLocalRetriever::CLocalPosition &A, 
+01264                                                                                   const NLPACS::CLocalRetriever::CLocalPosition &B, 
+01265                                                                                   std::vector<NLPACS::CVector2s> &path, 
+01266                                                                                   NLPACS::CCollisionSurfaceTemp &cst) const
+01267 {
+01268         if (A.Surface != B.Surface)
+01269         {
+01270                 nlwarning("in NLPACS::CLocalRetriever::findPath()");
+01271                 nlerror("Try to find a path between 2 points that are not in the same surface (A=%d, B=%d)", A.Surface, B.Surface);
+01272         }
+01273 
+01274         CVector         a = A.Estimation,
+01275                                 b = B.Estimation,
+01276                                 n = CVector(a.y-b.y, b.x-a.x, 0.0f);
+01277 
+01278         _ChainQuad.selectEdges(a, b, cst);
+01279 
+01281         vector<CIntersectionMarker>     intersections;
+01282 
+01283         uint    i, j;
+01284         sint32  surfaceId = A.Surface;
+01285         const CRetrievableSurface       &surface = _Surfaces[surfaceId];
+01286 
+01287         for (i=0; i<cst.EdgeChainEntries.size(); ++i)
+01288         {
+01289                 CEdgeChainEntry         &entry = cst.EdgeChainEntries[i];
+01290                 const COrderedChain     &chain = _OrderedChains[entry.OChainId];
+01291 
+01292                 if (_Chains[chain.getParentId()].getLeft() != surfaceId &&
+01293                         _Chains[chain.getParentId()].getRight() != surfaceId)
+01294                         continue;
+01295 
+01296                 for (j=entry.EdgeStart; j<entry.EdgeEnd; ++j)
+01297                 {
+01298                         // here the edge collision test
+01299 
+01300                         CVector p0 = chain[j].unpack3f(),
+01301                                         p1 = chain[j+1].unpack3f();
+01302 
+01303                         float   vp0 = (p0-a)*n,
+01304                                         vp1 = (p1-a)*n;
+01305 
+01306                         if (vp0*vp1 <= 0.0f)
+01307                         {
+01308                                 CVector np = CVector(p0.y-p1.y, p1.x-p0.x, 0.0f);
+01309 
+01310                                 float   va = (a-p0)*np,
+01311                                                 vb = (b-p0)*np;
+01312 
+01313                                 // here we have an intersection
+01314                                 if (va*vb <= 0.0f)
+01315                                 {
+01316                                         const CChain    &parent = _Chains[chain.getParentId()];
+01317                                         bool                    isIn = (va-vb < 0.0f) ^ (parent.getLeft() == surfaceId) ^ chain.isForward();
+01318 
+01319                                         intersections.push_back(CIntersectionMarker(va/(va-vb), entry.OChainId, j, isIn));
+01320                                 }
+01321                         }
+01322                 }
+01323         }
+01324 
+01325         sort(intersections.begin(), intersections.end());
+01326 
+01327         uint    intersStart = 0;
+01328         uint    intersEnd = intersections.size();
+01329 
+01330         if (intersEnd > 0)
+01331         {
+01332                 while (intersStart < intersections.size() &&
+01333                            intersections[intersStart].In && intersections[intersStart].Position < 1.0e-4f)
+01334                         ++intersStart;
+01335 
+01336                 while (intersStart < intersEnd &&
+01337                            !intersections[intersEnd-1].In && intersections[intersEnd-1].Position > 1.0f-1.0e-4f)
+01338                         --intersEnd;
+01339 
+01340                 // Check intersections have a valid order
+01341                 if ((intersEnd-intersStart) & 1)
+01342                 {
+01343                         nlwarning("in NLPACS::CLocalRetriever::findPath()");
+01344                         nlerror("Found an odd (%d) number of intersections", intersections.size());
+01345                 }
+01346                 
+01347                 for (i=intersStart; i<intersEnd; )
+01348                 {
+01349                         uint    exitLoop, enterLoop;
+01350 
+01351                         const CChain    &exitChain = _Chains[_OrderedChains[intersections[i].OChain].getParentId()];
+01352                         exitLoop = (exitChain.getLeft() == surfaceId) ? exitChain.getLeftLoop() : exitChain.getRightLoop();
+01353 
+01354                         if (intersections[i++].In)
+01355                         {
+01356                                 nlwarning("in NLPACS::CLocalRetriever::findPath()");
+01357                                 nlerror("Entered the surface before exited", intersections.size());
+01358                         }
+01359 
+01360                         const CChain    &enterChain = _Chains[_OrderedChains[intersections[i].OChain].getParentId()];
+01361                         enterLoop = (enterChain.getLeft() == surfaceId) ? enterChain.getLeftLoop() : enterChain.getRightLoop();
+01362 
+01363                         if (!intersections[i++].In)
+01364                         {
+01365                                 nlwarning("in NLPACS::CLocalRetriever::findPath()");
+01366                                 nlerror("Exited twice the surface", intersections.size());
+01367                         }
+01368 
+01369                         if (exitLoop != enterLoop)
+01370                         {
+01371                                 nlwarning("in NLPACS::CLocalRetriever::findPath()");
+01372                                 nlerror("Exited and rentered by a different loop");
+01373                         }
+01374                 }
+01375         }
+01376 
+01377 //      dumpSurface(surfaceId);
+01378 
+01379         path.push_back(CVector2s(A.Estimation));
+01380 
+01381         for (i=intersStart; i<intersEnd; )
+01382         {
+01383                 uint                                                            exitChainId = _OrderedChains[intersections[i].OChain].getParentId(),
+01384                                                                                         enterChainId = _OrderedChains[intersections[i+1].OChain].getParentId();
+01385                 const CChain                                            &exitChain = _Chains[exitChainId],
+01386                                                                                         &enterChain = _Chains[enterChainId];
+01387                 uint                                                            loopId, exitLoopIndex, enterLoopIndex;
+01388 
+01389                 if (exitChain.getLeft() == surfaceId)
+01390                 {
+01391                         loopId = exitChain.getLeftLoop();
+01392                         exitLoopIndex = exitChain.getLeftLoopIndex();
+01393                 }
+01394                 else
+01395                 {
+01396                         loopId = exitChain.getRightLoop();
+01397                         exitLoopIndex = exitChain.getRightLoopIndex();
+01398                 }
+01399 
+01400                 const CRetrievableSurface::TLoop        &loop = surface._Loops[loopId];
+01401 
+01402                 if (enterChain.getLeft() == surfaceId)
+01403                         enterLoopIndex = enterChain.getLeftLoopIndex();
+01404                 else
+01405                         enterLoopIndex = enterChain.getRightLoopIndex();
+01406 
+01407                 float                   forwardLength = (exitChain.getLength()+enterChain.getLength())*0.5f;
+01408 
+01409                 sint    loopIndex = exitLoopIndex;
+01410                 uint    thisChainId = exitChainId;
+01411                 bool    thisChainForward = (enterChain.getLeft() == surfaceId);
+01412                 uint    thisOChainId = intersections[i].OChain;
+01413                 sint    thisOChainIndex = _OrderedChains[thisOChainId].getIndexInParent();
+01414                 bool    forward;
+01415 
+01416                 if (exitChainId != enterChainId)
+01417                 {
+01418                         for (j=(exitLoopIndex+1)%loop.size(); j!=enterLoopIndex; j=(j+1)%loop.size())
+01419                                 forwardLength += _Chains[surface._Chains[loop[j]].Chain].getLength();
+01420                         forward = (forwardLength <= loop.Length-forwardLength);
+01421                 }
+01422                 else
+01423                 {
+01424                         forward = !thisChainForward ^ (_OrderedChains[intersections[i].OChain].getIndexInParent() < _OrderedChains[intersections[i+1].OChain].getIndexInParent());
+01425                 }
+01426 
+01427                 path.push_back(CVector2s(A.Estimation+intersections[i].Position*(B.Estimation-A.Estimation)));
+01428 
+01429                 while (true)
+01430                 {
+01431                         sint    from = (thisOChainId == intersections[i].OChain) ? intersections[i].Edge : -1,
+01432                                         to = (thisOChainId == intersections[i+1].OChain) ? intersections[i+1].Edge : -1;
+01433                         bool    oforward = thisChainForward ^ forward ^ _OrderedChains[thisOChainId].isForward();
+01434 
+01435                         if (from != -1 && to != -1)
+01436                                 oforward = (intersections[i].Edge < intersections[i+1].Edge);
+01437 
+01438                         _OrderedChains[thisOChainId].traverse(from, to, oforward, path);
+01439 
+01440                         if (thisOChainId == intersections[i+1].OChain)
+01441                                 break;
+01442 
+01443                         thisOChainIndex = (thisChainForward ^ forward) ? thisOChainIndex-1 : thisOChainIndex+1;
+01444 
+01445                         if (thisOChainIndex < 0 || thisOChainIndex >= (sint)_Chains[thisChainId]._SubChains.size())
+01446                         {
+01447                                 if (forward)
+01448                                 {
+01449                                         loopIndex++;
+01450                                         if (loopIndex == (sint)loop.size())
+01451                                                 loopIndex = 0;
+01452                                 }
+01453                                 else
+01454                                 {
+01455                                         loopIndex--;
+01456                                         if (loopIndex < 0)
+01457                                                 loopIndex = loop.size()-1;
+01458                                 }
+01459 
+01460                                 thisChainId = surface._Chains[loop[loopIndex]].Chain;
+01461                                 thisChainForward = (_Chains[thisChainId].getLeft() == surfaceId);
+01462                                 thisOChainIndex = (thisChainForward && forward || !thisChainForward && !forward) ? 
+01463                                         0 : _Chains[thisChainId]._SubChains.size()-1;
+01464                         }
+01465 
+01466                         thisOChainId = _Chains[thisChainId]._SubChains[thisOChainIndex];
+01467                 }
+01468 
+01469                 path.push_back(CVector2s(A.Estimation+intersections[i+1].Position*(B.Estimation-A.Estimation)));
+01470                 i += 2;
+01471         }
+01472 
+01473         path.push_back(CVector2s(B.Estimation));
+01474 }
+01475 #ifdef NL_OS_WINDOWS
+01476 #pragma optimize( "", on ) 
+01477 #endif // NL_OS_WINDOWS
+01478 
+01479 // ***************************************************************************
+01480 // ***************************************************************************
+01481 // Collisions part.
+01482 // ***************************************************************************
+01483 // ***************************************************************************
+01484 
+01485 
+01486 // ***************************************************************************
+01487 void    NLPACS::CLocalRetriever::computeCollisionChainQuad()
+01488 {
+01489         _ChainQuad.build(_OrderedChains);
+01490 }
+01491 
+01492 
+01493 // ***************************************************************************
+01494 void    NLPACS::CLocalRetriever::testCollision(CCollisionSurfaceTemp &cst, const CAABBox &bboxMove, const CVector2f &transBase) const
+01495 {
+01496 //      H_AUTO(PACS_LR_testCollision);
+01497 
+01498         sint    i;
+01499 
+01500         // 0. select ordered chains in the chainquad.
+01501         //=====================================
+01502 //      H_BEFORE(PACS_LR_testCol_selEdges);
+01503         sint    nEce= _ChainQuad.selectEdges(bboxMove, cst);
+01504 //      H_AFTER(PACS_LR_testCol_selEdges);
+01505         // NB: cst.OChainLUT is assured to be full of 0xFFFF after this call (if was right before).
+01506 
+01507 
+01508         // 1. regroup them in chains. build cst.CollisionChains
+01509         //=====================================
+01510         // NB: use cst.OChainLUT to look if a Chain has been inserted before.
+01511         uint16  *chainLUT= cst.OChainLUT;
+01512 
+01513         // bkup where we begin to add chains.
+01514         uint    firstChainAdded= cst.CollisionChains.size();
+01515 
+01516         // For all edgechain entry.
+01517         for(i=0;i<nEce;i++)
+01518         {
+01519                 CEdgeChainEntry         &ece= cst.EdgeChainEntries[i];
+01520                 // this is the ordered chain in the retriever.
+01521                 const   COrderedChain   &oChain= this->getOrderedChains()[ece.OChainId];
+01522                 // this is the id of the chain is the local retriever.
+01523                 uint16                          chainId= oChain.getParentId();
+01524 
+01525 
+01526                 // add/retrieve the id in cst.CollisionChains.
+01527                 //=================================
+01528                 uint                            ccId;
+01529                 // if never added.
+01530                 if(chainLUT[chainId]==0xFFFF)
+01531                 {
+01532 //                      H_AUTO(PACS_LR_testCol_addToLUT);
+01533                         // add a new CCollisionChain.
+01534                         ccId= cst.CollisionChains.size();
+01535                         cst.CollisionChains.push_back(CCollisionChain());
+01536                         // Fill it with default.
+01537                         cst.CollisionChains[ccId].Tested= false;
+01538                         cst.CollisionChains[ccId].ExteriorEdge = false;
+01539                         cst.CollisionChains[ccId].FirstEdgeCollide= 0xFFFFFFFF;
+01540                         cst.CollisionChains[ccId].ChainId= chainId;
+01541                         // Fill Left right info.
+01542                         cst.CollisionChains[ccId].LeftSurface.SurfaceId= this->getChains()[chainId].getLeft();
+01543                         cst.CollisionChains[ccId].RightSurface.SurfaceId= this->getChains()[chainId].getRight();
+01544                         // NB: cst.CollisionChains[ccId].*Surface.RetrieverInstanceId is not filled here because we don't have
+01545                         // this info at this level.
+01546 
+01547                         // store this Id in the LUT of chains.
+01548                         chainLUT[chainId]= ccId;
+01549                 }
+01550                 else
+01551                 {
+01552                         // get the id of this collision chain.
+01553                         ccId= chainLUT[chainId];
+01554                 }
+01555 
+01556                 // add edge collide to the list.
+01557                 //=================================
+01558 //              H_BEFORE(PACS_LR_testCol_addToList);
+01559                 CCollisionChain                 &colChain= cst.CollisionChains[ccId];
+01560                 const std::vector<CVector2s>    &oChainVertices= oChain.getVertices();
+01561                 for(sint edge=ece.EdgeStart; edge<ece.EdgeEnd; edge++)
+01562                 {
+01563                         CVector2f       p0= oChainVertices[edge].unpack();
+01564                         CVector2f       p1= oChainVertices[edge+1].unpack();
+01565 
+01566                         // alloc a new edgeCollide.
+01567                         uint32  ecnId= cst.allocEdgeCollideNode();
+01568                         CEdgeCollideNode        &ecn= cst.getEdgeCollideNode(ecnId);
+01569 
+01570                         // append to the front of the list.
+01571                         ecn.Next= colChain.FirstEdgeCollide;
+01572                         colChain.FirstEdgeCollide= ecnId;
+01573 
+01574                         // build this edge.
+01575                         p0+= transBase;
+01576                         p1+= transBase;
+01577                         ecn.make(p0, p1);
+01578                 }
+01579 //              H_AFTER(PACS_LR_testCol_addToList);
+01580         }
+01581 
+01582 
+01583 
+01584         // 2. Reset LUT to 0xFFFF.
+01585         //=====================================
+01586 
+01587 //      H_BEFORE(PACS_LR_testCol_resetLUT);
+01588         // for all collisions chains inserted (starting from firstChainAdded), reset LUT.
+01589         for(i=firstChainAdded; i<(sint)cst.CollisionChains.size(); i++)
+01590         {
+01591                 uint    ccId= cst.CollisionChains[i].ChainId;
+01592                 chainLUT[ccId]= 0xFFFF;
+01593         }
+01594 //      H_AFTER(PACS_LR_testCol_resetLUT);
+01595 }
+01596 
+01597 
+01598 // ***************************************************************************
+01599 // ***************************************************************************
+01600 // ***************************************************************************
+01601 // ***************************************************************************
+01602 
+01603 
+01604 // ***************************************************************************
+01605 void    NLPACS::CLocalRetriever::buildInteriorSurfaceBBoxes(std::vector<NLMISC::CAABBox>        &surfaceBBoxes) const
+01606 {
+01607         // resize dest, and init.
+01608         vector<bool>    firstTriangle;
+01609         surfaceBBoxes.clear();
+01610         surfaceBBoxes.resize(_Surfaces.size());
+01611         firstTriangle.resize(_Surfaces.size(), true);
+01612 
+01613         // For all _InteriorFaces.
+01614         for(uint iIntFace=0; iIntFace<_InteriorFaces.size(); iIntFace++)
+01615         {
+01616                 const CInteriorFace     &intFace= _InteriorFaces[iIntFace];
+01617 
+01618                 // Extend the surface of this face with her 3 points.
+01619 
+01620                 // check good id.
+01621                 if(intFace.Surface==(uint)-1)
+01622                         continue;
+01623                 nlassert(intFace.Surface<_Surfaces.size());
+01624 
+01625                 // If first time we extend the bbox of this surface
+01626                 if(firstTriangle[intFace.Surface])
+01627                 {
+01628                         surfaceBBoxes[intFace.Surface].setCenter(_InteriorVertices[intFace.Verts[0]] );
+01629                         firstTriangle[intFace.Surface]= false;
+01630                 }
+01631                 else
+01632                         surfaceBBoxes[intFace.Surface].extend(_InteriorVertices[intFace.Verts[0]] );
+01633 
+01634                 // extend with other 2 points
+01635                 surfaceBBoxes[intFace.Surface].extend(_InteriorVertices[intFace.Verts[1]] );
+01636                 surfaceBBoxes[intFace.Surface].extend(_InteriorVertices[intFace.Verts[2]] );
+01637         }
+01638 
+01639 }
+
local_retriever.cpp
