在簡單地形上小車運動軌跡的數學表達(一)

圖形學課上的小小總結

前提:

假設地形函數為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要大得多的。