UNIT matrices;
{Все украдено с исходников POV-Ray
Ported to PAS by IE
}
			INTERFACE
Uses Uvector;
CONST
 M_PI     =3.1415926535897932384626;
 M_PI_2   =1.57079632679489661923;
 TWO_M_PI =6.283185307179586476925286766560;
 M_PI_180 =0.01745329251994329576;
 M_PI_360 =0.00872664625997164788;

type
  DBL = double;
  UV_VECT  = array [0..1] of DBL;
  VECTOR_4D = array [0..3] of DBL;
  TMATRIX = array [0..3,0..3] of DBL;

  TTRANSFORM = record
    matrix, inverse : TMATRIX;
  end;


Procedure MZero (var result:TMATRIX);
Procedure MIdentity (var result:TMATRIX);
Procedure MTimes (var result:TMATRIX; matrix1,matrix2:TMATRIX);
{Procedure MAdd (var result:TMATRIX; matrix1,matrix2:TMATRIX);
Procedure MSub (var result:TMATRIX; matrix1,matrix2:TMATRIX);
Procedure MScale (var result:TMATRIX; matrix1:TMATRIX; DBL amount);}
Procedure MTranspose (var result:TMATRIX; matrix1:TMATRIX);

Procedure MTransPoint (var result:tVec;vector:tVec;trans:TTRANSFORM);
Procedure MInvTransPoint (var result:tVec;vector:tVec;trans:TTRANSFORM);
Procedure MTransDirection (var result:tVec;vector:tVec;trans:TTRANSFORM);
Procedure MInvTransDirection (var result:tVec;vector:tVec;trans:TTRANSFORM);
Procedure MTransNormal (var result:tVec;vector:tVec;trans:TTRANSFORM);
Procedure MInvTransNormal (var result:tVec;vector:tVec;trans:TTRANSFORM);

Procedure Compute_Matrix_Transform (var result:TTRANSFORM; matrix:TMATRIX);
Procedure Compute_Scaling_Transform (var result:TTRANSFORM; vector:tVec);
{Procedure Compute_Inversion_Transform (var result:TTRANSFORM);}
Procedure Compute_Translation_Transform (var result:TTRANSFORM; vector:tVec);
Procedure Compute_Rotation_Transform (var transform:TTRANSFORM; vector:tVec);
{Procedure Compute_Look_At_Transform (var transform:TTRANSFORM; Look_At, Up, Right:tVec);}
Procedure Compose_Transforms (var Original_Transform, New_Transform:TTRANSFORM);
Procedure Compute_Axis_Rotation_Transform (var transform:TTRANSFORM; V1:tVec; angle:DBL);
Procedure Compute_Coordinate_Transform (var trans:TTRANSFORM; origin, up:tVec; r,len:DBL);

Procedure MInvers (var r,m:TMATRIX);
  

                      IMPLEMENTATION
{-------------------------------------------------------------}
Procedure MZero (var result:TMATRIX);
var
  i, j:integer;
begin
  for i := 0 to 3 do
    for j := 0 to 3 do    
      result[i,j] := 0.0;
end;
{-------------------------------------------------------------}
Procedure MIdentity (var result:TMATRIX);
var
  i, j:integer;
begin
  for i := 0 to 3 do
    for j := 0 to 3 do
     if i=j then
      result[i,j] := 1.0
     else
      result[i,j] := 0.0;
end;
{-------------------------------------------------------------}
Procedure MTimes (var result:TMATRIX; matrix1,matrix2:TMATRIX);
var
  i, j, k : integer;
  temp_matrix : TMATRIX;
begin
  for i := 0 to 3 do
    for j := 0 to 3 do
    begin
      temp_matrix[i,j] := 0.0;

      for k := 0 to 3  do
        temp_matrix[i,j] := temp_matrix[i,j]+matrix1[i,k] * matrix2[k,j];
    end;

  for i := 0 to 3 do
    for j := 0 to 3 do 
      result[i,j] := temp_matrix[i,j];
end;
{-------------------------------------------------------------}
Procedure MTranspose (var result:TMATRIX; matrix1:TMATRIX);
var
  i, j : integer;
  temp_matrix : TMATRIX;
begin

  for i := 0 to 3 do
    for j := 0 to 3 do
      temp_matrix[i,j] := matrix1[j,i];

  for i := 0 to 3 do
    for j := 0 to 3 do
      result[i,j] := temp_matrix[i,j];
end;
{-------------------------------------------------------------}
Procedure MTransPoint (var result:tVec;vector:tVec; trans:TTRANSFORM);
var
 I : integer;
 answer_array : array [0..3] of DBL;
 matrix : TMATRIX;
