Инверсия матрицы 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)