fork download
copy 
  1. #!/usr/bin/python
  2. # coding: utf8
  3.  
  4. # 3点からなる平面の法線ベクトルの傾きを求める
  5. import math
  6. import sys
  7.  
  8. # ２点からベクトルを求める
  9. # 引数 p1, p2 : (x, y, z)のタプル
  10. def get_vector(p1, p2):
  11.     x = p2[0] - p1[0]
  12.     y = p2[1] - p1[1]
  13.     z = p2[2] - p1[2]
  14.     return (x, y, z)
  15.  
  16. # ２つのベクトルから外積を求める
  17. # 引数 v1, v2 : (x, y, z)のタプル
  18. def get_cross_product(v1, v2):
  19.     x = (v1[1] * v2[2]) - (v1[2] * v2[1])
  20.     y = (v1[2] * v2[0]) - (v1[0] * v2[2])
  21.     z = (v1[0] * v2[1]) - (v1[1] * v2[0])
  22.     return (x, y, z)
  23.  
  24. # ベクトルの長さを求める
  25. def get_length(v):
  26.     t = v[0]*v[0] + v[1]*v[1] + v[2]*v[2]
  27.     tr = math.sqrt(t)
  28.     return tr
  29.  
  30. # ベクトルの正規化
  31. # 引数ベクトルの単位ベクトルを返す
  32. def get_norm_vec(v):
  33.     pass
  34.     vl = get_length(v)
  35.     i = v[0] / vl
  36.     j = v[1] / vl
  37.     k = v[2] / vl
  38.     return (i, j, k )
  39.  
  40. # Z軸回りの回転行列へ角度をセット
  41. # th:角度(rad)
  42. def set_rotate_matrix_Z(th):
  43.     cos_th = math.cos(th)
  44.     sin_th = math.sin(th)
  45.     mat = (cos_th, -sin_th,     0,
  46.            sin_th,  cos_th,     0,
  47.                 0,       0,     1 )
  48.     return mat
  49.  
  50. # Y軸回りの回転行列へ角度をセット
  51. # th:角度(rad)
  52. def set_rotate_matrix_Y(th):
  53.     cos_th = math.cos(th)
  54.     sin_th = math.sin(th)
  55.     mat = (cos_th,       0, sin_th,
  56.                 0,       1,      0,
  57.           -sin_th,       0, cos_th )
  58.     return mat
  59.  
  60. # ベクトルの回転
  61. # v: ベクトル
  62. # mat:回転行列 
  63. def rotate_vec(v, mat):
  64.     x = mat[0]*v[0] + mat[1]*v[1] + mat[2]*v[2]
  65.     y = mat[3]*v[0] + mat[4]*v[1] + mat[5]*v[2] 
  66.     z = mat[6]*v[0] + mat[7]*v[1] + mat[8]*v[2]
  67.     return (x, y, z)
  68.  
  69. # cos(x)からsin(x)を求める
  70. def get_sin(cos_x):
  71.     return  math.sqrt(1 - (cos_x * cos_x))
  72.  
  73. # メイン
  74. def main():
  75.     pa = (50, 600, 900)   
  76.     pb = (385, 630, 905)    
  77.     pc = (260, 30, 915)
  78.  
  79.     #ベクトルをつくる
  80.     vab = get_vector(pa, pb)    # a から b へ向かうベクトル
  81.     vac = get_vector(pa, pc)    # a から c へ向かうベクトル       
  82.     vabn = get_norm_vec(vab)    # 単位ベクトルへ変換
  83.     vacn = get_norm_vec(vac)    # 単位ベクトルへ変換
  84.  
  85.     # ベクトルvabnとvacnで作られる平面の法線ベクトル(=vabnとvacnの外積)を求める
  86.     vn = get_cross_product(vacn, vabn)
  87.     vnn= get_norm_vec(vn)       # 単位ベクトルへ変換
  88.     print("法線ベクトル vnn = {}".format(vnn))
  89.  
  90.     '''
  91.     法線ベクトルvnnが、Z軸単位ベクトル(0,0,1)を
  92.     1) Y軸回りに角度θ (rad) 範囲: 0 <= θ <= π
  93.     2) Z軸回りに角度φ (rad) 範囲: -π <= φ <= π
  94.     だけ回転させたものと考えれば
  95.     法線ベクトルvnnを
  96.     1) Z軸回りに角度 -φ
  97.     2) Y軸回りに角度 -θ
  98.     だけ回転させればZ軸単位ベクトル(0,0,1)と重なるはず
  99.     '''
  100.     # 法線ベクトルvnnとX軸のなす角度φを求める
  101.     # φ = atan(y / x )
  102.     ph = math.atan2(vnn[1], vnn[0])
  103.     print('vnnとx軸のなす角 φ : {}(rad), {}(deg)'.format(ph, ph*(180/math.pi)))
  104.  
  105.     # 法線ベクトルvnnとZ軸のなす角度θを求める
  106.     # θ = atan( sqrt(x*x + y*y) / z ) 
  107.     th = math.atan2(math.sqrt(vnn[0]*vnn[0] + vnn[1]*vnn[1]), vnn[2])
  108.     print('vnnとz軸のなす角 θ : {}(rad), {}(deg)'.format(th, th*(180/math.pi)))
  109.  
  110.     # 回転行列に値をセット
  111.     mat_rot_z = set_rotate_matrix_Z(-ph)    # Z軸回りの回転行列
  112.     mat_rot_y = set_rotate_matrix_Y(-th)    # Y軸回りの回転行列
  113.  
  114.     #法線ベクトルの回転確認
  115.     #vn_rz = rotate_vec(vnn, mat_rot_z)     # Z軸回りに -φ
  116.     #vn_ry = rotate_vec(vn_rz, mat_rot_y)  # Y軸回りに -θ　
  117.     #print('vnn   = {}'.format(vnn))
  118.     #print('vn_rph= {}'.format(vn_rph))
  119.     #print('vn_rth= {}'.format(vn_rth))
  120.     #print('')
  121.  
  122.     # ベクトルvab, vacに上記の回転を適用する
  123.     vab_rz = rotate_vec(vab, mat_rot_z)     
  124.     vab_ry = rotate_vec(vab_rz, mat_rot_y)    
  125.     vac_rz = rotate_vec(vac, mat_rot_z)
  126.     vac_ry = rotate_vec(vac_rz, mat_rot_y)
  127.  
  128.     # vab, vacは 点b,c から 点aを始点とするベクトルだったので点a分をシフト
  129.     # φ,θ補正後の3点の座標
  130.     pa2 = pa
  131.     pb2 = (vab_ry[0]+pa[0], vab_ry[1]+pa[1], vab_ry[2]+pa[2])
  132.     pc2 = (vac_ry[0]+pa[0], vac_ry[1]+pa[1], vac_ry[2]+pa[2])
  133.     print('pa2= {}'.format(pa2))
  134.     print('pb2= {}'.format(pb2))
  135.     print('pc2= {}'.format(pc2))
  136.  
  137.  
  138. # プログラム開始
  139. if __name__ == '__main__':
  140.     main()
  141.  
Success #stdin #stdout 0.03s 27728KB
comments ()
stdin
copy 
Standard input is empty
stdout
copy 
法線ベクトル vnn = (-0.01672430256251988, 0.020145182632126224, 0.9996571758960746)
vnnとx軸のなす角 φ : 2.263675869072698(rad), 129.6990734834743(deg)
vnnとz軸のなす角 θ : 0.026185633228062395(rad), 1.5003262678455052(deg)
pa2= (50, 600, 900)
pb2= (-140.96622213860886, 323.08502748658117, 900.0)
pc2= (-522.8986664158268, 802.5144884174149, 900.0)
https://ideone.com/MGeLTh
language:
Python 3 nbc (python 3.7.3)
created:
7 years ago
visibility:
public

Share or Embed source code

Discover > Sphere Engine API

The brand new service which powers Ideone!

Discover > IDE Widget

Widget for compiling and running the source code in a web browser!