Some progress on IK

I didn't get it quite right. There are still some kinks. The main
issue I had was assuming that the repeated rotation of child bones by
the parent's rotation was okay. I think that accumulated a lot of
error quickly.

The following blog post helped me:
https://www.ryanjuckett.com/cyclic-coordinate-descent-in-2d/

Dependencies/gamelib

@@ -1 +1 @@
-Subproject commit a460f3fcd8cab8de22ff9381edb9f55954438868
+Subproject commit 8197da79316180b9c182e97642e04c044bc15a69

data/TestPuppet_skeleton.cakedata

@@ -2,257 +2,321 @@
  :bones (array (skeleton-bone :shape-id "Arm_Upper_Right"
  :parent-shape-id "UpperBody"
  :start-position (vec 435.000 255.000)
- :rest-position (vec 249.000 251.000)
- :end-position (vec 247.000 252.000)) (skeleton-bone :shape-id "Arm_Lower_Right"
+ :end-position (vec 250.000 253.000)
+ :pose-start-position (vec 435.000 255.000)
+ :pose-end-position (vec 249.000 254.000)) (skeleton-bone :shape-id "Arm_Lower_Right"
  :parent-shape-id "Arm_Upper_Right"
  :start-position (vec 268.000 256.000)
- :rest-position (vec 102.000 255.000)
- :end-position (vec 101.000 255.000)) (skeleton-bone :shape-id "Arm_Upper_Left"
+ :end-position (vec 110.000 255.000)
+ :pose-start-position (vec 268.000 256.000)
+ :pose-end-position (vec 115.000 254.000)) (skeleton-bone :shape-id "Arm_Upper_Left"
  :parent-shape-id "UpperBody"
  :start-position (vec 584.000 253.000)
- :rest-position (vec 794.000 254.000)
- :end-position (vec 772.000 254.000)) (skeleton-bone :shape-id "Arm_Lower_Left"
+ :end-position (vec 772.000 254.000)
+ :pose-start-position (vec 584.000 253.000)
+ :pose-end-position (vec 770.000 255.000)) (skeleton-bone :shape-id "Arm_Lower_Left"
  :parent-shape-id "Arm_Upper_Left"
  :start-position (vec 758.000 256.000)
- :rest-position (vec 913.000 254.000)
- :end-position (vec 919.000 254.000)) (skeleton-bone :shape-id "Hand_Right"
+ :end-position (vec 919.000 254.000)
+ :pose-start-position (vec 758.000 256.000)
+ :pose-end-position (vec 913.000 254.000)) (skeleton-bone :shape-id "Hand_Right"
  :parent-shape-id "Arm_Lower_Right"
- :start-position (vec 95.000 255.000)
- :rest-position (vec 65.000 257.000)
- :end-position (vec 59.000 256.000)) (skeleton-bone :shape-id "Hand_Left"
+ :start-position (vec 96.000 255.000)
+ :end-position (vec 63.000 256.000)
+ :pose-start-position (vec 96.000 255.000)
+ :pose-end-position (vec 63.000 256.000)) (skeleton-bone :shape-id "Hand_Left"
  :parent-shape-id "Arm_Lower_Left"
  :start-position (vec 922.000 255.000)
- :rest-position (vec 997.000 252.000)
- :end-position (vec 963.000 255.000)) (skeleton-bone :shape-id "Waist"
+ :end-position (vec 963.000 255.000)
+ :pose-start-position (vec 922.000 255.000)
+ :pose-end-position (vec 997.000 252.000)) (skeleton-bone :shape-id "Waist"
  :parent-shape-id "Hip"
  :start-position (vec 511.000 449.000)
- :rest-position (vec 515.000 354.000)
- :end-position (vec 515.000 353.000)) (skeleton-bone :shape-id "UpperBody"
+ :end-position (vec 514.000 353.000)
+ :pose-start-position (vec 511.000 449.000)
+ :pose-end-position (vec 515.000 354.000)) (skeleton-bone :shape-id "UpperBody"
  :parent-shape-id "Waist"
- :start-position (vec 511.000 371.000)
- :rest-position (vec 508.000 230.000)
- :end-position (vec 509.000 230.000)) (skeleton-bone :shape-id "Neck"
+ :start-position (vec 507.000 372.000)
+ :end-position (vec 508.000 231.000)
+ :pose-start-position (vec 507.000 372.000)
+ :pose-end-position (vec 508.000 230.000)) (skeleton-bone :shape-id "Neck"
  :parent-shape-id "UpperBody"
  :start-position (vec 509.000 223.000)
- :rest-position (vec 510.000 196.000)
- :end-position (vec 510.000 195.000)) (skeleton-bone :shape-id "Head"
+ :end-position (vec 510.000 195.000)
+ :pose-start-position (vec 509.000 223.000)
+ :pose-end-position (vec 510.000 196.000)) (skeleton-bone :shape-id "Head"
  :parent-shape-id "Neck"
- :start-position (vec 509.000 181.000)
- :rest-position (vec 510.000 16.000)
- :end-position (vec 510.000 16.000)) (skeleton-bone :shape-id "Leg_Upper_Right"
+ :start-position (vec 509.000 177.000)
+ :end-position (vec 510.000 14.000)
+ :pose-start-position (vec 509.000 177.000)
+ :pose-end-position (vec 509.000 14.000)) (skeleton-bone :shape-id "Leg_Upper_Right"
  :parent-shape-id "Hip"
  :start-position (vec 437.000 484.000)
- :rest-position (vec 439.000 737.000)
- :end-position (vec 439.000 738.000)) (skeleton-bone :shape-id "Leg_Upper_Left"
+ :end-position (vec 439.000 738.000)
+ :pose-start-position (vec 437.000 484.000)
+ :pose-end-position (vec 440.000 736.000)) (skeleton-bone :shape-id "Leg_Upper_Left"
  :parent-shape-id "Hip"
  :start-position (vec 579.000 474.000)
- :rest-position (vec 582.000 737.000)
- :end-position (vec 581.000 739.000)) (skeleton-bone :shape-id "Leg_Lower_Left"
+ :end-position (vec 581.000 739.000)
+ :pose-start-position (vec 579.000 474.000)
+ :pose-end-position (vec 582.000 737.000)) (skeleton-bone :shape-id "Leg_Lower_Left"
  :parent-shape-id "Leg_Upper_Left"
  :start-position (vec 578.000 701.000)
- :rest-position (vec 580.000 944.000)
- :end-position (vec 580.000 944.000)) (skeleton-bone :shape-id "Leg_Lower_Right"
+ :end-position (vec 580.000 944.000)
+ :pose-start-position (vec 578.000 701.000)
+ :pose-end-position (vec 580.000 944.000)) (skeleton-bone :shape-id "Leg_Lower_Right"
  :parent-shape-id "Leg_Upper_Right"
  :start-position (vec 439.000 705.000)
- :rest-position (vec 438.000 937.000)
- :end-position (vec 438.000 951.000)) (skeleton-bone :shape-id "Foot_Right"
+ :end-position (vec 438.000 951.000)
+ :pose-start-position (vec 439.000 705.000)
+ :pose-end-position (vec 438.000 937.000)) (skeleton-bone :shape-id "Foot_Right"
  :parent-shape-id "Leg_Lower_Right"
  :start-position (vec 438.000 940.000)
- :rest-position (vec 438.000 1008.000)
- :end-position (vec 438.000 1009.000)) (skeleton-bone :shape-id "Foot_Left"
+ :end-position (vec 438.000 1009.000)
+ :pose-start-position (vec 438.000 940.000)
+ :pose-end-position (vec 438.000 1008.000)) (skeleton-bone :shape-id "Foot_Left"
  :parent-shape-id "Leg_Lower_Left"
  :start-position (vec 579.000 953.000)
- :rest-position (vec 577.000 1002.000)
- :end-position (vec 577.000 1002.000)) (skeleton-bone :shape-id "Hip"
+ :end-position (vec 577.000 1002.000)
+ :pose-start-position (vec 579.000 953.000)
+ :pose-end-position (vec 577.000 1002.000)) (skeleton-bone :shape-id "Hip"
  :parent-shape-id "Hip"
  :start-position (vec 510.000 465.000)
- :rest-position (vec 593.000 465.000)
- :end-position (vec 593.000 464.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 593.000 463.000)
+ :pose-start-position (vec 510.000 465.000)
+ :pose-end-position (vec 593.000 465.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000)) (skeleton-bone :shape-id ""
  :parent-shape-id ""
  :start-position (vec 0.000 0.000)
- :rest-position (vec 0.000 0.000)
- :end-position (vec 0.000 0.000))))
+ :end-position (vec 0.000 0.000)
+ :pose-start-position (vec 0.000 0.000)
+ :pose-end-position (vec 0.000 0.000))))

