unit ugluMatrixExHelper;

{$mode objfpc}{$H+}

interface

uses
  Classes, SysUtils,
  ugluVectorExHelper;

type
  generic TgluMatrixHelper<T> = class
  public type
    TVectorHelper = specialize TgluVectorHelper<T>;
    TVectorHelperF = specialize TgluVectorHelperF<T>;
    TBaseType = T;
    PBaseType = ^T;

    TMat2 = array[0..1] of TVectorHelper.TVector2;
    TMat3 = array[0..2] of TVectorHelper.TVector3;
    TMat4 = array[0..3] of TVectorHelper.TVector4;

    PMat2 = ^TMat2;
    PMat3 = ^TMat3;
    PMat4 = ^TMat4;

  public
    class function Equals(const m1, m2: TMat2): Boolean; overload; inline;
    class function Equals(const m1, m2: TMat3): Boolean; overload; inline;
    class function Equals(const m1, m2: TMat4): Boolean; overload; inline;

    class function ToString(const m: TMat2; const aRound: Integer = -3): String; overload; inline;
    class function ToString(const m: TMat3; const aRound: Integer = -3): String; overload; inline;
    class function ToString(const m: TMat4; const aRound: Integer = -3): String; overload; inline;

    class function TryFromString(const s: String; out m: TMat2): Boolean; overload; inline;
    class function TryFromString(const s: String; out m: TMat3): Boolean; overload; inline;
    class function TryFromString(const s: String; out m: TMat4): Boolean; overload; inline;

    class function Transpose(const m: TMat2): TMat2; overload; inline;
    class function Transpose(const m: TMat3): TMat3; overload; inline;
    class function Transpose(const m: TMat4): TMat4; overload; inline;

    class function Sub(const m: TMat2; const c, r: Integer): TBaseType; overload; inline;
    class function Sub(const m: TMat3; const c, r: Integer): TMat2; overload; inline;
    class function Sub(const m: TMat4; const c, r: Integer): TMat3; overload; inline;

    class function Determinant(const m: TMat2): Double; overload; inline;
    class function Determinant(const m: TMat3): Double; overload; inline;
    class function Determinant(const m: TMat4): Double; overload;

    class function Adjoint(const m: TMat3): TMat3; overload;
    class function Adjoint(const m: TMat4): TMat4; overload;

    class function Multiply(const m1, m2: TMat2): TMat2; overload; inline;
    class function Multiply(const m1, m2: TMat3): TMat3; overload; inline;
    class function Multiply(const m1, m2: TMat4): TMat4; overload; inline;

    class function Multiply(const m: TMat2; const v: TVectorHelper.TVector2): TVectorHelper.TVector2; overload; inline;
    class function Multiply(const m: TMat3; const v: TVectorHelper.TVector3): TVectorHelper.TVector3; overload; inline;
    class function Multiply(const m: TMat4; const v: TVectorHelper.TVector4): TVectorHelper.TVector4; overload; inline;

    class function Multiply(const m: TMat2; const v: TBaseType): TMat2; overload; inline;
    class function Multiply(const m: TMat3; const v: TBaseType): TMat3; overload; inline;
    class function Multiply(const m: TMat4; const v: TBaseType): TMat4; overload; inline;

    class function Invert(m: TMat2): TMat2; overload; inline;
    class function Invert(m: TMat3): TMat3; overload; inline;
    class function Invert(m: TMat4): TMat4; overload; inline;

    class function CreateTranslate(const v: TVectorHelper.TVector2): TMat3;
    class function CreateTranslate(const v: TVectorHelper.TVector3): TMat4;

    class function CreateScale(const v: TVectorHelper.TVector2): TMat3;
    class function CreateScale(const v: TVectorHelper.TVector3): TMat4;

    class function CreateRotate(a: Double): TMat3;
    class function CreateRotate(axis: TVectorHelper.TVector3; a: Double): TMat4;

    class function CreateShear(const v: TVectorHelper.TVector2): TMat3;
    class function CreateShear(const x, y, z: TVectorHelper.TVector2): TMat4;
  private
    class function GetElement(p: PBaseType; x, y, sz: Integer): TBaseType; inline;
    class procedure SetElement(p: PBaseType; x, y, sz: Integer; v: TBaseType); inline;

    class procedure Transpose(src, dst: PBaseType; sz: Integer);
    class procedure Sub(src, dst: PBaseType; sz, c, r: Integer);
    class procedure Mult(p1, p2, r: PBaseType; c1r2, r1, c2: Integer);
    class procedure Mult(src, dst: PBaseType; c, r: Integer; v: TBaseType);

    class function TryFromString(const s: String; p: PBaseType; r, c: Integer): Boolean;
  end;

  TgluMatrixF = specialize TgluMatrixHelper<Single>;
  TgluMatrixD = specialize TgluMatrixHelper<Double>;

