二维平面刚体变换坐标求解

 

在坐标系XOY中，已知△y1、△y2、△x、X0、Y0、Y1、L等等参数，可以得到坐标P0{（X0，Y0）}，P1{（X1，Y1）}，P2{（X2，Y2）}，

   根据向量P0P1//P0PW，向量P0P1⊥P2Pw，可以计算得到点PW{（Xw，Yw）}，进而可得到点Pt{(Xt,Yt)}和点P{(Xp,Yp)}.

解析：

向量P0P1:  (X1-X0, Y1-Y0)

向量P0Pw:  (Xw-X0, Yw-Y0)

向量P2Pw:  (Xw-X2, Yw-Y2)

易得方程：

（Xw-X0）（Y1-Y0）- (Yw-Y0)(X1-X0)=0     ①

（Xw-X2）（X1-X0）+ (Yw-Y2)(Y1-Y0)=0     ②

解方程得：

Xw=(E1+F1)/((X1-X0)^2+(Y1-Y0)^2)

Yw=(F2-E2)/((X1-X0)^2+(Y1-Y0)^2)

其中：

  E1=E0(Y1-Y0)

F1=F0(X1-X0)

E2=E0(X1-X0)

    F2=F0(Y1-Y0)

E0=X0(Y1-Y0)-Y0(X1-X0)

F0=X2(X1-X0)+Y2(Y1-Y0)

记，旋转角度：θ=-arctan((△y0-△y1)/L)，顺时针旋转为负，逆时针旋转为正。

则：

1）当逆时针旋转，θ>=0时，

   Xt=Xw-C*Cosθ

   Yt=Yw-C*Sinθ

2）当顺时针旋转，θ<0时，

Xt=Xw-C*Cosθ

   Yt=Yw-C*Sinθ

  

得平移矩阵：

   T=[Xt,Yt]’

旋转矩阵：

 R=[ Cosθ, -Sinθ

     Sinθ, Cosθ] 其中：顺时针旋转θ为负，逆时针旋转θ为正

则最终点P的坐标：

[Xp,YP]’=R*[a,b]’+T

或

[Xp,YP]’=R*[A,B]’+T – R*T, 其中：A=a+Xt，B=b+Yt