Biped leg code

# Fully functional biped demo

@name HoloLeg Mk2 Proto
@inputs [Hip Foot]:entity Hippos1:vector
@outputs [CorAngle HypeAngle]:angle Out3
@persist Hippos:vector
runOnTick(1)
if(->Hippos1){
Hippos = Hippos1
}else{
Hippos = vec(0,0,0)
}

Thighlength = 4*12
Shinlength = 4*12

PhxMetallic = "Phoenix_storms/mat/mat_phx_metallic"
Grey = vec(1,1,1)*50

function holoData (Index,Model:string,Scale:vector,Pos:vector,Ang:angle,Material:string,Color:vector,Parent:entity){
    holoCreate(Index)
    holoModel(Index,Model)
    holoScale(Index,Scale)
    holoPos(Index,Pos)
    holoAng(Index,Ang)
    holoMaterial(Index,Material)
    holoColor(Index,Color)
    holoParent(Index,Parent)

}


function number angconverter (Ang1,Ang2){
    if((Ang1 - Ang2) > 180){Out1b = -(360 - (Ang1))}else{Out1b = Ang1}
    if((Ang2 - Ang1) > 180){Out2b = -(360 - (Ang2))}else{Out2b = Ang2}
    Out3 = Out2b - Out1b
    return Out3
}


if(first()){
    holoData(1,"cube",vec(1,1,1)*2,Hip:toWorld(vec(0,20,-20)),Hip:angles(),PhxMetallic,Grey,Hip) # Hip joint
    holoData(2,"cube",vec(1,1,1)*2,Hip:toWorld(vec(0,50,0)),Hip:angles(),PhxMetallic,vec(50,255,50),Hip) # hypotenuse
    holoData(3,"cube",vec(1,1,1)*2,holoEntity(2):toWorld(vec(0,50,0)),Hip:angles(),PhxMetallic,vec(255,50,50),holoEntity(2)) # rotated hypotenuse

    holoData(4,"cube",vec(Thighlength/12,1,1),holoEntity(1):toWorld(vec(Thighlength/2,0,0)),Hip:angles(),PhxMetallic,Grey,holoEntity(1)) # thigh decor
    holoData(5,"cube",vec(1,1,1)*1.25,holoEntity(1):toWorld(vec(Thighlength,0,0)),holoEntity(1):angles(),PhxMetallic,Grey,holoEntity(1)) # knee joint
    holoData(6,"cube",vec(Shinlength/12,1,1),holoEntity(5):toWorld(vec(-Shinlength/2,0,0)),holoEntity(5):angles(),PhxMetallic,Grey,holoEntity(5)) # shin decor
}

Avrgpoint = (Hip:toWorld(Hippos) + Foot:pos())/2
Hypelength = clamp((Hip:toWorld(Hippos) - Foot:pos()):length(),0,Thighlength+Shinlength)
HypeAngle = (Hip:toWorld(Hippos) - Foot:pos()):toAngle() + ang(90,0,0)

Out3 = angconverter(Hip:angles():yaw(),HypeAngle:yaw())

CorAngle = HypeAngle - ang(0,Out3,0)
holoScale(1,vec(1,1,1)*1.5)

holoPos(2,Avrgpoint)
holoScale(2,vec(0.25,1,Hypelength/12)*1)
holoAng(2,HypeAngle)

holoPos(3,Avrgpoint)
holoScale(3,vec(1,0.25,Hypelength/15)*1)
holoAng(3,holoEntity(2):toWorld(ang(0,-Out3,0)))

holoPos(1,holoEntity(3):toWorld(vec(0,0,Hypelength/2)))

HipAng = acos((Thighlength^2 + Hypelength^2 - Shinlength^2)/(2*Thighlength*Hypelength)) - 90
holoAng(1,holoEntity(3):toWorld(ang(-HipAng,0,0)))
KneeAng = acos((Thighlength^2 + Shinlength^2 - Hypelength^2)/(2*Thighlength*Shinlength))
holoAng(5,holoEntity(1):toWorld(ang(-KneeAng,0,0)))