平面坐标系间 同一点(向量)的坐标

#!/usr/bin/env python
# coding=utf-8
import numpy as np
import math

# 平面坐标系间 同一点(向量)的坐标
# 已知当前坐标系A下一点Ap,目标坐标系B以及AB的变换关系，求Ap在B下的坐标

# AB坐标系方向是否相同,判断当x方向对齐时y方向是否一致，一致为1 否则-1
f = 1 # A坐标反向 （改变输入和变换）
f2= 1 # B坐标反向（改变输出）
# 设B为固定坐标系，测量A的x轴相对B的x轴之间的夹角，逆时针方向为正。
theta = 3.1415 / 4
# 设A为固定坐标系，要让目标坐标系B的原点与A原点对齐，测量其在A坐标系下位移量
offsetx = 0
offsety = 0

f = np.array([[1, 0], [0, 1 * f]])
offset = np.array([[offsetx], [offsety]])
b = np.array([[1], [0]])
# -----------------------rota
co = np.cos(theta)
si = np.sin(theta)
rota = np.array([[co, -si], [si, co]])
# print (rota)
# -----------------------
b1 = np.dot(f, b)
c1 = np.dot(rota, b1)
result = np.add(offset, c1)


# rota_t=np.linalg.inv(rota)

# print (b1)
# print(c1)
# print(result)


# 变换数据的类，将参数变换为矩阵
class Coordinate2DTransData():

    def __init__(self, f=1, f2=1,theta=0, offsetx=0, offsety=0, zoomx=1, zoomy=1):
        self.set_transform_info(f, f2,theta, offsetx, offsety, zoomx, zoomy)
    #以变换方式设置数据，直接设置让点怎么移动。
    def set_transform_info(self,f=1, f2=1,theta=0, offsetx=0, offsety=0, zoomx=1, zoomy=1):
        self.f = f #A坐标系y轴需要反向
        self.f2 = f2 #B坐标系y轴需要反向
        self.theta = theta #B坐标系转theta度x轴与A重合
        self.offsetx = offsetx #B与A原点重合 x方向的位移
        self.offsety = offsety* f #B与A原点重合 y方向的位移
        self.zoomx = zoomx #x缩放
        self.zoomy = zoomy #y缩放
        # trans A坐标系y轴反向变换矩阵
        self.f_mat = np.array([[1, 0], [0, 1 * f]])
        # trans B坐标系y轴反向变换矩阵
        self.f2_mat = np.array([[1, 0], [0, 1 * f2]])
        # offset #B与A原点重合 xy方向的位移
        self.offset = np.array([[self.offsetx], [self.offsety]])
        self.zoom = np.array([[self.zoomx,0], [0,self.zoomy]])
        # rota #角度转弧度
        thetapi = math.pi * theta / 180.0
        co = np.cos(thetapi)
        si = np.sin(thetapi)
        #旋转矩阵
        self.rota_mat = np.array([[co, -si], [si, co]])

    #以坐标系信息方式设置数据，用坐标角位信息变成让点怎么移动。
    def set_coordinate_info(self, Af=1, Bf2=1,Btheta=0, Bposix=0, Bposiy=0, Bzoomx=1, Bzoomy=1):
        #B在A中的角度 Btheta，旋转变化为 -Btheta,
        #B在A中的x Bposix,位移变化为 -Bposix,
        #B在A中的y Bposiy,位移变化为 -Bposiy,
        self.set_transform_info(Af, Bf2,-Btheta, -Bposix, -Bposiy, Bzoomx, Bzoomy)


