// Функция, которая сохраняет Val на отрезке [ vMin; vMax ] ( проекция на отрезок )
template <class Type>
__forceinline Type satur(Type Val, Type vMin, Type vMax) { return (ValvMax)?vMax:Val); }
// lo, hi - вектора, содержащие нижние и верхние пределы для множителя
VecN Solve_SOR_PGS(MatMN &A, VecN &b, VecN *lo, VecN *hi, float w = 0.9f, int kMax = 5)
{
if (A.m != b.n) return b;
int i, j, n = A.m;
VecN x = b;
float delta;
// Projected Gauss-Seidel with Successive OverRelaxation Solver
for (int k = 0; k < kMax; k++)
{
for (i = 0; i < n; i++)
{
delta = 0.0f;
for (j = 0; j < i; j++) delta += A[i][j]*x[j];
for (j = i+1; j < n; j++) delta += A[i][j]*x[j];
delta = (b[i] - delta) / A[i][i];
x[i] += w * (delta - x[i]);
x[i] = satur<float>(x[i], (lo)?(lo->v[i]):(-DEF_INFINITE), (hi)?(hi->v[i]):(DEF_INFINITE));
}
}
return x;
}Использование разреженности матрицы
Код, предложенный выше, позволяет моделировать динамику довольно маленького количества тел, потому как количество контактных соединений велико и перебор всех элементов матрицы A влечет за собой огромное количество вычислений. Однако факт, что матрица A является довольно разреженной вследствие того, что в соединения вовлечены два тела, позволяет намного понизить ресурсоемкость ( снизить требования не только к вычислительной мощности, но и к объему оперативной памяти ) итеративных методов. Считать следует исходя из того, что матрица A имеет блочную структуру.
Первое, что нужно сделать, это посчитать диагональ матрицы A ( для того, чтобы дулить на Ai,i ), сформировав при этом вектор такого же размера, как и количество строк в матрице A ( или количество столбцов, без разницы, так как A - квадратная матрица ).
Затем следует учесть тот факт, что в ограничения накладываются максимум на два тела, и поэтому размерность конечной матрицы Якобиан ( Jacobian matrix ) станет c x 6*2 вместо c x 6*n, где c - суммарное количество ограниченных степеней свободы; n - количество тел в системе.
Так же в этом случае солвер имеет гораздо большую степень интеграции в физ. движок, потому что чтобы избежать перемножения матрицы Якобиан на вектор множителей Лагранжа для получения вектора ограничивающих сил. И поэтому на выходе солвер будет давать сразу вектор ускорений для тел в системе ( в случае если на тело не действует ни одно ограничение, соостветствующее значение вектора ускорений будет равно нуль-вектору ). Так же из-за этого в самом солвере придется использовать приближение ускорения как a = V2-V1/dt и модифицировать вектор правой стороны ( right hand side - RHS, здесь это вектор b ) для учета влияния тел друг на друга и внешних сил на тела: g = b/dt - J(V1 + M-1*F(e), где J - матрица Якобиан; M-1 - инвертированная полная матрица масс ( диагональная, состоит из блоков 1/m*E и I-1 ); и F(e) - вектор-аккумулятор внешних сил, действующих на тела.
Для учета разреженности используется 3 матрицы: матрица Jsp, Bsp и Jmp. Матрица Jsp - уже описанная выше, "сложенная" матрица Якобиан размерностью c x 12, Bsp - вспомогательная матрица, равная M-1 * JT, имеет размерность 12 x c, а матрица Jmp используется для индексации тел, участвующих в i-том ограничении.
Теперь, когда нужные пояснения сделаны, можно представлять код получившийся в результяте этих махинаций:
Работы по теме
[ 1] David Baraff, "Fast Contact Force Computation for Nonpenetrating Rigid Bodies"
[ 2] Michael Cline, "Rigid Body Simulation with Contact and Constraints"
[ 3] Kenny Erleben, "Stable, Robust, and Versatile Multibody Dynamics Animation"
[ 4] Erin Catto, "Iterative Dynamics with Temporal Coherence"
[ 5] Katta Murty, "Linear Complementarity, Linear and Nonlinear programming"
#dynamics, #LCP, #физика, #rigid body
15 апреля 2006
(Обновление: 21 янв 2010)
Комментарии [2]