diff --git a/apps/openmw/mwmechanics/pathfinding.cpp b/apps/openmw/mwmechanics/pathfinding.cpp index e727d8585d..1cc4526193 100644 --- a/apps/openmw/mwmechanics/pathfinding.cpp +++ b/apps/openmw/mwmechanics/pathfinding.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include "../mwbase/world.hpp" #include "../mwbase/environment.hpp" @@ -315,12 +316,13 @@ namespace MWMechanics while (mPath.size() > 1 && sqrDistanceIgnoreZ(mPath.front(), position) < pointTolerance * pointTolerance) mPath.pop_front(); + const IsValidShortcut isValidShortcut { + MWBase::Environment::get().getWorld()->getNavigator(), + halfExtents, flags + }; + if (shortenIfAlmostStraight) { - const IsValidShortcut isValidShortcut { - MWBase::Environment::get().getWorld()->getNavigator(), - halfExtents, flags - }; while (mPath.size() > 2 && isAlmostStraight(mPath[0], mPath[1], mPath[2], pointTolerance) && isValidShortcut(mPath[0], mPath[2])) mPath.erase(mPath.begin() + 1); @@ -329,6 +331,19 @@ namespace MWMechanics mPath.pop_front(); } + if (mPath.size() > 1) + { + std::size_t begin = 0; + for (std::size_t i = 1; i < mPath.size(); ++i) + { + const float sqrDistance = Misc::getVectorToLine(position, mPath[i - 1], mPath[i]).length2(); + if (sqrDistance < pointTolerance * pointTolerance && isValidShortcut(position, mPath[i])) + begin = i; + } + for (std::size_t i = 0; i < begin; ++i) + mPath.pop_front(); + } + if (mPath.size() == 1) { float distSqr; diff --git a/components/misc/math.hpp b/components/misc/math.hpp new file mode 100644 index 0000000000..27e29675f2 --- /dev/null +++ b/components/misc/math.hpp @@ -0,0 +1,16 @@ +#ifndef OPENMW_COMPONENTS_MISC_MATH_H +#define OPENMW_COMPONENTS_MISC_MATH_H + +#include + +namespace Misc +{ + inline osg::Vec3f getVectorToLine(const osg::Vec3f& position, const osg::Vec3f& a, const osg::Vec3f& b) + { + osg::Vec3f direction = b - a; + direction.normalize(); + return (position - a) ^ direction; + } +} + +#endif