global_retriever.cpp

Go to the documentation of this file.

/* Copyright, 2001 Nevrax Ltd.
 *
 * This file is part of NEVRAX NEL.
 * NEVRAX NEL is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.

 * NEVRAX NEL is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * General Public License for more details.

 * You should have received a copy of the GNU General Public License
 * along with NEVRAX NEL; see the file COPYING. If not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330, Boston,
 * MA 02111-1307, USA.
 */

#include "stdpacs.h"

#include "nel/misc/path.h"
#include "nel/misc/line.h"
#include "nel/misc/async_file_manager.h"
#include "nel/misc/common.h"
#include "nel/misc/variable.h"

#include "nel/misc/hierarchical_timer.h"

#include "global_retriever.h"
#include "retriever_bank.h"

#include <set>


#include "nel/misc/time_nl.h"
NLMISC::TTicks          AStarTicks;
NLMISC::TTicks          PathTicks;
NLMISC::TTicks          ChainTicks;
NLMISC::TTicks          SurfTicks;
NLMISC::TTicks          ThisAStarTicks;
NLMISC::TTicks          ThisPathTicks;
NLMISC::TTicks          ThisChainTicks;
NLMISC::TTicks          ThisSurfTicks;

uint                    PacsRetrieveVerbose = 0;

using namespace std;
using namespace NLMISC;

const float     InsureSurfaceThreshold = 0.5f;  // the threshold distance between 2 surfaces below which we insure the retrieved position to be inside the surface

H_AUTO_DECL ( NLPACS_Refresh_LR_Around )
H_AUTO_DECL ( NLPACS_Retrieve_Position )

#define NLPACS_HAUTO_REFRESH_LR_AROUND  H_AUTO_USE ( NLPACS_Refresh_LR_Around )
#define NLPACS_HAUTO_RETRIEVE_POSITION  H_AUTO_USE ( NLPACS_Retrieve_Position )

// CGlobalRetriever methods implementation

NLPACS::CGlobalRetriever::~CGlobalRetriever()
{
    // must be sure all current async loading is ended
    waitEndOfAsyncLoading();
}

//
void    NLPACS::CGlobalRetriever::init()
{
    _BBox.setCenter(CVector::Null);
    _BBox.setHalfSize(CVector::Null);

    _InstanceGrid.create(128, 160.0f);
}

void    NLPACS::CGlobalRetriever::initQuadGrid()
{
    _InstanceGrid.clear();
    _InstanceGrid.create(128, 160.0f);

    uint    i;
    for (i=0; i<_Instances.size(); ++i)
        _InstanceGrid.insert(_Instances[i].getBBox().getMin(), _Instances[i].getBBox().getMax(), i);
}

void    NLPACS::CGlobalRetriever::initRetrieveTable()
{
    uint    i;
    uint    size = 0;

    for (i=0; i<_Instances.size(); ++i)
    {
        if (_Instances[i].getInstanceId() != -1 && _Instances[i].getRetrieverId() != -1)
        {
            const CLocalRetriever   &retriever = getRetriever(_Instances[i].getRetrieverId());
            size =  std::max((uint)retriever.getSurfaces().size(), size);
        }
    }

    _RetrieveTable.resize(size);
    for (i=0; i<size; ++i)
        _RetrieveTable[i] = 0;
}

//

bool    NLPACS::CGlobalRetriever::selectInstances(const NLMISC::CAABBox &bbox, CCollisionSurfaceTemp &cst, UGlobalPosition::TType type) const
{
    _InstanceGrid.select(bbox.getMin(), bbox.getMax());
    cst.CollisionInstances.clear();

    bool    allLoaded = true;

    NLPACS::CQuadGrid<uint32>::CIterator    it;
    for (it=_InstanceGrid.begin(); it!=_InstanceGrid.end(); ++it)
    {
        if ((type == UGlobalPosition::Landscape && _Instances[*it].getType() == CLocalRetriever::Interior) ||
            (type == UGlobalPosition::Interior && _Instances[*it].getType() == CLocalRetriever::Landscape))
            continue;

        if (_Instances[*it].getBBox().intersect(bbox))
        {
            if (!_RetrieverBank->isLoaded(_Instances[*it].getRetrieverId()))
                allLoaded = false;
            cst.CollisionInstances.push_back(*it);
        }
    }

    return allLoaded;
}

//

void    NLPACS::CGlobalRetriever::serial(NLMISC::IStream &f)
{
    /*
    Version 0:
        - base version.
    */
    (void)f.serialVersion(0);

    f.serialCont(_Instances);
    f.serial(_BBox);

    if (f.isReading())
        initAll(false);
}

//

void    NLPACS::CGlobalRetriever::check() const
{
    uint    i, j, k;

    for (i=0; i<_Instances.size(); ++i)
    {
        if (_Instances[i].getInstanceId() == -1)
        {
            nlwarning("Uninitialized instance %d", i);
            continue;
        }

        if (_Instances[i].getInstanceId() != (sint)i)
            nlwarning("InstanceId for instance %d is not correctly initialized", i);

        if (_Instances[i].getRetrieverId() == -1)
        {
            nlwarning("No retriever at instance %d", i);
            continue;
        }

        const CRetrieverInstance    &instance = _Instances[i];

        if (instance.getRetrieverId()<0 || instance.getRetrieverId()>=(sint)_RetrieverBank->getRetrievers().size())
        {
            nlwarning("Instance %d has wrong retriever reference", i);
            continue;
        }

        const CLocalRetriever       &retriever = _RetrieverBank->getRetriever(instance.getRetrieverId());

        for (j=0; j<retriever.getChains().size(); ++j)
        {
            const CChain    &chain = retriever.getChain(j);
            for (k=0; k<chain.getSubChains().size(); ++k)
            {
                if (chain.getSubChain(k) >= retriever.getOrderedChains().size())
                {
                    nlwarning("retriever %d, chain %d: subchain %d reference is not valid", instance.getRetrieverId(), j, k);
                    continue;
                }

                if (retriever.getOrderedChain(chain.getSubChain(k)).getParentId() != j)
                {
                    nlwarning("retriever %d, ochain %d: reference on parent is not valid", instance.getRetrieverId(), chain.getSubChain(k));
                    continue;
                }

                if (retriever.getOrderedChain(chain.getSubChain(k)).getIndexInParent() != k)
                {
                    nlwarning("retriever %d, ochain %d: index on parent is not valid", instance.getRetrieverId(), chain.getSubChain(k));
                    continue;
                }
            }

            if (chain.getLeft()<0 || chain.getLeft()>=(sint)retriever.getSurfaces().size())
            {
                nlwarning("retriever %d, chain %d: reference on left surface is not valid", instance.getRetrieverId(), j);
            }

            if (chain.getRight()>=(sint)retriever.getSurfaces().size() ||
                chain.getRight()<=CChain::getDummyBorderChainId() && !CChain::isBorderChainId(chain.getRight()))
            {
                nlwarning("retriever %d, chain %d: reference on right surface is not valid", instance.getRetrieverId(), j);
            }

            if (CChain::isBorderChainId(chain.getRight()))
            {
                sint    link = chain.getBorderChainIndex();

                if (link<0 || link>=(sint)instance.getBorderChainLinks().size())
                {
                    nlwarning("retriever %d, instance %d, chain %d: reference on right link is not valid", instance.getRetrieverId(), instance.getInstanceId(), j);
                }
                else
                {
                    CRetrieverInstance::CLink   lnk = instance.getBorderChainLink(link);

                    if (lnk.Instance != 0xFFFF || lnk.SurfaceId != 0xFFFF ||
                        lnk.ChainId != 0xFFFF || lnk.BorderChainId != 0xFFFF)
                    {
                        if (lnk.Instance >= _Instances.size() ||
                            _Instances[lnk.Instance].getRetrieverId()<0 ||
                            _Instances[lnk.Instance].getRetrieverId()>(sint)_RetrieverBank->getRetrievers().size() ||
                            lnk.SurfaceId >= getRetriever(_Instances[lnk.Instance].getRetrieverId()).getSurfaces().size() ||
                            ((lnk.ChainId >= getRetriever(_Instances[lnk.Instance].getRetrieverId()).getChains().size() ||
                            lnk.BorderChainId >= getRetriever(_Instances[lnk.Instance].getRetrieverId()).getBorderChains().size()) && instance.getType() != CLocalRetriever::Interior ))
                        {
                            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);
                        }
                    }
                }
            }
        }
    }
}

//

float   NLPACS::CGlobalRetriever::distanceToBorder(const UGlobalPosition &pos) const
{
    if (pos.InstanceId < 0 || pos.InstanceId > (sint)_Instances.size())
        return 0.0f;

    return getRetriever(_Instances[pos.InstanceId].getRetrieverId()).distanceToBorder(pos.LocalPosition);
}

void    NLPACS::CGlobalRetriever::getBorders(const UGlobalPosition &pos, std::vector<std::pair<NLMISC::CLine, uint8> > &edges)
{
    edges.clear();

    if (pos.InstanceId < 0)
        return;

    CVectorD        gpos = getDoubleGlobalPosition(pos);
    CAABBox         sbox;
    sbox.setCenter(gpos);
    sbox.setHalfSize(CVector(50.0f, 50.0f, 100.0f));

    getBorders(sbox, edges);
}

void    NLPACS::CGlobalRetriever::getBorders(const CAABBox &sbox, std::vector<std::pair<NLMISC::CLine, uint8> > &edges)
{
    edges.clear();

    selectInstances(sbox, _InternalCST);

    uint    inst;
    for (inst=0; inst<_InternalCST.CollisionInstances.size(); ++inst)
    {
        CRetrieverInstance  &instance = _Instances[_InternalCST.CollisionInstances[inst]];
        CLocalRetriever     &retriever = const_cast<CLocalRetriever &>(getRetriever(instance.getRetrieverId()));
        if (!retriever.isLoaded())
            continue;

        CChainQuad          &chainquad = retriever.getChainQuad();

        CAABBox             box;
        CVector             origin = instance.getOrigin();
        box.setCenter(sbox.getCenter()-origin);
        box.setHalfSize(sbox.getHalfSize());
        chainquad.selectEdges(box, _InternalCST);

        uint        ece;

        CVector     dz(0.0f, 0.0f, 0.5f);
        float       zp = (float)sbox.getCenter().z;
        for (ece=0; ece<_InternalCST.EdgeChainEntries.size(); ++ece)
        {
            CEdgeChainEntry     &entry = _InternalCST.EdgeChainEntries[ece];

            //
            const CChain    &fchain = retriever.getChain(retriever.getOrderedChain(entry.OChainId).getParentId());
            uint8           chainType = (fchain.getRight() >= 0 ? 1 : (fchain.isBorderChain() ? 2 : 0));

            //
            if (chainType == 1)
            {
                uint    left = fchain.getLeft();
                uint    right = fchain.getRight();

                const CRetrievableSurface   &lsurface = retriever.getSurface(left);
                const CRetrievableSurface   &rsurface = retriever.getSurface(right);

                bool    luw = (lsurface.getFlags() & (1 << CRetrievableSurface::IsUnderWaterBit)) != 0;
                bool    ruw = (rsurface.getFlags() & (1 << CRetrievableSurface::IsUnderWaterBit)) != 0;

                if (luw && !ruw || !luw && ruw)
                    chainType = 3;
            }

            if (retriever.getFullOrderedChains().size() > 0)
            {
                const COrderedChain3f   &ochain = retriever.getFullOrderedChain(entry.OChainId);

                uint    edge;
                for (edge=entry.EdgeStart; edge<entry.EdgeEnd; ++edge)
                {
                    edges.push_back(make_pair(CLine(), chainType));
                    edges.back().first.V0 = ochain[edge] + origin;
                    edges.back().first.V1 = ochain[edge+1] + origin;
/*
                    edges.push_back(make_pair(CLine(), chainType));
                    edges.back().first.V0 = ochain[edge] + origin;
                    edges.back().first.V1 = ochain[edge] + origin +dz;

                    edges.push_back(make_pair(CLine(), chainType));
                    edges.back().first.V0 = ochain[edge+1] + origin;
                    edges.back().first.V1 = ochain[edge+1] + origin +dz;
*/
                }
            }
            else
            {
                const COrderedChain &ochain = retriever.getOrderedChain(entry.OChainId);

                uint    edge;
                for (edge=entry.EdgeStart; edge<entry.EdgeEnd; ++edge)
                {
                    edges.push_back(make_pair(CLine(), chainType));
                    edges.back().first.V0 = ochain[edge].unpack3f() + origin;
                    edges.back().first.V0.z = zp;
                    edges.back().first.V1 = ochain[edge+1].unpack3f() + origin;
                    edges.back().first.V1.z = zp;
                }
            }
        }
        // Bind edges for exterior mesh
        const CExteriorMesh &em = retriever.getExteriorMesh();
        const CExteriorMesh::CEdge *previousEdge = NULL;
        for(uint k = 0; k < em.getEdges().size(); ++k)
        {
            if (previousEdge)
            {
                edges.push_back(make_pair(CLine(), previousEdge->Link < 0 ? 4 : 5));
                edges.back().first.V0 = previousEdge->Start + origin;
                edges.back().first.V1 = em.getEdges()[k].Start + origin;
            }
            previousEdge = em.getEdges()[k].Link != -2 ? &em.getEdges()[k] : NULL;
        }
    }
}


