ported matlab IK function