' Vector Math Library — add, subtract, dot, cross, normalize
#COMPILE EXE
#DIM ALL

TYPE Vec3
    x AS DOUBLE
    y AS DOUBLE
    z AS DOUBLE
END TYPE

SUB VecAdd(a AS Vec3, b AS Vec3, BYREF result AS Vec3)
    result.x = a.x + b.x
    result.y = a.y + b.y
    result.z = a.z + b.z
END SUB

SUB VecSub(a AS Vec3, b AS Vec3, BYREF result AS Vec3)
    result.x = a.x - b.x
    result.y = a.y - b.y
    result.z = a.z - b.z
END SUB

SUB VecScale(a AS Vec3, s AS DOUBLE, BYREF result AS Vec3)
    result.x = a.x * s
    result.y = a.y * s
    result.z = a.z * s
END SUB

FUNCTION VecDot(a AS Vec3, b AS Vec3) AS DOUBLE
    FUNCTION = a.x * b.x + a.y * b.y + a.z * b.z
END FUNCTION

SUB VecCross(a AS Vec3, b AS Vec3, BYREF result AS Vec3)
    result.x = a.y * b.z - a.z * b.y
    result.y = a.z * b.x - a.x * b.z
    result.z = a.x * b.y - a.y * b.x
END SUB

FUNCTION VecLength(a AS Vec3) AS DOUBLE
    FUNCTION = SQR(a.x * a.x + a.y * a.y + a.z * a.z)
END FUNCTION

SUB VecNormalize(a AS Vec3, BYREF result AS Vec3)
    LOCAL len AS DOUBLE
    len = SQR(a.x * a.x + a.y * a.y + a.z * a.z)
    IF len > 0.000001 THEN
        result.x = a.x / len
        result.y = a.y / len
        result.z = a.z / len
    ELSE
        result.x = 0
        result.y = 0
        result.z = 0
    END IF
END SUB

SUB PrintVec(label AS STRING, v AS Vec3)
    PRINT label & "(" & FORMAT$(v.x, "##.####") & ", " & FORMAT$(v.y, "##.####") & ", " & FORMAT$(v.z, "##.####") & ")"
END SUB

SUB VectorBasics()
    LOCAL a AS Vec3
    LOCAL b AS Vec3
    LOCAL c AS Vec3
    a.x = 3.0
    a.y = 4.0
    a.z = 0.0
    b.x = 1.0
    b.y = 2.0
    b.z = 3.0
    PRINT "=== VECTOR BASICS ==="
    CALL PrintVec("a = ", a)
    CALL PrintVec("b = ", b)
    CALL VecAdd(a, b, c)
    CALL PrintVec("a + b = ", c)
    CALL VecSub(a, b, c)
    CALL PrintVec("a - b = ", c)
    CALL VecScale(a, 2.5, c)
    CALL PrintVec("a * 2.5 = ", c)
    PRINT "|a| = " & FORMAT$(VecLength(a), "##.####")
    PRINT "|b| = " & FORMAT$(VecLength(b), "##.####")
    PRINT
END SUB

SUB DotAndCross()
    LOCAL a AS Vec3
    LOCAL b AS Vec3
    LOCAL c AS Vec3
    a.x = 2.0
    a.y = 3.0
    a.z = 4.0
    b.x = 5.0
    b.y = 6.0
    b.z = 7.0
    PRINT "=== DOT & CROSS PRODUCT ==="
    CALL PrintVec("a = ", a)
    CALL PrintVec("b = ", b)
    PRINT "a . b = " & FORMAT$(VecDot(a, b), "##.####")
    CALL VecCross(a, b, c)
    CALL PrintVec("a x b = ", c)
    PRINT "a . (a x b) = " & FORMAT$(VecDot(a, c), "##.####") & " (should be 0)"
    PRINT "b . (a x b) = " & FORMAT$(VecDot(b, c), "##.####") & " (should be 0)"
    PRINT
END SUB

SUB NormalizeDemo()
    LOCAL a AS Vec3
    LOCAL n AS Vec3
    a.x = 10.0
    a.y = 0.0
    a.z = 0.0
    CALL VecNormalize(a, n)
    CALL PrintVec("Normalize(10,0,0) = ", n)
    a.x = 1.0
    a.y = 1.0
    a.z = 1.0
    CALL VecNormalize(a, n)
    CALL PrintVec("Normalize(1,1,1) = ", n)
    PRINT "Length = " & FORMAT$(VecLength(n), "##.####") & " (should be 1)"
    PRINT
END SUB

SUB DistanceAndDirection()
    LOCAL p1 AS Vec3
    LOCAL p2 AS Vec3
    LOCAL dir AS Vec3
    LOCAL dist AS DOUBLE
    p1.x = 0.0
    p1.y = 0.0
    p1.z = 0.0
    p2.x = 3.0
    p2.y = 4.0
    p2.z = 12.0
    PRINT "=== DISTANCE & DIRECTION ==="
    CALL PrintVec("Point 1 = ", p1)
    CALL PrintVec("Point 2 = ", p2)
    CALL VecSub(p2, p1, dir)
    dist = VecLength(dir)
    PRINT "Distance = " & FORMAT$(dist, "##.####")
    CALL VecNormalize(dir, dir)
    CALL PrintVec("Direction = ", dir)
    PRINT
END SUB

FUNCTION PBMAIN() AS LONG
    PRINT "========================"
    PRINT " Vector Math Library"
    PRINT "========================"
    PRINT
    CALL VectorBasics()
    CALL DotAndCross()
    CALL NormalizeDemo()
    CALL DistanceAndDirection()
    PRINT "Vector math complete."
    FUNCTION = 0
END FUNCTION