begin

  matrix := trans.matrix;

  for i := 0 to 2 do
    answer_array[i] := vector.X * matrix[0,i] +
                       vector.Y * matrix[1,i] +
                       vector.Z * matrix[2,i] + matrix[3,i];

  result.X := answer_array[0];
  result.Y := answer_array[1];
  result.Z := answer_array[2];
end;
{-------------------------------------------------------------}
Procedure MInvTransPoint (var result:tVec;vector:tVec;trans:TTRANSFORM);
var
 I : integer;
 answer_array : array [0..3] of DBL;
 matrix : TMATRIX;
begin

  matrix := trans.inverse;

  for i := 0 to 3 do
    answer_array[i] := vector.X * matrix[0,i] +
                       vector.Y * matrix[1,i] +
                       vector.Z * matrix[2,i] + matrix[3,i];

  result.X := answer_array[0];
  result.Y := answer_array[1];
  result.Z := answer_array[2];
end;
{-------------------------------------------------------------}
Procedure MTransDirection (var result:tVec;vector:tVec;trans:TTRANSFORM);
var
 I : integer;
 answer_array : array [0..3] of DBL;
 matrix : TMATRIX;
begin

  matrix := trans.matrix;

  for i := 0 to 3 do
    answer_array[i] := vector.X * matrix[0,i] +
                      vector.Y * matrix[1,i] +
                      vector.Z * matrix[2,i];

  result.X := answer_array[0];
  result.Y := answer_array[1];
  result.Z := answer_array[2];
end;
{-------------------------------------------------------------}
Procedure MInvTransDirection (var result:tVec;vector:tVec;trans:TTRANSFORM);
var
 I : integer;
 answer_array : array [0..3] of DBL;
 matrix : TMATRIX;
begin

  matrix := trans.inverse;

  for i := 0 to 3 do
    answer_array[i] := vector.X * matrix[0,i] +
                       vector.Y * matrix[1,i] +
                       vector.Z * matrix[2,i];

  result.X := answer_array[0];
  result.Y := answer_array[1];
  result.Z := answer_array[2];
end;
{-------------------------------------------------------------}
Procedure MTransNormal (var result:tVec;vector:tVec;trans:TTRANSFORM);
var
 I : integer;
 answer_array : array [0..3] of DBL;
 matrix : TMATRIX;
begin

  matrix := trans.matrix;

  for i := 0 to 3 do
    answer_array[i] := vector.X * matrix[i,0] +
                       vector.Y * matrix[i,1] +
                       vector.Z * matrix[i,2];

  result.X := answer_array[0];
  result.Y := answer_array[1];
  result.Z := answer_array[2];
end;
{-------------------------------------------------------------}
Procedure MInvTransNormal (var result:tVec;vector:tVec;trans:TTRANSFORM);
var
 I : integer;
 answer_array : array [0..3] of DBL;
 matrix : TMATRIX;
begin

  matrix := trans.inverse;

  for i := 0 to 3 do
    answer_array[i] := vector.X * matrix[i,0] +
                      vector.Y * matrix[i,1] +
                      vector.Z * matrix[i,2];

  result.X := answer_array[0];
  result.Y := answer_array[1];
  result.Z := answer_array[2];
end;
{-------------------------------------------------------------}
Procedure Compute_Matrix_Transform (var result:TTRANSFORM; matrix:TMATRIX);
var
 I : INTEGER;
begin

  for i := 0 to 3 do
  begin
    result.matrix[i,0] := matrix[i,0];
    result.matrix[i,1] := matrix[i,1];
    result.matrix[i,2] := matrix[i,2];
    result.matrix[i,3] := matrix[i,3];
  end;

  MInvers(result.inverse, result.matrix);
end;
{-------------------------------------------------------------}
Procedure Compute_Scaling_Transform (var result:TTRANSFORM; vector:tVec);
begin
  MIdentity (result.matrix);

  result.matrix[0,0] := vector.X;
  result.matrix[1,1] := vector.Y;
  result.matrix[2,2] := vector.Z;

  MIdentity (result.inverse);

  result.inverse[0,0] := 1.0 / vector.X;
  result.inverse[1,1] := 1.0 / vector.Y;
  result.inverse[2,2] := 1.0 / vector.Z;
