Attribute VB_Name = "modVector3d"
' this is the 3d Vector code module

Option Explicit

Global Const pi As Double = 3.1415926536
Global Const rad As Double = (pi / 180#)
Global Const deg As Double = (180# / pi)

' note the (north,east,down) to (x,y,z) mapping
' agrees with spacial orientation and
' coordinate conventions

Global Const c_N = 0     ' north
Global Const c_E = 1    ' east
Global Const c_D = 2    ' down

Global Const c_x = 0
Global Const c_y = 1
Global Const c_z = 2

Function acos(theta As Double) As Double
    acos = Atn(-theta / Sqr(-theta ^ 2 + 1)) + pi / 2
End Function


Function atan(x As Double, y As Double) As Double
    If x > 0 Then
        If y > 0 Then
            atan = Atn(y / x)
        Else
            atan = 2 * pi + Atn(y / x)
        End If
    ElseIf x < 0 Then
        atan = pi + Atn(y / x)
    Else
        If y > 0 Then
            atan = pi
        ElseIf y < 0 Then
            atan = -pi
        Else
            atan = 0#
        End If
    End If
End Function

Sub cartesian3d_average(v1 As Cartesian3D, v2 As Cartesian3D, v As Cartesian3D)
    v.x = (v1.x + v2.x) / 2#
    v.y = (v1.y + v2.y) / 2#
    v.z = (v1.z + v2.z) / 2#
End Sub

Function Cartesian3d_DiffAngle(v1 As Cartesian3D, v2 As Cartesian3D) As Double
    Cartesian3d_DiffAngle = deg * acos(Cartesian3d_DotProduct(v1, v2) / v1.mag / v2.mag)
End Function



Sub Cartesian3d_Difference(v1 As Cartesian3D, v2 As Cartesian3D, v As Cartesian3D)
    v.x = v1.x - v2.x
    v.y = v1.y - v2.y
    v.z = v1.z - v2.z
End Sub

Function Cartesian3d_DotProduct(v1 As Cartesian3D, v2 As Cartesian3D) As Double
    Cartesian3d_DotProduct = (v1.x * v2.x) + (v1.y * v2.y) + (v1.z * v2.z)
End Function

' v1 : the vector to rotate
' axis : the axis to rotate around
' theta : the angle to rotate in radians
' v : the resultant vector
Sub Cartesian3d_Rotate(v1 As Cartesian3D, axis As Integer, theta As Double, v As Cartesian3D)
    Dim a0 As Integer, a1 As Integer   ' the axis that will be changed
    Dim c0 As Double, c1 As Double    ' temp values for the coordinates
    Dim ct As Double, st As Double    ' sin and cos of theta

    'Select Case axis
    'Case c_x
    '    a0 = c_y: a1 = c_z  ' rotate yz plane around x
    'Case c_y
    '    a0 = c_z: a1 = c_x  ' rotate zx plane around y
    'Case c_z
    '    a0 = c_x: a1 = c_y  ' rotate xy plane around z
    'End Select
    
    'ct = Cos(theta * rad): st = Sin(theta * rad)
    ' store temp values just in case v1=v
    'c0 = v1.C(a0): c1 = v1.C(a1)
    ' this is just standard trig
    'v.C(axis) = v1.C(axis)
    'v.C(a0) = c0 * ct + c1 * st
    'v.C(a1) = c1 * ct - c0 * st
End Sub

Function Cartesian3d_ScalarDiv(v1 As Cartesian3D, r As Double) As Cartesian3D
    Set Cartesian3d_ScalarDiv = New Cartesian3D

    Cartesian3d_ScalarDiv.x = v1.x / r
    Cartesian3d_ScalarDiv.y = v1.y / r
    Cartesian3d_ScalarDiv.z = v1.z / r
End Function

Sub Cartesian3d_ScalarMult(v1 As Cartesian3D, r As Double, v As Cartesian3D)
    v.x = r * v1.x
    v.y = r * v1.y
    v.z = r * v1.z
End Sub

Sub Cartesian3d_Square(v1 As Cartesian3D, v As Cartesian3D)
    v.x = v1.x ^ 2
    v.y = v1.y ^ 2
    v.z = v1.z ^ 2
End Sub

Sub Cartesian3d_SquareRoot(v1 As Cartesian3D, v As Cartesian3D)
    v.x = Sqr(v1.x)
    v.y = Sqr(v1.y)
    v.z = Sqr(v1.z)
End Sub

Function Cartesian3d_Sum(v1 As Cartesian3D, v2 As Cartesian3D) As Cartesian3D
    Set Cartesian3d_Sum = New Cartesian3D
    Cartesian3d_Sum.x = v1.x + v2.x
    Cartesian3d_Sum.y = v1.y + v2.y
    Cartesian3d_Sum.z = v1.z + v2.z
End Function

Sub Cartesian3d_Zero(v As Cartesian3D)
    v.x = 0
    v.y = 0
    v.z = 0
End Sub

Function RadToDeg(ang As Double, Optional chVal As Boolean = False) As Double
    ' This function returns an angle in degrees from some radian value
    RadToDeg = ang * deg
    If chVal Then
        ' We always want to return the declination between 0 and 360
        While (RadToDeg > 360#)
            RadToDeg = RadToDeg - 360
        Wend
        While (RadToDeg < 0)
            RadToDeg = RadToDeg + 360
        Wend
    End If
End Function

Function DegToRad(ang As Double) As Double
    ' This function returns an angle in radians from some degree value
    DegToRad = ang * rad
End Function

Public Function atan2(ByVal xC As Double, ByVal yC As Double) As Double
    ' Returns the angle from x clockwise to y, in radians.
    ' "xc" is in the x direction, "yc" is in the y direction.

    If (xC = 0 And yC >= 0) Then
        atan2 = pi / 2
    ElseIf (xC = 0 And yC < 0) Then
        atan2 = 3 * pi / 2
    Else
        atan2 = Atn(Abs(yC / xC))
        If (yC >= 0 And xC > 0) Then
        ElseIf (yC >= 0 And xC < 0) Then
            atan2 = pi - atan2
        ElseIf (yC <= 0 And xC < 0) Then
            atan2 = pi + atan2
        ElseIf (yC <= 0 And xC > 0) Then
            atan2 = 2 * pi - atan2
        End If
    End If
End Function

Public Function arcos(ByVal ar As Double)
    ' if ar is between -1 AND 1, return the corresponding arccos
    ' in radians

    If ar >= 1 Then arcos = 0: Return
    If ar <= -1 Then arcos = pi: Return
    arcos = atan2(ar, Sqr(1 - ar ^ 2))
End Function

Sub Angular3dToCartesian3d(g As Angular3D, C As Cartesian3D)
    Dim p As Double
    p = Cos(g.inc * rad)
    C.x = g.mag * p * Cos(g.dec * rad)
    C.y = g.mag * p * Sin(g.dec * rad)
    C.z = g.mag * Sin(g.inc * rad)
End Sub

Sub Cartesian3dToAngular3d(C As Cartesian3D, g As Angular3D)
    Dim s As Double
    s = C.x ^ 2 + C.y ^ 2
    g.mag = Sqr(s + C.z ^ 2)
    g.inc = deg * atan(Sqr(s), C.z)
    If g.inc > 180 Then g.inc = g.inc - 360
    g.dec = deg * atan(C.x, C.y)
End Sub


