public Result<FindDistanceToWallResult> findDistanceToWall(long startRef, float[] centerPos, float maxRadius,
            QueryFilter filter) {

        // Validate input
        if (!m_nav.isValidPolyRef(startRef) || Objects.isNull(centerPos) || !vIsFinite(centerPos) || maxRadius < 0
                || !Float.isFinite(maxRadius) || Objects.isNull(filter)) {
            return Result.invalidParam();
        }

        m_nodePool.clear();
        m_openList.clear();

        Node startNode = m_nodePool.getNode(startRef);
        vCopy(startNode.pos, centerPos);
        startNode.pidx = 0;
        startNode.cost = 0;
        startNode.total = 0;
        startNode.id = startRef;
        startNode.flags = Node.DT_NODE_OPEN;
        m_openList.push(startNode);

        float radiusSqr = sqr(maxRadius);
        float[] hitPos = new float[3];
        VectorPtr bestvj = null;
        VectorPtr bestvi = null;
        while (!m_openList.isEmpty()) {
            Node bestNode = m_openList.pop();
            bestNode.flags &= ~Node.DT_NODE_OPEN;
            bestNode.flags |= Node.DT_NODE_CLOSED;

            // Get poly and tile.
            // The API input has been cheked already, skip checking internal data.
            long bestRef = bestNode.id;
            Tupple2<MeshTile, Poly> tileAndPoly = m_nav.getTileAndPolyByRefUnsafe(bestRef);
            MeshTile bestTile = tileAndPoly.first;
            Poly bestPoly = tileAndPoly.second;

            // Get parent poly and tile.
            long parentRef = 0;
            MeshTile parentTile = null;
            Poly parentPoly = null;
            if (bestNode.pidx != 0) {
                parentRef = m_nodePool.getNodeAtIdx(bestNode.pidx).id;
            }
            if (parentRef != 0) {
                tileAndPoly = m_nav.getTileAndPolyByRefUnsafe(parentRef);
                parentTile = tileAndPoly.first;
                parentPoly = tileAndPoly.second;
            }

            // Hit test walls.
            for (int i = 0, j = bestPoly.vertCount - 1; i < bestPoly.vertCount; j = i++) {
                // Skip non-solid edges.
                if ((bestPoly.neis[j] & NavMesh.DT_EXT_LINK) != 0) {
                    // Tile border.
                    boolean solid = true;
                    for (int k = bestPoly.firstLink; k != NavMesh.DT_NULL_LINK; k = bestTile.links.get(k).next) {
                        Link link = bestTile.links.get(k);
                        if (link.edge == j) {
                            if (link.ref != 0) {
                                tileAndPoly = m_nav.getTileAndPolyByRefUnsafe(link.ref);
                                MeshTile neiTile = tileAndPoly.first;
                                Poly neiPoly = tileAndPoly.second;
                                if (filter.passFilter(link.ref, neiTile, neiPoly)) {
                                    solid = false;
                                }
                            }
                            break;
                        }
                    }
                    if (!solid) {
                        continue;
                    }
                } else if (bestPoly.neis[j] != 0) {
                    // Internal edge
                    int idx = (bestPoly.neis[j] - 1);
                    long ref = m_nav.getPolyRefBase(bestTile) | idx;
                    if (filter.passFilter(ref, bestTile, bestTile.data.polys[idx])) {
                        continue;
                    }
                }

                // Calc distance to the edge.
                int vj = bestPoly.verts[j] * 3;
                int vi = bestPoly.verts[i] * 3;
                Tupple2<Float, Float> distseg = distancePtSegSqr2D(centerPos, bestTile.data.verts, vj, vi);
                float distSqr = distseg.first;
                float tseg = distseg.second;

                // Edge is too far, skip.
                if (distSqr > radiusSqr) {
                    continue;
                }

                // Hit wall, update radius.
                radiusSqr = distSqr;
                // Calculate hit pos.
                hitPos[0] = bestTile.data.verts[vj] + (bestTile.data.verts[vi] - bestTile.data.verts[vj]) * tseg;
                hitPos[1] = bestTile.data.verts[vj + 1]
                        + (bestTile.data.verts[vi + 1] - bestTile.data.verts[vj + 1]) * tseg;
                hitPos[2] = bestTile.data.verts[vj + 2]
                        + (bestTile.data.verts[vi + 2] - bestTile.data.verts[vj + 2]) * tseg;
                bestvj = new VectorPtr(bestTile.data.verts, vj);
                bestvi = new VectorPtr(bestTile.data.verts, vi);
            }

            for (int i = bestPoly.firstLink; i != NavMesh.DT_NULL_LINK; i = bestTile.links.get(i).next) {
                Link link = bestTile.links.get(i);
                long neighbourRef = link.ref;
                // Skip invalid neighbours and do not follow back to parent.
                if (neighbourRef == 0 || neighbourRef == parentRef) {
                    continue;
                }

                // Expand to neighbour.
                tileAndPoly = m_nav.getTileAndPolyByRefUnsafe(neighbourRef);
                MeshTile neighbourTile = tileAndPoly.first;
                Poly neighbourPoly = tileAndPoly.second;

                // Skip off-mesh connections.
                if (neighbourPoly.getType() == Poly.DT_POLYTYPE_OFFMESH_CONNECTION) {
                    continue;
                }

                // Calc distance to the edge.
                int va = bestPoly.verts[link.edge] * 3;
                int vb = bestPoly.verts[(link.edge + 1) % bestPoly.vertCount] * 3;
                Tupple2<Float, Float> distseg = distancePtSegSqr2D(centerPos, bestTile.data.verts, va, vb);
                float distSqr = distseg.first;
                // If the circle is not touching the next polygon, skip it.
                if (distSqr > radiusSqr) {
                    continue;
                }

                if (!filter.passFilter(neighbourRef, neighbourTile, neighbourPoly)) {
                    continue;
                }

                Node neighbourNode = m_nodePool.getNode(neighbourRef);

                if ((neighbourNode.flags & Node.DT_NODE_CLOSED) != 0) {
                    continue;
                }

                // Cost
                if (neighbourNode.flags == 0) {
                    Result<float[]> midPoint = getEdgeMidPoint(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly,
                            neighbourTile);
                    if (midPoint.succeeded()) {
                        neighbourNode.pos = midPoint.result;
                    }
                }

                float total = bestNode.total + vDist(bestNode.pos, neighbourNode.pos);

                // The node is already in open list and the new result is worse, skip.
                if ((neighbourNode.flags & Node.DT_NODE_OPEN) != 0 && total >= neighbourNode.total) {
                    continue;
                }

                neighbourNode.id = neighbourRef;
                neighbourNode.flags = (neighbourNode.flags & ~Node.DT_NODE_CLOSED);
                neighbourNode.pidx = m_nodePool.getNodeIdx(bestNode);
                neighbourNode.total = total;

                if ((neighbourNode.flags & Node.DT_NODE_OPEN) != 0) {
                    m_openList.modify(neighbourNode);
                } else {
                    neighbourNode.flags |= Node.DT_NODE_OPEN;
                    m_openList.push(neighbourNode);
                }
            }
        }

        // Calc hit normal.
        float[] hitNormal = new float[3];
        if (bestvi != null && bestvj != null) {
            float[] tangent = vSub(bestvi, bestvj);
            hitNormal[0] = tangent[2];
            hitNormal[1] = 0;
            hitNormal[2] = -tangent[0];
            vNormalize(hitNormal);
        }
        return Result.success(new FindDistanceToWallResult((float) Math.sqrt(radiusSqr), hitPos, hitNormal));
    }