end;
{-------------------------------------------------------------}
Procedure Compute_Translation_Transform (var result:TTRANSFORM; vector:tVec);
begin
  MIdentity (result.matrix);

  result.matrix[3,0] := vector.X;
  result.matrix[3,1] := vector.Y;
  result.matrix[3,2] := vector.Z;

  MIdentity (result.inverse);

  result.inverse[3,0] := -vector.X;
  result.inverse[3,1] := -vector.Y;
  result.inverse[3,2] := -vector.Z;
end;
{-------------------------------------------------------------}
Procedure Compute_Rotation_Transform (var transform:TTRANSFORM; vector:tVec);
var
  cosx, cosy, cosz, sinx, siny, sinz : DBL;
  Matrix : TMATRIX;
  Radian_Vector : tVec;
begin
  VLinear (Radian_Vector, vector, vector, M_PI_180,0);

  MIdentity (transform.matrix);

  cosx := cos (Radian_Vector.X);
  sinx := sin (Radian_Vector.X);
  cosy := cos (Radian_Vector.Y);
  siny := sin (Radian_Vector.Y);
  cosz := cos (Radian_Vector.Z);
  sinz := sin (Radian_Vector.Z);

  transform.matrix[1,1] := cosx;
  transform.matrix[2,2] := cosx;
  transform.matrix[1,2] := sinx;
  transform.matrix[2,1] := 0.0 - sinx;

  MTranspose (transform.inverse, transform.matrix);

  MIdentity (Matrix);

  Matrix[0,0] := cosy;
  Matrix[2,2] := cosy;
  Matrix[0,2] := 0.0 - siny;
  Matrix[2,0] := siny;

  MTimes (transform.matrix, transform.matrix, Matrix);

  MTranspose (Matrix, Matrix);

  MTimes (transform.inverse, Matrix, transform.inverse);

  MIdentity (Matrix);

  Matrix[0,0] := cosz;
  Matrix[1,1] := cosz;
  Matrix[0,1] := sinz;
  Matrix[1,0] := 0.0 - sinz;

  MTimes(transform.matrix, transform.matrix, Matrix);

  MTranspose (Matrix, Matrix);

  MTimes (transform.inverse, Matrix, transform.inverse);
end;
{-------------------------------------------------------------}
Procedure Compose_Transforms (var Original_Transform, New_Transform:TTRANSFORM);
begin
  MTimes(Original_Transform.matrix, Original_Transform.matrix,  New_Transform.matrix);

  MTimes(Original_Transform.inverse, New_Transform.inverse, Original_Transform.inverse);
end;
{-------------------------------------------------------------}
Procedure Compute_Axis_Rotation_Transform (var transform:TTRANSFORM; V1:tVec; angle:DBL);
var
  l, cosx, sinx: DBL;
begin

  VNorm(V1, 1);

  MIdentity(transform.matrix);

  cosx := cos(angle);
  sinx := sin(angle);

  transform.matrix[0,0] := V1.X * V1.X + cosx * (1.0 - V1.X * V1.X);
  transform.matrix[0,1] := V1.X * V1.Y * (1.0 - cosx) + V1.Z * sinx;
  transform.matrix[0,2] := V1.X * V1.Z * (1.0 - cosx) - V1.Y * sinx;

  transform.matrix[1,0] := V1.X * V1.Y * (1.0 - cosx) - V1.Z * sinx;
  transform.matrix[1,1] := V1.Y * V1.Y + cosx * (1.0 - V1.Y * V1.Y);
  transform.matrix[1,2] := V1.Y * V1.Z * (1.0 - cosx) + V1.X * sinx;

  transform.matrix[2,0] := V1.X * V1.Z * (1.0 - cosx) + V1.Y * sinx;
  transform.matrix[2,1] := V1.Y * V1.Z * (1.0 - cosx) - V1.X * sinx;
  transform.matrix[2,2] := V1.Z * V1.Z + cosx * (1.0 - V1.Z * V1.Z);

  MTranspose(transform.inverse, transform.matrix);
end;
{-------------------------------------------------------------}
Procedure Compute_Coordinate_Transform (var trans:TTRANSFORM; origin, up:tVec; r,len:DBL);
var
  trans2 : TTRANSFORM ;
  tmpv   : tVec;
begin
  VSet(tmpv, r, r, len);

  Compute_Scaling_Transform(trans, tmpv);

  if abs(up.Z) > (1.0 - EPS) then
  begin
    VSet(tmpv, 1.0, 0.0, 0.0);
    if Up.Z > 0 then up.Z := 1.0
                else up.Z := -1.0;
  end
  else
    VSet(tmpv, -up.Y, up.X, 0.0);

  Compute_Axis_Rotation_Transform(trans2, tmpv, ArcCos(up.Z) );

  Compose_Transforms(trans, trans2);

  Compute_Translation_Transform(trans2, origin);

  Compose_Transforms(trans, trans2);
