Hilbert Circle using inverse kinematics

robo_hilbert.jl

using Colors
using Javis
using HilbertSpaceFillingCurve

mutable struct Segment 
    start :: Point 
    angle :: Float64
    length :: Float64
    head :: Point
end

struct ColoredPoint
    p :: Point 
    c :: HSL{Float64}
end

function get_color(arm::Vector{Segment})
    an = [(seg.angle+π)/2π for seg in arm]
    return HSL(an[1]*360, an[2], an[3]*0.8+0.2)
end

function Segment(start, angle, length) 
    s = Segment(start, angle, length, O)
    sethead!(s)
    return s
end

function setstart!(seg::Segment)
    dx = cos(seg.angle)*seg.length
    dy = sin(seg.angle)*seg.length
    seg.start = seg.head - Point(dx, dy)
end

function sethead!(seg::Segment)
    dx = cos(seg.angle)*seg.length
    dy = sin(seg.angle)*seg.length
    seg.head = seg.start + Point(dx, dy)
end

function draw_arm(segs::Vector{Segment}, p::Point, points)
    @JShape begin
        reach!(segs, p)
        setline(3)
        for seg in segs 
            line(seg.start, seg.head, :stroke)
        end
        push!(points, ColoredPoint(segs[end].head, get_color(segs)))
        for point in points
            sethue(point.c)
            circle(point.p, 5, :fill)
        end
    end p=p
end

function reach!(arm::Vector{Segment}, p::Point)
    end_point = p 
    locked_point = arm[1].start
    loop = false
    if distance(locked_point, end_point) <= sum(s->s.length, arm)
        loop = true
    end
    loop_counter = 0
    while loop_counter == 0 || loop
        p = end_point
        for i in length(arm):-1:1
            reach!(arm[i], p)
            p = arm[i].start
        end
        # move the arm back 
        arm[1].start = locked_point
        sethead!(arm[1])
        for i in 2:length(arm)
            arm[i].start = arm[i-1].head
            sethead!(arm[i])
        end

        # check such that we don't get into an infinite loop or are already close enough
        loop_counter += 1
        if loop_counter == 10 || distance(end_point, arm[end].head) <= 0.1
            break
        end
    end
end

function reach!(seg::Segment, p::Point)
    sethead!(seg)
    seg.angle = atan(p.y-seg.start.y, p.x-seg.start.x)
    seg.head = p
    setstart!(seg)
end

function ground(args...)
    background("black")
    sethue("white")
end

function main()
    vid = Video(512, 512)

    nframes = 2000
    stepsize = 131
    Background(1:nframes, ground)

    ps = Vector{Point}()
    for i in 0:nframes-1
        h = hilbert(i*stepsize, 2, 9)
        push!(ps, Point(h[1],h[2])-Point(vid.width/2, vid.height/2))
    end
    p = ps[1]

    segs = [Segment(O, 0, 100), Segment(Point(100,0), 0, 75), Segment(Point(175,0), 0, 50)]
    points = ColoredPoint[]
    arm_obj = Object(1:nframes, draw_arm(segs, p, points))

    anim_p = Animation(
        [i/(length(ps)-1) for i in 0:length(ps)-1], # must go from 0 to 1
        ps,
        [linear() for _ in 1:length(ps)-1],
    )

    act!(arm_obj, Action(1:nframes, anim_p, change(:p)))

    render(vid; pathname="hilbert.gif")
end