If it helps, here's my TVector type:
(it has the Length (called 'Magnitude'), Cross Product, and Dot Product functions)
//------------------------------------------------------------------ V e c t o r
TYPE TVector
x
y
z
FUNCTION Set%: x, y, z
self.x = x
self.y = y
self.z = z
ENDFUNCTION
FUNCTION Clear:
self.x = 0
self.y = 0
self.z = 0
ENDFUNCTION
FUNCTION Copy: v_from AS TVector
self.x = v_from.x
self.y = v_from.y
self.z = v_from.z
ENDFUNCTION
FUNCTION Add: v AS TVector
INC self.x, v.x
INC self.y, v.y
INC self.z, v.z
ENDFUNCTION
FUNCTION Subtract AS TVector: v1 AS TVector
LOCAL v AS TVector
v.x = self.x - v1.x
v.y = self.y - v1.y
v.z = self.z - v1.z
RETURN v
ENDFUNCTION
FUNCTION MidPoint: v1 AS TVector, v2 AS TVector
self.x = (v2.x - v1.x) / 2.0 + v1.x
self.y = (v2.y - v1.y) / 2.0 + v1.y
self.z = (v2.z - v1.z) / 2.0 + v1.z
ENDFUNCTION
FUNCTION IsEqual%: v AS TVector
IF self.x <> v.x THEN RETURN FALSE
IF self.y <> v.y THEN RETURN FALSE
IF self.z <> v.z THEN RETURN FALSE
RETURN TRUE
ENDFUNCTION
// Calc the magnitude / length of a vector
FUNCTION Magnitude:
RETURN SQR((self.x * self.x) + (self.y * self.y) + (self.z * self.z))
ENDFUNCTION
FUNCTION Normalize:
LOCAL TOLLERANCE = 0.0001
LOCAL m
//normalize a vector so it's length = 1 AND apply tolerance based on our constant tolerance value
m = self.Magnitude()
IF m <= TOLLERANCE THEN m = 1.0
self.x = self.x / m
self.y = self.y / m
self.z = self.z / m
IF ABS(self.x) < TOLLERANCE THEN self.x = 0
IF ABS(self.y) < TOLLERANCE THEN self.y = 0
IF ABS(self.z) < TOLLERANCE THEN self.z = 0
ENDFUNCTION
// Reverse a vector
FUNCTION Reverse AS TVector:
LOCAL v AS TVector
v.x = -self.x
v.y = -self.y
v.z = -self.z
RETURN v
ENDFUNCTION
// Vector : Scalar Multiply -> used TO scale a vector by 'scale'
FUNCTION ScaleSet AS TVector: amount
LOCAL v AS TVector
v.x = self.x * amount
v.y = self.y * amount
v.z = self.z * amount
RETURN v
ENDFUNCTION
// Vector : Scalar Divide
FUNCTION ScalarDivide: amount#
self.x = self.x / amount
self.y = self.y / amount
self.z = self.z / amount
ENDFUNCTION
// Takes v1 AND v2 AND returns the cross product v1 X v2.
// The cross product is a vector perpendicular TO both v1 AND v2
// This is the normal of 2 vectors
FUNCTION CrossProduct AS TVector: v2 AS TVector
LOCAL v AS TVector
v.x = (self.y * v2.z) - (self.z * v2.y)
v.y = (self.z * v2.x) - (self.x * v2.z)
v.z = (self.x * v2.y) - (self.y * v2.x)
RETURN v
ENDFUNCTION
// Calculate AND RETURN the dot product of 2 vectors
FUNCTION DotProduct#: v2 AS TVector
LOCAL dp
dp = (self.x * v2.x) + (self.y * v2.y) + (self.z * v2.z)
RETURN dp
ENDFUNCTION
// Calculate the triple scalar FUNCTION AND RETURN it
FUNCTION TripleScalarProduct: v2 AS TVector, v3 AS TVector
LOCAL v
v = self.x * ( ( v2.y * v3.z) - (v2.z * v3.y) )
v = v + ( self.y * ( (-v2.x * v3.z) + (v2.z * v3.x) ) )
v = v + ( self.z * ( ( v2.x * v3.y) + (v2.y * v3.x) ) )
RETURN v
ENDFUNCTION
FUNCTION RotateAroundPointX: pivot AS TVector, angle#
LOCAL v AS TVector
LOCAL y, z, length
v.y = self.y - pivot.y
v.z = self.z - pivot.z
length = v.Magnitude()
angle = ATAN(v.z, v.y) + angle
self.y = COS(angle) * length + pivot.y
self.z = SIN(angle) * length + pivot.z
ENDFUNCTION
FUNCTION RotateAroundPointY: pivot AS TVector, angle#
LOCAL v AS TVector
LOCAL x, z, length
v.x = self.x - pivot.x
v.z = self.z - pivot.z
length = v.Magnitude()
angle = ATAN(v.z, v.x) + angle
self.x = COS(angle) * length + pivot.x
self.z = SIN(angle) * length + pivot.z
ENDFUNCTION
FUNCTION RotateAroundPointZ: pivot AS TVector, angle#
LOCAL v AS TVector
LOCAL x, y, length
v.x = self.x - pivot.x
v.y = self.y - pivot.y
length = v.Magnitude()
angle = ATAN(v.y, v.x) + angle
self.x = COS(angle) * length + pivot.x
self.y = SIN(angle) * length + pivot.y
ENDFUNCTION
FUNCTION RotateAroundVec AS TVector: v2 AS TVector, angle
LOCAL v AS TVector
LOCAL cosa, sina, ecosa
cosa = COS(angle)
sina = SIN(angle)
ecosa = 1.0 - cosa
v.x = self.x * (cosa + v2.x * v2.x * ecosa) + self.y * (v2.x * v2.y * ecosa - v2.z * sina) + self.z * (v2.x * v2.z * ecosa + v2.y * sina)
v.y = self.x * (v2.y * v2.x * ecosa + v2.z * sina) + self.y * (cosa + v2.y * v2.y * ecosa) + self.z * (v2.y * v2.z * ecosa - v2.x * sina)
v.z = self.x * (v2.z * v2.x * ecosa - v2.y * sina) + self.y * (v2.z * v2.y * ecosa + v2.x * sina) + self.z * (cosa + v2.z * v2.z * ecosa)
RETURN v
ENDFUNCTION
ENDTYPE
[Edit] Updated code to be self-contained. It was referencing other libraries.