Вывод якобиана связи на примере бол-сокет соединения между твердым и гибким телом

как вывести якобиан, пример для болл-сокета гибкого и твердого тел

В этой статейке я приведу пример вычислений, необходимых для получения якобиана некоторой связи.

Часто возникает желание использовать какую-либо нестандартную связь. Для того, чтобы добавить в свой движок новую связь, в первую очередь надо вычислить ее якобиан. Это может остановить многих желающих, но на самом деле это достаточно простая задача. По определению, якобиан - это матрица производных уравнений связи по пространственным координатам. На практике, я советую вычислять производную уравнений связи по времени, а потом выносить скорости, то есть производные пространственных координат по времени. То, что останется - как раз производная уравнений по координатам, т.е. якобиан.

Рассмотрим пример болл-сокета, связывающего твердое и гибкое тело. Твердое тело представлено обычным образом - координаты центра масс, кватернион, задающий ориентацию в пространстве, линейная и угловая скорости, и т. д. Гибкое тело представлено своими вершинами, относительное положение которых может меняться каждый кадр. Если ваш солвер способен совместно рассчитывать динамику твердых тел и скрепленных точек, то вы можете задать связь между гибким и твердым телом.

Вначале необходимо определиться с уравнением связи. Уравнение связи должно зависеть от координат связываемых узлов (тел или точек), и в случае, если связь не нарушена, должно равняться нулю. Количество уравнений для одной связи соответствует числу степеней свободы, которые данная связь устраняет из системы. В случае болл-сокета, это число равняется трем. Для начала заметим, что болл-сокет можно задать простым равенством глобальных координат двух точек, зафиксированных в локальных координатах двух тел: p0_wcs - p1_wcs = 0 (векторно). Проблема состоит в выражении этих глобальных координат через зафиксированные локальные и изменяющиеся координаты самих тел. Для твердого тела это выражение хорошо известно: p0_wcs = pos0 + Rot0 * p0_local, где pos0 - глобальные координаты тела, а Rot0 - матрица вращения, задающая его ориентацию. Для болл-сокета, связывающего два твердых тела, уравнение связи будет pos0 + Rot0 * p0_local - pos1 - Rot1 * p1_local = 0. Для гибкого тела все несколько сложнее. Все его вершины могут сдвигаться независимо, и возникает вопрос, за какие вершины цеплять гибкое тело. Можно просто использовать координаты одной из его вершин, но нам хотелось бы иметь большую свободу в выборе точки крепления. Любую точку на поверхности гибкого тела можно задать интерполяцией трех вершин, образующих ту грань, на которой находится точка - достаточно найти барицентрические координаты этой точки в этом треугольнике. Однако мы хотели бы иметь возможность выбирать точку крепления и над поверхностью гибкого тела. Представляется логичным выразить высоту точки над гранью через нормаль этой грани. Итак, точка гибкого тела будет задаваться так: p0_wcs = posA * alfa + posB * beta + posC * gamma + normABC * delta, где alfa, beta, gamma - барицентрические координаты проекции точки крепления на поверхность гибкого тела в треугольнике грани ABC, в которую эта проекция попадает, delta - высота точки крепления над своей проекцией на поверхность объекта по направлению нормали треугольника ABC; posA, posB, posC - координаты вершин A, B, C; normABC - нормаль грани ABC (зависящая от координат вершин A, B, C). Уравнение связи болл-сокет между твердым и гибким телом будет выглядеть так: posA * alfa + posB * beta + posC * gamma + normABC * delta - pos1 - Rot1 * p1_local = 0.

Далее приступим к дифференцированию этого выражения по времени. Вполне очевидно, что часть, зависимая от координат и ориентации твердого тела будет дифференцироваться аналогично случаю обыкновенного болл-сокета, и соответствующая часть производной будет равняться следующему:
(0; -p1_local.z; p1_local.y).dot(angular_vel1) + (-1; 0; 0).dot(linear_vel1)
(p1_local.z; 0; -p1_local.x).dot(angular_vel1) + (0; -1; 0).dot(linear_vel1)
(-p1_local.y; p1_local.x; 0).dot(angular_vel1) + (0; 0; -1).dot(linear_vel1)

Что касается производной выражения posA * alfa + posB * beta + posC * gamma, то она просто равна velA * alfa + velB * beta + velC * gamma (векторно). Таким образом, основная сложность состоит в вычислении производной нормали.

