SETTLE約束演演算法中的座標變換問題
技術背景
在之前的兩篇文章中，我們分別講解了SETTLE演演算法的原理和基本實現和SETTLE約束演演算法的批次化處理。SETTLE約束演演算法在水分子體系中經常被用到，該約束演演算法具有速度快、可並行、精度高的優點。本文我們需要探討的是該約束演演算法中的一個細節，問題是這樣定義的，給定座標系\(XYZ\)下的兩個已知三角形\(\Delta A_0B_0C_0\)和三角形\(\Delta A_1B_1C_1\)，以三角形\(\Delta A_0B_0C_0\)構造一個平面\(\pi_0\)，將\(\pi_0\)平移到三角形\(\Delta A_1B_1C_1\)的質心位置，作為新座標系的\(X'Y'\)平面，再使得\(Y'Z'\)平面過\(A_1\)點，以此來構造一個新的座標系\(X'Y'Z'\)，求兩個座標系之間的變換。
理論推導
座標系\(OXYZ\)和\(O'X'Y'Z'\)之間的變換，只有平移和旋轉，沒有伸縮。那麼關於平移的部分，我們只需要考慮兩個原點位置之間的向量差即可。而旋轉部分，需要一些技巧，至少我們需要找到三個合適的點用於計算這個旋轉矩陣。比如說，假定三角形\(\Delta A_1B_1C_1\)在座標系\(OXYZ\)和\(O'X'Y'Z'\)之中的位置都是已知的，那麼我們就可以按照下述公式來計算旋轉矩陣\(R\)：
然後在等式兩邊各乘上一個逆矩陣就可以得到旋轉矩陣：
然而不幸的是，我們並不能直接得到三角形\(\Delta A_1B_1C_1\)在座標系\(O'X'Y'Z'\)之中的位置，這需要一些計算。因此，我們可以考慮另闢蹊徑，找其他更容易計算的三個向量，用來計算我們所需要的旋轉矩陣。
第一個向量
我們找的第一個向量是\(Z'\)軸，或者用向量表示就是\(\vec{O'Z'}=[0, 0, 1]^T\)，因為\(Z'\)軸跟平面\(\pi_0\)是垂直的關係，也就是垂直於三角形\(\Delta A_0B_0C_0\)。因此對應的可以用三角形\(\Delta A_0B_0C_0\)的任意兩條邊的外積來計算向量\(\vec{O'Z'}=R\cdot[\vec{A_0B_0}\times \vec{A_0C_0}]\)（注意做歸一化處理）。
第二個向量
如果分別用\(D_1\)和\(D'_1\)來表示三角形\(\Delta A_1B_1C_1\)在座標系\(OXYZ\)和座標系\(O'X'Y'Z'\)下的質心位置。這裡我們找的第二個向量，就是\(\vec{D'_1A'_1}\)。這裡因為\(A'_1\)點在\(Y'Z'\)平面上，因此\(X'_{A'_1}=0\)。而向量\(\vec{D'_1A'_1}\)和\(Z'\)軸的夾角，我們可以在座標系\(OXYZ\)下計算：
計算出來夾角之後，就可以得到\(\vec{D'_1A'_1}=[0, sin\theta, cos\theta]^T\)，即：\(\vec{D'_1A'_1}=R\cdot\vec{D_1A_1}\)。
第三個向量
到這一步為止，其實我們還是沒有計算出\(\vec{D'_1B'_1}\)和\(\vec{D'_1C'_1}\)的值，因此我們第三個向量，在前兩個向量的基礎之上，用叉乘的方法再構造一個\(X'\)軸的向量，即\(\vec{O'X'}=[1, 0, 0]^T\)，旋轉矩陣計算方法為：
小結
這樣一來，我們就得到了三個向量在兩個座標系下的座標，可以用於建立方程組，計算兩個座標之間的變換關係，如果寫成矩陣乘法形式就是：
程式碼實現
這裡先提一下程式碼實現和測試的思路。我們首先用Python來構造2個三角形，得到一個新的三角形。然後我們再根據上述的公式，計算得到一個座標旋轉矩陣。最後我們再輸入一些便於手動計算的點（或者是直接用前面三角形的三個角，或者是中間的一些向量都是可以的），用旋轉矩陣進行變換，來測試一下是否我們所需要的座標變換之後的結果。
In [1]: import numpy as np
In [2]: T0 = np.array([[0, 0, 1], [0, -1, 0],[0, 1, 0]], np.float32)
In [3]: T1 = np.array([[1, 0, 1], [0, -1, 0], [0, 1, 0]], np.float32)
In [4]: D0 = np.mean(T0, axis=-2)
In [5]: D1 = np.mean(T1, axis=-2)
In [6]: A0B0 = T0[1]-T0[0]
In [7]: A0C0 = T0[2]-T0[0]
In [8]: v0 = np.cross(A0B0, A0C0)
In [9]: v0 /= np.linalg.norm(v0)
In [10]: v1 = T1[0]-D1
In [11]: v1 /= np.linalg.norm(v1)
In [12]: v2 = np.cross(v0, v1)
In [13]: v2 /= np.linalg.norm(v2)
In [14]: M1 = np.vstack((v0, v1, v2))
In [15]: M1 = M1.T
In [16]: iM1 = np.linalg.inv(M1)
In [17]: cost = np.dot(v0, v1)/np.linalg.norm(v0)/np.linalg.norm(v1)
In [18]: M0 = np.array([[0, 0, 1], [0, np.sqrt(1 - cost**2), 0], [1, cost, 0]])
In [19]: R = np.dot(M0, iM1)
In [20]: R
Out[20]:
array([[ 0.00000000e+00, -1.00000000e+00, 0.00000000e+00],
[ 0.00000000e+00, 0.00000000e+00, 9.99999916e-01],
[ 1.00000000e+00, 0.00000000e+00, 5.00651538e-08]])
In [21]: np.dot(R, v0)
Out[21]: array([0., 0., 1.])
In [22]: np.dot(R, v1)
Out[22]: array([0. , 0.70710671, 0.7071068 ])
In [23]: np.dot(R, v2)
Out[23]: array([1., 0., 0.])
In [24]: np.dot(R, T1[0]-D1)
Out[24]: array([0. , 0.66666657, 0.66666666])
In [25]: np.dot(R, T1[1]-D1)
Out[25]: array([ 1. , -0.33333332, -0.33333336])
In [26]: np.dot(R, T1[2]-D1)
Out[26]: array([-1. , -0.33333332, -0.33333336])
上面這個案例的流程是這樣的，我們先建立兩個不一樣大小的綠色三角形和紅色三角形，我們將要做的事情是以綠色三角形為\(X'Y'\)平面，紅色三角形的質心為原點，並使得\(Y'Z'\)平面過\(A_1\)點，以此來構造一個新的座標系，並計算前後兩個座標系之間的變換。
這裡需要一些空間想象能力，我們可以先將綠色的三角形平面平移到過紅色三角形的質心位置，同時將座標系原點移動到紅色三角形的質心位置，再旋轉座標軸，使得\(Y'Z'\)平面過\(A_1\)點。這樣一來通過上一個章節中的旋轉矩陣的構造方法，我們就可以計算出所有的向量在兩個座標系下的旋轉變換。
當然，需要注意的是，這個變換隻是一個旋轉變換，由於座標系發生了平移，所以需要有一個固定的參考點，才能夠精確的得到某一個給定的點的座標變換。比如我們上述python程式碼中的24、25、26都是對紅色三角形的三個頂點關於質心的相對位置的座標變換，在座標變換前後，頂點座標都需要減去質心的座標。
總結概要
在已知兩個三角形頂點座標的情況下，我們要以其中的一個三角形平面去構造一個新的座標系，並且需要找到新舊座標系之間的變換關係。這是一個比較簡單的立體幾何的問題，尋找兩個座標系之間的變換矩陣。如果是常規思路，可以先根據兩個三角形之間的相對位置去計算一下在新座標系下兩個三角形的新的頂點座標，從而可以取三個點來構造一個座標變換矩陣，進而推廣到所有向量在這兩個座標系之間的變換關係。而本文提供了一種相對更容易求解、也比較直接的思路，並給出了相關的Python程式碼實現過程。
版權宣告
本文首發連結為：https://www.cnblogs.com/dechinphy/p/xyz-transform.html
作者ID：DechinPhy
更多原著文章請參考：https://www.cnblogs.com/dechinphy/
打賞專用連結：https://www.cnblogs.com/dechinphy/gallery/image/379634.html
騰訊雲專欄同步：https://cloud.tencent.com/developer/column/91958
CSDN同步連結：https://blog.csdn.net/baidu_37157624?spm=1008.2028.3001.5343
51CTO同步連結：https://blog.51cto.com/u_15561675