unity2moveit2/unity-project/Assets/Scripts/Core/VisualizationEngine.Workspace.cs

using UnityEngine;

namespace UnityMoveIt2.Core
{
    /// <summary>
    /// 可视化引擎 - 工作空间可视化部分 / Visualization Engine - Workspace Visualization Part
    /// </summary>
    public partial class VisualizationEngine
    {
        #region Workspace Fields

        [Header("工作空间可视化设置 / Workspace Visualization Settings")]
        [SerializeField]
        private Color workspaceColor = new Color(0f, 0.5f, 1f, 0.2f);

        [SerializeField]
        [Tooltip("工作空间透明度 / Workspace Transparency")]
        private float workspaceTransparency = 0.2f;

        [SerializeField]
        [Tooltip("显示工作空间网格 / Show Workspace Grid")]
        private bool showWorkspaceGrid = true;

        [SerializeField]
        [Tooltip("网格大小 / Grid Size")]
        private float gridSize = 0.1f;

        [SerializeField]
        private Color gridColor = new Color(0.8f, 0.8f, 0.8f, 0.5f);

        [SerializeField]
        [Tooltip("工作空间边界类型 / Workspace Boundary Type")]
        private WorkspaceBoundaryType boundaryType = WorkspaceBoundaryType.Sphere;

        private GameObject workspaceBoundary;
        private GameObject workspaceGrid;
        private MeshRenderer boundaryRenderer;
        private Vector3 workspaceMin = new Vector3(-0.5f, -0.5f, -0.1f);
        private Vector3 workspaceMax = new Vector3(0.5f, 0.5f, 0.8f);

        #endregion

        #region Workspace Initialization

        private void InitializeWorkspaceVisualization()
        {
            CreateWorkspaceBoundary();

            if (showWorkspaceGrid)
            {
                CreateWorkspaceGrid();
            }

            Debug.Log("[VisualizationEngine] 工作空间可视化已初始化 / Workspace visualization initialized");
        }

        private void CreateWorkspaceBoundary()
        {
            workspaceBoundary = GetOrCreateVisualizationObject("WorkspaceBoundary", workspaceContainer.transform);

            // 根据边界类型创建网格 / Create mesh based on boundary type
            MeshFilter meshFilter = workspaceBoundary.GetComponent<MeshFilter>();
            if (meshFilter == null)
            {
                meshFilter = workspaceBoundary.AddComponent<MeshFilter>();
            }

            switch (boundaryType)
            {
                case WorkspaceBoundaryType.Sphere:
                    meshFilter.mesh = CreateSphereMesh();
                    break;
                case WorkspaceBoundaryType.Box:
                    meshFilter.mesh = CreateBoxMesh();
                    break;
                case WorkspaceBoundaryType.Cylinder:
                    meshFilter.mesh = CreateCylinderMesh();
                    break;
            }

            // 创建材质 / Create material
            boundaryRenderer = workspaceBoundary.GetComponent<MeshRenderer>();
            if (boundaryRenderer == null)
            {
                boundaryRenderer = workspaceBoundary.AddComponent<MeshRenderer>();
            }

            Material mat = CreateTransparentMaterial(workspaceColor, workspaceTransparency);
            boundaryRenderer.material = mat;

            // 设置位置 / Set position
            if (robotAdapter != null)
            {
                workspaceBoundary.transform.position = robotAdapter.BaseLink.position;
            }
        }

        private Mesh CreateSphereMesh()
        {
            GameObject sphere = GameObject.CreatePrimitive(PrimitiveType.Sphere);
            Mesh mesh = sphere.GetComponent<MeshFilter>().mesh;
            Destroy(sphere);
            return mesh;
        }

        private Mesh CreateBoxMesh()
        {
            GameObject box = GameObject.CreatePrimitive(PrimitiveType.Cube);
            Mesh mesh = box.GetComponent<MeshFilter>().mesh;
            Destroy(box);
            return mesh;
        }

        private Mesh CreateCylinderMesh()
        {
            GameObject cylinder = GameObject.CreatePrimitive(PrimitiveType.Cylinder);
            Mesh mesh = cylinder.GetComponent<MeshFilter>().mesh;
            Destroy(cylinder);
            return mesh;
        }

        private void CreateWorkspaceGrid()
        {
            workspaceGrid = GetOrCreateVisualizationObject("WorkspaceGrid", workspaceContainer.transform);

            // 创建网格线 / Create grid lines
            LineRenderer[] gridLines = CreateGridLines();

            foreach (var line in gridLines)
            {
                line.transform.SetParent(workspaceGrid.transform);
            }
        }

        private LineRenderer[] CreateGridLines()
        {
            Vector3 size = workspaceMax - workspaceMin;
            int xLines = Mathf.CeilToInt(size.x / gridSize);
            int zLines = Mathf.CeilToInt(size.z / gridSize);

            LineRenderer[] lines = new LineRenderer[xLines + zLines + 2];
            int index = 0;

            // X方向线 / X-direction lines
            for (int i = 0; i <= xLines; i++)
            {
                float x = workspaceMin.x + i * gridSize;
                GameObject lineObj = new GameObject($"GridLine_X_{i}");
                LineRenderer line = lineObj.AddComponent<LineRenderer>();

                ConfigureGridLine(line, gridColor);
                line.SetPosition(0, new Vector3(x, workspaceMin.y, workspaceMin.z));
                line.SetPosition(1, new Vector3(x, workspaceMin.y, workspaceMax.z));

                lines[index++] = line;
            }

            // Z方向线 / Z-direction lines
            for (int i = 0; i <= zLines; i++)
            {
                float z = workspaceMin.z + i * gridSize;
                GameObject lineObj = new GameObject($"GridLine_Z_{i}");
                LineRenderer line = lineObj.AddComponent<LineRenderer>();

                ConfigureGridLine(line, gridColor);
                line.SetPosition(0, new Vector3(workspaceMin.x, workspaceMin.y, z));
                line.SetPosition(1, new Vector3(workspaceMax.x, workspaceMin.y, z));

                lines[index++] = line;
            }

            return lines;
        }

