Code: Select all
' ******************************************************************
' Quaternion library
' Adapted from C++ library by Andrew Burbanks
' http://www.lboro.ac.uk/departments/ma/gallery/quat/src.html
' ******************************************************************
TYPE Quaternion
X AS DOUBLE
Y AS DOUBLE
Z AS DOUBLE
T AS DOUBLE
END TYPE
DIM SHARED ErrCode AS INTEGER ' Error code
SUB QSet(X AS DOUBLE, Y AS DOUBLE, Z AS DOUBLE, T AS DOUBLE, Q AS Quaternion)
' Set quaternion Q = (X, Y, Z, T)
Q.X = X
Q.Y = Y
Q.Z = Z
Q.T = T
END SUB
SUB QAdd(Q1 AS Quaternion, Q2 AS Quaternion, Q AS Quaternion)
' Addition: Q = Q1 + Q2
Q.X = Q1.X + Q2.X
Q.Y = Q1.Y + Q2.Y
Q.Z = Q1.Z + Q2.Z
Q.T = Q1.T + Q2.T
END SUB
SUB QSub(Q1 AS Quaternion, Q2 AS Quaternion, Q AS Quaternion)
' Addition: Q = Q1 - Q2
Q.X = Q1.X - Q2.X
Q.Y = Q1.Y - Q2.Y
Q.Z = Q1.Z - Q2.Z
Q.T = Q1.T - Q2.T
END SUB
SUB QRMul(R AS DOUBLE, Q1 AS Quaternion, Q AS Quaternion)
' Multiplication by real: Q = R * Q1
Q.X = R * Q1.X
Q.Y = R * Q1.Y
Q.Z = R * Q1.Z
Q.T = R * Q1.T
END SUB
SUB QMul(Q1 AS Quaternion, Q2 AS Quaternion, Q AS Quaternion)
' Multiplication: Q = Q1 * Q2 (not commutative!)
DIM AS DOUBLE T0, T1, T2, T3, T4, T5, T6, T7, T8, T9
T0 = (Q1.T - Q1.Z) * (Q2.Z - Q2.T)
T1 = (Q1.X + Q1.Y) * (Q2.X + Q2.Y)
T2 = (Q1.X - Q1.Y) * (Q2.Z + Q2.T)
T3 = (Q1.T + Q1.Z) * (Q2.X - Q2.Y)
T4 = (Q1.T - Q1.Y) * (Q2.Y - Q2.Z)
T5 = (Q1.T + Q1.Y) * (Q2.Y + Q2.Z)
T6 = (Q1.X + Q1.Z) * (Q2.X - Q2.T)
T7 = (Q1.X - Q1.Z) * (Q2.X + Q2.T)
T8 = T5 + T6 + T7
T9 = 0.5 * (T4 + T8)
Q.X = T0 - T5 + T9
Q.Y = T1 - T8 + T9
Q.Z = T2 - T7 + T9
Q.T = T3 - T6 + T9
END SUB
SUB QConj(Q1 AS Quaternion, Q AS Quaternion)
' Conjugate: Q = Conj(Q1)
Q.X = Q1.X
Q.Y = - Q1.Y
Q.Z = - Q1.Z
Q.T = - Q1.T
END SUB
FUNCTION QAbs(Q AS Quaternion) AS DOUBLE
' Absolute value
QAbs = SQR(Q.X * Q.X + Q.Y * Q.Y + Q.Z * Q.Z + Q.T * Q.T)
END FUNCTION
SUB QInv(Q1 AS Quaternion, Q AS Quaternion)
' Inverse: Q = 1 / Q1
ErrCode = 0
IF Q1.X = 0 AND Q1.Y = 0 AND Q1.Z = 0 AND Q1.T = 0 THEN
PRINT "Undefined inverse"
ErrCode = -1
EXIT SUB
END IF
DIM R AS DOUBLE
R = 1 / (Q1.X * Q1.X + Q1.Y * Q1.Y + Q1.Z * Q1.Z + Q1.T * Q1.T)
Q.X = R * Q1.X
Q.Y = - R * Q1.Y
Q.Z = - R * Q1.Z
Q.T = - R * Q1.T
END SUB
SUB QDiv(Q1 AS Quaternion, Q2 AS Quaternion, Q AS Quaternion)
' Division: Q = Q1 / Q2 = Q1 * Inv(Q2)
dim as Quaternion InvQ2
QInv Q2, InvQ2
if ErrCode then
print "Undefined division"
exit sub
end if
QMul Q1, InvQ2, Q
END SUB
SUB QPrint(Q AS Quaternion)
' Prints a quaternion
PRINT "("; Q.X; ","; Q.Y; ","; Q.Z; ","; Q.T; " )"
END SUB
' ******************************************************************
' Demo program
' ******************************************************************
DIM AS Quaternion Q1, Q2, Q
QSet 1, 2, 3, 4, Q1
QSet 5, 6, 7, 8, Q2
PRINT "Q1 = "; : QPrint Q1
PRINT "Q2 = "; : QPrint Q2
PRINT
QAdd Q1, Q2, Q
PRINT "Q1 + Q2 = "; : QPrint Q
PRINT
QSub Q1, Q2, Q
PRINT "Q1 - Q2 = "; : QPrint Q
PRINT
QRMul 5, Q1, Q
PRINT "5 * Q1 = "; : QPrint Q
PRINT
QMul Q1, Q2, Q
PRINT "Q1 * Q2 = "; : QPrint Q
PRINT
QMul Q2, Q1, Q
PRINT "Q2 * Q1 = "; : QPrint Q
PRINT
QConj Q1, Q
PRINT "Q1 conjugate = "; : QPrint Q
PRINT
PRINT "Abs(Q1) = "; QAbs(Q1)
PRINT
QInv Q1, Q
PRINT "Q1 inverse = "; : QPrint Q
PRINT
QDiv Q1, Q2, Q
PRINT "Q1 / Q2 = "; : QPrint Q
PRINT
QDiv Q2, Q1, Q
PRINT "Q2 / Q1 = "; : QPrint Q
PRINT
SLEEP