'GeoSub
' A set of subroutines and functions written in Visual Basic
Option Explicit
DefSng A-Z
Global iv(3), jv(3), kv(3)
Global Const pi = 3.141593
Global Const DtoR = pi / 180
Global Const RtoD = 180 / pi
Global i As Integer, j As Integer, k As Integer, l As Integer

Function ArcCos (x)
'Given a cosine, yields an angle between 0 and pi radians
Dim cosang, y
cosang = x
If Abs(cosang) > 1 Then cosang = Sgn(cosang)
If Abs(cosang) <= .0000001 Then
ArcCos = 1
Else
y = Atn(Sqr(1 - cosang * cosang) / Abs(cosang))
If cosang >= 0 Then ArcCos = y Else ArcCos = pi - y
End If
End Function

Function ArcSin (s)

'Given a sine, yields an angle between -pi/2 and pi/2 radians

If Abs(s) >= 1 Then

ArcSin = Sgn(s) * 3.1415927 / 2

Else

ArcSin = Sgn(s) * Atn(Abs(s) / Sqr(1 - s * s))

End If

End Function

Sub CliffMult (u(), v(), uv())

'Clifford product computed as dot product - cross product; so defined, the

'product is not associative, but as we never Clifford multiply more than 2

'vectors, we don't run into any problems.

Dim i As Integer

ReDim subTemp(3)

subTemp(0) = VDot(u(), v())

VCross v(), u(), subTemp()

For i = 0 To 3: uv(i) = subTemp(i): Next i

End Sub

Sub FitFunction (x(), y(), xx(), xy(), par(), stage)

Dim nIn As Integer, nOut As Integer

nIn = UBound(x)

nOut = UBound(y)

ReDim xxInv(nIn, nIn)

If stage = 1 Then

For i = 1 To nIn: For j = 1 To nIn

xx(i, j) = xx(i, j) + x(i) * x(j)

Next j, i

For i = 1 To nIn: For j = 1 To nOut

xy(i, j) = xy(i, j) + x(i) * y(j)

Next j, i

Else

MatInv xx(), xxInv()

MatMult xxInv(), xy(), par()

End If

End Sub

Sub GramSchmidt (r())

'Converts a 3 by 3 matrix into an orthogonal 3 by 3 matrix

Dim nr1, ip, nr3

nr1 = Sqr(r(1, 1) * r(1, 1) + r(1, 2) * r(1, 2) + r(1, 3) * r(1, 3))

r(1, 1) = r(1, 1) / nr1: r(1, 2) = r(1, 2) / nr1: r(1, 3) = r(1, 3) / nr1

ip = r(1, 1) * r(3, 1) + r(1, 2) * r(3, 2) + r(1, 3) * r(3, 3)

r(3, 1) = r(3, 1) - ip * r(1, 1)

r(3, 2) = r(3, 2) - ip * r(1, 2)

r(3, 3) = r(3, 3) - ip * r(1, 3)

nr3 = Sqr(r(3, 1) * r(3, 1) + r(3, 2) * r(3, 2) + r(3, 3) * r(3, 3))

r(3, 1) = r(3, 1) / nr3: r(3, 2) = r(3, 2) / nr3: r(3, 3) = r(3, 3) / nr3

r(2, 1) = r(3, 2) * r(1, 3) - r(3, 3) * r(1, 2)

r(2, 2) = r(3, 3) * r(1, 1) - r(3, 1) * r(1, 3)

r(2, 3) = r(3, 1) * r(1, 2) - r(3, 2) * r(1, 1)

End Sub

Sub MatInv (subM(), subIM())

'Inverts a square matrix

Dim column As Integer, nDim As Integer, tem

nDim = UBound(subM, 1)

If nDim <> UBound(subM, 2) Then

Dim response As Integer

response = MsgBox("Invert nonsquare matrix", 48, "Booboo")

Exit Sub

End If

ReDim r(nDim, nDim)

For i = 1 To nDim: For j = 1 To nDim

If i = j Then subIM(i, j) = 1 Else subIM(i, j) = 0

Next j, i

For column = 1 To nDim

For i = 1 To nDim: For j = 1 To nDim

r(i, j) = subM(i, j)

Next j, i

For k = 1 To nDim

If r(k, k) <> 0 Then

tem = 1 / r(k, k)

Else

tem = 1000000

End If

For j = k To nDim

r(k, j) = r(k, j) * tem

Next j

subIM(k, column) = subIM(k, column) * tem

For j = 1 To nDim

If k <> j Then

tem = r(j, k)

For l = k To nDim

r(j, l) = r(j, l) - r(k, l) * tem

Next l

subIM(j, column) = subIM(j, column) - subIM(k, column) * tem

End If

Next j

Next k

Next column

End Sub

Sub MatMult (subA(), subB(), subAB())

'Multiplies two matrices; numbers of rows and columns must correspond

Dim i As Integer, j As Integer, k As Integer