implementation

uses
  Math;

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//TgluMatrixHelper//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
class function TgluMatrixHelper.Equals(const m1, m2: TMat2): Boolean;
begin
  result :=
    TVectorHelper.Equals(m1[0], m2[0]) and
    TVectorHelper.Equals(m1[1], m2[1]);
end;

class function TgluMatrixHelper.Equals(const m1, m2: TMat3): Boolean;
begin
  result :=
    TVectorHelper.Equals(m1[0], m2[0]) and
    TVectorHelper.Equals(m1[1], m2[1]) and
    TVectorHelper.Equals(m1[2], m2[2]);
end;

class function TgluMatrixHelper.Equals(const m1, m2: TMat4): Boolean;
begin
  result :=
    TVectorHelper.Equals(m1[0], m2[0]) and
    TVectorHelper.Equals(m1[1], m2[1]) and
    TVectorHelper.Equals(m1[2], m2[2]) and
    TVectorHelper.Equals(m1[3], m2[3]);
end;

class function TgluMatrixHelper.ToString(const m: TMat2; const aRound: Integer): String;
begin
  result := format('(%s); (%s)', [
    TVectorHelper.ToString(m[0], aRound),
    TVectorHelper.ToString(m[1], aRound)]);
end;

class function TgluMatrixHelper.ToString(const m: TMat3; const aRound: Integer): String;
begin
  result := format('(%s); (%s); (%s)', [
    TVectorHelper.ToString(m[0], aRound),
    TVectorHelper.ToString(m[1], aRound),
    TVectorHelper.ToString(m[2], aRound)]);
end;

class function TgluMatrixHelper.ToString(const m: TMat4; const aRound: Integer): String;
begin
  result := format('(%s); (%s); (%s); (%s)', [
    TVectorHelper.ToString(m[0], aRound),
    TVectorHelper.ToString(m[1], aRound),
    TVectorHelper.ToString(m[2], aRound),
    TVectorHelper.ToString(m[3], aRound)]);
end;

class function TgluMatrixHelper.TryFromString(const s: String; out m: TMat2): Boolean;
begin
  result := TryFromString(s, @m[0,0], 2, 2);
end;

class function TgluMatrixHelper.TryFromString(const s: String; out m: TMat3): Boolean;
begin
  result := TryFromString(s, @m[0,0], 3, 3);
end;

class function TgluMatrixHelper.TryFromString(const s: String; out m: TMat4): Boolean;
begin
  result := TryFromString(s, @m[0,0], 4, 4);
end;

class function TgluMatrixHelper.Transpose(const m: TMat2): TMat2;
begin
  Transpose(@m[0,0], @result[0,0], Length(m));
end;

class function TgluMatrixHelper.Transpose(const m: TMat3): TMat3;
begin
  Transpose(@m[0,0], @result[0,0], Length(m));
end;

class function TgluMatrixHelper.Transpose(const m: TMat4): TMat4;
begin
  Transpose(@m[0,0], @result[0,0], Length(m));
end;

class function TgluMatrixHelper.Sub(const m: TMat2; const c, r: Integer): TBaseType;
begin
  Sub(@m[0,0], @result, 2, c, r);
end;

class function TgluMatrixHelper.Sub(const m: TMat3; const c, r: Integer): TMat2;
begin
  Sub(@m[0,0], @result[0,0], 3, c, r);
end;

