【算法】计算阶梯坐标转角（斜率）

在最近开发中遇到一个计算阶梯转角的问题，当转角时Z轴依次偏移一个单位

实现方式

本文采用X、Y进行斜率计算，如果是其他方向，可以自行更换

实现思路

• 既然时阶梯转角，至少需要有三个点位坐标；
• 1-2，2-3号点分别比较斜率，如果两条斜率乘积为**<0** ,则表示发生了变化，即发生了转弯
• 斜率公式：（y1-y2）/(x1-x2)

具体代码

/// <summary>
/// 传递三个坐标值
/// </summary>
public class TurnDetection
{
    
    
    public static List<Tuple<float, float, float>> lastPos = new List<Tuple<float, float, float>>();

    //向lastpos存储数据，3个以上开始判断
    public static bool TurnDetectionS(float x,float y,float z)
    {
    
    
        if (lastPos.Count < 3)
        {
    
    
            lastPos.Add(Tuple.Create(x,y,z));
            return false;
        }
        else
        {
    
    
            lastPos.Add(Tuple.Create(x,y,z));
            lastPos.RemoveAt(0);
        }
        // 判断是否发生转向
        bool turnOccurred = CheckTurn(lastPos);
        return turnOccurred;
    }


    //斜率转向判断
    public static bool CheckTurn(List<Tuple<float, float, float>> points)
    {
    
    
        double slopePrev = 0.0;

        for (int i = 1; i < points.Count; i++)
        {
    
    
            double x1 = points[i - 1].Item1;
            double y1 = points[i - 1].Item2;
            double x2 = points[i].Item1;
            double y2 = points[i].Item2;

            // 计算斜率
            double slope = (y2 - y1) / (x2 - x1);

            if (i > 1)
            {
    
    
                // 检查斜率变化
                if (slopePrev * slope < 0)
                {
    
    
                    return true;
                }
            }

            slopePrev = slope;
        }

        return false;
    }
}