Khaganat-NeL/khanat-opennel-code
2
0
Fork 0
Code Issues 21 Pull requests Projects Releases 1 Packages Wiki Activity
khanat-opennel-code/code/nel/src/pacs/global_retriever.cpp

2713 lines
81 KiB
C++
Raw Blame History

// NeL - MMORPG Framework <http://dev.ryzom.com/projects/nel/>
// Copyright (C) 2010 Winch Gate Property Limited
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU Affero General Public License as
// published by the Free Software Foundation, either version 3 of the
// License, or (at your option) any later version.
//
// This program 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 Affero General Public License for more details.
//
// You should have received a copy of the GNU Affero General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "stdpacs.h"
#include "nel/pacs/global_retriever.h"
#include "nel/pacs/retriever_bank.h"
#include "nel/misc/async_file_manager.h"
#include "nel/misc/common.h"
#include "nel/misc/hierarchical_timer.h"
#include "nel/misc/line.h"
#include "nel/misc/path.h"
#include "nel/misc/time_nl.h"
#include "nel/misc/variable.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)
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 (const 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[(uint)ochain.getVertices().size()/2].unpack3f();
}
else
{
surf.End.Estimation = (ochain[(uint)ochain.getVertices().size()/2].unpack3f()+
ochain[(uint)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= (sint)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= (sint)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 another 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];
/// TODO Tests Ben
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(0.0f, 0.0f);
// 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;
}
/// TODO Tests Ben
nlassert(collidedSurface.RetrieverInstanceId < (sint)_Instances.size());
// insert or replace this collision in collisionDescs.
// NB: yes this looks like a N algorithm (so N^2). But not so many collisions may arise, so don't bother.
sint indexInsert= (sint)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
Reference in a new issue View git blame Copy permalink