class function TgluMatrixHelper.Sub(const m: TMat4; const c, r: Integer): TMat3;
begin
  Sub(@m[0,0], @result[0,0], 4, c, r);
end;

class function TgluMatrixHelper.Determinant(const m: TMat2): Double;
begin
  result := m[0,0] * m[1,1] - m[1,0] * m[0,1];
end;

class function TgluMatrixHelper.Determinant(const m: TMat3): Double;
begin
  result :=
    m[0,0] * m[1,1] * m[2,2] +
    m[1,0] * m[2,1] * m[0,2] +
    m[2,0] * m[0,1] * m[1,2] -
    m[2,0] * m[1,1] * m[0,2] -
    m[1,0] * m[0,1] * m[2,2] -
    m[0,0] * m[2,1] * m[1,2];
end;

class function TgluMatrixHelper.Determinant(const m: TMat4): Double;
var
  i: Integer;
begin
  result := 0.0;
  for i := 0 to 3 do
    result := result + power(-1, i) * m[i,0] * Determinant(Sub(m, i, 0));
end;

class function TgluMatrixHelper.Adjoint(const m: TMat3): TMat3;
var
  i, j: Integer;
begin
  for i := 0 to 2 do
    for j := 0 to 2 do
      result[i,j] := power(-1, i+j) * Determinant(Sub(m, i, j));
  result := Transpose(result{%H-});
end;

class function TgluMatrixHelper.Adjoint(const m: TMat4): TMat4;
var
  i, j: Integer;
begin
  for i := 0 to 3 do
    for j := 0 to 3 do
      result[i,j] := power(-1, i+j) * Determinant(Sub(m, i, j));
  result := Transpose(result{%H-});
end;

class function TgluMatrixHelper.Multiply(const m1, m2: TMat2): TMat2;
begin
  Mult(@m1[0,0], @m2[0,0], @result[0,0], 2, 2, 2);
end;

class function TgluMatrixHelper.Multiply(const m1, m2: TMat3): TMat3;
begin
  Mult(@m1[0,0], @m2[0,0], @result[0,0], 3, 3, 3);
end;

class function TgluMatrixHelper.Multiply(const m1, m2: TMat4): TMat4;
begin
  Mult(@m1[0,0], @m2[0,0], @result[0,0], 4, 4, 4);
end;

class function TgluMatrixHelper.Multiply(const m: TMat2; const v: TVectorHelper.TVector2): TVectorHelper.TVector2;
begin
  Mult(@m[0,0], @v[0], @result[0], 2, 2, 1);
end;

class function TgluMatrixHelper.Multiply(const m: TMat3; const v: TVectorHelper.TVector3): TVectorHelper.TVector3;
begin
  Mult(@m[0,0], @v[0], @result[0], 3, 3, 1);
end;

class function TgluMatrixHelper.Multiply(const m: TMat4; const v: TVectorHelper.TVector4): TVectorHelper.TVector4;
begin
  Mult(@m[0,0], @v[0], @result[0], 4, 4, 1);
end;

class function TgluMatrixHelper.Multiply(const m: TMat2; const v: TBaseType): TMat2;
begin
  Mult(@m[0,0], @result[0,0], 2, 2, v);
end;

class function TgluMatrixHelper.Multiply(const m: TMat3; const v: TBaseType): TMat3;
begin
  Mult(@m[0,0], @result[0,0], 3, 3, v);
end;

class function TgluMatrixHelper.Multiply(const m: TMat4; const v: TBaseType): TMat4;
begin
  Mult(@m[0,0], @result[0,0], 4, 4, v);
end;

class function TgluMatrixHelper.Invert(m: TMat2): TMat2;
begin
  result[0,0] :=  m[1,1];
  result[0,1] := -m[0,1];
  result[1,0] := -m[1,0];
  result[1,1] :=  m[0,0];
  m := result;
  Mult(@m[0,0], @result[0,0], 2, 2, 1 / Determinant(m));
end;

class function TgluMatrixHelper.Invert(m: TMat3): TMat3;
var d: TBaseType;
begin
  d := Determinant(m);
  m := Adjoint(m);
  Mult(@m[0,0], @result[0,0], 3, 3, 1 / d);