//

void    NLPACS::CGlobalRetriever::makeLinks(uint n)
{
    CRetrieverInstance  &instance = _Instances[n];

    selectInstances(instance.getBBox(), _InternalCST);

    uint    i;
    for (i=0; i<_InternalCST.CollisionInstances.size(); ++i)
    {
        CRetrieverInstance  &neighbor = _Instances[_InternalCST.CollisionInstances[i]];

        if (neighbor.getInstanceId() == instance.getInstanceId())
            continue;

        try
        {
            instance.link(neighbor, _RetrieverBank->getRetrievers());
            neighbor.link(instance, _RetrieverBank->getRetrievers());
        }
        catch (Exception &e)
        {
            nlwarning("in NLPACS::CGlobalRetriever::makeLinks()");
            nlwarning("caught an exception during linkage of %d and %d: %s", instance.getInstanceId(), neighbor.getInstanceId(), e.what());
        }
    }

    if (getRetriever(instance.getRetrieverId()).getType() == CLocalRetriever::Interior)
        instance.linkEdgeQuad(*this);
}

void    NLPACS::CGlobalRetriever::resetAllLinks()
{
    uint    n;
    for (n=0; n<_Instances.size(); ++n)
        _Instances[n].unlink(_Instances);
}


void    NLPACS::CGlobalRetriever::makeAllLinks()
{
    resetAllLinks();

    uint    n;
    for (n=0; n<_Instances.size(); ++n)
        makeLinks(n);
}

void    NLPACS::CGlobalRetriever::initAll(bool initInstances)
{
    if (initInstances)
    {
        uint    n;
        for (n=0; n<_Instances.size(); ++n)
            if (_Instances[n].getInstanceId() != -1 && _Instances[n].getRetrieverId() != -1)
                _Instances[n].init(_RetrieverBank->getRetriever(_Instances[n].getRetrieverId()));
    }

    initQuadGrid();
    initRetrieveTable();
}

//

const NLPACS::CRetrieverInstance    &NLPACS::CGlobalRetriever::makeInstance(uint32 retrieverId, uint8 orientation, const CVector &origin)
{
    uint    id;
    for (id=0; id<_Instances.size() && _Instances[id].getInstanceId()!=-1; ++id)
        ;

    if (id == _Instances.size())
        _Instances.resize(id+1);

    CRetrieverInstance      &instance = _Instances[id];
    const CLocalRetriever   &retriever = getRetriever(retrieverId);

    if (_RetrieveTable.size() < retriever.getSurfaces().size())
        _RetrieveTable.resize(retriever.getSurfaces().size(), 0);

    instance.make(id, retrieverId, retriever, orientation, origin);

    CVector hsize = instance.getBBox().getHalfSize();
    hsize.z = 0.0f;
    if (hsize != CVector::Null)
    {
        if (_BBox.getHalfSize() == CVector::Null)
        {
            _BBox = instance.getBBox();
        }
        else
        {
            _BBox.extend(instance.getBBox().getMin());
            _BBox.extend(instance.getBBox().getMax());
        }

        if (getRetriever(instance.getRetrieverId()).getType() == CLocalRetriever::Interior)
            instance.initEdgeQuad(*this);

        _InstanceGrid.insert(instance.getBBox().getMin(), instance.getBBox().getMax(), instance.getInstanceId());
    }

    return instance;
}

//

NLPACS::UGlobalPosition NLPACS::CGlobalRetriever::retrievePosition(const CVector &estimated, float threshold) const
{
    return retrievePosition(CVectorD(estimated), (double)threshold, UGlobalPosition::Unspecified);
}

NLPACS::UGlobalPosition NLPACS::CGlobalRetriever::retrievePosition(const CVectorD &estimated, double threshold) const
{
    return retrievePosition(estimated, threshold, UGlobalPosition::Unspecified);
}

NLPACS::UGlobalPosition NLPACS::CGlobalRetriever::retrievePosition(const CVector &estimated) const
{
    return retrievePosition(estimated, 1.0e10f, UGlobalPosition::Unspecified);
}

NLPACS::UGlobalPosition NLPACS::CGlobalRetriever::retrievePosition(const CVectorD &estimated) const
{
    return retrievePosition(estimated, 1.0e10, UGlobalPosition::Unspecified);
}

// Retrieves the position of an estimated point in the global retriever (double instead.)
NLPACS::UGlobalPosition NLPACS::CGlobalRetriever::retrievePosition(const CVectorD &estimated, double /* threshold */, NLPACS::UGlobalPosition::TType retrieveSpec) const
{
    NLPACS_HAUTO_RETRIEVE_POSITION

    // the retrieved position
    CGlobalPosition             result = CGlobalPosition(-1, CLocalRetriever::CLocalPosition(-1, estimated));

    if (!_BBox.include(CVector((float)estimated.x, (float)estimated.y, (float)estimated.z)))
    {
        _ForbiddenInstances.clear();
        return result;
    }


    // get the best matching instances
    CAABBox bbpos;
    bbpos.setCenter(estimated);
    bbpos.setHalfSize(CVector(0.5f, 0.5f, 0.5f));
    if (!selectInstances(bbpos, _InternalCST, retrieveSpec))
    {
        return result;
    }

    uint    i;

    _InternalCST.SortedSurfaces.clear();

    // for each instance, try to retrieve the position
    for (i=0; i<_InternalCST.CollisionInstances.size(); ++i)
    {
        uint32                          id = _InternalCST.CollisionInstances[i];
        const CRetrieverInstance        &instance = _Instances[id];
        const CLocalRetriever           &retriever = _RetrieverBank->getRetriever(instance.getRetrieverId());

        uint    j;
        for (j=0; j<_ForbiddenInstances.size(); ++j)
            if (_ForbiddenInstances[j] == (sint32)id)
                break;

        if (j<_ForbiddenInstances.size() || !retriever.isLoaded())
            continue;

        instance.retrievePosition(estimated, retriever, _InternalCST);
    }

    _ForbiddenInstances.clear();

    if (!_InternalCST.SortedSurfaces.empty())
    {
        // if there are some selected surfaces, sort them
        std::sort(_InternalCST.SortedSurfaces.begin(), _InternalCST.SortedSurfaces.end(), CCollisionSurfaceTemp::CDistanceSurface());

        uint    selInstance;
        float   bestDist = 1.0e10f;
        for (selInstance=0; selInstance<_InternalCST.SortedSurfaces.size(); ++selInstance)
        {
            uint32                      id = _InternalCST.SortedSurfaces[selInstance].Instance;
            const CRetrieverInstance    &instance = _Instances[id];

            if (instance.getType() == CLocalRetriever::Interior && _InternalCST.SortedSurfaces[selInstance].Distance < bestDist+6.0f)
                break;

            if (selInstance == 0)
                bestDist = _InternalCST.SortedSurfaces[0].Distance;
        }

        if (selInstance >= _InternalCST.SortedSurfaces.size())
            selInstance = 0;

        uint32                          id = _InternalCST.SortedSurfaces[selInstance].Instance;
        const CRetrieverInstance        &instance = _Instances[id];
        const CLocalRetriever           &retriever = _RetrieverBank->getRetriever(instance.getRetrieverId());

        // get the UGlobalPosition of the estimation for this surface
        result.InstanceId = id;
        result.LocalPosition.Surface = _InternalCST.SortedSurfaces[selInstance].Surface;
        result.LocalPosition.Estimation = instance.getLocalPosition(estimated);

        CRetrieverInstance::snapVector(result.LocalPosition.Estimation);

        // if there are more than 1 one possible (and best matching) surface, insure the position within the surface (by moving the point)
//      if (_InternalCST.SortedSurfaces.size() >= 2 &&
//          _InternalCST.SortedSurfaces[1].Distance-_InternalCST.SortedSurfaces[0].Distance < InsureSurfaceThreshold)
        if (_InternalCST.SortedSurfaces[selInstance].FoundCloseEdge)
        {
            bool    moved;
            uint    numMove = 0;
            do
            {
                moved = retriever.insurePosition(result.LocalPosition);
                ++numMove;
            }
            while (moved && numMove < 100);
            // the algo won't loop infinitely

            if (numMove > 1)
            {
                nldebug("PACS: insured position inside surface (%d,%d)-(%f,%f,%f), %d moves needed", result.InstanceId, result.LocalPosition.Surface, estimated.x, estimated.y, estimated.z, numMove-1);
            }

            if (moved)
            {
                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);
            }
        }

        // and after selecting the best surface (and some replacement) snap the point to the surface
        instance.snap(result.LocalPosition, retriever);


        if (PacsRetrieveVerbose)
            nlinfo("retrievePosition(%f,%f,%f) -> %d/%d/(%f,%f,%f) - %s/%s",
                    estimated.x, estimated.y, estimated.z,
                    result.InstanceId, result.LocalPosition.Surface,
                    result.LocalPosition.Estimation.x, result.LocalPosition.Estimation.y, result.LocalPosition.Estimation.z,
                    retriever.getIdentifier().c_str(),
                    retriever.getType() == CLocalRetriever::Interior ? "Interior" : "Landscape");
    }
    else
    {
        if (PacsRetrieveVerbose)
            nlwarning("PACS: unable to retrieve correct position (%f,%f,%f)", estimated.x, estimated.y, estimated.z);
//      nlSleep(1);
    }

    return result;
}

//