Dim nRowsA As Integer, nColsA As Integer, nColsB As Integer

nRowsA = UBound(subA, 1)

nColsA = UBound(subA, 2)

nColsB = UBound(subB, 2)

ReDim subTemp(nRowsA, nColsB)

For i = 0 To nRowsA: For j = 0 To nColsB: subTemp(i, j) = 0

For k = 0 To nColsA

subTemp(i, j) = subTemp(i, j) + subA(i, k) * subB(k, j)

Next k

Next j, i

For i = 0 To nRowsA: For j = 0 To nColsB

subAB(i, j) = subTemp(i, j)

Next j, i

End Sub

Sub MatTranspose (subM(), subMT())

'Transposes a matrix

Dim nRows As Integer, nCols As Integer

nRows = UBound(subM, 1)

nCols = UBound(subM, 2)

ReDim subTemp(nCols, nRows)

For i = 0 To nCols: For j = 0 To nRows

subTemp(i, j) = subM(j, i)

Next j, i

For i = 0 To nCols: For j = 0 To nRows

subMT(i, j) = subTemp(i, j)

Next j, i

End Sub

Sub MatVect (subM(), subV(), subMV())

'Left multiplies a vector by a matrix

Dim nRowsM As Integer, nColsM As Integer

nRowsM = UBound(subM, 1)

nColsM = UBound(subM, 2)

ReDim subTemp(nRowsM)

For i = 0 To nRowsM: For j = 0 To nColsM

subTemp(i) = subTemp(i) + subM(i, j) * subV(j)

Next j, i

For i = 0 To nRowsM: subMV(i) = subTemp(i): Next i

End Sub

Sub MoorePenrose (mat(), mp())

'Computes Moore-Penrose pseudoinverse of a matrix

Dim nRows As Integer, nCols As Integer, dimInv As Integer

nRows = UBound(mat, 1)

nCols = UBound(mat, 2)

ReDim mt(nCols, nRows): ReDim mtm(nCols, nCols): ReDim mmt(nRows, nRows)

If nRows < nCols Then dimInv = nRows Else dimInv = nCols

ReDim inv(dimInv, dimInv)

MatTranspose mat(), mt()

If nRows >= nCols Then

MatMult mt(), mat(), mtm()

MatInv mtm(), inv()

MatMult inv(), mt(), mp()

Else

MatMult mat(), mt(), mmt()

MatInv mmt(), inv()

MatMult mt(), inv(), mp()

End If

End Sub

Sub QConj (p(), q(), pqpi())

'Conjugates quaternion or vector q with quaternion p

ReDim pq(3), Temp(3)

pq(0) = p(0) * q(0) - p(1) * q(1) - p(2) * q(2) - p(3) * q(3)

pq(1) = p(0) * q(1) + p(1) * q(0) + p(2) * q(3) - p(3) * q(2)

pq(2) = p(0) * q(2) + p(2) * q(0) - p(1) * q(3) + p(3) * q(1)

pq(3) = p(0) * q(3) + p(3) * q(0) + p(1) * q(2) - p(2) * q(1)

Temp(0) = pq(0) * p(0) + pq(1) * p(1) + pq(2) * p(2) + pq(3) * p(3)

Temp(1) = -pq(0) * p(1) + pq(1) * p(0) - pq(2) * p(3) + pq(3) * p(2)

Temp(2) = -pq(0) * p(2) + pq(2) * p(0) + pq(1) * p(3) - pq(3) * p(1)

Temp(3) = -pq(0) * p(3) + pq(3) * p(0) - pq(1) * p(2) + pq(2) * p(1)

For i = 0 To 3: pqpi(i) = Temp(i): Next i

End Sub

Sub QInv (q(), iq())

'Inverts a quaternion

Dim magSqr

magSqr = q(0) * q(0) + q(1) * q(1) + q(2) * q(2) + q(3) * q(3)

iq(0) = q(0) / magSqr

For i = 1 To 3: iq(i) = -q(i) / magSqr: Next i

End Sub

Sub QMult (p(), q(), pq())

'Multiplies two quaternions

ReDim subTemp(3)

subTemp(0) = p(0) * q(0) - p(1) * q(1) - p(2) * q(2) - p(3) * q(3)

subTemp(1) = p(0) * q(1) + p(1) * q(0) + p(2) * q(3) - p(3) * q(2)

subTemp(2) = p(0) * q(2) + p(2) * q(0) - p(1) * q(3) + p(3) * q(1)

subTemp(3) = p(0) * q(3) + p(3) * q(0) + p(1) * q(2) - p(2) * q(1)

For i = 0 To 3: pq(i) = subTemp(i): Next i

End Sub

Sub QPower (power, q(), r())

'Power of a quaternion; doesn't work for large rotations

Dim vecMag, a, s, n1, n2, n3, qMag

ReDim nq(3)

VNorm q(), nq()

