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/path.h"
+00029 #include "nel/misc/line.h"
+00030 
+00031 #include "nel/misc/hierarchical_timer.h"
+00032 
+00033 #include "pacs/global_retriever.h"
+00034 #include "pacs/retriever_bank.h"
+00035 
+00036 
+00037 #include "nel/misc/time_nl.h"
+00038 NLMISC::TTicks                  AStarTicks;
+00039 NLMISC::TTicks                  PathTicks;
+00040 NLMISC::TTicks                  ChainTicks;
+00041 NLMISC::TTicks                  SurfTicks;
+00042 NLMISC::TTicks                  ThisAStarTicks;
+00043 NLMISC::TTicks                  ThisPathTicks;
+00044 NLMISC::TTicks                  ThisChainTicks;
+00045 NLMISC::TTicks                  ThisSurfTicks;
+00046 
+00047 using namespace std;
+00048 using namespace NLMISC;
+00049 
+00050 const float             InsureSurfaceThreshold = 0.5f;  // the threshold distance between 2 surfaces below which we insure the retrieved position to be inside the surface
+00051 
+00052 // CGlobalRetriever methods implementation
+00053 
+00054 //
+00055 void    NLPACS::CGlobalRetriever::init()
+00056 {
+00057         _BBox.setCenter(CVector::Null);
+00058         _BBox.setHalfSize(CVector::Null);
+00059 
+00060         _InstanceGrid.create(128, 160.0f);
+00061 }
+00062 
+00063 void    NLPACS::CGlobalRetriever::initQuadGrid()
+00064 {
+00065         _InstanceGrid.clear();
+00066         _InstanceGrid.create(128, 160.0f);
+00067 
+00068         uint    i;
+00069         for (i=0; i<_Instances.size(); ++i)
+00070                 _InstanceGrid.insert(_Instances[i].getBBox().getMin(), _Instances[i].getBBox().getMax(), i);
+00071 }
+00072 
+00073 void    NLPACS::CGlobalRetriever::initRetrieveTable()
+00074 {
+00075         uint    i;
+00076         uint    size = 0;
+00077 
+00078         for (i=0; i<_Instances.size(); ++i)
+00079         {
+00080                 if (_Instances[i].getInstanceId() != -1 && _Instances[i].getRetrieverId() != -1)
+00081                 {
+00082                         const CLocalRetriever   &retriever = getRetriever(_Instances[i].getRetrieverId());
+00083                         size =  std::max(retriever.getSurfaces().size(), size);
+00084                 }
+00085         }
+00086 
+00087         _RetrieveTable.resize(size);
+00088         for (i=0; i<size; ++i)
+00089                 _RetrieveTable[i] = 0;
+00090 }
+00091 
+00092 //
+00093 
+00094 void    NLPACS::CGlobalRetriever::selectInstances(const NLMISC::CAABBox &bbox, CCollisionSurfaceTemp &cst) const
+00095 {
+00096         _InstanceGrid.select(bbox.getMin(), bbox.getMax());
+00097         cst.CollisionInstances.clear();
+00098 
+00099         NLPACS::CQuadGrid<uint32>::CIterator    it;
+00100         for (it=_InstanceGrid.begin(); it!=_InstanceGrid.end(); ++it)
+00101                 if (_Instances[*it].getBBox().intersect(bbox))
+00102                         cst.CollisionInstances.push_back(*it);
+00103 }
+00104 
+00105 //
+00106 
+00107 void    NLPACS::CGlobalRetriever::serial(NLMISC::IStream &f)
+00108 {
+00109         /*
+00110         Version 0:
+00111                 - base version.
+00112         */
+00113         (void)f.serialVersion(0);
+00114 
+00115         f.serialCont(_Instances);
+00116         f.serial(_BBox);
+00117 
+00118         if (f.isReading())
+00119         {
+00120                 initQuadGrid();
+00121                 initRetrieveTable();
+00122         }
+00123 }
+00124 
+00125 //
+00126 
+00127 void    NLPACS::CGlobalRetriever::check() const
+00128 {
+00129         uint    i, j, k;
+00130 
+00131         for (i=0; i<_Instances.size(); ++i)
+00132         {
+00133                 if (_Instances[i].getInstanceId() == -1)
+00134                 {
+00135                         nlwarning("Uninitialized instance %d", i);
+00136                         continue;
+00137                 }
+00138 
+00139                 if (_Instances[i].getInstanceId() != (sint)i)
+00140                         nlwarning("InstanceId for instance %d is not correctly initialized", i);
+00141 
+00142                 if (_Instances[i].getRetrieverId() == -1)
+00143                 {
+00144                         nlwarning("No retriever at instance %d", i);
+00145                         continue;
+00146                 }
+00147 
+00148                 const CRetrieverInstance        &instance = _Instances[i];
+00149 
+00150                 if (instance.getRetrieverId()<0 || instance.getRetrieverId()>=(sint)_RetrieverBank->getRetrievers().size())
+00151                 {
+00152                         nlwarning("Instance %d has wrong retriever reference", i);
+00153                         continue;
+00154                 }
+00155 
+00156                 const CLocalRetriever           &retriever = _RetrieverBank->getRetriever(instance.getRetrieverId());
+00157 
+00158                 for (j=0; j<retriever.getChains().size(); ++j)
+00159                 {
+00160                         const CChain    &chain = retriever.getChain(j);
+00161                         for (k=0; k<chain.getSubChains().size(); ++k)
+00162                         {
+00163                                 if (chain.getSubChain(k) >= retriever.getOrderedChains().size())
+00164                                 {
+00165                                         nlwarning("retriever %d, chain %d: subchain %d reference is not valid", instance.getRetrieverId(), j, k);
+00166                                         continue;
+00167                                 }
+00168 
+00169                                 if (retriever.getOrderedChain(chain.getSubChain(k)).getParentId() != j)
+00170                                 {
+00171                                         nlwarning("retriever %d, ochain %d: reference on parent is not valid", instance.getRetrieverId(), chain.getSubChain(k));
+00172                                         continue;
+00173                                 }
+00174 
+00175                                 if (retriever.getOrderedChain(chain.getSubChain(k)).getIndexInParent() != k)
+00176                                 {
+00177                                         nlwarning("retriever %d, ochain %d: index on parent is not valid", instance.getRetrieverId(), chain.getSubChain(k));
+00178                                         continue;
+00179                                 }
+00180                         }
+00181 
+00182                         if (chain.getLeft()<0 || chain.getLeft()>=(sint)retriever.getSurfaces().size())
+00183                         {
+00184                                 nlwarning("retriever %d, chain %d: reference on left surface is not valid", instance.getRetrieverId(), j);
+00185                         }
+00186 
+00187                         if (chain.getRight()>=(sint)retriever.getSurfaces().size() ||
+00188                                 chain.getRight()<=CChain::getDummyBorderChainId() && !CChain::isBorderChainId(chain.getRight()))
+00189                         {
+00190                                 nlwarning("retriever %d, chain %d: reference on right surface is not valid", instance.getRetrieverId(), j);
+00191                         }
+00192 
+00193                         if (CChain::isBorderChainId(chain.getRight()))
+00194                         {
+00195                                 sint    link = chain.getBorderChainIndex();
+00196 
+00197                                 if (link<0 || link>=(sint)instance.getBorderChainLinks().size())
+00198                                 {
+00199                                         nlwarning("retriever %d, instance %d, chain %d: reference on right link is not valid", instance.getRetrieverId(), instance.getInstanceId(), j);
+00200                                 }
+00201                                 else
+00202                                 {
+00203                                         CRetrieverInstance::CLink       lnk = instance.getBorderChainLink(link);
+00204 
+00205                                         if (lnk.Instance != 0xFFFF || lnk.SurfaceId != 0xFFFF ||
+00206                                                 lnk.ChainId != 0xFFFF || lnk.BorderChainId != 0xFFFF)
+00207                                         {
+00208                                                 if (lnk.Instance >= _Instances.size() ||
+00209                                                         _Instances[lnk.Instance].getRetrieverId()<0 ||
+00210                                                         _Instances[lnk.Instance].getRetrieverId()>(sint)_RetrieverBank->getRetrievers().size() ||
+00211                                                         lnk.SurfaceId >= getRetriever(_Instances[lnk.Instance].getRetrieverId()).getSurfaces().size() ||
+00212                                                         ((lnk.ChainId >= getRetriever(_Instances[lnk.Instance].getRetrieverId()).getChains().size() ||
+00213                                                         lnk.BorderChainId >= getRetriever(_Instances[lnk.Instance].getRetrieverId()).getBorderChains().size()) && instance.getType() != CLocalRetriever::Interior ))
+00214                                                 {
+00215                                                         nlwarning("retriever %d, instance %d, link %d: reference on instance may be not valid [Inst=%d, Surf=%d, Chain=%d, BorderChain=%d]", instance.getRetrieverId(), instance.getInstanceId(), link, lnk.Instance, lnk.SurfaceId, lnk.ChainId, lnk.BorderChainId);
+00216                                                 }
+00217                                         }
+00218                                 }
+00219                         }
+00220                 }
+00221         }
+00222 }
+00223 
+00224 //
+00225 
+00226 float   NLPACS::CGlobalRetriever::distanceToBorder(const UGlobalPosition &pos) const
+00227 {
+00228         if (pos.InstanceId < 0 || pos.InstanceId > (sint)_Instances.size())
+00229                 return 0.0f;
+00230 
+00231         return getRetriever(_Instances[pos.InstanceId].getRetrieverId()).distanceToBorder(pos.LocalPosition);
+00232 }
+00233 
+00234 void    NLPACS::CGlobalRetriever::getBorders(const UGlobalPosition &pos, std::vector<NLMISC::CLine> &edges)
+00235 {
+00236         edges.clear();
+00237 
+00238         if (pos.InstanceId < 0)
+00239                 return;
+00240 
+00241         CRetrieverInstance      &instance = _Instances[pos.InstanceId];
+00242         CLocalRetriever         &retriever = const_cast<CLocalRetriever &>(getRetriever(instance.getRetrieverId()));
+00243         CChainQuad                      &chainquad = retriever.getChainQuad();
+00244 
+00245         CAABBox                         box;
+00246         box.setCenter(pos.LocalPosition.Estimation);
+00247         box.setHalfSize(CVector(15.0f, 15.0f, 100.0f));
+00248         chainquad.selectEdges(box, _InternalCST);
+00249 
+00250         uint            ece;
+00251         CVector         origin = instance.getOrigin();
+00252         float   zp = pos.LocalPosition.Estimation.z+origin.z;
+00253         for (ece=0; ece<_InternalCST.EdgeChainEntries.size(); ++ece)
+00254         {
+00255                 CEdgeChainEntry         &entry = _InternalCST.EdgeChainEntries[ece];
+00256                 const COrderedChain     &ochain = retriever.getOrderedChain(entry.OChainId);
+00257 
+00258                 uint    edge;
+00259                 for (edge=entry.EdgeStart; edge+1<entry.EdgeEnd; ++edge)
+00260                 {
+00261                         edges.push_back(CLine());
+00262                         edges.back().V0 = ochain[edge].unpack3f() + origin;
+00263                         edges.back().V0.z = zp;
+00264                         edges.back().V1 = ochain[edge+1].unpack3f() + origin;
+00265                         edges.back().V1.z = zp;
+00266                 }
+00267         }
+00268 }
+00269 
+00270 
+00271 //
+00272 
+00273 void    NLPACS::CGlobalRetriever::makeLinks(uint n)
+00274 {
+00275         CRetrieverInstance      &instance = _Instances[n];
+00276 
+00277         selectInstances(instance.getBBox(), _InternalCST);
+00278 
+00279         uint    i;
+00280         for (i=0; i<_InternalCST.CollisionInstances.size(); ++i)
+00281         {
+00282                 CRetrieverInstance      &neighbor = _Instances[_InternalCST.CollisionInstances[i]];
+00283 
+00284                 if (neighbor.getInstanceId() == instance.getInstanceId())
+00285                         continue;
+00286 
+00287                 try
+00288                 {
+00289                         instance.link(neighbor, _RetrieverBank->getRetrievers());
+00290                         neighbor.link(instance, _RetrieverBank->getRetrievers());
+00291                 }
+00292                 catch (Exception &e)
+00293                 {
+00294                         nlwarning("in NLPACS::CGlobalRetriever::makeLinks()");
+00295                         nlwarning("caught an exception during linkage of %d and %d: %s", instance.getInstanceId(), neighbor.getInstanceId(), e.what());
+00296                 }
+00297         }
+00298 
+00299         if (getRetriever(instance.getRetrieverId()).getType() == CLocalRetriever::Interior)
+00300                 instance.linkEdgeQuad(*this);
+00301 }
+00302 
+00303 void    NLPACS::CGlobalRetriever::resetAllLinks()
+00304 {
+00305         uint    n;
+00306         for (n=0; n<_Instances.size(); ++n)
+00307                 _Instances[n].unlink(_Instances);
+00308 }
+00309 
+00310 
+00311 void    NLPACS::CGlobalRetriever::makeAllLinks()
+00312 {
+00313         resetAllLinks();
+00314 
+00315         uint    n;
+00316         for (n=0; n<_Instances.size(); ++n)
+00317                 makeLinks(n);
+00318 }
+00319 
+00320 void    NLPACS::CGlobalRetriever::initAll()
+00321 {
+00322         uint    n;
+00323         for (n=0; n<_Instances.size(); ++n)
+00324                 if (_Instances[n].getInstanceId() != -1 && _Instances[n].getRetrieverId() != -1)
+00325                         _Instances[n].init(_RetrieverBank->getRetriever(_Instances[n].getRetrieverId()));
+00326 
+00327         initQuadGrid();
+00328         initRetrieveTable();
+00329 }
+00330 
+00331 //
+00332 
+00333 const NLPACS::CRetrieverInstance        &NLPACS::CGlobalRetriever::makeInstance(uint32 retrieverId, uint8 orientation, const CVector &origin)
+00334 {
+00335         uint    id;
+00336         for (id=0; id<_Instances.size() && _Instances[id].getInstanceId()!=-1; ++id)
+00337                 ;
+00338 
+00339         if (id == _Instances.size())
+00340                 _Instances.resize(id+1);
+00341 
+00342         CRetrieverInstance              &instance = _Instances[id];
+00343         const CLocalRetriever   &retriever = getRetriever(retrieverId);
+00344 
+00345         if (_RetrieveTable.size() < retriever.getSurfaces().size())
+00346                 _RetrieveTable.resize(retriever.getSurfaces().size(), 0);
+00347 
+00348         instance.make(id, retrieverId, retriever, orientation, origin);
+00349 
+00350         CVector hsize = instance.getBBox().getHalfSize();
+00351         hsize.z = 0.0f;
+00352         if (hsize != CVector::Null)
+00353         {
+00354                 if (_BBox.getHalfSize() == CVector::Null)
+00355                 {
+00356                         _BBox = instance.getBBox();
+00357                 }
+00358                 else
+00359                 {
+00360                         _BBox.extend(instance.getBBox().getMin());
+00361                         _BBox.extend(instance.getBBox().getMax());
+00362                 }
+00363 
+00364                 if (getRetriever(instance.getRetrieverId()).getType() == CLocalRetriever::Interior)
+00365                         instance.initEdgeQuad(*this);
+00366 
+00367                 _InstanceGrid.insert(instance.getBBox().getMin(), instance.getBBox().getMax(), instance.getInstanceId());
+00368         }
+00369 
+00370         return instance;
+00371 }
+00372 
+00373 //
+00374 
+00375 /*
+00376 NLPACS::UGlobalPosition NLPACS::CGlobalRetriever::retrievePosition(const CVector &estimated, float threshold) const
+00377 {
+00378         // the retrieved position
+00379         CGlobalPosition                         result = CGlobalPosition(-1, CLocalRetriever::CLocalPosition(-1, estimated));
+00380 
+00381         if (!_BBox.include(estimated))
+00382                 return result;
+00383         
+00384         // get the best matching instances
+00385         CAABBox bbpos;
+00386         bbpos.setCenter(estimated);
+00387         bbpos.setHalfSize(CVector(0.5f, 0.5f, 0.5f));
+00388         selectInstances(bbpos, _InternalCST);
+00389 
+00390         uint    i;
+00391         float   bestDist = threshold;
+00392         bool    snapinterior = false;
+00393 
+00394         // for each instance, try to retrieve the position
+00395         for (i=0; i<_InternalCST.CollisionInstances.size(); ++i)
+00396         {
+00397                 uint32  id = _InternalCST.CollisionInstances[i];
+00398                 // if the retrieved position is on a surface and it best match the estimated position
+00399                 // remember it
+00400                 const CRetrieverInstance                &instance = _Instances[id];
+00401                 CVector                                                 lestim = instance.getLocalPosition(estimated);
+00402                 CLocalRetriever::CLocalPosition ret = instance.retrievePosition(estimated, _RetrieverBank->getRetriever(_Instances[id].getRetrieverId()), _InternalCST);
+00403                 // go preferably on interior instances
+00404                 float                                                   d = (float)fabs(lestim.z-ret.Estimation.z);
+00405                 bool                                                    itype = (instance.getType() == CLocalRetriever::Interior);
+00406                 if (ret.Surface != -1 && (snapinterior && itype && d < bestDist ||
+00407                                                                   !snapinterior && itype && d < 1.0f ||
+00408                                                                   !snapinterior && itype && d >= 1.0f && d < bestDist ||
+00409                                                                   !snapinterior && !itype && d < bestDist))
+00410                 {
+00411                         if (itype && d < 1.0f)
+00412                                 snapinterior = true;
+00413                         bestDist = d;
+00414                         result.LocalPosition = ret;
+00415                         result.InstanceId = id;
+00416                 }
+00417         }
+00418 
+00419         return result;
+00420 }
+00421 */
+00422 NLPACS::UGlobalPosition NLPACS::CGlobalRetriever::retrievePosition(const CVector &estimated, float threshold) const
+00423 {
+00424         return retrievePosition(CVectorD(estimated), (double)threshold);
+00425 }
+00426 
+00427 /*
+00428 NLPACS::UGlobalPosition NLPACS::CGlobalRetriever::retrievePosition(const CVectorD &estimated, double threshold) const
+00429 {
+00430         // the retrieved position
+00431         CGlobalPosition                         result = CGlobalPosition(-1, CLocalRetriever::CLocalPosition(-1, estimated));
+00432 
+00433         if (!_BBox.include(CVector((float)estimated.x, (float)estimated.y, (float)estimated.z)))
+00434                 return result;
+00435         
+00436         
+00437         // get the best matching instances
+00438         CAABBox bbpos;
+00439         bbpos.setCenter(estimated);
+00440         bbpos.setHalfSize(CVector(0.5f, 0.5f, 0.5f));
+00441         selectInstances(bbpos, _InternalCST);
+00442 
+00443         uint    i;
+00444         double  bestDist = threshold;
+00445         bool    snapinterior = false;
+00446 
+00447         // for each instance, try to retrieve the position
+00448         for (i=0; i<_InternalCST.CollisionInstances.size(); ++i)
+00449         {
+00450                 uint32  id = _InternalCST.CollisionInstances[i];
+00451                 // if the retrieved position is on a surface and it best match the estimated position
+00452                 // remember it
+00453                 const CRetrieverInstance                &instance = _Instances[id];
+00454                 CVector                                                 lestim = instance.getLocalPosition(estimated);
+00455                 CLocalRetriever::CLocalPosition ret = instance.retrievePosition(estimated, _RetrieverBank->getRetriever(_Instances[id].getRetrieverId()), _InternalCST);
+00456                 // go preferably on interior instances
+00457                 double                                                  d = fabs(lestim.z-ret.Estimation.z);
+00458                 bool                                                    itype = (instance.getType() == CLocalRetriever::Interior);
+00459                 if (ret.Surface != -1 && (snapinterior && itype && d < bestDist ||
+00460                                                                   !snapinterior && itype && d < 1.0f ||
+00461                                                                   !snapinterior && itype && d >= 1.0f && d < bestDist ||
+00462                                                                   !snapinterior && !itype && d < bestDist))
+00463                 {
+00464                         if (itype && d < 1.0)
+00465                                 snapinterior = true;
+00466                         bestDist = d;
+00467                         result.LocalPosition = ret;
+00468                         result.InstanceId = id;
+00469                 }
+00470 
+00471 //              double  d = fabs(lestim.z-ret.Estimation.z) * (_Instances[id].getType() == CLocalRetriever::Interior ? 0.5 : 1.0);
+00472 //              if (ret.Surface != -1 && d < bestDist)
+00473 //              {
+00474 //                      bestDist = d;
+00475 //                      result.LocalPosition = ret;
+00476 //                      result.InstanceId = _Instances[id].getInstanceId();
+00477 //              }
+00478         }
+00479 
+00480         return result;
+00481 }
+00482 */
+00483 
+00484 NLPACS::UGlobalPosition NLPACS::CGlobalRetriever::retrievePosition(const CVectorD &estimated, double threshold) const
+00485 {
+00486         // the retrieved position
+00487         CGlobalPosition                         result = CGlobalPosition(-1, CLocalRetriever::CLocalPosition(-1, estimated));
+00488 
+00489         if (!_BBox.include(CVector((float)estimated.x, (float)estimated.y, (float)estimated.z)))
+00490                 return result;
+00491         
+00492         
+00493         // get the best matching instances
+00494         CAABBox bbpos;
+00495         bbpos.setCenter(estimated);
+00496         bbpos.setHalfSize(CVector(0.5f, 0.5f, 0.5f));
+00497         selectInstances(bbpos, _InternalCST);
+00498 
+00499         uint    i;
+00500 
+00501         _InternalCST.SortedSurfaces.clear();
+00502 
+00503         // for each instance, try to retrieve the position
+00504         for (i=0; i<_InternalCST.CollisionInstances.size(); ++i)
+00505         {
+00506                 uint32                                                  id = _InternalCST.CollisionInstances[i];
+00507                 const CRetrieverInstance                &instance = _Instances[id];
+00508                 const CLocalRetriever                   &retriever = _RetrieverBank->getRetriever(instance.getRetrieverId());
+00509                 instance.retrievePosition(estimated, retriever, _InternalCST);
+00510         }
+00511 
+00512         if (!_InternalCST.SortedSurfaces.empty())
+00513         {
+00514                 // if there are some selected surfaces, sort them
+00515                 std::sort(_InternalCST.SortedSurfaces.begin(), _InternalCST.SortedSurfaces.end(), CCollisionSurfaceTemp::CDistanceSurface());
+00516 
+00517                 uint32                                                  id = _InternalCST.SortedSurfaces[0].Instance;
+00518                 const CRetrieverInstance                &instance = _Instances[id];
+00519                 const CLocalRetriever                   &retriever = _RetrieverBank->getRetriever(instance.getRetrieverId());
+00520 
+00521                 // get the UGlobalPosition of the estimation for this surface
+00522                 result.InstanceId = id;
+00523                 result.LocalPosition.Surface = _InternalCST.SortedSurfaces[0].Surface;
+00524                 result.LocalPosition.Estimation = instance.getLocalPosition(estimated);
+00525 
+00526                 CRetrieverInstance::snapVector(result.LocalPosition.Estimation);
+00527 
+00528                 // if there are more than 1 one possible (and best matching) surface, insure the position within the surface (by moving the point)
+00529 //              if (_InternalCST.SortedSurfaces.size() >= 2 && 
+00530 //                      _InternalCST.SortedSurfaces[1].Distance-_InternalCST.SortedSurfaces[0].Distance < InsureSurfaceThreshold)
+00531                 if (_InternalCST.SortedSurfaces[0].FoundCloseEdge)
+00532                 {
+00533                         bool    moved;
+00534                         uint    numMove = 0;
+00535                         do
+00536                         {
+00537                                 moved = retriever.insurePosition(result.LocalPosition);
+00538                                 ++numMove;
+00539 
+00540                                 if (moved)
+00541                                 {
+00542                                         nlinfo("PACS: insured position inside surface (%d,%d)", result.InstanceId, result.LocalPosition.Surface);
+00543                                 }
+00544                         }
+00545                         while (moved && numMove < 100);
+00546                         // the algo won't loop infinitely
+00547 
+00548                         if (moved)
+00549                         {
+00550                                 nlwarning ("PACS: couldn't insure position (%.f,%.f) within the surface (surf=%d,inst=%d) after 100 retries", result.LocalPosition.Estimation.x, result.LocalPosition.Estimation.y, result.LocalPosition.Surface, result.InstanceId);
+00551                         }
+00552                 }
+00553 
+00554                 // and after selecting the best surface (and some replacement) snap the point to the surface
+00555                 instance.snap(result.LocalPosition, retriever);
+00556         }
+00557         else
+00558         {
+00559 //              nlwarning("PACS: unable to retrieve correct position (%f,%f,%f)", estimated.x, estimated.y, estimated.z);
+00560 //              nlSleep(1);
+00561         }
+00562 
+00563         return result;
+00564 }
+00565 
+00566 NLPACS::UGlobalPosition NLPACS::CGlobalRetriever::retrievePosition(const CVector &estimated) const
+00567 {
+00568         return retrievePosition(estimated, 1.0e10f);
+00569 }
+00570 
+00571 NLPACS::UGlobalPosition NLPACS::CGlobalRetriever::retrievePosition(const CVectorD &estimated) const
+00572 {
+00573         return retrievePosition(estimated, 1.0e10);
+00574 }
+00575 
+00576 //
+00577 
+00578 sint32                  NLPACS::CGlobalRetriever::getIdentifier(const string &id) const
+00579 {
+00580         sint32  i;
+00581         for (i=0; i<(sint32)(_RetrieverBank->getRetrievers().size()); ++i)
+00582                 if (getRetriever(i).getIdentifier() == id)
+00583                         return i;
+00584 
+00585         return -1;
+00586 }
+00587 
+00588 const string    &NLPACS::CGlobalRetriever::getIdentifier(const NLPACS::UGlobalPosition &position) const
+00589 {
+00590         static const string             nullString = string("");
+00591 
+00592         if (position.InstanceId == -1)
+00593                 return nullString;
+00594 
+00595         return getRetriever(_Instances[position.InstanceId].getRetrieverId()).getIdentifier();
+00596 }
+00597 
+00598 //
+00599 
+00600 bool                    NLPACS::CGlobalRetriever::buildInstance(const string &id, const NLMISC::CVectorD &position, sint32 &instanceId)
+00601 {
+00602         NL_ALLOC_CONTEXT( Pacs )
+00603 
+00604         sint32  retrieverId = getIdentifier(id);
+00605 
+00606         instanceId = -1;
+00607 
+00608         // check retriever exists
+00609         if (retrieverId < 0)
+00610                 return false;
+00611 
+00612         const CRetrieverInstance        &instance = makeInstance(retrieverId, 0, CVector(position));
+00613 
+00614         // check make instance success
+00615         if (&instance == NULL || instance.getInstanceId() == -1 || instance.getRetrieverId() != retrieverId)
+00616                 return false;
+00617 
+00618         // links new instance to its neighbors
+00619         makeLinks(instance.getInstanceId());
+00620 
+00621         instanceId = instance.getInstanceId();
+00622 
+00623         return true;
+00624 }
+00625 
+00626 //
+00627 
+00628 void            NLPACS::CGlobalRetriever::removeInstance(sint32 instanceId)
+00629 {
+00630         if (instanceId < 0 || instanceId >= (sint32)_Instances.size() || _Instances[instanceId].getInstanceId() < 0)
+00631         {
+00632                 nlwarning("CGlobalRetriever::removeInstance(): Can't unlink instance %d, doesn't exist", instanceId);
+00633                 return;
+00634         }
+00635 
+00636         // get instance
+00637         CRetrieverInstance      &instance = _Instances[instanceId];
+00638 
+00639         // unlink it from others
+00640         instance.unlink(_Instances);
+00641 
+00642 
+00643 }
+00644 
+00645 //
+00646 
+00647 //
+00648 /*
+00649 void            NLPACS::CGlobalRetriever::snapToInteriorGround(UGlobalPosition &position) const
+00650 {
+00651         const CRetrieverInstance        &instance = _Instances[position.InstanceId];
+00652         if (instance.getType() != CLocalRetriever::Interior)
+00653                 return;
+00654 
+00655         const CLocalRetriever           &retriever = getRetriever(instance.getRetrieverId());
+00656         instance.snapToInteriorGround(position.LocalPosition, retriever);
+00657 }
+00658 */
+00659 
+00660 //
+00661 CVector         NLPACS::CGlobalRetriever::getGlobalPosition(const UGlobalPosition &global) const
+00662 {
+00663         if (global.InstanceId >= 0)
+00664         {
+00665                 return _Instances[global.InstanceId].getGlobalPosition(global.LocalPosition.Estimation);
+00666         }
+00667         else
+00668         {
+00669                 // it should be an error here
+00670                 return global.LocalPosition.Estimation;
+00671         }
+00672 }
+00673 
+00674 CVectorD        NLPACS::CGlobalRetriever::getDoubleGlobalPosition(const NLPACS::UGlobalPosition &global) const
+00675 {
+00676         if (global.InstanceId >= 0)
+00677         {
+00678                 return _Instances[global.InstanceId].getDoubleGlobalPosition(global.LocalPosition.Estimation);
+00679         }
+00680         else
+00681         {
+00682                 // it should be an error here
+00683                 return CVectorD(global.LocalPosition.Estimation);
+00684         }
+00685 }
+00686 
+00687 //
+00688 
+00689 void            NLPACS::CGlobalRetriever::findAStarPath(const NLPACS::UGlobalPosition &begin,
+00690                                                                                                         const NLPACS::UGlobalPosition &end,
+00691                                                                                                         vector<NLPACS::CRetrieverInstance::CAStarNodeAccess> &path,
+00692                                                                                                         uint32 forbidFlags) const
+00693 {
+00694         TTicks  astarStart;
+00695         ThisAStarTicks = 0;
+00696         astarStart = CTime::getPerformanceTime();
+00697 
+00698         // open and close lists
+00699         // TODO: Use a smart allocator to avoid huge alloc/free and memory fragmentation
+00700         // open is a priority queue (implemented as a stl multimap)
+00701         multimap<float, CRetrieverInstance::CAStarNodeAccess>   open;
+00702         // close is a simple stl vector
+00703         vector<CRetrieverInstance::CAStarNodeAccess>                    close;
+00704 
+00705         // inits start node and info
+00706         CRetrieverInstance::CAStarNodeAccess                                    beginNode;
+00707         beginNode.InstanceId = begin.InstanceId;
+00708         beginNode.NodeId = (uint16)begin.LocalPosition.Surface;
+00709         CRetrieverInstance::CAStarNodeInfo                                              &beginInfo = getNode(beginNode);
+00710 
+00711         // inits end node and info.
+00712         CRetrieverInstance::CAStarNodeAccess                                    endNode;
+00713         endNode.InstanceId = end.InstanceId;
+00714         endNode.NodeId = (uint16)end.LocalPosition.Surface;
+00715         CRetrieverInstance::CAStarNodeInfo                                              &endInfo = getNode(endNode);
+00716 
+00717         // set up first node...
+00718         CRetrieverInstance::CAStarNodeAccess                                    node = beginNode;
+00719         beginInfo.Parent.InstanceId = -1;
+00720         beginInfo.Parent.NodeId = 0;
+00721         beginInfo.Parent.ThroughChain = 0;
+00722         beginInfo.Cost = 0;
+00723         beginInfo.F = (endInfo.Position-beginInfo.Position).norm();
+00724 
+00725         // ... and inserts it in the open list.
+00726         open.insert(make_pair(beginInfo.F, node));
+00727 
+00728         // TO DO: use a CVector2f instead
+00729         CVector2f                                                                                               endPosition = CVector2f(getGlobalPosition(end));
+00730 
+00731         uint    i;
+00732 
+00733         path.clear();
+00734 
+00735         while (true)
+00736         {
+00737                 if (open.empty())
+00738                 {
+00739                         // couldn't find a path
+00740                         return;
+00741                 }
+00742 
+00743                 multimap<float, CRetrieverInstance::CAStarNodeAccess>::iterator it;
+00744 
+00745                 it = open.begin();
+00746                 node = it->second;
+00747                 open.erase(it);
+00748 
+00749                 if (node == endNode)
+00750                 {
+00751                         // found a path
+00752                         CRetrieverInstance::CAStarNodeAccess                    pathNode = node;
+00753                         uint                                                                                    numNodes = 0;
+00754                         while (pathNode.InstanceId != -1)
+00755                         {
+00756                                 ++numNodes;
+00757                                 CRetrieverInstance                                                      &instance = _Instances[pathNode.InstanceId];
+00758                                 CRetrieverInstance::CAStarNodeInfo                      &pathInfo = instance._NodesInformation[pathNode.NodeId];
+00759                                 pathNode = pathInfo.Parent;
+00760                         }
+00761 
+00762                         path.resize(numNodes);
+00763                         pathNode = node;
+00764                         while (pathNode.InstanceId != -1)
+00765                         {
+00766                                 path[--numNodes] = pathNode;
+00767                                 CRetrieverInstance                                                      &instance = _Instances[pathNode.InstanceId];
+00768                                 CRetrieverInstance::CAStarNodeInfo                      &pathInfo = instance._NodesInformation[pathNode.NodeId];
+00769                                 pathNode = pathInfo.Parent;
+00770                         }
+00771 
+00772                         ThisAStarTicks += (CTime::getPerformanceTime()-astarStart);
+00773 
+00774                         nlinfo("found a path");
+00775                         for (i=0; i<path.size(); ++i)
+00776                         {
+00777                                 CRetrieverInstance                                                      &instance = _Instances[path[i].InstanceId];
+00778                                 const CLocalRetriever                                           &retriever = _RetrieverBank->getRetriever(instance.getRetrieverId());
+00779                                 nlinfo("pathNode %d = (Inst=%d, Node=%d, Through=%d)", i, path[i].InstanceId, path[i].NodeId, path[i].ThroughChain);
+00780                                 if (path[i].ThroughChain != 0xffff)
+00781                                 {
+00782                                         const CChain                                                            &chain = retriever.getChain(path[i].ThroughChain);
+00783                                         nlinfo("   chain: left=%d right=%d", chain.getLeft(), chain.getRight());
+00784                                         if (CChain::isBorderChainId(chain.getRight()))
+00785                                         {
+00786                                                 CRetrieverInstance::CLink       lnk = instance.getBorderChainLink(CChain::convertBorderChainId(chain.getRight()));
+00787                                                 sint    instanceid = lnk.Instance;
+00788                                                 sint    id = lnk.SurfaceId;
+00789                                                 nlinfo("      right: instance=%d surf=%d", instanceid, id);
+00790                                         }
+00791                                 }
+00792                         }
+00793                         nlinfo("open.size()=%d", open.size());
+00794                         nlinfo("close.size()=%d", close.size());
+00795 
+00796                         return;
+00797                 }
+00798 
+00799                 // push successors of the current node
+00800                 CRetrieverInstance                                                              &inst = _Instances[node.InstanceId];
+00801                 const CLocalRetriever                                                   &retriever = _RetrieverBank->getRetriever(inst.getRetrieverId());
+00802                 const CRetrievableSurface                                               &surf = retriever.getSurface(node.NodeId);
+00803                 const vector<CRetrievableSurface::CSurfaceLink> &chains = surf.getChains();
+00804 
+00805                 CRetrieverInstance                                                              *nextInstance;
+00806                 const CLocalRetriever                                                   *nextRetriever;
+00807                 const CRetrievableSurface                                               *nextSurface;
+00808 
+00809                 nlinfo("examine node (instance=%d,surf=%d,cost=%g)", node.InstanceId, node.NodeId, inst._NodesInformation[node.NodeId].Cost);
+00810 
+00811                 for (i=0; i<chains.size(); ++i)
+00812                 {
+00813                         sint32  nextNodeId = chains[i].Surface;
+00814                         CRetrieverInstance::CAStarNodeAccess            nextNode;
+00815 
+00816                         if (CChain::isBorderChainId(nextNodeId))
+00817                         {
+00818                                 // if the chain points to another retriever
+00819 
+00820                                 // first get the edge on the retriever
+00821                                 CRetrieverInstance::CLink       lnk = inst.getBorderChainLink(CChain::convertBorderChainId(nextNodeId));
+00822                                 nextNode.InstanceId = lnk.Instance;
+00823 
+00824                                 if (nextNode.InstanceId < 0)
+00825                                         continue;
+00826 
+00827                                 nextInstance = &_Instances[nextNode.InstanceId];
+00828                                 nextRetriever = &(_RetrieverBank->getRetriever(nextInstance->getRetrieverId()));
+00829 
+00830                                 sint    nodeId = lnk.SurfaceId;
+00831                                 nlassert(nodeId >= 0);
+00832                                 nextNode.NodeId = (uint16)nodeId;
+00833                         }
+00834                         else if (nextNodeId >= 0)
+00835                         {
+00836                                 // if the chain points to the same instance
+00837                                 nextNode.InstanceId = node.InstanceId;
+00838                                 nextNode.NodeId = (uint16) nextNodeId;
+00839                                 nextInstance = &inst;
+00840                                 nextRetriever = &retriever;
+00841                         }
+00842                         else
+00843                         {
+00844                                 // if the chain cannot be crossed
+00845                                 continue;
+00846                         }
+00847 
+00848                         nextSurface = &(nextRetriever->getSurface(nextNode.NodeId));
+00849 
+00850                         if (nextSurface->getFlags() & forbidFlags)
+00851                                 continue;
+00852 
+00853                         // compute new node value (heuristic and cost)
+00854 
+00855                         CRetrieverInstance::CAStarNodeInfo      &nextInfo = nextInstance->_NodesInformation[nextNode.NodeId];
+00856                         float   stepCost = (nextInfo.Position-inst._NodesInformation[node.NodeId].Position).norm();
+00857                         float   nextCost = inst._NodesInformation[node.NodeId].Cost+stepCost;
+00858                         float   nextHeuristic = (nextInfo.Position-endPosition).norm();
+00859                         float   nextF = nextCost+nextHeuristic;
+00860 
+00861                         vector<CRetrieverInstance::CAStarNodeAccess>::iterator                  closeIt;
+00862                         for (closeIt=close.begin(); closeIt!=close.end() && *closeIt!=nextNode; ++closeIt)
+00863                                 ;
+00864 
+00865                         if (closeIt != close.end() && nextInfo.F < nextF)
+00866                                 continue;
+00867                         
+00868                         multimap<float, CRetrieverInstance::CAStarNodeAccess>::iterator openIt;
+00869                         for (openIt=open.begin(); openIt!=open.end() && openIt->second!=nextNode; ++openIt)
+00870                                 ;
+00871 
+00872                         if (openIt != open.end() && nextInfo.F < nextF)
+00873                                 continue;
+00874 
+00875                         if (openIt != open.end())
+00876                                 open.erase(openIt);
+00877 
+00878                         if (closeIt != close.end())
+00879                                 close.erase(closeIt);
+00880 
+00881                         nextInfo.Parent = node;
+00882                         nextInfo.Parent.ThroughChain = (uint16)(chains[i].Chain);
+00883                         nextInfo.Cost = nextCost;
+00884                         nextInfo.F = nextF;
+00885 
+00886                         nlinfo("  adding node (instance=%d,surf=%d) f=%g, through=%d", nextNode.InstanceId, nextNode.NodeId, nextInfo.F, i);
+00887 
+00888                         open.insert(make_pair(nextInfo.F, nextNode));
+00889                 }
+00890                 close.push_back(node);
+00891         }
+00892 }
+00893 
+00894 
+00895 
+00896 void    NLPACS::CGlobalRetriever::findPath(const NLPACS::UGlobalPosition &begin, 
+00897                                                                                    const NLPACS::UGlobalPosition &end, 
+00898                                                                                    NLPACS::CGlobalRetriever::CGlobalPath &path,
+00899                                                                                    uint32 forbidFlags) const
+00900 {
+00901 
+00902         vector<CRetrieverInstance::CAStarNodeAccess>    astarPath;
+00903         findAStarPath(begin, end, astarPath, forbidFlags);
+00904 
+00905         TTicks  surfStart;
+00906         TTicks  chainStart;
+00907 
+00908         ThisChainTicks = 0;
+00909         ThisSurfTicks = 0;
+00910         ThisPathTicks = 0;
+00911 
+00912         path.clear();
+00913         path.resize(astarPath.size());
+00914 
+00915         uint    i, j;
+00916         for (i=0; i<astarPath.size(); ++i)
+00917         {
+00918                 chainStart = CTime::getPerformanceTime();
+00919                 CLocalPath              &surf = path[i];
+00920                 surf.InstanceId = astarPath[i].InstanceId;
+00921                 const CLocalRetriever   &retriever = _RetrieverBank->getRetriever(_Instances[surf.InstanceId].getRetrieverId());
+00922 
+00923                 // computes start point
+00924                 if (i == 0)
+00925                 {
+00926                         // if it is the first point, just copy the begin
+00927                         surf.Start.ULocalPosition::operator= (begin.LocalPosition);
+00928                 }
+00929                 else
+00930                 {
+00931                         // else, take the previous value and convert it in the current instance axis
+00932                         // TODO: avoid this if the instances are the same
+00933                         CVector prev = _Instances[path[i-1].InstanceId].getGlobalPosition(path[i-1].End.Estimation);
+00934                         CVector current = _Instances[surf.InstanceId].getLocalPosition(prev);
+00935                         surf.Start.Surface = astarPath[i].NodeId;
+00936                         surf.Start.Estimation = current;
+00937                 }
+00938 
+00939                 // computes end point
+00940                 if (i == astarPath.size()-1)
+00941                 {
+00942                         surf.End.ULocalPosition::operator= (end.LocalPosition);
+00943                 }
+00944                 else
+00945                 {
+00946                         // get to the middle of the chain
+00947                         // first get the chain between the 2 surfaces
+00948                         const CChain                    &chain = retriever.getChain(astarPath[i].ThroughChain);
+00949                         float                                   cumulLength = 0.0f, midLength=chain.getLength()*0.5f;
+00950                         for (j=0; j<chain.getSubChains().size() && cumulLength<=midLength; ++j)
+00951                                 cumulLength += retriever.getOrderedChain(chain.getSubChain(j)).getLength();
+00952                         --j;
+00953                         const COrderedChain             &ochain = retriever.getOrderedChain(chain.getSubChain(j));
+00954                         surf.End.Surface = astarPath[i].NodeId;
+00955                         {
+00956                                 if (ochain.getVertices().size() & 1)
+00957                                 {
+00958                                         surf.End.Estimation = ochain[ochain.getVertices().size()/2].unpack3f();
+00959                                 }
+00960                                 else
+00961                                 {
+00962                                         surf.End.Estimation = (ochain[ochain.getVertices().size()/2].unpack3f()+
+00963                                                                                   ochain[ochain.getVertices().size()/2-1].unpack3f())*0.5f;
+00964                                 }
+00965                         }
+00966                 }
+00967                 ThisChainTicks += (CTime::getPerformanceTime()-chainStart);
+00968 
+00969                 surfStart = CTime::getPerformanceTime();
+00970                 retriever.findPath(surf.Start, surf.End, surf.Path, _InternalCST);
+00971                 ThisSurfTicks += (CTime::getPerformanceTime()-surfStart);
+00972         }
+00973 
+00974         ThisPathTicks = ThisAStarTicks+ThisChainTicks+ThisSurfTicks;
+00975         PathTicks += ThisPathTicks;
+00976         SurfTicks += ThisSurfTicks;
+00977         AStarTicks += ThisAStarTicks;
+00978         ChainTicks += ThisChainTicks;
+00979 }
+00980 
+00981 
+00982 // ***************************************************************************
+00983 
+00984 // ***************************************************************************
+00985 // ***************************************************************************
+00986 // Collisions part.
+00987 // ***************************************************************************
+00988 // ***************************************************************************
+00989 
+00990 
+00991 
+00992 // ***************************************************************************
+00993 const NLPACS::CRetrievableSurface       *NLPACS::CGlobalRetriever::getSurfaceById(const NLPACS::CSurfaceIdent &surfId) const
+00994 {
+00995         if(surfId.RetrieverInstanceId>=0 && surfId.SurfaceId>=0)
+00996         {
+00997                 sint32  locRetId= this->getInstance(surfId.RetrieverInstanceId).getRetrieverId();
+00998                 const CRetrievableSurface       &surf= _RetrieverBank->getRetriever(locRetId).getSurface(surfId.SurfaceId);
+00999                 return &surf;
+01000         }
+01001         else
+01002                 return NULL;
+01003 }
+01004 
+01005 
+01006 
+01007 // ***************************************************************************
+01008 void    NLPACS::CGlobalRetriever::findCollisionChains(CCollisionSurfaceTemp &cst, const NLMISC::CAABBox &bboxMove, const NLMISC::CVector &origin) const
+01009 {
+01010 //      H_AUTO(PACS_GR_findCollisionChains);
+01011 
+01012         sint    i,j;
+01013 
+01014         // 0. reset.
+01015         //===========
+01016         // reset possible chains.
+01017 //      H_BEFORE(PACS_GR_findCC_reset);
+01018         cst.CollisionChains.clear();
+01019         cst.resetEdgeCollideNodes();
+01020 //      H_AFTER(PACS_GR_findCC_reset);
+01021 
+01022         // 1. Find Instances which may hit this movement.
+01023         //===========
+01024 //      H_BEFORE(PACS_GR_findCC_selectInstances);
+01025         CAABBox         bboxMoveGlobal= bboxMove;
+01026         bboxMoveGlobal.setCenter(bboxMoveGlobal.getCenter()+origin);
+01027         selectInstances(bboxMoveGlobal, cst);
+01028 //      H_AFTER(PACS_GR_findCC_selectInstances);
+01029         // \todo yoyo: TODO_INTERIOR: add interiors meshes (static/dynamic houses etc...) to this list.
+01030         // -> done automatically with the select
+01031 
+01032 
+01033         // 2. Fill CollisionChains.
+01034         //===========
+01035         // For each possible surface mesh, test collision.
+01036         for(i=0 ; i<(sint)cst.CollisionInstances.size(); i++)
+01037         {
+01038 //              H_BEFORE(PACS_GR_findCC_getAndComputeMove);
+01039                 // get retrieverInstance.
+01040                 sint32  curInstance= cst.CollisionInstances[i];
+01041                 const CRetrieverInstance        &retrieverInstance= getInstance(curInstance);
+01042 
+01043                 // Retrieve the localRetriever of this instance.
+01044                 sint32  localRetrieverId= retrieverInstance.getRetrieverId();
+01045                 // If invalid one (hole), continue.
+01046                 if(localRetrieverId<0)
+01047                         continue;
+01048                 const CLocalRetriever           &localRetriever= _RetrieverBank->getRetriever(localRetrieverId);
+01049 
+01050                 // get delta between startPos.instance and curInstance.
+01051                 CVector         deltaOrigin;
+01052                 deltaOrigin= origin - retrieverInstance.getOrigin();
+01053 
+01054                 // compute movement relative to this localRetriever.
+01055                 CAABBox         bboxMoveLocal= bboxMove;
+01056                 bboxMoveLocal.setCenter(bboxMoveLocal.getCenter()+deltaOrigin);
+01057         
+01058                 // add possible collision chains with movement.
+01059                 //================
+01060                 sint            firstCollisionChain= cst.CollisionChains.size();
+01061                 CVector2f       transBase(-deltaOrigin.x, -deltaOrigin.y);
+01062 //              H_AFTER(PACS_GR_findCC_getAndComputeMove);
+01063 
+01064 //              H_BEFORE(PACS_GR_findCC_testCollision);
+01065                 // Go! fill collision chains that this movement intersect.
+01066                 localRetriever.testCollision(cst, bboxMoveLocal, transBase);
+01067                 // if an interior, also test for external collisions
+01069                 if (retrieverInstance.getType() == CLocalRetriever::Interior)
+01070                         retrieverInstance.testExteriorCollision(cst, bboxMoveLocal, transBase, localRetriever);
+01071 
+01072                 // how many collision chains added?  : nCollisionChain-firstCollisionChain.
+01073                 sint            nCollisionChain= cst.CollisionChains.size();
+01074 //              H_AFTER(PACS_GR_findCC_testCollision);
+01075 
+01076 
+01077                 // For all collision chains added, fill good SurfaceIdent info.
+01078                 //================
+01079 //              H_BEFORE(PACS_GR_findCC_fillSurfIdent);
+01080                 for(j=firstCollisionChain; j<nCollisionChain; j++)
+01081                 {
+01082                         CCollisionChain         &cc= cst.CollisionChains[j];
+01083 
+01084                         // info are already filled for exterior chains.
+01085                         if (cc.ExteriorEdge)
+01086                                 continue;
+01087 
+01088                         // LeftSurface retrieverInstance is always curInstance.
+01089                         cc.LeftSurface.RetrieverInstanceId= curInstance;
+01090 
+01091                         // If RightSurface is not an "edgeId" ie a pointer on a neighbor surface on an other retrieverInstance.
+01092                         const   CChain          &originalChain= localRetriever.getChain(cc.ChainId);
+01093                         if( !originalChain.isBorderChainId(cc.RightSurface.SurfaceId) )
+01094                         {
+01095                                 cc.RightSurface.RetrieverInstanceId= curInstance;
+01096                         }
+01097                         else
+01098                         {
+01099                                 // we must find the surfaceIdent of the neighbor.
+01100                                 // \todo yoyo: TODO_INTERIOR: this work only for zone. Must work too for houses.
+01101 
+01102                                 CRetrieverInstance::CLink       link;
+01103                                 // get the link to the next surface from the instance
+01104                                 link = retrieverInstance.getBorderChainLink(CChain::convertBorderChainId(cc.RightSurface.SurfaceId));
+01105 
+01106                                 // get the neighbor instanceId.
+01107                                 sint    neighborInstanceId= (sint16)link.Instance;
+01108                                 // store in the current collisionChain Right.
+01109                                 cc.RightSurface.RetrieverInstanceId= neighborInstanceId;
+01110 
+01111                                 // If no instance near us, this is a WALL.
+01112                                 if(neighborInstanceId<0)
+01113                                 {
+01114                                         // mark as a Wall.
+01115                                         cc.RightSurface.SurfaceId= -1;
+01116                                 }
+01117                                 else
+01118                                 {
+01119                                         // Get the good neighbor surfaceId.
+01120                                         cc.RightSurface.SurfaceId= (sint16)link.SurfaceId;
+01121                                 }
+01122                         }
+01123 
+01124                         nlassert(cc.LeftSurface.RetrieverInstanceId < (sint)_Instances.size());
+01125                         nlassert(cc.RightSurface.RetrieverInstanceId < (sint)_Instances.size());
+01126                 }
+01127 //              H_AFTER(PACS_GR_findCC_fillSurfIdent);
+01128 
+01129 
+01130                 // For all collision chains added, look if they are a copy of preceding collsion chain (same Left/Right). Then delete them.
+01131                 //================
+01132                 // \todo yoyo: TODO_OPTIMIZE: this is a N� complexity.
+01133 //              H_BEFORE(PACS_GR_findCC_removeDouble);
+01134                 for(j=firstCollisionChain; j<nCollisionChain; j++)
+01135                 {
+01136                         const CCollisionChain   &cj = cst.CollisionChains[j];
+01137 
+01138                         if (cj.ExteriorEdge && cj.LeftSurface.RetrieverInstanceId!=-1)
+01139                                 continue;
+01140 
+01141                         // test for all collisionChain inserted before.
+01142                         for(sint k=0; k<firstCollisionChain; k++)
+01143                         {
+01144                                 const CCollisionChain   &ck = cst.CollisionChains[k];
+01145 
+01146                                 if (cj.LeftSurface.RetrieverInstanceId != cj.RightSurface.RetrieverInstanceId &&
+01147                                         cj.LeftSurface == ck.RightSurface && cj.RightSurface == ck.LeftSurface)
+01148                                 {
+01149                                         const CRetrieverInstance        &instj = getInstance(cj.LeftSurface.RetrieverInstanceId),
+01150                                                                                                 &instk = getInstance(ck.LeftSurface.RetrieverInstanceId);
+01151                                         const CLocalRetriever           &retrj = getRetriever(instj.getRetrieverId()),
+01152                                                                                                 &retrk = getRetriever(instk.getRetrieverId());
+01153 
+01154                                         nlassert(retrj.getChain(cj.ChainId).isBorderChain() && retrk.getChain(ck.ChainId).isBorderChain());
+01155 
+01156                                         if (instj.getBorderChainLink(retrj.getChain(cj.ChainId).getBorderChainIndex()).ChainId != ck.ChainId ||
+01157                                                 instk.getBorderChainLink(retrk.getChain(ck.ChainId).getBorderChainIndex()).ChainId != cj.ChainId)
+01158                                         {
+01159                                                 continue;
+01160                                         }
+01161 
+01162                                         // remove this jth entry.
+01163                                         // by swapping with last entry. Only if not already last.
+01164                                         if(j<nCollisionChain-1)
+01165                                         {
+01166                                                 swap(cst.CollisionChains[j], cst.CollisionChains[nCollisionChain-1]);
+01167                                                 // NB: some holes remain in cst._EdgeCollideNodes, but do not matters since reseted at 
+01168                                                 // each collision test.
+01169                                         }
+01170 
+01171                                         // pop last entry.
+01172                                         nCollisionChain--;
+01173                                         cst.CollisionChains.resize(nCollisionChain);
+01174 
+01175                                         // next entry??
+01176                                         j--;
+01177                                         break;
+01178                                 }
+01179 /*
+01180                                 // if same surface Ident Left/Right==Left/Right or swapped Left/Right==Right/Left
+01181                                 if( cst.CollisionChains[j].sameSurfacesThan(cst.CollisionChains[k]) )
+01182                                 {
+01183                                         // remove this jth entry.
+01184                                         // by swapping with last entry. Only if not already last.
+01185                                         if(j<nCollisionChain-1)
+01186                                         {
+01187                                                 swap(cst.CollisionChains[j], cst.CollisionChains[nCollisionChain-1]);
+01188                                                 // NB: some holes remain in cst._EdgeCollideNodes, but do not matters since reseted at 
+01189                                                 // each collision test.
+01190                                         }
+01191 
+01192                                         // pop last entry.
+01193                                         nCollisionChain--;
+01194                                         cst.CollisionChains.resize(nCollisionChain);
+01195 
+01196                                         // next entry??
+01197                                         j--;
+01198                                         break;
+01199                                 }
+01200 */
+01201                         }
+01202 
+01203                 }
+01204 //              H_AFTER(PACS_GR_findCC_removeDouble);
+01205         }
+01206 
+01207 }
+01208 
+01209 
+01210 // ***************************************************************************
+01211 void    NLPACS::CGlobalRetriever::testCollisionWithCollisionChains(CCollisionSurfaceTemp &cst, const CVector2f &startCol, const CVector2f &deltaCol,
+01212                 CSurfaceIdent startSurface, float radius, const CVector2f bboxStart[4], TCollisionType colType) const
+01213 {
+01214 //      H_AUTO(PACS_GR_testCollisionWithCollisionChains);
+01215 
+01216         // start currentSurface with surface start.
+01217         CSurfaceIdent   currentSurface= startSurface;
+01218         uint                    nextCollisionSurfaceTested=0;
+01219         sint                    i;
+01220 
+01221         // reset result.
+01222         cst.CollisionDescs.clear();
+01223         // reset all collisionChain to not tested.
+01224         for(i=0; i<(sint)cst.CollisionChains.size(); i++)
+01225         {
+01226                 CCollisionChain         &colChain= cst.CollisionChains[i];
+01227                 colChain.Tested= false;
+01228         }
+01229 
+01230 
+01231         /*
+01232                 To manage recovery, we must use such an algorithm, so we are sure to trace the way across all surfaces really 
+01233                 collided, and discard any other (such as other floor or ceiling).
+01234         */
+01235         while(true)
+01236         {
+01237                 // run all collisionChain.
+01238                 //========================
+01239                 for(i=0; i<(sint)cst.CollisionChains.size(); i++)
+01240                 {
+01241                         CCollisionChain         &colChain= cst.CollisionChains[i];
+01242 
+01244                         nlassert(colChain.LeftSurface.RetrieverInstanceId < (sint)_Instances.size());
+01245                         nlassert(colChain.RightSurface.RetrieverInstanceId < (sint)_Instances.size());
+01246 
+01247                         // test only currentSurface/X. And don't test chains already tested before.
+01248                         if(colChain.hasSurface(currentSurface) && !colChain.Tested)
+01249                         {
+01250                                 // we are testing this chain.
+01251                                 colChain.Tested= true;
+01252 
+01253 
+01254                                 // test all edges of this chain, and get tmin
+01255                                 //========================
+01256                                 float           t=0.0, tMin=1;
+01257                                 CVector2f       normal, normalMin;
+01258                                 // run list of edge.
+01259                                 sint32          curEdge= colChain.FirstEdgeCollide;
+01260                                 while(curEdge!=(sint32)0xFFFFFFFF)
+01261                                 {
+01262                                         // get the edge.
+01263                                         CEdgeCollideNode        &colEdge= cst.getEdgeCollideNode(curEdge);
+01264 
+01265                                         // test collision with this edge.
+01266                                         if(colType==CGlobalRetriever::Circle)
+01267                                                 t= colEdge.testCircleMove(startCol, deltaCol, radius, normal);
+01268                                         else if(colType==CGlobalRetriever::BBox)
+01269                                                 t= colEdge.testBBoxMove(startCol, deltaCol, bboxStart, normal);
+01270 
+01271                                         // earlier collision??
+01272                                         if(t<tMin)
+01273                                         {
+01274                                                 tMin= t;
+01275                                                 normalMin= normal;
+01276                                         }
+01277 
+01278                                         // next edge.
+01279                                         curEdge= colEdge.Next;
+01280                                 }
+01281 
+01282 
+01283                                 // If collision with this chain, must insert it in the array of collision.
+01284                                 //========================
+01285                                 if(tMin<1)
+01286                                 {
+01287                                         CSurfaceIdent   collidedSurface= colChain.getOtherSurface(currentSurface);
+01288 
+01290                                         nlassert(collidedSurface.RetrieverInstanceId < (sint)_Instances.size());
+01291 
+01292                                         // insert or replace this collision in collisionDescs.
+01293                                         // NB: yes this looks like a N algorithm (so N�). But not so many collisions may arise, so don't bother.
+01294                                         sint    indexInsert= cst.CollisionDescs.size();
+01295                                         sint    colFound= -1;
+01296 
+01297                                         // start to search with nextCollisionSurfaceTested, because can't insert before.
+01298                                         for(sint j= nextCollisionSurfaceTested; j<(sint)cst.CollisionDescs.size(); j++)
+01299                                         {
+01300                                                 // we must keep time order.
+01301                                                 if(tMin < cst.CollisionDescs[j].ContactTime)
+01302                                                 {
+01303                                                         indexInsert= min(j, indexInsert);
+01304                                                 }
+01305                                                 // Does the collision with this surface already exist??
+01306                                                 if(cst.CollisionDescs[j].ContactSurface==collidedSurface)
+01307                                                 {
+01308                                                         colFound= j;
+01309                                                         // if we have found our old collision, stop, there is no need to search more.
+01310                                                         break;
+01311                                                 }
+01312                                         }
+01313 
+01314                                         // Insert only if the surface was not already collided, or that new collision arise before old.
+01315                                         if(colFound==-1 || indexInsert<=colFound)
+01316                                         {
+01317                                                 CCollisionSurfaceDesc   newCol;
+01318                                                 newCol.ContactSurface= collidedSurface;
+01319                                                 newCol.ContactTime= tMin;
+01320                                                 newCol.ContactNormal.set(normalMin.x, normalMin.y, 0);
+01321 
+01322                                                 // if, by chance, indexInsert==colFound, just replace old collision descriptor.
+01323                                                 if(colFound==indexInsert)
+01324                                                 {
+01325                                                         cst.CollisionDescs[indexInsert]= newCol;
+01326                                                 }
+01327                                                 else
+01328                                                 {
+01329                                                         // if any, erase old collision against this surface. NB: here, colFound>indexInsert.
+01330                                                         if(colFound!=-1)
+01331                                                                 cst.CollisionDescs.erase(cst.CollisionDescs.begin() + colFound);
+01332 
+01333                                                         // must insert the collision.
+01334                                                         cst.CollisionDescs.insert(cst.CollisionDescs.begin() + indexInsert, newCol);
+01335                                                 }
+01336                                         }
+01337                                 }
+01338                         }
+01339                 }
+01340 
+01341                 // Find next surface to test.
+01342                 //========================
+01343                 // No more?? so this is the end.
+01344                 if(nextCollisionSurfaceTested>=cst.CollisionDescs.size())
+01345                         break;
+01346                 // else next one.
+01347                 else
+01348                 {
+01349                         // NB: with this algorithm, we are sure that no more collisions will arise before currentCollisionSurfaceTested.
+01350                         // so just continue with following surface.
+01351                         currentSurface= cst.CollisionDescs[nextCollisionSurfaceTested].ContactSurface;
+01352 
+01353                         // Do we touch a wall??
+01354                         bool    isWall;
+01355                         if(currentSurface.SurfaceId<0)
+01356                                 isWall= true;
+01357                         else
+01358                         {
+01359                                 // test if it is a walkable wall.
+01360                                 sint32  locRetId= this->getInstance(currentSurface.RetrieverInstanceId).getRetrieverId();
+01361                                 const CRetrievableSurface       &surf= _RetrieverBank->getRetriever(locRetId).getSurface(currentSurface.SurfaceId);
+01362                                 isWall= !(surf.isFloor() || surf.isCeiling());
+01363                         }
+01364 
+01365                         // If we touch a wall, this is the end of search.
+01366                         if(isWall)
+01367                         {
+01368                                 // There can be no more collision after this one.
+01369                                 cst.CollisionDescs.resize(nextCollisionSurfaceTested+1);
+01370                                 break;
+01371                         }
+01372                         else
+01373                         {
+01374                                 // Next time, we will test the following (NB: the array may grow during next pass, or reorder, 
+01375                                 // but only after nextCollisionSurfaceTested).
+01376                                 nextCollisionSurfaceTested++;
+01377                         }
+01378                 }
+01379         }
+01380 
+01381 }
+01382 
+01383 
+01384 // ***************************************************************************
+01385 bool                    NLPACS::CGlobalRetriever::verticalChain(const CCollisionChain &colChain) const
+01386 {
+01387         // retrieve surfaces.
+01388         const CRetrievableSurface       *left= getSurfaceById(colChain.LeftSurface);
+01389         const CRetrievableSurface       *right= getSurfaceById(colChain.RightSurface);
+01390 
+01391         // test if left surface is a wall.
+01392         bool                                            leftWall;
+01393         if(!left)
+01394                 leftWall= true;
+01395         else
+01396                 leftWall= !(left->isFloor() || left->isCeiling());
+01397 
+01398         // test if right surface is a wall.
+01399         bool                                            rightWall;
+01400         if(!right)
+01401                 rightWall= true;
+01402         else
+01403                 rightWall= !(right->isFloor() || right->isCeiling());
+01404 
+01405         // true if both are a wall.
+01406         return leftWall && rightWall;
+01407 }
+01408 
+01409 
+01410 // ***************************************************************************
+01411 NLPACS::CSurfaceIdent   NLPACS::CGlobalRetriever::testMovementWithCollisionChains(CCollisionSurfaceTemp &cst, const CVector2f &startCol, const CVector2f &endCol,
+01412                 CSurfaceIdent startSurface) const
+01413 {
+01414 //      H_AUTO(PACS_GR_testMovementWithCollisionChains);
+01415 
+01416         // start currentSurface with surface start.
+01417         CSurfaceIdent   currentSurface= startSurface;
+01418         sint                    i;
+01419 
+01420         // reset result.
+01421         cst.MoveDescs.clear();
+01422 
+01423 
+01424         /*
+01425                 To manage recovery, we must use such an algorithm, so we are sure to trace the way across all surfaces really 
+01426                 collided, and discard any other (such as other floor or ceiling).
+01427 
+01428                 This function is quite different from testCollisionWithCollisionChains() because she must detect all collisions
+01429                 with all edges of any chains (and not the minimum collision with a chain).
+01430                 This is done in 3 parts:
+01431                         - detect collisions with all edges.
+01432                         - sort.
+01433                         - leave only real collisions.
+01434         */
+01435         // run all collisionChain.
+01436         //========================
+01437         for(i=0; i<(sint)cst.CollisionChains.size(); i++)
+01438         {
+01439                 CCollisionChain         &colChain= cst.CollisionChains[i];
+01440 
+01441 
+01442                 // test all edges of this chain, and insert if necessary.
+01443                 //========================
+01444                 CRational64             t;
+01445                 // run list of edge.
+01446                 sint32          curEdge= colChain.FirstEdgeCollide;
+01447                 while(curEdge!=(sint32)0xFFFFFFFF)
+01448                 {
+01449                         // get the edge.
+01450                         CEdgeCollideNode        &colEdge= cst.getEdgeCollideNode(curEdge);
+01451 
+01452                         // test collision with this edge.
+01453                         CEdgeCollide::TPointMoveProblem         pmpb;
+01454                         t= colEdge.testPointMove(startCol, endCol, pmpb);
+01455                         // manage multiple problems of precision.
+01456                         if(t== -1)
+01457                         {
+01458                                 static const string     errs[CEdgeCollide::PointMoveProblemCount]= {
+01459                                         "ParallelEdges", "StartOnEdge", "StopOnEdge", "TraverseEndPoint", "EdgeNull"};
+01460                                 // return a "Precision Problem" ident. movement is invalid. 
+01461                                 // BUT if startOnEdge, which should never arrive.
+01462                                 if(pmpb==CEdgeCollide::StartOnEdge)
+01463                                 {
+01464                                         nlinfo("COL: Precision Problem: %s", errs[pmpb].c_str());
+01465                                         return CSurfaceIdent(-1, -1);   // so in this case, block....
+01466                                 }
+01467                                 else if(pmpb==CEdgeCollide::EdgeNull)
+01468                                 {
+01469                                         /*
+01470                                         // verify if it is an edge which separate 2 walls. in this case, ignore it. else, error.
+01471                                         if(verticalChain(colChain))
+01472                                         {
+01473                                                 t=1;    // no collision with this edge.
+01474                                         }
+01475                                         else
+01476                                         {
+01477                                                 nlinfo("COL: Precision Problem: %s", errs[pmpb]);
+01478                                                 nlstop;         // this should not append.
+01479                                                 return CSurfaceIdent(-1, -1);
+01480                                         }*/
+01481                                         /* Actually, this is never a problem: we never get through this edge.
+01482                                                 Instead, we'll get through the neighbors edge.
+01483                                                 So just disable this edge.
+01484                                         */
+01485                                         t= 1;
+01486                                 }
+01487                                 else
+01488                                         return  CSurfaceIdent(-2, -2);
+01489                         }
+01490 
+01491                         // collision??
+01492                         if(t<1)
+01493                         {
+01494                                 // insert in list.
+01495                                 cst.MoveDescs.push_back(CMoveSurfaceDesc(t, colChain.LeftSurface, colChain.RightSurface));
+01496                         }
+01497 
+01498                         // next edge.
+01499                         curEdge= colEdge.Next;
+01500                 }
+01501         }
+01502 
+01503 
+01504         // sort.
+01505         //================
+01506         // sort the collisions in ascending time order.
+01507         sort(cst.MoveDescs.begin(), cst.MoveDescs.end());
+01508 
+01509 
+01510         // Traverse the array of collisions.
+01511         //========================
+01512         for(i=0;i<(sint)cst.MoveDescs.size();i++)
+01513         {
+01514                 // Do we collide with this chain??
+01515                 if(cst.MoveDescs[i].hasSurface(currentSurface))
+01516                 {
+01517                         currentSurface= cst.MoveDescs[i].getOtherSurface(currentSurface);
+01518 
+01519                         // Do we touch a wall?? should not happens, but important for security.
+01520                         bool    isWall;
+01521                         if(currentSurface.SurfaceId<0)
+01522                                 isWall= true;
+01523                         else
+01524                         {
+01525                                 // test if it is a walkable wall.
+01526                                 sint32  locRetId= this->getInstance(currentSurface.RetrieverInstanceId).getRetrieverId();
+01527                                 const CRetrievableSurface       &surf= _RetrieverBank->getRetriever(locRetId).getSurface(currentSurface.SurfaceId);
+01528                                 isWall= !(surf.isFloor() || surf.isCeiling());
+01529                         }
+01530 
+01531                         // If we touch a wall, this is the end of search.
+01532                         if(isWall)
+01533                         {
+01534                                 // return a Wall ident. movement is invalid.
+01535                                 return  CSurfaceIdent(-1, -1);
+01536                         }
+01537                 }
+01538         }
+01539 
+01540 
+01541         return currentSurface;
+01542 }
+01543 
+01544 
+01545 
+01546 // ***************************************************************************
+01547 const   NLPACS::TCollisionSurfaceDescVector     
+01548         *NLPACS::CGlobalRetriever::testCylinderMove(const UGlobalPosition &startPos, const NLMISC::CVector &delta, float radius, CCollisionSurfaceTemp &cst) const
+01549 {
+01550 //      H_AUTO(PACS_GR_testCylinderMove);
+01551 
+01552         CSurfaceIdent   startSurface(startPos.InstanceId, startPos.LocalPosition.Surface);
+01553 
+01554         // 0. reset.
+01555         //===========
+01556         // reset result.
+01557         cst.CollisionDescs.clear();
+01558 
+01559         // In a surface ?
+01560         if (startPos.InstanceId==-1)
+01561         {
+01562                 // Warning this primitive is not on a surface
+01563                 //nlassertonce (0);
+01564 
+01565                 // Return NULL when lost
+01566                 return NULL;
+01567         }
+01568         // store this request in cst.
+01569         cst.PrecStartSurface= startSurface;
+01570         cst.PrecStartPos= startPos.LocalPosition.Estimation;
+01571         cst.PrecDeltaPos= delta;
+01572         cst.PrecValid= true;
+01573 
+01574         // 0.bis
+01575         //===========
+01576         // Abort if deltamove is 0,0,0.
+01577         if (delta.isNull())
+01578                 return &cst.CollisionDescs;
+01579 
+01580         // 1. Choose a local basis.
+01581         //===========
+01582         // Take the retrieverInstance of startPos as a local basis.
+01583         CVector         origin;
+01584         origin= getInstance(startPos.InstanceId).getOrigin();
+01585 
+01586 
+01587         // 2. compute bboxmove.
+01588         //===========
+01589         CAABBox         bboxMove;
+01590         // bounds the movement in a bbox.
+01591         // compute start and end, relative to the retriever instance.
+01592         CVector         start= startPos.LocalPosition.Estimation;
+01593         CVector         end= start+delta;
+01594         // extend the bbox.
+01595         bboxMove.setCenter(start-CVector(radius, radius, 0));
+01596         bboxMove.extend(start+CVector(radius, radius, 0));
+01597         bboxMove.extend(end-CVector(radius, radius, 0));
+01598         bboxMove.extend(end+CVector(radius, radius, 0));
+01599 
+01600 
+01601         // 3. find possible collisions in bboxMove+origin. fill cst.CollisionChains.
+01602         //===========
+01603         findCollisionChains(cst, bboxMove, origin);
+01604 
+01605 
+01606 
+01607         // 4. test collisions with CollisionChains.
+01608         //===========
+01609         CVector2f       startCol(start.x, start.y);
+01610         CVector2f       deltaCol(delta.x, delta.y);
+01611         CVector2f       obbDummy[4];    // dummy OBB (not obb here so don't bother)
+01612         testCollisionWithCollisionChains(cst, startCol, deltaCol, startSurface, radius, obbDummy, CGlobalRetriever::Circle);
+01613 
+01614         // result.
+01615         return &cst.CollisionDescs;
+01616 }
+01617 
+01618 
+01619 // ***************************************************************************
+01620 const   NLPACS::TCollisionSurfaceDescVector     
+01621         *NLPACS::CGlobalRetriever::testBBoxMove(const UGlobalPosition &startPos, const NLMISC::CVector &delta, 
+01622         const NLMISC::CVector &locI, const NLMISC::CVector &locJ, CCollisionSurfaceTemp &cst) const
+01623 {
+01624 //      H_AUTO(PACS_GR_testBBoxMove);
+01625 
+01626         CSurfaceIdent   startSurface(startPos.InstanceId, startPos.LocalPosition.Surface);
+01627 
+01628         // 0. reset.
+01629         //===========
+01630         // reset result.
+01631         cst.CollisionDescs.clear();
+01632 
+01633         // In a surface ?
+01634         if (startPos.InstanceId==-1)
+01635         {
+01636                 // Warning this primitive is not on a surface
+01637                 //nlassertonce (0);
+01638 
+01639                 // Return NULL when lost
+01640                 return NULL;
+01641         }
+01642         
+01643         // store this request in cst.
+01644         cst.PrecStartSurface= startSurface;
+01645         cst.PrecStartPos= startPos.LocalPosition.Estimation;
+01646         cst.PrecDeltaPos= delta;
+01647         cst.PrecValid= true;
+01648 
+01649         // 0.bis
+01650         //===========
+01651         // Abort if deltamove is 0,0,0.
+01652         if (delta.isNull())
+01653                 return &cst.CollisionDescs;
+01654 
+01655         // 1. Choose a local basis.
+01656         //===========
+01657         // Take the retrieverInstance of startPos as a local basis.
+01658         CVector         origin;
+01659         origin= getInstance(startPos.InstanceId).getOrigin();
+01660 
+01661 
+01662         // 2. compute OBB.
+01663         //===========
+01664         CVector2f       obbStart[4];
+01665         // compute start, relative to the retriever instance.
+01666         CVector         start= startPos.LocalPosition.Estimation;
+01667         CVector2f       obbCenter(start.x, start.y);
+01668         CVector2f       locI2d(locI.x, locI.y);
+01669         CVector2f       locJ2d(locJ.x, locJ.y);
+01670 
+01671         // build points in CCW.
+01672         obbStart[0]= obbCenter - locI2d - locJ2d;
+01673         obbStart[1]= obbCenter + locI2d - locJ2d;
+01674         obbStart[2]= obbCenter + locI2d + locJ2d;
+01675         obbStart[3]= obbCenter - locI2d + locJ2d;
+01676 
+01677         // 3. compute bboxmove.
+01678         //===========
+01679         CAABBox         bboxMove;
+01680         // extend the bbox.
+01681         bboxMove.setCenter(CVector(obbStart[0].x, obbStart[0].y, 0));
+01682         bboxMove.extend(CVector(obbStart[1].x, obbStart[1].y, 0));
+01683         bboxMove.extend(CVector(obbStart[2].x, obbStart[2].y, 0));
+01684         bboxMove.extend(CVector(obbStart[3].x, obbStart[3].y, 0));
+01685         bboxMove.extend(CVector(obbStart[0].x, obbStart[0].y, 0) + delta);
+01686         bboxMove.extend(CVector(obbStart[1].x, obbStart[1].y, 0) + delta);
+01687         bboxMove.extend(CVector(obbStart[2].x, obbStart[2].y, 0) + delta);
+01688         bboxMove.extend(CVector(obbStart[3].x, obbStart[3].y, 0) + delta);
+01689 
+01690 
+01691 
+01692         // 4. find possible collisions in bboxMove+origin. fill cst.CollisionChains.
+01693         //===========
+01694         findCollisionChains(cst, bboxMove, origin);
+01695 
+01696 
+01697 
+01698         // 5. test collisions with CollisionChains.
+01699         //===========
+01700         CVector2f       startCol(start.x, start.y);
+01701         CVector2f       deltaCol(delta.x, delta.y);
+01702         testCollisionWithCollisionChains(cst, startCol, deltaCol, startSurface, 0, obbStart, CGlobalRetriever::BBox);
+01703 
+01704         // result.
+01705         return &cst.CollisionDescs;
+01706 }
+01707 
+01708 
+01709 
+01710 // ***************************************************************************
+01711 NLPACS::UGlobalPosition         
+01712         NLPACS::CGlobalRetriever::doMove(const NLPACS::UGlobalPosition &startPos, const NLMISC::CVector &delta, float t, NLPACS::CCollisionSurfaceTemp &cst, bool rebuildChains) const
+01713 {
+01714 //      H_AUTO(PACS_GR_doMove);
+01715 
+01716         CSurfaceIdent   startSurface(startPos.InstanceId, startPos.LocalPosition.Surface);
+01717 
+01718         // clamp factor.
+01719         clamp(t, 0.0f, 1.0f);
+01720 
+01721         // 0. reset.
+01722         //===========
+01723         // reset CollisionDescs.
+01724         cst.CollisionDescs.clear();
+01725 
+01726         // In a surface ?
+01727         if (startPos.InstanceId==-1)
+01728         {
+01729                 // Warining: this primitive is not on a surface
+01730                 //nlassertonce (0);
+01731 
+01732                 // Return startpos
+01733                 return startPos;
+01734         }
+01735         
+01736         if(!rebuildChains)
+01737         {
+01738                 // same move request than prec testMove() ??.
+01739                 if( cst.PrecStartSurface != startSurface || 
+01740                         cst.PrecStartPos!=startPos.LocalPosition.Estimation || 
+01741                         cst.PrecDeltaPos!=delta ||
+01742                         !cst.PrecValid)
+01743                 {
+01744                         // if not, just return start.
+01745                         nlstop;
+01746                         return startPos;
+01747                 }
+01748                 // Since we are sure we have same movement than prec testMove(), no need to rebuild cst.CollisionChains.
+01749         }
+01750         else
+01751         {
+01752                 // we don't have same movement than prec testMove(), we must rebuild cst.CollisionChains.
+01753                 // Prec settings no more valids.
+01754                 cst.PrecValid= false;
+01755         }
+01756 
+01757 
+01758 
+01759 
+01760         // 1. Choose a local basis (same than in testMove()).
+01761         //===========
+01762         // Take the retrieverInstance of startPos as a local basis.
+01763         CVector         origin;
+01764         origin= getInstance(startPos.InstanceId).getOrigin();
+01765 
+01766 
+01767         // 2. test collisions with CollisionChains.
+01768         //===========
+01769         CVector         start= startPos.LocalPosition.Estimation;
+01770         // compute end with real delta position.
+01771         CVector         end= start + delta*t;
+01772 
+01773         // If asked, we must rebuild array of collision chains.
+01774         if(rebuildChains)
+01775         {
+01776 //              H_AUTO(PACS_GR_doMove_rebuildChains);
+01777 
+01778                 // compute bboxmove.
+01779                 CAABBox         bboxMove;
+01780                 // must add some extent, to be sure to include snapped CLocalRetriever vertex (2.0f/256 should be sufficient).
+01781                 // Nb: this include the precision problem just below (move a little).
+01782                 float   radius= 4.0f/Vector2sAccuracy;
+01783                 bboxMove.setCenter(start-CVector(radius, radius, 0));
+01784                 bboxMove.extend(start+CVector(radius, radius, 0));
+01785                 bboxMove.extend(end-CVector(radius, radius, 0));
+01786                 bboxMove.extend(end+CVector(radius, radius, 0));
+01787 
+01788                 // find possible collisions in bboxMove+origin. fill cst.CollisionChains.
+01789                 findCollisionChains(cst, bboxMove, origin);
+01790         }
+01791 
+01792 
+01793         // look where we arrive.
+01794         CSurfaceIdent   endSurface;
+01795         CVector                 endRequest= end;
+01796         const sint              maxPbPrec= 32;  // move away from 4 mm at max, in each 8 direction.
+01797         sint                    pbPrecNum= 0;
+01798 
+01799         // must snap the end position.
+01800         CRetrieverInstance::snapVector(endRequest);
+01801         end= endRequest;
+01802 
+01803         // verify start is already snapped
+01804         {
+01805                 CVector         startTest= start;
+01806                 CRetrieverInstance::snapVector(startTest);
+01807                 nlassert( start == startTest );
+01808         }
+01809 
+01810 
+01811         // Normally, just one iteration is made in this loop (but if precision problem (stopOnEdge, startOnEdge....).
+01812         while(true)
+01813         {
+01814                 // must snap the end position.
+01815                 CRetrieverInstance::snapVector(end);
+01816 
+01817                 CVector2f       startCol(start.x, start.y);
+01818                 CVector2f       endCol(end.x, end.y);
+01819 
+01820                 // If same 2D position, just return startPos (suppose no movement)
+01821                 if(endCol==startCol)
+01822                 {
+01823                         UGlobalPosition         res;
+01824                         res= startPos;
+01825                         // keep good z movement.
+01826                         res.LocalPosition.Estimation.z= end.z;
+01827                         return res;
+01828                 }
+01829 
+01830                 // search destination problem.
+01831                 endSurface= testMovementWithCollisionChains(cst, startCol, endCol, startSurface);
+01832 
+01833                 // if no precision problem, Ok, we have found our destination surface (or anormal collide against a wall).
+01834                 if(endSurface.SurfaceId!=-2)
+01835                         break;
+01836                 /* else we are in deep chit, for one on those reason:
+01837                         - traverse on point.
+01838                         - stop on a edge (dist==0).
+01839                         - start on a edge (dist==0).
+01840                         - run // on a edge (NB: dist==0 too).
+01841                 */
+01842                 else
+01843                 {
+01844                         // For simplicty, just try to move a little the end position
+01845                         if(pbPrecNum<maxPbPrec)
+01846                         {
+01847                                 static struct   {sint x,y;}   dirs[8]= { {1,0}, {1,1}, {0,1}, {-1,1}, {-1,0}, {-1,-1}, {0,-1}, {1,-1}};
+01848                                 sint    dir= pbPrecNum%8;
+01849                                 sint    dist= pbPrecNum/8+1;
+01850                                 CVector         dta;
+01851 
+01852                                 // compute small move.
+01853                                 dta.x= dirs[dir].x * dist * 1.0f/SnapPrecision;
+01854                                 dta.y= dirs[dir].y * dist * 1.0f/SnapPrecision;
+01855                                 dta.z= 0;
+01856 
+01857                                 // add it to the original end pos requested.
+01858                                 end= endRequest + dta;
+01859 
+01860                                 pbPrecNum++;
+01861                         }
+01862                         else
+01863                         {
+01864                                 // do not move at all.
+01865                                 endSurface= CSurfaceIdent(-1,-1);
+01866                                 break;
+01867                         }
+01868                 }
+01869         }
+01870 
+01871         // 3. return result.
+01872         //===========
+01873         // Problem?? do not move.
+01874         if(endSurface.SurfaceId==-1)
+01875                 return startPos;
+01876         else
+01877         {
+01878                 // else must return good GlobalPosition.
+01879                 CGlobalPosition         res;
+01880 
+01881                 res.InstanceId= endSurface.RetrieverInstanceId;
+01882                 res.LocalPosition.Surface= endSurface.SurfaceId;
+01883 
+01884                 // compute newPos, localy to the endSurface.
+01885                 // get delta between startPos.instance and curInstance.
+01886                 // NB: for float precision, it is important to compute deltaOrigin, and after compute newPos in local.
+01887                 CVector         deltaOrigin;
+01888                 deltaOrigin= origin - getInstance(res.InstanceId).getOrigin();
+01889 
+01890                 // Because Origin precision is 1 meter, and end precision is 1/1024 meter, we have no precision problem.
+01891                 // this is true because we cannot move more than, say 4*160 meters in one doMove().
+01892                 // So global position should not be bigger than 1024 * 1024/1024 meters.  => Hence 20 bits of precision is 
+01893                 // required. We have 23 with floats.
+01894                 res.LocalPosition.Estimation= end + deltaOrigin;
+01895 
+01896 
+01897                 // result.
+01898                 return res;
+01899         }
+01900 
+01901 }
+01902 
+01903 
+01904 // ***************************************************************************
+01905 const NLPACS::TCollisionSurfaceDescVector       &NLPACS::CGlobalRetriever::testBBoxRot(const CGlobalPosition &startPos, 
+01906         const NLMISC::CVector &locI, const NLMISC::CVector &locJ, CCollisionSurfaceTemp &cst) const
+01907 {
+01908 //      H_AUTO(PACS_GR_testBBoxRot);
+01909 
+01910         CSurfaceIdent   startSurface(startPos.InstanceId, startPos.LocalPosition.Surface);
+01911 
+01912         // 0. reset.
+01913         //===========
+01914         // reset result.
+01915         cst.CollisionDescs.clear();
+01916 
+01917         // should not doMove() after a testBBoxRot.
+01918         cst.PrecValid= false;
+01919 
+01920 
+01921         // 1. Choose a local basis.
+01922         //===========
+01923         // Take the retrieverInstance of startPos as a local basis.
+01924         CVector         origin;
+01925         origin= getInstance(startPos.InstanceId).getOrigin();
+01926 
+01927 
+01928         // 2. compute OBB.
+01929         //===========
+01930         CVector2f       obbStart[4];
+01931         // compute start, relative to the retriever instance.
+01932         CVector         start= startPos.LocalPosition.Estimation;
+01933         CVector2f       obbCenter(start.x, start.y);
+01934         CVector2f       locI2d(locI.x, locI.y);
+01935         CVector2f       locJ2d(locJ.x, locJ.y);
+01936 
+01937         // build points in CCW.
+01938         obbStart[0]= obbCenter - locI2d - locJ2d;
+01939         obbStart[1]= obbCenter + locI2d - locJ2d;
+01940         obbStart[2]= obbCenter + locI2d + locJ2d;
+01941         obbStart[3]= obbCenter - locI2d + locJ2d;
+01942 
+01943         // 3. compute bboxmove.
+01944         //===========
+01945         CAABBox         bboxMove;
+01946         // extend the bbox.
+01947         bboxMove.setCenter(CVector(obbStart[0].x, obbStart[0].y, 0));
+01948         bboxMove.extend(CVector(obbStart[1].x, obbStart[1].y, 0));
+01949         bboxMove.extend(CVector(obbStart[2].x, obbStart[2].y, 0));
+01950         bboxMove.extend(CVector(obbStart[3].x, obbStart[3].y, 0));
+01951 
+01952 
+01953 
+01954         // 4. find possible collisions in bboxMove+origin. fill cst.CollisionChains.
+01955         //===========
+01956         findCollisionChains(cst, bboxMove, origin);
+01957 
+01958 
+01959 
+01960         // 5. test Rotcollisions with CollisionChains.
+01961         //===========
+01962         CVector2f       startCol(start.x, start.y);
+01963         testRotCollisionWithCollisionChains(cst, startCol, startSurface, obbStart);
+01964 
+01965 
+01966         // result.
+01967         return cst.CollisionDescs;
+01968 }
+01969 
+01970 
+01971 // ***************************************************************************
+01972 void    NLPACS::CGlobalRetriever::testRotCollisionWithCollisionChains(CCollisionSurfaceTemp &cst, const CVector2f &startCol, CSurfaceIdent startSurface, const CVector2f bbox[4]) const
+01973 {
+01974 //      H_AUTO(PACS_GR_testRotCollisionWithCollisionChains);
+01975 
+01976         // start currentSurface with surface start.
+01977         CSurfaceIdent   currentSurface= startSurface;
+01978         sint                    i;
+01979 
+01980         // reset result.
+01981         cst.RotDescs.clear();
+01982         cst.CollisionDescs.clear();
+01983 
+01984 
+01985         /*
+01986                 Test collisions with all collision chains. Then, to manage recovery, test the graph of surfaces.
+01987         */
+01988         // run all collisionChain.
+01989         //========================
+01990         for(i=0; i<(sint)cst.CollisionChains.size(); i++)
+01991         {
+01992                 CCollisionChain         &colChain= cst.CollisionChains[i];
+01993 
+01994 
+01995                 // test all edges of this chain, and insert if necessary.
+01996                 //========================
+01997                 // run list of edge.
+01998                 sint32          curEdge= colChain.FirstEdgeCollide;
+01999                 while(curEdge!=(sint32)0xFFFFFFFF)
+02000                 {
+02001                         // get the edge.
+02002                         CEdgeCollideNode        &colEdge= cst.getEdgeCollideNode(curEdge);
+02003 
+02004                         // test collision with this edge.
+02005                         if(colEdge.testBBoxCollide(bbox))
+02006                         {
+02007                                 // yes we have a 2D collision with this chain.
+02008                                 cst.RotDescs.push_back(CRotSurfaceDesc(colChain.LeftSurface, colChain.RightSurface));
+02009                                 break;
+02010                         }
+02011 
+02012                         // next edge.
+02013                         curEdge= colEdge.Next;
+02014                 }
+02015         }
+02016 
+02017 
+02018         // Traverse the array of collisions.
+02019         //========================
+02020         sint    indexCD=0;
+02021         while(true)
+02022         {
+02023                 // What surfaces collided do we reach from this currentSurface??
+02024                 for(i=0;i<(sint)cst.RotDescs.size();i++)
+02025                 {
+02026                         // Do we collide with this chain?? chain not tested??
+02027                         if(cst.RotDescs[i].hasSurface(currentSurface) && !cst.RotDescs[i].Tested)
+02028                         {
+02029                                 cst.RotDescs[i].Tested= true;
+02030 
+02031                                 // insert the collision with the other surface.
+02032                                 CCollisionSurfaceDesc   col;
+02033                                 col.ContactTime= 0;
+02034                                 col.ContactNormal= CVector::Null;
+02035                                 col.ContactSurface= cst.RotDescs[i].getOtherSurface(currentSurface);
+02036                                 cst.CollisionDescs.push_back(col);
+02037                         }
+02038                 }
+02039 
+02040                 // get the next currentSurface from surface collided (traverse the graph of collisions).
+02041                 if(indexCD<(sint)cst.CollisionDescs.size())
+02042                         currentSurface= cst.CollisionDescs[indexCD++].ContactSurface;
+02043                 else
+02044                         break;
+02045         }
+02046 
+02047 }
+02048 
+02049 // ***************************************************************************
+02050 
+02051 NLPACS::UGlobalRetriever *NLPACS::UGlobalRetriever::createGlobalRetriever (const char *globalRetriever, const NLPACS::URetrieverBank *retrieverBank)
+02052 {
+02053         NL_ALLOC_CONTEXT( Pacs )
+02054 
+02055         // Cast
+02056 //      nlassert (dynamic_cast<const NLPACS::CRetrieverBank*>(retrieverBank));
+02057         const NLPACS::CRetrieverBank*   bank=static_cast<const NLPACS::CRetrieverBank*>(retrieverBank);
+02058 
+02059         CIFile  file;
+02060         if (file.open(CPath::lookup(globalRetriever)))
+02061         {
+02062                 CGlobalRetriever        *retriever = new CGlobalRetriever();
+02063 
+02064                 // always set the retriever bank before serializing !!
+02065                 retriever->setRetrieverBank(bank);
+02066 
+02067                 file.serial(*retriever);
+02068                 retriever->initAll();
+02069 
+02070                 return static_cast<UGlobalRetriever *>(retriever);
+02071         }
+02072         else
+02073                 return NULL;
+02074 }
+02075 
+02076 // ***************************************************************************
+02077 
+02078 void NLPACS::UGlobalRetriever::deleteGlobalRetriever (UGlobalRetriever *retriever)
+02079 {
+02080         // Cast
+02081         nlassert (dynamic_cast<NLPACS::CGlobalRetriever*>(retriever));
+02082         NLPACS::CGlobalRetriever* r=static_cast<NLPACS::CGlobalRetriever*>(retriever);
+02083 
+02084         // Delete
+02085         delete r;
+02086 }
+02087 
+02088 // ***************************************************************************
+02089 
+02090 float                   NLPACS::CGlobalRetriever::getMeanHeight(const UGlobalPosition &pos) const
+02091 {
+02092         // for wrong positions, leave it unchanged
+02093         if ((pos.InstanceId==-1)||(pos.LocalPosition.Surface==-1))
+02094                 return pos.LocalPosition.Estimation.z;
+02095         
+02096         // get instance/localretriever.
+02097         const CRetrieverInstance        &instance = getInstance(pos.InstanceId);
+02098         const CLocalRetriever           &retriever= _RetrieverBank->getRetriever(instance.getRetrieverId());
+02099 
+02100         // return height from local retriever
+02101         return retriever.getHeight(pos.LocalPosition);
+02102 }
+02103 
+02104 // ***************************************************************************
+02105 
+02106 bool NLPACS::CGlobalRetriever::testRaytrace (const CVectorD &v0, const CVectorD &v1)
+02107 {
+02108         // TODO: implement raytrace
+02109         return false;
+02110 }
+02111 
+02112 // ***************************************************************************
+02113 
+02114 
+02115 // end of CGlobalRetriever methods implementation
+
global_retriever.cpp
