Интерполяция формы с кватернио с использованием Eigen в C ++ / OpenGL

Я пытаюсь реализовать интерполяцию формы между двумя треугольниками, используя кватернионы, как обсуждалось в статье 2000 года Интерполяция максимально жесткой формы Алекса, Коэн-Ор и Левин.

Скажем, треугольник 1 имеет точки A, B, C, а треугольник 2 имеет точки M, N, O. Две соответствующие матрицы (T1 и T2, соответственно) следующие:

T1 = {A.x, B.x, C.x},
{A.y, B.y, C.y},
{1,   1,   1  }

T2 = {M.x, N.x, N.x},
{M.y, N.y, N.y},
{1,   1,   1  }

Затем я беру обратное значение T1 и умножаю его на T2, чтобы выполнить разложение по сингулярным значениям, чтобы получить матрицу SVD, чтобы найти матрицу вращения и масштабирования.

Eigen::Matrix3f A = T1.inverse().eval() * T2;
Eigen::JacobiSVD<MatrixXf> SVD;
SVD.compute(A, ComputeFullU | ComputeFullV);
Eigen::MatrixXf D = SVD.singularValues().asDiagonal().toDenseMatrix();
Eigen::Matrix3f rotationMatrix = SVD.MatrixU() * SVD.MatrixV();
Eigen::Matrix3f sMatrix = SVD.MatrixV() * D * SVD.MatrixV().transpose();
Eigen::Quaternionf quat = rotationMatrix;

Затем я использую этот кватернион и sMatrix для интерполяции. Примечание: таймер начинается с 0.0f и увеличивается на 0.0001f (небольшая постоянная), пока не достигнет 1.0f. оживить трансформацию.

Eigen::MatrixXf Rtime = (Quaternionf::Identity().slerp(timer, quat)).toRotationMatrix();
MatrixXf Stime = MatrixXf::Identity(3, 3) * (1 - timer) + sMatrix * timer;
Eigen::MatrixXf A = Rtime * ((1 - timer) * MatrixXf::Identity(3, 3) + timer * Stime);

Затем я превращаю каждую точку P исходного треугольника в собственный вектор 3×1 и умножаю его на A.

Eigen::Vector3f v;
v(0) = P.x;
v(1) = P.y;
v(2) = 1;
v = At * v;

Наконец, я рисую точку на новых v (0) и v (1) и повторяю для двух других вершин треугольника.

В то время как я получаю новый треугольник в месте исходного треугольника (T1), когда таймер очень маленький (близко к 0), при увеличении таймера новый треугольник масштабируется и перемещается от T1, но не движется в направлении последнего треугольника. (Т2).

Линейная интерполяция, кажется, работает, если я просто использую матрицу, которую я вычислил, взяв T1.inverse () и умножив на T2. Итак, я думаю, что моя проблема с реализацией slerp или с SVD, но я не уверен.

Есть ли что-то еще, что я должен включить, чтобы определить, где я ошибся?

РЕДАКТИРОВАТЬ: Добавлена ​​Stime-матрица, переключен вызов транспонирования на V-матрицу.