ls_unity/Scripts/URDF/URDFMeshResolver.cs

using System.Collections.Generic;
using System.IO;
using UnityEngine;
#if UNITY_EDITOR
using UnityEditor;
#endif

namespace URDF
{
    /// <summary>
    /// Utility class for resolving URDF package:// URIs to Unity asset paths
    /// </summary>
    public static class URDFMeshResolver
    {
        /// <summary>
        /// Resolve a URDF mesh filename (package:// URI) to a Unity asset path
        /// </summary>
        /// <param name="urdfFilename">URDF filename like "package://Little_Sophia_Face/meshes/Little_Sophia_Torso_.obj"</param>
        /// <param name="urdfFilePath">Path to the URDF file for relative resolution</param>
        /// <returns>Unity asset path relative to Assets folder, or null if not found</returns>
        public static string ResolveMeshPath(string urdfFilename, string urdfFilePath)
        {
            if (string.IsNullOrEmpty(urdfFilename))
            {
                Debug.LogWarning("URDF mesh filename is null or empty");
                return null;
            }

            // Handle package:// URIs
            if (urdfFilename.StartsWith("package://"))
            {
                // Remove package:// prefix
                string relativePath = urdfFilename.Substring("package://".Length);
                
                // Extract just the filename from the package path
                string meshFileName = Path.GetFileName(relativePath);
                string originalExtension = Path.GetExtension(meshFileName).ToLower();
                string fileNameWithoutExt = Path.GetFileNameWithoutExtension(meshFileName);
                
                Debug.Log($"Resolving mesh: {urdfFilename} -> looking for {meshFileName} (extension: {originalExtension})");
                
                // Build list of extensions to try: first the one specified in URDF, then fallbacks
                List<string> extensionsToTry = new List<string>();
                if (!string.IsNullOrEmpty(originalExtension))
                {
                    extensionsToTry.Add(originalExtension); // Try the specified extension first
                }
                // Add fallback extensions (excluding the one we already added)
                string[] fallbackExtensions = { ".obj", ".glb", ".fbx" };
                foreach (string ext in fallbackExtensions)
                {
                    if (ext != originalExtension)
                    {
                        extensionsToTry.Add(ext);
                    }
                }
                
                // Try direct resolution first: robots/LittleSophia/meshes/filename
                // This is the most reliable method
                foreach (string ext in extensionsToTry)
                {
                    string testFileName = fileNameWithoutExt + ext;
                    string directPath = $"robots/LittleSophia/meshes/{testFileName}";
                    #if UNITY_EDITOR
                    // Normalize path separators for Unity (always use forward slashes)
                    directPath = directPath.Replace('\\', '/');
                    
                    string fullPath = Path.Combine(Application.dataPath, directPath);
                    fullPath = fullPath.Replace('/', Path.DirectorySeparatorChar);
                    
                    if (File.Exists(fullPath))
                    {
                        Debug.Log($"Found mesh at: {directPath} (full path: {fullPath})");
                        return directPath;
                    }
                    else
                    {
                        Debug.Log($"Checking for mesh: {fullPath} - File does not exist");
                    }
                    #endif
                }
                
                // Fallback: Try to resolve relative to the URDF file location
                string urdfDir = Path.GetDirectoryName(urdfFilePath);
                if (!string.IsNullOrEmpty(urdfDir))
                {
                    string meshesDir = Path.Combine(urdfDir, "..", "meshes");
                    meshesDir = Path.GetFullPath(meshesDir);
                    
                    foreach (string ext in extensionsToTry)
                    {
                        string testFileName = fileNameWithoutExt + ext;
                        string meshPath = Path.Combine(meshesDir, testFileName);
                        
                        if (File.Exists(meshPath))
                        {
                            // Convert to Unity asset path (relative to Assets folder)
                            string assetsPath = "Assets" + meshPath.Replace(Application.dataPath, "").Replace('\\', '/');
                            Debug.Log($"Found mesh at: {assetsPath}");
                            return assetsPath;
                        }
                    }
                }
            }
            // Handle relative paths
            else if (!Path.IsPathRooted(urdfFilename))
            {
                string urdfDir = Path.GetDirectoryName(urdfFilePath);
                string meshPath = Path.Combine(urdfDir, urdfFilename);
                meshPath = Path.GetFullPath(meshPath);
                
                if (File.Exists(meshPath))
                {
                    string assetsPath = "Assets" + meshPath.Replace(Application.dataPath, "").Replace('\\', '/');
                    return assetsPath;
                }
            }
            // Handle absolute paths
            else
            {
                if (File.Exists(urdfFilename))
                {
                    string assetsPath = "Assets" + urdfFilename.Replace(Application.dataPath, "").Replace('\\', '/');
                    return assetsPath;
                }
            }

            Debug.LogWarning($"Could not resolve mesh path: {urdfFilename} (URDF file: {urdfFilePath})");
            return null;
        }

        /// <summary>
        /// Load a mesh from a Unity asset path
        /// </summary>
        public static Mesh LoadMesh(string assetPath)
        {
            if (string.IsNullOrEmpty(assetPath))
                return null;

            #if UNITY_EDITOR
            // Try loading as a mesh asset
            Mesh mesh = AssetDatabase.LoadAssetAtPath<Mesh>(assetPath);
            if (mesh != null)
                return mesh;

            // Try loading as a model (FBX, OBJ, etc.) - Unity imports these as GameObjects
            GameObject prefab = AssetDatabase.LoadAssetAtPath<GameObject>(assetPath);
            if (prefab != null)
            {
                // Extract mesh from the prefab
                MeshFilter meshFilter = prefab.GetComponentInChildren<MeshFilter>();
                if (meshFilter != null && meshFilter.sharedMesh != null)
                {
                    return meshFilter.sharedMesh;
                }
            }
            #endif

            Debug.LogWarning($"Could not load mesh from: {assetPath}");
            return null;
        }
    }
}