ПрограммированиеПодсказкиОбщее

Инверсия матрицы 4x4

Автор:

Ниже приведен вариант инверсии для матриц 4x4, где порядок элементов строки горизонтальный (RowMajor). Не самый быстрый, но вполне рабочий (проверено под OpenGL) код для Pascal/Delphi.

type
  PMatrix3 = ^TMatrix3;
  PMatrix4 = ^TMatrix4;
  TVector3 = array[0..2] of Single;
  TVector4 = array[0..3] of Single;
  TMatrix3 = array[0..2] of TVector3;
  TMatrix4 = array[0..3] of TVector4;

type
  PMinorMatrix4 = ^TMinorMatrix4;
  TMinorMatrix4 = packed record
    a0, b0, c0, d0,
    a1, b1, c1, d1,
    a2, b2, c2, d2,
    a3, b3, c3, d3: Single;
  end;

procedure InitMatrix(var M: TMatrix4);
const
  SingleOne: Single = 1.0;

asm
  .NOFRAME

  movss xmm0, [SingleOne]

  movups dqword ptr [M + 00], xmm0 // (1,0,0,0)
  pslldq xmm0, 4
  movups dqword ptr [M + 16], xmm0 // (0,1,0,0)
  pslldq xmm0, 4
  movups dqword ptr [M + 32], xmm0 // (0,0,1,0)
  pslldq xmm0, 4
  movups dqword ptr [M + 48], xmm0 // (0,0,0,1)
end;

{$EXCESSPRECISION OFF}
function GetMinor4L(var AMatrix: TMatrix4; ACol, ARow: Integer): Single;
var
  NC: Integer;
  T: TMatrix4;

begin
  // [0, 1] // 0 - Строка, 1 - Столбец
  // [2, 3] // 2 - Строка, 3 - Столбец

  //------------------
  // Исключаем строки
  //------------------

  if (ARow = 0) then
  begin
    T[0] := AMatrix[1];
    T[1] := AMatrix[2];
    T[2] := AMatrix[3];
  end
  else
  if (ARow = 1) then
  begin
    T[0] := AMatrix[0];
    T[1] := AMatrix[2];
    T[2] := AMatrix[3];
  end
  else
  if (ARow = 2) then
  begin
    T[0] := AMatrix[0];
    T[1] := AMatrix[1];
    T[2] := AMatrix[3];
  end
  else
  if (ARow = 3) then
  begin
    T[0] := AMatrix[0];
    T[1] := AMatrix[1];
    T[2] := AMatrix[2];
  end;

  //-------------------
  // Исключаем столбцы
  //-------------------

  NC := 0; // Столбцы (X)

  // Исключаем столбец (0) матрицы M
  if (ACol = 0) then Inc(NC);

  // Копируем в столбец 0
  T[0, 0] := T[0, NC];
  T[1, 0] := T[1, NC];
  T[2, 0] := T[2, NC];

  Inc(NC);

  // Исключаем столбец (1) матрицы M
  if (ACol = 1) then Inc(NC);

  // Копируем в столбец 1
  T[0, 1] := T[0, NC];
  T[1, 1] := T[1, NC];
  T[2, 1] := T[2, NC];

  Inc(NC);

  // Исключаем столбец (2) матрицы M
  if (ACol = 2) then Inc(NC);

  // Копируем в столбец 2
  T[0, 2] := T[0, NC];
  T[1, 2] := T[1, NC];
  T[2, 2] := T[2, NC];

  //    a     b     c
  //[0, 0][1, 0][2, 0] //0
  //[0, 1][1, 1][2, 1] //1
  //[0, 2][1, 2][2, 2] //2
  //[y, x][y, x][y, x]

  //-----------------------
  // Формула
  //-----------------------
  // detA = a0 * b1 * c2 +
  //        a2 * b0 * c1 +
  //        a1 * b2 * c0 -
  //        a2 * b1 * c0 -
  //        a0 * b2 * c1 -
  //        a1 * b0 * c2
  //-----------------------

  with TMinorMatrix4(T) do
    Result := a0 * b1 * c2 +
              a2 * b0 * c1 +
              a1 * b2 * c0 -
              a2 * b1 * c0 -
              a0 * b2 * c1 -
              a1 * b0 * c2;
end;
{$EXCESSPRECISION ON}

{$EXCESSPRECISION OFF}
procedure InvertMatrix4L(var M, RM: TMatrix4);
var
  detA, A, B: Single;

begin
  // 1. Находим определитель матрицы 4x4
  // Столбцы и строки меняем местами! (Транспозиция!)
  detA := M[0, 0] * GetMinor4L(M, 0, 0) -
          M[0, 1] * GetMinor4L(M, 1, 0) +
          M[0, 2] * GetMinor4L(M, 2, 0) -
          M[0, 3] * GetMinor4L(M, 3, 0);

  // Обратная матрица существует при (detA ≠ 0)
  if (detA <> 0.0) then
  begin
    A := (1.0 / detA); // Положительный множитель
    B := -A; // Отрицательный множитель

    // 2. Находим матрицу миноров
    // 3. и учитываем матрицу алгебраических дополнений
    // [+, -, +, -]
    // [-, +, -, +]
    // [+, -, +, -]
    // [-, +, -, +]

    RM[0, 0] := GetMinor4L(M, 0, 0) * A; // +
    RM[0, 1] := GetMinor4L(M, 0, 1) * B; // -
    RM[0, 2] := GetMinor4L(M, 0, 2) * A; // +
    RM[0, 3] := GetMinor4L(M, 0, 3) * B; // -
    RM[1, 0] := GetMinor4L(M, 1, 0) * B; // -
    RM[1, 1] := GetMinor4L(M, 1, 1) * A; // +
    RM[1, 2] := GetMinor4L(M, 1, 2) * B; // -
    RM[1, 3] := GetMinor4L(M, 1, 3) * A; // +
    RM[2, 0] := GetMinor4L(M, 2, 0) * A; // +
    RM[2, 1] := GetMinor4L(M, 2, 1) * B; // -
    RM[2, 2] := GetMinor4L(M, 2, 2) * A; // +
    RM[2, 3] := GetMinor4L(M, 2, 3) * B; // -
    RM[3, 0] := GetMinor4L(M, 3, 0) * B; // -
    RM[3, 1] := GetMinor4L(M, 3, 1) * A; // +
    RM[3, 2] := GetMinor4L(M, 3, 2) * B; // -
    RM[3, 3] := GetMinor4L(M, 3, 3) * A; // +
  end
  else
  begin
    InitMatrix(RM);
  end;
end;
{$EXCESSPRECISION ON}

Скорость на процессоре Intel Core i7-6950X 3.0 ГГц:
Pascal-версия — 3 627 435 вызовов в секунду (~1158 тактов/вызов).
SIMD-версия — 13 326 824 вызовов в секунду (~315 тактов/вызов).

Возможно существуют более быстрые способы инверсии,
но на данный момент мне они неизвестны.

Всем удачи!

25 января 2020 (Обновление: 24 июля 2022)

Комментарии [87]