@name Yiffe IK
@inputs [Chassis Foot]:entity
interval(100)
if(first())
{
    runOnLast(1)
    
    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)))
    }
    
    holoCreate(1, Chassis:toWorld(vec()), vec(0.25), Chassis:toWorld(ang()))
    holoCreate(5, holoEntity(1):pos(), vec(0.25), Chassis:toWorld(ang()))
    holoCreate(2, Chassis:toWorld(vec(15, 0, 0)), vec(0.25), Chassis:toWorld(ang())) #This holo will serve as the hip for the bit of IK
    
    holoAlpha(1, 100)
    
    holoParent(1, Chassis)
    holoParent(5, 1)
    holoParent(2, 1)
    
    holoCreate(3, holoEntity(2):toWorld(vec(0, 0, 5)), vec(0.25), Chassis:toWorld(ang()))
    holoCreate(4, holoEntity(3):toWorld(vec(0, 0, 15)), vec(0.25), Chassis:toWorld(ang()))
    
    holoParent(3, 2)
    holoParent(4, 3)
}

holoAng(1, (Foot:pos()-holoEntity(1):pos()):toAngle())
ik(5, 15, holoEntity(2), Foot, Chassis, 2, 3)
holoAng(5, Chassis:toWorld(quat(((Chassis:toLocalAxis(Foot:pos() - holoEntity(2):pos())):toAngle():setRoll(-bearing(holoEntity(2):pos(), Chassis:angles(), Foot:pos())))):toAngle()))
