ARBD. Шаг1. Двунаправленные ограничения [WIP]

Двунаправленные ограничения

Итак, простейшее движение твердых тел у нас есть. Они могут свободно летать, падать под действием гравитации, никак не взаимодействуя между собой. Но это слишком просто и неинтересно. Простейшее взаимодействие между телами можно описать задав так называемое двустороннее ограничение между ними.
Свободно двигающееся тело имеет 6 степеней свободы ( degree of freedom, линейное движение по каждой из трех осей + вращательное движение по трем осям, итого 6 ). Двунаправленное ограничение убирает одну или более степени свободы у тел, на которых оно действует. Двунаправленным оно называется, потому что может как притягивать так и отталкивать тела. В нашем симуляторе будут рассмотрены ограничения, действующие только на пары тел, причем хотя бы одно тело из этой пары должно быть движимым. Т. е. можно связать одно движимое тело например, с физическим миром или с фиксированным телом, а можно и с другим движимым телом.
Простейшим примером двунаправленного ограничения служит ограничение по расстоянию ( distance constraint ), оно заставляет пару точек двух тел быть на расстоянии L друг от друга. Для того, чтобы определить это ограничение, зададим так называемую ограничивающую функцию; для того, чтобы соблюдалось ограничение, нужно, чтобы эта функция была равна 0.
Для ограничения по расстоянию эта функция будет выглядить следующим образом:
C(x₁, q₁, x₂, q₂) = 0.5 * ((p₂ - p₁)² – L₂);
где p1, p2 – точки тела, между которыми и должно соблюдаться расстояние L. Задаются точки, как было показано в предыдущем шаге, формулами:
p₁ = x₁ + q₁ x r₁;
p₂ = x₂ + q₂ x r₂;
где r₁, r₂ – радиус векторы точек в локальной системе координат тел.
Далее, если C = 0, значит точки находятся на нужном расстоянии. Коэффициент 0.5 поставлен потому что ограничение действует на два тела, в самой функции роли это не играет, но в дальнейшем мы увидим что он там действительно нужен.
Следующим шагом будет выделение базиса, в котором действует ограничивающая функция. В дальнейшем это потребуется для того, чтобы определить, какую силу нужно применить к телам, чтобы они поставленные ограничения соблюдали. Для того, чтобы этот базис определить, нам нужно знать, как поведет себя функция в некоторой окрестности точки x:
dC / dt = (dC / dt) * (dx / dx) = (dC / dx) * (dx / dt) = (dC / dx) * U;
где U – вектор скоростей тела в данной точке.
Фактически, dC / dx является матрицей Якоби для ограничивающей функции.
Матрица Якоби ( Jacobian matrix, в сокращении просто Jacobian ) – матрица частных производных для систем некоторых функций. Эта матрица определяет поведение этой системы функций в окрестности некоторой точки X. В математическом виде матрица Якоби для системы функций F(X) = { fi(x1, ..., xn) }, где i = 1...m ( m – кол-во функций в системе, n – кол-во компонент X ), выглядит следующим образом:
(df₁ / dx₁)  (df₁ / dx₂)  ...  (df₁ / dx_n)
(df₂ / dx₁)
...              ...
(df_m / dx₁)      ...          (df_m / dx_n)
Определяя матрицу Якоби для ограничивающей функции, мы можем получить производную этой функции по времени с помощью следующей формулы ( что было доказано выше ):
dC / dt = J * U;            (1)
где U – вектор скоростей тел. Обозначаем его буквой U а не V потому что V – это вектор мгновенных скоростей тел на текущем моменте, вектор U же сочетает в себе вектор мгновенных скоростей и еще влияние внешних и внутренних сил ( пример внешней силы – гравитация, пример внутренних сил в системе – ограничивающая сила ( constraint force ) ). Формула для записи вектора скоростей U следующая:
U = (V + dt * M^-1 * F_ext + dt * M^-1 * F_c);    (2)
где V – вектор мгновенных скоростей, M – матрица масс ( mass-matrix ), F_ext - вектор внешних сил и F_c - вектор ограничивающих сил. Все вектора, содержащиеся в данной формуле записаны в виде так называемого твиста, в нем хранятся как линейная так и угловая состовляющие; например, V = { v1, w1, v2, w2, ..., vn, wn } – вектор мгновенных скоростей для n тел.
Зная базис, в котором действует ограничивающая функция, можно записать формулу для вычисления так называемой ограничивающей силы ( constraint force ) – силы, которая будет действовать на тела, заставляя их подчиняться поставленному нами ограничению:
F_c = J^T * lambda;            (3)
где lambda – вектор так называемых множителей Лагранжа, знаковых чисел, которые показываю, на какую величину должен быть умножен базисный вектор для получения нужной ограничивающей силы.
Теперь рассмотрим на примере ранее заданного ограничения по расстоянию, как можно вычислить матрицу Якоби:
Для начала надо взять производнуую ограничивающей функции по времени ( здесь и далее, обозначим dF / dt = F’, т. е. апостроф по умолчанию означает дифференцирование по времени, если не указано другого ):
C = 0.5 * ((p₂ - p₁)² – L₂);
C’ = 0.5 * 2 * (p₂ - p₁) * (p₂ - p₁)’;
C’ = (p₂ - p₁) * (x₂ + q₂ x r₂ – x₁ – q₁ x r₁)’;
C’ = (p₂ - p₁) * (v₂ + w₂ x r₂ – v₁ – w₁ x r₁);
C’ = (p₂ – p₁) * v₂ + (p₂ – p₁) * (w₂ x r₂) - (p₂ – p₁) * v₁ - (p₂ – p₁) * (w₁ x r₁);
По правилу циклической перестановки для смешанного произведения трех векторов ( a * (b x c) = b * (c x a) )
=>