        private void ConfigureGridLine(LineRenderer line, Color color)
        {
            line.startWidth = 0.002f;
            line.endWidth = 0.002f;
            line.material = new Material(Shader.Find("Sprites/Default"));
            line.startColor = color;
            line.endColor = color;
            line.positionCount = 2;
            line.useWorldSpace = true;
        }

        #endregion

        #region Workspace Update

        private void UpdateWorkspaceVisualization()
        {
            if (workspaceBoundary == null)
                return;

            // 更新工作空间位置 / Update workspace position
            if (robotAdapter != null)
            {
                workspaceBoundary.transform.position = robotAdapter.BaseLink.position;

                if (workspaceGrid != null)
                {
                    workspaceGrid.transform.position = robotAdapter.BaseLink.position;
                }
            }

            // 根据机器人配置调整大小 / Adjust size based on robot configuration
            UpdateWorkspaceSize();
        }

        private void UpdateWorkspaceSize()
        {
            if (robotAdapter == null || workspaceBoundary == null)
                return;

            float radius = robotAdapter.DOF * 0.15f; // 简化计算 / Simplified calculation
            Vector3 size = workspaceMax - workspaceMin; // 计算工作空间尺寸 / Calculate workspace size

            switch (boundaryType)
            {
                case WorkspaceBoundaryType.Sphere:
                    workspaceBoundary.transform.localScale = Vector3.one * radius * 2;
                    break;

                case WorkspaceBoundaryType.Box:
                    workspaceBoundary.transform.localScale = size;
                    break;

                case WorkspaceBoundaryType.Cylinder:
                    workspaceBoundary.transform.localScale = new Vector3(radius * 2, size.y / 2, radius * 2);
                    break;
            }
        }

        #endregion

        #region Public Workspace Methods

        /// <summary>
        /// 设置工作空间边界 / Set Workspace Boundaries
        /// </summary>
        public void SetWorkspaceBoundaries(Vector3 min, Vector3 max)
        {
            workspaceMin = min;
            workspaceMax = max;

            UpdateWorkspaceSize();

            if (showWorkspaceGrid && workspaceGrid != null)
            {
                // 重新创建网格 / Recreate grid
                Destroy(workspaceGrid);
                CreateWorkspaceGrid();
            }
        }

        /// <summary>
        /// 设置工作空间边界类型 / Set Workspace Boundary Type
        /// </summary>
        public void SetWorkspaceBoundaryType(WorkspaceBoundaryType type)
        {
            boundaryType = type;

            if (workspaceBoundary != null)
            {
                Destroy(workspaceBoundary);
                CreateWorkspaceBoundary();
            }
        }

        /// <summary>
        /// 清除工作空间可视化 / Clear Workspace Visualization
        /// </summary>
        public void ClearWorkspaceVisualization()
        {
            if (workspaceBoundary != null)
            {
                Destroy(workspaceBoundary);
                workspaceBoundary = null;
            }

            if (workspaceGrid != null)
            {
                Destroy(workspaceGrid);
                workspaceGrid = null;
            }

            Debug.Log("[VisualizationEngine] 已清除工作空间可视化 / Cleared workspace visualization");
        }

        /// <summary>
        /// 设置网格显示 / Set Grid Display
        /// </summary>
        public void SetShowGrid(bool show)
        {
            showWorkspaceGrid = show;

            if (workspaceGrid != null)
            {
                workspaceGrid.SetActive(show);
            }
            else if (show)
            {
                CreateWorkspaceGrid();
            }
        }

        /// <summary>
        /// 获取工作空间边界 / Get Workspace Boundaries
        /// </summary>
        public void GetWorkspaceBoundaries(out Vector3 min, out Vector3 max)
        {
            min = workspaceMin;
            max = workspaceMax;
        }

        #endregion

        #region Workspace Gizmos

        private void DrawWorkspaceGizmos()
        {
            Gizmos.color = workspaceColor;

            if (robotAdapter == null)
                return;

            Vector3 basePos = robotAdapter.BaseLink.position;

            switch (boundaryType)
            {
                case WorkspaceBoundaryType.Sphere:
                    float radius = Vector3.Distance(workspaceMin, workspaceMax) / 2;
                    Gizmos.DrawWireSphere(basePos, radius);
                    break;

                case WorkspaceBoundaryType.Box:
                    Vector3 center = basePos + (workspaceMin + workspaceMax) / 2;
                    Vector3 size = workspaceMax - workspaceMin;
                    Gizmos.DrawWireCube(center, size);
                    break;

                case WorkspaceBoundaryType.Cylinder:
                    // 简化为球体显示 / Simplified as sphere display
                    Gizmos.DrawWireSphere(basePos, (workspaceMax.x - workspaceMin.x) / 2);
                    break;
            }
        }

        #endregion
    }

    #region Workspace Enums

    /// <summary>
    /// 工作空间边界类型 / Workspace Boundary Type
    /// </summary>
    public enum WorkspaceBoundaryType
    {
        Sphere,
        Box,
        Cylinder
    }

    #endregion
}