end;
{-------------------------------------------------------------}
Procedure MInvers (var r,m:TMATRIX);
var
  d00, d01, d02, d03 : DBL;
  d10, d11, d12, d13 : DBL;
  d20, d21, d22, d23 : DBL;
  d30, d31, d32, d33 : DBL;
  m00, m01, m02, m03 : DBL;
  m10, m11, m12, m13 : DBL;
  m20, m21, m22, m23 : DBL;
  m30, m31, m32, m33 : DBL;
  D : DBL;
begin
  m00 := m[0,0];  m01 := m[0,1];  m02 := m[0,2];  m03 := m[0,3];
  m10 := m[1,0];  m11 := m[1,1];  m12 := m[1,2];  m13 := m[1,3];
  m20 := m[2,0];  m21 := m[2,1];  m22 := m[2,2];  m23 := m[2,3];
  m30 := m[3,0];  m31 := m[3,1];  m32 := m[3,2];  m33 := m[3,3];

  d00 := m11*m22*m33 + m12*m23*m31 + m13*m21*m32 - m31*m22*m13 - m32*m23*m11 - m33*m21*m12;
  d01 := m10*m22*m33 + m12*m23*m30 + m13*m20*m32 - m30*m22*m13 - m32*m23*m10 - m33*m20*m12;
  d02 := m10*m21*m33 + m11*m23*m30 + m13*m20*m31 - m30*m21*m13 - m31*m23*m10 - m33*m20*m11;
  d03 := m10*m21*m32 + m11*m22*m30 + m12*m20*m31 - m30*m21*m12 - m31*m22*m10 - m32*m20*m11;

  d10 := m01*m22*m33 + m02*m23*m31 + m03*m21*m32 - m31*m22*m03 - m32*m23*m01 - m33*m21*m02;
  d11 := m00*m22*m33 + m02*m23*m30 + m03*m20*m32 - m30*m22*m03 - m32*m23*m00 - m33*m20*m02;
  d12 := m00*m21*m33 + m01*m23*m30 + m03*m20*m31 - m30*m21*m03 - m31*m23*m00 - m33*m20*m01;
  d13 := m00*m21*m32 + m01*m22*m30 + m02*m20*m31 - m30*m21*m02 - m31*m22*m00 - m32*m20*m01;

  d20 := m01*m12*m33 + m02*m13*m31 + m03*m11*m32 - m31*m12*m03 - m32*m13*m01 - m33*m11*m02;
  d21 := m00*m12*m33 + m02*m13*m30 + m03*m10*m32 - m30*m12*m03 - m32*m13*m00 - m33*m10*m02;
  d22 := m00*m11*m33 + m01*m13*m30 + m03*m10*m31 - m30*m11*m03 - m31*m13*m00 - m33*m10*m01;
  d23 := m00*m11*m32 + m01*m12*m30 + m02*m10*m31 - m30*m11*m02 - m31*m12*m00 - m32*m10*m01;

  d30 := m01*m12*m23 + m02*m13*m21 + m03*m11*m22 - m21*m12*m03 - m22*m13*m01 - m23*m11*m02;
  d31 := m00*m12*m23 + m02*m13*m20 + m03*m10*m22 - m20*m12*m03 - m22*m13*m00 - m23*m10*m02;
  d32 := m00*m11*m23 + m01*m13*m20 + m03*m10*m21 - m20*m11*m03 - m21*m13*m00 - m23*m10*m01;
  d33 := m00*m11*m22 + m01*m12*m20 + m02*m10*m21 - m20*m11*m02 - m21*m12*m00 - m22*m10*m01;

  D := m00*d00 - m01*d01 + m02*d02 - m03*d03;

  if (D = 0.0) then
  begin
   WriteLn('Singular matrix in MInvers.\n');
   Halt(255);
  end;

  r[0,0] :=  d00/D; r[0,1] := -d10/D;  r[0,2] :=  d20/D; r[0,3] := -d30/D;
  r[1,0] := -d01/D; r[1,1] :=  d11/D;  r[1,2] := -d21/D; r[1,3] :=  d31/D;
  r[2,0] :=  d02/D; r[2,1] := -d12/D;  r[2,2] :=  d22/D; r[2,3] := -d32/D;
  r[3,0] := -d03/D; r[3,1] :=  d13/D;  r[3,2] := -d23/D; r[3,3] :=  d33/D;
end;

END.