// Retrieves the position of an estimated point in the global retriever using layer hint
NLPACS::UGlobalPosition NLPACS::CGlobalRetriever::retrievePosition(const CVectorD &estimated, uint h, sint &res) const
{
    // the retrieved position
    CGlobalPosition             result = CGlobalPosition(-1, CLocalRetriever::CLocalPosition(-1, estimated));

    if (!_BBox.include(CVector((float)estimated.x, (float)estimated.y, (float)estimated.z)))
    {
        _ForbiddenInstances.clear();
        res = Failed;
        return result;
    }


    // get the best matching instances
    CAABBox bbpos;
    bbpos.setCenter(estimated);
    bbpos.setHalfSize(CVector(0.5f, 0.5f, 0.5f));
    bool    canGet = selectInstances(bbpos, _InternalCST);

    if (!canGet)
    {
        res = MissingLr;
        return result;
    }

    uint    i;

    _InternalCST.SortedSurfaces.clear();

    // for each instance, try to retrieve the position
    for (i=0; i<_InternalCST.CollisionInstances.size(); ++i)
    {
        uint32                          id = _InternalCST.CollisionInstances[i];
        const CRetrieverInstance        &instance = _Instances[id];
        const CLocalRetriever           &retriever = _RetrieverBank->getRetriever(instance.getRetrieverId());

        uint    j;
        for (j=0; j<_ForbiddenInstances.size(); ++j)
            if (_ForbiddenInstances[j] == (sint32)id)
                break;

        if (j<_ForbiddenInstances.size() || !retriever.isLoaded())
            continue;

        instance.retrievePosition(estimated, retriever, _InternalCST, false);
    }

    _ForbiddenInstances.clear();

    if (!_InternalCST.SortedSurfaces.empty())
    {
        // if there are some selected surfaces, sort them
        std::sort(_InternalCST.SortedSurfaces.begin(), _InternalCST.SortedSurfaces.end(), CCollisionSurfaceTemp::CDistanceSurface());

        if (h >= _InternalCST.SortedSurfaces.size())
        {
            // found less surfaces than expected, abort
            res = Failed;
            return result;
        }

        uint32                          id = _InternalCST.SortedSurfaces[h].Instance;
        const CRetrieverInstance        &instance = _Instances[id];
        const CLocalRetriever           &retriever = _RetrieverBank->getRetriever(instance.getRetrieverId());

        // get the UGlobalPosition of the estimation for this surface
        result.InstanceId = id;
        result.LocalPosition.Surface = _InternalCST.SortedSurfaces[h].Surface;
        result.LocalPosition.Estimation = instance.getLocalPosition(estimated);

        CRetrieverInstance::snapVector(result.LocalPosition.Estimation);

        // if there are more than 1 one possible (and best matching) surface, insure the position within the surface (by moving the point)
//      if (_InternalCST.SortedSurfaces.size() >= 2 &&
//          _InternalCST.SortedSurfaces[1].Distance-_InternalCST.SortedSurfaces[0].Distance < InsureSurfaceThreshold)
        if (_InternalCST.SortedSurfaces[h].FoundCloseEdge)
        {
            bool    moved;
            uint    numMove = 0;
            do
            {
                moved = retriever.insurePosition(result.LocalPosition);
                ++numMove;
            }
            while (moved && numMove < 100);
            // the algo won't loop infinitely

            if (numMove > 1)
            {
                nldebug("PACS: insured position inside surface (%d,%d)-(%f,%f,%f), %d moves needed", result.InstanceId, result.LocalPosition.Surface, estimated.x, estimated.y, estimated.z, numMove-1);
            }

            if (moved)
            {
                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);
            }
        }

        // and after selecting the best surface (and some replacement) snap the point to the surface
        instance.snap(result.LocalPosition, retriever);
    }
    else
    {
        res = Failed;
//      nlwarning("PACS: unable to retrieve correct position (%f,%f,%f)", estimated.x, estimated.y, estimated.z);
//      nlSleep(1);
    }

    res = Success;
    return result;
}


//

sint32          NLPACS::CGlobalRetriever::getIdentifier(const string &id) const
{
    sint32  i;
    for (i=0; i<(sint32)(_RetrieverBank->getRetrievers().size()); ++i)
        if (getRetriever(i).getIdentifier() == id)
            return i;

    return -1;
}

const string    &NLPACS::CGlobalRetriever::getIdentifier(const NLPACS::UGlobalPosition &position) const
{
    static const string     nullString = string("");

    if (position.InstanceId == -1)
        return nullString;

    return getRetriever(_Instances[position.InstanceId].getRetrieverId()).getIdentifier();
}

sint32  NLPACS::CGlobalRetriever::getLocalRetrieverId(const NLPACS::UGlobalPosition &position) const
{
    if (position.InstanceId == -1)
        return -1;

    return _Instances[position.InstanceId].getRetrieverId();
}

//

bool            NLPACS::CGlobalRetriever::buildInstance(const string &id, const NLMISC::CVectorD &position, sint32 &instanceId)
{

    sint32  retrieverId = getIdentifier(id);

    instanceId = -1;

    // check retriever exists
    if (retrieverId < 0)
        return false;

    const CRetrieverInstance    &instance = makeInstance(retrieverId, 0, CVector(position));

    // check make instance success
    if (&instance == NULL || instance.getInstanceId() == -1 || instance.getRetrieverId() != retrieverId)
        return false;

    // links new instance to its neighbors
    makeLinks(instance.getInstanceId());

    instanceId = instance.getInstanceId();

    return true;
}

//

void        NLPACS::CGlobalRetriever::removeInstance(sint32 instanceId)
{
    if (instanceId < 0 || instanceId >= (sint32)_Instances.size() || _Instances[instanceId].getInstanceId() < 0)
    {
        nlwarning("CGlobalRetriever::removeInstance(): Can't unlink instance %d, doesn't exist", instanceId);
        return;
    }

    // get instance
    CRetrieverInstance  &instance = _Instances[instanceId];

    // unlink it from others
    instance.unlink(_Instances);


}

//

//
/*
void        NLPACS::CGlobalRetriever::snapToInteriorGround(UGlobalPosition &position) const
{
    const CRetrieverInstance    &instance = _Instances[position.InstanceId];
    if (instance.getType() != CLocalRetriever::Interior)
        return;

    const CLocalRetriever       &retriever = getRetriever(instance.getRetrieverId());
    instance.snapToInteriorGround(position.LocalPosition, retriever);
}
*/

//
CVector     NLPACS::CGlobalRetriever::getGlobalPosition(const UGlobalPosition &global) const
{
    if (global.InstanceId >= 0)
    {
        return _Instances[global.InstanceId].getGlobalPosition(global.LocalPosition.Estimation);
    }
    else
    {
        // it should be an error here
        return global.LocalPosition.Estimation;
    }
}

CVectorD    NLPACS::CGlobalRetriever::getDoubleGlobalPosition(const NLPACS::UGlobalPosition &global) const
{
    if (global.InstanceId >= 0)
    {
        return _Instances[global.InstanceId].getDoubleGlobalPosition(global.LocalPosition.Estimation);
    }
    else
    {
        // it should be an error here
        return CVectorD(global.LocalPosition.Estimation);
    }
}

//

void        NLPACS::CGlobalRetriever::findAStarPath(const NLPACS::UGlobalPosition &begin,
                                                    const NLPACS::UGlobalPosition &end,
                                                    vector<NLPACS::CRetrieverInstance::CAStarNodeAccess> &path,
                                                    uint32 forbidFlags) const
{
    TTicks  astarStart;
    ThisAStarTicks = 0;
    astarStart = CTime::getPerformanceTime();

    // open and close lists
    // TODO: Use a smart allocator to avoid huge alloc/free and memory fragmentation
    // open is a priority queue (implemented as a stl multimap)
    multimap<float, CRetrieverInstance::CAStarNodeAccess>   open;
    // close is a simple stl vector
    vector<CRetrieverInstance::CAStarNodeAccess>            close;

    // inits start node and info
    CRetrieverInstance::CAStarNodeAccess                    beginNode;
    beginNode.InstanceId = begin.InstanceId;
    beginNode.NodeId = (uint16)begin.LocalPosition.Surface;
    CRetrieverInstance::CAStarNodeInfo                      &beginInfo = getNode(beginNode);

    // inits end node and info.
    CRetrieverInstance::CAStarNodeAccess                    endNode;
    endNode.InstanceId = end.InstanceId;
    endNode.NodeId = (uint16)end.LocalPosition.Surface;
    CRetrieverInstance::CAStarNodeInfo                      &endInfo = getNode(endNode);

    // set up first node...
    CRetrieverInstance::CAStarNodeAccess                    node = beginNode;
    beginInfo.Parent.InstanceId = -1;
    beginInfo.Parent.NodeId = 0;
    beginInfo.Parent.ThroughChain = 0;
    beginInfo.Cost = 0;
    beginInfo.F = (endInfo.Position-beginInfo.Position).norm();

    // ... and inserts it in the open list.
    open.insert(make_pair(beginInfo.F, node));

    // TO DO: use a CVector2f instead
    CVector2f                                               endPosition = CVector2f(getGlobalPosition(end));

    uint    i;

    path.clear();

    for(;;)
    {
        if (open.empty())
        {
            // couldn't find a path
            return;
        }

        multimap<float, CRetrieverInstance::CAStarNodeAccess>::iterator it;

        it = open.begin();
        node = it->second;
        open.erase(it);

        if (node == endNode)
        {
            // found a path
            CRetrieverInstance::CAStarNodeAccess            pathNode = node;
            uint                                            numNodes = 0;
            while (pathNode.InstanceId != -1)
            {
                ++numNodes;
                CRetrieverInstance                          &instance = _Instances[pathNode.InstanceId];
                CRetrieverInstance::CAStarNodeInfo          &pathInfo = instance._NodesInformation[pathNode.NodeId];
                pathNode = pathInfo.Parent;
            }

            path.resize(numNodes);
            pathNode = node;
            while (pathNode.InstanceId != -1)
            {
                path[--numNodes] = pathNode;
                CRetrieverInstance                          &instance = _Instances[pathNode.InstanceId];
                CRetrieverInstance::CAStarNodeInfo          &pathInfo = instance._NodesInformation[pathNode.NodeId];
                pathNode = pathInfo.Parent;
            }

            ThisAStarTicks += (CTime::getPerformanceTime()-astarStart);

            nlinfo("found a path");
            for (i=0; i<path.size(); ++i)
            {
                CRetrieverInstance                          &instance = _Instances[path[i].InstanceId];
                const CLocalRetriever                       &retriever = _RetrieverBank->getRetriever(instance.getRetrieverId());
                nlinfo("pathNode %d = (Inst=%d, Node=%d, Through=%d)", i, path[i].InstanceId, path[i].NodeId, path[i].ThroughChain);
                if (path[i].ThroughChain != 0xffff)
                {
                    const CChain                                &chain = retriever.getChain(path[i].ThroughChain);
                    nlinfo("   chain: left=%d right=%d", chain.getLeft(), chain.getRight());
                    if (CChain::isBorderChainId(chain.getRight()))
                    {
                        CRetrieverInstance::CLink   lnk = instance.getBorderChainLink(CChain::convertBorderChainId(chain.getRight()));
                        sint    instanceid = lnk.Instance;
                        sint    id = lnk.SurfaceId;
                        nlinfo("      right: instance=%d surf=%d", instanceid, id);
                    }
                }
            }
            nlinfo("open.size()=%d", open.size());
            nlinfo("close.size()=%d", close.size());

            return;
        }

        // push successors of the current node
        CRetrieverInstance                              &inst = _Instances[node.InstanceId];
        const CLocalRetriever                           &retriever = _RetrieverBank->getRetriever(inst.getRetrieverId());
        const CRetrievableSurface                       &surf = retriever.getSurface(node.NodeId);
        const vector<CRetrievableSurface::CSurfaceLink> &chains = surf.getChains();

        CRetrieverInstance                              *nextInstance;
        const CLocalRetriever                           *nextRetriever;
        const CRetrievableSurface                       *nextSurface;

        nlinfo("examine node (instance=%d,surf=%d,cost=%g)", node.InstanceId, node.NodeId, inst._NodesInformation[node.NodeId].Cost);

        for (i=0; i<chains.size(); ++i)
        {
            sint32  nextNodeId = chains[i].Surface;
            CRetrieverInstance::CAStarNodeAccess        nextNode;

            if (CChain::isBorderChainId(nextNodeId))
            {
                // if the chain points to another retriever

                // first get the edge on the retriever
                CRetrieverInstance::CLink   lnk = inst.getBorderChainLink(CChain::convertBorderChainId(nextNodeId));
                nextNode.InstanceId = lnk.Instance;

                if (nextNode.InstanceId < 0)
                    continue;

                nextInstance = &_Instances[nextNode.InstanceId];
                nextRetriever = &(_RetrieverBank->getRetriever(nextInstance->getRetrieverId()));

                sint    nodeId = lnk.SurfaceId;
                nlassert(nodeId >= 0);
                nextNode.NodeId = (uint16)nodeId;
            }
            else if (nextNodeId >= 0)
            {
                // if the chain points to the same instance
                nextNode.InstanceId = node.InstanceId;
                nextNode.NodeId = (uint16) nextNodeId;
                nextInstance = &inst;
                nextRetriever = &retriever;
            }
            else
            {
                // if the chain cannot be crossed
                continue;
            }

            nextSurface = &(nextRetriever->getSurface(nextNode.NodeId));

            if (nextSurface->getFlags() & forbidFlags)
                continue;

            // compute new node value (heuristic and cost)

            CRetrieverInstance::CAStarNodeInfo  &nextInfo = nextInstance->_NodesInformation[nextNode.NodeId];
            float   stepCost = (nextInfo.Position-inst._NodesInformation[node.NodeId].Position).norm();
            float   nextCost = inst._NodesInformation[node.NodeId].Cost+stepCost;
            float   nextHeuristic = (nextInfo.Position-endPosition).norm();
            float   nextF = nextCost+nextHeuristic;

            vector<CRetrieverInstance::CAStarNodeAccess>::iterator          closeIt;
            for (closeIt=close.begin(); closeIt!=close.end() && *closeIt!=nextNode; ++closeIt)
                ;

            if (closeIt != close.end() && nextInfo.F < nextF)
                continue;

            multimap<float, CRetrieverInstance::CAStarNodeAccess>::iterator openIt;
            for (openIt=open.begin(); openIt!=open.end() && openIt->second!=nextNode; ++openIt)
                ;

            if (openIt != open.end() && nextInfo.F < nextF)
                continue;

            if (openIt != open.end())
                open.erase(openIt);

            if (closeIt != close.end())
                close.erase(closeIt);

            nextInfo.Parent = node;
            nextInfo.Parent.ThroughChain = (uint16)(chains[i].Chain);
            nextInfo.Cost = nextCost;
            nextInfo.F = nextF;

            nlinfo("  adding node (instance=%d,surf=%d) f=%g, through=%d", nextNode.InstanceId, nextNode.NodeId, nextInfo.F, i);

            open.insert(make_pair(nextInfo.F, nextNode));
        }
        close.push_back(node);
    }
}



