OregonCore  revision fb2a440-git
Your Favourite TBC server
PathFinder.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of the OregonCore Project. See AUTHORS file for Copyright information
3  *
4  * This program is free software; you can redistribute it and/or modify it
5  * under the terms of the GNU General Public License as published by the
6  * Free Software Foundation; either version 2 of the License, or (at your
7  * option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful, but WITHOUT
10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
11  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
12  * more details.
13  *
14  * You should have received a copy of the GNU General Public License along
15  * with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
18 #include "MoveMap.h"
19 #include "Map.h"
20 #include "Creature.h"
21 #include "PathFinder.h"
22 #include "Log.h"
23 
24 #include "DetourCommon.h"
25 
27 PathInfo::PathInfo(const Unit* owner) :
28  m_polyLength(0), m_type(PATHFIND_BLANK),
29  m_useStraightPath(true), m_forceDestination(false), m_pointPathLimit(MAX_POINT_PATH_LENGTH),
30  m_sourceUnit(owner), m_navMesh(NULL), m_navMeshQuery(NULL)
31 {
32  //DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ PathInfo::PathInfo for %u \n", m_sourceUnit->GetGUIDLow());
33 
34  uint32 mapId = m_sourceUnit->GetMapId();
36  {
38  m_navMesh = mmap->GetNavMesh(mapId);
40  }
41 
42  createFilter();
43 }
44 
46 {
47  //DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ PathInfo::~PathInfo() for %u \n", m_sourceUnit->GetGUIDLow());
48 }
49 
50 bool PathInfo::Update(float destX, float destY, float destZ, bool forceDest)
51 {
52  float x, y, z;
53  m_sourceUnit->GetPosition(x, y, z);
54 
55  if (!Oregon::IsValidMapCoord(destX, destY, destZ) || !Oregon::IsValidMapCoord(x, y, z))
56  {
57  sLog.outMMap("PathInfo::Update() called with invalid map coords, destX: %f destY: %f destZ: %f x: %f y: %f z: %f for creature %u", destX, destY, destZ, x, y, z, m_sourceUnit->GetGUIDLow());
59  return false;
60  }
61 
62  Vector3 oldDest = getEndPosition();
63  Vector3 newDest(destX, destY, destZ);
64  setEndPosition(newDest);
65 
66  Vector3 newStart(x, y, z);
67  setStartPosition(newStart);
68 
69  m_forceDestination = forceDest;
70 
71  sLog.outMMap("PathInfo::Update() for %u \n", m_sourceUnit->GetGUIDLow());
72 
73  // make sure navMesh works - we can run on map w/o mmap
74  // check if the start and end point have a .mmtile loaded (can we pass via not loaded tile on the way?)
76  !HaveTile(newStart) || !HaveTile(newDest))
77  {
78  BuildShortcut();
80  return true;
81  }
82 
83  updateFilter();
84 
85  BuildPolyPath(newStart, newDest);
86  return true;
87 }
88 
89 dtPolyRef PathInfo::getPathPolyByPosition(const dtPolyRef* polyPath, uint32 polyPathSize, const float* point, float* distance) const
90 {
91  if (!polyPath || !polyPathSize)
92  return INVALID_POLYREF;
93 
94  dtPolyRef nearestPoly = INVALID_POLYREF;
95  float minDist2d = FLT_MAX;
96  float minDist3d = 0.0f;
97 
98  for (uint32 i = 0; i < polyPathSize; ++i)
99  {
100  float closestPoint[VERTEX_SIZE];
101  if (dtStatusFailed(m_navMeshQuery->closestPointOnPoly(polyPath[i], point, closestPoint, NULL)))
102  continue;
103 
104  float d = dtVdist2DSqr(point, closestPoint);
105  if (d < minDist2d)
106  {
107  minDist2d = d;
108  nearestPoly = polyPath[i];
109  minDist3d = dtVdistSqr(point, closestPoint);
110  }
111 
112  if (minDist2d < 1.0f) // shortcut out - close enough for us
113  break;
114  }
115 
116  if (distance)
117  *distance = dtMathSqrtf(minDist3d);
118 
119  return (minDist2d < 3.0f) ? nearestPoly : INVALID_POLYREF;
120 }
121 
122 dtPolyRef PathInfo::getPolyByLocation(const float* point, float* distance) const
123 {
124  // first we check the current path
125  // if the current path doesn't contain the current poly,
126  // we need to use the expensive navMesh.findNearestPoly
127  dtPolyRef polyRef = getPathPolyByPosition(m_pathPolyRefs, m_polyLength, point, distance);
128  if (polyRef != INVALID_POLYREF)
129  return polyRef;
130 
131  // we don't have it in our old path
132  // try to get it by findNearestPoly()
133  // first try with low search box
134  float extents[VERTEX_SIZE] = {3.0f, 5.0f, 3.0f}; // bounds of poly search area
135  float closestPoint[VERTEX_SIZE] = {0.0f, 0.0f, 0.0f};
136  if (dtStatusSucceed(m_navMeshQuery->findNearestPoly(point, extents, &m_filter, &polyRef, closestPoint)) && polyRef != INVALID_POLYREF)
137  {
138  *distance = dtVdist(closestPoint, point);
139  return polyRef;
140  }
141 
142  // still nothing ..
143  // try with bigger search box
144  extents[1] = 200.0f;
145 
146  if (dtStatusSucceed(m_navMeshQuery->findNearestPoly(point, extents, &m_filter, &polyRef, closestPoint)) && polyRef != INVALID_POLYREF)
147  {
148  *distance = dtVdist(closestPoint, point);
149  return polyRef;
150  }
151 
152  return INVALID_POLYREF;
153 }
154 
155 void PathInfo::BuildPolyPath(const Vector3& startPos, const Vector3& endPos)
156 {
157  // *** getting start/end poly logic ***
158 
159  float distToStartPoly, distToEndPoly;
160  float startPoint[VERTEX_SIZE] = {startPos.y, startPos.z, startPos.x};
161  float endPoint[VERTEX_SIZE] = {endPos.y, endPos.z, endPos.x};
162 
163  dtPolyRef startPoly = getPolyByLocation(startPoint, &distToStartPoly);
164  dtPolyRef endPoly = getPolyByLocation(endPoint, &distToEndPoly);
165 
166  dtStatus dtResult;
167 
168  // we have a hole in our mesh
169  // make shortcut path and mark it as NOPATH ( with flying and swimming exception )
170  // its up to caller how he will use this info
171  if (startPoly == INVALID_POLYREF || endPoly == INVALID_POLYREF)
172  {
173  sLog.outMMap("BuildPolyPath :: (startPoly == 0 || endPoly == 0)\n");
174  BuildShortcut();
175 
176  bool path = m_sourceUnit->GetTypeId() == TYPEID_UNIT && m_sourceUnit->ToCreature()->CanFly();
177 
178  bool waterPath = m_sourceUnit->GetTypeId() == TYPEID_UNIT && m_sourceUnit->ToCreature()->CanSwim();
179  if (waterPath)
180  {
181  // Check both start and end points, if they're both in water, then we can *safely* let the creature move
182  for (uint32 i = 0; i < m_pathPoints.size(); ++i)
183  {
185  // One of the points is not in the water, cancel movement.
186  if (status == LIQUID_MAP_NO_WATER)
187  {
188  waterPath = false;
189  break;
190  }
191  }
192  }
193 
195  return;
196  }
197 
198  // we may need a better number here
199  bool farFromPoly = (distToStartPoly > 7.0f || distToEndPoly > 7.0f);
200  if (farFromPoly)
201  {
202  sLog.outMMap("BuildPolyPath :: farFromPoly distToStartPoly=%.3f distToEndPoly=%.3f\n", distToStartPoly, distToEndPoly);
203 
204  bool buildShotrcut = false;
206  {
207  Creature* owner = (Creature*)m_sourceUnit;
208 
209  Vector3 p = (distToStartPoly > 7.0f) ? startPos : endPos;
210  if (m_sourceUnit->GetMap()->IsUnderWater(p.x, p.y, p.z))
211  {
212  sLog.outMMap("BuildPolyPath :: underwater case\n");
213  if (owner->CanSwim())
214  buildShotrcut = true;
215  }
216  else
217  {
218  sLog.outMMap("BuildPolyPath :: flying case\n");
219  if (owner->CanFly())
220  buildShotrcut = true;
221  }
222  }
223 
224  if (buildShotrcut)
225  {
226  BuildShortcut();
228  return;
229  }
230  else
231  {
232  float closestPoint[VERTEX_SIZE];
233  // we may want to use closestPointOnPolyBoundary instead
234  if (dtStatusSucceed(m_navMeshQuery->closestPointOnPoly(endPoly, endPoint, closestPoint, NULL)))
235  {
236  dtVcopy(endPoint, closestPoint);
237  setActualEndPosition(Vector3(endPoint[2], endPoint[0], endPoint[1]));
238  }
239 
241  }
242  }
243 
244  // *** poly path generating logic ***
245 
246  // start and end are on same polygon
247  // just need to move in straight line
248  if (startPoly == endPoly)
249  {
250  sLog.outMMap("BuildPolyPath :: (startPoly == endPoly)\n");
251 
252  BuildShortcut();
253 
254  m_pathPolyRefs[0] = startPoly;
255  m_polyLength = 1;
256 
257  m_type = farFromPoly ? PATHFIND_INCOMPLETE : PATHFIND_NORMAL;
258  sLog.outMMap("BuildPolyPath :: path type %d\n", m_type);
259  return;
260  }
261 
262  // look for startPoly/endPoly in current path
263  // TODO: we can merge it with getPathPolyByPosition() loop
264  bool startPolyFound = false;
265  bool endPolyFound = false;
266  uint32 pathStartIndex = 0;
267  uint32 pathEndIndex = 0;
268 
269  if (m_polyLength)
270  {
271  for (; pathStartIndex < m_polyLength; ++pathStartIndex)
272  {
273  // here to catch few bugs
274  if (m_pathPolyRefs[pathStartIndex] == INVALID_POLYREF)
275  {
276  sLog.outMMap("Invalid poly ref in BuildPolyPath.");
277  break;
278  }
279 
280  if (m_pathPolyRefs[pathStartIndex] == startPoly)
281  {
282  startPolyFound = true;
283  break;
284  }
285  }
286 
287  for (pathEndIndex = m_polyLength - 1; pathEndIndex > pathStartIndex; --pathEndIndex)
288  if (m_pathPolyRefs[pathEndIndex] == endPoly)
289  {
290  endPolyFound = true;
291  break;
292  }
293  }
294 
295  if (startPolyFound && endPolyFound)
296  {
297  //DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ BuildPolyPath :: (startPolyFound && endPolyFound)\n");
298 
299  // we moved along the path and the target did not move out of our old poly-path
300  // our path is a simple subpath case, we have all the data we need
301  // just "cut" it out
302 
303  m_polyLength = pathEndIndex - pathStartIndex + 1;
304  memmove(m_pathPolyRefs, m_pathPolyRefs + pathStartIndex, m_polyLength * sizeof(dtPolyRef));
305  }
306  else if (startPolyFound && !endPolyFound)
307  {
308  // DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ BuildPolyPath :: (startPolyFound && !endPolyFound)\n");
309 
310  // we are moving on the old path but target moved out
311  // so we have atleast part of poly-path ready
312 
313  m_polyLength -= pathStartIndex;
314 
315  // try to adjust the suffix of the path instead of recalculating entire length
316  // at given interval the target cannot get too far from its last location
317  // thus we have less poly to cover
318  // sub-path of optimal path is optimal
319 
320  // take ~80% of the original length
321  // TODO : play with the values here
322  uint32 prefixPolyLength = uint32(m_polyLength * 0.8f + 0.5f);
323  memmove(m_pathPolyRefs, m_pathPolyRefs + pathStartIndex, prefixPolyLength * sizeof(dtPolyRef));
324 
325  dtPolyRef suffixStartPoly = m_pathPolyRefs[prefixPolyLength - 1];
326 
327  // we need any point on our suffix start poly to generate poly-path, so we need last poly in prefix data
328  float suffixEndPoint[VERTEX_SIZE];
329  dtResult = m_navMeshQuery->closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint, NULL);
330  if (dtStatusFailed(dtResult))
331  {
332  // we can hit offmesh connection as last poly - closestPointOnPoly() don't like that
333  // try to recover by using prev polyref
334  --prefixPolyLength;
335  suffixStartPoly = m_pathPolyRefs[prefixPolyLength - 1];
336  dtResult = m_navMeshQuery->closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint, NULL);
337  if (dtStatusFailed(dtResult))
338  {
339  // suffixStartPoly is still invalid, error state
340  BuildShortcut();
342  return;
343  }
344  }
345 
346  // generate suffix
347  uint32 suffixPolyLength = 0;
348  dtResult = m_navMeshQuery->findPath(
349  suffixStartPoly, // start polygon
350  endPoly, // end polygon
351  suffixEndPoint, // start position
352  endPoint, // end position
353  &m_filter, // polygon search filter
354  m_pathPolyRefs + prefixPolyLength - 1, // [out] path
355  (int*)&suffixPolyLength,
356  MAX_PATH_LENGTH - prefixPolyLength); // max number of polygons in output path
357 
358  if (!suffixPolyLength || dtStatusFailed(dtResult))
359  {
360  // this is probably an error state, but we'll leave it
361  // and hopefully recover on the next Update
362  // we still need to copy our preffix
363  sLog.outError("%u's Path Build failed: 0 length path", m_sourceUnit->GetGUIDLow());
364  }
365 
366  // DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ m_polyLength=%u prefixPolyLength=%u suffixPolyLength=%u \n", m_polyLength, prefixPolyLength, suffixPolyLength);
367 
368  // new path = prefix + suffix - overlap
369  m_polyLength = prefixPolyLength + suffixPolyLength - 1;
370  }
371  else
372  {
373  //DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ BuildPolyPath :: (!startPolyFound && !endPolyFound)\n");
374 
375  // either we have no path at all -> first run
376  // or something went really wrong -> we aren't moving along the path to the target
377  // just generate new path
378 
379  // free and invalidate old path data
380  clear();
381 
382  dtResult = m_navMeshQuery->findPath(
383  startPoly, // start polygon
384  endPoly, // end polygon
385  startPoint, // start position
386  endPoint, // end position
387  &m_filter, // polygon search filter
388  m_pathPolyRefs, // [out] path
389  (int*)&m_polyLength,
390  MAX_PATH_LENGTH); // max number of polygons in output path
391 
392  if (!m_polyLength || dtStatusFailed(dtResult))
393  {
394  // only happens if we passed bad data to findPath(), or navmesh is messed up
395  sLog.outError("%u's Path Build failed: 0 length path", m_sourceUnit->GetGUIDLow());
396  BuildShortcut();
398  return;
399  }
400  }
401 
402  // by now we know what type of path we can get
403  if (m_pathPolyRefs[m_polyLength - 1] == endPoly && !(m_type & PATHFIND_INCOMPLETE))
405  else
407 
408  // generate the point-path out of our up-to-date poly-path
409  BuildPointPath(startPoint, endPoint);
410 }
411 
412 void PathInfo::BuildPointPath(const float* startPoint, const float* endPoint)
413 {
414  float pathPoints[MAX_POINT_PATH_LENGTH * VERTEX_SIZE];
415  uint32 pointCount = 0;
416  dtStatus dtResult = DT_FAILURE;
417  if (m_useStraightPath)
418  {
419  dtResult = m_navMeshQuery->findStraightPath(
420  startPoint, // start position
421  endPoint, // end position
422  m_pathPolyRefs, // current path
423  m_polyLength, // lenth of current path
424  pathPoints, // [out] path corner points
425  NULL, // [out] flags
426  NULL, // [out] shortened path
427  (int*)&pointCount,
428  m_pointPathLimit); // maximum number of points/polygons to use
429  }
430  else
431  {
432  dtResult = findSmoothPath(
433  startPoint, // start position
434  endPoint, // end position
435  m_pathPolyRefs, // current path
436  m_polyLength, // length of current path
437  pathPoints, // [out] path corner points
438  (int*)&pointCount,
439  m_pointPathLimit); // maximum number of points
440  }
441 
442  if (pointCount < 2 || dtStatusFailed(dtResult))
443  {
444  // only happens if pass bad data to findStraightPath or navmesh is broken
445  // single point paths can be generated here
446  // TODO : check the exact cases
447  sLog.outMMap("PathInfo::BuildPointPath FAILED! path sized %d returned\n", pointCount);
448  BuildShortcut();
450  return;
451  }
452  else if (pointCount == m_pointPathLimit)
453  {
454  sLog.outMMap("PathInfo::BuildPointPath pointCount %u == m_pointPathLimit\n", pointCount);
455  BuildShortcut();
457  return;
458  }
459 
460  m_pathPoints.resize(pointCount);
461  for (uint32 i = 0; i < pointCount; ++i)
462  m_pathPoints[i] = Vector3(pathPoints[i * VERTEX_SIZE + 2], pathPoints[i * VERTEX_SIZE], pathPoints[i * VERTEX_SIZE + 1]);
463 
464  NormalizePath();
465 
466  // first point is always our current location - we need the next one
468  setActualEndPosition(m_pathPoints[pointCount - 1]);
469 
470  // force the given destination, if needed
471  if (m_forceDestination &&
473  {
474  // we may want to keep partial subpath
477  {
479  m_pathPoints[m_pathPoints.size() - 1] = getEndPosition();
480  }
481  else
482  {
484  BuildShortcut();
485  }
486 
487  m_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
488  }
489 
490  //DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ PathInfo::BuildPointPath path type %d size %d poly-size %d\n", m_type, pointCount, m_polyLength);
491 }
492 
494 {
495  for (uint32 i = 0; i < m_pathPoints.size(); ++i)
497 }
498 
500 {
501  //DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ BuildShortcut :: making shortcut\n");
502 
503  clear();
504 
505  // make two point path, our curr pos is the start, and dest is the end
506  m_pathPoints.resize(2);
507 
508  // set start and a default next position
511 
512  NormalizePath();
513 
516 }
517 
519 {
520  uint16 includeFlags = 0;
521  uint16 excludeFlags = 0;
522 
524  {
525  Creature* creature = (Creature*)m_sourceUnit;
526  if (creature->CanWalk())
527  includeFlags |= NAV_GROUND; // walk
528 
529  // creatures don't take environmental damage
530  if (creature->CanSwim())
531  includeFlags |= (NAV_WATER | NAV_MAGMA | NAV_SLIME); // swim
532  }
533  else if (m_sourceUnit->GetTypeId() == TYPEID_PLAYER)
534  {
535  // perfect support not possible, just stay 'safe'
536  includeFlags |= (NAV_GROUND | NAV_WATER);
537  }
538 
539  m_filter.setIncludeFlags(includeFlags);
540  m_filter.setExcludeFlags(excludeFlags);
541 
542  updateFilter();
543 }
544 
546 {
547  // allow creatures to cheat and use different movement types if they are moved
548  // forcefully into terrain they can't normally move in
550  {
551  uint16 includedFlags = m_filter.getIncludeFlags();
552  includedFlags |= getNavTerrain(m_sourceUnit->GetPositionX(),
555 
556  m_filter.setIncludeFlags(includedFlags);
557  }
558 }
559 
560 NavTerrain PathInfo::getNavTerrain(float x, float y, float z)
561 {
562  LiquidData data;
563  ZLiquidStatus liquidStatus = m_sourceUnit->GetMap()->getLiquidStatus(x, y, z, MAP_ALL_LIQUIDS, &data);
564  if (liquidStatus == LIQUID_MAP_NO_WATER)
565  return NAV_GROUND;
566 
567  switch (data.type_flags)
568  {
571  return NAV_WATER;
573  return NAV_MAGMA;
575  return NAV_SLIME;
576  default:
577  return NAV_GROUND;
578  }
579 }
580 
581 bool PathInfo::HaveTile(const Vector3& p) const
582 {
583  int tx = -1, ty = -1;
584  float point[VERTEX_SIZE] = {p.y, p.z, p.x};
585 
586  m_navMesh->calcTileLoc(point, &tx, &ty);
587 
591  if (tx < 0 || ty < 0)
592  return false;
593 
594  return (m_navMesh->getTileAt(tx, ty, 0) != NULL);
595 }
596 
597 uint32 PathInfo::fixupCorridor(dtPolyRef* path, uint32 npath, uint32 maxPath,
598  const dtPolyRef* visited, uint32 nvisited)
599 {
600  int32 furthestPath = -1;
601  int32 furthestVisited = -1;
602 
603  // Find furthest common polygon.
604  for (int32 i = npath - 1; i >= 0; --i)
605  {
606  bool found = false;
607  for (int32 j = nvisited - 1; j >= 0; --j)
608  {
609  if (path[i] == visited[j])
610  {
611  furthestPath = i;
612  furthestVisited = j;
613  found = true;
614  }
615  }
616  if (found)
617  break;
618  }
619 
620  // If no intersection found just return current path.
621  if (furthestPath == -1 || furthestVisited == -1)
622  return npath;
623 
624  // Concatenate paths.
625 
626  // Adjust beginning of the buffer to include the visited.
627  uint32 req = nvisited - furthestVisited;
628  uint32 orig = uint32(furthestPath + 1) < npath ? furthestPath + 1 : npath;
629  uint32 size = npath - orig > 0 ? npath - orig : 0;
630  if (req + size > maxPath)
631  size = maxPath - req;
632 
633  if (size)
634  memmove(path + req, path + orig, size * sizeof(dtPolyRef));
635 
636  // Store visited
637  for (uint32 i = 0; i < req; ++i)
638  path[i] = visited[(nvisited - 1) - i];
639 
640  return req + size;
641 }
642 
643 bool PathInfo::getSteerTarget(const float* startPos, const float* endPos,
644  float minTargetDist, const dtPolyRef* path, uint32 pathSize,
645  float* steerPos, unsigned char& steerPosFlag, dtPolyRef& steerPosRef)
646 {
647  // Find steer target.
648  static const uint32 MAX_STEER_POINTS = 3;
649  float steerPath[MAX_STEER_POINTS * VERTEX_SIZE];
650  unsigned char steerPathFlags[MAX_STEER_POINTS];
651  dtPolyRef steerPathPolys[MAX_STEER_POINTS];
652  uint32 nsteerPath = 0;
653  dtStatus dtResult = m_navMeshQuery->findStraightPath(startPos, endPos, path, pathSize,
654  steerPath, steerPathFlags, steerPathPolys, (int*)&nsteerPath, MAX_STEER_POINTS);
655  if (!nsteerPath || dtStatusFailed(dtResult))
656  return false;
657 
658  // Find vertex far enough to steer to.
659  uint32 ns = 0;
660  while (ns < nsteerPath)
661  {
662  // Stop at Off-Mesh link or when point is further than slop away.
663  if ((steerPathFlags[ns] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ||
664  !inRangeYZX(&steerPath[ns * VERTEX_SIZE], startPos, minTargetDist, 1000.0f))
665  break;
666  ns++;
667  }
668  // Failed to find good point to steer to.
669  if (ns >= nsteerPath)
670  return false;
671 
672  dtVcopy(steerPos, &steerPath[ns * VERTEX_SIZE]);
673  steerPos[1] = startPos[1]; // keep Z value
674  steerPosFlag = steerPathFlags[ns];
675  steerPosRef = steerPathPolys[ns];
676 
677  return true;
678 }
679 
680 dtStatus PathInfo::findSmoothPath(const float* startPos, const float* endPos,
681  const dtPolyRef* polyPath, uint32 polyPathSize,
682  float* smoothPath, int* smoothPathSize, uint32 maxSmoothPathSize)
683 {
684  *smoothPathSize = 0;
685  uint32 nsmoothPath = 0;
686 
687  dtPolyRef polys[MAX_PATH_LENGTH];
688  memcpy(polys, polyPath, sizeof(dtPolyRef)*polyPathSize);
689  uint32 npolys = polyPathSize;
690 
691  float iterPos[VERTEX_SIZE], targetPos[VERTEX_SIZE];
692  if (dtStatusFailed(m_navMeshQuery->closestPointOnPolyBoundary(polys[0], startPos, iterPos)))
693  return DT_FAILURE;
694 
695  if (dtStatusFailed(m_navMeshQuery->closestPointOnPolyBoundary(polys[npolys - 1], endPos, targetPos)))
696  return DT_FAILURE;
697 
698  dtVcopy(&smoothPath[nsmoothPath * VERTEX_SIZE], iterPos);
699  nsmoothPath++;
700 
701  // Move towards target a small advancement at a time until target reached or
702  // when ran out of memory to store the path.
703  while (npolys && nsmoothPath < maxSmoothPathSize)
704  {
705  // Find location to steer towards.
706  float steerPos[VERTEX_SIZE];
707  unsigned char steerPosFlag;
708  dtPolyRef steerPosRef = INVALID_POLYREF;
709 
710  if (!getSteerTarget(iterPos, targetPos, SMOOTH_PATH_SLOP, polys, npolys, steerPos, steerPosFlag, steerPosRef))
711  break;
712 
713  bool endOfPath = (steerPosFlag & DT_STRAIGHTPATH_END);
714  bool offMeshConnection = (steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION);
715 
716  // Find movement delta.
717  float delta[VERTEX_SIZE];
718  dtVsub(delta, steerPos, iterPos);
719  float len = dtMathSqrtf(dtVdot(delta, delta));
720  // If the steer target is end of path or off-mesh link, do not move past the location.
721  if ((endOfPath || offMeshConnection) && len < SMOOTH_PATH_STEP_SIZE)
722  len = 1.0f;
723  else
724  len = SMOOTH_PATH_STEP_SIZE / len;
725 
726  float moveTgt[VERTEX_SIZE];
727  dtVmad(moveTgt, iterPos, delta, len);
728 
729  // Move
730  float result[VERTEX_SIZE];
731  const static uint32 MAX_VISIT_POLY = 16;
732  dtPolyRef visited[MAX_VISIT_POLY];
733 
734  uint32 nvisited = 0;
735  m_navMeshQuery->moveAlongSurface(polys[0], iterPos, moveTgt, &m_filter, result, visited, (int*)&nvisited, MAX_VISIT_POLY);
736  npolys = fixupCorridor(polys, npolys, MAX_PATH_LENGTH, visited, nvisited);
737 
738  m_navMeshQuery->getPolyHeight(polys[0], result, &result[1]);
739  result[1] += 0.5f;
740  dtVcopy(iterPos, result);
741 
742  // Handle end of path and off-mesh links when close enough.
743  if (endOfPath && inRangeYZX(iterPos, steerPos, SMOOTH_PATH_SLOP, 1.0f))
744  {
745  // Reached end of path.
746  dtVcopy(iterPos, targetPos);
747  if (nsmoothPath < maxSmoothPathSize)
748  {
749  dtVcopy(&smoothPath[nsmoothPath * VERTEX_SIZE], iterPos);
750  nsmoothPath++;
751  }
752  break;
753  }
754  else if (offMeshConnection && inRangeYZX(iterPos, steerPos, SMOOTH_PATH_SLOP, 1.0f))
755  {
756  // Advance the path up to and over the off-mesh connection.
757  dtPolyRef prevRef = INVALID_POLYREF;
758  dtPolyRef polyRef = polys[0];
759  uint32 npos = 0;
760  while (npos < npolys && polyRef != steerPosRef)
761  {
762  prevRef = polyRef;
763  polyRef = polys[npos];
764  npos++;
765  }
766 
767  for (uint32 i = npos; i < npolys; ++i)
768  polys[i - npos] = polys[i];
769 
770  npolys -= npos;
771 
772  // Handle the connection.
773  float startPos[VERTEX_SIZE], endPos[VERTEX_SIZE];
774  if (dtStatusSucceed(m_navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos)))
775  {
776  if (nsmoothPath < maxSmoothPathSize)
777  {
778  dtVcopy(&smoothPath[nsmoothPath * VERTEX_SIZE], startPos);
779  nsmoothPath++;
780  }
781  // Move position at the other side of the off-mesh link.
782  dtVcopy(iterPos, endPos);
783  m_navMeshQuery->getPolyHeight(polys[0], iterPos, &iterPos[1]);
784  iterPos[1] += 0.5f;
785  }
786  }
787 
788  // Store results.
789  if (nsmoothPath < maxSmoothPathSize)
790  {
791  dtVcopy(&smoothPath[nsmoothPath * VERTEX_SIZE], iterPos);
792  nsmoothPath++;
793  }
794  }
795 
796  *smoothPathSize = nsmoothPath;
797 
798  // this is most likely a loop
799  return nsmoothPath < MAX_POINT_PATH_LENGTH ? DT_SUCCESS : DT_FAILURE;
800 }
801 
802 bool PathInfo::inRangeYZX(const float* v1, const float* v2, float r, float h) const
803 {
804  const float dx = v2[0] - v1[0];
805  const float dy = v2[1] - v1[1]; // elevation
806  const float dz = v2[2] - v1[2];
807  return (dx * dx + dz * dz) < r * r && fabsf(dy) < h;
808 }
809 
810 bool PathInfo::inRange(const Vector3& p1, const Vector3& p2, float r, float h) const
811 {
812  const float dx = p2.x - p1.x;
813  const float dy = p2.y - p1.y;
814  const float dz = p2.z - p1.z;
815  return (dx * dx + dy * dy) < r * r && fabsf(dz) < h;
816 }
817 
818 float PathInfo::dist3DSqr(const Vector3& p1, const Vector3& p2) const
819 {
820  const float dx = p2.x - p1.x;
821  const float dy = p2.y - p1.y;
822  const float dz = p2.z - p1.z;
823  return (dx * dx + dy * dy + dz * dz);
824 }
void setActualEndPosition(Vector3 point)
Definition: PathFinder.h:165
Map * GetMap() const
Definition: Object.h:841
PathType m_type
Definition: PathFinder.h:135
#define INVALID_POLYREF
Definition: PathFinder.h:42
const dtNavMesh * m_navMesh
Definition: PathFinder.h:147
Vector3 getActualEndPosition() const
Definition: PathFinder.h:113
bool m_useStraightPath
Definition: PathFinder.h:137
float dist3DSqr(const Vector3 &p1, const Vector3 &p2) const
Definition: PathFinder.cpp:818
void UpdateAllowedPositionZ(float x, float y, float &z) const
Definition: Object.cpp:1419
void setNextPosition(Vector3 point)
Definition: PathFinder.h:152
bool CanFly() const
Definition: Creature.h:493
#define sLog
Log class singleton.
Definition: Log.h:187
NavTerrain getNavTerrain(float x, float y, float z)
Definition: PathFinder.cpp:560
ZLiquidStatus
Definition: Map.h:126
ACE_INT32 int32
Definition: Define.h:67
#define VERTEX_SIZE
Definition: PathFinder.h:41
const Unit *const m_sourceUnit
Definition: PathFinder.h:146
NULL Dbg ErrDB Arena Chat Char Map MMap false
Definition: Log.cpp:556
bool IsValidMapCoord(float c)
Definition: GridDefines.h:192
void setEndPosition(Vector3 point)
Definition: PathFinder.h:160
#define MAX_PATH_LENGTH
Definition: PathFinder.h:35
uint32 GetGUIDLow() const
Definition: Object.h:166
void BuildPointPath(const float *startPoint, const float *endPoint)
Definition: PathFinder.cpp:412
bool CanSwim() const
Definition: Creature.h:492
const dtNavMeshQuery * m_navMeshQuery
Definition: PathFinder.h:148
virtual bool IsUnderWater() const
Definition: Unit.cpp:3659
dtPolyRef m_pathPolyRefs[MAX_PATH_LENGTH]
Definition: PathFinder.h:131
void createFilter()
Definition: PathFinder.cpp:518
#define MAP_LIQUID_TYPE_MAGMA
Definition: Map.h:137
uint8 GetTypeId() const
Definition: Object.h:210
dtQueryFilter m_filter
Definition: PathFinder.h:150
Vector3 getEndPosition() const
Definition: PathFinder.h:109
PathInfo(Unit const *owner)
Definition: PathFinder.cpp:27
ZLiquidStatus getLiquidStatus(float x, float y, float z, uint8 ReqLiquidType, LiquidData *data=0) const
Definition: Map.cpp:1728
float GetPositionY() const
Definition: Position.h:98
void GetPosition(float &x, float &y) const
Definition: Position.h:102
uint32 GetInstanceId() const
Definition: Object.h:688
float GetPositionZ() const
Definition: Position.h:99
#define MAX_POINT_PATH_LENGTH
Definition: PathFinder.h:36
void BuildPolyPath(const Vector3 &startPos, const Vector3 &endPos)
Definition: PathFinder.cpp:155
dtNavMesh const * GetNavMesh(uint32 mapId)
Definition: MoveMap.cpp:358
bool HaveTile(const Vector3 &p) const
Definition: PathFinder.cpp:581
PointsArray m_pathPoints
Definition: PathFinder.h:134
uint32 GetMapId() const
Definition: Object.h:591
uint32 type_flags
Definition: Map.h:149
Vector3 getStartPosition() const
Definition: PathFinder.h:101
bool IsUnderWater(float x, float y, float z) const
Definition: Map.cpp:1849
#define MAP_LIQUID_TYPE_WATER
Definition: Map.h:140
bool CanWalk() const
Definition: Creature.h:491
#define SMOOTH_PATH_SLOP
Definition: PathFinder.h:39
dtPolyRef getPolyByLocation(const float *point, float *distance) const
Definition: PathFinder.cpp:122
bool inRangeYZX(const float *v1, const float *v2, float r, float h) const
Definition: PathFinder.cpp:802
dtStatus findSmoothPath(const float *startPos, const float *endPos, const dtPolyRef *polyPath, uint32 polyPathSize, float *smoothPath, int *smoothPathSize, uint32 smoothPathMaxSize)
Definition: PathFinder.cpp:680
uint32 m_polyLength
Definition: PathFinder.h:132
Map const * GetBaseMap() const
Definition: Object.cpp:1984
#define MAP_ALL_LIQUIDS
Definition: Map.h:142
uint32 m_pointPathLimit
Definition: PathFinder.h:139
Creature * ToCreature()
Definition: Object.h:395
void updateFilter()
Definition: PathFinder.cpp:545
#define SMOOTH_PATH_STEP_SIZE
Definition: PathFinder.h:38
virtual bool IsInWater() const
Definition: Unit.cpp:3654
bool HasUnitState(const uint32 f) const
Definition: Unit.h:1030
PathType
Definition: PathFinder.h:44
void clear()
Definition: PathFinder.h:172
dtPolyRef getPathPolyByPosition(const dtPolyRef *polyPath, uint32 polyPathSize, const float *point, float *distance=NULL) const
Definition: PathFinder.cpp:89
dtNavMeshQuery const * GetNavMeshQuery(uint32 mapId, uint32 instanceId)
Definition: MoveMap.cpp:367
static MMapManager * createOrGetMMapManager()
Definition: MoveMap.cpp:34
bool inRange(const Vector3 &p1, const Vector3 &p2, float r, float h) const
Definition: PathFinder.cpp:810
bool getSteerTarget(const float *startPos, const float *endPos, float minTargetDist, const dtPolyRef *path, uint32 pathSize, float *steerPos, unsigned char &steerPosFlag, dtPolyRef &steerPosRef)
Definition: PathFinder.cpp:643
static bool IsPathfindingEnabled(uint32 mapId)
Definition: MoveMap.cpp:61
ACE_UINT16 uint16
Definition: Define.h:72
uint32 fixupCorridor(dtPolyRef *path, uint32 npath, uint32 maxPath, const dtPolyRef *visited, uint32 nvisited)
Definition: PathFinder.cpp:597
#define MAP_LIQUID_TYPE_SLIME
Definition: Map.h:139
void NormalizePath()
Definition: PathFinder.cpp:493
ACE_UINT32 uint32
Definition: Define.h:71
float GetPositionX() const
Definition: Position.h:97
bool Update(float destX, float destY, float destZ, bool forceDest=false)
Definition: PathFinder.cpp:50
true
Definition: Log.cpp:534
Definition: Unit.h:908
#define MAP_LIQUID_TYPE_OCEAN
Definition: Map.h:138
bool m_forceDestination
Definition: PathFinder.h:138
void BuildShortcut()
Definition: PathFinder.cpp:499
void setStartPosition(Vector3 point)
Definition: PathFinder.h:156