# 变换类，输入变换数据后可以做变换
class Coordinate2DTrans():

    def __init__(self, td):
        self._f = td.f
        self._f_mat = td.f_mat
        self._offset = td.offset
        self._rota_mat = td.rota_mat
        self._f2_mat = td.f2_mat
        self._zoom =td.zoom

    def p2mat(self, x, y):
        return np.array([[x], [y]])#

    def mat2p(self, mat_p):
        # lst=mat_p.tolist()
        # return lst[0][0],lst[1][0]
        # print type(mat_p.tolist())
        return mat_p[0][0], mat_p[1][0]

    def trans_f(self, mat_p):
        result = np.dot(self._f_mat, mat_p)
        return result

    def trans_rota(self, mat_p):
        result = np.dot(self._rota_mat, mat_p)
        return result

    def trans(self, mat_p):
        result = np.dot(self._f_mat, mat_p)
        print("t")
        print(result)
        result = np.dot(self._rota_mat, result)
        print("r")
        print(result)
        result = np.add(self._offset, result)
        print("o")
        print(result)
        return result

    def trans2(self, mat_p):
        #输入反转变换
        result = np.dot(self._f_mat, mat_p)
        #print("t")
        #print(result)
        # 位移变换
        result = np.add(self._offset, result)
        #print(self._offset)
        #print("o")
        #print(result)
        # 旋转变换
        result = np.dot(self._rota_mat, result)
        #print("r")
        #print(result)
        # 输出放缩变换
        result = np.dot(self._zoom, result)
        # 输出反转变换
        result = np.dot(self._f2_mat, result)

        return result

    def trans_xy2xy(self, x, y):
        return self.mat2p(self.trans(self.p2mat(x, y)))


