# Home    # nevrax.com   
Nevrax
Nevrax.org
#News
#Mailing-list
#Documentation
#CVS
#Bugs
#License
Docs
 
Documentation  
Main Page   Namespace List   Class Hierarchy   Alphabetical List   Compound List   File List   Namespace Members   Compound Members   File Members   Related Pages   Search  

global_retriever.cpp

Go to the documentation of this file.
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