CoordinateSystem.py 文件源码

def __init__(self,origin, pt1, pt2, name=None):
        """
        origin: 3x1 vector
        pt1: 3x1 vector
        pt2: 3x1 vector
        """
        self.__origin=origin    
        vec1 = np.array([pt1[0] - origin[0] , pt1[1] - origin[1] , pt1[2] - origin[2]])
        vec2 = np.array([pt2[0] - origin[0] , pt2[1] - origin[1] , pt2[2] - origin[2]])
        cos = np.dot(vec1, vec2)/np.linalg.norm(vec1)/np.linalg.norm(vec2)
        if  cos == 1 or cos == -1:
            raise Exception("Three points should not in a line!!")        
        self.__x = vec1/np.linalg.norm(vec1)
        z = np.cross(vec1, vec2)
        self.__z = z/np.linalg.norm(z)
        self.__y = np.cross(self.z, self.x)
        self.__name=uuid.uuid1() if name==None else name