Unit QMATRIX;   (* 6/20/1989 *)
                (* PC port started 6/8/1990 *)
  (* The quick matrix routines collection. *)
  (* minimal or no error checking is done *)
(******************************************)

INTERFACE

const
  MAXROWS = 36;
  MAXCOLS = MAXROWS;

type
  px1_   =  record
      rows: integer; (* the dynamic length the vector *)
      ve  : array(.1..MAXROWS.) of real; (* short real-vector *)
    end;

  marray = array(.1..MAXROWS, 1..MAXCOLS.) of real;
  matrix = record
             el : ^marray;
             rows, cols : integer;
           end;

var
  MatFail : BOOLEAN;
  StdErr  : text;

procedure MatDim (var a : matrix; r, c : integer);
procedure MatReDim (var a : matrix; r, c : integer);
procedure MatRead  (var f : text; var a : matrix);
procedure MatWrite (var f : text; var a : matrix);
procedure MatMul   (var a, b, c : matrix);
procedure MatAdd   (var a, b, c : matrix);
procedure MatSub   (var a, b, c : matrix);
procedure MatScale (var a, b : matrix; c : real);
procedure MatU2L   (var a : matrix);
procedure MatL2U   (var a : matrix);
procedure MatTran  (var a, b : matrix);
procedure MatFill  (var a : matrix; b : real);
procedure MatChol  (var A : matrix);
procedure MatInvL  (var L : matrix);
procedure MatLpL   (var L : matrix);
procedure MatSInv  (var A : matrix);
procedure MatSHInv (var A : matrix);
procedure ZeroOut  (var a : matrix; r1, r2, c1, c2 : integer);
procedure VecWrite (var f : text; var v : px1_);
procedure MatxVec  (var b : px1_; var A  : matrix; var x  : px1_);
procedure VecFill  (var x : px1_; r : real);
function  VecSumAbs(var x : px1_) : real;
procedure AddOP    (var m : matrix; var v : px1_);
procedure VecAdd   (var v1, v2, v3 : px1_);      (* v1 <- v1 + v2 *)

(******************************************)

IMPLEMENTATION

type
  string80_ = string[80];

const
  MatMsg1 = 'Arguments out of range';
  MatMsg2 = 'Matrices not conformable';
  MatMsg3 = 'Invalid matrix dimension';
  MatMsg4 = 'EOF reached';
  MatMsg5 = 'Singular matrix';
  MatMsg6 = 'Negative definite matrix';
  HALTYE  = TRUE;
  HALTNO  = FALSE;
(************************)
procedure err (s : string80_; haltflag : BOOLEAN);
  (* central error control: all errors routed here *)
Begin
  writeln(s);
  if haltflag then HALT;
End;


(******************************************)
procedure MatL2U; (* reflect lower half to upper half *)
  (* matrix must be square *)
const
  myname = 'MatL2U: ';
var  i, j : integer;
Begin (* MatL2U *)
  with a do
  begin
    for i := 2 to rows do
    for j := 1 to pred(i) do el^(.j,i.) := el^(.i,j.);
  end;
End; (* MatL2U *)
(******************************************)
procedure MatU2L; (* reflect upper half to lower half *)
  (* matrix must be square *)
const
  myname = 'MatU2L: ';
var i, j : integer;
Begin (* MatU2L *)
  with a do
  begin
    for i := 2 to rows do
    for j := 1 to pred(i) do el^(.i,j.) := el^(.j,i.);
  end;
End; (* MatU2L *)
(******************************************)
procedure MatDim; (* allocates memory *)
const
  myname = 'MatDim: ';
Begin
  if (r > MAXROWS) or (c > MAXCOLS) or (r < 1) or (c < 1) then
  begin
    writeln (StdErr, myname, MatMsg1);
    writeln (StdErr, 'r = ', r : 4, '  c = ', c : 4);
    MatFail := TRUE;
    EXIT;
  end;
  MatFail := FALSE;
  new (a.el);
  a.rows := r;
  a.cols := c;
End;

(******************************************)
procedure MatReDim; (* no memory allocation *)
const
  myname = 'MatReDim: ';