void    NLPACS::CGlobalRetriever::findPath(const NLPACS::UGlobalPosition &begin,
                                           const NLPACS::UGlobalPosition &end,
                                           NLPACS::CGlobalRetriever::CGlobalPath &path,
                                           uint32 forbidFlags) const
{

    vector<CRetrieverInstance::CAStarNodeAccess>    astarPath;
    findAStarPath(begin, end, astarPath, forbidFlags);

    TTicks  surfStart;
    TTicks  chainStart;

    ThisChainTicks = 0;
    ThisSurfTicks = 0;
    ThisPathTicks = 0;

    path.clear();
    path.resize(astarPath.size());

    uint    i, j;
    for (i=0; i<astarPath.size(); ++i)
    {
        chainStart = CTime::getPerformanceTime();
        CLocalPath      &surf = path[i];
        surf.InstanceId = astarPath[i].InstanceId;
        const CLocalRetriever   &retriever = _RetrieverBank->getRetriever(_Instances[surf.InstanceId].getRetrieverId());

        // computes start point
        if (i == 0)
        {
            // if it is the first point, just copy the begin
            surf.Start.ULocalPosition::operator= (begin.LocalPosition);
        }
        else
        {
            // else, take the previous value and convert it in the current instance axis
            // TODO: avoid this if the instances are the same
            CVector prev = _Instances[path[i-1].InstanceId].getGlobalPosition(path[i-1].End.Estimation);
            CVector current = _Instances[surf.InstanceId].getLocalPosition(prev);
            surf.Start.Surface = astarPath[i].NodeId;
            surf.Start.Estimation = current;
        }

        // computes end point
        if (i == astarPath.size()-1)
        {
            surf.End.ULocalPosition::operator= (end.LocalPosition);
        }
        else
        {
            // get to the middle of the chain
            // first get the chain between the 2 surfaces
            const CChain            &chain = retriever.getChain(astarPath[i].ThroughChain);
            float                   cumulLength = 0.0f, midLength=chain.getLength()*0.5f;
            for (j=0; j<chain.getSubChains().size() && cumulLength<=midLength; ++j)
                cumulLength += retriever.getOrderedChain(chain.getSubChain(j)).getLength();
            --j;
            const COrderedChain     &ochain = retriever.getOrderedChain(chain.getSubChain(j));
            surf.End.Surface = astarPath[i].NodeId;
            {
                if (ochain.getVertices().size() & 1)
                {
                    surf.End.Estimation = ochain[ochain.getVertices().size()/2].unpack3f();
                }
                else
                {
                    surf.End.Estimation = (ochain[ochain.getVertices().size()/2].unpack3f()+
                                          ochain[ochain.getVertices().size()/2-1].unpack3f())*0.5f;
                }
            }
        }
        ThisChainTicks += (CTime::getPerformanceTime()-chainStart);

        surfStart = CTime::getPerformanceTime();
        retriever.findPath(surf.Start, surf.End, surf.Path, _InternalCST);
        ThisSurfTicks += (CTime::getPerformanceTime()-surfStart);
    }

    ThisPathTicks = ThisAStarTicks+ThisChainTicks+ThisSurfTicks;
    PathTicks += ThisPathTicks;
    SurfTicks += ThisSurfTicks;
    AStarTicks += ThisAStarTicks;
    ChainTicks += ThisChainTicks;
}


// ***************************************************************************

// ***************************************************************************
// ***************************************************************************
// Collisions part.
// ***************************************************************************
// ***************************************************************************



// ***************************************************************************
const NLPACS::CRetrievableSurface   *NLPACS::CGlobalRetriever::getSurfaceById(const NLPACS::CSurfaceIdent &surfId) const
{
    if(surfId.RetrieverInstanceId>=0 && surfId.SurfaceId>=0)
    {
        sint32  locRetId= this->getInstance(surfId.RetrieverInstanceId).getRetrieverId();
        const CLocalRetriever       &retr = _RetrieverBank->getRetriever(locRetId);
        if (!retr.isLoaded() || surfId.SurfaceId >= (sint)retr.getSurfaces().size())
            return NULL;
        const CRetrievableSurface   &surf= retr.getSurface(surfId.SurfaceId);
        return &surf;
    }
    else
        return NULL;
}



// ***************************************************************************
void    NLPACS::CGlobalRetriever::findCollisionChains(CCollisionSurfaceTemp &cst, const NLMISC::CAABBox &bboxMove, const NLMISC::CVector &origin) const
{
//  H_AUTO(PACS_GR_findCollisionChains);

    sint    i,j;

    // 0. reset.
    //===========
    // reset possible chains.
//  H_BEFORE(PACS_GR_findCC_reset);
    cst.CollisionChains.clear();
    cst.resetEdgeCollideNodes();
//  H_AFTER(PACS_GR_findCC_reset);

    // 1. Find Instances which may hit this movement.
    //===========
//  H_BEFORE(PACS_GR_findCC_selectInstances);
    CAABBox     bboxMoveGlobal= bboxMove;
    bboxMoveGlobal.setCenter(bboxMoveGlobal.getCenter()+origin);
    selectInstances(bboxMoveGlobal, cst);
//  H_AFTER(PACS_GR_findCC_selectInstances);

    // 2. Fill CollisionChains.
    //===========
    // For each possible surface mesh, test collision.
    for(i=0 ; i<(sint)cst.CollisionInstances.size(); i++)
    {
//      H_BEFORE(PACS_GR_findCC_getAndComputeMove);
        // get retrieverInstance.
        sint32  curInstance= cst.CollisionInstances[i];
        const CRetrieverInstance    &retrieverInstance= getInstance(curInstance);

        // Retrieve the localRetriever of this instance.
        sint32  localRetrieverId= retrieverInstance.getRetrieverId();
        // If invalid one (hole), continue.
        if(localRetrieverId<0)
            continue;
        const CLocalRetriever       &localRetriever= _RetrieverBank->getRetriever(localRetrieverId);

        if (!localRetriever.isLoaded())
        {
            nlwarning("local retriever %d in %s not loaded, findCollisionChains in this retriever aborted", localRetrieverId, _RetrieverBank->getNamePrefix().c_str());
            continue;
        }

        // get delta between startPos.instance and curInstance.
        CVector     deltaOrigin;
        deltaOrigin= origin - retrieverInstance.getOrigin();

        // compute movement relative to this localRetriever.
        CAABBox     bboxMoveLocal= bboxMove;
        bboxMoveLocal.setCenter(bboxMoveLocal.getCenter()+deltaOrigin);

        // add possible collision chains with movement.
        //================
        sint        firstCollisionChain= cst.CollisionChains.size();
        CVector2f   transBase(-deltaOrigin.x, -deltaOrigin.y);
//      H_AFTER(PACS_GR_findCC_getAndComputeMove);

//      H_BEFORE(PACS_GR_findCC_testCollision);
        // Go! fill collision chains that this movement intersect.
        localRetriever.testCollision(cst, bboxMoveLocal, transBase);
        // if an interior, also test for external collisions
        if (retrieverInstance.getType() == CLocalRetriever::Interior)
            retrieverInstance.testExteriorCollision(cst, bboxMoveLocal, transBase, localRetriever);

        // how many collision chains added?  : nCollisionChain-firstCollisionChain.
        sint        nCollisionChain= cst.CollisionChains.size();
//      H_AFTER(PACS_GR_findCC_testCollision);


        // For all collision chains added, fill good SurfaceIdent info.
        //================
//      H_BEFORE(PACS_GR_findCC_fillSurfIdent);
        for(j=firstCollisionChain; j<nCollisionChain; j++)
        {
            CCollisionChain     &cc= cst.CollisionChains[j];

            // info are already filled for exterior chains.
            if (cc.ExteriorEdge)
                continue;

            // LeftSurface retrieverInstance is always curInstance.
            cc.LeftSurface.RetrieverInstanceId= curInstance;

            // If RightSurface is not an "edgeId" ie a pointer on a neighbor surface on an other retrieverInstance.
            const   CChain      &originalChain= localRetriever.getChain(cc.ChainId);
            if( !originalChain.isBorderChainId(cc.RightSurface.SurfaceId) )
            {
                cc.RightSurface.RetrieverInstanceId= curInstance;
            }
            else
            {
                // we must find the surfaceIdent of the neighbor.

                CRetrieverInstance::CLink   link;
                // get the link to the next surface from the instance
                link = retrieverInstance.getBorderChainLink(CChain::convertBorderChainId(cc.RightSurface.SurfaceId));

                // get the neighbor instanceId.
                sint    neighborInstanceId= (sint16)link.Instance;
                // store in the current collisionChain Right.
                cc.RightSurface.RetrieverInstanceId= neighborInstanceId;

                // If no instance near us, this is a WALL.
                if(neighborInstanceId<0)
                {
                    // mark as a Wall.
                    cc.RightSurface.SurfaceId= -1;
                }
                else
                {
                    // Get the good neighbor surfaceId.
                    cc.RightSurface.SurfaceId= (sint16)link.SurfaceId;
                }
            }

            nlassert(cc.LeftSurface.RetrieverInstanceId < (sint)_Instances.size());
            nlassert(cc.RightSurface.RetrieverInstanceId < (sint)_Instances.size());
        }
//      H_AFTER(PACS_GR_findCC_fillSurfIdent);


        // For all collision chains added, look if they are a copy of preceding collsion chain (same Left/Right). Then delete them.
        //================
//      H_BEFORE(PACS_GR_findCC_removeDouble);
        for(j=firstCollisionChain; j<nCollisionChain; j++)
        {
            const CCollisionChain   &cj = cst.CollisionChains[j];

            if (cj.ExteriorEdge && cj.LeftSurface.RetrieverInstanceId!=-1)
                continue;

            // test for all collisionChain inserted before.
            for(sint k=0; k<firstCollisionChain; k++)
            {
                const CCollisionChain   &ck = cst.CollisionChains[k];

                if (cj.LeftSurface.RetrieverInstanceId != cj.RightSurface.RetrieverInstanceId &&
                    cj.LeftSurface == ck.RightSurface && cj.RightSurface == ck.LeftSurface)
                {
                    const CRetrieverInstance    &instj = getInstance(cj.LeftSurface.RetrieverInstanceId),
                                                &instk = getInstance(ck.LeftSurface.RetrieverInstanceId);
                    const CLocalRetriever       &retrj = getRetriever(instj.getRetrieverId()),
                                                &retrk = getRetriever(instk.getRetrieverId());

                    if (!retrj.isLoaded() || !retrk.isLoaded())
                    {
                        nlwarning("using not loaded retriever %d or %d in bank '%s', aborted", instj.getRetrieverId(), instk.getRetrieverId(), _RetrieverBank->getNamePrefix().c_str());
                        continue;
                    }

                    nlassert(retrj.getChain(cj.ChainId).isBorderChain() && retrk.getChain(ck.ChainId).isBorderChain());

                    if (instj.getBorderChainLink(retrj.getChain(cj.ChainId).getBorderChainIndex()).ChainId != ck.ChainId ||
                        instk.getBorderChainLink(retrk.getChain(ck.ChainId).getBorderChainIndex()).ChainId != cj.ChainId)
                    {
                        continue;
                    }

                    // remove this jth entry.
                    // by swapping with last entry. Only if not already last.
                    if(j<nCollisionChain-1)
                    {
                        swap(cst.CollisionChains[j], cst.CollisionChains[nCollisionChain-1]);
                        // NB: some holes remain in cst._EdgeCollideNodes, but do not matters since reseted at
                        // each collision test.
                    }

                    // pop last entry.
                    nCollisionChain--;
                    cst.CollisionChains.resize(nCollisionChain);

                    // next entry??
                    j--;
                    break;
                }
/*
                // if same surface Ident Left/Right==Left/Right or swapped Left/Right==Right/Left
                if( cst.CollisionChains[j].sameSurfacesThan(cst.CollisionChains[k]) )
                {
                    // remove this jth entry.
                    // by swapping with last entry. Only if not already last.
                    if(j<nCollisionChain-1)
                    {
                        swap(cst.CollisionChains[j], cst.CollisionChains[nCollisionChain-1]);
                        // NB: some holes remain in cst._EdgeCollideNodes, but do not matters since reseted at
                        // each collision test.
                    }

                    // pop last entry.
                    nCollisionChain--;
                    cst.CollisionChains.resize(nCollisionChain);

                    // next entry??
                    j--;
                    break;
                }
*/
            }

        }
//      H_AFTER(PACS_GR_findCC_removeDouble);
    }

}


