APM/Assets/Scripting/Robot/PathVisualizer.cs

using System.Collections.Generic;
using System.Linq;
using UnityEngine;

public class PathVisualizer
{
    private readonly Material lineMaterial;
    private readonly Material nodeMaterial;
    private readonly Transform transform;
    private readonly List<GameObject> nodeObjects = new();
    private GameObject lineRendererObject;
    private readonly int trajectoryResolution;

    public PathVisualizer(Material lineMaterial, Material nodeMaterial, Transform transform, int trajectoryResolution)
    {
        this.lineMaterial = lineMaterial;
        this.nodeMaterial = nodeMaterial;
        this.transform = transform;
        this.trajectoryResolution = trajectoryResolution;
    }

    public void DrawPath(Node[] nodes, Edge[] edges)
    {
        if (lineRendererObject != null)
        {
            Object.Destroy(lineRendererObject);
        }
        foreach (var node in nodeObjects)
        {
            if (node != null) Object.Destroy(node); // Kiểm tra null để an toàn
        }
        nodeObjects.Clear();

        lineRendererObject = new GameObject("PathLineRenderer");
        LineRenderer lineRenderer = lineRendererObject.AddComponent<LineRenderer>();
        lineRenderer.material = lineMaterial != null ? lineMaterial : new Material(Shader.Find("Sprites/Default"));
        lineRenderer.startWidth = 0.1f;
        lineRenderer.endWidth = 0.1f;
        lineRenderer.startColor = Color.green;
        lineRenderer.endColor = Color.green;

        var sortedEdges = edges.OrderBy(e => e.SequenceId).ToList();
        List<Vector3> pathPoints = new();

        foreach (var edge in sortedEdges)
        {
            if (edge.Trajectory == null || edge.Trajectory.ControlPoints == null || edge.Trajectory.ControlPoints.Length == 0)
            {
                continue;
            }

            if (edge.Trajectory.Degree == 1)
            {
                foreach (var point in edge.Trajectory.ControlPoints)
                {
                    Vector3 pos = new(point.X, transform.position.y, point.Y);
                    pathPoints.Add(pos);
                }
            }
            else
            {
                var points = GenerateBSplinePoints(edge.Trajectory, trajectoryResolution);
                if (points.Count > 0)
                {
                    var lastControlPoint = edge.Trajectory.ControlPoints.Last();
                    points[^1] = new Vector3(lastControlPoint.X, transform.position.y, lastControlPoint.Y);
                    pathPoints.AddRange(points);
                }
            }
        }

        pathPoints.RemoveAll(p => p.x == 0 && p.z == 0);
        if (pathPoints.Count == 0)
        {
            Debug.LogError("Không có điểm hợp lệ để vẽ đường đi!");
            lineRenderer.positionCount = 0;
            return;
        }

        lineRenderer.positionCount = pathPoints.Count;
        lineRenderer.SetPositions(pathPoints.ToArray());

        foreach (var node in nodes.OrderBy(n => n.SequenceId))
        {
            GameObject nodeObject = GameObject.CreatePrimitive(PrimitiveType.Sphere);
            nodeObject.transform.position = new Vector3(node.NodePosition.X, transform.position.y, node.NodePosition.Y);
            nodeObject.transform.localScale = Vector3.one * 0.1f;
            nodeObject.name = $"Node_{node.NodeDescription}";
            nodeObject.GetComponent<Renderer>().material = nodeMaterial != null ? nodeMaterial : new Material(Shader.Find("Sprites/Default"));
            nodeObject.GetComponent<Renderer>().material.color = Color.red;
            nodeObjects.Add(nodeObject);
        }
    }

    public void ClearPath()
    {
        if (lineRendererObject != null)
        {
            Object.Destroy(lineRendererObject);
            lineRendererObject = null;
            Debug.Log("Đã xóa lineRendererObject.");
        }
        foreach (var node in nodeObjects)
        {
            if (node != null) Object.Destroy(node); // Khôi phục lệnh xóa node
        }
        nodeObjects.Clear();
    }

    private List<Vector3> GenerateBSplinePoints(Trajectory trajectory, int resolution)
    {
        List<Vector3> points = new();
        int degree = trajectory.Degree;
        float[] knots = trajectory.KnotVector;
        ControlPoint[] controlPoints = trajectory.ControlPoints;

        if (controlPoints == null || controlPoints.Length < degree + 1 || knots == null || knots.Length < controlPoints.Length + degree + 1)
        {
            Debug.LogError($"Dữ liệu Trajectory không hợp lệ: Degree={degree}, ControlPoints={controlPoints?.Length}, Knots={knots?.Length}");
            return points;
        }

        float minU = knots[degree];
        float maxU = knots[controlPoints.Length];
        if (minU >= maxU)
        {
            Debug.LogError($"KnotVector không hợp lệ: minU={minU}, maxU={maxU}");
            return points;
        }

        for (int i = 0; i <= resolution; i++)
        {
            float u = minU + (maxU - minU) * i / (float)resolution;
            Vector2 point = CalculateBSplinePoint(u, degree, knots, controlPoints);
            if (point != Vector2.zero)
            {
                Vector3 pos = new(point.x, transform.position.y, point.y);
                points.Add(pos);
            }
        }

        return points;
    }

    private Vector2 CalculateBSplinePoint(float u, int degree, float[] knots, ControlPoint[] controlPoints)
    {
        Vector2 point = Vector2.zero;
        int n = controlPoints.Length - 1;

        for (int i = 0; i <= n; i++)
        {
            float basis = CalculateBasisFunction(i, degree, u, knots);
            point += new Vector2(controlPoints[i].X, controlPoints[i].Y) * basis;
        }

        return point;
    }

    private float CalculateBasisFunction(int i, int degree, float u, float[] knots)
    {
        if (degree == 0)
        {
            return (u >= knots[i] && u < knots[i + 1]) ? 1f : 0f;
        }

        float left = 0f;
        float right = 0f;

        float denom1 = knots[i + degree] - knots[i];
        if (denom1 > 0)
        {
            left = ((u - knots[i]) / denom1) * CalculateBasisFunction(i, degree - 1, u, knots);
        }

        float denom2 = knots[i + degree + 1] - knots[i + 1];
        if (denom2 > 0)
        {
            right = ((knots[i + degree + 1] - u) / denom2) * CalculateBasisFunction(i + 1, degree - 1, u, knots);
        }

        return left + right;
    }
}