turtle-r7rs/vector.sld

(define-library (turtle vector)
  (import (scheme base)
          (scheme inexact))
  (export rotate vector-sum scale-vector negate-vector vector-magnitude
          vector-difference vector-distance unit-vector vector-dot-product
          degrees->radians radians->degrees vector-cross-product
          vector-angle)
  (begin
    (define (rotate vec pvec theta-degs)
      (define theta (degrees->radians theta-degs))
      (vector-sum (scale-vector vec
                                (cos theta))
                  (scale-vector pvec
                                (sin theta))))
    
    (define (vector-sum . vs)
      (list->vector (apply map + (map vector->list vs))))

    (define (vector-difference . vs)
      (if (null? (cdr vs))
          (negate-vector (car vs))
          (apply vector-sum
                 (car vs)
                 (map negate-vector (cdr vs)))))

    (define (vector-distance v1 v2)
      (vector-magnitude
       (vector-sum v1 (scale-vector v2 -1.0))))
    
    (define (scale-vector v k)
      (vector-map (lambda (x)
                    (* x k))
                  v))
    
    (define (negate-vector v)
      (scale-vector v -1))

    (define (unit-vector v)
      (scale-vector v (/ (vector-magnitude v))))
    
    (define pi (* 2 (acos 0)))
    
    (define (degrees->radians degs)
      (* degs (/ pi 180)))

    (define (radians->degrees rads)
      (/ rads (/ pi 180)))

    (define (vector-magnitude v)
      (sqrt (apply + (map square (vector->list v)))))

    (define (vector-dot-product . l)
      (apply + (vector->list (apply vector-map * l))))

    (define (vector-cross-product v1 v2)
      (define s0 (- (* (vector-ref v1 1)
                       (vector-ref v2 2))
                    (* (vector-ref v1 2)
                       (vector-ref v2 1))))
      (define s1 (- (* (vector-ref v1 2)
                       (vector-ref v2 0))
                    (* (vector-ref v1 0)
                       (vector-ref v2 2))))
      (define s2 (- (* (vector-ref v1 0)
                       (vector-ref v2 1))
                    (* (vector-ref v1 1)
                       (vector-ref v2 0))))
      (vector s0 s1 s2))

    (define (vector-angle v1 v2)
      (define n (vector-dot-product v1 v2))
      (define m (* (vector-magnitude v1)
                   (vector-magnitude v2)))
      (acos (/ n m)))))