end;

class function TgluMatrixHelper.Invert(m: TMat4): TMat4;
var d: TBaseType;
begin
  d := Determinant(m);
  result := Adjoint(m);
  Mult(@m[0,0], @result[0,0], 4, 4, 1 / d);
end;

class function TgluMatrixHelper.CreateTranslate(const v: TVectorHelper.TVector2): TMat3;
begin
  result[0, 0] := 1.0;
  result[0, 1] := 0.0;
  result[0, 2] := 0.0;
  result[1, 0] := 0.0;
  result[1, 1] := 1.0;
  result[1, 2] := 0.0;
  result[2, 0] := v[0];
  result[2, 1] := v[1];
  result[2, 2] := 1.0;
end;

class function TgluMatrixHelper.CreateTranslate(const v: TVectorHelper.TVector3): TMat4;
begin
  result[0, 0] := 1.0;
  result[0, 1] := 0.0;
  result[0, 2] := 0.0;
  result[0, 3] := 0.0;
  result[1, 0] := 0.0;
  result[1, 1] := 1.0;
  result[1, 2] := 0.0;
  result[1, 3] := 0.0;
  result[2, 0] := 0.0;
  result[2, 1] := 0.0;
  result[2, 2] := 1.0;
  result[2, 3] := 0.0;
  result[3, 0] := v[0];
  result[3, 1] := v[1];
  result[3, 2] := v[2];
  result[3, 3] := 1.0;
end;

class function TgluMatrixHelper.CreateScale(const v: TVectorHelper.TVector2): TMat3;
begin
  result[0, 0] := v[0];
  result[0, 1] := 0.0;
  result[0, 2] := 0.0;
  result[1, 0] := 0.0;
  result[1, 1] := v[1];
  result[1, 2] := 0.0;
  result[2, 0] := 0.0;
  result[2, 1] := 0.0;
  result[2, 2] := 1.0;
end;

class function TgluMatrixHelper.CreateScale(const v: TVectorHelper.TVector3): TMat4;
begin
  result[0, 0] := v[0];
  result[0, 1] := 0.0;
  result[0, 2] := 0.0;
  result[0, 3] := 0.0;
  result[1, 0] := 0.0;
  result[1, 1] := v[1];
  result[1, 2] := 0.0;
  result[1, 3] := 0.0;
  result[2, 0] := 0.0;
  result[2, 1] := 0.0;
  result[2, 2] := v[2];
  result[2, 3] := 0.0;
  result[3, 0] := 0.0;
  result[3, 1] := 0.0;
  result[3, 2] := 0.0;
  result[3, 3] := 1.0;
end;

class function TgluMatrixHelper.CreateRotate(a: Double): TMat3;
begin
  a := DegToRad(a);
  result[0, 0] :=  cos(a);
  result[0, 1] := -sin(a);
  result[0, 2] :=  0.0;
  result[1, 0] :=  sin(a);
  result[1, 1] :=  cos(a);
  result[1, 2] :=  0.0;
  result[2, 0] :=  0.0;
  result[2, 1] :=  0.0;
  result[2, 2] :=  1.0;
end;

class function TgluMatrixHelper.CreateRotate(axis: TVectorHelper.TVector3; a: Double): TMat4;
var
  X, Y, Z, s, c: Single;
begin
  a := DegToRad(a);
  axis := TVectorHelperF.Normalize(axis);
  X := axis[0];
  Y := axis[1];
  Z := axis[2];
  s := sin(a);
  c := cos(a);
  result[0,0] := SQR(X) + (1-SQR(X))*c;
  result[0,1] := X*Y*(1-c) + Z*s;
  result[0,2] := X*Z*(1-c) - Y*s;
  result[0,3] := 0.0;
  result[1,0] := X*Y*(1-c) - Z*s;
  result[1,1] := SQR(Y) + (1-SQR(Y))*c;
  result[1,2] := Y*Z*(1-c) + X*s;
  result[1,3] := 0.0;
  result[2,0] := X*Z*(1-c) + Y*s;
  result[2,1] := Y*Z*(1-c) - X*s;
  result[2,2] := SQR(Z) + (1-SQR(Z))*c;
  result[2,3] := 0.0;
  result[3,0] := 0.0;
  result[3,1] := 0.0;
  result[3,2] := 0.0;
  result[3,3] := 1.0;
