turtle-r7rs/turtle3.body.scm

(define (image-rotate x y theta1 delta-theta)
  (when canvas-image-rotate
    (canvas-image-rotate x y theta1 delta-theta)))

(define (line-color color)
  (if canvas-line-color
      (canvas-line-color color)
      (error "line-color" "Not supported by the implementation")))

(define (bg-color color)
  (if canvas-bg-color
      (canvas-bg-color color)
      (error "bg-color" "Not supported by the implementation")))

(define (line-width width)
  (if canvas-line-width
      (canvas-line-width width)
      (error "line-width" "Not supported by the implementation")))

(define (normalize-angle theta)
  (cond
   ((>= theta 360)
    (normalize-angle (- theta 360)))
   ((< theta 0)
    (normalize-angle (+ theta 360)))
   (else theta)))

(define (xy->facing x y)
  (define as-degree (/ (* 180 (atan (/ (inexact y) x))) 3.1415926))
  (normalize-angle
   (if (>= x 0)
       as-degree
       (+ 180 as-degree))))

(define-operation (get-pos turt))
(define-operation (set-pos! turt new-pos))
(define-operation (get-orient turt))
(define-operation (set-orient! turt new-H new-L new-U))
(define-operation (pen-down? turt))
(define-operation (shown? turt))
(define-operation (show! turt))
(define-operation (hide! turt))
(define-operation (pen-up! turt))
(define-operation (pen-down! turt))
(define-operation (forward! turt dist))
(define-operation (yaw! turt theta))
(define-operation (pitch! turt theta))
(define-operation (roll! turt theta))
(define-operation (nutate! turt))
(define-operation (left! turt theta))
(define-operation (right! turt theta))
(define-operation (back! turt dist))

(define (make-turtle)
  (define pos #(0.0 0.0 0.0))
  (define H #(1.0 0.0 0.0))
  (define L #(0.0 1.0 0.0))
  (define U #(0.0 0.0 1.0))
  (define pen-state #t)
  (define shown-state #f)
  (object
    ((get-pos self)
     pos)

    ((set-pos! self new-pos)
     (let ((start-pos pos))
       (draw-turtle-line (vector-ref start-pos 0)
                         (vector-ref start-pos 1)
                         (vector-ref new-pos 0)
                         (vector-ref new-pos 1)
                         (pen-down? self)
                         (shown? self))
       (let ((updated-vec (vector-copy start-pos)))
         (vector-set! updated-vec 0 (vector-ref new-pos 0))
         (vector-set! updated-vec 1 (vector-ref new-pos 1))
         (when (>= (vector-length new-pos) 3)
           (vector-set! updated-vec 2 (vector-ref new-pos 2)))
         (set! pos updated-vec))))

    ((get-orient self)
     (list H L U))

    ((set-orient! self new-H new-L new-U)
     (when shown-state
       (let ((old-tilt-x (vector-ref H 0))
             (old-tilt-y (vector-ref H 1))
             (new-tilt-x (vector-ref new-H 0))
             (new-tilt-y (vector-ref new-H 1)))

         (define old-tilt (xy->facing old-tilt-x old-tilt-y))
         (define new-tilt (xy->facing new-tilt-x new-tilt-y))

         (image-rotate (vector-ref pos 0)
                       (vector-ref pos 1)
                       old-tilt
                       (- new-tilt old-tilt))))

     (set! H new-H)
     (set! L new-L)
     (set! U new-U))

    ((pen-down? self)
     pen-state)

    ((shown? self)
     shown-state)

    ((show! self)
     (set! shown-state #t)
     (when canvas-show-turtle
       (let ((h-x (vector-ref H 0))
             (h-y (vector-ref H 1)))
         (canvas-show-turtle (vector-ref pos 0)
                             (vector-ref pos 1)
                             (+ (/ (* 180 (atan (/ h-y h-x))) 3.14159265)
                                 (if (negative? h-x)
                                     180
                                     0))))))

    ((hide! self)
     (set! shown-state #f)
     (when canvas-hide-turtle
       (canvas-hide-turtle)))

    ((pen-up! self)
     (set! pen-state #f))

    ((pen-down! self)
     (set! pen-state #t))

    ((forward! self dist)
     ;; dist is a vector of 3 real numbers.
     (let ((start-pos pos))
       (define new-pos (vector-sum start-pos
                                   (scale-vector H dist)))
       (set-pos! self new-pos)))

    ((yaw! self theta)
     ;; theta is a real number.
     (set-orient! self
                  (rotate H L theta)
                  (rotate L (negate-vector H) theta)
                  U))

    ((pitch! self theta)
     (set-orient! self
                  (rotate H U theta)
                  L
                  (rotate U (negate-vector H) theta)))
    ((roll! self theta)
     (set-orient! self
                  H
                  (rotate L U theta)
                  (rotate U (negate-vector L) theta)))

    ((nutate! self)
     (pitch! self -47.85)
     (yaw! self -11.43)
     (roll! self 43.32))
    
    ;; These are aliases that are equivalent to (turtle simple)
    ;; therefore removing the need for a separate simple library
    ((left! self theta)
     (yaw! self theta))

    ((right! self theta)
     (yaw! self (- theta)))

    ((back! self dist)
     (forward! self (- dist)))))

(define fd! forward!)
(define bk! back!)
(define lt! left!)
(define rt! right!)
(define pu! pen-up!)
(define pd! pen-down!)

(define (get-xcor t)
  (vector-ref (get-pos t) 0))

(define (get-ycor t)
  (vector-ref (get-pos t) 1))

(define (get-zcor t)
  (vector-ref (get-pos t) 2))

(define (set-xcor! t new-xcor)
  (define start-pos (get-pos t))
  (set-pos! t (vector new-xcor
                      (vector-ref start-pos 1)
                      (vector-ref start-pos 2))))

(define (set-ycor! t new-ycor)
  (define start-pos (get-pos t))
  (set-pos! t (vector (vector-ref start-pos 0)
                      new-ycor
                      (vector-ref start-pos 2))))

(define (set-zcor! t new-zcor)
  (define start-pos (get-pos t))
  (set-pos! t (vector (vector-ref start-pos 0)
                      (vector-ref start-pos 1)
                      new-zcor)))

(define (face! t face-pos)
  (define actual-face-pos
    (vector
     (vector-ref face-pos 0)
     (vector-ref face-pos 1)
     (if (= (vector-length face-pos) 3)
         (vector-ref face-pos 2)
         0.0)))

  (define old-orient (get-orient t))

  (define new-H (unit-vector (vector-difference actual-face-pos (get-pos t))))
  (define U (list-ref old-orient 2))
  (define new-L (vector-cross-product U new-H))

  (define new-orient (list new-H new-L U))

  (apply set-orient! t new-orient))