欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页  >  IT编程

Unity实现汽车前后轮倒车轨迹计算

程序员文章站 2022-06-22 08:25:35
汽车前后轮倒车轨迹计算附c#源码(unity),供大家参考,具体内容如下原理很简单, 都是高中的几何数学部分需要的参数有: 车前后轴距; 车宽(左前轮与右前轮距离); 当前车轮角度(...

汽车前后轮倒车轨迹计算附c#源码(unity),供大家参考,具体内容如下

原理很简单, 都是高中的几何数学部分

Unity实现汽车前后轮倒车轨迹计算

需要的参数有:

  • 车前后轴距;
  • 车宽(左前轮与右前轮距离);
  • 当前车轮角度(多数车33.5°);
  • 是否要绘制前轮轨迹线;
///<summary>
/// 获取行车轨迹预测index = 0 left轨迹
/// </summary>
/// <param name="steeringangle">方向盘角度</param>
/// <param name="carwheelbase">汽车前后轴距</param>
/// <param name="carwidth">车宽</param>
/// <param name="length">点位密度</param>
/// <param name="isfront">是否是前轮</param>
/// <param name="maxangle">轨迹的最大转弯角度</param>
/// <returns>交叉数组,下标为0的是右边线, 下表为1的是左边线</returns>
public vector3[][] getcartrack(float steeringangle, float carwheelbase, float carwidth, float length, bool isfront, float maxangle = 90f)
        {
            float maxsteerangle = _carcontrol._vehiclecontroller.steering.maxsteerangle;
            float theta = mathf.abs(steeringangle / 180 * mathf.pi);
            vector3[][] track = new vector3[2][];
            list<vector3> trackleft = new list<vector3>();
            list<vector3> trackright = new list<vector3>();
            if (theta == 0)
            {
                for (float i = 0; i < length; i++)
                {
                    float x = i / length * 5;
                    if (isfront)
                    {
                        x *= 1;
                        trackleft.add(new vector3(x, 0f, carwidth));
                        trackright.add(new vector3(x, 0f, 0f));
                    }
                    else
                    {
                        x *= -1;
                        trackleft.add(new vector3(x, 0, carwidth));
                        trackright.add(new vector3(x, 0, 0));
                    }
                }
            }
            else
            {
                if (isfront)
                {
                    float r = (carwheelbase / mathf.tan(theta) + carwidth / 2) / mathf.cos(theta);
                    float rmin = mathf.cos(theta) * r - carwidth;
                    float theta1 = mathf.atan(carwheelbase / rmin);
                    rmin = rmin / mathf.cos(theta1);
                    float rmax = rmin + carwidth;
                    float lineangle = carwheelbase / (rmax * 2f * mathf.pi) * 360f;
                    
                    for (int i = 0; i <= length; i++)
                    {
                        if (i / length >= (maxangle - lineangle) / maxangle)
                        {
                            float x = rmin * mathf.cos(maxangle / length * i * mathf.deg2rad);
                            float z = rmin * mathf.sin(maxangle / length * i * mathf.deg2rad);
                            if (x >= carwheelbase)
                            {
                                if (steeringangle > 0)
                                {
                                    trackright.add(new vector3(x - carwheelbase, 0, z - rmin + steeringangle / maxsteerangle * (carwidth / 2)));
                                }
                                else
                                {
                                    trackright.add(new vector3(x - carwheelbase, 0, -(z - rmin) + carwidth + steeringangle / maxsteerangle * (carwidth / 2)));
                                }
                            }

                            x = rmax * mathf.cos(maxangle / length * i * mathf.deg2rad);
                            z = rmax * mathf.sin(maxangle / length * i * mathf.deg2rad);
                            if (x >= carwheelbase)
                            {
                                if (steeringangle > 0)
                                {
                                    trackleft.add(new vector3(x - carwheelbase, 0, z - rmin + steeringangle / maxsteerangle * (carwidth / 2)));
                                }
                                else
                                {
                                    trackleft.add(new vector3(x - carwheelbase, 0, -(z - rmin) + carwidth + steeringangle / maxsteerangle * (carwidth / 2)));
                                }
                            }
                        }
                    }
                    trackright[trackright.count - 1] = vector3.forward * trackright[trackright.count - 1].z;
                    trackleft[trackleft.count - 1] = vector3.forward * trackleft[trackleft.count - 1].z;
                }
                else
                {
                    float r = (carwheelbase / mathf.tan(theta) + carwidth / 2) / mathf.cos(theta);
                    float rmin = mathf.cos(theta) * r - carwidth;
                    float rmax = rmin + carwidth;
                    float lineangle = carwheelbase / (rmin * 2f * mathf.pi) * 360f;
                    for (int i = 0; i <= length; i++)
                    {
                        if (i / length >= (maxangle - lineangle) / maxangle)
                        {
                            float x = -rmin * mathf.cos(maxangle / length * i * mathf.deg2rad);
                            float z = rmin * mathf.sin(maxangle / length * i * mathf.deg2rad);
                            if (steeringangle > 0)
                            {
                                trackright.add(new vector3(x, 0, z - rmin));
                            }
                            else
                            {
                                trackright.add(new vector3(x, 0, -(z - rmin) + carwidth));
                            }

                            x = -rmax * mathf.cos(maxangle / length * i * mathf.deg2rad);
                            z = rmax * mathf.sin(maxangle / length * i * mathf.deg2rad);
                            if (steeringangle > 0)
                            {
                                trackleft.add(new vector3(x, 0, z - rmin));
                            }
                            else
                            {
                                trackleft.add(new vector3(x, 0, -(z - rmin) + carwidth));
                            }
                        }
                    }
                    trackright[trackright.count - 1] = vector3.forward * trackright[trackright.count - 1].z;
                    trackleft[trackleft.count - 1] = vector3.forward * trackleft[trackleft.count - 1].z;
                }
            }
            track[0] = trackleft.toarray();
            track[1] = trackright.toarray();
            trackleft = trackright = null;
            return track;
}

以上就是本文的全部内容,希望对大家的学习有所帮助,也希望大家多多支持。

相关标签: Unity 轨迹计算