75 lines
2.6 KiB
Python
75 lines
2.6 KiB
Python
import numpy as np
|
|
|
|
def case1(E: float, F: float, G: float, pG: np.ndarray, pE: np.ndarray):
|
|
j11 = 2 * E * pE[0]
|
|
j12 = 2 * F * pE[1]
|
|
j13 = 2 * G * pE[2]
|
|
j21 = F * pE[1] - (pE[1] - pG[1]) * E
|
|
j22 = (pE[0] - pG[0]) * F - E * pE[0]
|
|
j23 = 0
|
|
j31 = G * pE[2] - (pE[2] - pG[2]) * E
|
|
j32 = 0
|
|
j33 = (pE[0] - pG[0]) * G - E * pE[0]
|
|
|
|
detJ = j11 * j22 * j33 - j21 * j12 * j33 - j31 * j13 * j22
|
|
|
|
invJ = 1/detJ * np.array([[j22*j33, -j12*j33, -j13*j22],
|
|
[-j21*j33, j11*j33-j13*j31, j13*j21],
|
|
[-j22*j31, j12*j31, j11*j22-j12*j21]])
|
|
|
|
fxE = np.array([E*pE[0]**2 + F*pE[1]**2 + G*pE[2]**2 - 1,
|
|
(pE[0]-pG[0]) * F*pE[1] - (pE[1]-pG[1]) * E*pE[0],
|
|
(pE[0]-pG[0]) * G*pE[2] - (pE[2]-pG[2]) * E*pE[0]])
|
|
|
|
return invJ, fxE
|
|
|
|
def case2(E: float, F: float, G: float, pG: np.ndarray, pE: np.ndarray):
|
|
j11 = 2 * E * pE[0]
|
|
j12 = 2 * F * pE[1]
|
|
j13 = 2 * G * pE[2]
|
|
j21 = F * pE[1] - (pE[1] - pG[1]) * E
|
|
j22 = (pE[0] - pG[0]) * F - E * pE[0]
|
|
j23 = 0
|
|
j31 = 0
|
|
j32 = G * pE[2] - (pE[2] - pG[2]) * F
|
|
j33 = (pE[1] - pG[1]) * G - F * pE[1]
|
|
|
|
detJ = j11 * j22 * j33 - j21 * j12 * j33 + j21 * j13 * j32
|
|
if detJ == 0:
|
|
invJ, fxE = case3(E, F, G, pG, pE)
|
|
else:
|
|
invJ = 1/detJ * np.array([[j22*j33, -(j12*j33-j13*j32), -j13*j22],
|
|
[-j21*j33, j11*j33, j13*j21],
|
|
[j21*j32, -j11*j32, j11*j22-j12*j21]])
|
|
|
|
fxE = np.array([E*pE[0]**2 + F*pE[1]**2 + G*pE[2]**2 - 1,
|
|
(pE[0]-pG[0]) * F*pE[1] - (pE[1]-pG[1]) * E*pE[0],
|
|
(pE[1]-pG[1]) * G*pE[2] - (pE[2]-pG[2]) * F*pE[1]])
|
|
|
|
return invJ, fxE
|
|
|
|
def case3(E: float, F: float, G: float, pG: np.ndarray, pE: np.ndarray):
|
|
j11 = 2 * E * pE[0]
|
|
j12 = 2 * F * pE[1]
|
|
j13 = 2 * G * pE[2]
|
|
j21 = G * pE[2] - (pE[2] - pG[2]) * E
|
|
j22 = 0
|
|
j23 = (pE[0] - pG[0]) * G - E * pE[0]
|
|
j31 = 0
|
|
j32 = G * pE[2] - (pE[2] - pG[2]) * F
|
|
j33 = (pE[1] - pG[1]) * G - F * pE[1]
|
|
|
|
detJ = -j11 * j23 * j32 - j21 * j12 * j33 + j21 * j13 * j32
|
|
if detJ == 0:
|
|
invJ, fxE = case2(E, F, G, pG, pE)
|
|
else:
|
|
invJ = 1/detJ * np.array([[-j23*j32, -(j12*j33-j13*j32), j12*j23],
|
|
[-j21*j33, j11*j33, -(j11*j23-j13*j21)],
|
|
[j21*j32, -j11*j32, -j12*j21]])
|
|
|
|
fxE = np.array([E*pE[0]**2 + F*pE[1]**2 + G*pE[2]**2 - 1,
|
|
(pE[0]-pG[0]) * G*pE[2] - (pE[2]-pG[2]) * E*pE[0],
|
|
(pE[1]-pG[1]) * G*pE[2] - (pE[2]-pG[2]) * F*pE[1]])
|
|
|
|
return invJ, fxE
|