RobotApp/RobotApp.Common.Shares/MathExtensions.cs

using RobotApp.Common.Shares.Model;

namespace RobotApp.Common.Shares;

public static class MathExtensions
{
    public static double NormalizeAngle(double angle)
    {
        angle = angle % 360;
        if (angle > 180)  angle -= 360;
        else if (angle < -180) angle += 360;
        return angle;
    }

    public static (double x, double y) CurveDegreeTwo(double t, double x1, double y1, double controlPointX, double controlPointY, double x2, double y2)
    {
        var x = (1 - t) * (1 - t) * x1 + 2 * t * (1 - t) * controlPointX + t * t * x2;
        var y = (1 - t) * (1 - t) * y1 + 2 * t * (1 - t) * controlPointY + t * t * y2;
        return (x, y);
    }

    public static (double x, double y) CurveDegreeThree(double t, double x1, double y1, double controlPoint1X, double controlPoint1Y, double controlPoint2X, double controlPoint2Y, double x2, double y2)
    {
        var x = Math.Pow(1 - t, 3) * x1 + 3 * Math.Pow(1 - t, 2) * t * controlPoint1X + 3 * Math.Pow(t, 2) * (1 - t) * controlPoint2X + Math.Pow(t, 3) * x2; ;
        var y = Math.Pow(1 - t, 3) * y1 + 3 * Math.Pow(1 - t, 2) * t * controlPoint1Y + 3 * Math.Pow(t, 2) * (1 - t) * controlPoint2Y + Math.Pow(t, 3) * y2;
        return (x, y);
    }


    public static (double x, double y) Curve(double t, EdgeCalculatorModel edge)
    {
        if (edge.TrajectoryDegree == Enums.TrajectoryDegree.One)
        {
            return (edge.X1 + t * (edge.X2 - edge.X1), edge.Y1 + t * (edge.Y2 - edge.Y1));
        }
        else if (edge.TrajectoryDegree == Enums.TrajectoryDegree.Two)
        {
            return CurveDegreeTwo(t, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.X2, edge.Y2);
        }
        else
        {
            return CurveDegreeThree(t, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.ControlPoint2X, edge.ControlPoint2Y, edge.X2, edge.Y2);
        }
    }

    public static (double x, double y) Curve(this EdgeCalculatorModel edge, double t)
    {
        return Curve(t, edge);
    }

    public static double GetEdgeLength(this EdgeCalculatorModel edge)
    {
        double distance = 0;
        if (edge.TrajectoryDegree == Enums.TrajectoryDegree.One)
        {
            distance = Math.Sqrt(Math.Pow(edge.X1 - edge.X2, 2) + Math.Pow(edge.Y1 - edge.Y2, 2));
        }
        else if (edge.TrajectoryDegree == Enums.TrajectoryDegree.Two)
        {
            var length = Math.Sqrt(Math.Pow(edge.X1 - edge.X2, 2) + Math.Pow(edge.Y1 - edge.Y2, 2));
            if (length == 0) return 0;
            double step = 0.1 / length;

            for (double t = step; t <= 1.001; t += step)
            {
                (double x1, double y1) = CurveDegreeTwo(t - step, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.X2, edge.Y2);
                (double x2, double y2) = CurveDegreeTwo(t, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.X2, edge.Y2);
                distance += Math.Sqrt(Math.Pow(x1 - x2, 2) + Math.Pow(y1 - y2, 2));
            }
        }
        else
        {
            var length = Math.Sqrt(Math.Pow(edge.X1 - edge.X2, 2) + Math.Pow(edge.Y1 - edge.Y2, 2));
            if (length == 0) return 0;
            double step = 0.1 / length;
            for (double t = step; t <= 1.001; t += step)
            {
                var sTime = t - step;
                (var sx, var sy) = CurveDegreeThree(1 - sTime, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.ControlPoint2X, edge.ControlPoint2Y, edge.X2, edge.Y2);
                sTime = t;
                (var ex, var ey) = CurveDegreeThree(1 - sTime, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.ControlPoint2X, edge.ControlPoint2Y, edge.X2, edge.Y2);

                distance += Math.Sqrt(Math.Pow(sx - ex, 2) + Math.Pow(sy - ey, 2));
            }
        }
        return distance;
    }

    public static double GetVectorAngle(double originNodeX, double originNodeY, double vector1X, double vector1Y, double vector2X, double vector2Y)
    {
        double BA_x = vector1X - originNodeX;
        double BA_y = vector1Y - originNodeY;
        double BC_x = vector2X - originNodeX;
        double BC_y = vector2Y - originNodeY;
        // Tính độ dài của các vector AB và BC
        double lengthAB = Math.Sqrt(BA_x * BA_x + BA_y * BA_y);
        double lengthBC = Math.Sqrt(BC_x * BC_x + BC_y * BC_y);
        // Tính tích vô hướng của AB và BC
        double dotProduct = BA_x * BC_x + BA_y * BC_y;
        if (lengthAB * lengthBC == 0) return 0;
        if (dotProduct / (lengthAB * lengthBC) > 1) return 0;
        if (dotProduct / (lengthAB * lengthBC) < -1) return 180;
        return Math.Acos(dotProduct / (lengthAB * lengthBC)) * (180.0 / Math.PI);
    }

    public static double Distance(double x1, double y1, double x2, double y2)
    {
        return Math.Sqrt(Math.Pow(x2 - x1, 2) + Math.Pow(y2 - y1, 2));
    }
}