src/VectorPuppetShow.cake

@@ -30,6 +30,7 @@
 (define-keybind s-view-svg-keybind (array SDL_SCANCODE_F3))
 (define-keybind s-view-puppet-keybind (array SDL_SCANCODE_F4))
 
+(define-keybind s-do-inverse-kinematics-keybind (array SDL_SCANCODE_SPACE))
 (define-keybind s-assigning-parent-keybind (array SDL_SCANCODE_P))
 
 (define-keybind s-toggle-fullscreen-keybind (array SDL_SCANCODE_F11))
@@ -67,8 +68,9 @@
   shape-id (array 64 char)
   parent-shape-id (array 64 char)
   start-position vec2 (override 'vec2)
-  rest-position vec2 (override 'vec2)
-  end-position vec2 (override 'vec2))
+  end-position vec2 (override 'vec2)
+  pose-start-position vec2 (override 'vec2)
+  pose-end-position vec2 (override 'vec2))
 
 (introspect-override-register-handler 'write-s-expr
     (metadata-field-has-tag field "'vec2")
@@ -117,6 +119,12 @@
             "%s" svg-name)
   (snprintf filename-out filename-out-size "%s_skeleton.cakedata" trimmed-svg-name))
 
+(defun-local fixup-skeleton-data (skeleton (addr skeleton-data))
+  (return) ;; Not necessary any more
+  (each-item-addr-in-array (path skeleton > bones) i bone (addr skeleton-bone)
+    ;; (set (path bone > pose-end-position) (path bone > pose-position))
+    (set (path bone > pose-start-position) (path bone > start-position))))
+
 (defun-local load-skeleton-for-svg (skeleton-out (addr skeleton-data)
                                     svg-name (addr (const char)))
   (var filename (array 2048 char) (array 0))
@@ -131,7 +139,8 @@
               file-contents
               malloc
               null)
-       (vpslog "Failed to read skeleton data\n")))
+       (vpslog "Failed to read skeleton data\n"))
+     (fixup-skeleton-data skeleton-out))
     (scope
      (vpslog "Could not find skeleton for SVG %s (expected %s)\n"
              svg-name filename))))
