直交座標系のずれ量を飛跡の角度に相当するRadial Lateral空間に変換するコード

放射線の飛跡のように、座標(cx, cy) 角度 (ax = dx/dz, ay = dy/dz)で表すことの出来る量の比較をする時、しばしば飛跡の進行方向(Radial方向)と垂直方向(Lateral方向)に分離して考えることがある。

飛跡1 (cx1, cy1, ax1, ay1)、飛跡2 (cx2, cy2, ax2, ay2) があるとき、角度差は次のように求めることができる。

import math

# 方法1
def xy2radial(cx1,cy1,ax1,ay1,cx2,cy2,ax2,ay2):
    ax = (ax1 + ax2) * 0.5
    ay = (ay1 + ay2) * 0.5
    r = (ax ** 2 + ay ** 2) ** 0.5
    ax /= r
    ay /= r
    dax = ax2 - ax1
    day = ay2 - ay1
    dar = ax * dax + ay * day
    dal = -ay * dax + ax * day
    dcx = cx2 - cx1
    dcy = cy2 - cy1
    dcr = ax * dcx + ay * dcy
    dcl = -ay * dcx + ax * dcy

    return dcr,dcl,dar,dal

# 方法2
def xy2radial2(cx1,cy1,ax1,ay1,cx2,cy2,ax2,ay2):
    ax = (ax1 + ax2) * 0.5
    ay = (ay1 + ay2) * 0.5
    rotation_cos = math.cos(-math.atan2(ay,ax))
    rotation_sin = math.sin(-math.atan2(ay,ax))
    dax = ax2 - ax1
    day = ay2 - ay1
    dar = dax * rotation_cos - day * rotation_sin
    dal = dax * rotation_sin + day * rotation_cos
    dx = cx2 - cx1
    dy = cy2 - cy1
    dcr = dx * rotation_cos - dy * rotation_sin
    dcl = dx * rotation_sin + dy * rotation_cos

    return dcr,dcl,dar,dal