unit grampsmath;

{$mode objfpc}{$H+}

interface

uses
  Classes, SysUtils, grampstypen, math;

function KreuzproduktZ(P: tExtendedArray; A1,A2,B1,B2: tPerson): extended; inline;
function QAbstand(P: tExtendedArray; A1,A2: tPerson): extended; inline;
function Skalarprodukt(P: tExtendedArray; A1,A2,B1,B2: tPerson): extended; inline;
function rootDet(mat: array of tExtendedarray): extended;

implementation

function KreuzproduktZ(P: tExtendedArray; A1,A2,B1,B2: tPerson): extended;
begin
  result:=            //   ( (A1-A2) x (B1-B2) ).z =
      (P[A1.p1]-P[A2.p1])*(P[B1.p2]-P[B2.p2])      //   (A1.x - A2.x) * (B1.y - B2.y)
    - (P[B1.p1]-P[B2.p1])*(P[A1.p2]-P[A2.p2]);     // - (B1.x - B2.x) * (A1.y - A2.y)
(*  writeln((P[A1.p1]-P[A2.p1])*(P[B1.p2]-P[B2.p2]) - (P[B1.p1]-P[B2.p1])*(P[A1.p2]-P[A2.p2]));
  writeln('(',P[A1.p1],' - ',P[A2.p1],') * (',P[B1.p2],' - ',P[B2.p2],') - (',P[B1.p1],' - ',P[B2.p1],') * (',P[A1.p2],' - ',P[A2.p2],')');
  writeln('(',A1.p1,' - ',A2.p1,') * (',B1.p2,' - ',B2.p2,') - (',B1.p1,' - ',B2.p1,') * (',A1.p2,' - ',A2.p2,')'); *)
end;

function QAbstand(P: tExtendedArray; A1,A2: tPerson): extended;
begin
 result:=
   sqr(P[A1.p1]-P[A2.p1]) +
   sqr(P[A1.p2]-P[A2.p2]);
end;

function Skalarprodukt(P: tExtendedArray; A1,A2,B1,B2: tPerson): extended;
begin
 result:=
   (P[A1.p1]-P[A2.p1])*(P[B1.p1]-P[B2.p1]) +
   (P[A1.p2]-P[A2.p2])*(P[B1.p2]-P[B2.p2]);
end;

function rootDet(mat: array of tExtendedarray): extended;
var
  dim,i,j,k: longint;
  fak,tmp:   extended;
begin
  dim:=length(mat);
  result:=1;

  for i:=0 to dim-1 do begin          // die zu eliminierende Spalte
    k:=-1;
    tmp:=0;
    for j:=i to dim-1 do
      if abs(mat[j,i])>tmp then begin
        k:=j;
        tmp:=abs(mat[j,i]);
      end;

    if tmp<1e-40 then begin           // nur noch 0en in Spalte i
      result:=0;
      break;
    end;

    if k<>i then                      // Zeile k und i tauschen
      for j:=i to dim-1 do begin
        tmp:=mat[i,j];
        mat[i,j]:=mat[k,j];
        mat[k,j]:=tmp;
      end;
    result:=result*power(abs(mat[i,i]),1/dim);
    for j:=i+1 to dim-1 do begin
      fak:=-mat[j,i]/mat[i,i];
      for k:=i to dim-1 do
        mat[j,k]:=mat[j,k]+fak*mat[i,k];
    end;
  end;
end;

end.