// ***************************************************************************
void    NLPACS::CGlobalRetriever::testCollisionWithCollisionChains(CCollisionSurfaceTemp &cst, const CVector2f &startCol, const CVector2f &deltaCol,
        CSurfaceIdent startSurface, float radius, const CVector2f bboxStart[4], TCollisionType colType) const
{
//  H_AUTO(PACS_GR_testCollisionWithCollisionChains);

    // start currentSurface with surface start.
    CSurfaceIdent   currentSurface= startSurface;
    uint            nextCollisionSurfaceTested=0;
    sint            i;

    // reset result.
    cst.CollisionDescs.clear();
    // reset all collisionChain to not tested.
    for(i=0; i<(sint)cst.CollisionChains.size(); i++)
    {
        CCollisionChain     &colChain= cst.CollisionChains[i];
        colChain.Tested= false;
    }

    vector<pair<sint32, bool> > checkedExtEdges;


    /*
        To manage recovery, we must use such an algorithm, so we are sure to trace the way across all surfaces really
        collided, and discard any other (such as other floor or ceiling).
    */
    for(;;)
    {
        // run all collisionChain.
        //========================
        for(i=0; i<(sint)cst.CollisionChains.size(); i++)
        {
            CCollisionChain     &colChain= cst.CollisionChains[i];

            nlassert(colChain.LeftSurface.RetrieverInstanceId < (sint)_Instances.size());
            nlassert(colChain.RightSurface.RetrieverInstanceId < (sint)_Instances.size());

            // test only currentSurface/X. And don't test chains already tested before.
            if(colChain.hasSurface(currentSurface) && !colChain.Tested)
            {
                // we are testing this chain.
                colChain.Tested= true;

                // avoid checking twice a door
                if (colChain.ExteriorEdge && colChain.LeftSurface.RetrieverInstanceId != -1)
                {
                    bool    enterInterior = (currentSurface.RetrieverInstanceId == colChain.RightSurface.RetrieverInstanceId);

                    uint    j;
                    sint32  cmp = (colChain.LeftSurface.RetrieverInstanceId<<16) + colChain.ChainId;
                    for (j=0; j<checkedExtEdges.size() && (checkedExtEdges[j].first != cmp); ++j)
                        ;
                    // if already crossed this edge, abort
                    // this a door that is crossing a surface frontier
                    if (j < checkedExtEdges.size())
                    {
                        if (checkedExtEdges[j].second != enterInterior)
                            continue;
                    }
                    else
                        checkedExtEdges.push_back(make_pair(cmp, enterInterior));
                }

                // test all edges of this chain, and get tmin
                //========================

                float       t=0.0, tMin=1;
                CVector2f   normal, normalMin;
                // run list of edge.
                sint32      curEdge= colChain.FirstEdgeCollide;
                while(curEdge!=(sint32)0xFFFFFFFF)
                {
                    // get the edge.
                    CEdgeCollideNode    &colEdge= cst.getEdgeCollideNode(curEdge);

                    // test collision with this edge.
                    if(colType==CGlobalRetriever::Circle)
                        t= colEdge.testCircleMove(startCol, deltaCol, radius, normal);
                    else if(colType==CGlobalRetriever::BBox)
                        t= colEdge.testBBoxMove(startCol, deltaCol, bboxStart, normal);

                    // earlier collision??
                    if(t<tMin)
                    {
                        tMin= t;
                        normalMin= normal;
                    }

                    // next edge.
                    curEdge= colEdge.Next;
                }


                // If collision with this chain, must insert it in the array of collision.
                //========================
                if(tMin<1)
                {
                    CSurfaceIdent   collidedSurface= colChain.getOtherSurface(currentSurface);

                    // if flag as an interior/landscape interface and leave interior surf, retrieve correct surface
                    if (colChain.ExteriorEdge && currentSurface == colChain.LeftSurface)
                    {
                        // p= position until the bounding object collide the exterior edge
                        CVector2f       p = startCol + deltaCol*tMin;
                        // get the interior origin
                        CVector         ori = getInstance(startSurface.RetrieverInstanceId).getOrigin();
                        ori.z = 0.0f;

                        // Estimate current Z
                        UGlobalPosition rp;
                        rp.InstanceId = currentSurface.RetrieverInstanceId;
                        rp.LocalPosition.Surface = currentSurface.SurfaceId;
                        rp.LocalPosition.Estimation = p;
                        // NB: getMeanHeight() should work here since we are still deep in the interior surface (edge collided with bounding volume)
                        float   estimatedZ= getMeanHeight(rp);

                        // retrieve the position, with the estimated Z
                        CVectorD        zp = CVectorD(p.x, p.y, estimatedZ) + CVectorD(ori);
                        // Do not allow the current interior instance
                        _ForbiddenInstances.clear();
                        _ForbiddenInstances.push_back(currentSurface.RetrieverInstanceId);
                        UGlobalPosition gp = retrievePosition(zp);

                        collidedSurface.RetrieverInstanceId = gp.InstanceId;
                        collidedSurface.SurfaceId = gp.LocalPosition.Surface;
                    }

                    nlassert(collidedSurface.RetrieverInstanceId < (sint)_Instances.size());

                    // insert or replace this collision in collisionDescs.
                    // NB: yes this looks like a N algorithm (so N²). But not so many collisions may arise, so don't bother.
                    sint    indexInsert= cst.CollisionDescs.size();
                    sint    colFound= -1;

                    // start to search with nextCollisionSurfaceTested, because can't insert before.
                    for(sint j= nextCollisionSurfaceTested; j<(sint)cst.CollisionDescs.size(); j++)
                    {
                        // we must keep time order.
                        if(tMin < cst.CollisionDescs[j].ContactTime)
                        {
                            indexInsert= min(j, indexInsert);
                        }
                        // Does the collision with this surface already exist??
                        if(cst.CollisionDescs[j].ContactSurface==collidedSurface)
                        {
                            colFound= j;
                            // if we have found our old collision, stop, there is no need to search more.
                            break;
                        }
                    }

                    // Insert only if the surface was not already collided, or that new collision arise before old.
                    if(colFound==-1 || indexInsert<=colFound)
                    {
                        CCollisionSurfaceDesc   newCol;
                        newCol.ContactSurface= collidedSurface;
                        newCol.ContactTime= tMin;
                        newCol.ContactNormal.set(normalMin.x, normalMin.y, 0);

                        // if, by chance, indexInsert==colFound, just replace old collision descriptor.
                        if(colFound==indexInsert)
                        {
                            cst.CollisionDescs[indexInsert]= newCol;
                        }
                        else
                        {
                            // if any, erase old collision against this surface. NB: here, colFound>indexInsert.
                            if(colFound!=-1)
                                cst.CollisionDescs.erase(cst.CollisionDescs.begin() + colFound);

                            // must insert the collision.
                            cst.CollisionDescs.insert(cst.CollisionDescs.begin() + indexInsert, newCol);
                        }
                    }
                }
            }
        }

        // Find next surface to test.
        //========================
        // No more?? so this is the end.
        if(nextCollisionSurfaceTested>=cst.CollisionDescs.size())
            break;
        // else next one.
        else
        {
            // NB: with this algorithm, we are sure that no more collisions will arise before currentCollisionSurfaceTested.
            // so just continue with following surface.
            currentSurface= cst.CollisionDescs[nextCollisionSurfaceTested].ContactSurface;

            // Do we touch a wall??
            bool    isWall;
            if(currentSurface.SurfaceId<0)
                isWall= true;
            else
            {
                // test if it is a walkable wall.
                sint32  locRetId= this->getInstance(currentSurface.RetrieverInstanceId).getRetrieverId();

                if (!_RetrieverBank->getRetriever(locRetId).isLoaded())
                {
                    nextCollisionSurfaceTested++;
                    continue;
                }

                const CLocalRetriever       &retr = _RetrieverBank->getRetriever(locRetId);
                if (currentSurface.SurfaceId < (sint)retr.getSurfaces().size())
                {
                    const CRetrievableSurface   &surf= _RetrieverBank->getRetriever(locRetId).getSurface(currentSurface.SurfaceId);
                    isWall= !(surf.isFloor() || surf.isCeiling());
                }
                else
                {
                    isWall = true;
                }
            }

            // If we touch a wall, this is the end of search.
            if(isWall)
            {
                // There can be no more collision after this one.
                cst.CollisionDescs.resize(nextCollisionSurfaceTested+1);
                break;
            }
            else
            {
                // Next time, we will test the following (NB: the array may grow during next pass, or reorder,
                // but only after nextCollisionSurfaceTested).
                nextCollisionSurfaceTested++;
            }
        }
    }

}


// ***************************************************************************
bool            NLPACS::CGlobalRetriever::verticalChain(const CCollisionChain &colChain) const
{
    // retrieve surfaces.
    const CRetrievableSurface   *left= getSurfaceById(colChain.LeftSurface);
    const CRetrievableSurface   *right= getSurfaceById(colChain.RightSurface);

    // test if left surface is a wall.
    bool                        leftWall;
    if(!left)
        leftWall= true;
    else
        leftWall= !(left->isFloor() || left->isCeiling());

    // test if right surface is a wall.
    bool                        rightWall;
    if(!right)
        rightWall= true;
    else
        rightWall= !(right->isFloor() || right->isCeiling());

    // true if both are a wall.
    return leftWall && rightWall;
}