Begin (* MatReDim *)
  if (r > MAXROWS) or (c > MAXCOLS) or (r < 1) or (c < 1) then
  begin
    writeln (StdErr, myname, MatMsg1);
    writeln (StdErr, 'r = ', r : 4, '  c = ', c : 4);
    MatFail := TRUE;
    EXIT;
  end;
  MatFail := FALSE;
  a.rows := r;
  a.cols := c;
End;  (* MatReDim *)

(******************************************)
procedure MatRead;
const
  myname = 'MatRead: ';
var
  i, j : integer;
Begin
  with a do
  for i := 1 to rows do for j := 1 to cols do
  begin
    if EOF(f) then
    begin
      writeln(StdErr, myname, MatMsg4);
      MatFail := TRUE;
      EXIT;
    end;
    read (f, el^(.i,j.));
  end;
  MatFail := FALSE;
End;
(******************************************)
procedure MatWrite;
const
  myname = 'MatWrite: ';
  SP = ' ';
  CPP = 12; (* max columns per page *)
var
  i, j, column : integer;
Begin
  with a do
  for i := 1 to rows do
  begin
    column := 0;
    write ('(' : 1, i : 2, ') ' : 2 );
    for j := 1 to cols do
    begin
      column := succ(column);  (* bump *)
      if (column > CPP) then   (* end of line? *)
      begin
        column := 1;           (* new line *)
        writeln (f);
        write (f, '':5);
      end;
      write (f, el^(.i,j.)  : 8 : 5, '' : 2); (* at column *)
    end;
    writeln (f);
  end;
  writeln (f);
  MatFail := FALSE;
End;
(******************************************)
procedure MatMul;
   (* a := b * c *)
const
  myname = 'MatMul: ';
var
  i, j, k : integer;
  s : real;
Begin
  if (b.cols <> c.rows) then
  begin
    writeln (StdErr, myname, MatMsg2);
    MatFail := TRUE;
    EXIT;
  end;
  MatReDim (a, b.rows, c.cols);
  for i := 1 to b.rows do
  for j := 1 to c.cols do
  begin
    s := 0;
    for k := 1 to b.cols do
       s := s + b.el^(.i, k.) * c.el^(.k, j.);
    a.el^(.i, j.) :=  s;
  end;
End;
(******************************************)
procedure MatAdd;
   (* a := b + c *)
const
  myname = 'MatAdd: ';
var
  i, j : integer;
Begin
  if not ((b.rows = c.rows) and (b.cols = c.cols)) then
  begin
    writeln (StdErr, myname, MatMsg2);
    MatFail := TRUE;
    EXIT;
  end;
  MatReDim (a, b.rows, b.cols);
  for i := 1 to b.rows do
  for j := 1 to b.cols do
  begin
    a.el^(.i,j.) :=  b.el^(.i, j.) + c.el^(.i, j.);
  end;
End;
(******************************************)
procedure MatSub;
   (* a := b - c *)
const
  myname = 'MatSub: ';
var
  i, j : integer;
Begin
  if not ((b.rows = c.rows) and (b.cols = c.cols)) then
  begin
    writeln (StdErr, myname, MatMsg2);
    MatFail := TRUE;
    EXIT;
  end;
  MatReDim (a, b.rows, b.cols);
  for i := 1 to b.rows do
  for j := 1 to b.cols do
  begin
    a.el^(.i,j.) :=  b.el^(.i, j.) - c.el^(.i, j.);
  end;
End;
(******************************************)
procedure MatScale;
   (* a := b * c *)
const
  myname = 'MatScale: ';
var
  i, j : integer;
Begin
  MatReDim (a, b.rows, b.cols);
  for i := 1 to b.rows do
  for j := 1 to b.cols do
  begin
    a.el^(.i, j.) := b.el^(.i, j.) * c;
  end;