Вспомним в первую очередь, что нормаль можно представить нормализованным векторным произведением ребер треугольника. Условимся здесь и далее, что мы работаем в правосторонней системе координат, а вершины треугольника нумерованы в таком порядке, что на лицевой стороне грани последовательные вершины идут против часовой стрелки . Тогда normABC = ((B-A) x (C-A))/sqrt(((B-A) x (C-A)).dot((B-A) x (C-A))). Это выражение предстоит дифференцировать. Вначале заметим, что [(B - A)x(C-A)]' = (B-A)x(velC-velA) + (velB-velA)x(C-A) = (B-A)x velC + (A-C)x velB +(C-B)x velA. Далее, обозначим n = (B - A)x(C-A) и, соотвественно, |n| = sqrt(((B-A) x (C-A)).dot((B-A) x (C-A))); тогда выражение нормали принимает вид normABC = n/|n|. Вспоминая формулу производной частного, получим (n/|n|)' = n'/|n| - n * |n|' / |n|^2. Будем считать это выражение почленно - обозначим (I): n'/|n| и (II): n * |n|' / |n|^2. Учитывая выведенную выше формулу производной векторного произведения, (I) будет равно просто-напросто ((B-A)/ |n|)x velC + ((A-C)/ |n|) x velB +((C-B)/ |n|)x velA. Для вычисления второй части выражения выведем вначале |n|' = [sqrt(n dot n)]' = (0.5 / sqrt(n dot n)) * 2 (n dot n') = (n dot n') / sqrt(n dot n). Тогда n * |n|' / |n|^2 = n * (n dot n') / |n|^3. Из этого выражения нам надо вытянуть скорости вершин ABC. Для этого, пользуясь выведенной выше формулой производной векторного произведения, вычислим n dot n' = n dot[ (B-A)x velC] + n dot[ (A-C)x velB] +n dot[ (C-B)x velA] = velC dot [n x (B-A)] + velB dot [n x (A -C)] + velA dot [n x (C-B)] (по формуле тройного произведения). Дальше полезно вспомнить, что скалярное произведение можно заменить на матричное произведение, если один из участвующих векторов транспонировать. Применяя это преобразование к последнему выражению, получаем [n x (B-A)]^t * velC + [n x (A -C)]^t * velB + [n x (C-B)]^t * velA. Тогда мы получаем n * (n dot n') / |n|^3 =(n/|n|^3) * [n x (B-A)]^t * velC +(n/|n|^3) * [n x (A -C)]^t * velB + (n/|n|^3) * [n x (C-B)]^t * velA. Поскольку формула становится чрезмерно громоздкой, целесообразно ввести дополнительные обозначения, чтобы сократить запись. В последней формуле можно заметить три матрицы, получающиеся от произведения двух векторов на манер матриц ковариации; обозначим их CovC = (n/|n|^3) * [n x (B-A)]^t ; CovB = (n/|n|^3) * [n x (A -C)]^t ; CovA = (n/|n|^3) * [n x (C-B)]^t. Тогда последнее выражение записывается как CovC * velC + CovB * velB + CovA * velA. Введем также сокращенные обозначения для наших результатов для выражения (I). Для этого нам понадобится определить особое преобразование вектора в матрицу; результатом такого преобразования для некого вектора v будет такая матрица, что при умножении на нее справа другого вектора w результат будет равен векторному произведению v x w. Такая матрица задается от координат вектора XYZ следующим образом:
VecToMatCP(XYZ) =
0 -Z Y
Z 0 -X
-Y X 0

Используя это преобразование, введем матрицы CpA = VecToMatCP((C-B)/ |n|); CpB = VecToMatCP((A-C)/ |n|) ; CpC = VecToMatCP((B-A)/ |n|). Тогда выражение (I) можно переписать как CpA * velA + CpB * velB + CpC * velC. Используя новые обозначения, сгруппируем результаты для выражений (I), (II): normABC' = (CpA-CovA) * velA + (CpB - CovB) * velB + (CpC - CovC) * velC. Используя этот результат, легко получить (delta * normABC)' = delta * (CpA-CovA) * velA + delta * (CpB - CovB) * velB + delta * (CpC - CovC) * velC. Тогда [ posA * alfa + posB * beta + posC * gamma + normABC * delta]' = [alfa * I + delta * (CpA-CovA)] * velA + [beta * I + delta * (CpB-CovB)] * velB + [gamma * I + delta * (CpC-CovC)] * velC, где I - единичная матрица.

Выражение производной для всего уравнения связи тогда будет равно [alfa * I + delta * (CpA-CovA)] * velA + [beta * I + delta * (CpB-CovB)] * velB + [gamma * I + delta * (CpC-CovC)] * velC + VecToMatCP(p1_local) * angular_vel1 - I * linear_vel1. Это выражение легко разделятся на производную связи по координатам (тот самый якобиан!) и производную координат по времени (скорости). Окончательно получим J = {[alfa * I + delta * (CpA-CovA)] , [beta * I + delta * (CpB-CovB)], [gamma * I + delta * (CpC-CovC)], [VecToMatCP(p1_local)], [-I] }, где в квадратных скобках дани блоки размерностью 3 на 3 и порядок связи таков: точки A, B, C, вращательная часть твердого тела, линейная часть твердого тела.

21 марта 2010