public Result < FindDistanceToWallResult > findDistanceToWall ( long startRef , float [ ] centerPos , float maxRadius , QueryFilter filter ) { if ( ! m_nav . isValidPolyRef ( startRef ) || Objects . isNull ( centerPos ) || ! vIsFinite ( centerPos ) || maxRadius < 0 || ! Float . isFinite ( maxRadius ) || Objects . isNull ( filter ) ) { return Result . invalidParam ( ) ; } m_nodePool . clear ( ) ; m_openList . clear ( ) ; Node startNode = m_nodePool . getNode ( startRef ) ; vCopy ( startNode . pos , centerPos ) ; startNode . pidx = 0 ; startNode . cost = 0 ; startNode . total = 0 ; startNode . id = startRef ; startNode . flags = Node . DT_NODE_OPEN ; m_openList . push ( startNode ) ; float radiusSqr = sqr ( maxRadius ) ; float [ ] hitPos = new float [ 3 ] ; VectorPtr bestvj = null ; VectorPtr bestvi = null ; while ( ! m_openList . isEmpty ( ) ) { Node bestNode = m_openList . pop ( ) ; bestNode . flags &= ~ Node . DT_NODE_OPEN ; bestNode . flags |= Node . DT_NODE_CLOSED ; long bestRef = bestNode . id ; Tupple2 < MeshTile , Poly > tileAndPoly = m_nav . getTileAndPolyByRefUnsafe ( bestRef ) ; MeshTile bestTile = tileAndPoly . first ; Poly bestPoly = tileAndPoly . second ; long parentRef = 0 ; MeshTile parentTile = null ; Poly parentPoly = null ; if ( bestNode . pidx != 0 ) { parentRef = m_nodePool . getNodeAtIdx ( bestNode . pidx ) . id ; } if ( parentRef != 0 ) { tileAndPoly = m_nav . getTileAndPolyByRefUnsafe ( parentRef ) ; parentTile = tileAndPoly . first ; parentPoly = tileAndPoly . second ; } for ( int i = 0 , j = bestPoly . vertCount - 1 ; i < bestPoly . vertCount ; j = i ++ ) { if ( ( bestPoly . neis [ j ] & NavMesh . DT_EXT_LINK ) != 0 ) { boolean solid = true ; for ( int k = bestPoly . firstLink ; k != NavMesh . DT_NULL_LINK ; k = bestTile . links . get ( k ) . next ) { Link link = bestTile . links . get ( k ) ; if ( link . edge == j ) { if ( link . ref != 0 ) { tileAndPoly = m_nav . getTileAndPolyByRefUnsafe ( link . ref ) ; MeshTile neiTile = tileAndPoly . first ; Poly neiPoly = tileAndPoly . second ; if ( filter . passFilter ( link . ref , neiTile , neiPoly ) ) { solid = false ; } } break ; } } if ( ! solid ) { continue ; } } else if ( bestPoly . neis [ j ] != 0 ) { int idx = ( bestPoly . neis [ j ] - 1 ) ; long ref = m_nav . getPolyRefBase ( bestTile ) | idx ; if ( filter . passFilter ( ref , bestTile , bestTile . data . polys [ idx ] ) ) { continue ; } } int vj = bestPoly . verts [ j ] * 3 ; int vi = bestPoly . verts [ i ] * 3 ; Tupple2 < Float , Float > distseg = distancePtSegSqr2D ( centerPos , bestTile . data . verts , vj , vi ) ; float distSqr = distseg . first ; float tseg = distseg . second ; if ( distSqr > radiusSqr ) { continue ; } radiusSqr = distSqr ; hitPos [ 0 ] = bestTile . data . verts [ vj ] + ( bestTile . data . verts [ vi ] - bestTile . data . verts [ vj ] ) * tseg ; hitPos [ 1 ] = bestTile . data . verts [ vj + 1 ] + ( bestTile . data . verts [ vi + 1 ] - bestTile . data . verts [ vj + 1 ] ) * tseg ; hitPos [ 2 ] = bestTile . data . verts [ vj + 2 ] + ( bestTile . data . verts [ vi + 2 ] - bestTile . data . verts [ vj + 2 ] ) * tseg ; bestvj = new VectorPtr ( bestTile . data . verts , vj ) ; bestvi = new VectorPtr ( bestTile . data . verts , vi ) ; } for ( int i = bestPoly . firstLink ; i != NavMesh . DT_NULL_LINK ; i = bestTile . links . get ( i ) . next ) { Link link = bestTile . links . get ( i ) ; long neighbourRef = link . ref ; if ( neighbourRef == 0 || neighbourRef == parentRef ) { continue ; } tileAndPoly = m_nav . getTileAndPolyByRefUnsafe ( neighbourRef ) ; MeshTile neighbourTile = tileAndPoly . first ; Poly neighbourPoly = tileAndPoly . second ; if ( neighbourPoly . getType ( ) == Poly . DT_POLYTYPE_OFFMESH_CONNECTION ) { continue ; } int va = bestPoly . verts [ link . edge ] * 3 ; int vb = bestPoly . verts [ ( link . edge + 1 ) % bestPoly . vertCount ] * 3 ; Tupple2 < Float , Float > distseg = distancePtSegSqr2D ( centerPos , bestTile . data . verts , va , vb ) ; float distSqr = distseg . first ; if ( distSqr > radiusSqr ) { continue ; } if ( ! filter . passFilter ( neighbourRef , neighbourTile , neighbourPoly ) ) { continue ; } Node neighbourNode = m_nodePool . getNode ( neighbourRef ) ; if ( ( neighbourNode . flags & Node . DT_NODE_CLOSED ) != 0 ) { continue ; } if ( neighbourNode . flags == 0 ) { Result < float [ ] > midPoint = getEdgeMidPoint ( bestRef , bestPoly , bestTile , neighbourRef , neighbourPoly , neighbourTile ) ; if ( midPoint . succeeded ( ) ) { neighbourNode . pos = midPoint . result ; } } float total = bestNode . total + vDist ( bestNode . pos , neighbourNode . pos ) ; if ( ( neighbourNode . flags & Node . DT_NODE_OPEN ) != 0 && total >= neighbourNode . total ) { continue ; } neighbourNode . id = neighbourRef ; neighbourNode . flags = ( neighbourNode . flags & ~ Node . DT_NODE_CLOSED ) ; neighbourNode . pidx = m_nodePool . getNodeIdx ( bestNode ) ; neighbourNode . total = total ; if ( ( neighbourNode . flags & Node . DT_NODE_OPEN ) != 0 ) { m_openList . modify ( neighbourNode ) ; } else { neighbourNode . flags |= Node . DT_NODE_OPEN ; m_openList . push ( neighbourNode ) ; } } } float [ ] hitNormal = new float [ 3 ] ; if ( bestvi != null && bestvj != null ) { float [ ] tangent = vSub ( bestvi , bestvj ) ; hitNormal [ 0 ] = tangent [ 2 ] ; hitNormal [ 1 ] = 0 ; hitNormal [ 2 ] = - tangent [ 0 ] ; vNormalize ( hitNormal ) ; } return Result . success ( new FindDistanceToWallResult ( ( float ) Math . sqrt ( radiusSqr ) , hitPos , hitNormal ) ) ; }
/ @returns The status flags for the query.