Связь между твёрдыми телами: шарнирная петля

[vanger]

Пусть есть два твёрдых тела, положение которых в пространстве задаётся координатами центров масс и единичными кватернионами, задающими поворот ортонормированного базиса в неподвижной ИСО (будем называть её мировой системой координат и обозначим W) в базис, жёстко связанный с телом (так и будем называть его базис тела A / базис тела B; обозначать будем A'/B'). Так что если $\vec v$ -- координатный столбец вектора в W, то его координатный столбец в A' -- $q_a (0, \vec v) \bar q_b$. Скорости пусть задаются координатами вектора скорости центра масс в W и координатами вектора угловой скорости в A'/B'. Я хочу ограничить относительное движение тел так, чтобы они могли свободно (и, для простоты, неограниченно) вращаться относительно общей оси.

(Как на картинке.)

Как выглядят правильные функции связей (функции, обращающиеся в ноль тогда и только тогда, когда координаты удовлетворяют связям (в идеале; на крайний случай -- в некоторой области корфигурационного пространства))? Сосредоточимся на связи, ограничивающей вращения, т.к. трансляционная тривиальна.

Если бы ось вращения была бы коллинеарна оси Z в A', то всё было бы просто: кватернион "относительной ориентации" $q = \bar q_a q_b = q_w + q_x i + q_y j + q_z k$ должен иметь мнимую часть только "вдоль третьей компоненты": $q_x = q_y = 0$.

Но общий случай можно свести к нему, подправив $q$ следующим образом: $q = \bar \alpha \bar q_a q_b \beta$, где $\alpha$ -- какой-нибудь кватернион, описывающий вращение вектора с координатами $(0,0,1)$ в $\vec v_a$ -- столбец с координатами единичного направляющего вектора оси вращения в системе координат A'; $\beta$ -- аналогично. $\alpha$ и $\beta$ постоянны. Не ошибаюсь ли я где-нибудь?

Хочется связей именно такого вида -- непосредственно на кватернионы, а не что-то типа $\vec f_{a1} \cdot \vec v_a = \vec f_{a2} \cdot \vec v_a = 0$ для тела A и аналогично для B, где $f_{a1}, f_{a2}$ -- неколлинеарные орты, нормальные к $\vec v_a$, жёство связанные с телом A. С этим можно жить, но такой подход имеет недостатки. Во-первых, информация об ориентирующих кватернионах используется неявно, а во-вторых, такие уравнения связей выполнены и тогда, когда связь нарушена: если одно из тел повернуть на $180$ градусов "в запретном направлении" (вокруг $\vec f_{a1}$, например): $\vec f_{a1} \cdot \vec v_a = -\vec f_{a2} \cdot \vec v_a = 0$. Что неприятно как само по себе, так и потому, что при численном решении уравнений (да, весь сыр-бор ради этого), лагранжевой системы со связями, сходимость в последнем (бистабильном) случае явно будет хуже, т.к. "возвращающая сила" (пропорциональная множителям Лагранжа), по идее, будет слабеть, при приближении системы к точке поворота на $90$ градусов в запретном направлении, обращаясь в ноль при нём (такая прикидка: при повороте на $1$ градус возвращающая сила в одну сторону вращает, при повороте на $180-1$ -- в другую; непрерывная функция принимающая значения разных знаков на концах отрезка где-то зануляется -- где это делать, как не посередине).