// ***************************************************************************
NLPACS::CSurfaceIdent   NLPACS::CGlobalRetriever::testMovementWithCollisionChains(CCollisionSurfaceTemp &cst, const CVector2f &startCol, const CVector2f &endCol,
        CSurfaceIdent startSurface, UGlobalPosition &restart) const
{
//  H_AUTO(PACS_GR_testMovementWithCollisionChains);

    // start currentSurface with surface start.
    CSurfaceIdent   currentSurface= startSurface;
    sint            i;

    // reset result.
    cst.MoveDescs.clear();


    static vector<pair<sint32, bool> >  checkedExtEdges;

    /*
        To manage recovery, we must use such an algorithm, so we are sure to trace the way across all surfaces really
        collided, and discard any other (such as other floor or ceiling).

        This function is quite different from testCollisionWithCollisionChains() because she must detect all collisions
        with all edges of any chains (and not the minimum collision with a chain).
        This is done in 3 parts:
            - detect collisions with all edges.
            - sort.
            - leave only real collisions.
    */
    // run all collisionChain.
    //========================
    for(i=0; i<(sint)cst.CollisionChains.size(); i++)
    {
        CCollisionChain     &colChain= cst.CollisionChains[i];

        if (colChain.ExteriorEdge)
        {
            sint32  cmp = (colChain.LeftSurface.RetrieverInstanceId<<16) + colChain.ChainId;

            uint    j;
            for (j=0; j<checkedExtEdges.size() && (checkedExtEdges[j].first != cmp); ++j)
                ;
            // if already crossed this edge, abort
            // this a door that is crossing a surface frontier
            if (j < checkedExtEdges.size())
                continue;
        }

        // test all edges of this chain, and insert if necessary.
        //========================
        CRational64     t;
        // run list of edge.
        sint32      curEdge= colChain.FirstEdgeCollide;
        while(curEdge!=(sint32)0xFFFFFFFF)
        {
            // get the edge.
            CEdgeCollideNode    &colEdge= cst.getEdgeCollideNode(curEdge);

            // test collision with this edge.
            CEdgeCollide::TPointMoveProblem     pmpb;
            t= colEdge.testPointMove(startCol, endCol, pmpb);
            // manage multiple problems of precision.
            if(t== -1)
            {
                static const string errs[CEdgeCollide::PointMoveProblemCount]= {
                    "ParallelEdges", "StartOnEdge", "StopOnEdge", "TraverseEndPoint", "EdgeNull"};
                // return a "Precision Problem" ident. movement is invalid.
                // BUT if startOnEdge, which should never arrive.
                if(pmpb==CEdgeCollide::StartOnEdge)
                {
                    nlinfo("COL: Precision Problem: %s", errs[pmpb].c_str());
                    checkedExtEdges.clear();
                    return CSurfaceIdent(-1, -1);   // so in this case, block....
                }
                else if(pmpb==CEdgeCollide::EdgeNull)
                {
                    /*
                    // verify if it is an edge which separate 2 walls. in this case, ignore it. else, error.
                    if(verticalChain(colChain))
                    {
                        t=1;    // no collision with this edge.
                    }
                    else
                    {
                        nlinfo("COL: Precision Problem: %s", errs[pmpb]);
                        nlstop;     // this should not append.
                        return CSurfaceIdent(-1, -1);
                    }*/
                    /* Actually, this is never a problem: we never get through this edge.
                        Instead, we'll get through the neighbors edge.
                        So just disable this edge.
                    */
                    t= 1;
                }
                else
                    return  CSurfaceIdent(-2, -2);
            }

            // collision??
            if(t<1)
            {
                // insert in list.
                cst.MoveDescs.push_back(CMoveSurfaceDesc(t, colChain.LeftSurface, colChain.RightSurface));
                cst.MoveDescs.back().ExteriorEdge = colChain.ExteriorEdge;
                cst.MoveDescs.back().ChainId = (uint16)colChain.ChainId;
                cst.MoveDescs.back().MovementSens= colEdge.Norm*(endCol-startCol)>=0;
            }

            // next edge.
            curEdge= colEdge.Next;
        }
    }


    // sort.
    //================
    // sort the collisions in ascending time order.
    sort(cst.MoveDescs.begin(), cst.MoveDescs.end());


    // Traverse the array of collisions.
    //========================
    for(i=0;i<(sint)cst.MoveDescs.size();i++)
    {
        CMoveSurfaceDesc    &msd = cst.MoveDescs[i];

        // Do we collide with this chain??
        if(msd.hasSurface(currentSurface))
        {
            // if flag as an interior/landscape interface and leave interior surf, retrieve correct surface
            if (msd.ExteriorEdge && msd.LeftSurface.RetrieverInstanceId != -1)
            {
                bool    enterInterior = (currentSurface.RetrieverInstanceId == msd.RightSurface.RetrieverInstanceId);

                // msd.MovementSens is true if we "geometrically" leave the interior.
                // If logic and geometric disagree, discard
                if(enterInterior == msd.MovementSens)
                    continue;

                uint    j;
                sint32  cmp = (msd.LeftSurface.RetrieverInstanceId<<16) + msd.ChainId;
                for (j=0; j<checkedExtEdges.size() && (checkedExtEdges[j].first != cmp); ++j)
                    ;
                // if already crossed this edge, abort
                // this a door that is crossing a surface frontier
                if (j < checkedExtEdges.size())
                {
                    if (checkedExtEdges[j].second != enterInterior)
                        continue;
                }
                else
                    checkedExtEdges.push_back(make_pair(cmp, enterInterior));

                // if leave interior, retrieve good position
                if (!enterInterior)
                {
                    // p= position until the object center point collide the exterior edge
                    float           ctime = (float)((double)(msd.ContactTime.Numerator)/(double)(msd.ContactTime.Denominator));
                    CVector2f       p = startCol*(1.0f-ctime) + endCol*ctime;
                    // get the interior origin
                    CVector         ori = getInstance(startSurface.RetrieverInstanceId).getOrigin();
                    ori.z = 0.0f;

                    // Estimate current Z
                    UGlobalPosition rp;
                    rp.InstanceId = currentSurface.RetrieverInstanceId;
                    rp.LocalPosition.Surface = currentSurface.SurfaceId;
                    rp.LocalPosition.Estimation = p;
                    /* WE HAVE A PRECISION PROBLEM HERE (yoyo 12/04/2006)
                        Since the point p has moved close to the exterior edge, because of precision, it may be actually
                        OUT the interior surface!!
                        thus getMeanHeight() will return 0!!
                        Then the chosen landscape position can be completly false. eg:
                            actual InteriorHeight: -84
                            new possibles landscape surfaces heights: -84 and -16
                            if we estimate by error InteriorHeight= 0, then we will
                                have Best Landscape Surface == the one which has height=-16 !

                        Hence we use a specific method that look a bit outisde the triangles
                    */
                    float estimatedZ = getInteriorHeightAround(rp, 0.1f);

                    // retrieve the position, with the estimated Z
                    CVectorD        zp = CVectorD(p.x, p.y, estimatedZ) + CVectorD(ori);
                    // Do not allow the current interior instance
                    _ForbiddenInstances.clear();
                    _ForbiddenInstances.push_back(currentSurface.RetrieverInstanceId);
                    restart = retrievePosition(zp);

                    return CSurfaceIdent(-3, -3);
                }
                else
                {
                    currentSurface= msd.getOtherSurface(currentSurface);
                }
            }
            else
            {
                currentSurface= msd.getOtherSurface(currentSurface);
            }

            // Do we touch a wall?? should not happens, but important for security.
            bool    isWall;
            if(currentSurface.SurfaceId<0)
                isWall= true;
            else
            {
                // test if it is a walkable wall.
                sint32  locRetId= this->getInstance(currentSurface.RetrieverInstanceId).getRetrieverId();

                if (!_RetrieverBank->getRetriever(locRetId).isLoaded())
                    continue;

                const CRetrievableSurface   &surf= _RetrieverBank->getRetriever(locRetId).getSurface(currentSurface.SurfaceId);
                isWall= !(surf.isFloor() || surf.isCeiling());
            }

            // If we touch a wall, this is the end of search.
            if(isWall)
            {
                // return a Wall ident. movement is invalid.
                checkedExtEdges.clear();
                return  CSurfaceIdent(-1, -1);
            }
        }
    }

    checkedExtEdges.clear();
    return currentSurface;
}



// ***************************************************************************
const   NLPACS::TCollisionSurfaceDescVector
    *NLPACS::CGlobalRetriever::testCylinderMove(const UGlobalPosition &startPos, const NLMISC::CVector &delta, float radius, CCollisionSurfaceTemp &cst) const
{
//  H_AUTO(PACS_GR_testCylinderMove);

    CSurfaceIdent   startSurface(startPos.InstanceId, startPos.LocalPosition.Surface);

    // 0. reset.
    //===========
    // reset result.
    cst.CollisionDescs.clear();

    // In a surface ?
    if (startPos.InstanceId==-1)
    {
        // Warning this primitive is not on a surface
        //nlassertonce (0);

        // Return NULL when lost
        return NULL;
    }
    // store this request in cst.
    cst.PrecStartSurface= startSurface;
    cst.PrecStartPos= startPos.LocalPosition.Estimation;
    cst.PrecDeltaPos= delta;
    cst.PrecValid= true;

    // 0.bis
    //===========
    // Abort if deltamove is 0,0,0.
    if (delta.isNull())
        return &cst.CollisionDescs;

    // 1. Choose a local basis.
    //===========
    // Take the retrieverInstance of startPos as a local basis.
    CVector     origin;
    origin= getInstance(startPos.InstanceId).getOrigin();


    // 2. compute bboxmove.
    //===========
    CAABBox     bboxMove;
    // bounds the movement in a bbox.
    // compute start and end, relative to the retriever instance.
    CVector     start= startPos.LocalPosition.Estimation;
    CVector     end= start+delta;
    // extend the bbox.
    bboxMove.setCenter(start-CVector(radius, radius, 0));
    bboxMove.extend(start+CVector(radius, radius, 0));
    bboxMove.extend(end-CVector(radius, radius, 0));
    bboxMove.extend(end+CVector(radius, radius, 0));


    // 3. find possible collisions in bboxMove+origin. fill cst.CollisionChains.
    //===========
    findCollisionChains(cst, bboxMove, origin);



    // 4. test collisions with CollisionChains.
    //===========
    CVector2f   startCol(start.x, start.y);
    CVector2f   deltaCol(delta.x, delta.y);
    CVector2f   obbDummy[4];    // dummy OBB (not obb here so don't bother)
    testCollisionWithCollisionChains(cst, startCol, deltaCol, startSurface, radius, obbDummy, CGlobalRetriever::Circle);

    // result.
    return &cst.CollisionDescs;
}


