ccd-ik-link.glisp

(defn ccd-ik-root
	{:inverse (fn [[target edge] [pos length angle out]]
	              (let [p0 pos
	                    diffAngle (- (vec2/angle (vec2/- target p0)) (vec2/angle (vec2/- edge p0)))
	                    newAngle (+ angle diffAngle)
	                    p1 (vec2/+ p0 (vec2/dir newAngle length))
	                    ]
	    							[pos length newAngle {:p0 p0 :p1 p1}]))}
	[pos length angle out]
	(let [p0 pos
				p1 (vec2/+ p0 (vec2/dir angle length))]
	     {:p0 p0 :p1 p1 :angle angle
	      :path [(line p0 p1)]}
	      ))
(defn ccd-ik-limb
	{:inverse (fn [[target edge] [length angle out parent]]
	              (let [p0 (get parent :p1)
	                    diffAngle (- (vec2/angle (vec2/- target p0)) (vec2/angle (vec2/- edge p0)))
	                    newAngle (+ angle diffAngle)
	                    p1 (vec2/+ p0 (vec2/dir newAngle length))
	                    newEdge (vec2/rotate p0 diffAngle edge)
	    								param [target newEdge]]
	    							[length newAngle {:p0 p0 :p1 p1} param]))}
	[length angle out parent]
	(let [p0 (get parent :p1)
	      angle (+ angle (get parent :angle))
	      p1 (vec2/+ p0 (vec2/dir angle length))]
	     {:p0 p0 :p1 p1 :angle angle
	      :path (concat (get parent :path) [(line p0 p1)])}
	      ))
(defn ccd-ik-edge
	{:inverse (fn [target [length angle out parent]]
	              (let [p0 (get parent :p1)
	                    absAngle (vec2/angle (vec2/- target p0))
	    								relAngle (- absAngle (get parent :angle))
	    								edge (vec2/+ p0 (vec2/dir absAngle length))
	    								param [target edge]]
	    							[length relAngle {:p0 p0 :p1 edge} param]))}
	[length angle out parent]
	(let [p0 (get parent :p1)
	      angle (+ angle (get parent :angle))
	      p1 (vec2/+ p0 (vec2/dir angle length))]
	     {:p0 p0 :p1 p1 :angle angle
	      :path (concat (get parent :path) [(line p0 p1)])}
	      ))
(defn ccd-ik-target
	{:inverse (fn [p [pos edge]] [p p])}
	[pos edge]
	(get edge :path)
	)
(defn target-dispatcher
	{:handles {:draw (fn [{:params [pos radius phase]}] [{:type "point" :pos pos}])
	           :drag (fn [{:pos p :params [pos radius phase & children]}]
	                     (concat [p radius (+ phase 0.08)]
	                             (map (fn [v]
	                              (path/position-at (mod (+ (fit v 0 (count children) 0 1) phase) 1)
                           (circle p radius))) (range (count children)))))}}
	[pos radius phase & children]
	children)

:start-sketch

(defvar wrist [64.56845689587308 -7.159262365309299])

(style (stroke "salmon" 2)
	(target-dispatcher [111.95691207051277 -8.375194295775145] 8 202.21000000001257 (ccd-ik-target [117.43328022041727 -2.543403408324651] (ccd-ik-edge 5 (deg 6487.743554738277) {:p0 [74.4362008214624 -8.437634002811324] :p1 [79.38987240618866 -7.758562702669891]} (ccd-ik-root wrist 10 -113.03139015890311 {:p0 [64.43620670504319 -8.448481656064889] :p1 [74.41447062455768 -7.789505820001663]}))) (ccd-ik-target [109.64402627315292 -0.714623734070673] (ccd-ik-edge 5 (deg 3955.589516210486) {:p0 [78.22486632918537 -6.681450418969841] :p1 [83.13706923935763 -5.748571772362908]} (ccd-ik-limb 6 (deg 3252.0387875013707) {:p0 [72.43376633866444 -8.250896467672543] :p1 [78.30180609893793 -6.999453551735348]} (ccd-ik-root wrist 8 -125.59566613072445 {:p0 [64.43620670504319 -8.448481656064889] :p1 [72.4176960744522 -7.9045814391032]})))) (ccd-ik-target [104.16655049196646 -6.54790364100361] (ccd-ik-edge 5 (deg 4303.822908454433) {:p0 [81.18090695189048 -5.93089493910795] :p1 [86.17910652857434 -6.0650627133209385]} (ccd-ik-limb 7 (deg 3244.6566271584397) {:p0 [74.40815950234558 -7.700045013206552] :p1 [81.38505340263036 -7.131756790410164]} (ccd-ik-root wrist 10 -131.87942361135688 {:p0 [64.43620670504319 -8.448481656064889] :p1 [74.41345579023994 -7.77431499125509]})))) (ccd-ik-target [106.48054392060827 -14.206985183225637] (ccd-ik-edge 5 (deg 2506.079587489801) {:p0 [77.43153327606765 -8.212963082739053] :p1 [82.32837375177014 -9.22338544765475]} (ccd-ik-limb 6 (deg 3957.0050670663604) {:p0 [71.4362065939568 -8.449728736434123] :p1 [77.42801154966013 -8.763214577915031]} (ccd-ik-root wrist 7 -113.13429899771332 {:p0 [64.43620670504319 -8.448481656064889] :p1 [71.4314252064856 -8.707167019156547]})))) (ccd-ik-target [114.26979786787263 -16.035764857479617] (ccd-ik-edge 4 (deg -1799.0049987230927) {:p0 [75.32928517356858 -9.93121480644009] :p1 [79.28102176625853 -10.55071291135664]} (ccd-ik-limb 5 (deg 5036.493318571835) {:p0 [70.40380638266522 -9.071180690627681] :p1 [75.39444474559926 -9.377005363293376]} (ccd-ik-root wrist 6 -56.66766810790649 {:p0 [64.43620670504319 -8.448481656064889] :p1 [70.39377357033328 -9.160799735013951]})))) (ccd-ik-target [119.74727364905908 -10.202484950546683] (ccd-ik-edge 15 (deg 3603.0327527422965) {:p0 [49.59121806430965 -6.3332375579890785] :p1 wrist} (ccd-ik-limb 20 (deg 2882.8749310014778) {:p0 [29.707804483386298 -4.176883141397791] :p1 [49.68263244034879 -3.1737639611656423]} (ccd-ik-root [0 0] 30 -113.22155831762164 {:p0 [0 0] :p1 [29.768827985926485 -3.717106447805864]}))))))