End;
(******************************************)
procedure MatTran;
  (* a := b' *)
const
  myname = 'MatTran: ';
var
  i, j : integer;
Begin
  MatReDim (a, b.cols, b.rows);
  with a do
  for i := 1 to b.rows do
  for j := 1 to b.cols do el^(.j, i.) := b.el^(.i, j.);
End;
(******************************************)
procedure MatFill;
var
  i, j : integer;
Begin
  with a do
  for i := 1 to rows do
  for j := 1 to cols do
  begin
    el^(.i, j.) :=  b;
  end;
  MatFail := FALSE;
End;

(************************)
procedure VecWrite (var f : text; var v : px1_);
var
  i : integer;

Begin (* VecWrite *)
  with v do
  for i := 1 to rows do
  writeln (f, i:8, '':2, ve(.i.) : 8 : 5 );
End;  (* VecWrite *)
(************************)
procedure ZeroOut (var a : matrix; r1, r2, c1, c2 : integer);
var
  row, col : integer;
Begin
  with a do
  begin
    for row := r1 to r2 do
    for col := c1 to c2 do
    el^(.row, col.) := 0
  end
End;
(************************)
procedure MatxVec (var b  : px1_;
                    var A  : matrix;
                    var x  : px1_);
        (* d <- A * x *)
        (* must: A.cols == x.rows *)
var
  i, j : integer;
  w : real;
Begin (* MatxVec *)
  if (A.cols <> x.rows) then
    err('MatxVec: nonconformable dimensions.', HALTYE);

  b.rows := A.rows;
  with A do
  for i := 1 to rows do
  begin
    w := 0;
    for j := 1 to cols do w := w + el^(.i,j.) * x.ve(.j.);
    b.ve(.i.) := w;
  end;
End; (* MatxVec *)
(************************)
procedure VecFill (var x  : px1_;
                       r  : real);
var
  i : integer;
Begin (* VecFill *)
  with x do
    for i := 1 to rows do ve(.i.) := r;
End; (* VecFill *)
(************************)
function  VecSumAbs (var x  : px1_) : real;
var
  i : integer;
  w : real;
Begin (* VecSumAbs *)
  w := 0;
  with x do
    for i := 1 to rows do w := w + abs(ve(.i.));
  VecSumAbs  := w;
End; (* VecSumAbs *)
(************************)
procedure AddOP (var m : matrix;
                 var v : px1_);
      (* m  <- m  + outer product of v *)
var
  i, j : integer;  c : real;

Begin (*  AddOP *)
  if (m.rows <> m.cols) or (m.rows <> v.rows) then
  err('AddOP: Nonconformable', HALTYE);
  with m  do
  begin
    for i := 1 to rows do el^(.i,i.) := el^(.i,i.) + sqr(v.ve(.i.));
    for i := 1 to pred(rows) do
    for j := succ(i) to rows do
    begin
      c := v.ve(.i.)*v.ve(.j.);
      el^(.i,j.) := el^(.i,j.) + c;
      el^(.j,i.) := el^(.j,i.) + c;
    end;
  end;
End;  (*  AddOP *)
(************************)
procedure  VecAdd (var v1, v2, v3 : px1_);      (* v1 <- v1 + v2 *)
var
  i : integer;

Begin (*   VecAdd *)
  if (v2.rows <> v3.rows) then
  err('VecAdd: Nonconformable', HALTYE);
  v1.rows := v2.rows;
  with v1 do
  for i := 1 to rows do ve(.i.) := v2.ve(.i.) + v3.ve(.i.);
End;  (*   VecAdd *)


(* ============================================================= *)
(*
  Invert a symmetric positive definite matrix in situ
  using Cholesky  decomposition
  Translated for the matrix procedures. 6/20/1989
*)
(* ============================================================= *)
(* By  Bahjat Qaqish               2/8 /1988                     *)
(* procs: MatSInv, MatChol, MatInvL, MatSInv, MatSHInv   *)
(* all what is needed to invert a symmetric positive definite    *)
(* matrix in situ                                                *)
(* ============================================================= *)
procedure MatChol ;
(*
       Cholesky decomposition of a symmetric positive definite
       matrix A = L L' . L is returned in the
       lower triangle of A, upper A not accessed.
*)
const
  epsilon = 1.0E-9;
  myname = 'MatChol: ';
var
    k, i, j, n :integer ;
    s, w       : real ;

begin  (* MatChol *)
   n := a.rows;
   with a do
   for k := 1 to n   do
   begin ;   (* k-loop *)
     for i := 1 to pred(k) do
     begin ;     (* i-loop *)
       s := 0   ;
       for j := 1 to pred(i)  do
         s := s + el^(.i, j.) * el^(.k, j.);
         w := el^(.i,i.);
         if (w < 0) then
         begin
           MatFail := TRUE;
           writeln (StdErr, myname, MatMsg6);
           EXIT;
         end;
         if (w < epsilon) then
         begin
           MatFail := TRUE;
           writeln (StdErr, myname, MatMsg5);
           EXIT;
        end;
       el^(.k, i.) := (el^(.k, i.)-s) / w;
     end ;       (* i-loop *)
     s := 0;
     for j := 1 to k-1 do s := s + sqr (el^(.k,j.));
     w := el^(.k,k.) - s;
     if (w < 0) then
     begin
       MatFail := TRUE;
       writeln (StdErr, myname, MatMsg6);
       EXIT;
     end;
     el^(.k, k.) :=  sqrt (w);
   end ; (* k-loop *)
   MatFail := FALSE;
end ; (*  MatChol  *)
(* ============================================================= *)
procedure MatInvL ;
(*
  by Bahjat Qaqish 1/21/1987
  invert a lower triangular matrix L in situ
  assumes L is invertible. Upper L not changed
*)
const
  epsilon = 1.0E-9;
  myname = 'MatInvL: ';
var
  i,j,p, n : integer ;
  s,w   : real ;

begin (* MatInvL  *)
  n := L.rows;
  with L do
  for i:= 1 to n do
  begin
    for j:=1 to pred(i) do
    begin
      s:=0 ;
      for p:=j to pred(i) do
        s := s + el^(.i,p.)*el^(.p,j.) ;
      w := el^(.i,i.);
      if (abs(w) < epsilon) then
      begin
        MatFail := TRUE;
        writeln (StdErr, myname, MatMsg5);
        EXIT;
      end;
      el^(.i,j.) := -s / w;
    end ;
    w := el^(.i,i.);
    if (abs(w) < epsilon) then
    begin
      MatFail := TRUE;
      writeln (StdErr, myname, MatMsg5);
      EXIT;
    end;
    el^(.i,i.) := 1 / w;
  end ;
  MatFail := FALSE;
end ; (* MatInvL  *)
(* ============================================================= *)
procedure MatLpL ;
(*
  by Bahjat Qaqish 1/21/1987
  compute L'L , where L is lower triangular
  result is returned in L
*)
var
  i,j,k: integer ;
  s    : real ;

begin (* MatLpL *)
  with L do
  (* first compute lower triangle of LPL *)
  for i:= 1 to rows do
  for j:= 1 to i do
  begin
    s:=0 ;
    for k := i to rows do s:=s+el^(.k,i.)*el^(.k,j.);
    el^(.i,j.) := s;
  end ;
  (* next miror image into upper triangle *)
  MatL2U (L);
end ; (* MatLpL *)
(* ============================================================= *)
procedure MatSInv;
(*
  by Bahjat Qaqish    1/21/1987
  inverts a symmetric positive definite matrix A in situ
*)
begin  (* MatSInv *)
  MatChol    (A);  (* A =LL' (L in A ) *)
  if MatFail then EXIT;
  MatInvL  (A);  (* A <- inverse of A *)
  if MatFail then EXIT;
  MatLpL     (A);  (* A <- A'A *)
end ;  (* MatSInv *)
(* ============================================================= *)
procedure MatSHInv ;
(*
  by Bahjat Qaqish    1/21/1987
  returns the inverse of the Cholesky root of A  into lower A
*)
begin  (* MatSHInv *)
  MatChol (A);  (* A    <- lower Cholesky root of A *)
  MatInvL (A);  (* A    <- inverse of lower A *)
end ;  (* MatSHInv *)
(* ============================================================= *)

Begin (* qmatrix *)
  assign (StdErr, 'con');
  rewrite (StdErr);
End.  (* qmatrix *)