// ***************************************************************************
const   NLPACS::TCollisionSurfaceDescVector
    *NLPACS::CGlobalRetriever::testBBoxMove(const UGlobalPosition &startPos, const NLMISC::CVector &delta,
    const NLMISC::CVector &locI, const NLMISC::CVector &locJ, CCollisionSurfaceTemp &cst) const
{
//  H_AUTO(PACS_GR_testBBoxMove);

    CSurfaceIdent   startSurface(startPos.InstanceId, startPos.LocalPosition.Surface);

    // 0. reset.
    //===========
    // reset result.
    cst.CollisionDescs.clear();

    // In a surface ?
    if (startPos.InstanceId==-1)
    {
        // Warning this primitive is not on a surface
        //nlassertonce (0);

        // Return NULL when lost
        return NULL;
    }

    // store this request in cst.
    cst.PrecStartSurface= startSurface;
    cst.PrecStartPos= startPos.LocalPosition.Estimation;
    cst.PrecDeltaPos= delta;
    cst.PrecValid= true;

    // 0.bis
    //===========
    // Abort if deltamove is 0,0,0.
    if (delta.isNull())
        return &cst.CollisionDescs;

    // 1. Choose a local basis.
    //===========
    // Take the retrieverInstance of startPos as a local basis.
    CVector     origin;
    origin= getInstance(startPos.InstanceId).getOrigin();


    // 2. compute OBB.
    //===========
    CVector2f   obbStart[4];
    // compute start, relative to the retriever instance.
    CVector     start= startPos.LocalPosition.Estimation;
    CVector2f   obbCenter(start.x, start.y);
    CVector2f   locI2d(locI.x, locI.y);
    CVector2f   locJ2d(locJ.x, locJ.y);

    // build points in CCW.
    obbStart[0]= obbCenter - locI2d - locJ2d;
    obbStart[1]= obbCenter + locI2d - locJ2d;
    obbStart[2]= obbCenter + locI2d + locJ2d;
    obbStart[3]= obbCenter - locI2d + locJ2d;

    // 3. compute bboxmove.
    //===========
    CAABBox     bboxMove;
    // extend the bbox.
    bboxMove.setCenter(CVector(obbStart[0].x, obbStart[0].y, 0));
    bboxMove.extend(CVector(obbStart[1].x, obbStart[1].y, 0));
    bboxMove.extend(CVector(obbStart[2].x, obbStart[2].y, 0));
    bboxMove.extend(CVector(obbStart[3].x, obbStart[3].y, 0));
    bboxMove.extend(CVector(obbStart[0].x, obbStart[0].y, 0) + delta);
    bboxMove.extend(CVector(obbStart[1].x, obbStart[1].y, 0) + delta);
    bboxMove.extend(CVector(obbStart[2].x, obbStart[2].y, 0) + delta);
    bboxMove.extend(CVector(obbStart[3].x, obbStart[3].y, 0) + delta);



    // 4. find possible collisions in bboxMove+origin. fill cst.CollisionChains.
    //===========
    findCollisionChains(cst, bboxMove, origin);



    // 5. test collisions with CollisionChains.
    //===========
    CVector2f   startCol(start.x, start.y);
    CVector2f   deltaCol(delta.x, delta.y);
    testCollisionWithCollisionChains(cst, startCol, deltaCol, startSurface, 0, obbStart, CGlobalRetriever::BBox);

    // result.
    return &cst.CollisionDescs;
}



// ***************************************************************************
NLPACS::UGlobalPosition
    NLPACS::CGlobalRetriever::doMove(const NLPACS::UGlobalPosition &startPos, const NLMISC::CVector &delta, float t, NLPACS::CCollisionSurfaceTemp &cst, bool rebuildChains) const
{
//  H_AUTO(PACS_GR_doMove);

    CSurfaceIdent   startSurface(startPos.InstanceId, startPos.LocalPosition.Surface);

    // clamp factor.
    clamp(t, 0.0f, 1.0f);

    // 0. reset.
    //===========
    // reset CollisionDescs.
    cst.CollisionDescs.clear();

    // In a surface ?
    if (startPos.InstanceId==-1)
    {
        // Warining: this primitive is not on a surface
        //nlassertonce (0);

        // Return startpos
        return startPos;
    }

    if(!rebuildChains)
    {
        // same move request than prec testMove() ??.
        if( cst.PrecStartSurface != startSurface ||
            cst.PrecStartPos!=startPos.LocalPosition.Estimation ||
            cst.PrecDeltaPos!=delta ||
            !cst.PrecValid)
        {
            // if not, just return start.
            //nlstop;
            //nlwarning ("BEN: you must fix this, it s happen!!!");
            return startPos;
        }
        // Since we are sure we have same movement than prec testMove(), no need to rebuild cst.CollisionChains.
    }
    else
    {
        // we don't have same movement than prec testMove(), we must rebuild cst.CollisionChains.
        // Prec settings no more valids.
        cst.PrecValid= false;
    }




    // 1. Choose a local basis (same than in testMove()).
    //===========
    // Take the retrieverInstance of startPos as a local basis.
    CVector     origin;
    origin= getInstance(startPos.InstanceId).getOrigin();


    // 2. test collisions with CollisionChains.
    //===========
    CVector     start= startPos.LocalPosition.Estimation;
    // compute end with real delta position.
    CVector     end= start + delta*t;

    // If asked, we must rebuild array of collision chains.
    if(rebuildChains)
    {
//      H_AUTO(PACS_GR_doMove_rebuildChains);

        // compute bboxmove.
        CAABBox     bboxMove;
        // must add some extent, to be sure to include snapped CLocalRetriever vertex (2.0f/256 should be sufficient).
        // Nb: this include the precision problem just below (move a little).
        float   radius= 4.0f/Vector2sAccuracy;
        bboxMove.setCenter(start-CVector(radius, radius, 0));
        bboxMove.extend(start+CVector(radius, radius, 0));
        bboxMove.extend(end-CVector(radius, radius, 0));
        bboxMove.extend(end+CVector(radius, radius, 0));

        // find possible collisions in bboxMove+origin. fill cst.CollisionChains.
        findCollisionChains(cst, bboxMove, origin);
    }


    // look where we arrive.
    CSurfaceIdent   endSurface;
    CVector         endRequest= end;
    const sint      maxPbPrec= 32;  // move away from 4 mm at max, in each 8 direction.
    sint            pbPrecNum= 0;

    // must snap the end position.
    CRetrieverInstance::snapVector(endRequest);
    end= endRequest;

    // verify start is already snapped
    {
        CVector     startTest= start;
        CRetrieverInstance::snapVector(startTest);
        nlassert( start == startTest );
    }


    // Normally, just one iteration is made in this loop (but if precision problem (stopOnEdge, startOnEdge....).
    for(;;)
    {
        // must snap the end position.
        CRetrieverInstance::snapVector(end);

        CVector2f   startCol(start.x, start.y);
        CVector2f   endCol(end.x, end.y);

        // If same 2D position, just return startPos (suppose no movement)
        if(endCol==startCol)
        {
            UGlobalPosition     res;
            res= startPos;
            // keep good z movement.
            res.LocalPosition.Estimation.z= end.z;
            return res;
        }

        // search destination problem.
        UGlobalPosition restart;
        endSurface= testMovementWithCollisionChains(cst, startCol, endCol, startSurface, restart);

        // if no precision problem, Ok, we have found our destination surface (or anormal collide against a wall).
        if (endSurface.SurfaceId >= -1)
        {
            break;
        }
        // left an interior, retrieved position and ask to restart collision from retrieved position
        else if (endSurface.SurfaceId == -3)
        {
            start = getDoubleGlobalPosition(restart) - origin;
            startSurface.RetrieverInstanceId = restart.InstanceId;
            startSurface.SurfaceId = restart.LocalPosition.Surface;
            // should be snapped here
            CVector     startTest= start;
            CRetrieverInstance::snapVector(startTest);
            nlassert( start == startTest );
        }
        /* else we are in deep chit, for one on those reason:
            - traverse on point.
            - stop on a edge (dist==0).
            - start on a edge (dist==0).
            - run // on a edge (NB: dist==0 too).
        */
        else if (endSurface.SurfaceId == -2)
        {
            // For simplicty, just try to move a little the end position
            if(pbPrecNum<maxPbPrec)
            {
                static struct   {sint x,y;}   dirs[8]= { {1,0}, {1,1}, {0,1}, {-1,1}, {-1,0}, {-1,-1}, {0,-1}, {1,-1}};
                sint    dir= pbPrecNum%8;
                sint    dist= pbPrecNum/8+1;
                CVector     dta;

                // compute small move.
                dta.x= dirs[dir].x * dist * 1.0f/SnapPrecision;
                dta.y= dirs[dir].y * dist * 1.0f/SnapPrecision;
                dta.z= 0;

                // add it to the original end pos requested.
                end= endRequest + dta;

                pbPrecNum++;
            }
            else
            {
                // do not move at all.
                endSurface= CSurfaceIdent(-1,-1);
                break;
            }
        }
    }

    // 3. return result.
    //===========
    // Problem?? do not move.
    if(endSurface.SurfaceId==-1)
        return startPos;
    else
    {
        // else must return good GlobalPosition.
        CGlobalPosition     res;

        res.InstanceId= endSurface.RetrieverInstanceId;
        res.LocalPosition.Surface= endSurface.SurfaceId;

        // compute newPos, localy to the endSurface.
        // get delta between startPos.instance and curInstance.
        // NB: for float precision, it is important to compute deltaOrigin, and after compute newPos in local.
        CVector     deltaOrigin;
        deltaOrigin= origin - getInstance(res.InstanceId).getOrigin();

        // Because Origin precision is 1 meter, and end precision is 1/1024 meter, we have no precision problem.
        // this is true because we cannot move more than, say 4*160 meters in one doMove().
        // So global position should not be bigger than 1024 * 1024/1024 meters.  => Hence 20 bits of precision is
        // required. We have 23 with floats.
        res.LocalPosition.Estimation= end + deltaOrigin;


        // result.
        return res;
    }

}


// ***************************************************************************
const NLPACS::TCollisionSurfaceDescVector   &NLPACS::CGlobalRetriever::testBBoxRot(const CGlobalPosition &startPos,
    const NLMISC::CVector &locI, const NLMISC::CVector &locJ, CCollisionSurfaceTemp &cst) const
{
//  H_AUTO(PACS_GR_testBBoxRot);

    CSurfaceIdent   startSurface(startPos.InstanceId, startPos.LocalPosition.Surface);

    // 0. reset.
    //===========
    // reset result.
    cst.CollisionDescs.clear();

    // should not doMove() after a testBBoxRot.
    cst.PrecValid= false;


    // 1. Choose a local basis.
    //===========
    // Take the retrieverInstance of startPos as a local basis.
    CVector     origin;
    origin= getInstance(startPos.InstanceId).getOrigin();


    // 2. compute OBB.
    //===========
    CVector2f   obbStart[4];
    // compute start, relative to the retriever instance.
    CVector     start= startPos.LocalPosition.Estimation;
    CVector2f   obbCenter(start.x, start.y);
    CVector2f   locI2d(locI.x, locI.y);
    CVector2f   locJ2d(locJ.x, locJ.y);

    // build points in CCW.
    obbStart[0]= obbCenter - locI2d - locJ2d;
    obbStart[1]= obbCenter + locI2d - locJ2d;
    obbStart[2]= obbCenter + locI2d + locJ2d;
    obbStart[3]= obbCenter - locI2d + locJ2d;

    // 3. compute bboxmove.
    //===========
    CAABBox     bboxMove;
    // extend the bbox.
    bboxMove.setCenter(CVector(obbStart[0].x, obbStart[0].y, 0));
    bboxMove.extend(CVector(obbStart[1].x, obbStart[1].y, 0));
    bboxMove.extend(CVector(obbStart[2].x, obbStart[2].y, 0));
    bboxMove.extend(CVector(obbStart[3].x, obbStart[3].y, 0));



    // 4. find possible collisions in bboxMove+origin. fill cst.CollisionChains.
    //===========
    findCollisionChains(cst, bboxMove, origin);



    // 5. test Rotcollisions with CollisionChains.
    //===========
    CVector2f   startCol(start.x, start.y);
    testRotCollisionWithCollisionChains(cst, startCol, startSurface, obbStart);


    // result.
    return cst.CollisionDescs;
}


