二維旋轉公式

ros的tf工具包可以很方便的實現任意座標系之間的座標轉換。但是，如果只是想簡單的測試想法，而又不想編寫過於龐雜的程式碼，考慮自己寫二維旋轉的函式。而與二維旋轉問題對偶的另一個問題便是二維座標系旋轉變換。這兩個問題的形式基本一樣，只是旋轉的角度相差一個負號。就是這個容易搞混，所以做個筆記，以備查用。

1. 二維旋轉公式（演算法）

而（此文只針對二維）旋轉則是表示某一座標點 $(x_1$

推導：
假定 $v=(x, y)$, $v'=(x',y')$,如上圖有 $x=rcos(\phi),y=rsin(\phi),x'=rcos(\phi+\theta),y'=rsin(\phi+\theta)$(注意，上圖有幾處錯誤，座標軸邊上的 $cos/sin(\theta)$應改為 $cos/sin(\phi+\theta$)。展開 $x',y'$可得：
$x'=rcos(\phi)cos(\theta)-rsin(\phi)sin(\theta)=xcos(\theta)-ysin(\theta)$
$y'=rsin(\phi)cos(\theta)+rcos(\phi)sin(\theta)=xsin(\theta)+ycos(\theta)$

矩陣形式為: $\left[\begin{matrix}x' \\ y'\end{matrix}\right]=\left[\begin{matrix} cos(\theta) & -sin(\theta) \\ sin(\theta) & cos(\theta)\end{matrix}\right]\left[\begin{matrix}x \\ y\end{matrix}\right]$

則二維旋轉矩陣為： $A=\left[\begin{matrix} cos(\theta) & -sin(\theta) \\ sin(\theta) & cos(\theta)\end{matrix}\right] \tag{1}$

void Rotate2(double x1, double y1, double alpha, double& x2, double& y2)
{
	x2 = x1 * cos(alpha) - y1 * sin(alpha);
	y2 = x1 * sin(alpha) + y1 * cos(alpha);
}

2. 二維座標系旋轉變換

假設有一座標系 $XOY$，經過逆時針(正向)旋轉角度 $\theta$後，得到新的座標系 $X'O‘Y'$。得到原來座標系中的座標 $(x,y)$在新座標系下的座標值被稱為座標系轉換。

$x'=xcos(\theta)+ysin(\theta)=xcos(-\theta)-ysin(-\theta)$
$y'=-xsin(\theta)+ycos(\theta)=xsin(-\theta)+ycos(-\theta)$

所以二維座標旋轉變換矩陣為: $\begin{array}{}\text{(2)}& B=\left[\begin{array}{cc}cos\left(\theta \right)& sin\left(\theta \right)\\ -sin\left(\theta \right)& cos\left(\theta \right)\end{array}\right]=\end{array}$