@@ -144,11 +153,13 @@
   (var points-to-render (array point-color)
     (array
      (array (addr (path bone > start-position))
-            (array 255 0 0))
+            (array 155 0 0))
      (array (addr (path bone > end-position))
-            (array 0 255 0))
-     (array (addr (path bone > rest-position))
-            (array 0 0 255))))
+            (array 0 155 0))
+     (array (addr (path bone > pose-start-position))
+            (array 255 0))
+     (array (addr (path bone > pose-end-position))
+            (array 0 255 0))))
   (each-item-addr-in-array points-to-render i point (addr point-color)
     (var point-rect SDL_Rect
       (array (- (type-cast (path point > point > X) int)
@@ -187,11 +198,64 @@
   shape-id hash-string-id
   position vec2)
 
+(var g-debug-renderer (addr SDL_Renderer) null)
+(defun-local debug-render-line (start vec2
+                                end vec2
+                                r (unsigned char) g (unsigned char) b (unsigned char))
+  (unless g-debug-renderer
+    (vpslog "Need to set g-debug-renderer to render vectors\n")
+    (return))
+
+  (SDL_SetRenderDrawColor g-debug-renderer r g b 255)
+  (SDL_RenderDrawLine g-debug-renderer
+                      (type-cast (field start X) int)
+                      (type-cast (field start Y) int)
+                      (type-cast (field end X) int)
+                      (type-cast (field end Y) int)))
+
+(defun-local debug-render-vec2-at-mouse (vec vec2
+                                   r (unsigned char) g (unsigned char) b (unsigned char))
+  (unless g-debug-renderer
+    (vpslog "Need to set g-debug-renderer to render vectors\n")
+    (return))
+
+  (var mouse-x int 0)
+  (var mouse-y int 0)
+  (SDL_GetMouseState (addr mouse-x) (addr mouse-y))
+  (var mouse-vec vec2 (array (type-cast mouse-x float)
+                             (type-cast mouse-y float)))
+  (var target vec2 vec)
+  (set target (vec2-add target mouse-vec))
+
+  (SDL_SetRenderDrawColor g-debug-renderer r g b 255)
+  (SDL_RenderDrawLine g-debug-renderer mouse-x mouse-y
+                      (type-cast (field target X) int)
+                      (type-cast (field target Y) int)))
+
+(defun-local reset-skeleton-pose (skeleton (addr skeleton-data))
+  (each-item-addr-in-array (path skeleton > bones) i bone (addr skeleton-bone)
+    (set (path bone > pose-start-position) (path bone > start-position))
+    (set (path bone > pose-end-position) (path bone > end-position))))
+
 (defun-local do-inverse-kinematics (skeleton (addr skeleton-data)
                                     effectors (addr skeleton-effector)
                                     num-effectors int)
+  ;; TODO: This obviously won't work when it's time to animate
+  (reset-skeleton-pose skeleton)
+  (each-in-range 1 iteration-number
   (each-in-range num-effectors effector-index
     (var root-effector (addr skeleton-effector) (addr (at effector-index effectors)))
+    ;; The root bone's posed end is what we want to move to reach the effector position
+    (var root-bone (addr skeleton-bone) null)
+    ;; As we refine, this is the position we are rotating to track how far we should rotate each bone
+    ;; By only rotating this point, we ensure error doesn't accumulate at each bone
+    (var root-bone-working-end-position vec2 (array 0))
+    (var bone-chain (array 10 (addr skeleton-bone)) (array 0))
+    (var bone-chain-write-head (addr (addr skeleton-bone)) bone-chain)
+    ;; Store rotations of all bones while we work on them. We'll rotate them once we're done. We
+    ;; rotate the end position virtually to avoid error accumulation
+    (var bone-rotations-turns (array 16 float) (array 0))
+    (var bone-rotation-write-head (addr float) bone-rotations-turns)
     (var next-chain-effector skeleton-effector (deref root-effector))
     ;; TODO: This is only to catch loops, not refine the descent
     (var max-iterations (const int) 10)
@@ -205,15 +269,177 @@
                                    (strlen (path bone > shape-id)))
                    (field next-chain-effector shape-id))
           (continue))
-        (vpslog "Do kinematics on %s\n" (string-from-hash-string-id (addr g-string-table)
-                                                                    (field next-chain-effector shape-id)))
+        (unless root-bone
+          (set root-bone bone)
+          (set root-bone-working-end-position (path root-bone > pose-end-position)))
+        (var bone-pose-position vec2
+          (vec2-subtract
+           root-bone-working-end-position
+           (path bone > pose-start-position)))
+        (var effector-target-position vec2
+          (vec2-subtract
+           (path root-effector > position)
+           (path bone > pose-start-position)))
+        (var pose-target-difference vec2
+          (vec2-subtract effector-target-position
+                         bone-pose-position))
+        ;; TODO This doesn't quite solve the nan angles issue
+        (var close-enough-distance (const float) 0.01f)
+        (when (vec2-equals-tolerance bone-pose-position effector-target-position close-enough-distance)
+          ;; (vpslog "close enough!\n")
+          (break))
+        (var root-angle-turns float
+          (get-angle-turns-between-vec2
+           bone-pose-position
+           effector-target-position))
+        (var close-enough-turns float 0.01f)
+        (when (< (fabs root-angle-turns) close-enough-turns)
+          ;; (vpslog "Close enough turns!\n")
+          (break))
+
+        ;; Rotate the working pose position towards the effector
+         (var rotated-end-position vec2 (vec2-subtract root-bone-working-end-position
+                                                       (path bone > pose-start-position)))
+         (var rotation-matrix mat2 (mat2-rotate-turns root-angle-turns))
+         (set root-bone-working-end-position
+              (vec2-add
+               (mat2-transform-vec2 rotation-matrix rotated-end-position)
+               (path bone > pose-start-position)))
+         (set (deref bone-rotation-write-head)
+              (+ (deref bone-rotation-write-head) root-angle-turns))
+         (incr bone-rotation-write-head)
+         (when (>= (- bone-rotation-write-head bone-rotations-turns)
+                   (array-size bone-rotations-turns))
+           (vpslog "Reached the max number of bones to store rotations (%ld)! Exiting early\n"
+                   (array-size bone-rotations-turns))
+           (break))
+
+         ;; (debug-render-line (path bone > pose-start-position) root-bone-working-end-position
+         ;;                    128 64 99)
+
+        ;; (vpslog "Do kinematics on %s to get %s to target by rotating %f degrees\n"
+        ;;         (string-from-hash-string-id (addr g-string-table)
+        ;;                                     (field next-chain-effector shape-id))
+        ;;         (string-from-hash-string-id (addr g-string-table)
+        ;;                                     (path root-effector > shape-id))
+        ;;         (turns-to-degrees root-angle-turns))
+        ;; (debug-render-line (path bone > pose-start-position) (path root-effector > position)
+                           ;; 0 0 255)
+        ;; (debug-render-line (path bone > pose-start-position) (path root-effector > position)
+                           ;; 0 0 255)
+        ;; (scope
+        ;;  (var rotated-end-position vec2 (vec2-subtract (path bone > pose-end-position)
+        ;;                                                (path bone > pose-start-position)))
+        ;;  (var rotation-matrix mat2 (mat2-rotate-turns root-angle-turns))
+        ;;  (set rotated-end-position (vec2-add
+        ;;                             (mat2-transform-vec2 rotation-matrix rotated-end-position)
+        ;;                             (path bone > pose-start-position)))
+        ;;  (debug-render-line (path bone > pose-start-position) rotated-end-position
+        ;;                     128 64 99)
+        ;;  (scope ;; Actually make it happen!
+        ;;   ;; (var old-pose-end-position vec2 (path bone > pose-end-position))
+        ;;   (set (path bone > pose-end-position) rotated-end-position)
+        ;;   ;; Apply the rotation to all children in the chain
+        ;;   (var parent-bone (addr skeleton-bone) bone)
+        ;;   (each-item-in-array bone-chain chain-index child-bone (addr skeleton-bone)
+        ;;     (when (or (= (+ bone-chain chain-index) bone-chain-write-head)
+        ;;               (not child-bone))
+        ;;       (break))
+        ;;     (var child-pose-end-position vec2
+        ;;       (vec2-add
+        ;;        (path parent-bone > pose-start-position)
+        ;;        (mat2-transform-vec2 rotation-matrix
+        ;;                             (vec2-subtract (path child-bone > pose-end-position)
+        ;;                                            (path parent-bone > pose-start-position)))))
+        ;;     (var child-pose-start-position vec2
+        ;;       (vec2-add
+        ;;        (path parent-bone > pose-start-position)
+        ;;        (mat2-transform-vec2 rotation-matrix
+        ;;                             (vec2-subtract (path child-bone > pose-start-position)
+        ;;                                            (path parent-bone > pose-start-position)))))
+        ;;     (var child-end-position vec2
+        ;;       (vec2-add
+        ;;        (path parent-bone > pose-start-position)
+        ;;        (mat2-transform-vec2 rotation-matrix
+        ;;                             (vec2-subtract (path child-bone > pose-end-position)
+        ;;                                            (path parent-bone > pose-start-position)))))
+        ;;     (set (path child-bone > pose-start-position) child-pose-start-position)
+        ;;     (set (path child-bone > pose-end-position) child-end-position)
+        ;;     (debug-render-line (path child-bone > pose-start-position) (path child-bone > pose-end-position)
+        ;;                        128 0 99)
+
+        ;;     (set parent-bone child-bone))))
+        (set (deref bone-chain-write-head) bone)
+        (incr bone-chain-write-head)
+        (when (> (- bone-chain-write-head bone-chain) (array-size bone-chain))
+          (vpslog "Max bone chain size %ld reached\n" (array-size bone-chain))
+          (break))
         (set (field next-chain-effector shape-id) 0)
-        (when (at 0 (path bone > parent-shape-id))
+        (when (and (at 0 (path bone > parent-shape-id))
+                   (!= 0 (strcmp (path bone > parent-shape-id) (path bone > shape-id))))
           (set (field next-chain-effector shape-id)
                (id-from-string (addr g-string-table)
                                (path bone > parent-shape-id)
                                (strlen (path bone > parent-shape-id)))))
-        (break)))))
+        (break))
+      (when (> (- bone-chain-write-head bone-chain) (array-size bone-chain))
+        (break)))
+
+    (each-in-closed-interval-descending (- (array-size bone-rotations-turns) 1) 0 turn-index
+      (var bone-turns float (at turn-index bone-rotations-turns))
+      (unless bone-turns
+        (continue))
+      (vpslog "Rotate bone %s by %f turns\n" (path (at turn-index bone-chain) > shape-id)
+              bone-turns)
+      (var rotation-matrix mat2 (mat2-rotate-turns bone-turns))
+      (var parent-bone (addr skeleton-bone) (at turn-index bone-chain))
+      (var parent-pose-end-position vec2
+        (vec2-add
+         (path parent-bone > pose-start-position)
+         (mat2-transform-vec2 rotation-matrix
+                              (vec2-subtract (path parent-bone > pose-end-position)
+                                             (path parent-bone > pose-start-position)))))
+      (set (path parent-bone > pose-end-position) parent-pose-end-position)
+
+      (var child-bone-index int (- turn-index 1))
+      (unless (>= child-bone-index 0)
+        (break))
+      ;; (set (at child-bone-index bone-rotations-turns)
+      ;;      (+ (at child-bone-index bone-rotations-turns) bone-turns))
+      (var child-bone (addr skeleton-bone) (at child-bone-index bone-chain))
+      ;; (var child-pose-end-position vec2
+      ;;   (vec2-add
+      ;;    (path parent-bone > pose-start-position)
+      ;;    (mat2-transform-vec2
+      ;;     rotation-matrix
+      ;;     (vec2-subtract (path child-bone > pose-end-position)
+      ;;                    (path parent-bone > pose-start-position)))))
+      ;; (var child-pose-start-position vec2
+      ;;   (vec2-add
+      ;;    (path parent-bone > pose-start-position)
+      ;;    (mat2-transform-vec2
+      ;;     rotation-matrix
+      ;;     (vec2-subtract (path child-bone > pose-start-position)
+      ;;                    (path parent-bone > pose-start-position)))))
+
+      ;; Reset the child's pose position, then move it to our new end position. Because we our
+      ;; rotation to the child's rotation, the child will become the parent and rotate itself
+      ;; appropriately. We reset the position because otherwise the relationships between the bones
+      ;; could change if the child is rotated in the space of the pose start position.
+      (var child-pose-end-position vec2
+        (vec2-add
+         (path parent-bone > pose-end-position)
+         (vec2-subtract (path child-bone > end-position)
+                        (path parent-bone > end-position))))
+      (var child-pose-start-position vec2
+        (vec2-add
+         (path parent-bone > pose-end-position)
+         (vec2-subtract (path child-bone > start-position)
+                        (path parent-bone > end-position))))
+      ;; (set (path child-bone > pose-start-position) parent-pose-end-position)
+      (set (path child-bone > pose-start-position) child-pose-start-position)
+      (set (path child-bone > pose-end-position) child-pose-end-position)
+      ))))
 
 (defun main (&return int)
   (hash-crc32-initialize)
@@ -240,6 +466,8 @@
     (return 1))
   (defer (SDL_DestroyRenderer renderer))
 
