np_station/utils_geometry.py

   1
   2 # ##### BEGIN GPL LICENSE BLOCK #####
   3 #
   4 #  This program is free software; you can redistribute it and/or
   5 #  modify it under the terms of the GNU General Public License
   6 #  as published by the Free Software Foundation; either version 2
   7 #  of the License, or (at your option) any later version.
   8 #
   9 #  This program is distributed in the hope that it will be useful,
  10 #  but WITHOUT ANY WARRANTY; without even the implied warranty of
  11 #  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12 #  GNU General Public License for more details.
  13 #
  14 #  You should have received a copy of the GNU General Public License
  15 #  along with this program; if not, write to the Free Software Foundation,
  16 #  Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
  17 #
  18 # ##### END GPL LICENSE BLOCK #####
  19
  20 import bpy
  21 import copy
  22 import bgl
  23 import blf
  24 import mathutils
  25 from mathutils import *
  26 from math import *
  27 #from math import sin, cos, tan, atan, degrees, radians, asin, acos
  28 from bpy_extras import view3d_utils
  29 from bpy.app.handlers import persistent
  30
  31 from .utils_function import *
  32
  33
  34 def scene_cast(region, rv3d, co2d):
  35
  36     ray_origin = view3d_utils.region_2d_to_origin_3d(region, rv3d, co2d)
  37     #np_print('ray_origin', ray_origin)
  38     view_vector = view3d_utils.region_2d_to_vector_3d(region, rv3d, co2d)
  39     #np_print('view_vector', view_vector)
  40     ray_target = ray_origin + view_vector * 1000
  41     #np_print('ray_target', ray_target)
  42     if bpy.app.version[0] > 1 and  bpy.app.version[1] > 76:
  43         scenecast = bpy.context.scene.ray_cast(ray_origin, view_vector)
  44         #np_print('scenecast', scenecast)
  45         result = scenecast[0]
  46         hitloc = scenecast[1]
  47         normal = scenecast[2]
  48         face_index = scenecast[3]
  49         hitob = scenecast[4]
  50         matrix = scenecast[5]
  51     else:
  52         scenecast = bpy.context.scene.ray_cast(ray_origin, ray_target)
  53         #np_print('scenecast', scenecast)
  54         result = scenecast[0]
  55         hitloc = scenecast[3]
  56         normal = scenecast[4]
  57         hitob = scenecast[1]
  58         matrix = scenecast[2]
  59         if hitob is not None:
  60             matrix = hitob.matrix_world.copy()
  61             matrix_inv = matrix.inverted()
  62             ray_origin_obj = matrix_inv * ray_origin
  63             ray_target_obj = matrix_inv * ray_target
  64             obcast = hitob.ray_cast(ray_origin_obj, ray_target_obj)
  65             #np_print('obcast', obcast)
  66             face_index = obcast[2]
  67         else:
  68             face_index = -1
  69
  70     return (result, hitloc, normal, face_index, hitob, matrix, ray_origin, view_vector, ray_target)
  71
  72
  73 def construct_roto_widget(alpha_0, alpha_1, fac, r1, r2, angstep):
  74     if alpha_1 >= alpha_0: alpha = alpha_1 - alpha_0
  75     else: alpha = alpha_1 + (360 - alpha_0)
  76     ring_sides = abs(int(alpha / angstep)) + 1
  77     np_print('ring_sides', ring_sides)
  78     ring_angstep = alpha / ring_sides
  79     np_print('ring_sides, ring_angstep', ring_sides, ring_angstep)
  80     ring = []
  81     ang = alpha_0
  82     co1 = Vector((0.0, 0.0, 0.0))
  83     co1[0] = round(cos(radians(ang)), 8)
  84     co1[1] = round(sin(radians(ang)), 8)
  85     co1 = co1 * r1 * fac
  86     ring.append(co1)
  87     co2 = Vector((0.0, 0.0, 0.0))
  88     co2[0] = round(cos(radians(ang)), 8)
  89     co2[1] = round(sin(radians(ang)), 8)
  90     co2 = co2 * r2 * fac
  91     ring.append(co2)
  92     for i in range(2, (2 + ring_sides)):
  93         co = Vector((0.0, 0.0, 0.0))
  94         ang = ang + ring_angstep
  95         co[0] = round(cos(radians(ang)), 8)
  96         co[1] = round(sin(radians(ang)), 8)
  97         co = co * r2 * fac
  98         #np_print('co', co)
  99         #np_print('ring', ring)
 100         ring.append(co)
 101     np_print('one done')
 102     for i in range((2 + ring_sides) + 1, (2 + (2 * ring_sides)+2)):
 103         co = Vector((0.0, 0.0, 0.0))
 104         co[0] = round(cos(radians(ang)), 8)
 105         co[1] = round(sin(radians(ang)), 8)
 106         ang = ang - ring_angstep
 107         #np_print('co', co)
 108         #np_print('ring', ring)
 109         co = co * r1 * fac
 110         ring.append(co)
 111     np_print('two done')
 112     return ring
 113
 114
 115 def construct_circle_2d(r, angstep):
 116
 117     sides = int(360 / angstep) + 1
 118     angstep = 360 / sides
 119     circle = []
 120     ang = 0
 121     for i in range(0, sides):
 122         co = [0.0, 0.0]
 123         co[0] = round(cos(radians(ang)), 8)*r
 124         co[1] = round(sin(radians(ang)), 8)*r
 125         ang = ang + angstep
 126         circle.append(co)
 127     #np_print('circle', circle)
 128     return circle
 129
 130
 131
 132 def get_ro_x_from_iso(region, rv3d, co2d, centerloc):
 133
 134     scenecast = scene_cast(region, rv3d, co2d)
 135     ray_origin = scenecast[6]
 136     view_vector = scenecast[7]
 137     ray_target = scenecast[8]
 138     n = scenecast[2]
 139     if n == Vector((0.0, 0.0, 0.0)):
 140         if ray_origin[2] > 0:
 141             if ray_target[2] < ray_origin[2]: n = Vector((0.0, 0.0, 1.0))
 142             else:
 143                 if view_vector[1] > 0 and abs(view_vector[0]) <= view_vector[1]: n = Vector((0.0, -1.0, 0.0))
 144                 elif view_vector[1] < 0 and abs(view_vector[0]) <= abs(view_vector[1]): n = Vector((0.0, 1.0, 0.0))
 145                 elif view_vector[0] < 0 and abs(view_vector[1]) <= abs(view_vector[0]): n = Vector((1.0, 0.0, 0.0))
 146                 elif view_vector[0] > 0 and abs(view_vector[1]) <= view_vector[0]: n = Vector((-1.0, 0.0, 0.0))
 147         if ray_origin[2] < 0:
 148             if ray_target[2] > ray_origin[2]: n = Vector((0.0, 0.0, 1.0))
 149             else:
 150                 if view_vector[1] > 0 and abs(view_vector[0]) <= view_vector[1]: n = Vector((0.0, -1.0, 0.0))
 151                 elif view_vector[1] < 0 and abs(view_vector[0]) <= abs(view_vector[1]): n = Vector((0.0, 1.0, 0.0))
 152                 elif view_vector[0] < 0 and abs(view_vector[1]) <= abs(view_vector[0]): n = Vector((1.0, 0.0, 0.0))
 153                 elif view_vector[0] > 0 and abs(view_vector[1]) <= view_vector[0]: n = Vector((-1.0, 0.0, 0.0))
 154         if ray_origin[2] == 0:
 155             if view_vector[1] > 0 and abs(view_vector[0]) <= view_vector[1]: n = Vector((0.0, -1.0, 0.0))
 156             elif view_vector[1] < 0 and abs(view_vector[0]) <= abs(view_vector[1]): n = Vector((0.0, 1.0, 0.0))
 157             elif view_vector[0] < 0 and abs(view_vector[1]) <= abs(view_vector[0]): n = Vector((1.0, 0.0, 0.0))
 158             elif view_vector[0] > 0 and abs(view_vector[1]) <= view_vector[0]: n = Vector((-1.0, 0.0, 0.0))
 159     isohipse = mathutils.geometry.intersect_plane_plane(centerloc, n, centerloc, Vector((0.0, 0.0, 1.0)))
 160     #np_print('n = ', n)
 161     #np_print('isohipse = ', isohipse)
 162     if isohipse == (None, None): viso = Vector((0.0, 0.0, 0.0)) + Vector((1.0, 0.0, 0.0))
 163     else: viso = Vector((0.0, 0.0, 0.0)) - isohipse[1]
 164     if isohipse == (None, None): isohipse = (centerloc, (centerloc + Vector((1.0, 0.0, 0.0))))
 165     else: isohipse = (isohipse[0], isohipse[0] - isohipse[1])
 166     viso = viso.normalized()
 167
 168     v = Vector((1.0, 0.0, 0.0))
 169     ro_hor = v.rotation_difference(viso)
 170     if viso[0] == -1.0000:
 171         ro_hor = Quaternion ((0.0000, 0.0000, -0.0000, 1.000))
 172     #ro_hor_deg = degrees(ro_hor)
 173     #np_print('viso = ', viso)
 174     #np_print( viso[0], viso[1], viso[2])
 175     #np_print('ro_hor = ', ro_hor)
 176     #np_print('ro_hor_deg = ', ro_hor_deg)
 177     return (ro_hor, isohipse)
 178
 179 def get_ro_normal_from_vertical(region, rv3d, co2d):
 180
 181     scenecast = scene_cast(region, rv3d, co2d)
 182     ray_origin = scenecast[6]
 183     view_vector = scenecast[7]
 184     ray_target = scenecast[8]
 185     hitloc = scenecast[1]
 186     n = scenecast[2]
 187     if n == Vector((0.0, 0.0, 0.0)):
 188         if ray_origin[2] > 0:
 189             if ray_target[2] < ray_origin[2]:
 190                 n = Vector((0.0, 0.0, 1.0))
 191                 if ray_origin[2] > 10: draw_plane_point = Vector((0.0, 0.0, (ray_origin[2] - 10)))
 192                 else: draw_plane_point = Vector((0.0, 0.0, 0.0))
 193             else:
 194                 if view_vector[1] > 0 and abs(view_vector[0]) <= view_vector[1]:
 195                     n = Vector((0.0, -1.0, 0.0))
 196                     if ray_origin[1] > 0 or ray_origin[1] < -10: draw_plane_point = Vector((0.0, (ray_origin[1] + 10), 0.0))
 197                     else: draw_plane_point = Vector((0.0, 0.0, 0.0))
 198                 elif view_vector[1] < 0 and abs(view_vector[0]) <= abs(view_vector[1]):
 199                     n = Vector((0.0, 1.0, 0.0))
 200                     if ray_origin[1] < 0 or ray_origin[1] > 10: draw_plane_point = Vector((0.0, (ray_origin[1] - 10), 0.0))
 201                     else: draw_plane_point = Vector((0.0, 0.0, 0.0))
 202                 elif view_vector[0] < 0 and abs(view_vector[1]) <= abs(view_vector[0]):
 203                     n = Vector((1.0, 0.0, 0.0))
 204                     if ray_origin[0] > 10 or ray_origin[0] < 0: draw_plane_point = Vector(((ray_origin[0] - 10), 0.0, 0.0))
 205                     else: draw_plane_point = Vector((0.0, 0.0, 0.0))
 206                 elif view_vector[0] > 0 and abs(view_vector[1]) <= view_vector[0]:
 207                     n = Vector((-1.0, 0.0, 0.0))
 208                     if ray_origin[0] < -10 or ray_origin[0] > 0: draw_plane_point = Vector(((ray_origin[0] + 10), 0.0, 0.0))
 209                     else: draw_plane_point = Vector((0.0, 0.0, 0.0))
 210         if ray_origin[2] < 0:
 211             if ray_target[2] > ray_origin[2]:
 212                 n = Vector((0.0, 0.0, -1.0))
 213                 if ray_origin[2] < -10: draw_plane_point = Vector((0.0, 0.0, (ray_origin[2] + 10)))
 214                 else: draw_plane_point = Vector((0.0, 0.0, 0.0))
 215             else:
 216                 if view_vector[1] > 0 and abs(view_vector[0]) <= view_vector[1]:
 217                     n = Vector((0.0, -1.0, 0.0))
 218                     if ray_origin[1] > 0 or ray_origin[1] < -10: draw_plane_point = Vector((0.0, (ray_origin[1] + 10), 0.0))
 219                     else: draw_plane_point = Vector((0.0, 0.0, 0.0))
 220                 elif view_vector[1] < 0 and abs(view_vector[0]) <= abs(view_vector[1]):
 221                     n = Vector((0.0, 1.0, 0.0))
 222                     if ray_origin[1] < 0 or ray_origin[1] > 10: draw_plane_point = Vector((0.0, (ray_origin[1] - 10), 0.0))
 223                     else: draw_plane_point = Vector((0.0, 0.0, 0.0))
 224                 elif view_vector[0] < 0 and abs(view_vector[1]) <= abs(view_vector[0]):
 225                     n = Vector((1.0, 0.0, 0.0))
 226                     if ray_origin[0] > 10 or ray_origin[0] < 0: draw_plane_point = Vector(((ray_origin[0] - 10), 0.0, 0.0))
 227                     else: draw_plane_point = Vector((0.0, 0.0, 0.0))
 228                 elif view_vector[0] > 0 and abs(view_vector[1]) <= view_vector[0]:
 229                     n = Vector((-1.0, 0.0, 0.0))
 230                     if ray_origin[0] < -10 or ray_origin[0] > 0: draw_plane_point = Vector(((ray_origin[0] + 10), 0.0, 0.0))
 231                     else: draw_plane_point = Vector((0.0, 0.0, 0.0))
 232         if ray_origin[2] == 0:
 233             if view_vector[1] > 0 and abs(view_vector[0]) <= view_vector[1]:
 234                 n = Vector((0.0, -1.0, 0.0))
 235                 if ray_origin[1] > 0 or ray_origin[1] < -10: draw_plane_point = Vector((0.0, (ray_origin[1] + 10), 0.0))
 236                 else: draw_plane_point = Vector((0.0, 0.0, 0.0))
 237             elif view_vector[1] < 0 and abs(view_vector[0]) <= abs(view_vector[1]):
 238                 n = Vector((0.0, 1.0, 0.0))
 239                 if ray_origin[1] < 0 or ray_origin[1] > 10: draw_plane_point = Vector((0.0, (ray_origin[1] - 10), 0.0))
 240                 else: draw_plane_point = Vector((0.0, 0.0, 0.0))
 241             elif view_vector[0] < 0 and abs(view_vector[1]) <= abs(view_vector[0]):
 242                 n = Vector((1.0, 0.0, 0.0))
 243                 if ray_origin[0] > 10 or ray_origin[0] < 0: draw_plane_point = Vector(((ray_origin[0] - 10), 0.0, 0.0))
 244                 else: draw_plane_point = Vector((0.0, 0.0, 0.0))
 245             elif view_vector[0] > 0 and abs(view_vector[1]) <= view_vector[0]:
 246                 n = Vector((-1.0, 0.0, 0.0))
 247                 if ray_origin[0] < -10 or ray_origin[0] > 0: draw_plane_point = Vector(((ray_origin[0] + 10), 0.0, 0.0))
 248                 else: draw_plane_point = Vector((0.0, 0.0, 0.0))
 249         hitloc = mathutils.geometry.intersect_line_plane(ray_origin, (ray_origin + view_vector), draw_plane_point, n)
 250
 251     v = Vector((0.0, 0.0, 1.0))
 252     np_print('n', n)
 253     q = v.rotation_difference(n)
 254     v = Vector((0.0, 0.0, 1.0))
 255     ro = v.rotation_difference(n)
 256     #np_print('ro = ', ro)
 257
 258     return (n, ro, hitloc)
 259
 260
 261
 262 def get_fac_from_view_loc_plane(region, rv3d, rmin, centerloc, q):
 263
 264     # writing the dots for circle at center of scene:
 265     radius = 1
 266     ang = 0.0
 267     circle = [(0.0 ,0.0 ,0.0)]
 268     while ang < 360.0:
 269         circle.append(((cos(radians(ang)) * radius), (sin(radians(ang)) * radius), (0.0)))
 270         ang += 10
 271     circle.append(((cos(radians(0.0)) * radius), (sin(radians(0.0)) * radius), (0.0)))
 272
 273     # rotating and translating the circle to user picked angle and place:
 274     circle = rotate_graphic(circle, q)
 275     circle = translate_graphic(circle, centerloc)
 276
 277     rmax = 1
 278     for i, co in enumerate(circle):
 279         co = view3d_utils.location_3d_to_region_2d(region, rv3d, co)
 280         circle[i] = co
 281     for i in range(1, 18):
 282         r = (circle[0] - circle[i]).length
 283         r1 = (circle[0] - circle[i + 18]).length
 284         #if (r + r1) > rmax and abs(r - r1) < min(r, r1)/5: rmax = (r+r1)/2
 285         #if (r + r1) > rmax and abs(r - r1) < min(r, r1)/10: rmax = r + r1
 286         if (r + r1) > rmax and (r + r1) / 2 < rmin: rmax = (r + r1)
 287         elif (r + r1) > rmax and (r + r1) / 2 >= rmin: rmax = (r + r1) * rmin / (((r + r1) / 2)- ((r + r1) / 2) - rmin)
 288         rmax = abs(rmax)
 289         circle[i] = co
 290         #np_print('rmin', rmin)
 291         #np_print('rmax', rmax)
 292         fac = (rmin * 2) / rmax
 293     return fac
 294
 295 def rotate_graphic(graphic, q):
 296     # rotating the graphic to the angle of chosen normal:
 297     if q != None:
 298         for i, co in enumerate(graphic):
 299             if type(co) == tuple:
 300                 vco = Vector(co)
 301             else: vco = co
 302             if vco != None:
 303                 vco = q * vco
 304             graphic[i] = vco
 305     return graphic
 306
 307 def translate_graphic(graphic, vectorloc):
 308     # translating graphic to the chosen point in scene:
 309     m = copy.deepcopy(graphic[0])
 310     for i, co in enumerate(graphic):
 311         #np_print('co', co)
 312         if type(co) == tuple:
 313             vco = Vector(co)
 314         else: vco = co
 315         if vco != None:
 316             vco = vco + vectorloc
 317         graphic[i] = vco
 318     return graphic
 319
 320
 321 def get_angle_from_iso_planar(centerloc, normal, proj):
 322     np_print('normal', normal)
 323     np_print('centerloc', centerloc)
 324     np_print('proj', proj)
 325     isohipse = mathutils.geometry.intersect_plane_plane(centerloc, normal, centerloc, Vector((0.0, 0.0, 1.0)))
 326     np_print('isohipse', isohipse)
 327     if isohipse == (None, None):
 328         isohipse = (centerloc, (centerloc + Vector((1.0, 0.0, 0.0))))
 329         hor = True
 330     else:
 331         isohipse = (isohipse[0], isohipse[0] - isohipse[1])
 332         hor = False
 333     np_print('isohipse', isohipse)
 334     vco1 = isohipse[1] - isohipse[0]
 335     vco2 = proj - centerloc
 336     np_print('vco1', vco1)
 337     np_print('vco2', vco2)
 338     if hor:
 339         if proj[1] < centerloc[1]: isoang = 360 - degrees(vco1.angle(vco2))
 340         else: isoang = degrees(vco1.angle(vco2))
 341     else:
 342         if proj[2] < centerloc[2]: isoang = 360 - degrees(vco1.angle(vco2))
 343         else: isoang = degrees(vco1.angle(vco2))
 344
 345     return isoang, isohipse
 346     '''
 347     Return the intersection between two planes
 348     Parameters:
 349
 350         plane_a_co (mathutils.Vector) – Point on the first plane
 351         plane_a_no (mathutils.Vector) – Normal of the first plane
 352         plane_b_co (mathutils.Vector) – Point on the second plane
 353         plane_b_no (mathutils.Vector) – Normal of the second plane
 354
 355     Returns:
 356
 357     The line of the intersection represented as a point and a vector
 358     Return type:
 359
 360     tuple pair of mathutils.Vector or None if the intersection can’t be calculated
 361     '''
 362
 363 def get_angle_vector_from_vector(centerloc, startloc, endloc):
 364     np_print('centerloc', centerloc)
 365     np_print('startloc', startloc)
 366     np_print('endloc', endloc)
 367
 368     vco1 = endloc - centerloc
 369     vco2 = startloc - centerloc
 370     np_print('vco1', vco1)
 371     np_print('vco2', vco2)
 372     alpha = degrees(vco1.angle(vco2))
 373     return alpha
 374
 375
 376 def get_eul_z_angle_dif_in_rotated_system(eul_0, eul_1, n):
 377
 378     np_print('eul_0 =', eul_0)
 379     np_print('eul_1 =', eul_1)
 380     v = Vector ((0.0, 0.0, 1.0))
 381     if n[2] < 0:
 382         n = Vector((-n[0], -n[1], -n[2]))
 383         n_dif_quat = n.rotation_difference(v)
 384         np_print('n_dif_quat =', n_dif_quat)
 385         n_dif_eul = n_dif_quat.to_euler()
 386         np_print('n_dif_eul =', n_dif_eul)
 387         a = copy.deepcopy(eul_0)
 388         np_print('a =', a)
 389         b = copy.deepcopy(eul_1)
 390         np_print('b =', b)
 391         a.rotate(n_dif_eul)
 392         b.rotate(n_dif_eul)
 393         np_print('a =', a)
 394         np_print('b =', b)
 395         a_z = degrees(a.z)
 396         b_z = degrees(b.z)
 397         np_print('a_z =', a_z)
 398         np_print('b_z =', b_z)
 399         alpha_real = a_z - b_z
 400     else:
 401         n_dif_quat = n.rotation_difference(v)
 402         np_print('n_dif_quat =', n_dif_quat)
 403         n_dif_eul = n_dif_quat.to_euler()
 404         np_print('n_dif_eul =', n_dif_eul)
 405         a = copy.deepcopy(eul_0)
 406         np_print('a =', a)
 407         b = copy.deepcopy(eul_1)
 408         np_print('b =', b)
 409         a.rotate(n_dif_eul)
 410         b.rotate(n_dif_eul)
 411         np_print('a =', a)
 412         np_print('b =', b)
 413         a_z = degrees(a.z)
 414         b_z = degrees(b.z)
 415         np_print('a_z =', a_z)
 416         np_print('b_z =', b_z)
 417         alpha_real = a_z - b_z
 418         alpha_real = -alpha_real
 419     return alpha_real
 420
 421
 422 def get_eul_z_angle_diff_in_rotated_system(eul_0, eul_1, eul_zero, ro_hor):
 423
 424
 425     np_print('eul_0 =', eul_0)
 426     np_print('eul_1 =', eul_1)
 427     np_print('eul_zero =', eul_zero)
 428
 429     a = copy.deepcopy(eul_0)
 430     b = copy.deepcopy(eul_1)
 431     zero = copy.deepcopy(eul_zero)
 432     np_print('a =', a)
 433     np_print('b =', b)
 434     np_print('zero =', zero)
 435     a.x = -a.x
 436     a.y = -a.y
 437     a.z = -a.z
 438     eul_0.rotate(a)
 439     #b.rotate(-ro_hor)
 440     b.rotate(a)
 441     np_print('a =', a)
 442     np_print('eul_0 =', eul_0)
 443     np_print('b =', b)
 444     b_z = degrees(round((b.z), 4))
 445     np_print('b_z =', b_z)
 446     alpha_real = b_z
 447
 448
 449     return alpha_real
 450
 451
 452
 453 def get_eul_z_angle_difff_in_rotated_system(eul_0, eul_1, n):
 454
 455     np_print('eul_0 =', eul_0)
 456     np_print('eul_1 =', eul_1)
 457
 458     eul_delta = Euler(((eul_1.x - eul_0.x), (eul_1.y - eul_0.y), (eul_1.z - eul_0.z)), 'XYZ')
 459     np_print('eul_delta =', eul_delta)
 460     np_print('n =', n)
 461     v = Vector ((0.0, 0.0, 1.0))
 462
 463     n_dif_quat = n.rotation_difference(v)
 464     np_print('n_dif_quat =', n_dif_quat)
 465     n_dif_eul = n_dif_quat.to_euler()
 466     np_print('n_dif_eul =', n_dif_eul)
 467     eul_delta.rotate(n_dif_eul)
 468     np_print('eul_delta', eul_delta)
 469     alpha_real = degrees(eul_delta.z)
 470     np_print('alpha_real', alpha_real)
 471
 472     return alpha_real
 473
 474
 475
 476 def get_eul_z_angle_diffff_in_rotated_system(eul_0, eul_1, n):
 477
 478     np_print('eul_0 =', eul_0)
 479     np_print('eul_1 =', eul_1)
 480     np_print('n =', n)
 481     v = Vector ((0.0, 0.0, 1.0))
 482     n_dif_quat = n.rotation_difference(v)
 483     np_print('n_dif_quat =', n_dif_quat)
 484     a = copy.deepcopy(eul_0)
 485     b = copy.deepcopy(eul_1)
 486     a.rotate(n_dif_quat)
 487     b.rotate(n_dif_quat)
 488     np_print('eul_0 =', a)
 489     np_print('eul_1 =', b)
 490     alpha_real = degrees(b.z - a.z )
 491
 492     '''
 493
 494     if int(b.y) == -3 and b.z > 0:
 495         alpha_real = alpha_real - (180*int(b.z / 1.55))
 496     elif int(b.y) == -6 and b.z > 0:
 497         alpha_real = alpha_real - (180*int(b.z / 1.55))
 498     elif int(b.y) == -0 and b.z > 3:
 499         alpha_real = alpha_real - (180*int(b.z / 1.55))
 500     elif int(b.y) == -3 and b.z < 0:
 501         alpha_real = alpha_real + (180*int(b.z / 1.55))
 502     elif int(b.y) == -6 and b.z < 0:
 503         alpha_real = alpha_real + (180*int(b.z / 1.55))
 504     elif int(b.y) == -0 and b.z < -3:
 505         alpha_real = alpha_real + (180*int(b.z / 1.55))
 506
 507     '''
 508
 509     np_print('alpha_real', alpha_real)
 510
 511     return alpha_real
 512
 513
 514
 515 def get_eul_z_angle_difffff_in_rotated_system(eul_0, eul_1, n):
 516
 517
 518     np_print('n =', n)
 519     c = copy.deepcopy(eul_0)
 520     d = copy.deepcopy(eul_1)
 521     np_print('------- eul_0 =', eul_0)
 522     np_print('------- eul_0 =', round(degrees(eul_0.x),2), round(degrees(eul_0.y),2), round(degrees(eul_0.z),2))
 523     np_print('------- eul_1 =', round(degrees(eul_1.x),2), round(degrees(eul_1.y),2), round(degrees(eul_1.z),2))
 524     v = Vector ((0.0, 0.0, 1.0))
 525     if n[2] > 0:
 526         m = copy.deepcopy(n)
 527         if abs(n[0]) == 0.0000: m[0] = 0.001
 528         if abs(n[1]) == 0.0000: m[1] = 0.001
 529     elif n[2] < 0:
 530         m = copy.deepcopy(-n)
 531         if abs(n[0]) == 0.0000: m[0] = 0.001
 532         if abs(n[1]) == 0.0000: m[1] = 0.001
 533     else:
 534         if abs(n[0]) == 0.0000: m = Vector ((0.001, n[1], 0.001))
 535         elif abs(n[1]) == 0.0000: m = Vector ((n[0], 0.001, 0.001))
 536         else: m = Vector ((n[0], n[1], 0.001))
 537     np_print('................m =', m)
 538     if abs(eul_0.x) == 0: c.x = 0.001
 539     if abs(eul_0.y) == 0: c.y = 0.001
 540     if abs(eul_0.z) == 0: c.z = 0.001
 541     if round(eul_0.x, 4) == 1.5708: c.x = 1.5700
 542     if round(eul_0.y, 4) == 1.5708: c.y = 1.5700
 543     if round(eul_0.z, 4) == 1.5708: c.z = 1.5700
 544     if round(eul_0.x, 4) == -1.5708: c.x = -1.5700
 545     if round(eul_0.y, 4) == -1.5708: c.y = -1.5700
 546     if round(eul_0.z, 4) == -1.5708: c.z = -1.5700
 547     np_print('------- eul_0 =', c)
 548     n_dif_quat = m.rotation_difference(v)
 549     n_dif_eul = n_dif_quat.to_euler()
 550     np_print('``````` n_dif_eul =', round(degrees(n_dif_eul.x),2), round(degrees(n_dif_eul.y),2), round(degrees(n_dif_eul.z),2))
 551     a = copy.deepcopy(c)
 552     b = copy.deepcopy(d)
 553     a.rotate(n_dif_eul)
 554     b.rotate(n_dif_eul)
 555     np_print('>>>>>>> eul_0 =', round(degrees(a.x),2), round(degrees(a.y),2), round(degrees(a.z),2))
 556     np_print('>>>>>>> eul_1 =', round(degrees(b.x),2), round(degrees(b.y),2), round(degrees(b.z),2))
 557     np_print('------- eul_1 =', b)
 558
 559     '''
 560     if int(b.y) == -3 and b.z > 0:
 561         alpha_real = alpha_real - (180*int(b.z / 1.55))
 562     elif int(b.y) == -6 and b.z > 0:
 563         alpha_real = alpha_real - (180*int(b.z / 1.55))
 564     elif int(b.y) == -0 and b.z > 3:
 565         alpha_real = alpha_real - (180*int(b.z / 1.55))
 566     elif int(b.y) == -3 and b.z < 0:
 567         alpha_real = alpha_real + (180*int(b.z / 1.55))
 568     elif int(b.y) == -6 and b.z < 0:
 569         alpha_real = alpha_real + (180*int(b.z / 1.55))
 570     elif int(b.y) == -0 and b.z < -3:
 571         alpha_real = alpha_real + (180*int(b.z / 1.55))
 572     '''
 573     alpha_real = degrees(b.z - a.z )
 574     if n[2] < 0: alpha_real = - alpha_real
 575     np_print('alpha_real', alpha_real)
 576
 577     return alpha_real
 578
 579
 580
 581 def get_eul_z_angle_diffffff_in_rotated_system(eul_0, eul_1, n): # v6?
 582
 583
 584     np_print('n =', n)
 585     c = eul_0.copy()
 586     d = eul_1.copy()
 587     np_print('------- eul_0 =', eul_0)
 588     np_print('------- eul_0 =', round(degrees(eul_0.x),6), round(degrees(eul_0.y),6), round(degrees(eul_0.z),6))
 589     np_print('------- eul_1 =', round(degrees(eul_1.x),6), round(degrees(eul_1.y),6), round(degrees(eul_1.z),6))
 590     v = Vector ((0.0, 0.0, 1.0))
 591     if n[2] > 0:
 592         m = n.copy()
 593         if abs(n[0]) == 0.0: m[0] = 0.000001
 594         if abs(n[1]) == 0.0: m[1] = 0.000001
 595     elif n[2] < 0:
 596         m = -n.copy()
 597         if abs(n[0]) == 0.0: m[0] = 0.000001
 598         if abs(n[1]) == 0.0: m[1] = 0.000001
 599     else:
 600         if abs(n[0]) == 0.0: m = Vector ((0.000001, n[1], 0.000001))
 601         elif abs(n[1]) == 0.0: m = Vector ((n[0], 0.000001, 0.000001))
 602         else: m = Vector ((n[0], n[1], 0.000001))
 603     np_print('................m =', m)
 604     if abs(eul_0.x) == 0: c.x = 0.000001
 605     if abs(eul_0.y) == 0: c.y = 0.000001
 606     if abs(eul_0.z) == 0: c.z = 0.000001
 607     # 90 deg == 1.5707963268 rad
 608     if round(eul_0.x, 6) == 1.570796: c.x = 1.5708
 609     if round(eul_0.y, 6) == 1.570796: c.y = 1.5708
 610     if round(eul_0.z, 6) == 1.570796: c.z = 1.5708
 611     if round(eul_0.x, 6) == -1.570796: c.x = -1.5708
 612     if round(eul_0.y, 6) == -1.570796: c.y = -1.5708
 613     if round(eul_0.z, 6) == -1.570796: c.z = -1.5708
 614     np_print('------- eul_0 =', c)
 615     n_dif_quat = m.rotation_difference(v)
 616     n_dif_eul = n_dif_quat.to_euler()
 617     np_print('``````` n_dif_eul =', round(degrees(n_dif_eul.x),6), round(degrees(n_dif_eul.y),6), round(degrees(n_dif_eul.z),6))
 618     a = c.copy()
 619     b = d.copy()
 620     a.rotate(n_dif_eul)
 621     b.rotate(n_dif_eul)
 622     np_print('>>>>>>> eul_0 =', round(degrees(a.x),6), round(degrees(a.y),6), round(degrees(a.z),6))
 623     np_print('>>>>>>> eul_1 =', round(degrees(b.x),6), round(degrees(b.y),6), round(degrees(b.z),6))
 624     np_print('------- eul_1 =', b)
 625
 626     alpha_real = degrees(b.z - a.z )
 627     if n[2] < 0: alpha_real = - alpha_real
 628     np_print('alpha_real', alpha_real)
 629
 630     return alpha_real