C’ = v₂ * (p₂ – p₁) + w₂ * (r₂ x (p₂ – p₁)) - v₁ * (p₂ – p₁) - w₁ * (r₁ x (p₂ – p₁));
Выносим скорости:
C’ = ( -(p₂ – p₁)^T  -(r₁ x (p₂ – p₁))^T  (p₂ - p₁)^T  (r₂ x (p₂ – p₁))^T ) * U;
Полученная матрица коэффициентов для вектора скоростей U и есть нужная матрица Якобиан, а соответствии с (1):
J = ( -(p₂ – p₁)^T  -(r₁ x (p₂ – p₁))^T  (p₂ - p₁)^T  (r₂ x (p₂ – p₁))^T );

Рассмотрев нахождение базиса для ограничивающих сил, опишем в общем виде, как найти вторую часть формулы для нахождения ограничивающей силы, т. е. вектор множителей Лагранжа lambda. Начнем с подстановки формулы (2) в формулу (1):
C’ = J * (V + dt * M^-1 * F_ext + dt * M^-1 * Fc)
Далее, заменим вектор ограничивающий силы, в соответствии с (3):
C’ = J * (V + dt * M^-1 * F_ext + dt * M^-1 * J^T * lambda)
Выделяем неизвестную часть:
C’ = J * (V + dt * M^-1 * F_ext) + dt * J * M^-1 * J^T * lambda
Переносим:
dt * (J * M^-1 * J^T) * lambda = C’ - J * (V + dt * M^-1 * F_ext)
Делим на dt:
(J * M^-1 * J^T) * lambda = С’ / dt - J * (V / dt + M-1 * F_ext)

Если заменить полученное в соответствии с
A = J * M^-1 * J^T;
b = С’ / dt - J * (V / dt + M^-1 * F_ext);
то получим СЛАУ, систему линейных алгебраических уравнений ( СЛАУ ), где неизвестный вектор – вектор множителей Лагранжа:
A * x = b;

А теперь неплохо бы это СЛАУ решить
// Стабилизация Баумгарте

#dynamics, #физика, #rigid body

29 ноября 2007 (Обновление: 12 сен 2008)