mmath.bas

用于矩阵加减乘除运算、高斯消元法解方程等

Dims1 = u1 - l1 + 1: Dims2 = u2 - l2 + 1
If Dims1 > Dims2 Then
  MatrixInvert = 1   '维数不等(对)
  MsgBox "Dimesion of Matrix invalid, Canot MatrixInvert it !", 16, "Function MatrixInvert()"
  Exit Function
End If
'行数小于列数,只对前面列计算
'If Dims1 <> Dims2 Then
'  MsgBox "Dimesion2 of Matrix > dimesion1, MatrixInvert about front !", 32, "Function MatrixInvert()"
'End If
u2 = (u1 - l1 + 1) * 2 + l2 - 1
ReDim a(l1 To u1, l2 To u2) As Double
For i = l1 To u1
  a(i, Dims1 + i - l1 + l2) = 1#
  For j = l1 To u1
  a(i, j - l1 + l2) = SA(i, j)
Next j, i
For i = l1 To u1
  MaxElement = a(i, i - l1 + l2)
  MaxNo = i
  For j = i To u1 - 1
    If Abs(a(j, i - l1 + l2)) > Abs(MaxElement) Then MaxElement = Abs(a(j, i - l1 + l2)): MaxNo = j
  Next j
  If MaxNo <> i Then
    For j = l2 To u2
      ExChg = a(MaxNo, j): a(MaxNo, j) = a(i, j): a(i, j) = ExChg
    Next j
  End If
  If a(i, i - l1 + l2) = 0 Then
    MatrixInvert = 3  '不可逆
    MsgBox "Error! Bad Matrix, No MatrixInvertion!", 16, "Function MatrixInvert()"
    Exit Function
  End If
  MainElement = a(i, i - l1 + l2)
  For j = i - l1 + l2 To u2
    a(i, j) = a(i, j) / MainElement
  Next j
  For j = i + 1 To u1
    If a(j, i - l1 + l2) <> 0 Then
      MainElement = a(j, i - l1 + l2)
      For k = i - l1 + l2 To u2
        a(j, k) = a(j, k) / MainElement - a(i, k)
      Next k
    End If
  Next j
Next i
For j = u1 - l1 + l2 To l2 Step -1
  For i = l1 To j - l2 + l1 - 1
    MainElement = a(i, j)
    If MainElement <> 0 Then
      For k = j To u2
        a(i, k) = a(i, k) - MainElement * a(j - l2 + l1, k)
      Next k
    End If
  Next i
Next j
For i = rl1 To rl1 + Dims1 - 1
  For j = rl2 To rl2 + Dims1 - 1
    rsl(i, j) = a(i - rl1 + l1, j - rl2 + l2 + Dims1)
  Next j
Next i
Erase a
End Function

Function RadToDeg(ByVal dAng As Double) As Double
   RadToDeg = dAng * 180# / (Atn(1) * 4#)
End Function

