使用SCL实现WPR角度计算-优快云博客

本文链接：https://blog.youkuaiyun.com/potatosandpotato/article/details/130825355

该函数Trans_WPR用于计算三维空间中绕X、Y、Z轴的旋转角度。输入包括原点和目标点的坐标值，通过计算得到旋转矩阵的元素，进一步求解出W、P、R三个旋转角度。函数中使用了ATAN2函数求解角度，并转换为度制表示。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

FUNCTION "Trans_WPR" : Void
{ S7_Optimized_Access := 'TRUE' }
VERSION : 0.1
VAR_INPUT
Oz : Real; // 原点Z值
Xz : Real; // X正方向的Z值
Yz : Real; // Y正方向的Z值
OX : Real; // 原点X的坐标值
OY : Real; // 原点Y的坐标值
Xx : Real; // X方向点X的坐标值
Xy : Real; // X方向点Y的坐标值
Yx : Real; // Y方向点X的坐标值
Yy : Real; // Y方向点Y的坐标值
END_VAR

VAR_OUTPUT
W : Real; // 绕X旋转角度
P : Real; // 绕Y旋转角度
R : Real; // 绕Z旋转角度
END_VAR

VAR_TEMP
r11 : Real;
r12 : Real;
r13 : Real;
r21 : Real;
r22 : Real;
r23 : Real;
r31 : Real;
r32 : Real;
r33 : Real;
sqretr32r33 : Real;
x0abs : Real;
y0abs : Real;
END_VAR

VAR CONSTANT
PI : Real := 3.141593;
END_VAR

BEGIN
   //
   #x0abs := SQRT_REAL(IN := (#Xx-#OX) * (#Xx-#OX)+(#Xy-#OY)*(#Xy-#OY) + (#Xz - #Oz) * (#Xz - #Oz));
   #y0abs := SQRT_REAL(IN := (#Yx-#OX) *(#Yx-#OX)+ (#Yy-#OY)*(#Yy-#OY) + (#Yz - #Oz) * (#Yz - #Oz));

   #r11 := (#Xx-#OX) / #x0abs;
   #r21 := (#Xy-#OY) / #x0abs;
   #r31 := (#Xz - #Oz) / #x0abs;

   #r12 := (#Yx-#OX)/#y0abs;
   #r22 := (#Yy-#OY) / #y0abs;
   #r32 := (#Yz - #Oz) / #x0abs;

   // #r13 := - #r22 * #r31;
   // #r23 := - #r32 * #r11;
   // #r33 := #r11 * #r22 ;

   #r13 := #r21 * #r32 - #r22 * #r31;
   #r23 := #r12 * #r31 - #r32 * #r11;
   #r33 := #r11 * #r22 - #r12 * #r21;

   #sqretr32r33 := SQRT_REAL(IN := (#r32 * #r32 + #r33 * #r33));

   #W := "ATAN2"(X := #r33, Y := #r32)/#PI*180;
   #P := "ATAN2"(X := #sqretr32r33, Y := - #r31)/#PI*180;
   #R := "ATAN2"(X := #r11, Y:=#r21)/#PI*180;

END_FUNCTION