IK

//Jacobian Transpose 2D Inverse Kinematics demo
//PipeDream
//7/27/2007
//Public domain

library IK
    struct vector2
        real x
        real y
    
        static method CreateXY takes real x, real y returns vector2
            local vector2 v = vector2.create()
            set v.x = x
            set v.y = y
            return v
        endmethod
    endstruct

    globals
        real endx
        real endy
        real enddx
        real enddy
    endglobals

    struct link
        real x0
        real y0
        real r
        real a
        real a_update
    
        lightning zap
    
        link next
    
        static method Create takes real x0, real y0, real r, real a, link next returns link
            local link l = link.create()
            set l.x0 = x0
            set l.y0 = y0
            set l.r = r
            set    l.a = a
            set l.zap = AddLightningEx("DRAL",true,x0,y0,0.,x0+r*Cos(a),y0+r*Sin(a),0.)
            set l.next = next
            return l
        endmethod
        
        method FK takes real x, real y returns nothing
            local real x1
            local real y1
            set .x0 = x
            set .y0 = y            
            
            set x1 = x + .r*Cos(.a)
            set y1 = y + .r*Sin(.a)
            
            call MoveLightningEx(.zap,true,x,y,0.,x1,y1,0.)

            if .next != 0 then
                call .next.FK(x1,y1)
            endif
        endmethod
    
        method IK takes real tx, real ty, real h returns nothing
            local real rcosa
            local real rsina
            local real r
            local real dx
            local real dy
            if .next == 0 then

                set rcosa = .r*Cos(.a)
                set rsina = .r*Sin(.a)
                
                set endx = .x0 + rcosa
                set endy = .y0 + rsina

                set enddx = tx - endx
                set enddy = ty - endy
                    
            else
                call .next.IK(tx,ty,h)
            
                set dx = endx - .x0
                set dy = endy - .y0
                set r = SquareRoot(dx*dx + dy*dy)
                
                set rcosa = r*Cos(.a)
                set rsina = r*Sin(.a)
            endif
            set .a = .a + h*(rcosa*enddy - rsina*enddx)
        endmethod
    endstruct
endlibrary

globals
    link chain
    integer n = 0
    real dt = 0.03
    unit ub
    unit ue
    real h = 0.000025
endglobals

function test_periodic takes nothing returns nothing
    local real x0 = 0.
    local real y0 = 500*Sin(0.3*dt*n)
    local real x1 = 500.*Cos(1.0*dt*n)
    local real y1 = 500.*Sin(1.0*dt*n)

    call chain.IK(x1,y1,h*dt)            // Reach towards target
    call chain.FK(x0,y0)                    // Set base position
    call SetUnitX(ub,x0)
    call SetUnitY(ub,y0)
    call SetUnitX(ue,x1)
    call SetUnitY(ue,y1)
    
    set n = n + 1
endfunction

function test takes nothing returns nothing
    local integer i
    
    call BJDebugMsg("Testing")
    
    set ub = CreateUnit(Player(0),'hfoo',0.,0.,0.)
    set ue = CreateUnit(Player(0),'hfoo',0.,0.,0.)

    set i = 0
    set chain = 0
    loop
        exitwhen i >= 10
        set chain = link.Create(0.,0.,80.,GetRandomReal(-0.5,0.5),chain)
        set i = i + 1
    endloop
    
    call BJDebugMsg("Lightning created")

    call chain.FK(0.,0.)
    
    call BJDebugMsg("Running IK loop")
    
    call TimerStart(CreateTimer(),dt,true,function test_periodic)
endfunction