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