UXO_TEM_Widget.py 文件源码

def computeRotMatrix(self,Phi=False):

        #######################################
        # COMPUTE ROTATION MATRIX SUCH THAT m(t) = A*L(t)*A'*Hp
        # Default set such that phi1,phi2 = 0 is UXO pointed towards North

        if Phi is False:
            phi1 = np.radians(self.phi[0])
            phi2 = np.radians(self.phi[1])
            phi3 = np.radians(self.phi[2])
        else:
            phi1 = np.radians(Phi[0])           # Roll (CCW)
            phi2 = np.radians(Phi[1])           # Inclination (+ve is nose pointing down)
            phi3 = np.radians(Phi[2])           # Declination (degrees CW from North)

        # A1 = np.r_[np.c_[np.cos(phi1),-np.sin(phi1),0.],np.c_[np.sin(phi1),np.cos(phi1),0.],np.c_[0.,0.,1.]] # CCW Rotation about z-axis
        # A2 = np.r_[np.c_[1.,0.,0.],np.c_[0.,np.cos(phi2),np.sin(phi2)],np.c_[0.,-np.sin(phi2),np.cos(phi2)]] # CW Rotation about x-axis (rotates towards North)
        # A3 = np.r_[np.c_[np.cos(phi3),-np.sin(phi3),0.],np.c_[np.sin(phi3),np.cos(phi3),0.],np.c_[0.,0.,1.]] # CCW Rotation about z-axis (direction of head of object)

        A1 = np.r_[np.c_[np.cos(phi1),np.sin(phi1),0.],np.c_[-np.sin(phi1),np.cos(phi1),0.],np.c_[0.,0.,1.]] # CW Rotation about z-axis
        A2 = np.r_[np.c_[1.,0.,0.],np.c_[0.,np.cos(phi2),np.sin(phi2)],np.c_[0.,-np.sin(phi2),np.cos(phi2)]] # CW Rotation about x-axis (rotates towards North)
        A3 = np.r_[np.c_[np.cos(phi3),np.sin(phi3),0.],np.c_[-np.sin(phi3),np.cos(phi3),0.],np.c_[0.,0.,1.]] # CW Rotation about z-axis (direction of head of object)

        return np.dot(A3,np.dot(A2,A1))