~rebel/horde3d/trunk

// This function checks if the path has a small U-turn, that is,

// a polygon further in the path is adjacent to the first polygon

// in the path. If that happens, a shortcut is taken.

// This can happen if the target (T) location is at tile boundary,

// and we're (S) approaching it parallel to the tile edge.

// The choice at the vertex can be arbitrary, 

//  +---+---+

//  |:::|:::|

//  +-S-+-T-+

//  |:::|   | <-- the step can end up in here, resulting U-turn path.

//  +---+---+

static int fixupShortcuts(dtPolyRef* path, int npath, dtNavMeshQuery* navQuery)

{

        if (npath < 3)

                return npath;

 

        // Get connected polygons

        static const int maxNeis = 16;

        dtPolyRef neis[maxNeis];

        int nneis = 0;

 

        const dtMeshTile* tile = 0;

        const dtPoly* poly = 0;

        if (dtStatusFailed(navQuery->getAttachedNavMesh()->getTileAndPolyByRef(path[0], &tile, &poly)))

                return npath;

        

        for (unsigned int k = poly->firstLink; k != DT_NULL_LINK; k = tile->links[k].next)

        {

                const dtLink* link = &tile->links[k];

                if (link->ref != 0)

                {

                        if (nneis < maxNeis)

                                neis[nneis++] = link->ref;

                }

        }

 

        // If any of the neighbour polygons is within the next few polygons

        // in the path, short cut to that polygon directly.

        static const int maxLookAhead = 6;

        int cut = 0;

        for (int i = dtMin(maxLookAhead, npath) - 1; i > 1 && cut == 0; i--) {

                for (int j = 0; j < nneis; j++)

                {

                        if (path[i] == neis[j]) {

                                cut = i;

                                break;

                        }

                }

        }

        if (cut > 1)

        {

                int offset = cut-1;

                npath -= offset;

                for (int i = 1; i < npath; i++)

                        path[i] = path[i+offset];

        }

 

        return npath;

}

 

                        m_navQuery->closestPointOnPoly(m_startRef, m_spos, m_iterPos, 0);

                        m_navQuery->closestPointOnPoly(m_pathIterPolys[m_pathIterPolyCount-1], m_epos, m_targetPos, 0);

        m_pathIterPolyCount = fixupShortcuts(m_pathIterPolys, m_pathIterPolyCount, m_navQuery);

 

                                m_navQuery->closestPointOnPoly(m_polys[m_npolys-1], m_epos, epos, 0);

                                                                                         m_straightPathPolys, &m_nstraightPath, MAX_POLYS, DT_STRAIGHTPATH_ALL_CROSSINGS);

                                m_navQuery->closestPointOnPoly(m_startRef, m_spos, iterPos, 0);

                                m_navQuery->closestPointOnPoly(polys[npolys-1], m_epos, targetPos, 0);

                                        len = dtMathSqrtf(dtVdot(delta, delta));

 

                                        npolys = fixupShortcuts(polys, npolys, m_navQuery);

 

                                        m_navQuery->closestPointOnPoly(m_polys[m_npolys-1], m_epos, epos, 0);

                        m_pathFindStatus = m_navQuery->initSlicedFindPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter, DT_FINDPATH_ANY_ANGLE);

                                dtVlerp(m_hitPos, m_spos, m_epos, t);

                        // Adjust height.

                        if (m_npolys > 0)

                        {

                                float h = 0;

                                m_navQuery->getPolyHeight(m_polys[m_npolys-1], m_hitPos, &h);

                                m_hitPos[1] = h;

                        }

                                unsigned int col;

                                unsigned int col;

                                else if (m_straightPathFlags[i] & DT_STRAIGHTPATH_END)

                                dd.vertex(m_straightPath[i*3], m_straightPath[i*3+1]+0.4f, m_straightPath[i*3+2], col);

                        dd.vertex(p[0],p[1]+0.1f,p[2], duRGBA(220,32,16,192));