# HG changeset patch # User Steve Losh # Date 1471302577 0 # Node ID b9ba5d3627e3a93493245232e839fd5d344e6872 # Parent 28dc985f4d510bc25166560e271f49bbdcdcf7ac Episode 45: Kinematics Part 3 diff -r 28dc985f4d51 -r b9ba5d3627e3 .lispwords --- 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) diff -r 28dc985f4d51 -r b9ba5d3627e3 package.lisp --- 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 diff -r 28dc985f4d51 -r b9ba5d3627e3 src/2d/demo.lisp --- 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*)) ;; ;; )