Convert 2 3D Points to Directional Vectors to Euler Angles

不羁岁月 提交于 2019-12-06 11:55:46

What you want is a transformation from Cartesian coordinates of the vector

v = (v_x, v_y, v_z)

to the spherical coordinates r, ψ and θ where

v = ( r*COS(ψ)*COS(θ), r*SIN(θ), r*SIN(ψ)*COS(θ) )

This is done with the following equations

r = SQRT(v_x^2+v_y^2+v_z^2) 
TAN(ψ) = (v_z)/(v_x)
TAN(θ) = (v_y)/(v_x^2+v_z^2)

To get the angles ψ and θ, use the ATAN2(dy,dx) function as in

ψ = ATAN2(v_z, v_x)
θ = ATAN2(v_y, SQRT(v_x^2+v_z^2))

Now that you have the along direction vector

j = ( COS(ψ)*COS(θ), SIN(θ), SIN(ψ)*COS(θ) )

you can get the two perpendicular vectors from

i = ( SIN(ψ), 0, -COS(ψ) )
k = ( COS(ψ)*SIN(θ), -COS(θ), SIN(ψ)*SIN(θ) )

These three vectors make up the columns of the 3×3 rotation matrix

             |  SIN(ψ)   COS(ψ)*COS(θ)    COS(ψ)*SIN(θ)  |
E =[i j k] = |    0          SIN(θ)          -COS(θ)     |
             | -COS(ψ)   SIN(ψ)*COS(θ)    SIN(ψ)*SIN(θ)  |

In terms of Euler angles the above is equivalent to

E = RY(π/2-ψ)*RX(π/2-θ)

Example

Two points p_1=(3,2,3) and p_2=(5,6,4) define the vector

v = (5,6,4) - (3,2,3) = (2,4,1)

NOTE: I am using the notation of v[i] for the i-th element of the vector, as in v[1]=2 above. This is neither like C, Python which is zero based, nor like VB, FORTRAN or MATLAB which uses parens () for the index.

Using the expressions above you get

r = √(2^2+4^2+1^2) = √21
TAN(ψ) = 1/2 
TAN(θ) = 4/√(2^2+1^2) = 4/√5

ψ = ATAN2(1,2) = 0.463647 
θ = ATAN2(4,√5) = 1.061057

Now to find the direction vectors

j = ( COS(ψ)*COS(θ), SIN(θ), SIN(ψ)*COS(θ) ) = (0.4364, 0.87287, 0.21822 )
i = ( SIN(ψ), 0, -COS(ψ) ) = (0.44721, 0, -0.89443 )
k = ( COS(ψ)*SIN(θ), -COS(θ), SIN(ψ)*SIN(θ) ) = (0.78072, -0.48795, 0.39036) 

Put the direction vectors as columns of the local to world coordinate transformation (rotation)

E[1,1] = i[1]    E[1,2] = j[1]    E[1,3] = k[1]
E[2,1] = i[2]    E[2,2] = j[2]    E[2,3] = k[2]
E[3,1] = i[3]    E[3,2] = j[3]    E[3,3] = k[3]


    |  0.447213595499957  0.436435780471984   0.780720058358826 |
    |                                                           |
E = |          0          0.872871560943969  -0.487950036474266 |
    |                                                           |
    | -0.894427190999915  0.218217890235992   0.390360029179413 |
易学教程内所有资源均来自网络或用户发布的内容,如有违反法律规定的内容欢迎反馈
该文章没有解决你所遇到的问题?点击提问,说说你的问题,让更多的人一起探讨吧!