Решение обратной задачи кинематики

Имеется кинематическая схема механизма. 6 степеней свободы. 5 степеней с ограничениями.
Необходимо алгоритмическое решение обратной задачи кинематики желательно на языке Си.