Python几何计算程序集1、矢量计算1.1、矢量的模1.2、矢量夹角(代码由deepseek生成1.3、点到直线的垂足和距离2、 张量计算2.1、质心惯性张量转换到其他坐标系2.2、欧拉123旋转变换到 坐标变换矩阵2.3、坐标变换矩阵变成欧拉313旋转角1、矢量计算1.1、矢量的模importnumpyasnp np.linalg.norm(a,b)1.2、矢量夹角(代码由deepseek生成importnumpyasnpdefvector_angle(a,b,degreesFalse):# 计算点积和模长dotnp.dot(a,b)norm_anp.linalg.norm(a)norm_bnp.linalg.norm(b)# 处理零向量ifnorm_a0ornorm_b0:raiseValueError(向量不能为零向量)# 计算余弦值限制浮点范围cos_thetanp.clip(dot/(norm_a*norm_b),-1.0,1.0)angle_radnp.arccos(cos_theta)returnnp.degrees(angle_rad)ifdegreeselseangle_rad# 示例用法anp.array([1,2,3])bnp.array([4,5,6])print(f弧度:{vector_angle(a,b):.4f})# 输出 0.2257print(f角度:{vector_angle(a,b,True):.2f}°)# 输出 12.93°1.3、点到直线的垂足和距离importnumpyasnpdeffoot_point(P0,v,Q,posFalse): 计算点Q到直线过点P0方向向量v的垂足坐标 :param P0: 直线上一点形状为(n,)的数组 :param v: 直线单位方向向量形状为(n,)的数组模为单位1 :param Q: 外部点形状为(n,)的数组 :return: 垂足坐标F形状与P0相同,点到直线的距离d Q_minus_P0Q-P0 tnp.dot(Q_minus_P0,v)FP0t*v dnp.linalg.norm(F-Q)returnFifposelsed2、 张量计算2.1、质心惯性张量转换到其他坐标系importnumpyasnpdeftransform_mass_matrix(MgNone,mNoneErgNone,dis_grNone): :param Mg质心坐标系Eg下的质量矩阵Mg (3x3) 坐标旋转计算Er坐标系下的质量矩阵Mr Erg * Mg * Erg.T :param Erg是Er坐标系到Eg坐标系的变换矩阵是Eg基在Er下的投影 (3x3) !!!##Egr是Eg坐标系到Er坐标系的变换矩阵是Er基在Eg下的投影!!! #-------------- 坐标平移计算Er_d坐标系下的质量矩阵 :param dis_gr是Eg到Er的距离矢量在Er下的投影 (3x1) deltaM[y^2z^2 -xy -xz -xy x^2z^2 -yz -xz -yz x^2y^2] Mr_dMrm*deltaM :param m质量 :return: Mr或则Er_d坐标系下质量矩阵Mr_d ifnotMgNoneandnotEgrNone:Mgnp.array(Mg)Ergnp.array(Erg)#Ergnp.array(Egr).TMrErg Mg Erg.T# 矩阵乘法顺序不可颠倒ifnotmNoneandnotdis_grNone:xdis_gr[0]ydis_gr[1]zdis_gr[2]deltaM[y^2z^2-xy-xz-xy x^2z^2-yz-xz-yz x^2y^2]deltaMnp.array(deltaM)Mr_dMrm*deltaM MrMr_dreturnMr2.2、欧拉123旋转变换到 坐标变换矩阵importnumpyasnpdefeuler123_to_erb(phi,theta,psi): 将Er经欧拉123序列(绕x-y-z轴旋转)旋转到Eb,求Erb,即Eb在Er的投影在Er下度量Eb) 参数: phi: 绕x轴旋转角度(弧度) theta: 绕y轴旋转角度(弧度) psi: 绕z轴旋转角度(弧度) 返回: 3x3 Erb # 计算各旋转矩阵Erb1np.array([[1,0,0],[0,np.cos(phi),-np.sin(phi)],[0,np.sin(phi),np.cos(phi)]])Erb2np.array([[np.cos(theta),0,np.sin(theta)],[0,1,0],[-np.sin(theta),0,np.cos(theta)]])Erb3np.array([[np.cos(psi),-np.sin(psi),0],[np.sin(psi),np.cos(psi),0],[0,0,1]])# 组合旋转矩阵(注意顺序是R3*R2*R1)ErbErb3 Erb2 Erb1returnErb2.3、坐标变换矩阵变成欧拉313旋转角importnumpyasnpfromscipy.spatial.transformimportRotationasRdefdcm_to_euler_313(dcm): 将方向余弦矩阵(DCM)转换为 3-1-3 (Z-X-Z) 欧拉角 参数: dcm: 3x3 numpy array, 旋转矩阵 返回: euler_angles: numpy array [psi, theta, phi] 对应绕Z轴(psi), 绕X轴(theta), 绕Z轴(phi)的角度(弧度) # 创建 Rotation 对象rotationR.from_dcm(dcm)# 转换为 zxz 顺序的欧拉角# degreesFalse 表示返回弧度True 表示返回角度euler_anglesrotation.as_euler(zxz,degreesFalse)returneuler_angles# --- 示例验证 ---if__name____main__:# 1. 定义一组 3-1-3 欧拉角 (psi, theta, phi)psinp.pi/4# 45度thetanp.pi/6# 30度phinp.pi/3# 60度# 2. 手动构建 3-1-3 旋转矩阵用于测试# R_z(psi) * R_x(theta) * R_z(phi)defrot_z(angle):c,snp.cos(angle),np.sin(angle)returnnp.array([[c,-s,0],[s,c,0],[0,0,1]])defrot_x(angle):c,snp.cos(angle),np.sin(angle)returnnp.array([[1,0,0],[0,c,-s],[0,s,c]])R_z1rot_z(psi)R_xrot_x(theta)R_z2rot_z(phi)# 注意如果是主动旋转向量旋转通常右乘如果是坐标系变换需注意左乘/右乘约定# SciPy 默认使用 extrinsic (静态轴) 或 intrinsic (动态轴) 的逻辑需匹配# zxz 在 scipy 中默认指 intrinsic rotations (内旋/动态轴)即 Z - X - Z# 这与经典的 3-1-3 欧拉角定义一致dcm_testR_z1 R_x R_z2# 3. 转换回欧拉角recovered_eulersdcm_to_euler_313(dcm_test)print(f原始欧拉角 (rad): [{psi:.4f},{theta:.4f},{phi:.4f}])print(f恢复欧拉角 (rad):{recovered_eulers})以上代码计算中发现是按静态轴变换很奇怪按手动计算如下再试试看主要用于ProE中测量零件位置——方向余弦矩阵Adams中按欧拉313动态旋转进行定位# _*_ coding:UTF-8 _*_importnumpyasnpdefdcm_to_euler_313_numpy(R): 纯NumPy实现从旋转矩阵提取 3-1-3 (Z-X-Z) 动态轴欧拉角 返回: psi, theta, phi (弧度) # 防止浮点误差导致 acos 域溢出defsafe_acos(x):returnnp.arccos(np.clip(x,-1.0,1.0))# 提取 theta (绕中间轴 X 的旋转)# R[2,2] 对应 cos(theta)thetasafe_acos(R[2,2])sin_thetanp.sin(theta)# 设置一个阈值来判断是否处于奇异点 (万向节死锁)epsilon1e-6ifsin_thetaepsilon:# 非奇异情况# psi (绕第一个 Z 轴): atan2(R[0,2], -R[1,2])# 注意: R[0,2] sin(psi)*sin(theta), -R[1,2] cos(psi)*sin(theta)psinp.arctan2(R[0,2],-R[1,2])# phi (绕最后一个 Z 轴): atan2(R[2,0], R[2,1])# 注意: R[2,0] sin(theta)*sin(phi), R[2,1] sin(theta)*cos(phi)phinp.arctan2(R[2,0],R[2,1])elifsin_theta-epsilon:# 这种情况理论上 acos 返回 [0, pi]sin(theta) 应该非负。# 但如果数值误差导致处理逻辑类似只是符号可能反转。# 通常 acos 返回正值sin_theta 0。psinp.arctan2(-R[0,2],R[1,2])phinp.arctan2(-R[2,0],-R[2,1])else:# 奇异点: theta 接近 0 或 pi# 此时 sin(theta) 0, R[0,2]R[1,2]R[2,0]R[2,1]0# 我们只能求出 psi phi 或 psi - phi# convention: 设 psi 0, 求 phiifthetaepsilon:# theta ~ 0# R 简化为绕 Z 轴旋转 (psi phi)# R[0,0] cos(psiphi), R[0,1] -sin(psiphi)# 令 psi 0, 则 phi atan2(-R[0,1], R[0,0])psi0.0phinp.arctan2(-R[0,1],R[0,0])else:# theta ~ pi# R 简化为...# 令 psi 0, 求 phipsi0.0phinp.arctan2(R[0,1],-R[0,0])returnnp.array([psi,theta,phi])# --- 测试 ---if__name____main__:# 构造一个测试矩阵psi,theta,phi0.5,1.0,0.2cz1,sz1np.cos(psi),np.sin(psi)cx,sxnp.cos(theta),np.sin(theta)cz2,sz2np.cos(phi),np.sin(phi)# 手动构建 3-1-3 动态轴矩阵 (根据上述公式)R_testnp.array([[cz1*cz2-sz1*cx*sz2,-cz1*sz2-sz1*cx*cz2,sz1*sx],[sz1*cz2cz1*cx*sz2,-sz1*sz2cz1*cx*cz2,-cz1*sx],[sx*sz2,sx*cz2,cx]])anglesdcm_to_euler_313_numpy(R_test)print(psi,theta,phi)print(angles) Python几何计算程序集
Python几何计算程序集1、矢量计算1.1、矢量的模1.2、矢量夹角(代码由deepseek生成1.3、点到直线的垂足和距离2、 张量计算2.1、质心惯性张量转换到其他坐标系2.2、欧拉123旋转变换到 坐标变换矩阵2.3、坐标变换矩阵变成欧拉313旋转角1、矢量计算1.1、矢量的模importnumpyasnp np.linalg.norm(a,b)1.2、矢量夹角(代码由deepseek生成importnumpyasnpdefvector_angle(a,b,degreesFalse):# 计算点积和模长dotnp.dot(a,b)norm_anp.linalg.norm(a)norm_bnp.linalg.norm(b)# 处理零向量ifnorm_a0ornorm_b0:raiseValueError(向量不能为零向量)# 计算余弦值限制浮点范围cos_thetanp.clip(dot/(norm_a*norm_b),-1.0,1.0)angle_radnp.arccos(cos_theta)returnnp.degrees(angle_rad)ifdegreeselseangle_rad# 示例用法anp.array([1,2,3])bnp.array([4,5,6])print(f弧度:{vector_angle(a,b):.4f})# 输出 0.2257print(f角度:{vector_angle(a,b,True):.2f}°)# 输出 12.93°1.3、点到直线的垂足和距离importnumpyasnpdeffoot_point(P0,v,Q,posFalse): 计算点Q到直线过点P0方向向量v的垂足坐标 :param P0: 直线上一点形状为(n,)的数组 :param v: 直线单位方向向量形状为(n,)的数组模为单位1 :param Q: 外部点形状为(n,)的数组 :return: 垂足坐标F形状与P0相同,点到直线的距离d Q_minus_P0Q-P0 tnp.dot(Q_minus_P0,v)FP0t*v dnp.linalg.norm(F-Q)returnFifposelsed2、 张量计算2.1、质心惯性张量转换到其他坐标系importnumpyasnpdeftransform_mass_matrix(MgNone,mNoneErgNone,dis_grNone): :param Mg质心坐标系Eg下的质量矩阵Mg (3x3) 坐标旋转计算Er坐标系下的质量矩阵Mr Erg * Mg * Erg.T :param Erg是Er坐标系到Eg坐标系的变换矩阵是Eg基在Er下的投影 (3x3) !!!##Egr是Eg坐标系到Er坐标系的变换矩阵是Er基在Eg下的投影!!! #-------------- 坐标平移计算Er_d坐标系下的质量矩阵 :param dis_gr是Eg到Er的距离矢量在Er下的投影 (3x1) deltaM[y^2z^2 -xy -xz -xy x^2z^2 -yz -xz -yz x^2y^2] Mr_dMrm*deltaM :param m质量 :return: Mr或则Er_d坐标系下质量矩阵Mr_d ifnotMgNoneandnotEgrNone:Mgnp.array(Mg)Ergnp.array(Erg)#Ergnp.array(Egr).TMrErg Mg Erg.T# 矩阵乘法顺序不可颠倒ifnotmNoneandnotdis_grNone:xdis_gr[0]ydis_gr[1]zdis_gr[2]deltaM[y^2z^2-xy-xz-xy x^2z^2-yz-xz-yz x^2y^2]deltaMnp.array(deltaM)Mr_dMrm*deltaM MrMr_dreturnMr2.2、欧拉123旋转变换到 坐标变换矩阵importnumpyasnpdefeuler123_to_erb(phi,theta,psi): 将Er经欧拉123序列(绕x-y-z轴旋转)旋转到Eb,求Erb,即Eb在Er的投影在Er下度量Eb) 参数: phi: 绕x轴旋转角度(弧度) theta: 绕y轴旋转角度(弧度) psi: 绕z轴旋转角度(弧度) 返回: 3x3 Erb # 计算各旋转矩阵Erb1np.array([[1,0,0],[0,np.cos(phi),-np.sin(phi)],[0,np.sin(phi),np.cos(phi)]])Erb2np.array([[np.cos(theta),0,np.sin(theta)],[0,1,0],[-np.sin(theta),0,np.cos(theta)]])Erb3np.array([[np.cos(psi),-np.sin(psi),0],[np.sin(psi),np.cos(psi),0],[0,0,1]])# 组合旋转矩阵(注意顺序是R3*R2*R1)ErbErb3 Erb2 Erb1returnErb2.3、坐标变换矩阵变成欧拉313旋转角importnumpyasnpfromscipy.spatial.transformimportRotationasRdefdcm_to_euler_313(dcm): 将方向余弦矩阵(DCM)转换为 3-1-3 (Z-X-Z) 欧拉角 参数: dcm: 3x3 numpy array, 旋转矩阵 返回: euler_angles: numpy array [psi, theta, phi] 对应绕Z轴(psi), 绕X轴(theta), 绕Z轴(phi)的角度(弧度) # 创建 Rotation 对象rotationR.from_dcm(dcm)# 转换为 zxz 顺序的欧拉角# degreesFalse 表示返回弧度True 表示返回角度euler_anglesrotation.as_euler(zxz,degreesFalse)returneuler_angles# --- 示例验证 ---if__name____main__:# 1. 定义一组 3-1-3 欧拉角 (psi, theta, phi)psinp.pi/4# 45度thetanp.pi/6# 30度phinp.pi/3# 60度# 2. 手动构建 3-1-3 旋转矩阵用于测试# R_z(psi) * R_x(theta) * R_z(phi)defrot_z(angle):c,snp.cos(angle),np.sin(angle)returnnp.array([[c,-s,0],[s,c,0],[0,0,1]])defrot_x(angle):c,snp.cos(angle),np.sin(angle)returnnp.array([[1,0,0],[0,c,-s],[0,s,c]])R_z1rot_z(psi)R_xrot_x(theta)R_z2rot_z(phi)# 注意如果是主动旋转向量旋转通常右乘如果是坐标系变换需注意左乘/右乘约定# SciPy 默认使用 extrinsic (静态轴) 或 intrinsic (动态轴) 的逻辑需匹配# zxz 在 scipy 中默认指 intrinsic rotations (内旋/动态轴)即 Z - X - Z# 这与经典的 3-1-3 欧拉角定义一致dcm_testR_z1 R_x R_z2# 3. 转换回欧拉角recovered_eulersdcm_to_euler_313(dcm_test)print(f原始欧拉角 (rad): [{psi:.4f},{theta:.4f},{phi:.4f}])print(f恢复欧拉角 (rad):{recovered_eulers})以上代码计算中发现是按静态轴变换很奇怪按手动计算如下再试试看主要用于ProE中测量零件位置——方向余弦矩阵Adams中按欧拉313动态旋转进行定位# _*_ coding:UTF-8 _*_importnumpyasnpdefdcm_to_euler_313_numpy(R): 纯NumPy实现从旋转矩阵提取 3-1-3 (Z-X-Z) 动态轴欧拉角 返回: psi, theta, phi (弧度) # 防止浮点误差导致 acos 域溢出defsafe_acos(x):returnnp.arccos(np.clip(x,-1.0,1.0))# 提取 theta (绕中间轴 X 的旋转)# R[2,2] 对应 cos(theta)thetasafe_acos(R[2,2])sin_thetanp.sin(theta)# 设置一个阈值来判断是否处于奇异点 (万向节死锁)epsilon1e-6ifsin_thetaepsilon:# 非奇异情况# psi (绕第一个 Z 轴): atan2(R[0,2], -R[1,2])# 注意: R[0,2] sin(psi)*sin(theta), -R[1,2] cos(psi)*sin(theta)psinp.arctan2(R[0,2],-R[1,2])# phi (绕最后一个 Z 轴): atan2(R[2,0], R[2,1])# 注意: R[2,0] sin(theta)*sin(phi), R[2,1] sin(theta)*cos(phi)phinp.arctan2(R[2,0],R[2,1])elifsin_theta-epsilon:# 这种情况理论上 acos 返回 [0, pi]sin(theta) 应该非负。# 但如果数值误差导致处理逻辑类似只是符号可能反转。# 通常 acos 返回正值sin_theta 0。psinp.arctan2(-R[0,2],R[1,2])phinp.arctan2(-R[2,0],-R[2,1])else:# 奇异点: theta 接近 0 或 pi# 此时 sin(theta) 0, R[0,2]R[1,2]R[2,0]R[2,1]0# 我们只能求出 psi phi 或 psi - phi# convention: 设 psi 0, 求 phiifthetaepsilon:# theta ~ 0# R 简化为绕 Z 轴旋转 (psi phi)# R[0,0] cos(psiphi), R[0,1] -sin(psiphi)# 令 psi 0, 则 phi atan2(-R[0,1], R[0,0])psi0.0phinp.arctan2(-R[0,1],R[0,0])else:# theta ~ pi# R 简化为...# 令 psi 0, 求 phipsi0.0phinp.arctan2(R[0,1],-R[0,0])returnnp.array([psi,theta,phi])# --- 测试 ---if__name____main__:# 构造一个测试矩阵psi,theta,phi0.5,1.0,0.2cz1,sz1np.cos(psi),np.sin(psi)cx,sxnp.cos(theta),np.sin(theta)cz2,sz2np.cos(phi),np.sin(phi)# 手动构建 3-1-3 动态轴矩阵 (根据上述公式)R_testnp.array([[cz1*cz2-sz1*cx*sz2,-cz1*sz2-sz1*cx*cz2,sz1*sx],[sz1*cz2cz1*cx*sz2,-sz1*sz2cz1*cx*cz2,-cz1*sx],[sx*sz2,sx*cz2,cx]])anglesdcm_to_euler_313_numpy(R_test)print(psi,theta,phi)print(angles)