fork download
copy 
  1. #!/usr/bin/python
  2. # coding: utf8
  3.  
  4. # 3点からなる平面の法線ベクトルの傾きを求める
  5. import math
  6.  
  7. # ２点からベクトルを求める
  8. # 引数 p1, p2 : (x, y, z)のタプル
  9. def get_vector(p1, p2):
  10.     x = p2[0] - p1[0]
  11.     y = p2[1] - p1[1]
  12.     z = p2[2] - p1[2]
  13.     return (x, y, z)
  14.  
  15. # ２つのベクトルから外積を求める
  16. # 引数 v1, v2 : (x, y, z)のタプル
  17. def get_cross_product(v1, v2):
  18.     x = (v1[1] * v2[2]) - (v1[2] * v2[1])
  19.     y = (v1[2] * v2[0]) - (v1[0] * v2[2])
  20.     z = (v1[0] * v2[1]) - (v1[1] * v2[0])
  21.     return (x, y, z)
  22.  
  23. # ベクトルの長さを求める
  24. def get_length(v):
  25.     t = v[0]*v[0] + v[1]*v[1] + v[2]*v[2]
  26.     tr = math.sqrt(t)
  27.     return tr
  28.  
  29. # ベクトルの正規化
  30. # 引数ベクトルの単位ベクトルを返す
  31. def get_norm_vec(v):
  32.     pass
  33.     vl = get_length(v)
  34.     i = v[0] / vl
  35.     j = v[1] / vl
  36.     k = v[2] / vl
  37.     return (i, j, k )
  38.  
  39. # Z軸回りの回転行列へ角度をセット
  40. # th:角度(rad)
  41. def set_rotate_matrix_Z(th):
  42.     cos_th = math.cos(th)
  43.     sin_th = math.sin(th)
  44.     mat = (cos_th, -sin_th,     0,
  45.            sin_th,  cos_th,     0,
  46.                 0,       0,     1 )
  47.     return mat
  48.  
  49. # Y軸回りの回転行列へ角度をセット
  50. # th:角度(rad)
  51. def set_rotate_matrix_Y(th):
  52.     cos_th = math.cos(th)
  53.     sin_th = math.sin(th)
  54.     mat = (cos_th,       0, sin_th,
  55.                 0,       1,      0,
  56.           -sin_th,       0, cos_th )
  57.     return mat
  58.  
  59. # ベクトルの回転
  60. # v: ベクトル
  61. # mat:回転行列 
  62. def rotate_vec(v, mat):
  63.     x = mat[0]*v[0] + mat[1]*v[1] + mat[2]*v[2]
  64.     y = mat[3]*v[0] + mat[4]*v[1] + mat[5]*v[2] 
  65.     z = mat[6]*v[0] + mat[7]*v[1] + mat[8]*v[2]
  66.     return (x, y, z)
  67.  
  68. # メイン
  69. def main():
  70.     pa = (50, 600, 900)   
  71.     pb = (385, 630, 905)    
  72.     pc = (260, 30, 915)
  73.  
  74.     #ベクトルをつくる
  75.     vab = get_vector(pa, pb)    # a から b へ向かうベクトル
  76.     vac = get_vector(pa, pc)    # a から c へ向かうベクトル       
  77.     vabn = get_norm_vec(vab)    # 単位ベクトルへ変換
  78.     vacn = get_norm_vec(vac)    # 単位ベクトルへ変換
  79.  
  80.     # ベクトルvabnとvacnで作られる平面の法線ベクトル(=vabnとvacnの外積)を求める
  81.     vn = get_cross_product(vacn, vabn)
  82.     vnn= get_norm_vec(vn)       # 単位ベクトルへ変換
  83.     print("法線ベクトル vnn = {}".format(vnn))
  84.  
  85.     '''
  86.     法線ベクトルvnnが、Z軸単位ベクトル(0,0,1)を
  87.     1) Y軸回りに角度θ (rad) 範囲: 0 <= θ <= π
  88.     2) Z軸回りに角度φ (rad) 範囲: -π <= φ <= π
  89.     だけ回転させたものと考えれば
  90.     法線ベクトルvnnを
  91.     1) Z軸回りに角度 -φ
  92.     2) Y軸回りに角度 -θ
  93.     だけ回転させればZ軸単位ベクトル(0,0,1)と重なるはず
  94.     '''
  95.     # 法線ベクトルvnnとX軸のなす角度φを求める
  96.     # φ = atan(y / x )
  97.     ph = math.atan2(vnn[1], vnn[0])
  98.     print('vnnとx軸のなす角 φ : {}(rad), {}(deg)'.format(ph, ph*(180/math.pi)))
  99.  
  100.     # 法線ベクトルvnnとZ軸のなす角度θを求める
  101.     # θ = atan( sqrt(x*x + y*y) / z ) 
  102.     th = math.atan2(math.sqrt(vnn[0]*vnn[0] + vnn[1]*vnn[1]), vnn[2])
  103.     print('vnnとz軸のなす角 θ : {}(rad), {}(deg)'.format(th, th*(180/math.pi)))
  104.  
  105.     # 回転行列に値をセット
  106.     mat_rot_z = set_rotate_matrix_Z(-ph)    # Z軸回りの回転行列
  107.     mat_rot_y = set_rotate_matrix_Y(-th)    # Y軸回りの回転行列
  108.  
  109.     #pa, pb pcに回転行列を適用
  110.     pa_rz = rotate_vec(pa, mat_rot_z)     
  111.     pa_ry = rotate_vec(pa_rz, mat_rot_y)    
  112.     pb_rz = rotate_vec(pb, mat_rot_z)     
  113.     pb_ry = rotate_vec(pb_rz, mat_rot_y)    
  114.     pc_rz = rotate_vec(pc, mat_rot_z)     
  115.     pc_ry = rotate_vec(pc_rz, mat_rot_y)    
  116.     print('補正後 pa = {}'.format(pa_ry))
  117.     print('補正後 pb = {}'.format(pb_ry))
  118.     print('補正後 pc = {}'.format(pc_ry))
  119.  
  120. # プログラム開始
  121. if __name__ == '__main__':
  122.     main()
  123.  
Success #stdin #stdout 0.01s 27712KB
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)
補正後 pa = (405.9964707639152, -421.7237196075919, 910.942352757617)
補正後 pb = (215.03024862530637, -698.6386921210106, 910.942352757617)
補正後 pc = (-166.90219565191154, -219.2092311901769, 910.9423527576171)
https://ideone.com/RAJQSo
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!