if __name__ == "__main__":
    # 测试1  AB坐标系同向
    td = Coordinate2DTransData()  # 创建转换数据
    td.set_coordinate_info( 1,1, 30, 1.732, 1)
    test_ct = Coordinate2DTrans(td)
    mat_p = test_ct.p2mat(-1.732, -1)
    mat_r = test_ct.trans2(mat_p)  # 变换
    x1, y1 = test_ct.mat2p(mat_r)
    print ("test1")
    print (x1,y1)
    # 测试2  AB坐标系不同向，B反向
    td = Coordinate2DTransData()  # 创建转换数据
    td.set_coordinate_info( 1,-1, 120, 1.732, 1)
    test_ct = Coordinate2DTrans(td)
    mat_p = test_ct.p2mat(-1.732, -1)
    mat_r = test_ct.trans2(mat_p)  # 变换
    x1, y1 = test_ct.mat2p(mat_r)
    print ("test2")
    print (x1,y1)
    # 测试2  AB坐标系不同向，A反向
    td = Coordinate2DTransData()  # 创建转换数据
    td.set_coordinate_info( -1,1, -60, 1, 1.732)
    test_ct = Coordinate2DTrans(td)
    mat_p = test_ct.p2mat(-1,-1.732 )
    mat_r = test_ct.trans2(mat_p)  # 变换
    x1, y1 = test_ct.mat2p(mat_r)
    print ("test3")
    print (x1,y1)
    # 测试2  AB坐标系不同向，A反向
    td = Coordinate2DTransData()  # 创建转换数据
    td.set_coordinate_info( -1,1, -105, 1, 1.732)
    test_ct = Coordinate2DTrans(td)
    mat_p = test_ct.p2mat(-1,-1.732 )
    mat_r = test_ct.trans2(mat_p)  # 变换
    x1, y1 = test_ct.mat2p(mat_r)
    print ("test4")
    print (x1,y1)

    print("--------------")
    # 已知A系的坐标点，求其在B中的坐标
    # 设A为固定坐标系，要让B的原点与A原点对齐，测量B在A坐标系下位移量
    # 测量B在A坐标系下x轴对齐需要转过的角度，顺时针为正
    td = Coordinate2DTransData(1,1, -20, -4, -4)
    test_ct = Coordinate2DTrans(td)

    mat_p = test_ct.p2mat(6, 6)
    mat_r = test_ct.trans2(mat_p)  # 先平移后旋转
    x1, y1 = test_ct.mat2p(mat_r)
    # x1,y1=test_ct.trans_xy2xy(6,6)
    print(x1, y1)

    print("--------------")
    # 已知A系的坐标点，求其在B中的坐标
    # 设A为固定坐标系，
    # 测量B在A坐标系下位坐标
    # 测量B在A坐标系下x轴角度
    td = Coordinate2DTransData()
    td.set_coordinate_info( 1, 1,20, 4, 4)
    test_ct = Coordinate2DTrans(td)

    mat_p = test_ct.p2mat(6, 6)
    mat_r = test_ct.trans2(mat_p)  # 先平移后旋转
    x1, y1 = test_ct.mat2p(mat_r)

    print(x1, y1)



    #td = Coordinate2DTransData(1,1, 20, 4, 4)
    #test_ct = Coordinate2DTrans(td)
    #x1, y1 = test_ct.trans_xy2xy(2.56, 1.2)
    print(x1, y1)
    td = Coordinate2DTransData(1,1, -45, -1, -1)
    test_ct = Coordinate2DTrans(td)
    mat_p = test_ct.p2mat(-1, -1)
    mat_r = test_ct.trans2(mat_p)  # 先平移后旋转
    x1, y1 = test_ct.mat2p(mat_r)
    print(x1, y1)
    td = Coordinate2DTransData(-1,1, -45, -1, 1)
    test_ct = Coordinate2DTrans(td)
    mat_p = test_ct.p2mat(-1, 1)
    mat_r = test_ct.trans2(mat_p)  # 先平移后旋转
    x1, y1 = test_ct.mat2p(mat_r)
    print(x1, y1)
    td = Coordinate2DTransData(1,-1, -45, -1, -1)
    test_ct = Coordinate2DTrans(td)
    mat_p = test_ct.p2mat(-1, -1)
    mat_r = test_ct.trans2(mat_p)  # 先平移后旋转
    x1, y1 = test_ct.mat2p(mat_r)
    print(x1, y1)


    print("系统的应用")
    #=======================================================================
    #                            系统的应用
    #=======================================================================
    #              . uwb
    #            .   .       N
    #          .       .     |   
    #        .           .   | world 
    #      .               . .---------E
    # field 
    #   A-----width-----B    
    #   |               |
    # height            |
    #   |               |
    #   C---------------D


    # uwb转world
    uwb_world_td = Coordinate2DTransData()  # 创建转换数据
    uwb_world_td.set_coordinate_info( -1,1, 90, 1.5, 1.2)#坐标系关系
    uwb_world_c2dt = Coordinate2DTrans(uwb_world_td)  # 创建转换实例
    # world 转 field
    world_field_td = Coordinate2DTransData()  # 创建转换数据
    world_field_td.set_coordinate_info( 1,-1, -90, 1.0, 1.3)#坐标系关系
    world_field_c2dt = Coordinate2DTrans(world_field_td)  # 创建转换实例
    # field 转 screen 


    def screen_select_field_corner(w,h,zx,zy,flag="A"):

        field_screen_td = Coordinate2DTransData()  # 创建转换数据
        # field corner flag 
        #
        #   A-----width-----B    
        #   |
        # height
        #   |
        #   C               D
        #   
        #   select one corner for screen origin point, for example:
        #
        # screen show 
        #
        #   B-----C   height
        #   |
        #   |
        #   |
        # width
        #   |
        #   |
        #   A     D
        #   






        if flag not in["A","B","C","D" ]:
            flag="A" 
        # 坐标系关系
        if flag=="A":
            field_screen_td.set_coordinate_info( 1,1,0, 0, 0,screen_zoomx,screen_zoomy) 
        elif flag=="B":
            field_screen_td.set_coordinate_info( 1,1,-90, field_width, 0,screen_zoomx,screen_zoomy) 
        elif flag=="C":
            field_screen_td.set_coordinate_info( 1,1,-180, field_width, field_height, screen_zoomx,screen_zoomy) 
        elif flag=="D":
            field_screen_td.set_coordinate_info( 1,1,-270, 0, field_height, screen_zoomx,screen_zoomy) 
        # 创建转换实例
        return Coordinate2DTrans(field_screen_td)

    #场地大小(米)
    field_width=8
    field_height=4.5
    #像素比例尺(像素/米)
    screen_zoomx=100
    screen_zoomy=100

    field_screen_c2dt = screen_select_field_corner(field_width,field_height,screen_zoomx,screen_zoomy,"A")  # 创建转换实例

    #教学系统uwb或gnss最终会把数据点转换到world下，因此screen的显示只考虑从接收world系下的点就ok
    #这里 测试变换：uwb系下一点，经过 world field 最终转到 screen下的示例
    

    def uwb2screen(x,y):
        p_uwb =uwb_world_c2dt.p2mat(x,y)
        p_world = uwb_world_c2dt.trans2(p_uwb)  # 变换
        p_field = world_field_c2dt.trans2(p_world) 
        p_screen = field_screen_c2dt.trans2(p_field) 
        x1, y1 = field_screen_c2dt.mat2p(p_screen)    

        print(x1, y1)

    lst=[[0,0],[1,1],[8,4.5]]
    for e in lst:
        uwb2screen(e[0],e[1])
    
pass