bmesh docs now written in sphinx doc generator.
[blender.git] / extern / recastnavigation / Detour / Source / DetourStatNavMesh.cpp
1 //
2 // Copyright (c) 2009 Mikko Mononen memon@inside.org
3 //
4 // This software is provided 'as-is', without any express or implied
5 // warranty.  In no event will the authors be held liable for any damages
6 // arising from the use of this software.
7 // Permission is granted to anyone to use this software for any purpose,
8 // including commercial applications, and to alter it and redistribute it
9 // freely, subject to the following restrictions:
10 // 1. The origin of this software must not be misrepresented; you must not
11 //    claim that you wrote the original software. If you use this software
12 //    in a product, an acknowledgment in the product documentation would be
13 //    appreciated but is not required.
14 // 2. Altered source versions must be plainly marked as such, and must not be
15 //    misrepresented as being the original software.
16 // 3. This notice may not be removed or altered from any source distribution.
17 //
18
19 #include <math.h>
20 #include <float.h>
21 #include <string.h>
22 #include <stdio.h>
23 #include "DetourStatNavMesh.h"
24 #include "DetourNode.h"
25 #include "DetourCommon.h"
26
27
28 //////////////////////////////////////////////////////////////////////////////////////////
29 dtStatNavMesh::dtStatNavMesh() :
30         m_data(0),
31         m_dataSize(0),
32         m_header(0),
33         m_nodePool(0),
34         m_openList(0)
35 {
36 }
37
38 dtStatNavMesh::~dtStatNavMesh()
39 {
40         delete m_nodePool;
41         delete m_openList;
42         if (m_data)
43                 delete [] m_data;
44 }
45
46 bool dtStatNavMesh::init(unsigned char* data, int dataSize, bool ownsData)
47 {
48         dtStatNavMeshHeader* header = (dtStatNavMeshHeader*)data;
49         
50         if (header->magic != DT_STAT_NAVMESH_MAGIC)
51                 return false;
52         if (header->version != DT_STAT_NAVMESH_VERSION)
53                 return false;
54
55         const int headerSize = sizeof(dtStatNavMeshHeader);
56         const int vertsSize = sizeof(float)*3*header->nverts;
57         const int polysSize = sizeof(dtStatPoly)*header->npolys;
58         const int nodesSize = sizeof(dtStatBVNode)*header->npolys*2;
59         const int detailMeshesSize = sizeof(dtStatPolyDetail)*header->ndmeshes;
60         const int detailVertsSize = sizeof(float)*3*header->ndverts;
61         const int detailTrisSize = sizeof(unsigned char)*4*header->ndtris;
62         
63
64         unsigned char* d = data + headerSize;
65         header->verts = (float*)d; d += vertsSize;
66         header->polys = (dtStatPoly*)d; d += polysSize;
67         header->bvtree = (dtStatBVNode*)d; d += nodesSize;
68         header->dmeshes = (dtStatPolyDetail*)d; d += detailMeshesSize;
69         header->dverts = (float*)d; d += detailVertsSize;
70         header->dtris = (unsigned char*)d; d += detailTrisSize;
71         
72         m_nodePool = new dtNodePool(2048, 256);
73         if (!m_nodePool)
74                 return false;
75                 
76         m_openList = new dtNodeQueue(2048);
77         if (!m_openList)
78                 return false;
79         
80         if (ownsData)
81         {
82                 m_data = data;
83                 m_dataSize = dataSize;
84         }
85
86         m_header = header;
87
88         return true;
89 }
90
91 const dtStatPoly* dtStatNavMesh::getPolyByRef(dtStatPolyRef ref) const
92 {
93         if (!m_header || ref == 0 || (int)ref > m_header->npolys) return 0;
94         return &m_header->polys[ref-1];
95 }
96
97 int dtStatNavMesh::getPolyIndexByRef(dtStatPolyRef ref) const
98 {
99         if (!m_header || ref == 0 || (int)ref > m_header->npolys) return -1;
100         return (int)ref-1;
101 }
102
103 int dtStatNavMesh::findPath(dtStatPolyRef startRef, dtStatPolyRef endRef,
104                                                         const float* startPos, const float* endPos,
105                                                         dtStatPolyRef* path, const int maxPathSize)
106 {
107         if (!m_header) return 0;
108         
109         if (!startRef || !endRef)
110                 return 0;
111
112         if (!maxPathSize)
113                 return 0;
114
115         if (startRef == endRef)
116         {
117                 path[0] = startRef;
118                 return 1;
119         }
120
121         m_nodePool->clear();
122         m_openList->clear();
123
124         static const float H_SCALE = 1.1f;      // Heuristic scale.
125         
126         dtNode* startNode = m_nodePool->getNode(startRef);
127         startNode->pidx = 0;
128         startNode->cost = 0;
129         startNode->total = vdist(startPos, endPos) * H_SCALE;
130         startNode->id = startRef;
131         startNode->flags = DT_NODE_OPEN;
132         m_openList->push(startNode);
133
134         dtNode* lastBestNode = startNode;
135         float lastBestNodeCost = startNode->total;
136         while (!m_openList->empty())
137         {
138                 dtNode* bestNode = m_openList->pop();
139         
140                 if (bestNode->id == endRef)
141                 {
142                         lastBestNode = bestNode;
143                         break;
144                 }
145
146                 const dtStatPoly* poly = getPoly(bestNode->id-1);
147                 for (int i = 0; i < (int)poly->nv; ++i)
148                 {
149                         dtStatPolyRef neighbour = poly->n[i];
150                         if (neighbour)
151                         {
152                                 // Skip parent node.
153                                 if (bestNode->pidx && m_nodePool->getNodeAtIdx(bestNode->pidx)->id == neighbour)
154                                         continue;
155
156                                 dtNode* parent = bestNode;
157                                 dtNode newNode;
158                                 newNode.pidx = m_nodePool->getNodeIdx(parent);
159                                 newNode.id = neighbour;
160
161                                 // Calculate cost.
162                                 float p0[3], p1[3];
163                                 if (!parent->pidx)
164                                         vcopy(p0, startPos);
165                                 else
166                                         getEdgeMidPoint(m_nodePool->getNodeAtIdx(parent->pidx)->id, parent->id, p0);
167                                 getEdgeMidPoint(parent->id, newNode.id, p1);
168                                 newNode.cost = parent->cost + vdist(p0,p1);
169                                 // Special case for last node.
170                                 if (newNode.id == endRef)
171                                         newNode.cost += vdist(p1, endPos);
172                                 
173                                 // Heuristic
174                                 const float h = vdist(p1,endPos)*H_SCALE;
175                                 newNode.total = newNode.cost + h;
176                                 
177                                 dtNode* actualNode = m_nodePool->getNode(newNode.id);
178                                 if (!actualNode)
179                                         continue;
180                                                 
181                                 if (!((actualNode->flags & DT_NODE_OPEN) && newNode.total > actualNode->total) &&
182                                         !((actualNode->flags & DT_NODE_CLOSED) && newNode.total > actualNode->total))
183                                 {
184                                         actualNode->flags &= ~DT_NODE_CLOSED;
185                                         actualNode->pidx = newNode.pidx;
186                                         actualNode->cost = newNode.cost;
187                                         actualNode->total = newNode.total;
188
189                                         if (h < lastBestNodeCost)
190                                         {
191                                                 lastBestNodeCost = h;
192                                                 lastBestNode = actualNode;
193                                         }
194
195                                         if (actualNode->flags & DT_NODE_OPEN)
196                                         {
197                                                 m_openList->modify(actualNode);
198                                         }
199                                         else
200                                         {
201                                                 actualNode->flags |= DT_NODE_OPEN;
202                                                 m_openList->push(actualNode);
203                                         }
204                                 }
205                         }
206                 }
207                 bestNode->flags |= DT_NODE_CLOSED;
208         }
209
210         // Reverse the path.
211         dtNode* prev = 0;
212         dtNode* node = lastBestNode;
213         do
214         {
215                 dtNode* next = m_nodePool->getNodeAtIdx(node->pidx);
216                 node->pidx = m_nodePool->getNodeIdx(prev);
217                 prev = node;
218                 node = next;
219         }
220         while (node);
221         
222         // Store path
223         node = prev;
224         int n = 0;
225         do
226         {
227                 path[n++] = node->id;
228                 node = m_nodePool->getNodeAtIdx(node->pidx);
229         }
230         while (node && n < maxPathSize);
231
232         return n;
233 }
234
235 bool dtStatNavMesh::closestPointToPoly(dtStatPolyRef ref, const float* pos, float* closest) const
236 {
237         int idx = getPolyIndexByRef(ref);
238         if (idx == -1)
239                 return false;
240
241         float closestDistSqr = FLT_MAX;
242         const dtStatPoly* p = getPoly(idx);
243         const dtStatPolyDetail* pd = getPolyDetail(idx);
244
245         for (int j = 0; j < pd->ntris; ++j)
246         {
247                 const unsigned char* t = getDetailTri(pd->tbase+j);
248                 const float* v[3];
249                 for (int k = 0; k < 3; ++k)
250                 {
251                         if (t[k] < p->nv)
252                                 v[k] = getVertex(p->v[t[k]]);
253                         else
254                                 v[k] = getDetailVertex(pd->vbase+(t[k]-p->nv));
255                 }
256                 float pt[3];
257                 closestPtPointTriangle(pt, pos, v[0], v[1], v[2]);
258                 float d = vdistSqr(pos, pt);
259                 if (d < closestDistSqr)
260                 {
261                         vcopy(closest, pt);
262                         closestDistSqr = d;
263                 }
264         }
265         
266         return true;
267 }
268
269 bool dtStatNavMesh::getPolyHeight(dtStatPolyRef ref, const float* pos, float* height) const
270 {
271         int idx = getPolyIndexByRef(ref);
272         if (idx == -1)
273                 return false;
274         
275         const dtStatPoly* p = getPoly(idx);
276         const dtStatPolyDetail* pd = getPolyDetail(idx);
277
278         for (int i = 0; i < pd->ntris; ++i)
279         {
280                 const unsigned char* t = getDetailTri(pd->tbase+i);
281                 const float* v[3];
282                 for (int j = 0; j < 3; ++j)
283                 {
284                         if (t[j] < p->nv)
285                                 v[j] = getVertex(p->v[t[j]]);
286                         else
287                                 v[j] = getDetailVertex(pd->vbase+(t[j]-p->nv));
288                 }
289                 float h;
290                 if (closestHeightPointTriangle(pos, v[0], v[1], v[2], h))
291                 {
292                         if (height)
293                                 *height = h;
294                         return true;
295                 }
296         }
297         
298         return false;
299 }
300
301 int dtStatNavMesh::findStraightPath(const float* startPos, const float* endPos,
302                                                                         const dtStatPolyRef* path, const int pathSize,
303                                                                         float* straightPath, const int maxStraightPathSize)
304 {
305         if (!m_header) return 0;
306         
307         if (!maxStraightPathSize)
308                 return 0;
309
310         if (!path[0])
311                 return 0;
312
313         int straightPathSize = 0;
314         
315         float closestStartPos[3];
316         if (!closestPointToPoly(path[0], startPos, closestStartPos))
317                 return 0;
318
319         // Add start point.
320         vcopy(&straightPath[straightPathSize*3], closestStartPos);
321         straightPathSize++;
322         if (straightPathSize >= maxStraightPathSize)
323                 return straightPathSize;
324
325         float closestEndPos[3];
326         if (!closestPointToPoly(path[pathSize-1], endPos, closestEndPos))
327                 return 0;
328
329         float portalApex[3], portalLeft[3], portalRight[3];
330
331         if (pathSize > 1)
332         {
333                 vcopy(portalApex, closestStartPos);
334                 vcopy(portalLeft, portalApex);
335                 vcopy(portalRight, portalApex);
336                 int apexIndex = 0;
337                 int leftIndex = 0;
338                 int rightIndex = 0;
339
340                 for (int i = 0; i < pathSize; ++i)
341                 {
342                         float left[3], right[3];
343                         if (i < pathSize-1)
344                         {
345                                 // Next portal.
346                                 getPortalPoints(path[i], path[i+1], left, right);
347                         }
348                         else
349                         {
350                                 // End of the path.
351                                 vcopy(left, closestEndPos);
352                                 vcopy(right, closestEndPos);
353                         }
354
355                         // Right vertex.
356                         if (vequal(portalApex, portalRight))
357                         {
358                                 vcopy(portalRight, right);
359                                 rightIndex = i;
360                         }
361                         else
362                         {
363                                 if (triArea2D(portalApex, portalRight, right) <= 0.0f)
364                                 {
365                                         if (triArea2D(portalApex, portalLeft, right) > 0.0f)
366                                         {
367                                                 vcopy(portalRight, right);
368                                                 rightIndex = i;
369                                         }
370                                         else
371                                         {
372                                                 vcopy(portalApex, portalLeft);
373                                                 apexIndex = leftIndex;
374
375                                                 if (!vequal(&straightPath[(straightPathSize-1)*3], portalApex))
376                                                 {
377                                                         vcopy(&straightPath[straightPathSize*3], portalApex);
378                                                         straightPathSize++;
379                                                         if (straightPathSize >= maxStraightPathSize)
380                                                                 return straightPathSize;
381                                                 }
382
383                                                 vcopy(portalLeft, portalApex);
384                                                 vcopy(portalRight, portalApex);
385                                                 leftIndex = apexIndex;
386                                                 rightIndex = apexIndex;
387
388                                                 // Restart
389                                                 i = apexIndex;
390
391                                                 continue;
392                                         }
393                                 }
394                         }
395
396                         // Left vertex.
397                         if (vequal(portalApex, portalLeft))
398                         {
399                                 vcopy(portalLeft, left);
400                                 leftIndex = i;
401                         }
402                         else
403                         {
404                                 if (triArea2D(portalApex, portalLeft, left) >= 0.0f)
405                                 {
406                                         if (triArea2D(portalApex, portalRight, left) < 0.0f)
407                                         {
408                                                 vcopy(portalLeft, left);
409                                                 leftIndex = i;
410                                         }
411                                         else
412                                         {
413                                                 vcopy(portalApex, portalRight);
414                                                 apexIndex = rightIndex;
415
416                                                 if (!vequal(&straightPath[(straightPathSize-1)*3], portalApex))
417                                                 {
418                                                         vcopy(&straightPath[straightPathSize*3], portalApex);
419                                                         straightPathSize++;
420                                                         if (straightPathSize >= maxStraightPathSize)
421                                                                 return straightPathSize;
422                                                 }
423
424                                                 vcopy(portalLeft, portalApex);
425                                                 vcopy(portalRight, portalApex);
426                                                 leftIndex = apexIndex;
427                                                 rightIndex = apexIndex;
428
429                                                 // Restart
430                                                 i = apexIndex;
431
432                                                 continue;
433                                         }
434                                 }
435                         }
436                 }
437         }
438
439         // Add end point.
440         vcopy(&straightPath[straightPathSize*3], closestEndPos);
441         straightPathSize++;
442         
443         return straightPathSize;
444 }
445
446 int dtStatNavMesh::getPolyVerts(dtStatPolyRef ref, float* verts) const
447 {
448         if (!m_header) return 0;
449         const dtStatPoly* poly = getPolyByRef(ref);
450         if (!poly) return 0;
451         float* v = verts;
452         for (int i = 0; i < (int)poly->nv; ++i)
453         {
454                 const float* cv = &m_header->verts[poly->v[i]*3];
455                 *v++ = cv[0];
456                 *v++ = cv[1];
457                 *v++ = cv[2];
458         }
459         return (int)poly->nv;
460 }
461
462 int dtStatNavMesh::raycast(dtStatPolyRef centerRef, const float* startPos, const float* endPos,
463                                           float& t, dtStatPolyRef* path, const int pathSize)
464 {
465         if (!m_header) return 0;
466         if (!centerRef) return 0;
467         
468         dtStatPolyRef prevRef = centerRef;
469         dtStatPolyRef curRef = centerRef;
470         t = 0;
471
472         float verts[DT_STAT_VERTS_PER_POLYGON*3];
473         int n = 0;
474
475         while (curRef)
476         {
477                 // Cast ray against current polygon.
478                 int nv = getPolyVerts(curRef, verts);
479                 if (nv < 3)
480                 {
481                         // Hit bad polygon, report hit.
482                         return n;
483                 }
484                 
485                 float tmin, tmax;
486                 int segMin, segMax;
487                 if (!intersectSegmentPoly2D(startPos, endPos, verts, nv, tmin, tmax, segMin, segMax))
488                 {
489                         // Could not a polygon, keep the old t and report hit.
490                         return n;
491                 }
492                 // Keep track of furthest t so far.
493                 if (tmax > t)
494                         t = tmax;
495
496                 if (n < pathSize)
497                         path[n++] = curRef;
498
499                 // Check the neighbour of this polygon.
500                 const dtStatPoly* poly = getPolyByRef(curRef);
501                 dtStatPolyRef nextRef = poly->n[segMax];
502                 if (!nextRef)
503                 {
504                         // No neighbour, we hit a wall.
505                         return n;
506                 }
507                 
508                 // No hit, advance to neighbour polygon.
509                 prevRef = curRef;
510                 curRef = nextRef;
511         }
512         
513         return n;
514 }
515
516
517 float dtStatNavMesh::findDistanceToWall(dtStatPolyRef centerRef, const float* centerPos, float maxRadius,
518                                                                   float* hitPos, float* hitNormal)
519 {
520         if (!m_header) return 0;
521         if (!centerRef) return 0;
522         
523         m_nodePool->clear();
524         m_openList->clear();
525         
526         dtNode* startNode = m_nodePool->getNode(centerRef);
527         startNode->pidx = 0;
528         startNode->cost = 0;
529         startNode->total = 0;
530         startNode->id = centerRef;
531         startNode->flags = DT_NODE_OPEN;
532         m_openList->push(startNode);
533         
534         float radiusSqr = sqr(maxRadius);
535         
536         hitNormal[0] = 1;
537         hitNormal[1] = 0;
538         hitNormal[2] = 0;
539         
540         while (!m_openList->empty())
541         {
542                 dtNode* bestNode = m_openList->pop();
543                 const dtStatPoly* poly = getPoly(bestNode->id-1);
544                 
545                 // Hit test walls.
546                 for (int i = 0, j = (int)poly->nv-1; i < (int)poly->nv; j = i++)
547                 {
548                         // Skip non-solid edges.
549                         if (poly->n[j]) continue;
550                         
551                         // Calc distance to the edge.
552                         const float* vj = getVertex(poly->v[j]);
553                         const float* vi = getVertex(poly->v[i]);
554                         float tseg;
555                         float distSqr = distancePtSegSqr2D(centerPos, vj, vi, tseg);
556
557                         // Edge is too far, skip.
558                         if (distSqr > radiusSqr)
559                                 continue;
560                                 
561                         // Hit wall, update radius.
562                         radiusSqr = distSqr;
563                         // Calculate hit pos.
564                         hitPos[0] = vj[0] + (vi[0] - vj[0])*tseg;
565                         hitPos[1] = vj[1] + (vi[1] - vj[1])*tseg;
566                         hitPos[2] = vj[2] + (vi[2] - vj[2])*tseg;
567                 }
568
569                 // Check to see if the circle expands to one of the neighbours and expand.
570                 for (int i = 0, j = (int)poly->nv-1; i < (int)poly->nv; j = i++)
571                 {
572                         // Skip solid edges.
573                         if (!poly->n[j]) continue;
574                         
575                         // Expand to neighbour if not visited yet.
576                         dtStatPolyRef neighbour = poly->n[j];
577                         
578                         // Skip parent node.
579                         if (bestNode->pidx && m_nodePool->getNodeAtIdx(bestNode->pidx)->id == neighbour)
580                                 continue;
581                         
582                         // Calc distance to the edge.
583                         const float* vj = getVertex(poly->v[j]);
584                         const float* vi = getVertex(poly->v[i]);
585                         float tseg;
586                         float distSqr = distancePtSegSqr2D(centerPos, vj, vi, tseg);
587                         
588                         // Edge is too far, skip.
589                         if (distSqr > radiusSqr)
590                                 continue;
591                         
592                         dtNode* parent = bestNode;
593                         dtNode newNode;
594                         newNode.pidx = m_nodePool->getNodeIdx(parent);
595                         newNode.id = neighbour;
596                         
597                         // Cost
598                         float p0[3], p1[3];
599                         if (!parent->pidx)
600                                 vcopy(p0, centerPos);
601                         else
602                                 getEdgeMidPoint(m_nodePool->getNodeAtIdx(parent->pidx)->id, parent->id, p0);
603                         getEdgeMidPoint(parent->id, newNode.id, p1);
604                         newNode.total = parent->total + vdist(p0,p1);
605                         
606                         dtNode* actualNode = m_nodePool->getNode(newNode.id);
607                         if (!actualNode)
608                                 continue;
609
610                         if (!((actualNode->flags & DT_NODE_OPEN) && newNode.total > actualNode->total) &&
611                                 !((actualNode->flags & DT_NODE_CLOSED) && newNode.total > actualNode->total))
612                         {
613                                 actualNode->flags &= ~DT_NODE_CLOSED;
614                                 actualNode->pidx = newNode.pidx;
615                                 actualNode->total = newNode.total;
616                                 
617                                 if (actualNode->flags & DT_NODE_OPEN)
618                                 {
619                                         m_openList->modify(actualNode);
620                                 }
621                                 else
622                                 {
623                                         actualNode->flags |= DT_NODE_OPEN;
624                                         m_openList->push(actualNode);
625                                 }
626                         }
627                 }
628                 bestNode->flags |= DT_NODE_CLOSED;
629         }
630
631         // Calc hit normal.
632         vsub(hitNormal, centerPos, hitPos);
633         vnormalize(hitNormal);
634         
635         return sqrtf(radiusSqr);
636 }
637
638 int dtStatNavMesh::findPolysAround(dtStatPolyRef centerRef, const float* centerPos, float radius,
639                                                                    dtStatPolyRef* resultRef, dtStatPolyRef* resultParent, float* resultCost,
640                                                                    const int maxResult)
641 {
642         if (!m_header) return 0;
643         if (!centerRef) return 0;
644
645         m_nodePool->clear();
646         m_openList->clear();
647
648         dtNode* startNode = m_nodePool->getNode(centerRef);
649         startNode->pidx = 0;
650         startNode->cost = 0;
651         startNode->total = 0;
652         startNode->id = centerRef;
653         startNode->flags = DT_NODE_OPEN;
654         m_openList->push(startNode);
655
656         int n = 0;
657         if (n < maxResult)
658         {
659                 if (resultRef)
660                         resultRef[n] = startNode->id;
661                 if (resultParent)
662                         resultParent[n] = 0;
663                 if (resultCost)
664                         resultCost[n] = 0;
665                 ++n;
666         }
667
668         const float radiusSqr = sqr(radius);
669
670         while (!m_openList->empty())
671         {
672                 dtNode* bestNode = m_openList->pop();
673                 const dtStatPoly* poly = getPoly(bestNode->id-1);
674                 for (unsigned i = 0, j = (int)poly->nv-1; i < (int)poly->nv; j=i++)
675                 {
676                         dtStatPolyRef neighbour = poly->n[j];
677
678                         if (neighbour)
679                         {
680                                 // Skip parent node.
681                                 if (bestNode->pidx && m_nodePool->getNodeAtIdx(bestNode->pidx)->id == neighbour)
682                                         continue;
683                                         
684                                 // Calc distance to the edge.
685                                 const float* vj = getVertex(poly->v[j]);
686                                 const float* vi = getVertex(poly->v[i]);
687                                 float tseg;
688                                 float distSqr = distancePtSegSqr2D(centerPos, vj, vi, tseg);
689                                 
690                                 // If the circle is not touching the next polygon, skip it.
691                                 if (distSqr > radiusSqr)
692                                         continue;
693                                 
694                                 dtNode* parent = bestNode;
695                                 dtNode newNode;
696                                 newNode.pidx = m_nodePool->getNodeIdx(parent);
697                                 newNode.id = neighbour;
698
699                                 // Cost
700                                 float p0[3], p1[3];
701                                 if (!parent->pidx)
702                                         vcopy(p0, centerPos);
703                                 else
704                                         getEdgeMidPoint(m_nodePool->getNodeAtIdx(parent->pidx)->id, parent->id, p0);
705                                 getEdgeMidPoint(parent->id, newNode.id, p1);
706                                 newNode.total = parent->total + vdist(p0,p1);
707                                 
708                                 dtNode* actualNode = m_nodePool->getNode(newNode.id);
709                                 if (!actualNode)
710                                         continue;
711
712                                 if (!((actualNode->flags & DT_NODE_OPEN) && newNode.total > actualNode->total) &&
713                                         !((actualNode->flags & DT_NODE_CLOSED) && newNode.total > actualNode->total))
714                                 {
715                                         actualNode->flags &= ~DT_NODE_CLOSED;
716                                         actualNode->pidx = newNode.pidx;
717                                         actualNode->total = newNode.total;
718
719                                         if (actualNode->flags & DT_NODE_OPEN)
720                                         {
721                                                 m_openList->modify(actualNode);
722                                         }
723                                         else
724                                         {
725                                                 if (n < maxResult)
726                                                 {
727                                                         if (resultRef)
728                                                                 resultRef[n] = actualNode->id;
729                                                         if (resultParent)
730                                                                 resultParent[n] = m_nodePool->getNodeAtIdx(actualNode->pidx)->id;
731                                                         if (resultCost)
732                                                                 resultCost[n] = actualNode->total;
733                                                         ++n;
734                                                 }
735                                                 actualNode->flags |= DT_NODE_OPEN;
736                                                 m_openList->push(actualNode);
737                                         }
738                                 }
739                         }
740                 }
741                 bestNode->flags |= DT_NODE_CLOSED;
742                 
743         }
744
745         return n;
746 }
747
748 // Returns polygons which are withing certain radius from the query location.
749 int dtStatNavMesh::queryPolygons(const float* center, const float* extents,
750                                                                  dtStatPolyRef* polys, const int maxIds)
751 {
752         if (!m_header) return 0;
753         
754         const dtStatBVNode* node = &m_header->bvtree[0];
755         const dtStatBVNode* end = &m_header->bvtree[m_header->nnodes];
756
757         // Calculate quantized box
758         const float ics = 1.0f / m_header->cs;
759         unsigned short bmin[3], bmax[3];
760         // Clamp query box to world box.
761         float minx = clamp(center[0] - extents[0], m_header->bmin[0], m_header->bmax[0]) - m_header->bmin[0];
762         float miny = clamp(center[1] - extents[1], m_header->bmin[1], m_header->bmax[1]) - m_header->bmin[1];
763         float minz = clamp(center[2] - extents[2], m_header->bmin[2], m_header->bmax[2]) - m_header->bmin[2];
764         float maxx = clamp(center[0] + extents[0], m_header->bmin[0], m_header->bmax[0]) - m_header->bmin[0];
765         float maxy = clamp(center[1] + extents[1], m_header->bmin[1], m_header->bmax[1]) - m_header->bmin[1];
766         float maxz = clamp(center[2] + extents[2], m_header->bmin[2], m_header->bmax[2]) - m_header->bmin[2];
767         // Quantize
768         bmin[0] = (unsigned short)(ics * minx) & 0xfffe;
769         bmin[1] = (unsigned short)(ics * miny) & 0xfffe;
770         bmin[2] = (unsigned short)(ics * minz) & 0xfffe;
771         bmax[0] = (unsigned short)(ics * maxx + 1) | 1;
772         bmax[1] = (unsigned short)(ics * maxy + 1) | 1;
773         bmax[2] = (unsigned short)(ics * maxz + 1) | 1;
774         
775         // Traverse tree
776         int n = 0;
777         while (node < end)
778         {
779                 bool overlap = checkOverlapBox(bmin, bmax, node->bmin, node->bmax);
780                 bool isLeafNode = node->i >= 0;
781                 
782                 if (isLeafNode && overlap)
783                 {
784                         if (n < maxIds)
785                         {
786                                 polys[n] = (dtStatPolyRef)node->i;
787                                 n++;
788                         }
789                 }
790                 
791                 if (overlap || isLeafNode)
792                         node++;
793                 else
794                 {
795                         const int escapeIndex = -node->i;
796                         node += escapeIndex;
797                 }
798         }
799         
800         return n;
801 }
802
803 dtStatPolyRef dtStatNavMesh::findNearestPoly(const float* center, const float* extents)
804 {
805         if (!m_header) return 0;
806         
807         // Get nearby polygons from proximity grid.
808         dtStatPolyRef polys[128];
809         int npolys = queryPolygons(center, extents, polys, 128);
810
811         // Find nearest polygon amongst the nearby polygons.
812         dtStatPolyRef nearest = 0;
813         float nearestDistanceSqr = FLT_MAX;
814         for (int i = 0; i < npolys; ++i)
815         {
816                 dtStatPolyRef ref = polys[i];
817                 float closest[3];
818                 if (!closestPointToPoly(ref, center, closest))
819                         continue;
820                 float d = vdistSqr(center, closest);
821                 if (d < nearestDistanceSqr)
822                 {
823                         nearestDistanceSqr = d;
824                         nearest = ref;
825                 }
826         }
827
828         return nearest;
829 }
830
831 bool dtStatNavMesh::getPortalPoints(dtStatPolyRef from, dtStatPolyRef to, float* left, float* right) const
832 {
833         const dtStatPoly* fromPoly = getPolyByRef(from);
834         if (!fromPoly)
835                 return false;
836
837         // Find common edge between the polygons and returns the segment end points.
838         for (unsigned i = 0, j = (int)fromPoly->nv - 1; i < (int)fromPoly->nv; j = i++)
839         {
840                 unsigned short neighbour = fromPoly->n[j];
841                 if (neighbour == to)
842                 {
843                         vcopy(left, getVertex(fromPoly->v[j]));
844                         vcopy(right, getVertex(fromPoly->v[i]));
845                         return true;
846                 }
847         }
848
849         return false;
850 }
851
852 bool dtStatNavMesh::getEdgeMidPoint(dtStatPolyRef from, dtStatPolyRef to, float* mid) const
853 {
854         float left[3], right[3];
855         if (!getPortalPoints(from, to, left,right)) return false;
856         mid[0] = (left[0]+right[0])*0.5f;
857         mid[1] = (left[1]+right[1])*0.5f;
858         mid[2] = (left[2]+right[2])*0.5f;
859         return true;
860 }
861
862 bool dtStatNavMesh::isInClosedList(dtStatPolyRef ref) const
863 {
864         if (!m_nodePool) return false;
865         const dtNode* node = m_nodePool->findNode(ref);
866         return node && node->flags & DT_NODE_CLOSED;
867 }
868
869 int dtStatNavMesh::getMemUsed() const
870 {
871         if (!m_nodePool || ! m_openList)
872                 return 0;
873         return sizeof(*this) + m_dataSize +
874                         m_nodePool->getMemUsed() +
875                         m_openList->getMemUsed();
876 }