Я и имею в виду по скорости.
Да какие там матричные преобразования? Всё тривиально. У нас же не 6-звенный манипулятор.
Конечная точка однозначно определяет положение серв.
1. Угол ноги в плоскости поверхности однозначно определяет положение сервы в основании;
2. Вычисляем из этого положение точки крепления второй сервы.
3. Из угла между точкой вращения второй сервой и конечной точкой вычисляем положение 3-й сервы.
4. Вычисляем положение 2-й сервы.
Готово.
Добавлено спустя 16 секунд:
Табличные тригонометрические функции завести и всё.