4

SETTLE约束算法中的坐标变换问题 - DECHIN

 2 years ago
source link: https://www.cnblogs.com/dechinphy/p/xyz-transform.html
Go to the source link to view the article. You can view the picture content, updated content and better typesetting reading experience. If the link is broken, please click the button below to view the snapshot at that time.
neoserver,ios ssh client

SETTLE约束算法中的坐标变换问题 - DECHIN - 博客园

在之前的两篇文章中，我们分别讲解了SETTLE算法的原理和基本实现和SETTLE约束算法的批量化处理。SETTLE约束算法在水分子体系中经常被用到，该约束算法具有速度快、可并行、精度高的优点。本文我们需要探讨的是该约束算法中的一个细节，问题是这样定义的，给定坐标系XYZXYZ下的两个已知三角形ΔA0B0C0ΔA0B0C0和三角形ΔA1B1C1ΔA1B1C1，以三角形ΔA0B0C0ΔA0B0C0构造一个平面π0π0，将π0π0平移到三角形ΔA1B1C1ΔA1B1C1的质心位置，作为新坐标系的X′Y′X′Y′平面，再使得Y′Z′Y′Z′平面过A1A1点，以此来构造一个新的坐标系X′Y′Z′X′Y′Z′，求两个坐标系之间的变换。

坐标系OXYZOXYZ和O′X′Y′Z′O′X′Y′Z′之间的变换，只有平移和旋转，没有伸缩。那么关于平移的部分，我们只需要考虑两个原点位置之间的向量差即可。而旋转部分，需要一些技巧，至少我们需要找到三个合适的点用于计算这个旋转矩阵。比如说，假定三角形ΔA1B1C1ΔA1B1C1在坐标系OXYZOXYZ和O′X′Y′Z′O′X′Y′Z′之中的位置都是已知的，那么我们就可以按照下述公式来计算旋转矩阵RR：

R⎡⎣⎢XAYAZAXBYBZBXCYCZC⎤⎦⎥=⎡⎣⎢⎢X′AY′AZ′AX′BY′BZ′BX′CY′CZ′C⎤⎦⎥⎥R[XAXBXCYAYBYCZAZBZC]=[XA′XB′XC′YA′YB′YC′ZA′ZB′ZC′]

然后在等式两边各乘上一个逆矩阵就可以得到旋转矩阵：

R=⎡⎣⎢⎢X′AY′AZ′AX′BY′BZ′BX′CY′CZ′C⎤⎦⎥⎥⎡⎣⎢XAYAZAXBYBZBXCYCZC⎤⎦⎥−1R=[XA′XB′XC′YA′YB′YC′ZA′ZB′ZC′][XAXBXCYAYBYCZAZBZC]−1

然而不幸的是，我们并不能直接得到三角形ΔA1B1C1ΔA1B1C1在坐标系O′X′Y′Z′O′X′Y′Z′之中的位置，这需要一些计算。因此，我们可以考虑另辟蹊径，找其他更容易计算的三个向量，用来计算我们所需要的旋转矩阵。

第一个向量

我们找的第一个向量是Z′Z′轴，或者用向量表示就是O′Z′→=[0,0,1]TO′Z′→=[0,0,1]T，因为Z′Z′轴跟平面π0π0是垂直的关系，也就是垂直于三角形ΔA0B0C0ΔA0B0C0。因此对应的可以用三角形ΔA0B0C0ΔA0B0C0的任意两条边的外积来计算向量O′Z′→=R⋅[A0B0→×A0C0→]O′Z′→=R⋅[A0B0→×A0C0→]（注意做归一化处理）。

第二个向量

如果分别用D1D1和D′1D1′来表示三角形ΔA1B1C1ΔA1B1C1在坐标系OXYZOXYZ和坐标系O′X′Y′Z′O′X′Y′Z′下的质心位置。这里我们找的第二个向量，就是D′1A′1→D1′A1′→。这里因为A′1A1′点在Y′Z′Y′Z′平面上，因此X′A′1=0XA1′′=0。而向量D′1A′1→D1′A1′→和Z′Z′轴的夹角，我们可以在坐标系OXYZOXYZ下计算：

cosθ=(A0B0→×A0C0→)⋅D1A1→|A0B0→×A0C0→|⋅|D1A1→|,sinθ=1−cos2θ−−−−−−−−√cosθ=(A0B0→×A0C0→)⋅D1A1→|A0B0→×A0C0→|⋅|D1A1→|,sinθ=1−cos2θ

计算出来夹角之后，就可以得到D′1A′1→=[0,sinθ,cosθ]TD1′A1′→=[0,sinθ,cosθ]T，即：D′1A′1→=R⋅D1A1→D1′A1′→=R⋅D1A1→。

第三个向量

到这一步为止，其实我们还是没有计算出D′1B′1→D1′B1′→和D′1C′1→D1′C1′→的值，因此我们第三个向量，在前两个向量的基础之上，用叉乘的方法再构造一个X′X′轴的向量，即O′X′→=[1,0,0]TO′X′→=[1,0,0]T，旋转矩阵计算方法为：

O′X′→=R⋅[(A0B0→×A0C0→)×D1A1→]O′X′→=R⋅[(A0B0→×A0C0→)×D1A1→]

这样一来，我们就得到了三个向量在两个坐标系下的坐标，可以用于建立方程组，计算两个坐标之间的变换关系，如果写成矩阵乘法形式就是：

R=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢00101−((A0B0→×A0C0→)⋅D1A1→|A0B0→×A0C0→|⋅|D1A1→|)2−−−−−−−−−−−−−−−−−−√(A0B0→×A0C0→)⋅D1A1→|A0B0→×A0C0→|⋅|D1A1→|100⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎛⎝⎜⎜⎜⎜⎜⎡⎣⎢⎢⎢⎢A0B0→×A0C0→D1A1→(A0B0→×A0C0→)×D1A1→⎤⎦⎥⎥⎥⎥T⎞⎠⎟⎟⎟⎟⎟−1R=[00101−((A0B0→×A0C0→)⋅D1A1→|A0B0→×A0C0→|⋅|D1A1→|)201(A0B0→×A0C0→)⋅D1A1→|A0B0→×A0C0→|⋅|D1A1→|0]([A0B0→×A0C0→D1A1→(A0B0→×A0C0→)×D1A1→]T)−1

这里先提一下代码实现和测试的思路。我们首先用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′X′Y′平面，红色三角形的质心为原点，并使得Y′Z′Y′Z′平面过A1A1点，以此来构造一个新的坐标系，并计算前后两个坐标系之间的变换。



	
2277440-20220831110103199-890163978.png




这里需要一些空间想象能力，我们可以先将绿色的三角形平面平移到过红色三角形的质心位置，同时将坐标系原点移动到红色三角形的质心位置，再旋转坐标轴，使得Y′Z′Y′Z′平面过A1A1点。这样一来通过上一个章节中的旋转矩阵的构造方法，我们就可以计算出所有的向量在两个坐标系下的旋转变换。



	
2277440-20220831110644759-297526985.png




当然，需要注意的是，这个变换只是一个旋转变换，由于坐标系发生了平移，所以需要有一个固定的参考点，才能够精确的得到某一个给定的点的坐标变换。比如我们上述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



__EOF__

        

        

            

                report
            

        

    







        
Recommend

        



                

    
About Joyk

    
 





Aggregate valuable and interesting links.

Joyk means Joy of geeK