固定一个点A，让点B (结构式) 围绕点A旋转，使A, B, C在同一直线上Python实现

川长思鸟来 2023-03-04 15:27 46阅读 0赞

直接上代码

import numpy as np
    
    #结构式旋转的函数。只需要四个参数即可。这几个参数的意义：
        #p1: 固定不动的那个点。p1不要求是坐标原点，可以是任意坐标。
        #p2: 要移动的那个点。
        #p3: 移动的目标点。这个点与p2移动之后的点不一定是重合的，但p1, p2'(p2移动后的点), p3一定是在同一直线上的
        #listPoint: 要旋转的结构式
        #具体原理：根据p1, p2, p3，计算出旋转矩阵。有了旋转矩阵之后，旋转公式与每一个点都点乘，即可得到旋转之后的坐标，
        #对于整个结构式而言，得到的是旋转之后的结构式，且每个点的相对位置保持不变，且移动到所希望的位置。
        @staticmethod
        def structureRoate_fixOnePoint(p1, p2, p3, listPoint):
            #计算旋转矩阵
            rotationMatrix = Move_and_Rotation.calculationRoationMatrixByThreePoints(p1, p2, p3)
    
            listPoint2 = []
            for point in listPoint:
                #起始位置。注意，- p1.x，         - p1.y和- p1.z的目的是让p1点变成坐标原点，计算完之后再加上这些数据即可。
                #如果p1不是坐标原点，计算结果是错误的
                resourceMatrix = np.array([point.x - p1.x, point.y - p1.y, point.z - p1.z]).T
                
                #利用旋转矩阵对原始坐标进行旋转
                targetMatrix = np.dot(rotationMatrix, resourceMatrix)
                
                #point旋转之后得到point2，其坐标如下。记得要加上p1的坐标
                point2 = Point(targetMatrix[0] + p1.x, targetMatrix[1] + p1.y, targetMatrix[2] + p1.z)
                listPoint2.append(point2)
                
            return listPoint2
            
        #矩阵的叉乘。这里其实是向量的叉乘
        @staticmethod
        def crossProduct(a, b):
            c = []
            c.append(a[1] * b[2] - a[2] *b[1])
            c.append(a[2] * b[0] - a[0] *b[2])
            c.append(a[0] * b[1] - a[1] *b[0])
            return c
    
        #矩阵的点乘。这里其实是向量的点乘
        @staticmethod
        def dotProduct(a, b):
            c = a[0] * b[0] + a[1] * b[1] + a[2] *b[2]
            return c    
        
        #矩阵的Normalize。这里求的是向量的长度，用来将向量转化成单位向量
        @staticmethod
        def normalize(v):
            return math.sqrt(v[0]**2 + v[1]**2 + v[2]**2)    
        
        #根据罗德里格斯旋转公式，计算旋转矩阵。
        #注意，得到旋转矩阵之后，可以进一步计算出绕x轴、y轴、z轴的角度是多少。虽然这些角度在本例中没有意义
        #angle: 两个空间向量之间的角度; u:三个坐标轴
        @staticmethod
        def getRotationMatrix(angle, u):
            norm = Move_and_Rotation.normalize(u)
            rotationMatrix = []
            u[0] /= norm #转换成单位向量
            u[1] /= norm #转换成单位向量
            u[2] /= norm #转换成单位向量
            
            cos = math.cos(angle)
            sin = math.sin(angle)
            
            #布置好矩阵的size
            for i in range(3):
                rotationMatrix.append([])
                for j in range(3):
                    rotationMatrix[-1].append([])
            
            rotationMatrix[0][0] = cos + u[0] * u[0] * (1 - cos)
            
            #https://www.cnblogs.com/xpvincent/archive/2013/02/15/2912836.html中的评论提到
            #rotatinMatrix[0, 1]里的括号似乎也有点小问题.
            #原来的表达式为：
            #rotationMatrix[0][1] = u[0] * u[1] * (1 - cos - u[2] * sin)
            #应该修改为如下表达式： 
            rotationMatrix[0][1] = u[0] * u[1] * (1 - cos) - u[2] * sin
            rotationMatrix[0][2] = u[1] * sin + u[0] * u[2] * (1 - cos)
             
            rotationMatrix[1][0] = u[2] * sin + u[0] * u[1] * (1 - cos)
            rotationMatrix[1][1] = cos + u[1] * u[1] * (1 - cos)
            rotationMatrix[1][2] = -u[0] * sin + u[1] * u[2] * (1 - cos)
     
            rotationMatrix[2][0] = -u[1] * sin + u[0] * u[2] * (1 - cos)
            rotationMatrix[2][1] = u[0] * sin + u[1] * u[2] * (1 - cos)
            rotationMatrix[2][2] = cos + u[2] * u[2] * (1 - cos)
    
            return rotationMatrix
            
        #根据两个向量，计算旋转矩阵。外部代码不要直接调用本函数
        #根据旋转前后的向量值求旋转矩阵： https://wenku.baidu.com/view/d914b9b96137ee06eff9189f.html
        @staticmethod
        def calculationRoationMatrixByTwoVector(vectorBefore, vectorAfter):   
            rotationAxis = Move_and_Rotation.crossProduct(vectorBefore, vectorAfter)
            rotationAngle=math.acos(Move_and_Rotation.dotProduct(vectorBefore,vectorAfter) /Move_and_Rotation.normalize(vectorBefore) /Move_and_Rotation.normalize(vectorAfter))
            
            rotationMatrix = Move_and_Rotation.getRotationMatrix(rotationAngle, rotationAxis)
            return rotationMatrix
            
        #根据3个Point，计算旋转矩阵，
        #其中p1是固定不动的点，p2经过一定的角度后移动到p2'。 p1， p2'， p3等三个点在同一直线上
        #其中     p2' 是虚拟点。参数列表中只给出p1， p2， p3等三个点。由于在后续计算中将两个向量都转化成单位向量了，所以不需要计算出 p2'的坐标
        #另外，这里p1必须是坐标原点，也就是(0, 0, 0)，如果不是坐标原点，会导致计算错误
        #外部代码调用本函数
        @staticmethod
        def calculationRoationMatrixByThreePoints(p1, p2, p3):
            vectorBefore = [p2.x - p1.x, p2.y - p1.y, p2.z - p1.z]
            vectorAfter =  [p3.x - p1.x, p3.y - p1.y, p3.z - p1.z]
            #print(p1.toString())
            rotationMatrix = np.array(Move_and_Rotation.calculationRoationMatrixByTwoVector(vectorBefore, vectorAfter)) 
            return rotationMatrix

ps: 结构式平移、结构式绕着空间中一条直线旋转、结构式转向一个点，这三块代码花了我一两个月的时间。其中艰难不必多言，以此文纪念那段时间。