@ -314,61 +314,6 @@
( 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 ) )
@ -404,42 +349,30 @@
( var child-bone-index int ( - turn-index 1 ) )
( unless ( >= child-bone-index 0 )
( break ) )
;; This makes the end effector get closer sometimes, but in other cases it gets further
;; (set (at child-bone-index bone-rotations-turns)
;; (+ (at child-bone-index bone-rotations-turns) bone-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 ) ) ) )
;; Rotate the offset between the parent's end position and the child's start position, then
;; reposition the child's start (with now rotated offset) to the rotated parent's end pose
( 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)
( mat2-transform-vec2
rotation-matrix
( vec2-subtract ( path child-bone > start-position )
( path parent-bone > end-position ) ) ) ) )
;; Keep the exact relationship between start and end for the child bone (bone length and
;; angle at rest). It will rotate itself next loop when it becomes the parent bone.
( var child-pose-end-position vec2
( vec2-add
child-pose-start-position
( vec2-subtract ( path child-bone > end-position )
( path child-bone > start-position ) ) ) )
( set ( path child-bone > pose-start-position ) child-pose-start-position )
( set ( path child-bone > pose-end-position ) child-pose-end-position )
) ) ) )
( set ( path child-bone > pose-end-position ) child-pose-end-position ) ) ) ) )
( defun main ( &return int )
( hash-crc32-initialize )