Wrong path is generated by FlipEdgeNetwork::iterativeShorten #258

#include <algorithm>
#include <cmath>
#include <iostream>
#include <memory>
#include <string>
#include <vector>

#include "geometrycentral/surface/edge_length_geometry.h"
#include "geometrycentral/surface/flip_geodesics.h"
#include "geometrycentral/surface/manifold_surface_mesh.h"
#include "geometrycentral/surface/mesh_graph_algorithms.h"
#include "geometrycentral/surface/meshio.h"
#include "geometrycentral/surface/vertex_position_geometry.h"

namespace GC = geometrycentral;
namespace GCS = geometrycentral::surface;

// Converts a list of contiguous vertex indices into a Halfedge path
std::vector<GCS::Halfedge> build_halfedge_path(
    GCS::ManifoldSurfaceMesh &mesh,
    const std::vector<size_t> &vIndices ) {
    std::vector<GCS::Halfedge> hePath;
    if ( vIndices.size() < 2 )
        return hePath;

    for ( size_t i = 0; i < vIndices.size() - 1; ++i ) {
        GCS::Vertex v1 = mesh.vertex( vIndices[i] );
        GCS::Vertex v2 = mesh.vertex( vIndices[i + 1] );

        GCS::Halfedge targetHe;
        for ( GCS::Halfedge he : v1.outgoingHalfedges() ) {
            if ( he.tipVertex() == v2 ) {
                targetHe = he;
                break;
            }
        }

        if ( !targetHe.getMesh() ) {
            std::cerr << "Error: No direct halfedge found from V" << vIndices[i]
                      << " to V" << vIndices[i + 1] << "\n";
            return {};
        }
        hePath.push_back( targetHe );
    }
    return hePath;
}

// Helper to find candidate faces for a SurfacePoint
std::vector<GCS::Face> getCandidateFaces( const GCS::SurfacePoint &p ) {
    std::vector<GCS::Face> faces;
    if ( p.type == GCS::SurfacePointType::Face ) {
        faces.push_back( p.face );
    } else if ( p.type == GCS::SurfacePointType::Edge ) {
        GCS::Halfedge he = p.edge.halfedge();
        if ( he.isInterior() )
            faces.push_back( he.face() );
        if ( he.twin().isInterior() )
            faces.push_back( he.twin().face() );
    } else if ( p.type == GCS::SurfacePointType::Vertex ) {
        for ( GCS::Face f : p.vertex.adjacentFaces() ) {
            faces.push_back( f );
        }
    }
    return faces;
}

// Helper to find a shared face between two adjacent SurfacePoints
GCS::Face findSharedFace( const GCS::SurfacePoint &p1, const GCS::SurfacePoint &p2 ) {
    auto faces1 = getCandidateFaces( p1 );
    auto faces2 = getCandidateFaces( p2 );
    for ( GCS::Face f1 : faces1 ) {
        for ( GCS::Face f2 : faces2 ) {
            if ( f1 == f2 )
                return f1;
        }
    }
    return GCS::Face();
}

// Helper to get barycentric coordinates of SurfacePoint relative to face f
GC::Vector3 getBarycentricInFace( const GCS::SurfacePoint &p, GCS::Face f ) {
    GCS::Halfedge he0 = f.halfedge();
    GCS::Halfedge he1 = he0.next();
    GCS::Halfedge he2 = he1.next();

    if ( p.type == GCS::SurfacePointType::Vertex ) {
        if ( p.vertex == he0.vertex() )
            return { 1.0, 0.0, 0.0 };
        if ( p.vertex == he1.vertex() )
            return { 0.0, 1.0, 0.0 };
        if ( p.vertex == he2.vertex() )
            return { 0.0, 0.0, 1.0 };
    } else if ( p.type == GCS::SurfacePointType::Edge ) {
        if ( p.edge == he0.edge() ) {
            double t = ( p.edge.halfedge() == he0 ) ? p.tEdge : ( 1.0 - p.tEdge );
            return { 1.0 - t, t, 0.0 };
        }
        if ( p.edge == he1.edge() ) {
            double t = ( p.edge.halfedge() == he1 ) ? p.tEdge : ( 1.0 - p.tEdge );
            return { 0.0, 1.0 - t, t };
        }
        if ( p.edge == he2.edge() ) {
            double t = ( p.edge.halfedge() == he2 ) ? p.tEdge : ( 1.0 - p.tEdge );
            return { t, 0.0, 1.0 - t };
        }
    } else if ( p.type == GCS::SurfacePointType::Face ) {
        if ( p.face == f )
            return p.faceCoords;
    }
    return { 0.0, 0.0, 0.0 };
}

