Function SphereAimAnglesRev11
    Integer center_index_, tool_index_
    Double cx_, cy_, cz_, tx_, ty_, tz_
    Double ax_, ay_, az_
    Double bx_, by_, bz_
    Double vx_, vy_, vz_, s_, c_, vx2_, vy2_, vz2_, denom_
    Double m00_, m01_, m02_, m10_, m11_, m12_, m20_, m21_, m22_
    Double rx_, ry_, rz_, length_

    ' Define tool forward direction as +X
    ax_ = 1
    ay_ = 0
    az_ = 0

    ' Get vector to target: center - tool
    center_index_ = 0
    tool_index_ = 70

    cx_ = CX(P(center_index_))
    cy_ = CY(P(center_index_))
    cz_ = CZ(P(center_index_))

    tx_ = CX(P(tool_index_))
    ty_ = CY(P(tool_index_))
    tz_ = CZ(P(tool_index_))

    bx_ = cx_ - tx_
    by_ = cy_ - ty_
    bz_ = cz_ - tz_

    ' Normalize b_
    length_ = Sqr(bx_ * bx_ + by_ * by_ + bz_ * bz_)
    bx_ = bx_ / length_
    by_ = by_ / length_
    bz_ = bz_ / length_

    ' Cross product: v_ = a_ × b_
    vx_ = ay_ * bz_ - az_ * by_
    vy_ = az_ * bx_ - ax_ * bz_
    vz_ = ax_ * by_ - ay_ * bx_

    ' Dot product: a_ · b_
    c_ = ax_ * bx_ + ay_ * by_ + az_ * bz_

    ' Sine = ||v_||
    s_ = Sqr(vx_ * vx_ + vy_ * vy_ + vz_ * vz_)

    ' Rotation matrix: R = I + V + V^2 * ((1 - c) / s^2)
    If s_ > 0.00001 Then
        vx2_ = vx_ * vx_
        vy2_ = vy_ * vy_
        vz2_ = vz_ * vz_
        denom_ = (1 - c_) / (s_ * s_)

        m00_ = 1 + 0 + 0 + denom_ * (-vy2_ - vz2_)
        m01_ = -vz_ + 0 + denom_ * (vx_ * vy_)
        m02_ = vy_ + 0 + denom_ * (vx_ * vz_)

        m10_ = vz_ + 0 + denom_ * (vy_ * vx_)
        m11_ = 1 + 0 + 0 + denom_ * (-vx2_ - vz2_)
        m12_ = -vx_ + 0 + denom_ * (vy_ * vz_)

        m20_ = -vy_ + 0 + denom_ * (vz_ * vx_)
        m21_ = vx_ + 0 + denom_ * (vz_ * vy_)
        m22_ = 1 + 0 + 0 + denom_ * (-vx2_ - vy2_)
    Else
        ' Vectors are aligned: use identity
        m00_ = 1; m01_ = 0; m02_ = 0
        m10_ = 0; m11_ = 1; m12_ = 0
        m20_ = 0; m21_ = 0; m22_ = 1
    EndIf

    ' Extract Euler angles from rotation matrix (ZYX order)
    ry_ = Atan2(m02_, m22_) * 180 / 3.14159
    rx_ = Asin(-m12_) * 180 / 3.14159
    rz_ = Atan2(m10_, m11_) * 180 / 3.14159

    ' Final move
    P(999) = XY(tx_, ty_, tz_, rz_, rx_, ry_)
    Print P999
    Go P(999)
Fend