+  (set g-debug-renderer renderer)
+
   ;;
   ;; Fonts
   ;;
@@ -505,6 +733,8 @@
     (var mouse-y int 0)
     (var mouse-button-state Uint32
       (SDL_GetMouseState (addr mouse-x) (addr mouse-y)))
+    (var mouse-vec vec2 (array (type-cast mouse-x float)
+                               (type-cast mouse-y float)))
 
     (when (and view-atlas puppet-atlas-texture)
       (var atlas-rectangle SDL_Rect
@@ -615,14 +845,16 @@
       hovered-shape-id-buffer))
 
     (when packed-rectangles
-      (var effector skeleton-effector (array 0))
-      (set-fields effector
-        position (path selected-bone > end-position))
-      (var effector-target (addr (const char)) "Hand_Right")
-      (set (field effector shape-id) (id-from-string (addr g-string-table)
-                                                     effector-target
-                                                     (strlen effector-target)))
-      (do-inverse-kinematics (addr skeleton) (addr effector) 1)
+      (when (or true (keybind-tapped (addr s-do-inverse-kinematics-keybind) (addr s-key-states)))
+        (var effector skeleton-effector (array 0))
+        (var effector-target (addr (const char)) "Hand_Right")
+        (var hard-coded-target vec2 (array 297 463))
+        (set-fields effector
+          position mouse-vec ;; hard-coded-target
+          shape-id (id-from-string (addr g-string-table)
+                                   effector-target
+                                   (strlen effector-target)))
+        (do-inverse-kinematics (addr skeleton) (addr effector) 1))
       (var current-rectangle (addr stbrp_rect) packed-rectangles)
       (each-shape-in-svg-image puppet-image shape
         (unless (bit-and (path shape > flags) NSVG_FLAGS_VISIBLE)
@@ -645,45 +877,46 @@
           (SDL_RenderDrawRect renderer (addr draw-rectangle)))
 
         (when view-puppet
+          (var this-bone (addr skeleton-bone) selected-bone)
+          (each-item-addr-in-array (field skeleton bones) i bone (addr skeleton-bone)
+            (unless (= 0 (strcmp (path bone > shape-id)
+                                 (path shape > id)))
+              (continue))
+            (set this-bone bone)
+            (break))
           ;; We are ever so slightly off due to this, but it's pretty close
           (var sized-bounds (array 4 float) (array 0))
           (nanosvg-get-shape-bounds-with-stroke-float shape sized-bounds)
+          (var pose-position-delta vec2
+            (vec2-subtract (path this-bone > pose-start-position)
+                           (path this-bone > start-position)))
           (var to-rectangle SDL_Rect
-            (array (type-cast (at 0 sized-bounds) int)
-                   (type-cast (at 1 sized-bounds) int)
+            (array (type-cast (+ (at 0 sized-bounds) (field pose-position-delta X)) int)
+                   (type-cast (+ (at 1 sized-bounds) (field pose-position-delta Y)) int)
                    (path current-rectangle > w)
                    (path current-rectangle > h)))
           (var rest-vector vec2
-            (vec2-normalize (vec2-subtract (path selected-bone > end-position)
-                                           (path selected-bone > start-position))))
+            (vec2-normalize (vec2-subtract (path this-bone > end-position)
+                                           (path this-bone > start-position))))
           (var point-at-vector vec2
-            (vec2-normalize (vec2-subtract (path selected-bone > rest-position)
-                                           (path selected-bone > start-position))))
-          (var angle float
-            (acosf (vec2-dot
-                    rest-vector
-                    point-at-vector)))
-          ;; We need to find the perpendicular vector to know which way we need to rotate
-          (var perpendicular vec2 rest-vector)
-          ;; In 2D we can find the perpendicular vector by swapping the axes and negating one
-          (var temp float (field perpendicular X))
-          (set (field perpendicular X) (field perpendicular Y))
-          (set (field perpendicular Y) (negate temp))
-          (var sign float (vec2-dot point-at-vector perpendicular))
-          (set angle (radians-to-degrees angle))
-          (when (> sign 0.f)
-            (set angle (negate angle)))
-         (var rotate-center SDL_Point
-           (array
-            (type-cast (- (path selected-bone > start-position . X) (at 0 sized-bounds)) int)
-            (type-cast (- (path selected-bone > start-position . Y) (at 1 sized-bounds)) int)))
-         (unless (= 0 (SDL_RenderCopyEx renderer puppet-atlas-texture
-                                        (addr draw-rectangle) (addr to-rectangle)
-                                        (? (= shape selected-shape) angle 0.f)
-                                        (addr rotate-center)
-                                        SDL_FLIP_NONE))
-           (sdl-print-error)
-           (set exit-reason "Failed to render atlas texture")))
+            (vec2-normalize (vec2-subtract (path this-bone > pose-end-position)
+                                           (path this-bone > pose-start-position))))
+          (var angle-turns float (get-angle-turns-between-vec2 rest-vector point-at-vector))
+          ;; (vpslog "Rotate %s %f and move %f %f (before difference: %f %f %f %f)\n" (path this-bone > shape-id) angle-turns
+          ;;         (vec-xy pose-position-delta)
+          ;;         (vec-xy (path this-bone > start-position))
+          ;;         (vec-xy (path this-bone > pose-start-position)))
+          (var rotate-center SDL_Point
+            (array
+             (type-cast (- (path this-bone > start-position . X) (at 0 sized-bounds)) int)
+             (type-cast (- (path this-bone > start-position . Y) (at 1 sized-bounds)) int)))
+          (unless (= 0 (SDL_RenderCopyEx renderer puppet-atlas-texture
+                                         (addr draw-rectangle) (addr to-rectangle)
+                                         (turns-to-degrees angle-turns)
+                                         (addr rotate-center)
+                                         SDL_FLIP_NONE))
+            (sdl-print-error)
+            (set exit-reason "Failed to render atlas texture")))
         (incr current-rectangle)))
 
     (cond
@@ -692,8 +925,8 @@
                 (at SDL_SCANCODE_RCTRL (field s-key-states this-frame-states))))
        (set-fields (deref selected-bone)
          shape selected-shape
-         (rest-position X) mouse-x
-         (rest-position Y) mouse-y))
+         (pose-end-position X) mouse-x
+         (pose-end-position Y) mouse-y))
       ((bit-and mouse-button-state SDL_BUTTON_MMASK)
        (set-fields (deref selected-bone)
          shape selected-shape
@@ -741,16 +974,16 @@
     (when exit-reason
       (break)))
 
-  (scope ;; Save edited skeleton
-   (var filename (array 2048 char) (array 0))
-   (get-skeleton-filename-from-svg-filename svg-filename filename (sizeof filename))
-   (if-open-file-scoped filename "wb" out-file
-     (write-introspect-struct-s-expr
-      skeleton-data--metadata
-      (addr skeleton)
-      write-introspect-struct-file-writer out-file
-      write-introspect-struct-add-newline)
-     (vpslog "Failed to write skeleton to %s\n" filename)))
+  ;; (scope ;; Save edited skeleton
+  ;;  (var filename (array 2048 char) (array 0))
+  ;;  (get-skeleton-filename-from-svg-filename svg-filename filename (sizeof filename))
+  ;;  (if-open-file-scoped filename "wb" out-file
+  ;;    (write-introspect-struct-s-expr
+  ;;     skeleton-data--metadata
+  ;;     (addr skeleton)
+  ;;     write-introspect-struct-file-writer out-file
+  ;;     write-introspect-struct-add-newline)
+  ;;    (vpslog "Failed to write skeleton to %s\n" filename)))
 
   (when exit-reason
     (vpslog "Exited reason: %s\n" exit-reason))