Function Dms(ByVal Aang As Double) As Double
Dim Angsign As Double
Dim ad As Double
Dim am As Double
Dim asd As Double
Angsign = Sgn(Aang)
Aang = Abs(Aang)
ad = Int(Aang)
am = Aang - ad: am = am * 60#
asd = am - Int(am)
am = Int(am)
asd = asd * 60#
Dms = (ad + am / 100# + asd / 10000#) * Angsign
End Function

Function DegtoRad(ByVal ang As Double) As Double
  Dim pii As Double
  pii = Atn(1#) * 4#
  DegtoRad = ang / 180# * pii
End Function

Function Deg(ByVal Aang As Double) As Double
Dim i As Integer
Dim Angsign As Integer
Dim DStr As String
Dim dLen As Integer
Dim ad As String
Dim am As String
Dim snd As String
Angsign = Sgn(Aang)
DStr = str$(Abs(Aang))
dLen = Len(DStr$)
For i = 1 To dLen%
  If Mid$(DStr$, i, 1) = "." Then
    While (dLen% - i) < 4
      DStr$ = DStr$ + "0"
      dLen% = dLen% + 1
    Wend
    ad$ = Left$(DStr$, i - 1)
    am$ = Mid$(DStr$, i + 1, 2)
    snd$ = Mid$(DStr$, i + 3, 2) + "." + Mid$(DStr$, i + 5)
    Exit For
  End If
Next i
If i = dLen% + 1 Then
  Deg = Aang
Else
  Deg = (Val(ad$) + Val(am$) / 60# + Val(snd$) / 3600#) * Angsign
End If
End Function

Function FmtDms(Aang As Double, SecondPre As Integer) As String
Dim i As Integer
Dim HourMode As Boolean
Dim DStr As String
Dim dLen As Integer
Dim ad As String
Dim am As String
Dim snd As String
If SecondPre >= 100 Then
  HourMode = True
  SecondPre = SecondPre - 100
Else
  HourMode = False
End If
DStr$ = str$(Aang)
dLen% = Len(DStr$)
For i = 1 To dLen%
  If Mid$(DStr$, i, 1) = "." Then
    While (dLen% - i) < 4
      DStr$ = DStr$ + "0"
      dLen% = dLen% + 1
    Wend
    ad$ = Left$(DStr$, i - 1)
    am$ = Mid$(DStr$, i + 1, 2)
    snd$ = Mid$(DStr$, i + 3)
    While Len(snd$) < 3 + SecondPre
      snd$ = snd$ + "0"
    Wend
    snd$ = Trim(str$(Int(Val(Left$(snd$, 3 + SecondPre)) / 10 + 0.5)))
    While Len(snd$) < 2 + SecondPre
      snd$ = "0" + snd$
    Wend
    If SecondPre = 0 Then
      snd$ = Left(snd$, 2)
    Else
      snd$ = Left(snd$, 2) & "." & Mid(snd$, 3)
    End If
    Exit For
  End If
Next i
If i = dLen% + 1 Then:  ad$ = DStr$: am$ = "00": snd$ = "00"
If HourMode Then
  'FmtDms = ad$ + "小时" + am$ + "分" + snd$ + "秒"
  FmtDms = ad$ + "h" + am$ + "m" + snd$ + "s"
Else
  FmtDms = ad$ + "°" + am$ + "′" + snd$ + "″"
End If
End Function

Public Function TriangleDirect(p1x As Double, p1y As Double, p2x As Double, p2y As Double, p3x As Double, p3y As Double, Optional IsMathCoordSys As Boolean = True, Optional ByRef Direct As String) As Boolean
Dim a As Double
a = CalcTriangleArea(p1x, p1y, p2x, p2y, p3x, p3y, IsMathCoordSys)
If a > 0# Then
   TriangleDirect = True
   Direct = "123"
ElseIf a < 0# Then
   TriangleDirect = True
   Direct = "321"
Else
   TriangleDirect = False
   Direct = ""
End If
End Function

Public Function CalcTriangleArea(p1x As Double, p1y As Double, p2x As Double, p2y As Double, p3x As Double, p3y As Double, Optional IsMathCoordSys As Boolean = True) As Double
'计算三角形面积，结果为正则顺时针
'IsMathCoordSys=是否数学坐标系
  Dim s As Double
  If IsMathCoordSys Then  '保证结果按数学坐标系计算
     s = ((p2x - p3x) * p1y + (p3x - p1x) * p2y + (p1x - p2x) * p3y) * 0.5
  Else
     s = ((p2y - p3y) * p1x + (p3y - p1y) * p2x + (p1y - p2y) * p3x) * 0.5
  End If
  CalcTriangleArea = s
End Function

Public Function PtInsideTriangle(pX As Double, pY As Double, p1x As Double, p1y As Double, p2x As Double, p2y As Double, p3x As Double, p3y As Double) As String
'返回8种结果
Dim s1 As Double
Dim s2 As Double
Dim s3 As Double
s1 = CalcTriangleArea(pX, pY, p1x, p1y, p2x, p2y, True)
s2 = CalcTriangleArea(pX, pY, p2x, p2y, p3x, p3y, True)
s3 = CalcTriangleArea(pX, pY, p3x, p3y, p1x, p1y, True)
If (s1 >= 0# And s2 >= 0# And s3 >= 0#) Or (s1 <= 0# And s2 <= 0# And s3 <= 0#) Then
   If s1 = 0# And s2 = 0# Then
      PtInsideTriangle = "VECTP2"
   ElseIf s2 = 0# And s3 = 0# Then
      PtInsideTriangle = "VECTP3"
   ElseIf s1 = 0# And s3 = 0# Then
      PtInsideTriangle = "VECTP1"
   ElseIf s1 = 0# Then
      PtInsideTriangle = "BORDER3"
   ElseIf s2 = 0# Then
      PtInsideTriangle = "BORDER1"
   ElseIf s3 = 0# Then
      PtInsideTriangle = "BORDER2"
   Else
      PtInsideTriangle = "INSIDE"
   End If
Else
   PtInsideTriangle = "OUTSIDE"
End If
End Function


Public Function Distance(ByVal p1x As Double, p1y As Double, p2x As Double, p2y As Double) As Double
  Distance = Sqr((p1x - p2x) * (p1x - p2x) + (p2y - p1y) * (p2y - p1y))
End Function

Public Function GetSlopeRate(ByVal p1x As Double, ByVal p1y As Double, ByVal p2x As Double, ByVal p2y As Double, ByRef a As Double, ByRef b As Double) As Boolean
'计算两点斜率
  If p1x = p2x Then
     GetSlopeRate = False
  Else
     a = (p2y - p1y) / (p2x - p1x)
     b = p1y - a * p1x
     GetSlopeRate = True
  End If
End Function

Public Function GetSINLineAngle(ByVal x1 As Double, ByVal y1 As Double, ByVal x2 As Double, ByVal y2 As Double, ByVal x3 As Double, ByVal y3 As Double, ByVal x4 As Double, ByVal y4 As Double) As Double
'求两直线(小)夹角的正弦值
  Dim pX As Double
  Dim pY As Double
  Dim ix As Double
  Dim iy As Double
  GetPerpPoint x1, y1, x2, y2, x3, y3, pX, pY
  GetInterPt x1, y1, x2, y2, x3, y3, x4, y4, ix, iy
  GetSINLineAngle = Distance(pX, pY, x3, y3) / Distance(x3, y3, ix, iy)
End Function

Public Function GetPerpDist(ByVal x1 As Double, ByVal y1 As Double, ByVal x2 As Double, ByVal y2 As Double, ByVal X As Double, ByVal Y As Double) As Boolean
'求点到直线距离
Dim pX As Double
Dim pY As Double
  GetPerpPoint x1, y1, x2, y2, X, Y, pX, pY
  GetPerpDist = Sqr((X - pX) * (X - pX) + (Y - pY) * (Y - pY))
End Function


Public Function GetPerpPoint(ByVal x1 As Double, ByVal y1 As Double, ByVal x2 As Double, ByVal y2 As Double, ByVal X As Double, ByVal Y As Double, ByRef pX As Double, ByRef pY As Double) As Boolean
'求点到直线垂足
Dim aa As Double
Dim bb As Double
Dim a As Double
Dim b As Double
   If GetSlopeRate(x1, y1, x2, y2, a, b) Then
      If a = 0# Then
         GetPerpPoint = True
         pX = X
         pY = y1
      Else
         aa = -1# / a
         bb = Y - aa * X
         If X = 0 Then
            GetPerpPoint = GetInterPt(x1, y1, x2, y2, X, Y, bb * a, 0#, pX, pY)
         Else
            GetPerpPoint = GetInterPt(x1, y1, x2, y2, X, Y, 0#, bb, pX, pY)
         End If
      End If
   Else
      '垂直线
      GetPerpPoint = True
      pX = x1
      pY = Y
   End If
End Function

Public Function GetInterPt(ByVal p1x As Double, ByVal p1y As Double, ByVal p2x As Double, ByVal p2y As Double, ByVal p3x As Double, ByVal p3y As Double, ByVal p4x As Double, ByVal p4y As Double, ByRef X As Double, ByRef Y As Double, Optional ByVal RtnRealInterP As Boolean = False) As Boolean
'RtnRealInterP－－是否得到实交点
'求两直线交点
   Dim a1 As Double
   Dim B1 As Double
   Dim a2 As Double
   Dim B2 As Double
   Dim r1 As Boolean
   Dim r2 As Boolean
   '
   Dim bl As Boolean
   r1 = GetSlopeRate(p1x, p1y, p2x, p2y, a1, B1)
   r2 = GetSlopeRate(p3x, p3y, p4x, p4y, a2, B2)
   If (Not r1) And (Not r2) Then
      bl = False
   ElseIf r1 And r2 Then
      If a1 <> a2 Then
         X = -1# * (B2 - B1) / (a2 - a1)
         Y = a1 * X + B1
         bl = True
      Else
         bl = False
      End If
   ElseIf (Not r1) And r2 Then
      X = p1x
      Y = a2 * X + B2
      bl = True
   Else
      X = p3x
      Y = a1 * X + B1
      bl = True
   End If
   If bl And RtnRealInterP Then
      If Abs(Distance(p1x, p1y, X, Y) + Distance(X, Y, p2x, p2y) - Distance(p1x, p1y, p2x, p2y)) <= 0.0001 And _
         Abs(Distance(p3x, p3y, X, Y) + Distance(X, Y, p4x, p4y) - Distance(p3x, p3y, p4x, p4y)) <= 0.0001 Then
         GetInterPt = True
      Else
         GetInterPt = False
      End If
   Else
      GetInterPt = bl
   End If
End Function


Public Function GetSSPPoint(ByVal p1x As Double, ByVal p1y As Double, ByVal p2x As Double, ByVal p2y As Double, ByVal s1 As Double, ByVal s2 As Double, ByVal IsMathCoord As Boolean, ByRef x1 As Double, ByRef y1 As Double, Optional ByRef x2 As Double, Optional ByRef y2 As Double) As Boolean
'求边交会点
'能交返回true
'在数学坐标系下，返回的前一点:P1 - 前点 - P2为顺时针
'                      后一点:P1 - 后点 - P2为逆时针
Dim s As Double
Dim a As Double
Dim b As Double
Dim dx As Double
Dim dy As Double
Dim KA As Double
Dim KB As Double
Dim KC As Double
Dim Direct As String
  s = Distance(p1x, p1y, p2x, p2y)
  If Not (((s1 + s2) > s) And ((s + s2) > s1) And ((s + s1) > s2)) Then
     GetSSPPoint = False
  Else
     dx = p2x - p1x
     dy = p2y - p1y
     a = -1# * dx / dy
     b = (s1 * s1 - s2 * s2 + dx * dx + dy * dy) / dy / 2#
     KA = a * a + 1#
     KB = 2# * a * b
     KC = b * b - s1 * s1
     x1 = (-1# * KB + Sqr(KB * KB - 4 * KA * KC)) / 2# / KA
     y1 = x1 * a + b
     x2 = (-1# * KB - Sqr(KB * KB - 4 * KA * KC)) / 2# / KA
     y2 = x2 * a + b
     '
     x1 = x1 + p1x: y1 = y1 + p1y
     x2 = x2 + p1x: y2 = y2 + p1y
     TriangleDirect p1x, p1y, x2, y2, p2x, p2y, IsMathCoord, Direct
     If Direct = "123" Then
        dx = x1: x1 = x2: x2 = dx
        dy = y1: y1 = y2: y2 = dy
     End If
     GetSSPPoint = True
  End If
End Function



Public Function GetPerpCutLine(ByVal p1x As Double, ByVal p1y As Double, ByVal p2x As Double, ByVal p2y As Double, ByVal IsMathCoord As Boolean, ByRef x1 As Double, ByRef y1 As Double, ByRef x2 As Double, ByRef y2 As Double) As Boolean
'作垂直平分线，返回两点与原两点组成正方形
'能作返回true
'在数学坐标系下，返回的前一点:P1 - 前点 - P2 -后点为顺时针
Dim dx As Double
Dim dy As Double
Dim Direct As String
If p1x = p2x And p1y = p2y Then
  GetPerpCutLine = False
Else
  x1 = (p1x + p2x) / 2# + (p1y - p2y) / 2#
  x2 = (p1x + p2x) / 2# - (p1y - p2y) / 2#
  y1 = (p1y + p2y) / 2# - (p1x - p2x) / 2#
  y2 = (p1y + p2y) / 2# + (p1x - p2x) / 2#
     TriangleDirect p1x, p1y, x1, y1, p2x, p2y, IsMathCoord, Direct
     If Direct = "321" Then
        dx = x1: x1 = x2: x2 = dx
        dy = y1: y1 = y2: y2 = dy
     End If
  GetPerpCutLine = True
End If
End Function

Public Function Get3PCCenter(ByVal p1x As Double, ByVal p1y As Double, ByVal p2x As Double, ByVal p2y As Double, ByVal p3x As Double, ByVal p3y As Double, ByRef X As Double, ByRef Y As Double) As Boolean
'求3点外接圆心
Dim bP1 As Boolean
Dim bP2 As Boolean
Dim r1x As Double
Dim r1y As Double
Dim r2x As Double
Dim r2y As Double
Dim r3x As Double
Dim r3y As Double
Dim r4x As Double
Dim r4y As Double
   bP1 = GetPerpCutLine(p1x, p1y, p2x, p2y, True, r1x, r1y, r2x, r2y)
   bP2 = GetPerpCutLine(p1x, p1y, p3x, p3y, True, r3x, r3y, r4x, r4y)
   If bP1 And bP2 Then
      Get3PCCenter = GetInterPt(r1x, r1y, r2x, r2y, r3x, r3y, r4x, r4y, X, Y)
   Else
      Get3PCCenter = False
   End If
End Function


Public Function Get3DPlanZRate(ByVal x1 As Double, ByVal y1 As Double, ByVal z1 As Double, _
                               ByVal x2 As Double, ByVal y2 As Double, ByVal z2 As Double, _
                               ByVal x3 As Double, ByVal y3 As Double, ByVal z3 As Double, _
                               ByRef ra As Double, ByRef rB As Double, ByRef rc As Double) As Boolean
'由3点求空间平面高程插值系数 Z=Ax+By+C 中的A、B、C
'相当于求解方程组 { Ax1 + By1 + C - z1 = 0
'                   Ax2 + By2 + C - z2 = 0
'                   Ax3 + By3 + C - z3 = 0  }的A、B、C未知数
  Dim r() As Double
  Dim a(0 To 2, 0 To 3) As Double
  Dim bl As Integer
  
  a(0, 0) = x1: a(0, 1) = y1: a(0, 2) = 1: a(0, 3) = -1 * z1
  a(1, 0) = x2: a(1, 1) = y2: a(1, 2) = 1: a(1, 3) = -1 * z2
  a(2, 0) = x3: a(2, 1) = y3: a(2, 2) = 1: a(2, 3) = -1 * z3
  
  bl = EQSolution(a, r, False)
  If bl = 0 Then
     ra = r(LBound(r))
     rB = r(LBound(r) + 1)
     rc = r(LBound(r) + 2)
     Get3DPlanZRate = True
  Else
     Get3DPlanZRate = False
  End If
End Function


Public Function GetZValOfInsertP(ByVal x1 As Double, ByVal y1 As Double, ByVal z1 As Double, _
                                 ByVal x2 As Double, ByVal y2 As Double, ByVal z2 As Double, _
                                 ByVal x3 As Double, ByVal y3 As Double, ByVal z3 As Double, _
                                 ByRef ix As Double, ByRef iy As Double, Optional ByRef CalcSucess As Boolean) As Double
'求平面三角形内(外)插点高程
Dim ra As Double
Dim rB As Double
Dim rc As Double

   If Get3DPlanZRate(x1, y1, z1, x2, y2, z2, x3, y3, z3, ra, rB, rc) Then
      GetZValOfInsertP = ra * ix + rB * iy + rc
      CalcSucess = True
   Else
      GetZValOfInsertP = 0#
      CalcSucess = False
   End If
End Function

Public Function GetInsPInLine(ByVal p1x As Double, ByVal p1y As Double, ByVal p1z As Double, ByVal p2x As Double, ByVal p2y As Double, ByVal p2z As Double, ByVal InsZVal As Double, ByRef rX As Double, ByRef rY As Double) As Boolean
'在一个三维线段上找一点，使其高程等于给定高程(内插或延长)
Dim deltaX As Double
Dim deltaY As Double
Dim deltaZ As Double
If p1z = p2z Then
   GetInsPInLine = False
Else
   deltaX = p2x - p1x
   deltaY = p2y - p1y
   deltaZ = p2z - p1z
   rX = p1x + deltaX * (InsZVal - p1z) / deltaZ
   rY = p1y + deltaY * (InsZVal - p1z) / deltaZ
   GetInsPInLine = True
 End If
End Function