end;

class function TgluMatrixHelper.CreateShear(const v: TVectorHelper.TVector2): TMat3;
begin
  result[0, 0] := 1.0;
  result[0, 1] := v[1];
  result[0, 2] := 0.0;
  result[1, 0] := v[0];
  result[1, 1] := 1.0;
  result[1, 2] := 0.0;
  result[2, 0] := 0.0;
  result[2, 1] := 0.0;
  result[2, 2] := 1.0;
end;

class function TgluMatrixHelper.CreateShear(const x, y, z: TVectorHelper.TVector2): TMat4;
begin
  result[0,0] := 1.0;
  result[0,1] := y[0];
  result[0,2] := z[0];
  result[0,3] := 0.0;
  result[1,0] := x[0];
  result[1,1] := 1.0;
  result[1,2] := z[1];
  result[1,3] := 0.0;
  result[2,0] := x[1];
  result[2,1] := y[1];
  result[2,2] := 1.0;
  result[2,3] := 0.0;
  result[3,0] := 0.0;
  result[3,1] := 0.0;
  result[3,2] := 0.0;
  result[3,3] := 1.0;
end;

class function TgluMatrixHelper.GetElement(p: PBaseType; x, y, sz: Integer): TBaseType;
begin
  result := (p + (x * sz) + y)^;
end;

class procedure TgluMatrixHelper.SetElement(p: PBaseType; x, y, sz: Integer; v: TBaseType);
begin
  (p + (x * sz) + y)^ := v;
end;

class procedure TgluMatrixHelper.Transpose(src, dst: PBaseType; sz: Integer);
var
  i, j: Integer;
begin
  for i := 0 to sz-1 do
    for j := 0 to sz-1 do
      SetElement(dst, i, j, sz, GetElement(src, j, i, sz));
end;

class procedure TgluMatrixHelper.Sub(src, dst: PBaseType; sz, c, r: Integer);
var
  x, y, i, j: Integer;
begin
  for i := 0 to sz-1 do begin
    for j := 0 to sz-1 do begin
      x := i;
      y := j;
      if (i >= c) then inc(x);
      if (j >= r) then inc(y);
      SetElement(dst, i, j, sz-1, GetElement(src, x, y, sz));
    end;
  end;
end;

class procedure TgluMatrixHelper.Mult(p1, p2, r: PBaseType; c1r2, r1, c2: Integer);
var
  x, y, i: Integer;
  sum: TBaseType;
begin
  for x := 0 to c2-1 do begin
    for y := 0 to r1-1 do begin
      sum := 0;
      for i := 0 to c1r2-1 do
        sum := sum + GetElement(p1, i, y, c1r2) * GetElement(p2, x, i, c2);
      SetElement(r, x, y, c2, sum);
    end;
  end;
end;

class procedure TgluMatrixHelper.Mult(src, dst: PBaseType; c, r: Integer; v: TBaseType);
var
  i, j: Integer;
begin
  for i := 0 to c-1 do
    for j := 0 to r-1 do
      SetElement(dst, i, j, c, v * GetElement(src, i, j, c));
end;

class function TgluMatrixHelper.TryFromString(const s: String; p: PBaseType; r, c: Integer): Boolean;
var
  i, j, l: Integer;
begin
  result := true;
  l := Length(s);
  i := 1;
  j := l;
  while (i <= l) and (c > 0) and result do begin
    if (s[i] = '(') then begin
      j := i+1;
    end else if (s[i] = ')') then begin
      result := TVectorHelper.TryFromString(trim(copy(s, j, i-j)), p, r);
      j := l;
      inc(p, r);
      dec(c);
    end;
    inc(i);
  end;

  result := (c = 0);
  while (c > 0) do begin
    for i := 0 to r-1 do begin
      p^ := 0;
      inc(p);
    end;
    dec(c);
  end;
end;

end.