def get_grad_S_inv(v):
x, y, z = v
grad_S_inv = np.zeros((3, 3))
r = np.linalg.norm(v)
if _jit_isclose(r, 0):
pass
elif _jit_isclose(x**2 + y**2, 0):
grad_S_inv[0, 0] = 0.
grad_S_inv[0, 1] = 0.
grad_S_inv[0, 2] = 1
grad_S_inv[1, 0] = -1 / z
grad_S_inv[1, 1] = -1 / z
grad_S_inv[1, 2] = 0.
grad_S_inv[2, 0] = 0.
grad_S_inv[2, 1] = 0.
grad_S_inv[2, 2] = 0.
else:
grad_S_inv[0, 0] = x / r
grad_S_inv[0, 1] = y / r
grad_S_inv[0, 2] = z / r
grad_S_inv[1, 0] = -x * z / (sqrt(x**2 + y**2) * r**2)
grad_S_inv[1, 1] = -y * z / (sqrt(x**2 + y**2) * r**2)
grad_S_inv[1, 2] = sqrt(x**2 + y**2) / r**2
grad_S_inv[2, 0] = y / (x**2 + y**2)
grad_S_inv[2, 1] = -x / (x**2 + y**2)
grad_S_inv[2, 2] = 0.
return grad_S_inv
def get_S(C, j):
S = np.zeros(3)
r, alpha, delta = C[:, j]
if _jit_isclose(alpha, np.pi):
S[2] = r
elif _jit_isclose(alpha, 0):
S[2] = -r
else:
S[0] = r * sin(alpha) * cos(delta)
S[1] = -r * sin(alpha) * sin(delta)
S[2] = -r * cos(alpha)
return S
def get_B(X, c_table, j):
B = np.empty((3, 3))
ref_pos = get_ref_pos(X, c_table[:, j])
BA = ref_pos[:, 1] - ref_pos[:, 0]
if _jit_isclose(BA, 0.).all():
return (ERR_CODE_InvalidReference, B)
AD = ref_pos[:, 2] - ref_pos[:, 1]
B[:, 2] = -_jit_normalize(BA)
N = _jit_cross(AD, BA)
if _jit_isclose(N, 0.).all():
return (ERR_CODE_InvalidReference, B)
B[:, 1] = _jit_normalize(N)
B[:, 0] = _jit_cross(B[:, 1], B[:, 2])
return (ERR_CODE_OK, B)