vecMag = Sqr(nq(1) * nq(1) + nq(2) * nq(2) + nq(3) * nq(3))

If Abs(nq(0)) > vecMag Then a = ArcCos(nq(0)) Else a = ArcSin(vecMag)

s = Sin(a)

If s = 0 Then

n1 = 0: n2 = 0: n3 = 0

Else

n1 = nq(1) / s: n2 = nq(2) / s: n3 = nq(3) / s

End If

a = a * power

qMag = VMag(q()) ^ power

r(0) = qMag * Cos(a): s = qMag * Sin(a)

r(1) = s * n1: r(2) = s * n2: r(3) = s * n3

End Sub

Sub Rmtoq (rm(), q())

'Converts a rotation matrix to the unit quaternion with nonnegative scalar

'component that represents the same rotation.

q(0) = rm(1, 1) + rm(2, 2) + rm(3, 3) + 1

If q(0) > 0 Then q(0) = .5 * Sqr(q(0)) Else q(0) = .0000001

q(1) = (rm(3, 2) - rm(2, 3)) / (4 * q(0))

q(2) = (rm(1, 3) - rm(3, 1)) / (4 * q(0))

q(3) = (rm(2, 1) - rm(1, 2)) / (4 * q(0))

End Sub

Sub TestFit (x(), y(), xx(), par(), er(), fConf(), stage)

'Computes errors and confidence intervals for a function fit

Dim numIn As Integer, numOut As Integer, fuNum As Integer

numIn = UBound(x)

numIn = UBound(y)

ReDim xxInv(numIn, numIn), pred(numOut), sumSqEr(numOut)

MatInv xx(), xxInv()

If stage = 1 Then

fuNum = fuNum + 1

For i = 1 To numOut: pred(i) = 0

For j = 1 To numIn: pred(i) = pred(i) + x(j) * par(j, i): Next j

er(i) = y(i) - pred(i)

sumSqEr(i) = sumSqEr(i) + er(i) * er(i)

Next i

Else

For i = 1 To numOut

sumSqEr(i) = sumSqEr(i) / (fuNum - numIn)

er(i) = Sqr(sumSqEr(i))

Next i

For i = 1 To numOut: For j = 1 To numIn

fConf(j, i) = sumSqEr(i) * xxInv(j, j)

If fConf(j, i) > 0 Then

fConf(j, i) = 2 * Sqr(fConf(j, i))

Else

fConf(j, i) = -1

End If

Next j, i

End If

End Sub

Function VADot (x(), a As Integer, y(), b As Integer)

'Dot product of vectors a and b in arrays x and y

VADot = x(a, 0) * y(b, 0) + x(a, 1) * y(b, 1) + x(a, 2) * y(b, 2) + x(a, 3) * y(b, 3)

End Function

Function VAMag (x(), a)

'Magnitude of vector a in array x

Dim magSqr

magSqr = x(a, 0) * x(a, 0) + x(a, 1) * x(a, 1) + x(a, 2) * x(a, 2) + x(a, 3) * x(a, 3)

If magSqr > 0 Then VAMag = Sqr(magSqr) Else VAMag = 0

End Function

Function VAng (u(), v())

'Gives angle in DEGREES between two vectors

Dim cosang, sinang, y, magU, magV

ReDim x(3)

magU = VMag(u()): magV = VMag(v())

If magU = 0 Or magV = 0 Then

VAng = 0

Exit Function

End If

cosang = VDot(u(), v()) / (VMag(u()) * VMag(v()))

If Abs(cosang) < .99 Then

VAng = ArcCos(cosang) * RtoD

Else

VCross u(), v(), x()

sinang = VMag(x()) / (VMag(u()) * VMag(v()))

y = ArcSin(sinang) * RtoD

If VDot(u(), v()) >= 0 Then VAng = y Else VAng = 180 - y

End If

End Function

Sub VCross (u(), v(), x())

'Cross product of two vectors

ReDim subTemp(3)

subTemp(1) = u(2) * v(3) - u(3) * v(2)

subTemp(2) = u(3) * v(1) - u(1) * v(3)

subTemp(3) = u(1) * v(2) - u(2) * v(1)

For i = 1 To 3: x(i) = subTemp(i): Next i

End Sub

Function VDot (u(), v())

'Dot product of two vectors

VDot = u(0) * v(0) + u(1) * v(1) + u(2) * v(2) + u(3) * v(3)

End Function

Function VMag (v())

'Magnitude of a vector

Dim magSqr

magSqr = v(0) * v(0) + v(1) * v(1) + v(2) * v(2) + v(3) * v(3)

If magSqr > 0 Then VMag = Sqr(magSqr) Else VMag = 0

End Function

Sub VNorm (v(), n())

'Normalizes a vector

Dim vm
vm = VMag(v())

If vm <= 0 Then

For i = 0 To 3: n(i) = 0: Next i

Else

For i = 0 To 3: n(i) = v(i) / vm: Next i

End If

End Sub