%局部坐标与全局坐标的转换函数
function [R]=CoordTransform(x,y,BarLength)
    C=(x(2)-x(1))/BarLength; S=(y(2)-y(1))/BarLength;
    R=[C S 0 0 0 0;-S C 0 0 0 0;0 0 1 0 0 0;0 0 0 C S 0;0 0 0 -S C 0;0 0 0 0 0 1];
end