// Seam-safe local face UV interpolation
GC::Vector2 get_uv_in_face( const GCS::SurfacePoint &p, GCS::Face f, const GCS::CornerData<GC::Vector2> &uvs ) {
    GCS::Halfedge he0 = f.halfedge();
    GCS::Halfedge he1 = he0.next();
    GCS::Halfedge he2 = he1.next();

    GC::Vector3 b = getBarycentricInFace( p, f );
    return b.x * uvs[he0.corner()] + b.y * uvs[he1.corner()] + b.z * uvs[he2.corner()];
}

int main() {
    bool reversePath = 1;

    std::string meshFilename = "c:/prj/test_data/relative/_tmp/8/8a/buste.obj";
    std::vector<size_t> pathVerts = { 46961, 46962, 46963, 50001, 49999, 47345, 50000, 47346, 49998, 46987, 46988 };

    if ( reversePath ) {
        std::cout << "[Info] Reversing input path sequence...\n";
        std::reverse( pathVerts.begin(), pathVerts.end() );
    }

    // 1. Load mesh, 3D positions, and corner UVs
    std::cout << "Loading mesh and UV coordinates: " << meshFilename << "...\n";
    std::unique_ptr<GCS::ManifoldSurfaceMesh> mesh;
    std::unique_ptr<GCS::VertexPositionGeometry> geometry;
    std::unique_ptr<GCS::CornerData<GC::Vector2>> uvs;

    std::tie( mesh, geometry, uvs ) = GCS::readParameterizedManifoldSurfaceMesh( meshFilename );

    if ( !uvs ) {
        std::cerr << "Error: OBJ file contains no UV coordinates.\n";
        return 1;
    }

    // 2. Compute 2D UV metric edge lengths for all mesh edges
    GCS::EdgeData<double> uvEdgeLengths( *mesh );
    for ( GCS::Edge e : mesh->edges() ) {
        GCS::Halfedge he = e.halfedge();
        GC::Vector2 uvTail = ( *uvs )[he.corner()];
        GC::Vector2 uvTip = ( *uvs )[he.next().corner()];
        uvEdgeLengths[e] = ( uvTip - uvTail ).norm();
    }

    // Wrap UV metric into EdgeLengthGeometry for FlipEdgeNetwork
    GCS::EdgeLengthGeometry uvGeometry( *mesh, uvEdgeLengths );

    // 3. Build input path
    std::vector<GCS::Halfedge> hePath = build_halfedge_path( *mesh, pathVerts );
    if ( hePath.empty() ) {
        std::cerr << "Failed to construct valid halfedge path.\n";
        return 1;
    }

    // 4. Print Input Path Edge Lengths
    std::cout << "\n=== Input Path Edge Lengths ===\n";
    std::cout << "Seg\tFrom -> To\t\tSurface Length (3D)\tUV Length (2D)\n";
    std::cout << "----------------------------------------------------------------------\n";

    double totalInputSurfaceLength = 0.0;
    double totalInputUVLength = 0.0;

    geometry->requireEdgeLengths();

    for ( size_t i = 0; i < hePath.size(); ++i ) {
        GCS::Halfedge he = hePath[i];
        GCS::Edge e = he.edge();

        double surfaceLen = geometry->edgeLengths[e];
        double uvLen = uvEdgeLengths[e];

        totalInputSurfaceLength += surfaceLen;
        totalInputUVLength += uvLen;

        std::cout << i + 1 << "\tV" << he.tailVertex().getIndex()
                  << " -> V" << he.tipVertex().getIndex()
                  << "\t\t" << surfaceLen
                  << "\t\t" << uvLen << "\n";
    }

    std::cout << "----------------------------------------------------------------------\n";
    std::cout << "Total Input Surface Length (3D): " << totalInputSurfaceLength << "\n";
    std::cout << "Total Input UV Length (2D):      " << totalInputUVLength << "\n\n";

    // 5. Initialize and run geodesic shortening using UV Metric
    std::cout << "Running FlipEdgeNetwork iterativeShorten() using UV Metric...\n";
    std::vector<std::vector<GCS::Halfedge>> gcPaths = { hePath };

    GCS::FlipEdgeNetwork edgeNet( *mesh, uvGeometry, gcPaths );
    edgeNet.posGeom = geometry.get(); // Kept for 3D coordinate evaluation
    edgeNet.addAllWedgesToAngleQueue();

    try {
        edgeNet.iterativeShorten();
        edgeNet.validate();
    } catch ( const std::exception &e ) {
        std::cerr << "GeometryCentral threw exception: " << e.what() << "\n";
        return 1;
    }

    // 6. Inspect geodesic path points and print edge lengths
    std::vector<std::vector<GCS::SurfacePoint>> refinedPaths = edgeNet.getPathPolyline();
    const auto &geodesicPath = refinedPaths[0];

    std::cout << "\n=== Shortened Geodesic Path Points (" << geodesicPath.size() << " points) ===\n";
    for ( size_t i = 0; i < geodesicPath.size(); ++i ) {
        const auto &sp = geodesicPath[i];
        if ( sp.type == GCS::SurfacePointType::Vertex ) {
            std::cout << "  [" << i << "] Vertex V" << sp.vertex.getIndex() << "\n";
        } else if ( sp.type == GCS::SurfacePointType::Edge ) {
            std::cout << "  [" << i << "] Edge E" << sp.edge.getIndex()
                      << " (t=" << sp.tEdge << ")\n";
        } else if ( sp.type == GCS::SurfacePointType::Face ) {
            std::cout << "  [" << i << "] Face F" << sp.face.getIndex()
                      << " (coords=[" << sp.faceCoords.x << ", "
                      << sp.faceCoords.y << ", "
                      << sp.faceCoords.z << "])\n";
        }
    }

    auto formatPointStr = []( const GCS::SurfacePoint &sp ) -> std::string {
        if ( sp.type == GCS::SurfacePointType::Vertex ) {
            return "V" + std::to_string( sp.vertex.getIndex() );
        } else if ( sp.type == GCS::SurfacePointType::Edge ) {
            return "E" + std::to_string( sp.edge.getIndex() ) + "(t=" + std::to_string( sp.tEdge ).substr( 0, 4 ) + ")";
        } else if ( sp.type == GCS::SurfacePointType::Face ) {
            return "F" + std::to_string( sp.face.getIndex() );
        }
        return "Unknown";
    };

    // Seam-safe UV distance calculation
    auto compute_uv_distance = [&]( const GCS::SurfacePoint &p1, const GCS::SurfacePoint &p2 ) -> double {
        GCS::Face f = findSharedFace( p1, p2 );
        if ( !f.getMesh() )
            return 0.0;

        GC::Vector2 uv1 = get_uv_in_face( p1, f, *uvs );
        GC::Vector2 uv2 = get_uv_in_face( p2, f, *uvs );

        return ( uv2 - uv1 ).norm();
    };

    std::cout << "\n=== Shortened Geodesic Path Edge Lengths ===\n";
    std::cout << "Seg\tFrom -> To\t\t\t\tSurface Length (3D)\tUV Length (2D)\n";
    std::cout << "--------------------------------------------------------------------------------\n";

    double totalGeodesicSurfaceLength = 0.0;
    double totalGeodesicUVLength = 0.0;

    for ( size_t i = 0; i < geodesicPath.size() - 1; ++i ) {
        const GCS::SurfacePoint &p1 = geodesicPath[i];
        const GCS::SurfacePoint &p2 = geodesicPath[i + 1];

        // 3D position distance
        GC::Vector3 pos1 = p1.interpolate( geometry->inputVertexPositions );
        GC::Vector3 pos2 = p2.interpolate( geometry->inputVertexPositions );
        double surfaceLen = ( pos2 - pos1 ).norm();

        // Seam-safe local 2D UV distance
        double uvLen = compute_uv_distance( p1, p2 );

        totalGeodesicSurfaceLength += surfaceLen;
        totalGeodesicUVLength += uvLen;

        std::string p1Label = formatPointStr( p1 );
        std::string p2Label = formatPointStr( p2 );

        std::cout << i + 1 << "\t" << p1Label << " -> " << p2Label
                  << "\t\t" << surfaceLen
                  << "\t\t" << uvLen << "\n";
    }

    std::cout << "--------------------------------------------------------------------------------\n";
    std::cout << "Total Geodesic Surface Length (3D): " << totalGeodesicSurfaceLength << "\n";
    std::cout << "Total Geodesic UV Length (2D):      " << totalGeodesicUVLength << "\n\n";

    return 0;
}