, Visual C#


         


MatrixIdentity(S)
S(2, 2) = -1
' :
m3MatMultiply(M12, T, R1) : m3MatMultiply(M34, r2, S)
m3MatMultiply(M1234, M12, M34)
m3MatMultiply(M56, R2i, R1i)
m3MatMultiply(M567, M56, Ti)
m3MatMultiply(M, M1234, M567)
End Sub
' theta
' , (p1, p2, p3)
' <d1, d2, d3>.
' theta ,
' ,
' :
Public Sub m3LineRotate(ByRef M(,) As Single, _
ByVal p1 As Single, ByVal p2 As Single, ByVal p3 As Single, _
ByVal d1 As Single, ByVal d2 As Single, ByVal d3 As Single, _
ByVal theta As Single)
Dim T(4, 4) As Single '.
Dim R1(4, 4) As Single ' 1.
Dim r2(4, 4) As Single ' 2.
Dim Rot3(4, 4) As Single '.
Dim R2i(4, 4) As Single ' 2.
Dim R1i(4, 4) As Single ' 1.
Dim Ti(4, 4) As Single ' .
Dim D As Single : Dim L As Single
Dim M12(4, 4) As Single : Dim M34(4, 4) As Single
Dim M1234(4, 4) As Single
Dim M56(4, 4) As Single : Dim M567(4, 4) As Single
' :
MatrixTranslate(T, -p1, -p2, -p3)
MatrixTranslate(Ti, p1, p2, p3)
' z,
' y-z :
MatrixIdentity(R1)
D = CSng(Sqrt(d1 * d1 + d2 * d2))
R1(1, 1) = d2 / D : R1(1, 2) = d1 / D
R1(2, 1) = -R1(1, 2) : R1(2, 2) = R1(1, 1)
MatrixIdentity(R1i)
R1i(1, 1) = R1(1, 1) : R1i(1, 2) = -R1(1, 2)
R1i(2, 1) = -R1(2, 1) : R1i(2, 2) = R1(2, 2)
' x, y:
MatrixIdentity(r2)
L = CSng(Sqrt(d1 * d1 + d2 * d2 + d3 * d3))