, Visual C#


         

Tx, Ty,


r2(2, 2) = D / L : r2(2, 3) = -d3 / L
r2(3, 2) = -r2(2, 3) : r2(3, 3) = r2(2, 2)
MatrixIdentity(R2i)
R2i(2, 2) = r2(2, 2) : R2i(2, 3) = -r2(2, 3)
R2i(3, 2) = -r2(3, 2) : R2i(3, 3) = r2(3, 3)
' ( y):
MatrixYRotate(Rot3, theta)
' :
m3MatMultiply(M12, T, R1)
m3MatMultiply(M34, r2, Rot3)
m3MatMultiply(M1234, M12, M34)
m3MatMultiply(M56, R2i, R1i)
m3MatMultiply(M567, M56, Ti)
m3MatMultiply(M, M1234, M567)
End Sub
' (3-D transformation matrix)
' Tx, Ty, Tz:
Public Sub MatrixTranslate(ByRef M(,) As Single, _
ByVal Tx As Single, ByVal Ty As Single, ByVal Tz As Single)
MatrixIdentity(M)
M(4, 1) = Tx : M(4, 2) = Ty : M(4, 3) = Tz
End Sub
' (3-D transformation matrix)
' y ( - ):
Public Sub MatrixYRotate(ByRef M(,) As Single, _
ByVal theta As Single)
MatrixIdentity(M)
M(1, 1) = CSng(Cos(theta))
M(3, 3) = M(1, 1)
M(3, 1) = CSng(Sin(theta))
M(1, 3) = -M(3, 1)
End Sub
' ,
' 0, 0, 0, 1
' .
'x y , z :
Public Sub MatrixApplyFull(ByRef V() As Single, _
ByRef M(,) As Single, ByRef Result() As Single)
Dim i As Integer : Dim j As Integer
Dim value As Single
For i = 1 To 4
value = 0
For j = 1 To 4
value = value + V(j) * M(j, i)
Next j
Result(i) = value
Next i
' (value = Result(4)):
If value <> 0 Then
Result(1) = Result(1) / value
Result(2) = Result(2) / value