在简单地形上小车运动轨迹的数学表达(一)

图形学课上的小小总结

前提:

假设地形函数为f(x)为f(x)=sin(x)^[*]。
小车由四个轮子组成。
实现基于OpenGL。

[*] 实际效果图的f(x) = 0.5 * sin(x)

问题一：如何画一个轮子？

具体的问题描述应该是"如何在已知坐标x的情况下画出一个轮子?"

该轮子拥有的唯一特点是：与其所依附的地形应该是相切的。

下面是当"x0 = π/4"地形函数(图-1)：

图-1

假设在地形上的一点(x0,y0)，由它所唯一对应的轮子圆心为(x1,y1)。

不失一般性，(x1,y1)的计算过程如下：

这里有几个需要注意的地方：

k0为0的情况，即在x=x0处地形斜率与x轴相切的情况下：

x1 = x0

y1 = y0 + R
关于x1的±：

if k0 > 0

　　x1 = x0 + R * cosβ

else

　　x1 = x0 - R * cosβ

综合以上可得最终实现如下：

void get_wheel_center(float _x0, float _y0, float _r, float &_x1, float &_y1){
	float k0, k1, cos_beta, sin_beta;

	k0 = derived_f(_x0);
	
	// 考虑c++的浮点计算会丢失精度，此等价于if (k0 == 0) ...
	if (k0 < 0.00001f){
		_x1 = _x0;
		_y1 = _y0 + _r;
		return;
	}

	k1 = -1 / k0;

	cos_beta = sqrt(1 / ((k1 * k1) + 1));

	sin_beta = sqrt((k1 * k1) / (1 + k1*k1));

	_y1 = _y0 + _r * sin_beta;
	
	if (k0 > 0)
		_x1 = _x0 - _r * cos_beta;
	else
		_x1 = _x0 + _r * cos_beta;
}

实现效果图(图-2)：

问题1需要注意的地方：

根据上面所讲的画轮子的方法可以看出，若需要在x=x0处画一个轮子，所实际画出的轮子有可能与x=x0这条轴是有偏差的，这里的偏差是指实际画出的轮子的圆心并不在x=x0上，而是在偏移了±R*cosβ的位置上。
根据以上的公式推到是可以算出来一个精确解的，即轮子所在的坐标，但这也是在保证了计算精度的前提下的，不过就实际效果来看还是十分满意的。

问题2：如何画两个轮子？

这里的两个轮子有两层意思：

画出车子不同侧的另一个轮子。
画出车子同侧的另一个轮子。

对于第一层意思：

在获得到第一个轮子的坐标(x0,y0,z0)后，只需要将此点沿z轴平移±w个单位得到(x0,y0,z0±w)，这里的w应该是车宽。再根据这个坐标再画出一个轮子，这样实际中的前轮(后轮)就画出来了，这个问题就得到了解决，实际效果可以见图-2。

而第二层意思是一个完全不一样的问题：

对于这个问题，有几点前提与假设：

前轮或后轮已经画好，现在要画剩下的那一边^[*]的轮子。这里假设问题一已经解决，即已经画好了后轮，现在就剩下前轮没画了。
前轮的圆心要保证与后轮圆心距离为L(车长)。
前轮也需要与所在地形相切。

[*] “边”的意思是指前边或后边，其含义有别于“侧”。

若视角在车子的顶部，能看到：(=：轮子，-：车子的边缘)

1	 2
=----=
=----=
3	 4

1和2称为同侧，1和3成为同边。在此文中1和3称为后轮，2和4称为前轮。当出现比较前轮与后轮距离时，所指的是同侧的两轮圆心距离。

因此，问题简化为，如何在已知后轮数据和车长数据的前提下画出前轮？

根据车长L为定长可得，前轮的圆心O1一定在以O0为圆心，L为半径的圆弧上，大意图如下(图-3)

设O0为(x0,y0),O1为(x1,y2)

则一定在(x0,+∞)中存在一点x，由此x做的相切于地形的轮子的圆心O^' (x^',y^')，满足：

|O^' - O0| = L 即 ((x^'-x0)² +((y^'-y0)²)^1/2 = L

因此可以从x0开区间沿着x轴正方向出发一个一个试探性的画圆，判断是否有满足以上条件的，换句话说也就是说不断的做圆，直到满足两圆圆心距离为L。

如此一来这个问题也解决了，但是有一个效率问题，这个效率太慢了，如果在计算机上实现的话也不是特别容易准确实现，主要有这么几个难点：

由于计算机实现是离散的变量，故向前搜索的时候需要设置一个合适的增量，若设置太大，则搜索间隔太大，漏掉准确解的概率较大，但是搜索所消耗时间比较少；若设置太小，则搜索间隔太小，导致搜索次数太多，漏掉准确解的概率相比前者要小得多，但是搜索所消耗时间会变得太大。
无效的搜索过多，几乎前面的搜索就是临近O0的搜索可能都是无效点，而这些点都要进行画圆的计算。

于是针对以上几个问题，有如下的解决方案：

针对搜素精确度的问题：由于计算机计算浮点会有误差，故当计算结果满足一定误差范围内就可以认为找到了精确解了；
针对搜索次数导致效率底下的问题，可以采取二分法进行搜索。

由解决方案2中的二分法进而引发了一个问题：二分法的边界？

由于需要在(x0,+∞)中进行二分法，故需要首先确定这个+∞能否有一个确切的值？有的话是多少？

由问题一中末尾的“注意”可以得到，这个最大值+∞可以是L+R，不妨考虑一种极端情况，即两边的圆所在的地形都是垂直的，在这种情况中最大的搜索值就是b,而b=L+R。(图-4)

因此，二分法的最大范围定下来了，就是(x0,x0+L+R)。

具体的二分法流程如下(图-5)：

使用c++实现如下：

void get_righ_wheel_center(float _x0, float _y0, float _L, float &_x1, float &_y1) {
	float
		beg = _x0,
		end = _x0 + _L + _R;

	float mid = 0.0f,dis = 0.0f,x1 = 0.0f,y1 = 0.0f;

	while (true) {
		mid = (beg + end) / 2;

		get_wheel_center(mid, f(mid), _R, x1, y1);

		dis = dis_between_points(x1, y1, _x0, _y0);

		if (abs(beg - end) < 0.00001 || abs(dis - L) < 0.0001){
			_x1 = x1;
			_y1 = y1;
			return;
		}

		if (dis > _L){
			end = mid;
		}
		else{
			beg = mid;
		}
	}
}

如此一来，只要确定了后轮，前轮就可以由二分法计算得出了，可以比较一下计算效率，若从计算次数比较的话，使用二分法大约只需要12^[*]次就可以得到十分精确的结果，而使用之前的方法，在保证同样精度的情况下次数是显然要比12要大得多的。