b9ba5d3627e3

--- a/.lispwords	Mon Aug 15 22:22:59 2016 +0000
+++ b/.lispwords	Mon Aug 15 23:09:37 2016 +0000
@@ -8,4 +8,4 @@
 (1 with-elapsed)
 (1 tween-places!)
 (1 with-normalized-time)
-(1 with-arm)
+(1 with-arm with-iks)

--- a/package.lisp	Mon Aug 15 22:22:59 2016 +0000
+++ b/package.lisp	Mon Aug 15 23:09:37 2016 +0000
@@ -278,6 +278,7 @@
 (defpackage #:coding-math.2d.demo
   (:use
     #:cl
+    #:cl-arrows
     #:losh
     #:sketch
     #:iterate

--- a/src/2d/demo.lisp	Mon Aug 15 22:22:59 2016 +0000
+++ b/src/2d/demo.lisp	Mon Aug 15 23:09:37 2016 +0000
@@ -7,6 +7,8 @@
 
 (defparameter *center-x* (/ *width* 2))
 (defparameter *center-y* (/ *height* 2))
+(defparameter *center* (vec2 *center-x* *center-y*))
+
 
 (defvar *shift* nil)
 (defvar *control* nil)
@@ -33,84 +35,75 @@
 
 
 ;;;; Episode
-(defstruct (arm (:constructor %make-arm))
-  pos length angle parent center-angle rotation-range phase-offset)
+(defparameter *arm-pen* (make-pen :weight 10 :stroke (gray 0.1)))
 
-(defun make-arm (length center-angle rotation-range parent phase-offset)
-  (%make-arm :pos (vec2 0 0)
-             :length length
-             :angle center-angle
-             :parent parent
-             :center-angle center-angle
-             :rotation-range rotation-range
-             :phase-offset phase-offset))
+(defstruct (arm (:constructor make-arm (pos length angle &optional parent)))
+  pos length angle parent)
 
 (define-with-macro arm
-  pos length angle parent center-angle rotation-range phase-offset)
-
+  pos length angle parent)
 
-(defun arm-set-phase (arm phase)
-  (with-arm (arm)
-    (setf angle (+ center-angle (* rotation-range (sin (+ phase-offset phase)))))))
-
-(defun arm-total-angle (arm)
-  (loop :for a = arm :then (arm-parent a)
-        :while a
-        :sum (arm-angle a)))
 
 (defun arm-end (arm)
   (with-arm (arm)
-    (vec2-add pos (vec2-magdir length (arm-total-angle arm)))))
-
-(defparameter *arm-pen* (make-pen :stroke (gray 0.1) :weight 5))
+    (vec2-add pos (vec2-magdir length angle))))
 
 (defun draw-arm (arm &optional (pen *arm-pen*))
+  (with-pen pen (draw-line (arm-pos arm) (arm-end arm))))
+
+(defun arm-point-at (arm target)
   (with-arm (arm)
-    (with-pen pen (draw-line pos (arm-end arm)))))
+    (setf angle (-<> target
+                  (vec2-sub <> pos)
+                  vec2-angle))))
+
+(defun arm-drag (arm target)
+  (arm-point-at arm target)
+  (with-arm (arm)
+    (setf pos (vec2-sub target
+                        (vec2-magdir length angle)))
+    (when parent
+      (arm-drag parent pos))))
 
 
-(defclass forward-kinematics-system ()
-  ((origin :type vec2 :initarg :origin :reader fks-origin)
-   (arms   :type vector :initarg :arms :accessor fks-arms)
-   (last-arm :initarg :last-arm :accessor fks-last-arm)
-   (phase :initarg :phase :accessor fks-phase)
-   (speed :initform 0.05 :accessor fks-speed)))
-
-(defun make-fks (x y &optional (starting-phase 0.0))
-  (make-instance 'forward-kinematics-system
-                 :origin (vec2 x y)
-                 :arms (make-array 10 :fill-pointer 0)
-                 :last-arm nil
-                 :phase starting-phase))
+(defun vector-last (vector)
+  (let ((l (length vector)))
+    (if (zerop l)
+      nil
+      (aref vector (1- l)))))
 
 
-(defun fks-update (fks)
-  (iterate (with phase = (fks-phase fks))
-           (for arm :in-vector (fks-arms fks))
-           (for parent = (arm-parent arm))
-           (arm-set-phase arm phase)
-           (setf (arm-pos arm)
-                 (if parent
-                   (arm-end parent)
-                   (fks-origin fks))))
-  (incf (fks-phase fks) (fks-speed fks))
-  (wrapf (fks-phase fks) 0 tau)
-  )
+(defclass inverse-kinematic-system ()
+  ((origin :initarg :origin
+           :accessor iks-origin)
+   (arms :initform (make-array 10 :fill-pointer 0)
+         :accessor iks-arms)))
+
+(define-with-macro iks origin arms)
 
 
-(defun fks-add-arm (fks length center-angle rotation-range &optional (phase-offset 0.0))
-  (let* ((arm (make-arm length center-angle rotation-range
-                        (fks-last-arm fks) phase-offset)))
-    (vector-push-extend arm (fks-arms fks) 10)
-    (setf (fks-last-arm fks) arm))
-  (fks-update fks))
+(defun make-iks (origin)
+  (make-instance 'inverse-kinematic-system :origin origin))
+
+(defun iks-last-arm (iks)
+  (vector-last (iks-arms iks)))
 
-(defun draw-fks (fks)
-  (map nil #'draw-arm (fks-arms fks)))
+(defun iks-add-arm (iks length)
+  (with-iks (iks)
+    (let ((parent (iks-last-arm iks)))
+      (vector-push-extend (make-arm (if parent
+                                      (arm-pos parent)
+                                      origin)
+                                    length 0 parent)
+                        arms))))
 
-(defun fks-rotate-arm (fks arm-index angle)
-  (setf (arm-angle (aref (fks-arms fks) arm-index))
-        angle))
+(defun draw-iks (iks)
+  (map nil #'draw-arm (iks-arms iks)))
+
+(defun iks-drag (iks target)
+  (let ((arm (iks-last-arm iks)))
+    (when arm
+      (arm-drag arm target))))
 
 
 ;;;; Sketch
@@ -161,16 +154,10 @@
      (previous-time 0)
      (total-time 0)
      ;; Data
-     (a 0.0)
-     (leg0 (make-fks 0 0))
-     (leg1 (make-fks 0 0 pi))
+     (iks (make-iks (vec2 0 0)))
      (lol (progn
-            (fks-add-arm leg0 90 (* tau  -1/4) (* tau 1/6))
-            (fks-add-arm leg0 60 (* tau -1/12) (* tau 1/12) -1.5)
-            (fks-add-arm leg0 20 (* tau   1/5) (* tau (- 1/4 1/5)) -1.5)
-            (fks-add-arm leg1 90 (* tau  -1/4) (* tau 1/6))
-            (fks-add-arm leg1 60 (* tau -1/12) (* tau 1/12) -1.5)
-            (fks-add-arm leg1 20 (* tau   1/5) (* tau (- 1/4 1/5)) -1.5)
+            (iterate (repeat 5)
+                     (iks-add-arm iks (random-around 30 10)))
             ))
      ;; Pens
      (particle-pen (make-pen :fill (gray 0.9) :stroke (gray 0.4)))
@@ -187,11 +174,11 @@
   (with-setup
     (in-context
       (translate *center-x* *center-y*)
+      (draw-axes *width* *height*)
 
-      (fks-update leg0)
-      (fks-update leg1)
-      (draw-fks leg0)
-      (draw-fks leg1)
+      (iks-drag iks mouse)
+      (draw-iks iks)
+
       ))
   ;;
 
@@ -201,8 +188,7 @@
 ;;;; Mouse
 (defun mousemove (instance x y)
   (with-slots (mouse) instance
-    (setf (vec2-x mouse) x
-          (vec2-y mouse) (- *height* y))
+    (setf mouse (vec2-sub (vec2 x (- *height* y)) *center*))
     ;;
     ;;
     )