// ***************************************************************************
void    NLPACS::CGlobalRetriever::testRotCollisionWithCollisionChains(CCollisionSurfaceTemp &cst, const CVector2f &/* startCol */, CSurfaceIdent startSurface, const CVector2f bbox[4]) const
{
//  H_AUTO(PACS_GR_testRotCollisionWithCollisionChains);

    // start currentSurface with surface start.
    CSurfaceIdent   currentSurface= startSurface;
    sint            i;

    // reset result.
    cst.RotDescs.clear();
    cst.CollisionDescs.clear();


    /*
        Test collisions with all collision chains. Then, to manage recovery, test the graph of surfaces.
    */
    // run all collisionChain.
    //========================
    for(i=0; i<(sint)cst.CollisionChains.size(); i++)
    {
        CCollisionChain     &colChain= cst.CollisionChains[i];


        // test all edges of this chain, and insert if necessary.
        //========================
        // run list of edge.
        sint32      curEdge= colChain.FirstEdgeCollide;
        while(curEdge!=(sint32)0xFFFFFFFF)
        {
            // get the edge.
            CEdgeCollideNode    &colEdge= cst.getEdgeCollideNode(curEdge);

            // test collision with this edge.
            if(colEdge.testBBoxCollide(bbox))
            {
                // yes we have a 2D collision with this chain.
                cst.RotDescs.push_back(CRotSurfaceDesc(colChain.LeftSurface, colChain.RightSurface));
                break;
            }

            // next edge.
            curEdge= colEdge.Next;
        }
    }


    // Traverse the array of collisions.
    //========================
    sint    indexCD=0;
    for(;;)
    {
        // What surfaces collided do we reach from this currentSurface??
        for(i=0;i<(sint)cst.RotDescs.size();i++)
        {
            // Do we collide with this chain?? chain not tested??
            if(cst.RotDescs[i].hasSurface(currentSurface) && !cst.RotDescs[i].Tested)
            {
                cst.RotDescs[i].Tested= true;

                // insert the collision with the other surface.
                CCollisionSurfaceDesc   col;
                col.ContactTime= 0;
                col.ContactNormal= CVector::Null;
                col.ContactSurface= cst.RotDescs[i].getOtherSurface(currentSurface);
                cst.CollisionDescs.push_back(col);
            }
        }

        // get the next currentSurface from surface collided (traverse the graph of collisions).
        if(indexCD<(sint)cst.CollisionDescs.size())
            currentSurface= cst.CollisionDescs[indexCD++].ContactSurface;
        else
            break;
    }

}

// ***************************************************************************

NLPACS::UGlobalRetriever *NLPACS::UGlobalRetriever::createGlobalRetriever (const char *globalRetriever, const NLPACS::URetrieverBank *retrieverBank)
{

    // Cast
//  nlassert (dynamic_cast<const NLPACS::CRetrieverBank*>(retrieverBank));
    const NLPACS::CRetrieverBank*   bank=static_cast<const NLPACS::CRetrieverBank*>(retrieverBank);

    CIFile  file;
    if (file.open(CPath::lookup(globalRetriever)))
    {
        CGlobalRetriever    *retriever = new CGlobalRetriever();

        // always set the retriever bank before serializing !!
        retriever->setRetrieverBank(bank);

        file.serial(*retriever);
        retriever->initAll(false);  // don't init instances as we serialized them

        return static_cast<UGlobalRetriever *>(retriever);
    }
    else
        return NULL;
}

// ***************************************************************************

void NLPACS::UGlobalRetriever::deleteGlobalRetriever (UGlobalRetriever *retriever)
{
    // Cast
    nlassert (dynamic_cast<NLPACS::CGlobalRetriever*>(retriever));
    NLPACS::CGlobalRetriever* r=static_cast<NLPACS::CGlobalRetriever*>(retriever);

    // Delete
    delete r;
}

// ***************************************************************************

float           NLPACS::CGlobalRetriever::getMeanHeight(const UGlobalPosition &pos) const
{
    // for wrong positions, leave it unchanged
    if ((pos.InstanceId==-1)||(pos.LocalPosition.Surface==-1))
        return pos.LocalPosition.Estimation.z;

    // get instance/localretriever.
    const CRetrieverInstance    &instance = getInstance(pos.InstanceId);
    const CLocalRetriever       &retriever= _RetrieverBank->getRetriever(instance.getRetrieverId());

    if (!retriever.isLoaded())
        return pos.LocalPosition.Estimation.z;

    // return height from local retriever
    return retriever.getHeight(pos.LocalPosition);
}

// ***************************************************************************
float           NLPACS::CGlobalRetriever::getInteriorHeightAround(const UGlobalPosition &pos, float outsideTolerance) const
{
    // for wrong positions, leave it unchanged
    if ((pos.InstanceId==-1)||(pos.LocalPosition.Surface==-1))
        return pos.LocalPosition.Estimation.z;

    // get instance/localretriever.
    const CRetrieverInstance    &instance = getInstance(pos.InstanceId);
    const CLocalRetriever       &retriever= _RetrieverBank->getRetriever(instance.getRetrieverId());

    if (!retriever.isLoaded())
        return pos.LocalPosition.Estimation.z;

    // return height from local retriever
    return retriever.getInteriorHeightAround(pos.LocalPosition, outsideTolerance);
}

// ***************************************************************************

bool NLPACS::CGlobalRetriever::testRaytrace (const CVectorD &/* v0 */, const CVectorD &/* v1 */)
{
    // TODO: implement raytrace
    return false;
}

// ***************************************************************************

void    NLPACS::CGlobalRetriever::refreshLrAround(const CVector &position, float radius)
{
    NLPACS_HAUTO_REFRESH_LR_AROUND

    // check if retriever bank is all loaded, and if yes don't refresh it
    if (_RetrieverBank->allLoaded())
        return;

    std::list<CLrLoader>::iterator ite = _LrLoaderList.begin();
    while (ite != _LrLoaderList.end())
    {
        // Finished loaded a lr, stream it into rbank
        if (ite->Finished && ite->Successful)
        {
            if (!ite->_Buffer.isReading())
                ite->_Buffer.invert();

            ite->_Buffer.resetBufPos();

            //      NLMEMORY::CheckHeap (true);

            const_cast<CRetrieverBank*>(_RetrieverBank)->loadRetriever(ite->LrId, ite->_Buffer);

            //      NLMEMORY::CheckHeap (true);

            ite->_Buffer.clear();

            //      NLMEMORY::CheckHeap (true);

            nlinfo("Lr '%s' loading task complete", ite->LoadFile.c_str());

            // Remove this entry
            _LrLoaderList.erase (ite);

            break;
        }

        // Next lr
        ite++;
    }

    CAABBox box;
    box.setCenter(position);
    box.setHalfSize(CVector(radius, radius, 1000.0f));

    selectInstances(box, _InternalCST);

    set<uint>   newlr, in, out;
    map<uint, CVector>  lrPosition;

    uint    i;
    for (i=0; i<_InternalCST.CollisionInstances.size(); ++i)
    {
        uint lrId = (uint)(_Instances[_InternalCST.CollisionInstances[i]].getRetrieverId());
        newlr.insert(lrId);
        lrPosition.insert (map<uint, CVector>::value_type(lrId, _Instances[_InternalCST.CollisionInstances[i]].getBBox().getCenter()));
    }

    const_cast<CRetrieverBank*>(_RetrieverBank)->diff(newlr, in, out);

    set<uint>::iterator it;

    // unload all possible retrievers
    for (it=out.begin(); it!=out.end(); ++it)
    {
        const_cast<CRetrieverBank*>(_RetrieverBank)->unloadRetriever(*it);
        nlinfo("Freed Lr '%s'", (_RetrieverBank->getNamePrefix() + "_" + toString(*it) + ".lr").c_str());
    }

    // if load task idle and more lr to load, setup load task
    set<uint>::iterator iteIn = in.begin();
    while (iteIn != in.end())
    {
        // Already exist ?
        ite = _LrLoaderList.begin();
        while (ite != _LrLoaderList.end())
        {
            if (ite->LrId == *iteIn)
                break;

            ite++;
        }

        // Not found ?
        if (ite == _LrLoaderList.end())
        {
            // Get the position fot this LR
            map<uint, CVector>::iterator iteLR = lrPosition.find(*iteIn);
            nlassert (iteLR != lrPosition.end());

            _LrLoaderList.push_back (CLrLoader (iteLR->second));
            CLrLoader &loader = _LrLoaderList.back();
            loader.Finished = false;
            loader.LrId = *iteIn;
            loader.LoadFile = _RetrieverBank->getNamePrefix() + "_" + toString(loader.LrId) + ".lr";

            CAsyncFileManager::getInstance().addLoadTask(&loader);

            nlinfo("Lr '%s' added to load", loader.LoadFile.c_str());
        }

        // Next lr to load
        iteIn++;
    }
}

// ***************************************************************************
void    NLPACS::CGlobalRetriever::waitEndOfAsyncLoading()
{
    while (!_LrLoaderList.empty ())
    {
        std::list<CLrLoader>::iterator ite = _LrLoaderList.begin();
        while (ite != _LrLoaderList.end())
        {
            // Finished loaded a lr, stream it into rbank
            if (ite->Finished)
            {
                if (!ite->_Buffer.isReading())
                    ite->_Buffer.invert();

                const_cast<CRetrieverBank*>(_RetrieverBank)->loadRetriever(ite->LrId, ite->_Buffer);

                ite->_Buffer.clear();

                // Remove this from the list
                _LrLoaderList.erase(ite);

                break;
            }

            //
            ite++;
        }

        if (!_LrLoaderList.empty())
            nlSleep(0);
    }

}

// ***************************************************************************
void    NLPACS::CGlobalRetriever::refreshLrAroundNow(const CVector &position, float radius)
{
    // check if retriever bank is all loaded, and if yes don't refresh it
    if (_RetrieverBank->allLoaded())
        return;

    // must wait all current have finished loading
    waitEndOfAsyncLoading();

    // Select new to load
    CAABBox box;
    box.setCenter(position);
    box.setHalfSize(CVector(radius, radius, 1000.0f));

    selectInstances(box, _InternalCST);

    set<uint>   newlr, in, out;
    uint    i;
    for (i=0; i<_InternalCST.CollisionInstances.size(); ++i)
        newlr.insert((uint)(_Instances[_InternalCST.CollisionInstances[i]].getRetrieverId()));

    const_cast<CRetrieverBank*>(_RetrieverBank)->diff(newlr, in, out);

    set<uint>::iterator it;

    // unload all possible retrievers
    for (it=out.begin(); it!=out.end(); ++it)
        const_cast<CRetrieverBank*>(_RetrieverBank)->unloadRetriever(*it);

    // unload all possible retrievers
    for (it=in.begin(); it!=in.end(); ++it)
    {
        string      fname = _RetrieverBank->getNamePrefix() + "_" + toString(*it) + ".lr";
        CIFile      f;
        if (!f.open(CPath::lookup(fname, false)))
        {
            nlwarning("Couldn't find file '%s' to load, retriever loading aborted", fname.c_str());
            continue;
        }

        const_cast<CRetrieverBank*>(_RetrieverBank)->loadRetriever(*it, f);
    }
}

void    NLPACS::CGlobalRetriever::CLrLoader::run()
{
    CIFile      f;

    // async
    f.setAsyncLoading(true);
    f.setCacheFileOnOpen(true);

    Successful = false;

    if (!f.open(CPath::lookup(LoadFile, false)))
    {
        nlwarning("Couldn't find file '%s' to load, retriever loading aborted", LoadFile.c_str());
        _Buffer.clear();
        Finished = true;
        return;
    }

    if (!_Buffer.isReading())
        _Buffer.invert();

    uint8   *buffer = _Buffer.bufferToFill(f.getFileSize());
    f.serialBuffer(buffer, f.getFileSize());

    Successful = true;
    Finished = true;
}

// ***************************************************************************
void    NLPACS::CGlobalRetriever::CLrLoader::getName (std::string &result) const
{
    result = "LoadLR(" + LoadFile + ")";
}


//
NLMISC_CATEGORISED_VARIABLE(nel, uint, PacsRetrieveVerbose, "Allow retrieve position to dump info");

// end of CGlobalRetriever methods implementation