@name IK Test 2
@inputs Foot:entity
@persist E:entity Hip:entity L1 L2

if (first())
{
    L1 = 50
    L2 = 50
    
    function number cosine(A, B, C)
    {
        return acos((A^2 + B^2 - C^2) / (2*A*B))
    }
    function ik(L1, L2, Hip:entity, Foot:entity, Chassis:entity, I1, I2)
    {    
        local Axis = Chassis:toLocalAxis(Foot:pos() - Hip:pos())
        local Angle = Axis:toAngle():setRoll(-bearing(Hip:pos(), Chassis:angles(), Foot:pos()))
        local Dist = min(Axis:length(), L1 + L2)
        local Quat = quat(Angle) * qRotation(vec(0, 90 + cosine(Dist, L1, L2), 0))
        
        holoAng(I1, Chassis:toWorld(Quat:toAngle()))
        holoAng(I2, Hip:toWorld(ang(cosine(L2, L1, Dist) + 180, 0, 0)))
    }

    Hip = holoCreate(1, entity():toWorld(vec(-15, 20, 0)))
    holoCreate(2, holoEntity(1):toWorld(vec(0, 0, L1)))
    holoCreate(3, holoEntity(2):toWorld(vec(0, 0, L2)))

    holoParent(1, entity())
    holoParent(2, 1)
    holoParent(3, 2)
}

interval(100)

ik(L1, L2, Hip, Foot, entity(), 1, 2)
