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