(define (new_point x y z)
  (let
    ((p (make-vector 3)))
    (vector-set! p _X x)
    (vector-set! p _Y y)
    (vector-set! p _Z z)
    p))

(define (eq_point p1 p2)
  (let
    ((x1 (vector-ref p1 _X))
     (y1 (vector-ref p1 _Y))
     (z1 (vector-ref p1 _Z))
     (x2 (vector-ref p2 _X))
     (y2 (vector-ref p2 _Y))
     (z2 (vector-ref p2 _Z)))
    (and
      (= x1 x2)
      (= y1 y2)
      (= z1 z2))))

(define (copy_point p1 p2)
  (vector-set! p1 _X (vector-ref p2 _X))
  (vector-set! p1 _Y (vector-ref p2 _Y))
  (vector-set! p1 _Z (vector-ref p2 _Z)))

(define (direction_vec p1 p2 p3)
  (let*
    ((delta_x (- (vector-ref p2 _X) (vector-ref p1 _X)))
     (delta_y (- (vector-ref p2 _Y) (vector-ref p1 _Y)))
     (delta_z (- (vector-ref p2 _Z) (vector-ref p1 _Z)))
     (max_delta (max (abs delta_x) (max (abs delta_y) (abs delta_z)))))

    (vector-set! p3 _X (/ delta_x max_delta))
    (vector-set! p3 _Y (/ delta_y max_delta))
    (vector-set! p3 _Z (/ delta_z max_delta))))

(define (zero_adj p)
  (if (= (vector-ref p _X) 0.0)
    (vector-set! p _X epsilon))
  (if (= (vector-ref p _Y) 0.0)
    (vector-set! p _Y epsilon))
  (if (= (vector-ref p _Z) 0.0)
    (vector-set! p _Z epsilon))
)

(define (vec_add p1 p2 p3)
  (vector-set! p3 _X (+ (vector-ref p1 _X) (vector-ref p2 _X)))
  (vector-set! p3 _Y (+ (vector-ref p1 _Y) (vector-ref p2 _Y)))
  (vector-set! p3 _Z (+ (vector-ref p1 _Z) (vector-ref p2 _Z))))

(define (vec_sub p1 p2 p3)
  (vector-set! p3 _X (- (vector-ref p1 _X) (vector-ref p2 _X)))
  (vector-set! p3 _Y (- (vector-ref p1 _Y) (vector-ref p2 _Y)))
  (vector-set! p3 _Z (- (vector-ref p1 _Z) (vector-ref p2 _Z))))

(define (vec_scale a p)
  (vector-set! p _X (* a (vector-ref p _X)))
  (vector-set! p _Y (* a (vector-ref p _Y)))
  (vector-set! p _Z (* a (vector-ref p _Z))))

(define (vec_copy p1 p2)
  (vector-set! p2 _X (vector-ref p1 _X))
  (vector-set! p2 _Y (vector-ref p1 _Y))
  (vector-set! p2 _Z (vector-ref p1 _Z)))

(define (vec_length1 x y z)
  (+ (* x x) (* y y) (* z z)))

(define (vec_length x y z)
  (sqrt (+ (* x x) (* y y) (* z z))))

(define (clip_upper p)
  (if (>= (vector-ref p _X) 1.0)
    (vector-set! p _X 0.9999999))
  (if (>= (vector-ref p _Y) 1.0)
    (vector-set! p _Y 0.9999999))
  (if (>= (vector-ref p _Z) 1.0)
    (vector-set! p _Z 0.9999999)))

(define (vec_color_scale p1 p2 p3)
  (vector-set! p3 _X (* (vector-ref p1 _X) (vector-ref p2 _X)))
  (vector-set! p3 _Y (* (vector-ref p1 _Y) (vector-ref p2 _Y)))
  (vector-set! p3 _Z (* (vector-ref p1 _Z) (vector-ref p2 _Z))))

(define (dot p1 p2)
  (+
    (* (vector-ref p1 _X) (vector-ref p2 _X))
    (* (vector-ref p1 _Y) (vector-ref p2 _Y))
    (* (vector-ref p1 _Z) (vector-ref p2 _Z))))

(define (zero_adjust x)
  (if (= x 0.0)
    epsilon
    x))

(define (normalize p n)
  (let*
    ((a (vector-ref p _X))
     (b (vector-ref p _Y))
     (c (vector-ref p _Z))
     (mag (zero_adjust (vec_length a b c))))
    (vector-set! n _X (/ a mag))
    (vector-set! n _Y (/ b mag))
    (vector-set! n _Z (/ c mag))))

(define (crossproduct p1 p2)
  (new_point
    (- (* (vector-ref p1 _Y) (vector-ref p2 _Z))
       (* (vector-ref p1 _Z) (vector-ref p2 _Y)))
    (- (* (vector-ref p1 _Z) (vector-ref p2 _X))
       (* (vector-ref p1 _X) (vector-ref p2 _Z)))
    (- (* (vector-ref p1 _X) (vector-ref p2 _Y))
       (* (vector-ref p1 _Y) (vector-ref p2 _X)))))

(define (crossproduct_destructive v1 v2 v3)
  (vector-set! v3 _X
    (- (* (vector-ref v1 _Y) (vector-ref v2 _Z))
       (* (vector-ref v1 _Z) (vector-ref v2 _Y))))
  (vector-set! v3 _Y
    (- (* (vector-ref v1 _Z) (vector-ref v2 _X))
       (* (vector-ref v1 _X) (vector-ref v2 _Z))))
  (vector-set! v3 _Z
    (- (* (vector-ref v1 _X) (vector-ref v2 _Y))
       (* (vector-ref v1 _Y) (vector-ref v2 _X)))))

(define (perpendicular_to_plane v1 v2)
  (normalize (crossproduct v1 v2) temp_normalized)
  (new_point
    (vector-ref temp_normalized _X)
    (vector-ref temp_normalized _Y)
    (vector-ref temp_normalized _Z)))

(define (distance p1 p2)
  (let
    ((x_diff (- (vector-ref p2 _X) (vector-ref p1 _X)))
     (y_diff (- (vector-ref p2 _Y) (vector-ref p1 _Y)))
     (z_diff (- (vector-ref p2 _Z) (vector-ref p1 _Z))))
    (vec_length x_diff y_diff z_diff)))