三维坐标转换程序运行时旋转角的值一直为0
Dim k3#, Ex#, Ey#, Ez#, dX3#, dY3#, dZ3# '尺度参数,三个旋转参数,三个平移参数
Dim X3#, Y3#, Z3#, Xx3#, Yy3#, Zz3# '三维坐标转换的正算数值
'将度.分秒形式化为弧度:输入为度.分秒形式,输出为弧度
Public Function DoToHu(ByVal DoFenMiao As Double) As Single
Const pi As Double = 3.14159265358979
Dim du%, fen%, miao%, angle#
du = Fix(DoFenMiao)
DoFenMiao = (DoFenMiao - du) * 100
fen = Fix(DoFenMiao)
miao = (DoFenMiao - fen) * 100
angle = du + fen / 60 + miao / 3600
DoToHu = angle * pi / 180
End Function
'弧度化为度.分秒的形式:输入弧度值,输出度.分秒(各占两位)
Public Function HuToDo(ByVal Hu As Double) As Single
Const pi As Double = 3.14159265358979
Dim du%, fen%, miao%
Hu = Hu * 180 / pi
du = Fix(Hu)
Hu = (Hu - du) * 60
fen = Fix(Hu)
Hu = (Hu - fen) * 60
miao = Fix(Hu + 0.5)
If miao = 60 Then
fen = fen + 1
miao = 0
End If
HuToDo = du + fen / 100 + miao / 10000
End Function
'矩阵转置的通用过程
Public Sub MatrixTrans(A() As Double, At() As Double)
Dim i As Integer, j As Integer, rows As Integer, cols As Integer
rows = UBound(A, 1)
cols = UBound(A, 2)
ReDim At(1 To cols, 1 To rows)
For i = 1 To rows
For j = 1 To cols
At(j, i) = A(i, j)
Next j
Next i
End Sub
'矩阵相乘:输入矩阵或数Qa、Qb,自动识别它们的维数,并输出它们的乘积Qn
Public Sub Matrix_Multy(Qn, Qa, Qb)
Dim ia%, ib%, ic%
Dim ai%, bi%, ci%
Dim e1 As Boolean, e2 As Boolean, e3 As Boolean, e4 As Boolean, e5 As Boolean, e6 As Boolean, e7 As Boolean
On Error Resume Next '看Qa是不是一维数组
ic = UBound(Qa, 2) - LBound(Qa, 2)
If Err Then e1 = True
On Error Resume Next '看Qa是不是一维数组
ib = UBound(Qb, 2) - LBound(Qb, 2)
If Err Then e2 = True
If e1 = False And e2 = False Then '二维矩阵相乘
For ai = LBound(Qa, 1) To UBound(Qa, 1)
For bi = LBound(Qb, 2) To UBound(Qb, 2)
For ci = LBound(Qa, 2) To UBound(Qa, 2)
Qn(ai, bi) = Qn(ai, bi) + Qa(ai, ci) * Qb(ci, bi)
Next ci
Next bi
Next ai
ElseIf e1 = True And e2 = False Then
On Error Resume Next
ia = UBound(Qa) - LBound(Qa)
If Err Then e6 = True
If e6 Then '数乘以二维矩阵
For ai = LBound(Qb, 1) To UBound(Qb, 1)
For bi = LBound(Qb, 2) To UBound(Qb, 2)
Qn(ai, bi) = Qa * Qb(ai, bi)
Next bi
Next ai
Else '一维矩阵乘以二维矩阵
For ci = LBound(Qb, 2) To UBound(Qb, 2)
For ai = LBound(Qa, 1) To UBound(Qa, 1)
Qn(ci) = Qn(ci) + Qa(ai) * Qb(ai, ci)
Next ai
Next ci
End If
ElseIf e1 = False And e2 = True Then
On Error Resume Next
ic = UBound(Qb) - LBound(Qb)
If Err Then e7 = True
If e7 Then '二维矩阵乘以数
For ai = LBound(Qa, 1) To UBound(Qa, 1)
For bi = LBound(Qa, 2) To UBound(Qa, 2)
Qn(ai, bi) = Qa(ai, bi) * Qb
Next bi
Next ai
Else '二维矩阵乘以一维矩阵
For ai = LBound(Qa, 1) To UBound(Qa, 1)
For bi = LBound(Qa, 2) To UBound(Qa, 2)
Qn(ai) = Qn(ai) + Qa(ai, bi) * Qb(bi)
Next bi
Next ai
End If
Else
Dim errT As Integer
On Error Resume Next '结果是否是一个数
errT = UBound(Qn)
If Err Then e3 = True
If e3 Then '一维矩阵乘以一维矩阵得一个数
For ai = LBound(Qa, 1) To UBound(Qa, 1)
For bi = LBound(Qa, 2) To UBound(Qa, 2)
Qn = Qn + Qa(ai) * Qb(bi)
Next bi
Next ai
Exit Sub
End If
On Error Resume Next '是否是数乘一维矩阵
ia = UBound(Qa) - LBound(Qa)
If Err Then e4 = True
If e4 Then
For bi = LBound(Qa, 2) To UBound(Qa, 2)
Qn(bi) = Qa * Qb(bi)
Next bi
Exit Sub
End If
On Error Resume Next '是否是一维矩阵乘数
ib = UBound(Qb) - LBound(Qb)
If Err Then e5 = True
If e5 Then
For ai = LBound(Qa, 1) To UBound(Qa, 1)
Qn(ai) = Qa(ai) * Qb
Next ai
Exit Sub
End If
'一维矩阵相乘结果是二维矩阵
For ai = LBound(Qa, 1) To UBound(Qa, 1)
For bi = LBound(Qa, 2) To UBound(Qa, 2)
Qn(ai, bi) = Qa(ai) * Qb(bi)
Next bi
Next ai
End If
End Sub
'列选主元法Guass约化求解线性方程组
Public Sub MajorInColGuass(A, b, x)
Dim Row%, Col%, n% '矩阵大小
Dim iStep%, iRow%, iCol% '循环变量
Dim L() As Double '各行的约化系数
'计算并检查矩阵的大小
Row = UBound(A, 1) - LBound(A, 1) + 1
Col = UBound(A, 2) - LBound(A, 2) + 1
If Row <> Col Then
MsgBox "方程组的系数矩阵有误!"
Exit Sub
End If
'准备约化过程的变量和数组
n = UBound(b) - LBound(b) + 1
If n <> Row Then
MsgBox "方程组的系数矩阵与常数项大小不符!"
Exit Sub
End If
ReDim L(2 To Row) As Double
Dim sumAX As Double, iPos%, temp#
'约化过程
For iStep = 1 To n - 1
'列选主元
iPos = 0
For iRow = iStep + 1 To n
If Abs(A(iRow, iStep)) > Abs(A(iStep, iStep)) Then
iPos = iRow
End If
Next iRow
If iPos > iStep Then '需要换主元
For iCol = iStep To n
temp = A(iStep, iCol)
A(iStep, iCol) = A(iPos, iCol)
A(iPos, iCol) = temp
Next iCol
temp = b(iStep)
b(iStep) = b(iPos)
b(iPos) = temp
End If
'约化过程
For iRow = iStep + 1 To n
L(iRow) = A(iRow, iStep) / A(iStep, iStep)
For iCol = iStep To n
A(iRow, iCol) = A(iRow, iCol) - L(iRow) * A(iStep, iCol)
Next iCol
b(iRow) = b(iRow) - L(iRow) * b(iStep)
Next iRow
Next iStep
'回代过程
x(n) = b(n) / A(n, n)
For iRow = n - 1 To 1 Step -1
sumAX = 0
For iCol = n To iRow + 1 Step -1
sumAX = sumAX + A(iRow, iCol) * x(iCol)
Next iCol
x(iRow) = (b(iRow) - sumAX) / A(iRow, iRow)
Next iRow
End Sub
Private Sub Check1_Click()
If Check1.Value = 1 Then
Check1.Height = 5175
ElseIf Check1.Value = 0 Then
Check1.Height = 4440
End If
End Sub
Private Sub cmdBrowFile_Click()
CommonDialog1.Filter = "控制点文件 (*.gcp)|*.gcp|所有文件 (*.*)|*.*"
CommonDialog1.Action = 1
txtFileName.Text = CommonDialog1.FileName
End Sub
Private Sub cmdCalc_Click()
Dim s As String, n As Integer, i As Integer
Dim x1() As Double, y1() As Double, z1() As Double
Dim x2() As Double, y2() As Double, z2() As Double
Dim A() As Double, At() As Double, Naa() As Double, W() As Double, L() As Double
Dim x(1 To 4) As Double
Dim Ex As Double, k3 As Double
Dim du As Integer, fen As Integer
' 打开文件并读取数据
Open txtFileName.Text For Input As #1
Line Input #1, s
n = Val(s)
Debug.Print "Number of points: " & n
If n <= 0 Then
MsgBox "Invalid number of points."
Exit Sub
End If
ReDim x1(n), y1(n), z1(n), x2(n), y2(n), z2(n)
For i = 1 To n
If EOF(1) Then
MsgBox "File ended unexpectedly. Not enough data points."
Exit Sub
End If
Input #1, x1(i), y1(i), z1(i), x2(i), y2(i), z2(i)
Next i
Close #1
' 初始化矩阵
ReDim A(1 To 2 * n, 1 To 4)
ReDim L(1 To 2 * n)
ReDim At(1 To 4, 1 To 2 * n)
ReDim Naa(1 To 4, 1 To 4)
ReDim W(1 To 4)
' 填充矩阵 A 和 L
For i = 1 To n
If i > UBound(x1) Or i > UBound(y1) Or i > UBound(z1) Or i > UBound(x2) Or i > UBound(y2) Or i > UBound(z2) Then
MsgBox "Array index out of bounds."
Exit Sub
End If
A(2 * i - 1, 1) = 1: A(2 * i - 1, 2) = 0: A(2 * i - 1, 3) = x1(i): A(2 * i - 1, 4) = y1(i)
A(2 * i, 1) = 0: A(2 * i, 2) = 1: A(2 * i, 3) = -z1(i): A(2 * i, 4) = x1(i)
L(2 * i - 1) = x2(i): L(2 * i) = y2(i)
Next i
If UBound(A, 1) <> 2 * n Or UBound(A, 2) <> 4 Then
MsgBox "Matrix A dimensions are incorrect."
Exit Sub
End If
If UBound(At, 1) <> 4 Or UBound(At, 2) <> 2 * n Then
MsgBox "Matrix At dimensions are incorrect."
Exit Sub
End If
If UBound(Naa, 1) <> 4 Or UBound(Naa, 2) <> 4 Then
MsgBox "Matrix Naa dimensions are incorrect."
Exit Sub
End If
If UBound(W, 1) <> 4 Then
MsgBox "Matrix W dimensions are incorrect."
Exit Sub
End If
If UBound(L, 1) <> 2 * n Then
MsgBox "Matrix L dimensions are incorrect."
Exit Sub
End If
' 矩阵运算
MatrixTrans A, At
Matrix_Multy Naa, At, A
Matrix_Multy W, At, L
MajorInColGuass Naa, W, x
' 分离旋转和尺度参数
If Abs(x(3)) < 0.00000001 Then
If x(4) > 0 Then
Ex = pi / 2
Else
Ex = pi * 3 / 2
End If
Else
Ex = Atn(x(4) / x(3))
If x(3) < 0 And x(4) > 0 Then
Ex = pi - Ex
ElseIf x(3) < 0 And x(4) < 0 Then
Ex = pi + Ex
ElseIf x(3) > 0 And x(4) < 0 Then
Ex = pi * 2 + Ex
End If
End If
k3 = x(3) / Cos(Ex)
' 将转换参数写入相应文本框
txtK3 = Str(k3 - 1)
Ex = Ex * pi / 180
du = Int(Ex): Ex = (Ex - du) * 60
fen = Int(Ex): Ex = (Ex - fen) * 60
Ex = Val(Format(Ex, "0.00"))
Ex = du + fen / 100# + Ex / 10000
txtEx.Text = Str(Ex)
txtEy.Text = Str(Ey)
txtEz.Text = Str(Ez)
txtdX3.Text = Str(x(1))
txtdY3.Text = Str(x(2))
txtdZ3.Text = Str(x(3))
End Sub
Private Sub cmdCalc3_Click()
k3 = Val(txtK3.Text)
Ex = Val(txtEx.Text): Ex = DoToHu(Ex)
Ey = Val(txtEy.Text): Ey = DoToHu(Ey)
Ez = Val(txtEz.Text): Ez = DoToHu(Ez)
dX3 = Val(txtdX3.Text): dY3 = Val(txtdY3.Text): dZ3 = Val(txtdZ3.Text)
X3 = Val(txtX3.Text): Y3 = Val(txtY3.Text): Z3 = Val(txtZ3.Text)
Xx3 = (k3 + 1) * (X3 * Cos(Ey) * Cos(Ez) + Y3 * Cos(Ey) * Sin(Ez) - Z3 * Sin(Ey)) + dX3
Yy3 = (k3 + 1) * (X3 * (-Cos(Ex) * Sin(Ez) + Sin(Ex) * Sin(Ey) * Cos(Ez)) + Y3 * (Cos(Ex) * Cos(Ez) + _
Sin(Ex) * Sin(Ey) * Sin(Ez)) + Z3 * (Sin(Ex) * Cos(Ey))) + dY3
Zz3 = (k3 + 1) * (X3 * (Sin(Ex) * Sin(Ez) + Cos(Ex) * Sin(Ey) * Cos(Ez)) + Y3 * (-Sin(Ex) * Cos(Ez) + _
Cos(Ex) * Sin(Ey) * Sin(Ez)) + Z3 * (Cos(Ex) * Cos(Ey))) + dZ3
txtXx3.Text = Format(Xx3, "0.0000")
txtYy3.Text = Format(Yy3, "0.0000")
txtZz3.Text = Format(Zz3, "0.0000")
End Sub
Private Sub cmdClear3_Click()
txtK3.Text = ""
txtEx.Text = ""
txtEy.Text = ""
txtEz.Text = ""
txtdX3.Text = ""
txtdY3.Text = ""
txtdZ3.Text = ""
txtX3.Text = ""
txtY3.Text = ""
txtZ3.Text = ""
txtXx3.Text = ""
txtYy3.Text = ""
txtZz3.Text = ""
End Sub
Private Sub cmdconCalc3_Click()
k3 = Val(txtK3.Text)
Ex = Val(txtEx.Text): Ex = DoToHu(Ex): Ey = Val(txtEy.Text)
Ey = DoToHu(Ey): Ez = Val(txtEz.Text): Ez = DoToHu(Ez)
dX3 = Val(txtdX3.Text): dY3 = Val(txtdY3.Text): dZ3 = Val(txtdZ3.Text)
Xx3 = Val(txtXx3.Text): Yy3 = Val(txtYy3.Text): Zz3 = Val(txtZz3.Text)
X3 = ((Xx3 - dX3) * Cos(Ey) * Cos(Ez) + (Yy3 - dY3) * (-Cos(Ex) * Sin(Ez) + Sin(Ex) * Sin(Ey) * Cos(Ez)) + _
(Zz3 - dZ3) * (Sin(Ex) * Sin(Ez) + Cos(Ex) * Sin(Ey) * Cos(Ez))) / (k3 + 1)
Y3 = ((Xx3 - dX3) * Cos(Ey) * Sin(Ez) + (Yy3 - dY3) * (Sin(Ex) * Sin(Ey) * Sin(Ez) + Cos(Ex) * Cos(Ez)) + _
(Zz3 - dZ3) * (-Sin(Ex) * Cos(Ez) + Cos(Ex) * Sin(Ey) * Sin(Ez))) / (k3 + 1)
Z3 = ((Xx3 - dX3) * (-Sin(Ey)) + (Yy3 - dY3) * Sin(Ex) * Cos(Ey) + (Zz3 - dZ3) * (Cos(Ex) * Cos(Ey))) / (k3 + 1)
txtX3.Text = Format(X3, "0.0000")
txtY3.Text = Format(Y3, "0.0000")
txtZ3.Text = Format(Z3, "0.0000")
End Sub
Private Sub Command12_Click()
End
End Sub