[Algorithm] Calculate the step coordinate rotation angle (slope)

In recent development, I encountered a problem in calculating the step angle. When the corner is turned, the Z axis is offset by one unit.

Method to realize

This article uses X and Y to calculate the slope. If it is in other directions, you can change it yourself.

Implementation ideas

• Since it is a step corner, at least three point coordinates are required;
• Compare the slopes at points 1-2 and 2-3 respectively. If the product of the two slopes is **<0**, it means that a change has occurred, that is, a turn has occurred.
• Slope formula:(y1-y2)/(x1-x2)

specific code

/// <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;
    }
}