FABRIK implementation in C